1 /*
2 * Copyright (c) 2024 Google LLC
3 * SPDX-License-Identifier: Apache-2.0
4 */
5 #include "bma4xx.h"
6 #include "bma4xx_emul.h"
7
8 #include "zephyr/sys/util.h"
9
10 #include <errno.h>
11 #include <stdint.h>
12 #include <string.h>
13
14 #include <zephyr/device.h>
15 #include <zephyr/drivers/emul.h>
16 #include <zephyr/drivers/emul_sensor.h>
17 #include <zephyr/drivers/sensor.h>
18 #include <zephyr/drivers/i2c.h>
19 #include <zephyr/drivers/i2c_emul.h>
20 #include <zephyr/logging/log.h>
21 #include <zephyr/sys/math_extras.h>
22
23 #define DT_DRV_COMPAT bosch_bma4xx
24
25 LOG_MODULE_REGISTER(bma4xx_emul, CONFIG_SENSOR_LOG_LEVEL);
26
27 struct bma4xx_emul_data {
28 /* Holds register data. */
29 uint8_t regs[BMA4XX_NUM_REGS];
30 };
31
32 struct bma4xx_emul_cfg {
33 };
34
bma4xx_emul_set_reg(const struct emul * target,uint8_t reg_addr,const uint8_t * val,size_t count)35 void bma4xx_emul_set_reg(const struct emul *target, uint8_t reg_addr, const uint8_t *val,
36 size_t count)
37 {
38 struct bma4xx_emul_data *data = target->data;
39
40 __ASSERT_NO_MSG(reg_addr + count <= BMA4XX_NUM_REGS);
41 memcpy(data->regs + reg_addr, val, count);
42 }
43
bma4xx_emul_get_reg(const struct emul * target,uint8_t reg_addr,uint8_t * val,size_t count)44 void bma4xx_emul_get_reg(const struct emul *target, uint8_t reg_addr, uint8_t *val, size_t count)
45 {
46 struct bma4xx_emul_data *data = target->data;
47
48 __ASSERT_NO_MSG(reg_addr + count <= BMA4XX_NUM_REGS);
49 memcpy(val, data->regs + reg_addr, count);
50 }
51
bma4xx_emul_get_interrupt_config(const struct emul * target,uint8_t * int1_io_ctrl,bool * latched_mode)52 uint8_t bma4xx_emul_get_interrupt_config(const struct emul *target, uint8_t *int1_io_ctrl,
53 bool *latched_mode)
54 {
55 struct bma4xx_emul_data *data = target->data;
56
57 *int1_io_ctrl = data->regs[BMA4XX_REG_INT1_IO_CTRL];
58 *latched_mode = data->regs[BMA4XX_REG_INT_LATCH];
59 return data->regs[BMA4XX_REG_INT_MAP_DATA];
60 }
61
bma4xx_emul_read_byte(const struct emul * target,int reg,uint8_t * val,int bytes)62 static int bma4xx_emul_read_byte(const struct emul *target, int reg, uint8_t *val, int bytes)
63 {
64 bma4xx_emul_get_reg(target, reg, val, bytes);
65
66 return 0;
67 }
68
bma4xx_emul_write_byte(const struct emul * target,int reg,uint8_t val,int bytes)69 static int bma4xx_emul_write_byte(const struct emul *target, int reg, uint8_t val, int bytes)
70 {
71 struct bma4xx_emul_data *data = target->data;
72
73 if (bytes != 1) {
74 LOG_ERR("multi-byte writes are not supported");
75 return -ENOTSUP;
76 }
77
78 switch (reg) {
79 case BMA4XX_REG_ACCEL_CONFIG:
80 if ((val & 0xF0) != 0xA0) {
81 LOG_ERR("unsupported acc_bwp/acc_perf_mode: %#x", val);
82 return -EINVAL;
83 }
84 data->regs[reg] = val & GENMASK(1, 0);
85 return 0;
86 case BMA4XX_REG_ACCEL_RANGE:
87 if ((val & GENMASK(1, 0)) != val) {
88 LOG_ERR("reserved bits set in ACC_RANGE write: %#x", val);
89 return -EINVAL;
90 }
91 data->regs[reg] = val;
92 return 0;
93 case BMA4XX_REG_FIFO_CONFIG_1:
94 if (val & ~BMA4XX_FIFO_ACC_EN) {
95 LOG_ERR("unsupported bits set in FIFO_CONFIG_1"
96 " write: %#x",
97 val);
98 return -EINVAL;
99 }
100 data->regs[reg] = (val & BMA4XX_FIFO_ACC_EN) != 0;
101 return 0;
102 case BMA4XX_REG_INT1_IO_CTRL:
103 data->regs[reg] = val;
104 return 0;
105 case BMA4XX_REG_INT_LATCH:
106 if ((val & ~1) != 0) {
107 LOG_ERR("reserved bits set in INT_LATCH: %#x", val);
108 return -EINVAL;
109 }
110 data->regs[reg] = (val & 1) == 1;
111 return 0;
112 case BMA4XX_REG_INT_MAP_DATA:
113 data->regs[reg] = val;
114 return 0;
115 case BMA4XX_REG_NV_CONFIG:
116 if (val & GENMASK(7, 4)) {
117 LOG_ERR("reserved bits set in NV_CONF write: %#x", val);
118 return -EINVAL;
119 }
120 data->regs[reg] = val;
121 return 0;
122 case BMA4XX_REG_OFFSET_0:
123 case BMA4XX_REG_OFFSET_1:
124 case BMA4XX_REG_OFFSET_2:
125 data->regs[reg] = val;
126 return 0;
127 case BMA4XX_REG_POWER_CTRL:
128 if ((val & ~BMA4XX_BIT_ACC_EN) != 0) {
129 LOG_ERR("unhandled bits in POWER_CTRL write: %#x", val);
130 return -ENOTSUP;
131 }
132 data->regs[reg] = (val & BMA4XX_BIT_ACC_EN) != 0;
133 return 0;
134 case BMA4XX_REG_CMD:
135 if (val == BMA4XX_CMD_FIFO_FLUSH) { /* fifo_flush */
136 data->regs[BMA4XX_REG_FIFO_DATA] = 0;
137 data->regs[BMA4XX_REG_FIFO_LENGTH_0] = 0;
138 data->regs[BMA4XX_REG_FIFO_LENGTH_1] = 0;
139 return 0;
140 }
141 break;
142 }
143
144 LOG_WRN("unhandled I2C write to register %#x", reg);
145 return -ENOTSUP;
146 }
147
bma4xx_emul_init(const struct emul * target,const struct device * parent)148 static int bma4xx_emul_init(const struct emul *target, const struct device *parent)
149 {
150 struct bma4xx_emul_data *data = target->data;
151
152 data->regs[BMA4XX_REG_CHIP_ID] = BMA4XX_CHIP_ID_BMA422;
153 data->regs[BMA4XX_REG_ACCEL_RANGE] = BMA4XX_RANGE_4G;
154
155 return 0;
156 }
157
bma4xx_emul_transfer_i2c(const struct emul * target,struct i2c_msg * msgs,int num_msgs,int addr)158 static int bma4xx_emul_transfer_i2c(const struct emul *target, struct i2c_msg *msgs, int num_msgs,
159 int addr)
160 {
161 __ASSERT_NO_MSG(msgs && num_msgs);
162 if (num_msgs != 2) {
163 return 0;
164 }
165
166 i2c_dump_msgs_rw(target->dev, msgs, num_msgs, addr, false);
167
168 if (msgs->flags & I2C_MSG_READ) {
169 LOG_ERR("Unexpected read");
170 return -EIO;
171 }
172 if (msgs->len != 1) {
173 LOG_ERR("Unexpected msg0 length %d", msgs->len);
174 return -EIO;
175 }
176
177 uint32_t reg = msgs->buf[0];
178
179 msgs++;
180 if (msgs->flags & I2C_MSG_READ) {
181 /* Reads from regs in target->data to msgs->buf */
182 bma4xx_emul_read_byte(target, reg, msgs->buf, msgs->len);
183 } else {
184 /* Writes msgs->buf[0] to regs in target->data */
185 bma4xx_emul_write_byte(target, reg, msgs->buf[0], msgs->len);
186 }
187
188 return 0;
189 };
190
bma4xx_emul_set_accel_data(const struct emul * target,q31_t value,int8_t shift,int8_t reg)191 void bma4xx_emul_set_accel_data(const struct emul *target, q31_t value, int8_t shift, int8_t reg)
192 {
193
194 struct bma4xx_emul_data *data = target->data;
195
196 /* floor(9.80665 * 2^(31−4)) q31_t in (-2^4, 2^4) => range_g = shift of 4 */
197 int64_t g = 1316226282;
198 int64_t range_g = 4;
199
200 int64_t intermediate, unshifted;
201 int16_t reg_val;
202
203 /* 0x00 -> +/-2g; 0x01 -> +/-4g; 0x02 -> +/-8g; 0x03 -> +/- 16g; */
204 int64_t accel_range = (2 << data->regs[BMA4XX_REG_ACCEL_RANGE]);
205
206 unshifted = shift < 0 ? ((int64_t)value >> -shift) : ((int64_t)value << shift);
207
208 intermediate = (unshifted * BIT(11)) / (g << range_g);
209
210 intermediate /= accel_range;
211
212 reg_val = CLAMP(intermediate, -2048, 2047);
213
214 /* lsb register uses top 12 of 16 bits to hold value so shift by 4 to fill it */
215 data->regs[reg] = FIELD_GET(GENMASK(3, 0), reg_val) << 4;
216 data->regs[reg + 1] = FIELD_GET(GENMASK(11, 4), reg_val);
217 }
218
bma4xx_emul_backend_set_channel(const struct emul * target,struct sensor_chan_spec ch,const q31_t * value,int8_t shift)219 static int bma4xx_emul_backend_set_channel(const struct emul *target, struct sensor_chan_spec ch,
220 const q31_t *value, int8_t shift)
221 {
222
223 if (!target || !target->data) {
224 return -EINVAL;
225 }
226
227 struct bma4xx_emul_data *data = target->data;
228
229 switch (ch.chan_type) {
230 case SENSOR_CHAN_ACCEL_X:
231 bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_8);
232 break;
233 case SENSOR_CHAN_ACCEL_Y:
234 bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_10);
235 break;
236 case SENSOR_CHAN_ACCEL_Z:
237 bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_12);
238 break;
239 case SENSOR_CHAN_ACCEL_XYZ:
240 bma4xx_emul_set_accel_data(target, value[0], shift, BMA4XX_REG_DATA_8);
241 bma4xx_emul_set_accel_data(target, value[1], shift, BMA4XX_REG_DATA_10);
242 bma4xx_emul_set_accel_data(target, value[2], shift, BMA4XX_REG_DATA_12);
243 default:
244 return -ENOTSUP;
245 }
246
247 /* Set data ready flag */
248 data->regs[BMA4XX_REG_INT_STAT_1] |= BMA4XX_ACC_DRDY_INT;
249
250 return 0;
251 }
252
bma4xx_emul_backend_get_sample_range(const struct emul * target,struct sensor_chan_spec ch,q31_t * lower,q31_t * upper,q31_t * epsilon,int8_t * shift)253 static int bma4xx_emul_backend_get_sample_range(const struct emul *target,
254 struct sensor_chan_spec ch, q31_t *lower,
255 q31_t *upper, q31_t *epsilon, int8_t *shift)
256 {
257 if (!lower || !upper || !epsilon || !shift) {
258 return -EINVAL;
259 }
260
261 switch (ch.chan_type) {
262 case SENSOR_CHAN_ACCEL_X:
263 case SENSOR_CHAN_ACCEL_Y:
264 case SENSOR_CHAN_ACCEL_Z:
265 case SENSOR_CHAN_ACCEL_XYZ:
266 break;
267 default:
268 return -ENOTSUP;
269 }
270
271 struct bma4xx_emul_data *data = target->data;
272
273 switch (data->regs[BMA4XX_REG_ACCEL_RANGE]) {
274 case BMA4XX_RANGE_2G:
275 *shift = 5;
276 *upper = (q31_t)(2 * 9.80665 * BIT(31 - 5));
277 *lower = -*upper;
278 /* (1 << (31 - shift) >> 12) * 2 (where 2 comes from 2g range) */
279 *epsilon = BIT(31 - 5 - 12 + 1);
280 break;
281 case BMA4XX_RANGE_4G:
282 *shift = 6;
283 *upper = (q31_t)(4 * 9.80665 * BIT(31 - 6));
284 *lower = -*upper;
285 /* (1 << (31 - shift) >> 12) * 4 (where 4 comes from 4g range) */
286 *epsilon = BIT(31 - 6 - 12 + 2);
287 break;
288 case BMA4XX_RANGE_8G:
289 *shift = 7;
290 *upper = (q31_t)(8 * 9.80665 * BIT(31 - 7));
291 *lower = -*upper;
292 /* (1 << (31 - shift) >> 12) * 8 (where 8 comes from 8g range) */
293 *epsilon = BIT(31 - 7 - 12 + 3);
294 break;
295 case BMA4XX_RANGE_16G:
296 *shift = 8;
297 *upper = (q31_t)(16 * 9.80665 * BIT(31 - 8));
298 *lower = -*upper;
299 /* (1 << (31 - shift) >> 12) * 16 (where 16 comes from 16g range) */
300 *epsilon = BIT(31 - 8 - 12 + 4);
301 break;
302 default:
303 return -ENOTSUP;
304 }
305
306 return 0;
307 }
308
309 static const struct emul_sensor_driver_api bma4xx_emul_sensor_driver_api = {
310 .set_channel = bma4xx_emul_backend_set_channel,
311 .get_sample_range = bma4xx_emul_backend_get_sample_range,
312 };
313
314 static struct i2c_emul_api bma4xx_emul_api_i2c = {
315 .transfer = bma4xx_emul_transfer_i2c,
316 };
317
318 #define INIT_BMA4XX(n) \
319 static struct bma4xx_emul_data bma4xx_emul_data_##n = {}; \
320 static const struct bma4xx_emul_cfg bma4xx_emul_cfg_##n = {}; \
321 EMUL_DT_INST_DEFINE(n, bma4xx_emul_init, &bma4xx_emul_data_##n, &bma4xx_emul_cfg_##n, \
322 &bma4xx_emul_api_i2c, &bma4xx_emul_sensor_driver_api);
323
324 DT_INST_FOREACH_STATUS_OKAY(INIT_BMA4XX)
325
