| /* |
| * Copyright (c) 2024 Google LLC |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| #include "bma4xx.h" |
| #include "bma4xx_emul.h" |
| |
| #include "zephyr/sys/util.h" |
| |
| #include <errno.h> |
| #include <stdint.h> |
| #include <string.h> |
| |
| #include <zephyr/device.h> |
| #include <zephyr/drivers/emul.h> |
| #include <zephyr/drivers/emul_sensor.h> |
| #include <zephyr/drivers/sensor.h> |
| #include <zephyr/drivers/i2c.h> |
| #include <zephyr/drivers/i2c_emul.h> |
| #include <zephyr/logging/log.h> |
| #include <zephyr/sys/math_extras.h> |
| |
| #define DT_DRV_COMPAT bosch_bma4xx |
| |
| LOG_MODULE_REGISTER(bma4xx_emul, CONFIG_SENSOR_LOG_LEVEL); |
| |
| struct bma4xx_emul_data { |
| /* Holds register data. */ |
| uint8_t regs[BMA4XX_NUM_REGS]; |
| }; |
| |
| struct bma4xx_emul_cfg { |
| }; |
| |
| void bma4xx_emul_set_reg(const struct emul *target, uint8_t reg_addr, const uint8_t *val, |
| size_t count) |
| { |
| struct bma4xx_emul_data *data = target->data; |
| |
| __ASSERT_NO_MSG(reg_addr + count <= BMA4XX_NUM_REGS); |
| memcpy(data->regs + reg_addr, val, count); |
| } |
| |
| void bma4xx_emul_get_reg(const struct emul *target, uint8_t reg_addr, uint8_t *val, size_t count) |
| { |
| struct bma4xx_emul_data *data = target->data; |
| |
| __ASSERT_NO_MSG(reg_addr + count <= BMA4XX_NUM_REGS); |
| memcpy(val, data->regs + reg_addr, count); |
| } |
| |
| uint8_t bma4xx_emul_get_interrupt_config(const struct emul *target, uint8_t *int1_io_ctrl, |
| bool *latched_mode) |
| { |
| struct bma4xx_emul_data *data = target->data; |
| |
| *int1_io_ctrl = data->regs[BMA4XX_REG_INT1_IO_CTRL]; |
| *latched_mode = data->regs[BMA4XX_REG_INT_LATCH]; |
| return data->regs[BMA4XX_REG_INT_MAP_DATA]; |
| } |
| |
| static int bma4xx_emul_read_byte(const struct emul *target, int reg, uint8_t *val, int bytes) |
| { |
| bma4xx_emul_get_reg(target, reg, val, bytes); |
| |
| return 0; |
| } |
| |
| static int bma4xx_emul_write_byte(const struct emul *target, int reg, uint8_t val, int bytes) |
| { |
| struct bma4xx_emul_data *data = target->data; |
| |
| if (bytes != 1) { |
| LOG_ERR("multi-byte writes are not supported"); |
| return -ENOTSUP; |
| } |
| |
| switch (reg) { |
| case BMA4XX_REG_ACCEL_CONFIG: |
| if ((val & 0xF0) != 0xA0) { |
| LOG_ERR("unsupported acc_bwp/acc_perf_mode: %#x", val); |
| return -EINVAL; |
| } |
| data->regs[reg] = val & GENMASK(1, 0); |
| return 0; |
| case BMA4XX_REG_ACCEL_RANGE: |
| if ((val & GENMASK(1, 0)) != val) { |
| LOG_ERR("reserved bits set in ACC_RANGE write: %#x", val); |
| return -EINVAL; |
| } |
| data->regs[reg] = val; |
| return 0; |
| case BMA4XX_REG_FIFO_CONFIG_1: |
| if (val & ~BMA4XX_FIFO_ACC_EN) { |
| LOG_ERR("unsupported bits set in FIFO_CONFIG_1" |
| " write: %#x", |
| val); |
| return -EINVAL; |
| } |
| data->regs[reg] = (val & BMA4XX_FIFO_ACC_EN) != 0; |
| return 0; |
| case BMA4XX_REG_INT1_IO_CTRL: |
| data->regs[reg] = val; |
| return 0; |
| case BMA4XX_REG_INT_LATCH: |
| if ((val & ~1) != 0) { |
| LOG_ERR("reserved bits set in INT_LATCH: %#x", val); |
| return -EINVAL; |
| } |
| data->regs[reg] = (val & 1) == 1; |
| return 0; |
| case BMA4XX_REG_INT_MAP_DATA: |
| data->regs[reg] = val; |
| return 0; |
| case BMA4XX_REG_NV_CONFIG: |
| if (val & GENMASK(7, 4)) { |
| LOG_ERR("reserved bits set in NV_CONF write: %#x", val); |
| return -EINVAL; |
| } |
| data->regs[reg] = val; |
| return 0; |
| case BMA4XX_REG_OFFSET_0: |
| case BMA4XX_REG_OFFSET_1: |
| case BMA4XX_REG_OFFSET_2: |
| data->regs[reg] = val; |
| return 0; |
| case BMA4XX_REG_POWER_CTRL: |
| if ((val & ~BMA4XX_BIT_ACC_EN) != 0) { |
| LOG_ERR("unhandled bits in POWER_CTRL write: %#x", val); |
| return -ENOTSUP; |
| } |
| data->regs[reg] = (val & BMA4XX_BIT_ACC_EN) != 0; |
| return 0; |
| case BMA4XX_REG_CMD: |
| if (val == BMA4XX_CMD_FIFO_FLUSH) { /* fifo_flush */ |
| data->regs[BMA4XX_REG_FIFO_DATA] = 0; |
| data->regs[BMA4XX_REG_FIFO_LENGTH_0] = 0; |
| data->regs[BMA4XX_REG_FIFO_LENGTH_1] = 0; |
| return 0; |
| } |
| break; |
| } |
| |
| LOG_WRN("unhandled I2C write to register %#x", reg); |
| return -ENOTSUP; |
| } |
| |
| static int bma4xx_emul_init(const struct emul *target, const struct device *parent) |
| { |
| struct bma4xx_emul_data *data = target->data; |
| |
| data->regs[BMA4XX_REG_CHIP_ID] = BMA4XX_CHIP_ID_BMA422; |
| data->regs[BMA4XX_REG_ACCEL_RANGE] = BMA4XX_RANGE_4G; |
| |
| return 0; |
| } |
| |
| static int bma4xx_emul_transfer_i2c(const struct emul *target, struct i2c_msg *msgs, int num_msgs, |
| int addr) |
| { |
| __ASSERT_NO_MSG(msgs && num_msgs); |
| if (num_msgs != 2) { |
| return 0; |
| } |
| |
| i2c_dump_msgs_rw(target->dev, msgs, num_msgs, addr, false); |
| |
| if (msgs->flags & I2C_MSG_READ) { |
| LOG_ERR("Unexpected read"); |
| return -EIO; |
| } |
| if (msgs->len != 1) { |
| LOG_ERR("Unexpected msg0 length %d", msgs->len); |
| return -EIO; |
| } |
| |
| uint32_t reg = msgs->buf[0]; |
| |
| msgs++; |
| if (msgs->flags & I2C_MSG_READ) { |
| /* Reads from regs in target->data to msgs->buf */ |
| bma4xx_emul_read_byte(target, reg, msgs->buf, msgs->len); |
| } else { |
| /* Writes msgs->buf[0] to regs in target->data */ |
| bma4xx_emul_write_byte(target, reg, msgs->buf[0], msgs->len); |
| } |
| |
| return 0; |
| }; |
| |
| void bma4xx_emul_set_accel_data(const struct emul *target, q31_t value, int8_t shift, int8_t reg) |
| { |
| |
| struct bma4xx_emul_data *data = target->data; |
| |
| /* floor(9.80665 * 2^(31−4)) q31_t in (-2^4, 2^4) => range_g = shift of 4 */ |
| int64_t g = 1316226282; |
| int64_t range_g = 4; |
| |
| int64_t intermediate, unshifted; |
| int16_t reg_val; |
| |
| /* 0x00 -> +/-2g; 0x01 -> +/-4g; 0x02 -> +/-8g; 0x03 -> +/- 16g; */ |
| int64_t accel_range = (2 << data->regs[BMA4XX_REG_ACCEL_RANGE]); |
| |
| unshifted = shift < 0 ? ((int64_t)value >> -shift) : ((int64_t)value << shift); |
| |
| intermediate = (unshifted * BIT(11)) / (g << range_g); |
| |
| intermediate /= accel_range; |
| |
| reg_val = CLAMP(intermediate, -2048, 2047); |
| |
| /* lsb register uses top 12 of 16 bits to hold value so shift by 4 to fill it */ |
| data->regs[reg] = FIELD_GET(GENMASK(3, 0), reg_val) << 4; |
| data->regs[reg + 1] = FIELD_GET(GENMASK(11, 4), reg_val); |
| } |
| |
| static int bma4xx_emul_backend_set_channel(const struct emul *target, enum sensor_channel ch, |
| const q31_t *value, int8_t shift) |
| { |
| |
| if (!target || !target->data) { |
| return -EINVAL; |
| } |
| |
| struct bma4xx_emul_data *data = target->data; |
| |
| switch (ch) { |
| case SENSOR_CHAN_ACCEL_X: |
| bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_8); |
| break; |
| case SENSOR_CHAN_ACCEL_Y: |
| bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_10); |
| break; |
| case SENSOR_CHAN_ACCEL_Z: |
| bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_12); |
| break; |
| case SENSOR_CHAN_ACCEL_XYZ: |
| bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_8); |
| bma4xx_emul_set_accel_data(target, value[1], shift, BMA4XX_REG_DATA_10); |
| bma4xx_emul_set_accel_data(target, value[2], shift, BMA4XX_REG_DATA_12); |
| default: |
| return -ENOTSUP; |
| } |
| |
| /* Set data ready flag */ |
| data->regs[BMA4XX_REG_INT_STAT_1] |= BMA4XX_ACC_DRDY_INT; |
| |
| return 0; |
| } |
| |
| static int bma4xx_emul_backend_get_sample_range(const struct emul *target, enum sensor_channel ch, |
| q31_t *lower, q31_t *upper, q31_t *epsilon, |
| int8_t *shift) |
| { |
| if (!lower || !upper || !epsilon || !shift) { |
| return -EINVAL; |
| } |
| |
| switch (ch) { |
| case SENSOR_CHAN_ACCEL_X: |
| case SENSOR_CHAN_ACCEL_Y: |
| case SENSOR_CHAN_ACCEL_Z: |
| case SENSOR_CHAN_ACCEL_XYZ: |
| break; |
| default: |
| return -ENOTSUP; |
| } |
| |
| struct bma4xx_emul_data *data = target->data; |
| |
| switch (data->regs[BMA4XX_REG_ACCEL_RANGE]) { |
| case BMA4XX_RANGE_2G: |
| *shift = 5; |
| *upper = (q31_t)(2 * 9.80665 * BIT(31 - 5)); |
| *lower = -*upper; |
| /* (1 << (31 - shift) >> 12) * 2 (where 2 comes from 2g range) */ |
| *epsilon = BIT(31 - 5 - 12 + 1); |
| break; |
| case BMA4XX_RANGE_4G: |
| *shift = 6; |
| *upper = (q31_t)(4 * 9.80665 * BIT(31 - 6)); |
| *lower = -*upper; |
| /* (1 << (31 - shift) >> 12) * 4 (where 4 comes from 4g range) */ |
| *epsilon = BIT(31 - 6 - 12 + 2); |
| break; |
| case BMA4XX_RANGE_8G: |
| *shift = 7; |
| *upper = (q31_t)(8 * 9.80665 * BIT(31 - 7)); |
| *lower = -*upper; |
| /* (1 << (31 - shift) >> 12) * 8 (where 8 comes from 8g range) */ |
| *epsilon = BIT(31 - 7 - 12 + 3); |
| break; |
| case BMA4XX_RANGE_16G: |
| *shift = 8; |
| *upper = (q31_t)(16 * 9.80665 * BIT(31 - 8)); |
| *lower = -*upper; |
| /* (1 << (31 - shift) >> 12) * 16 (where 16 comes from 16g range) */ |
| *epsilon = BIT(31 - 8 - 12 + 4); |
| break; |
| default: |
| return -ENOTSUP; |
| } |
| |
| return 0; |
| } |
| |
| static const struct emul_sensor_backend_api bma4xx_emul_sensor_backend_api = { |
| .set_channel = bma4xx_emul_backend_set_channel, |
| .get_sample_range = bma4xx_emul_backend_get_sample_range, |
| }; |
| |
| static struct i2c_emul_api bma4xx_emul_api_i2c = { |
| .transfer = bma4xx_emul_transfer_i2c, |
| }; |
| |
| #define INIT_BMA4XX(n) \ |
| static struct bma4xx_emul_data bma4xx_emul_data_##n = {}; \ |
| static const struct bma4xx_emul_cfg bma4xx_emul_cfg_##n = {}; \ |
| EMUL_DT_INST_DEFINE(n, bma4xx_emul_init, &bma4xx_emul_data_##n, &bma4xx_emul_cfg_##n, \ |
| &bma4xx_emul_api_i2c, &bma4xx_emul_sensor_backend_api); |
| |
| DT_INST_FOREACH_STATUS_OKAY(INIT_BMA4XX) |