diff options
-rw-r--r-- | zephyr/test/drivers/overlay.dts | 74 | ||||
-rw-r--r-- | zephyr/test/drivers/prj.conf | 6 | ||||
-rw-r--r-- | zephyr/test/drivers/src/bmi260.c | 1838 | ||||
-rw-r--r-- | zephyr/test/drivers/src/main.c | 2 |
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(); } |