1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * Copyright (C) 2020 Invensense, Inc.
6 #include <linux/kernel.h>
7 #include <linux/device.h>
8 #include <linux/mutex.h>
9 #include <linux/pm_runtime.h>
10 #include <linux/regmap.h>
11 #include <linux/delay.h>
12 #include <linux/math64.h>
13 #include <linux/iio/iio.h>
14 #include <linux/iio/buffer.h>
15 #include <linux/iio/kfifo_buf.h>
17 #include "inv_icm42600.h"
18 #include "inv_icm42600_temp.h"
19 #include "inv_icm42600_buffer.h"
20 #include "inv_icm42600_timestamp.h"
22 #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
24 .type = IIO_ANGL_VEL, \
26 .channel2 = _modifier, \
27 .info_mask_separate = \
28 BIT(IIO_CHAN_INFO_RAW) | \
29 BIT(IIO_CHAN_INFO_CALIBBIAS), \
30 .info_mask_shared_by_type = \
31 BIT(IIO_CHAN_INFO_SCALE), \
32 .info_mask_shared_by_type_available = \
33 BIT(IIO_CHAN_INFO_SCALE) | \
34 BIT(IIO_CHAN_INFO_CALIBBIAS), \
35 .info_mask_shared_by_all = \
36 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
37 .info_mask_shared_by_all_available = \
38 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
39 .scan_index = _index, \
44 .endianness = IIO_BE, \
46 .ext_info = _ext_info, \
49 enum inv_icm42600_gyro_scan {
50 INV_ICM42600_GYRO_SCAN_X,
51 INV_ICM42600_GYRO_SCAN_Y,
52 INV_ICM42600_GYRO_SCAN_Z,
53 INV_ICM42600_GYRO_SCAN_TEMP,
54 INV_ICM42600_GYRO_SCAN_TIMESTAMP,
57 static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
58 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
62 static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
63 INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X,
64 inv_icm42600_gyro_ext_infos),
65 INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y,
66 inv_icm42600_gyro_ext_infos),
67 INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
68 inv_icm42600_gyro_ext_infos),
69 INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
70 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
74 * IIO buffer data: size must be a power of 2 and timestamp aligned
75 * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp
77 struct inv_icm42600_gyro_buffer {
78 struct inv_icm42600_fifo_sensor_data gyro;
80 int64_t timestamp __aligned(8);
83 #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
84 (BIT(INV_ICM42600_GYRO_SCAN_X) | \
85 BIT(INV_ICM42600_GYRO_SCAN_Y) | \
86 BIT(INV_ICM42600_GYRO_SCAN_Z))
88 #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
90 static const unsigned long inv_icm42600_gyro_scan_masks[] = {
91 /* 3-axis gyro + temperature */
92 INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
96 /* enable gyroscope sensor and FIFO write */
97 static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
98 const unsigned long *scan_mask)
100 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
101 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
102 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
103 unsigned int fifo_en = 0;
104 unsigned int sleep_gyro = 0;
105 unsigned int sleep_temp = 0;
109 mutex_lock(&st->lock);
111 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
112 /* enable temp sensor */
113 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
116 fifo_en |= INV_ICM42600_SENSOR_TEMP;
119 if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
120 /* enable gyro sensor */
121 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
122 ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
125 fifo_en |= INV_ICM42600_SENSOR_GYRO;
128 /* update data FIFO write */
129 inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
130 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
134 ret = inv_icm42600_buffer_update_watermark(st);
137 mutex_unlock(&st->lock);
138 /* sleep maximum required time */
139 if (sleep_gyro > sleep_temp)
148 static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
149 struct iio_chan_spec const *chan,
152 struct device *dev = regmap_get_device(st->map);
153 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
158 if (chan->type != IIO_ANGL_VEL)
161 switch (chan->channel2) {
163 reg = INV_ICM42600_REG_GYRO_DATA_X;
166 reg = INV_ICM42600_REG_GYRO_DATA_Y;
169 reg = INV_ICM42600_REG_GYRO_DATA_Z;
175 pm_runtime_get_sync(dev);
176 mutex_lock(&st->lock);
178 /* enable gyro sensor */
179 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
180 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
184 /* read gyro register data */
185 data = (__be16 *)&st->buffer[0];
186 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
190 *val = (int16_t)be16_to_cpup(data);
191 if (*val == INV_ICM42600_DATA_INVALID)
194 mutex_unlock(&st->lock);
195 pm_runtime_mark_last_busy(dev);
196 pm_runtime_put_autosuspend(dev);
200 /* IIO format int + nano */
201 static const int inv_icm42600_gyro_scale[] = {
202 /* +/- 2000dps => 0.001065264 rad/s */
203 [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0,
204 [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264,
205 /* +/- 1000dps => 0.000532632 rad/s */
206 [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0,
207 [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632,
208 /* +/- 500dps => 0.000266316 rad/s */
209 [2 * INV_ICM42600_GYRO_FS_500DPS] = 0,
210 [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316,
211 /* +/- 250dps => 0.000133158 rad/s */
212 [2 * INV_ICM42600_GYRO_FS_250DPS] = 0,
213 [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158,
214 /* +/- 125dps => 0.000066579 rad/s */
215 [2 * INV_ICM42600_GYRO_FS_125DPS] = 0,
216 [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579,
217 /* +/- 62.5dps => 0.000033290 rad/s */
218 [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0,
219 [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290,
220 /* +/- 31.25dps => 0.000016645 rad/s */
221 [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0,
222 [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645,
223 /* +/- 15.625dps => 0.000008322 rad/s */
224 [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0,
225 [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322,
228 static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st,
233 idx = st->conf.gyro.fs;
235 *val = inv_icm42600_gyro_scale[2 * idx];
236 *val2 = inv_icm42600_gyro_scale[2 * idx + 1];
237 return IIO_VAL_INT_PLUS_NANO;
240 static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st,
243 struct device *dev = regmap_get_device(st->map);
245 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
248 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) {
249 if (val == inv_icm42600_gyro_scale[idx] &&
250 val2 == inv_icm42600_gyro_scale[idx + 1])
253 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale))
258 pm_runtime_get_sync(dev);
259 mutex_lock(&st->lock);
261 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
263 mutex_unlock(&st->lock);
264 pm_runtime_mark_last_busy(dev);
265 pm_runtime_put_autosuspend(dev);
270 /* IIO format int + micro */
271 static const int inv_icm42600_gyro_odr[] = {
290 static const int inv_icm42600_gyro_odr_conv[] = {
291 INV_ICM42600_ODR_12_5HZ,
292 INV_ICM42600_ODR_25HZ,
293 INV_ICM42600_ODR_50HZ,
294 INV_ICM42600_ODR_100HZ,
295 INV_ICM42600_ODR_200HZ,
296 INV_ICM42600_ODR_1KHZ_LN,
297 INV_ICM42600_ODR_2KHZ_LN,
298 INV_ICM42600_ODR_4KHZ_LN,
301 static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
307 odr = st->conf.gyro.odr;
309 for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) {
310 if (inv_icm42600_gyro_odr_conv[i] == odr)
313 if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv))
316 *val = inv_icm42600_gyro_odr[2 * i];
317 *val2 = inv_icm42600_gyro_odr[2 * i + 1];
319 return IIO_VAL_INT_PLUS_MICRO;
322 static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
325 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
326 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
327 struct device *dev = regmap_get_device(st->map);
329 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
332 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) {
333 if (val == inv_icm42600_gyro_odr[idx] &&
334 val2 == inv_icm42600_gyro_odr[idx + 1])
337 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr))
340 conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
342 pm_runtime_get_sync(dev);
343 mutex_lock(&st->lock);
345 ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
346 iio_buffer_enabled(indio_dev));
350 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
353 inv_icm42600_buffer_update_fifo_period(st);
354 inv_icm42600_buffer_update_watermark(st);
357 mutex_unlock(&st->lock);
358 pm_runtime_mark_last_busy(dev);
359 pm_runtime_put_autosuspend(dev);
365 * Calibration bias values, IIO range format int + nano.
366 * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps.
368 static int inv_icm42600_gyro_calibbias[] = {
369 -1, 117010721, /* min: -1.117010721 rad/s */
370 0, 545415, /* step: 0.000545415 rad/s */
371 1, 116465306, /* max: 1.116465306 rad/s */
374 static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
375 struct iio_chan_spec const *chan,
378 struct device *dev = regmap_get_device(st->map);
386 if (chan->type != IIO_ANGL_VEL)
389 switch (chan->channel2) {
391 reg = INV_ICM42600_REG_OFFSET_USER0;
394 reg = INV_ICM42600_REG_OFFSET_USER1;
397 reg = INV_ICM42600_REG_OFFSET_USER3;
403 pm_runtime_get_sync(dev);
404 mutex_lock(&st->lock);
406 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
407 memcpy(data, st->buffer, sizeof(data));
409 mutex_unlock(&st->lock);
410 pm_runtime_mark_last_busy(dev);
411 pm_runtime_put_autosuspend(dev);
415 /* 12 bits signed value */
416 switch (chan->channel2) {
418 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
421 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
424 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
431 * convert raw offset to dps then to rad/s
432 * 12 bits signed raw max 64 to dps: 64 / 2048
433 * dps to rad: Pi / 180
434 * result in nano (1000000000)
435 * (offset * 64 * Pi * 1000000000) / (2048 * 180)
437 val64 = (int64_t)offset * 64LL * 3141592653LL;
438 /* for rounding, add + or - divisor (2048 * 180) divided by 2 */
440 val64 += 2048 * 180 / 2;
442 val64 -= 2048 * 180 / 2;
443 bias = div_s64(val64, 2048 * 180);
444 *val = bias / 1000000000L;
445 *val2 = bias % 1000000000L;
447 return IIO_VAL_INT_PLUS_NANO;
450 static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
451 struct iio_chan_spec const *chan,
454 struct device *dev = regmap_get_device(st->map);
455 int64_t val64, min, max;
456 unsigned int reg, regval;
460 if (chan->type != IIO_ANGL_VEL)
463 switch (chan->channel2) {
465 reg = INV_ICM42600_REG_OFFSET_USER0;
468 reg = INV_ICM42600_REG_OFFSET_USER1;
471 reg = INV_ICM42600_REG_OFFSET_USER3;
477 /* inv_icm42600_gyro_calibbias: min - step - max in nano */
478 min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL +
479 (int64_t)inv_icm42600_gyro_calibbias[1];
480 max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL +
481 (int64_t)inv_icm42600_gyro_calibbias[5];
482 val64 = (int64_t)val * 1000000000LL + (int64_t)val2;
483 if (val64 < min || val64 > max)
487 * convert rad/s to dps then to raw value
488 * rad to dps: 180 / Pi
489 * dps to raw 12 bits signed, max 64: 2048 / 64
490 * val in nano (1000000000)
491 * val * 180 * 2048 / (Pi * 1000000000 * 64)
493 val64 = val64 * 180LL * 2048LL;
494 /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */
496 val64 += 3141592653LL * 64LL / 2LL;
498 val64 -= 3141592653LL * 64LL / 2LL;
499 offset = div64_s64(val64, 3141592653LL * 64LL);
501 /* clamp value limited to 12 bits signed */
504 else if (offset > 2047)
507 pm_runtime_get_sync(dev);
508 mutex_lock(&st->lock);
510 switch (chan->channel2) {
512 /* OFFSET_USER1 register is shared */
513 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
517 st->buffer[0] = offset & 0xFF;
518 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
521 /* OFFSET_USER1 register is shared */
522 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
526 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
527 st->buffer[1] = offset & 0xFF;
530 /* OFFSET_USER4 register is shared */
531 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
535 st->buffer[0] = offset & 0xFF;
536 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
543 ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
546 mutex_unlock(&st->lock);
547 pm_runtime_mark_last_busy(dev);
548 pm_runtime_put_autosuspend(dev);
552 static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
553 struct iio_chan_spec const *chan,
554 int *val, int *val2, long mask)
556 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
560 switch (chan->type) {
564 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
570 case IIO_CHAN_INFO_RAW:
571 ret = iio_device_claim_direct_mode(indio_dev);
574 ret = inv_icm42600_gyro_read_sensor(st, chan, &data);
575 iio_device_release_direct_mode(indio_dev);
580 case IIO_CHAN_INFO_SCALE:
581 return inv_icm42600_gyro_read_scale(st, val, val2);
582 case IIO_CHAN_INFO_SAMP_FREQ:
583 return inv_icm42600_gyro_read_odr(st, val, val2);
584 case IIO_CHAN_INFO_CALIBBIAS:
585 return inv_icm42600_gyro_read_offset(st, chan, val, val2);
591 static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev,
592 struct iio_chan_spec const *chan,
594 int *type, int *length, long mask)
596 if (chan->type != IIO_ANGL_VEL)
600 case IIO_CHAN_INFO_SCALE:
601 *vals = inv_icm42600_gyro_scale;
602 *type = IIO_VAL_INT_PLUS_NANO;
603 *length = ARRAY_SIZE(inv_icm42600_gyro_scale);
604 return IIO_AVAIL_LIST;
605 case IIO_CHAN_INFO_SAMP_FREQ:
606 *vals = inv_icm42600_gyro_odr;
607 *type = IIO_VAL_INT_PLUS_MICRO;
608 *length = ARRAY_SIZE(inv_icm42600_gyro_odr);
609 return IIO_AVAIL_LIST;
610 case IIO_CHAN_INFO_CALIBBIAS:
611 *vals = inv_icm42600_gyro_calibbias;
612 *type = IIO_VAL_INT_PLUS_NANO;
613 return IIO_AVAIL_RANGE;
619 static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
620 struct iio_chan_spec const *chan,
621 int val, int val2, long mask)
623 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
626 if (chan->type != IIO_ANGL_VEL)
630 case IIO_CHAN_INFO_SCALE:
631 ret = iio_device_claim_direct_mode(indio_dev);
634 ret = inv_icm42600_gyro_write_scale(st, val, val2);
635 iio_device_release_direct_mode(indio_dev);
637 case IIO_CHAN_INFO_SAMP_FREQ:
638 return inv_icm42600_gyro_write_odr(indio_dev, val, val2);
639 case IIO_CHAN_INFO_CALIBBIAS:
640 ret = iio_device_claim_direct_mode(indio_dev);
643 ret = inv_icm42600_gyro_write_offset(st, chan, val, val2);
644 iio_device_release_direct_mode(indio_dev);
651 static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
652 struct iio_chan_spec const *chan,
655 if (chan->type != IIO_ANGL_VEL)
659 case IIO_CHAN_INFO_SCALE:
660 return IIO_VAL_INT_PLUS_NANO;
661 case IIO_CHAN_INFO_SAMP_FREQ:
662 return IIO_VAL_INT_PLUS_MICRO;
663 case IIO_CHAN_INFO_CALIBBIAS:
664 return IIO_VAL_INT_PLUS_NANO;
670 static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
673 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
676 mutex_lock(&st->lock);
678 st->fifo.watermark.gyro = val;
679 ret = inv_icm42600_buffer_update_watermark(st);
681 mutex_unlock(&st->lock);
686 static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
689 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
695 mutex_lock(&st->lock);
697 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
699 ret = st->fifo.nb.gyro;
701 mutex_unlock(&st->lock);
706 static const struct iio_info inv_icm42600_gyro_info = {
707 .read_raw = inv_icm42600_gyro_read_raw,
708 .read_avail = inv_icm42600_gyro_read_avail,
709 .write_raw = inv_icm42600_gyro_write_raw,
710 .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
711 .debugfs_reg_access = inv_icm42600_debugfs_reg,
712 .update_scan_mode = inv_icm42600_gyro_update_scan_mode,
713 .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
714 .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
717 struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
719 struct device *dev = regmap_get_device(st->map);
721 struct inv_icm42600_timestamp *ts;
722 struct iio_dev *indio_dev;
723 struct iio_buffer *buffer;
726 name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
728 return ERR_PTR(-ENOMEM);
730 indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
732 return ERR_PTR(-ENOMEM);
734 buffer = devm_iio_kfifo_allocate(dev);
736 return ERR_PTR(-ENOMEM);
738 ts = iio_priv(indio_dev);
739 inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
741 iio_device_set_drvdata(indio_dev, st);
742 indio_dev->name = name;
743 indio_dev->info = &inv_icm42600_gyro_info;
744 indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
745 indio_dev->channels = inv_icm42600_gyro_channels;
746 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
747 indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
748 indio_dev->setup_ops = &inv_icm42600_buffer_ops;
750 iio_device_attach_buffer(indio_dev, buffer);
752 ret = devm_iio_device_register(dev, indio_dev);
759 int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
761 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
762 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
765 const void *accel, *gyro, *timestamp;
769 struct inv_icm42600_gyro_buffer buffer;
771 /* parse all fifo packets */
772 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
773 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
774 &accel, &gyro, &temp, ×tamp, &odr);
775 /* quit if error or FIFO is empty */
779 /* skip packet if no gyro data or data is invalid */
780 if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
784 if (odr & INV_ICM42600_SENSOR_GYRO)
785 inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
786 st->fifo.nb.total, no);
788 /* buffer is copied to userspace, zeroing it to avoid any data leak */
789 memset(&buffer, 0, sizeof(buffer));
790 memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
791 /* convert 8 bits FIFO temperature in high resolution format */
792 buffer.temp = temp ? (*temp * 64) : 0;
793 ts_val = inv_icm42600_timestamp_pop(ts);
794 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);