2 * Copyright (C) 2012 Invensense, Inc.
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.
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.
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"
28 int inv_reset_fifo(struct iio_dev *indio_dev)
32 struct inv_mpu6050_state *st = iio_priv(indio_dev);
34 /* disable interrupt */
35 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
37 dev_err(&st->client->dev, "int_enable failed %d\n", result);
40 /* disable the sensor output to FIFO */
41 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
44 /* disable fifo reading */
45 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
50 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
51 INV_MPU6050_BIT_FIFO_RST);
54 /* enable interrupt */
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);
62 /* enable FIFO reading and I2C master interface*/
63 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
64 INV_MPU6050_BIT_FIFO_EN);
67 /* enable sensor output to FIFO */
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);
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);
87 static void inv_clear_kfifo(struct inv_mpu6050_state *st)
91 /* take the spin lock sem to avoid interrupt kick in */
92 spin_lock_irqsave(&st->time_stamp_lock, flags);
93 kfifo_reset(&st->timestamps);
94 spin_unlock_irqrestore(&st->time_stamp_lock, flags);
98 * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
100 irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
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);
107 timestamp = iio_get_time_ns();
108 spin_lock(&st->time_stamp_lock);
109 kfifo_in(&st->timestamps, ×tamp, 1);
110 spin_unlock(&st->time_stamp_lock);
112 return IRQ_WAKE_THREAD;
116 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
118 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
120 struct iio_poll_func *pf = p;
121 struct iio_dev *indio_dev = pf->indio_dev;
122 struct inv_mpu6050_state *st = iio_priv(indio_dev);
123 size_t bytes_per_datum;
125 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
130 mutex_lock(&indio_dev->mlock);
131 if (!(st->chip_config.accl_fifo_enable |
132 st->chip_config.gyro_fifo_enable))
135 if (st->chip_config.accl_fifo_enable)
136 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
138 if (st->chip_config.gyro_fifo_enable)
139 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
142 * read fifo_count register to know how many bytes inside FIFO
145 result = i2c_smbus_read_i2c_block_data(st->client,
146 st->reg->fifo_count_h,
147 INV_MPU6050_FIFO_COUNT_BYTE, data);
148 if (result != INV_MPU6050_FIFO_COUNT_BYTE)
150 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
151 if (fifo_count < bytes_per_datum)
153 /* fifo count can't be odd number, if it is odd, reset fifo*/
156 if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
158 /* Timestamp mismatch. */
159 if (kfifo_len(&st->timestamps) >
160 fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
162 while (fifo_count >= bytes_per_datum) {
163 result = i2c_smbus_read_i2c_block_data(st->client,
165 bytes_per_datum, data);
166 if (result != bytes_per_datum)
169 result = kfifo_out(&st->timestamps, ×tamp, 1);
170 /* when there is no timestamp, put timestamp as 0 */
175 tmp[DIV_ROUND_UP(bytes_per_datum, 8)] = timestamp;
176 result = iio_push_to_buffers(indio_dev, data);
179 fifo_count -= bytes_per_datum;
183 mutex_unlock(&indio_dev->mlock);
184 iio_trigger_notify_done(indio_dev->trig);
189 /* Flush HW and SW FIFOs. */
190 inv_reset_fifo(indio_dev);
192 mutex_unlock(&indio_dev->mlock);
193 iio_trigger_notify_done(indio_dev->trig);