1 /*
2 * Copyright (c) 2023 Google LLC
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #define DT_DRV_COMPAT invensense_icm42688
7
8 #include <zephyr/device.h>
9 #include <zephyr/drivers/emul.h>
10 #include <zephyr/drivers/emul_sensor.h>
11 #include <zephyr/drivers/spi.h>
12 #include <zephyr/drivers/spi_emul.h>
13 #include <zephyr/logging/log.h>
14
15 #include <icm42688_reg.h>
16
17 LOG_MODULE_DECLARE(ICM42688, CONFIG_SENSOR_LOG_LEVEL);
18
19 #define NUM_REGS (UINT8_MAX >> 1)
20
21 struct icm42688_emul_data {
22 uint8_t reg[NUM_REGS];
23 };
24
25 struct icm42688_emul_cfg {
26 };
27
icm42688_emul_set_reg(const struct emul * target,uint8_t reg_addr,const uint8_t * val,size_t count)28 void icm42688_emul_set_reg(const struct emul *target, uint8_t reg_addr, const uint8_t *val,
29 size_t count)
30 {
31 struct icm42688_emul_data *data = target->data;
32
33 __ASSERT_NO_MSG(reg_addr + count < NUM_REGS);
34 memcpy(data->reg + reg_addr, val, count);
35 }
36
icm42688_emul_get_reg(const struct emul * target,uint8_t reg_addr,uint8_t * val,size_t count)37 void icm42688_emul_get_reg(const struct emul *target, uint8_t reg_addr, uint8_t *val, size_t count)
38 {
39 struct icm42688_emul_data *data = target->data;
40
41 __ASSERT_NO_MSG(reg_addr + count < NUM_REGS);
42 memcpy(val, data->reg + reg_addr, count);
43 }
44
icm42688_emul_handle_write(const struct emul * target,uint8_t regn,uint8_t value)45 static void icm42688_emul_handle_write(const struct emul *target, uint8_t regn, uint8_t value)
46 {
47 struct icm42688_emul_data *data = target->data;
48
49 switch (regn) {
50 case REG_DEVICE_CONFIG:
51 if (FIELD_GET(BIT_SOFT_RESET, value) == 1) {
52 /* Perform a soft reset */
53 memset(data->reg, 0, NUM_REGS);
54 /* Initialized the who-am-i register */
55 data->reg[REG_WHO_AM_I] = WHO_AM_I_ICM42688;
56 /* Set the bit for the reset being done */
57 data->reg[REG_INT_STATUS] |= BIT_INT_STATUS_RESET_DONE;
58 }
59 break;
60 }
61 }
62
icm42688_emul_io_spi(const struct emul * target,const struct spi_config * config,const struct spi_buf_set * tx_bufs,const struct spi_buf_set * rx_bufs)63 static int icm42688_emul_io_spi(const struct emul *target, const struct spi_config *config,
64 const struct spi_buf_set *tx_bufs,
65 const struct spi_buf_set *rx_bufs)
66 {
67 struct icm42688_emul_data *data = target->data;
68 const struct spi_buf *tx, *rx;
69 uint8_t regn;
70 bool is_read;
71
72 ARG_UNUSED(config);
73 __ASSERT_NO_MSG(tx_bufs != NULL);
74
75 tx = tx_bufs->buffers;
76 __ASSERT_NO_MSG(tx != NULL);
77 __ASSERT_NO_MSG(tx->len > 0);
78
79 regn = *(uint8_t *)tx->buf;
80 is_read = FIELD_GET(REG_SPI_READ_BIT, regn);
81 regn &= GENMASK(6, 0);
82 if (is_read) {
83 __ASSERT_NO_MSG(rx_bufs != NULL);
84 __ASSERT_NO_MSG(rx_bufs->count > 1);
85
86 rx = &rx_bufs->buffers[1];
87 __ASSERT_NO_MSG(rx->buf != NULL);
88 __ASSERT_NO_MSG(rx->len > 0);
89 for (uint16_t i = 0; i < rx->len; ++i) {
90 ((uint8_t *)rx->buf)[i] = data->reg[regn + i];
91 }
92 } else {
93 /* Writing to regn */
94 uint8_t value;
95
96 __ASSERT_NO_MSG(tx_bufs->count > 1);
97 tx = &tx_bufs->buffers[1];
98
99 __ASSERT_NO_MSG(tx->len > 0);
100 value = ((uint8_t *)tx->buf)[0];
101 icm42688_emul_handle_write(target, regn, value);
102 }
103
104 return 0;
105 }
106
icm42688_emul_init(const struct emul * target,const struct device * parent)107 static int icm42688_emul_init(const struct emul *target, const struct device *parent)
108 {
109 struct icm42688_emul_data *data = target->data;
110
111 /* Initialized the who-am-i register */
112 data->reg[REG_WHO_AM_I] = WHO_AM_I_ICM42688;
113
114 return 0;
115 }
116
117 static const struct spi_emul_api icm42688_emul_spi_api = {
118 .io = icm42688_emul_io_spi,
119 };
120
121 #define Q31_SCALE ((int64_t)INT32_MAX + 1)
122
123 /**
124 * @brief Get current full-scale range in g's based on register config, along with corresponding
125 * sensitivity and shift. See datasheet section 3.2, table 2.
126 */
icm42688_emul_get_accel_settings(const struct emul * target,int * fs_g,int * sensitivity,int8_t * shift)127 static void icm42688_emul_get_accel_settings(const struct emul *target, int *fs_g, int *sensitivity,
128 int8_t *shift)
129 {
130 uint8_t reg;
131
132 int sensitivity_out, fs_g_out;
133 int8_t shift_out;
134
135 icm42688_emul_get_reg(target, REG_ACCEL_CONFIG0, ®, 1);
136
137 switch ((reg & MASK_ACCEL_UI_FS_SEL) >> 5) {
138 case BIT_ACCEL_UI_FS_16:
139 fs_g_out = 16;
140 sensitivity_out = 2048;
141 /* shift is based on `fs_g * 9.8` since the final numbers will be in SI units of
142 * m/s^2, not g's
143 */
144 shift_out = 8;
145 break;
146 case BIT_ACCEL_UI_FS_8:
147 fs_g_out = 8;
148 sensitivity_out = 4096;
149 shift_out = 7;
150 break;
151 case BIT_ACCEL_UI_FS_4:
152 fs_g_out = 4;
153 sensitivity_out = 8192;
154 shift_out = 6;
155 break;
156 case BIT_ACCEL_UI_FS_2:
157 fs_g_out = 2;
158 sensitivity_out = 16384;
159 shift_out = 5;
160 break;
161 default:
162 __ASSERT_UNREACHABLE;
163 }
164
165 if (fs_g) {
166 *fs_g = fs_g_out;
167 }
168 if (sensitivity) {
169 *sensitivity = sensitivity_out;
170 }
171 if (shift) {
172 *shift = shift_out;
173 }
174 }
175
176 /**
177 * @brief Helper function for calculating accelerometer ranges. Considers the current full-scale
178 * register config (i.e. +/-2g, +/-4g, etc...)
179 */
icm42688_emul_get_accel_ranges(const struct emul * target,q31_t * lower,q31_t * upper,q31_t * epsilon,int8_t * shift)180 static void icm42688_emul_get_accel_ranges(const struct emul *target, q31_t *lower, q31_t *upper,
181 q31_t *epsilon, int8_t *shift)
182 {
183 int fs_g;
184 int sensitivity;
185
186 icm42688_emul_get_accel_settings(target, &fs_g, &sensitivity, shift);
187
188 /* Epsilon is equal to 1.5 bit-counts worth of error. */
189 *epsilon = (3 * SENSOR_G * Q31_SCALE / sensitivity / 1000000LL / 2) >> *shift;
190 *upper = (fs_g * SENSOR_G * Q31_SCALE / 1000000LL) >> *shift;
191 *lower = -*upper;
192 }
193
194 /**
195 * @brief Get current full-scale gyro range in milli-degrees per second based on register config,
196 * along with corresponding sensitivity and shift. See datasheet section 3.1, table 1.
197 */
icm42688_emul_get_gyro_settings(const struct emul * target,int * fs_mdps,int * sensitivity,int8_t * shift)198 static void icm42688_emul_get_gyro_settings(const struct emul *target, int *fs_mdps,
199 int *sensitivity, int8_t *shift)
200 {
201 uint8_t reg;
202
203 int sensitivity_out, fs_mdps_out;
204 int8_t shift_out;
205
206 icm42688_emul_get_reg(target, REG_GYRO_CONFIG0, ®, 1);
207
208 switch ((reg & MASK_GYRO_UI_FS_SEL) >> 5) {
209 case BIT_GYRO_UI_FS_2000:
210 /* Milli-degrees per second */
211 fs_mdps_out = 2000000;
212 /* 10x LSBs/deg/s */
213 sensitivity_out = 164;
214 /* Shifts are based on rad/s: `(fs_mdps * pi / 180 / 1000)` */
215 shift_out = 6; /* +/- 34.90659 */
216 break;
217 case BIT_GYRO_UI_FS_1000:
218 fs_mdps_out = 1000000;
219 sensitivity_out = 328;
220 shift_out = 5; /* +/- 17.44444 */
221 break;
222 case BIT_GYRO_UI_FS_500:
223 fs_mdps_out = 500000;
224 sensitivity_out = 655;
225 shift_out = 4; /* +/- 8.72222 */
226 break;
227 case BIT_GYRO_UI_FS_250:
228 fs_mdps_out = 250000;
229 sensitivity_out = 1310;
230 shift_out = 3; /* +/- 4.36111 */
231 break;
232 case BIT_GYRO_UI_FS_125:
233 fs_mdps_out = 125000;
234 sensitivity_out = 2620;
235 shift_out = 2; /* +/- 2.18055 */
236 break;
237 case BIT_GYRO_UI_FS_62_5:
238 fs_mdps_out = 62500;
239 sensitivity_out = 5243;
240 shift_out = 1; /* +/- 1.09027 */
241 break;
242 case BIT_GYRO_UI_FS_31_25:
243 fs_mdps_out = 31250;
244 sensitivity_out = 10486;
245 shift_out = 0; /* +/- 0.54513 */
246 break;
247 case BIT_GYRO_UI_FS_15_625:
248 fs_mdps_out = 15625;
249 sensitivity_out = 20972;
250 shift_out = -1; /* +/- 0.27256 */
251 break;
252 default:
253 __ASSERT_UNREACHABLE;
254 }
255
256 if (fs_mdps) {
257 *fs_mdps = fs_mdps_out;
258 }
259 if (sensitivity) {
260 *sensitivity = sensitivity_out;
261 }
262 if (shift) {
263 *shift = shift_out;
264 }
265 }
266
267 /**
268 * @brief Helper function for calculating gyroscope ranges. Considers the current full-scale
269 * register config
270 */
icm42688_emul_get_gyro_ranges(const struct emul * target,q31_t * lower,q31_t * upper,q31_t * epsilon,int8_t * shift)271 static void icm42688_emul_get_gyro_ranges(const struct emul *target, q31_t *lower, q31_t *upper,
272 q31_t *epsilon, int8_t *shift)
273 {
274 /* millidegrees/second */
275 int fs_mdps;
276 /* 10x LSBs per degrees/second*/
277 int sensitivity;
278
279 icm42688_emul_get_gyro_settings(target, &fs_mdps, &sensitivity, shift);
280
281 /* Reduce the actual range of gyroscope values. Some full-scale ranges actually exceed the
282 * size of an int16 by a small margin. For example, FS_SEL=0 has a +/-2000 deg/s range with
283 * 16.4 bits/deg/s sensitivity (Section 3.1, Table 1). This works out to register values of
284 * +/-2000 * 16.4 = +/-32800. This will cause the expected value to get clipped when
285 * setting the register and throw off the actual reading. Therefore, scale down the range
286 * to 99% to avoid the top and bottom edges.
287 */
288
289 fs_mdps *= 0.99;
290
291 /* Epsilon is equal to 1.5 bit-counts worth of error. */
292 *epsilon = (3 * SENSOR_PI * Q31_SCALE * 10LL / 1000000LL / 180LL / sensitivity / 2LL) >>
293 *shift;
294 *upper = (((fs_mdps * SENSOR_PI / 1000000LL) * Q31_SCALE) / 1000LL / 180LL) >> *shift;
295 *lower = -*upper;
296 }
297
icm42688_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)298 static int icm42688_emul_backend_get_sample_range(const struct emul *target, enum sensor_channel ch,
299 q31_t *lower, q31_t *upper, q31_t *epsilon,
300 int8_t *shift)
301 {
302 if (!lower || !upper || !epsilon || !shift) {
303 return -EINVAL;
304 }
305
306 switch (ch) {
307 case SENSOR_CHAN_DIE_TEMP:
308 /* degrees C = ([16-bit signed temp_data register] / 132.48) + 25 */
309 *shift = 9;
310 *lower = (int64_t)(-222.342995169 * Q31_SCALE) >> *shift;
311 *upper = (int64_t)(272.33544686 * Q31_SCALE) >> *shift;
312 *epsilon = (int64_t)(0.0076 * Q31_SCALE) >> *shift;
313 break;
314 case SENSOR_CHAN_ACCEL_X:
315 case SENSOR_CHAN_ACCEL_Y:
316 case SENSOR_CHAN_ACCEL_Z:
317 icm42688_emul_get_accel_ranges(target, lower, upper, epsilon, shift);
318 break;
319 case SENSOR_CHAN_GYRO_X:
320 case SENSOR_CHAN_GYRO_Y:
321 case SENSOR_CHAN_GYRO_Z:
322 icm42688_emul_get_gyro_ranges(target, lower, upper, epsilon, shift);
323 break;
324 default:
325 return -ENOTSUP;
326 }
327
328 return 0;
329 }
330
icm42688_emul_backend_set_channel(const struct emul * target,enum sensor_channel ch,q31_t value,int8_t shift)331 static int icm42688_emul_backend_set_channel(const struct emul *target, enum sensor_channel ch,
332 q31_t value, int8_t shift)
333 {
334 if (!target || !target->data) {
335 return -EINVAL;
336 }
337
338 struct icm42688_emul_data *data = target->data;
339
340 int sensitivity;
341 uint8_t reg_addr;
342 int32_t reg_val;
343 int64_t value_unshifted =
344 shift < 0 ? ((int64_t)value >> -shift) : ((int64_t)value << shift);
345
346 switch (ch) {
347 case SENSOR_CHAN_DIE_TEMP:
348 reg_addr = REG_TEMP_DATA1;
349 reg_val = ((value_unshifted - (25 * Q31_SCALE)) * 13248) / (100 * Q31_SCALE);
350 break;
351 case SENSOR_CHAN_ACCEL_X:
352 case SENSOR_CHAN_ACCEL_Y:
353 case SENSOR_CHAN_ACCEL_Z:
354 switch (ch) {
355 case SENSOR_CHAN_ACCEL_X:
356 reg_addr = REG_ACCEL_DATA_X1;
357 break;
358 case SENSOR_CHAN_ACCEL_Y:
359 reg_addr = REG_ACCEL_DATA_Y1;
360 break;
361 case SENSOR_CHAN_ACCEL_Z:
362 reg_addr = REG_ACCEL_DATA_Z1;
363 break;
364 default:
365 __ASSERT_UNREACHABLE;
366 }
367 icm42688_emul_get_accel_settings(target, NULL, &sensitivity, NULL);
368 reg_val = ((value_unshifted * sensitivity / Q31_SCALE) * 1000000LL) / SENSOR_G;
369 break;
370 case SENSOR_CHAN_GYRO_X:
371 case SENSOR_CHAN_GYRO_Y:
372 case SENSOR_CHAN_GYRO_Z:
373 switch (ch) {
374 case SENSOR_CHAN_GYRO_X:
375 reg_addr = REG_GYRO_DATA_X1;
376 break;
377 case SENSOR_CHAN_GYRO_Y:
378 reg_addr = REG_GYRO_DATA_Y1;
379 break;
380 case SENSOR_CHAN_GYRO_Z:
381 reg_addr = REG_GYRO_DATA_Z1;
382 break;
383 default:
384 __ASSERT_UNREACHABLE;
385 }
386 icm42688_emul_get_gyro_settings(target, NULL, &sensitivity, NULL);
387 reg_val =
388 CLAMP((((value_unshifted * sensitivity * 180LL) / Q31_SCALE) * 1000000LL) /
389 SENSOR_PI / 10LL,
390 INT16_MIN, INT16_MAX);
391 break;
392 default:
393 return -ENOTSUP;
394 }
395
396 data->reg[reg_addr] = (reg_val >> 8) & 0xFF;
397 data->reg[reg_addr + 1] = reg_val & 0xFF;
398
399 /* Set data ready flag */
400 data->reg[REG_INT_STATUS] |= BIT_INT_STATUS_DATA_RDY;
401
402 return 0;
403 }
404
405 static const struct emul_sensor_backend_api icm42688_emul_sensor_backend_api = {
406 .set_channel = icm42688_emul_backend_set_channel,
407 .get_sample_range = icm42688_emul_backend_get_sample_range,
408 };
409
410 #define ICM42688_EMUL_DEFINE(n, api) \
411 EMUL_DT_INST_DEFINE(n, icm42688_emul_init, &icm42688_emul_data_##n, \
412 &icm42688_emul_cfg_##n, &api, &icm42688_emul_sensor_backend_api)
413
414 #define ICM42688_EMUL_SPI(n) \
415 static struct icm42688_emul_data icm42688_emul_data_##n; \
416 static const struct icm42688_emul_cfg icm42688_emul_cfg_##n; \
417 ICM42688_EMUL_DEFINE(n, icm42688_emul_spi_api)
418
419 DT_INST_FOREACH_STATUS_OKAY(ICM42688_EMUL_SPI)
420
