zephyr: drivers: add accelgyro_bmi260 test suite

BMI260 driver is tested using BMI emulator. All functions exposed
in accelgyro_drv are tested for both accelerometer and gyroscope
sensors.

BUG=b:184856157
BRANCH=none
TEST=run zmake drivers test

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

4 files changed, 1919 insertions, 1 deletions

diff --git a/zephyr/test/drivers/overlay.dts b/zephyr/test/drivers/overlay.dts
index 924a223628..cdbe4b3662 100644
--- a/zephyr/test/drivers/overlay.dts
+++ b/zephyr/test/drivers/overlay.dts
@@ -6,6 +6,9 @@
 #include <dt-bindings/gpio_defines.h>
 
 / {
+	aliases {
+		bmi260-int = &ms_bmi260_accel;
+	};
 	named-gpios {
 		compatible = "named-gpios";
 
@@ -63,7 +66,7 @@
 			enum-name = "I2C_PORT_EEPROM";
 			label = "EEPROM";
 		};
-		accel {
+		i2c_accel: accel {
 			i2c-port = <&i2c0>;
 			enum-name = "I2C_PORT_ACCEL";
 			label = "ACCEL";
@@ -158,6 +161,64 @@
 			adc = <&adc_fan>;
 		};
 	};
+
+	/*
+	 * Declare mutexes used by sensor drivers.
+	 * A mutex node is used to create an instance of mutex_t.
+	 * A mutex node is referenced by a sensor node if the
+	 * corresponding sensor driver needs to use the
+	 * instance of the mutex.
+	 */
+	motionsense-mutex {
+		compatible = "cros-ec,motionsense-mutex";
+		mutex_bmi260: bmi260-mutex {
+			label = "BMI260_MUTEX";
+		};
+	};
+
+	/*
+	 * Driver specific data. A driver-specific data can be shared with
+	 * different motion sensors while they are using the same driver.
+	 */
+	motionsense-sensor-data {
+		bmi260_data: bmi260-drv-data {
+			compatible = "cros-ec,drvdata-bmi260";
+			status = "okay";
+		};
+	};
+
+	/*
+	 * List of motion sensors that creates motion_sensors array.
+	 * The label "lid_accel" and "base_accel" are used to indicate
+	 * motion sensor IDs for lid angle calculation.
+	 */
+	motionsense-sensor {
+		ms_bmi260_accel: ms-bmi260-accel {
+			compatible = "cros-ec,bmi260-accel";
+			status = "okay";
+
+			label = "BMI260 emul accel";
+			location = "MOTIONSENSE_LOC_BASE";
+			mutex = <&mutex_bmi260>;
+			port = <&i2c_accel>;
+			drv-data = <&bmi260_data>;
+			default-range = <4>;
+			i2c-spi-addr-flags = "BMI260_ADDR0_FLAGS";
+		};
+
+		ms_bmi260_gyro: ms-bmi260-gyro {
+			compatible = "cros-ec,bmi260-gyro";
+			status = "okay";
+
+			label = "BMI260 emul gyro";
+			location = "MOTIONSENSE_LOC_BASE";
+			mutex = <&mutex_bmi260>;
+			port = <&i2c_accel>;
+			drv-data = <&bmi260_data>;
+			default-range = <1000>; /* dps */
+			i2c-spi-addr-flags = "BMI260_ADDR0_FLAGS";
+		};
+	};
 };
 
 &gpio0 {
@@ -205,4 +266,15 @@
 		reg = <0x41>;
 		label = "SYV682X_EMUL";
 	};
+
+	accel_bmi260: bmi260@68 {
+		compatible = "zephyr,bmi";
+		reg = <0x68>;
+		label = "BMI260";
+		device-model = "BMI_EMUL_260";
+		error-on-ro-write;
+		error-on-wo-read;
+		error-on-reserved-bit-write;
+		simulate-command-exec-time;
+	};
 };
diff --git a/zephyr/test/drivers/prj.conf b/zephyr/test/drivers/prj.conf
index c4694f0644..4534f323db 100644
--- a/zephyr/test/drivers/prj.conf
+++ b/zephyr/test/drivers/prj.conf
@@ -19,6 +19,7 @@ CONFIG_ADC=y
 CONFIG_ADC_EMUL=y
 CONFIG_HEAP_MEM_POOL_SIZE=1024
 CONFIG_EMUL_BMA255=y
+CONFIG_EMUL_BMI=y
 
 CONFIG_PLATFORM_EC_BATTERY_PRESENT_GPIO=y
 CONFIG_PLATFORM_EC_EXTPOWER_GPIO=y
@@ -51,6 +52,11 @@ CONFIG_PLATFORM_EC_STEINHART_HART_3V3_30K9_47K_4050B=y
 CONFIG_PLATFORM_EC_STEINHART_HART_3V3_51K1_47K_4050B=y
 CONFIG_PLATFORM_EC_ACCEL_BMA255=y
 CONFIG_PLATFORM_EC_MOTIONSENSE=y
+CONFIG_PLATFORM_EC_ACCELGYRO_BMI260=y
+CONFIG_PLATFORM_EC_ACCELGYRO_BMI_COMM_I2C=y
+CONFIG_PLATFORM_EC_ACCEL_INTERRUPTS=y
+CONFIG_PLATFORM_EC_ACCEL_FIFO=y
+CONFIG_PLATFORM_EC_SENSOR_TIGHT_TIMESTAMPS=y
 
 # Things that default to on, but aren't working yet
 CONFIG_PLATFORM_EC_BACKLIGHT_LID=n
