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/minmax.h>
14 #include <linux/units.h>
15
16 #include <linux/iio/buffer.h>
17 #include <linux/iio/common/inv_sensors_timestamp.h>
18 #include <linux/iio/events.h>
19 #include <linux/iio/iio.h>
20 #include <linux/iio/kfifo_buf.h>
21
22 #include "inv_icm42600.h"
23 #include "inv_icm42600_temp.h"
24 #include "inv_icm42600_buffer.h"
25
26 #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
27 { \
28 .type = IIO_ACCEL, \
29 .modified = 1, \
30 .channel2 = _modifier, \
31 .info_mask_separate = \
32 BIT(IIO_CHAN_INFO_RAW) | \
33 BIT(IIO_CHAN_INFO_CALIBBIAS), \
34 .info_mask_shared_by_type = \
35 BIT(IIO_CHAN_INFO_SCALE), \
36 .info_mask_shared_by_type_available = \
37 BIT(IIO_CHAN_INFO_SCALE) | \
38 BIT(IIO_CHAN_INFO_CALIBBIAS), \
39 .info_mask_shared_by_all = \
40 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
41 .info_mask_shared_by_all_available = \
42 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
43 .scan_index = _index, \
44 .scan_type = { \
45 .sign = 's', \
46 .realbits = 16, \
47 .storagebits = 16, \
48 .endianness = IIO_BE, \
49 }, \
50 .ext_info = _ext_info, \
51 }
52
53 #define INV_ICM42600_ACCEL_EVENT_CHAN(_modifier, _events, _events_nb) \
54 { \
55 .type = IIO_ACCEL, \
56 .modified = 1, \
57 .channel2 = _modifier, \
58 .event_spec = _events, \
59 .num_event_specs = _events_nb, \
60 .scan_index = -1, \
61 }
62
63 enum inv_icm42600_accel_scan {
64 INV_ICM42600_ACCEL_SCAN_X,
65 INV_ICM42600_ACCEL_SCAN_Y,
66 INV_ICM42600_ACCEL_SCAN_Z,
67 INV_ICM42600_ACCEL_SCAN_TEMP,
68 INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
69 };
70
71 static const char * const inv_icm42600_accel_power_mode_items[] = {
72 "low-noise",
73 "low-power",
74 };
75 static const int inv_icm42600_accel_power_mode_values[] = {
76 INV_ICM42600_SENSOR_MODE_LOW_NOISE,
77 INV_ICM42600_SENSOR_MODE_LOW_POWER,
78 };
79 static const int inv_icm42600_accel_filter_values[] = {
80 INV_ICM42600_FILTER_BW_ODR_DIV_2,
81 INV_ICM42600_FILTER_AVG_16X,
82 };
83
inv_icm42600_accel_power_mode_set(struct iio_dev * indio_dev,const struct iio_chan_spec * chan,unsigned int idx)84 static int inv_icm42600_accel_power_mode_set(struct iio_dev *indio_dev,
85 const struct iio_chan_spec *chan,
86 unsigned int idx)
87 {
88 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
89 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
90 int power_mode, filter;
91
92 if (chan->type != IIO_ACCEL)
93 return -EINVAL;
94
95 if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values))
96 return -EINVAL;
97
98 power_mode = inv_icm42600_accel_power_mode_values[idx];
99 filter = inv_icm42600_accel_filter_values[idx];
100
101 guard(mutex)(&st->lock);
102
103 /* cannot change if accel sensor is on */
104 if (st->conf.accel.mode != INV_ICM42600_SENSOR_MODE_OFF)
105 return -EBUSY;
106
107 /* prevent change if power mode is not supported by the ODR */
108 switch (power_mode) {
109 case INV_ICM42600_SENSOR_MODE_LOW_NOISE:
110 if (st->conf.accel.odr >= INV_ICM42600_ODR_6_25HZ_LP &&
111 st->conf.accel.odr <= INV_ICM42600_ODR_1_5625HZ_LP)
112 return -EPERM;
113 break;
114 case INV_ICM42600_SENSOR_MODE_LOW_POWER:
115 default:
116 if (st->conf.accel.odr <= INV_ICM42600_ODR_1KHZ_LN)
117 return -EPERM;
118 break;
119 }
120
121 accel_st->power_mode = power_mode;
122 accel_st->filter = filter;
123
124 return 0;
125 }
126
inv_icm42600_accel_power_mode_get(struct iio_dev * indio_dev,const struct iio_chan_spec * chan)127 static int inv_icm42600_accel_power_mode_get(struct iio_dev *indio_dev,
128 const struct iio_chan_spec *chan)
129 {
130 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
131 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
132 unsigned int idx;
133 int power_mode;
134
135 if (chan->type != IIO_ACCEL)
136 return -EINVAL;
137
138 guard(mutex)(&st->lock);
139
140 /* if sensor is on, returns actual power mode and not configured one */
141 switch (st->conf.accel.mode) {
142 case INV_ICM42600_SENSOR_MODE_LOW_POWER:
143 case INV_ICM42600_SENSOR_MODE_LOW_NOISE:
144 power_mode = st->conf.accel.mode;
145 break;
146 default:
147 power_mode = accel_st->power_mode;
148 break;
149 }
150
151 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_power_mode_values); ++idx) {
152 if (power_mode == inv_icm42600_accel_power_mode_values[idx])
153 break;
154 }
155 if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values))
156 return -EINVAL;
157
158 return idx;
159 }
160
161 static const struct iio_enum inv_icm42600_accel_power_mode_enum = {
162 .items = inv_icm42600_accel_power_mode_items,
163 .num_items = ARRAY_SIZE(inv_icm42600_accel_power_mode_items),
164 .set = inv_icm42600_accel_power_mode_set,
165 .get = inv_icm42600_accel_power_mode_get,
166 };
167
168 static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
169 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
170 IIO_ENUM_AVAILABLE("power_mode", IIO_SHARED_BY_TYPE,
171 &inv_icm42600_accel_power_mode_enum),
172 IIO_ENUM("power_mode", IIO_SHARED_BY_TYPE,
173 &inv_icm42600_accel_power_mode_enum),
174 { }
175 };
176
177 /* WoM event: rising ROC */
178 static const struct iio_event_spec inv_icm42600_wom_events[] = {
179 {
180 .type = IIO_EV_TYPE_ROC,
181 .dir = IIO_EV_DIR_RISING,
182 .mask_separate = BIT(IIO_EV_INFO_ENABLE) |
183 BIT(IIO_EV_INFO_VALUE),
184 },
185 };
186
187 static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
188 INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X,
189 inv_icm42600_accel_ext_infos),
190 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y,
191 inv_icm42600_accel_ext_infos),
192 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
193 inv_icm42600_accel_ext_infos),
194 INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
195 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
196 INV_ICM42600_ACCEL_EVENT_CHAN(IIO_MOD_X_OR_Y_OR_Z, inv_icm42600_wom_events,
197 ARRAY_SIZE(inv_icm42600_wom_events)),
198 };
199
200 /*
201 * IIO buffer data: size must be a power of 2 and timestamp aligned
202 * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp
203 */
204 struct inv_icm42600_accel_buffer {
205 struct inv_icm42600_fifo_sensor_data accel;
206 s16 temp;
207 aligned_s64 timestamp;
208 };
209
210 #define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
211 (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
212 BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
213 BIT(INV_ICM42600_ACCEL_SCAN_Z))
214
215 #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
216
217 static const unsigned long inv_icm42600_accel_scan_masks[] = {
218 /* 3-axis accel + temperature */
219 INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
220 0,
221 };
222
223 /* enable accelerometer sensor and FIFO write */
inv_icm42600_accel_update_scan_mode(struct iio_dev * indio_dev,const unsigned long * scan_mask)224 static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
225 const unsigned long *scan_mask)
226 {
227 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
228 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
229 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
230 unsigned int fifo_en = 0;
231 unsigned int sleep_temp = 0;
232 unsigned int sleep_accel = 0;
233 unsigned int sleep;
234 int ret;
235
236 mutex_lock(&st->lock);
237
238 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
239 /* enable temp sensor */
240 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
241 if (ret)
242 goto out_unlock;
243 fifo_en |= INV_ICM42600_SENSOR_TEMP;
244 }
245
246 if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
247 /* enable accel sensor */
248 conf.mode = accel_st->power_mode;
249 conf.filter = accel_st->filter;
250 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
251 if (ret)
252 goto out_unlock;
253 fifo_en |= INV_ICM42600_SENSOR_ACCEL;
254 }
255
256 /* update data FIFO write */
257 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
258
259 out_unlock:
260 mutex_unlock(&st->lock);
261 /* sleep maximum required time */
262 sleep = max(sleep_accel, sleep_temp);
263 if (sleep)
264 msleep(sleep);
265 return ret;
266 }
267
inv_icm42600_accel_read_sensor(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,s16 * val)268 static int inv_icm42600_accel_read_sensor(struct iio_dev *indio_dev,
269 struct iio_chan_spec const *chan,
270 s16 *val)
271 {
272 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
273 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
274 struct device *dev = regmap_get_device(st->map);
275 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
276 unsigned int reg;
277 __be16 *data;
278 int ret;
279
280 if (chan->type != IIO_ACCEL)
281 return -EINVAL;
282
283 switch (chan->channel2) {
284 case IIO_MOD_X:
285 reg = INV_ICM42600_REG_ACCEL_DATA_X;
286 break;
287 case IIO_MOD_Y:
288 reg = INV_ICM42600_REG_ACCEL_DATA_Y;
289 break;
290 case IIO_MOD_Z:
291 reg = INV_ICM42600_REG_ACCEL_DATA_Z;
292 break;
293 default:
294 return -EINVAL;
295 }
296
297 pm_runtime_get_sync(dev);
298 mutex_lock(&st->lock);
299
300 /* enable accel sensor */
301 conf.mode = accel_st->power_mode;
302 conf.filter = accel_st->filter;
303 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
304 if (ret)
305 goto exit;
306
307 /* read accel register data */
308 data = (__be16 *)&st->buffer[0];
309 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
310 if (ret)
311 goto exit;
312
313 *val = (s16)be16_to_cpup(data);
314 if (*val == INV_ICM42600_DATA_INVALID)
315 ret = -EINVAL;
316 exit:
317 mutex_unlock(&st->lock);
318 pm_runtime_mark_last_busy(dev);
319 pm_runtime_put_autosuspend(dev);
320 return ret;
321 }
322
inv_icm42600_accel_convert_roc_to_wom(u64 roc,int accel_hz,int accel_uhz)323 static unsigned int inv_icm42600_accel_convert_roc_to_wom(u64 roc,
324 int accel_hz, int accel_uhz)
325 {
326 /* 1000/256mg per LSB converted in µm/s² */
327 const unsigned int convert = (9807U * (MICRO / MILLI)) / 256U;
328 u64 value;
329 u64 freq_uhz;
330
331 /* return 0 only if roc is 0 */
332 if (roc == 0)
333 return 0;
334
335 freq_uhz = (u64)accel_hz * MICRO + (u64)accel_uhz;
336 value = div64_u64(roc * MICRO, freq_uhz * (u64)convert);
337
338 /* limit value to 8 bits and prevent 0 */
339 return clamp(value, 1, 255);
340 }
341
inv_icm42600_accel_convert_wom_to_roc(unsigned int threshold,int accel_hz,int accel_uhz)342 static u64 inv_icm42600_accel_convert_wom_to_roc(unsigned int threshold,
343 int accel_hz, int accel_uhz)
344 {
345 /* 1000/256mg per LSB converted in µm/s² */
346 const unsigned int convert = (9807U * (MICRO / MILLI)) / 256U;
347 u64 value;
348 u64 freq_uhz;
349
350 value = threshold * convert;
351 freq_uhz = (u64)accel_hz * MICRO + (u64)accel_uhz;
352
353 /* compute the differential by multiplying by the frequency */
354 return div_u64(value * freq_uhz, MICRO);
355 }
356
inv_icm42600_accel_set_wom_threshold(struct inv_icm42600_state * st,u64 value,int accel_hz,int accel_uhz)357 static int inv_icm42600_accel_set_wom_threshold(struct inv_icm42600_state *st,
358 u64 value,
359 int accel_hz, int accel_uhz)
360 {
361 unsigned int threshold;
362 int ret;
363
364 /* convert roc to wom threshold and convert back to handle clipping */
365 threshold = inv_icm42600_accel_convert_roc_to_wom(value, accel_hz, accel_uhz);
366 value = inv_icm42600_accel_convert_wom_to_roc(threshold, accel_hz, accel_uhz);
367
368 dev_dbg(regmap_get_device(st->map), "wom_threshold: 0x%x\n", threshold);
369
370 /* set accel WoM threshold for the 3 axes */
371 st->buffer[0] = threshold;
372 st->buffer[1] = threshold;
373 st->buffer[2] = threshold;
374 ret = regmap_bulk_write(st->map, INV_ICM42600_REG_ACCEL_WOM_X_THR, st->buffer, 3);
375 if (ret)
376 return ret;
377
378 st->apex.wom.value = value;
379
380 return 0;
381 }
382
_inv_icm42600_accel_enable_wom(struct iio_dev * indio_dev)383 static int _inv_icm42600_accel_enable_wom(struct iio_dev *indio_dev)
384 {
385 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
386 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
387 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
388 unsigned int sleep_ms = 0;
389 int ret;
390
391 scoped_guard(mutex, &st->lock) {
392 /* turn on accel sensor */
393 conf.mode = accel_st->power_mode;
394 conf.filter = accel_st->filter;
395 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_ms);
396 if (ret)
397 return ret;
398 }
399
400 if (sleep_ms)
401 msleep(sleep_ms);
402
403 scoped_guard(mutex, &st->lock) {
404 ret = inv_icm42600_enable_wom(st);
405 if (ret)
406 return ret;
407 st->apex.on++;
408 st->apex.wom.enable = true;
409 }
410
411 return 0;
412 }
413
inv_icm42600_accel_enable_wom(struct iio_dev * indio_dev)414 static int inv_icm42600_accel_enable_wom(struct iio_dev *indio_dev)
415 {
416 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
417 struct device *pdev = regmap_get_device(st->map);
418 int ret;
419
420 ret = pm_runtime_resume_and_get(pdev);
421 if (ret)
422 return ret;
423
424 ret = _inv_icm42600_accel_enable_wom(indio_dev);
425 if (ret) {
426 pm_runtime_mark_last_busy(pdev);
427 pm_runtime_put_autosuspend(pdev);
428 return ret;
429 }
430
431 return 0;
432 }
433
_inv_icm42600_accel_disable_wom(struct iio_dev * indio_dev)434 static int _inv_icm42600_accel_disable_wom(struct iio_dev *indio_dev)
435 {
436 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
437 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
438 unsigned int sleep_ms = 0;
439 int ret;
440
441 scoped_guard(mutex, &st->lock) {
442 /*
443 * Consider that turning off WoM is always working to avoid
444 * blocking the chip in on mode and prevent going back to sleep.
445 * If there is an error, the chip will anyway go back to sleep
446 * and the feature will not work anymore.
447 */
448 st->apex.wom.enable = false;
449 st->apex.on--;
450 ret = inv_icm42600_disable_wom(st);
451 if (ret)
452 return ret;
453 /* turn off accel sensor if not used */
454 if (!st->apex.on && !iio_buffer_enabled(indio_dev)) {
455 conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
456 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_ms);
457 if (ret)
458 return ret;
459 }
460 }
461
462 if (sleep_ms)
463 msleep(sleep_ms);
464
465 return 0;
466 }
467
inv_icm42600_accel_disable_wom(struct iio_dev * indio_dev)468 static int inv_icm42600_accel_disable_wom(struct iio_dev *indio_dev)
469 {
470 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
471 struct device *pdev = regmap_get_device(st->map);
472 int ret;
473
474 ret = _inv_icm42600_accel_disable_wom(indio_dev);
475
476 pm_runtime_mark_last_busy(pdev);
477 pm_runtime_put_autosuspend(pdev);
478
479 return ret;
480 }
481
inv_icm42600_accel_handle_events(struct iio_dev * indio_dev,unsigned int status2,unsigned int status3,s64 timestamp)482 void inv_icm42600_accel_handle_events(struct iio_dev *indio_dev,
483 unsigned int status2, unsigned int status3,
484 s64 timestamp)
485 {
486 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
487 u64 ev_code;
488
489 /* handle WoM event */
490 if (st->apex.wom.enable && (status2 & INV_ICM42600_INT_STATUS2_WOM_INT)) {
491 ev_code = IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, IIO_MOD_X_OR_Y_OR_Z,
492 IIO_EV_TYPE_ROC, IIO_EV_DIR_RISING);
493 iio_push_event(indio_dev, ev_code, timestamp);
494 }
495 }
496
497 /* IIO format int + nano */
498 static const int inv_icm42600_accel_scale[] = {
499 /* +/- 16G => 0.004788403 m/s-2 */
500 [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
501 [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
502 /* +/- 8G => 0.002394202 m/s-2 */
503 [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
504 [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
505 /* +/- 4G => 0.001197101 m/s-2 */
506 [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
507 [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
508 /* +/- 2G => 0.000598550 m/s-2 */
509 [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
510 [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
511 };
512 static const int inv_icm42686_accel_scale[] = {
513 /* +/- 32G => 0.009576807 m/s-2 */
514 [2 * INV_ICM42686_ACCEL_FS_32G] = 0,
515 [2 * INV_ICM42686_ACCEL_FS_32G + 1] = 9576807,
516 /* +/- 16G => 0.004788403 m/s-2 */
517 [2 * INV_ICM42686_ACCEL_FS_16G] = 0,
518 [2 * INV_ICM42686_ACCEL_FS_16G + 1] = 4788403,
519 /* +/- 8G => 0.002394202 m/s-2 */
520 [2 * INV_ICM42686_ACCEL_FS_8G] = 0,
521 [2 * INV_ICM42686_ACCEL_FS_8G + 1] = 2394202,
522 /* +/- 4G => 0.001197101 m/s-2 */
523 [2 * INV_ICM42686_ACCEL_FS_4G] = 0,
524 [2 * INV_ICM42686_ACCEL_FS_4G + 1] = 1197101,
525 /* +/- 2G => 0.000598550 m/s-2 */
526 [2 * INV_ICM42686_ACCEL_FS_2G] = 0,
527 [2 * INV_ICM42686_ACCEL_FS_2G + 1] = 598550,
528 };
529
inv_icm42600_accel_read_scale(struct iio_dev * indio_dev,int * val,int * val2)530 static int inv_icm42600_accel_read_scale(struct iio_dev *indio_dev,
531 int *val, int *val2)
532 {
533 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
534 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
535 unsigned int idx;
536
537 idx = st->conf.accel.fs;
538
539 *val = accel_st->scales[2 * idx];
540 *val2 = accel_st->scales[2 * idx + 1];
541 return IIO_VAL_INT_PLUS_NANO;
542 }
543
inv_icm42600_accel_write_scale(struct iio_dev * indio_dev,int val,int val2)544 static int inv_icm42600_accel_write_scale(struct iio_dev *indio_dev,
545 int val, int val2)
546 {
547 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
548 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
549 struct device *dev = regmap_get_device(st->map);
550 unsigned int idx;
551 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
552 int ret;
553
554 for (idx = 0; idx < accel_st->scales_len; idx += 2) {
555 if (val == accel_st->scales[idx] &&
556 val2 == accel_st->scales[idx + 1])
557 break;
558 }
559 if (idx >= accel_st->scales_len)
560 return -EINVAL;
561
562 conf.fs = idx / 2;
563
564 pm_runtime_get_sync(dev);
565 mutex_lock(&st->lock);
566
567 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
568
569 mutex_unlock(&st->lock);
570 pm_runtime_mark_last_busy(dev);
571 pm_runtime_put_autosuspend(dev);
572
573 return ret;
574 }
575
576 /* IIO format int + micro */
577 static const int inv_icm42600_accel_odr[] = {
578 /* 1.5625Hz */
579 1, 562500,
580 /* 3.125Hz */
581 3, 125000,
582 /* 6.25Hz */
583 6, 250000,
584 /* 12.5Hz */
585 12, 500000,
586 /* 25Hz */
587 25, 0,
588 /* 50Hz */
589 50, 0,
590 /* 100Hz */
591 100, 0,
592 /* 200Hz */
593 200, 0,
594 /* 1kHz */
595 1000, 0,
596 /* 2kHz */
597 2000, 0,
598 /* 4kHz */
599 4000, 0,
600 };
601
602 static const int inv_icm42600_accel_odr_conv[] = {
603 INV_ICM42600_ODR_1_5625HZ_LP,
604 INV_ICM42600_ODR_3_125HZ_LP,
605 INV_ICM42600_ODR_6_25HZ_LP,
606 INV_ICM42600_ODR_12_5HZ,
607 INV_ICM42600_ODR_25HZ,
608 INV_ICM42600_ODR_50HZ,
609 INV_ICM42600_ODR_100HZ,
610 INV_ICM42600_ODR_200HZ,
611 INV_ICM42600_ODR_1KHZ_LN,
612 INV_ICM42600_ODR_2KHZ_LN,
613 INV_ICM42600_ODR_4KHZ_LN,
614 };
615
inv_icm42600_accel_read_odr(struct inv_icm42600_state * st,int * val,int * val2)616 static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
617 int *val, int *val2)
618 {
619 unsigned int odr;
620 unsigned int i;
621
622 odr = st->conf.accel.odr;
623
624 for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
625 if (inv_icm42600_accel_odr_conv[i] == odr)
626 break;
627 }
628 if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
629 return -EINVAL;
630
631 *val = inv_icm42600_accel_odr[2 * i];
632 *val2 = inv_icm42600_accel_odr[2 * i + 1];
633
634 return IIO_VAL_INT_PLUS_MICRO;
635 }
636
inv_icm42600_accel_write_odr(struct iio_dev * indio_dev,int val,int val2)637 static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
638 int val, int val2)
639 {
640 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
641 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
642 struct inv_sensors_timestamp *ts = &accel_st->ts;
643 struct device *dev = regmap_get_device(st->map);
644 unsigned int idx;
645 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
646 int ret;
647
648 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
649 if (val == inv_icm42600_accel_odr[idx] &&
650 val2 == inv_icm42600_accel_odr[idx + 1])
651 break;
652 }
653 if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
654 return -EINVAL;
655
656 conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
657
658 pm_runtime_get_sync(dev);
659 mutex_lock(&st->lock);
660
661 ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
662 iio_buffer_enabled(indio_dev));
663 if (ret)
664 goto out_unlock;
665
666 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
667 if (ret)
668 goto out_unlock;
669 /* update wom threshold since roc is dependent on sampling frequency */
670 ret = inv_icm42600_accel_set_wom_threshold(st, st->apex.wom.value, val, val2);
671 if (ret)
672 goto out_unlock;
673 inv_icm42600_buffer_update_fifo_period(st);
674 inv_icm42600_buffer_update_watermark(st);
675
676 out_unlock:
677 mutex_unlock(&st->lock);
678 pm_runtime_mark_last_busy(dev);
679 pm_runtime_put_autosuspend(dev);
680
681 return ret;
682 }
683
684 /*
685 * Calibration bias values, IIO range format int + micro.
686 * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
687 */
688 static int inv_icm42600_accel_calibbias[] = {
689 -10, 42010, /* min: -10.042010 m/s² */
690 0, 4903, /* step: 0.004903 m/s² */
691 10, 37106, /* max: 10.037106 m/s² */
692 };
693
inv_icm42600_accel_read_offset(struct inv_icm42600_state * st,struct iio_chan_spec const * chan,int * val,int * val2)694 static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
695 struct iio_chan_spec const *chan,
696 int *val, int *val2)
697 {
698 struct device *dev = regmap_get_device(st->map);
699 s64 val64;
700 s32 bias;
701 unsigned int reg;
702 s16 offset;
703 u8 data[2];
704 int ret;
705
706 if (chan->type != IIO_ACCEL)
707 return -EINVAL;
708
709 switch (chan->channel2) {
710 case IIO_MOD_X:
711 reg = INV_ICM42600_REG_OFFSET_USER4;
712 break;
713 case IIO_MOD_Y:
714 reg = INV_ICM42600_REG_OFFSET_USER6;
715 break;
716 case IIO_MOD_Z:
717 reg = INV_ICM42600_REG_OFFSET_USER7;
718 break;
719 default:
720 return -EINVAL;
721 }
722
723 pm_runtime_get_sync(dev);
724 mutex_lock(&st->lock);
725
726 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
727 memcpy(data, st->buffer, sizeof(data));
728
729 mutex_unlock(&st->lock);
730 pm_runtime_mark_last_busy(dev);
731 pm_runtime_put_autosuspend(dev);
732 if (ret)
733 return ret;
734
735 /* 12 bits signed value */
736 switch (chan->channel2) {
737 case IIO_MOD_X:
738 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
739 break;
740 case IIO_MOD_Y:
741 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
742 break;
743 case IIO_MOD_Z:
744 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
745 break;
746 default:
747 return -EINVAL;
748 }
749
750 /*
751 * convert raw offset to g then to m/s²
752 * 12 bits signed raw step 0.5mg to g: 5 / 10000
753 * g to m/s²: 9.806650
754 * result in micro (1000000)
755 * (offset * 5 * 9.806650 * 1000000) / 10000
756 */
757 val64 = (s64)offset * 5LL * 9806650LL;
758 /* for rounding, add + or - divisor (10000) divided by 2 */
759 if (val64 >= 0)
760 val64 += 10000LL / 2LL;
761 else
762 val64 -= 10000LL / 2LL;
763 bias = div_s64(val64, 10000L);
764 *val = bias / 1000000L;
765 *val2 = bias % 1000000L;
766
767 return IIO_VAL_INT_PLUS_MICRO;
768 }
769
inv_icm42600_accel_write_offset(struct inv_icm42600_state * st,struct iio_chan_spec const * chan,int val,int val2)770 static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
771 struct iio_chan_spec const *chan,
772 int val, int val2)
773 {
774 struct device *dev = regmap_get_device(st->map);
775 s64 val64;
776 s32 min, max;
777 unsigned int reg, regval;
778 s16 offset;
779 int ret;
780
781 if (chan->type != IIO_ACCEL)
782 return -EINVAL;
783
784 switch (chan->channel2) {
785 case IIO_MOD_X:
786 reg = INV_ICM42600_REG_OFFSET_USER4;
787 break;
788 case IIO_MOD_Y:
789 reg = INV_ICM42600_REG_OFFSET_USER6;
790 break;
791 case IIO_MOD_Z:
792 reg = INV_ICM42600_REG_OFFSET_USER7;
793 break;
794 default:
795 return -EINVAL;
796 }
797
798 /* inv_icm42600_accel_calibbias: min - step - max in micro */
799 min = inv_icm42600_accel_calibbias[0] * 1000000L +
800 inv_icm42600_accel_calibbias[1];
801 max = inv_icm42600_accel_calibbias[4] * 1000000L +
802 inv_icm42600_accel_calibbias[5];
803 val64 = (s64)val * 1000000LL + (s64)val2;
804 if (val64 < min || val64 > max)
805 return -EINVAL;
806
807 /*
808 * convert m/s² to g then to raw value
809 * m/s² to g: 1 / 9.806650
810 * g to raw 12 bits signed, step 0.5mg: 10000 / 5
811 * val in micro (1000000)
812 * val * 10000 / (9.806650 * 1000000 * 5)
813 */
814 val64 = val64 * 10000LL;
815 /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */
816 if (val64 >= 0)
817 val64 += 9806650 * 5 / 2;
818 else
819 val64 -= 9806650 * 5 / 2;
820 offset = div_s64(val64, 9806650 * 5);
821
822 /* clamp value limited to 12 bits signed */
823 if (offset < -2048)
824 offset = -2048;
825 else if (offset > 2047)
826 offset = 2047;
827
828 pm_runtime_get_sync(dev);
829 mutex_lock(&st->lock);
830
831 switch (chan->channel2) {
832 case IIO_MOD_X:
833 /* OFFSET_USER4 register is shared */
834 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
835 ®val);
836 if (ret)
837 goto out_unlock;
838 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
839 st->buffer[1] = offset & 0xFF;
840 break;
841 case IIO_MOD_Y:
842 /* OFFSET_USER7 register is shared */
843 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
844 ®val);
845 if (ret)
846 goto out_unlock;
847 st->buffer[0] = offset & 0xFF;
848 st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
849 break;
850 case IIO_MOD_Z:
851 /* OFFSET_USER7 register is shared */
852 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
853 ®val);
854 if (ret)
855 goto out_unlock;
856 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
857 st->buffer[1] = offset & 0xFF;
858 break;
859 default:
860 ret = -EINVAL;
861 goto out_unlock;
862 }
863
864 ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
865
866 out_unlock:
867 mutex_unlock(&st->lock);
868 pm_runtime_mark_last_busy(dev);
869 pm_runtime_put_autosuspend(dev);
870 return ret;
871 }
872
inv_icm42600_accel_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)873 static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
874 struct iio_chan_spec const *chan,
875 int *val, int *val2, long mask)
876 {
877 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
878 s16 data;
879 int ret;
880
881 switch (chan->type) {
882 case IIO_ACCEL:
883 break;
884 case IIO_TEMP:
885 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
886 default:
887 return -EINVAL;
888 }
889
890 switch (mask) {
891 case IIO_CHAN_INFO_RAW:
892 if (!iio_device_claim_direct(indio_dev))
893 return -EBUSY;
894 ret = inv_icm42600_accel_read_sensor(indio_dev, chan, &data);
895 iio_device_release_direct(indio_dev);
896 if (ret)
897 return ret;
898 *val = data;
899 return IIO_VAL_INT;
900 case IIO_CHAN_INFO_SCALE:
901 return inv_icm42600_accel_read_scale(indio_dev, val, val2);
902 case IIO_CHAN_INFO_SAMP_FREQ:
903 return inv_icm42600_accel_read_odr(st, val, val2);
904 case IIO_CHAN_INFO_CALIBBIAS:
905 return inv_icm42600_accel_read_offset(st, chan, val, val2);
906 default:
907 return -EINVAL;
908 }
909 }
910
inv_icm42600_accel_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)911 static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
912 struct iio_chan_spec const *chan,
913 const int **vals,
914 int *type, int *length, long mask)
915 {
916 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
917
918 if (chan->type != IIO_ACCEL)
919 return -EINVAL;
920
921 switch (mask) {
922 case IIO_CHAN_INFO_SCALE:
923 *vals = accel_st->scales;
924 *type = IIO_VAL_INT_PLUS_NANO;
925 *length = accel_st->scales_len;
926 return IIO_AVAIL_LIST;
927 case IIO_CHAN_INFO_SAMP_FREQ:
928 *vals = inv_icm42600_accel_odr;
929 *type = IIO_VAL_INT_PLUS_MICRO;
930 *length = ARRAY_SIZE(inv_icm42600_accel_odr);
931 return IIO_AVAIL_LIST;
932 case IIO_CHAN_INFO_CALIBBIAS:
933 *vals = inv_icm42600_accel_calibbias;
934 *type = IIO_VAL_INT_PLUS_MICRO;
935 return IIO_AVAIL_RANGE;
936 default:
937 return -EINVAL;
938 }
939 }
940
inv_icm42600_accel_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)941 static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
942 struct iio_chan_spec const *chan,
943 int val, int val2, long mask)
944 {
945 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
946 int ret;
947
948 if (chan->type != IIO_ACCEL)
949 return -EINVAL;
950
951 switch (mask) {
952 case IIO_CHAN_INFO_SCALE:
953 if (!iio_device_claim_direct(indio_dev))
954 return -EBUSY;
955 ret = inv_icm42600_accel_write_scale(indio_dev, val, val2);
956 iio_device_release_direct(indio_dev);
957 return ret;
958 case IIO_CHAN_INFO_SAMP_FREQ:
959 return inv_icm42600_accel_write_odr(indio_dev, val, val2);
960 case IIO_CHAN_INFO_CALIBBIAS:
961 if (!iio_device_claim_direct(indio_dev))
962 return -EBUSY;
963 ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
964 iio_device_release_direct(indio_dev);
965 return ret;
966 default:
967 return -EINVAL;
968 }
969 }
970
inv_icm42600_accel_write_raw_get_fmt(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,long mask)971 static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
972 struct iio_chan_spec const *chan,
973 long mask)
974 {
975 if (chan->type != IIO_ACCEL)
976 return -EINVAL;
977
978 switch (mask) {
979 case IIO_CHAN_INFO_SCALE:
980 return IIO_VAL_INT_PLUS_NANO;
981 case IIO_CHAN_INFO_SAMP_FREQ:
982 return IIO_VAL_INT_PLUS_MICRO;
983 case IIO_CHAN_INFO_CALIBBIAS:
984 return IIO_VAL_INT_PLUS_MICRO;
985 default:
986 return -EINVAL;
987 }
988 }
989
inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev * indio_dev,unsigned int val)990 static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
991 unsigned int val)
992 {
993 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
994 int ret;
995
996 mutex_lock(&st->lock);
997
998 st->fifo.watermark.accel = val;
999 ret = inv_icm42600_buffer_update_watermark(st);
1000
1001 mutex_unlock(&st->lock);
1002
1003 return ret;
1004 }
1005
inv_icm42600_accel_hwfifo_flush(struct iio_dev * indio_dev,unsigned int count)1006 static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
1007 unsigned int count)
1008 {
1009 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
1010 int ret;
1011
1012 if (count == 0)
1013 return 0;
1014
1015 mutex_lock(&st->lock);
1016
1017 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
1018 if (!ret)
1019 ret = st->fifo.nb.accel;
1020
1021 mutex_unlock(&st->lock);
1022
1023 return ret;
1024 }
1025
inv_icm42600_accel_read_event_config(struct iio_dev * indio_dev,const struct iio_chan_spec * chan,enum iio_event_type type,enum iio_event_direction dir)1026 static int inv_icm42600_accel_read_event_config(struct iio_dev *indio_dev,
1027 const struct iio_chan_spec *chan,
1028 enum iio_event_type type,
1029 enum iio_event_direction dir)
1030 {
1031 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
1032
1033 /* handle only WoM (roc rising) event */
1034 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING)
1035 return -EINVAL;
1036
1037 guard(mutex)(&st->lock);
1038
1039 return st->apex.wom.enable ? 1 : 0;
1040 }
1041
inv_icm42600_accel_write_event_config(struct iio_dev * indio_dev,const struct iio_chan_spec * chan,enum iio_event_type type,enum iio_event_direction dir,bool state)1042 static int inv_icm42600_accel_write_event_config(struct iio_dev *indio_dev,
1043 const struct iio_chan_spec *chan,
1044 enum iio_event_type type,
1045 enum iio_event_direction dir,
1046 bool state)
1047 {
1048 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
1049
1050 /* handle only WoM (roc rising) event */
1051 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING)
1052 return -EINVAL;
1053
1054 scoped_guard(mutex, &st->lock) {
1055 if (st->apex.wom.enable == state)
1056 return 0;
1057 }
1058
1059 if (state)
1060 return inv_icm42600_accel_enable_wom(indio_dev);
1061
1062 return inv_icm42600_accel_disable_wom(indio_dev);
1063 }
1064
inv_icm42600_accel_read_event_value(struct iio_dev * indio_dev,const struct iio_chan_spec * chan,enum iio_event_type type,enum iio_event_direction dir,enum iio_event_info info,int * val,int * val2)1065 static int inv_icm42600_accel_read_event_value(struct iio_dev *indio_dev,
1066 const struct iio_chan_spec *chan,
1067 enum iio_event_type type,
1068 enum iio_event_direction dir,
1069 enum iio_event_info info,
1070 int *val, int *val2)
1071 {
1072 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
1073 u32 rem;
1074
1075 /* handle only WoM (roc rising) event value */
1076 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING)
1077 return -EINVAL;
1078
1079 guard(mutex)(&st->lock);
1080
1081 /* return value in micro */
1082 *val = div_u64_rem(st->apex.wom.value, MICRO, &rem);
1083 *val2 = rem;
1084 return IIO_VAL_INT_PLUS_MICRO;
1085 }
1086
_inv_icm42600_accel_wom_value(struct inv_icm42600_state * st,int val,int val2)1087 static int _inv_icm42600_accel_wom_value(struct inv_icm42600_state *st,
1088 int val, int val2)
1089 {
1090 u64 value;
1091 unsigned int accel_hz, accel_uhz;
1092 int ret;
1093
1094 guard(mutex)(&st->lock);
1095
1096 ret = inv_icm42600_accel_read_odr(st, &accel_hz, &accel_uhz);
1097 if (ret < 0)
1098 return ret;
1099
1100 value = (u64)val * MICRO + (u64)val2;
1101
1102 return inv_icm42600_accel_set_wom_threshold(st, value,
1103 accel_hz, accel_uhz);
1104 }
1105
inv_icm42600_accel_write_event_value(struct iio_dev * indio_dev,const struct iio_chan_spec * chan,enum iio_event_type type,enum iio_event_direction dir,enum iio_event_info info,int val,int val2)1106 static int inv_icm42600_accel_write_event_value(struct iio_dev *indio_dev,
1107 const struct iio_chan_spec *chan,
1108 enum iio_event_type type,
1109 enum iio_event_direction dir,
1110 enum iio_event_info info,
1111 int val, int val2)
1112 {
1113 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
1114 struct device *dev = regmap_get_device(st->map);
1115 int ret;
1116
1117 /* handle only WoM (roc rising) event value */
1118 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING)
1119 return -EINVAL;
1120
1121 if (val < 0 || val2 < 0)
1122 return -EINVAL;
1123
1124 ret = pm_runtime_resume_and_get(dev);
1125 if (ret)
1126 return ret;
1127
1128 ret = _inv_icm42600_accel_wom_value(st, val, val2);
1129
1130 pm_runtime_mark_last_busy(dev);
1131 pm_runtime_put_autosuspend(dev);
1132
1133 return ret;
1134 }
1135
1136 static const struct iio_info inv_icm42600_accel_info = {
1137 .read_raw = inv_icm42600_accel_read_raw,
1138 .read_avail = inv_icm42600_accel_read_avail,
1139 .write_raw = inv_icm42600_accel_write_raw,
1140 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
1141 .debugfs_reg_access = inv_icm42600_debugfs_reg,
1142 .update_scan_mode = inv_icm42600_accel_update_scan_mode,
1143 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
1144 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
1145 .read_event_config = inv_icm42600_accel_read_event_config,
1146 .write_event_config = inv_icm42600_accel_write_event_config,
1147 .read_event_value = inv_icm42600_accel_read_event_value,
1148 .write_event_value = inv_icm42600_accel_write_event_value,
1149 };
1150
inv_icm42600_accel_init(struct inv_icm42600_state * st)1151 struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
1152 {
1153 struct device *dev = regmap_get_device(st->map);
1154 const char *name;
1155 struct inv_icm42600_sensor_state *accel_st;
1156 struct inv_sensors_timestamp_chip ts_chip;
1157 struct iio_dev *indio_dev;
1158 int ret;
1159
1160 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
1161 if (!name)
1162 return ERR_PTR(-ENOMEM);
1163
1164 indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st));
1165 if (!indio_dev)
1166 return ERR_PTR(-ENOMEM);
1167 accel_st = iio_priv(indio_dev);
1168
1169 switch (st->chip) {
1170 case INV_CHIP_ICM42686:
1171 accel_st->scales = inv_icm42686_accel_scale;
1172 accel_st->scales_len = ARRAY_SIZE(inv_icm42686_accel_scale);
1173 break;
1174 default:
1175 accel_st->scales = inv_icm42600_accel_scale;
1176 accel_st->scales_len = ARRAY_SIZE(inv_icm42600_accel_scale);
1177 break;
1178 }
1179 /* low-power by default at init */
1180 accel_st->power_mode = INV_ICM42600_SENSOR_MODE_LOW_POWER;
1181 accel_st->filter = INV_ICM42600_FILTER_AVG_16X;
1182
1183 /*
1184 * clock period is 32kHz (31250ns)
1185 * jitter is +/- 2% (20 per mille)
1186 */
1187 ts_chip.clock_period = 31250;
1188 ts_chip.jitter = 20;
1189 ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr);
1190 inv_sensors_timestamp_init(&accel_st->ts, &ts_chip);
1191
1192 iio_device_set_drvdata(indio_dev, st);
1193 indio_dev->name = name;
1194 indio_dev->info = &inv_icm42600_accel_info;
1195 indio_dev->modes = INDIO_DIRECT_MODE;
1196 indio_dev->channels = inv_icm42600_accel_channels;
1197 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
1198 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
1199
1200 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
1201 &inv_icm42600_buffer_ops);
1202 if (ret)
1203 return ERR_PTR(ret);
1204
1205 ret = devm_iio_device_register(dev, indio_dev);
1206 if (ret)
1207 return ERR_PTR(ret);
1208
1209 /* accel events are wakeup capable */
1210 ret = devm_device_init_wakeup(&indio_dev->dev);
1211 if (ret)
1212 return ERR_PTR(ret);
1213
1214 return indio_dev;
1215 }
1216
inv_icm42600_accel_parse_fifo(struct iio_dev * indio_dev)1217 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
1218 {
1219 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
1220 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
1221 struct inv_sensors_timestamp *ts = &accel_st->ts;
1222 ssize_t i, size;
1223 unsigned int no;
1224 const void *accel, *gyro, *timestamp;
1225 const int8_t *temp;
1226 unsigned int odr;
1227 int64_t ts_val;
1228 /* buffer is copied to userspace, zeroing it to avoid any data leak */
1229 struct inv_icm42600_accel_buffer buffer = { };
1230
1231 /* parse all fifo packets */
1232 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
1233 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
1234 &accel, &gyro, &temp, ×tamp, &odr);
1235 /* quit if error or FIFO is empty */
1236 if (size <= 0)
1237 return size;
1238
1239 /* skip packet if no accel data or data is invalid */
1240 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
1241 continue;
1242
1243 /* update odr */
1244 if (odr & INV_ICM42600_SENSOR_ACCEL)
1245 inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
1246 st->fifo.nb.total, no);
1247
1248 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
1249 /* convert 8 bits FIFO temperature in high resolution format */
1250 buffer.temp = temp ? (*temp * 64) : 0;
1251 ts_val = inv_sensors_timestamp_pop(ts);
1252 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
1253 }
1254
1255 return 0;
1256 }
1257