?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge tag 'block-5.11-2021-01-10' of git://git.kernel.dk/linux-block
[linux/fpc-iii.git] / drivers / iio / imu / inv_icm42600 / inv_icm42600_gyro.c
blobaee7b9ff4bf4f2aae9994ee18f8149b6da687000
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 #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
49 enum 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
57 static 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
62 static 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 * IIO buffer data: size must be a power of 2 and timestamp aligned
75 * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp
76 */
77 struct 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
90 static const unsigned long inv_icm42600_gyro_scan_masks[] = {
91 /* 3-axis gyro + temperature */
92 INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
93 0,
94 };
95
96 /* enable gyroscope sensor and FIFO write */
97 static 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 /* 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_GYRO_3AXIS) {
120 /* enable gyro sensor */
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 /* update data FIFO write */
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
136 out_unlock:
137 mutex_unlock(&st->lock);
138 /* sleep maximum required time */
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
148 static 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 /* enable gyro sensor */
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 /* read gyro register data */
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;
193 exit:
194 mutex_unlock(&st->lock);
195 pm_runtime_mark_last_busy(dev);
196 pm_runtime_put_autosuspend(dev);
197 return ret;
198 }
199
200 /* IIO format int + nano */
201 static const int inv_icm42600_gyro_scale[] = {
202 /* +/- 2000dps => 0.001065264 rad/s */
203 [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0,
204 [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264,
205 /* +/- 1000dps => 0.000532632 rad/s */
206 [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0,
207 [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632,
208 /* +/- 500dps => 0.000266316 rad/s */
209 [2 * INV_ICM42600_GYRO_FS_500DPS] = 0,
210 [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316,
211 /* +/- 250dps => 0.000133158 rad/s */
212 [2 * INV_ICM42600_GYRO_FS_250DPS] = 0,
213 [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158,
214 /* +/- 125dps => 0.000066579 rad/s */
215 [2 * INV_ICM42600_GYRO_FS_125DPS] = 0,
216 [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579,
217 /* +/- 62.5dps => 0.000033290 rad/s */
218 [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0,
219 [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290,
220 /* +/- 31.25dps => 0.000016645 rad/s */
221 [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0,
222 [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645,
223 /* +/- 15.625dps => 0.000008322 rad/s */
224 [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0,
225 [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322,
226 };
227
228 static 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
240 static 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 /* IIO format int + micro */
271 static const int inv_icm42600_gyro_odr[] = {
272 /* 12.5Hz */
273 12, 500000,
274 /* 25Hz */
275 25, 0,
276 /* 50Hz */
277 50, 0,
278 /* 100Hz */
279 100, 0,
280 /* 200Hz */
281 200, 0,
282 /* 1kHz */
283 1000, 0,
284 /* 2kHz */
285 2000, 0,
286 /* 4kHz */
287 4000, 0,
288 };
289
290 static 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
301 static 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
322 static 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
356 out_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 * Calibration bias values, IIO range format int + nano.
366 * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps.
367 */
368 static int inv_icm42600_gyro_calibbias[] = {
369 -1, 117010721, /* min: -1.117010721 rad/s */
370 0, 545415, /* step: 0.000545415 rad/s */
371 1, 116465306, /* max: 1.116465306 rad/s */
372 };
373
374 static 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 /* 12 bits signed value */
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 * convert raw offset to dps then to rad/s
432 * 12 bits signed raw max 64 to dps: 64 / 2048
433 * dps to rad: Pi / 180
434 * result in nano (1000000000)
435 * (offset * 64 * Pi * 1000000000) / (2048 * 180)
436 */
437 val64 = (int64_t)offset * 64LL * 3141592653LL;
438 /* for rounding, add + or - divisor (2048 * 180) divided by 2 */
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
450 static 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 /* inv_icm42600_gyro_calibbias: min - step - max in nano */
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 * convert rad/s to dps then to raw value
488 * rad to dps: 180 / Pi
489 * dps to raw 12 bits signed, max 64: 2048 / 64
490 * val in nano (1000000000)
491 * val * 180 * 2048 / (Pi * 1000000000 * 64)
492 */
493 val64 = val64 * 180LL * 2048LL;
494 /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */
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 /* clamp value limited to 12 bits signed */
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 /* OFFSET_USER1 register is shared */
513 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
514 &regval);
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 /* OFFSET_USER1 register is shared */
522 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
523 &regval);
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 /* OFFSET_USER4 register is shared */
531 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
532 &regval);
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
545 out_unlock:
546 mutex_unlock(&st->lock);
547 pm_runtime_mark_last_busy(dev);
548 pm_runtime_put_autosuspend(dev);
549 return ret;
550 }
551
552 static 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
591 static 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
619 static 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
651 static 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
670 static 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
686 static 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
706 static 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
717 struct 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 struct iio_buffer *buffer;
724 int ret;
725
726 name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
727 if (!name)
728 return ERR_PTR(-ENOMEM);
729
730 indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
731 if (!indio_dev)
732 return ERR_PTR(-ENOMEM);
733
734 buffer = devm_iio_kfifo_allocate(dev);
735 if (!buffer)
736 return ERR_PTR(-ENOMEM);
737
738 ts = iio_priv(indio_dev);
739 inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
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 | INDIO_BUFFER_SOFTWARE;
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 iio_device_attach_buffer(indio_dev, buffer);
751
752 ret = devm_iio_device_register(dev, indio_dev);
753 if (ret)
754 return ERR_PTR(ret);
755
756 return indio_dev;
757 }
758
759 int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
760 {
761 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
762 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
763 ssize_t i, size;
764 unsigned int no;
765 const void *accel, *gyro, *timestamp;
766 const int8_t *temp;
767 unsigned int odr;
768 int64_t ts_val;
769 struct inv_icm42600_gyro_buffer buffer;
770
771 /* parse all fifo packets */
772 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
773 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
774 &accel, &gyro, &temp, &timestamp, &odr);
775 /* quit if error or FIFO is empty */
776 if (size <= 0)
777 return size;
778
779 /* skip packet if no gyro data or data is invalid */
780 if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
781 continue;
782
783 /* update odr */
784 if (odr & INV_ICM42600_SENSOR_GYRO)
785 inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
786 st->fifo.nb.total, no);
787
788 /* buffer is copied to userspace, zeroing it to avoid any data leak */
789 memset(&buffer, 0, sizeof(buffer));
790 memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
791 /* convert 8 bits FIFO temperature in high resolution format */
792 buffer.temp = temp ? (*temp * 64) : 0;
793 ts_val = inv_icm42600_timestamp_pop(ts);
794 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
795 }
796
797 return 0;
798 }
Linux with added FPC-III support