diff --git a/zephyr/test/drivers/src/bmi260.c b/zephyr/test/drivers/src/bmi260.c
new file mode 100644
index 0000000000..a8aa303765
--- /dev/null
+++ b/zephyr/test/drivers/src/bmi260.c
@@ -0,0 +1,1838 @@
+/* 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.
+ */
+
+#include <zephyr.h>
+#include <ztest.h>
+
+#include "common.h"
+#include "i2c.h"
+#include "emul/emul_bmi.h"
+
+#include "motion_sense_fifo.h"
+#include "driver/accelgyro_bmi260.h"
+#include "driver/accelgyro_bmi_common.h"
+
+#define BMI_ORD			DT_DEP_ORD(DT_NODELABEL(accel_bmi260))
+#define BMI_ACC_SENSOR_ID	SENSOR_ID(DT_NODELABEL(ms_bmi260_accel))
+#define BMI_GYR_SENSOR_ID	SENSOR_ID(DT_NODELABEL(ms_bmi260_gyro))
+#define BMI_INT_EVENT		\
+	TASK_EVENT_MOTION_SENSOR_INTERRUPT(SENSOR_ID(DT_ALIAS(bmi260_int)))
+
+/** How accurate comparision of vectors should be */
+#define V_EPS		8
+
+/** Convert from one type of vector to another */
+#define convert_int3v_int16(v, r) do {	\
+		r[0] = v[0];		\
+		r[1] = v[1];		\
+		r[2] = v[2];		\
+	} while (0)
+
+/** Rotation used in some tests */
+static const mat33_fp_t test_rotation = {
+	{ 0, FLOAT_TO_FP(1), 0},
+	{ FLOAT_TO_FP(-1), 0, 0},
+	{ 0, 0, FLOAT_TO_FP(-1)}
+};
+/** Rotate given vector by test rotation */
+static void rotate_int3v_by_test_rotation(intv3_t v)
+{
+	int16_t t;
+
+	t = v[0];
+	v[0] = -v[1];
+	v[1] = t;
+	v[2] = -v[2];
+}
+
+/** Set emulator accelerometer offset values to intv3_t vector */
+static void set_emul_acc_offset(struct i2c_emul *emul, intv3_t offset)
+{
+	bmi_emul_set_off(emul, BMI_EMUL_ACC_X, offset[0]);
+	bmi_emul_set_off(emul, BMI_EMUL_ACC_Y, offset[1]);
+	bmi_emul_set_off(emul, BMI_EMUL_ACC_Z, offset[2]);
+}
+
+/** Save emulator accelerometer offset values to intv3_t vector */
+static void get_emul_acc_offset(struct i2c_emul *emul, intv3_t offset)
+{
+	offset[0] = bmi_emul_get_off(emul, BMI_EMUL_ACC_X);
+	offset[1] = bmi_emul_get_off(emul, BMI_EMUL_ACC_Y);
+	offset[2] = bmi_emul_get_off(emul, BMI_EMUL_ACC_Z);
+}
+
+/** Set emulator accelerometer values to intv3_t vector */
+static void set_emul_acc(struct i2c_emul *emul, intv3_t acc)
+{
+	bmi_emul_set_value(emul, BMI_EMUL_ACC_X, acc[0]);
+	bmi_emul_set_value(emul, BMI_EMUL_ACC_Y, acc[1]);
+	bmi_emul_set_value(emul, BMI_EMUL_ACC_Z, acc[2]);
+}
+
+/** Set emulator gyroscope offset values to intv3_t vector */
+static void set_emul_gyr_offset(struct i2c_emul *emul, intv3_t offset)
+{
+	bmi_emul_set_off(emul, BMI_EMUL_GYR_X, offset[0]);
+	bmi_emul_set_off(emul, BMI_EMUL_GYR_Y, offset[1]);
+	bmi_emul_set_off(emul, BMI_EMUL_GYR_Z, offset[2]);
+}
+
+/** Save emulator gyroscope offset values to intv3_t vector */
+static void get_emul_gyr_offset(struct i2c_emul *emul, intv3_t offset)
+{
+	offset[0] = bmi_emul_get_off(emul, BMI_EMUL_GYR_X);
+	offset[1] = bmi_emul_get_off(emul, BMI_EMUL_GYR_Y);
+	offset[2] = bmi_emul_get_off(emul, BMI_EMUL_GYR_Z);
+}
+
+/** Set emulator gyroscope values to vector of three int16_t */
+static void set_emul_gyr(struct i2c_emul *emul, intv3_t gyr)
+{
+	bmi_emul_set_value(emul, BMI_EMUL_GYR_X, gyr[0]);
+	bmi_emul_set_value(emul, BMI_EMUL_GYR_Y, gyr[1]);
+	bmi_emul_set_value(emul, BMI_EMUL_GYR_Z, gyr[2]);
+}
+
+/** Convert accelerometer read to units used by emulator */
+static void drv_acc_to_emul(intv3_t drv, int range, intv3_t out)
+{
+	const int scale = MOTION_SCALING_FACTOR / BMI_EMUL_1G;
+
+	out[0] = drv[0] * range / scale;
+	out[1] = drv[1] * range / scale;
+	out[2] = drv[2] * range / scale;
+}
+
+/** Convert gyroscope read to units used by emulator */
+static void drv_gyr_to_emul(intv3_t drv, int range, intv3_t out)
+{
+	const int scale = MOTION_SCALING_FACTOR / BMI_EMUL_125_DEG_S;
+
+	range /= 125;
+	out[0] = drv[0] * range / scale;
+	out[1] = drv[1] * range / scale;
+	out[2] = drv[2] * range / scale;
+}
+
+/** Compare two vectors of intv3_t type */
+static void compare_int3v_f(intv3_t exp_v, intv3_t v, int eps, int line)
+{
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		zassert_within(exp_v[i], v[i], eps,
+			"Expected [%d; %d; %d], got [%d; %d; %d]; line: %d",
+			exp_v[0], exp_v[1], exp_v[2], v[0], v[1], v[2], line);
+	}
+}
+#define compare_int3v_eps(exp_v, v, e) compare_int3v_f(exp_v, v, e, __LINE__)
+#define compare_int3v(exp_v, v) compare_int3v_eps(exp_v, v, V_EPS)
+
+/** Test get accelerometer offset with and without rotation */
+static void test_bmi_acc_get_offset(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	int16_t ret[3];
+	intv3_t ret_v;
+	intv3_t exp_v;
+	int16_t temp;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	/* Set emulator offset */
+	exp_v[0] = BMI_EMUL_1G / 10;
+	exp_v[1] = BMI_EMUL_1G / 20;
+	exp_v[2] = -(int)BMI_EMUL_1G / 30;
+	set_emul_acc_offset(emul, exp_v);
+	/* BMI driver returns value in mg units */
+	exp_v[0] = 1000 / 10;
+	exp_v[1] = 1000 / 20;
+	exp_v[2] = -1000 / 30;
+
+	/* Test fail on offset read */
+	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70);
+	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
+	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
+	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Disable rotation */
+	ms->rot_standard_ref = NULL;
+
+	/* Test get offset without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->get_offset(ms, ret, &temp),
+		      NULL);
+	zassert_equal(temp, (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP, NULL);
+	convert_int3v_int16(ret, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Setup rotation and rotate expected offset */
+	ms->rot_standard_ref = &test_rotation;
+	rotate_int3v_by_test_rotation(exp_v);
+
+	/* Test get offset with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->get_offset(ms, ret, &temp),
+		      NULL);
+	zassert_equal(temp, (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP, NULL);
+	convert_int3v_int16(ret, ret_v);
+	compare_int3v(exp_v, ret_v);
+}
+
+/** Test get gyroscope offset with and without rotation */
+static void test_bmi_gyr_get_offset(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	int16_t ret[3];
+	intv3_t ret_v;
+	intv3_t exp_v;
+	int16_t temp;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Do not fail on read */
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Set emulator offset */
+	exp_v[0] = BMI_EMUL_125_DEG_S / 100;
+	exp_v[1] = BMI_EMUL_125_DEG_S / 200;
+	exp_v[2] = -(int)BMI_EMUL_125_DEG_S / 300;
+	set_emul_gyr_offset(emul, exp_v);
+	/* BMI driver returns value in mdeg/s units */
+	exp_v[0] = 125000 / 100;
+	exp_v[1] = 125000 / 200;
+	exp_v[2] = -125000 / 300;
+
+	/* Test fail on offset read */
+	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70);
+	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 1);
+	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 2);
+	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
+	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Disable rotation */
+	ms->rot_standard_ref = NULL;
+
+	/* Test get offset without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->get_offset(ms, ret, &temp),
+		      NULL);
+	zassert_equal(temp, (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP, NULL);
+	convert_int3v_int16(ret, ret_v);
+	compare_int3v_eps(exp_v, ret_v, 64);
+
+	/* Setup rotation and rotate expected offset */
+	ms->rot_standard_ref = &test_rotation;
+	rotate_int3v_by_test_rotation(exp_v);
+
+	/* Test get offset with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->get_offset(ms, ret, &temp),
+		      NULL);
+	zassert_equal(temp, (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP, NULL);
+	convert_int3v_int16(ret, ret_v);
+	compare_int3v_eps(exp_v, ret_v, 64);
+}
+
+/**
+ * Test set accelerometer offset with and without rotation. Also test behaviour
+ * on I2C error.
+ */
+static void test_bmi_acc_set_offset(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	int16_t input_v[3];
+	int16_t temp = 0;
+	intv3_t ret_v;
+	intv3_t exp_v;
+	uint8_t nv_c;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	/* Test fail on NV CONF register read and write */
+	bmi_emul_set_read_fail_reg(emul, BMI260_NV_CONF);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	bmi_emul_set_write_fail_reg(emul, BMI260_NV_CONF);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test fail on offset write */
+	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Setup NV_CONF register value */
+	bmi_emul_set_reg(emul, BMI260_NV_CONF, 0x7);
+	/* Set input offset */
+	exp_v[0] = BMI_EMUL_1G / 10;
+	exp_v[1] = BMI_EMUL_1G / 20;
+	exp_v[2] = -(int)BMI_EMUL_1G / 30;
+	/* BMI driver accept value in mg units */
+	input_v[0] = 1000 / 10;
+	input_v[1] = 1000 / 20;
+	input_v[2] = -1000 / 30;
+	/* Disable rotation */
+	ms->rot_standard_ref = NULL;
+
+	/* Test set offset without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->set_offset(ms, input_v, temp), NULL);
+	get_emul_acc_offset(emul, ret_v);
+	/*
+	 * Depending on used range, accelerometer values may be up to 6 bits
+	 * more accurate then offset value resolution.
+	 */
+	compare_int3v_eps(exp_v, ret_v, 64);
+	nv_c = bmi_emul_get_reg(emul, BMI260_NV_CONF);
+	/* Only ACC_OFFSET_EN bit should be changed */
+	zassert_equal(0x7 | BMI260_ACC_OFFSET_EN, nv_c,
+		      "Expected 0x%x, got 0x%x",
+		      0x7 | BMI260_ACC_OFFSET_EN, nv_c);
+
+	/* Setup NV_CONF register value */
+	bmi_emul_set_reg(emul, BMI260_NV_CONF, 0);
+	/* Setup rotation and rotate input for set_offset function */
+	ms->rot_standard_ref = &test_rotation;
+	convert_int3v_int16(input_v, ret_v);
+	rotate_int3v_by_test_rotation(ret_v);
+	convert_int3v_int16(ret_v, input_v);
+
+	/* Test set offset with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->set_offset(ms, input_v, temp), NULL);
+	get_emul_acc_offset(emul, ret_v);
+	compare_int3v_eps(exp_v, ret_v, 64);
+	nv_c = bmi_emul_get_reg(emul, BMI260_NV_CONF);
+	/* Only ACC_OFFSET_EN bit should be changed */
+	zassert_equal(BMI260_ACC_OFFSET_EN, nv_c, "Expected 0x%x, got 0x%x",
+		      BMI260_ACC_OFFSET_EN, nv_c);
+}
+
+/**
+ * Test set gyroscope offset with and without rotation. Also test behaviour
+ * on I2C error.
+ */
+static void test_bmi_gyr_set_offset(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	int16_t input_v[3];
+	int16_t temp = 0;
+	intv3_t ret_v;
+	intv3_t exp_v;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Test fail on OFFSET EN GYR98 register read and write */
+	bmi_emul_set_read_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test fail on offset write */
+	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70 + 1);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70 + 2);
+	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Set input offset */
+	exp_v[0] = BMI_EMUL_125_DEG_S / 100;
+	exp_v[1] = BMI_EMUL_125_DEG_S / 200;
+	exp_v[2] = -(int)BMI_EMUL_125_DEG_S / 300;
+	/* BMI driver accept value in mdeg/s units */
+	input_v[0] = 125000 / 100;
+	input_v[1] = 125000 / 200;
+	input_v[2] = -125000 / 300;
+	/* Disable rotation */
+	ms->rot_standard_ref = NULL;
+
+	/* Test set offset without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->set_offset(ms, input_v, temp), NULL);
+	get_emul_gyr_offset(emul, ret_v);
+	/*
+	 * Depending on used range, gyroscope values may be up to 4 bits
+	 * more accurate then offset value resolution.
+	 */
+	compare_int3v_eps(exp_v, ret_v, 32);
+	/* Gyroscope offset should be enabled */
+	zassert_true(bmi_emul_get_reg(emul, BMI260_OFFSET_EN_GYR98) &
+		     BMI260_OFFSET_GYRO_EN, NULL);
+
+	/* Setup rotation and rotate input for set_offset function */
+	ms->rot_standard_ref = &test_rotation;
+	convert_int3v_int16(input_v, ret_v);
+	rotate_int3v_by_test_rotation(ret_v);
+	convert_int3v_int16(ret_v, input_v);
+
+	/* Test set offset with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->set_offset(ms, input_v, temp), NULL);
+	get_emul_gyr_offset(emul, ret_v);
+	compare_int3v_eps(exp_v, ret_v, 32);
+	zassert_true(bmi_emul_get_reg(emul, BMI260_OFFSET_EN_GYR98) &
+		     BMI260_OFFSET_GYRO_EN, NULL);
+}
+
+/**
+ * Try to set accelerometer range and check if expected range was set
+ * in driver and in emulator.
+ */
+static void check_set_acc_range_f(struct i2c_emul *emul,
+				  struct motion_sensor_t *ms, int range,
+				  int rnd, int exp_range, int line)
+{
+	uint8_t exp_range_reg;
+	uint8_t range_reg;
+
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, range, rnd),
+		      "set_range failed; line: %d", line);
+	zassert_equal(exp_range, ms->current_range,
+		      "Expected range %d, got %d; line %d",
+		      exp_range, ms->current_range, line);
+	range_reg = bmi_emul_get_reg(emul, BMI260_ACC_RANGE);
+
+	switch (exp_range) {
+	case 2:
+		exp_range_reg = BMI260_GSEL_2G;
+		break;
+	case 4:
+		exp_range_reg = BMI260_GSEL_4G;
+		break;
+	case 8:
+		exp_range_reg = BMI260_GSEL_8G;
+		break;
+	case 16:
+		exp_range_reg = BMI260_GSEL_16G;
+		break;
+	default:
+		/* Unknown expected range */
+		zassert_unreachable(
+			"Expected range %d not supported by device; line %d",
+			exp_range, line);
+		return;
+	}
+
+	zassert_equal(exp_range_reg, range_reg,
+		      "Expected range reg 0x%x, got 0x%x; line %d",
+		      exp_range_reg, range_reg, line);
+}
+#define check_set_acc_range(emul, ms, range, rnd, exp_range)	\
+	check_set_acc_range_f(emul, ms, range, rnd, exp_range, __LINE__)
+
+/** Test set accelerometer range with and without I2C errors */
+static void test_bmi_acc_set_range(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	int start_range;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	/* Setup starting range, shouldn't be changed on error */
+	start_range = 2;
+	ms->current_range = start_range;
+	bmi_emul_set_reg(emul, BMI260_ACC_RANGE, BMI260_GSEL_2G);
+	/* Setup emulator fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI260_ACC_RANGE);
+
+	/* Test fail on write */
+	zassert_equal(-EIO, ms->drv->set_range(ms, 12, 0), NULL);
+	zassert_equal(start_range, ms->current_range, NULL);
+	zassert_equal(BMI260_GSEL_2G,
+		      bmi_emul_get_reg(emul, BMI260_ACC_RANGE), NULL);
+	zassert_equal(-EIO, ms->drv->set_range(ms, 12, 1), NULL);
+	zassert_equal(start_range, ms->current_range, NULL);
+	zassert_equal(BMI260_GSEL_2G,
+		      bmi_emul_get_reg(emul, BMI260_ACC_RANGE), NULL);
+
+	/* Do not fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test setting range with rounding down */
+	check_set_acc_range(emul, ms, 1, 0, 2);
+	check_set_acc_range(emul, ms, 2, 0, 2);
+	check_set_acc_range(emul, ms, 3, 0, 2);
+	check_set_acc_range(emul, ms, 4, 0, 4);
+	check_set_acc_range(emul, ms, 5, 0, 4);
+	check_set_acc_range(emul, ms, 6, 0, 4);
+	check_set_acc_range(emul, ms, 7, 0, 4);
+	check_set_acc_range(emul, ms, 8, 0, 8);
+	check_set_acc_range(emul, ms, 9, 0, 8);
+	check_set_acc_range(emul, ms, 15, 0, 8);
+	check_set_acc_range(emul, ms, 16, 0, 16);
+	check_set_acc_range(emul, ms, 17, 0, 16);
+
+	/* Test setting range with rounding up */
+	check_set_acc_range(emul, ms, 1, 1, 2);
+	check_set_acc_range(emul, ms, 2, 1, 2);
+	check_set_acc_range(emul, ms, 3, 1, 4);
+	check_set_acc_range(emul, ms, 4, 1, 4);
+	check_set_acc_range(emul, ms, 5, 1, 8);
+	check_set_acc_range(emul, ms, 6, 1, 8);
+	check_set_acc_range(emul, ms, 7, 1, 8);
+	check_set_acc_range(emul, ms, 8, 1, 8);
+	check_set_acc_range(emul, ms, 9, 1, 16);
+	check_set_acc_range(emul, ms, 15, 1, 16);
+	check_set_acc_range(emul, ms, 16, 1, 16);
+	check_set_acc_range(emul, ms, 17, 1, 16);
+}
+
+/**
+ * Try to set gyroscope range and check if expected range was set in driver and
+ * in emulator.
+ */
+static void check_set_gyr_range_f(struct i2c_emul *emul,
+				  struct motion_sensor_t *ms, int range,
+				  int rnd, int exp_range, int line)
+{
+	uint8_t exp_range_reg;
+	uint8_t range_reg;
+
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, range, rnd),
+		      "set_range failed; line: %d", line);
+	zassert_equal(exp_range, ms->current_range,
+		      "Expected range %d, got %d; line %d",
+		      exp_range, ms->current_range, line);
+	range_reg = bmi_emul_get_reg(emul, BMI260_GYR_RANGE);
+
+	switch (exp_range) {
+	case 125:
+		exp_range_reg = BMI260_DPS_SEL_125;
+		break;
+	case 250:
+		exp_range_reg = BMI260_DPS_SEL_250;
+		break;
+	case 500:
+		exp_range_reg = BMI260_DPS_SEL_500;
+		break;
+	case 1000:
+		exp_range_reg = BMI260_DPS_SEL_1000;
+		break;
+	case 2000:
+		exp_range_reg = BMI260_DPS_SEL_2000;
+		break;
+	default:
+		/* Unknown expected range */
+		zassert_unreachable(
+			"Expected range %d not supported by device; line %d",
+			exp_range, line);
+		return;
+	}
+
+	zassert_equal(exp_range_reg, range_reg,
+		      "Expected range reg 0x%x, got 0x%x; line %d",
+		      exp_range_reg, range_reg, line);
+}
+#define check_set_gyr_range(emul, ms, range, rnd, exp_range)	\
+	check_set_gyr_range_f(emul, ms, range, rnd, exp_range, __LINE__)
+
+/** Test set gyroscope range with and without I2C errors */
+static void test_bmi_gyr_set_range(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	int start_range;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Setup starting range, shouldn't be changed on error */
+	start_range = 250;
+	ms->current_range = start_range;
+	bmi_emul_set_reg(emul, BMI260_GYR_RANGE, BMI260_DPS_SEL_250);
+	/* Setup emulator fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI260_GYR_RANGE);
+
+	/* Test fail on write */
+	zassert_equal(-EIO, ms->drv->set_range(ms, 125, 0), NULL);
+	zassert_equal(start_range, ms->current_range, NULL);
+	zassert_equal(BMI260_DPS_SEL_250,
+		      bmi_emul_get_reg(emul, BMI260_GYR_RANGE), NULL);
+	zassert_equal(-EIO, ms->drv->set_range(ms, 125, 1), NULL);
+	zassert_equal(start_range, ms->current_range, NULL);
+	zassert_equal(BMI260_DPS_SEL_250,
+		      bmi_emul_get_reg(emul, BMI260_GYR_RANGE), NULL);
+
+	/* Do not fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test setting range with rounding down */
+	check_set_gyr_range(emul, ms, 1, 0, 125);
+	check_set_gyr_range(emul, ms, 124, 0, 125);
+	check_set_gyr_range(emul, ms, 125, 0, 125);
+	check_set_gyr_range(emul, ms, 126, 0, 125);
+	check_set_gyr_range(emul, ms, 249, 0, 125);
+	check_set_gyr_range(emul, ms, 250, 0, 250);
+	check_set_gyr_range(emul, ms, 251, 0, 250);
+	check_set_gyr_range(emul, ms, 499, 0, 250);
+	check_set_gyr_range(emul, ms, 500, 0, 500);
+	check_set_gyr_range(emul, ms, 501, 0, 500);
+	check_set_gyr_range(emul, ms, 999, 0, 500);
+	check_set_gyr_range(emul, ms, 1000, 0, 1000);
+	check_set_gyr_range(emul, ms, 1001, 0, 1000);
+	check_set_gyr_range(emul, ms, 1999, 0, 1000);
+	check_set_gyr_range(emul, ms, 2000, 0, 2000);
+	check_set_gyr_range(emul, ms, 2001, 0, 2000);
+
+	/* Test setting range with rounding up */
+	check_set_gyr_range(emul, ms, 1, 1, 125);
+	check_set_gyr_range(emul, ms, 124, 1, 125);
+	check_set_gyr_range(emul, ms, 125, 1, 125);
+	check_set_gyr_range(emul, ms, 126, 1, 250);
+	check_set_gyr_range(emul, ms, 249, 1, 250);
+	check_set_gyr_range(emul, ms, 250, 1, 250);
+	check_set_gyr_range(emul, ms, 251, 1, 500);
+	check_set_gyr_range(emul, ms, 499, 1, 500);
+	check_set_gyr_range(emul, ms, 500, 1, 500);
+	check_set_gyr_range(emul, ms, 501, 1, 1000);
+	check_set_gyr_range(emul, ms, 999, 1, 1000);
+	check_set_gyr_range(emul, ms, 1000, 1, 1000);
+	check_set_gyr_range(emul, ms, 1001, 1, 2000);
+	check_set_gyr_range(emul, ms, 1999, 1, 2000);
+	check_set_gyr_range(emul, ms, 2000, 1, 2000);
+	check_set_gyr_range(emul, ms, 2001, 1, 2000);
+}
+
+/** Test get resolution of acclerometer and gyroscope sensor */
+static void test_bmi_get_resolution(void)
+{
+	struct motion_sensor_t *ms;
+
+	/* Test accelerometer */
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	/* Resolution should be always 16 bits */
+	zassert_equal(16, ms->drv->get_resolution(ms), NULL);
+
+	/* Test gyroscope */
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Resolution should be always 16 bits */
+	zassert_equal(16, ms->drv->get_resolution(ms), NULL);
+}
+
+/**
+ * Try to set accelerometer data rate and check if expected rate was set
+ * in driver and in emulator.
+ */
+static void check_set_acc_rate_f(struct i2c_emul *emul,
+				 struct motion_sensor_t *ms, int rate, int rnd,
+				 int exp_rate, int line)
+{
+	uint8_t exp_rate_reg;
+	uint8_t rate_reg;
+	int drv_rate;
+
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, rate, rnd),
+		      "set_data_rate failed; line: %d", line);
+	drv_rate = ms->drv->get_data_rate(ms);
+	zassert_equal(exp_rate, drv_rate, "Expected rate %d, got %d; line %d",
+		      exp_rate, drv_rate, line);
+	rate_reg = bmi_emul_get_reg(emul, BMI260_ACC_CONF);
+	rate_reg &= BMI_ODR_MASK;
+
+	switch (exp_rate) {
+	case 12500:
+		exp_rate_reg = 0x5;
+		break;
+	case 25000:
+		exp_rate_reg = 0x6;
+		break;
+	case 50000:
+		exp_rate_reg = 0x7;
+		break;
+	case 100000:
+		exp_rate_reg = 0x8;
+		break;
+	case 200000:
+		exp_rate_reg = 0x9;
+		break;
+	case 400000:
+		exp_rate_reg = 0xa;
+		break;
+	case 800000:
+		exp_rate_reg = 0xb;
+		break;
+	case 1600000:
+		exp_rate_reg = 0xc;
+		break;
+	default:
+		/* Unknown expected rate */
+		zassert_unreachable(
+			"Expected rate %d not supported by device; line %d",
+			exp_rate, line);
+		return;
+	}
+
+	zassert_equal(exp_rate_reg, rate_reg,
+		      "Expected rate reg 0x%x, got 0x%x; line %d",
+		      exp_rate_reg, rate_reg, line);
+}
+#define check_set_acc_rate(emul, ms, rate, rnd, exp_rate)	\
+	check_set_acc_rate_f(emul, ms, rate, rnd, exp_rate, __LINE__)
+
+/** Test set and get accelerometer rate with and without I2C errors */
+static void test_bmi_acc_rate(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	uint8_t reg_rate;
+	uint8_t pwr_ctrl;
+	int drv_rate;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	/* Test setting rate with rounding down */
+	check_set_acc_rate(emul, ms, 12500, 0, 12500);
+	check_set_acc_rate(emul, ms, 12501, 0, 12500);
+	check_set_acc_rate(emul, ms, 24999, 0, 12500);
+	check_set_acc_rate(emul, ms, 25000, 0, 25000);
+	check_set_acc_rate(emul, ms, 25001, 0, 25000);
+	check_set_acc_rate(emul, ms, 49999, 0, 25000);
+	check_set_acc_rate(emul, ms, 50000, 0, 50000);
+	check_set_acc_rate(emul, ms, 50001, 0, 50000);
+	check_set_acc_rate(emul, ms, 99999, 0, 50000);
+	check_set_acc_rate(emul, ms, 100000, 0, 100000);
+	check_set_acc_rate(emul, ms, 100001, 0, 100000);
+	check_set_acc_rate(emul, ms, 199999, 0, 100000);
+	check_set_acc_rate(emul, ms, 200000, 0, 200000);
+	check_set_acc_rate(emul, ms, 200001, 0, 200000);
+	check_set_acc_rate(emul, ms, 399999, 0, 200000);
+	/*
+	 * We cannot test frequencies from 400000 to 1600000 because
+	 * CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ is set to 250000
+	 */
+
+	/* Test setting rate with rounding up */
+	check_set_acc_rate(emul, ms, 6251, 1, 12500);
+	check_set_acc_rate(emul, ms, 12499, 1, 12500);
+	check_set_acc_rate(emul, ms, 12500, 1, 12500);
+	check_set_acc_rate(emul, ms, 12501, 1, 25000);
+	check_set_acc_rate(emul, ms, 24999, 1, 25000);
+	check_set_acc_rate(emul, ms, 25000, 1, 25000);
+	check_set_acc_rate(emul, ms, 25001, 1, 50000);
+	check_set_acc_rate(emul, ms, 49999, 1, 50000);
+	check_set_acc_rate(emul, ms, 50000, 1, 50000);
+	check_set_acc_rate(emul, ms, 50001, 1, 100000);
+	check_set_acc_rate(emul, ms, 99999, 1, 100000);
+	check_set_acc_rate(emul, ms, 100000, 1, 100000);
+	check_set_acc_rate(emul, ms, 100001, 1, 200000);
+	check_set_acc_rate(emul, ms, 199999, 1, 200000);
+	check_set_acc_rate(emul, ms, 200000, 1, 200000);
+
+	/* Test out of range rate with rounding down */
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 1, 0), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 12499, 0), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 400000, 0), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 2000000, 0), NULL);
+
+	/* Test out of range rate with rounding up */
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 1, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 6250, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 200001, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 400000, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 2000000, 1), NULL);
+
+	/* Current rate shouldn't be changed on error */
+	drv_rate = ms->drv->get_data_rate(ms);
+	reg_rate = bmi_emul_get_reg(emul, BMI260_ACC_CONF);
+
+	/* Setup emulator fail on read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_CONF);
+
+	/* Test fail on read */
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_ACC_CONF), NULL);
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 1), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_ACC_CONF), NULL);
+
+	/* Do not fail on read */
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Setup emulator fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI260_ACC_CONF);
+
+	/* Test fail on write */
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_ACC_CONF), NULL);
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 1), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_ACC_CONF), NULL);
+
+	/* Do not fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test disabling sensor */
+	bmi_emul_set_reg(emul, BMI260_PWR_CTRL,
+			 BMI260_AUX_EN | BMI260_GYR_EN | BMI260_ACC_EN);
+	bmi_emul_set_reg(emul, BMI260_ACC_CONF, BMI260_FILTER_PERF);
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 0, 0), NULL);
+
+	pwr_ctrl = bmi_emul_get_reg(emul, BMI260_PWR_CTRL);
+	reg_rate = bmi_emul_get_reg(emul, BMI260_ACC_CONF);
+	zassert_equal(BMI260_AUX_EN | BMI260_GYR_EN, pwr_ctrl, NULL);
+	zassert_true(!(reg_rate & BMI260_FILTER_PERF), NULL);
+
+	/* Test enabling sensor */
+	bmi_emul_set_reg(emul, BMI260_PWR_CTRL, 0);
+	bmi_emul_set_reg(emul, BMI260_ACC_CONF, 0);
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 50000, 0), NULL);
+
+	pwr_ctrl = bmi_emul_get_reg(emul, BMI260_PWR_CTRL);
+	reg_rate = bmi_emul_get_reg(emul, BMI260_ACC_CONF);
+	zassert_equal(BMI260_ACC_EN, pwr_ctrl, NULL);
+	zassert_true(reg_rate & BMI260_FILTER_PERF, NULL);
+}
+
+/**
+ * Try to set gyroscope data rate and check if expected rate was set
+ * in driver and in emulator.
+ */
+static void check_set_gyr_rate_f(struct i2c_emul *emul,
+				 struct motion_sensor_t *ms, int rate, int rnd,
+				 int exp_rate, int line)
+{
+	uint8_t exp_rate_reg;
+	uint8_t rate_reg;
+	int drv_rate;
+
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, rate, rnd),
+		      "set_data_rate failed; line: %d", line);
+	drv_rate = ms->drv->get_data_rate(ms);
+	zassert_equal(exp_rate, drv_rate, "Expected rate %d, got %d; line %d",
+		      exp_rate, drv_rate, line);
+	rate_reg = bmi_emul_get_reg(emul, BMI260_GYR_CONF);
+	rate_reg &= BMI_ODR_MASK;
+
+	switch (exp_rate) {
+	case 25000:
+		exp_rate_reg = 0x6;
+		break;
+	case 50000:
+		exp_rate_reg = 0x7;
+		break;
+	case 100000:
+		exp_rate_reg = 0x8;
+		break;
+	case 200000:
+		exp_rate_reg = 0x9;
+		break;
+	case 400000:
+		exp_rate_reg = 0xa;
+		break;
+	case 800000:
+		exp_rate_reg = 0xb;
+		break;
+	case 1600000:
+		exp_rate_reg = 0xc;
+		break;
+	case 3200000:
+		exp_rate_reg = 0xc;
+		break;
+	default:
+		/* Unknown expected rate */
+		zassert_unreachable(
+			"Expected rate %d not supported by device; line %d",
+			exp_rate, line);
+		return;
+	}
+
+	zassert_equal(exp_rate_reg, rate_reg,
+		      "Expected rate reg 0x%x, got 0x%x; line %d",
+		      exp_rate_reg, rate_reg, line);
+}
+#define check_set_gyr_rate(emul, ms, rate, rnd, exp_rate)	\
+	check_set_gyr_rate_f(emul, ms, rate, rnd, exp_rate, __LINE__)
+
+/** Test set and get gyroscope rate with and without I2C errors */
+static void test_bmi_gyr_rate(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	uint8_t reg_rate;
+	uint8_t pwr_ctrl;
+	int drv_rate;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Test setting rate with rounding down */
+	check_set_gyr_rate(emul, ms, 25000, 0, 25000);
+	check_set_gyr_rate(emul, ms, 25001, 0, 25000);
+	check_set_gyr_rate(emul, ms, 49999, 0, 25000);
+	check_set_gyr_rate(emul, ms, 50000, 0, 50000);
+	check_set_gyr_rate(emul, ms, 50001, 0, 50000);
+	check_set_gyr_rate(emul, ms, 99999, 0, 50000);
+	check_set_gyr_rate(emul, ms, 100000, 0, 100000);
+	check_set_gyr_rate(emul, ms, 100001, 0, 100000);
+	check_set_gyr_rate(emul, ms, 199999, 0, 100000);
+	check_set_gyr_rate(emul, ms, 200000, 0, 200000);
+	check_set_gyr_rate(emul, ms, 200001, 0, 200000);
+	check_set_gyr_rate(emul, ms, 399999, 0, 200000);
+	/*
+	 * We cannot test frequencies from 400000 to 3200000 because
+	 * CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ is set to 250000
+	 */
+
+	/* Test setting rate with rounding up */
+	check_set_gyr_rate(emul, ms, 12501, 1, 25000);
+	check_set_gyr_rate(emul, ms, 24999, 1, 25000);
+	check_set_gyr_rate(emul, ms, 25000, 1, 25000);
+	check_set_gyr_rate(emul, ms, 25001, 1, 50000);
+	check_set_gyr_rate(emul, ms, 49999, 1, 50000);
+	check_set_gyr_rate(emul, ms, 50000, 1, 50000);
+	check_set_gyr_rate(emul, ms, 50001, 1, 100000);
+	check_set_gyr_rate(emul, ms, 99999, 1, 100000);
+	check_set_gyr_rate(emul, ms, 100000, 1, 100000);
+	check_set_gyr_rate(emul, ms, 100001, 1, 200000);
+	check_set_gyr_rate(emul, ms, 199999, 1, 200000);
+	check_set_gyr_rate(emul, ms, 200000, 1, 200000);
+
+	/* Test out of range rate with rounding down */
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 1, 0), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 24999, 0), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 400000, 0), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 4000000, 0), NULL);
+
+	/* Test out of range rate with rounding up */
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 1, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 12499, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 200001, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 400000, 1), NULL);
+	zassert_equal(EC_RES_INVALID_PARAM,
+		      ms->drv->set_data_rate(ms, 4000000, 1), NULL);
+
+	/* Current rate shouldn't be changed on error */
+	drv_rate = ms->drv->get_data_rate(ms);
+	reg_rate = bmi_emul_get_reg(emul, BMI260_GYR_CONF);
+
+	/* Setup emulator fail on read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_CONF);
+
+	/* Test fail on read */
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_GYR_CONF), NULL);
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 1), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_GYR_CONF), NULL);
+
+	/* Do not fail on read */
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Setup emulator fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI260_GYR_CONF);
+
+	/* Test fail on write */
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_GYR_CONF), NULL);
+	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 1), NULL);
+	zassert_equal(drv_rate, ms->drv->get_data_rate(ms), NULL);
+	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_GYR_CONF), NULL);
+
+	/* Do not fail on write */
+	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test disabling sensor */
+	bmi_emul_set_reg(emul, BMI260_PWR_CTRL,
+			 BMI260_AUX_EN | BMI260_GYR_EN | BMI260_ACC_EN);
+	bmi_emul_set_reg(emul, BMI260_GYR_CONF,
+			 BMI260_FILTER_PERF | BMI260_GYR_NOISE_PERF);
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 0, 0), NULL);
+
+	pwr_ctrl = bmi_emul_get_reg(emul, BMI260_PWR_CTRL);
+	reg_rate = bmi_emul_get_reg(emul, BMI260_GYR_CONF);
+	zassert_equal(BMI260_AUX_EN | BMI260_ACC_EN, pwr_ctrl, NULL);
+	zassert_true(!(reg_rate & (BMI260_FILTER_PERF | BMI260_GYR_NOISE_PERF)),
+		     NULL);
+
+	/* Test enabling sensor */
+	bmi_emul_set_reg(emul, BMI260_PWR_CTRL, 0);
+	bmi_emul_set_reg(emul, BMI260_GYR_CONF, 0);
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 50000, 0), NULL);
+
+	pwr_ctrl = bmi_emul_get_reg(emul, BMI260_PWR_CTRL);
+	reg_rate = bmi_emul_get_reg(emul, BMI260_GYR_CONF);
+	zassert_equal(BMI260_GYR_EN, pwr_ctrl, NULL);
+	zassert_true(reg_rate & (BMI260_FILTER_PERF | BMI260_GYR_NOISE_PERF),
+		     NULL);
+}
+
+/**
+ * Test setting and getting scale in accelerometer and gyroscope sensors.
+ * Correct appling scale to results is checked in "read" test.
+ */
+static void test_bmi_scale(void)
+{
+	struct motion_sensor_t *ms;
+	int16_t ret_scale[3];
+	int16_t exp_scale[3] = {100, 231, 421};
+	int16_t t;
+
+	/* Test accelerometer */
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	zassert_equal(EC_SUCCESS, ms->drv->set_scale(ms, exp_scale, 0), NULL);
+	zassert_equal(EC_SUCCESS, ms->drv->get_scale(ms, ret_scale, &t), NULL);
+
+	zassert_equal(t, (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP, NULL);
+	zassert_equal(exp_scale[0], ret_scale[0], NULL);
+	zassert_equal(exp_scale[1], ret_scale[1], NULL);
+	zassert_equal(exp_scale[2], ret_scale[2], NULL);
+
+	/* Test gyroscope */
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	zassert_equal(EC_SUCCESS, ms->drv->set_scale(ms, exp_scale, 0), NULL);
+	zassert_equal(EC_SUCCESS, ms->drv->get_scale(ms, ret_scale, &t), NULL);
+
+	zassert_equal(t, (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP, NULL);
+	zassert_equal(exp_scale[0], ret_scale[0], NULL);
+	zassert_equal(exp_scale[1], ret_scale[1], NULL);
+	zassert_equal(exp_scale[2], ret_scale[2], NULL);
+}
+
+/** Test reading temperature using accelerometer and gyroscope sensors */
+static void test_bmi_read_temp(void)
+{
+	struct motion_sensor_t *ms_acc, *ms_gyr;
+	struct i2c_emul *emul;
+	int ret_temp;
+	int exp_temp;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms_acc = &motion_sensors[BMI_ACC_SENSOR_ID];
+	ms_gyr = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Setup emulator fail on read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_TEMPERATURE_0);
+	zassert_equal(EC_ERROR_NOT_POWERED,
+		      ms_acc->drv->read_temp(ms_acc, &ret_temp), NULL);
+	zassert_equal(EC_ERROR_NOT_POWERED,
+		      ms_gyr->drv->read_temp(ms_gyr, &ret_temp), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_TEMPERATURE_1);
+	zassert_equal(EC_ERROR_NOT_POWERED,
+		      ms_acc->drv->read_temp(ms_acc, &ret_temp), NULL);
+	zassert_equal(EC_ERROR_NOT_POWERED,
+		      ms_gyr->drv->read_temp(ms_gyr, &ret_temp), NULL);
+	/* Do not fail on read */
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Fail on invalid temperature */
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_0, 0x00);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_1, 0x80);
+	zassert_equal(EC_ERROR_NOT_POWERED,
+		      ms_acc->drv->read_temp(ms_acc, &ret_temp), NULL);
+	zassert_equal(EC_ERROR_NOT_POWERED,
+		      ms_gyr->drv->read_temp(ms_gyr, &ret_temp), NULL);
+
+	/*
+	 * Test correct values. Both motion sensors should return the same
+	 * temperature.
+	 */
+	exp_temp = C_TO_K(23);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_0, 0x00);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_1, 0x00);
+	zassert_equal(EC_SUCCESS, ms_acc->drv->read_temp(ms_acc, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+	zassert_equal(EC_SUCCESS, ms_gyr->drv->read_temp(ms_gyr, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+
+	exp_temp = C_TO_K(87);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_0, 0xff);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_1, 0x7f);
+	zassert_equal(EC_SUCCESS, ms_acc->drv->read_temp(ms_acc, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+	zassert_equal(EC_SUCCESS, ms_gyr->drv->read_temp(ms_gyr, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+
+	exp_temp = C_TO_K(-41);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_0, 0x01);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_1, 0x80);
+	zassert_equal(EC_SUCCESS, ms_acc->drv->read_temp(ms_acc, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+	zassert_equal(EC_SUCCESS, ms_gyr->drv->read_temp(ms_gyr, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+
+	exp_temp = C_TO_K(47);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_0, 0x00);
+	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_1, 0x30);
+	zassert_equal(EC_SUCCESS, ms_acc->drv->read_temp(ms_acc, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+	zassert_equal(EC_SUCCESS, ms_gyr->drv->read_temp(ms_gyr, &ret_temp),
+		      NULL);
+	zassert_equal(exp_temp, ret_temp, NULL);
+}
+
+/** Test reading accelerometer sensor data */
+static void test_bmi_acc_read(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	intv3_t ret_v;
+	intv3_t exp_v;
+	int16_t scale[3] = {MOTION_SENSE_DEFAULT_SCALE,
+			    MOTION_SENSE_DEFAULT_SCALE,
+			    MOTION_SENSE_DEFAULT_SCALE};
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	/* Set offset 0 to simplify test */
+	bmi_emul_set_off(emul, BMI_EMUL_ACC_X, 0);
+	bmi_emul_set_off(emul, BMI_EMUL_ACC_Y, 0);
+	bmi_emul_set_off(emul, BMI_EMUL_ACC_Z, 0);
+
+	/* Fail on read status */
+	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* When not ready, driver should return saved raw value */
+	exp_v[0] = 100;
+	exp_v[1] = 200;
+	exp_v[2] = 300;
+	ms->raw_xyz[0] = exp_v[0];
+	ms->raw_xyz[1] = exp_v[1];
+	ms->raw_xyz[2] = exp_v[2];
+
+	/* Status not ready */
+	bmi_emul_set_reg(emul, BMI260_STATUS, 0);
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	compare_int3v(exp_v, ret_v);
+
+	/* Status only GYR ready */
+	bmi_emul_set_reg(emul, BMI260_STATUS, BMI260_DRDY_GYR);
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	compare_int3v(exp_v, ret_v);
+
+	/* Status ACC ready */
+	bmi_emul_set_reg(emul, BMI260_STATUS, BMI260_DRDY_ACC);
+
+	/* Set input accelerometer values */
+	exp_v[0] = BMI_EMUL_1G / 10;
+	exp_v[1] = BMI_EMUL_1G / 20;
+	exp_v[2] = -(int)BMI_EMUL_1G / 30;
+	set_emul_acc(emul, exp_v);
+	/* Disable rotation */
+	ms->rot_standard_ref = NULL;
+	/* Set scale */
+	zassert_equal(EC_SUCCESS, ms->drv->set_scale(ms, scale, 0), NULL);
+	/* Set range to 2G */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 2, 0), NULL);
+
+	/* Test read without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_acc_to_emul(ret_v, 2, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Set range to 4G */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 4, 0), NULL);
+
+	/* Test read without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_acc_to_emul(ret_v, 4, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Setup rotation and rotate expected vector */
+	ms->rot_standard_ref = &test_rotation;
+	rotate_int3v_by_test_rotation(exp_v);
+	/* Set range to 2G */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 2, 0), NULL);
+
+	/* Test read with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_acc_to_emul(ret_v, 2, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Set range to 4G */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 4, 0), NULL);
+
+	/* Test read with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_acc_to_emul(ret_v, 4, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Fail on read of data registers */
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_X_L_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_X_H_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Y_L_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Y_H_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Z_L_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Z_H_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	ms->rot_standard_ref = NULL;
+}
+
+/** Test reading gyroscope sensor data */
+static void test_bmi_gyr_read(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	intv3_t ret_v;
+	intv3_t exp_v;
+	int16_t scale[3] = {MOTION_SENSE_DEFAULT_SCALE,
+			    MOTION_SENSE_DEFAULT_SCALE,
+			    MOTION_SENSE_DEFAULT_SCALE};
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Set offset 0 to simplify test */
+	bmi_emul_set_off(emul, BMI_EMUL_GYR_X, 0);
+	bmi_emul_set_off(emul, BMI_EMUL_GYR_Y, 0);
+	bmi_emul_set_off(emul, BMI_EMUL_GYR_Z, 0);
+
+	/* Fail on read status */
+	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* When not ready, driver should return saved raw value */
+	exp_v[0] = 100;
+	exp_v[1] = 200;
+	exp_v[2] = 300;
+	ms->raw_xyz[0] = exp_v[0];
+	ms->raw_xyz[1] = exp_v[1];
+	ms->raw_xyz[2] = exp_v[2];
+
+	/* Status not ready */
+	bmi_emul_set_reg(emul, BMI260_STATUS, 0);
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	compare_int3v(exp_v, ret_v);
+
+	/* Status only ACC ready */
+	bmi_emul_set_reg(emul, BMI260_STATUS, BMI260_DRDY_ACC);
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	compare_int3v(exp_v, ret_v);
+
+	/* Status GYR ready */
+	bmi_emul_set_reg(emul, BMI260_STATUS, BMI260_DRDY_GYR);
+
+	/* Set input accelerometer values */
+	exp_v[0] = BMI_EMUL_125_DEG_S / 10;
+	exp_v[1] = BMI_EMUL_125_DEG_S / 20;
+	exp_v[2] = -(int)BMI_EMUL_125_DEG_S / 30;
+	set_emul_gyr(emul, exp_v);
+	/* Disable rotation */
+	ms->rot_standard_ref = NULL;
+	/* Set scale */
+	zassert_equal(EC_SUCCESS, ms->drv->set_scale(ms, scale, 0), NULL);
+	/* Set range to 125°/s */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 125, 0), NULL);
+
+	/* Test read without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_gyr_to_emul(ret_v, 125, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Set range to 1000°/s */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 1000, 0), NULL);
+
+	/* Test read without rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_gyr_to_emul(ret_v, 1000, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Setup rotation and rotate expected vector */
+	ms->rot_standard_ref = &test_rotation;
+	rotate_int3v_by_test_rotation(exp_v);
+	/* Set range to 125°/s */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 125, 0), NULL);
+
+	/* Test read with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_gyr_to_emul(ret_v, 125, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Set range to 1000°/s */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, 1000, 0), NULL);
+
+	/* Test read with rotation */
+	zassert_equal(EC_SUCCESS, ms->drv->read(ms, ret_v), NULL);
+	drv_gyr_to_emul(ret_v, 1000, ret_v);
+	compare_int3v(exp_v, ret_v);
+
+	/* Fail on read of data registers */
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_X_L_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_X_H_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Y_L_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Y_H_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Z_L_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Z_H_G);
+	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
+
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	ms->rot_standard_ref = NULL;
+}
+
+/** Test acceleromtere calibration */
+static void test_bmi_acc_perform_calib(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	intv3_t start_off;
+	intv3_t exp_off;
+	intv3_t ret_off;
+	int range;
+	int rate;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+
+	/* Range and rate cannot change after calibration */
+	range = 4;
+	rate = 50000;
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, range, 0), NULL);
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, rate, 0), NULL);
+
+	/* Set offset 0 */
+	start_off[0] = 0;
+	start_off[1] = 0;
+	start_off[2] = 0;
+	set_emul_acc_offset(emul, start_off);
+
+	/* Set input accelerometer values */
+	exp_off[0] = BMI_EMUL_1G / 10;
+	exp_off[1] = BMI_EMUL_1G / 20;
+	exp_off[2] = BMI_EMUL_1G - (int)BMI_EMUL_1G / 30;
+	set_emul_acc(emul, exp_off);
+
+	/* Expected offset is [-X, -Y, 1G - Z] */
+	exp_off[0] = -exp_off[0];
+	exp_off[1] = -exp_off[1];
+	exp_off[2] = BMI_EMUL_1G - exp_off[2];
+
+	/* Test success on disabling calibration */
+	zassert_equal(EC_SUCCESS, ms->drv->perform_calib(ms, 0), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on rate read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_CONF);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on status read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on data not ready */
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	bmi_emul_set_reg(emul, BMI260_STATUS, 0);
+	zassert_equal(EC_ERROR_TIMEOUT, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Setup data status ready for rest of the test */
+	bmi_emul_set_reg(emul, BMI260_STATUS, BMI260_DRDY_ACC);
+
+	/* Test fail on data read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_X_L_G);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on setting offset */
+	bmi_emul_set_read_fail_reg(emul, BMI260_NV_CONF);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test successful offset compenastion */
+	zassert_equal(EC_SUCCESS, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+	get_emul_acc_offset(emul, ret_off);
+	/*
+	 * Depending on used range, accelerometer values may be up to 6 bits
+	 * more accurate then offset value resolution.
+	 */
+	compare_int3v_eps(exp_off, ret_off, 64);
+}
+
+/** Test gyroscope calibration */
+static void test_bmi_gyr_perform_calib(void)
+{
+	struct motion_sensor_t *ms;
+	struct i2c_emul *emul;
+	intv3_t start_off;
+	intv3_t exp_off;
+	intv3_t ret_off;
+	int range;
+	int rate;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Range and rate cannot change after calibration */
+	range = 125;
+	rate = 50000;
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, range, 0), NULL);
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, rate, 0), NULL);
+
+	/* Set offset 0 */
+	start_off[0] = 0;
+	start_off[1] = 0;
+	start_off[2] = 0;
+	set_emul_gyr_offset(emul, start_off);
+
+	/* Set input accelerometer values */
+	exp_off[0] = BMI_EMUL_125_DEG_S / 100;
+	exp_off[1] = BMI_EMUL_125_DEG_S / 200;
+	exp_off[2] = -(int)BMI_EMUL_125_DEG_S / 300;
+	set_emul_gyr(emul, exp_off);
+
+	/* Expected offset is [-X, -Y, -Z] */
+	exp_off[0] = -exp_off[0];
+	exp_off[1] = -exp_off[1];
+	exp_off[2] = -exp_off[2];
+
+	/* Test success on disabling calibration */
+	zassert_equal(EC_SUCCESS, ms->drv->perform_calib(ms, 0), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on rate read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_CONF);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on status read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on data not ready */
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	bmi_emul_set_reg(emul, BMI260_STATUS, 0);
+	zassert_equal(EC_ERROR_TIMEOUT, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/*
+	 * Setup data status ready for rest of the test. Gyroscope calibration
+	 * should check DRDY_GYR bit, but current driver check only for ACC.
+	 */
+	bmi_emul_set_reg(emul, BMI260_STATUS,
+			 BMI260_DRDY_ACC | BMI260_DRDY_GYR);
+
+	/* Test fail on data read */
+	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_X_L_G);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	/* Test fail on setting offset */
+	bmi_emul_set_read_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
+	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test successful offset compenastion */
+	zassert_equal(EC_SUCCESS, ms->drv->perform_calib(ms, 1), NULL);
+	zassert_equal(range, ms->current_range, NULL);
+	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
+	get_emul_gyr_offset(emul, ret_off);
+	/*
+	 * Depending on used range, gyroscope values may be up to 4 bits
+	 * more accurate then offset value resolution.
+	 */
+	compare_int3v_eps(exp_off, ret_off, 32);
+}
+
+/**
+ * Custom emulatro read function which always return INIT OK status in
+ * INTERNAL STATUS register. Used in init test.
+ */
+static int emul_init_ok(struct i2c_emul *emul, int reg, int byte, void *data)
+{
+	bmi_emul_set_reg(emul, BMI260_INTERNAL_STATUS, BMI260_INIT_OK);
+
+	return 1;
+}
+
+/** Test init function of BMI260 accelerometer and gyroscope sensors */
+static void test_bmi_init(void)
+{
+	struct motion_sensor_t *ms_acc, *ms_gyr;
+	struct i2c_emul *emul;
+	int ret;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms_acc = &motion_sensors[BMI_ACC_SENSOR_ID];
+	ms_gyr = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/*
+	 * Test successful init. It is needed custom function to set value of
+	 * BMI260_INTERNAL_STATUS register, because init function triggers reset
+	 * which clears value set in this register before test.
+	 */
+	bmi_emul_set_read_func(emul, emul_init_ok, NULL);
+	zassert_equal(EC_RES_SUCCESS, ms_acc->drv->init(ms_acc), NULL);
+
+	zassert_equal(EC_RES_SUCCESS, ms_gyr->drv->init(ms_gyr), NULL);
+
+	/* Remove custom emulator read function */
+	bmi_emul_set_read_func(emul, NULL, NULL);
+}
+
+/** Data for custom emulator read function used in FIFO test */
+struct fifo_func_data {
+	uint16_t interrupts;
+};
+
+/**
+ * Custom emulator read function used in FIFO test. It sets interrupt registers
+ * to value passed as additional data. It sets interrupt registers to 0 after
+ * access.
+ */
+static int emul_fifo_func(struct i2c_emul *emul, int reg, int byte, void *data)
+{
+	struct fifo_func_data *d = data;
+
+	if (reg + byte == BMI260_INT_STATUS_0) {
+		bmi_emul_set_reg(emul, BMI260_INT_STATUS_0,
+				 d->interrupts & 0xff);
+		d->interrupts &= 0xff00;
+	} else if (reg + byte == BMI260_INT_STATUS_1) {
+		bmi_emul_set_reg(emul, BMI260_INT_STATUS_1,
+				 (d->interrupts >> 8) & 0xff);
+		d->interrupts &= 0xff;
+	}
+
+	return 1;
+}
+
+/**
+ * Run irq handler on accelerometer sensor and check if committed data in FIFO
+ * match what was set in FIFO frames in emulator.
+ */
+static void check_fifo_f(struct motion_sensor_t *ms_acc,
+			 struct motion_sensor_t *ms_gyr,
+			 struct bmi_emul_frame *frame,
+			 int acc_range, int gyr_range,
+			 int line)
+{
+	struct ec_response_motion_sensor_data vector;
+	struct bmi_emul_frame *f_acc, *f_gyr;
+	uint32_t event = BMI_INT_EVENT;
+	uint16_t size;
+	intv3_t exp_v;
+	intv3_t ret_v;
+
+	/* Find first frame of acc and gyr type */
+	f_acc = frame;
+	while (f_acc != NULL && !(f_acc->type & BMI_EMUL_FRAME_ACC)) {
+		f_acc = f_acc->next;
+	}
+
+	f_gyr = frame;
+	while (f_gyr != NULL && !(f_gyr->type & BMI_EMUL_FRAME_GYR)) {
+		f_gyr = f_gyr->next;
+	}
+
+	/* Read FIFO in driver */
+	zassert_equal(EC_SUCCESS, ms_acc->drv->irq_handler(ms_acc, &event),
+		      NULL);
+
+	/* Read all data committed to FIFO */
+	while (motion_sense_fifo_read(sizeof(vector), 1, &vector, &size)) {
+		/* Ignore timestamp frames */
+		if (vector.flags == MOTIONSENSE_SENSOR_FLAG_TIMESTAMP) {
+			continue;
+		}
+
+		/* Check acclerometer frames */
+		if (ms_acc - motion_sensors == vector.sensor_num) {
+			if (f_acc == NULL) {
+				zassert_unreachable(
+					"Not expected acclerometer data in FIFO, line %d",
+					line);
+			}
+
+			convert_int3v_int16(vector.data, ret_v);
+			drv_acc_to_emul(ret_v, acc_range, ret_v);
+			exp_v[0] = f_acc->acc_x;
+			exp_v[1] = f_acc->acc_y;
+			exp_v[2] = f_acc->acc_z;
+			compare_int3v_f(exp_v, ret_v, V_EPS, line);
+			f_acc = f_acc->next;
+		}
+
+		/* Check gyroscope frames */
+		if (ms_gyr - motion_sensors == vector.sensor_num) {
+			if (f_gyr == NULL) {
+				zassert_unreachable(
+					"Not expected gyroscope data in FIFO, line %d",
+					line);
+			}
+
+			convert_int3v_int16(vector.data, ret_v);
+			drv_gyr_to_emul(ret_v, gyr_range, ret_v);
+			exp_v[0] = f_gyr->gyr_x;
+			exp_v[1] = f_gyr->gyr_y;
+			exp_v[2] = f_gyr->gyr_z;
+			compare_int3v_f(exp_v, ret_v, V_EPS, line);
+			f_gyr = f_gyr->next;
+		}
+	}
+
+	/* Skip frames of different type at the end */
+	while (f_acc != NULL && !(f_acc->type & BMI_EMUL_FRAME_ACC)) {
+		f_acc = f_acc->next;
+	}
+
+	while (f_gyr != NULL && !(f_gyr->type & BMI_EMUL_FRAME_GYR)) {
+		f_gyr = f_gyr->next;
+	}
+
+	/* All frames are readed */
+	zassert_is_null(f_acc, "Not all accelerometer frames are read, line %d",
+			line);
+	zassert_is_null(f_gyr, "Not all gyroscope frames are read, line %d",
+			line);
+}
+#define check_fifo(ms_acc, ms_gyr, frame, acc_range, gyr_range)		\
+	check_fifo_f(ms_acc, ms_gyr, frame, acc_range, gyr_range, __LINE__)
+
+/** Test irq handler of accelerometer sensor */
+static void test_bmi_acc_fifo(void)
+{
+	struct ec_response_motion_sensor_data vector;
+	struct motion_sensor_t *ms, *ms_gyr;
+	struct fifo_func_data func_data;
+	struct bmi_emul_frame f[3];
+	struct i2c_emul *emul;
+	int gyr_range = 125;
+	int acc_range = 2;
+	int event;
+
+	emul = bmi_emul_get(BMI_ORD);
+	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
+	ms_gyr = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Need to be set to collect all data in FIFO */
+	ms->oversampling_ratio = 1;
+	ms_gyr->oversampling_ratio = 1;
+	/* Only BMI event should be handled */
+	event = 0x1234 & ~BMI_INT_EVENT;
+	zassert_equal(EC_ERROR_NOT_HANDLED, ms->drv->irq_handler(ms, &event),
+		      NULL);
+
+	event = BMI_INT_EVENT;
+
+	/* Test fail to read interrupt status registers */
+	bmi_emul_set_read_fail_reg(emul, BMI260_INT_STATUS_0);
+	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI260_INT_STATUS_1);
+	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
+	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+
+	/* Test no interrupt */
+	bmi_emul_set_reg(emul, BMI260_INT_STATUS_0, 0);
+	bmi_emul_set_reg(emul, BMI260_INT_STATUS_1, 0);
+
+	/* Trigger irq handler and check results */
+	check_fifo(ms, ms_gyr, NULL, acc_range, gyr_range);
+
+	/* Set custom function for FIFO test */
+	bmi_emul_set_read_func(emul, emul_fifo_func, &func_data);
+	/* Enable sensor FIFO */
+	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 50000, 0), NULL);
+	/* Set range */
+	zassert_equal(EC_SUCCESS, ms->drv->set_range(ms, acc_range, 0), NULL);
+	zassert_equal(EC_SUCCESS, ms_gyr->drv->set_range(ms_gyr, gyr_range, 0),
+		      NULL);
+	/* Setup single accelerometer frame */
+	f[0].type = BMI_EMUL_FRAME_ACC;
+	f[0].acc_x = BMI_EMUL_1G / 10;
+	f[0].acc_y = BMI_EMUL_1G / 20;
+	f[0].acc_z = -(int)BMI_EMUL_1G / 30;
+	f[0].next = NULL;
+	bmi_emul_append_frame(emul, f);
+	/* Setup interrupts register */
+	func_data.interrupts = BMI260_FWM_INT;
+
+	/* Trigger irq handler and check results */
+	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
+
+	/* Setup second accelerometer frame */
+	f[1].type = BMI_EMUL_FRAME_ACC;
+	f[1].acc_x = -(int)BMI_EMUL_1G / 40;
+	f[1].acc_y = BMI_EMUL_1G / 50;
+	f[1].acc_z = BMI_EMUL_1G / 60;
+	f[0].next = &(f[1]);
+	f[1].next = NULL;
+	bmi_emul_append_frame(emul, f);
+	/* Setup interrupts register */
+	func_data.interrupts = BMI260_FWM_INT;
+
+	/* Trigger irq handler and check results */
+	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
+
+	/* Enable sensor FIFO */
+	zassert_equal(EC_SUCCESS, ms_gyr->drv->set_data_rate(ms_gyr, 50000, 0),
+		      NULL);
+
+	/* Setup first gyroscope frame (after two accelerometer frames) */
+	f[2].type = BMI_EMUL_FRAME_GYR;
+	f[2].gyr_x = -(int)BMI_EMUL_125_DEG_S / 100;
+	f[2].gyr_y = BMI_EMUL_125_DEG_S / 200;
+	f[2].gyr_z = BMI_EMUL_125_DEG_S / 300;
+	f[1].next = &(f[2]);
+	f[2].next = NULL;
+	bmi_emul_append_frame(emul, f);
+	/* Setup interrupts register */
+	func_data.interrupts = BMI260_FWM_INT;
+
+	/* Trigger irq handler and check results */
+	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
+
+	/* Setup second accelerometer frame to by gyroscope frame too */
+	f[1].type |= BMI_EMUL_FRAME_GYR;
+	f[1].gyr_x = -(int)BMI_EMUL_125_DEG_S / 300;
+	f[1].gyr_y = BMI_EMUL_125_DEG_S / 400;
+	f[1].gyr_z = BMI_EMUL_125_DEG_S / 500;
+	bmi_emul_append_frame(emul, f);
+	/* Setup interrupts register */
+	func_data.interrupts = BMI260_FWM_INT;
+
+	/* Trigger irq handler and check results */
+	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
+
+	/* Skip frame should be ignored by driver */
+	bmi_emul_set_skipped_frames(emul, 8);
+	bmi_emul_append_frame(emul, f);
+	/* Setup interrupts register */
+	func_data.interrupts = BMI260_FWM_INT;
+
+	/* Trigger irq handler and check results */
+	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
+
+	/* Setup second frame as an config frame */
+	f[1].type = BMI_EMUL_FRAME_CONFIG;
+	/* Indicate that accelerometer range changed */
+	f[1].config = 0x1;
+	bmi_emul_append_frame(emul, f);
+	/* Setup interrupts register */
+	func_data.interrupts = BMI260_FWM_INT;
+
+	/* Trigger irq handler and check results */
+	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
+
+	/* Remove custom emulator read function */
+	bmi_emul_set_read_func(emul, NULL, NULL);
+}
+
+/** Test irq handler of gyroscope sensor */
+static void test_bmi_gyr_fifo(void)
+{
+	struct motion_sensor_t *ms;
+	uint32_t event;
+
+	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
+
+	/* Interrupt shuldn't be triggered for gyroscope motion sense */
+	event = BMI_INT_EVENT;
+	zassert_equal(EC_ERROR_NOT_HANDLED, ms->drv->irq_handler(ms, &event),
+		      NULL);
+}
+
+void test_suite_bmi260(void)
+{
+	ztest_test_suite(bmi260,
+			 ztest_user_unit_test(test_bmi_acc_get_offset),
+			 ztest_user_unit_test(test_bmi_gyr_get_offset),
+			 ztest_user_unit_test(test_bmi_acc_set_offset),
+			 ztest_user_unit_test(test_bmi_gyr_set_offset),
+			 ztest_user_unit_test(test_bmi_acc_set_range),
+			 ztest_user_unit_test(test_bmi_gyr_set_range),
+			 ztest_user_unit_test(test_bmi_get_resolution),
+			 ztest_user_unit_test(test_bmi_acc_rate),
+			 ztest_user_unit_test(test_bmi_gyr_rate),
+			 ztest_user_unit_test(test_bmi_scale),
+			 ztest_user_unit_test(test_bmi_read_temp),
+			 ztest_user_unit_test(test_bmi_acc_read),
+			 ztest_user_unit_test(test_bmi_gyr_read),
+			 ztest_user_unit_test(test_bmi_acc_perform_calib),
+			 ztest_user_unit_test(test_bmi_gyr_perform_calib),
+			 ztest_user_unit_test(test_bmi_init),
+			 ztest_user_unit_test(test_bmi_acc_fifo),
+			 ztest_user_unit_test(test_bmi_gyr_fifo));
+	ztest_run_test_suite(bmi260);
+}
diff --git a/zephyr/test/drivers/src/main.c b/zephyr/test/drivers/src/main.c
index dcff8301e4..906d05f36f 100644
--- a/zephyr/test/drivers/src/main.c
+++ b/zephyr/test/drivers/src/main.c
@@ -15,6 +15,7 @@ extern void test_suite_temp_sensor(void);
 extern void test_suite_bma2x2(void);
 extern void test_suite_bc12(void);
 extern void test_suite_ppc(void);
+extern void test_suite_bmi260(void);
 
 void test_main(void)
 {
@@ -31,4 +32,5 @@ void test_main(void)
 	test_suite_bma2x2();
 	test_suite_bc12();
 	test_suite_ppc();
+	test_suite_bmi260();
 }