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 ret = iio_device_claim_direct_mode(indio_dev);
689 if (ret)
690 return ret;
691 ret = inv_icm42600_accel_read_sensor(indio_dev, chan, &data);
692 iio_device_release_direct_mode(indio_dev);
693 if (ret)
694 return ret;
695 *val = data;
696 return IIO_VAL_INT;
697 case IIO_CHAN_INFO_SCALE:
698 return inv_icm42600_accel_read_scale(indio_dev, val, val2);
699 case IIO_CHAN_INFO_SAMP_FREQ:
700 return inv_icm42600_accel_read_odr(st, val, val2);
701 case IIO_CHAN_INFO_CALIBBIAS:
702 return inv_icm42600_accel_read_offset(st, chan, val, val2);
703 default:
704 return -EINVAL;
705 }
706 }
707
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)708 static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
709 struct iio_chan_spec const *chan,
710 const int **vals,
711 int *type, int *length, long mask)
712 {
713 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
714
715 if (chan->type != IIO_ACCEL)
716 return -EINVAL;
717
718 switch (mask) {
719 case IIO_CHAN_INFO_SCALE:
720 *vals = accel_st->scales;
721 *type = IIO_VAL_INT_PLUS_NANO;
722 *length = accel_st->scales_len;
723 return IIO_AVAIL_LIST;
724 case IIO_CHAN_INFO_SAMP_FREQ:
725 *vals = inv_icm42600_accel_odr;
726 *type = IIO_VAL_INT_PLUS_MICRO;
727 *length = ARRAY_SIZE(inv_icm42600_accel_odr);
728 return IIO_AVAIL_LIST;
729 case IIO_CHAN_INFO_CALIBBIAS:
730 *vals = inv_icm42600_accel_calibbias;
731 *type = IIO_VAL_INT_PLUS_MICRO;
732 return IIO_AVAIL_RANGE;
733 default:
734 return -EINVAL;
735 }
736 }
737
inv_icm42600_accel_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)738 static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
739 struct iio_chan_spec const *chan,
740 int val, int val2, long mask)
741 {
742 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
743 int ret;
744
745 if (chan->type != IIO_ACCEL)
746 return -EINVAL;
747
748 switch (mask) {
749 case IIO_CHAN_INFO_SCALE:
750 ret = iio_device_claim_direct_mode(indio_dev);
751 if (ret)
752 return ret;
753 ret = inv_icm42600_accel_write_scale(indio_dev, val, val2);
754 iio_device_release_direct_mode(indio_dev);
755 return ret;
756 case IIO_CHAN_INFO_SAMP_FREQ:
757 return inv_icm42600_accel_write_odr(indio_dev, val, val2);
758 case IIO_CHAN_INFO_CALIBBIAS:
759 ret = iio_device_claim_direct_mode(indio_dev);
760 if (ret)
761 return ret;
762 ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
763 iio_device_release_direct_mode(indio_dev);
764 return ret;
765 default:
766 return -EINVAL;
767 }
768 }
769
inv_icm42600_accel_write_raw_get_fmt(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,long mask)770 static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
771 struct iio_chan_spec const *chan,
772 long mask)
773 {
774 if (chan->type != IIO_ACCEL)
775 return -EINVAL;
776
777 switch (mask) {
778 case IIO_CHAN_INFO_SCALE:
779 return IIO_VAL_INT_PLUS_NANO;
780 case IIO_CHAN_INFO_SAMP_FREQ:
781 return IIO_VAL_INT_PLUS_MICRO;
782 case IIO_CHAN_INFO_CALIBBIAS:
783 return IIO_VAL_INT_PLUS_MICRO;
784 default:
785 return -EINVAL;
786 }
787 }
788
inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev * indio_dev,unsigned int val)789 static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
790 unsigned int val)
791 {
792 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
793 int ret;
794
795 mutex_lock(&st->lock);
796
797 st->fifo.watermark.accel = val;
798 ret = inv_icm42600_buffer_update_watermark(st);
799
800 mutex_unlock(&st->lock);
801
802 return ret;
803 }
804
inv_icm42600_accel_hwfifo_flush(struct iio_dev * indio_dev,unsigned int count)805 static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
806 unsigned int count)
807 {
808 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
809 int ret;
810
811 if (count == 0)
812 return 0;
813
814 mutex_lock(&st->lock);
815
816 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
817 if (!ret)
818 ret = st->fifo.nb.accel;
819
820 mutex_unlock(&st->lock);
821
822 return ret;
823 }
824
825 static const struct iio_info inv_icm42600_accel_info = {
826 .read_raw = inv_icm42600_accel_read_raw,
827 .read_avail = inv_icm42600_accel_read_avail,
828 .write_raw = inv_icm42600_accel_write_raw,
829 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
830 .debugfs_reg_access = inv_icm42600_debugfs_reg,
831 .update_scan_mode = inv_icm42600_accel_update_scan_mode,
832 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
833 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
834 };
835
inv_icm42600_accel_init(struct inv_icm42600_state * st)836 struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
837 {
838 struct device *dev = regmap_get_device(st->map);
839 const char *name;
840 struct inv_icm42600_sensor_state *accel_st;
841 struct inv_sensors_timestamp_chip ts_chip;
842 struct iio_dev *indio_dev;
843 int ret;
844
845 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
846 if (!name)
847 return ERR_PTR(-ENOMEM);
848
849 indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st));
850 if (!indio_dev)
851 return ERR_PTR(-ENOMEM);
852 accel_st = iio_priv(indio_dev);
853
854 switch (st->chip) {
855 case INV_CHIP_ICM42686:
856 accel_st->scales = inv_icm42686_accel_scale;
857 accel_st->scales_len = ARRAY_SIZE(inv_icm42686_accel_scale);
858 break;
859 default:
860 accel_st->scales = inv_icm42600_accel_scale;
861 accel_st->scales_len = ARRAY_SIZE(inv_icm42600_accel_scale);
862 break;
863 }
864 /* low-power by default at init */
865 accel_st->power_mode = INV_ICM42600_SENSOR_MODE_LOW_POWER;
866 accel_st->filter = INV_ICM42600_FILTER_AVG_16X;
867
868 /*
869 * clock period is 32kHz (31250ns)
870 * jitter is +/- 2% (20 per mille)
871 */
872 ts_chip.clock_period = 31250;
873 ts_chip.jitter = 20;
874 ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr);
875 inv_sensors_timestamp_init(&accel_st->ts, &ts_chip);
876
877 iio_device_set_drvdata(indio_dev, st);
878 indio_dev->name = name;
879 indio_dev->info = &inv_icm42600_accel_info;
880 indio_dev->modes = INDIO_DIRECT_MODE;
881 indio_dev->channels = inv_icm42600_accel_channels;
882 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
883 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
884
885 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
886 &inv_icm42600_buffer_ops);
887 if (ret)
888 return ERR_PTR(ret);
889
890 ret = devm_iio_device_register(dev, indio_dev);
891 if (ret)
892 return ERR_PTR(ret);
893
894 return indio_dev;
895 }
896
inv_icm42600_accel_parse_fifo(struct iio_dev * indio_dev)897 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
898 {
899 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
900 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
901 struct inv_sensors_timestamp *ts = &accel_st->ts;
902 ssize_t i, size;
903 unsigned int no;
904 const void *accel, *gyro, *timestamp;
905 const int8_t *temp;
906 unsigned int odr;
907 int64_t ts_val;
908 struct inv_icm42600_accel_buffer buffer;
909
910 /* parse all fifo packets */
911 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
912 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
913 &accel, &gyro, &temp, ×tamp, &odr);
914 /* quit if error or FIFO is empty */
915 if (size <= 0)
916 return size;
917
918 /* skip packet if no accel data or data is invalid */
919 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
920 continue;
921
922 /* update odr */
923 if (odr & INV_ICM42600_SENSOR_ACCEL)
924 inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
925 st->fifo.nb.total, no);
926
927 /* buffer is copied to userspace, zeroing it to avoid any data leak */
928 memset(&buffer, 0, sizeof(buffer));
929 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
930 /* convert 8 bits FIFO temperature in high resolution format */
931 buffer.temp = temp ? (*temp * 64) : 0;
932 ts_val = inv_sensors_timestamp_pop(ts);
933 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
934 }
935
936 return 0;
937 }
938