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/slab.h>
16 #include <linux/err.h>
17 #include <linux/delay.h>
18 #include <linux/sysfs.h>
19 #include <linux/jiffies.h>
20 #include <linux/irq.h>
21 #include <linux/interrupt.h>
22 #include <linux/kfifo.h>
23 #include <linux/poll.h>
24 #include "inv_mpu_iio.h"
26 static void inv_clear_kfifo(struct inv_mpu6050_state
*st
)
30 /* take the spin lock sem to avoid interrupt kick in */
31 spin_lock_irqsave(&st
->time_stamp_lock
, flags
);
32 kfifo_reset(&st
->timestamps
);
33 spin_unlock_irqrestore(&st
->time_stamp_lock
, flags
);
36 int inv_reset_fifo(struct iio_dev
*indio_dev
)
40 struct inv_mpu6050_state
*st
= iio_priv(indio_dev
);
42 /* disable interrupt */
43 result
= regmap_write(st
->map
, st
->reg
->int_enable
, 0);
45 dev_err(regmap_get_device(st
->map
), "int_enable failed %d\n",
49 /* disable the sensor output to FIFO */
50 result
= regmap_write(st
->map
, st
->reg
->fifo_en
, 0);
53 /* disable fifo reading */
54 result
= regmap_write(st
->map
, st
->reg
->user_ctrl
, 0);
59 result
= regmap_write(st
->map
, st
->reg
->user_ctrl
,
60 INV_MPU6050_BIT_FIFO_RST
);
64 /* clear timestamps fifo */
67 /* enable interrupt */
68 if (st
->chip_config
.accl_fifo_enable
||
69 st
->chip_config
.gyro_fifo_enable
) {
70 result
= regmap_write(st
->map
, st
->reg
->int_enable
,
71 INV_MPU6050_BIT_DATA_RDY_EN
);
75 /* enable FIFO reading and I2C master interface*/
76 result
= regmap_write(st
->map
, st
->reg
->user_ctrl
,
77 INV_MPU6050_BIT_FIFO_EN
);
80 /* enable sensor output to FIFO */
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
= regmap_write(st
->map
, st
->reg
->fifo_en
, d
);
93 dev_err(regmap_get_device(st
->map
), "reset fifo failed %d\n", result
);
94 result
= regmap_write(st
->map
, st
->reg
->int_enable
,
95 INV_MPU6050_BIT_DATA_RDY_EN
);
101 * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
103 irqreturn_t
inv_mpu6050_irq_handler(int irq
, void *p
)
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
);
110 timestamp
= iio_get_time_ns(indio_dev
);
111 kfifo_in_spinlocked(&st
->timestamps
, ×tamp
, 1,
112 &st
->time_stamp_lock
);
114 return IRQ_WAKE_THREAD
;
118 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
120 irqreturn_t
inv_mpu6050_read_fifo(int irq
, void *p
)
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
;
127 u8 data
[INV_MPU6050_OUTPUT_DATA_SIZE
];
131 mutex_lock(&indio_dev
->mlock
);
132 if (!(st
->chip_config
.accl_fifo_enable
|
133 st
->chip_config
.gyro_fifo_enable
))
136 if (st
->chip_config
.accl_fifo_enable
)
137 bytes_per_datum
+= INV_MPU6050_BYTES_PER_3AXIS_SENSOR
;
139 if (st
->chip_config
.gyro_fifo_enable
)
140 bytes_per_datum
+= INV_MPU6050_BYTES_PER_3AXIS_SENSOR
;
143 * read fifo_count register to know how many bytes inside FIFO
146 result
= regmap_bulk_read(st
->map
, st
->reg
->fifo_count_h
, data
,
147 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
= regmap_bulk_read(st
->map
, st
->reg
->fifo_r_w
,
164 data
, bytes_per_datum
);
168 result
= kfifo_out(&st
->timestamps
, ×tamp
, 1);
169 /* when there is no timestamp, put timestamp as 0 */
173 result
= iio_push_to_buffers_with_timestamp(indio_dev
, data
,
177 fifo_count
-= bytes_per_datum
;
181 mutex_unlock(&indio_dev
->mlock
);
182 iio_trigger_notify_done(indio_dev
->trig
);
187 /* Flush HW and SW FIFOs. */
188 inv_reset_fifo(indio_dev
);
189 mutex_unlock(&indio_dev
->mlock
);
190 iio_trigger_notify_done(indio_dev
->trig
);