1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * Copyright (C) 2020 Invensense, Inc.
4 */
5
6 #include <linux/kernel.h>
7 #include <linux/device.h>
8 #include <linux/mutex.h>
9 #include <linux/pm_runtime.h>
10 #include <linux/regmap.h>
11 #include <linux/delay.h>
12 #include <linux/math64.h>
13
14 #include <linux/iio/buffer.h>
15 #include <linux/iio/common/inv_sensors_timestamp.h>
16 #include <linux/iio/iio.h>
17 #include <linux/iio/kfifo_buf.h>
18
19 #include "inv_icm42600.h"
20 #include "inv_icm42600_temp.h"
21 #include "inv_icm42600_buffer.h"
22
23 #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
24 { \
25 .type = IIO_ACCEL, \
26 .modified = 1, \
27 .channel2 = _modifier, \
28 .info_mask_separate = \
29 BIT(IIO_CHAN_INFO_RAW) | \
30 BIT(IIO_CHAN_INFO_CALIBBIAS), \
31 .info_mask_shared_by_type = \
32 BIT(IIO_CHAN_INFO_SCALE), \
33 .info_mask_shared_by_type_available = \
34 BIT(IIO_CHAN_INFO_SCALE) | \
35 BIT(IIO_CHAN_INFO_CALIBBIAS), \
36 .info_mask_shared_by_all = \
37 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
38 .info_mask_shared_by_all_available = \
39 BIT(IIO_CHAN_INFO_SAMP_FREQ), \
40 .scan_index = _index, \
41 .scan_type = { \
42 .sign = 's', \
43 .realbits = 16, \
44 .storagebits = 16, \
45 .endianness = IIO_BE, \
46 }, \
47 .ext_info = _ext_info, \
48 }
49
50 enum inv_icm42600_accel_scan {
51 INV_ICM42600_ACCEL_SCAN_X,
52 INV_ICM42600_ACCEL_SCAN_Y,
53 INV_ICM42600_ACCEL_SCAN_Z,
54 INV_ICM42600_ACCEL_SCAN_TEMP,
55 INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
56 };
57
58 static const char * const inv_icm42600_accel_power_mode_items[] = {
59 "low-noise",
60 "low-power",
61 };
62 static const int inv_icm42600_accel_power_mode_values[] = {
63 INV_ICM42600_SENSOR_MODE_LOW_NOISE,
64 INV_ICM42600_SENSOR_MODE_LOW_POWER,
65 };
66 static const int inv_icm42600_accel_filter_values[] = {
67 INV_ICM42600_FILTER_BW_ODR_DIV_2,
68 INV_ICM42600_FILTER_AVG_16X,
69 };
70
inv_icm42600_accel_power_mode_set(struct iio_dev * indio_dev,const struct iio_chan_spec * chan,unsigned int idx)71 static int inv_icm42600_accel_power_mode_set(struct iio_dev *indio_dev,
72 const struct iio_chan_spec *chan,
73 unsigned int idx)
74 {
75 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
76 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
77 int power_mode, filter;
78
79 if (chan->type != IIO_ACCEL)
80 return -EINVAL;
81
82 if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values))
83 return -EINVAL;
84
85 if (iio_buffer_enabled(indio_dev))
86 return -EBUSY;
87
88 power_mode = inv_icm42600_accel_power_mode_values[idx];
89 filter = inv_icm42600_accel_filter_values[idx];
90
91 guard(mutex)(&st->lock);
92
93 /* prevent change if power mode is not supported by the ODR */
94 switch (power_mode) {
95 case INV_ICM42600_SENSOR_MODE_LOW_NOISE:
96 if (st->conf.accel.odr >= INV_ICM42600_ODR_6_25HZ_LP &&
97 st->conf.accel.odr <= INV_ICM42600_ODR_1_5625HZ_LP)
98 return -EPERM;
99 break;
100 case INV_ICM42600_SENSOR_MODE_LOW_POWER:
101 default:
102 if (st->conf.accel.odr <= INV_ICM42600_ODR_1KHZ_LN)
103 return -EPERM;
104 break;
105 }
106
107 accel_st->power_mode = power_mode;
108 accel_st->filter = filter;
109
110 return 0;
111 }
112
inv_icm42600_accel_power_mode_get(struct iio_dev * indio_dev,const struct iio_chan_spec * chan)113 static int inv_icm42600_accel_power_mode_get(struct iio_dev *indio_dev,
114 const struct iio_chan_spec *chan)
115 {
116 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
117 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
118 unsigned int idx;
119 int power_mode;
120
121 if (chan->type != IIO_ACCEL)
122 return -EINVAL;
123
124 guard(mutex)(&st->lock);
125
126 /* if sensor is on, returns actual power mode and not configured one */
127 switch (st->conf.accel.mode) {
128 case INV_ICM42600_SENSOR_MODE_LOW_POWER:
129 case INV_ICM42600_SENSOR_MODE_LOW_NOISE:
130 power_mode = st->conf.accel.mode;
131 break;
132 default:
133 power_mode = accel_st->power_mode;
134 break;
135 }
136
137 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_power_mode_values); ++idx) {
138 if (power_mode == inv_icm42600_accel_power_mode_values[idx])
139 break;
140 }
141 if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values))
142 return -EINVAL;
143
144 return idx;
145 }
146
147 static const struct iio_enum inv_icm42600_accel_power_mode_enum = {
148 .items = inv_icm42600_accel_power_mode_items,
149 .num_items = ARRAY_SIZE(inv_icm42600_accel_power_mode_items),
150 .set = inv_icm42600_accel_power_mode_set,
151 .get = inv_icm42600_accel_power_mode_get,
152 };
153
154 static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
155 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
156 IIO_ENUM_AVAILABLE("power_mode", IIO_SHARED_BY_TYPE,
157 &inv_icm42600_accel_power_mode_enum),
158 IIO_ENUM("power_mode", IIO_SHARED_BY_TYPE,
159 &inv_icm42600_accel_power_mode_enum),
160 { }
161 };
162
163 static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
164 INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X,
165 inv_icm42600_accel_ext_infos),
166 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y,
167 inv_icm42600_accel_ext_infos),
168 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
169 inv_icm42600_accel_ext_infos),
170 INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
171 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
172 };
173
174 /*
175 * IIO buffer data: size must be a power of 2 and timestamp aligned
176 * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp
177 */
178 struct inv_icm42600_accel_buffer {
179 struct inv_icm42600_fifo_sensor_data accel;
180 int16_t temp;
181 aligned_s64 timestamp;
182 };
183
184 #define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
185 (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
186 BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
187 BIT(INV_ICM42600_ACCEL_SCAN_Z))
188
189 #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
190
191 static const unsigned long inv_icm42600_accel_scan_masks[] = {
192 /* 3-axis accel + temperature */
193 INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
194 0,
195 };
196
197 /* enable accelerometer sensor and FIFO write */
inv_icm42600_accel_update_scan_mode(struct iio_dev * indio_dev,const unsigned long * scan_mask)198 static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
199 const unsigned long *scan_mask)
200 {
201 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
202 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
203 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
204 unsigned int fifo_en = 0;
205 unsigned int sleep_temp = 0;
206 unsigned int sleep_accel = 0;
207 unsigned int sleep;
208 int ret;
209
210 mutex_lock(&st->lock);
211
212 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
213 /* enable temp sensor */
214 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
215 if (ret)
216 goto out_unlock;
217 fifo_en |= INV_ICM42600_SENSOR_TEMP;
218 }
219
220 if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
221 /* enable accel sensor */
222 conf.mode = accel_st->power_mode;
223 conf.filter = accel_st->filter;
224 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
225 if (ret)
226 goto out_unlock;
227 fifo_en |= INV_ICM42600_SENSOR_ACCEL;
228 }
229
230 /* update data FIFO write */
231 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
232
233 out_unlock:
234 mutex_unlock(&st->lock);
235 /* sleep maximum required time */
236 sleep = max(sleep_accel, sleep_temp);
237 if (sleep)
238 msleep(sleep);
239 return ret;
240 }
241
inv_icm42600_accel_read_sensor(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int16_t * val)242 static int inv_icm42600_accel_read_sensor(struct iio_dev *indio_dev,
243 struct iio_chan_spec const *chan,
244 int16_t *val)
245 {
246 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
247 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
248 struct device *dev = regmap_get_device(st->map);
249 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
250 unsigned int reg;
251 __be16 *data;
252 int ret;
253
254 if (chan->type != IIO_ACCEL)
255 return -EINVAL;
256
257 switch (chan->channel2) {
258 case IIO_MOD_X:
259 reg = INV_ICM42600_REG_ACCEL_DATA_X;
260 break;
261 case IIO_MOD_Y:
262 reg = INV_ICM42600_REG_ACCEL_DATA_Y;
263 break;
264 case IIO_MOD_Z:
265 reg = INV_ICM42600_REG_ACCEL_DATA_Z;
266 break;
267 default:
268 return -EINVAL;
269 }
270
271 pm_runtime_get_sync(dev);
272 mutex_lock(&st->lock);
273
274 /* enable accel sensor */
275 conf.mode = accel_st->power_mode;
276 conf.filter = accel_st->filter;
277 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
278 if (ret)
279 goto exit;
280
281 /* read accel register data */
282 data = (__be16 *)&st->buffer[0];
283 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
284 if (ret)
285 goto exit;
286
287 *val = (int16_t)be16_to_cpup(data);
288 if (*val == INV_ICM42600_DATA_INVALID)
289 ret = -EINVAL;
290 exit:
291 mutex_unlock(&st->lock);
292 pm_runtime_mark_last_busy(dev);
293 pm_runtime_put_autosuspend(dev);
294 return ret;
295 }
296
297 /* IIO format int + nano */
298 static const int inv_icm42600_accel_scale[] = {
299 /* +/- 16G => 0.004788403 m/s-2 */
300 [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
301 [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
302 /* +/- 8G => 0.002394202 m/s-2 */
303 [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
304 [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
305 /* +/- 4G => 0.001197101 m/s-2 */
306 [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
307 [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
308 /* +/- 2G => 0.000598550 m/s-2 */
309 [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
310 [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
311 };
312 static const int inv_icm42686_accel_scale[] = {
313 /* +/- 32G => 0.009576807 m/s-2 */
314 [2 * INV_ICM42686_ACCEL_FS_32G] = 0,
315 [2 * INV_ICM42686_ACCEL_FS_32G + 1] = 9576807,
316 /* +/- 16G => 0.004788403 m/s-2 */
317 [2 * INV_ICM42686_ACCEL_FS_16G] = 0,
318 [2 * INV_ICM42686_ACCEL_FS_16G + 1] = 4788403,
319 /* +/- 8G => 0.002394202 m/s-2 */
320 [2 * INV_ICM42686_ACCEL_FS_8G] = 0,
321 [2 * INV_ICM42686_ACCEL_FS_8G + 1] = 2394202,
322 /* +/- 4G => 0.001197101 m/s-2 */
323 [2 * INV_ICM42686_ACCEL_FS_4G] = 0,
324 [2 * INV_ICM42686_ACCEL_FS_4G + 1] = 1197101,
325 /* +/- 2G => 0.000598550 m/s-2 */
326 [2 * INV_ICM42686_ACCEL_FS_2G] = 0,
327 [2 * INV_ICM42686_ACCEL_FS_2G + 1] = 598550,
328 };
329
inv_icm42600_accel_read_scale(struct iio_dev * indio_dev,int * val,int * val2)330 static int inv_icm42600_accel_read_scale(struct iio_dev *indio_dev,
331 int *val, int *val2)
332 {
333 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
334 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
335 unsigned int idx;
336
337 idx = st->conf.accel.fs;
338
339 *val = accel_st->scales[2 * idx];
340 *val2 = accel_st->scales[2 * idx + 1];
341 return IIO_VAL_INT_PLUS_NANO;
342 }
343
inv_icm42600_accel_write_scale(struct iio_dev * indio_dev,int val,int val2)344 static int inv_icm42600_accel_write_scale(struct iio_dev *indio_dev,
345 int val, int val2)
346 {
347 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
348 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
349 struct device *dev = regmap_get_device(st->map);
350 unsigned int idx;
351 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
352 int ret;
353
354 for (idx = 0; idx < accel_st->scales_len; idx += 2) {
355 if (val == accel_st->scales[idx] &&
356 val2 == accel_st->scales[idx + 1])
357 break;
358 }
359 if (idx >= accel_st->scales_len)
360 return -EINVAL;
361
362 conf.fs = idx / 2;
363
364 pm_runtime_get_sync(dev);
365 mutex_lock(&st->lock);
366
367 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
368
369 mutex_unlock(&st->lock);
370 pm_runtime_mark_last_busy(dev);
371 pm_runtime_put_autosuspend(dev);
372
373 return ret;
374 }
375
376 /* IIO format int + micro */
377 static const int inv_icm42600_accel_odr[] = {
378 /* 1.5625Hz */
379 1, 562500,
380 /* 3.125Hz */
381 3, 125000,
382 /* 6.25Hz */
383 6, 250000,
384 /* 12.5Hz */
385 12, 500000,
386 /* 25Hz */
387 25, 0,
388 /* 50Hz */
389 50, 0,
390 /* 100Hz */
391 100, 0,
392 /* 200Hz */
393 200, 0,
394 /* 1kHz */
395 1000, 0,
396 /* 2kHz */
397 2000, 0,
398 /* 4kHz */
399 4000, 0,
400 };
401
402 static const int inv_icm42600_accel_odr_conv[] = {
403 INV_ICM42600_ODR_1_5625HZ_LP,
404 INV_ICM42600_ODR_3_125HZ_LP,
405 INV_ICM42600_ODR_6_25HZ_LP,
406 INV_ICM42600_ODR_12_5HZ,
407 INV_ICM42600_ODR_25HZ,
408 INV_ICM42600_ODR_50HZ,
409 INV_ICM42600_ODR_100HZ,
410 INV_ICM42600_ODR_200HZ,
411 INV_ICM42600_ODR_1KHZ_LN,
412 INV_ICM42600_ODR_2KHZ_LN,
413 INV_ICM42600_ODR_4KHZ_LN,
414 };
415
inv_icm42600_accel_read_odr(struct inv_icm42600_state * st,int * val,int * val2)416 static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
417 int *val, int *val2)
418 {
419 unsigned int odr;
420 unsigned int i;
421
422 odr = st->conf.accel.odr;
423
424 for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
425 if (inv_icm42600_accel_odr_conv[i] == odr)
426 break;
427 }
428 if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
429 return -EINVAL;
430
431 *val = inv_icm42600_accel_odr[2 * i];
432 *val2 = inv_icm42600_accel_odr[2 * i + 1];
433
434 return IIO_VAL_INT_PLUS_MICRO;
435 }
436
inv_icm42600_accel_write_odr(struct iio_dev * indio_dev,int val,int val2)437 static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
438 int val, int val2)
439 {
440 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
441 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
442 struct inv_sensors_timestamp *ts = &accel_st->ts;
443 struct device *dev = regmap_get_device(st->map);
444 unsigned int idx;
445 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
446 int ret;
447
448 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
449 if (val == inv_icm42600_accel_odr[idx] &&
450 val2 == inv_icm42600_accel_odr[idx + 1])
451 break;
452 }
453 if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
454 return -EINVAL;
455
456 conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
457
458 pm_runtime_get_sync(dev);
459 mutex_lock(&st->lock);
460
461 ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
462 iio_buffer_enabled(indio_dev));
463 if (ret)
464 goto out_unlock;
465
466 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
467 if (ret)
468 goto out_unlock;
469 inv_icm42600_buffer_update_fifo_period(st);
470 inv_icm42600_buffer_update_watermark(st);
471
472 out_unlock:
473 mutex_unlock(&st->lock);
474 pm_runtime_mark_last_busy(dev);
475 pm_runtime_put_autosuspend(dev);
476
477 return ret;
478 }
479
480 /*
481 * Calibration bias values, IIO range format int + micro.
482 * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
483 */
484 static int inv_icm42600_accel_calibbias[] = {
485 -10, 42010, /* min: -10.042010 m/s² */
486 0, 4903, /* step: 0.004903 m/s² */
487 10, 37106, /* max: 10.037106 m/s² */
488 };
489
inv_icm42600_accel_read_offset(struct inv_icm42600_state * st,struct iio_chan_spec const * chan,int * val,int * val2)490 static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
491 struct iio_chan_spec const *chan,
492 int *val, int *val2)
493 {
494 struct device *dev = regmap_get_device(st->map);
495 int64_t val64;
496 int32_t bias;
497 unsigned int reg;
498 int16_t offset;
499 uint8_t data[2];
500 int ret;
501
502 if (chan->type != IIO_ACCEL)
503 return -EINVAL;
504
505 switch (chan->channel2) {
506 case IIO_MOD_X:
507 reg = INV_ICM42600_REG_OFFSET_USER4;
508 break;
509 case IIO_MOD_Y:
510 reg = INV_ICM42600_REG_OFFSET_USER6;
511 break;
512 case IIO_MOD_Z:
513 reg = INV_ICM42600_REG_OFFSET_USER7;
514 break;
515 default:
516 return -EINVAL;
517 }
518
519 pm_runtime_get_sync(dev);
520 mutex_lock(&st->lock);
521
522 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
523 memcpy(data, st->buffer, sizeof(data));
524
525 mutex_unlock(&st->lock);
526 pm_runtime_mark_last_busy(dev);
527 pm_runtime_put_autosuspend(dev);
528 if (ret)
529 return ret;
530
531 /* 12 bits signed value */
532 switch (chan->channel2) {
533 case IIO_MOD_X:
534 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
535 break;
536 case IIO_MOD_Y:
537 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
538 break;
539 case IIO_MOD_Z:
540 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
541 break;
542 default:
543 return -EINVAL;
544 }
545
546 /*
547 * convert raw offset to g then to m/s²
548 * 12 bits signed raw step 0.5mg to g: 5 / 10000
549 * g to m/s²: 9.806650
550 * result in micro (1000000)
551 * (offset * 5 * 9.806650 * 1000000) / 10000
552 */
553 val64 = (int64_t)offset * 5LL * 9806650LL;
554 /* for rounding, add + or - divisor (10000) divided by 2 */
555 if (val64 >= 0)
556 val64 += 10000LL / 2LL;
557 else
558 val64 -= 10000LL / 2LL;
559 bias = div_s64(val64, 10000L);
560 *val = bias / 1000000L;
561 *val2 = bias % 1000000L;
562
563 return IIO_VAL_INT_PLUS_MICRO;
564 }
565
inv_icm42600_accel_write_offset(struct inv_icm42600_state * st,struct iio_chan_spec const * chan,int val,int val2)566 static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
567 struct iio_chan_spec const *chan,
568 int val, int val2)
569 {
570 struct device *dev = regmap_get_device(st->map);
571 int64_t val64;
572 int32_t min, max;
573 unsigned int reg, regval;
574 int16_t offset;
575 int ret;
576
577 if (chan->type != IIO_ACCEL)
578 return -EINVAL;
579
580 switch (chan->channel2) {
581 case IIO_MOD_X:
582 reg = INV_ICM42600_REG_OFFSET_USER4;
583 break;
584 case IIO_MOD_Y:
585 reg = INV_ICM42600_REG_OFFSET_USER6;
586 break;
587 case IIO_MOD_Z:
588 reg = INV_ICM42600_REG_OFFSET_USER7;
589 break;
590 default:
591 return -EINVAL;
592 }
593
594 /* inv_icm42600_accel_calibbias: min - step - max in micro */
595 min = inv_icm42600_accel_calibbias[0] * 1000000L +
596 inv_icm42600_accel_calibbias[1];
597 max = inv_icm42600_accel_calibbias[4] * 1000000L +
598 inv_icm42600_accel_calibbias[5];
599 val64 = (int64_t)val * 1000000LL + (int64_t)val2;
600 if (val64 < min || val64 > max)
601 return -EINVAL;
602
603 /*
604 * convert m/s² to g then to raw value
605 * m/s² to g: 1 / 9.806650
606 * g to raw 12 bits signed, step 0.5mg: 10000 / 5
607 * val in micro (1000000)
608 * val * 10000 / (9.806650 * 1000000 * 5)
609 */
610 val64 = val64 * 10000LL;
611 /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */
612 if (val64 >= 0)
613 val64 += 9806650 * 5 / 2;
614 else
615 val64 -= 9806650 * 5 / 2;
616 offset = div_s64(val64, 9806650 * 5);
617
618 /* clamp value limited to 12 bits signed */
619 if (offset < -2048)
620 offset = -2048;
621 else if (offset > 2047)
622 offset = 2047;
623
624 pm_runtime_get_sync(dev);
625 mutex_lock(&st->lock);
626
627 switch (chan->channel2) {
628 case IIO_MOD_X:
629 /* OFFSET_USER4 register is shared */
630 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
631 ®val);
632 if (ret)
633 goto out_unlock;
634 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
635 st->buffer[1] = offset & 0xFF;
636 break;
637 case IIO_MOD_Y:
638 /* OFFSET_USER7 register is shared */
639 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
640 ®val);
641 if (ret)
642 goto out_unlock;
643 st->buffer[0] = offset & 0xFF;
644 st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
645 break;
646 case IIO_MOD_Z:
647 /* OFFSET_USER7 register is shared */
648 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
649 ®val);
650 if (ret)
651 goto out_unlock;
652 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
653 st->buffer[1] = offset & 0xFF;
654 break;
655 default:
656 ret = -EINVAL;
657 goto out_unlock;
658 }
659
660 ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
661
662 out_unlock:
663 mutex_unlock(&st->lock);
664 pm_runtime_mark_last_busy(dev);
665 pm_runtime_put_autosuspend(dev);
666 return ret;
667 }
668
inv_icm42600_accel_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)669 static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
670 struct iio_chan_spec const *chan,
671 int *val, int *val2, long mask)
672 {
673 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
674 int16_t data;
675 int ret;
676
677 switch (chan->type) {
678 case IIO_ACCEL:
679 break;
680 case IIO_TEMP:
681 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
682 default:
683 return -EINVAL;
684 }
685
686 switch (mask) {
687 case IIO_CHAN_INFO_RAW:
688 if (!iio_device_claim_direct(indio_dev))
689 return -EBUSY;
690 ret = inv_icm42600_accel_read_sensor(indio_dev, chan, &data);
691 iio_device_release_direct(indio_dev);
692 if (ret)
693 return ret;
694 *val = data;
695 return IIO_VAL_INT;
696 case IIO_CHAN_INFO_SCALE:
697 return inv_icm42600_accel_read_scale(indio_dev, val, val2);
698 case IIO_CHAN_INFO_SAMP_FREQ:
699 return inv_icm42600_accel_read_odr(st, val, val2);
700 case IIO_CHAN_INFO_CALIBBIAS:
701 return inv_icm42600_accel_read_offset(st, chan, val, val2);
702 default:
703 return -EINVAL;
704 }
705 }
706
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)707 static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
708 struct iio_chan_spec const *chan,
709 const int **vals,
710 int *type, int *length, long mask)
711 {
712 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
713
714 if (chan->type != IIO_ACCEL)
715 return -EINVAL;
716
717 switch (mask) {
718 case IIO_CHAN_INFO_SCALE:
719 *vals = accel_st->scales;
720 *type = IIO_VAL_INT_PLUS_NANO;
721 *length = accel_st->scales_len;
722 return IIO_AVAIL_LIST;
723 case IIO_CHAN_INFO_SAMP_FREQ:
724 *vals = inv_icm42600_accel_odr;
725 *type = IIO_VAL_INT_PLUS_MICRO;
726 *length = ARRAY_SIZE(inv_icm42600_accel_odr);
727 return IIO_AVAIL_LIST;
728 case IIO_CHAN_INFO_CALIBBIAS:
729 *vals = inv_icm42600_accel_calibbias;
730 *type = IIO_VAL_INT_PLUS_MICRO;
731 return IIO_AVAIL_RANGE;
732 default:
733 return -EINVAL;
734 }
735 }
736
inv_icm42600_accel_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)737 static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
738 struct iio_chan_spec const *chan,
739 int val, int val2, long mask)
740 {
741 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
742 int ret;
743
744 if (chan->type != IIO_ACCEL)
745 return -EINVAL;
746
747 switch (mask) {
748 case IIO_CHAN_INFO_SCALE:
749 if (!iio_device_claim_direct(indio_dev))
750 return -EBUSY;
751 ret = inv_icm42600_accel_write_scale(indio_dev, val, val2);
752 iio_device_release_direct(indio_dev);
753 return ret;
754 case IIO_CHAN_INFO_SAMP_FREQ:
755 return inv_icm42600_accel_write_odr(indio_dev, val, val2);
756 case IIO_CHAN_INFO_CALIBBIAS:
757 if (!iio_device_claim_direct(indio_dev))
758 return -EBUSY;
759 ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
760 iio_device_release_direct(indio_dev);
761 return ret;
762 default:
763 return -EINVAL;
764 }
765 }
766
inv_icm42600_accel_write_raw_get_fmt(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,long mask)767 static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
768 struct iio_chan_spec const *chan,
769 long mask)
770 {
771 if (chan->type != IIO_ACCEL)
772 return -EINVAL;
773
774 switch (mask) {
775 case IIO_CHAN_INFO_SCALE:
776 return IIO_VAL_INT_PLUS_NANO;
777 case IIO_CHAN_INFO_SAMP_FREQ:
778 return IIO_VAL_INT_PLUS_MICRO;
779 case IIO_CHAN_INFO_CALIBBIAS:
780 return IIO_VAL_INT_PLUS_MICRO;
781 default:
782 return -EINVAL;
783 }
784 }
785
inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev * indio_dev,unsigned int val)786 static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
787 unsigned int val)
788 {
789 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
790 int ret;
791
792 mutex_lock(&st->lock);
793
794 st->fifo.watermark.accel = val;
795 ret = inv_icm42600_buffer_update_watermark(st);
796
797 mutex_unlock(&st->lock);
798
799 return ret;
800 }
801
inv_icm42600_accel_hwfifo_flush(struct iio_dev * indio_dev,unsigned int count)802 static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
803 unsigned int count)
804 {
805 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
806 int ret;
807
808 if (count == 0)
809 return 0;
810
811 mutex_lock(&st->lock);
812
813 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
814 if (!ret)
815 ret = st->fifo.nb.accel;
816
817 mutex_unlock(&st->lock);
818
819 return ret;
820 }
821
822 static const struct iio_info inv_icm42600_accel_info = {
823 .read_raw = inv_icm42600_accel_read_raw,
824 .read_avail = inv_icm42600_accel_read_avail,
825 .write_raw = inv_icm42600_accel_write_raw,
826 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
827 .debugfs_reg_access = inv_icm42600_debugfs_reg,
828 .update_scan_mode = inv_icm42600_accel_update_scan_mode,
829 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
830 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
831 };
832
inv_icm42600_accel_init(struct inv_icm42600_state * st)833 struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
834 {
835 struct device *dev = regmap_get_device(st->map);
836 const char *name;
837 struct inv_icm42600_sensor_state *accel_st;
838 struct inv_sensors_timestamp_chip ts_chip;
839 struct iio_dev *indio_dev;
840 int ret;
841
842 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
843 if (!name)
844 return ERR_PTR(-ENOMEM);
845
846 indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st));
847 if (!indio_dev)
848 return ERR_PTR(-ENOMEM);
849 accel_st = iio_priv(indio_dev);
850
851 switch (st->chip) {
852 case INV_CHIP_ICM42686:
853 accel_st->scales = inv_icm42686_accel_scale;
854 accel_st->scales_len = ARRAY_SIZE(inv_icm42686_accel_scale);
855 break;
856 default:
857 accel_st->scales = inv_icm42600_accel_scale;
858 accel_st->scales_len = ARRAY_SIZE(inv_icm42600_accel_scale);
859 break;
860 }
861 /* low-power by default at init */
862 accel_st->power_mode = INV_ICM42600_SENSOR_MODE_LOW_POWER;
863 accel_st->filter = INV_ICM42600_FILTER_AVG_16X;
864
865 /*
866 * clock period is 32kHz (31250ns)
867 * jitter is +/- 2% (20 per mille)
868 */
869 ts_chip.clock_period = 31250;
870 ts_chip.jitter = 20;
871 ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr);
872 inv_sensors_timestamp_init(&accel_st->ts, &ts_chip);
873
874 iio_device_set_drvdata(indio_dev, st);
875 indio_dev->name = name;
876 indio_dev->info = &inv_icm42600_accel_info;
877 indio_dev->modes = INDIO_DIRECT_MODE;
878 indio_dev->channels = inv_icm42600_accel_channels;
879 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
880 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
881
882 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
883 &inv_icm42600_buffer_ops);
884 if (ret)
885 return ERR_PTR(ret);
886
887 ret = devm_iio_device_register(dev, indio_dev);
888 if (ret)
889 return ERR_PTR(ret);
890
891 return indio_dev;
892 }
893
inv_icm42600_accel_parse_fifo(struct iio_dev * indio_dev)894 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
895 {
896 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
897 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
898 struct inv_sensors_timestamp *ts = &accel_st->ts;
899 ssize_t i, size;
900 unsigned int no;
901 const void *accel, *gyro, *timestamp;
902 const int8_t *temp;
903 unsigned int odr;
904 int64_t ts_val;
905 struct inv_icm42600_accel_buffer buffer;
906
907 /* parse all fifo packets */
908 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
909 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
910 &accel, &gyro, &temp, ×tamp, &odr);
911 /* quit if error or FIFO is empty */
912 if (size <= 0)
913 return size;
914
915 /* skip packet if no accel data or data is invalid */
916 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
917 continue;
918
919 /* update odr */
920 if (odr & INV_ICM42600_SENSOR_ACCEL)
921 inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
922 st->fifo.nb.total, no);
923
924 /* buffer is copied to userspace, zeroing it to avoid any data leak */
925 memset(&buffer, 0, sizeof(buffer));
926 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
927 /* convert 8 bits FIFO temperature in high resolution format */
928 buffer.temp = temp ? (*temp * 64) : 0;
929 ts_val = inv_sensors_timestamp_pop(ts);
930 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
931 }
932
933 return 0;
934 }
935