1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright (C) 2012 Invensense, Inc.
4 */
5
6 #include <linux/module.h>
7 #include <linux/slab.h>
8 #include <linux/err.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 <asm/unaligned.h>
17 #include "inv_mpu_iio.h"
18
19 /**
20 * inv_mpu6050_update_period() - Update chip internal period estimation
21 *
22 * @st: driver state
23 * @timestamp: the interrupt timestamp
24 * @nb: number of data set in the fifo
25 *
26 * This function uses interrupt timestamps to estimate the chip period and
27 * to choose the data timestamp to come.
28 */
inv_mpu6050_update_period(struct inv_mpu6050_state * st,s64 timestamp,size_t nb)29 static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
30 s64 timestamp, size_t nb)
31 {
32 /* Period boundaries for accepting timestamp */
33 const s64 period_min =
34 (NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100;
35 const s64 period_max =
36 (NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100;
37 const s32 divider = INV_MPU6050_FREQ_DIVIDER(st);
38 s64 delta, interval;
39 bool use_it_timestamp = false;
40
41 if (st->it_timestamp == 0) {
42 /* not initialized, forced to use it_timestamp */
43 use_it_timestamp = true;
44 } else if (nb == 1) {
45 /*
46 * Validate the use of it timestamp by checking if interrupt
47 * has been delayed.
48 * nb > 1 means interrupt was delayed for more than 1 sample,
49 * so it's obviously not good.
50 * Compute the chip period between 2 interrupts for validating.
51 */
52 delta = div_s64(timestamp - st->it_timestamp, divider);
53 if (delta > period_min && delta < period_max) {
54 /* update chip period and use it timestamp */
55 st->chip_period = (st->chip_period + delta) / 2;
56 use_it_timestamp = true;
57 }
58 }
59
60 if (use_it_timestamp) {
61 /*
62 * Manage case of multiple samples in the fifo (nb > 1):
63 * compute timestamp corresponding to the first sample using
64 * estimated chip period.
65 */
66 interval = (nb - 1) * st->chip_period * divider;
67 st->data_timestamp = timestamp - interval;
68 }
69
70 /* save it timestamp */
71 st->it_timestamp = timestamp;
72 }
73
74 /**
75 * inv_mpu6050_get_timestamp() - Return the current data timestamp
76 *
77 * @st: driver state
78 * @return: current data timestamp
79 *
80 * This function returns the current data timestamp and prepares for next one.
81 */
inv_mpu6050_get_timestamp(struct inv_mpu6050_state * st)82 static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
83 {
84 s64 ts;
85
86 /* return current data timestamp and increment */
87 ts = st->data_timestamp;
88 st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st);
89
90 return ts;
91 }
92
inv_reset_fifo(struct iio_dev * indio_dev)93 int inv_reset_fifo(struct iio_dev *indio_dev)
94 {
95 int result;
96 u8 d;
97 struct inv_mpu6050_state *st = iio_priv(indio_dev);
98
99 /* reset it timestamp validation */
100 st->it_timestamp = 0;
101
102 /* disable interrupt */
103 result = regmap_write(st->map, st->reg->int_enable, 0);
104 if (result) {
105 dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
106 result);
107 return result;
108 }
109 /* disable the sensor output to FIFO */
110 result = regmap_write(st->map, st->reg->fifo_en, 0);
111 if (result)
112 goto reset_fifo_fail;
113 /* disable fifo reading */
114 result = regmap_write(st->map, st->reg->user_ctrl,
115 st->chip_config.user_ctrl);
116 if (result)
117 goto reset_fifo_fail;
118
119 /* reset FIFO*/
120 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
121 result = regmap_write(st->map, st->reg->user_ctrl, d);
122 if (result)
123 goto reset_fifo_fail;
124
125 /* enable interrupt */
126 if (st->chip_config.accl_fifo_enable ||
127 st->chip_config.gyro_fifo_enable) {
128 result = regmap_write(st->map, st->reg->int_enable,
129 INV_MPU6050_BIT_DATA_RDY_EN);
130 if (result)
131 return result;
132 }
133 /* enable FIFO reading */
134 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
135 result = regmap_write(st->map, st->reg->user_ctrl, d);
136 if (result)
137 goto reset_fifo_fail;
138 /* enable sensor output to FIFO */
139 d = 0;
140 if (st->chip_config.gyro_fifo_enable)
141 d |= INV_MPU6050_BITS_GYRO_OUT;
142 if (st->chip_config.accl_fifo_enable)
143 d |= INV_MPU6050_BIT_ACCEL_OUT;
144 result = regmap_write(st->map, st->reg->fifo_en, d);
145 if (result)
146 goto reset_fifo_fail;
147
148 return 0;
149
150 reset_fifo_fail:
151 dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
152 result = regmap_write(st->map, st->reg->int_enable,
153 INV_MPU6050_BIT_DATA_RDY_EN);
154
155 return result;
156 }
157
158 /**
159 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
160 */
inv_mpu6050_read_fifo(int irq,void * p)161 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
162 {
163 struct iio_poll_func *pf = p;
164 struct iio_dev *indio_dev = pf->indio_dev;
165 struct inv_mpu6050_state *st = iio_priv(indio_dev);
166 size_t bytes_per_datum;
167 int result;
168 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
169 u16 fifo_count;
170 s64 timestamp;
171 int int_status;
172 size_t i, nb;
173
174 mutex_lock(&st->lock);
175
176 /* ack interrupt and check status */
177 result = regmap_read(st->map, st->reg->int_status, &int_status);
178 if (result) {
179 dev_err(regmap_get_device(st->map),
180 "failed to ack interrupt\n");
181 goto flush_fifo;
182 }
183 if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) {
184 dev_warn(regmap_get_device(st->map),
185 "spurious interrupt with status 0x%x\n", int_status);
186 goto end_session;
187 }
188
189 if (!(st->chip_config.accl_fifo_enable |
190 st->chip_config.gyro_fifo_enable))
191 goto end_session;
192 bytes_per_datum = 0;
193 if (st->chip_config.accl_fifo_enable)
194 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
195
196 if (st->chip_config.gyro_fifo_enable)
197 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
198
199 if (st->chip_type == INV_ICM20602)
200 bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;
201
202 /*
203 * read fifo_count register to know how many bytes are inside the FIFO
204 * right now
205 */
206 result = regmap_bulk_read(st->map, st->reg->fifo_count_h, data,
207 INV_MPU6050_FIFO_COUNT_BYTE);
208 if (result)
209 goto end_session;
210 fifo_count = get_unaligned_be16(&data[0]);
211
212 /*
213 * Handle fifo overflow by resetting fifo.
214 * Reset if there is only 3 data set free remaining to mitigate
215 * possible delay between reading fifo count and fifo data.
216 */
217 nb = 3 * bytes_per_datum;
218 if (fifo_count >= st->hw->fifo_size - nb) {
219 dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
220 goto flush_fifo;
221 }
222
223 /* compute and process all complete datum */
224 nb = fifo_count / bytes_per_datum;
225 inv_mpu6050_update_period(st, pf->timestamp, nb);
226 for (i = 0; i < nb; ++i) {
227 result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
228 data, bytes_per_datum);
229 if (result)
230 goto flush_fifo;
231 /* skip first samples if needed */
232 if (st->skip_samples) {
233 st->skip_samples--;
234 continue;
235 }
236 timestamp = inv_mpu6050_get_timestamp(st);
237 iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
238 }
239
240 end_session:
241 mutex_unlock(&st->lock);
242 iio_trigger_notify_done(indio_dev->trig);
243
244 return IRQ_HANDLED;
245
246 flush_fifo:
247 /* Flush HW and SW FIFOs. */
248 inv_reset_fifo(indio_dev);
249 mutex_unlock(&st->lock);
250 iio_trigger_notify_done(indio_dev->trig);
251
252 return IRQ_HANDLED;
253 }
254