1 /*
2 * Copyright (c) 2023 Google LLC
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "icm4268x_decoder.h"
8 #include "icm4268x_reg.h"
9 #include "icm4268x.h"
10 #include <errno.h>
11
12 #include <zephyr/logging/log.h>
13 #include <zephyr/drivers/sensor_clock.h>
14
15 LOG_MODULE_REGISTER(ICM4268X_DECODER, CONFIG_SENSOR_LOG_LEVEL);
16
17 #define DT_DRV_COMPAT invensense_icm4268x
18
19 static const struct icm4268x_reg_val_pair table_accel_shift_to_reg[][5] = {
20 [ICM4268X_VARIANT_ICM42688] = {
21 {.val = 8, .reg = ICM42688_DT_ACCEL_FS_16},
22 {.val = 7, .reg = ICM42688_DT_ACCEL_FS_8},
23 {.val = 6, .reg = ICM42688_DT_ACCEL_FS_4},
24 {.val = 5, .reg = ICM42688_DT_ACCEL_FS_2},
25 },
26 [ICM4268X_VARIANT_ICM42686] = {
27 {.val = 9, .reg = ICM42686_DT_ACCEL_FS_32},
28 {.val = 8, .reg = ICM42686_DT_ACCEL_FS_16},
29 {.val = 7, .reg = ICM42686_DT_ACCEL_FS_8},
30 {.val = 6, .reg = ICM42686_DT_ACCEL_FS_4},
31 {.val = 5, .reg = ICM42686_DT_ACCEL_FS_2},
32 },
33 };
34
35 static const struct icm4268x_reg_val_pair table_gyro_shift_to_reg[][8] = {
36 [ICM4268X_VARIANT_ICM42688] = {
37 {.val = 6, .reg = ICM42688_DT_GYRO_FS_2000},
38 {.val = 5, .reg = ICM42688_DT_GYRO_FS_1000},
39 {.val = 4, .reg = ICM42688_DT_GYRO_FS_500},
40 {.val = 3, .reg = ICM42688_DT_GYRO_FS_250},
41 {.val = 2, .reg = ICM42688_DT_GYRO_FS_125},
42 {.val = 1, .reg = ICM42688_DT_GYRO_FS_62_5},
43 {.val = 0, .reg = ICM42688_DT_GYRO_FS_31_25},
44 {.val = -1, .reg = ICM42688_DT_GYRO_FS_15_625},
45 },
46 [ICM4268X_VARIANT_ICM42686] = {
47 {.val = 7, .reg = ICM42686_DT_GYRO_FS_4000},
48 {.val = 6, .reg = ICM42686_DT_GYRO_FS_2000},
49 {.val = 5, .reg = ICM42686_DT_GYRO_FS_1000},
50 {.val = 4, .reg = ICM42686_DT_GYRO_FS_500},
51 {.val = 3, .reg = ICM42686_DT_GYRO_FS_250},
52 {.val = 2, .reg = ICM42686_DT_GYRO_FS_125},
53 {.val = 1, .reg = ICM42686_DT_GYRO_FS_62_5},
54 {.val = 0, .reg = ICM42686_DT_GYRO_FS_31_25},
55 },
56 };
57
icm4268x_get_shift(enum sensor_channel channel,int accel_fs,int gyro_fs,enum icm4268x_variant variant,int8_t * shift)58 static int icm4268x_get_shift(enum sensor_channel channel, int accel_fs, int gyro_fs,
59 enum icm4268x_variant variant, int8_t *shift)
60 {
61 switch (channel) {
62 case SENSOR_CHAN_ACCEL_XYZ:
63 case SENSOR_CHAN_ACCEL_X:
64 case SENSOR_CHAN_ACCEL_Y:
65 case SENSOR_CHAN_ACCEL_Z:
66 for (uint8_t i = 0 ; i < table_accel_fs_to_reg_array_size[variant] ; i++) {
67 if (accel_fs == table_accel_shift_to_reg[variant][i].reg) {
68 *shift = table_accel_shift_to_reg[variant][i].val;
69 return 0;
70 }
71 }
72 return -EINVAL;
73 case SENSOR_CHAN_GYRO_XYZ:
74 case SENSOR_CHAN_GYRO_X:
75 case SENSOR_CHAN_GYRO_Y:
76 case SENSOR_CHAN_GYRO_Z:
77 for (uint8_t i = 0 ; i < table_gyro_fs_to_reg_array_size[variant] ; i++) {
78 if (gyro_fs == table_gyro_shift_to_reg[variant][i].reg) {
79 *shift = table_gyro_shift_to_reg[variant][i].val;
80 return 0;
81 }
82 }
83 return -EINVAL;
84 case SENSOR_CHAN_DIE_TEMP:
85 *shift = 9;
86 return 0;
87 default:
88 return -EINVAL;
89 }
90 }
91
icm4268x_convert_raw_to_q31(struct icm4268x_cfg * cfg,enum sensor_channel chan,int32_t reading,q31_t * out)92 int icm4268x_convert_raw_to_q31(struct icm4268x_cfg *cfg, enum sensor_channel chan, int32_t reading,
93 q31_t *out)
94 {
95 int32_t whole;
96 int32_t fraction;
97 int64_t intermediate;
98 int8_t shift;
99 int rc;
100
101 rc = icm4268x_get_shift(chan, cfg->accel_fs, cfg->gyro_fs, cfg->variant, &shift);
102 if (rc != 0) {
103 return rc;
104 }
105
106 switch (chan) {
107 case SENSOR_CHAN_ACCEL_XYZ:
108 case SENSOR_CHAN_ACCEL_X:
109 case SENSOR_CHAN_ACCEL_Y:
110 case SENSOR_CHAN_ACCEL_Z:
111 icm4268x_accel_ms(cfg, reading, &whole, &fraction);
112 break;
113 case SENSOR_CHAN_GYRO_XYZ:
114 case SENSOR_CHAN_GYRO_X:
115 case SENSOR_CHAN_GYRO_Y:
116 case SENSOR_CHAN_GYRO_Z:
117 icm4268x_gyro_rads(cfg, reading, &whole, &fraction);
118 break;
119 case SENSOR_CHAN_DIE_TEMP:
120 icm4268x_temp_c(reading, &whole, &fraction);
121 break;
122 default:
123 return -ENOTSUP;
124 }
125 intermediate = ((int64_t)whole * INT64_C(1000000) + fraction);
126 if (shift < 0) {
127 intermediate =
128 intermediate * ((int64_t)INT32_MAX + 1) * (1 << -shift) / INT64_C(1000000);
129 } else {
130 intermediate =
131 intermediate * ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000));
132 }
133 *out = CLAMP(intermediate, INT32_MIN, INT32_MAX);
134
135 return 0;
136 }
137
icm4268x_get_channel_position(enum sensor_channel chan)138 static int icm4268x_get_channel_position(enum sensor_channel chan)
139 {
140 switch (chan) {
141 case SENSOR_CHAN_DIE_TEMP:
142 return 0;
143 case SENSOR_CHAN_ACCEL_XYZ:
144 case SENSOR_CHAN_ACCEL_X:
145 return 1;
146 case SENSOR_CHAN_ACCEL_Y:
147 return 2;
148 case SENSOR_CHAN_ACCEL_Z:
149 return 3;
150 case SENSOR_CHAN_GYRO_XYZ:
151 case SENSOR_CHAN_GYRO_X:
152 return 4;
153 case SENSOR_CHAN_GYRO_Y:
154 return 5;
155 case SENSOR_CHAN_GYRO_Z:
156 return 6;
157 default:
158 return 0;
159 }
160 }
161
icm4268x_encode_channel(enum sensor_channel chan)162 static uint8_t icm4268x_encode_channel(enum sensor_channel chan)
163 {
164 uint8_t encode_bmask = 0;
165
166 switch (chan) {
167 case SENSOR_CHAN_DIE_TEMP:
168 case SENSOR_CHAN_ACCEL_X:
169 case SENSOR_CHAN_ACCEL_Y:
170 case SENSOR_CHAN_ACCEL_Z:
171 case SENSOR_CHAN_GYRO_X:
172 case SENSOR_CHAN_GYRO_Y:
173 case SENSOR_CHAN_GYRO_Z:
174 encode_bmask = BIT(icm4268x_get_channel_position(chan));
175 break;
176 case SENSOR_CHAN_ACCEL_XYZ:
177 encode_bmask = BIT(icm4268x_get_channel_position(SENSOR_CHAN_ACCEL_X)) |
178 BIT(icm4268x_get_channel_position(SENSOR_CHAN_ACCEL_Y)) |
179 BIT(icm4268x_get_channel_position(SENSOR_CHAN_ACCEL_Z));
180 break;
181 case SENSOR_CHAN_GYRO_XYZ:
182 encode_bmask = BIT(icm4268x_get_channel_position(SENSOR_CHAN_GYRO_X)) |
183 BIT(icm4268x_get_channel_position(SENSOR_CHAN_GYRO_Y)) |
184 BIT(icm4268x_get_channel_position(SENSOR_CHAN_GYRO_Z));
185 break;
186 default:
187 break;
188 }
189
190 return encode_bmask;
191 }
192
icm4268x_encode(const struct device * dev,const struct sensor_chan_spec * const channels,const size_t num_channels,uint8_t * buf)193 int icm4268x_encode(const struct device *dev, const struct sensor_chan_spec *const channels,
194 const size_t num_channels, uint8_t *buf)
195 {
196 struct icm4268x_dev_data *data = dev->data;
197 struct icm4268x_encoded_data *edata = (struct icm4268x_encoded_data *)buf;
198 uint64_t cycles;
199 int rc;
200
201 edata->channels = 0;
202
203 for (int i = 0; i < num_channels; i++) {
204 edata->channels |= icm4268x_encode_channel(channels[i].chan_type);
205 }
206
207 rc = sensor_clock_get_cycles(&cycles);
208 if (rc != 0) {
209 return rc;
210 }
211
212 edata->header.is_fifo = false;
213 edata->header.variant = data->cfg.variant;
214 edata->header.accel_fs = data->cfg.accel_fs;
215 edata->header.gyro_fs = data->cfg.gyro_fs;
216 edata->header.axis_align[0] = data->cfg.axis_align[0];
217 edata->header.axis_align[1] = data->cfg.axis_align[1];
218 edata->header.axis_align[2] = data->cfg.axis_align[2];
219 edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
220
221 return 0;
222 }
223
224 #define IS_ACCEL(chan) ((chan) >= SENSOR_CHAN_ACCEL_X && (chan) <= SENSOR_CHAN_ACCEL_XYZ)
225 #define IS_GYRO(chan) ((chan) >= SENSOR_CHAN_GYRO_X && (chan) <= SENSOR_CHAN_GYRO_XYZ)
226
icm4268x_read_temperature_from_packet(const uint8_t * pkt)227 static inline q31_t icm4268x_read_temperature_from_packet(const uint8_t *pkt)
228 {
229 int32_t temperature;
230 int32_t whole;
231 int32_t fraction;
232
233 /* Temperature always assumes a shift of 9 for a range of (-273,273) C */
234 if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) {
235 temperature = (pkt[0xd] << 8) | pkt[0xe];
236
237 icm4268x_temp_c(temperature, &whole, &fraction);
238 } else {
239 if (FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1 &&
240 FIELD_GET(FIFO_HEADER_GYRO, pkt[0]) == 1) {
241 temperature = pkt[0xd];
242 } else {
243 temperature = pkt[0x7];
244 }
245
246 int64_t sensitivity = 207;
247 int64_t temperature100 = (temperature * 100) + (25 * sensitivity);
248
249 whole = temperature100 / sensitivity;
250 fraction =
251 ((temperature100 - whole * sensitivity) * INT64_C(1000000)) / sensitivity;
252 }
253 __ASSERT_NO_MSG(whole >= -512 && whole <= 511);
254 return FIELD_PREP(GENMASK(31, 22), whole) | (fraction * GENMASK64(21, 0) / 1000000);
255 }
256
icm4268x_read_imu_from_packet(const uint8_t * pkt,bool is_accel,int fs,uint8_t axis_offset,q31_t * out)257 static int icm4268x_read_imu_from_packet(const uint8_t *pkt, bool is_accel, int fs,
258 uint8_t axis_offset, q31_t *out)
259 {
260 uint32_t unsigned_value;
261 int32_t signed_value;
262 bool is_hires = FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1;
263 int offset = 1 + (axis_offset * 2);
264
265 const uint32_t scale[2][2] = {
266 /* low-res, hi-res */
267 {35744, 2235}, /* gyro */
268 {40168, 2511}, /* accel */
269 };
270
271 if (!is_accel && FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1) {
272 offset += 6;
273 }
274
275 unsigned_value = (pkt[offset] << 8) | pkt[offset + 1];
276
277 if (is_hires) {
278 uint32_t mask = is_accel ? GENMASK(7, 4) : GENMASK(3, 0);
279 offset = 17 + axis_offset;
280 unsigned_value = (unsigned_value << 4) | FIELD_GET(mask, pkt[offset]);
281 signed_value = unsigned_value | (0 - (unsigned_value & BIT(19)));
282
283 /*
284 * By default, INTF_CONFIG0 is set to 0x30 and thus FIFO_HOLD_LAST_DATA_EN is set to
285 * 0. For 20-bit FIFO packets, -524288 indicates invalid data.
286 *
287 * At the time of writing, INTF_CONFIG0 is not configured explicitly.
288 *
289 * TODO: Enable/disable this check based on FIFO_HOLD_LAST_DATA_EN if INTF_CONFIG0
290 * is configured explicitly.
291 */
292 if (signed_value == -524288) {
293 return -ENODATA;
294 }
295 } else {
296 signed_value = unsigned_value | (0 - (unsigned_value & BIT(16)));
297 }
298
299 *out = (q31_t)(signed_value * scale[is_accel][is_hires]);
300 return 0;
301 }
302
303 static const uint32_t accel_period_ns[] = {
304 [ICM42688_DT_ACCEL_ODR_1_5625] = UINT32_C(10000000000000) / 15625,
305 [ICM42688_DT_ACCEL_ODR_3_125] = UINT32_C(10000000000000) / 31250,
306 [ICM42688_DT_ACCEL_ODR_6_25] = UINT32_C(10000000000000) / 62500,
307 [ICM42688_DT_ACCEL_ODR_12_5] = UINT32_C(10000000000000) / 125000,
308 [ICM42688_DT_ACCEL_ODR_25] = UINT32_C(1000000000) / 25,
309 [ICM42688_DT_ACCEL_ODR_50] = UINT32_C(1000000000) / 50,
310 [ICM42688_DT_ACCEL_ODR_100] = UINT32_C(1000000000) / 100,
311 [ICM42688_DT_ACCEL_ODR_200] = UINT32_C(1000000000) / 200,
312 [ICM42688_DT_ACCEL_ODR_500] = UINT32_C(1000000000) / 500,
313 [ICM42688_DT_ACCEL_ODR_1000] = UINT32_C(1000000),
314 [ICM42688_DT_ACCEL_ODR_2000] = UINT32_C(1000000) / 2,
315 [ICM42688_DT_ACCEL_ODR_4000] = UINT32_C(1000000) / 4,
316 [ICM42688_DT_ACCEL_ODR_8000] = UINT32_C(1000000) / 8,
317 [ICM42688_DT_ACCEL_ODR_16000] = UINT32_C(1000000) / 16,
318 [ICM42688_DT_ACCEL_ODR_32000] = UINT32_C(1000000) / 32,
319 };
320
321 static const uint32_t gyro_period_ns[] = {
322 [ICM42688_DT_GYRO_ODR_12_5] = UINT32_C(10000000000000) / 125000,
323 [ICM42688_DT_GYRO_ODR_25] = UINT32_C(1000000000) / 25,
324 [ICM42688_DT_GYRO_ODR_50] = UINT32_C(1000000000) / 50,
325 [ICM42688_DT_GYRO_ODR_100] = UINT32_C(1000000000) / 100,
326 [ICM42688_DT_GYRO_ODR_200] = UINT32_C(1000000000) / 200,
327 [ICM42688_DT_GYRO_ODR_500] = UINT32_C(1000000000) / 500,
328 [ICM42688_DT_GYRO_ODR_1000] = UINT32_C(1000000),
329 [ICM42688_DT_GYRO_ODR_2000] = UINT32_C(1000000) / 2,
330 [ICM42688_DT_GYRO_ODR_4000] = UINT32_C(1000000) / 4,
331 [ICM42688_DT_GYRO_ODR_8000] = UINT32_C(1000000) / 8,
332 [ICM42688_DT_GYRO_ODR_16000] = UINT32_C(1000000) / 16,
333 [ICM42688_DT_GYRO_ODR_32000] = UINT32_C(1000000) / 32,
334 };
335
icm4268x_calc_timestamp_delta(int rtc_freq,int chan_type,int dt_odr,int frame_count,uint64_t * out_delta)336 static int icm4268x_calc_timestamp_delta(int rtc_freq, int chan_type, int dt_odr, int frame_count,
337 uint64_t *out_delta)
338 {
339 uint32_t period;
340
341 if (IS_ACCEL(chan_type)) {
342 period = accel_period_ns[dt_odr];
343 } else if (IS_GYRO(chan_type)) {
344 period = gyro_period_ns[dt_odr];
345 } else {
346 return -EINVAL;
347 }
348
349 /*
350 * When ODR is set to r and an external clock with frequency f is used,
351 * the actual ODR = f * r / 32000.
352 */
353 *out_delta = (uint64_t)period * frame_count * 32000 / rtc_freq;
354
355 return 0;
356 }
357
icm4268x_fifo_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)358 static int icm4268x_fifo_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
359 uint32_t *fit, uint16_t max_count, void *data_out)
360 {
361 const struct icm4268x_fifo_data *edata = (const struct icm4268x_fifo_data *)buffer;
362 const uint8_t *buffer_end = buffer + sizeof(struct icm4268x_fifo_data) + edata->fifo_count;
363 int accel_frame_count = 0;
364 int gyro_frame_count = 0;
365 int count = 0;
366 int rc = 0;
367
368 if ((uintptr_t)buffer_end <= *fit || chan_spec.chan_idx != 0) {
369 return 0;
370 }
371
372 ((struct sensor_data_header *)data_out)->base_timestamp_ns = edata->header.timestamp;
373
374 buffer += sizeof(struct icm4268x_fifo_data);
375 while (count < max_count && buffer < buffer_end) {
376 const bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]) == 1;
377 const bool has_accel = FIELD_GET(FIFO_HEADER_ACCEL, buffer[0]) == 1;
378 const bool has_gyro = FIELD_GET(FIFO_HEADER_GYRO, buffer[0]) == 1;
379 const uint8_t *frame_end = buffer;
380
381 if (is_20b) {
382 frame_end += 20;
383 } else if (has_accel && has_gyro) {
384 frame_end += 16;
385 } else {
386 frame_end += 8;
387 }
388 if (has_accel) {
389 accel_frame_count++;
390 }
391 if (has_gyro) {
392 gyro_frame_count++;
393 }
394
395 if ((uintptr_t)buffer < *fit) {
396 /* This frame was already decoded, move on to the next frame */
397 buffer = frame_end;
398 continue;
399 }
400 if (chan_spec.chan_type == SENSOR_CHAN_DIE_TEMP) {
401 struct sensor_q31_data *data = (struct sensor_q31_data *)data_out;
402 uint64_t ts_delta;
403
404 if (has_accel) {
405 rc = icm4268x_calc_timestamp_delta(
406 edata->rtc_freq, SENSOR_CHAN_ACCEL_XYZ, edata->accel_odr,
407 accel_frame_count - 1, &ts_delta);
408 } else {
409 rc = icm4268x_calc_timestamp_delta(
410 edata->rtc_freq, SENSOR_CHAN_GYRO_XYZ, edata->gyro_odr,
411 gyro_frame_count - 1, &ts_delta);
412 }
413 if (rc < 0) {
414 buffer = frame_end;
415 continue;
416 }
417
418 /*
419 * TODO: For some extreme combination of ODR and FIFO count, using uint32_t
420 * to store timestamp delta will overflow. Better error reporting?
421 */
422 if (ts_delta > UINT32_MAX) {
423 LOG_ERR("Timestamp delta overflow");
424 buffer = frame_end;
425 continue;
426 }
427
428 data->readings[count].timestamp_delta = ts_delta;
429
430 data->shift = 9;
431 data->readings[count].temperature =
432 icm4268x_read_temperature_from_packet(buffer);
433 } else if (IS_ACCEL(chan_spec.chan_type) && has_accel) {
434 /* Decode accel */
435 struct sensor_three_axis_data *data =
436 (struct sensor_three_axis_data *)data_out;
437 uint64_t ts_delta;
438
439 icm4268x_get_shift(SENSOR_CHAN_ACCEL_XYZ, edata->header.accel_fs,
440 edata->header.gyro_fs, edata->header.variant,
441 &data->shift);
442
443 rc = icm4268x_calc_timestamp_delta(edata->rtc_freq, SENSOR_CHAN_ACCEL_XYZ,
444 edata->accel_odr, accel_frame_count - 1,
445 &ts_delta);
446 if (rc < 0) {
447 buffer = frame_end;
448 continue;
449 }
450
451 /*
452 * TODO: For some extreme combination of ODR and FIFO count, using uint32_t
453 * to store timestamp delta will overflow. Better error reporting?
454 */
455 if (ts_delta > UINT32_MAX) {
456 LOG_ERR("Timestamp delta overflow");
457 buffer = frame_end;
458 continue;
459 }
460
461 data->readings[count].timestamp_delta = ts_delta;
462
463 q31_t reading[3];
464
465 for (int i = 0; i < 3; i++) {
466 rc |= icm4268x_read_imu_from_packet(
467 buffer, true, edata->header.accel_fs, i, &reading[i]);
468 }
469
470 for (int i = 0; i < 3; i++) {
471 data->readings[count].values[i] =
472 edata->header.axis_align[i].sign*
473 reading[edata->header.axis_align[i].index];
474 }
475
476 if (rc != 0) {
477 accel_frame_count--;
478 buffer = frame_end;
479 continue;
480 }
481 } else if (IS_GYRO(chan_spec.chan_type) && has_gyro) {
482 /* Decode gyro */
483 struct sensor_three_axis_data *data =
484 (struct sensor_three_axis_data *)data_out;
485 uint64_t ts_delta;
486
487 icm4268x_get_shift(SENSOR_CHAN_GYRO_XYZ, edata->header.accel_fs,
488 edata->header.gyro_fs, edata->header.variant,
489 &data->shift);
490
491 rc = icm4268x_calc_timestamp_delta(edata->rtc_freq, SENSOR_CHAN_GYRO_XYZ,
492 edata->gyro_odr, gyro_frame_count - 1,
493 &ts_delta);
494 if (rc < 0) {
495 buffer = frame_end;
496 continue;
497 }
498
499 /*
500 * TODO: For some extreme combination of ODR and FIFO count, using uint32_t
501 * to store timestamp delta will overflow. Better error reporting?
502 */
503 if (ts_delta > UINT32_MAX) {
504 LOG_ERR("Timestamp delta overflow");
505 buffer = frame_end;
506 continue;
507 }
508
509 data->readings[count].timestamp_delta = ts_delta;
510
511 q31_t reading[3];
512
513 for (int i = 0; i < 3; i++) {
514 rc |= icm4268x_read_imu_from_packet(
515 buffer, false, edata->header.gyro_fs, i, &reading[i]);
516 }
517
518 for (int i = 0; i < 3; i++) {
519 data->readings[count].values[i] =
520 edata->header.axis_align[i].sign*
521 reading[edata->header.axis_align[i].index];
522 }
523
524 if (rc != 0) {
525 gyro_frame_count--;
526 buffer = frame_end;
527 continue;
528 }
529 } else {
530 CODE_UNREACHABLE;
531 }
532 buffer = frame_end;
533 *fit = (uintptr_t)frame_end;
534 count++;
535 }
536 return count;
537 }
538
icm4268x_one_shot_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)539 static int icm4268x_one_shot_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
540 uint32_t *fit, uint16_t max_count, void *data_out)
541 {
542 const struct icm4268x_encoded_data *edata = (const struct icm4268x_encoded_data *)buffer;
543 const struct icm4268x_decoder_header *header = &edata->header;
544 struct icm4268x_cfg cfg = {
545 .accel_fs = edata->header.accel_fs,
546 .gyro_fs = edata->header.gyro_fs,
547 .variant = edata->header.variant,
548 };
549 uint8_t channel_request;
550 int rc;
551
552 if (*fit != 0) {
553 return 0;
554 }
555 if (max_count == 0 || chan_spec.chan_idx != 0) {
556 return -EINVAL;
557 }
558
559 switch (chan_spec.chan_type) {
560 case SENSOR_CHAN_ACCEL_X:
561 case SENSOR_CHAN_ACCEL_Y:
562 case SENSOR_CHAN_ACCEL_Z:
563 case SENSOR_CHAN_GYRO_X:
564 case SENSOR_CHAN_GYRO_Y:
565 case SENSOR_CHAN_GYRO_Z:
566 case SENSOR_CHAN_DIE_TEMP: {
567 channel_request = icm4268x_encode_channel(chan_spec.chan_type);
568 if ((channel_request & edata->channels) != channel_request) {
569 return -ENODATA;
570 }
571
572 struct sensor_q31_data *out = data_out;
573
574 out->header.base_timestamp_ns = edata->header.timestamp;
575 out->header.reading_count = 1;
576
577 rc = icm4268x_get_shift(chan_spec.chan_type, header->accel_fs, header->gyro_fs,
578 header->variant, &out->shift);
579 if (rc != 0) {
580 return -EINVAL;
581 }
582
583 icm4268x_convert_raw_to_q31(
584 &cfg, chan_spec.chan_type,
585 edata->readings[icm4268x_get_channel_position(chan_spec.chan_type)],
586 &out->readings[0].value);
587 *fit = 1;
588 return 1;
589 }
590 case SENSOR_CHAN_ACCEL_XYZ:
591 case SENSOR_CHAN_GYRO_XYZ: {
592 channel_request = icm4268x_encode_channel(chan_spec.chan_type);
593 if ((channel_request & edata->channels) != channel_request) {
594 return -ENODATA;
595 }
596
597 struct sensor_three_axis_data *out = data_out;
598
599 out->header.base_timestamp_ns = edata->header.timestamp;
600 out->header.reading_count = 1;
601 rc = icm4268x_get_shift(chan_spec.chan_type, header->accel_fs, header->gyro_fs,
602 header->variant, &out->shift);
603 if (rc != 0) {
604 return -EINVAL;
605 }
606
607 icm4268x_convert_raw_to_q31(
608 &cfg, chan_spec.chan_type - 3,
609 edata->readings[icm4268x_get_channel_position(chan_spec.chan_type - 3)],
610 &out->readings[0].x);
611 icm4268x_convert_raw_to_q31(
612 &cfg, chan_spec.chan_type - 2,
613 edata->readings[icm4268x_get_channel_position(chan_spec.chan_type - 2)],
614 &out->readings[0].y);
615 icm4268x_convert_raw_to_q31(
616 &cfg, chan_spec.chan_type - 1,
617 edata->readings[icm4268x_get_channel_position(chan_spec.chan_type - 1)],
618 &out->readings[0].z);
619 *fit = 1;
620 return 1;
621 }
622 default:
623 return -EINVAL;
624 }
625 }
626
icm4268x_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)627 static int icm4268x_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
628 uint32_t *fit, uint16_t max_count, void *data_out)
629 {
630 const struct icm4268x_decoder_header *header =
631 (const struct icm4268x_decoder_header *)buffer;
632
633 if (header->is_fifo) {
634 return icm4268x_fifo_decode(buffer, chan_spec, fit, max_count, data_out);
635 }
636 return icm4268x_one_shot_decode(buffer, chan_spec, fit, max_count, data_out);
637 }
638
icm4268x_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)639 static int icm4268x_decoder_get_frame_count(const uint8_t *buffer,
640 struct sensor_chan_spec chan_spec,
641 uint16_t *frame_count)
642 {
643 const struct icm4268x_fifo_data *data = (const struct icm4268x_fifo_data *)buffer;
644 const struct icm4268x_encoded_data *enc_data = (const struct icm4268x_encoded_data *)buffer;
645 const struct icm4268x_decoder_header *header = &data->header;
646
647 if (chan_spec.chan_idx != 0) {
648 return -ENOTSUP;
649 }
650
651 uint8_t channel_request = icm4268x_encode_channel(chan_spec.chan_type);
652
653
654 if ((!enc_data->header.is_fifo) &&
655 (enc_data->channels & channel_request) != channel_request) {
656 return -ENODATA;
657 }
658
659 if (!header->is_fifo) {
660 switch (chan_spec.chan_type) {
661 case SENSOR_CHAN_ACCEL_X:
662 case SENSOR_CHAN_ACCEL_Y:
663 case SENSOR_CHAN_ACCEL_Z:
664 case SENSOR_CHAN_ACCEL_XYZ:
665 case SENSOR_CHAN_GYRO_X:
666 case SENSOR_CHAN_GYRO_Y:
667 case SENSOR_CHAN_GYRO_Z:
668 case SENSOR_CHAN_GYRO_XYZ:
669 case SENSOR_CHAN_DIE_TEMP:
670 *frame_count = 1;
671 return 0;
672 default:
673 return -ENOTSUP;
674 }
675 return 0;
676 }
677
678 /* Skip the header */
679 buffer += sizeof(struct icm4268x_fifo_data);
680
681 uint16_t count = 0;
682 const uint8_t *end = buffer + data->fifo_count;
683
684 while (buffer < end) {
685 bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]);
686 int size = is_20b ? 3 : 2;
687
688 if (FIELD_GET(FIFO_HEADER_ACCEL, buffer[0])) {
689 size += 6;
690 }
691 if (FIELD_GET(FIFO_HEADER_GYRO, buffer[0])) {
692 size += 6;
693 }
694 if (FIELD_GET(FIFO_HEADER_TIMESTAMP_FSYNC, buffer[0])) {
695 size += 2;
696 }
697 if (is_20b) {
698 size += 3;
699 }
700
701 buffer += size;
702 ++count;
703 }
704
705 *frame_count = count;
706 return 0;
707 }
708
icm4268x_decoder_get_size_info(struct sensor_chan_spec chan_spec,size_t * base_size,size_t * frame_size)709 static int icm4268x_decoder_get_size_info(struct sensor_chan_spec chan_spec, size_t *base_size,
710 size_t *frame_size)
711 {
712 switch (chan_spec.chan_type) {
713 case SENSOR_CHAN_ACCEL_XYZ:
714 case SENSOR_CHAN_GYRO_XYZ:
715 *base_size = sizeof(struct sensor_three_axis_data);
716 *frame_size = sizeof(struct sensor_three_axis_sample_data);
717 return 0;
718 case SENSOR_CHAN_ACCEL_X:
719 case SENSOR_CHAN_ACCEL_Y:
720 case SENSOR_CHAN_ACCEL_Z:
721 case SENSOR_CHAN_GYRO_X:
722 case SENSOR_CHAN_GYRO_Y:
723 case SENSOR_CHAN_GYRO_Z:
724 case SENSOR_CHAN_DIE_TEMP:
725 *base_size = sizeof(struct sensor_q31_data);
726 *frame_size = sizeof(struct sensor_q31_sample_data);
727 return 0;
728 default:
729 return -ENOTSUP;
730 }
731 }
732
icm4268x_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)733 static bool icm4268x_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
734 {
735 const struct icm4268x_fifo_data *edata = (const struct icm4268x_fifo_data *)buffer;
736
737 if (!edata->header.is_fifo) {
738 return false;
739 }
740
741 switch (trigger) {
742 case SENSOR_TRIG_DATA_READY:
743 return FIELD_GET(BIT_DATA_RDY_INT, edata->int_status);
744 case SENSOR_TRIG_FIFO_WATERMARK:
745 return FIELD_GET(BIT_FIFO_THS_INT, edata->int_status);
746 case SENSOR_TRIG_FIFO_FULL:
747 return FIELD_GET(BIT_FIFO_FULL_INT, edata->int_status);
748 default:
749 return false;
750 }
751 }
752
753 SENSOR_DECODER_API_DT_DEFINE() = {
754 .get_frame_count = icm4268x_decoder_get_frame_count,
755 .get_size_info = icm4268x_decoder_get_size_info,
756 .decode = icm4268x_decoder_decode,
757 .has_trigger = icm4268x_decoder_has_trigger,
758 };
759
icm4268x_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)760 int icm4268x_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
761 {
762 ARG_UNUSED(dev);
763 *decoder = &SENSOR_DECODER_NAME();
764
765 return 0;
766 }
767