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 kfifo_in_spinlocked(&st
->timestamps
, ×tamp
, 1,
109 &st
->time_stamp_lock
);
111 return IRQ_WAKE_THREAD
;
115 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
117 irqreturn_t
inv_mpu6050_read_fifo(int irq
, void *p
)
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
;
124 u8 data
[INV_MPU6050_OUTPUT_DATA_SIZE
];
128 mutex_lock(&indio_dev
->mlock
);
129 if (!(st
->chip_config
.accl_fifo_enable
|
130 st
->chip_config
.gyro_fifo_enable
))
133 if (st
->chip_config
.accl_fifo_enable
)
134 bytes_per_datum
+= INV_MPU6050_BYTES_PER_3AXIS_SENSOR
;
136 if (st
->chip_config
.gyro_fifo_enable
)
137 bytes_per_datum
+= INV_MPU6050_BYTES_PER_3AXIS_SENSOR
;
140 * read fifo_count register to know how many bytes inside FIFO
143 result
= i2c_smbus_read_i2c_block_data(st
->client
,
144 st
->reg
->fifo_count_h
,
145 INV_MPU6050_FIFO_COUNT_BYTE
, data
);
146 if (result
!= INV_MPU6050_FIFO_COUNT_BYTE
)
148 fifo_count
= be16_to_cpup((__be16
*)(&data
[0]));
149 if (fifo_count
< bytes_per_datum
)
151 /* fifo count can't be odd number, if it is odd, reset fifo*/
154 if (fifo_count
> INV_MPU6050_FIFO_THRESHOLD
)
156 /* Timestamp mismatch. */
157 if (kfifo_len(&st
->timestamps
) >
158 fifo_count
/ bytes_per_datum
+ INV_MPU6050_TIME_STAMP_TOR
)
160 while (fifo_count
>= bytes_per_datum
) {
161 result
= i2c_smbus_read_i2c_block_data(st
->client
,
163 bytes_per_datum
, data
);
164 if (result
!= bytes_per_datum
)
167 result
= kfifo_out(&st
->timestamps
, ×tamp
, 1);
168 /* when there is no timestamp, put timestamp as 0 */
172 result
= iio_push_to_buffers_with_timestamp(indio_dev
, data
,
176 fifo_count
-= bytes_per_datum
;
180 mutex_unlock(&indio_dev
->mlock
);
181 iio_trigger_notify_done(indio_dev
->trig
);
186 /* Flush HW and SW FIFOs. */
187 inv_reset_fifo(indio_dev
);
189 mutex_unlock(&indio_dev
->mlock
);
190 iio_trigger_notify_done(indio_dev
->trig
);