1 // SPDX-License-Identifier: GPL-2.0-only
3 * Copyright (C) 2012 Invensense, Inc.
6 #include <linux/module.h>
7 #include <linux/slab.h>
9 #include <linux/delay.h>
10 #include <linux/sysfs.h>
11 #include <linux/jiffies.h>
12 #include <linux/irq.h>
13 #include <linux/interrupt.h>
14 #include <linux/poll.h>
15 #include <linux/math64.h>
16 #include "inv_mpu_iio.h"
19 * inv_mpu6050_update_period() - Update chip internal period estimation
22 * @timestamp: the interrupt timestamp
23 * @nb: number of data set in the fifo
25 * This function uses interrupt timestamps to estimate the chip period and
26 * to choose the data timestamp to come.
28 static void inv_mpu6050_update_period(struct inv_mpu6050_state
*st
,
29 s64 timestamp
, size_t nb
)
31 /* Period boundaries for accepting timestamp */
32 const s64 period_min
=
33 (NSEC_PER_MSEC
* (100 - INV_MPU6050_TS_PERIOD_JITTER
)) / 100;
34 const s64 period_max
=
35 (NSEC_PER_MSEC
* (100 + INV_MPU6050_TS_PERIOD_JITTER
)) / 100;
36 const s32 divider
= INV_MPU6050_FREQ_DIVIDER(st
);
38 bool use_it_timestamp
= false;
40 if (st
->it_timestamp
== 0) {
41 /* not initialized, forced to use it_timestamp */
42 use_it_timestamp
= true;
45 * Validate the use of it timestamp by checking if interrupt
47 * nb > 1 means interrupt was delayed for more than 1 sample,
48 * so it's obviously not good.
49 * Compute the chip period between 2 interrupts for validating.
51 delta
= div_s64(timestamp
- st
->it_timestamp
, divider
);
52 if (delta
> period_min
&& delta
< period_max
) {
53 /* update chip period and use it timestamp */
54 st
->chip_period
= (st
->chip_period
+ delta
) / 2;
55 use_it_timestamp
= true;
59 if (use_it_timestamp
) {
61 * Manage case of multiple samples in the fifo (nb > 1):
62 * compute timestamp corresponding to the first sample using
63 * estimated chip period.
65 interval
= (nb
- 1) * st
->chip_period
* divider
;
66 st
->data_timestamp
= timestamp
- interval
;
69 /* save it timestamp */
70 st
->it_timestamp
= timestamp
;
74 * inv_mpu6050_get_timestamp() - Return the current data timestamp
77 * @return: current data timestamp
79 * This function returns the current data timestamp and prepares for next one.
81 static s64
inv_mpu6050_get_timestamp(struct inv_mpu6050_state
*st
)
85 /* return current data timestamp and increment */
86 ts
= st
->data_timestamp
;
87 st
->data_timestamp
+= st
->chip_period
* INV_MPU6050_FREQ_DIVIDER(st
);
92 static int inv_reset_fifo(struct iio_dev
*indio_dev
)
95 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
97 /* disable fifo and reenable it */
98 inv_mpu6050_prepare_fifo(st
, false);
99 result
= inv_mpu6050_prepare_fifo(st
, true);
101 goto reset_fifo_fail
;
106 dev_err(regmap_get_device(st
->map
), "reset fifo failed %d\n", result
);
107 result
= regmap_write(st
->map
, st
->reg
->int_enable
,
108 INV_MPU6050_BIT_DATA_RDY_EN
);
114 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
116 irqreturn_t
inv_mpu6050_read_fifo(int irq
, void *p
)
118 struct iio_poll_func
*pf
= p
;
119 struct iio_dev
*indio_dev
= pf
->indio_dev
;
120 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
121 size_t bytes_per_datum
;
128 mutex_lock(&st
->lock
);
130 /* ack interrupt and check status */
131 result
= regmap_read(st
->map
, st
->reg
->int_status
, &int_status
);
133 dev_err(regmap_get_device(st
->map
),
134 "failed to ack interrupt\n");
137 if (!(int_status
& INV_MPU6050_BIT_RAW_DATA_RDY_INT
))
140 if (!(st
->chip_config
.accl_fifo_enable
|
141 st
->chip_config
.gyro_fifo_enable
|
142 st
->chip_config
.magn_fifo_enable
))
145 if (st
->chip_config
.accl_fifo_enable
)
146 bytes_per_datum
+= INV_MPU6050_BYTES_PER_3AXIS_SENSOR
;
148 if (st
->chip_config
.gyro_fifo_enable
)
149 bytes_per_datum
+= INV_MPU6050_BYTES_PER_3AXIS_SENSOR
;
151 if (st
->chip_config
.temp_fifo_enable
)
152 bytes_per_datum
+= INV_MPU6050_BYTES_PER_TEMP_SENSOR
;
154 if (st
->chip_config
.magn_fifo_enable
)
155 bytes_per_datum
+= INV_MPU9X50_BYTES_MAGN
;
158 * read fifo_count register to know how many bytes are inside the FIFO
161 result
= regmap_bulk_read(st
->map
, st
->reg
->fifo_count_h
,
162 st
->data
, INV_MPU6050_FIFO_COUNT_BYTE
);
165 fifo_count
= be16_to_cpup((__be16
*)&st
->data
[0]);
168 * Handle fifo overflow by resetting fifo.
169 * Reset if there is only 3 data set free remaining to mitigate
170 * possible delay between reading fifo count and fifo data.
172 nb
= 3 * bytes_per_datum
;
173 if (fifo_count
>= st
->hw
->fifo_size
- nb
) {
174 dev_warn(regmap_get_device(st
->map
), "fifo overflow reset\n");
178 /* compute and process all complete datum */
179 nb
= fifo_count
/ bytes_per_datum
;
180 inv_mpu6050_update_period(st
, pf
->timestamp
, nb
);
181 for (i
= 0; i
< nb
; ++i
) {
182 result
= regmap_noinc_read(st
->map
, st
->reg
->fifo_r_w
,
183 st
->data
, bytes_per_datum
);
186 /* skip first samples if needed */
187 if (st
->skip_samples
) {
191 timestamp
= inv_mpu6050_get_timestamp(st
);
192 iio_push_to_buffers_with_timestamp(indio_dev
, st
->data
, timestamp
);
196 mutex_unlock(&st
->lock
);
197 iio_trigger_notify_done(indio_dev
->trig
);
202 /* Flush HW and SW FIFOs. */
203 inv_reset_fifo(indio_dev
);
204 mutex_unlock(&st
->lock
);
205 iio_trigger_notify_done(indio_dev
->trig
);