1
2
3
4
5
6
7
8
9
10
11
12
13
14#include <linux/module.h>
15#include <linux/init.h>
16#include <linux/slab.h>
17#include <linux/i2c.h>
18#include <linux/err.h>
19#include <linux/delay.h>
20#include <linux/sysfs.h>
21#include <linux/jiffies.h>
22#include <linux/irq.h>
23#include <linux/interrupt.h>
24#include <linux/kfifo.h>
25#include <linux/poll.h>
26#include "inv_mpu_iio.h"
27
28int inv_reset_fifo(struct iio_dev *indio_dev)
29{
30 int result;
31 u8 d;
32 struct inv_mpu6050_state *st = iio_priv(indio_dev);
33
34
35 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
36 if (result) {
37 dev_err(&st->client->dev, "int_enable failed %d\n", result);
38 return result;
39 }
40
41 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
42 if (result)
43 goto reset_fifo_fail;
44
45 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
46 if (result)
47 goto reset_fifo_fail;
48
49
50 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
51 INV_MPU6050_BIT_FIFO_RST);
52 if (result)
53 goto reset_fifo_fail;
54
55 if (st->chip_config.accl_fifo_enable ||
56 st->chip_config.gyro_fifo_enable) {
57 result = inv_mpu6050_write_reg(st, st->reg->int_enable,
58 INV_MPU6050_BIT_DATA_RDY_EN);
59 if (result)
60 return result;
61 }
62
63 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
64 INV_MPU6050_BIT_FIFO_EN);
65 if (result)
66 goto reset_fifo_fail;
67
68 d = 0;
69 if (st->chip_config.gyro_fifo_enable)
70 d |= INV_MPU6050_BITS_GYRO_OUT;
71 if (st->chip_config.accl_fifo_enable)
72 d |= INV_MPU6050_BIT_ACCEL_OUT;
73 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d);
74 if (result)
75 goto reset_fifo_fail;
76
77 return 0;
78
79reset_fifo_fail:
80 dev_err(&st->client->dev, "reset fifo failed %d\n", result);
81 result = inv_mpu6050_write_reg(st, st->reg->int_enable,
82 INV_MPU6050_BIT_DATA_RDY_EN);
83
84 return result;
85}
86
87static void inv_clear_kfifo(struct inv_mpu6050_state *st)
88{
89 unsigned long flags;
90
91
92 spin_lock_irqsave(&st->time_stamp_lock, flags);
93 kfifo_reset(&st->timestamps);
94 spin_unlock_irqrestore(&st->time_stamp_lock, flags);
95}
96
97
98
99
100irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
101{
102 struct iio_poll_func *pf = p;
103 struct iio_dev *indio_dev = pf->indio_dev;
104 struct inv_mpu6050_state *st = iio_priv(indio_dev);
105 s64 timestamp;
106
107 timestamp = iio_get_time_ns();
108 kfifo_in_spinlocked(&st->timestamps, ×tamp, 1,
109 &st->time_stamp_lock);
110
111 return IRQ_WAKE_THREAD;
112}
113
114
115
116
117irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
118{
119 struct iio_poll_func *pf = p;
120 struct iio_dev *indio_dev = pf->indio_dev;
121 struct inv_mpu6050_state *st = iio_priv(indio_dev);
122 size_t bytes_per_datum;
123 int result;
124 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
125 u16 fifo_count;
126 s64 timestamp;
127 u64 *tmp;
128
129 mutex_lock(&indio_dev->mlock);
130 if (!(st->chip_config.accl_fifo_enable |
131 st->chip_config.gyro_fifo_enable))
132 goto end_session;
133 bytes_per_datum = 0;
134 if (st->chip_config.accl_fifo_enable)
135 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
136
137 if (st->chip_config.gyro_fifo_enable)
138 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
139
140
141
142
143
144 result = i2c_smbus_read_i2c_block_data(st->client,
145 st->reg->fifo_count_h,
146 INV_MPU6050_FIFO_COUNT_BYTE, data);
147 if (result != INV_MPU6050_FIFO_COUNT_BYTE)
148 goto end_session;
149 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
150 if (fifo_count < bytes_per_datum)
151 goto end_session;
152
153 if (fifo_count & 1)
154 goto flush_fifo;
155 if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
156 goto flush_fifo;
157
158 if (kfifo_len(&st->timestamps) >
159 fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
160 goto flush_fifo;
161 while (fifo_count >= bytes_per_datum) {
162 result = i2c_smbus_read_i2c_block_data(st->client,
163 st->reg->fifo_r_w,
164 bytes_per_datum, data);
165 if (result != bytes_per_datum)
166 goto flush_fifo;
167
168 result = kfifo_out(&st->timestamps, ×tamp, 1);
169
170 if (0 == result)
171 timestamp = 0;
172
173 tmp = (u64 *)data;
174 tmp[DIV_ROUND_UP(bytes_per_datum, 8)] = timestamp;
175 result = iio_push_to_buffers(indio_dev, data);
176 if (result)
177 goto flush_fifo;
178 fifo_count -= bytes_per_datum;
179 }
180
181end_session:
182 mutex_unlock(&indio_dev->mlock);
183 iio_trigger_notify_done(indio_dev->trig);
184
185 return IRQ_HANDLED;
186
187flush_fifo:
188
189 inv_reset_fifo(indio_dev);
190 inv_clear_kfifo(st);
191 mutex_unlock(&indio_dev->mlock);
192 iio_trigger_notify_done(indio_dev->trig);
193
194 return IRQ_HANDLED;
195}
196