1 /*
2 * Copyright (C) 2012 Invensense, Inc.
3 *
4 * This software is licensed under the terms of the GNU General Public
5 * License version 2, as published by the Free Software Foundation, and
6 * may be copied, distributed, and modified under those terms.
7 *
8 * This program is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 */
13
14 #include <linux/module.h>
15 #include <linux/slab.h>
16 #include <linux/i2c.h>
17 #include <linux/err.h>
18 #include <linux/delay.h>
19 #include <linux/sysfs.h>
20 #include <linux/jiffies.h>
21 #include <linux/irq.h>
22 #include <linux/interrupt.h>
23 #include <linux/kfifo.h>
24 #include <linux/poll.h>
25 #include "inv_mpu_iio.h"
26
inv_clear_kfifo(struct inv_mpu6050_state * st)27 static void inv_clear_kfifo(struct inv_mpu6050_state *st)
28 {
29 unsigned long flags;
30
31 /* take the spin lock sem to avoid interrupt kick in */
32 spin_lock_irqsave(&st->time_stamp_lock, flags);
33 kfifo_reset(&st->timestamps);
34 spin_unlock_irqrestore(&st->time_stamp_lock, flags);
35 }
36
inv_reset_fifo(struct iio_dev * indio_dev)37 int inv_reset_fifo(struct iio_dev *indio_dev)
38 {
39 int result;
40 u8 d;
41 struct inv_mpu6050_state *st = iio_priv(indio_dev);
42
43 /* disable interrupt */
44 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
45 if (result) {
46 dev_err(&st->client->dev, "int_enable failed %d\n", result);
47 return result;
48 }
49 /* disable the sensor output to FIFO */
50 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
51 if (result)
52 goto reset_fifo_fail;
53 /* disable fifo reading */
54 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
55 if (result)
56 goto reset_fifo_fail;
57
58 /* reset FIFO*/
59 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
60 INV_MPU6050_BIT_FIFO_RST);
61 if (result)
62 goto reset_fifo_fail;
63
64 /* clear timestamps fifo */
65 inv_clear_kfifo(st);
66
67 /* enable interrupt */
68 if (st->chip_config.accl_fifo_enable ||
69 st->chip_config.gyro_fifo_enable) {
70 result = inv_mpu6050_write_reg(st, st->reg->int_enable,
71 INV_MPU6050_BIT_DATA_RDY_EN);
72 if (result)
73 return result;
74 }
75 /* enable FIFO reading and I2C master interface*/
76 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
77 INV_MPU6050_BIT_FIFO_EN);
78 if (result)
79 goto reset_fifo_fail;
80 /* enable sensor output to FIFO */
81 d = 0;
82 if (st->chip_config.gyro_fifo_enable)
83 d |= INV_MPU6050_BITS_GYRO_OUT;
84 if (st->chip_config.accl_fifo_enable)
85 d |= INV_MPU6050_BIT_ACCEL_OUT;
86 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d);
87 if (result)
88 goto reset_fifo_fail;
89
90 return 0;
91
92 reset_fifo_fail:
93 dev_err(&st->client->dev, "reset fifo failed %d\n", result);
94 result = inv_mpu6050_write_reg(st, st->reg->int_enable,
95 INV_MPU6050_BIT_DATA_RDY_EN);
96
97 return result;
98 }
99
100 /**
101 * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
102 */
inv_mpu6050_irq_handler(int irq,void * p)103 irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
104 {
105 struct iio_poll_func *pf = p;
106 struct iio_dev *indio_dev = pf->indio_dev;
107 struct inv_mpu6050_state *st = iio_priv(indio_dev);
108 s64 timestamp;
109
110 timestamp = iio_get_time_ns();
111 kfifo_in_spinlocked(&st->timestamps, ×tamp, 1,
112 &st->time_stamp_lock);
113
114 return IRQ_WAKE_THREAD;
115 }
116
117 /**
118 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
119 */
inv_mpu6050_read_fifo(int irq,void * p)120 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
121 {
122 struct iio_poll_func *pf = p;
123 struct iio_dev *indio_dev = pf->indio_dev;
124 struct inv_mpu6050_state *st = iio_priv(indio_dev);
125 size_t bytes_per_datum;
126 int result;
127 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
128 u16 fifo_count;
129 s64 timestamp;
130
131 mutex_lock(&indio_dev->mlock);
132 if (!(st->chip_config.accl_fifo_enable |
133 st->chip_config.gyro_fifo_enable))
134 goto end_session;
135 bytes_per_datum = 0;
136 if (st->chip_config.accl_fifo_enable)
137 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
138
139 if (st->chip_config.gyro_fifo_enable)
140 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
141
142 /*
143 * read fifo_count register to know how many bytes inside FIFO
144 * right now
145 */
146 result = i2c_smbus_read_i2c_block_data(st->client,
147 st->reg->fifo_count_h,
148 INV_MPU6050_FIFO_COUNT_BYTE, data);
149 if (result != INV_MPU6050_FIFO_COUNT_BYTE)
150 goto end_session;
151 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
152 if (fifo_count < bytes_per_datum)
153 goto end_session;
154 /* fifo count can't be odd number, if it is odd, reset fifo*/
155 if (fifo_count & 1)
156 goto flush_fifo;
157 if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
158 goto flush_fifo;
159 /* Timestamp mismatch. */
160 if (kfifo_len(&st->timestamps) >
161 fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
162 goto flush_fifo;
163 while (fifo_count >= bytes_per_datum) {
164 result = i2c_smbus_read_i2c_block_data(st->client,
165 st->reg->fifo_r_w,
166 bytes_per_datum, data);
167 if (result != bytes_per_datum)
168 goto flush_fifo;
169
170 result = kfifo_out(&st->timestamps, ×tamp, 1);
171 /* when there is no timestamp, put timestamp as 0 */
172 if (0 == result)
173 timestamp = 0;
174
175 result = iio_push_to_buffers_with_timestamp(indio_dev, data,
176 timestamp);
177 if (result)
178 goto flush_fifo;
179 fifo_count -= bytes_per_datum;
180 }
181
182 end_session:
183 mutex_unlock(&indio_dev->mlock);
184 iio_trigger_notify_done(indio_dev->trig);
185
186 return IRQ_HANDLED;
187
188 flush_fifo:
189 /* Flush HW and SW FIFOs. */
190 inv_reset_fifo(indio_dev);
191 mutex_unlock(&indio_dev->mlock);
192 iio_trigger_notify_done(indio_dev->trig);
193
194 return IRQ_HANDLED;
195 }
196