blob: b5c96d298171754204045818e53c2fbb475e3635 [file] [log] [blame]
/*
* 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)