1// SPDX-License-Identifier: GPL-2.0-or-later
2/*
3 * Copyright (C) 2020 Invensense, Inc.
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
14#include <linux/iio/buffer.h>
15#include <linux/iio/common/inv_sensors_timestamp.h>
16#include <linux/iio/iio.h>
17#include <linux/iio/kfifo_buf.h>
18
19#include "inv_icm42600.h"
20#include "inv_icm42600_temp.h"
21#include "inv_icm42600_buffer.h"
22
23#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
24 { \
25 .type = IIO_ACCEL, \
26 .modified = 1, \
27 .channel2 = _modifier, \
28 .info_mask_separate = \
29 BIT(IIO_CHAN_INFO_RAW) | \
30 BIT(IIO_CHAN_INFO_CALIBBIAS), \
31 .info_mask_shared_by_type = \
32 BIT(IIO_CHAN_INFO_SCALE), \
33 .info_mask_shared_by_type_available = \
34 BIT(IIO_CHAN_INFO_SCALE) | \
35 BIT(IIO_CHAN_INFO_CALIBBIAS), \
36 .info_mask_shared_by_all = \
37 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
38 .info_mask_shared_by_all_available = \
39 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
40 .scan_index = _index, \
41 .scan_type = { \
42 .sign = 's', \
43 .realbits = 16, \
44 .storagebits = 16, \
45 .endianness = IIO_BE, \
46 }, \
47 .ext_info = _ext_info, \
48 }
49
50enum inv_icm42600_accel_scan {
51 INV_ICM42600_ACCEL_SCAN_X,
52 INV_ICM42600_ACCEL_SCAN_Y,
53 INV_ICM42600_ACCEL_SCAN_Z,
54 INV_ICM42600_ACCEL_SCAN_TEMP,
55 INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
56};
57
58static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
59 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
60 {},
61};
62
63static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
64 INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X,
65 inv_icm42600_accel_ext_infos),
66 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y,
67 inv_icm42600_accel_ext_infos),
68 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
69 inv_icm42600_accel_ext_infos),
70 INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
71 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
72};
73
74/*
75 * IIO buffer data: size must be a power of 2 and timestamp aligned
76 * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp
77 */
78struct inv_icm42600_accel_buffer {
79 struct inv_icm42600_fifo_sensor_data accel;
80 int16_t temp;
81 int64_t timestamp __aligned(8);
82};
83
84#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
85 (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
86 BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
87 BIT(INV_ICM42600_ACCEL_SCAN_Z))
88
89#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
90
91static const unsigned long inv_icm42600_accel_scan_masks[] = {
92 /* 3-axis accel + temperature */
93 INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
94 0,
95};
96
97/* enable accelerometer sensor and FIFO write */
98static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
99 const unsigned long *scan_mask)
100{
101 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
102 struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
103 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
104 unsigned int fifo_en = 0;
105 unsigned int sleep_temp = 0;
106 unsigned int sleep_accel = 0;
107 unsigned int sleep;
108 int ret;
109
110 mutex_lock(&st->lock);
111
112 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
113 /* enable temp sensor */
114 ret = inv_icm42600_set_temp_conf(st, enable: true, sleep_ms: &sleep_temp);
115 if (ret)
116 goto out_unlock;
117 fifo_en |= INV_ICM42600_SENSOR_TEMP;
118 }
119
120 if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
121 /* enable accel sensor */
122 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
123 ret = inv_icm42600_set_accel_conf(st, conf: &conf, sleep_ms: &sleep_accel);
124 if (ret)
125 goto out_unlock;
126 fifo_en |= INV_ICM42600_SENSOR_ACCEL;
127 }
128
129 /* update data FIFO write */
130 inv_sensors_timestamp_apply_odr(ts, fifo_period: 0, fifo_nb: 0, fifo_no: 0);
131 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en: fifo_en | st->fifo.en);
132 if (ret)
133 goto out_unlock;
134
135 ret = inv_icm42600_buffer_update_watermark(st);
136
137out_unlock:
138 mutex_unlock(lock: &st->lock);
139 /* sleep maximum required time */
140 sleep = max(sleep_accel, sleep_temp);
141 if (sleep)
142 msleep(msecs: sleep);
143 return ret;
144}
145
146static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
147 struct iio_chan_spec const *chan,
148 int16_t *val)
149{
150 struct device *dev = regmap_get_device(map: st->map);
151 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
152 unsigned int reg;
153 __be16 *data;
154 int ret;
155
156 if (chan->type != IIO_ACCEL)
157 return -EINVAL;
158
159 switch (chan->channel2) {
160 case IIO_MOD_X:
161 reg = INV_ICM42600_REG_ACCEL_DATA_X;
162 break;
163 case IIO_MOD_Y:
164 reg = INV_ICM42600_REG_ACCEL_DATA_Y;
165 break;
166 case IIO_MOD_Z:
167 reg = INV_ICM42600_REG_ACCEL_DATA_Z;
168 break;
169 default:
170 return -EINVAL;
171 }
172
173 pm_runtime_get_sync(dev);
174 mutex_lock(&st->lock);
175
176 /* enable accel sensor */
177 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
178 ret = inv_icm42600_set_accel_conf(st, conf: &conf, NULL);
179 if (ret)
180 goto exit;
181
182 /* read accel register data */
183 data = (__be16 *)&st->buffer[0];
184 ret = regmap_bulk_read(map: st->map, reg, val: data, val_count: sizeof(*data));
185 if (ret)
186 goto exit;
187
188 *val = (int16_t)be16_to_cpup(p: data);
189 if (*val == INV_ICM42600_DATA_INVALID)
190 ret = -EINVAL;
191exit:
192 mutex_unlock(lock: &st->lock);
193 pm_runtime_mark_last_busy(dev);
194 pm_runtime_put_autosuspend(dev);
195 return ret;
196}
197
198/* IIO format int + nano */
199static const int inv_icm42600_accel_scale[] = {
200 /* +/- 16G => 0.004788403 m/s-2 */
201 [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
202 [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
203 /* +/- 8G => 0.002394202 m/s-2 */
204 [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
205 [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
206 /* +/- 4G => 0.001197101 m/s-2 */
207 [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
208 [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
209 /* +/- 2G => 0.000598550 m/s-2 */
210 [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
211 [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
212};
213
214static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st,
215 int *val, int *val2)
216{
217 unsigned int idx;
218
219 idx = st->conf.accel.fs;
220
221 *val = inv_icm42600_accel_scale[2 * idx];
222 *val2 = inv_icm42600_accel_scale[2 * idx + 1];
223 return IIO_VAL_INT_PLUS_NANO;
224}
225
226static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st,
227 int val, int val2)
228{
229 struct device *dev = regmap_get_device(map: st->map);
230 unsigned int idx;
231 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
232 int ret;
233
234 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) {
235 if (val == inv_icm42600_accel_scale[idx] &&
236 val2 == inv_icm42600_accel_scale[idx + 1])
237 break;
238 }
239 if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale))
240 return -EINVAL;
241
242 conf.fs = idx / 2;
243
244 pm_runtime_get_sync(dev);
245 mutex_lock(&st->lock);
246
247 ret = inv_icm42600_set_accel_conf(st, conf: &conf, NULL);
248
249 mutex_unlock(lock: &st->lock);
250 pm_runtime_mark_last_busy(dev);
251 pm_runtime_put_autosuspend(dev);
252
253 return ret;
254}
255
256/* IIO format int + micro */
257static const int inv_icm42600_accel_odr[] = {
258 /* 12.5Hz */
259 12, 500000,
260 /* 25Hz */
261 25, 0,
262 /* 50Hz */
263 50, 0,
264 /* 100Hz */
265 100, 0,
266 /* 200Hz */
267 200, 0,
268 /* 1kHz */
269 1000, 0,
270 /* 2kHz */
271 2000, 0,
272 /* 4kHz */
273 4000, 0,
274};
275
276static const int inv_icm42600_accel_odr_conv[] = {
277 INV_ICM42600_ODR_12_5HZ,
278 INV_ICM42600_ODR_25HZ,
279 INV_ICM42600_ODR_50HZ,
280 INV_ICM42600_ODR_100HZ,
281 INV_ICM42600_ODR_200HZ,
282 INV_ICM42600_ODR_1KHZ_LN,
283 INV_ICM42600_ODR_2KHZ_LN,
284 INV_ICM42600_ODR_4KHZ_LN,
285};
286
287static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
288 int *val, int *val2)
289{
290 unsigned int odr;
291 unsigned int i;
292
293 odr = st->conf.accel.odr;
294
295 for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
296 if (inv_icm42600_accel_odr_conv[i] == odr)
297 break;
298 }
299 if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
300 return -EINVAL;
301
302 *val = inv_icm42600_accel_odr[2 * i];
303 *val2 = inv_icm42600_accel_odr[2 * i + 1];
304
305 return IIO_VAL_INT_PLUS_MICRO;
306}
307
308static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
309 int val, int val2)
310{
311 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
312 struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
313 struct device *dev = regmap_get_device(map: st->map);
314 unsigned int idx;
315 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
316 int ret;
317
318 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
319 if (val == inv_icm42600_accel_odr[idx] &&
320 val2 == inv_icm42600_accel_odr[idx + 1])
321 break;
322 }
323 if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
324 return -EINVAL;
325
326 conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
327
328 pm_runtime_get_sync(dev);
329 mutex_lock(&st->lock);
330
331 ret = inv_sensors_timestamp_update_odr(ts, period: inv_icm42600_odr_to_period(odr: conf.odr),
332 fifo: iio_buffer_enabled(indio_dev));
333 if (ret)
334 goto out_unlock;
335
336 ret = inv_icm42600_set_accel_conf(st, conf: &conf, NULL);
337 if (ret)
338 goto out_unlock;
339 inv_icm42600_buffer_update_fifo_period(st);
340 inv_icm42600_buffer_update_watermark(st);
341
342out_unlock:
343 mutex_unlock(lock: &st->lock);
344 pm_runtime_mark_last_busy(dev);
345 pm_runtime_put_autosuspend(dev);
346
347 return ret;
348}
349
350/*
351 * Calibration bias values, IIO range format int + micro.
352 * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
353 */
354static int inv_icm42600_accel_calibbias[] = {
355 -10, 42010, /* min: -10.042010 m/s² */
356 0, 4903, /* step: 0.004903 m/s² */
357 10, 37106, /* max: 10.037106 m/s² */
358};
359
360static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
361 struct iio_chan_spec const *chan,
362 int *val, int *val2)
363{
364 struct device *dev = regmap_get_device(map: st->map);
365 int64_t val64;
366 int32_t bias;
367 unsigned int reg;
368 int16_t offset;
369 uint8_t data[2];
370 int ret;
371
372 if (chan->type != IIO_ACCEL)
373 return -EINVAL;
374
375 switch (chan->channel2) {
376 case IIO_MOD_X:
377 reg = INV_ICM42600_REG_OFFSET_USER4;
378 break;
379 case IIO_MOD_Y:
380 reg = INV_ICM42600_REG_OFFSET_USER6;
381 break;
382 case IIO_MOD_Z:
383 reg = INV_ICM42600_REG_OFFSET_USER7;
384 break;
385 default:
386 return -EINVAL;
387 }
388
389 pm_runtime_get_sync(dev);
390 mutex_lock(&st->lock);
391
392 ret = regmap_bulk_read(map: st->map, reg, val: st->buffer, val_count: sizeof(data));
393 memcpy(data, st->buffer, sizeof(data));
394
395 mutex_unlock(lock: &st->lock);
396 pm_runtime_mark_last_busy(dev);
397 pm_runtime_put_autosuspend(dev);
398 if (ret)
399 return ret;
400
401 /* 12 bits signed value */
402 switch (chan->channel2) {
403 case IIO_MOD_X:
404 offset = sign_extend32(value: ((data[0] & 0xF0) << 4) | data[1], index: 11);
405 break;
406 case IIO_MOD_Y:
407 offset = sign_extend32(value: ((data[1] & 0x0F) << 8) | data[0], index: 11);
408 break;
409 case IIO_MOD_Z:
410 offset = sign_extend32(value: ((data[0] & 0xF0) << 4) | data[1], index: 11);
411 break;
412 default:
413 return -EINVAL;
414 }
415
416 /*
417 * convert raw offset to g then to m/s²
418 * 12 bits signed raw step 0.5mg to g: 5 / 10000
419 * g to m/s²: 9.806650
420 * result in micro (1000000)
421 * (offset * 5 * 9.806650 * 1000000) / 10000
422 */
423 val64 = (int64_t)offset * 5LL * 9806650LL;
424 /* for rounding, add + or - divisor (10000) divided by 2 */
425 if (val64 >= 0)
426 val64 += 10000LL / 2LL;
427 else
428 val64 -= 10000LL / 2LL;
429 bias = div_s64(dividend: val64, divisor: 10000L);
430 *val = bias / 1000000L;
431 *val2 = bias % 1000000L;
432
433 return IIO_VAL_INT_PLUS_MICRO;
434}
435
436static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
437 struct iio_chan_spec const *chan,
438 int val, int val2)
439{
440 struct device *dev = regmap_get_device(map: st->map);
441 int64_t val64;
442 int32_t min, max;
443 unsigned int reg, regval;
444 int16_t offset;
445 int ret;
446
447 if (chan->type != IIO_ACCEL)
448 return -EINVAL;
449
450 switch (chan->channel2) {
451 case IIO_MOD_X:
452 reg = INV_ICM42600_REG_OFFSET_USER4;
453 break;
454 case IIO_MOD_Y:
455 reg = INV_ICM42600_REG_OFFSET_USER6;
456 break;
457 case IIO_MOD_Z:
458 reg = INV_ICM42600_REG_OFFSET_USER7;
459 break;
460 default:
461 return -EINVAL;
462 }
463
464 /* inv_icm42600_accel_calibbias: min - step - max in micro */
465 min = inv_icm42600_accel_calibbias[0] * 1000000L +
466 inv_icm42600_accel_calibbias[1];
467 max = inv_icm42600_accel_calibbias[4] * 1000000L +
468 inv_icm42600_accel_calibbias[5];
469 val64 = (int64_t)val * 1000000LL + (int64_t)val2;
470 if (val64 < min || val64 > max)
471 return -EINVAL;
472
473 /*
474 * convert m/s² to g then to raw value
475 * m/s² to g: 1 / 9.806650
476 * g to raw 12 bits signed, step 0.5mg: 10000 / 5
477 * val in micro (1000000)
478 * val * 10000 / (9.806650 * 1000000 * 5)
479 */
480 val64 = val64 * 10000LL;
481 /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */
482 if (val64 >= 0)
483 val64 += 9806650 * 5 / 2;
484 else
485 val64 -= 9806650 * 5 / 2;
486 offset = div_s64(dividend: val64, divisor: 9806650 * 5);
487
488 /* clamp value limited to 12 bits signed */
489 if (offset < -2048)
490 offset = -2048;
491 else if (offset > 2047)
492 offset = 2047;
493
494 pm_runtime_get_sync(dev);
495 mutex_lock(&st->lock);
496
497 switch (chan->channel2) {
498 case IIO_MOD_X:
499 /* OFFSET_USER4 register is shared */
500 ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER4,
501 val: &regval);
502 if (ret)
503 goto out_unlock;
504 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
505 st->buffer[1] = offset & 0xFF;
506 break;
507 case IIO_MOD_Y:
508 /* OFFSET_USER7 register is shared */
509 ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER7,
510 val: &regval);
511 if (ret)
512 goto out_unlock;
513 st->buffer[0] = offset & 0xFF;
514 st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
515 break;
516 case IIO_MOD_Z:
517 /* OFFSET_USER7 register is shared */
518 ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER7,
519 val: &regval);
520 if (ret)
521 goto out_unlock;
522 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
523 st->buffer[1] = offset & 0xFF;
524 break;
525 default:
526 ret = -EINVAL;
527 goto out_unlock;
528 }
529
530 ret = regmap_bulk_write(map: st->map, reg, val: st->buffer, val_count: 2);
531
532out_unlock:
533 mutex_unlock(lock: &st->lock);
534 pm_runtime_mark_last_busy(dev);
535 pm_runtime_put_autosuspend(dev);
536 return ret;
537}
538
539static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
540 struct iio_chan_spec const *chan,
541 int *val, int *val2, long mask)
542{
543 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
544 int16_t data;
545 int ret;
546
547 switch (chan->type) {
548 case IIO_ACCEL:
549 break;
550 case IIO_TEMP:
551 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
552 default:
553 return -EINVAL;
554 }
555
556 switch (mask) {
557 case IIO_CHAN_INFO_RAW:
558 ret = iio_device_claim_direct_mode(indio_dev);
559 if (ret)
560 return ret;
561 ret = inv_icm42600_accel_read_sensor(st, chan, val: &data);
562 iio_device_release_direct_mode(indio_dev);
563 if (ret)
564 return ret;
565 *val = data;
566 return IIO_VAL_INT;
567 case IIO_CHAN_INFO_SCALE:
568 return inv_icm42600_accel_read_scale(st, val, val2);
569 case IIO_CHAN_INFO_SAMP_FREQ:
570 return inv_icm42600_accel_read_odr(st, val, val2);
571 case IIO_CHAN_INFO_CALIBBIAS:
572 return inv_icm42600_accel_read_offset(st, chan, val, val2);
573 default:
574 return -EINVAL;
575 }
576}
577
578static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
579 struct iio_chan_spec const *chan,
580 const int **vals,
581 int *type, int *length, long mask)
582{
583 if (chan->type != IIO_ACCEL)
584 return -EINVAL;
585
586 switch (mask) {
587 case IIO_CHAN_INFO_SCALE:
588 *vals = inv_icm42600_accel_scale;
589 *type = IIO_VAL_INT_PLUS_NANO;
590 *length = ARRAY_SIZE(inv_icm42600_accel_scale);
591 return IIO_AVAIL_LIST;
592 case IIO_CHAN_INFO_SAMP_FREQ:
593 *vals = inv_icm42600_accel_odr;
594 *type = IIO_VAL_INT_PLUS_MICRO;
595 *length = ARRAY_SIZE(inv_icm42600_accel_odr);
596 return IIO_AVAIL_LIST;
597 case IIO_CHAN_INFO_CALIBBIAS:
598 *vals = inv_icm42600_accel_calibbias;
599 *type = IIO_VAL_INT_PLUS_MICRO;
600 return IIO_AVAIL_RANGE;
601 default:
602 return -EINVAL;
603 }
604}
605
606static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
607 struct iio_chan_spec const *chan,
608 int val, int val2, long mask)
609{
610 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
611 int ret;
612
613 if (chan->type != IIO_ACCEL)
614 return -EINVAL;
615
616 switch (mask) {
617 case IIO_CHAN_INFO_SCALE:
618 ret = iio_device_claim_direct_mode(indio_dev);
619 if (ret)
620 return ret;
621 ret = inv_icm42600_accel_write_scale(st, val, val2);
622 iio_device_release_direct_mode(indio_dev);
623 return ret;
624 case IIO_CHAN_INFO_SAMP_FREQ:
625 return inv_icm42600_accel_write_odr(indio_dev, val, val2);
626 case IIO_CHAN_INFO_CALIBBIAS:
627 ret = iio_device_claim_direct_mode(indio_dev);
628 if (ret)
629 return ret;
630 ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
631 iio_device_release_direct_mode(indio_dev);
632 return ret;
633 default:
634 return -EINVAL;
635 }
636}
637
638static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
639 struct iio_chan_spec const *chan,
640 long mask)
641{
642 if (chan->type != IIO_ACCEL)
643 return -EINVAL;
644
645 switch (mask) {
646 case IIO_CHAN_INFO_SCALE:
647 return IIO_VAL_INT_PLUS_NANO;
648 case IIO_CHAN_INFO_SAMP_FREQ:
649 return IIO_VAL_INT_PLUS_MICRO;
650 case IIO_CHAN_INFO_CALIBBIAS:
651 return IIO_VAL_INT_PLUS_MICRO;
652 default:
653 return -EINVAL;
654 }
655}
656
657static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
658 unsigned int val)
659{
660 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
661 int ret;
662
663 mutex_lock(&st->lock);
664
665 st->fifo.watermark.accel = val;
666 ret = inv_icm42600_buffer_update_watermark(st);
667
668 mutex_unlock(lock: &st->lock);
669
670 return ret;
671}
672
673static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
674 unsigned int count)
675{
676 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
677 int ret;
678
679 if (count == 0)
680 return 0;
681
682 mutex_lock(&st->lock);
683
684 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
685 if (!ret)
686 ret = st->fifo.nb.accel;
687
688 mutex_unlock(lock: &st->lock);
689
690 return ret;
691}
692
693static const struct iio_info inv_icm42600_accel_info = {
694 .read_raw = inv_icm42600_accel_read_raw,
695 .read_avail = inv_icm42600_accel_read_avail,
696 .write_raw = inv_icm42600_accel_write_raw,
697 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
698 .debugfs_reg_access = inv_icm42600_debugfs_reg,
699 .update_scan_mode = inv_icm42600_accel_update_scan_mode,
700 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
701 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
702};
703
704struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
705{
706 struct device *dev = regmap_get_device(map: st->map);
707 const char *name;
708 struct inv_sensors_timestamp_chip ts_chip;
709 struct inv_sensors_timestamp *ts;
710 struct iio_dev *indio_dev;
711 int ret;
712
713 name = devm_kasprintf(dev, GFP_KERNEL, fmt: "%s-accel", st->name);
714 if (!name)
715 return ERR_PTR(error: -ENOMEM);
716
717 indio_dev = devm_iio_device_alloc(parent: dev, sizeof_priv: sizeof(*ts));
718 if (!indio_dev)
719 return ERR_PTR(error: -ENOMEM);
720
721 /*
722 * clock period is 32kHz (31250ns)
723 * jitter is +/- 2% (20 per mille)
724 */
725 ts_chip.clock_period = 31250;
726 ts_chip.jitter = 20;
727 ts_chip.init_period = inv_icm42600_odr_to_period(odr: st->conf.accel.odr);
728 ts = iio_priv(indio_dev);
729 inv_sensors_timestamp_init(ts, chip: &ts_chip);
730
731 iio_device_set_drvdata(indio_dev, data: st);
732 indio_dev->name = name;
733 indio_dev->info = &inv_icm42600_accel_info;
734 indio_dev->modes = INDIO_DIRECT_MODE;
735 indio_dev->channels = inv_icm42600_accel_channels;
736 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
737 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
738
739 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
740 &inv_icm42600_buffer_ops);
741 if (ret)
742 return ERR_PTR(error: ret);
743
744 ret = devm_iio_device_register(dev, indio_dev);
745 if (ret)
746 return ERR_PTR(error: ret);
747
748 return indio_dev;
749}
750
751int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
752{
753 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
754 struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
755 ssize_t i, size;
756 unsigned int no;
757 const void *accel, *gyro, *timestamp;
758 const int8_t *temp;
759 unsigned int odr;
760 int64_t ts_val;
761 struct inv_icm42600_accel_buffer buffer;
762
763 /* parse all fifo packets */
764 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
765 size = inv_icm42600_fifo_decode_packet(packet: &st->fifo.data[i],
766 accel: &accel, gyro: &gyro, temp: &temp, timestamp: &timestamp, odr: &odr);
767 /* quit if error or FIFO is empty */
768 if (size <= 0)
769 return size;
770
771 /* skip packet if no accel data or data is invalid */
772 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(s: accel))
773 continue;
774
775 /* update odr */
776 if (odr & INV_ICM42600_SENSOR_ACCEL)
777 inv_sensors_timestamp_apply_odr(ts, fifo_period: st->fifo.period,
778 fifo_nb: st->fifo.nb.total, fifo_no: no);
779
780 /* buffer is copied to userspace, zeroing it to avoid any data leak */
781 memset(&buffer, 0, sizeof(buffer));
782 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
783 /* convert 8 bits FIFO temperature in high resolution format */
784 buffer.temp = temp ? (*temp * 64) : 0;
785 ts_val = inv_sensors_timestamp_pop(ts);
786 iio_push_to_buffers_with_timestamp(indio_dev, data: &buffer, timestamp: ts_val);
787 }
788
789 return 0;
790}
791

source code of linux/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c