?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
printf: Remove unused 'bprintf'
[drm/drm-misc.git] / drivers / iio / imu / inv_icm42600 / inv_icm42600_accel.c
blob56ac198142500a2e1fc40b62cdd465cc736d8bf0
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
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
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 int64_t timestamp __aligned(8);
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 */
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_sensors_timestamp *ts = &accel_st->ts;
204 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
205 unsigned int fifo_en = 0;
206 unsigned int sleep_temp = 0;
207 unsigned int sleep_accel = 0;
208 unsigned int sleep;
209 int ret;
210
211 mutex_lock(&st->lock);
212
213 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
214 /* enable temp sensor */
215 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
216 if (ret)
217 goto out_unlock;
218 fifo_en |= INV_ICM42600_SENSOR_TEMP;
219 }
220
221 if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
222 /* enable accel sensor */
223 conf.mode = accel_st->power_mode;
224 conf.filter = accel_st->filter;
225 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
226 if (ret)
227 goto out_unlock;
228 fifo_en |= INV_ICM42600_SENSOR_ACCEL;
229 }
230
231 /* update data FIFO write */
232 inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
233 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
234
235 out_unlock:
236 mutex_unlock(&st->lock);
237 /* sleep maximum required time */
238 sleep = max(sleep_accel, sleep_temp);
239 if (sleep)
240 msleep(sleep);
241 return ret;
242 }
243
244 static int inv_icm42600_accel_read_sensor(struct iio_dev *indio_dev,
245 struct iio_chan_spec const *chan,
246 int16_t *val)
247 {
248 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
249 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
250 struct device *dev = regmap_get_device(st->map);
251 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
252 unsigned int reg;
253 __be16 *data;
254 int ret;
255
256 if (chan->type != IIO_ACCEL)
257 return -EINVAL;
258
259 switch (chan->channel2) {
260 case IIO_MOD_X:
261 reg = INV_ICM42600_REG_ACCEL_DATA_X;
262 break;
263 case IIO_MOD_Y:
264 reg = INV_ICM42600_REG_ACCEL_DATA_Y;
265 break;
266 case IIO_MOD_Z:
267 reg = INV_ICM42600_REG_ACCEL_DATA_Z;
268 break;
269 default:
270 return -EINVAL;
271 }
272
273 pm_runtime_get_sync(dev);
274 mutex_lock(&st->lock);
275
276 /* enable accel sensor */
277 conf.mode = accel_st->power_mode;
278 conf.filter = accel_st->filter;
279 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
280 if (ret)
281 goto exit;
282
283 /* read accel register data */
284 data = (__be16 *)&st->buffer[0];
285 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
286 if (ret)
287 goto exit;
288
289 *val = (int16_t)be16_to_cpup(data);
290 if (*val == INV_ICM42600_DATA_INVALID)
291 ret = -EINVAL;
292 exit:
293 mutex_unlock(&st->lock);
294 pm_runtime_mark_last_busy(dev);
295 pm_runtime_put_autosuspend(dev);
296 return ret;
297 }
298
299 /* IIO format int + nano */
300 static const int inv_icm42600_accel_scale[] = {
301 /* +/- 16G => 0.004788403 m/s-2 */
302 [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
303 [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
304 /* +/- 8G => 0.002394202 m/s-2 */
305 [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
306 [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
307 /* +/- 4G => 0.001197101 m/s-2 */
308 [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
309 [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
310 /* +/- 2G => 0.000598550 m/s-2 */
311 [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
312 [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
313 };
314 static const int inv_icm42686_accel_scale[] = {
315 /* +/- 32G => 0.009576807 m/s-2 */
316 [2 * INV_ICM42686_ACCEL_FS_32G] = 0,
317 [2 * INV_ICM42686_ACCEL_FS_32G + 1] = 9576807,
318 /* +/- 16G => 0.004788403 m/s-2 */
319 [2 * INV_ICM42686_ACCEL_FS_16G] = 0,
320 [2 * INV_ICM42686_ACCEL_FS_16G + 1] = 4788403,
321 /* +/- 8G => 0.002394202 m/s-2 */
322 [2 * INV_ICM42686_ACCEL_FS_8G] = 0,
323 [2 * INV_ICM42686_ACCEL_FS_8G + 1] = 2394202,
324 /* +/- 4G => 0.001197101 m/s-2 */
325 [2 * INV_ICM42686_ACCEL_FS_4G] = 0,
326 [2 * INV_ICM42686_ACCEL_FS_4G + 1] = 1197101,
327 /* +/- 2G => 0.000598550 m/s-2 */
328 [2 * INV_ICM42686_ACCEL_FS_2G] = 0,
329 [2 * INV_ICM42686_ACCEL_FS_2G + 1] = 598550,
330 };
331
332 static int inv_icm42600_accel_read_scale(struct iio_dev *indio_dev,
333 int *val, int *val2)
334 {
335 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
336 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
337 unsigned int idx;
338
339 idx = st->conf.accel.fs;
340
341 *val = accel_st->scales[2 * idx];
342 *val2 = accel_st->scales[2 * idx + 1];
343 return IIO_VAL_INT_PLUS_NANO;
344 }
345
346 static int inv_icm42600_accel_write_scale(struct iio_dev *indio_dev,
347 int val, int val2)
348 {
349 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
350 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
351 struct device *dev = regmap_get_device(st->map);
352 unsigned int idx;
353 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
354 int ret;
355
356 for (idx = 0; idx < accel_st->scales_len; idx += 2) {
357 if (val == accel_st->scales[idx] &&
358 val2 == accel_st->scales[idx + 1])
359 break;
360 }
361 if (idx >= accel_st->scales_len)
362 return -EINVAL;
363
364 conf.fs = idx / 2;
365
366 pm_runtime_get_sync(dev);
367 mutex_lock(&st->lock);
368
369 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
370
371 mutex_unlock(&st->lock);
372 pm_runtime_mark_last_busy(dev);
373 pm_runtime_put_autosuspend(dev);
374
375 return ret;
376 }
377
378 /* IIO format int + micro */
379 static const int inv_icm42600_accel_odr[] = {
380 /* 1.5625Hz */
381 1, 562500,
382 /* 3.125Hz */
383 3, 125000,
384 /* 6.25Hz */
385 6, 250000,
386 /* 12.5Hz */
387 12, 500000,
388 /* 25Hz */
389 25, 0,
390 /* 50Hz */
391 50, 0,
392 /* 100Hz */
393 100, 0,
394 /* 200Hz */
395 200, 0,
396 /* 1kHz */
397 1000, 0,
398 /* 2kHz */
399 2000, 0,
400 /* 4kHz */
401 4000, 0,
402 };
403
404 static const int inv_icm42600_accel_odr_conv[] = {
405 INV_ICM42600_ODR_1_5625HZ_LP,
406 INV_ICM42600_ODR_3_125HZ_LP,
407 INV_ICM42600_ODR_6_25HZ_LP,
408 INV_ICM42600_ODR_12_5HZ,
409 INV_ICM42600_ODR_25HZ,
410 INV_ICM42600_ODR_50HZ,
411 INV_ICM42600_ODR_100HZ,
412 INV_ICM42600_ODR_200HZ,
413 INV_ICM42600_ODR_1KHZ_LN,
414 INV_ICM42600_ODR_2KHZ_LN,
415 INV_ICM42600_ODR_4KHZ_LN,
416 };
417
418 static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
419 int *val, int *val2)
420 {
421 unsigned int odr;
422 unsigned int i;
423
424 odr = st->conf.accel.odr;
425
426 for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
427 if (inv_icm42600_accel_odr_conv[i] == odr)
428 break;
429 }
430 if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
431 return -EINVAL;
432
433 *val = inv_icm42600_accel_odr[2 * i];
434 *val2 = inv_icm42600_accel_odr[2 * i + 1];
435
436 return IIO_VAL_INT_PLUS_MICRO;
437 }
438
439 static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
440 int val, int val2)
441 {
442 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
443 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
444 struct inv_sensors_timestamp *ts = &accel_st->ts;
445 struct device *dev = regmap_get_device(st->map);
446 unsigned int idx;
447 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
448 int ret;
449
450 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
451 if (val == inv_icm42600_accel_odr[idx] &&
452 val2 == inv_icm42600_accel_odr[idx + 1])
453 break;
454 }
455 if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
456 return -EINVAL;
457
458 conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
459
460 pm_runtime_get_sync(dev);
461 mutex_lock(&st->lock);
462
463 ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
464 iio_buffer_enabled(indio_dev));
465 if (ret)
466 goto out_unlock;
467
468 ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
469 if (ret)
470 goto out_unlock;
471 inv_icm42600_buffer_update_fifo_period(st);
472 inv_icm42600_buffer_update_watermark(st);
473
474 out_unlock:
475 mutex_unlock(&st->lock);
476 pm_runtime_mark_last_busy(dev);
477 pm_runtime_put_autosuspend(dev);
478
479 return ret;
480 }
481
482 /*
483 * Calibration bias values, IIO range format int + micro.
484 * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
485 */
486 static int inv_icm42600_accel_calibbias[] = {
487 -10, 42010, /* min: -10.042010 m/s² */
488 0, 4903, /* step: 0.004903 m/s² */
489 10, 37106, /* max: 10.037106 m/s² */
490 };
491
492 static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
493 struct iio_chan_spec const *chan,
494 int *val, int *val2)
495 {
496 struct device *dev = regmap_get_device(st->map);
497 int64_t val64;
498 int32_t bias;
499 unsigned int reg;
500 int16_t offset;
501 uint8_t data[2];
502 int ret;
503
504 if (chan->type != IIO_ACCEL)
505 return -EINVAL;
506
507 switch (chan->channel2) {
508 case IIO_MOD_X:
509 reg = INV_ICM42600_REG_OFFSET_USER4;
510 break;
511 case IIO_MOD_Y:
512 reg = INV_ICM42600_REG_OFFSET_USER6;
513 break;
514 case IIO_MOD_Z:
515 reg = INV_ICM42600_REG_OFFSET_USER7;
516 break;
517 default:
518 return -EINVAL;
519 }
520
521 pm_runtime_get_sync(dev);
522 mutex_lock(&st->lock);
523
524 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
525 memcpy(data, st->buffer, sizeof(data));
526
527 mutex_unlock(&st->lock);
528 pm_runtime_mark_last_busy(dev);
529 pm_runtime_put_autosuspend(dev);
530 if (ret)
531 return ret;
532
533 /* 12 bits signed value */
534 switch (chan->channel2) {
535 case IIO_MOD_X:
536 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
537 break;
538 case IIO_MOD_Y:
539 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
540 break;
541 case IIO_MOD_Z:
542 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
543 break;
544 default:
545 return -EINVAL;
546 }
547
548 /*
549 * convert raw offset to g then to m/s²
550 * 12 bits signed raw step 0.5mg to g: 5 / 10000
551 * g to m/s²: 9.806650
552 * result in micro (1000000)
553 * (offset * 5 * 9.806650 * 1000000) / 10000
554 */
555 val64 = (int64_t)offset * 5LL * 9806650LL;
556 /* for rounding, add + or - divisor (10000) divided by 2 */
557 if (val64 >= 0)
558 val64 += 10000LL / 2LL;
559 else
560 val64 -= 10000LL / 2LL;
561 bias = div_s64(val64, 10000L);
562 *val = bias / 1000000L;
563 *val2 = bias % 1000000L;
564
565 return IIO_VAL_INT_PLUS_MICRO;
566 }
567
568 static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
569 struct iio_chan_spec const *chan,
570 int val, int val2)
571 {
572 struct device *dev = regmap_get_device(st->map);
573 int64_t val64;
574 int32_t min, max;
575 unsigned int reg, regval;
576 int16_t offset;
577 int ret;
578
579 if (chan->type != IIO_ACCEL)
580 return -EINVAL;
581
582 switch (chan->channel2) {
583 case IIO_MOD_X:
584 reg = INV_ICM42600_REG_OFFSET_USER4;
585 break;
586 case IIO_MOD_Y:
587 reg = INV_ICM42600_REG_OFFSET_USER6;
588 break;
589 case IIO_MOD_Z:
590 reg = INV_ICM42600_REG_OFFSET_USER7;
591 break;
592 default:
593 return -EINVAL;
594 }
595
596 /* inv_icm42600_accel_calibbias: min - step - max in micro */
597 min = inv_icm42600_accel_calibbias[0] * 1000000L +
598 inv_icm42600_accel_calibbias[1];
599 max = inv_icm42600_accel_calibbias[4] * 1000000L +
600 inv_icm42600_accel_calibbias[5];
601 val64 = (int64_t)val * 1000000LL + (int64_t)val2;
602 if (val64 < min || val64 > max)
603 return -EINVAL;
604
605 /*
606 * convert m/s² to g then to raw value
607 * m/s² to g: 1 / 9.806650
608 * g to raw 12 bits signed, step 0.5mg: 10000 / 5
609 * val in micro (1000000)
610 * val * 10000 / (9.806650 * 1000000 * 5)
611 */
612 val64 = val64 * 10000LL;
613 /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */
614 if (val64 >= 0)
615 val64 += 9806650 * 5 / 2;
616 else
617 val64 -= 9806650 * 5 / 2;
618 offset = div_s64(val64, 9806650 * 5);
619
620 /* clamp value limited to 12 bits signed */
621 if (offset < -2048)
622 offset = -2048;
623 else if (offset > 2047)
624 offset = 2047;
625
626 pm_runtime_get_sync(dev);
627 mutex_lock(&st->lock);
628
629 switch (chan->channel2) {
630 case IIO_MOD_X:
631 /* OFFSET_USER4 register is shared */
632 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
633 &regval);
634 if (ret)
635 goto out_unlock;
636 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
637 st->buffer[1] = offset & 0xFF;
638 break;
639 case IIO_MOD_Y:
640 /* OFFSET_USER7 register is shared */
641 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
642 &regval);
643 if (ret)
644 goto out_unlock;
645 st->buffer[0] = offset & 0xFF;
646 st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
647 break;
648 case IIO_MOD_Z:
649 /* OFFSET_USER7 register is shared */
650 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
651 &regval);
652 if (ret)
653 goto out_unlock;
654 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
655 st->buffer[1] = offset & 0xFF;
656 break;
657 default:
658 ret = -EINVAL;
659 goto out_unlock;
660 }
661
662 ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
663
664 out_unlock:
665 mutex_unlock(&st->lock);
666 pm_runtime_mark_last_busy(dev);
667 pm_runtime_put_autosuspend(dev);
668 return ret;
669 }
670
671 static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
672 struct iio_chan_spec const *chan,
673 int *val, int *val2, long mask)
674 {
675 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
676 int16_t data;
677 int ret;
678
679 switch (chan->type) {
680 case IIO_ACCEL:
681 break;
682 case IIO_TEMP:
683 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
684 default:
685 return -EINVAL;
686 }
687
688 switch (mask) {
689 case IIO_CHAN_INFO_RAW:
690 ret = iio_device_claim_direct_mode(indio_dev);
691 if (ret)
692 return ret;
693 ret = inv_icm42600_accel_read_sensor(indio_dev, chan, &data);
694 iio_device_release_direct_mode(indio_dev);
695 if (ret)
696 return ret;
697 *val = data;
698 return IIO_VAL_INT;
699 case IIO_CHAN_INFO_SCALE:
700 return inv_icm42600_accel_read_scale(indio_dev, val, val2);
701 case IIO_CHAN_INFO_SAMP_FREQ:
702 return inv_icm42600_accel_read_odr(st, val, val2);
703 case IIO_CHAN_INFO_CALIBBIAS:
704 return inv_icm42600_accel_read_offset(st, chan, val, val2);
705 default:
706 return -EINVAL;
707 }
708 }
709
710 static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
711 struct iio_chan_spec const *chan,
712 const int **vals,
713 int *type, int *length, long mask)
714 {
715 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
716
717 if (chan->type != IIO_ACCEL)
718 return -EINVAL;
719
720 switch (mask) {
721 case IIO_CHAN_INFO_SCALE:
722 *vals = accel_st->scales;
723 *type = IIO_VAL_INT_PLUS_NANO;
724 *length = accel_st->scales_len;
725 return IIO_AVAIL_LIST;
726 case IIO_CHAN_INFO_SAMP_FREQ:
727 *vals = inv_icm42600_accel_odr;
728 *type = IIO_VAL_INT_PLUS_MICRO;
729 *length = ARRAY_SIZE(inv_icm42600_accel_odr);
730 return IIO_AVAIL_LIST;
731 case IIO_CHAN_INFO_CALIBBIAS:
732 *vals = inv_icm42600_accel_calibbias;
733 *type = IIO_VAL_INT_PLUS_MICRO;
734 return IIO_AVAIL_RANGE;
735 default:
736 return -EINVAL;
737 }
738 }
739
740 static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
741 struct iio_chan_spec const *chan,
742 int val, int val2, long mask)
743 {
744 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
745 int ret;
746
747 if (chan->type != IIO_ACCEL)
748 return -EINVAL;
749
750 switch (mask) {
751 case IIO_CHAN_INFO_SCALE:
752 ret = iio_device_claim_direct_mode(indio_dev);
753 if (ret)
754 return ret;
755 ret = inv_icm42600_accel_write_scale(indio_dev, val, val2);
756 iio_device_release_direct_mode(indio_dev);
757 return ret;
758 case IIO_CHAN_INFO_SAMP_FREQ:
759 return inv_icm42600_accel_write_odr(indio_dev, val, val2);
760 case IIO_CHAN_INFO_CALIBBIAS:
761 ret = iio_device_claim_direct_mode(indio_dev);
762 if (ret)
763 return ret;
764 ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
765 iio_device_release_direct_mode(indio_dev);
766 return ret;
767 default:
768 return -EINVAL;
769 }
770 }
771
772 static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
773 struct iio_chan_spec const *chan,
774 long mask)
775 {
776 if (chan->type != IIO_ACCEL)
777 return -EINVAL;
778
779 switch (mask) {
780 case IIO_CHAN_INFO_SCALE:
781 return IIO_VAL_INT_PLUS_NANO;
782 case IIO_CHAN_INFO_SAMP_FREQ:
783 return IIO_VAL_INT_PLUS_MICRO;
784 case IIO_CHAN_INFO_CALIBBIAS:
785 return IIO_VAL_INT_PLUS_MICRO;
786 default:
787 return -EINVAL;
788 }
789 }
790
791 static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
792 unsigned int val)
793 {
794 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
795 int ret;
796
797 mutex_lock(&st->lock);
798
799 st->fifo.watermark.accel = val;
800 ret = inv_icm42600_buffer_update_watermark(st);
801
802 mutex_unlock(&st->lock);
803
804 return ret;
805 }
806
807 static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
808 unsigned int count)
809 {
810 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
811 int ret;
812
813 if (count == 0)
814 return 0;
815
816 mutex_lock(&st->lock);
817
818 ret = inv_icm42600_buffer_hwfifo_flush(st, count);
819 if (!ret)
820 ret = st->fifo.nb.accel;
821
822 mutex_unlock(&st->lock);
823
824 return ret;
825 }
826
827 static const struct iio_info inv_icm42600_accel_info = {
828 .read_raw = inv_icm42600_accel_read_raw,
829 .read_avail = inv_icm42600_accel_read_avail,
830 .write_raw = inv_icm42600_accel_write_raw,
831 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
832 .debugfs_reg_access = inv_icm42600_debugfs_reg,
833 .update_scan_mode = inv_icm42600_accel_update_scan_mode,
834 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
835 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
836 };
837
838 struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
839 {
840 struct device *dev = regmap_get_device(st->map);
841 const char *name;
842 struct inv_icm42600_sensor_state *accel_st;
843 struct inv_sensors_timestamp_chip ts_chip;
844 struct iio_dev *indio_dev;
845 int ret;
846
847 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
848 if (!name)
849 return ERR_PTR(-ENOMEM);
850
851 indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st));
852 if (!indio_dev)
853 return ERR_PTR(-ENOMEM);
854 accel_st = iio_priv(indio_dev);
855
856 switch (st->chip) {
857 case INV_CHIP_ICM42686:
858 accel_st->scales = inv_icm42686_accel_scale;
859 accel_st->scales_len = ARRAY_SIZE(inv_icm42686_accel_scale);
860 break;
861 default:
862 accel_st->scales = inv_icm42600_accel_scale;
863 accel_st->scales_len = ARRAY_SIZE(inv_icm42600_accel_scale);
864 break;
865 }
866 /* low-power by default at init */
867 accel_st->power_mode = INV_ICM42600_SENSOR_MODE_LOW_POWER;
868 accel_st->filter = INV_ICM42600_FILTER_AVG_16X;
869
870 /*
871 * clock period is 32kHz (31250ns)
872 * jitter is +/- 2% (20 per mille)
873 */
874 ts_chip.clock_period = 31250;
875 ts_chip.jitter = 20;
876 ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr);
877 inv_sensors_timestamp_init(&accel_st->ts, &ts_chip);
878
879 iio_device_set_drvdata(indio_dev, st);
880 indio_dev->name = name;
881 indio_dev->info = &inv_icm42600_accel_info;
882 indio_dev->modes = INDIO_DIRECT_MODE;
883 indio_dev->channels = inv_icm42600_accel_channels;
884 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
885 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
886
887 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
888 &inv_icm42600_buffer_ops);
889 if (ret)
890 return ERR_PTR(ret);
891
892 ret = devm_iio_device_register(dev, indio_dev);
893 if (ret)
894 return ERR_PTR(ret);
895
896 return indio_dev;
897 }
898
899 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
900 {
901 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
902 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
903 struct inv_sensors_timestamp *ts = &accel_st->ts;
904 ssize_t i, size;
905 unsigned int no;
906 const void *accel, *gyro, *timestamp;
907 const int8_t *temp;
908 unsigned int odr;
909 int64_t ts_val;
910 struct inv_icm42600_accel_buffer buffer;
911
912 /* parse all fifo packets */
913 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
914 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
915 &accel, &gyro, &temp, &timestamp, &odr);
916 /* quit if error or FIFO is empty */
917 if (size <= 0)
918 return size;
919
920 /* skip packet if no accel data or data is invalid */
921 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
922 continue;
923
924 /* update odr */
925 if (odr & INV_ICM42600_SENSOR_ACCEL)
926 inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
927 st->fifo.nb.total, no);
928
929 /* buffer is copied to userspace, zeroing it to avoid any data leak */
930 memset(&buffer, 0, sizeof(buffer));
931 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
932 /* convert 8 bits FIFO temperature in high resolution format */
933 buffer.temp = temp ? (*temp * 64) : 0;
934 ts_val = inv_sensors_timestamp_pop(ts);
935 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
936 }
937
938 return 0;
939 }
Kernel DRM miscellaneous fixes and cross-tree changes