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_accel_scan {
24 INV_ICM45600_ACCEL_SCAN_X,
25 INV_ICM45600_ACCEL_SCAN_Y,
26 INV_ICM45600_ACCEL_SCAN_Z,
27 INV_ICM45600_ACCEL_SCAN_TEMP,
28 INV_ICM45600_ACCEL_SCAN_TIMESTAMP,
29 };
30
31 static const struct iio_chan_spec_ext_info inv_icm45600_accel_ext_infos[] = {
32 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm45600_get_mount_matrix),
33 { }
34 };
35
36 #define INV_ICM45600_ACCEL_CHAN(_modifier, _index, _ext_info) \
37 { \
38 .type = IIO_ACCEL, \
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_accel_channels[] = {
64 INV_ICM45600_ACCEL_CHAN(IIO_MOD_X, INV_ICM45600_ACCEL_SCAN_X,
65 inv_icm45600_accel_ext_infos),
66 INV_ICM45600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM45600_ACCEL_SCAN_Y,
67 inv_icm45600_accel_ext_infos),
68 INV_ICM45600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM45600_ACCEL_SCAN_Z,
69 inv_icm45600_accel_ext_infos),
70 INV_ICM45600_TEMP_CHAN(INV_ICM45600_ACCEL_SCAN_TEMP),
71 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM45600_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_icm45600_accel_buffer {
79 struct inv_icm45600_fifo_sensor_data accel;
80 s16 temp;
81 aligned_s64 timestamp;
82 };
83
84 static const unsigned long inv_icm45600_accel_scan_masks[] = {
85 /* 3-axis accel + temperature */
86 BIT(INV_ICM45600_ACCEL_SCAN_X) |
87 BIT(INV_ICM45600_ACCEL_SCAN_Y) |
88 BIT(INV_ICM45600_ACCEL_SCAN_Z) |
89 BIT(INV_ICM45600_ACCEL_SCAN_TEMP),
90 0
91 };
92
93 /* enable accelerometer sensor and FIFO write */
inv_icm45600_accel_update_scan_mode(struct iio_dev * indio_dev,const unsigned long * scan_mask)94 static int inv_icm45600_accel_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 *accel_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_ACCEL_SCAN_TEMP))
106 fifo_en |= INV_ICM45600_SENSOR_TEMP;
107
108 if (*scan_mask & (BIT(INV_ICM45600_ACCEL_SCAN_X) |
109 BIT(INV_ICM45600_ACCEL_SCAN_Y) |
110 BIT(INV_ICM45600_ACCEL_SCAN_Z))) {
111 /* enable accel sensor */
112 conf.mode = accel_st->power_mode;
113 ret = inv_icm45600_set_accel_conf(st, &conf, &sleep);
114 if (ret)
115 return ret;
116 fifo_en |= INV_ICM45600_SENSOR_ACCEL;
117 }
118
119 /* Update data FIFO write. */
120 ret = inv_icm45600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
121 }
122
123 /* Sleep required time. */
124 if (sleep)
125 msleep(sleep);
126
127 return ret;
128 }
129
_inv_icm45600_accel_read_sensor(struct inv_icm45600_state * st,struct inv_icm45600_sensor_state * accel_st,unsigned int reg,int * val)130 static int _inv_icm45600_accel_read_sensor(struct inv_icm45600_state *st,
131 struct inv_icm45600_sensor_state *accel_st,
132 unsigned int reg, int *val)
133 {
134 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
135 int ret;
136
137 /* enable accel sensor */
138 conf.mode = accel_st->power_mode;
139 ret = inv_icm45600_set_accel_conf(st, &conf, NULL);
140 if (ret)
141 return ret;
142
143 /* read accel register data */
144 ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16));
145 if (ret)
146 return ret;
147
148 *val = sign_extend32(le16_to_cpup(&st->buffer.u16), 15);
149 if (*val == INV_ICM45600_DATA_INVALID)
150 return -ENODATA;
151
152 return 0;
153 }
154
inv_icm45600_accel_read_sensor(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val)155 static int inv_icm45600_accel_read_sensor(struct iio_dev *indio_dev,
156 struct iio_chan_spec const *chan,
157 int *val)
158 {
159 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
160 struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev);
161 struct device *dev = regmap_get_device(st->map);
162 unsigned int reg;
163 int ret;
164
165 if (chan->type != IIO_ACCEL)
166 return -EINVAL;
167
168 switch (chan->channel2) {
169 case IIO_MOD_X:
170 reg = INV_ICM45600_REG_ACCEL_DATA_X;
171 break;
172 case IIO_MOD_Y:
173 reg = INV_ICM45600_REG_ACCEL_DATA_Y;
174 break;
175 case IIO_MOD_Z:
176 reg = INV_ICM45600_REG_ACCEL_DATA_Z;
177 break;
178 default:
179 return -EINVAL;
180 }
181
182 ret = pm_runtime_resume_and_get(dev);
183 if (ret)
184 return ret;
185
186 scoped_guard(mutex, &st->lock)
187 ret = _inv_icm45600_accel_read_sensor(st, accel_st, reg, val);
188
189 pm_runtime_put_autosuspend(dev);
190
191 return ret;
192 }
193
194 /* IIO format int + nano */
195 const int inv_icm45600_accel_scale[][2] = {
196 /* +/- 16G => 0.004788403 m/s-2 */
197 [INV_ICM45600_ACCEL_FS_16G] = { 0, 4788403 },
198 /* +/- 8G => 0.002394202 m/s-2 */
199 [INV_ICM45600_ACCEL_FS_8G] = { 0, 2394202 },
200 /* +/- 4G => 0.001197101 m/s-2 */
201 [INV_ICM45600_ACCEL_FS_4G] = { 0, 1197101 },
202 /* +/- 2G => 0.000598550 m/s-2 */
203 [INV_ICM45600_ACCEL_FS_2G] = { 0, 598550 },
204 };
205
206 const int inv_icm45686_accel_scale[][2] = {
207 /* +/- 32G => 0.009576806 m/s-2 */
208 [INV_ICM45686_ACCEL_FS_32G] = { 0, 9576806 },
209 /* +/- 16G => 0.004788403 m/s-2 */
210 [INV_ICM45686_ACCEL_FS_16G] = { 0, 4788403 },
211 /* +/- 8G => 0.002394202 m/s-2 */
212 [INV_ICM45686_ACCEL_FS_8G] = { 0, 2394202 },
213 /* +/- 4G => 0.001197101 m/s-2 */
214 [INV_ICM45686_ACCEL_FS_4G] = { 0, 1197101 },
215 /* +/- 2G => 0.000598550 m/s-2 */
216 [INV_ICM45686_ACCEL_FS_2G] = { 0, 598550 },
217 };
218
inv_icm45600_accel_read_scale(struct iio_dev * indio_dev,int * val,int * val2)219 static int inv_icm45600_accel_read_scale(struct iio_dev *indio_dev,
220 int *val, int *val2)
221 {
222 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
223 struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev);
224 unsigned int idx;
225
226 idx = st->conf.accel.fs;
227
228 /* Full scale register starts at 1 for not High FSR parts */
229 if (accel_st->scales == (const int *)&inv_icm45600_accel_scale)
230 idx--;
231
232 *val = accel_st->scales[2 * idx];
233 *val2 = accel_st->scales[2 * idx + 1];
234 return IIO_VAL_INT_PLUS_NANO;
235 }
236
inv_icm45600_accel_write_scale(struct iio_dev * indio_dev,int val,int val2)237 static int inv_icm45600_accel_write_scale(struct iio_dev *indio_dev,
238 int val, int val2)
239 {
240 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
241 struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev);
242 struct device *dev = regmap_get_device(st->map);
243 unsigned int idx;
244 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
245 int ret;
246
247 for (idx = 0; idx < accel_st->scales_len; idx += 2) {
248 if (val == accel_st->scales[idx] &&
249 val2 == accel_st->scales[idx + 1])
250 break;
251 }
252 if (idx == accel_st->scales_len)
253 return -EINVAL;
254
255 conf.fs = idx / 2;
256
257 /* Full scale register starts at 1 for not High FSR parts */
258 if (accel_st->scales == (const int *)&inv_icm45600_accel_scale)
259 conf.fs++;
260
261 ret = pm_runtime_resume_and_get(dev);
262 if (ret)
263 return ret;
264
265 scoped_guard(mutex, &st->lock)
266 ret = inv_icm45600_set_accel_conf(st, &conf, NULL);
267
268 pm_runtime_put_autosuspend(dev);
269
270 return ret;
271 }
272
273 /* IIO format int + micro */
274 static const int inv_icm45600_accel_odr[] = {
275 1, 562500, /* 1.5625Hz */
276 3, 125000, /* 3.125Hz */
277 6, 250000, /* 6.25Hz */
278 12, 500000, /* 12.5Hz */
279 25, 0, /* 25Hz */
280 50, 0, /* 50Hz */
281 100, 0, /* 100Hz */
282 200, 0, /* 200Hz */
283 400, 0, /* 400Hz */
284 800, 0, /* 800Hz */
285 1600, 0, /* 1.6kHz */
286 3200, 0, /* 3.2kHz */
287 6400, 0, /* 6.4kHz */
288 };
289
290 static const int inv_icm45600_accel_odr_conv[] = {
291 INV_ICM45600_ODR_1_5625HZ_LP,
292 INV_ICM45600_ODR_3_125HZ_LP,
293 INV_ICM45600_ODR_6_25HZ_LP,
294 INV_ICM45600_ODR_12_5HZ,
295 INV_ICM45600_ODR_25HZ,
296 INV_ICM45600_ODR_50HZ,
297 INV_ICM45600_ODR_100HZ,
298 INV_ICM45600_ODR_200HZ,
299 INV_ICM45600_ODR_400HZ,
300 INV_ICM45600_ODR_800HZ_LN,
301 INV_ICM45600_ODR_1600HZ_LN,
302 INV_ICM45600_ODR_3200HZ_LN,
303 INV_ICM45600_ODR_6400HZ_LN,
304 };
305
inv_icm45600_accel_read_odr(struct inv_icm45600_state * st,int * val,int * val2)306 static int inv_icm45600_accel_read_odr(struct inv_icm45600_state *st,
307 int *val, int *val2)
308 {
309 unsigned int odr;
310 unsigned int i;
311
312 odr = st->conf.accel.odr;
313
314 for (i = 0; i < ARRAY_SIZE(inv_icm45600_accel_odr_conv); ++i) {
315 if (inv_icm45600_accel_odr_conv[i] == odr)
316 break;
317 }
318 if (i >= ARRAY_SIZE(inv_icm45600_accel_odr_conv))
319 return -EINVAL;
320
321 *val = inv_icm45600_accel_odr[2 * i];
322 *val2 = inv_icm45600_accel_odr[2 * i + 1];
323
324 return IIO_VAL_INT_PLUS_MICRO;
325 }
326
_inv_icm45600_accel_write_odr(struct iio_dev * indio_dev,int odr)327 static int _inv_icm45600_accel_write_odr(struct iio_dev *indio_dev, int odr)
328 {
329 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
330 struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev);
331 struct inv_sensors_timestamp *ts = &accel_st->ts;
332 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
333 int ret;
334
335 conf.odr = odr;
336 ret = inv_sensors_timestamp_update_odr(ts, inv_icm45600_odr_to_period(conf.odr),
337 iio_buffer_enabled(indio_dev));
338 if (ret)
339 return ret;
340
341 if (st->conf.accel.mode != INV_ICM45600_SENSOR_MODE_OFF)
342 conf.mode = accel_st->power_mode;
343
344 ret = inv_icm45600_set_accel_conf(st, &conf, NULL);
345 if (ret)
346 return ret;
347
348 inv_icm45600_buffer_update_fifo_period(st);
349 inv_icm45600_buffer_update_watermark(st);
350
351 return 0;
352 }
353
inv_icm45600_accel_write_odr(struct iio_dev * indio_dev,int val,int val2)354 static int inv_icm45600_accel_write_odr(struct iio_dev *indio_dev,
355 int val, int val2)
356 {
357 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
358 struct device *dev = regmap_get_device(st->map);
359 unsigned int idx;
360 int odr;
361 int ret;
362
363 for (idx = 0; idx < ARRAY_SIZE(inv_icm45600_accel_odr); idx += 2) {
364 if (val == inv_icm45600_accel_odr[idx] &&
365 val2 == inv_icm45600_accel_odr[idx + 1])
366 break;
367 }
368 if (idx >= ARRAY_SIZE(inv_icm45600_accel_odr))
369 return -EINVAL;
370
371 odr = inv_icm45600_accel_odr_conv[idx / 2];
372
373 ret = pm_runtime_resume_and_get(dev);
374 if (ret)
375 return ret;
376
377 scoped_guard(mutex, &st->lock)
378 ret = _inv_icm45600_accel_write_odr(indio_dev, odr);
379
380 pm_runtime_put_autosuspend(dev);
381
382 return ret;
383 }
384
385 /*
386 * Calibration bias values, IIO range format int + micro.
387 * Value is limited to +/-1g coded on 14 bits signed. Step is 0.125mg.
388 */
389 static int inv_icm45600_accel_calibbias[] = {
390 -9, 806650, /* min: -9.806650 m/s² */
391 0, 1197, /* step: 0.001197 m/s² */
392 9, 805453, /* max: 9.805453 m/s² */
393 };
394
inv_icm45600_accel_read_offset(struct inv_icm45600_state * st,struct iio_chan_spec const * chan,int * val,int * val2)395 static int inv_icm45600_accel_read_offset(struct inv_icm45600_state *st,
396 struct iio_chan_spec const *chan,
397 int *val, int *val2)
398 {
399 struct device *dev = regmap_get_device(st->map);
400 s64 val64;
401 s32 bias;
402 unsigned int reg;
403 s16 offset;
404 int ret;
405
406 if (chan->type != IIO_ACCEL)
407 return -EINVAL;
408
409 switch (chan->channel2) {
410 case IIO_MOD_X:
411 reg = INV_ICM45600_IPREG_SYS2_REG_24;
412 break;
413 case IIO_MOD_Y:
414 reg = INV_ICM45600_IPREG_SYS2_REG_32;
415 break;
416 case IIO_MOD_Z:
417 reg = INV_ICM45600_IPREG_SYS2_REG_40;
418 break;
419 default:
420 return -EINVAL;
421 }
422
423 ret = pm_runtime_resume_and_get(dev);
424 if (ret)
425 return ret;
426
427 scoped_guard(mutex, &st->lock)
428 ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16));
429
430 pm_runtime_put_autosuspend(dev);
431 if (ret)
432 return ret;
433
434 offset = le16_to_cpup(&st->buffer.u16) & INV_ICM45600_ACCEL_OFFUSER_MASK;
435 /* 14 bits signed value */
436 offset = sign_extend32(offset, 13);
437
438 /*
439 * convert raw offset to g then to m/s²
440 * 14 bits signed raw step 1/8192g
441 * g to m/s²: 9.806650
442 * result in micro (* 1000000)
443 * (offset * 9806650) / 8192
444 */
445 val64 = (s64)offset * 9806650LL;
446 /* for rounding, add + or - divisor (8192) divided by 2 */
447 if (val64 >= 0)
448 val64 += 8192LL / 2LL;
449 else
450 val64 -= 8192LL / 2LL;
451 bias = div_s64(val64, 8192L);
452 *val = bias / 1000000L;
453 *val2 = bias % 1000000L;
454
455 return IIO_VAL_INT_PLUS_MICRO;
456 }
457
inv_icm45600_accel_write_offset(struct inv_icm45600_state * st,struct iio_chan_spec const * chan,int val,int val2)458 static int inv_icm45600_accel_write_offset(struct inv_icm45600_state *st,
459 struct iio_chan_spec const *chan,
460 int val, int val2)
461 {
462 struct device *dev = regmap_get_device(st->map);
463 s64 val64;
464 s32 min, max;
465 unsigned int reg;
466 s16 offset;
467 int ret;
468
469 if (chan->type != IIO_ACCEL)
470 return -EINVAL;
471
472 switch (chan->channel2) {
473 case IIO_MOD_X:
474 reg = INV_ICM45600_IPREG_SYS2_REG_24;
475 break;
476 case IIO_MOD_Y:
477 reg = INV_ICM45600_IPREG_SYS2_REG_32;
478 break;
479 case IIO_MOD_Z:
480 reg = INV_ICM45600_IPREG_SYS2_REG_40;
481 break;
482 default:
483 return -EINVAL;
484 }
485
486 /* inv_icm45600_accel_calibbias: min - step - max in micro */
487 min = inv_icm45600_accel_calibbias[0] * 1000000L -
488 inv_icm45600_accel_calibbias[1];
489 max = inv_icm45600_accel_calibbias[4] * 1000000L +
490 inv_icm45600_accel_calibbias[5];
491 val64 = (s64)val * 1000000LL;
492 if (val >= 0)
493 val64 += (s64)val2;
494 else
495 val64 -= (s64)val2;
496 if (val64 < min || val64 > max)
497 return -EINVAL;
498
499 /*
500 * convert m/s² to g then to raw value
501 * m/s² to g: 1 / 9.806650
502 * g to raw 14 bits signed, step 1/8192g: * 8192
503 * val in micro (1000000)
504 * val * 8192 / (9.806650 * 1000000)
505 */
506 val64 = val64 * 8192LL;
507 /* for rounding, add + or - divisor (9806650) divided by 2 */
508 if (val64 >= 0)
509 val64 += 9806650 / 2;
510 else
511 val64 -= 9806650 / 2;
512 offset = div_s64(val64, 9806650);
513
514 /* clamp value limited to 14 bits signed */
515 offset = clamp(offset, -8192, 8191);
516
517 st->buffer.u16 = cpu_to_le16(offset & INV_ICM45600_ACCEL_OFFUSER_MASK);
518
519 ret = pm_runtime_resume_and_get(dev);
520 if (ret)
521 return ret;
522
523 scoped_guard(mutex, &st->lock)
524 ret = regmap_bulk_write(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16));
525
526 pm_runtime_put_autosuspend(dev);
527 return ret;
528 }
529
inv_icm45600_accel_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)530 static int inv_icm45600_accel_read_raw(struct iio_dev *indio_dev,
531 struct iio_chan_spec const *chan,
532 int *val, int *val2, long mask)
533 {
534 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
535 int ret;
536
537 switch (chan->type) {
538 case IIO_ACCEL:
539 break;
540 case IIO_TEMP:
541 return inv_icm45600_temp_read_raw(indio_dev, chan, val, val2, mask);
542 default:
543 return -EINVAL;
544 }
545
546 switch (mask) {
547 case IIO_CHAN_INFO_RAW:
548 if (!iio_device_claim_direct(indio_dev))
549 return -EBUSY;
550 ret = inv_icm45600_accel_read_sensor(indio_dev, chan, val);
551 iio_device_release_direct(indio_dev);
552 if (ret)
553 return ret;
554 return IIO_VAL_INT;
555 case IIO_CHAN_INFO_SCALE:
556 return inv_icm45600_accel_read_scale(indio_dev, val, val2);
557 case IIO_CHAN_INFO_SAMP_FREQ:
558 return inv_icm45600_accel_read_odr(st, val, val2);
559 case IIO_CHAN_INFO_CALIBBIAS:
560 return inv_icm45600_accel_read_offset(st, chan, val, val2);
561 default:
562 return -EINVAL;
563 }
564 }
565
inv_icm45600_accel_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)566 static int inv_icm45600_accel_read_avail(struct iio_dev *indio_dev,
567 struct iio_chan_spec const *chan,
568 const int **vals,
569 int *type, int *length, long mask)
570 {
571 struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev);
572
573 if (chan->type != IIO_ACCEL)
574 return -EINVAL;
575
576 switch (mask) {
577 case IIO_CHAN_INFO_SCALE:
578 *vals = accel_st->scales;
579 *type = IIO_VAL_INT_PLUS_NANO;
580 *length = accel_st->scales_len;
581 return IIO_AVAIL_LIST;
582 case IIO_CHAN_INFO_SAMP_FREQ:
583 *vals = inv_icm45600_accel_odr;
584 *type = IIO_VAL_INT_PLUS_MICRO;
585 *length = ARRAY_SIZE(inv_icm45600_accel_odr);
586 return IIO_AVAIL_LIST;
587 case IIO_CHAN_INFO_CALIBBIAS:
588 *vals = inv_icm45600_accel_calibbias;
589 *type = IIO_VAL_INT_PLUS_MICRO;
590 return IIO_AVAIL_RANGE;
591 default:
592 return -EINVAL;
593 }
594 }
595
inv_icm45600_accel_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)596 static int inv_icm45600_accel_write_raw(struct iio_dev *indio_dev,
597 struct iio_chan_spec const *chan,
598 int val, int val2, long mask)
599 {
600 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
601 int ret;
602
603 if (chan->type != IIO_ACCEL)
604 return -EINVAL;
605
606 switch (mask) {
607 case IIO_CHAN_INFO_SCALE:
608 if (!iio_device_claim_direct(indio_dev))
609 return -EBUSY;
610 ret = inv_icm45600_accel_write_scale(indio_dev, val, val2);
611 iio_device_release_direct(indio_dev);
612 return ret;
613 case IIO_CHAN_INFO_SAMP_FREQ:
614 return inv_icm45600_accel_write_odr(indio_dev, val, val2);
615 case IIO_CHAN_INFO_CALIBBIAS:
616 if (!iio_device_claim_direct(indio_dev))
617 return -EBUSY;
618 ret = inv_icm45600_accel_write_offset(st, chan, val, val2);
619 iio_device_release_direct(indio_dev);
620 return ret;
621 default:
622 return -EINVAL;
623 }
624 }
625
inv_icm45600_accel_write_raw_get_fmt(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,long mask)626 static int inv_icm45600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
627 struct iio_chan_spec const *chan,
628 long mask)
629 {
630 if (chan->type != IIO_ACCEL)
631 return -EINVAL;
632
633 switch (mask) {
634 case IIO_CHAN_INFO_SCALE:
635 return IIO_VAL_INT_PLUS_NANO;
636 case IIO_CHAN_INFO_SAMP_FREQ:
637 return IIO_VAL_INT_PLUS_MICRO;
638 case IIO_CHAN_INFO_CALIBBIAS:
639 return IIO_VAL_INT_PLUS_MICRO;
640 default:
641 return -EINVAL;
642 }
643 }
644
inv_icm45600_accel_hwfifo_set_watermark(struct iio_dev * indio_dev,unsigned int val)645 static int inv_icm45600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
646 unsigned int val)
647 {
648 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
649
650 guard(mutex)(&st->lock);
651
652 st->fifo.watermark.accel = val;
653 return inv_icm45600_buffer_update_watermark(st);
654 }
655
inv_icm45600_accel_hwfifo_flush(struct iio_dev * indio_dev,unsigned int count)656 static int inv_icm45600_accel_hwfifo_flush(struct iio_dev *indio_dev,
657 unsigned int count)
658 {
659 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
660 int ret;
661
662 if (count == 0)
663 return 0;
664
665 guard(mutex)(&st->lock);
666
667 ret = inv_icm45600_buffer_hwfifo_flush(st, count);
668 if (ret)
669 return ret;
670
671 return st->fifo.nb.accel;
672 }
673
674 static const struct iio_info inv_icm45600_accel_info = {
675 .read_raw = inv_icm45600_accel_read_raw,
676 .read_avail = inv_icm45600_accel_read_avail,
677 .write_raw = inv_icm45600_accel_write_raw,
678 .write_raw_get_fmt = inv_icm45600_accel_write_raw_get_fmt,
679 .debugfs_reg_access = inv_icm45600_debugfs_reg,
680 .update_scan_mode = inv_icm45600_accel_update_scan_mode,
681 .hwfifo_set_watermark = inv_icm45600_accel_hwfifo_set_watermark,
682 .hwfifo_flush_to_buffer = inv_icm45600_accel_hwfifo_flush,
683 };
684
inv_icm45600_accel_init(struct inv_icm45600_state * st)685 struct iio_dev *inv_icm45600_accel_init(struct inv_icm45600_state *st)
686 {
687 struct device *dev = regmap_get_device(st->map);
688 struct inv_icm45600_sensor_state *accel_st;
689 struct inv_sensors_timestamp_chip ts_chip;
690 struct iio_dev *indio_dev;
691 const char *name;
692 int ret;
693
694 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->chip_info->name);
695 if (!name)
696 return ERR_PTR(-ENOMEM);
697
698 indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st));
699 if (!indio_dev)
700 return ERR_PTR(-ENOMEM);
701 accel_st = iio_priv(indio_dev);
702
703 accel_st->scales = st->chip_info->accel_scales;
704 accel_st->scales_len = st->chip_info->accel_scales_len * 2;
705
706 /* low-power (LP) mode by default at init, no ULP mode */
707 accel_st->power_mode = INV_ICM45600_SENSOR_MODE_LOW_POWER;
708 ret = regmap_set_bits(st->map, INV_ICM45600_REG_SMC_CONTROL_0,
709 INV_ICM45600_SMC_CONTROL_0_ACCEL_LP_CLK_SEL);
710 if (ret)
711 return ERR_PTR(ret);
712
713 /*
714 * clock period is 32kHz (31250ns)
715 * jitter is +/- 2% (20 per mille)
716 */
717 ts_chip.clock_period = 31250;
718 ts_chip.jitter = 20;
719 ts_chip.init_period = inv_icm45600_odr_to_period(st->conf.accel.odr);
720 inv_sensors_timestamp_init(&accel_st->ts, &ts_chip);
721
722 iio_device_set_drvdata(indio_dev, st);
723 indio_dev->name = name;
724 indio_dev->info = &inv_icm45600_accel_info;
725 indio_dev->modes = INDIO_DIRECT_MODE;
726 indio_dev->channels = inv_icm45600_accel_channels;
727 indio_dev->num_channels = ARRAY_SIZE(inv_icm45600_accel_channels);
728 indio_dev->available_scan_masks = inv_icm45600_accel_scan_masks;
729
730 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
731 &inv_icm45600_buffer_ops);
732 if (ret)
733 return ERR_PTR(ret);
734
735 ret = devm_iio_device_register(dev, indio_dev);
736 if (ret)
737 return ERR_PTR(ret);
738
739 return indio_dev;
740 }
741
inv_icm45600_accel_parse_fifo(struct iio_dev * indio_dev)742 int inv_icm45600_accel_parse_fifo(struct iio_dev *indio_dev)
743 {
744 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
745 struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev);
746 struct inv_sensors_timestamp *ts = &accel_st->ts;
747 ssize_t i, size;
748 unsigned int no;
749
750 /* parse all fifo packets */
751 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
752 struct inv_icm45600_accel_buffer buffer = { };
753 const struct inv_icm45600_fifo_sensor_data *accel, *gyro;
754 const __le16 *timestamp;
755 const s8 *temp;
756 unsigned int odr;
757 s64 ts_val;
758
759 size = inv_icm45600_fifo_decode_packet(&st->fifo.data[i],
760 &accel, &gyro, &temp, ×tamp, &odr);
761 /* quit if error or FIFO is empty */
762 if (size <= 0)
763 return size;
764
765 /* skip packet if no accel data or data is invalid */
766 if (accel == NULL || !inv_icm45600_fifo_is_data_valid(accel))
767 continue;
768
769 /* update odr */
770 if (odr & INV_ICM45600_SENSOR_ACCEL)
771 inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
772 st->fifo.nb.total, no);
773
774 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
775 /* convert 8 bits FIFO temperature in high resolution format */
776 buffer.temp = temp ? (*temp * 64) : 0;
777 ts_val = inv_sensors_timestamp_pop(ts);
778 iio_push_to_buffers_with_ts(indio_dev, &buffer, sizeof(buffer), ts_val);
779 }
780
781 return 0;
782 }
783