1 /*
2 * Copyright (c) 2023 Google LLC
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "icm42688_decoder.h"
8 #include "icm42688_reg.h"
9 #include "icm42688.h"
10 #include <errno.h>
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(ICM42688_DECODER, CONFIG_SENSOR_LOG_LEVEL);
14
15 #define DT_DRV_COMPAT invensense_icm42688
16
icm42688_get_shift(enum sensor_channel channel,int accel_fs,int gyro_fs,int8_t * shift)17 static int icm42688_get_shift(enum sensor_channel channel, int accel_fs, int gyro_fs, int8_t *shift)
18 {
19 switch (channel) {
20 case SENSOR_CHAN_ACCEL_XYZ:
21 case SENSOR_CHAN_ACCEL_X:
22 case SENSOR_CHAN_ACCEL_Y:
23 case SENSOR_CHAN_ACCEL_Z:
24 switch (accel_fs) {
25 case ICM42688_DT_ACCEL_FS_2:
26 *shift = 5;
27 return 0;
28 case ICM42688_DT_ACCEL_FS_4:
29 *shift = 6;
30 return 0;
31 case ICM42688_DT_ACCEL_FS_8:
32 *shift = 7;
33 return 0;
34 case ICM42688_DT_ACCEL_FS_16:
35 *shift = 8;
36 return 0;
37 default:
38 return -EINVAL;
39 }
40 case SENSOR_CHAN_GYRO_XYZ:
41 case SENSOR_CHAN_GYRO_X:
42 case SENSOR_CHAN_GYRO_Y:
43 case SENSOR_CHAN_GYRO_Z:
44 switch (gyro_fs) {
45 case ICM42688_DT_GYRO_FS_15_625:
46 *shift = -1;
47 return 0;
48 case ICM42688_DT_GYRO_FS_31_25:
49 *shift = 0;
50 return 0;
51 case ICM42688_DT_GYRO_FS_62_5:
52 *shift = 1;
53 return 0;
54 case ICM42688_DT_GYRO_FS_125:
55 *shift = 2;
56 return 0;
57 case ICM42688_DT_GYRO_FS_250:
58 *shift = 3;
59 return 0;
60 case ICM42688_DT_GYRO_FS_500:
61 *shift = 4;
62 return 0;
63 case ICM42688_DT_GYRO_FS_1000:
64 *shift = 5;
65 return 0;
66 case ICM42688_DT_GYRO_FS_2000:
67 *shift = 6;
68 return 0;
69 default:
70 return -EINVAL;
71 }
72 case SENSOR_CHAN_DIE_TEMP:
73 *shift = 9;
74 return 0;
75 default:
76 return -EINVAL;
77 }
78 }
79
icm42688_convert_raw_to_q31(struct icm42688_cfg * cfg,enum sensor_channel chan,int32_t reading,q31_t * out)80 int icm42688_convert_raw_to_q31(struct icm42688_cfg *cfg, enum sensor_channel chan, int32_t reading,
81 q31_t *out)
82 {
83 int32_t whole;
84 int32_t fraction;
85 int64_t intermediate;
86 int8_t shift;
87 int rc;
88
89 rc = icm42688_get_shift(chan, cfg->accel_fs, cfg->gyro_fs, &shift);
90 if (rc != 0) {
91 return rc;
92 }
93
94 switch (chan) {
95 case SENSOR_CHAN_ACCEL_XYZ:
96 case SENSOR_CHAN_ACCEL_X:
97 case SENSOR_CHAN_ACCEL_Y:
98 case SENSOR_CHAN_ACCEL_Z:
99 icm42688_accel_ms(cfg, reading, &whole, &fraction);
100 break;
101 case SENSOR_CHAN_GYRO_XYZ:
102 case SENSOR_CHAN_GYRO_X:
103 case SENSOR_CHAN_GYRO_Y:
104 case SENSOR_CHAN_GYRO_Z:
105 icm42688_gyro_rads(cfg, reading, &whole, &fraction);
106 break;
107 case SENSOR_CHAN_DIE_TEMP:
108 icm42688_temp_c(reading, &whole, &fraction);
109 break;
110 default:
111 return -ENOTSUP;
112 }
113 intermediate = ((int64_t)whole * INT64_C(1000000) + fraction);
114 if (shift < 0) {
115 intermediate =
116 intermediate * ((int64_t)INT32_MAX + 1) * (1 << -shift) / INT64_C(1000000);
117 } else if (shift > 0) {
118 intermediate =
119 intermediate * ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000));
120 }
121 *out = CLAMP(intermediate, INT32_MIN, INT32_MAX);
122
123 return 0;
124 }
125
icm42688_get_channel_position(enum sensor_channel chan)126 static int icm42688_get_channel_position(enum sensor_channel chan)
127 {
128 switch (chan) {
129 case SENSOR_CHAN_DIE_TEMP:
130 return 0;
131 case SENSOR_CHAN_ACCEL_XYZ:
132 case SENSOR_CHAN_ACCEL_X:
133 return 1;
134 case SENSOR_CHAN_ACCEL_Y:
135 return 2;
136 case SENSOR_CHAN_ACCEL_Z:
137 return 3;
138 case SENSOR_CHAN_GYRO_XYZ:
139 case SENSOR_CHAN_GYRO_X:
140 return 4;
141 case SENSOR_CHAN_GYRO_Y:
142 return 5;
143 case SENSOR_CHAN_GYRO_Z:
144 return 6;
145 default:
146 return 0;
147 }
148 }
149
icm42688_encode_channel(enum sensor_channel chan)150 static uint8_t icm42688_encode_channel(enum sensor_channel chan)
151 {
152 uint8_t encode_bmask = 0;
153
154 switch (chan) {
155 case SENSOR_CHAN_DIE_TEMP:
156 case SENSOR_CHAN_ACCEL_X:
157 case SENSOR_CHAN_ACCEL_Y:
158 case SENSOR_CHAN_ACCEL_Z:
159 case SENSOR_CHAN_GYRO_X:
160 case SENSOR_CHAN_GYRO_Y:
161 case SENSOR_CHAN_GYRO_Z:
162 encode_bmask = BIT(icm42688_get_channel_position(chan));
163 break;
164 case SENSOR_CHAN_ACCEL_XYZ:
165 encode_bmask = BIT(icm42688_get_channel_position(SENSOR_CHAN_ACCEL_X)) |
166 BIT(icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Y)) |
167 BIT(icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Z));
168 break;
169 case SENSOR_CHAN_GYRO_XYZ:
170 encode_bmask = BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_X)) |
171 BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_Y)) |
172 BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_Z));
173 break;
174 default:
175 break;
176 }
177
178 return encode_bmask;
179 }
180
icm42688_encode(const struct device * dev,const struct sensor_chan_spec * const channels,const size_t num_channels,uint8_t * buf)181 int icm42688_encode(const struct device *dev, const struct sensor_chan_spec *const channels,
182 const size_t num_channels, uint8_t *buf)
183 {
184 struct icm42688_dev_data *data = dev->data;
185 struct icm42688_encoded_data *edata = (struct icm42688_encoded_data *)buf;
186
187 edata->channels = 0;
188
189 for (int i = 0; i < num_channels; i++) {
190 edata->channels |= icm42688_encode_channel(channels[i].chan_type);
191 }
192
193 edata->header.is_fifo = false;
194 edata->header.accel_fs = data->cfg.accel_fs;
195 edata->header.gyro_fs = data->cfg.gyro_fs;
196 edata->header.timestamp = k_ticks_to_ns_floor64(k_uptime_ticks());
197
198 return 0;
199 }
200
201 #define IS_ACCEL(chan) ((chan) >= SENSOR_CHAN_ACCEL_X && (chan) <= SENSOR_CHAN_ACCEL_XYZ)
202 #define IS_GYRO(chan) ((chan) >= SENSOR_CHAN_GYRO_X && (chan) <= SENSOR_CHAN_GYRO_XYZ)
203
icm42688_read_temperature_from_packet(const uint8_t * pkt)204 static inline q31_t icm42688_read_temperature_from_packet(const uint8_t *pkt)
205 {
206 int32_t temperature;
207 int32_t whole;
208 int32_t fraction;
209
210 /* Temperature always assumes a shift of 9 for a range of (-273,273) C */
211 if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) {
212 temperature = (pkt[0xd] << 8) | pkt[0xe];
213
214 icm42688_temp_c(temperature, &whole, &fraction);
215 } else {
216 if (FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1 &&
217 FIELD_GET(FIFO_HEADER_GYRO, pkt[0]) == 1) {
218 temperature = pkt[0xd];
219 } else {
220 temperature = pkt[0x7];
221 }
222
223 int64_t sensitivity = 207;
224 int64_t temperature100 = (temperature * 100) + (25 * sensitivity);
225
226 whole = temperature100 / sensitivity;
227 fraction =
228 ((temperature100 - whole * sensitivity) * INT64_C(1000000)) / sensitivity;
229 }
230 __ASSERT_NO_MSG(whole >= -512 && whole <= 511);
231 return FIELD_PREP(GENMASK(31, 22), whole) | (fraction * GENMASK64(21, 0) / 1000000);
232 }
233
icm42688_read_imu_from_packet(const uint8_t * pkt,bool is_accel,int fs,uint8_t axis_offset,q31_t * out)234 static int icm42688_read_imu_from_packet(const uint8_t *pkt, bool is_accel, int fs,
235 uint8_t axis_offset, q31_t *out)
236 {
237 int32_t value;
238 int64_t scale = 0;
239 int32_t max = BIT(15);
240 int offset = 1 + (axis_offset * 2);
241
242 if (is_accel) {
243 switch (fs) {
244 case ICM42688_DT_ACCEL_FS_2:
245 scale = INT64_C(2) * BIT(31 - 5) * 9.80665;
246 break;
247 case ICM42688_DT_ACCEL_FS_4:
248 scale = INT64_C(4) * BIT(31 - 6) * 9.80665;
249 break;
250 case ICM42688_DT_ACCEL_FS_8:
251 scale = INT64_C(8) * BIT(31 - 7) * 9.80665;
252 break;
253 case ICM42688_DT_ACCEL_FS_16:
254 scale = INT64_C(16) * BIT(31 - 8) * 9.80665;
255 break;
256 }
257 } else {
258 switch (fs) {
259 case ICM42688_DT_GYRO_FS_2000:
260 scale = 164;
261 break;
262 case ICM42688_DT_GYRO_FS_1000:
263 scale = 328;
264 break;
265 case ICM42688_DT_GYRO_FS_500:
266 scale = 655;
267 break;
268 case ICM42688_DT_GYRO_FS_250:
269 scale = 1310;
270 break;
271 case ICM42688_DT_GYRO_FS_125:
272 scale = 2620;
273 break;
274 case ICM42688_DT_GYRO_FS_62_5:
275 scale = 5243;
276 break;
277 case ICM42688_DT_GYRO_FS_31_25:
278 scale = 10486;
279 break;
280 case ICM42688_DT_GYRO_FS_15_625:
281 scale = 20972;
282 break;
283 }
284 }
285
286 if (!is_accel && FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1) {
287 offset += 7;
288 }
289
290 value = (int16_t)sys_le16_to_cpu((pkt[offset] << 8) | pkt[offset + 1]);
291
292 if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) {
293 uint32_t mask = is_accel ? GENMASK(7, 4) : GENMASK(3, 0);
294
295 offset = 0x11 + axis_offset;
296 value = (value << 4) | FIELD_GET(mask, pkt[offset]);
297 /* In 20 bit mode, FS can only be +/-16g and +/-2000dps */
298 scale = is_accel ? (INT64_C(16) * BIT(8) * 9.80665) : 131;
299 max = is_accel ? BIT(18) : BIT(19);
300 if (value == -524288) {
301 /* Invalid 20 bit value */
302 return -ENODATA;
303 }
304 } else {
305 if (value <= -32767) {
306 /* Invalid 16 bit value */
307 return -ENODATA;
308 }
309 }
310
311 *out = (q31_t)(value * scale / max);
312 return 0;
313 }
314
315 static uint32_t accel_period_ns[] = {
316 [ICM42688_DT_ACCEL_ODR_1_5625] = UINT32_C(10000000000000) / 15625,
317 [ICM42688_DT_ACCEL_ODR_3_125] = UINT32_C(10000000000000) / 31250,
318 [ICM42688_DT_ACCEL_ODR_6_25] = UINT32_C(10000000000000) / 62500,
319 [ICM42688_DT_ACCEL_ODR_12_5] = UINT32_C(10000000000000) / 12500,
320 [ICM42688_DT_ACCEL_ODR_25] = UINT32_C(1000000000) / 25,
321 [ICM42688_DT_ACCEL_ODR_50] = UINT32_C(1000000000) / 50,
322 [ICM42688_DT_ACCEL_ODR_100] = UINT32_C(1000000000) / 100,
323 [ICM42688_DT_ACCEL_ODR_200] = UINT32_C(1000000000) / 200,
324 [ICM42688_DT_ACCEL_ODR_500] = UINT32_C(1000000000) / 500,
325 [ICM42688_DT_ACCEL_ODR_1000] = UINT32_C(1000000),
326 [ICM42688_DT_ACCEL_ODR_2000] = UINT32_C(1000000) / 2,
327 [ICM42688_DT_ACCEL_ODR_4000] = UINT32_C(1000000) / 4,
328 [ICM42688_DT_ACCEL_ODR_8000] = UINT32_C(1000000) / 8,
329 [ICM42688_DT_ACCEL_ODR_16000] = UINT32_C(1000000) / 16,
330 [ICM42688_DT_ACCEL_ODR_32000] = UINT32_C(1000000) / 32,
331 };
332
333 static uint32_t gyro_period_ns[] = {
334 [ICM42688_DT_GYRO_ODR_12_5] = UINT32_C(10000000000000) / 12500,
335 [ICM42688_DT_GYRO_ODR_25] = UINT32_C(1000000000) / 25,
336 [ICM42688_DT_GYRO_ODR_50] = UINT32_C(1000000000) / 50,
337 [ICM42688_DT_GYRO_ODR_100] = UINT32_C(1000000000) / 100,
338 [ICM42688_DT_GYRO_ODR_200] = UINT32_C(1000000000) / 200,
339 [ICM42688_DT_GYRO_ODR_500] = UINT32_C(1000000000) / 500,
340 [ICM42688_DT_GYRO_ODR_1000] = UINT32_C(1000000),
341 [ICM42688_DT_GYRO_ODR_2000] = UINT32_C(1000000) / 2,
342 [ICM42688_DT_GYRO_ODR_4000] = UINT32_C(1000000) / 4,
343 [ICM42688_DT_GYRO_ODR_8000] = UINT32_C(1000000) / 8,
344 [ICM42688_DT_GYRO_ODR_16000] = UINT32_C(1000000) / 16,
345 [ICM42688_DT_GYRO_ODR_32000] = UINT32_C(1000000) / 32,
346 };
347
icm42688_fifo_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)348 static int icm42688_fifo_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
349 uint32_t *fit, uint16_t max_count, void *data_out)
350 {
351 const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer;
352 const uint8_t *buffer_end = buffer + sizeof(struct icm42688_fifo_data) + edata->fifo_count;
353 int accel_frame_count = 0;
354 int gyro_frame_count = 0;
355 int count = 0;
356 int rc;
357
358 if ((uintptr_t)buffer_end <= *fit || chan_spec.chan_idx != 0) {
359 return 0;
360 }
361
362 ((struct sensor_data_header *)data_out)->base_timestamp_ns = edata->header.timestamp;
363
364 buffer += sizeof(struct icm42688_fifo_data);
365 while (count < max_count && buffer < buffer_end) {
366 const bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]) == 1;
367 const bool has_accel = FIELD_GET(FIFO_HEADER_ACCEL, buffer[0]) == 1;
368 const bool has_gyro = FIELD_GET(FIFO_HEADER_GYRO, buffer[0]) == 1;
369 const uint8_t *frame_end = buffer;
370
371 if (is_20b) {
372 frame_end += 20;
373 } else if (has_accel && has_gyro) {
374 frame_end += 16;
375 } else {
376 frame_end += 8;
377 }
378 if (has_accel) {
379 accel_frame_count++;
380 }
381 if (has_gyro) {
382 gyro_frame_count++;
383 }
384
385 if ((uintptr_t)buffer < *fit) {
386 /* This frame was already decoded, move on to the next frame */
387 buffer = frame_end;
388 continue;
389 }
390 if (chan_spec.chan_type == SENSOR_CHAN_DIE_TEMP) {
391 struct sensor_q31_data *data = (struct sensor_q31_data *)data_out;
392
393 data->shift = 9;
394 if (has_accel) {
395 data->readings[count].timestamp_delta =
396 accel_period_ns[edata->accel_odr] * (accel_frame_count - 1);
397 } else {
398 data->readings[count].timestamp_delta =
399 gyro_period_ns[edata->gyro_odr] * (gyro_frame_count - 1);
400 }
401 data->readings[count].temperature =
402 icm42688_read_temperature_from_packet(buffer);
403 } else if (IS_ACCEL(chan_spec.chan_type) && has_accel) {
404 /* Decode accel */
405 struct sensor_three_axis_data *data =
406 (struct sensor_three_axis_data *)data_out;
407 uint64_t period_ns = accel_period_ns[edata->accel_odr];
408
409 icm42688_get_shift(SENSOR_CHAN_ACCEL_XYZ, edata->header.accel_fs,
410 edata->header.gyro_fs, &data->shift);
411
412 data->readings[count].timestamp_delta = (accel_frame_count - 1) * period_ns;
413 rc = icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 0,
414 &data->readings[count].x);
415 rc |= icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 1,
416 &data->readings[count].y);
417 rc |= icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 2,
418 &data->readings[count].z);
419 if (rc != 0) {
420 accel_frame_count--;
421 buffer = frame_end;
422 continue;
423 }
424 } else if (IS_GYRO(chan_spec.chan_type) && has_gyro) {
425 /* Decode gyro */
426 struct sensor_three_axis_data *data =
427 (struct sensor_three_axis_data *)data_out;
428 uint64_t period_ns = accel_period_ns[edata->gyro_odr];
429
430 icm42688_get_shift(SENSOR_CHAN_GYRO_XYZ, edata->header.accel_fs,
431 edata->header.gyro_fs, &data->shift);
432
433 data->readings[count].timestamp_delta = (gyro_frame_count - 1) * period_ns;
434 rc = icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 0,
435 &data->readings[count].x);
436 rc |= icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 1,
437 &data->readings[count].y);
438 rc |= icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 2,
439 &data->readings[count].z);
440 if (rc != 0) {
441 gyro_frame_count--;
442 buffer = frame_end;
443 continue;
444 }
445 }
446 buffer = frame_end;
447 *fit = (uintptr_t)frame_end;
448 count++;
449 }
450 return count;
451 }
452
icm42688_one_shot_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)453 static int icm42688_one_shot_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
454 uint32_t *fit, uint16_t max_count, void *data_out)
455 {
456 const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer;
457 const struct icm42688_decoder_header *header = &edata->header;
458 struct icm42688_cfg cfg = {
459 .accel_fs = edata->header.accel_fs,
460 .gyro_fs = edata->header.gyro_fs,
461 };
462 uint8_t channel_request;
463 int rc;
464
465 if (*fit != 0) {
466 return 0;
467 }
468 if (max_count == 0 || chan_spec.chan_idx != 0) {
469 return -EINVAL;
470 }
471
472 switch (chan_spec.chan_type) {
473 case SENSOR_CHAN_ACCEL_X:
474 case SENSOR_CHAN_ACCEL_Y:
475 case SENSOR_CHAN_ACCEL_Z:
476 case SENSOR_CHAN_ACCEL_XYZ: {
477 channel_request = icm42688_encode_channel(SENSOR_CHAN_ACCEL_XYZ);
478 if ((channel_request & edata->channels) != channel_request) {
479 return -ENODATA;
480 }
481
482 struct sensor_three_axis_data *out = data_out;
483
484 out->header.base_timestamp_ns = edata->header.timestamp;
485 out->header.reading_count = 1;
486 rc = icm42688_get_shift(SENSOR_CHAN_ACCEL_XYZ, header->accel_fs, header->gyro_fs,
487 &out->shift);
488 if (rc != 0) {
489 return -EINVAL;
490 }
491
492 icm42688_convert_raw_to_q31(
493 &cfg, SENSOR_CHAN_ACCEL_X,
494 edata->readings[icm42688_get_channel_position(SENSOR_CHAN_ACCEL_X)],
495 &out->readings[0].x);
496 icm42688_convert_raw_to_q31(
497 &cfg, SENSOR_CHAN_ACCEL_Y,
498 edata->readings[icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Y)],
499 &out->readings[0].y);
500 icm42688_convert_raw_to_q31(
501 &cfg, SENSOR_CHAN_ACCEL_Z,
502 edata->readings[icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Z)],
503 &out->readings[0].z);
504 *fit = 1;
505 return 1;
506 }
507 case SENSOR_CHAN_GYRO_X:
508 case SENSOR_CHAN_GYRO_Y:
509 case SENSOR_CHAN_GYRO_Z:
510 case SENSOR_CHAN_GYRO_XYZ: {
511 channel_request = icm42688_encode_channel(SENSOR_CHAN_GYRO_XYZ);
512 if ((channel_request & edata->channels) != channel_request) {
513 return -ENODATA;
514 }
515
516 struct sensor_three_axis_data *out = data_out;
517
518 out->header.base_timestamp_ns = edata->header.timestamp;
519 out->header.reading_count = 1;
520 rc = icm42688_get_shift(SENSOR_CHAN_GYRO_XYZ, header->accel_fs, header->gyro_fs,
521 &out->shift);
522 if (rc != 0) {
523 return -EINVAL;
524 }
525
526 out->readings[0].timestamp_delta = 0;
527 icm42688_convert_raw_to_q31(
528 &cfg, SENSOR_CHAN_GYRO_X,
529 edata->readings[icm42688_get_channel_position(SENSOR_CHAN_GYRO_X)],
530 &out->readings[0].x);
531 icm42688_convert_raw_to_q31(
532 &cfg, SENSOR_CHAN_GYRO_Y,
533 edata->readings[icm42688_get_channel_position(SENSOR_CHAN_GYRO_Y)],
534 &out->readings[0].y);
535 icm42688_convert_raw_to_q31(
536 &cfg, SENSOR_CHAN_GYRO_Z,
537 edata->readings[icm42688_get_channel_position(SENSOR_CHAN_GYRO_Z)],
538 &out->readings[0].z);
539 *fit = 1;
540 return 1;
541 }
542 case SENSOR_CHAN_DIE_TEMP: {
543 channel_request = icm42688_encode_channel(SENSOR_CHAN_DIE_TEMP);
544 if ((channel_request & edata->channels) != channel_request) {
545 return -ENODATA;
546 }
547
548 struct sensor_q31_data *out = data_out;
549
550 out->header.base_timestamp_ns = edata->header.timestamp;
551 out->header.reading_count = 1;
552
553 rc = icm42688_get_shift(SENSOR_CHAN_DIE_TEMP, header->accel_fs, header->gyro_fs,
554 &out->shift);
555 if (rc != 0) {
556 return -EINVAL;
557 }
558 out->readings[0].timestamp_delta = 0;
559 icm42688_convert_raw_to_q31(
560 &cfg, SENSOR_CHAN_DIE_TEMP,
561 edata->readings[icm42688_get_channel_position(SENSOR_CHAN_DIE_TEMP)],
562 &out->readings[0].temperature);
563 *fit = 1;
564 return 1;
565 }
566 default:
567 return -EINVAL;
568 }
569 }
570
icm42688_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)571 static int icm42688_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
572 uint32_t *fit, uint16_t max_count, void *data_out)
573 {
574 const struct icm42688_decoder_header *header =
575 (const struct icm42688_decoder_header *)buffer;
576
577 if (header->is_fifo) {
578 return icm42688_fifo_decode(buffer, chan_spec, fit, max_count, data_out);
579 }
580 return icm42688_one_shot_decode(buffer, chan_spec, fit, max_count, data_out);
581 }
582
icm42688_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)583 static int icm42688_decoder_get_frame_count(const uint8_t *buffer,
584 struct sensor_chan_spec chan_spec,
585 uint16_t *frame_count)
586 {
587 const struct icm42688_fifo_data *data = (const struct icm42688_fifo_data *)buffer;
588 const struct icm42688_decoder_header *header = &data->header;
589
590 if (chan_spec.chan_idx != 0) {
591 return -ENOTSUP;
592 }
593
594 if (!header->is_fifo) {
595 switch (chan_spec.chan_type) {
596 case SENSOR_CHAN_ACCEL_X:
597 case SENSOR_CHAN_ACCEL_Y:
598 case SENSOR_CHAN_ACCEL_Z:
599 case SENSOR_CHAN_ACCEL_XYZ:
600 case SENSOR_CHAN_GYRO_X:
601 case SENSOR_CHAN_GYRO_Y:
602 case SENSOR_CHAN_GYRO_Z:
603 case SENSOR_CHAN_GYRO_XYZ:
604 case SENSOR_CHAN_DIE_TEMP:
605 *frame_count = 1;
606 return 0;
607 default:
608 return -ENOTSUP;
609 }
610 return 0;
611 }
612
613 /* Skip the header */
614 buffer += sizeof(struct icm42688_fifo_data);
615
616 uint16_t count = 0;
617 const uint8_t *end = buffer + data->fifo_count;
618
619 while (buffer < end) {
620 bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]);
621 int size = is_20b ? 3 : 2;
622
623 if (FIELD_GET(FIFO_HEADER_ACCEL, buffer[0])) {
624 size += 6;
625 }
626 if (FIELD_GET(FIFO_HEADER_GYRO, buffer[0])) {
627 size += 6;
628 }
629 if (FIELD_GET(FIFO_HEADER_TIMESTAMP_FSYNC, buffer[0])) {
630 size += 2;
631 }
632 if (is_20b) {
633 size += 3;
634 }
635
636 buffer += size;
637 ++count;
638 }
639
640 *frame_count = count;
641 return 0;
642 }
643
icm42688_decoder_get_size_info(struct sensor_chan_spec chan_spec,size_t * base_size,size_t * frame_size)644 static int icm42688_decoder_get_size_info(struct sensor_chan_spec chan_spec, size_t *base_size,
645 size_t *frame_size)
646 {
647 switch (chan_spec.chan_type) {
648 case SENSOR_CHAN_ACCEL_X:
649 case SENSOR_CHAN_ACCEL_Y:
650 case SENSOR_CHAN_ACCEL_Z:
651 case SENSOR_CHAN_ACCEL_XYZ:
652 case SENSOR_CHAN_GYRO_X:
653 case SENSOR_CHAN_GYRO_Y:
654 case SENSOR_CHAN_GYRO_Z:
655 case SENSOR_CHAN_GYRO_XYZ:
656 *base_size = sizeof(struct sensor_three_axis_data);
657 *frame_size = sizeof(struct sensor_three_axis_sample_data);
658 return 0;
659 case SENSOR_CHAN_DIE_TEMP:
660 *base_size = sizeof(struct sensor_q31_data);
661 *frame_size = sizeof(struct sensor_q31_sample_data);
662 return 0;
663 default:
664 return -ENOTSUP;
665 }
666 }
667
icm24688_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)668 static bool icm24688_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
669 {
670 const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer;
671
672 if (!edata->header.is_fifo) {
673 return false;
674 }
675
676 switch (trigger) {
677 case SENSOR_TRIG_DATA_READY:
678 return FIELD_GET(BIT_INT_STATUS_DATA_RDY, edata->int_status);
679 case SENSOR_TRIG_FIFO_WATERMARK:
680 return FIELD_GET(BIT_INT_STATUS_FIFO_THS, edata->int_status);
681 case SENSOR_TRIG_FIFO_FULL:
682 return FIELD_GET(BIT_INT_STATUS_FIFO_FULL, edata->int_status);
683 default:
684 return false;
685 }
686 }
687
688 SENSOR_DECODER_API_DT_DEFINE() = {
689 .get_frame_count = icm42688_decoder_get_frame_count,
690 .get_size_info = icm42688_decoder_get_size_info,
691 .decode = icm42688_decoder_decode,
692 .has_trigger = icm24688_decoder_has_trigger,
693 };
694
icm42688_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)695 int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
696 {
697 ARG_UNUSED(dev);
698 *decoder = &SENSOR_DECODER_NAME();
699
700 return 0;
701 }
702
