diff options
Diffstat (limited to 'zephyr')
| -rw-r--r-- | zephyr/dts/bindings/emul/zephyr,bmi.yaml | 41 | ||||
| -rw-r--r-- | zephyr/emul/CMakeLists.txt | 2 | ||||
| -rw-r--r-- | zephyr/emul/Kconfig | 6 | ||||
| -rw-r--r-- | zephyr/emul/emul_bmi.c | 1342 | ||||
| -rw-r--r-- | zephyr/emul/emul_bmi160.c | 747 | ||||
| -rw-r--r-- | zephyr/include/emul/emul_bmi.h | 518 |
6 files changed, 2656 insertions, 0 deletions
diff --git a/zephyr/dts/bindings/emul/zephyr,bmi.yaml b/zephyr/dts/bindings/emul/zephyr,bmi.yaml new file mode 100644 index 0000000000..0067407e61 --- /dev/null +++ b/zephyr/dts/bindings/emul/zephyr,bmi.yaml @@ -0,0 +1,41 @@ +# Copyright 2021 The Chromium OS Authors. All rights reserved. +# Use of this source code is governed by a BSD-style license that can be +# found in the LICENSE file. + +description: Zephyr BMI Emulator + +compatible: "zephyr,bmi" + +include: base.yaml + +properties: + device-model: + type: string + required: true + enum: + - BMI_EMUL_160 + description: Model of device that is emulated. + + error-on-ro-write: + type: boolean + description: + Flag indicating if error should be generated when read only register + is being written. + + error-on-wo-read: + type: boolean + description: + Flag indicating if error should be generated when write only register + is being read. + + error-on-reserved-bit-write: + type: boolean + description: + Flag indicating if error should be generated when reserved bit + is being written. + + simulate-command-exec-time: + type: boolean + description: + Flag indicating if emulator should wait the same amount of time before + finishing command as real device would. diff --git a/zephyr/emul/CMakeLists.txt b/zephyr/emul/CMakeLists.txt index fc91a74e21..763372cddc 100644 --- a/zephyr/emul/CMakeLists.txt +++ b/zephyr/emul/CMakeLists.txt @@ -6,3 +6,5 @@ zephyr_library_sources_ifdef(CONFIG_EMUL_SMART_BATTERY emul_smart_battery.c) zephyr_library_sources_ifdef(CONFIG_EMUL_BMA255 emul_bma255.c) zephyr_library_sources_ifdef(CONFIG_EMUL_BC12_DETECT_PI3USB9201 emul_pi3usb9201.c) zephyr_library_sources_ifdef(CONFIG_EMUL_PPC_SYV682X emul_syv682x.c) +zephyr_library_sources_ifdef(CONFIG_EMUL_BMI emul_bmi.c) +zephyr_library_sources_ifdef(CONFIG_EMUL_BMI emul_bmi160.c) diff --git a/zephyr/emul/Kconfig b/zephyr/emul/Kconfig index 5a5747d922..76b4cb5566 100644 --- a/zephyr/emul/Kconfig +++ b/zephyr/emul/Kconfig @@ -27,3 +27,9 @@ config EMUL_PPC_SYV682X help Enable the SYV682x emulator. SYV682 is a USB Type-C PPC. This driver uses the emulated I2C bus. +config EMUL_BMI + bool "BMI emulator" + help + Enable the BMI emulator. This driver use emulated I2C bus. + It is used to test bmi 160 and 260 drivers. Emulators API is + available in zephyr/include/emul/emul_bmi.h diff --git a/zephyr/emul/emul_bmi.c b/zephyr/emul/emul_bmi.c new file mode 100644 index 0000000000..0bcd01c4dc --- /dev/null +++ b/zephyr/emul/emul_bmi.c @@ -0,0 +1,1342 @@ +/* Copyright 2021 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +#define DT_DRV_COMPAT zephyr_bmi + +#define LOG_LEVEL CONFIG_I2C_LOG_LEVEL +#include <logging/log.h> +LOG_MODULE_REGISTER(emul_bmi); + +#include <device.h> +#include <emul.h> +#include <drivers/i2c.h> +#include <drivers/i2c_emul.h> + +#include "emul/emul_bmi.h" + +#include "driver/accelgyro_bmi160.h" +#include "driver/accelgyro_bmi260.h" +#include "driver/accelgyro_bmi_common.h" + +/** + * Describe if there is no ongoing I2C message or if there is message handled + * at the moment (last message doesn't ended with stop or write is not followed + * by read). + */ +enum bmi_emul_msg_state { + BMI_EMUL_NONE_MSG, + BMI_EMUL_IN_WRITE, + BMI_EMUL_IN_READ +}; + +/** Run-time data used by the emulator */ +struct bmi_emul_data { + /** I2C emulator detail */ + struct i2c_emul emul; + /** BMI device being emulated */ + const struct device *i2c; + /** Configuration information */ + const struct bmi_emul_cfg *cfg; + + /** Current state of all emulated BMI registers */ + uint8_t reg[BMI_EMUL_MAX_REG]; + /** Internal offset values used in calculations */ + int16_t off_acc_x; + int16_t off_acc_y; + int16_t off_acc_z; + int16_t off_gyr_x; + int16_t off_gyr_y; + int16_t off_gyr_z; + /** Internal values of sensors */ + int32_t acc_x; + int32_t acc_y; + int32_t acc_z; + int32_t gyr_x; + int32_t gyr_y; + int32_t gyr_z; + /** Current state of NVM where offset and configuration can be saved */ + uint8_t nvm[BMI_EMUL_MAX_NVM_REGS]; + + /** Return error when trying to write to RO register */ + bool error_on_ro_write; + /** Return error when trying to write 1 to reserved bit */ + bool error_on_rsvd_write; + /** + * If effect of command is vissable after simulated time from issuing + * command + */ + bool simulate_command_exec_time; + /** Return error when trying to read WO register */ + bool error_on_wo_read; + + /** Current state of I2C bus (if emulator is handling message) */ + enum bmi_emul_msg_state msg_state; + /** Number of already handled bytes in ongoing message */ + int msg_byte; + /** Register selected in last write command */ + uint8_t cur_reg; + /** Value of data byte in ongoing write message */ + uint8_t write_byte; + + /** Custom write function called on I2C write opperation */ + bmi_emul_write_func write_func; + /** Data passed to custom write function */ + void *write_func_data; + /** Custom read function called on I2C read opperation */ + bmi_emul_read_func read_func; + /** Data passed to custom read function */ + void *read_func_data; + + /** Control if read should fail on given register */ + int read_fail_reg; + /** Control if write should fail on given register */ + int write_fail_reg; + + /** List of FIFO frames */ + struct bmi_emul_frame *fifo_frame; + /** First FIFO frame in byte format */ + uint8_t fifo[21]; + /** Number of FIFO frames that were skipped */ + uint8_t fifo_skip; + /** Currently accessed byte of first frame */ + int fifo_frame_byte; + /** Length of first frame */ + int fifo_frame_len; + + /** Last time when emulator was resetted in sensor time units */ + int64_t zero_time; + /** Time when current command should end */ + uint32_t cmd_end_time; + + /** Emulated model of BMI */ + int type; + /** Pointer to data specific for emulated model of BMI */ + const struct bmi_emul_type_data *type_data; + + /** Mutex used to control access to emulator data */ + struct k_mutex data_mtx; +}; + +/** Static configuration for the emulator */ +struct bmi_emul_cfg { + /** Label of the I2C bus this emulator connects to */ + const char *i2c_label; + /** Pointer to run-time data */ + struct bmi_emul_data *data; + /** Address of BMI on i2c bus */ + uint16_t addr; +}; + +/** Check description in emul_bmi.h */ +int bmi_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + return k_mutex_lock(&data->data_mtx, timeout); +} + +/** Check description in emul_bmi.h */ +int bmi_emul_unlock_data(struct i2c_emul *emul) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + return k_mutex_unlock(&data->data_mtx); +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_write_func(struct i2c_emul *emul, + bmi_emul_write_func func, void *data) +{ + struct bmi_emul_data *emul_data; + + emul_data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + emul_data->write_func = func; + emul_data->write_func_data = data; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_read_func(struct i2c_emul *emul, + bmi_emul_read_func func, void *data) +{ + struct bmi_emul_data *emul_data; + + emul_data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + emul_data->read_func = func; + emul_data->read_func_data = data; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val) +{ + struct bmi_emul_data *data; + + if (reg < 0 || reg > BMI_EMUL_MAX_REG) { + return; + } + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + data->reg[reg] = val; +} + +/** Check description in emul_bmi.h */ +uint8_t bmi_emul_get_reg(struct i2c_emul *emul, int reg) +{ + struct bmi_emul_data *data; + + if (reg < 0 || reg > BMI_EMUL_MAX_REG) { + return 0; + } + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + return data->reg[reg]; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_read_fail_reg(struct i2c_emul *emul, int reg) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + data->read_fail_reg = reg; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_write_fail_reg(struct i2c_emul *emul, int reg) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + data->write_fail_reg = reg; +} + +/** + * @brief Convert @p val to two's complement representation. It makes sure that + * bit representation is correct even on platforms which represent + * signed inteager in different format. Unsigned bit representation + * allows to use well defined bitwise operations on returned value. + * + * @param val Inteager that is converted + * + * @return two's complement representation of @p val + */ +static uint32_t bmi_emul_val_to_twos_comp(int32_t val) +{ + uint32_t twos_comp_val; + + /* Make sure that value is converted to twos compliment format */ + if (val < 0) { + twos_comp_val = (uint32_t)(-val); + twos_comp_val = ~twos_comp_val + 1; + } else { + twos_comp_val = (uint32_t)val; + } + + return twos_comp_val; +} + +/** + * @brief Convert accelerometer value from NVM format (8bit, 0x01 == 3.9mg) + * to internal offset format (16bit, 0x01 == 0.061mg). + * + * @param nvm Value in NVM format (8bit, 0x01 == 3.9mg). This is binary + * representation of two's complement signed number. + * + * @return offset Internal representation of @p nvm (16bit, 0x01 == 0.061mg) + */ +static int16_t bmi_emul_acc_nvm_to_off(uint8_t nvm) +{ + int16_t offset; + int8_t sign; + + if (nvm & BIT(7)) { + sign = -1; + /* NVM value is in two's complement format */ + nvm = ~nvm + 1; + } else { + sign = 1; + } + + offset = (int16_t)nvm; + /* LSB in NVM is 3.9mg, while LSB in internal offset is 0.061mg */ + offset *= sign * 64; + + return offset; +} + +/** + * @brief Convert gyroscope value from NVM format (10bit, 0x01 == 0.061 °/s) + * to internal offset format (16bit, 0x01 == 0.0038 °/s) + * + * @param nvm Value in NVM format (10bit, 0x01 == 0.061 °/s). This is binary + * representation of two's complement signed number. + * + * @return offset Internal representation of @p nvm (16bit, 0x01 == 0.0038 °/s) + */ +static int16_t bmi_emul_gyr_nvm_to_off(uint16_t nvm) +{ + int16_t offset; + int8_t sign; + + if (nvm & BIT(9)) { + sign = -1; + /* NVM value is in two's complement format */ + nvm = ~nvm + 1; + } else { + sign = 1; + } + + /* Mask 10 bits which holds value */ + nvm &= 0x3ff; + + offset = (int16_t)nvm; + /* LSB in NVM is 0.061°/s, while LSB in internal offset is 0.0038°/s */ + offset *= sign * 16; + + return offset; +} + +/** + * @brief Convert accelerometer value from internal offset format + * (16bit, 0x01 == 0.061mg) to NVM format (8bit, 0x01 == 7.8mg). + * Function makes sure that NVM value is representation of two's + * complement signed number. + * + * @param val Value in internal offset format (16bit, 0x01 == 0.061mg). + * + * @return nvm NVM format representation of @p val (8bit, 0x01 == 3.9mg) + */ +static uint8_t bmi_emul_acc_off_to_nvm(int16_t off) +{ + uint32_t twos_comp_val; + uint8_t nvm = 0; + + twos_comp_val = bmi_emul_val_to_twos_comp(off); + + /* + * LSB in internal representation has value 0.061mg, while in NVM + * LSB is 3.9mg. Skip 0.06mg, 0.12mg, 0.24mg, 0.48mg, 0.97mg and + * 1.9mg bits. + */ + nvm |= (twos_comp_val >> 6) & 0x7f; + /* Set sign bit */ + nvm |= (twos_comp_val & BIT(31)) ? BIT(7) : 0x00; + + return nvm; +} + +/** + * @brief Convert gyroscope value from internal offset format + * (16bit, 0x01 == 0.0038°/s) to NVM format (10bit, 0x01 == 0.061°/s). + * Function makes sure that NVM value is representation of two's + * complement signed number. + * + * @param val Value in internal offset format (16bit, 0x01 == 0.0038°/s). + * + * @return nvm NVM format representation of @p val (10bit, 0x01 == 0.061°/s) + */ +static uint16_t bmi_emul_gyr_off_to_nvm(int16_t off) +{ + uint32_t twos_comp_val; + uint16_t nvm = 0; + + twos_comp_val = bmi_emul_val_to_twos_comp(off); + + /* + * LSB in internal representation has value 0.0038°/s, while in NVM + * LSB is 0.061°/s. Skip 0.0038°/s, 0.0076°/s, 0.015°/s, and + * 0.03°/s bits. + */ + nvm |= (twos_comp_val >> 4) & 0x1ff; + /* Set sign bit */ + nvm |= (twos_comp_val & BIT(31)) ? BIT(9) : 0x00; + + return nvm; +} + +/** Check description in emul_bmi.h */ +int16_t bmi_emul_get_off(struct i2c_emul *emul, enum bmi_emul_axis axis) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + switch (axis) { + case BMI_EMUL_ACC_X: + return data->off_acc_x; + case BMI_EMUL_ACC_Y: + return data->off_acc_y; + case BMI_EMUL_ACC_Z: + return data->off_acc_z; + case BMI_EMUL_GYR_X: + return data->off_gyr_x; + case BMI_EMUL_GYR_Y: + return data->off_gyr_y; + case BMI_EMUL_GYR_Z: + return data->off_gyr_z; + } + + return 0; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_off(struct i2c_emul *emul, enum bmi_emul_axis axis, + int16_t val) +{ + struct bmi_emul_data *data; + uint16_t gyr_off; + uint8_t gyr98_shift; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + switch (axis) { + case BMI_EMUL_ACC_X: + data->off_acc_x = val; + data->reg[data->type_data->acc_off_reg] = + bmi_emul_acc_off_to_nvm(data->off_acc_x); + break; + case BMI_EMUL_ACC_Y: + data->off_acc_y = val; + data->reg[data->type_data->acc_off_reg + 1] = + bmi_emul_acc_off_to_nvm(data->off_acc_y); + break; + case BMI_EMUL_ACC_Z: + data->off_acc_z = val; + data->reg[data->type_data->acc_off_reg + 2] = + bmi_emul_acc_off_to_nvm(data->off_acc_z); + break; + case BMI_EMUL_GYR_X: + data->off_gyr_x = val; + gyr_off = bmi_emul_gyr_off_to_nvm(data->off_gyr_x); + data->reg[data->type_data->gyr_off_reg] = gyr_off & 0xff; + gyr98_shift = 0; + data->reg[data->type_data->gyr98_off_reg] &= + ~(0x3 << gyr98_shift); + data->reg[data->type_data->gyr98_off_reg] |= + (gyr_off & 0x300) >> (8 - gyr98_shift); + break; + case BMI_EMUL_GYR_Y: + data->off_gyr_y = val; + gyr_off = bmi_emul_gyr_off_to_nvm(data->off_gyr_y); + data->reg[data->type_data->gyr_off_reg + 1] = gyr_off & 0xff; + gyr98_shift = 2; + data->reg[data->type_data->gyr98_off_reg] &= + ~(0x3 << gyr98_shift); + data->reg[data->type_data->gyr98_off_reg] |= + (gyr_off & 0x300) >> (8 - gyr98_shift); + break; + case BMI_EMUL_GYR_Z: + data->off_gyr_z = val; + gyr_off = bmi_emul_gyr_off_to_nvm(data->off_gyr_z); + data->reg[data->type_data->gyr_off_reg + 2] = gyr_off & 0xff; + gyr98_shift = 4; + data->reg[data->type_data->gyr98_off_reg] &= + ~(0x3 << gyr98_shift); + data->reg[data->type_data->gyr98_off_reg] |= + (gyr_off & 0x300) >> (8 - gyr98_shift); + break; + } +} + +/** Check description in emul_bmi.h */ +int32_t bmi_emul_get_value(struct i2c_emul *emul, enum bmi_emul_axis axis) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + switch (axis) { + case BMI_EMUL_ACC_X: + return data->acc_x; + case BMI_EMUL_ACC_Y: + return data->acc_y; + case BMI_EMUL_ACC_Z: + return data->acc_z; + case BMI_EMUL_GYR_X: + return data->gyr_x; + case BMI_EMUL_GYR_Y: + return data->gyr_y; + case BMI_EMUL_GYR_Z: + return data->gyr_z; + } + + return 0; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_value(struct i2c_emul *emul, enum bmi_emul_axis axis, + int32_t val) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + switch (axis) { + case BMI_EMUL_ACC_X: + data->acc_x = val; + break; + case BMI_EMUL_ACC_Y: + data->acc_y = val; + break; + case BMI_EMUL_ACC_Z: + data->acc_z = val; + break; + case BMI_EMUL_GYR_X: + data->gyr_x = val; + break; + case BMI_EMUL_GYR_Y: + data->gyr_y = val; + break; + case BMI_EMUL_GYR_Z: + data->gyr_z = val; + break; + } +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_err_on_ro_write(struct i2c_emul *emul, bool set) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + data->error_on_ro_write = set; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + data->error_on_rsvd_write = set; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_err_on_wo_read(struct i2c_emul *emul, bool set) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + data->error_on_wo_read = set; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_simulate_cmd_exec_time(struct i2c_emul *emul, bool set) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + data->simulate_command_exec_time = set; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_skipped_frames(struct i2c_emul *emul, uint8_t skip) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + data->fifo_skip = skip; +} + +/** + * @brief Convert current time to sensor time (39 us units) + * + * @return time in 39 us units + */ +static int64_t bmi_emul_get_sensortime(void) +{ + return k_uptime_ticks() * 1000000 / 39 / CONFIG_SYS_CLOCK_TICKS_PER_SEC; +} + +/** + * @brief Set registers at address @p reg with sensor time that elapsed since + * last reset of emulator + * + * @param emul Pointer to BMI emulator + * @param reg Pointer to 3 byte array, where current sensor time should be + * stored + */ +static void bmi_emul_set_sensortime_reg(struct i2c_emul *emul, uint8_t *reg) +{ + struct bmi_emul_data *data; + uint32_t twos_comp_val; + int64_t time; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + time = bmi_emul_get_sensortime(); + + twos_comp_val = bmi_emul_val_to_twos_comp(time - data->zero_time); + + *reg = twos_comp_val & 0xff; + *(reg + 1) = (twos_comp_val >> 8) & 0xff; + *(reg + 2) = (twos_comp_val >> 16) & 0xff; +} + +/** + * @brief Convert given sensor axis @p val from internal units to register + * units. It shifts value by @p shift bits to the right to account + * range set in emulator's registers. Result is saved at address @p reg + * + * @param emul Pointer to BMI emulator + * @param val Accelerometer or gyroscope value in internal units + * @param reg Pointer to 2 byte array, where sensor value should be stored + * @param shift How many bits should be shift to the right + */ +static void bmi_emul_set_data_reg(struct i2c_emul *emul, int32_t val, + uint8_t *reg, int shift) +{ + struct bmi_emul_data *data; + uint32_t twos_comp_val; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + twos_comp_val = bmi_emul_val_to_twos_comp(val); + + /* Shift unused bits because of selected range */ + twos_comp_val >>= shift; + + *reg = twos_comp_val & 0xff; + *(reg + 1) = (twos_comp_val >> 8) & 0xff; +} + +/** + * @brief Compute length of given FIFO @p frame. If frame is null then length + * of empty frame is returned. + * + * @param emul Pointer to BMI emulator + * @param frame Pointer to FIFO frame + * @param tag_time Indicate if sensor time should be included in empty frame + * @param header Indicate if header should be included in frame + * + * @return length of frame + */ +static uint8_t bmi_emul_get_frame_len(struct i2c_emul *emul, + struct bmi_emul_frame *frame, + bool tag_time, bool header) +{ + struct bmi_emul_data *data; + int len; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + /* Empty FIFO frame */ + if (frame == NULL) { + if (tag_time && header) { + /* Header of sensortime + sensortime + empty FIFO */ + return 5; + } + + /* Empty fifo */ + return 1; + } + + /* Config FIFO frame */ + if (frame->type & BMI_EMUL_FRAME_CONFIG) { + if (header) { + /* Header + byte of data */ + len = 2; + if (data->type_data->sensortime_follow_config_frame) { + /* Sensortime data */ + len += 3; + } + + return len; + } + + /* This frame doesn't exist in headerless mode */ + return 0; + } + + /* Sensor data FIFO frame */ + if (header) { + len = 1; + } else { + len = 0; + } + + if (frame->type & BMI_EMUL_FRAME_ACC) { + len += 6; + } + if (frame->type & BMI_EMUL_FRAME_MAG) { + len += 8; + } + if (frame->type & BMI_EMUL_FRAME_GYR) { + len += 6; + } + + return len; +} + +/** + * @brief Set given FIFO @p frame as current frame in fifo field of emulator + * data structure + * + * @param emul Pointer to BMI emulator + * @param frame Pointer to FIFO frame + * @param tag_time Indicate if sensor time should be included in empty frame + * @param header Indicate if header should be included in frame + * @param acc_shift How many bits should be right shifted from accelerometer + * data + * @param gyr_shift How many bits should be right shifted from gyroscope data + */ +static void bmi_emul_set_current_frame(struct i2c_emul *emul, + struct bmi_emul_frame *frame, + bool tag_time, bool header, + int acc_shift, int gyr_shift) +{ + struct bmi_emul_data *data; + int i = 0; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + data->fifo_frame_byte = 0; + data->fifo_frame_len = bmi_emul_get_frame_len(emul, frame, tag_time, + header); + /* Empty FIFO frame */ + if (frame == NULL) { + if (tag_time && header) { + /* Header */ + data->fifo[0] = BMI_EMUL_FIFO_HEAD_TIME; + bmi_emul_set_sensortime_reg(emul, &(data->fifo[1])); + i = 4; + } + + /* Empty header */ + data->fifo[i] = BMI_EMUL_FIFO_HEAD_EMPTY; + + return; + } + + /* Config FIFO frame */ + if (frame->type & BMI_EMUL_FRAME_CONFIG) { + /* Header */ + data->fifo[0] = BMI_EMUL_FIFO_HEAD_CONFIG; + data->fifo[1] = frame->config; + if (data->type_data->sensortime_follow_config_frame) { + bmi_emul_set_sensortime_reg(emul, &(data->fifo[2])); + } + + return; + } + + /* Sensor data FIFO frame */ + if (header) { + data->fifo[0] = BMI_EMUL_FIFO_HEAD_DATA; + data->fifo[0] |= frame->type & BMI_EMUL_FRAME_MAG ? + BMI_EMUL_FIFO_HEAD_DATA_MAG : 0; + data->fifo[0] |= frame->type & BMI_EMUL_FRAME_GYR ? + BMI_EMUL_FIFO_HEAD_DATA_GYR : 0; + data->fifo[0] |= frame->type & BMI_EMUL_FRAME_ACC ? + BMI_EMUL_FIFO_HEAD_DATA_ACC : 0; + data->fifo[0] |= frame->tag & BMI_EMUL_FIFO_HEAD_DATA_TAG_MASK; + i = 1; + } + + if (frame->type & BMI_EMUL_FRAME_MAG) { + bmi_emul_set_data_reg(emul, frame->mag_x, &(data->fifo[i]), 0); + i += 2; + bmi_emul_set_data_reg(emul, frame->mag_y, &(data->fifo[i]), 0); + i += 2; + bmi_emul_set_data_reg(emul, frame->mag_z, &(data->fifo[i]), 0); + i += 2; + bmi_emul_set_data_reg(emul, frame->rhall, &(data->fifo[i]), 0); + i += 2; + } + + if (frame->type & BMI_EMUL_FRAME_GYR) { + bmi_emul_set_data_reg(emul, frame->gyr_x, &(data->fifo[i]), + gyr_shift); + i += 2; + bmi_emul_set_data_reg(emul, frame->gyr_y, &(data->fifo[i]), + gyr_shift); + i += 2; + bmi_emul_set_data_reg(emul, frame->gyr_z, &(data->fifo[i]), + gyr_shift); + i += 2; + } + + if (frame->type & BMI_EMUL_FRAME_ACC) { + bmi_emul_set_data_reg(emul, frame->acc_x, &(data->fifo[i]), + acc_shift); + i += 2; + bmi_emul_set_data_reg(emul, frame->acc_y, &(data->fifo[i]), + acc_shift); + i += 2; + bmi_emul_set_data_reg(emul, frame->acc_z, &(data->fifo[i]), + acc_shift); + i += 2; + } +} + +/** + * @brief Update internal sensors offset values using values from emulated + * registers. + * + * @param emul Pointer to BMI emulator + */ +static void bmi_emul_updata_int_off(struct i2c_emul *emul) +{ + struct bmi_emul_data *data; + uint16_t gyr_nvm; + uint8_t gyr98; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + data->off_acc_x = bmi_emul_acc_nvm_to_off( + data->reg[data->type_data->acc_off_reg]); + data->off_acc_y = bmi_emul_acc_nvm_to_off( + data->reg[data->type_data->acc_off_reg + 1]); + data->off_acc_z = bmi_emul_acc_nvm_to_off( + data->reg[data->type_data->acc_off_reg + 2]); + + gyr98 = data->reg[data->type_data->gyr98_off_reg]; + + gyr_nvm = data->reg[data->type_data->gyr_off_reg]; + gyr_nvm |= (gyr98 & 0x3) << 8; + data->off_gyr_x = bmi_emul_gyr_nvm_to_off(gyr_nvm); + gyr_nvm = data->reg[data->type_data->gyr_off_reg + 1]; + gyr_nvm |= (gyr98 & 0xc) << 6; + data->off_gyr_y = bmi_emul_gyr_nvm_to_off(gyr_nvm); + gyr_nvm = data->reg[data->type_data->gyr_off_reg + 2]; + gyr_nvm |= (gyr98 & 0x30) << 4; + data->off_gyr_z = bmi_emul_gyr_nvm_to_off(gyr_nvm); +} + +/** + * @brief Restore registers backed in NVM to emulator's registers. Each model + * of BMI may have different set of NVM backed registers. + * + * @param emul Pointer to BMI emulator + */ +static void bmi_emul_restore_nvm(struct i2c_emul *emul) +{ + struct bmi_emul_data *data; + int i; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + ASSERT(data->type_data->nvm_len <= BMI_EMUL_MAX_NVM_REGS); + + /* Restore registers values */ + for (i = 0; i < data->type_data->nvm_len; i++) { + data->reg[data->type_data->nvm_reg[i]] = data->nvm[i]; + } + + bmi_emul_updata_int_off(emul); +} + +/** Check description in emul_bmi.h */ +void bmi_emul_flush_fifo(struct i2c_emul *emul, bool tag_time, bool header) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + data->fifo_skip = 0; + data->fifo_frame = NULL; + /* + * Gyroscope and accelerometer shift (last two arguments) + * are not important for NULL (empty) FIFO frame. + */ + bmi_emul_set_current_frame(emul, NULL, tag_time, header, 0, 0); +} + +/** Check description in emul_bmi.h */ +void bmi_emul_reset_common(struct i2c_emul *emul, bool tag_time, bool header) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + /* Restore registers backed in NVM */ + bmi_emul_restore_nvm(emul); + + /* Flush FIFO */ + bmi_emul_flush_fifo(emul, tag_time, header); + + /* Reset sensor timer */ + data->zero_time = bmi_emul_get_sensortime(); +} + +/** Check description in emul_bmi.h */ +void bmi_emul_set_cmd_end_time(struct i2c_emul *emul, int time) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + data->cmd_end_time = k_uptime_get_32() + time; +} + +/** Check description in emul_bmi.h */ +bool bmi_emul_is_cmd_end(struct i2c_emul *emul) +{ + struct bmi_emul_data *data; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + /* We are simulating command execution time and it doesn't expired */ + if (data->simulate_command_exec_time && + data->cmd_end_time > k_uptime_get_32()) { + return false; + } + + return true; +} + +/** + * @brief Handle I2C write message. Before any handling, custom function + * is called if provided. Next BMI model specific write function is + * called. It is checked if accessed register isn't RO and reserved bits + * are set to 0. Write set value of reg field of bmi emulator data + * ignoring reserved bits. If required internal sensor offset values are + * updated. Emulator may be configured to fail on write to specific + * register. + * + * @param emul Pointer to BMI emulator + * @param reg Register which is written + * @param byte Number of handled bytes in this write command + * @param val Value being written to @p reg + * + * @return 0 on success + * @return -EIO on error + */ +static int bmi_emul_handle_write(struct i2c_emul *emul, int reg, int byte, + uint8_t val) +{ + struct bmi_emul_data *data; + uint8_t rsvd_mask; + int ret; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + if (data->write_func) { + ret = data->write_func(emul, reg, byte, val, + data->write_func_data); + if (ret < 0) { + return -EIO; + } else if (ret == 0) { + return 0; + } + } + + ret = data->type_data->handle_write(data->reg, emul, ®, byte, val); + if (ret != 0) { + if (ret == BMI_EMUL_ACCESS_E) { + if (!data->error_on_ro_write) { + return 0; + } + LOG_ERR("Writing to reg 0x%x which is RO", reg); + } + + return -EIO; + } + + if (data->write_fail_reg == reg || + data->write_fail_reg == BMI_EMUL_FAIL_ALL_REG) { + return -EIO; + } + + rsvd_mask = data->type_data->rsvd_mask[reg]; + + if (data->error_on_rsvd_write && rsvd_mask & val) { + LOG_ERR("Writing 0x%x to reg 0x%x with rsvd bits mask 0x%x", + val, reg, rsvd_mask); + return -EIO; + } + + /* Ignore all reserved bits */ + val &= ~rsvd_mask; + val |= data->reg[reg] & rsvd_mask; + + data->reg[reg] = val; + + if ((reg >= data->type_data->acc_off_reg && + reg <= data->type_data->acc_off_reg + 2) || + (reg >= data->type_data->gyr_off_reg && + reg <= data->type_data->gyr_off_reg + 2) || + reg == data->type_data->gyr98_off_reg) { + /* + * Internal offset value should be updated to new value of + * offset registers + */ + bmi_emul_updata_int_off(emul); + } + + return 0; +} + +/** Check description in emul_bmi.h */ +void bmi_emul_state_to_reg(struct i2c_emul *emul, int acc_shift, + int gyr_shift, int acc_reg, int gyr_reg, + int sensortime_reg, bool acc_off_en, + bool gyr_off_en) +{ + struct bmi_emul_data *data; + int32_t val[3]; + int i; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + if (gyr_off_en) { + val[0] = data->gyr_x - data->off_gyr_x; + val[1] = data->gyr_y - data->off_gyr_y; + val[2] = data->gyr_z - data->off_gyr_z; + } else { + val[0] = data->gyr_x; + val[1] = data->gyr_y; + val[2] = data->gyr_z; + } + + for (i = 0; i < 3; i++) { + bmi_emul_set_data_reg(emul, val[i], + &(data->reg[gyr_reg + i * 2]), gyr_shift); + } + + if (acc_off_en) { + val[0] = data->acc_x - data->off_acc_x; + val[1] = data->acc_y - data->off_acc_y; + val[2] = data->acc_z - data->off_acc_z; + } else { + val[0] = data->acc_x; + val[1] = data->acc_y; + val[2] = data->acc_z; + } + + for (i = 0; i < 3; i++) { + bmi_emul_set_data_reg(emul, val[i], + &(data->reg[acc_reg + i * 2]), acc_shift); + } + + bmi_emul_set_sensortime_reg(emul, &(data->reg[sensortime_reg])); +} + +/** Check description in emul_bmi.h */ +void bmi_emul_append_frame(struct i2c_emul *emul, struct bmi_emul_frame *frame) +{ + struct bmi_emul_data *data; + struct bmi_emul_frame *tmp_frame; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + if (data->fifo_frame == NULL) { + data->fifo_frame = frame; + } else { + tmp_frame = data->fifo_frame; + while (tmp_frame->next != NULL) { + tmp_frame = tmp_frame->next; + } + tmp_frame->next = frame; + } +} + +/** Check description in emul_bmi.h */ +uint16_t bmi_emul_fifo_len(struct i2c_emul *emul, bool tag_time, bool header) +{ + struct bmi_emul_frame *frame; + struct bmi_emul_data *data; + uint16_t len = 0; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + if (data->fifo_skip != 0 && header) { + len += 2; + } + + frame = data->fifo_frame; + while (frame != NULL) { + len += bmi_emul_get_frame_len(emul, frame, tag_time, header); + frame = frame->next; + } + + len += bmi_emul_get_frame_len(emul, NULL, tag_time, header); + /* Do not count last empty frame byte */ + len--; + + return len; +} + +/** Check description in emul_bmi.h */ +uint8_t bmi_emul_get_fifo_data(struct i2c_emul *emul, int byte, + bool tag_time, bool header, int acc_shift, + int gyr_shift) +{ + struct bmi_emul_data *data; + int ret; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + if (byte == 0) { + /* Repeat uncompleated read of frame */ + bmi_emul_set_current_frame(emul, data->fifo_frame, tag_time, + header, acc_shift, gyr_shift); + + /* Return header for skip frame */ + if (data->fifo_skip != 0 && header) { + return BMI_EMUL_FIFO_HEAD_SKIP; + } + } + + if (data->fifo_skip != 0 && byte == 1 && header) { + /* Return number of skipped frames */ + ret = data->fifo_skip; + data->fifo_skip = 0; + + return ret; + } + + /* Get next valid frame */ + while (data->fifo_frame_byte >= data->fifo_frame_len) { + /* No data */ + if (data->fifo_frame == NULL) { + return 0; + } + data->fifo_frame = data->fifo_frame->next; + bmi_emul_set_current_frame(emul, data->fifo_frame, tag_time, + header, acc_shift, gyr_shift); + } + + return data->fifo[data->fifo_frame_byte++]; +} + +/** + * @brief Handle I2C read message. Before any handling, custom function + * is called if provided. Next BMI model specific read function is + * called. It is checked if accessed register isn't WO. Emulator may + * be configured to fail on given register read. + * + * @param emul Pointer to BMI emulator + * @param reg Register address to read + * @param byte Byte which is accessed during block read + * @param buf Pointer where result should be stored + * + * @return 0 on success + * @return -EIO on error + */ +static int bmi_emul_handle_read(struct i2c_emul *emul, int reg, int byte, + char *buf) +{ + struct bmi_emul_data *data; + uint16_t fifo_len; + int ret; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + + if (data->read_func) { + ret = data->read_func(emul, reg, byte, data->read_func_data); + if (ret < 0) { + return -EIO; + } else if (ret == 0) { + /* Immediately return value set by custom function */ + *buf = data->reg[reg]; + + return 0; + } + } + + ret = data->type_data->handle_read(data->reg, emul, ®, byte, buf); + if (ret == BMI_EMUL_ACCESS_E && data->error_on_wo_read) { + LOG_ERR("Reading reg 0x%x which is WO", reg); + } else if (ret != 0) { + return ret; + } + + if (data->read_fail_reg == reg || + data->read_fail_reg == BMI_EMUL_FAIL_ALL_REG) { + return -EIO; + } + + return 0; +} + +/** + * @biref Emulate an I2C transfer to a BMI accelerometer + * + * This handles simple reads and writes + * + * @param emul I2C emulation information + * @param msgs List of messages to process + * @param num_msgs Number of messages to process + * @param addr Address of the I2C target device + * + * @retval 0 If successful + * @retval -EIO General input / output error + */ +static int bmi_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs, + int num_msgs, int addr) +{ + const struct bmi_emul_cfg *cfg; + struct bmi_emul_data *data; + unsigned int len; + int ret, i; + bool read; + + data = CONTAINER_OF(emul, struct bmi_emul_data, emul); + cfg = data->cfg; + + if (cfg->addr != addr) { + LOG_ERR("Address mismatch, expected %02x, got %02x", cfg->addr, + addr); + return -EIO; + } + + i2c_dump_msgs("emul", msgs, num_msgs, addr); + + for (; num_msgs > 0; num_msgs--, msgs++) { + read = msgs->flags & I2C_MSG_READ; + + switch (data->msg_state) { + case BMI_EMUL_NONE_MSG: + data->msg_byte = 0; + break; + case BMI_EMUL_IN_WRITE: + if (read) { + data->msg_byte = 0; + } + break; + case BMI_EMUL_IN_READ: + if (!read) { + data->msg_byte = 0; + } + break; + } + data->msg_state = read ? BMI_EMUL_IN_READ : BMI_EMUL_IN_WRITE; + + if (msgs->flags & I2C_MSG_STOP) { + data->msg_state = BMI_EMUL_NONE_MSG; + } + + if (!read) { + /* Dispatch wrtie command */ + i = 0; + /* + * Save first byte of write command as currently + * accessed register. + */ + if (data->msg_byte == 0 && msgs->len) { + data->cur_reg = msgs->buf[0]; + i++; + data->msg_byte++; + } + + /* Handle rest of write command bytes */ + for (; i < msgs->len; i++) { + k_mutex_lock(&data->data_mtx, K_FOREVER); + ret = bmi_emul_handle_write(emul, data->cur_reg, + data->msg_byte, + msgs->buf[i]); + k_mutex_unlock(&data->data_mtx); + if (ret) { + return -EIO; + } + data->msg_byte++; + } + } else { + /* Dispatch read command */ + for (i = 0; i < msgs->len; i++) { + k_mutex_lock(&data->data_mtx, K_FOREVER); + ret = bmi_emul_handle_read(emul, data->cur_reg, + data->msg_byte, + &(msgs->buf[i])); + k_mutex_unlock(&data->data_mtx); + if (ret) { + return -EIO; + } + data->msg_byte++; + } + } + } + + return 0; +} + +/* Device instantiation */ + +static struct i2c_emul_api bmi_emul_api = { + .transfer = bmi_emul_transfer, +}; + +/** + * @brief Set up a new BMI emulator + * + * This should be called for each BMI device that needs to be + * emulated. It registers it with the I2C emulation controller. + * + * @param emul Emulation information + * @param parent Device to emulate + * + * @return 0 indicating success (always) + */ +static int bmi_emul_init(const struct emul *emul, + const struct device *parent) +{ + const struct bmi_emul_cfg *cfg = emul->cfg; + struct bmi_emul_data *data = cfg->data; + int ret; + + data->emul.api = &bmi_emul_api; + data->emul.addr = cfg->addr; + data->i2c = parent; + data->cfg = cfg; + k_mutex_init(&data->data_mtx); + + switch (data->type) { + case BMI_EMUL_160: + data->type_data = get_bmi160_emul_type_data(); + break; + } + + ret = i2c_emul_register(parent, emul->dev_label, &data->emul); + + data->type_data->reset(data->reg, &data->emul); + + return ret; +} + +#define BMI_EMUL(n) \ + static struct bmi_emul_data bmi_emul_data_##n = { \ + .error_on_ro_write = DT_INST_PROP(n, error_on_ro_write),\ + .error_on_wo_read = DT_INST_PROP(n, error_on_wo_read), \ + .error_on_rsvd_write = DT_INST_PROP(n, \ + error_on_reserved_bit_write), \ + .simulate_command_exec_time = DT_INST_PROP(n, \ + simulate_command_exec_time), \ + .type = DT_ENUM_TOKEN(DT_DRV_INST(n), device_model), \ + .msg_state = BMI_EMUL_NONE_MSG, \ + .cur_reg = 0, \ + .write_func = NULL, \ + .read_func = NULL, \ + .write_fail_reg = BMI_EMUL_NO_FAIL_REG, \ + .read_fail_reg = BMI_EMUL_NO_FAIL_REG, \ + }; \ + \ + static const struct bmi_emul_cfg bmi_emul_cfg_##n = { \ + .i2c_label = DT_INST_BUS_LABEL(n), \ + .data = &bmi_emul_data_##n, \ + .addr = DT_INST_REG_ADDR(n), \ + }; \ + EMUL_DEFINE(bmi_emul_init, DT_DRV_INST(n), &bmi_emul_cfg_##n) + +DT_INST_FOREACH_STATUS_OKAY(BMI_EMUL) + +#define BMI_EMUL_CASE(n) \ + case DT_INST_DEP_ORD(n): return &bmi_emul_data_##n.emul; + +/** Check description in emul_bmi.h */ +struct i2c_emul *bmi_emul_get(int ord) +{ + switch (ord) { + DT_INST_FOREACH_STATUS_OKAY(BMI_EMUL_CASE) + + default: + return NULL; + } +} diff --git a/zephyr/emul/emul_bmi160.c b/zephyr/emul/emul_bmi160.c new file mode 100644 index 0000000000..8f6572604f --- /dev/null +++ b/zephyr/emul/emul_bmi160.c @@ -0,0 +1,747 @@ +/* Copyright 2021 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +#define DT_DRV_COMPAT zephyr_bmi + +#define LOG_LEVEL CONFIG_I2C_LOG_LEVEL +#include <logging/log.h> +LOG_MODULE_REGISTER(emul_bmi160); + +#include <device.h> +#include <emul.h> +#include <drivers/i2c.h> +#include <drivers/i2c_emul.h> + +#include "emul/emul_bmi.h" + +#include "driver/accelgyro_bmi160.h" +#include "driver/accelgyro_bmi_common.h" + +/** Mask reserved bits in each register of BMI160 */ +static const uint8_t bmi_emul_160_rsvd_mask[] = { + [BMI160_CHIP_ID] = 0x00, + [0x01] = 0xff, /* Reserved */ + [BMI160_ERR_REG] = 0x00, + [BMI160_PMU_STATUS] = 0xc0, + [BMI160_MAG_X_L_G] = 0x00, + [BMI160_MAG_X_H_G] = 0x00, + [BMI160_MAG_Y_L_G] = 0x00, + [BMI160_MAG_Y_H_G] = 0x00, + [BMI160_MAG_Z_L_G] = 0x00, + [BMI160_MAG_Z_H_G] = 0x00, + [BMI160_RHALL_L_G] = 0x00, + [BMI160_RHALL_H_G] = 0x00, + [BMI160_GYR_X_L_G] = 0x00, + [BMI160_GYR_X_H_G] = 0x00, + [BMI160_GYR_Y_L_G] = 0x00, + [BMI160_GYR_Y_H_G] = 0x00, + [BMI160_GYR_Z_L_G] = 0x00, + [BMI160_GYR_Z_H_G] = 0x00, + [BMI160_ACC_X_L_G] = 0x00, + [BMI160_ACC_X_H_G] = 0x00, + [BMI160_ACC_Y_L_G] = 0x00, + [BMI160_ACC_Y_H_G] = 0x00, + [BMI160_ACC_Z_L_G] = 0x00, + [BMI160_ACC_Z_H_G] = 0x00, + [BMI160_SENSORTIME_0] = 0x00, + [BMI160_SENSORTIME_1] = 0x00, + [BMI160_SENSORTIME_2] = 0x00, + [BMI160_STATUS] = 0x01, + [BMI160_INT_STATUS_0] = 0x00, + [BMI160_INT_STATUS_1] = 0x03, + [BMI160_INT_STATUS_2] = 0x00, + [BMI160_INT_STATUS_3] = 0x00, + [BMI160_TEMPERATURE_0] = 0x00, + [BMI160_TEMPERATURE_1] = 0x00, + [BMI160_FIFO_LENGTH_0] = 0x00, + [BMI160_FIFO_LENGTH_1] = 0xf8, + [BMI160_FIFO_DATA] = 0x00, + [0x25 ... 0x3f] = 0xff, /* Reserved */ + [BMI160_ACC_CONF] = 0x00, + [BMI160_ACC_RANGE] = 0xf0, + [BMI160_GYR_CONF] = 0xc0, + [BMI160_GYR_RANGE] = 0xf8, + [BMI160_MAG_CONF] = 0xf0, + [BMI160_FIFO_DOWNS] = 0x00, + [BMI160_FIFO_CONFIG_0] = 0x00, + [BMI160_FIFO_CONFIG_1] = 0x01, + [0x48 ... 0x4a] = 0xff, /* Reserved */ + [BMI160_MAG_IF_0] = 0x01, + [BMI160_MAG_IF_1] = 0x40, + [BMI160_MAG_IF_2] = 0x00, + [BMI160_MAG_IF_3] = 0x00, + [BMI160_MAG_IF_4] = 0x00, + [BMI160_INT_EN_0] = 0x08, + [BMI160_INT_EN_1] = 0x80, + [BMI160_INT_EN_2] = 0xf0, + [BMI160_INT_OUT_CTRL] = 0x00, + [BMI160_INT_LATCH] = 0xc0, + [BMI160_INT_MAP_0] = 0x00, + [BMI160_INT_MAP_1] = 0x00, + [BMI160_INT_MAP_2] = 0x00, + [BMI160_INT_DATA_0] = 0x77, + [BMI160_INT_DATA_1] = 0x7f, + [BMI160_INT_LOW_HIGH_0] = 0x00, + [BMI160_INT_LOW_HIGH_1] = 0x00, + [BMI160_INT_LOW_HIGH_2] = 0x3c, + [BMI160_INT_LOW_HIGH_3] = 0x00, + [BMI160_INT_LOW_HIGH_4] = 0x00, + [BMI160_INT_MOTION_0] = 0x00, + [BMI160_INT_MOTION_1] = 0x00, + [BMI160_INT_MOTION_2] = 0x00, + [BMI160_INT_MOTION_3] = 0xc0, + [BMI160_INT_TAP_0] = 0x38, + [BMI160_INT_TAP_1] = 0xe0, + [BMI160_INT_ORIENT_0] = 0x00, + [BMI160_INT_ORIENT_1] = 0x00, + [BMI160_INT_FLAT_0] = 0xc0, + [BMI160_INT_FLAT_1] = 0xc8, + [BMI160_FOC_CONF] = 0x80, + [BMI160_CONF] = 0xfd, + [BMI160_IF_CONF] = 0xce, + [BMI160_PMU_TRIGGER] = 0x80, + [BMI160_SELF_TEST] = 0xe0, + [0x6e] = 0xff, /* Reserved */ + [0x6f] = 0xff, /* Reserved */ + [BMI160_NV_CONF] = 0xf0, + [BMI160_OFFSET_ACC70] = 0x00, + [BMI160_OFFSET_ACC70 + 1] = 0x00, + [BMI160_OFFSET_ACC70 + 2] = 0x00, + [BMI160_OFFSET_GYR70] = 0x00, + [BMI160_OFFSET_GYR70 + 1] = 0x00, + [BMI160_OFFSET_GYR70 + 2] = 0x00, + [BMI160_OFFSET_EN_GYR98] = 0x00, + [BMI160_STEP_CNT_0] = 0x00, + [BMI160_STEP_CNT_1] = 0x00, + [BMI160_STEP_CONF_0] = 0x00, + [BMI160_STEP_CONF_1] = 0xf0, + [0x7c] = 0xff, /* Reserved */ + [0x7d] = 0xff, /* Reserved */ + [BMI160_CMD_REG] = 0x00, +}; + +/** + * @brief Convert range in format of ACC_RANGE register to number of bits + * that should be shifted right to obtain 16 bit reported accelerometer + * value from internal 32 bit value + * + * @param range Value of ACC_RANGE register + * + * @return shift Number of LSB that should be ignored from internal + * accelerometer value + */ +static int bmi160_emul_acc_range_to_shift(uint8_t range) +{ + switch (range & 0xf) { + case BMI160_GSEL_2G: + return 0; + case BMI160_GSEL_4G: + return 1; + case BMI160_GSEL_8G: + return 2; + case BMI160_GSEL_16G: + return 3; + default: + return 0; + } +} + +/** + * @brief Convert range in format of GYR_RANGE register to number of bits + * that should be shifted right to obtain 16 bit reported gyroscope + * value from internal 32 bit value + * + * @param range Value of GYR_RANGE register + * + * @return shift Number of LSB that should be ignored from internal + * gyroscope value + */ +static int bmi160_emul_gyr_range_to_shift(uint8_t range) +{ + switch (range & 0x7) { + case BMI160_DPS_SEL_2000: + return 4; + case BMI160_DPS_SEL_1000: + return 3; + case BMI160_DPS_SEL_500: + return 2; + case BMI160_DPS_SEL_250: + return 1; + case BMI160_DPS_SEL_125: + return 0; + default: + return 0; + } +} + +/** + * @brief Reset registers to default values and restore registers backed by NVM + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + */ +static void bmi160_emul_reset(uint8_t *regs, struct i2c_emul *emul) +{ + bool tag_time; + bool header; + + regs[BMI160_CHIP_ID] = 0xd1; + regs[BMI160_ERR_REG] = 0x00; + regs[BMI160_PMU_STATUS] = 0x00; + regs[BMI160_MAG_X_L_G] = 0x00; + regs[BMI160_MAG_X_H_G] = 0x00; + regs[BMI160_MAG_Y_L_G] = 0x00; + regs[BMI160_MAG_Y_H_G] = 0x00; + regs[BMI160_MAG_Z_L_G] = 0x00; + regs[BMI160_MAG_Z_H_G] = 0x00; + regs[BMI160_RHALL_L_G] = 0x00; + regs[BMI160_RHALL_H_G] = 0x00; + regs[BMI160_GYR_X_L_G] = 0x00; + regs[BMI160_GYR_X_H_G] = 0x00; + regs[BMI160_GYR_Y_L_G] = 0x00; + regs[BMI160_GYR_Y_H_G] = 0x00; + regs[BMI160_GYR_Z_L_G] = 0x00; + regs[BMI160_GYR_Z_H_G] = 0x00; + regs[BMI160_ACC_X_L_G] = 0x00; + regs[BMI160_ACC_X_H_G] = 0x00; + regs[BMI160_ACC_Y_L_G] = 0x00; + regs[BMI160_ACC_Y_H_G] = 0x00; + regs[BMI160_ACC_Z_L_G] = 0x00; + regs[BMI160_ACC_Z_H_G] = 0x00; + regs[BMI160_SENSORTIME_0] = 0x00; + regs[BMI160_SENSORTIME_1] = 0x00; + regs[BMI160_SENSORTIME_2] = 0x00; + regs[BMI160_STATUS] = 0x01; + regs[BMI160_INT_STATUS_0] = 0x00; + regs[BMI160_INT_STATUS_1] = 0x00; + regs[BMI160_INT_STATUS_2] = 0x00; + regs[BMI160_INT_STATUS_3] = 0x00; + regs[BMI160_TEMPERATURE_0] = 0x00; + regs[BMI160_TEMPERATURE_1] = 0x00; + regs[BMI160_FIFO_LENGTH_0] = 0x00; + regs[BMI160_FIFO_LENGTH_1] = 0x00; + regs[BMI160_FIFO_DATA] = 0x00; + regs[BMI160_ACC_CONF] = 0x28; + regs[BMI160_ACC_RANGE] = 0x03; + regs[BMI160_GYR_CONF] = 0x28; + regs[BMI160_GYR_RANGE] = 0x00; + regs[BMI160_MAG_CONF] = 0x0b; + regs[BMI160_FIFO_DOWNS] = 0x88; + regs[BMI160_FIFO_CONFIG_0] = 0x80; + regs[BMI160_FIFO_CONFIG_1] = 0x10; + regs[BMI160_MAG_IF_0] = 0x20; + regs[BMI160_MAG_IF_1] = 0x80; + regs[BMI160_MAG_IF_2] = 0x42; + regs[BMI160_MAG_IF_3] = 0x4c; + regs[BMI160_MAG_IF_4] = 0x00; + regs[BMI160_INT_EN_0] = 0x00; + regs[BMI160_INT_EN_1] = 0x00; + regs[BMI160_INT_EN_2] = 0x00; + regs[BMI160_INT_OUT_CTRL] = 0x00; + regs[BMI160_INT_LATCH] = 0x00; + regs[BMI160_INT_MAP_0] = 0x00; + regs[BMI160_INT_MAP_1] = 0x00; + regs[BMI160_INT_MAP_2] = 0x00; + regs[BMI160_INT_DATA_0] = 0x00; + regs[BMI160_INT_DATA_1] = 0x00; + regs[BMI160_INT_LOW_HIGH_0] = 0x07; + regs[BMI160_INT_LOW_HIGH_1] = 0x30; + regs[BMI160_INT_LOW_HIGH_2] = 0x81; + regs[BMI160_INT_LOW_HIGH_3] = 0xdb; + regs[BMI160_INT_LOW_HIGH_4] = 0xc0; + regs[BMI160_INT_MOTION_0] = 0x00; + regs[BMI160_INT_MOTION_1] = 0x14; + regs[BMI160_INT_MOTION_2] = 0x14; + regs[BMI160_INT_MOTION_3] = 0x24; + regs[BMI160_INT_TAP_0] = 0x04; + regs[BMI160_INT_TAP_1] = 0xda; + regs[BMI160_INT_ORIENT_0] = 0x18; + regs[BMI160_INT_ORIENT_1] = 0x48; + regs[BMI160_INT_FLAT_0] = 0x08; + regs[BMI160_INT_FLAT_1] = 0x11; + regs[BMI160_FOC_CONF] = 0x00; + regs[BMI160_CONF] = 0x00; + regs[BMI160_IF_CONF] = 0x00; + regs[BMI160_PMU_TRIGGER] = 0x00; + regs[BMI160_SELF_TEST] = 0x00; + regs[BMI160_STEP_CNT_0] = 0x00; + regs[BMI160_STEP_CNT_1] = 0x00; + regs[BMI160_STEP_CONF_0] = 0x00; + regs[BMI160_STEP_CONF_1] = 0x15; + regs[BMI160_CMD_REG] = 0x03; + + /* Call generic reset */ + tag_time = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_TAG_TIME_EN; + header = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_HEADER_EN; + bmi_emul_reset_common(emul, tag_time, header); +} + +/** + * @brief Clear all interrupt registers + * + * @param regs Pointer to array of emulator's registers + */ +static void bmi160_emul_clear_int(uint8_t *regs) +{ + regs[BMI160_INT_STATUS_0] = 0x00; + regs[BMI160_INT_STATUS_1] = 0x00; + regs[BMI160_INT_STATUS_2] = 0x00; + regs[BMI160_INT_STATUS_3] = 0x00; +} + +/** + * @brief Get offset value for given gyroscope value. If gyroscope value is + * above maximum (belowe minimum), then minimum -31,25°/s + * (maximum 31,25°/s) offset value is returned. + * + * @param gyr Gyroscope value + */ +static int16_t bmi160_emul_get_gyr_target_off(int32_t gyr) +{ + if (gyr > (int32_t)BMI_EMUL_125_DEG_S / 4) { + return -((int32_t)BMI_EMUL_125_DEG_S / 4); + } + + if (gyr < -((int32_t)BMI_EMUL_125_DEG_S / 4)) { + return BMI_EMUL_125_DEG_S / 4; + } + + return -gyr; +} + +/** + * @brief Get offset value for given accelerometer value. If accelerometer + * value - target is above maximum (belowe minimum), then minimum -0.5g + * (maximum 0.5g) offset value is returned. + * + * @param acc Accelerometer value + * @param target Target value in FOC configuration register format + */ +static int16_t bmi160_emul_get_acc_target_off(int32_t acc, uint8_t target) +{ + switch (target) { + case BMI160_FOC_ACC_PLUS_1G: + acc -= BMI_EMUL_1G; + break; + case BMI160_FOC_ACC_MINUS_1G: + acc += BMI_EMUL_1G; + break; + } + + if (acc > (int32_t)BMI_EMUL_1G / 2) { + return -((int32_t)BMI_EMUL_1G / 2); + } + + if (acc < -((int32_t)BMI_EMUL_1G / 2)) { + return BMI_EMUL_1G / 2; + } + + return -acc; +} + +/** + * @brief Handle fast offset compensation. Check FOC configuration register + * and sets gyroscope and/or accelerometer offset using current emulator + * state. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + */ +static void bmi160_emul_handle_off_comp(uint8_t *regs, struct i2c_emul *emul) +{ + uint8_t target; + int16_t off; + int32_t val; + + if (regs[BMI160_FOC_CONF] & BMI160_FOC_GYRO_EN) { + val = bmi_emul_get_value(emul, BMI_EMUL_GYR_X); + off = bmi160_emul_get_gyr_target_off(val); + bmi_emul_set_off(emul, BMI_EMUL_GYR_X, off); + val = bmi_emul_get_value(emul, BMI_EMUL_GYR_Y); + off = bmi160_emul_get_gyr_target_off(val); + bmi_emul_set_off(emul, BMI_EMUL_GYR_Y, off); + val = bmi_emul_get_value(emul, BMI_EMUL_GYR_Z); + off = bmi160_emul_get_gyr_target_off(val); + bmi_emul_set_off(emul, BMI_EMUL_GYR_Z, off); + } + + target = (regs[BMI160_FOC_CONF] >> BMI160_FOC_ACC_X_OFFSET) & 0x3; + if (target) { + val = bmi_emul_get_value(emul, BMI_EMUL_ACC_X); + off = bmi160_emul_get_acc_target_off(val, target); + bmi_emul_set_off(emul, BMI_EMUL_ACC_X, off); + } + + target = (regs[BMI160_FOC_CONF] >> BMI160_FOC_ACC_Y_OFFSET) & 0x3; + if (target) { + val = bmi_emul_get_value(emul, BMI_EMUL_ACC_Y); + off = bmi160_emul_get_acc_target_off(val, target); + bmi_emul_set_off(emul, BMI_EMUL_ACC_Y, off); + } + + target = (regs[BMI160_FOC_CONF] >> BMI160_FOC_ACC_Z_OFFSET) & 0x3; + if (target) { + val = bmi_emul_get_value(emul, BMI_EMUL_ACC_Z); + off = bmi160_emul_get_acc_target_off(val, target); + bmi_emul_set_off(emul, BMI_EMUL_ACC_Z, off); + } +} + +/** + * @brief Execute first part of command. Emulate state of device which is + * during handling command (status bits etc). This function save time + * on which command should end. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + * @param cmd Command that is starting + * + * @return 0 on success + * @return -EIO on failure + */ +static int bmi160_emul_start_cmd(uint8_t *regs, struct i2c_emul *emul, int cmd) +{ + int time; + int ret; + + switch (cmd) { + case BMI160_CMD_SOFT_RESET: + time = 1; + break; + case BMI160_CMD_START_FOC: + if ((regs[BMI160_FOC_CONF] & BMI160_FOC_GYRO_EN) && + ((regs[BMI160_PMU_STATUS] & + (0x3 << BMI160_PMU_GYR_OFFSET)) != + BMI160_PMU_NORMAL << BMI160_PMU_GYR_OFFSET)) { + LOG_ERR("Starting gyroscope FOC in low power mode"); + return -EIO; + } + + if ((regs[BMI160_FOC_CONF] & ~BMI160_FOC_GYRO_EN) && + ((regs[BMI160_PMU_STATUS] & + (0x3 << BMI160_PMU_ACC_OFFSET)) != + BMI160_PMU_NORMAL << BMI160_PMU_ACC_OFFSET)) { + LOG_ERR("Starting accelerometer FOC in low power mode"); + return -EIO; + } + + regs[BMI160_STATUS] &= ~BMI160_FOC_RDY; + time = 250; + break; + case BMI160_CMD_ACC_MODE_SUSP: + case BMI160_CMD_GYR_MODE_SUSP: + case BMI160_CMD_MAG_MODE_SUSP: + time = 0; + break; + /* Real hardware probably switch faster if not in suspend mode */ + case BMI160_CMD_ACC_MODE_NORMAL: + case BMI160_CMD_ACC_MODE_LOWPOWER: + time = 4; + break; + case BMI160_CMD_GYR_MODE_NORMAL: + case BMI160_CMD_GYR_MODE_FAST_STARTUP: + time = 80; + break; + case BMI160_CMD_MAG_MODE_NORMAL: + case BMI160_CMD_MAG_MODE_LOWPOWER: + time = 1; + break; + case BMI160_CMD_FIFO_FLUSH: + time = 0; + break; + case BMI160_CMD_INT_RESET: + time = 0; + break; + default: + LOG_ERR("Unknown command 0x%x", cmd); + return -EIO; + } + + regs[BMI160_CMD_REG] = cmd; + bmi_emul_set_cmd_end_time(emul, time); + + return 0; +} + +/** + * @brief Emulate end of ongoing command. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + */ +static void bmi160_emul_end_cmd(uint8_t *regs, struct i2c_emul *emul) +{ + uint8_t pmu_status; + bool tag_time; + bool header; + int cmd; + + pmu_status = regs[BMI160_PMU_STATUS]; + cmd = regs[BMI160_CMD_REG]; + regs[BMI160_CMD_REG] = BMI160_CMD_NOOP; + tag_time = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_TAG_TIME_EN; + header = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_HEADER_EN; + + switch (cmd) { + case BMI160_CMD_SOFT_RESET: + bmi160_emul_reset(regs, emul); + break; + case BMI160_CMD_START_FOC: + bmi160_emul_handle_off_comp(regs, emul); + regs[BMI160_STATUS] |= BMI160_FOC_RDY; + break; + case BMI160_CMD_ACC_MODE_SUSP: + pmu_status &= ~(0x3 << BMI160_PMU_ACC_OFFSET); + pmu_status |= BMI160_PMU_SUSPEND << BMI160_PMU_ACC_OFFSET; + break; + case BMI160_CMD_ACC_MODE_NORMAL: + pmu_status &= ~(0x3 << BMI160_PMU_ACC_OFFSET); + pmu_status |= BMI160_PMU_NORMAL << BMI160_PMU_ACC_OFFSET; + break; + case BMI160_CMD_ACC_MODE_LOWPOWER: + pmu_status &= ~(0x3 << BMI160_PMU_ACC_OFFSET); + pmu_status |= BMI160_PMU_LOW_POWER << BMI160_PMU_ACC_OFFSET; + break; + case BMI160_CMD_GYR_MODE_SUSP: + pmu_status &= ~(0x3 << BMI160_PMU_GYR_OFFSET); + pmu_status |= BMI160_PMU_SUSPEND << BMI160_PMU_GYR_OFFSET; + break; + case BMI160_CMD_GYR_MODE_NORMAL: + pmu_status &= ~(0x3 << BMI160_PMU_GYR_OFFSET); + pmu_status |= BMI160_PMU_NORMAL << BMI160_PMU_GYR_OFFSET; + break; + case BMI160_CMD_GYR_MODE_FAST_STARTUP: + pmu_status &= ~(0x3 << BMI160_PMU_GYR_OFFSET); + pmu_status |= BMI160_PMU_FAST_STARTUP << BMI160_PMU_GYR_OFFSET; + break; + case BMI160_CMD_MAG_MODE_SUSP: + pmu_status &= ~(0x3 << BMI160_PMU_MAG_OFFSET); + pmu_status |= BMI160_PMU_SUSPEND << BMI160_PMU_MAG_OFFSET; + break; + case BMI160_CMD_MAG_MODE_NORMAL: + pmu_status &= ~(0x3 << BMI160_PMU_MAG_OFFSET); + pmu_status |= BMI160_PMU_NORMAL << BMI160_PMU_MAG_OFFSET; + break; + case BMI160_CMD_MAG_MODE_LOWPOWER: + pmu_status &= ~(0x3 << BMI160_PMU_MAG_OFFSET); + pmu_status |= BMI160_PMU_LOW_POWER << BMI160_PMU_MAG_OFFSET; + break; + case BMI160_CMD_FIFO_FLUSH: + bmi_emul_flush_fifo(emul, tag_time, header); + break; + case BMI160_CMD_INT_RESET: + bmi160_emul_clear_int(regs); + break; + } + + /* Clear FIFO on sensor on/off in headerless mode */ + if (pmu_status != regs[BMI160_PMU_STATUS] && !header) { + bmi_emul_flush_fifo(emul, tag_time, header); + } + + regs[BMI160_PMU_STATUS] = pmu_status; +} + +/** + * @brief BMI160 specific write function. It doesn't handle block writes. + * Check if read only register is not accessed. Before writing value, + * ongoing command is finished if possible. Write to CMD register is + * handled by BMI160 specific function. On changing of FIFO + * header/headerless mode, FIFO is flushed. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + * @param reg Pointer to accessed reg + * @param byte Number of handled bytes in this write command + * @param val Value that is being written + * + * @return 0 on success + * @return BMI_EMUL_ACCESS_E on RO register access + * @return -EIO on error + */ +static int bmi160_emul_handle_write(uint8_t *regs, struct i2c_emul *emul, + int *reg, int byte, uint8_t val) +{ + bool tag_time; + bool header; + int ret; + + if (byte > 1) { + LOG_ERR("Block writes are not allowed"); + return -EIO; + } + + if (*reg <= BMI160_FIFO_DATA || + (*reg >= BMI160_STEP_CNT_0 && *reg <= BMI160_STEP_CNT_1)) { + return BMI_EMUL_ACCESS_E; + } + + /* Stop on going command if required */ + if (regs[BMI160_CMD_REG] != BMI160_CMD_NOOP && + bmi_emul_is_cmd_end(emul)) { + bmi160_emul_end_cmd(regs, emul); + } + + switch (*reg) { + case BMI160_CMD_REG: + if (regs[BMI160_CMD_REG] != BMI160_CMD_NOOP) { + LOG_ERR("Issued command before previous end"); + return -EIO; + } + + return bmi160_emul_start_cmd(regs, emul, val); + case BMI160_FIFO_CONFIG_1: + tag_time = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_TAG_TIME_EN; + header = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_HEADER_EN; + /* + * Clear FIFO on transition between headerless and + * header mode + */ + if (!!(val & BMI160_FIFO_HEADER_EN) != header) { + bmi_emul_flush_fifo(emul, tag_time, header); + } + break; + } + + return 0; +} + +/** + * @brief BMI160 specific read function. It handle block reads but only if + * device is not suspended. FIFO data register is trap register, so + * after reaching it, register address is not increased on block reads. + * Before reading value, ongoing command is finished if possible. + * Read of sensor data traps current emulator state in registers. + * Read of FIFO length and FIFO data triggers default BMI functions. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + * @param reg Pointer to accessed reg + * @param byte Byte which is accessed during block read + * @param buf Pointer where read byte should be stored + * + * @return 0 on success + * @return BMI_EMUL_ACCESS_E on WO register access + * @return -EIO on other error + */ +static int bmi160_emul_handle_read(uint8_t *regs, struct i2c_emul *emul, + int *reg, int byte, char *buf) +{ + uint16_t fifo_len; + bool acc_off_en; + bool gyr_off_en; + bool tag_time; + bool header; + int gyr_shift; + int acc_shift; + int ret; + + /* + * If register is FIFO data, then read data from FIFO. + * Else block read access subsequent registers. + */ + if (*reg <= BMI160_FIFO_DATA && *reg + byte >= BMI160_FIFO_DATA) { + byte -= *reg - BMI160_FIFO_DATA; + *reg = BMI160_FIFO_DATA; + } else { + *reg += byte; + } + + /* Stop on going command if required */ + if (regs[BMI160_CMD_REG] != BMI160_CMD_NOOP && + bmi_emul_is_cmd_end(emul)) { + bmi160_emul_end_cmd(regs, emul); + } + + /* Burst reads are not supported if all sensors are in suspend mode */ + if ((regs[BMI160_PMU_STATUS] & 0x3f) == 0 && byte > 0) { + LOG_ERR("Block reads are not supported in suspend mode"); + return -EIO; + } + + tag_time = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_TAG_TIME_EN; + header = regs[BMI160_FIFO_CONFIG_1] & BMI160_FIFO_HEADER_EN; + acc_off_en = regs[BMI160_OFFSET_EN_GYR98] & BMI160_OFFSET_ACC_EN; + gyr_off_en = regs[BMI160_OFFSET_EN_GYR98] & BMI160_OFFSET_GYRO_EN; + gyr_shift = bmi160_emul_gyr_range_to_shift(regs[BMI160_GYR_RANGE]); + acc_shift = bmi160_emul_acc_range_to_shift(regs[BMI160_ACC_RANGE]); + + switch (*reg) { + case BMI160_GYR_X_L_G: + case BMI160_GYR_X_H_G: + case BMI160_GYR_Y_L_G: + case BMI160_GYR_Y_H_G: + case BMI160_GYR_Z_L_G: + case BMI160_GYR_Z_H_G: + case BMI160_ACC_X_L_G: + case BMI160_ACC_X_H_G: + case BMI160_ACC_Y_L_G: + case BMI160_ACC_Y_H_G: + case BMI160_ACC_Z_L_G: + case BMI160_ACC_Z_H_G: + case BMI160_SENSORTIME_0: + case BMI160_SENSORTIME_1: + case BMI160_SENSORTIME_2: + /* + * Snapshot of current emulator state is created on data read + * and shouldn't be changed until next I2C operation + */ + if (byte == 0) { + bmi_emul_state_to_reg(emul, acc_shift, gyr_shift, + BMI160_ACC_X_L_G, + BMI160_GYR_X_L_G, + BMI160_SENSORTIME_0, + acc_off_en, gyr_off_en); + } + break; + case BMI160_FIFO_LENGTH_0: + case BMI160_FIFO_LENGTH_1: + if (byte == 0) { + fifo_len = bmi_emul_fifo_len(emul, tag_time, header); + regs[BMI160_FIFO_LENGTH_0] = fifo_len & 0xff; + regs[BMI160_FIFO_LENGTH_1] = (fifo_len >> 8) & 0x7; + } + break; + case BMI160_FIFO_DATA: + regs[*reg] = bmi_emul_get_fifo_data(emul, byte, tag_time, + header, acc_shift, + gyr_shift); + break; + } + + *buf = regs[*reg]; + + return 0; +} + +/** Registers backed in NVM by BMI160 */ +const int bmi160_nvm_reg[] = {BMI160_NV_CONF, + BMI160_OFFSET_ACC70, + BMI160_OFFSET_ACC70 + 1, + BMI160_OFFSET_ACC70 + 2, + BMI160_OFFSET_GYR70, + BMI160_OFFSET_GYR70 + 1, + BMI160_OFFSET_GYR70 + 2, + BMI160_OFFSET_EN_GYR98}; + +/** Confguration of BMI160 */ +struct bmi_emul_type_data bmi160_emul = { + .sensortime_follow_config_frame = false, + .handle_write = bmi160_emul_handle_write, + .handle_read = bmi160_emul_handle_read, + .reset = bmi160_emul_reset, + .rsvd_mask = bmi_emul_160_rsvd_mask, + .nvm_reg = bmi160_nvm_reg, + .nvm_len = ARRAY_SIZE(bmi160_nvm_reg), + .gyr_off_reg = BMI160_OFFSET_GYR70, + .acc_off_reg = BMI160_OFFSET_ACC70, + .gyr98_off_reg = BMI160_OFFSET_EN_GYR98, +}; + +/** Check description in emul_bmi.h */ +const struct bmi_emul_type_data *get_bmi160_emul_type_data(void) +{ + return &bmi160_emul; +} diff --git a/zephyr/include/emul/emul_bmi.h b/zephyr/include/emul/emul_bmi.h new file mode 100644 index 0000000000..81d889a301 --- /dev/null +++ b/zephyr/include/emul/emul_bmi.h @@ -0,0 +1,518 @@ +/* Copyright 2021 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +/** + * @file + * + * @brief Backend API for BMI emulator + */ + +#ifndef __EMUL_BMI_H +#define __EMUL_BMI_H + +#include <emul.h> +#include <drivers/i2c.h> +#include <drivers/i2c_emul.h> + +/** + * @brief BMI emulator backend API + * @defgroup bmi_emul BMI emulator + * @{ + * + * BMI emulator supports responses to all write and read I2C messages. + * Accelerometer and gyroscope registers are obtained from internal emulator + * state, range register and offset. FIFO is fully simulated. Emulator can be + * extended to support more models of BMI. + * Application may alter emulator state: + * + * - define a Device Tree overlay file to set which inadvisable driver behaviour + * should be treated as errors and which model is emulated + * - call @ref bmi_emul_set_read_func and @ref bmi_emul_set_write_func to setup + * custom handlers for I2C messages + * - call @ref bmi_emul_set_reg and @ref bmi_emul_get_reg to set and get value + * of BMI registers + * - call @ref bmi_emul_set_off and @ref bmi_emul_get_off to set and get + * internal offset value + * - call @ref bmi_emul_set_value and @ref bmi_emul_get_value to set and get + * accelerometer or gyroscope value + * - call bmi_emul_set_err_* to change emulator behaviour on inadvisable driver + * behaviour + * - call @ref bmi_emul_simulate_cmd_exec_time to enable or disable simulation + * of command execution time + * - call @ref bmi_emul_set_read_fail_reg and @ref bmi_emul_set_write_fail_reg + * to configure emulator to fail on given register read or write + * - call @ref bmi_emul_append_frame to add frame to FIFO + * - call @reg bmi_emul_set_skipped_frames to generate skip frame on next access + * to FIFO + */ + +/** + * Axis argument used in @ref bmi_emul_set_value @ref bmi_emul_get_value + * @ref bmi_emul_set_off and @ref bmi_emul_get_off + */ +enum bmi_emul_axis { + BMI_EMUL_ACC_X, + BMI_EMUL_ACC_Y, + BMI_EMUL_ACC_Z, + BMI_EMUL_GYR_X, + BMI_EMUL_GYR_Y, + BMI_EMUL_GYR_Z, +}; + +/** BMI emulator models */ +#define BMI_EMUL_160 1 + +/** Last register supported by emulator */ +#define BMI_EMUL_MAX_REG 0x80 +/** Maximum number of registers that can be backed in NVM */ +#define BMI_EMUL_MAX_NVM_REGS 10 + +/** Headers used in FIFO frames */ +#define BMI_EMUL_FIFO_HEAD_SKIP 0x40 +#define BMI_EMUL_FIFO_HEAD_TIME 0x44 +#define BMI_EMUL_FIFO_HEAD_CONFIG 0x48 +#define BMI_EMUL_FIFO_HEAD_EMPTY 0x80 +#define BMI_EMUL_FIFO_HEAD_DATA 0x80 +#define BMI_EMUL_FIFO_HEAD_DATA_MAG BIT(4) +#define BMI_EMUL_FIFO_HEAD_DATA_GYR BIT(3) +#define BMI_EMUL_FIFO_HEAD_DATA_ACC BIT(2) +#define BMI_EMUL_FIFO_HEAD_DATA_TAG_MASK 0x03 + +/** + * Acceleration 1g in internal emulator units. It is helpful for using + * functions @ref bmi_emul_set_value @ref bmi_emul_get_value + * @ref bmi_emul_set_off and @ref bmi_emul_get_off + */ +#define BMI_EMUL_1G BIT(14) +/** + * Gyroscope 125°/s in internal emulator units. It is helpful for using + * functions @ref bmi_emul_set_value @ref bmi_emul_get_value + * @ref bmi_emul_set_off and @ref bmi_emul_get_off + */ +#define BMI_EMUL_125_DEG_S BIT(15) + +/** Type of frames that can be added to the emulator frames list */ +#define BMI_EMUL_FRAME_CONFIG BIT(0) +#define BMI_EMUL_FRAME_ACC BIT(1) +#define BMI_EMUL_FRAME_MAG BIT(2) +#define BMI_EMUL_FRAME_GYR BIT(3) + +/** + * Code returned by model specific handle_read and handle_write functions, when + * RO register is accessed on write or WO register is accessed on read + */ +#define BMI_EMUL_ACCESS_E 1 + +/** + * Special register values used in @ref bmi_emul_set_read_fail_reg and + * @ref bmi_emul_set_write_fail_reg + */ +#define BMI_EMUL_FAIL_ALL_REG (-1) +#define BMI_EMUL_NO_FAIL_REG (-2) + +/** Structure used to describe single FIFO frame */ +struct bmi_emul_frame { + /** Type of frame */ + uint8_t type; + /** Tag added to data frame */ + uint8_t tag; + /** Value used in config frame */ + uint8_t config; + /** Accelerometer sensor values in internal emulator units */ + int32_t acc_x; + int32_t acc_y; + int32_t acc_z; + /** Gyroscope sensor values in internal emulator units */ + int32_t gyr_x; + int32_t gyr_y; + int32_t gyr_z; + /** Magnetometer/other sensor values in internal emulator units */ + int32_t mag_x; + int32_t mag_y; + int32_t mag_z; + int32_t rhall; + + /** Pointer to next frame or NULL */ + struct bmi_emul_frame *next; +}; + +/** Structure describing specific BMI model */ +struct bmi_emul_type_data { + /** Indicate if time frame should follow config frame */ + bool sensortime_follow_config_frame; + + /** + * @brief Model specific write function. It should modify state of + * emulator if required. @p reg value should be updated to + * register which is acctually accessed. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + * @param reg Pointer to accessed reg. If different reg is accessed, + * this value should be modified. + * @param byte Number of handled bytes in this write command + * @param val Value that is being written + * + * @return 0 on success + * @return BMI_EMUL_ACCESS_E on RO register access + * @return other on error + */ + int (*handle_write)(uint8_t *regs, struct i2c_emul *emul, int *reg, + int byte, uint8_t val); + /** + * @brief Model specific read function. It should modify state of + * emulator if required. @p reg value should be updated to + * register which is acctually accessed. @p buf should be + * set to response value. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + * @param reg Pointer to accessed reg. If different reg is accessed, + * this value should be modified. + * @param byte Byte which is accessed during block read + * @param buf Pointer where read byte should be stored + * + * @return 0 on success + * @return BMI_EMUL_ACCESS_E on WO register access + * @return other on error + */ + int (*handle_read)(uint8_t *regs, struct i2c_emul *emul, int *reg, + int byte, char *buf); + /** + * @brief Model specific reset function. It should modify state of + * emulator to imitate after reset conditions. + * + * @param regs Pointer to array of emulator's registers + * @param emul Pointer to BMI emulator + */ + void (*reset)(uint8_t *regs, struct i2c_emul *emul); + + /** Array of reserved bits mask for each register */ + const uint8_t *rsvd_mask; + + /** Array of registers that are backed in NVM */ + const int *nvm_reg; + /** Number of registers backed in NVM */ + int nvm_len; + + /** Gyroscope X axis register */ + int gyr_off_reg; + /** Accelerometer X axis register */ + int acc_off_reg; + /** Gyroscope 9 and 8 bits register */ + int gyr98_off_reg; +}; + +/** + * @brief Get BMI160 model specific structure. + * + * @return Pointer to BMI160 specific structure + */ +const struct bmi_emul_type_data *get_bmi160_emul_type_data(void); + +/** + * @brief Get pointer to BMI emulator using device tree order number. + * + * @param ord Device tree order number obtained from DT_DEP_ORD macro + * + * @return Pointer to BMI emulator + */ +struct i2c_emul *bmi_emul_get(int ord); + +/** + * @brief Custom function type that is used as user-defined callback in read + * I2C messages handling. + * + * @param emul Pointer to BMI emulator + * @param reg Address which is now accessed by read command + * @param byte Byte which is accessed during block read + * @param data Pointer to custom user data + * + * @return 0 on success. Value of @p reg should be set by @ref bmi_emul_set_reg + * @return 1 continue with normal BMI emulator handler + * @return negative on error + */ +typedef int (*bmi_emul_read_func)(struct i2c_emul *emul, int reg, int byte, + void *data); + +/** + * @brief Custom function type that is used as user-defined callback in write + * I2C messages handling. + * + * @param emul Pointer to BMA255 emulator + * @param reg Address which is now accessed by write command + * @param byte Number of handled bytes in this write command. It does include + * first byte containing accessed register address. + * @param val Value which is being written to @p reg + * @param data Pointer to custom user data + * + * @return 0 on success + * @return 1 continue with normal BMI emulator handler + * @return negative on error + */ +typedef int (*bmi_emul_write_func)(struct i2c_emul *emul, int reg, int byte, + uint8_t val, void *data); + +/** + * @brief Lock access to BMI properties. After acquiring lock, user + * may change emulator behaviour in multi-thread setup. + * + * @param emul Pointer to BMI emulator + * @param timeout Timeout in getting lock + * + * @return k_mutex_lock return code + */ +int bmi_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout); + +/** + * @brief Unlock access to BMI properties. + * + * @param emul Pointer to BMI emulator + * + * @return k_mutex_unlock return code + */ +int bmi_emul_unlock_data(struct i2c_emul *emul); + +/** + * @brief Set write handler for I2C messages. This function is called before + * generic handler. + * + * @param emul Pointer to BMI emulator + * @param func Pointer to custom function + * @param data User data passed on call of custom function + */ +void bmi_emul_set_write_func(struct i2c_emul *emul, bmi_emul_write_func func, + void *data); + +/** + * @brief Set read handler for I2C messages. This function is called before + * generic handler. + * + * @param emul Pointer to BMI emulator + * @param func Pointer to custom function + * @param data User data passed on call of custom function + */ +void bmi_emul_set_read_func(struct i2c_emul *emul, bmi_emul_read_func func, + void *data); + +/** + * @brief Set value of given register of BMI + * + * @param emul Pointer to BMI emulator + * @param reg Register address which value will be changed + * @param val New value of the register + */ +void bmi_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val); + +/** + * @brief Get value of given register of BMI + * + * @param emul Pointer to BMI emulator + * @param reg Register address + * + * @return Value of the register + */ +uint8_t bmi_emul_get_reg(struct i2c_emul *emul, int reg); + +/** + * @brief Setup fail on read of given register of BMI + * + * @param emul Pointer to BMI emulator + * @param reg Register address or one of special values (BMI_EMUL_FAIL_ALL_REG, + * BMI_EMUL_NO_FAIL_REG) + */ +void bmi_emul_set_read_fail_reg(struct i2c_emul *emul, int reg); + +/** + * @brief Setup fail on write of given register of BMI + * + * @param emul Pointer to BMI emulator + * @param reg Register address or one of special values (BMI_EMUL_FAIL_ALL_REG, + * BMI_EMUL_NO_FAIL_REG) + */ +void bmi_emul_set_write_fail_reg(struct i2c_emul *emul, int reg); + +/** + * @brief Get internal value of offset for given axis and sensor + * + * @param emul Pointer to BMI emulator + * @param axis Axis to access + * + * @return Offset of given axis. LSB for accelerometer is 0.061mg and for + * gyroscope is 0.0037°/s. + */ +int16_t bmi_emul_get_off(struct i2c_emul *emul, enum bmi_emul_axis axis); + +/** + * @brief Set internal value of offset for given axis and sensor + * + * @param emul Pointer to BMI emulator + * @param axis Axis to access + * @param val New value of given axis. LSB for accelerometer is 0.061mg and for + * gyroscope is 0.0037°/s. + */ +void bmi_emul_set_off(struct i2c_emul *emul, enum bmi_emul_axis axis, + int16_t val); + +/** + * @brief Get internal value of sensor for given axis + * + * @param emul Pointer to BMI emulator + * @param axis Axis to access + * + * @return Sensor value of given axis. LSB for accelerometer is 0.061mg and for + * gyroscope is 0.0037°/s. + */ +int32_t bmi_emul_get_value(struct i2c_emul *emul, enum bmi_emul_axis axis); + +/** + * @brief Set internal value of sensor for given axis + * + * @param emul Pointer to BMI emulator + * @param axis Axis to access + * @param val New value of given axis. LSB for accelerometer is 0.061mg and for + * gyroscope is 0.0037°/s. + */ +void bmi_emul_set_value(struct i2c_emul *emul, enum bmi_emul_axis axis, + int32_t val); + +/** + * @brief Set if error should be generated when read only register is being + * written + * + * @param emul Pointer to BMI emulator + * @param set Check for this error + */ +void bmi_emul_set_err_on_ro_write(struct i2c_emul *emul, bool set); + +/** + * @brief Set if error should be generated when reserved bits of register are + * not set to 0 on write I2C message + * + * @param emul Pointer to BMI emulator + * @param set Check for this error + */ +void bmi_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set); + +/** + * @brief Set if error should be generated when write only register is read + * + * @param emul Pointer to BMI emulator + * @param set Check for this error + */ +void bmi_emul_set_err_on_wo_read(struct i2c_emul *emul, bool set); + +/** + * @brief Set if effect of simulated command should take place after simulated + * time pass from issuing command. + * + * @param emul Pointer to BMI emulator + * @param set Simulate command execution time + */ +void bmi_emul_simulate_cmd_exec_time(struct i2c_emul *emul, bool set); + +/** + * @brief Set number of skipped frames. It will generate skip frame on next + * access to FIFO. After that number of skipped frames is reset to 0. + * + * @param emul Pointer to BMI emulator + * @param skip Number of skipped frames + */ +void bmi_emul_set_skipped_frames(struct i2c_emul *emul, uint8_t skip); + +/** + * @brief Clear all FIFO frames, set current frame to empty and reset fifo_skip + * counter + * + * @param emul Pointer to BMI emulator + * @param tag_time Indicate if sensor time should be included in empty frame + * @param header Indicate if header should be included in frame + */ +void bmi_emul_flush_fifo(struct i2c_emul *emul, bool tag_time, bool header); + +/** + * @brief Restore registers backed by NVM, reset sensor time and flush FIFO + * + * @param emul Pointer to BMI emulator + */ +void bmi_emul_reset_common(struct i2c_emul *emul, bool tag_time, bool header); + +/** + * @brief Set command end time to @p time ms from now + * + * @param emul Pointer to BMI emulator + * @param time After this amount of ms command should end + */ +void bmi_emul_set_cmd_end_time(struct i2c_emul *emul, int time); + +/** + * @brief Check if command should end + * + * @param emul Pointer to BMI emulator + */ +bool bmi_emul_is_cmd_end(struct i2c_emul *emul); + +/** + * @brief Append FIFO @p frame to the emulator list of frames. It can be read + * using I2C interface. + * + * @param emul Pointer to BMI emulator + * @param frame Pointer to new FIFO frame. Pointed data has to be valid while + * emulator may use this frame (until flush of FIFO or reading + * it out through I2C) + */ +void bmi_emul_append_frame(struct i2c_emul *emul, struct bmi_emul_frame *frame); + +/** + * @brief Get length of all frames that are on the emulator list of frames. + * + * @param emul Pointer to BMI emulator + * @param tag_time Indicate if sensor time should be included in empty frame + * @param header Indicate if header should be included in frame + */ +uint16_t bmi_emul_fifo_len(struct i2c_emul *emul, bool tag_time, bool header); + +/** + * @brief Get next byte that should be returned on FIFO data access. + * + * @param emul Pointer to BMI emulator + * @param byte Which byte of block read command is currently handled + * @param tag_time Indicate if sensor time should be included in empty frame + * @param header Indicate if header should be included in frame + * @param acc_shift How many bits should be right shifted from accelerometer + * data + * @param gyr_shift How many bits should be right shifted from gyroscope data + * + * @return FIFO data byte + */ +uint8_t bmi_emul_get_fifo_data(struct i2c_emul *emul, int byte, + bool tag_time, bool header, int acc_shift, + int gyr_shift); + +/** + * @brief Saves current internal state of sensors to emulator's registers. + * + * @param emul Pointer to BMI emulator + * @param acc_shift How many bits should be right shifted from accelerometer + * data + * @param gyr_shift How many bits should be right shifted from gyroscope data + * @param acc_reg Register which holds LSB of accelerometer sensor + * @param gyr_reg Register which holds LSB of gyroscope sensor + * @param sensortime_reg Register which holds LSB of sensor time + * @param acc_off_en Indicate if accelerometer offset should be included to + * sensor data value + * @param gyr_off_en Indicate if gyroscope offset should be included to + * sensor data value + */ +void bmi_emul_state_to_reg(struct i2c_emul *emul, int acc_shift, + int gyr_shift, int acc_reg, int gyr_reg, + int sensortime_reg, bool acc_off_en, + bool gyr_off_en); + +/** + * @} + */ + +#endif /* __EMUL_BMI_H */ |