diff options
Diffstat (limited to 'zephyr/emul/emul_lis2dw12.c')
| -rw-r--r-- | zephyr/emul/emul_lis2dw12.c | 158 |
1 files changed, 158 insertions, 0 deletions
diff --git a/zephyr/emul/emul_lis2dw12.c b/zephyr/emul/emul_lis2dw12.c index 07fba6091f..e14c184504 100644 --- a/zephyr/emul/emul_lis2dw12.c +++ b/zephyr/emul/emul_lis2dw12.c @@ -29,10 +29,20 @@ struct lis2dw12_emul_data { struct i2c_common_emul_data common; /** Emulated who-am-i register */ uint8_t who_am_i_reg; + /** Emulated ctrl1 register */ + uint8_t ctrl1_reg; /** Emulated ctrl2 register */ uint8_t ctrl2_reg; + /** Emulated ctrl3 register */ + uint8_t ctrl3_reg; + /** Emulated ctrl6 register */ + uint8_t ctrl6_reg; + /** Emulated status register */ + uint8_t status_reg; /** Soft reset count */ uint32_t soft_reset_count; + /** Current X, Y, and Z output data registers */ + int16_t accel_data[3]; }; struct lis2dw12_emul_cfg { @@ -59,8 +69,14 @@ void lis2dw12_emul_reset(const struct emul *emul) i2c_common_emul_set_read_func(i2c_emul, NULL, NULL); i2c_common_emul_set_write_func(i2c_emul, NULL, NULL); data->who_am_i_reg = LIS2DW12_WHO_AM_I; + data->ctrl1_reg = 0; data->ctrl2_reg = 0; + data->ctrl3_reg = 0; + data->ctrl6_reg = 0; + data->status_reg = 0; data->soft_reset_count = 0; + + memset(data->accel_data, 0, sizeof(data->accel_data)); } void lis2dw12_emul_set_who_am_i(const struct emul *emul, uint8_t who_am_i) @@ -87,16 +103,103 @@ static int lis2dw12_emul_read_byte(struct i2c_emul *emul, int reg, uint8_t *val, __ASSERT_NO_MSG(bytes == 0); *val = data->who_am_i_reg; break; + case LIS2DW12_CTRL1_ADDR: + __ASSERT_NO_MSG(bytes == 0); + *val = data->ctrl1_reg; + break; case LIS2DW12_CTRL2_ADDR: __ASSERT_NO_MSG(bytes == 0); *val = data->ctrl2_reg; break; + case LIS2DW12_CTRL3_ADDR: + __ASSERT_NO_MSG(bytes == 0); + *val = data->ctrl3_reg; + break; + case LIS2DW12_CTRL6_ADDR: + __ASSERT_NO_MSG(bytes == 0); + *val = data->ctrl6_reg; + break; + case LIS2DW12_STATUS_REG: + __ASSERT_NO_MSG(bytes == 0); + *val = data->status_reg; + break; + case LIS2DW12_OUT_X_L_ADDR: + case LIS2DW12_OUT_X_H_ADDR: + case LIS2DW12_OUT_Y_L_ADDR: + case LIS2DW12_OUT_Y_H_ADDR: + case LIS2DW12_OUT_Z_L_ADDR: + case LIS2DW12_OUT_Z_H_ADDR: + /* Allow multi-byte reads within this range of registers. + * `bytes` is actually an offset past the starting register + * `reg`. + */ + + __ASSERT_NO_MSG(LIS2DW12_OUT_X_L_ADDR + bytes <= + LIS2DW12_OUT_Z_H_ADDR); + + /* 0 is OUT_X_L_ADDR .. 5 is OUT_Z_H_ADDR */ + int offset_into_odrs = reg - LIS2DW12_OUT_X_L_ADDR + bytes; + + /* Which of the 3 channels we're reading. 0 = X, 1 = Y, 2 = Z */ + int channel = offset_into_odrs / 2; + + if (offset_into_odrs % 2 == 0) { + /* Get the LSB (L reg) */ + *val = data->accel_data[channel] & 0xFF; + } else { + /* Get the MSB (H reg) */ + *val = (data->accel_data[channel] >> 8) & 0xFF; + } + break; default: + __ASSERT(false, "No read handler for register 0x%02x", reg); return -EINVAL; } return 0; } +uint8_t lis2dw12_emul_peek_reg(struct i2c_emul *emul, int reg) +{ + __ASSERT(emul, "emul is NULL"); + + uint8_t val; + int rv; + + rv = lis2dw12_emul_read_byte(emul, reg, &val, 0); + __ASSERT(rv == 0, "Read function returned non-zero: %d", rv); + + return val; +} + +uint8_t lis2dw12_emul_peek_odr(struct i2c_emul *emul) +{ + __ASSERT(emul, "emul is NULL"); + + uint8_t reg = lis2dw12_emul_peek_reg(emul, LIS2DW12_ACC_ODR_ADDR); + + return (reg & LIS2DW12_ACC_ODR_MASK) >> + __builtin_ctz(LIS2DW12_ACC_ODR_MASK); +} + +uint8_t lis2dw12_emul_peek_mode(struct i2c_emul *emul) +{ + __ASSERT(emul, "emul is NULL"); + + uint8_t reg = lis2dw12_emul_peek_reg(emul, LIS2DW12_ACC_MODE_ADDR); + + return (reg & LIS2DW12_ACC_MODE_MASK) >> + __builtin_ctz(LIS2DW12_ACC_MODE_MASK); +} + +uint8_t lis2dw12_emul_peek_lpmode(struct i2c_emul *emul) +{ + __ASSERT(emul, "emul is NULL"); + + uint8_t reg = lis2dw12_emul_peek_reg(emul, LIS2DW12_ACC_LPMODE_ADDR); + + return (reg & LIS2DW12_ACC_LPMODE_MASK); +} + static int lis2dw12_emul_write_byte(struct i2c_emul *emul, int reg, uint8_t val, int bytes) { @@ -106,6 +209,9 @@ static int lis2dw12_emul_write_byte(struct i2c_emul *emul, int reg, uint8_t val, case LIS2DW12_WHO_AM_I_REG: LOG_ERR("Can't write to who-am-i register"); return -EINVAL; + case LIS2DW12_CTRL1_ADDR: + data->ctrl1_reg = val; + break; case LIS2DW12_CTRL2_ADDR: __ASSERT_NO_MSG(bytes == 1); if ((val & LIS2DW12_SOFT_RESET_MASK) != 0) { @@ -114,7 +220,28 @@ static int lis2dw12_emul_write_byte(struct i2c_emul *emul, int reg, uint8_t val, } data->ctrl2_reg = val & ~LIS2DW12_SOFT_RESET_MASK; break; + case LIS2DW12_CTRL3_ADDR: + data->ctrl3_reg = val; + break; + case LIS2DW12_CTRL6_ADDR: + data->ctrl6_reg = val; + break; + case LIS2DW12_STATUS_REG: + __ASSERT(false, + "Attempt to write to read-only status register"); + return -EINVAL; + case LIS2DW12_OUT_X_L_ADDR: + case LIS2DW12_OUT_X_H_ADDR: + case LIS2DW12_OUT_Y_L_ADDR: + case LIS2DW12_OUT_Y_H_ADDR: + case LIS2DW12_OUT_Z_L_ADDR: + case LIS2DW12_OUT_Z_H_ADDR: + __ASSERT(false, + "Attempt to write to data output register 0x%02x", + reg); + return -EINVAL; default: + __ASSERT(false, "No write handler for register 0x%02x", reg); return -EINVAL; } return 0; @@ -137,6 +264,37 @@ static int emul_lis2dw12_init(const struct emul *emul, return i2c_emul_register(parent, emul->dev_label, &data->common.emul); } +int lis2dw12_emul_set_accel_reading(const struct emul *emul, intv3_t reading) +{ + __ASSERT(emul, "emul is NULL"); + struct lis2dw12_emul_data *data = emul->data; + + for (int i = X; i <= Z; i++) { + /* Ensure we fit in a 14-bit signed integer */ + if (reading[i] < LIS2DW12_SAMPLE_MIN || + reading[i] > LIS2DW12_SAMPLE_MAX) { + return -EINVAL; + } + /* Readings are left-aligned, so shift over by 2 */ + data->accel_data[i] = reading[i] * 4; + } + + /* Set the DRDY (data ready) bit */ + data->status_reg |= LIS2DW12_STS_DRDY_UP; + + return 0; +} + +void lis2dw12_emul_clear_accel_reading(const struct emul *emul) +{ + __ASSERT(emul, "emul is NULL"); + struct lis2dw12_emul_data *data = emul->data; + + /* Zero out the registers and reset DRDY bit */ + memset(data->accel_data, 0, sizeof(data->accel_data)); + data->status_reg &= ~LIS2DW12_STS_DRDY_UP; +} + #define INIT_LIS2DW12(n) \ static struct lis2dw12_emul_data lis2dw12_emul_data_##n = { \ .common = { \ |