1
2
3
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 "inv_mpu_iio.h"
17
18
19
20
21
22
23
24
25
26
27
28static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
29 s64 timestamp, size_t nb)
30{
31
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);
37 s64 delta, interval;
38 bool use_it_timestamp = false;
39
40 if (st->it_timestamp == 0) {
41
42 use_it_timestamp = true;
43 } else if (nb == 1) {
44
45
46
47
48
49
50
51 delta = div_s64(timestamp - st->it_timestamp, divider);
52 if (delta > period_min && delta < period_max) {
53
54 st->chip_period = (st->chip_period + delta) / 2;
55 use_it_timestamp = true;
56 }
57 }
58
59 if (use_it_timestamp) {
60
61
62
63
64
65 interval = (nb - 1) * st->chip_period * divider;
66 st->data_timestamp = timestamp - interval;
67 }
68
69
70 st->it_timestamp = timestamp;
71}
72
73
74
75
76
77
78
79
80
81static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
82{
83 s64 ts;
84
85
86 ts = st->data_timestamp;
87 st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st);
88
89 return ts;
90}
91
92static int inv_reset_fifo(struct iio_dev *indio_dev)
93{
94 int result;
95 struct inv_mpu6050_state *st = iio_priv(indio_dev);
96
97
98 inv_mpu6050_prepare_fifo(st, false);
99 result = inv_mpu6050_prepare_fifo(st, true);
100 if (result)
101 goto reset_fifo_fail;
102
103 return 0;
104
105reset_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);
109
110 return result;
111}
112
113
114
115
116irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
117{
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;
122 int result;
123 u16 fifo_count;
124 s64 timestamp;
125 int int_status;
126 size_t i, nb;
127
128 mutex_lock(&st->lock);
129
130
131 result = regmap_read(st->map, st->reg->int_status, &int_status);
132 if (result) {
133 dev_err(regmap_get_device(st->map),
134 "failed to ack interrupt\n");
135 goto flush_fifo;
136 }
137 if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
138 goto end_session;
139
140 if (!(st->chip_config.accl_fifo_enable |
141 st->chip_config.gyro_fifo_enable |
142 st->chip_config.magn_fifo_enable))
143 goto end_session;
144 bytes_per_datum = 0;
145 if (st->chip_config.accl_fifo_enable)
146 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
147
148 if (st->chip_config.gyro_fifo_enable)
149 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
150
151 if (st->chip_config.temp_fifo_enable)
152 bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
153
154 if (st->chip_config.magn_fifo_enable)
155 bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
156
157
158
159
160
161 result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
162 st->data, INV_MPU6050_FIFO_COUNT_BYTE);
163 if (result)
164 goto end_session;
165 fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
166
167
168
169
170
171
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");
175 goto flush_fifo;
176 }
177
178
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);
184 if (result)
185 goto flush_fifo;
186
187 if (st->skip_samples) {
188 st->skip_samples--;
189 continue;
190 }
191 timestamp = inv_mpu6050_get_timestamp(st);
192 iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp);
193 }
194
195end_session:
196 mutex_unlock(&st->lock);
197 iio_trigger_notify_done(indio_dev->trig);
198
199 return IRQ_HANDLED;
200
201flush_fifo:
202
203 inv_reset_fifo(indio_dev);
204 mutex_unlock(&st->lock);
205 iio_trigger_notify_done(indio_dev->trig);
206
207 return IRQ_HANDLED;
208}
209