zephyr: Add BMI260 emulator

Add extension to support BMI260 model.

BUG=b:184856157
BRANCH=none
TEST=none

Signed-off-by: Tomasz Michalec <tm@semihalf.com>
Change-Id: Iaffebdb5279001d085fd56868e81318528380380
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2997364
Reviewed-by: Simon Glass <sjg@chromium.org>
Reviewed-by: Jeremy Bettis <jbettis@chromium.org>

6 files changed, 573 insertions, 0 deletions

diff --git a/include/driver/accelgyro_bmi260.h b/include/driver/accelgyro_bmi260.h
index 9f39dd568a..86c05a0697 100644
--- a/include/driver/accelgyro_bmi260.h
+++ b/include/driver/accelgyro_bmi260.h
@@ -197,6 +197,8 @@
 	  ((_sensor) == MOTIONSENSE_TYPE_GYRO ? BMI260_FIFO_GYR_EN : \
 	   BMI260_FIFO_AUX_EN))
 
+#define BMI260_SATURATION       0x4a
+
 #define BMI260_AUX_DEV_ID       0x4b
 #define BMI260_AUX_I2C_ADDRESS          BMI260_AUX_DEV_ID
 
@@ -288,6 +290,8 @@
 #define BMI260_DRV              0x6c
 #define BMI260_ACC_SELF_TEST    0x6d
 
+#define BMI260_GYR_SELF_TEST_AXES 0x6e
+
 #define BMI260_NV_CONF          0x70
 #define BMI260_ACC_OFFSET_EN        BIT(3)
 
diff --git a/zephyr/dts/bindings/emul/zephyr,bmi.yaml b/zephyr/dts/bindings/emul/zephyr,bmi.yaml
index 0067407e61..a754287bcc 100644
--- a/zephyr/dts/bindings/emul/zephyr,bmi.yaml
+++ b/zephyr/dts/bindings/emul/zephyr,bmi.yaml
@@ -14,6 +14,7 @@ properties:
         required: true
         enum:
           - BMI_EMUL_160
+          - BMI_EMUL_260
         description: Model of device that is emulated.
 
     error-on-ro-write:
diff --git a/zephyr/emul/CMakeLists.txt b/zephyr/emul/CMakeLists.txt
index 763372cddc..d0d4bce0f1 100644
--- a/zephyr/emul/CMakeLists.txt
+++ b/zephyr/emul/CMakeLists.txt
@@ -8,3 +8,4 @@ zephyr_library_sources_ifdef(CONFIG_EMUL_BC12_DETECT_PI3USB9201	emul_pi3usb9201.
 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)
+zephyr_library_sources_ifdef(CONFIG_EMUL_BMI		emul_bmi260.c)
diff --git a/zephyr/emul/emul_bmi.c b/zephyr/emul/emul_bmi.c
index 0bcd01c4dc..b5a8de8b5a 100644
--- a/zephyr/emul/emul_bmi.c
+++ b/zephyr/emul/emul_bmi.c
@@ -1292,6 +1292,9 @@ static int bmi_emul_init(const struct emul *emul,
 	case BMI_EMUL_160:
 		data->type_data = get_bmi160_emul_type_data();
 		break;
+	case BMI_EMUL_260:
+		data->type_data = get_bmi260_emul_type_data();
+		break;
 	}
 
 	ret = i2c_emul_register(parent, emul->dev_label, &data->emul);
