Skip to content

Commit

Permalink
drivers: sensor: bma4xx: Add bma4xx emulator
Browse files Browse the repository at this point in the history 
Add emulator for bma4xx sensor.

Signed-off-by: Kshitij Shah <tij@google.com>
  • Loading branch information
@KshitijShah-GitHub @dleach02
KshitijShah-GitHub authored and dleach02 committed Feb 2, 2024
1 parent a8a0eb5 commit 24ebebd
Show file tree
Hide file tree
Showing 3 changed files with 371 additions and 1 deletion.
15 changes: 14 additions & 1 deletion drivers/sensor/bma4xx/bma4xx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@
#define BMA4XX_REG_HIGH_G_OUT (0x1F)
#define BMA4XX_REG_TEMPERATURE (0x22)
#define BMA4XX_REG_FIFO_LENGTH_0 (0x24)
#define BMA4XX_REG_FIFO_LENGTH_1 (0x25)
#define BMA4XX_REG_FIFO_DATA (0x26)
#define BMA4XX_REG_ACTIVITY_OUT (0x27)
#define BMA4XX_REG_ORIENTATION_OUT (0x28)
Expand All @@ -64,7 +65,7 @@
#define BMA4XX_REG_AUX_WR_DATA (0x4F)
#define BMA4XX_REG_INT1_IO_CTRL (0x53)
#define BMA4XX_REG_INT2_IO_CTRL (0x54)
#define BMA4XX_REG_INTR_LATCH (0x55)
#define BMA4XX_REG_INT_LATCH (0x55)
#define BMA4XX_REG_INT_MAP_1 (0x56)
#define BMA4XX_REG_INT_MAP_2 (0x57)
#define BMA4XX_REG_INT_MAP_DATA (0x58)
Expand Down Expand Up @@ -98,6 +99,15 @@

#define BMA4XX_BIT_ACC_EN BIT(2)

/* INT_STATUS_1 accelerometer data ready to interrupt */
#define BMA4XX_ACC_DRDY_INT BIT(7)

/* CMD: Clears all data in FIFO, does not change FIFO_CONFIG and FIFO_DOWNS register */
#define BMA4XX_CMD_FIFO_FLUSH (0xB0)

/* FIFO_CONFIG_1 enable: Store Accelerometer data in FIFO (all 3 axes) */
#define BMA4XX_FIFO_ACC_EN BIT(6)

/* Bandwidth parameters */
#define BMA4XX_BWP_OSR4_AVG1 (0x0)
#define BMA4XX_BWP_OSR2_AVG2 (0x1)
Expand Down Expand Up @@ -153,6 +163,9 @@
/* Range is -104C to 150C. Use shift of 8 (+/-256) */
#define BMA4XX_TEMP_SHIFT (8)

/* The total number of used registers specified in bma422 datasheet is 7E */
#define BMA4XX_NUM_REGS 0x7E

/*
* Types
*/
Expand Down
324 changes: 324 additions & 0 deletions drivers/sensor/bma4xx/bma4xx_emul.c
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,324 @@
/*
* 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)
Loading

0 comments on commit 24ebebd

Please sign in to comment.