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_GYRO_CHAN(_modifier, _index, _ext_info) \
24 { \
25 .type = IIO_ANGL_VEL, \
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_gyro_scan {
51 INV_ICM42600_GYRO_SCAN_X,
52 INV_ICM42600_GYRO_SCAN_Y,
53 INV_ICM42600_GYRO_SCAN_Z,
54 INV_ICM42600_GYRO_SCAN_TEMP,
55 INV_ICM42600_GYRO_SCAN_TIMESTAMP,
56 };
57
58 static const struct iio_chan_spec_ext_info inv_icm42600_gyro_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_gyro_channels[] = {
64 INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X,
65 inv_icm42600_gyro_ext_infos),
66 INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y,
67 inv_icm42600_gyro_ext_infos),
68 INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
69 inv_icm42600_gyro_ext_infos),
70 INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
71 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
72 };
73
74 /*
75 * IIO buffer data: size must be a power of 2 and timestamp aligned
76 * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp
77 */
78 struct inv_icm42600_gyro_buffer {
79 struct inv_icm42600_fifo_sensor_data gyro;
80 int16_t temp;
81 int64_t timestamp __aligned(8);
82 };
83
84 #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
85 (BIT(INV_ICM42600_GYRO_SCAN_X) | \
86 BIT(INV_ICM42600_GYRO_SCAN_Y) | \
87 BIT(INV_ICM42600_GYRO_SCAN_Z))
88
89 #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
90
91 static const unsigned long inv_icm42600_gyro_scan_masks[] = {
92 /* 3-axis gyro + temperature */
93 INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
94 0,
95 };
96
97 /* enable gyroscope sensor and FIFO write */
inv_icm42600_gyro_update_scan_mode(struct iio_dev * indio_dev,const unsigned long * scan_mask)98 static int inv_icm42600_gyro_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_gyro = 0;
106 unsigned int sleep_temp = 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, true, &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_GYRO_3AXIS) {
121 /* enable gyro sensor */
122 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
123 ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
124 if (ret)
125 goto out_unlock;
126 fifo_en |= INV_ICM42600_SENSOR_GYRO;
127 }
128
129 /* update data FIFO write */
130 inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
131 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
132
133 out_unlock:
134 mutex_unlock(&st->lock);
135 /* sleep maximum required time */
136 if (sleep_gyro > sleep_temp)
137 sleep = sleep_gyro;
138 else
139 sleep = sleep_temp;
140 if (sleep)
141 msleep(sleep);
142 return ret;
143 }
144
inv_icm42600_gyro_read_sensor(struct inv_icm42600_state * st,struct iio_chan_spec const * chan,int16_t * val)145 static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
146 struct iio_chan_spec const *chan,
147 int16_t *val)
148 {
149 struct device *dev = regmap_get_device(st->map);
150 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
151 unsigned int reg;
152 __be16 *data;
153 int ret;
154
155 if (chan->type != IIO_ANGL_VEL)
156 return -EINVAL;
157
158 switch (chan->channel2) {
159 case IIO_MOD_X:
160 reg = INV_ICM42600_REG_GYRO_DATA_X;
161 break;
162 case IIO_MOD_Y:
163 reg = INV_ICM42600_REG_GYRO_DATA_Y;
164 break;
165 case IIO_MOD_Z:
166 reg = INV_ICM42600_REG_GYRO_DATA_Z;
167 break;
168 default:
169 return -EINVAL;
170 }
171
172 pm_runtime_get_sync(dev);
173 mutex_lock(&st->lock);
174
175 /* enable gyro sensor */
176 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
177 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
178 if (ret)
179 goto exit;
180
181 /* read gyro register data */
182 data = (__be16 *)&st->buffer[0];
183 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
184 if (ret)
185 goto exit;
186
187 *val = (int16_t)be16_to_cpup(data);
188 if (*val == INV_ICM42600_DATA_INVALID)
189 ret = -EINVAL;
190 exit:
191 mutex_unlock(&st->lock);
192 pm_runtime_mark_last_busy(dev);
193 pm_runtime_put_autosuspend(dev);
194 return ret;
195 }
196
197 /* IIO format int + nano */
198 static const int inv_icm42600_gyro_scale[] = {
199 /* +/- 2000dps => 0.001065264 rad/s */
200 [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0,
201 [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264,
202 /* +/- 1000dps => 0.000532632 rad/s */
203 [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0,
204 [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632,
205 /* +/- 500dps => 0.000266316 rad/s */
206 [2 * INV_ICM42600_GYRO_FS_500DPS] = 0,
207 [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316,
208 /* +/- 250dps => 0.000133158 rad/s */
209 [2 * INV_ICM42600_GYRO_FS_250DPS] = 0,
210 [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158,
211 /* +/- 125dps => 0.000066579 rad/s */
212 [2 * INV_ICM42600_GYRO_FS_125DPS] = 0,
213 [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579,
214 /* +/- 62.5dps => 0.000033290 rad/s */
215 [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0,
216 [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290,
217 /* +/- 31.25dps => 0.000016645 rad/s */
218 [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0,
219 [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645,
220 /* +/- 15.625dps => 0.000008322 rad/s */
221 [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0,
222 [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322,
223 };
224
inv_icm42600_gyro_read_scale(struct inv_icm42600_state * st,int * val,int * val2)225 static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st,
226 int *val, int *val2)
227 {
228 unsigned int idx;
229
230 idx = st->conf.gyro.fs;
231
232 *val = inv_icm42600_gyro_scale[2 * idx];
233 *val2 = inv_icm42600_gyro_scale[2 * idx + 1];
234 return IIO_VAL_INT_PLUS_NANO;
235 }
236
inv_icm42600_gyro_write_scale(struct inv_icm42600_state * st,int val,int val2)237 static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st,
238 int val, int val2)
239 {
240 struct device *dev = regmap_get_device(st->map);
241 unsigned int idx;
242 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
243 int ret;
244
245 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) {
246 if (val == inv_icm42600_gyro_scale[idx] &&
247 val2 == inv_icm42600_gyro_scale[idx + 1])
248 break;
249 }
250 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale))
251 return -EINVAL;
252
253 conf.fs = idx / 2;
254
255 pm_runtime_get_sync(dev);
256 mutex_lock(&st->lock);
257
258 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
259
260 mutex_unlock(&st->lock);
261 pm_runtime_mark_last_busy(dev);
262 pm_runtime_put_autosuspend(dev);
263
264 return ret;
265 }
266
267 /* IIO format int + micro */
268 static const int inv_icm42600_gyro_odr[] = {
269 /* 12.5Hz */
270 12, 500000,
271 /* 25Hz */
272 25, 0,
273 /* 50Hz */
274 50, 0,
275 /* 100Hz */
276 100, 0,
277 /* 200Hz */
278 200, 0,
279 /* 1kHz */
280 1000, 0,
281 /* 2kHz */
282 2000, 0,
283 /* 4kHz */
284 4000, 0,
285 };
286
287 static const int inv_icm42600_gyro_odr_conv[] = {
288 INV_ICM42600_ODR_12_5HZ,
289 INV_ICM42600_ODR_25HZ,
290 INV_ICM42600_ODR_50HZ,
291 INV_ICM42600_ODR_100HZ,
292 INV_ICM42600_ODR_200HZ,
293 INV_ICM42600_ODR_1KHZ_LN,
294 INV_ICM42600_ODR_2KHZ_LN,
295 INV_ICM42600_ODR_4KHZ_LN,
296 };
297
inv_icm42600_gyro_read_odr(struct inv_icm42600_state * st,int * val,int * val2)298 static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
299 int *val, int *val2)
300 {
301 unsigned int odr;
302 unsigned int i;
303
304 odr = st->conf.gyro.odr;
305
306 for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) {
307 if (inv_icm42600_gyro_odr_conv[i] == odr)
308 break;
309 }
310 if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv))
311 return -EINVAL;
312
313 *val = inv_icm42600_gyro_odr[2 * i];
314 *val2 = inv_icm42600_gyro_odr[2 * i + 1];
315
316 return IIO_VAL_INT_PLUS_MICRO;
317 }
318
inv_icm42600_gyro_write_odr(struct iio_dev * indio_dev,int val,int val2)319 static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
320 int val, int val2)
321 {
322 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
323 struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
324 struct device *dev = regmap_get_device(st->map);
325 unsigned int idx;
326 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
327 int ret;
328
329 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) {
330 if (val == inv_icm42600_gyro_odr[idx] &&
331 val2 == inv_icm42600_gyro_odr[idx + 1])
332 break;
333 }
334 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr))
335 return -EINVAL;
336
337 conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
338
339 pm_runtime_get_sync(dev);
340 mutex_lock(&st->lock);
341
342 ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
343 iio_buffer_enabled(indio_dev));
344 if (ret)
345 goto out_unlock;
346
347 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
348 if (ret)
349 goto out_unlock;
350 inv_icm42600_buffer_update_fifo_period(st);
351 inv_icm42600_buffer_update_watermark(st);
352
353 out_unlock:
354 mutex_unlock(&st->lock);
355 pm_runtime_mark_last_busy(dev);
356 pm_runtime_put_autosuspend(dev);
357
358 return ret;
359 }
360
361 /*
362 * Calibration bias values, IIO range format int + nano.
363 * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps.
364 */
365 static int inv_icm42600_gyro_calibbias[] = {
366 -1, 117010721, /* min: -1.117010721 rad/s */
367 0, 545415, /* step: 0.000545415 rad/s */
368 1, 116465306, /* max: 1.116465306 rad/s */
369 };
370
inv_icm42600_gyro_read_offset(struct inv_icm42600_state * st,struct iio_chan_spec const * chan,int * val,int * val2)371 static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
372 struct iio_chan_spec const *chan,
373 int *val, int *val2)
374 {
375 struct device *dev = regmap_get_device(st->map);
376 int64_t val64;
377 int32_t bias;
378 unsigned int reg;
379 int16_t offset;
380 uint8_t data[2];
381 int ret;
382
383 if (chan->type != IIO_ANGL_VEL)
384 return -EINVAL;
385
386 switch (chan->channel2) {
387 case IIO_MOD_X:
388 reg = INV_ICM42600_REG_OFFSET_USER0;
389 break;
390 case IIO_MOD_Y:
391 reg = INV_ICM42600_REG_OFFSET_USER1;
392 break;
393 case IIO_MOD_Z:
394 reg = INV_ICM42600_REG_OFFSET_USER3;
395 break;
396 default:
397 return -EINVAL;
398 }
399
400 pm_runtime_get_sync(dev);
401 mutex_lock(&st->lock);
402
403 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
404 memcpy(data, st->buffer, sizeof(data));
405
406 mutex_unlock(&st->lock);
407 pm_runtime_mark_last_busy(dev);
408 pm_runtime_put_autosuspend(dev);
409 if (ret)
410 return ret;
411
412 /* 12 bits signed value */
413 switch (chan->channel2) {
414 case IIO_MOD_X:
415 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
416 break;
417 case IIO_MOD_Y:
418 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
419 break;
420 case IIO_MOD_Z:
421 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
422 break;
423 default:
424 return -EINVAL;
425 }
426
427 /*
428 * convert raw offset to dps then to rad/s
429 * 12 bits signed raw max 64 to dps: 64 / 2048
430 * dps to rad: Pi / 180
431 * result in nano (1000000000)
432 * (offset * 64 * Pi * 1000000000) / (2048 * 180)
433 */
434 val64 = (int64_t)offset * 64LL * 3141592653LL;
435 /* for rounding, add + or - divisor (2048 * 180) divided by 2 */
436 if (val64 >= 0)
437 val64 += 2048 * 180 / 2;
438 else
439 val64 -= 2048 * 180 / 2;
440 bias = div_s64(val64, 2048 * 180);
441 *val = bias / 1000000000L;
442 *val2 = bias % 1000000000L;
443
444 return IIO_VAL_INT_PLUS_NANO;
445 }
446
inv_icm42600_gyro_write_offset(struct inv_icm42600_state * st,struct iio_chan_spec const * chan,int val,int val2)447 static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
448 struct iio_chan_spec const *chan,
449 int val, int val2)
450 {
451 struct device *dev = regmap_get_device(st->map);
452 int64_t val64, min, max;
453 unsigned int reg, regval;
454 int16_t offset;
455 int ret;
456
457 if (chan->type != IIO_ANGL_VEL)
458 return -EINVAL;
459
460 switch (chan->channel2) {
461 case IIO_MOD_X:
462 reg = INV_ICM42600_REG_OFFSET_USER0;
463 break;
464 case IIO_MOD_Y:
465 reg = INV_ICM42600_REG_OFFSET_USER1;
466 break;
467 case IIO_MOD_Z:
468 reg = INV_ICM42600_REG_OFFSET_USER3;
469 break;
470 default:
471 return -EINVAL;
472 }
473
474 /* inv_icm42600_gyro_calibbias: min - step - max in nano */
475 min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL +
476 (int64_t)inv_icm42600_gyro_calibbias[1];
477 max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL +
478 (int64_t)inv_icm42600_gyro_calibbias[5];
479 val64 = (int64_t)val * 1000000000LL + (int64_t)val2;
480 if (val64 < min || val64 > max)
481 return -EINVAL;
482
483 /*
484 * convert rad/s to dps then to raw value
485 * rad to dps: 180 / Pi
486 * dps to raw 12 bits signed, max 64: 2048 / 64
487 * val in nano (1000000000)
488 * val * 180 * 2048 / (Pi * 1000000000 * 64)
489 */
490 val64 = val64 * 180LL * 2048LL;
491 /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */
492 if (val64 >= 0)
493 val64 += 3141592653LL * 64LL / 2LL;
494 else
495 val64 -= 3141592653LL * 64LL / 2LL;
496 offset = div64_s64(val64, 3141592653LL * 64LL);
497
498 /* clamp value limited to 12 bits signed */
499 if (offset < -2048)
500 offset = -2048;
501 else if (offset > 2047)
502 offset = 2047;
503
504 pm_runtime_get_sync(dev);
505 mutex_lock(&st->lock);
506
507 switch (chan->channel2) {
508 case IIO_MOD_X:
509 /* OFFSET_USER1 register is shared */
510 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
511 ®val);
512 if (ret)
513 goto out_unlock;
514 st->buffer[0] = offset & 0xFF;
515 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
516 break;
517 case IIO_MOD_Y:
518 /* OFFSET_USER1 register is shared */
519 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
520 ®val);
521 if (ret)
522 goto out_unlock;
523 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
524 st->buffer[1] = offset & 0xFF;
525 break;
526 case IIO_MOD_Z:
527 /* OFFSET_USER4 register is shared */
528 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
529 ®val);
530 if (ret)
531 goto out_unlock;
532 st->buffer[0] = offset & 0xFF;
533 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
534 break;
535 default:
536 ret = -EINVAL;
537 goto out_unlock;
538 }
539
540 ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
541
542 out_unlock:
543 mutex_unlock(&st->lock);
544 pm_runtime_mark_last_busy(dev);
545 pm_runtime_put_autosuspend(dev);
546 return ret;
547 }
548
inv_icm42600_gyro_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)549 static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
550 struct iio_chan_spec const *chan,
551 int *val, int *val2, long mask)
552 {
553 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
554 int16_t data;
555 int ret;
556
557 switch (chan->type) {
558 case IIO_ANGL_VEL:
559 break;
560 case IIO_TEMP:
561 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
562 default:
563 return -EINVAL;
564 }
565
566 switch (mask) {
567 case IIO_CHAN_INFO_RAW:
568 ret = iio_device_claim_direct_mode(indio_dev);
569 if (ret)
570 return ret;
571 ret = inv_icm42600_gyro_read_sensor(st, chan, &data);
572 iio_device_release_direct_mode(indio_dev);
573 if (ret)
574 return ret;
575 *val = data;
576 return IIO_VAL_INT;
577 case IIO_CHAN_INFO_SCALE:
578 return inv_icm42600_gyro_read_scale(st, val, val2);
579 case IIO_CHAN_INFO_SAMP_FREQ:
580 return inv_icm42600_gyro_read_odr(st, val, val2);
581 case IIO_CHAN_INFO_CALIBBIAS:
582 return inv_icm42600_gyro_read_offset(st, chan, val, val2);
583 default:
584 return -EINVAL;
585 }
586 }
587
inv_icm42600_gyro_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)588 static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev,
589 struct iio_chan_spec const *chan,
590 const int **vals,
591 int *type, int *length, long mask)
592 {
593 if (chan->type != IIO_ANGL_VEL)
594 return -EINVAL;
595
596 switch (mask) {
597 case IIO_CHAN_INFO_SCALE:
598 *vals = inv_icm42600_gyro_scale;
599 *type = IIO_VAL_INT_PLUS_NANO;
600 *length = ARRAY_SIZE(inv_icm42600_gyro_scale);
601 return IIO_AVAIL_LIST;
602 case IIO_CHAN_INFO_SAMP_FREQ:
603 *vals = inv_icm42600_gyro_odr;
604 *type = IIO_VAL_INT_PLUS_MICRO;
605 *length = ARRAY_SIZE(inv_icm42600_gyro_odr);
606 return IIO_AVAIL_LIST;
607 case IIO_CHAN_INFO_CALIBBIAS:
608 *vals = inv_icm42600_gyro_calibbias;
609 *type = IIO_VAL_INT_PLUS_NANO;
610 return IIO_AVAIL_RANGE;
611 default:
612 return -EINVAL;
613 }
614 }
615
inv_icm42600_gyro_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)616 static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
617 struct iio_chan_spec const *chan,
618 int val, int val2, long mask)
619 {
620 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
621 int ret;
622
623 if (chan->type != IIO_ANGL_VEL)
624 return -EINVAL;
625
626 switch (mask) {
627 case IIO_CHAN_INFO_SCALE:
628 ret = iio_device_claim_direct_mode(indio_dev);
629 if (ret)
630 return ret;
631 ret = inv_icm42600_gyro_write_scale(st, val, val2);
632 iio_device_release_direct_mode(indio_dev);
633 return ret;
634 case IIO_CHAN_INFO_SAMP_FREQ:
635 return inv_icm42600_gyro_write_odr(indio_dev, val, val2);
636 case IIO_CHAN_INFO_CALIBBIAS:
637 ret = iio_device_claim_direct_mode(indio_dev);
638 if (ret)
639 return ret;
640 ret = inv_icm42600_gyro_write_offset(st, chan, val, val2);
641 iio_device_release_direct_mode(indio_dev);
642 return ret;
643 default:
644 return -EINVAL;
645 }
646 }
647
inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,long mask)648 static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
649 struct iio_chan_spec const *chan,
650 long mask)
651 {
652 if (chan->type != IIO_ANGL_VEL)
653 return -EINVAL;
654
655 switch (mask) {
656 case IIO_CHAN_INFO_SCALE:
657 return IIO_VAL_INT_PLUS_NANO;
658 case IIO_CHAN_INFO_SAMP_FREQ:
659 return IIO_VAL_INT_PLUS_MICRO;
660 case IIO_CHAN_INFO_CALIBBIAS:
661 return IIO_VAL_INT_PLUS_NANO;
662 default:
663 return -EINVAL;
664 }
665 }
666
inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev * indio_dev,unsigned int val)667 static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
668 unsigned int val)
669 {
670 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
671 int ret;
672
673 mutex_lock(&st->lock);
674
675 st->fifo.watermark.gyro = val;
676 ret = inv_icm42600_buffer_update_watermark(st);
677
678 mutex_unlock(&st->lock);
679
680 return ret;
681 }
682
inv_icm42600_gyro_hwfifo_flush(struct iio_dev * indio_dev,unsigned int count)683 static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
684 unsigned int count)
685 {
686 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
687 int ret;
688
689 if (count == 0)
690 return 0;
691
692 mutex_lock(&st->lock);
693
694 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
695 if (!ret)
696 ret = st->fifo.nb.gyro;
697
698 mutex_unlock(&st->lock);
699
700 return ret;
701 }
702
703 static const struct iio_info inv_icm42600_gyro_info = {
704 .read_raw = inv_icm42600_gyro_read_raw,
705 .read_avail = inv_icm42600_gyro_read_avail,
706 .write_raw = inv_icm42600_gyro_write_raw,
707 .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
708 .debugfs_reg_access = inv_icm42600_debugfs_reg,
709 .update_scan_mode = inv_icm42600_gyro_update_scan_mode,
710 .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
711 .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
712 };
713
inv_icm42600_gyro_init(struct inv_icm42600_state * st)714 struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
715 {
716 struct device *dev = regmap_get_device(st->map);
717 const char *name;
718 struct inv_sensors_timestamp_chip ts_chip;
719 struct inv_sensors_timestamp *ts;
720 struct iio_dev *indio_dev;
721 int ret;
722
723 name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
724 if (!name)
725 return ERR_PTR(-ENOMEM);
726
727 indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
728 if (!indio_dev)
729 return ERR_PTR(-ENOMEM);
730
731 /*
732 * clock period is 32kHz (31250ns)
733 * jitter is +/- 2% (20 per mille)
734 */
735 ts_chip.clock_period = 31250;
736 ts_chip.jitter = 20;
737 ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr);
738 ts = iio_priv(indio_dev);
739 inv_sensors_timestamp_init(ts, &ts_chip);
740
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;
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;
749
750 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
751 &inv_icm42600_buffer_ops);
752 if (ret)
753 return ERR_PTR(ret);
754
755 ret = devm_iio_device_register(dev, indio_dev);
756 if (ret)
757 return ERR_PTR(ret);
758
759 return indio_dev;
760 }
761
inv_icm42600_gyro_parse_fifo(struct iio_dev * indio_dev)762 int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
763 {
764 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
765 struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
766 ssize_t i, size;
767 unsigned int no;
768 const void *accel, *gyro, *timestamp;
769 const int8_t *temp;
770 unsigned int odr;
771 int64_t ts_val;
772 struct inv_icm42600_gyro_buffer buffer;
773
774 /* parse all fifo packets */
775 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
776 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
777 &accel, &gyro, &temp, ×tamp, &odr);
778 /* quit if error or FIFO is empty */
779 if (size <= 0)
780 return size;
781
782 /* skip packet if no gyro data or data is invalid */
783 if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
784 continue;
785
786 /* update odr */
787 if (odr & INV_ICM42600_SENSOR_GYRO)
788 inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
789 st->fifo.nb.total, no);
790
791 /* buffer is copied to userspace, zeroing it to avoid any data leak */
792 memset(&buffer, 0, sizeof(buffer));
793 memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
794 /* convert 8 bits FIFO temperature in high resolution format */
795 buffer.temp = temp ? (*temp * 64) : 0;
796 ts_val = inv_sensors_timestamp_pop(ts);
797 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
798 }
799
800 return 0;
801 }
802