1
2
3
4
5
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>
16
17#include "inv_icm42600.h"
18#include "inv_icm42600_temp.h"
19#include "inv_icm42600_buffer.h"
20#include "inv_icm42600_timestamp.h"
21
22#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
23 { \
24 .type = IIO_ANGL_VEL, \
25 .modified = 1, \
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, \
40 .scan_type = { \
41 .sign = 's', \
42 .realbits = 16, \
43 .storagebits = 16, \
44 .endianness = IIO_BE, \
45 }, \
46 .ext_info = _ext_info, \
47 }
48
49enum 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,
55};
56
57static 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),
59 {},
60};
61
62static 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),
71};
72
73
74
75
76
77struct inv_icm42600_gyro_buffer {
78 struct inv_icm42600_fifo_sensor_data gyro;
79 int16_t temp;
80 int64_t timestamp __aligned(8);
81};
82
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))
87
88#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
89
90static const unsigned long inv_icm42600_gyro_scan_masks[] = {
91
92 INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
93 0,
94};
95
96
97static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
98 const unsigned long *scan_mask)
99{
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;
106 unsigned int sleep;
107 int ret;
108
109 mutex_lock(&st->lock);
110
111 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
112
113 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
114 if (ret)
115 goto out_unlock;
116 fifo_en |= INV_ICM42600_SENSOR_TEMP;
117 }
118
119 if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
120
121 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
122 ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
123 if (ret)
124 goto out_unlock;
125 fifo_en |= INV_ICM42600_SENSOR_GYRO;
126 }
127
128
129 inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
130 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
131 if (ret)
132 goto out_unlock;
133
134 ret = inv_icm42600_buffer_update_watermark(st);
135
136out_unlock:
137 mutex_unlock(&st->lock);
138
139 if (sleep_gyro > sleep_temp)
140 sleep = sleep_gyro;
141 else
142 sleep = sleep_temp;
143 if (sleep)
144 msleep(sleep);
145 return ret;
146}
147
148static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
149 struct iio_chan_spec const *chan,
150 int16_t *val)
151{
152 struct device *dev = regmap_get_device(st->map);
153 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
154 unsigned int reg;
155 __be16 *data;
156 int ret;
157
158 if (chan->type != IIO_ANGL_VEL)
159 return -EINVAL;
160
161 switch (chan->channel2) {
162 case IIO_MOD_X:
163 reg = INV_ICM42600_REG_GYRO_DATA_X;
164 break;
165 case IIO_MOD_Y:
166 reg = INV_ICM42600_REG_GYRO_DATA_Y;
167 break;
168 case IIO_MOD_Z:
169 reg = INV_ICM42600_REG_GYRO_DATA_Z;
170 break;
171 default:
172 return -EINVAL;
173 }
174
175 pm_runtime_get_sync(dev);
176 mutex_lock(&st->lock);
177
178
179 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
180 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
181 if (ret)
182 goto exit;
183
184
185 data = (__be16 *)&st->buffer[0];
186 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
187 if (ret)
188 goto exit;
189
190 *val = (int16_t)be16_to_cpup(data);
191 if (*val == INV_ICM42600_DATA_INVALID)
192 ret = -EINVAL;
193exit:
194 mutex_unlock(&st->lock);
195 pm_runtime_mark_last_busy(dev);
196 pm_runtime_put_autosuspend(dev);
197 return ret;
198}
199
200
201static const int inv_icm42600_gyro_scale[] = {
202
203 [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0,
204 [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264,
205
206 [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0,
207 [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632,
208
209 [2 * INV_ICM42600_GYRO_FS_500DPS] = 0,
210 [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316,
211
212 [2 * INV_ICM42600_GYRO_FS_250DPS] = 0,
213 [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158,
214
215 [2 * INV_ICM42600_GYRO_FS_125DPS] = 0,
216 [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579,
217
218 [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0,
219 [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290,
220
221 [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0,
222 [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645,
223
224 [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0,
225 [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322,
226};
227
228static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st,
229 int *val, int *val2)
230{
231 unsigned int idx;
232
233 idx = st->conf.gyro.fs;
234
235 *val = inv_icm42600_gyro_scale[2 * idx];
236 *val2 = inv_icm42600_gyro_scale[2 * idx + 1];
237 return IIO_VAL_INT_PLUS_NANO;
238}
239
240static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st,
241 int val, int val2)
242{
243 struct device *dev = regmap_get_device(st->map);
244 unsigned int idx;
245 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
246 int ret;
247
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])
251 break;
252 }
253 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale))
254 return -EINVAL;
255
256 conf.fs = idx / 2;
257
258 pm_runtime_get_sync(dev);
259 mutex_lock(&st->lock);
260
261 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
262
263 mutex_unlock(&st->lock);
264 pm_runtime_mark_last_busy(dev);
265 pm_runtime_put_autosuspend(dev);
266
267 return ret;
268}
269
270
271static const int inv_icm42600_gyro_odr[] = {
272
273 12, 500000,
274
275 25, 0,
276
277 50, 0,
278
279 100, 0,
280
281 200, 0,
282
283 1000, 0,
284
285 2000, 0,
286
287 4000, 0,
288};
289
290static 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,
299};
300
301static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
302 int *val, int *val2)
303{
304 unsigned int odr;
305 unsigned int i;
306
307 odr = st->conf.gyro.odr;
308
309 for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) {
310 if (inv_icm42600_gyro_odr_conv[i] == odr)
311 break;
312 }
313 if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv))
314 return -EINVAL;
315
316 *val = inv_icm42600_gyro_odr[2 * i];
317 *val2 = inv_icm42600_gyro_odr[2 * i + 1];
318
319 return IIO_VAL_INT_PLUS_MICRO;
320}
321
322static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
323 int val, int val2)
324{
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);
328 unsigned int idx;
329 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
330 int ret;
331
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])
335 break;
336 }
337 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr))
338 return -EINVAL;
339
340 conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
341
342 pm_runtime_get_sync(dev);
343 mutex_lock(&st->lock);
344
345 ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
346 iio_buffer_enabled(indio_dev));
347 if (ret)
348 goto out_unlock;
349
350 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
351 if (ret)
352 goto out_unlock;
353 inv_icm42600_buffer_update_fifo_period(st);
354 inv_icm42600_buffer_update_watermark(st);
355
356out_unlock:
357 mutex_unlock(&st->lock);
358 pm_runtime_mark_last_busy(dev);
359 pm_runtime_put_autosuspend(dev);
360
361 return ret;
362}
363
364
365
366
367
368static int inv_icm42600_gyro_calibbias[] = {
369 -1, 117010721,
370 0, 545415,
371 1, 116465306,
372};
373
374static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
375 struct iio_chan_spec const *chan,
376 int *val, int *val2)
377{
378 struct device *dev = regmap_get_device(st->map);
379 int64_t val64;
380 int32_t bias;
381 unsigned int reg;
382 int16_t offset;
383 uint8_t data[2];
384 int ret;
385
386 if (chan->type != IIO_ANGL_VEL)
387 return -EINVAL;
388
389 switch (chan->channel2) {
390 case IIO_MOD_X:
391 reg = INV_ICM42600_REG_OFFSET_USER0;
392 break;
393 case IIO_MOD_Y:
394 reg = INV_ICM42600_REG_OFFSET_USER1;
395 break;
396 case IIO_MOD_Z:
397 reg = INV_ICM42600_REG_OFFSET_USER3;
398 break;
399 default:
400 return -EINVAL;
401 }
402
403 pm_runtime_get_sync(dev);
404 mutex_lock(&st->lock);
405
406 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
407 memcpy(data, st->buffer, sizeof(data));
408
409 mutex_unlock(&st->lock);
410 pm_runtime_mark_last_busy(dev);
411 pm_runtime_put_autosuspend(dev);
412 if (ret)
413 return ret;
414
415
416 switch (chan->channel2) {
417 case IIO_MOD_X:
418 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
419 break;
420 case IIO_MOD_Y:
421 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
422 break;
423 case IIO_MOD_Z:
424 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
425 break;
426 default:
427 return -EINVAL;
428 }
429
430
431
432
433
434
435
436
437 val64 = (int64_t)offset * 64LL * 3141592653LL;
438
439 if (val64 >= 0)
440 val64 += 2048 * 180 / 2;
441 else
442 val64 -= 2048 * 180 / 2;
443 bias = div_s64(val64, 2048 * 180);
444 *val = bias / 1000000000L;
445 *val2 = bias % 1000000000L;
446
447 return IIO_VAL_INT_PLUS_NANO;
448}
449
450static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
451 struct iio_chan_spec const *chan,
452 int val, int val2)
453{
454 struct device *dev = regmap_get_device(st->map);
455 int64_t val64, min, max;
456 unsigned int reg, regval;
457 int16_t offset;
458 int ret;
459
460 if (chan->type != IIO_ANGL_VEL)
461 return -EINVAL;
462
463 switch (chan->channel2) {
464 case IIO_MOD_X:
465 reg = INV_ICM42600_REG_OFFSET_USER0;
466 break;
467 case IIO_MOD_Y:
468 reg = INV_ICM42600_REG_OFFSET_USER1;
469 break;
470 case IIO_MOD_Z:
471 reg = INV_ICM42600_REG_OFFSET_USER3;
472 break;
473 default:
474 return -EINVAL;
475 }
476
477
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)
484 return -EINVAL;
485
486
487
488
489
490
491
492
493 val64 = val64 * 180LL * 2048LL;
494
495 if (val64 >= 0)
496 val64 += 3141592653LL * 64LL / 2LL;
497 else
498 val64 -= 3141592653LL * 64LL / 2LL;
499 offset = div64_s64(val64, 3141592653LL * 64LL);
500
501
502 if (offset < -2048)
503 offset = -2048;
504 else if (offset > 2047)
505 offset = 2047;
506
507 pm_runtime_get_sync(dev);
508 mutex_lock(&st->lock);
509
510 switch (chan->channel2) {
511 case IIO_MOD_X:
512
513 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
514 ®val);
515 if (ret)
516 goto out_unlock;
517 st->buffer[0] = offset & 0xFF;
518 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
519 break;
520 case IIO_MOD_Y:
521
522 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
523 ®val);
524 if (ret)
525 goto out_unlock;
526 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
527 st->buffer[1] = offset & 0xFF;
528 break;
529 case IIO_MOD_Z:
530
531 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
532 ®val);
533 if (ret)
534 goto out_unlock;
535 st->buffer[0] = offset & 0xFF;
536 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
537 break;
538 default:
539 ret = -EINVAL;
540 goto out_unlock;
541 }
542
543 ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
544
545out_unlock:
546 mutex_unlock(&st->lock);
547 pm_runtime_mark_last_busy(dev);
548 pm_runtime_put_autosuspend(dev);
549 return ret;
550}
551
552static 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)
555{
556 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
557 int16_t data;
558 int ret;
559
560 switch (chan->type) {
561 case IIO_ANGL_VEL:
562 break;
563 case IIO_TEMP:
564 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
565 default:
566 return -EINVAL;
567 }
568
569 switch (mask) {
570 case IIO_CHAN_INFO_RAW:
571 ret = iio_device_claim_direct_mode(indio_dev);
572 if (ret)
573 return ret;
574 ret = inv_icm42600_gyro_read_sensor(st, chan, &data);
575 iio_device_release_direct_mode(indio_dev);
576 if (ret)
577 return ret;
578 *val = data;
579 return IIO_VAL_INT;
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);
586 default:
587 return -EINVAL;
588 }
589}
590
591static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev,
592 struct iio_chan_spec const *chan,
593 const int **vals,
594 int *type, int *length, long mask)
595{
596 if (chan->type != IIO_ANGL_VEL)
597 return -EINVAL;
598
599 switch (mask) {
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;
614 default:
615 return -EINVAL;
616 }
617}
618
619static 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)
622{
623 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
624 int ret;
625
626 if (chan->type != IIO_ANGL_VEL)
627 return -EINVAL;
628
629 switch (mask) {
630 case IIO_CHAN_INFO_SCALE:
631 ret = iio_device_claim_direct_mode(indio_dev);
632 if (ret)
633 return ret;
634 ret = inv_icm42600_gyro_write_scale(st, val, val2);
635 iio_device_release_direct_mode(indio_dev);
636 return ret;
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);
641 if (ret)
642 return ret;
643 ret = inv_icm42600_gyro_write_offset(st, chan, val, val2);
644 iio_device_release_direct_mode(indio_dev);
645 return ret;
646 default:
647 return -EINVAL;
648 }
649}
650
651static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
652 struct iio_chan_spec const *chan,
653 long mask)
654{
655 if (chan->type != IIO_ANGL_VEL)
656 return -EINVAL;
657
658 switch (mask) {
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;
665 default:
666 return -EINVAL;
667 }
668}
669
670static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
671 unsigned int val)
672{
673 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
674 int ret;
675
676 mutex_lock(&st->lock);
677
678 st->fifo.watermark.gyro = val;
679 ret = inv_icm42600_buffer_update_watermark(st);
680
681 mutex_unlock(&st->lock);
682
683 return ret;
684}
685
686static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
687 unsigned int count)
688{
689 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
690 int ret;
691
692 if (count == 0)
693 return 0;
694
695 mutex_lock(&st->lock);
696
697 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
698 if (!ret)
699 ret = st->fifo.nb.gyro;
700
701 mutex_unlock(&st->lock);
702
703 return ret;
704}
705
706static 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,
715};
716
717struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
718{
719 struct device *dev = regmap_get_device(st->map);
720 const char *name;
721 struct inv_icm42600_timestamp *ts;
722 struct iio_dev *indio_dev;
723 int ret;
724
725 name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
726 if (!name)
727 return ERR_PTR(-ENOMEM);
728
729 indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
730 if (!indio_dev)
731 return ERR_PTR(-ENOMEM);
732
733 ts = iio_priv(indio_dev);
734 inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
735
736 iio_device_set_drvdata(indio_dev, st);
737 indio_dev->name = name;
738 indio_dev->info = &inv_icm42600_gyro_info;
739 indio_dev->modes = INDIO_DIRECT_MODE;
740 indio_dev->channels = inv_icm42600_gyro_channels;
741 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
742 indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
743 indio_dev->setup_ops = &inv_icm42600_buffer_ops;
744
745 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
746 INDIO_BUFFER_SOFTWARE,
747 &inv_icm42600_buffer_ops);
748 if (ret)
749 return ERR_PTR(ret);
750
751 ret = devm_iio_device_register(dev, indio_dev);
752 if (ret)
753 return ERR_PTR(ret);
754
755 return indio_dev;
756}
757
758int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
759{
760 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
761 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
762 ssize_t i, size;
763 unsigned int no;
764 const void *accel, *gyro, *timestamp;
765 const int8_t *temp;
766 unsigned int odr;
767 int64_t ts_val;
768 struct inv_icm42600_gyro_buffer buffer;
769
770
771 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
772 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
773 &accel, &gyro, &temp, ×tamp, &odr);
774
775 if (size <= 0)
776 return size;
777
778
779 if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
780 continue;
781
782
783 if (odr & INV_ICM42600_SENSOR_GYRO)
784 inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
785 st->fifo.nb.total, no);
786
787
788 memset(&buffer, 0, sizeof(buffer));
789 memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
790
791 buffer.temp = temp ? (*temp * 64) : 0;
792 ts_val = inv_icm42600_timestamp_pop(ts);
793 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
794 }
795
796 return 0;
797}
798
