1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * Copyright (C) 2025 Invensense, Inc.
4 */
5
6 #include <linux/delay.h>
7 #include <linux/device.h>
8 #include <linux/err.h>
9 #include <linux/math64.h>
10 #include <linux/mutex.h>
11 #include <linux/pm_runtime.h>
12 #include <linux/regmap.h>
13 #include <linux/types.h>
14
15 #include <linux/iio/buffer.h>
16 #include <linux/iio/common/inv_sensors_timestamp.h>
17 #include <linux/iio/iio.h>
18 #include <linux/iio/kfifo_buf.h>
19
20 #include "inv_icm45600_buffer.h"
21 #include "inv_icm45600.h"
22
23 enum inv_icm45600_gyro_scan {
24 INV_ICM45600_GYRO_SCAN_X,
25 INV_ICM45600_GYRO_SCAN_Y,
26 INV_ICM45600_GYRO_SCAN_Z,
27 INV_ICM45600_GYRO_SCAN_TEMP,
28 INV_ICM45600_GYRO_SCAN_TIMESTAMP,
29 };
30
31 static const struct iio_chan_spec_ext_info inv_icm45600_gyro_ext_infos[] = {
32 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm45600_get_mount_matrix),
33 { }
34 };
35
36 #define INV_ICM45600_GYRO_CHAN(_modifier, _index, _ext_info) \
37 { \
38 .type = IIO_ANGL_VEL, \
39 .modified = 1, \
40 .channel2 = _modifier, \
41 .info_mask_separate = \
42 BIT(IIO_CHAN_INFO_RAW) | \
43 BIT(IIO_CHAN_INFO_CALIBBIAS), \
44 .info_mask_shared_by_type = \
45 BIT(IIO_CHAN_INFO_SCALE), \
46 .info_mask_shared_by_type_available = \
47 BIT(IIO_CHAN_INFO_SCALE) | \
48 BIT(IIO_CHAN_INFO_CALIBBIAS), \
49 .info_mask_shared_by_all = \
50 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
51 .info_mask_shared_by_all_available = \
52 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
53 .scan_index = _index, \
54 .scan_type = { \
55 .sign = 's', \
56 .realbits = 16, \
57 .storagebits = 16, \
58 .endianness = IIO_LE, \
59 }, \
60 .ext_info = _ext_info, \
61 }
62
63 static const struct iio_chan_spec inv_icm45600_gyro_channels[] = {
64 INV_ICM45600_GYRO_CHAN(IIO_MOD_X, INV_ICM45600_GYRO_SCAN_X,
65 inv_icm45600_gyro_ext_infos),
66 INV_ICM45600_GYRO_CHAN(IIO_MOD_Y, INV_ICM45600_GYRO_SCAN_Y,
67 inv_icm45600_gyro_ext_infos),
68 INV_ICM45600_GYRO_CHAN(IIO_MOD_Z, INV_ICM45600_GYRO_SCAN_Z,
69 inv_icm45600_gyro_ext_infos),
70 INV_ICM45600_TEMP_CHAN(INV_ICM45600_GYRO_SCAN_TEMP),
71 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM45600_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_icm45600_gyro_buffer {
79 struct inv_icm45600_fifo_sensor_data gyro;
80 s16 temp;
81 aligned_s64 timestamp;
82 };
83
84 static const unsigned long inv_icm45600_gyro_scan_masks[] = {
85 /* 3-axis gyro + temperature */
86 BIT(INV_ICM45600_GYRO_SCAN_X) |
87 BIT(INV_ICM45600_GYRO_SCAN_Y) |
88 BIT(INV_ICM45600_GYRO_SCAN_Z) |
89 BIT(INV_ICM45600_GYRO_SCAN_TEMP),
90 0
91 };
92
93 /* enable gyroscope sensor and FIFO write */
inv_icm45600_gyro_update_scan_mode(struct iio_dev * indio_dev,const unsigned long * scan_mask)94 static int inv_icm45600_gyro_update_scan_mode(struct iio_dev *indio_dev,
95 const unsigned long *scan_mask)
96 {
97 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
98 struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev);
99 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
100 unsigned int fifo_en = 0;
101 unsigned int sleep = 0;
102 int ret;
103
104 scoped_guard(mutex, &st->lock) {
105 if (*scan_mask & BIT(INV_ICM45600_GYRO_SCAN_TEMP))
106 fifo_en |= INV_ICM45600_SENSOR_TEMP;
107
108 if (*scan_mask & (BIT(INV_ICM45600_GYRO_SCAN_X) |
109 BIT(INV_ICM45600_GYRO_SCAN_Y) |
110 BIT(INV_ICM45600_GYRO_SCAN_Z))) {
111 /* enable gyro sensor */
112 conf.mode = gyro_st->power_mode;
113 ret = inv_icm45600_set_gyro_conf(st, &conf, &sleep);
114 if (ret)
115 return ret;
116 fifo_en |= INV_ICM45600_SENSOR_GYRO;
117 }
118 ret = inv_icm45600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
119 }
120 if (sleep)
121 msleep(sleep);
122
123 return ret;
124 }
125
_inv_icm45600_gyro_read_sensor(struct inv_icm45600_state * st,struct inv_icm45600_sensor_state * gyro_st,unsigned int reg,int * val)126 static int _inv_icm45600_gyro_read_sensor(struct inv_icm45600_state *st,
127 struct inv_icm45600_sensor_state *gyro_st,
128 unsigned int reg, int *val)
129 {
130 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
131 int ret;
132
133 /* enable gyro sensor */
134 conf.mode = gyro_st->power_mode;
135 ret = inv_icm45600_set_gyro_conf(st, &conf, NULL);
136 if (ret)
137 return ret;
138
139 /* read gyro register data */
140 ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16));
141 if (ret)
142 return ret;
143
144 *val = sign_extend32(le16_to_cpup(&st->buffer.u16), 15);
145 if (*val == INV_ICM45600_DATA_INVALID)
146 return -ENODATA;
147
148 return 0;
149 }
150
inv_icm45600_gyro_read_sensor(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val)151 static int inv_icm45600_gyro_read_sensor(struct iio_dev *indio_dev,
152 struct iio_chan_spec const *chan,
153 int *val)
154 {
155 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
156 struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev);
157 struct device *dev = regmap_get_device(st->map);
158 unsigned int reg;
159 int ret;
160
161 if (chan->type != IIO_ANGL_VEL)
162 return -EINVAL;
163
164 switch (chan->channel2) {
165 case IIO_MOD_X:
166 reg = INV_ICM45600_REG_GYRO_DATA_X;
167 break;
168 case IIO_MOD_Y:
169 reg = INV_ICM45600_REG_GYRO_DATA_Y;
170 break;
171 case IIO_MOD_Z:
172 reg = INV_ICM45600_REG_GYRO_DATA_Z;
173 break;
174 default:
175 return -EINVAL;
176 }
177
178 ret = pm_runtime_resume_and_get(dev);
179 if (ret)
180 return ret;
181
182 scoped_guard(mutex, &st->lock)
183 ret = _inv_icm45600_gyro_read_sensor(st, gyro_st, reg, val);
184
185 pm_runtime_put_autosuspend(dev);
186
187 return ret;
188 }
189
190 /* IIO format int + nano */
191 const int inv_icm45600_gyro_scale[][2] = {
192 /* +/- 2000dps => 0.001065264 rad/s */
193 [INV_ICM45600_GYRO_FS_2000DPS] = { 0, 1065264 },
194 /* +/- 1000dps => 0.000532632 rad/s */
195 [INV_ICM45600_GYRO_FS_1000DPS] = { 0, 532632 },
196 /* +/- 500dps => 0.000266316 rad/s */
197 [INV_ICM45600_GYRO_FS_500DPS] = { 0, 266316 },
198 /* +/- 250dps => 0.000133158 rad/s */
199 [INV_ICM45600_GYRO_FS_250DPS] = { 0, 133158 },
200 /* +/- 125dps => 0.000066579 rad/s */
201 [INV_ICM45600_GYRO_FS_125DPS] = { 0, 66579 },
202 /* +/- 62.5dps => 0.000033290 rad/s */
203 [INV_ICM45600_GYRO_FS_62_5DPS] = { 0, 33290 },
204 /* +/- 31.25dps => 0.000016645 rad/s */
205 [INV_ICM45600_GYRO_FS_31_25DPS] = { 0, 16645 },
206 /* +/- 15.625dps => 0.000008322 rad/s */
207 [INV_ICM45600_GYRO_FS_15_625DPS] = { 0, 8322 },
208 };
209
210 /* IIO format int + nano */
211 const int inv_icm45686_gyro_scale[][2] = {
212 /* +/- 4000dps => 0.002130529 rad/s */
213 [INV_ICM45686_GYRO_FS_4000DPS] = { 0, 2130529 },
214 /* +/- 2000dps => 0.001065264 rad/s */
215 [INV_ICM45686_GYRO_FS_2000DPS] = { 0, 1065264 },
216 /* +/- 1000dps => 0.000532632 rad/s */
217 [INV_ICM45686_GYRO_FS_1000DPS] = { 0, 532632 },
218 /* +/- 500dps => 0.000266316 rad/s */
219 [INV_ICM45686_GYRO_FS_500DPS] = { 0, 266316 },
220 /* +/- 250dps => 0.000133158 rad/s */
221 [INV_ICM45686_GYRO_FS_250DPS] = { 0, 133158 },
222 /* +/- 125dps => 0.000066579 rad/s */
223 [INV_ICM45686_GYRO_FS_125DPS] = { 0, 66579 },
224 /* +/- 62.5dps => 0.000033290 rad/s */
225 [INV_ICM45686_GYRO_FS_62_5DPS] = { 0, 33290 },
226 /* +/- 31.25dps => 0.000016645 rad/s */
227 [INV_ICM45686_GYRO_FS_31_25DPS] = { 0, 16645 },
228 /* +/- 15.625dps => 0.000008322 rad/s */
229 [INV_ICM45686_GYRO_FS_15_625DPS] = { 0, 8322 },
230 };
231
inv_icm45600_gyro_read_scale(struct iio_dev * indio_dev,int * val,int * val2)232 static int inv_icm45600_gyro_read_scale(struct iio_dev *indio_dev,
233 int *val, int *val2)
234 {
235 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
236 struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev);
237 unsigned int idx;
238
239 idx = st->conf.gyro.fs;
240
241 /* Full scale register starts at 1 for not High FSR parts */
242 if (gyro_st->scales == (const int *)&inv_icm45600_gyro_scale)
243 idx--;
244
245 *val = gyro_st->scales[2 * idx];
246 *val2 = gyro_st->scales[2 * idx + 1];
247 return IIO_VAL_INT_PLUS_NANO;
248 }
249
inv_icm45600_gyro_write_scale(struct iio_dev * indio_dev,int val,int val2)250 static int inv_icm45600_gyro_write_scale(struct iio_dev *indio_dev,
251 int val, int val2)
252 {
253 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
254 struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev);
255 struct device *dev = regmap_get_device(st->map);
256 unsigned int idx;
257 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
258 int ret;
259
260 for (idx = 0; idx < gyro_st->scales_len; idx += 2) {
261 if (val == gyro_st->scales[idx] &&
262 val2 == gyro_st->scales[idx + 1])
263 break;
264 }
265 if (idx == gyro_st->scales_len)
266 return -EINVAL;
267
268 conf.fs = idx / 2;
269
270 /* Full scale register starts at 1 for not High FSR parts */
271 if (gyro_st->scales == (const int *)&inv_icm45600_gyro_scale)
272 conf.fs++;
273
274 ret = pm_runtime_resume_and_get(dev);
275 if (ret)
276 return ret;
277
278 scoped_guard(mutex, &st->lock)
279 ret = inv_icm45600_set_gyro_conf(st, &conf, NULL);
280
281 pm_runtime_put_autosuspend(dev);
282
283 return ret;
284 }
285
286 /* IIO format int + micro */
287 static const int inv_icm45600_gyro_odr[] = {
288 1, 562500, /* 1.5625Hz */
289 3, 125000, /* 3.125Hz */
290 6, 250000, /* 6.25Hz */
291 12, 500000, /* 12.5Hz */
292 25, 0, /* 25Hz */
293 50, 0, /* 50Hz */
294 100, 0, /* 100Hz */
295 200, 0, /* 200Hz */
296 400, 0, /* 400Hz */
297 800, 0, /* 800Hz */
298 1600, 0, /* 1.6kHz */
299 3200, 0, /* 3.2kHz */
300 6400, 0, /* 6.4kHz */
301 };
302
303 static const int inv_icm45600_gyro_odr_conv[] = {
304 INV_ICM45600_ODR_1_5625HZ_LP,
305 INV_ICM45600_ODR_3_125HZ_LP,
306 INV_ICM45600_ODR_6_25HZ_LP,
307 INV_ICM45600_ODR_12_5HZ,
308 INV_ICM45600_ODR_25HZ,
309 INV_ICM45600_ODR_50HZ,
310 INV_ICM45600_ODR_100HZ,
311 INV_ICM45600_ODR_200HZ,
312 INV_ICM45600_ODR_400HZ,
313 INV_ICM45600_ODR_800HZ_LN,
314 INV_ICM45600_ODR_1600HZ_LN,
315 INV_ICM45600_ODR_3200HZ_LN,
316 INV_ICM45600_ODR_6400HZ_LN,
317 };
318
inv_icm45600_gyro_read_odr(struct inv_icm45600_state * st,int * val,int * val2)319 static int inv_icm45600_gyro_read_odr(struct inv_icm45600_state *st,
320 int *val, int *val2)
321 {
322 unsigned int odr;
323 unsigned int i;
324
325 odr = st->conf.gyro.odr;
326
327 for (i = 0; i < ARRAY_SIZE(inv_icm45600_gyro_odr_conv); ++i) {
328 if (inv_icm45600_gyro_odr_conv[i] == odr)
329 break;
330 }
331 if (i >= ARRAY_SIZE(inv_icm45600_gyro_odr_conv))
332 return -EINVAL;
333
334 *val = inv_icm45600_gyro_odr[2 * i];
335 *val2 = inv_icm45600_gyro_odr[2 * i + 1];
336
337 return IIO_VAL_INT_PLUS_MICRO;
338 }
339
_inv_icm45600_gyro_write_odr(struct iio_dev * indio_dev,int odr)340 static int _inv_icm45600_gyro_write_odr(struct iio_dev *indio_dev, int odr)
341 {
342 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
343 struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev);
344 struct inv_sensors_timestamp *ts = &gyro_st->ts;
345 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
346 int ret;
347
348 conf.odr = odr;
349 ret = inv_sensors_timestamp_update_odr(ts, inv_icm45600_odr_to_period(conf.odr),
350 iio_buffer_enabled(indio_dev));
351 if (ret)
352 return ret;
353
354 if (st->conf.gyro.mode != INV_ICM45600_SENSOR_MODE_OFF)
355 conf.mode = gyro_st->power_mode;
356
357 ret = inv_icm45600_set_gyro_conf(st, &conf, NULL);
358 if (ret)
359 return ret;
360
361 inv_icm45600_buffer_update_fifo_period(st);
362 inv_icm45600_buffer_update_watermark(st);
363
364 return 0;
365 }
366
inv_icm45600_gyro_write_odr(struct iio_dev * indio_dev,int val,int val2)367 static int inv_icm45600_gyro_write_odr(struct iio_dev *indio_dev,
368 int val, int val2)
369 {
370 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
371 struct device *dev = regmap_get_device(st->map);
372 unsigned int idx;
373 int odr;
374 int ret;
375
376 for (idx = 0; idx < ARRAY_SIZE(inv_icm45600_gyro_odr); idx += 2) {
377 if (val == inv_icm45600_gyro_odr[idx] &&
378 val2 == inv_icm45600_gyro_odr[idx + 1])
379 break;
380 }
381 if (idx >= ARRAY_SIZE(inv_icm45600_gyro_odr))
382 return -EINVAL;
383
384 odr = inv_icm45600_gyro_odr_conv[idx / 2];
385
386 ret = pm_runtime_resume_and_get(dev);
387 if (ret)
388 return ret;
389
390 scoped_guard(mutex, &st->lock)
391 ret = _inv_icm45600_gyro_write_odr(indio_dev, odr);
392
393 pm_runtime_put_autosuspend(dev);
394
395 return ret;
396 }
397
398 /*
399 * Calibration bias values, IIO range format int + nano.
400 * Value is limited to +/-62.5dps coded on 14 bits signed. Step is 7.5mdps.
401 */
402 static int inv_icm45600_gyro_calibbias[] = {
403 -1, 90830336, /* min: -1.090830336 rad/s */
404 0, 133158, /* step: 0.000133158 rad/s */
405 1, 90697178, /* max: 1.090697178 rad/s */
406 };
407
inv_icm45600_gyro_read_offset(struct inv_icm45600_state * st,struct iio_chan_spec const * chan,int * val,int * val2)408 static int inv_icm45600_gyro_read_offset(struct inv_icm45600_state *st,
409 struct iio_chan_spec const *chan,
410 int *val, int *val2)
411 {
412 struct device *dev = regmap_get_device(st->map);
413 s64 val64;
414 s32 bias;
415 unsigned int reg;
416 s16 offset;
417 int ret;
418
419 if (chan->type != IIO_ANGL_VEL)
420 return -EINVAL;
421
422 switch (chan->channel2) {
423 case IIO_MOD_X:
424 reg = INV_ICM45600_IPREG_SYS1_REG_42;
425 break;
426 case IIO_MOD_Y:
427 reg = INV_ICM45600_IPREG_SYS1_REG_56;
428 break;
429 case IIO_MOD_Z:
430 reg = INV_ICM45600_IPREG_SYS1_REG_70;
431 break;
432 default:
433 return -EINVAL;
434 }
435
436 ret = pm_runtime_resume_and_get(dev);
437 if (ret)
438 return ret;
439
440 scoped_guard(mutex, &st->lock)
441 ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16));
442
443 pm_runtime_put_autosuspend(dev);
444 if (ret)
445 return ret;
446
447 offset = le16_to_cpup(&st->buffer.u16) & INV_ICM45600_GYRO_OFFUSER_MASK;
448 /* 14 bits signed value */
449 offset = sign_extend32(offset, 13);
450
451 /*
452 * convert raw offset to dps then to rad/s
453 * 14 bits signed raw max 62.5 to dps: 625 / 81920
454 * dps to rad: Pi / 180
455 * result in nano (1000000000)
456 * (offset * 625 * Pi * 1000000000) / (81920 * 180)
457 */
458 val64 = (s64)offset * 625LL * 3141592653LL;
459 /* for rounding, add + or - divisor (81920 * 180) divided by 2 */
460 if (val64 >= 0)
461 val64 += 81920 * 180 / 2;
462 else
463 val64 -= 81920 * 180 / 2;
464 bias = div_s64(val64, 81920 * 180);
465 *val = bias / 1000000000L;
466 *val2 = bias % 1000000000L;
467
468 return IIO_VAL_INT_PLUS_NANO;
469 }
470
inv_icm45600_gyro_write_offset(struct inv_icm45600_state * st,struct iio_chan_spec const * chan,int val,int val2)471 static int inv_icm45600_gyro_write_offset(struct inv_icm45600_state *st,
472 struct iio_chan_spec const *chan,
473 int val, int val2)
474 {
475 struct device *dev = regmap_get_device(st->map);
476 s64 val64, min, max;
477 unsigned int reg;
478 s16 offset;
479 int ret;
480
481 if (chan->type != IIO_ANGL_VEL)
482 return -EINVAL;
483
484 switch (chan->channel2) {
485 case IIO_MOD_X:
486 reg = INV_ICM45600_IPREG_SYS1_REG_42;
487 break;
488 case IIO_MOD_Y:
489 reg = INV_ICM45600_IPREG_SYS1_REG_56;
490 break;
491 case IIO_MOD_Z:
492 reg = INV_ICM45600_IPREG_SYS1_REG_70;
493 break;
494 default:
495 return -EINVAL;
496 }
497
498 /* inv_icm45600_gyro_calibbias: min - step - max in nano */
499 min = (s64)inv_icm45600_gyro_calibbias[0] * 1000000000LL -
500 (s64)inv_icm45600_gyro_calibbias[1];
501 max = (s64)inv_icm45600_gyro_calibbias[4] * 1000000000LL +
502 (s64)inv_icm45600_gyro_calibbias[5];
503 val64 = (s64)val * 1000000000LL;
504 if (val >= 0)
505 val64 += (s64)val2;
506 else
507 val64 -= (s64)val2;
508 if (val64 < min || val64 > max)
509 return -EINVAL;
510
511 /*
512 * convert rad/s to dps then to raw value
513 * rad to dps: 180 / Pi
514 * dps to raw 14 bits signed, max 62.5: 8192 / 62.5
515 * val in nano (1000000000)
516 * val * 180 * 8192 / (Pi * 1000000000 * 62.5)
517 */
518 val64 = val64 * 180LL * 8192;
519 /* for rounding, add + or - divisor (314159265 * 625) divided by 2 */
520 if (val64 >= 0)
521 val64 += 314159265LL * 625LL / 2LL;
522 else
523 val64 -= 314159265LL * 625LL / 2LL;
524 offset = div64_s64(val64, 314159265LL * 625LL);
525
526 /* clamp value limited to 14 bits signed */
527 offset = clamp(offset, -8192, 8191);
528
529 st->buffer.u16 = cpu_to_le16(offset & INV_ICM45600_GYRO_OFFUSER_MASK);
530
531 ret = pm_runtime_resume_and_get(dev);
532 if (ret)
533 return ret;
534
535 scoped_guard(mutex, &st->lock)
536 ret = regmap_bulk_write(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16));
537
538 pm_runtime_put_autosuspend(dev);
539 return ret;
540 }
541
inv_icm45600_gyro_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)542 static int inv_icm45600_gyro_read_raw(struct iio_dev *indio_dev,
543 struct iio_chan_spec const *chan,
544 int *val, int *val2, long mask)
545 {
546 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
547 int ret;
548
549 switch (chan->type) {
550 case IIO_ANGL_VEL:
551 break;
552 case IIO_TEMP:
553 return inv_icm45600_temp_read_raw(indio_dev, chan, val, val2, mask);
554 default:
555 return -EINVAL;
556 }
557
558 switch (mask) {
559 case IIO_CHAN_INFO_RAW:
560 if (!iio_device_claim_direct(indio_dev))
561 return -EBUSY;
562 ret = inv_icm45600_gyro_read_sensor(indio_dev, chan, val);
563 iio_device_release_direct(indio_dev);
564 if (ret)
565 return ret;
566 return IIO_VAL_INT;
567 case IIO_CHAN_INFO_SCALE:
568 return inv_icm45600_gyro_read_scale(indio_dev, val, val2);
569 case IIO_CHAN_INFO_SAMP_FREQ:
570 return inv_icm45600_gyro_read_odr(st, val, val2);
571 case IIO_CHAN_INFO_CALIBBIAS:
572 return inv_icm45600_gyro_read_offset(st, chan, val, val2);
573 default:
574 return -EINVAL;
575 }
576 }
577
inv_icm45600_gyro_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)578 static int inv_icm45600_gyro_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 struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev);
584
585 if (chan->type != IIO_ANGL_VEL)
586 return -EINVAL;
587
588 switch (mask) {
589 case IIO_CHAN_INFO_SCALE:
590 *vals = gyro_st->scales;
591 *type = IIO_VAL_INT_PLUS_NANO;
592 *length = gyro_st->scales_len;
593 return IIO_AVAIL_LIST;
594 case IIO_CHAN_INFO_SAMP_FREQ:
595 *vals = inv_icm45600_gyro_odr;
596 *type = IIO_VAL_INT_PLUS_MICRO;
597 *length = ARRAY_SIZE(inv_icm45600_gyro_odr);
598 return IIO_AVAIL_LIST;
599 case IIO_CHAN_INFO_CALIBBIAS:
600 *vals = inv_icm45600_gyro_calibbias;
601 *type = IIO_VAL_INT_PLUS_NANO;
602 return IIO_AVAIL_RANGE;
603 default:
604 return -EINVAL;
605 }
606 }
607
inv_icm45600_gyro_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)608 static int inv_icm45600_gyro_write_raw(struct iio_dev *indio_dev,
609 struct iio_chan_spec const *chan,
610 int val, int val2, long mask)
611 {
612 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
613 int ret;
614
615 if (chan->type != IIO_ANGL_VEL)
616 return -EINVAL;
617
618 switch (mask) {
619 case IIO_CHAN_INFO_SCALE:
620 if (!iio_device_claim_direct(indio_dev))
621 return -EBUSY;
622 ret = inv_icm45600_gyro_write_scale(indio_dev, val, val2);
623 iio_device_release_direct(indio_dev);
624 return ret;
625 case IIO_CHAN_INFO_SAMP_FREQ:
626 return inv_icm45600_gyro_write_odr(indio_dev, val, val2);
627 case IIO_CHAN_INFO_CALIBBIAS:
628 if (!iio_device_claim_direct(indio_dev))
629 return -EBUSY;
630 ret = inv_icm45600_gyro_write_offset(st, chan, val, val2);
631 iio_device_release_direct(indio_dev);
632 return ret;
633 default:
634 return -EINVAL;
635 }
636 }
637
inv_icm45600_gyro_write_raw_get_fmt(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,long mask)638 static int inv_icm45600_gyro_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_ANGL_VEL)
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_NANO;
652 default:
653 return -EINVAL;
654 }
655 }
656
inv_icm45600_gyro_hwfifo_set_watermark(struct iio_dev * indio_dev,unsigned int val)657 static int inv_icm45600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
658 unsigned int val)
659 {
660 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
661
662 guard(mutex)(&st->lock);
663
664 st->fifo.watermark.gyro = val;
665 return inv_icm45600_buffer_update_watermark(st);
666 }
667
inv_icm45600_gyro_hwfifo_flush(struct iio_dev * indio_dev,unsigned int count)668 static int inv_icm45600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
669 unsigned int count)
670 {
671 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
672 int ret;
673
674 if (count == 0)
675 return 0;
676
677 guard(mutex)(&st->lock);
678
679 ret = inv_icm45600_buffer_hwfifo_flush(st, count);
680 if (ret)
681 return ret;
682
683 return st->fifo.nb.gyro;
684 }
685
686 static const struct iio_info inv_icm45600_gyro_info = {
687 .read_raw = inv_icm45600_gyro_read_raw,
688 .read_avail = inv_icm45600_gyro_read_avail,
689 .write_raw = inv_icm45600_gyro_write_raw,
690 .write_raw_get_fmt = inv_icm45600_gyro_write_raw_get_fmt,
691 .debugfs_reg_access = inv_icm45600_debugfs_reg,
692 .update_scan_mode = inv_icm45600_gyro_update_scan_mode,
693 .hwfifo_set_watermark = inv_icm45600_gyro_hwfifo_set_watermark,
694 .hwfifo_flush_to_buffer = inv_icm45600_gyro_hwfifo_flush,
695 };
696
inv_icm45600_gyro_init(struct inv_icm45600_state * st)697 struct iio_dev *inv_icm45600_gyro_init(struct inv_icm45600_state *st)
698 {
699 struct device *dev = regmap_get_device(st->map);
700 struct inv_icm45600_sensor_state *gyro_st;
701 struct inv_sensors_timestamp_chip ts_chip;
702 struct iio_dev *indio_dev;
703 const char *name;
704 int ret;
705
706 name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->chip_info->name);
707 if (!name)
708 return ERR_PTR(-ENOMEM);
709
710 indio_dev = devm_iio_device_alloc(dev, sizeof(*gyro_st));
711 if (!indio_dev)
712 return ERR_PTR(-ENOMEM);
713 gyro_st = iio_priv(indio_dev);
714
715 gyro_st->scales = st->chip_info->gyro_scales;
716 gyro_st->scales_len = st->chip_info->gyro_scales_len * 2;
717
718 /* low-noise by default at init */
719 gyro_st->power_mode = INV_ICM45600_SENSOR_MODE_LOW_NOISE;
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_icm45600_odr_to_period(st->conf.gyro.odr);
728 inv_sensors_timestamp_init(&gyro_st->ts, &ts_chip);
729
730 iio_device_set_drvdata(indio_dev, st);
731 indio_dev->name = name;
732 indio_dev->info = &inv_icm45600_gyro_info;
733 indio_dev->modes = INDIO_DIRECT_MODE;
734 indio_dev->channels = inv_icm45600_gyro_channels;
735 indio_dev->num_channels = ARRAY_SIZE(inv_icm45600_gyro_channels);
736 indio_dev->available_scan_masks = inv_icm45600_gyro_scan_masks;
737 indio_dev->setup_ops = &inv_icm45600_buffer_ops;
738
739 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
740 &inv_icm45600_buffer_ops);
741 if (ret)
742 return ERR_PTR(ret);
743
744 ret = devm_iio_device_register(dev, indio_dev);
745 if (ret)
746 return ERR_PTR(ret);
747
748 return indio_dev;
749 }
750
inv_icm45600_gyro_parse_fifo(struct iio_dev * indio_dev)751 int inv_icm45600_gyro_parse_fifo(struct iio_dev *indio_dev)
752 {
753 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
754 struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev);
755 struct inv_sensors_timestamp *ts = &gyro_st->ts;
756 ssize_t i, size;
757 unsigned int no;
758
759 /* parse all fifo packets */
760 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
761 struct inv_icm45600_gyro_buffer buffer = { };
762 const struct inv_icm45600_fifo_sensor_data *accel, *gyro;
763 const __le16 *timestamp;
764 const s8 *temp;
765 unsigned int odr;
766 s64 ts_val;
767
768 size = inv_icm45600_fifo_decode_packet(&st->fifo.data[i],
769 &accel, &gyro, &temp, ×tamp, &odr);
770 /* quit if error or FIFO is empty */
771 if (size <= 0)
772 return size;
773
774 /* skip packet if no gyro data or data is invalid */
775 if (gyro == NULL || !inv_icm45600_fifo_is_data_valid(gyro))
776 continue;
777
778 /* update odr */
779 if (odr & INV_ICM45600_SENSOR_GYRO)
780 inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
781 st->fifo.nb.total, no);
782
783 memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
784 /* convert 8 bits FIFO temperature in high resolution format */
785 buffer.temp = temp ? (*temp * 64) : 0;
786 ts_val = inv_sensors_timestamp_pop(ts);
787 iio_push_to_buffers_with_ts(indio_dev, &buffer, sizeof(buffer), ts_val);
788 }
789
790 return 0;
791 }
792