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 struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = { |
59 | IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix), |
60 | {}, |
61 | }; |
62 | |
63 | static const struct iio_chan_spec inv_icm42600_accel_channels[] = { |
64 | INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X, |
65 | inv_icm42600_accel_ext_infos), |
66 | INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y, |
67 | inv_icm42600_accel_ext_infos), |
68 | INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z, |
69 | inv_icm42600_accel_ext_infos), |
70 | INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP), |
71 | IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP), |
72 | }; |
73 | |
74 | /* |
75 | * IIO buffer data: size must be a power of 2 and timestamp aligned |
76 | * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp |
77 | */ |
78 | struct inv_icm42600_accel_buffer { |
79 | struct inv_icm42600_fifo_sensor_data accel; |
80 | int16_t temp; |
81 | int64_t timestamp __aligned(8); |
82 | }; |
83 | |
84 | #define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \ |
85 | (BIT(INV_ICM42600_ACCEL_SCAN_X) | \ |
86 | BIT(INV_ICM42600_ACCEL_SCAN_Y) | \ |
87 | BIT(INV_ICM42600_ACCEL_SCAN_Z)) |
88 | |
89 | #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP) |
90 | |
91 | static const unsigned long inv_icm42600_accel_scan_masks[] = { |
92 | /* 3-axis accel + temperature */ |
93 | INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP, |
94 | 0, |
95 | }; |
96 | |
97 | /* enable accelerometer sensor and FIFO write */ |
98 | static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev, |
99 | const unsigned long *scan_mask) |
100 | { |
101 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
102 | struct inv_sensors_timestamp *ts = iio_priv(indio_dev); |
103 | struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; |
104 | unsigned int fifo_en = 0; |
105 | unsigned int sleep_temp = 0; |
106 | unsigned int sleep_accel = 0; |
107 | unsigned int sleep; |
108 | int ret; |
109 | |
110 | mutex_lock(&st->lock); |
111 | |
112 | if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) { |
113 | /* enable temp sensor */ |
114 | ret = inv_icm42600_set_temp_conf(st, enable: true, sleep_ms: &sleep_temp); |
115 | if (ret) |
116 | goto out_unlock; |
117 | fifo_en |= INV_ICM42600_SENSOR_TEMP; |
118 | } |
119 | |
120 | if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) { |
121 | /* enable accel sensor */ |
122 | conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; |
123 | ret = inv_icm42600_set_accel_conf(st, conf: &conf, sleep_ms: &sleep_accel); |
124 | if (ret) |
125 | goto out_unlock; |
126 | fifo_en |= INV_ICM42600_SENSOR_ACCEL; |
127 | } |
128 | |
129 | /* update data FIFO write */ |
130 | inv_sensors_timestamp_apply_odr(ts, fifo_period: 0, fifo_nb: 0, fifo_no: 0); |
131 | ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en: fifo_en | st->fifo.en); |
132 | if (ret) |
133 | goto out_unlock; |
134 | |
135 | ret = inv_icm42600_buffer_update_watermark(st); |
136 | |
137 | out_unlock: |
138 | mutex_unlock(lock: &st->lock); |
139 | /* sleep maximum required time */ |
140 | sleep = max(sleep_accel, sleep_temp); |
141 | if (sleep) |
142 | msleep(msecs: sleep); |
143 | return ret; |
144 | } |
145 | |
146 | static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st, |
147 | struct iio_chan_spec const *chan, |
148 | int16_t *val) |
149 | { |
150 | struct device *dev = regmap_get_device(map: st->map); |
151 | struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; |
152 | unsigned int reg; |
153 | __be16 *data; |
154 | int ret; |
155 | |
156 | if (chan->type != IIO_ACCEL) |
157 | return -EINVAL; |
158 | |
159 | switch (chan->channel2) { |
160 | case IIO_MOD_X: |
161 | reg = INV_ICM42600_REG_ACCEL_DATA_X; |
162 | break; |
163 | case IIO_MOD_Y: |
164 | reg = INV_ICM42600_REG_ACCEL_DATA_Y; |
165 | break; |
166 | case IIO_MOD_Z: |
167 | reg = INV_ICM42600_REG_ACCEL_DATA_Z; |
168 | break; |
169 | default: |
170 | return -EINVAL; |
171 | } |
172 | |
173 | pm_runtime_get_sync(dev); |
174 | mutex_lock(&st->lock); |
175 | |
176 | /* enable accel sensor */ |
177 | conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; |
178 | ret = inv_icm42600_set_accel_conf(st, conf: &conf, NULL); |
179 | if (ret) |
180 | goto exit; |
181 | |
182 | /* read accel register data */ |
183 | data = (__be16 *)&st->buffer[0]; |
184 | ret = regmap_bulk_read(map: st->map, reg, val: data, val_count: sizeof(*data)); |
185 | if (ret) |
186 | goto exit; |
187 | |
188 | *val = (int16_t)be16_to_cpup(p: data); |
189 | if (*val == INV_ICM42600_DATA_INVALID) |
190 | ret = -EINVAL; |
191 | exit: |
192 | mutex_unlock(lock: &st->lock); |
193 | pm_runtime_mark_last_busy(dev); |
194 | pm_runtime_put_autosuspend(dev); |
195 | return ret; |
196 | } |
197 | |
198 | /* IIO format int + nano */ |
199 | static const int inv_icm42600_accel_scale[] = { |
200 | /* +/- 16G => 0.004788403 m/s-2 */ |
201 | [2 * INV_ICM42600_ACCEL_FS_16G] = 0, |
202 | [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403, |
203 | /* +/- 8G => 0.002394202 m/s-2 */ |
204 | [2 * INV_ICM42600_ACCEL_FS_8G] = 0, |
205 | [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202, |
206 | /* +/- 4G => 0.001197101 m/s-2 */ |
207 | [2 * INV_ICM42600_ACCEL_FS_4G] = 0, |
208 | [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101, |
209 | /* +/- 2G => 0.000598550 m/s-2 */ |
210 | [2 * INV_ICM42600_ACCEL_FS_2G] = 0, |
211 | [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550, |
212 | }; |
213 | |
214 | static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st, |
215 | int *val, int *val2) |
216 | { |
217 | unsigned int idx; |
218 | |
219 | idx = st->conf.accel.fs; |
220 | |
221 | *val = inv_icm42600_accel_scale[2 * idx]; |
222 | *val2 = inv_icm42600_accel_scale[2 * idx + 1]; |
223 | return IIO_VAL_INT_PLUS_NANO; |
224 | } |
225 | |
226 | static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st, |
227 | int val, int val2) |
228 | { |
229 | struct device *dev = regmap_get_device(map: st->map); |
230 | unsigned int idx; |
231 | struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; |
232 | int ret; |
233 | |
234 | for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) { |
235 | if (val == inv_icm42600_accel_scale[idx] && |
236 | val2 == inv_icm42600_accel_scale[idx + 1]) |
237 | break; |
238 | } |
239 | if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale)) |
240 | return -EINVAL; |
241 | |
242 | conf.fs = idx / 2; |
243 | |
244 | pm_runtime_get_sync(dev); |
245 | mutex_lock(&st->lock); |
246 | |
247 | ret = inv_icm42600_set_accel_conf(st, conf: &conf, NULL); |
248 | |
249 | mutex_unlock(lock: &st->lock); |
250 | pm_runtime_mark_last_busy(dev); |
251 | pm_runtime_put_autosuspend(dev); |
252 | |
253 | return ret; |
254 | } |
255 | |
256 | /* IIO format int + micro */ |
257 | static const int inv_icm42600_accel_odr[] = { |
258 | /* 12.5Hz */ |
259 | 12, 500000, |
260 | /* 25Hz */ |
261 | 25, 0, |
262 | /* 50Hz */ |
263 | 50, 0, |
264 | /* 100Hz */ |
265 | 100, 0, |
266 | /* 200Hz */ |
267 | 200, 0, |
268 | /* 1kHz */ |
269 | 1000, 0, |
270 | /* 2kHz */ |
271 | 2000, 0, |
272 | /* 4kHz */ |
273 | 4000, 0, |
274 | }; |
275 | |
276 | static const int inv_icm42600_accel_odr_conv[] = { |
277 | INV_ICM42600_ODR_12_5HZ, |
278 | INV_ICM42600_ODR_25HZ, |
279 | INV_ICM42600_ODR_50HZ, |
280 | INV_ICM42600_ODR_100HZ, |
281 | INV_ICM42600_ODR_200HZ, |
282 | INV_ICM42600_ODR_1KHZ_LN, |
283 | INV_ICM42600_ODR_2KHZ_LN, |
284 | INV_ICM42600_ODR_4KHZ_LN, |
285 | }; |
286 | |
287 | static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st, |
288 | int *val, int *val2) |
289 | { |
290 | unsigned int odr; |
291 | unsigned int i; |
292 | |
293 | odr = st->conf.accel.odr; |
294 | |
295 | for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) { |
296 | if (inv_icm42600_accel_odr_conv[i] == odr) |
297 | break; |
298 | } |
299 | if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv)) |
300 | return -EINVAL; |
301 | |
302 | *val = inv_icm42600_accel_odr[2 * i]; |
303 | *val2 = inv_icm42600_accel_odr[2 * i + 1]; |
304 | |
305 | return IIO_VAL_INT_PLUS_MICRO; |
306 | } |
307 | |
308 | static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev, |
309 | int val, int val2) |
310 | { |
311 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
312 | struct inv_sensors_timestamp *ts = iio_priv(indio_dev); |
313 | struct device *dev = regmap_get_device(map: st->map); |
314 | unsigned int idx; |
315 | struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; |
316 | int ret; |
317 | |
318 | for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) { |
319 | if (val == inv_icm42600_accel_odr[idx] && |
320 | val2 == inv_icm42600_accel_odr[idx + 1]) |
321 | break; |
322 | } |
323 | if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr)) |
324 | return -EINVAL; |
325 | |
326 | conf.odr = inv_icm42600_accel_odr_conv[idx / 2]; |
327 | |
328 | pm_runtime_get_sync(dev); |
329 | mutex_lock(&st->lock); |
330 | |
331 | ret = inv_sensors_timestamp_update_odr(ts, period: inv_icm42600_odr_to_period(odr: conf.odr), |
332 | fifo: iio_buffer_enabled(indio_dev)); |
333 | if (ret) |
334 | goto out_unlock; |
335 | |
336 | ret = inv_icm42600_set_accel_conf(st, conf: &conf, NULL); |
337 | if (ret) |
338 | goto out_unlock; |
339 | inv_icm42600_buffer_update_fifo_period(st); |
340 | inv_icm42600_buffer_update_watermark(st); |
341 | |
342 | out_unlock: |
343 | mutex_unlock(lock: &st->lock); |
344 | pm_runtime_mark_last_busy(dev); |
345 | pm_runtime_put_autosuspend(dev); |
346 | |
347 | return ret; |
348 | } |
349 | |
350 | /* |
351 | * Calibration bias values, IIO range format int + micro. |
352 | * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg. |
353 | */ |
354 | static int inv_icm42600_accel_calibbias[] = { |
355 | -10, 42010, /* min: -10.042010 m/s² */ |
356 | 0, 4903, /* step: 0.004903 m/s² */ |
357 | 10, 37106, /* max: 10.037106 m/s² */ |
358 | }; |
359 | |
360 | static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st, |
361 | struct iio_chan_spec const *chan, |
362 | int *val, int *val2) |
363 | { |
364 | struct device *dev = regmap_get_device(map: st->map); |
365 | int64_t val64; |
366 | int32_t bias; |
367 | unsigned int reg; |
368 | int16_t offset; |
369 | uint8_t data[2]; |
370 | int ret; |
371 | |
372 | if (chan->type != IIO_ACCEL) |
373 | return -EINVAL; |
374 | |
375 | switch (chan->channel2) { |
376 | case IIO_MOD_X: |
377 | reg = INV_ICM42600_REG_OFFSET_USER4; |
378 | break; |
379 | case IIO_MOD_Y: |
380 | reg = INV_ICM42600_REG_OFFSET_USER6; |
381 | break; |
382 | case IIO_MOD_Z: |
383 | reg = INV_ICM42600_REG_OFFSET_USER7; |
384 | break; |
385 | default: |
386 | return -EINVAL; |
387 | } |
388 | |
389 | pm_runtime_get_sync(dev); |
390 | mutex_lock(&st->lock); |
391 | |
392 | ret = regmap_bulk_read(map: st->map, reg, val: st->buffer, val_count: sizeof(data)); |
393 | memcpy(data, st->buffer, sizeof(data)); |
394 | |
395 | mutex_unlock(lock: &st->lock); |
396 | pm_runtime_mark_last_busy(dev); |
397 | pm_runtime_put_autosuspend(dev); |
398 | if (ret) |
399 | return ret; |
400 | |
401 | /* 12 bits signed value */ |
402 | switch (chan->channel2) { |
403 | case IIO_MOD_X: |
404 | offset = sign_extend32(value: ((data[0] & 0xF0) << 4) | data[1], index: 11); |
405 | break; |
406 | case IIO_MOD_Y: |
407 | offset = sign_extend32(value: ((data[1] & 0x0F) << 8) | data[0], index: 11); |
408 | break; |
409 | case IIO_MOD_Z: |
410 | offset = sign_extend32(value: ((data[0] & 0xF0) << 4) | data[1], index: 11); |
411 | break; |
412 | default: |
413 | return -EINVAL; |
414 | } |
415 | |
416 | /* |
417 | * convert raw offset to g then to m/s² |
418 | * 12 bits signed raw step 0.5mg to g: 5 / 10000 |
419 | * g to m/s²: 9.806650 |
420 | * result in micro (1000000) |
421 | * (offset * 5 * 9.806650 * 1000000) / 10000 |
422 | */ |
423 | val64 = (int64_t)offset * 5LL * 9806650LL; |
424 | /* for rounding, add + or - divisor (10000) divided by 2 */ |
425 | if (val64 >= 0) |
426 | val64 += 10000LL / 2LL; |
427 | else |
428 | val64 -= 10000LL / 2LL; |
429 | bias = div_s64(dividend: val64, divisor: 10000L); |
430 | *val = bias / 1000000L; |
431 | *val2 = bias % 1000000L; |
432 | |
433 | return IIO_VAL_INT_PLUS_MICRO; |
434 | } |
435 | |
436 | static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st, |
437 | struct iio_chan_spec const *chan, |
438 | int val, int val2) |
439 | { |
440 | struct device *dev = regmap_get_device(map: st->map); |
441 | int64_t val64; |
442 | int32_t min, max; |
443 | unsigned int reg, regval; |
444 | int16_t offset; |
445 | int ret; |
446 | |
447 | if (chan->type != IIO_ACCEL) |
448 | return -EINVAL; |
449 | |
450 | switch (chan->channel2) { |
451 | case IIO_MOD_X: |
452 | reg = INV_ICM42600_REG_OFFSET_USER4; |
453 | break; |
454 | case IIO_MOD_Y: |
455 | reg = INV_ICM42600_REG_OFFSET_USER6; |
456 | break; |
457 | case IIO_MOD_Z: |
458 | reg = INV_ICM42600_REG_OFFSET_USER7; |
459 | break; |
460 | default: |
461 | return -EINVAL; |
462 | } |
463 | |
464 | /* inv_icm42600_accel_calibbias: min - step - max in micro */ |
465 | min = inv_icm42600_accel_calibbias[0] * 1000000L + |
466 | inv_icm42600_accel_calibbias[1]; |
467 | max = inv_icm42600_accel_calibbias[4] * 1000000L + |
468 | inv_icm42600_accel_calibbias[5]; |
469 | val64 = (int64_t)val * 1000000LL + (int64_t)val2; |
470 | if (val64 < min || val64 > max) |
471 | return -EINVAL; |
472 | |
473 | /* |
474 | * convert m/s² to g then to raw value |
475 | * m/s² to g: 1 / 9.806650 |
476 | * g to raw 12 bits signed, step 0.5mg: 10000 / 5 |
477 | * val in micro (1000000) |
478 | * val * 10000 / (9.806650 * 1000000 * 5) |
479 | */ |
480 | val64 = val64 * 10000LL; |
481 | /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */ |
482 | if (val64 >= 0) |
483 | val64 += 9806650 * 5 / 2; |
484 | else |
485 | val64 -= 9806650 * 5 / 2; |
486 | offset = div_s64(dividend: val64, divisor: 9806650 * 5); |
487 | |
488 | /* clamp value limited to 12 bits signed */ |
489 | if (offset < -2048) |
490 | offset = -2048; |
491 | else if (offset > 2047) |
492 | offset = 2047; |
493 | |
494 | pm_runtime_get_sync(dev); |
495 | mutex_lock(&st->lock); |
496 | |
497 | switch (chan->channel2) { |
498 | case IIO_MOD_X: |
499 | /* OFFSET_USER4 register is shared */ |
500 | ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER4, |
501 | val: ®val); |
502 | if (ret) |
503 | goto out_unlock; |
504 | st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); |
505 | st->buffer[1] = offset & 0xFF; |
506 | break; |
507 | case IIO_MOD_Y: |
508 | /* OFFSET_USER7 register is shared */ |
509 | ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER7, |
510 | val: ®val); |
511 | if (ret) |
512 | goto out_unlock; |
513 | st->buffer[0] = offset & 0xFF; |
514 | st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0); |
515 | break; |
516 | case IIO_MOD_Z: |
517 | /* OFFSET_USER7 register is shared */ |
518 | ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER7, |
519 | val: ®val); |
520 | if (ret) |
521 | goto out_unlock; |
522 | st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); |
523 | st->buffer[1] = offset & 0xFF; |
524 | break; |
525 | default: |
526 | ret = -EINVAL; |
527 | goto out_unlock; |
528 | } |
529 | |
530 | ret = regmap_bulk_write(map: st->map, reg, val: st->buffer, val_count: 2); |
531 | |
532 | out_unlock: |
533 | mutex_unlock(lock: &st->lock); |
534 | pm_runtime_mark_last_busy(dev); |
535 | pm_runtime_put_autosuspend(dev); |
536 | return ret; |
537 | } |
538 | |
539 | static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev, |
540 | struct iio_chan_spec const *chan, |
541 | int *val, int *val2, long mask) |
542 | { |
543 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
544 | int16_t data; |
545 | int ret; |
546 | |
547 | switch (chan->type) { |
548 | case IIO_ACCEL: |
549 | break; |
550 | case IIO_TEMP: |
551 | return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask); |
552 | default: |
553 | return -EINVAL; |
554 | } |
555 | |
556 | switch (mask) { |
557 | case IIO_CHAN_INFO_RAW: |
558 | ret = iio_device_claim_direct_mode(indio_dev); |
559 | if (ret) |
560 | return ret; |
561 | ret = inv_icm42600_accel_read_sensor(st, chan, val: &data); |
562 | iio_device_release_direct_mode(indio_dev); |
563 | if (ret) |
564 | return ret; |
565 | *val = data; |
566 | return IIO_VAL_INT; |
567 | case IIO_CHAN_INFO_SCALE: |
568 | return inv_icm42600_accel_read_scale(st, val, val2); |
569 | case IIO_CHAN_INFO_SAMP_FREQ: |
570 | return inv_icm42600_accel_read_odr(st, val, val2); |
571 | case IIO_CHAN_INFO_CALIBBIAS: |
572 | return inv_icm42600_accel_read_offset(st, chan, val, val2); |
573 | default: |
574 | return -EINVAL; |
575 | } |
576 | } |
577 | |
578 | static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev, |
579 | struct iio_chan_spec const *chan, |
580 | const int **vals, |
581 | int *type, int *length, long mask) |
582 | { |
583 | if (chan->type != IIO_ACCEL) |
584 | return -EINVAL; |
585 | |
586 | switch (mask) { |
587 | case IIO_CHAN_INFO_SCALE: |
588 | *vals = inv_icm42600_accel_scale; |
589 | *type = IIO_VAL_INT_PLUS_NANO; |
590 | *length = ARRAY_SIZE(inv_icm42600_accel_scale); |
591 | return IIO_AVAIL_LIST; |
592 | case IIO_CHAN_INFO_SAMP_FREQ: |
593 | *vals = inv_icm42600_accel_odr; |
594 | *type = IIO_VAL_INT_PLUS_MICRO; |
595 | *length = ARRAY_SIZE(inv_icm42600_accel_odr); |
596 | return IIO_AVAIL_LIST; |
597 | case IIO_CHAN_INFO_CALIBBIAS: |
598 | *vals = inv_icm42600_accel_calibbias; |
599 | *type = IIO_VAL_INT_PLUS_MICRO; |
600 | return IIO_AVAIL_RANGE; |
601 | default: |
602 | return -EINVAL; |
603 | } |
604 | } |
605 | |
606 | static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev, |
607 | struct iio_chan_spec const *chan, |
608 | int val, int val2, long mask) |
609 | { |
610 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
611 | int ret; |
612 | |
613 | if (chan->type != IIO_ACCEL) |
614 | return -EINVAL; |
615 | |
616 | switch (mask) { |
617 | case IIO_CHAN_INFO_SCALE: |
618 | ret = iio_device_claim_direct_mode(indio_dev); |
619 | if (ret) |
620 | return ret; |
621 | ret = inv_icm42600_accel_write_scale(st, val, val2); |
622 | iio_device_release_direct_mode(indio_dev); |
623 | return ret; |
624 | case IIO_CHAN_INFO_SAMP_FREQ: |
625 | return inv_icm42600_accel_write_odr(indio_dev, val, val2); |
626 | case IIO_CHAN_INFO_CALIBBIAS: |
627 | ret = iio_device_claim_direct_mode(indio_dev); |
628 | if (ret) |
629 | return ret; |
630 | ret = inv_icm42600_accel_write_offset(st, chan, val, val2); |
631 | iio_device_release_direct_mode(indio_dev); |
632 | return ret; |
633 | default: |
634 | return -EINVAL; |
635 | } |
636 | } |
637 | |
638 | static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev, |
639 | struct iio_chan_spec const *chan, |
640 | long mask) |
641 | { |
642 | if (chan->type != IIO_ACCEL) |
643 | return -EINVAL; |
644 | |
645 | switch (mask) { |
646 | case IIO_CHAN_INFO_SCALE: |
647 | return IIO_VAL_INT_PLUS_NANO; |
648 | case IIO_CHAN_INFO_SAMP_FREQ: |
649 | return IIO_VAL_INT_PLUS_MICRO; |
650 | case IIO_CHAN_INFO_CALIBBIAS: |
651 | return IIO_VAL_INT_PLUS_MICRO; |
652 | default: |
653 | return -EINVAL; |
654 | } |
655 | } |
656 | |
657 | static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev, |
658 | unsigned int val) |
659 | { |
660 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
661 | int ret; |
662 | |
663 | mutex_lock(&st->lock); |
664 | |
665 | st->fifo.watermark.accel = val; |
666 | ret = inv_icm42600_buffer_update_watermark(st); |
667 | |
668 | mutex_unlock(lock: &st->lock); |
669 | |
670 | return ret; |
671 | } |
672 | |
673 | static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev, |
674 | unsigned int count) |
675 | { |
676 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
677 | int ret; |
678 | |
679 | if (count == 0) |
680 | return 0; |
681 | |
682 | mutex_lock(&st->lock); |
683 | |
684 | ret = inv_icm42600_buffer_hwfifo_flush(st, count); |
685 | if (!ret) |
686 | ret = st->fifo.nb.accel; |
687 | |
688 | mutex_unlock(lock: &st->lock); |
689 | |
690 | return ret; |
691 | } |
692 | |
693 | static const struct iio_info inv_icm42600_accel_info = { |
694 | .read_raw = inv_icm42600_accel_read_raw, |
695 | .read_avail = inv_icm42600_accel_read_avail, |
696 | .write_raw = inv_icm42600_accel_write_raw, |
697 | .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt, |
698 | .debugfs_reg_access = inv_icm42600_debugfs_reg, |
699 | .update_scan_mode = inv_icm42600_accel_update_scan_mode, |
700 | .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark, |
701 | .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush, |
702 | }; |
703 | |
704 | struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) |
705 | { |
706 | struct device *dev = regmap_get_device(map: st->map); |
707 | const char *name; |
708 | struct inv_sensors_timestamp_chip ts_chip; |
709 | struct inv_sensors_timestamp *ts; |
710 | struct iio_dev *indio_dev; |
711 | int ret; |
712 | |
713 | name = devm_kasprintf(dev, GFP_KERNEL, fmt: "%s-accel" , st->name); |
714 | if (!name) |
715 | return ERR_PTR(error: -ENOMEM); |
716 | |
717 | indio_dev = devm_iio_device_alloc(parent: dev, sizeof_priv: sizeof(*ts)); |
718 | if (!indio_dev) |
719 | return ERR_PTR(error: -ENOMEM); |
720 | |
721 | /* |
722 | * clock period is 32kHz (31250ns) |
723 | * jitter is +/- 2% (20 per mille) |
724 | */ |
725 | ts_chip.clock_period = 31250; |
726 | ts_chip.jitter = 20; |
727 | ts_chip.init_period = inv_icm42600_odr_to_period(odr: st->conf.accel.odr); |
728 | ts = iio_priv(indio_dev); |
729 | inv_sensors_timestamp_init(ts, chip: &ts_chip); |
730 | |
731 | iio_device_set_drvdata(indio_dev, data: st); |
732 | indio_dev->name = name; |
733 | indio_dev->info = &inv_icm42600_accel_info; |
734 | indio_dev->modes = INDIO_DIRECT_MODE; |
735 | indio_dev->channels = inv_icm42600_accel_channels; |
736 | indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels); |
737 | indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks; |
738 | |
739 | ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, |
740 | &inv_icm42600_buffer_ops); |
741 | if (ret) |
742 | return ERR_PTR(error: ret); |
743 | |
744 | ret = devm_iio_device_register(dev, indio_dev); |
745 | if (ret) |
746 | return ERR_PTR(error: ret); |
747 | |
748 | return indio_dev; |
749 | } |
750 | |
751 | int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev) |
752 | { |
753 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
754 | struct inv_sensors_timestamp *ts = iio_priv(indio_dev); |
755 | ssize_t i, size; |
756 | unsigned int no; |
757 | const void *accel, *gyro, *timestamp; |
758 | const int8_t *temp; |
759 | unsigned int odr; |
760 | int64_t ts_val; |
761 | struct inv_icm42600_accel_buffer buffer; |
762 | |
763 | /* parse all fifo packets */ |
764 | for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { |
765 | size = inv_icm42600_fifo_decode_packet(packet: &st->fifo.data[i], |
766 | accel: &accel, gyro: &gyro, temp: &temp, timestamp: ×tamp, odr: &odr); |
767 | /* quit if error or FIFO is empty */ |
768 | if (size <= 0) |
769 | return size; |
770 | |
771 | /* skip packet if no accel data or data is invalid */ |
772 | if (accel == NULL || !inv_icm42600_fifo_is_data_valid(s: accel)) |
773 | continue; |
774 | |
775 | /* update odr */ |
776 | if (odr & INV_ICM42600_SENSOR_ACCEL) |
777 | inv_sensors_timestamp_apply_odr(ts, fifo_period: st->fifo.period, |
778 | fifo_nb: st->fifo.nb.total, fifo_no: no); |
779 | |
780 | /* buffer is copied to userspace, zeroing it to avoid any data leak */ |
781 | memset(&buffer, 0, sizeof(buffer)); |
782 | memcpy(&buffer.accel, accel, sizeof(buffer.accel)); |
783 | /* convert 8 bits FIFO temperature in high resolution format */ |
784 | buffer.temp = temp ? (*temp * 64) : 0; |
785 | ts_val = inv_sensors_timestamp_pop(ts); |
786 | iio_push_to_buffers_with_timestamp(indio_dev, data: &buffer, timestamp: ts_val); |
787 | } |
788 | |
789 | return 0; |
790 | } |
791 | |