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