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