1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * Copyright (C) 2020 Invensense, Inc.
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>
13 #include <linux/iio/buffer.h>
14 #include <linux/iio/common/inv_sensors_timestamp.h>
15 #include <linux/iio/iio.h>
17 #include "inv_icm42600.h"
18 #include "inv_icm42600_buffer.h"
20 /* FIFO header: 1 byte */
21 #define INV_ICM42600_FIFO_HEADER_MSG BIT(7)
22 #define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6)
23 #define INV_ICM42600_FIFO_HEADER_GYRO BIT(5)
24 #define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2)
25 #define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1)
26 #define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0)
28 struct inv_icm42600_fifo_1sensor_packet
{
30 struct inv_icm42600_fifo_sensor_data data
;
33 #define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8
35 struct inv_icm42600_fifo_2sensors_packet
{
37 struct inv_icm42600_fifo_sensor_data accel
;
38 struct inv_icm42600_fifo_sensor_data gyro
;
42 #define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16
44 ssize_t
inv_icm42600_fifo_decode_packet(const void *packet
, const void **accel
,
45 const void **gyro
, const int8_t **temp
,
46 const void **timestamp
, unsigned int *odr
)
48 const struct inv_icm42600_fifo_1sensor_packet
*pack1
= packet
;
49 const struct inv_icm42600_fifo_2sensors_packet
*pack2
= packet
;
50 uint8_t header
= *((const uint8_t *)packet
);
53 if (header
& INV_ICM42600_FIFO_HEADER_MSG
) {
62 /* handle odr flags */
64 if (header
& INV_ICM42600_FIFO_HEADER_ODR_GYRO
)
65 *odr
|= INV_ICM42600_SENSOR_GYRO
;
66 if (header
& INV_ICM42600_FIFO_HEADER_ODR_ACCEL
)
67 *odr
|= INV_ICM42600_SENSOR_ACCEL
;
70 if ((header
& INV_ICM42600_FIFO_HEADER_ACCEL
) &&
71 (header
& INV_ICM42600_FIFO_HEADER_GYRO
)) {
72 *accel
= &pack2
->accel
;
75 *timestamp
= &pack2
->timestamp
;
76 return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE
;
80 if (header
& INV_ICM42600_FIFO_HEADER_ACCEL
) {
81 *accel
= &pack1
->data
;
85 return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE
;
89 if (header
& INV_ICM42600_FIFO_HEADER_GYRO
) {
94 return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE
;
97 /* invalid packet if here */
101 void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state
*st
)
103 uint32_t period_gyro
, period_accel
, period
;
105 if (st
->fifo
.en
& INV_ICM42600_SENSOR_GYRO
)
106 period_gyro
= inv_icm42600_odr_to_period(st
->conf
.gyro
.odr
);
108 period_gyro
= U32_MAX
;
110 if (st
->fifo
.en
& INV_ICM42600_SENSOR_ACCEL
)
111 period_accel
= inv_icm42600_odr_to_period(st
->conf
.accel
.odr
);
113 period_accel
= U32_MAX
;
115 if (period_gyro
<= period_accel
)
116 period
= period_gyro
;
118 period
= period_accel
;
120 st
->fifo
.period
= period
;
123 int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state
*st
,
124 unsigned int fifo_en
)
126 unsigned int mask
, val
;
129 /* update only FIFO EN bits */
130 mask
= INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN
|
131 INV_ICM42600_FIFO_CONFIG1_TEMP_EN
|
132 INV_ICM42600_FIFO_CONFIG1_GYRO_EN
|
133 INV_ICM42600_FIFO_CONFIG1_ACCEL_EN
;
136 if (fifo_en
& INV_ICM42600_SENSOR_GYRO
)
137 val
|= INV_ICM42600_FIFO_CONFIG1_GYRO_EN
;
138 if (fifo_en
& INV_ICM42600_SENSOR_ACCEL
)
139 val
|= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN
;
140 if (fifo_en
& INV_ICM42600_SENSOR_TEMP
)
141 val
|= INV_ICM42600_FIFO_CONFIG1_TEMP_EN
;
143 ret
= regmap_update_bits(st
->map
, INV_ICM42600_REG_FIFO_CONFIG1
, mask
, val
);
147 st
->fifo
.en
= fifo_en
;
148 inv_icm42600_buffer_update_fifo_period(st
);
153 static size_t inv_icm42600_get_packet_size(unsigned int fifo_en
)
157 if ((fifo_en
& INV_ICM42600_SENSOR_GYRO
) &&
158 (fifo_en
& INV_ICM42600_SENSOR_ACCEL
))
159 packet_size
= INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE
;
161 packet_size
= INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE
;
166 static unsigned int inv_icm42600_wm_truncate(unsigned int watermark
,
172 wm_size
= watermark
* packet_size
;
173 if (wm_size
> INV_ICM42600_FIFO_WATERMARK_MAX
)
174 wm_size
= INV_ICM42600_FIFO_WATERMARK_MAX
;
176 wm
= wm_size
/ packet_size
;
182 * inv_icm42600_buffer_update_watermark - update watermark FIFO threshold
183 * @st: driver internal state
185 * Returns 0 on success, a negative error code otherwise.
187 * FIFO watermark threshold is computed based on the required watermark values
188 * set for gyro and accel sensors. Since watermark is all about acceptable data
189 * latency, use the smallest setting between the 2. It means choosing the
190 * smallest latency but this is not as simple as choosing the smallest watermark
191 * value. Latency depends on watermark and ODR. It requires several steps:
192 * 1) compute gyro and accel latencies and choose the smallest value.
193 * 2) adapt the choosen latency so that it is a multiple of both gyro and accel
194 * ones. Otherwise it is possible that you don't meet a requirement. (for
195 * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the
196 * value of 4 will not meet accel latency requirement because 6 is not a
197 * multiple of 4. You need to use the value 2.)
198 * 3) Since all periods are multiple of each others, watermark is computed by
199 * dividing this computed latency by the smallest period, which corresponds
200 * to the FIFO frequency. Beware that this is only true because we are not
201 * using 500Hz frequency which is not a multiple of the others.
203 int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state
*st
)
205 size_t packet_size
, wm_size
;
206 unsigned int wm_gyro
, wm_accel
, watermark
;
207 uint32_t period_gyro
, period_accel
, period
;
208 uint32_t latency_gyro
, latency_accel
, latency
;
213 packet_size
= inv_icm42600_get_packet_size(st
->fifo
.en
);
215 /* compute sensors latency, depending on sensor watermark and odr */
216 wm_gyro
= inv_icm42600_wm_truncate(st
->fifo
.watermark
.gyro
, packet_size
);
217 wm_accel
= inv_icm42600_wm_truncate(st
->fifo
.watermark
.accel
, packet_size
);
218 /* use us for odr to avoid overflow using 32 bits values */
219 period_gyro
= inv_icm42600_odr_to_period(st
->conf
.gyro
.odr
) / 1000UL;
220 period_accel
= inv_icm42600_odr_to_period(st
->conf
.accel
.odr
) / 1000UL;
221 latency_gyro
= period_gyro
* wm_gyro
;
222 latency_accel
= period_accel
* wm_accel
;
224 /* 0 value for watermark means that the sensor is turned off */
225 if (wm_gyro
== 0 && wm_accel
== 0)
228 if (latency_gyro
== 0) {
229 watermark
= wm_accel
;
230 st
->fifo
.watermark
.eff_accel
= wm_accel
;
231 } else if (latency_accel
== 0) {
233 st
->fifo
.watermark
.eff_gyro
= wm_gyro
;
235 /* compute the smallest latency that is a multiple of both */
236 if (latency_gyro
<= latency_accel
)
237 latency
= latency_gyro
- (latency_accel
% latency_gyro
);
239 latency
= latency_accel
- (latency_gyro
% latency_accel
);
240 /* use the shortest period */
241 if (period_gyro
<= period_accel
)
242 period
= period_gyro
;
244 period
= period_accel
;
245 /* all this works because periods are multiple of each others */
246 watermark
= latency
/ period
;
249 /* update effective watermark */
250 st
->fifo
.watermark
.eff_gyro
= latency
/ period_gyro
;
251 if (st
->fifo
.watermark
.eff_gyro
< 1)
252 st
->fifo
.watermark
.eff_gyro
= 1;
253 st
->fifo
.watermark
.eff_accel
= latency
/ period_accel
;
254 if (st
->fifo
.watermark
.eff_accel
< 1)
255 st
->fifo
.watermark
.eff_accel
= 1;
258 /* compute watermark value in bytes */
259 wm_size
= watermark
* packet_size
;
261 /* changing FIFO watermark requires to turn off watermark interrupt */
262 ret
= regmap_update_bits_check(st
->map
, INV_ICM42600_REG_INT_SOURCE0
,
263 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN
,
268 raw_wm
= INV_ICM42600_FIFO_WATERMARK_VAL(wm_size
);
269 memcpy(st
->buffer
, &raw_wm
, sizeof(raw_wm
));
270 ret
= regmap_bulk_write(st
->map
, INV_ICM42600_REG_FIFO_WATERMARK
,
271 st
->buffer
, sizeof(raw_wm
));
275 /* restore watermark interrupt */
277 ret
= regmap_set_bits(st
->map
, INV_ICM42600_REG_INT_SOURCE0
,
278 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN
);
286 static int inv_icm42600_buffer_preenable(struct iio_dev
*indio_dev
)
288 struct inv_icm42600_state
*st
= iio_device_get_drvdata(indio_dev
);
289 struct device
*dev
= regmap_get_device(st
->map
);
290 struct inv_icm42600_sensor_state
*sensor_st
= iio_priv(indio_dev
);
291 struct inv_sensors_timestamp
*ts
= &sensor_st
->ts
;
293 pm_runtime_get_sync(dev
);
295 mutex_lock(&st
->lock
);
296 inv_sensors_timestamp_reset(ts
);
297 mutex_unlock(&st
->lock
);
303 * update_scan_mode callback is turning sensors on and setting data FIFO enable
306 static int inv_icm42600_buffer_postenable(struct iio_dev
*indio_dev
)
308 struct inv_icm42600_state
*st
= iio_device_get_drvdata(indio_dev
);
311 mutex_lock(&st
->lock
);
313 /* exit if FIFO is already on */
319 /* set FIFO threshold interrupt */
320 ret
= regmap_set_bits(st
->map
, INV_ICM42600_REG_INT_SOURCE0
,
321 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN
);
325 /* flush FIFO data */
326 ret
= regmap_write(st
->map
, INV_ICM42600_REG_SIGNAL_PATH_RESET
,
327 INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH
);
331 /* set FIFO in streaming mode */
332 ret
= regmap_write(st
->map
, INV_ICM42600_REG_FIFO_CONFIG
,
333 INV_ICM42600_FIFO_CONFIG_STREAM
);
337 /* workaround: first read of FIFO count after reset is always 0 */
338 ret
= regmap_bulk_read(st
->map
, INV_ICM42600_REG_FIFO_COUNT
, st
->buffer
, 2);
343 /* increase FIFO on counter */
346 mutex_unlock(&st
->lock
);
350 static int inv_icm42600_buffer_predisable(struct iio_dev
*indio_dev
)
352 struct inv_icm42600_state
*st
= iio_device_get_drvdata(indio_dev
);
355 mutex_lock(&st
->lock
);
357 /* exit if there are several sensors using the FIFO */
358 if (st
->fifo
.on
> 1) {
363 /* set FIFO in bypass mode */
364 ret
= regmap_write(st
->map
, INV_ICM42600_REG_FIFO_CONFIG
,
365 INV_ICM42600_FIFO_CONFIG_BYPASS
);
369 /* flush FIFO data */
370 ret
= regmap_write(st
->map
, INV_ICM42600_REG_SIGNAL_PATH_RESET
,
371 INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH
);
375 /* disable FIFO threshold interrupt */
376 ret
= regmap_clear_bits(st
->map
, INV_ICM42600_REG_INT_SOURCE0
,
377 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN
);
382 /* decrease FIFO on counter */
385 mutex_unlock(&st
->lock
);
389 static int inv_icm42600_buffer_postdisable(struct iio_dev
*indio_dev
)
391 struct inv_icm42600_state
*st
= iio_device_get_drvdata(indio_dev
);
392 struct device
*dev
= regmap_get_device(st
->map
);
394 unsigned int *watermark
;
395 struct inv_icm42600_sensor_conf conf
= INV_ICM42600_SENSOR_CONF_INIT
;
396 unsigned int sleep_temp
= 0;
397 unsigned int sleep_sensor
= 0;
401 if (indio_dev
== st
->indio_gyro
) {
402 sensor
= INV_ICM42600_SENSOR_GYRO
;
403 watermark
= &st
->fifo
.watermark
.gyro
;
404 } else if (indio_dev
== st
->indio_accel
) {
405 sensor
= INV_ICM42600_SENSOR_ACCEL
;
406 watermark
= &st
->fifo
.watermark
.accel
;
411 mutex_lock(&st
->lock
);
413 ret
= inv_icm42600_buffer_set_fifo_en(st
, st
->fifo
.en
& ~sensor
);
418 ret
= inv_icm42600_buffer_update_watermark(st
);
422 conf
.mode
= INV_ICM42600_SENSOR_MODE_OFF
;
423 if (sensor
== INV_ICM42600_SENSOR_GYRO
)
424 ret
= inv_icm42600_set_gyro_conf(st
, &conf
, &sleep_sensor
);
426 ret
= inv_icm42600_set_accel_conf(st
, &conf
, &sleep_sensor
);
430 /* if FIFO is off, turn temperature off */
432 ret
= inv_icm42600_set_temp_conf(st
, false, &sleep_temp
);
435 mutex_unlock(&st
->lock
);
437 /* sleep maximum required time */
438 sleep
= max(sleep_sensor
, sleep_temp
);
442 pm_runtime_mark_last_busy(dev
);
443 pm_runtime_put_autosuspend(dev
);
448 const struct iio_buffer_setup_ops inv_icm42600_buffer_ops
= {
449 .preenable
= inv_icm42600_buffer_preenable
,
450 .postenable
= inv_icm42600_buffer_postenable
,
451 .predisable
= inv_icm42600_buffer_predisable
,
452 .postdisable
= inv_icm42600_buffer_postdisable
,
455 int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state
*st
,
459 __be16
*raw_fifo_count
;
461 const void *accel
, *gyro
, *timestamp
;
466 /* reset all samples counters */
468 st
->fifo
.nb
.gyro
= 0;
469 st
->fifo
.nb
.accel
= 0;
470 st
->fifo
.nb
.total
= 0;
472 /* compute maximum FIFO read size */
474 max_count
= sizeof(st
->fifo
.data
);
476 max_count
= max
* inv_icm42600_get_packet_size(st
->fifo
.en
);
478 /* read FIFO count value */
479 raw_fifo_count
= (__be16
*)st
->buffer
;
480 ret
= regmap_bulk_read(st
->map
, INV_ICM42600_REG_FIFO_COUNT
,
481 raw_fifo_count
, sizeof(*raw_fifo_count
));
484 st
->fifo
.count
= be16_to_cpup(raw_fifo_count
);
486 /* check and clamp FIFO count value */
487 if (st
->fifo
.count
== 0)
489 if (st
->fifo
.count
> max_count
)
490 st
->fifo
.count
= max_count
;
492 /* read all FIFO data in internal buffer */
493 ret
= regmap_noinc_read(st
->map
, INV_ICM42600_REG_FIFO_DATA
,
494 st
->fifo
.data
, st
->fifo
.count
);
498 /* compute number of samples for each sensor */
499 for (i
= 0; i
< st
->fifo
.count
; i
+= size
) {
500 size
= inv_icm42600_fifo_decode_packet(&st
->fifo
.data
[i
],
501 &accel
, &gyro
, &temp
, ×tamp
, &odr
);
504 if (gyro
!= NULL
&& inv_icm42600_fifo_is_data_valid(gyro
))
506 if (accel
!= NULL
&& inv_icm42600_fifo_is_data_valid(accel
))
514 int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state
*st
)
516 struct inv_icm42600_sensor_state
*gyro_st
= iio_priv(st
->indio_gyro
);
517 struct inv_icm42600_sensor_state
*accel_st
= iio_priv(st
->indio_accel
);
518 struct inv_sensors_timestamp
*ts
;
521 if (st
->fifo
.nb
.total
== 0)
524 /* handle gyroscope timestamp and FIFO data parsing */
525 if (st
->fifo
.nb
.gyro
> 0) {
527 inv_sensors_timestamp_interrupt(ts
, st
->fifo
.watermark
.eff_gyro
,
529 ret
= inv_icm42600_gyro_parse_fifo(st
->indio_gyro
);
534 /* handle accelerometer timestamp and FIFO data parsing */
535 if (st
->fifo
.nb
.accel
> 0) {
537 inv_sensors_timestamp_interrupt(ts
, st
->fifo
.watermark
.eff_accel
,
538 st
->timestamp
.accel
);
539 ret
= inv_icm42600_accel_parse_fifo(st
->indio_accel
);
547 int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state
*st
,
550 struct inv_icm42600_sensor_state
*gyro_st
= iio_priv(st
->indio_gyro
);
551 struct inv_icm42600_sensor_state
*accel_st
= iio_priv(st
->indio_accel
);
552 struct inv_sensors_timestamp
*ts
;
553 int64_t gyro_ts
, accel_ts
;
556 gyro_ts
= iio_get_time_ns(st
->indio_gyro
);
557 accel_ts
= iio_get_time_ns(st
->indio_accel
);
559 ret
= inv_icm42600_buffer_fifo_read(st
, count
);
563 if (st
->fifo
.nb
.total
== 0)
566 if (st
->fifo
.nb
.gyro
> 0) {
568 inv_sensors_timestamp_interrupt(ts
, st
->fifo
.nb
.gyro
, gyro_ts
);
569 ret
= inv_icm42600_gyro_parse_fifo(st
->indio_gyro
);
574 if (st
->fifo
.nb
.accel
> 0) {
576 inv_sensors_timestamp_interrupt(ts
, st
->fifo
.nb
.accel
, accel_ts
);
577 ret
= inv_icm42600_accel_parse_fifo(st
->indio_accel
);
585 int inv_icm42600_buffer_init(struct inv_icm42600_state
*st
)
590 st
->fifo
.watermark
.eff_gyro
= 1;
591 st
->fifo
.watermark
.eff_accel
= 1;
594 * Default FIFO configuration (bits 7 to 5)
595 * - use invalid value
596 * - FIFO count in bytes
597 * - FIFO count in big endian
599 val
= INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN
;
600 ret
= regmap_update_bits(st
->map
, INV_ICM42600_REG_INTF_CONFIG0
,
606 * Enable FIFO partial read and continuous watermark interrupt.
607 * Disable all FIFO EN bits.
609 val
= INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD
|
610 INV_ICM42600_FIFO_CONFIG1_WM_GT_TH
;
611 return regmap_update_bits(st
->map
, INV_ICM42600_REG_FIFO_CONFIG1
,
612 GENMASK(6, 5) | GENMASK(3, 0), val
);