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_ACCEL_CHAN(_modifier, _index, _ext_info) \
23 { \
24 .type = IIO_ACCEL, \
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_accel_scan {
50 INV_ICM42600_ACCEL_SCAN_X,
51 INV_ICM42600_ACCEL_SCAN_Y,
52 INV_ICM42600_ACCEL_SCAN_Z,
53 INV_ICM42600_ACCEL_SCAN_TEMP,
54 INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
55};
56
57static const struct iio_chan_spec_ext_info inv_icm42600_accel_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_accel_channels[] = {
63 INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X,
64 inv_icm42600_accel_ext_infos),
65 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y,
66 inv_icm42600_accel_ext_infos),
67 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
68 inv_icm42600_accel_ext_infos),
69 INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
70 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
71};
72
73
74
75
76
77struct inv_icm42600_accel_buffer {
78 struct inv_icm42600_fifo_sensor_data accel;
79 int16_t temp;
80 int64_t timestamp __aligned(8);
81};
82
83#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
84 (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
85 BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
86 BIT(INV_ICM42600_ACCEL_SCAN_Z))
87
88#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
89
90static const unsigned long inv_icm42600_accel_scan_masks[] = {
91
92 INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
93 0,
94};
95
96
97static int inv_icm42600_accel_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_temp = 0;
105 unsigned int sleep_accel = 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_ACCEL_3AXIS) {
120
121 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
122 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
123 if (ret)
124 goto out_unlock;
125 fifo_en |= INV_ICM42600_SENSOR_ACCEL;
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_accel > sleep_temp)
140 sleep = sleep_accel;
141 else
142 sleep = sleep_temp;
143 if (sleep)
144 msleep(sleep);
145 return ret;
146}
147
148static int inv_icm42600_accel_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_ACCEL)
159 return -EINVAL;
160
161 switch (chan->channel2) {
162 case IIO_MOD_X:
163 reg = INV_ICM42600_REG_ACCEL_DATA_X;
164 break;
165 case IIO_MOD_Y:
166 reg = INV_ICM42600_REG_ACCEL_DATA_Y;
167 break;
168 case IIO_MOD_Z:
169 reg = INV_ICM42600_REG_ACCEL_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_accel_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_accel_scale[] = {
202
203 [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
204 [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
205
206 [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
207 [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
208
209 [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
210 [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
211
212 [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
213 [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
214};
215
216static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st,
217 int *val, int *val2)
218{
219 unsigned int idx;
220
221 idx = st->conf.accel.fs;
222
223 *val = inv_icm42600_accel_scale[2 * idx];
224 *val2 = inv_icm42600_accel_scale[2 * idx + 1];
225 return IIO_VAL_INT_PLUS_NANO;
226}
227
228static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st,
229 int val, int val2)
230{
231 struct device *dev = regmap_get_device(st->map);
232 unsigned int idx;
233 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
234 int ret;
235
236 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) {
237 if (val == inv_icm42600_accel_scale[idx] &&
238 val2 == inv_icm42600_accel_scale[idx + 1])
239 break;
240 }
241 if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale))
242 return -EINVAL;
243
244 conf.fs = idx / 2;
245
246 pm_runtime_get_sync(dev);
247 mutex_lock(&st->lock);
248
249 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
250
251 mutex_unlock(&st->lock);
252 pm_runtime_mark_last_busy(dev);
253 pm_runtime_put_autosuspend(dev);
254
255 return ret;
256}
257
258
259static const int inv_icm42600_accel_odr[] = {
260
261 12, 500000,
262
263 25, 0,
264
265 50, 0,
266
267 100, 0,
268
269 200, 0,
270
271 1000, 0,
272
273 2000, 0,
274
275 4000, 0,
276};
277
278static const int inv_icm42600_accel_odr_conv[] = {
279 INV_ICM42600_ODR_12_5HZ,
280 INV_ICM42600_ODR_25HZ,
281 INV_ICM42600_ODR_50HZ,
282 INV_ICM42600_ODR_100HZ,
283 INV_ICM42600_ODR_200HZ,
284 INV_ICM42600_ODR_1KHZ_LN,
285 INV_ICM42600_ODR_2KHZ_LN,
286 INV_ICM42600_ODR_4KHZ_LN,
287};
288
289static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
290 int *val, int *val2)
291{
292 unsigned int odr;
293 unsigned int i;
294
295 odr = st->conf.accel.odr;
296
297 for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
298 if (inv_icm42600_accel_odr_conv[i] == odr)
299 break;
300 }
301 if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
302 return -EINVAL;
303
304 *val = inv_icm42600_accel_odr[2 * i];
305 *val2 = inv_icm42600_accel_odr[2 * i + 1];
306
307 return IIO_VAL_INT_PLUS_MICRO;
308}
309
310static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
311 int val, int val2)
312{
313 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
314 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
315 struct device *dev = regmap_get_device(st->map);
316 unsigned int idx;
317 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
318 int ret;
319
320 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
321 if (val == inv_icm42600_accel_odr[idx] &&
322 val2 == inv_icm42600_accel_odr[idx + 1])
323 break;
324 }
325 if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
326 return -EINVAL;
327
328 conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
329
330 pm_runtime_get_sync(dev);
331 mutex_lock(&st->lock);
332
333 ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
334 iio_buffer_enabled(indio_dev));
335 if (ret)
336 goto out_unlock;
337
338 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
339 if (ret)
340 goto out_unlock;
341 inv_icm42600_buffer_update_fifo_period(st);
342 inv_icm42600_buffer_update_watermark(st);
343
344out_unlock:
345 mutex_unlock(&st->lock);
346 pm_runtime_mark_last_busy(dev);
347 pm_runtime_put_autosuspend(dev);
348
349 return ret;
350}
351
352
353
354
355
356static int inv_icm42600_accel_calibbias[] = {
357 -10, 42010,
358 0, 4903,
359 10, 37106,
360};
361
362static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
363 struct iio_chan_spec const *chan,
364 int *val, int *val2)
365{
366 struct device *dev = regmap_get_device(st->map);
367 int64_t val64;
368 int32_t bias;
369 unsigned int reg;
370 int16_t offset;
371 uint8_t data[2];
372 int ret;
373
374 if (chan->type != IIO_ACCEL)
375 return -EINVAL;
376
377 switch (chan->channel2) {
378 case IIO_MOD_X:
379 reg = INV_ICM42600_REG_OFFSET_USER4;
380 break;
381 case IIO_MOD_Y:
382 reg = INV_ICM42600_REG_OFFSET_USER6;
383 break;
384 case IIO_MOD_Z:
385 reg = INV_ICM42600_REG_OFFSET_USER7;
386 break;
387 default:
388 return -EINVAL;
389 }
390
391 pm_runtime_get_sync(dev);
392 mutex_lock(&st->lock);
393
394 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
395 memcpy(data, st->buffer, sizeof(data));
396
397 mutex_unlock(&st->lock);
398 pm_runtime_mark_last_busy(dev);
399 pm_runtime_put_autosuspend(dev);
400 if (ret)
401 return ret;
402
403
404 switch (chan->channel2) {
405 case IIO_MOD_X:
406 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
407 break;
408 case IIO_MOD_Y:
409 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
410 break;
411 case IIO_MOD_Z:
412 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
413 break;
414 default:
415 return -EINVAL;
416 }
417
418
419
420
421
422
423
424
425 val64 = (int64_t)offset * 5LL * 9806650LL;
426
427 if (val64 >= 0)
428 val64 += 10000LL / 2LL;
429 else
430 val64 -= 10000LL / 2LL;
431 bias = div_s64(val64, 10000L);
432 *val = bias / 1000000L;
433 *val2 = bias % 1000000L;
434
435 return IIO_VAL_INT_PLUS_MICRO;
436}
437
438static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
439 struct iio_chan_spec const *chan,
440 int val, int val2)
441{
442 struct device *dev = regmap_get_device(st->map);
443 int64_t val64;
444 int32_t min, max;
445 unsigned int reg, regval;
446 int16_t offset;
447 int ret;
448
449 if (chan->type != IIO_ACCEL)
450 return -EINVAL;
451
452 switch (chan->channel2) {
453 case IIO_MOD_X:
454 reg = INV_ICM42600_REG_OFFSET_USER4;
455 break;
456 case IIO_MOD_Y:
457 reg = INV_ICM42600_REG_OFFSET_USER6;
458 break;
459 case IIO_MOD_Z:
460 reg = INV_ICM42600_REG_OFFSET_USER7;
461 break;
462 default:
463 return -EINVAL;
464 }
465
466
467 min = inv_icm42600_accel_calibbias[0] * 1000000L +
468 inv_icm42600_accel_calibbias[1];
469 max = inv_icm42600_accel_calibbias[4] * 1000000L +
470 inv_icm42600_accel_calibbias[5];
471 val64 = (int64_t)val * 1000000LL + (int64_t)val2;
472 if (val64 < min || val64 > max)
473 return -EINVAL;
474
475
476
477
478
479
480
481
482 val64 = val64 * 10000LL;
483
484 if (val64 >= 0)
485 val64 += 9806650 * 5 / 2;
486 else
487 val64 -= 9806650 * 5 / 2;
488 offset = div_s64(val64, 9806650 * 5);
489
490
491 if (offset < -2048)
492 offset = -2048;
493 else if (offset > 2047)
494 offset = 2047;
495
496 pm_runtime_get_sync(dev);
497 mutex_lock(&st->lock);
498
499 switch (chan->channel2) {
500 case IIO_MOD_X:
501
502 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
503 ®val);
504 if (ret)
505 goto out_unlock;
506 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
507 st->buffer[1] = offset & 0xFF;
508 break;
509 case IIO_MOD_Y:
510
511 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
512 ®val);
513 if (ret)
514 goto out_unlock;
515 st->buffer[0] = offset & 0xFF;
516 st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
517 break;
518 case IIO_MOD_Z:
519
520 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
521 ®val);
522 if (ret)
523 goto out_unlock;
524 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
525 st->buffer[1] = offset & 0xFF;
526 break;
527 default:
528 ret = -EINVAL;
529 goto out_unlock;
530 }
531
532 ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
533
534out_unlock:
535 mutex_unlock(&st->lock);
536 pm_runtime_mark_last_busy(dev);
537 pm_runtime_put_autosuspend(dev);
538 return ret;
539}
540
541static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
542 struct iio_chan_spec const *chan,
543 int *val, int *val2, long mask)
544{
545 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
546 int16_t data;
547 int ret;
548
549 switch (chan->type) {
550 case IIO_ACCEL:
551 break;
552 case IIO_TEMP:
553 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
554 default:
555 return -EINVAL;
556 }
557
558 switch (mask) {
559 case IIO_CHAN_INFO_RAW:
560 ret = iio_device_claim_direct_mode(indio_dev);
561 if (ret)
562 return ret;
563 ret = inv_icm42600_accel_read_sensor(st, chan, &data);
564 iio_device_release_direct_mode(indio_dev);
565 if (ret)
566 return ret;
567 *val = data;
568 return IIO_VAL_INT;
569 case IIO_CHAN_INFO_SCALE:
570 return inv_icm42600_accel_read_scale(st, val, val2);
571 case IIO_CHAN_INFO_SAMP_FREQ:
572 return inv_icm42600_accel_read_odr(st, val, val2);
573 case IIO_CHAN_INFO_CALIBBIAS:
574 return inv_icm42600_accel_read_offset(st, chan, val, val2);
575 default:
576 return -EINVAL;
577 }
578}
579
580static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
581 struct iio_chan_spec const *chan,
582 const int **vals,
583 int *type, int *length, long mask)
584{
585 if (chan->type != IIO_ACCEL)
586 return -EINVAL;
587
588 switch (mask) {
589 case IIO_CHAN_INFO_SCALE:
590 *vals = inv_icm42600_accel_scale;
591 *type = IIO_VAL_INT_PLUS_NANO;
592 *length = ARRAY_SIZE(inv_icm42600_accel_scale);
593 return IIO_AVAIL_LIST;
594 case IIO_CHAN_INFO_SAMP_FREQ:
595 *vals = inv_icm42600_accel_odr;
596 *type = IIO_VAL_INT_PLUS_MICRO;
597 *length = ARRAY_SIZE(inv_icm42600_accel_odr);
598 return IIO_AVAIL_LIST;
599 case IIO_CHAN_INFO_CALIBBIAS:
600 *vals = inv_icm42600_accel_calibbias;
601 *type = IIO_VAL_INT_PLUS_MICRO;
602 return IIO_AVAIL_RANGE;
603 default:
604 return -EINVAL;
605 }
606}
607
608static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
609 struct iio_chan_spec const *chan,
610 int val, int val2, long mask)
611{
612 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
613 int ret;
614
615 if (chan->type != IIO_ACCEL)
616 return -EINVAL;
617
618 switch (mask) {
619 case IIO_CHAN_INFO_SCALE:
620 ret = iio_device_claim_direct_mode(indio_dev);
621 if (ret)
622 return ret;
623 ret = inv_icm42600_accel_write_scale(st, val, val2);
624 iio_device_release_direct_mode(indio_dev);
625 return ret;
626 case IIO_CHAN_INFO_SAMP_FREQ:
627 return inv_icm42600_accel_write_odr(indio_dev, val, val2);
628 case IIO_CHAN_INFO_CALIBBIAS:
629 ret = iio_device_claim_direct_mode(indio_dev);
630 if (ret)
631 return ret;
632 ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
633 iio_device_release_direct_mode(indio_dev);
634 return ret;
635 default:
636 return -EINVAL;
637 }
638}
639
640static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
641 struct iio_chan_spec const *chan,
642 long mask)
643{
644 if (chan->type != IIO_ACCEL)
645 return -EINVAL;
646
647 switch (mask) {
648 case IIO_CHAN_INFO_SCALE:
649 return IIO_VAL_INT_PLUS_NANO;
650 case IIO_CHAN_INFO_SAMP_FREQ:
651 return IIO_VAL_INT_PLUS_MICRO;
652 case IIO_CHAN_INFO_CALIBBIAS:
653 return IIO_VAL_INT_PLUS_MICRO;
654 default:
655 return -EINVAL;
656 }
657}
658
659static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
660 unsigned int val)
661{
662 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
663 int ret;
664
665 mutex_lock(&st->lock);
666
667 st->fifo.watermark.accel = val;
668 ret = inv_icm42600_buffer_update_watermark(st);
669
670 mutex_unlock(&st->lock);
671
672 return ret;
673}
674
675static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
676 unsigned int count)
677{
678 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
679 int ret;
680
681 if (count == 0)
682 return 0;
683
684 mutex_lock(&st->lock);
685
686 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
687 if (!ret)
688 ret = st->fifo.nb.accel;
689
690 mutex_unlock(&st->lock);
691
692 return ret;
693}
694
695static const struct iio_info inv_icm42600_accel_info = {
696 .read_raw = inv_icm42600_accel_read_raw,
697 .read_avail = inv_icm42600_accel_read_avail,
698 .write_raw = inv_icm42600_accel_write_raw,
699 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
700 .debugfs_reg_access = inv_icm42600_debugfs_reg,
701 .update_scan_mode = inv_icm42600_accel_update_scan_mode,
702 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
703 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
704};
705
706struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
707{
708 struct device *dev = regmap_get_device(st->map);
709 const char *name;
710 struct inv_icm42600_timestamp *ts;
711 struct iio_dev *indio_dev;
712 int ret;
713
714 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
715 if (!name)
716 return ERR_PTR(-ENOMEM);
717
718 indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
719 if (!indio_dev)
720 return ERR_PTR(-ENOMEM);
721
722 ts = iio_priv(indio_dev);
723 inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr));
724
725 iio_device_set_drvdata(indio_dev, st);
726 indio_dev->name = name;
727 indio_dev->info = &inv_icm42600_accel_info;
728 indio_dev->modes = INDIO_DIRECT_MODE;
729 indio_dev->channels = inv_icm42600_accel_channels;
730 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
731 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
732
733 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
734 INDIO_BUFFER_SOFTWARE,
735 &inv_icm42600_buffer_ops);
736 if (ret)
737 return ERR_PTR(ret);
738
739 ret = devm_iio_device_register(dev, indio_dev);
740 if (ret)
741 return ERR_PTR(ret);
742
743 return indio_dev;
744}
745
746int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
747{
748 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
749 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
750 ssize_t i, size;
751 unsigned int no;
752 const void *accel, *gyro, *timestamp;
753 const int8_t *temp;
754 unsigned int odr;
755 int64_t ts_val;
756 struct inv_icm42600_accel_buffer buffer;
757
758
759 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
760 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
761 &accel, &gyro, &temp, ×tamp, &odr);
762
763 if (size <= 0)
764 return size;
765
766
767 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
768 continue;
769
770
771 if (odr & INV_ICM42600_SENSOR_ACCEL)
772 inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
773 st->fifo.nb.total, no);
774
775
776 memset(&buffer, 0, sizeof(buffer));
777 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
778
779 buffer.temp = temp ? (*temp * 64) : 0;
780 ts_val = inv_icm42600_timestamp_pop(ts);
781 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
782 }
783
784 return 0;
785}
786