diff --git a/zephyr/emul/emul_bmi260.c b/zephyr/emul/emul_bmi260.c
new file mode 100644
index 0000000000..82ca5626ee
--- /dev/null
+++ b/zephyr/emul/emul_bmi260.c
@@ -0,0 +1,557 @@
+/* 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_bmi260);
+
+#include <device.h>
+#include <emul.h>
+#include <drivers/i2c.h>
+#include <drivers/i2c_emul.h>
+
+#include "emul/emul_bmi.h"
+
+#include "driver/accelgyro_bmi260.h"
+#include "driver/accelgyro_bmi_common.h"
+
+/** Mask reserved bits in each register of BMI260 */
+static const uint8_t bmi_emul_260_rsvd_mask[] = {
+	[BMI260_CHIP_ID]		= 0x00,
+	[0x01]				= 0xff, /* Reserved */
+	[BMI260_ERR_REG]		= 0x20,
+	[BMI260_STATUS]			= 0x0b,
+	[BMI260_AUX_X_L_G]		= 0x00,
+	[BMI260_AUX_X_H_G]		= 0x00,
+	[BMI260_AUX_Y_L_G]		= 0x00,
+	[BMI260_AUX_Y_H_G]		= 0x00,
+	[BMI260_AUX_Z_L_G]		= 0x00,
+	[BMI260_AUX_Z_H_G]		= 0x00,
+	[BMI260_AUX_R_L_G]		= 0x00,
+	[BMI260_AUX_R_H_G]		= 0x00,
+	[BMI260_ACC_X_L_G]		= 0x00,
+	[BMI260_ACC_X_H_G]		= 0x00,
+	[BMI260_ACC_Y_L_G]		= 0x00,
+	[BMI260_ACC_Y_H_G]		= 0x00,
+	[BMI260_ACC_Z_L_G]		= 0x00,
+	[BMI260_ACC_Z_H_G]		= 0x00,
+	[BMI260_GYR_X_L_G]		= 0x00,
+	[BMI260_GYR_X_H_G]		= 0x00,
+	[BMI260_GYR_Y_L_G]		= 0x00,
+	[BMI260_GYR_Y_H_G]		= 0x00,
+	[BMI260_GYR_Z_L_G]		= 0x00,
+	[BMI260_GYR_Z_H_G]		= 0x00,
+	[BMI260_SENSORTIME_0]		= 0x00,
+	[BMI260_SENSORTIME_1]		= 0x00,
+	[BMI260_SENSORTIME_2]		= 0x00,
+	[BMI260_EVENT]			= 0xe2,
+	[BMI260_INT_STATUS_0]		= 0x00,
+	[BMI260_INT_STATUS_1]		= 0x18,
+	[BMI260_SC_OUT_0]		= 0x00,
+	[BMI260_SC_OUT_1]		= 0x00,
+	[BMI260_ORIENT_ACT]		= 0xe0,
+	[BMI260_INTERNAL_STATUS]	= 0x00,
+	[BMI260_TEMPERATURE_0]		= 0x00,
+	[BMI260_TEMPERATURE_1]		= 0x00,
+	[BMI260_FIFO_LENGTH_0]		= 0x00,
+	[BMI260_FIFO_LENGTH_1]		= 0xc0,
+	[BMI160_FIFO_DATA]		= 0x00,
+	[0x27 ... 0x2e]			= 0xff, /* Reserved */
+	[BMI260_FEAT_PAGE]		= 0xf8,
+	[0x30 ... 0x3f]			= 0x00, /* Features */
+	[BMI260_ACC_CONF]		= 0x00,
+	[BMI260_ACC_RANGE]		= 0xfc,
+	[BMI260_GYR_CONF]		= 0x00,
+	[BMI260_GYR_RANGE]		= 0xf0,
+	[BMI260_AUX_CONF]		= 0x00,
+	[BMI260_FIFO_DOWNS]		= 0x00,
+	[BMI260_FIFO_WTM_0]		= 0x00,
+	[BMI260_FIFO_WTM_1]		= 0xe0,
+	[BMI260_FIFO_CONFIG_0]		= 0xfc,
+	[BMI260_FIFO_CONFIG_1]		= 0x00,
+	[BMI260_SATURATION]		= 0xc0,
+	[BMI260_AUX_DEV_ID]		= 0x01,
+	[BMI260_AUX_IF_CONF]		= 0x30,
+	[BMI260_AUX_RD_ADDR]		= 0x00,
+	[BMI260_AUX_WR_ADDR]		= 0x00,
+	[BMI260_AUX_WR_DATA]		= 0x00,
+	[0x50 ... 0x51]			= 0xff, /* Reserved */
+	[BMI260_ERR_REG_MSK]		= 0x20,
+	[BMI260_INT1_IO_CTRL]		= 0xe1,
+	[BMI260_INT2_IO_CTRL]		= 0xe1,
+	[BMI260_INT_LATCH]		= 0xfe,
+	[BMI260_INT1_MAP_FEAT]		= 0x00,
+	[BMI260_INT2_MAP_FEAT]		= 0x00,
+	[BMI260_INT_MAP_DATA]		= 0x00,
+	[BMI260_INIT_CTRL]		= 0x00,
+	[0x5a]				= 0xff, /* Reserved */
+	[BMI260_INIT_ADDR_0]		= 0xf0,
+	[BMI260_INIT_ADDR_1]		= 0x00,
+	[0x5d]				= 0xff, /* Reserved */
+	[BMI260_INIT_DATA]		= 0x00,
+	[BMI260_INTERNAL_ERROR]		= 0xe9,
+	[0x60 ... 0x67]			= 0xff, /* Reserved */
+	[BMI260_AUX_IF_TRIM]		= 0xf8,
+	[BMI260_GYR_CRT_CONF]		= 0xf2,
+	[BMI260_NVM_CONF]		= 0xfd,
+	[BMI260_IF_CONF]		= 0xcc,
+	[BMI260_DRV]			= 0x00,
+	[BMI260_ACC_SELF_TEST]		= 0xf2,
+	[BMI260_GYR_SELF_TEST_AXES]	= 0xf0,
+	[0x6f]				= 0xff, /* Reserved */
+	[BMI260_NV_CONF]		= 0xf0,
+	[BMI260_OFFSET_ACC70]		= 0x00,
+	[BMI260_OFFSET_ACC70 + 1]	= 0x00,
+	[BMI260_OFFSET_ACC70 + 2]	= 0x00,
+	[BMI260_OFFSET_GYR70]		= 0x00,
+	[BMI260_OFFSET_GYR70 + 1]	= 0x00,
+	[BMI260_OFFSET_GYR70 + 2]	= 0x00,
+	[BMI160_OFFSET_EN_GYR98]	= 0x00,
+	[0x78 ... 0x7b]			= 0xff, /* Reserved */
+	[BMI260_PWR_CONF]		= 0xf8,
+	[BMI260_PWR_CTRL]		= 0xf0,
+	[BMI260_CMD_REG]		= 0x00,
+};
+
+/**
+ * @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 bmi260_emul_reset(uint8_t *regs, struct i2c_emul *emul)
+{
+	bool tag_time;
+	bool header;
+
+	regs[BMI260_CHIP_ID]		= 0x27;
+	regs[BMI260_ERR_REG]		= 0x00;
+	regs[BMI260_STATUS]		= 0x10;
+	regs[BMI260_AUX_X_L_G]		= 0x00;
+	regs[BMI260_AUX_X_H_G]		= 0x00;
+	regs[BMI260_AUX_Y_L_G]		= 0x00;
+	regs[BMI260_AUX_Y_H_G]		= 0x00;
+	regs[BMI260_AUX_Z_L_G]		= 0x00;
+	regs[BMI260_AUX_Z_H_G]		= 0x00;
+	regs[BMI260_AUX_R_L_G]		= 0x00;
+	regs[BMI260_AUX_R_H_G]		= 0x00;
+	regs[BMI260_ACC_X_L_G]		= 0x00;
+	regs[BMI260_ACC_X_H_G]		= 0x00;
+	regs[BMI260_ACC_Y_L_G]		= 0x00;
+	regs[BMI260_ACC_Y_H_G]		= 0x00;
+	regs[BMI260_ACC_Z_L_G]		= 0x00;
+	regs[BMI260_ACC_Z_H_G]		= 0x00;
+	regs[BMI260_GYR_X_L_G]		= 0x00;
+	regs[BMI260_GYR_X_H_G]		= 0x00;
+	regs[BMI260_GYR_Y_L_G]		= 0x00;
+	regs[BMI260_GYR_Y_H_G]		= 0x00;
+	regs[BMI260_GYR_Z_L_G]		= 0x00;
+	regs[BMI260_GYR_Z_H_G]		= 0x00;
+	regs[BMI260_SENSORTIME_0]	= 0x00;
+	regs[BMI260_SENSORTIME_1]	= 0x00;
+	regs[BMI260_SENSORTIME_2]	= 0x00;
+	regs[BMI260_EVENT]		= 0x01;
+	regs[BMI260_INT_STATUS_0]	= 0x00;
+	regs[BMI260_INT_STATUS_1]	= 0x00;
+	regs[BMI260_SC_OUT_0]		= 0x00;
+	regs[BMI260_SC_OUT_1]		= 0x00;
+	regs[BMI260_ORIENT_ACT]		= 0x00;
+	regs[BMI260_INTERNAL_STATUS]	= 0x00;
+	regs[BMI260_TEMPERATURE_0]	= 0x00;
+	regs[BMI260_TEMPERATURE_1]	= 0x80;
+	regs[BMI260_FIFO_LENGTH_0]	= 0x00;
+	regs[BMI260_FIFO_LENGTH_1]	= 0x00;
+	regs[BMI160_FIFO_DATA]		= 0x00;
+	regs[BMI260_FEAT_PAGE]		= 0x00;
+	regs[BMI260_ACC_CONF]		= 0xa8;
+	regs[BMI260_ACC_RANGE]		= 0x02;
+	regs[BMI260_GYR_CONF]		= 0xa9;
+	regs[BMI260_GYR_RANGE]		= 0x00;
+	regs[BMI260_AUX_CONF]		= 0x46;
+	regs[BMI260_FIFO_DOWNS]		= 0x88;
+	regs[BMI260_FIFO_WTM_0]		= 0x00;
+	regs[BMI260_FIFO_WTM_1]		= 0x02;
+	regs[BMI260_FIFO_CONFIG_0]	= 0x02;
+	regs[BMI260_FIFO_CONFIG_1]	= 0x10;
+	regs[BMI260_SATURATION]		= 0x00;
+	regs[BMI260_AUX_DEV_ID]		= 0x20;
+	regs[BMI260_AUX_IF_CONF]	= 0x83;
+	regs[BMI260_AUX_RD_ADDR]	= 0x42;
+	regs[BMI260_AUX_WR_ADDR]	= 0x4c;
+	regs[BMI260_AUX_WR_DATA]	= 0x02;
+	regs[BMI260_ERR_REG_MSK]	= 0x00;
+	regs[BMI260_INT1_IO_CTRL]	= 0x00;
+	regs[BMI260_INT2_IO_CTRL]	= 0x00;
+	regs[BMI260_INT_LATCH]		= 0x00;
+	regs[BMI260_INT1_MAP_FEAT]	= 0x00;
+	regs[BMI260_INT2_MAP_FEAT]	= 0x00;
+	regs[BMI260_INT_MAP_DATA]	= 0x00;
+	regs[BMI260_INIT_CTRL]		= 0x00;
+	regs[BMI260_INIT_ADDR_0]	= 0x00;
+	regs[BMI260_INIT_ADDR_1]	= 0x00;
+	regs[BMI260_INIT_DATA]		= 0x00;
+	regs[BMI260_INTERNAL_ERROR]	= 0x00;
+	regs[BMI260_AUX_IF_TRIM]	= 0x01;
+	regs[BMI260_GYR_CRT_CONF]	= 0x00;
+	regs[BMI260_NVM_CONF]		= 0x00;
+	regs[BMI260_IF_CONF]		= 0x00;
+	regs[BMI260_DRV]		= 0xff;
+	regs[BMI260_ACC_SELF_TEST]	= 0x00;
+	regs[BMI260_GYR_SELF_TEST_AXES]	= 0x00;
+	regs[BMI260_PWR_CONF]		= 0x03;
+	regs[BMI260_PWR_CTRL]		= 0x00;
+	regs[BMI260_CMD_REG]		= 0x00;
+
+	/* Call generic reset */
+	tag_time = regs[BMI260_FIFO_CONFIG_0] & BMI260_FIFO_TIME_EN;
+	header = regs[BMI260_FIFO_CONFIG_1] & BMI260_FIFO_HEADER_EN;
+	bmi_emul_reset_common(emul, tag_time, header);
+}
+
+/**
+ * @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 bmi260_emul_acc_range_to_shift(uint8_t range)
+{
+	switch (range & 0xf) {
+	case BMI260_GSEL_2G:
+		return 0;
+	case BMI260_GSEL_4G:
+		return 1;
+	case BMI260_GSEL_8G:
+		return 2;
+	case BMI260_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 bmi260_emul_gyr_range_to_shift(uint8_t range)
+{
+	switch (range & 0x7) {
+	case BMI260_DPS_SEL_2000:
+		return 4;
+	case BMI260_DPS_SEL_1000:
+		return 3;
+	case BMI260_DPS_SEL_500:
+		return 2;
+	case BMI260_DPS_SEL_250:
+		return 1;
+	case BMI260_DPS_SEL_125:
+		return 0;
+	default:
+		return 0;
+	}
+}
+
+/**
+ * @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 bmi260_emul_start_cmd(uint8_t *regs, struct i2c_emul *emul, int cmd)
+{
+	int time;
+	int ret;
+
+	switch (cmd) {
+	case BMI260_CMD_SOFT_RESET:
+		time = 1;
+		break;
+	case BMI260_CMD_FIFO_FLUSH:
+		time = 0;
+		break;
+	default:
+		LOG_ERR("Unknown command 0x%x", cmd);
+		return -EIO;
+	}
+
+	regs[BMI260_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 bmi260_emul_end_cmd(uint8_t *regs, struct i2c_emul *emul)
+{
+	uint8_t pmu_status;
+	bool tag_time;
+	bool header;
+	int cmd;
+
+	cmd = regs[BMI260_CMD_REG];
+	regs[BMI260_CMD_REG] = 0;
+	tag_time = regs[BMI260_FIFO_CONFIG_0] & BMI260_FIFO_TIME_EN;
+	header = regs[BMI260_FIFO_CONFIG_1] & BMI260_FIFO_HEADER_EN;
+
+	switch (cmd) {
+	case BMI160_CMD_SOFT_RESET:
+		bmi260_emul_reset(regs, emul);
+		break;
+	case BMI160_CMD_FIFO_FLUSH:
+		bmi_emul_flush_fifo(emul, tag_time, header);
+		break;
+	}
+}
+
+/**
+ * @brief BMI260 specific write function. It handle block writes. Init data
+ *        register is trap register, so after reaching it, register address
+ *        is not increased on 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 BMI260 specific
+ *        function. On changing of FIFO header/headerless mode or
+ *        enabling/disabling sensor in 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 bmi260_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
+				    int *reg, int byte, uint8_t val)
+{
+	uint8_t mask;
+	bool tag_time;
+	bool header;
+	int ret;
+
+	/* Ignore first byte which sets starting register */
+	byte -= 1;
+
+	if (*reg <= BMI260_INIT_DATA && *reg + byte >= BMI260_INIT_DATA) {
+		byte -= *reg - BMI260_INIT_DATA;
+		*reg = BMI260_INIT_DATA;
+	} else {
+		*reg += byte;
+	}
+
+	if (*reg <= BMI260_FIFO_DATA || *reg == BMI260_GYR_SELF_TEST_AXES ||
+	    *reg == BMI260_INTERNAL_ERROR || *reg == BMI260_SATURATION) {
+		return BMI_EMUL_ACCESS_E;
+	}
+
+
+	/* Stop on going command if required */
+	if (regs[BMI260_CMD_REG] != 0 && bmi_emul_is_cmd_end(emul)) {
+		bmi260_emul_end_cmd(regs, emul);
+	}
+
+	tag_time = regs[BMI260_FIFO_CONFIG_0] & BMI260_FIFO_TIME_EN;
+	header = regs[BMI260_FIFO_CONFIG_1] & BMI260_FIFO_HEADER_EN;
+
+	switch (*reg) {
+	case BMI260_CMD_REG:
+		if (regs[BMI260_CMD_REG] != 0) {
+			LOG_ERR("Issued command before previous end");
+			return -EIO;
+		}
+
+		return bmi260_emul_start_cmd(regs, emul, val);
+	case BMI260_FIFO_CONFIG_1:
+		/*
+		 * Clear FIFO on transition between headerless and
+		 * header mode
+		 */
+		if (!!(val & BMI260_FIFO_HEADER_EN) != header) {
+			bmi_emul_flush_fifo(emul, tag_time, header);
+		}
+		break;
+	case BMI260_PWR_CTRL:
+		/*
+		 * Clear FIFO on enabling/disabling sensors in headerless
+		 * mode
+		 */
+		mask = BMI260_AUX_EN & BMI260_GYR_EN & BMI260_ACC_EN;
+		if ((val & mask) != (regs[BMI260_PWR_CTRL] & mask) && !header) {
+			bmi_emul_flush_fifo(emul, tag_time, header);
+		}
+		break;
+	}
+
+	return 0;
+}
+
+/**
+ * @brief BMI260 specific read function. It handle block reads. FIFO data
+ *        register and init data register are trap registers, 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 bmi260_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.
+	 * Init data is also block, but it is not implemented in emulator.
+	 * Else block read access subsequent registers.
+	 */
+	if (*reg <= BMI260_FIFO_DATA && *reg + byte >= BMI260_FIFO_DATA) {
+		byte -= *reg - BMI260_FIFO_DATA;
+		*reg = BMI260_FIFO_DATA;
+	} else if (*reg <= BMI260_INIT_DATA &&
+		   *reg + byte >= BMI260_INIT_DATA) {
+		byte -= *reg - BMI260_INIT_DATA;
+		*reg = BMI260_INIT_DATA;
+	} else {
+		*reg += byte;
+	}
+
+	if (*reg == BMI260_CMD_REG) {
+		*buf = 0;
+
+		return BMI_EMUL_ACCESS_E;
+	}
+
+	/* Stop on going command if required */
+	if (regs[BMI260_CMD_REG] != 0 && bmi_emul_is_cmd_end(emul)) {
+		bmi260_emul_end_cmd(regs, emul);
+	}
+
+	tag_time = regs[BMI260_FIFO_CONFIG_0] & BMI260_FIFO_TIME_EN;
+	header = regs[BMI260_FIFO_CONFIG_1] & BMI260_FIFO_HEADER_EN;
+	acc_off_en = regs[BMI260_NV_CONF] & BMI260_ACC_OFFSET_EN;
+	gyr_off_en = regs[BMI260_OFFSET_EN_GYR98] & BMI260_OFFSET_GYRO_EN;
+	gyr_shift = bmi260_emul_gyr_range_to_shift(regs[BMI260_GYR_RANGE]);
+	acc_shift = bmi260_emul_acc_range_to_shift(regs[BMI260_ACC_RANGE]);
+
+	switch (*reg) {
+	case BMI260_GYR_X_L_G:
+	case BMI260_GYR_X_H_G:
+	case BMI260_GYR_Y_L_G:
+	case BMI260_GYR_Y_H_G:
+	case BMI260_GYR_Z_L_G:
+	case BMI260_GYR_Z_H_G:
+	case BMI260_ACC_X_L_G:
+	case BMI260_ACC_X_H_G:
+	case BMI260_ACC_Y_L_G:
+	case BMI260_ACC_Y_H_G:
+	case BMI260_ACC_Z_L_G:
+	case BMI260_ACC_Z_H_G:
+	case BMI260_SENSORTIME_0:
+	case BMI260_SENSORTIME_1:
+	case BMI260_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,
+					      BMI260_ACC_X_L_G,
+					      BMI260_GYR_X_L_G,
+					      BMI260_SENSORTIME_0,
+					      acc_off_en, gyr_off_en);
+		}
+		break;
+	case BMI260_FIFO_LENGTH_0:
+	case BMI260_FIFO_LENGTH_1:
+		if (byte == 0) {
+			fifo_len = bmi_emul_fifo_len(emul, tag_time, header);
+			regs[BMI260_FIFO_LENGTH_0] = fifo_len & 0xff;
+			regs[BMI260_FIFO_LENGTH_1] = (fifo_len >> 8) & 0x7;
+		}
+		break;
+	case BMI260_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 BMI260 */
+const int bmi260_nvm_reg[] = {BMI260_AUX_IF_TRIM,
+			      BMI260_NV_CONF,
+			      BMI260_DRV,
+			      BMI260_OFFSET_ACC70,
+			      BMI260_OFFSET_ACC70 + 1,
+			      BMI260_OFFSET_ACC70 + 2,
+			      BMI260_OFFSET_GYR70,
+			      BMI260_OFFSET_GYR70 + 1,
+			      BMI260_OFFSET_GYR70 + 2,
+			      BMI260_OFFSET_EN_GYR98};
+
+/** Confguration of BMI260 */
+struct bmi_emul_type_data bmi260_emul = {
+	.sensortime_follow_config_frame = true,
+	.handle_write = bmi260_emul_handle_write,
+	.handle_read = bmi260_emul_handle_read,
+	.reset = bmi260_emul_reset,
+	.rsvd_mask = bmi_emul_260_rsvd_mask,
+	.nvm_reg = bmi260_nvm_reg,
+	.nvm_len = ARRAY_SIZE(bmi260_nvm_reg),
+	.gyr_off_reg = BMI260_OFFSET_GYR70,
+	.acc_off_reg = BMI260_OFFSET_ACC70,
+	.gyr98_off_reg = BMI260_OFFSET_EN_GYR98,
+};
+
+/** Check description in emul_bmi.h */
+const struct bmi_emul_type_data *get_bmi260_emul_type_data(void)
+{
+	return &bmi260_emul;
+}
diff --git a/zephyr/include/emul/emul_bmi.h b/zephyr/include/emul/emul_bmi.h
index 81d889a301..d70b975195 100644
--- a/zephyr/include/emul/emul_bmi.h
+++ b/zephyr/include/emul/emul_bmi.h
@@ -63,6 +63,7 @@ enum bmi_emul_axis {
 
 /** BMI emulator models */
 #define BMI_EMUL_160		1
+#define BMI_EMUL_260		2
 
 /** Last register supported by emulator */
 #define BMI_EMUL_MAX_REG	0x80
@@ -211,6 +212,12 @@ struct bmi_emul_type_data {
  * @return Pointer to BMI160 specific structure
  */
 const struct bmi_emul_type_data *get_bmi160_emul_type_data(void);
+/**
+ * @brief Get BMI260 model specific structure.
+ *
+ * @return Pointer to BMI260 specific structure
+ */
+const struct bmi_emul_type_data *get_bmi260_emul_type_data(void);
 
 /**
  * @brief Get pointer to BMI emulator using device tree order number.