zephyr: Add BMI160 emulator

Add BMI emulator which is emulated device on i2c bus. Emulated
accelerometer and gyroscope properties are defined through device tree,
but they can be changed in runtime through BMI emulator API. It allows
to set custom handlers for write and read messages to emulate more
complex scenarios or malfunctioning device.

BMI emulator is designed to implement support for different BMI models
as an extension to common emulator code.

BUG=b:184856157
BRANCH=none
TEST=none

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

10 files changed, 2668 insertions, 0 deletions

diff --git a/driver/accelgyro_bmi160.h b/include/driver/accelgyro_bmi160.h
index ee9e821022..c916576130 100644
--- a/driver/accelgyro_bmi160.h
+++ b/include/driver/accelgyro_bmi160.h
@@ -251,6 +251,12 @@
 #define BMI160_INT_DATA_0      0x58
 #define BMI160_INT_DATA_1      0x59
 
+#define BMI160_INT_LOW_HIGH_0  0x5a
+#define BMI160_INT_LOW_HIGH_1  0x5b
+#define BMI160_INT_LOW_HIGH_2  0x5c
+#define BMI160_INT_LOW_HIGH_3  0x5d
+#define BMI160_INT_LOW_HIGH_4  0x5e
+
 #define BMI160_INT_MOTION_0    0x5f
 #define BMI160_INT_MOTION_1    0x60
 /*
@@ -319,12 +325,18 @@
 #define BMI160_PMU_TRIGGER     0x6c
 #define BMI160_SELF_TEST       0x6d
 
+#define BMI160_NV_CONF         0x70
+
 #define BMI160_OFFSET_ACC70        0x71
 #define BMI160_OFFSET_GYR70        0x74
 #define BMI160_OFFSET_EN_GYR98     0x77
 #define BMI160_OFFSET_ACC_EN            BIT(6)
 #define BMI160_OFFSET_GYRO_EN           BIT(7)
 
+#define BMI160_STEP_CNT_0          0x78
+#define BMI160_STEP_CNT_1          0x79
+#define BMI160_STEP_CONF_0         0x7a
+#define BMI160_STEP_CONF_1         0x7b
 
 #define BMI160_CMD_REG             0x7e
 #define BMI160_CMD_SOFT_RESET      0xb6
diff --git a/driver/accelgyro_bmi260.h b/include/driver/accelgyro_bmi260.h
index 9f39dd568a..9f39dd568a 100644
--- a/driver/accelgyro_bmi260.h
+++ b/include/driver/accelgyro_bmi260.h
diff --git a/driver/accelgyro_bmi_common.h b/include/driver/accelgyro_bmi_common.h
index 398f04dc42..398f04dc42 100644
--- a/driver/accelgyro_bmi_common.h
+++ b/include/driver/accelgyro_bmi_common.h
diff --git a/driver/mag_bmm150.h b/include/driver/mag_bmm150.h
index 9f517f8097..9f517f8097 100644
--- a/driver/mag_bmm150.h
+++ b/include/driver/mag_bmm150.h
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, &reg, 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, &reg, 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 */