1 /*
2 * Copyright (c) 2024 TDK Invensense
3 * Copyright (c) 2022 Esco Medical ApS
4 * Copyright (c) 2020 TDK Invensense
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/kernel.h>
10 #include <zephyr/drivers/sensor.h>
11 #include <zephyr/init.h>
12 #include <zephyr/drivers/gpio.h>
13 #include <zephyr/pm/device.h>
14 #include <zephyr/sys/byteorder.h>
15 #include <zephyr/sys/__assert.h>
16 #include <zephyr/drivers/sensor/icm42x70.h>
17 #include <zephyr/drivers/sensor/tdk_apex.h>
18
19 #include "icm42x70.h"
20 #include "icm42x70_trigger.h"
21 #include "icm42670.h"
22
23 #include <zephyr/logging/log.h>
24 LOG_MODULE_REGISTER(ICM42X70, CONFIG_SENSOR_LOG_LEVEL);
25
26 /* Convert DT enum to sensor ODR selection */
27 #define ICM42X70_CONVERT_ENUM_TO_ODR_POS (4)
28
29 /* Maximum bytes to read/write on ICM42X70 serial interface */
30 #define ICM42X70_SERIAL_INTERFACE_MAX_READ (1024 * 32)
31 #define ICM42X70_SERIAL_INTERFACE_MAX_WRITE (1024 * 32)
32
icm42x70_reg_read(const struct device * dev,uint8_t reg,uint8_t * buf,uint32_t size)33 static inline int icm42x70_reg_read(const struct device *dev, uint8_t reg, uint8_t *buf,
34 uint32_t size)
35 {
36 const struct icm42x70_config *cfg = dev->config;
37
38 return cfg->bus_io->read(&cfg->bus, reg, buf, size);
39 }
40
inv_io_hal_read_reg(struct inv_imu_serif * serif,uint8_t reg,uint8_t * rbuffer,uint32_t rlen)41 static inline int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t *rbuffer,
42 uint32_t rlen)
43 {
44 return icm42x70_reg_read(serif->context, reg, rbuffer, rlen);
45 }
46
icm42x70_reg_write(const struct device * dev,uint8_t reg,const uint8_t * buf,uint32_t size)47 static inline int icm42x70_reg_write(const struct device *dev, uint8_t reg, const uint8_t *buf,
48 uint32_t size)
49 {
50 const struct icm42x70_config *cfg = dev->config;
51
52 return cfg->bus_io->write(&cfg->bus, reg, (uint8_t *)buf, size);
53 }
54
inv_io_hal_write_reg(struct inv_imu_serif * serif,uint8_t reg,const uint8_t * wbuffer,uint32_t wlen)55 static inline int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg,
56 const uint8_t *wbuffer, uint32_t wlen)
57 {
58 return icm42x70_reg_write(serif->context, reg, wbuffer, wlen);
59 }
60
inv_imu_sleep_us(uint32_t us)61 void inv_imu_sleep_us(uint32_t us)
62 {
63 k_sleep(K_USEC(us));
64 }
65
inv_imu_get_time_us(void)66 uint64_t inv_imu_get_time_us(void)
67 {
68 /* returns the elapsed time since the system booted, in milliseconds */
69 return k_uptime_get() * 1000;
70 }
71
convert_dt_enum_to_freq(uint8_t val)72 static uint16_t convert_dt_enum_to_freq(uint8_t val)
73 {
74 uint16_t freq;
75
76 switch (val) {
77 case 0:
78 freq = 0;
79 break;
80 case 1:
81 freq = 1600;
82 break;
83 case 2:
84 freq = 800;
85 break;
86 case 3:
87 freq = 400;
88 break;
89 case 4:
90 freq = 200;
91 break;
92 case 5:
93 freq = 100;
94 break;
95 case 6:
96 freq = 50;
97 break;
98 case 7:
99 freq = 25;
100 break;
101 case 8:
102 freq = 12;
103 break;
104 case 9:
105 freq = 6;
106 break;
107 case 10:
108 freq = 3;
109 break;
110 case 11:
111 freq = 1;
112 break;
113 default:
114 freq = 0;
115 break;
116 }
117 return freq;
118 }
119
convert_freq_to_bitfield(uint32_t val,uint16_t * freq)120 uint32_t convert_freq_to_bitfield(uint32_t val, uint16_t *freq)
121 {
122 uint32_t odr_bitfield = 0;
123
124 if (val < 3 && val >= 1) {
125 odr_bitfield = ACCEL_CONFIG0_ODR_1_5625_HZ; /*(= GYRO_CONFIG0_ODR_1_5625_HZ )*/
126 *freq = 1;
127 } else if (val < 6 && val >= 3) {
128 odr_bitfield = ACCEL_CONFIG0_ODR_3_125_HZ; /*(= GYRO_CONFIG0_ODR_3_125_HZ )*/
129 *freq = 3;
130 } else if (val < 12 && val >= 6) {
131 odr_bitfield = ACCEL_CONFIG0_ODR_6_25_HZ; /*(= GYRO_CONFIG0_ODR_6_25_HZ )*/
132 *freq = 6;
133 } else if (val < 25 && val >= 12) {
134 odr_bitfield = ACCEL_CONFIG0_ODR_12_5_HZ; /*(= GYRO_CONFIG0_ODR_12_5_HZ )*/
135 *freq = 12;
136 } else if (val < 50 && val >= 25) {
137 odr_bitfield = ACCEL_CONFIG0_ODR_25_HZ; /*(= GYRO_CONFIG0_ODR_25_HZ )*/
138 *freq = 25;
139 } else if (val < 100 && val >= 50) {
140 odr_bitfield = ACCEL_CONFIG0_ODR_50_HZ; /*(GYRO_CONFIG0_ODR_50_HZ)*/
141 *freq = 50;
142 } else if (val < 200 && val >= 100) {
143 odr_bitfield = ACCEL_CONFIG0_ODR_100_HZ; /*(= GYRO_CONFIG0_ODR_100_HZ )*/
144 *freq = 100;
145 } else if (val < 400 && val >= 200) {
146 odr_bitfield = ACCEL_CONFIG0_ODR_200_HZ; /*(= GYRO_CONFIG0_ODR_200_HZ )*/
147 *freq = 200;
148 } else if (val < 800 && val >= 400) {
149 odr_bitfield = ACCEL_CONFIG0_ODR_400_HZ; /*(= GYRO_CONFIG0_ODR_400_HZ )*/
150 *freq = 400;
151 } else if (val < 1600 && val >= 800) {
152 odr_bitfield = ACCEL_CONFIG0_ODR_800_HZ; /*(= GYRO_CONFIG0_ODR_800_HZ )*/
153 *freq = 800;
154 } else if (val == 1600) {
155 odr_bitfield = ACCEL_CONFIG0_ODR_1600_HZ; /*(= GYRO_CONFIG0_ODR_1600_HZ )*/
156 *freq = 1600;
157 }
158 return odr_bitfield;
159 }
160
convert_acc_fs_to_bitfield(uint32_t val,uint8_t * fs)161 static uint32_t convert_acc_fs_to_bitfield(uint32_t val, uint8_t *fs)
162 {
163 uint32_t bitfield = 0;
164
165 if (val < 4 && val >= 2) {
166 bitfield = ACCEL_CONFIG0_FS_SEL_2g;
167 *fs = 2;
168 } else if (val < 8 && val >= 4) {
169 bitfield = ACCEL_CONFIG0_FS_SEL_4g;
170 *fs = 4;
171 } else if (val < 16 && val >= 8) {
172 bitfield = ACCEL_CONFIG0_FS_SEL_8g;
173 *fs = 8;
174 } else if (val == 16) {
175 bitfield = ACCEL_CONFIG0_FS_SEL_16g;
176 *fs = 16;
177 }
178 return bitfield;
179 }
180
convert_ln_bw_to_bitfield(uint32_t val)181 uint32_t convert_ln_bw_to_bitfield(uint32_t val)
182 {
183 uint32_t bitfield = 0xFF;
184
185 if (val < 25 && val >= 16) {
186 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_16; /* (= GYRO_CONFIG1_GYRO_FILT_BW_16) */
187 } else if (val < 34 && val >= 25) {
188 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_25; /* (= GYRO_CONFIG1_GYRO_FILT_BW_25) */
189 } else if (val < 53 && val >= 34) {
190 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_34; /* (= GYRO_CONFIG1_GYRO_FILT_BW_34) */
191 } else if (val < 73 && val >= 53) {
192 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_53; /* (= GYRO_CONFIG1_GYRO_FILT_BW_53) */
193 } else if (val < 121 && val >= 73) {
194 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_73; /* (= GYRO_CONFIG1_GYRO_FILT_BW_73) */
195 } else if (val < 180 && val >= 121) {
196 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_121; /* (= GYRO_CONFIG1_GYRO_FILT_BW_121) */
197 } else if (val == 180) {
198 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_180; /* (= GYRO_CONFIG1_GYRO_FILT_BW_180) */
199 } else if (val == 0) {
200 bitfield = ACCEL_CONFIG1_ACCEL_FILT_BW_NO_FILTER;
201 /*(= GYRO_CONFIG1_GYRO_FILT_BW_NO_FILTER)*/
202 }
203 return bitfield;
204 }
205
convert_lp_avg_to_bitfield(uint32_t val)206 static uint32_t convert_lp_avg_to_bitfield(uint32_t val)
207 {
208 uint32_t bitfield = 0xFF;
209
210 if (val < 4 && val >= 2) {
211 bitfield = ACCEL_CONFIG1_ACCEL_FILT_AVG_2;
212 } else if (val < 8 && val >= 4) {
213 bitfield = ACCEL_CONFIG1_ACCEL_FILT_AVG_4;
214 } else if (val < 16 && val >= 8) {
215 bitfield = ACCEL_CONFIG1_ACCEL_FILT_AVG_8;
216 } else if (val < 32 && val >= 16) {
217 bitfield = ACCEL_CONFIG1_ACCEL_FILT_AVG_16;
218 } else if (val < 64 && val >= 32) {
219 bitfield = ACCEL_CONFIG1_ACCEL_FILT_AVG_32;
220 } else if (val == 64) {
221 bitfield = ACCEL_CONFIG1_ACCEL_FILT_AVG_64;
222 }
223 return bitfield;
224 }
225
convert_bitfield_to_acc_fs(uint8_t bitfield)226 static uint8_t convert_bitfield_to_acc_fs(uint8_t bitfield)
227 {
228 uint8_t acc_fs = 0;
229
230 if (bitfield == ACCEL_CONFIG0_FS_SEL_2g) {
231 acc_fs = 2;
232 } else if (bitfield == ACCEL_CONFIG0_FS_SEL_4g) {
233 acc_fs = 4;
234 } else if (bitfield == ACCEL_CONFIG0_FS_SEL_8g) {
235 acc_fs = 8;
236 } else if (bitfield == ACCEL_CONFIG0_FS_SEL_16g) {
237 acc_fs = 16;
238 }
239 return acc_fs;
240 }
241
icm42x70_set_accel_power_mode(struct icm42x70_data * drv_data,const struct sensor_value * val)242 static int icm42x70_set_accel_power_mode(struct icm42x70_data *drv_data,
243 const struct sensor_value *val)
244 {
245 if ((val->val1 == ICM42X70_LOW_POWER_MODE) &&
246 (drv_data->accel_pwr_mode != ICM42X70_LOW_POWER_MODE)) {
247 if (drv_data->accel_hz != 0) {
248 if (drv_data->accel_hz <= 400) {
249 inv_imu_enable_accel_low_power_mode(&drv_data->driver);
250 } else {
251 LOG_ERR("Not supported ATTR value");
252 return -EINVAL;
253 }
254 }
255 drv_data->accel_pwr_mode = val->val1;
256 } else if ((val->val1 == ICM42X70_LOW_NOISE_MODE) &&
257 (drv_data->accel_pwr_mode != ICM42X70_LOW_NOISE_MODE)) {
258 if (drv_data->accel_hz != 0) {
259 if (drv_data->accel_hz >= 12) {
260 inv_imu_enable_accel_low_noise_mode(&drv_data->driver);
261 } else {
262 LOG_ERR("Not supported ATTR value");
263 return -EINVAL;
264 }
265 }
266 drv_data->accel_pwr_mode = val->val1;
267 } else {
268 LOG_ERR("Not supported ATTR value");
269 return -EINVAL;
270 }
271 return 0;
272 }
273
icm42x70_set_accel_odr(struct icm42x70_data * drv_data,const struct sensor_value * val)274 static int icm42x70_set_accel_odr(struct icm42x70_data *drv_data, const struct sensor_value *val)
275 {
276 if (val->val1 <= 1600 && val->val1 >= 1) {
277 if (drv_data->accel_hz == 0) {
278 inv_imu_set_accel_frequency(
279 &drv_data->driver,
280 convert_freq_to_bitfield(val->val1, &drv_data->accel_hz));
281 if (drv_data->accel_pwr_mode == ICM42X70_LOW_POWER_MODE) {
282 inv_imu_enable_accel_low_power_mode(&drv_data->driver);
283 } else if (drv_data->accel_pwr_mode == ICM42X70_LOW_NOISE_MODE) {
284 inv_imu_enable_accel_low_noise_mode(&drv_data->driver);
285 }
286 } else {
287 inv_imu_set_accel_frequency(
288 &drv_data->driver,
289 convert_freq_to_bitfield(val->val1, &drv_data->accel_hz));
290 }
291 } else if (val->val1 == 0) {
292 inv_imu_disable_accel(&drv_data->driver);
293 drv_data->accel_hz = val->val1;
294 } else {
295 LOG_ERR("Incorrect sampling value");
296 return -EINVAL;
297 }
298 return 0;
299 }
300
icm42x70_set_accel_fs(struct icm42x70_data * drv_data,const struct sensor_value * val)301 static int icm42x70_set_accel_fs(struct icm42x70_data *drv_data, const struct sensor_value *val)
302 {
303 if (val->val1 > 16 || val->val1 < 2) {
304 LOG_ERR("Incorrect fullscale value");
305 return -EINVAL;
306 }
307 inv_imu_set_accel_fsr(&drv_data->driver,
308 convert_acc_fs_to_bitfield(val->val1, &drv_data->accel_fs));
309 LOG_DBG("Set accel full scale to: %d G", drv_data->accel_fs);
310 return 0;
311 }
312
icm42x70_accel_config(struct icm42x70_data * drv_data,enum sensor_attribute attr,const struct sensor_value * val)313 static int icm42x70_accel_config(struct icm42x70_data *drv_data, enum sensor_attribute attr,
314 const struct sensor_value *val)
315 {
316 if (attr == SENSOR_ATTR_CONFIGURATION) {
317 icm42x70_set_accel_power_mode(drv_data, val);
318
319 } else if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
320 icm42x70_set_accel_odr(drv_data, val);
321
322 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
323 icm42x70_set_accel_fs(drv_data, val);
324
325 } else if ((enum sensor_attribute_icm42x70)attr == SENSOR_ATTR_BW_FILTER_LPF) {
326 if (val->val1 > 180) {
327 LOG_ERR("Incorrect low pass filter bandwidth value");
328 return -EINVAL;
329 }
330 inv_imu_set_accel_ln_bw(&drv_data->driver, convert_ln_bw_to_bitfield(val->val1));
331
332 } else if ((enum sensor_attribute_icm42x70)attr == SENSOR_ATTR_AVERAGING) {
333 if (val->val1 > 64 || val->val1 < 2) {
334 LOG_ERR("Incorrect averaging filter value");
335 return -EINVAL;
336 }
337 inv_imu_set_accel_lp_avg(&drv_data->driver, convert_lp_avg_to_bitfield(val->val1));
338 } else {
339 LOG_ERR("Unsupported attribute");
340 return -EINVAL;
341 }
342 return 0;
343 }
344
icm42x70_sensor_init(const struct device * dev)345 static int icm42x70_sensor_init(const struct device *dev)
346 {
347 struct icm42x70_data *data = dev->data;
348 const struct icm42x70_config *config = dev->config;
349 int err = 0;
350
351 /* Initialize serial interface and device */
352 data->serif.context = (struct device *)dev;
353 data->serif.read_reg = inv_io_hal_read_reg;
354 data->serif.write_reg = inv_io_hal_write_reg;
355 data->serif.max_read = ICM42X70_SERIAL_INTERFACE_MAX_READ;
356 data->serif.max_write = ICM42X70_SERIAL_INTERFACE_MAX_WRITE;
357 data->serif.serif_type = config->serif_type;
358 err = inv_imu_init(&data->driver, &data->serif, NULL);
359 if (err < 0) {
360 LOG_ERR("Init failed: %d", err);
361 return err;
362 }
363
364 err = inv_imu_get_who_am_i(&data->driver, &data->chip_id);
365 if (err < 0) {
366 LOG_ERR("ID read failed: %d", err);
367 return err;
368 }
369
370 if (data->chip_id != data->imu_whoami) {
371 LOG_ERR("invalid WHO_AM_I value, was 0x%x but expected 0x%x for %s", data->chip_id,
372 data->imu_whoami, data->imu_name);
373 return -ENOTSUP;
374 }
375
376 LOG_DBG("\"%s\" %s OK", dev->name, data->imu_name);
377 return 0;
378 }
379
icm42x70_turn_on_sensor(const struct device * dev)380 static int icm42x70_turn_on_sensor(const struct device *dev)
381 {
382 struct icm42x70_data *data = dev->data;
383 const struct icm42x70_config *cfg = dev->config;
384 int err = 0;
385
386 err = inv_imu_set_accel_fsr(&data->driver,
387 (cfg->accel_fs << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS));
388 data->accel_fs =
389 convert_bitfield_to_acc_fs((cfg->accel_fs << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS));
390 if ((err < 0) || (data->accel_fs == 0)) {
391 LOG_ERR("Failed to configure accel FSR");
392 return -EIO;
393 }
394 LOG_DBG("Set accel full scale to: %d G", data->accel_fs);
395
396 #if CONFIG_USE_EMD_ICM42670
397 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
398 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
399 err = inv_imu_set_gyro_fsr(&data->driver,
400 (cfg->gyro_fs << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS));
401 data->gyro_fs = convert_bitfield_to_gyr_fs(
402 (cfg->gyro_fs << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS));
403 if ((err < 0) || (data->gyro_fs == 0)) {
404 LOG_ERR("Failed to configure gyro FSR");
405 return -EIO;
406 }
407 LOG_DBG("Set gyro full scale to: %d dps", data->gyro_fs);
408 }
409 #endif
410
411 err = inv_imu_set_accel_lp_avg(&data->driver,
412 (cfg->accel_avg << ACCEL_CONFIG1_ACCEL_UI_AVG_POS));
413 err |= inv_imu_set_accel_ln_bw(&data->driver,
414 (cfg->accel_filt_bw << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS));
415 #if CONFIG_USE_EMD_ICM42670
416 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
417 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
418 err |= inv_imu_set_gyro_ln_bw(
419 &data->driver, (cfg->gyro_filt_bw << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS));
420 }
421 #endif
422 if (err < 0) {
423 LOG_ERR("Failed to configure filtering.");
424 return -EIO;
425 }
426
427 if (cfg->accel_hz != 0) {
428 err |= inv_imu_set_accel_frequency(
429 &data->driver, cfg->accel_hz + ICM42X70_CONVERT_ENUM_TO_ODR_POS);
430 if ((cfg->accel_pwr_mode == ICM42X70_LOW_NOISE_MODE) &&
431 (convert_dt_enum_to_freq(cfg->accel_hz) >= 12)) {
432 err |= inv_imu_enable_accel_low_noise_mode(&data->driver);
433 } else if ((cfg->accel_pwr_mode == ICM42X70_LOW_POWER_MODE) &&
434 (convert_dt_enum_to_freq(cfg->accel_hz) <= 400)) {
435 err |= inv_imu_enable_accel_low_power_mode(&data->driver);
436 } else {
437 LOG_ERR("Not supported power mode value");
438 }
439 }
440 #if CONFIG_USE_EMD_ICM42670
441 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
442 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
443 if (cfg->gyro_hz != 0) {
444 err |= inv_imu_set_gyro_frequency(
445 &data->driver, cfg->gyro_hz + ICM42X70_CONVERT_ENUM_TO_ODR_POS);
446 err |= inv_imu_enable_gyro_low_noise_mode(&data->driver);
447 }
448 }
449 #endif
450 if (err < 0) {
451 LOG_ERR("Failed to configure ODR.");
452 return -EIO;
453 }
454
455 data->accel_pwr_mode = cfg->accel_pwr_mode;
456 data->accel_hz = convert_dt_enum_to_freq(cfg->accel_hz);
457 #if CONFIG_USE_EMD_ICM42670
458 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
459 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
460 data->gyro_hz = convert_dt_enum_to_freq(cfg->gyro_hz);
461 }
462 #endif
463
464 /*
465 * Accelerometer sensor need at least 10ms startup time
466 * Gyroscope sensor need at least 30ms startup time
467 */
468 k_msleep(100);
469
470 return 0;
471 }
472
icm42x70_convert_accel(struct sensor_value * val,int16_t raw_val,uint16_t fs)473 static void icm42x70_convert_accel(struct sensor_value *val, int16_t raw_val, uint16_t fs)
474 {
475 int64_t conv_val;
476
477 /* 16 bit accelerometer. 2^15 bits represent the range in G */
478 /* see datasheet section 3.2 for details */
479 conv_val = (int64_t)raw_val * SENSOR_G * fs / INT16_MAX;
480
481 val->val1 = conv_val / 1000000;
482 val->val2 = conv_val % 1000000;
483 }
484
icm42x70_convert_temp(struct sensor_value * val,int16_t raw_val)485 static void icm42x70_convert_temp(struct sensor_value *val, int16_t raw_val)
486 {
487 int64_t conv_val;
488
489 /* see datasheet section 15.9 for details */
490 conv_val = 25 * 1000000 + ((int64_t)raw_val * 1000000 / 2);
491 val->val1 = conv_val / 1000000;
492 val->val2 = conv_val % 1000000;
493 }
494
icm42x70_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)495 static int icm42x70_channel_get(const struct device *dev, enum sensor_channel chan,
496 struct sensor_value *val)
497 {
498 int res = 0;
499 struct icm42x70_data *data = dev->data;
500 #ifdef CONFIG_TDK_APEX
501 const struct icm42x70_config *cfg = dev->config;
502 #endif
503
504 icm42x70_lock(dev);
505
506 if (chan == SENSOR_CHAN_ACCEL_XYZ) {
507 icm42x70_convert_accel(&val[0], data->accel_x, data->accel_fs);
508 icm42x70_convert_accel(&val[1], data->accel_y, data->accel_fs);
509 icm42x70_convert_accel(&val[2], data->accel_z, data->accel_fs);
510 } else if (chan == SENSOR_CHAN_ACCEL_X) {
511 icm42x70_convert_accel(val, data->accel_x, data->accel_fs);
512 } else if (chan == SENSOR_CHAN_ACCEL_Y) {
513 icm42x70_convert_accel(val, data->accel_y, data->accel_fs);
514 } else if (chan == SENSOR_CHAN_ACCEL_Z) {
515 icm42x70_convert_accel(val, data->accel_z, data->accel_fs);
516 #if CONFIG_USE_EMD_ICM42670
517 } else if ((chan == SENSOR_CHAN_GYRO_XYZ) && ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
518 (data->imu_whoami == INV_ICM42670S_WHOAMI))) {
519 icm42670_convert_gyro(&val[0], data->gyro_x, data->gyro_fs);
520 icm42670_convert_gyro(&val[1], data->gyro_y, data->gyro_fs);
521 icm42670_convert_gyro(&val[2], data->gyro_z, data->gyro_fs);
522 } else if ((chan == SENSOR_CHAN_GYRO_X) && ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
523 (data->imu_whoami == INV_ICM42670S_WHOAMI))) {
524 icm42670_convert_gyro(val, data->gyro_x, data->gyro_fs);
525 } else if ((chan == SENSOR_CHAN_GYRO_Y) && ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
526 (data->imu_whoami == INV_ICM42670S_WHOAMI))) {
527 icm42670_convert_gyro(val, data->gyro_y, data->gyro_fs);
528 } else if ((chan == SENSOR_CHAN_GYRO_Z) && ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
529 (data->imu_whoami == INV_ICM42670S_WHOAMI))) {
530 icm42670_convert_gyro(val, data->gyro_z, data->gyro_fs);
531 #endif
532 } else if (chan == SENSOR_CHAN_DIE_TEMP) {
533 icm42x70_convert_temp(val, data->temp);
534 #ifdef CONFIG_TDK_APEX
535 } else if ((enum sensor_channel_tdk_apex)chan == SENSOR_CHAN_APEX_MOTION) {
536 if (cfg->apex == TDK_APEX_PEDOMETER) {
537 val[0].val1 = data->pedometer_cnt;
538 val[1].val1 = data->pedometer_activity;
539 icm42x70_apex_pedometer_cadence_convert(&val[2], data->pedometer_cadence,
540 data->dmp_odr_hz);
541 } else if (cfg->apex == TDK_APEX_WOM) {
542 val[0].val1 = (data->apex_status & ICM42X70_APEX_STATUS_MASK_WOM_X) ? 1 : 0;
543 val[1].val1 = (data->apex_status & ICM42X70_APEX_STATUS_MASK_WOM_Y) ? 1 : 0;
544 val[2].val1 = (data->apex_status & ICM42X70_APEX_STATUS_MASK_WOM_Z) ? 1 : 0;
545 } else if ((cfg->apex == TDK_APEX_TILT) || (cfg->apex == TDK_APEX_SMD)) {
546 val[0].val1 = data->apex_status;
547 }
548 #endif
549 } else {
550 res = -ENOTSUP;
551 }
552
553 icm42x70_unlock(dev);
554
555 return res;
556 }
557
558 #ifdef CONFIG_ICM42X70_TRIGGER
icm42x70_fetch_from_fifo(const struct device * dev)559 static int icm42x70_fetch_from_fifo(const struct device *dev)
560 {
561 struct icm42x70_data *data = dev->data;
562 int status = 0;
563 uint8_t int_status;
564 uint16_t packet_size = FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_GYRO_DATA_SIZE +
565 FIFO_TEMP_DATA_SIZE + FIFO_TS_FSYNC_SIZE;
566 uint16_t fifo_idx = 0;
567
568 /* Ensure data ready status bit is set */
569 status |= inv_imu_read_reg(&data->driver, INT_STATUS, 1, &int_status);
570 if (status != 0) {
571 return status;
572 }
573
574 if ((int_status & INT_STATUS_FIFO_THS_INT_MASK) ||
575 (int_status & INT_STATUS_FIFO_FULL_INT_MASK)) {
576 uint16_t packet_count;
577
578 /* Make sure RCOSC is enabled to guarrantee FIFO read */
579 status |= inv_imu_switch_on_mclk(&data->driver);
580
581 /* Read FIFO frame count */
582 status |= inv_imu_get_frame_count(&data->driver, &packet_count);
583
584 /* Check for error */
585 if (status != 0) {
586 status |= inv_imu_switch_off_mclk(&data->driver);
587 return status;
588 }
589
590 /* Read FIFO data */
591 status |= inv_imu_read_reg(&data->driver, FIFO_DATA, packet_size * packet_count,
592 (uint8_t *)&data->driver.fifo_data);
593
594 /* Check for error */
595 if (status != 0) {
596 status |= inv_imu_reset_fifo(&data->driver);
597 status |= inv_imu_switch_off_mclk(&data->driver);
598 return status;
599 }
600
601 for (uint16_t i = 0; i < packet_count; i++) {
602 inv_imu_sensor_event_t event;
603
604 status |= inv_imu_decode_fifo_frame(
605 &data->driver, &data->driver.fifo_data[fifo_idx], &event);
606 fifo_idx += packet_size;
607
608 /* Check for error */
609 if (status != 0) {
610 status |= inv_imu_reset_fifo(&data->driver);
611 status |= inv_imu_switch_off_mclk(&data->driver);
612 return status;
613 }
614
615 if (event.sensor_mask & (1 << INV_SENSOR_ACCEL)) {
616 data->accel_x = event.accel[0];
617 data->accel_y = event.accel[1];
618 data->accel_z = event.accel[2];
619 }
620 #if CONFIG_USE_EMD_ICM42670
621 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
622 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
623 if (event.sensor_mask & (1 << INV_SENSOR_GYRO)) {
624 data->gyro_x = event.gyro[0];
625 data->gyro_y = event.gyro[1];
626 data->gyro_z = event.gyro[2];
627 }
628 }
629 #endif
630 if (event.sensor_mask & (1 << INV_SENSOR_TEMPERATURE)) {
631 data->temp = event.temperature;
632 }
633 /*
634 * TODO use the sensor streaming interface with RTIO to handle multiple
635 * samples in FIFO
636 */
637
638 } /* end of FIFO read for loop */
639
640 status |= inv_imu_switch_off_mclk(&data->driver);
641 if (status < 0) {
642 return status;
643 }
644 } /*else: FIFO threshold was not reached and FIFO was not full */
645
646 return 0;
647 }
648 #endif
649
650 #ifndef CONFIG_ICM42X70_TRIGGER
icm42x70_sample_fetch_accel(const struct device * dev)651 static int icm42x70_sample_fetch_accel(const struct device *dev)
652 {
653 struct icm42x70_data *data = dev->data;
654 uint8_t buffer[ACCEL_DATA_SIZE];
655
656 int res = inv_imu_read_reg(&data->driver, ACCEL_DATA_X1, ACCEL_DATA_SIZE, buffer);
657
658 if (res) {
659 return res;
660 }
661
662 data->accel_x = (int16_t)sys_get_be16(&buffer[0]);
663 data->accel_y = (int16_t)sys_get_be16(&buffer[2]);
664 data->accel_z = (int16_t)sys_get_be16(&buffer[4]);
665
666 return 0;
667 }
668
icm42x70_sample_fetch_temp(const struct device * dev)669 static int icm42x70_sample_fetch_temp(const struct device *dev)
670 {
671 struct icm42x70_data *data = dev->data;
672 uint8_t buffer[TEMP_DATA_SIZE];
673
674 int res = inv_imu_read_reg(&data->driver, TEMP_DATA1, TEMP_DATA_SIZE, buffer);
675
676 if (res) {
677 return res;
678 }
679
680 data->temp = (int16_t)sys_get_be16(&buffer[0]);
681
682 return 0;
683 }
684
icm42x70_fetch_from_registers(const struct device * dev,enum sensor_channel chan)685 static int icm42x70_fetch_from_registers(const struct device *dev, enum sensor_channel chan)
686 {
687 struct icm42x70_data *data = dev->data;
688 int res = 0;
689 uint8_t int_status;
690
691 LOG_DBG("Fetch from reg");
692
693 icm42x70_lock(dev);
694
695 /* Ensure data ready status bit is set */
696 int err = inv_imu_read_reg(&data->driver, INT_STATUS_DRDY, 1, &int_status);
697
698 if (int_status & INT_STATUS_DRDY_DATA_RDY_INT_MASK) {
699 switch (chan) {
700 case SENSOR_CHAN_ALL:
701 err |= icm42x70_sample_fetch_accel(dev);
702 #if CONFIG_USE_EMD_ICM42670
703 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
704 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
705 err |= icm42670_sample_fetch_gyro(dev);
706 }
707 #endif
708 err |= icm42x70_sample_fetch_temp(dev);
709 break;
710 case SENSOR_CHAN_ACCEL_XYZ:
711 case SENSOR_CHAN_ACCEL_X:
712 case SENSOR_CHAN_ACCEL_Y:
713 case SENSOR_CHAN_ACCEL_Z:
714 err |= icm42x70_sample_fetch_accel(dev);
715 break;
716 #if CONFIG_USE_EMD_ICM42670
717 case SENSOR_CHAN_GYRO_XYZ:
718 case SENSOR_CHAN_GYRO_X:
719 case SENSOR_CHAN_GYRO_Y:
720 case SENSOR_CHAN_GYRO_Z:
721 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
722 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
723 err |= icm42670_sample_fetch_gyro(dev);
724 } else {
725 res = -ENOTSUP;
726 }
727 break;
728 #endif
729 case SENSOR_CHAN_DIE_TEMP:
730 err |= icm42x70_sample_fetch_temp(dev);
731 break;
732 default:
733 res = -ENOTSUP;
734 break;
735 }
736 }
737
738 icm42x70_unlock(dev);
739
740 if (err < 0) {
741 res = -EIO;
742 }
743 return res;
744 }
745 #endif
746
icm42x70_sample_fetch(const struct device * dev,enum sensor_channel chan)747 static int icm42x70_sample_fetch(const struct device *dev, enum sensor_channel chan)
748 {
749 #if CONFIG_USE_EMD_ICM42670
750 struct icm42x70_data *data = dev->data;
751 #endif
752 int status = -ENOTSUP;
753
754 icm42x70_lock(dev);
755
756 #ifdef CONFIG_TDK_APEX
757 if ((enum sensor_channel_tdk_apex)chan == SENSOR_CHAN_APEX_MOTION) {
758 status = icm42x70_apex_fetch_from_dmp(dev);
759 }
760 #endif
761
762 if ((chan == SENSOR_CHAN_ALL) || SENSOR_CHANNEL_IS_ACCEL(chan) ||
763 #if CONFIG_USE_EMD_ICM42670
764 (SENSOR_CHANNEL_IS_GYRO(chan) && ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
765 (data->imu_whoami == INV_ICM42670S_WHOAMI))) ||
766 #endif
767 (chan == SENSOR_CHAN_DIE_TEMP)) {
768 #ifdef CONFIG_ICM42X70_TRIGGER
769 status = icm42x70_fetch_from_fifo(dev);
770 #else
771 status = icm42x70_fetch_from_registers(dev, chan);
772 #endif
773 }
774
775 icm42x70_unlock(dev);
776 return status;
777 }
778
icm42x70_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)779 static int icm42x70_attr_set(const struct device *dev, enum sensor_channel chan,
780 enum sensor_attribute attr, const struct sensor_value *val)
781 {
782 struct icm42x70_data *drv_data = dev->data;
783
784 __ASSERT_NO_MSG(val != NULL);
785
786 icm42x70_lock(dev);
787
788 if ((enum sensor_channel_tdk_apex)chan == SENSOR_CHAN_APEX_MOTION) {
789 if (attr == SENSOR_ATTR_CONFIGURATION) {
790 #ifdef CONFIG_TDK_APEX
791 if (val->val1 == TDK_APEX_PEDOMETER) {
792 icm42x70_apex_enable(&drv_data->driver);
793 icm42x70_apex_enable_pedometer(dev, &drv_data->driver);
794 } else if (val->val1 == TDK_APEX_TILT) {
795 icm42x70_apex_enable(&drv_data->driver);
796 icm42x70_apex_enable_tilt(&drv_data->driver);
797 } else if (val->val1 == TDK_APEX_SMD) {
798 icm42x70_apex_enable(&drv_data->driver);
799 icm42x70_apex_enable_smd(&drv_data->driver);
800 } else if (val->val1 == TDK_APEX_WOM) {
801 icm42x70_apex_enable_wom(&drv_data->driver);
802 } else {
803 LOG_ERR("Not supported ATTR value");
804 }
805 #endif
806 } else {
807 LOG_ERR("Not supported ATTR");
808 return -EINVAL;
809 }
810 } else if (SENSOR_CHANNEL_IS_ACCEL(chan)) {
811 icm42x70_accel_config(drv_data, attr, val);
812 #if CONFIG_USE_EMD_ICM42670
813 } else if ((SENSOR_CHANNEL_IS_GYRO(chan)) &&
814 ((drv_data->imu_whoami == INV_ICM42670P_WHOAMI) ||
815 (drv_data->imu_whoami == INV_ICM42670S_WHOAMI))) {
816 icm42670_gyro_config(drv_data, attr, val);
817 #endif
818 } else {
819 LOG_ERR("Unsupported channel");
820 (void)drv_data;
821 return -EINVAL;
822 }
823
824 icm42x70_unlock(dev);
825
826 return 0;
827 }
828
icm42x70_attr_get(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,struct sensor_value * val)829 static int icm42x70_attr_get(const struct device *dev, enum sensor_channel chan,
830 enum sensor_attribute attr, struct sensor_value *val)
831 {
832 const struct icm42x70_data *data = dev->data;
833 const struct icm42x70_config *cfg = dev->config;
834 int res = 0;
835
836 __ASSERT_NO_MSG(val != NULL);
837
838 icm42x70_lock(dev);
839
840 switch (chan) {
841 case SENSOR_CHAN_ACCEL_X:
842 case SENSOR_CHAN_ACCEL_Y:
843 case SENSOR_CHAN_ACCEL_Z:
844 case SENSOR_CHAN_ACCEL_XYZ:
845 if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
846 val->val1 = data->accel_hz;
847 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
848 val->val1 = data->accel_fs;
849 } else {
850 LOG_ERR("Unsupported attribute");
851 res = -EINVAL;
852 }
853 break;
854 #if CONFIG_USE_EMD_ICM42670
855 case SENSOR_CHAN_GYRO_X:
856 case SENSOR_CHAN_GYRO_Y:
857 case SENSOR_CHAN_GYRO_Z:
858 case SENSOR_CHAN_GYRO_XYZ:
859 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
860 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
861 if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
862 val->val1 = data->gyro_hz;
863 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
864 val->val1 = data->gyro_fs;
865 } else {
866 LOG_ERR("Unsupported attribute");
867 res = -EINVAL;
868 }
869 } else {
870 res = -EINVAL;
871 }
872 break;
873 #endif
874 case SENSOR_CHAN_APEX_MOTION:
875 if (attr == SENSOR_ATTR_CONFIGURATION) {
876 val->val1 = cfg->apex;
877 }
878 break;
879
880 default:
881 LOG_ERR("Unsupported channel");
882 res = -EINVAL;
883 break;
884 }
885
886 icm42x70_unlock(dev);
887
888 return res;
889 }
890
icm42x70_bus_check(const struct device * dev)891 static inline int icm42x70_bus_check(const struct device *dev)
892 {
893 const struct icm42x70_config *cfg = dev->config;
894
895 return cfg->bus_io->check(&cfg->bus);
896 }
897
icm42x70_init(const struct device * dev)898 static int icm42x70_init(const struct device *dev)
899 {
900 struct icm42x70_data *data = dev->data;
901 int res = 0;
902
903 if (icm42x70_bus_check(dev) < 0) {
904 LOG_ERR("bus check failed");
905 return -ENODEV;
906 }
907
908 data->accel_x = 0;
909 data->accel_y = 0;
910 data->accel_z = 0;
911 #if CONFIG_USE_EMD_ICM42670
912 if ((data->imu_whoami == INV_ICM42670P_WHOAMI) ||
913 (data->imu_whoami == INV_ICM42670S_WHOAMI)) {
914 data->gyro_x = 0;
915 data->gyro_y = 0;
916 data->gyro_z = 0;
917 }
918 #endif
919 data->temp = 0;
920
921 if (icm42x70_sensor_init(dev)) {
922 LOG_ERR("could not initialize sensor");
923 return -EIO;
924 }
925
926 #ifdef CONFIG_ICM42X70_TRIGGER
927 res |= icm42x70_trigger_enable_interrupt(dev);
928 res |= icm42x70_trigger_init(dev);
929 if (res < 0) {
930 LOG_ERR("Failed to initialize interrupt.");
931 return res;
932 }
933 #endif
934
935 res |= icm42x70_turn_on_sensor(dev);
936
937 return res;
938 }
939
940 #ifndef CONFIG_ICM42X70_TRIGGER
941
icm42x70_lock(const struct device * dev)942 void icm42x70_lock(const struct device *dev)
943 {
944 ARG_UNUSED(dev);
945 }
946
icm42x70_unlock(const struct device * dev)947 void icm42x70_unlock(const struct device *dev)
948 {
949 ARG_UNUSED(dev);
950 }
951
952 #endif
953
954 static DEVICE_API(sensor, icm42x70_driver_api) = {
955 #ifdef CONFIG_ICM42X70_TRIGGER
956 .trigger_set = icm42x70_trigger_set,
957 #endif
958 .sample_fetch = icm42x70_sample_fetch,
959 .channel_get = icm42x70_channel_get,
960 .attr_set = icm42x70_attr_set,
961 .attr_get = icm42x70_attr_get,
962 };
963
964 /* device defaults to spi mode 0/3 support */
965 #define ICM42X70_SPI_CFG (SPI_WORD_SET(8) | SPI_TRANSFER_MSB | SPI_MODE_CPOL | SPI_MODE_CPHA)
966
967 /* Initializes a common struct icm42x70_config */
968 #define ICM42X70_CONFIG_COMMON(inst) \
969 IF_ENABLED(CONFIG_ICM42X70_TRIGGER, \
970 (.gpio_int = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}),)) \
971 .accel_fs = DT_INST_ENUM_IDX(inst, accel_fs), \
972 .accel_hz = DT_INST_ENUM_IDX(inst, accel_hz), \
973 .accel_avg = DT_INST_ENUM_IDX(inst, accel_avg), \
974 .accel_filt_bw = DT_INST_ENUM_IDX(inst, accel_filt_bw_hz), \
975 .accel_pwr_mode = DT_INST_ENUM_IDX(inst, power_mode), \
976 .apex = DT_INST_ENUM_IDX(inst, apex), \
977 .accel_pwr_mode = DT_INST_ENUM_IDX(inst, power_mode), \
978 .apex = DT_INST_ENUM_IDX(inst, apex), \
979 IF_ENABLED(CONFIG_USE_EMD_ICM42670, \
980 (.gyro_fs = DT_INST_ENUM_IDX(inst, gyro_fs),)) \
981 IF_ENABLED(CONFIG_USE_EMD_ICM42670, \
982 (.gyro_hz = DT_INST_ENUM_IDX(inst, gyro_hz),)) \
983 IF_ENABLED(CONFIG_USE_EMD_ICM42670, \
984 (.gyro_filt_bw = DT_INST_ENUM_IDX(inst, gyro_filt_bw_hz),))
985
986 /* Initializes the bus members for an instance on a SPI bus. */
987 #define ICM42X70_CONFIG_SPI(inst) \
988 {.bus.spi = SPI_DT_SPEC_INST_GET(inst, ICM42X70_SPI_CFG, 0), \
989 .bus_io = &icm42x70_bus_io_spi, \
990 .serif_type = UI_SPI4, \
991 ICM42X70_CONFIG_COMMON(inst)}
992
993 /* Initializes the bus members for an instance on an I2C bus. */
994 #define ICM42X70_CONFIG_I2C(inst) \
995 {.bus.i2c = I2C_DT_SPEC_INST_GET(inst), \
996 .bus_io = &icm42x70_bus_io_i2c, \
997 .serif_type = UI_I2C, \
998 ICM42X70_CONFIG_COMMON(inst)}
999
1000 /*
1001 * Main instantiation macro, which selects the correct bus-specific
1002 * instantiation macros for the instance.
1003 */
1004 #define ICM42X70_DEFINE(inst, name, whoami) \
1005 static struct icm42x70_data icm42x70_data_##inst = { \
1006 .imu_name = name, \
1007 .imu_whoami = whoami, \
1008 }; \
1009 static const struct icm42x70_config icm42x70_config_##inst = \
1010 COND_CODE_1(DT_INST_ON_BUS(inst, spi), \
1011 (ICM42X70_CONFIG_SPI(inst)), \
1012 (ICM42X70_CONFIG_I2C(inst))); \
1013 SENSOR_DEVICE_DT_INST_DEFINE(inst, icm42x70_init, NULL, &icm42x70_data_##inst, \
1014 &icm42x70_config_##inst, POST_KERNEL, \
1015 CONFIG_SENSOR_INIT_PRIORITY, &icm42x70_driver_api);
1016
1017 #define DT_DRV_COMPAT invensense_icm42670p
1018 #if DT_HAS_COMPAT_STATUS_OKAY(DT_DRV_COMPAT)
1019 DT_INST_FOREACH_STATUS_OKAY_VARGS(ICM42X70_DEFINE, INV_ICM42670P_STRING_ID, INV_ICM42670P_WHOAMI);
1020 #endif
1021 #undef DT_DRV_COMPAT
1022
1023 #define DT_DRV_COMPAT invensense_icm42670s
1024 #if DT_HAS_COMPAT_STATUS_OKAY(DT_DRV_COMPAT)
1025 DT_INST_FOREACH_STATUS_OKAY_VARGS(ICM42X70_DEFINE, INV_ICM42670S_STRING_ID, INV_ICM42670S_WHOAMI);
1026 #endif
1027 #undef DT_DRV_COMPAT
1028
1029 #define DT_DRV_COMPAT invensense_icm42370p
1030 #if DT_HAS_COMPAT_STATUS_OKAY(DT_DRV_COMPAT)
1031 DT_INST_FOREACH_STATUS_OKAY_VARGS(ICM42X70_DEFINE, INV_ICM42370P_STRING_ID, INV_ICM42370P_WHOAMI);
1032 #endif
1033
