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_GYRO_CHAN(_modifier, _index, _ext_info) \ |
24 | { \ |
25 | .type = IIO_ANGL_VEL, \ |
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_gyro_scan { |
51 | INV_ICM42600_GYRO_SCAN_X, |
52 | INV_ICM42600_GYRO_SCAN_Y, |
53 | INV_ICM42600_GYRO_SCAN_Z, |
54 | INV_ICM42600_GYRO_SCAN_TEMP, |
55 | INV_ICM42600_GYRO_SCAN_TIMESTAMP, |
56 | }; |
57 | |
58 | static const struct iio_chan_spec_ext_info inv_icm42600_gyro_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_gyro_channels[] = { |
64 | INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X, |
65 | inv_icm42600_gyro_ext_infos), |
66 | INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y, |
67 | inv_icm42600_gyro_ext_infos), |
68 | INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z, |
69 | inv_icm42600_gyro_ext_infos), |
70 | INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP), |
71 | IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP), |
72 | }; |
73 | |
74 | /* |
75 | * IIO buffer data: size must be a power of 2 and timestamp aligned |
76 | * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp |
77 | */ |
78 | struct inv_icm42600_gyro_buffer { |
79 | struct inv_icm42600_fifo_sensor_data gyro; |
80 | int16_t temp; |
81 | int64_t timestamp __aligned(8); |
82 | }; |
83 | |
84 | #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \ |
85 | (BIT(INV_ICM42600_GYRO_SCAN_X) | \ |
86 | BIT(INV_ICM42600_GYRO_SCAN_Y) | \ |
87 | BIT(INV_ICM42600_GYRO_SCAN_Z)) |
88 | |
89 | #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP) |
90 | |
91 | static const unsigned long inv_icm42600_gyro_scan_masks[] = { |
92 | /* 3-axis gyro + temperature */ |
93 | INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP, |
94 | 0, |
95 | }; |
96 | |
97 | /* enable gyroscope sensor and FIFO write */ |
98 | static int inv_icm42600_gyro_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_gyro = 0; |
106 | unsigned int sleep_temp = 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_GYRO_3AXIS) { |
121 | /* enable gyro sensor */ |
122 | conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; |
123 | ret = inv_icm42600_set_gyro_conf(st, conf: &conf, sleep_ms: &sleep_gyro); |
124 | if (ret) |
125 | goto out_unlock; |
126 | fifo_en |= INV_ICM42600_SENSOR_GYRO; |
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_gyro, sleep_temp); |
141 | if (sleep) |
142 | msleep(msecs: sleep); |
143 | return ret; |
144 | } |
145 | |
146 | static int inv_icm42600_gyro_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_ANGL_VEL) |
157 | return -EINVAL; |
158 | |
159 | switch (chan->channel2) { |
160 | case IIO_MOD_X: |
161 | reg = INV_ICM42600_REG_GYRO_DATA_X; |
162 | break; |
163 | case IIO_MOD_Y: |
164 | reg = INV_ICM42600_REG_GYRO_DATA_Y; |
165 | break; |
166 | case IIO_MOD_Z: |
167 | reg = INV_ICM42600_REG_GYRO_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 gyro sensor */ |
177 | conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; |
178 | ret = inv_icm42600_set_gyro_conf(st, conf: &conf, NULL); |
179 | if (ret) |
180 | goto exit; |
181 | |
182 | /* read gyro 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_gyro_scale[] = { |
200 | /* +/- 2000dps => 0.001065264 rad/s */ |
201 | [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0, |
202 | [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264, |
203 | /* +/- 1000dps => 0.000532632 rad/s */ |
204 | [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0, |
205 | [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632, |
206 | /* +/- 500dps => 0.000266316 rad/s */ |
207 | [2 * INV_ICM42600_GYRO_FS_500DPS] = 0, |
208 | [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316, |
209 | /* +/- 250dps => 0.000133158 rad/s */ |
210 | [2 * INV_ICM42600_GYRO_FS_250DPS] = 0, |
211 | [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158, |
212 | /* +/- 125dps => 0.000066579 rad/s */ |
213 | [2 * INV_ICM42600_GYRO_FS_125DPS] = 0, |
214 | [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579, |
215 | /* +/- 62.5dps => 0.000033290 rad/s */ |
216 | [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0, |
217 | [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290, |
218 | /* +/- 31.25dps => 0.000016645 rad/s */ |
219 | [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0, |
220 | [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645, |
221 | /* +/- 15.625dps => 0.000008322 rad/s */ |
222 | [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0, |
223 | [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322, |
224 | }; |
225 | |
226 | static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st, |
227 | int *val, int *val2) |
228 | { |
229 | unsigned int idx; |
230 | |
231 | idx = st->conf.gyro.fs; |
232 | |
233 | *val = inv_icm42600_gyro_scale[2 * idx]; |
234 | *val2 = inv_icm42600_gyro_scale[2 * idx + 1]; |
235 | return IIO_VAL_INT_PLUS_NANO; |
236 | } |
237 | |
238 | static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st, |
239 | int val, int val2) |
240 | { |
241 | struct device *dev = regmap_get_device(map: st->map); |
242 | unsigned int idx; |
243 | struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; |
244 | int ret; |
245 | |
246 | for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) { |
247 | if (val == inv_icm42600_gyro_scale[idx] && |
248 | val2 == inv_icm42600_gyro_scale[idx + 1]) |
249 | break; |
250 | } |
251 | if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale)) |
252 | return -EINVAL; |
253 | |
254 | conf.fs = idx / 2; |
255 | |
256 | pm_runtime_get_sync(dev); |
257 | mutex_lock(&st->lock); |
258 | |
259 | ret = inv_icm42600_set_gyro_conf(st, conf: &conf, NULL); |
260 | |
261 | mutex_unlock(lock: &st->lock); |
262 | pm_runtime_mark_last_busy(dev); |
263 | pm_runtime_put_autosuspend(dev); |
264 | |
265 | return ret; |
266 | } |
267 | |
268 | /* IIO format int + micro */ |
269 | static const int inv_icm42600_gyro_odr[] = { |
270 | /* 12.5Hz */ |
271 | 12, 500000, |
272 | /* 25Hz */ |
273 | 25, 0, |
274 | /* 50Hz */ |
275 | 50, 0, |
276 | /* 100Hz */ |
277 | 100, 0, |
278 | /* 200Hz */ |
279 | 200, 0, |
280 | /* 1kHz */ |
281 | 1000, 0, |
282 | /* 2kHz */ |
283 | 2000, 0, |
284 | /* 4kHz */ |
285 | 4000, 0, |
286 | }; |
287 | |
288 | static const int inv_icm42600_gyro_odr_conv[] = { |
289 | INV_ICM42600_ODR_12_5HZ, |
290 | INV_ICM42600_ODR_25HZ, |
291 | INV_ICM42600_ODR_50HZ, |
292 | INV_ICM42600_ODR_100HZ, |
293 | INV_ICM42600_ODR_200HZ, |
294 | INV_ICM42600_ODR_1KHZ_LN, |
295 | INV_ICM42600_ODR_2KHZ_LN, |
296 | INV_ICM42600_ODR_4KHZ_LN, |
297 | }; |
298 | |
299 | static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st, |
300 | int *val, int *val2) |
301 | { |
302 | unsigned int odr; |
303 | unsigned int i; |
304 | |
305 | odr = st->conf.gyro.odr; |
306 | |
307 | for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) { |
308 | if (inv_icm42600_gyro_odr_conv[i] == odr) |
309 | break; |
310 | } |
311 | if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv)) |
312 | return -EINVAL; |
313 | |
314 | *val = inv_icm42600_gyro_odr[2 * i]; |
315 | *val2 = inv_icm42600_gyro_odr[2 * i + 1]; |
316 | |
317 | return IIO_VAL_INT_PLUS_MICRO; |
318 | } |
319 | |
320 | static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev, |
321 | int val, int val2) |
322 | { |
323 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
324 | struct inv_sensors_timestamp *ts = iio_priv(indio_dev); |
325 | struct device *dev = regmap_get_device(map: st->map); |
326 | unsigned int idx; |
327 | struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; |
328 | int ret; |
329 | |
330 | for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) { |
331 | if (val == inv_icm42600_gyro_odr[idx] && |
332 | val2 == inv_icm42600_gyro_odr[idx + 1]) |
333 | break; |
334 | } |
335 | if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr)) |
336 | return -EINVAL; |
337 | |
338 | conf.odr = inv_icm42600_gyro_odr_conv[idx / 2]; |
339 | |
340 | pm_runtime_get_sync(dev); |
341 | mutex_lock(&st->lock); |
342 | |
343 | ret = inv_sensors_timestamp_update_odr(ts, period: inv_icm42600_odr_to_period(odr: conf.odr), |
344 | fifo: iio_buffer_enabled(indio_dev)); |
345 | if (ret) |
346 | goto out_unlock; |
347 | |
348 | ret = inv_icm42600_set_gyro_conf(st, conf: &conf, NULL); |
349 | if (ret) |
350 | goto out_unlock; |
351 | inv_icm42600_buffer_update_fifo_period(st); |
352 | inv_icm42600_buffer_update_watermark(st); |
353 | |
354 | out_unlock: |
355 | mutex_unlock(lock: &st->lock); |
356 | pm_runtime_mark_last_busy(dev); |
357 | pm_runtime_put_autosuspend(dev); |
358 | |
359 | return ret; |
360 | } |
361 | |
362 | /* |
363 | * Calibration bias values, IIO range format int + nano. |
364 | * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps. |
365 | */ |
366 | static int inv_icm42600_gyro_calibbias[] = { |
367 | -1, 117010721, /* min: -1.117010721 rad/s */ |
368 | 0, 545415, /* step: 0.000545415 rad/s */ |
369 | 1, 116465306, /* max: 1.116465306 rad/s */ |
370 | }; |
371 | |
372 | static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st, |
373 | struct iio_chan_spec const *chan, |
374 | int *val, int *val2) |
375 | { |
376 | struct device *dev = regmap_get_device(map: st->map); |
377 | int64_t val64; |
378 | int32_t bias; |
379 | unsigned int reg; |
380 | int16_t offset; |
381 | uint8_t data[2]; |
382 | int ret; |
383 | |
384 | if (chan->type != IIO_ANGL_VEL) |
385 | return -EINVAL; |
386 | |
387 | switch (chan->channel2) { |
388 | case IIO_MOD_X: |
389 | reg = INV_ICM42600_REG_OFFSET_USER0; |
390 | break; |
391 | case IIO_MOD_Y: |
392 | reg = INV_ICM42600_REG_OFFSET_USER1; |
393 | break; |
394 | case IIO_MOD_Z: |
395 | reg = INV_ICM42600_REG_OFFSET_USER3; |
396 | break; |
397 | default: |
398 | return -EINVAL; |
399 | } |
400 | |
401 | pm_runtime_get_sync(dev); |
402 | mutex_lock(&st->lock); |
403 | |
404 | ret = regmap_bulk_read(map: st->map, reg, val: st->buffer, val_count: sizeof(data)); |
405 | memcpy(data, st->buffer, sizeof(data)); |
406 | |
407 | mutex_unlock(lock: &st->lock); |
408 | pm_runtime_mark_last_busy(dev); |
409 | pm_runtime_put_autosuspend(dev); |
410 | if (ret) |
411 | return ret; |
412 | |
413 | /* 12 bits signed value */ |
414 | switch (chan->channel2) { |
415 | case IIO_MOD_X: |
416 | offset = sign_extend32(value: ((data[1] & 0x0F) << 8) | data[0], index: 11); |
417 | break; |
418 | case IIO_MOD_Y: |
419 | offset = sign_extend32(value: ((data[0] & 0xF0) << 4) | data[1], index: 11); |
420 | break; |
421 | case IIO_MOD_Z: |
422 | offset = sign_extend32(value: ((data[1] & 0x0F) << 8) | data[0], index: 11); |
423 | break; |
424 | default: |
425 | return -EINVAL; |
426 | } |
427 | |
428 | /* |
429 | * convert raw offset to dps then to rad/s |
430 | * 12 bits signed raw max 64 to dps: 64 / 2048 |
431 | * dps to rad: Pi / 180 |
432 | * result in nano (1000000000) |
433 | * (offset * 64 * Pi * 1000000000) / (2048 * 180) |
434 | */ |
435 | val64 = (int64_t)offset * 64LL * 3141592653LL; |
436 | /* for rounding, add + or - divisor (2048 * 180) divided by 2 */ |
437 | if (val64 >= 0) |
438 | val64 += 2048 * 180 / 2; |
439 | else |
440 | val64 -= 2048 * 180 / 2; |
441 | bias = div_s64(dividend: val64, divisor: 2048 * 180); |
442 | *val = bias / 1000000000L; |
443 | *val2 = bias % 1000000000L; |
444 | |
445 | return IIO_VAL_INT_PLUS_NANO; |
446 | } |
447 | |
448 | static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st, |
449 | struct iio_chan_spec const *chan, |
450 | int val, int val2) |
451 | { |
452 | struct device *dev = regmap_get_device(map: st->map); |
453 | int64_t val64, min, max; |
454 | unsigned int reg, regval; |
455 | int16_t offset; |
456 | int ret; |
457 | |
458 | if (chan->type != IIO_ANGL_VEL) |
459 | return -EINVAL; |
460 | |
461 | switch (chan->channel2) { |
462 | case IIO_MOD_X: |
463 | reg = INV_ICM42600_REG_OFFSET_USER0; |
464 | break; |
465 | case IIO_MOD_Y: |
466 | reg = INV_ICM42600_REG_OFFSET_USER1; |
467 | break; |
468 | case IIO_MOD_Z: |
469 | reg = INV_ICM42600_REG_OFFSET_USER3; |
470 | break; |
471 | default: |
472 | return -EINVAL; |
473 | } |
474 | |
475 | /* inv_icm42600_gyro_calibbias: min - step - max in nano */ |
476 | min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL + |
477 | (int64_t)inv_icm42600_gyro_calibbias[1]; |
478 | max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL + |
479 | (int64_t)inv_icm42600_gyro_calibbias[5]; |
480 | val64 = (int64_t)val * 1000000000LL + (int64_t)val2; |
481 | if (val64 < min || val64 > max) |
482 | return -EINVAL; |
483 | |
484 | /* |
485 | * convert rad/s to dps then to raw value |
486 | * rad to dps: 180 / Pi |
487 | * dps to raw 12 bits signed, max 64: 2048 / 64 |
488 | * val in nano (1000000000) |
489 | * val * 180 * 2048 / (Pi * 1000000000 * 64) |
490 | */ |
491 | val64 = val64 * 180LL * 2048LL; |
492 | /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */ |
493 | if (val64 >= 0) |
494 | val64 += 3141592653LL * 64LL / 2LL; |
495 | else |
496 | val64 -= 3141592653LL * 64LL / 2LL; |
497 | offset = div64_s64(dividend: val64, divisor: 3141592653LL * 64LL); |
498 | |
499 | /* clamp value limited to 12 bits signed */ |
500 | if (offset < -2048) |
501 | offset = -2048; |
502 | else if (offset > 2047) |
503 | offset = 2047; |
504 | |
505 | pm_runtime_get_sync(dev); |
506 | mutex_lock(&st->lock); |
507 | |
508 | switch (chan->channel2) { |
509 | case IIO_MOD_X: |
510 | /* OFFSET_USER1 register is shared */ |
511 | ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER1, |
512 | val: ®val); |
513 | if (ret) |
514 | goto out_unlock; |
515 | st->buffer[0] = offset & 0xFF; |
516 | st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8); |
517 | break; |
518 | case IIO_MOD_Y: |
519 | /* OFFSET_USER1 register is shared */ |
520 | ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER1, |
521 | val: ®val); |
522 | if (ret) |
523 | goto out_unlock; |
524 | st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); |
525 | st->buffer[1] = offset & 0xFF; |
526 | break; |
527 | case IIO_MOD_Z: |
528 | /* OFFSET_USER4 register is shared */ |
529 | ret = regmap_read(map: st->map, INV_ICM42600_REG_OFFSET_USER4, |
530 | val: ®val); |
531 | if (ret) |
532 | goto out_unlock; |
533 | st->buffer[0] = offset & 0xFF; |
534 | st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8); |
535 | break; |
536 | default: |
537 | ret = -EINVAL; |
538 | goto out_unlock; |
539 | } |
540 | |
541 | ret = regmap_bulk_write(map: st->map, reg, val: st->buffer, val_count: 2); |
542 | |
543 | out_unlock: |
544 | mutex_unlock(lock: &st->lock); |
545 | pm_runtime_mark_last_busy(dev); |
546 | pm_runtime_put_autosuspend(dev); |
547 | return ret; |
548 | } |
549 | |
550 | static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev, |
551 | struct iio_chan_spec const *chan, |
552 | int *val, int *val2, long mask) |
553 | { |
554 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
555 | int16_t data; |
556 | int ret; |
557 | |
558 | switch (chan->type) { |
559 | case IIO_ANGL_VEL: |
560 | break; |
561 | case IIO_TEMP: |
562 | return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask); |
563 | default: |
564 | return -EINVAL; |
565 | } |
566 | |
567 | switch (mask) { |
568 | case IIO_CHAN_INFO_RAW: |
569 | ret = iio_device_claim_direct_mode(indio_dev); |
570 | if (ret) |
571 | return ret; |
572 | ret = inv_icm42600_gyro_read_sensor(st, chan, val: &data); |
573 | iio_device_release_direct_mode(indio_dev); |
574 | if (ret) |
575 | return ret; |
576 | *val = data; |
577 | return IIO_VAL_INT; |
578 | case IIO_CHAN_INFO_SCALE: |
579 | return inv_icm42600_gyro_read_scale(st, val, val2); |
580 | case IIO_CHAN_INFO_SAMP_FREQ: |
581 | return inv_icm42600_gyro_read_odr(st, val, val2); |
582 | case IIO_CHAN_INFO_CALIBBIAS: |
583 | return inv_icm42600_gyro_read_offset(st, chan, val, val2); |
584 | default: |
585 | return -EINVAL; |
586 | } |
587 | } |
588 | |
589 | static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev, |
590 | struct iio_chan_spec const *chan, |
591 | const int **vals, |
592 | int *type, int *length, long mask) |
593 | { |
594 | if (chan->type != IIO_ANGL_VEL) |
595 | return -EINVAL; |
596 | |
597 | switch (mask) { |
598 | case IIO_CHAN_INFO_SCALE: |
599 | *vals = inv_icm42600_gyro_scale; |
600 | *type = IIO_VAL_INT_PLUS_NANO; |
601 | *length = ARRAY_SIZE(inv_icm42600_gyro_scale); |
602 | return IIO_AVAIL_LIST; |
603 | case IIO_CHAN_INFO_SAMP_FREQ: |
604 | *vals = inv_icm42600_gyro_odr; |
605 | *type = IIO_VAL_INT_PLUS_MICRO; |
606 | *length = ARRAY_SIZE(inv_icm42600_gyro_odr); |
607 | return IIO_AVAIL_LIST; |
608 | case IIO_CHAN_INFO_CALIBBIAS: |
609 | *vals = inv_icm42600_gyro_calibbias; |
610 | *type = IIO_VAL_INT_PLUS_NANO; |
611 | return IIO_AVAIL_RANGE; |
612 | default: |
613 | return -EINVAL; |
614 | } |
615 | } |
616 | |
617 | static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev, |
618 | struct iio_chan_spec const *chan, |
619 | int val, int val2, long mask) |
620 | { |
621 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
622 | int ret; |
623 | |
624 | if (chan->type != IIO_ANGL_VEL) |
625 | return -EINVAL; |
626 | |
627 | switch (mask) { |
628 | case IIO_CHAN_INFO_SCALE: |
629 | ret = iio_device_claim_direct_mode(indio_dev); |
630 | if (ret) |
631 | return ret; |
632 | ret = inv_icm42600_gyro_write_scale(st, val, val2); |
633 | iio_device_release_direct_mode(indio_dev); |
634 | return ret; |
635 | case IIO_CHAN_INFO_SAMP_FREQ: |
636 | return inv_icm42600_gyro_write_odr(indio_dev, val, val2); |
637 | case IIO_CHAN_INFO_CALIBBIAS: |
638 | ret = iio_device_claim_direct_mode(indio_dev); |
639 | if (ret) |
640 | return ret; |
641 | ret = inv_icm42600_gyro_write_offset(st, chan, val, val2); |
642 | iio_device_release_direct_mode(indio_dev); |
643 | return ret; |
644 | default: |
645 | return -EINVAL; |
646 | } |
647 | } |
648 | |
649 | static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev, |
650 | struct iio_chan_spec const *chan, |
651 | long mask) |
652 | { |
653 | if (chan->type != IIO_ANGL_VEL) |
654 | return -EINVAL; |
655 | |
656 | switch (mask) { |
657 | case IIO_CHAN_INFO_SCALE: |
658 | return IIO_VAL_INT_PLUS_NANO; |
659 | case IIO_CHAN_INFO_SAMP_FREQ: |
660 | return IIO_VAL_INT_PLUS_MICRO; |
661 | case IIO_CHAN_INFO_CALIBBIAS: |
662 | return IIO_VAL_INT_PLUS_NANO; |
663 | default: |
664 | return -EINVAL; |
665 | } |
666 | } |
667 | |
668 | static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev, |
669 | unsigned int val) |
670 | { |
671 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
672 | int ret; |
673 | |
674 | mutex_lock(&st->lock); |
675 | |
676 | st->fifo.watermark.gyro = val; |
677 | ret = inv_icm42600_buffer_update_watermark(st); |
678 | |
679 | mutex_unlock(lock: &st->lock); |
680 | |
681 | return ret; |
682 | } |
683 | |
684 | static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev, |
685 | unsigned int count) |
686 | { |
687 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
688 | int ret; |
689 | |
690 | if (count == 0) |
691 | return 0; |
692 | |
693 | mutex_lock(&st->lock); |
694 | |
695 | ret = inv_icm42600_buffer_hwfifo_flush(st, count); |
696 | if (!ret) |
697 | ret = st->fifo.nb.gyro; |
698 | |
699 | mutex_unlock(lock: &st->lock); |
700 | |
701 | return ret; |
702 | } |
703 | |
704 | static const struct iio_info inv_icm42600_gyro_info = { |
705 | .read_raw = inv_icm42600_gyro_read_raw, |
706 | .read_avail = inv_icm42600_gyro_read_avail, |
707 | .write_raw = inv_icm42600_gyro_write_raw, |
708 | .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt, |
709 | .debugfs_reg_access = inv_icm42600_debugfs_reg, |
710 | .update_scan_mode = inv_icm42600_gyro_update_scan_mode, |
711 | .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark, |
712 | .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush, |
713 | }; |
714 | |
715 | struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st) |
716 | { |
717 | struct device *dev = regmap_get_device(map: st->map); |
718 | const char *name; |
719 | struct inv_sensors_timestamp_chip ts_chip; |
720 | struct inv_sensors_timestamp *ts; |
721 | struct iio_dev *indio_dev; |
722 | int ret; |
723 | |
724 | name = devm_kasprintf(dev, GFP_KERNEL, fmt: "%s-gyro" , st->name); |
725 | if (!name) |
726 | return ERR_PTR(error: -ENOMEM); |
727 | |
728 | indio_dev = devm_iio_device_alloc(parent: dev, sizeof_priv: sizeof(*ts)); |
729 | if (!indio_dev) |
730 | return ERR_PTR(error: -ENOMEM); |
731 | |
732 | /* |
733 | * clock period is 32kHz (31250ns) |
734 | * jitter is +/- 2% (20 per mille) |
735 | */ |
736 | ts_chip.clock_period = 31250; |
737 | ts_chip.jitter = 20; |
738 | ts_chip.init_period = inv_icm42600_odr_to_period(odr: st->conf.accel.odr); |
739 | ts = iio_priv(indio_dev); |
740 | inv_sensors_timestamp_init(ts, chip: &ts_chip); |
741 | |
742 | iio_device_set_drvdata(indio_dev, data: st); |
743 | indio_dev->name = name; |
744 | indio_dev->info = &inv_icm42600_gyro_info; |
745 | indio_dev->modes = INDIO_DIRECT_MODE; |
746 | indio_dev->channels = inv_icm42600_gyro_channels; |
747 | indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels); |
748 | indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks; |
749 | indio_dev->setup_ops = &inv_icm42600_buffer_ops; |
750 | |
751 | ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, |
752 | &inv_icm42600_buffer_ops); |
753 | if (ret) |
754 | return ERR_PTR(error: ret); |
755 | |
756 | ret = devm_iio_device_register(dev, indio_dev); |
757 | if (ret) |
758 | return ERR_PTR(error: ret); |
759 | |
760 | return indio_dev; |
761 | } |
762 | |
763 | int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev) |
764 | { |
765 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
766 | struct inv_sensors_timestamp *ts = iio_priv(indio_dev); |
767 | ssize_t i, size; |
768 | unsigned int no; |
769 | const void *accel, *gyro, *timestamp; |
770 | const int8_t *temp; |
771 | unsigned int odr; |
772 | int64_t ts_val; |
773 | struct inv_icm42600_gyro_buffer buffer; |
774 | |
775 | /* parse all fifo packets */ |
776 | for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { |
777 | size = inv_icm42600_fifo_decode_packet(packet: &st->fifo.data[i], |
778 | accel: &accel, gyro: &gyro, temp: &temp, timestamp: ×tamp, odr: &odr); |
779 | /* quit if error or FIFO is empty */ |
780 | if (size <= 0) |
781 | return size; |
782 | |
783 | /* skip packet if no gyro data or data is invalid */ |
784 | if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(s: gyro)) |
785 | continue; |
786 | |
787 | /* update odr */ |
788 | if (odr & INV_ICM42600_SENSOR_GYRO) |
789 | inv_sensors_timestamp_apply_odr(ts, fifo_period: st->fifo.period, |
790 | fifo_nb: st->fifo.nb.total, fifo_no: no); |
791 | |
792 | /* buffer is copied to userspace, zeroing it to avoid any data leak */ |
793 | memset(&buffer, 0, sizeof(buffer)); |
794 | memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro)); |
795 | /* convert 8 bits FIFO temperature in high resolution format */ |
796 | buffer.temp = temp ? (*temp * 64) : 0; |
797 | ts_val = inv_sensors_timestamp_pop(ts); |
798 | iio_push_to_buffers_with_timestamp(indio_dev, data: &buffer, timestamp: ts_val); |
799 | } |
800 | |
801 | return 0; |
802 | } |
803 | |