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/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"
25 
26 static void inv_clear_kfifo(struct inv_mpu6050_state *st)
27 {
28 	unsigned long flags;
29 
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);
34 }
35 
36 int inv_reset_fifo(struct iio_dev *indio_dev)
37 {
38 	int result;
39 	u8 d;
40 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
41 
42 	/* disable interrupt */
43 	result = regmap_write(st->map, st->reg->int_enable, 0);
44 	if (result) {
45 		dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
46 			result);
47 		return result;
48 	}
49 	/* disable the sensor output to FIFO */
50 	result = regmap_write(st->map, st->reg->fifo_en, 0);
51 	if (result)
52 		goto reset_fifo_fail;
53 	/* disable fifo reading */
54 	result = regmap_write(st->map, st->reg->user_ctrl, 0);
55 	if (result)
56 		goto reset_fifo_fail;
57 
58 	/* reset FIFO*/
59 	result = regmap_write(st->map, 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 = regmap_write(st->map, 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 = regmap_write(st->map, 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 = regmap_write(st->map, 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(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);
96 
97 	return result;
98 }
99 
100 /**
101  * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
102  */
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, &timestamp, 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  */
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 = regmap_bulk_read(st->map, st->reg->fifo_count_h, data,
147 				  INV_MPU6050_FIFO_COUNT_BYTE);
148 	if (result)
149 		goto end_session;
150 	fifo_count = be16_to_cpup((__be16 *)(&data[0]));
151 	if (fifo_count < bytes_per_datum)
152 		goto end_session;
153 	/* fifo count can't be odd number, if it is odd, reset fifo*/
154 	if (fifo_count & 1)
155 		goto flush_fifo;
156 	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
157 		goto flush_fifo;
158 	/* Timestamp mismatch. */
159 	if (kfifo_len(&st->timestamps) >
160 	    fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
161 		goto flush_fifo;
162 	while (fifo_count >= bytes_per_datum) {
163 		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
164 					  data, bytes_per_datum);
165 		if (result)
166 			goto flush_fifo;
167 
168 		result = kfifo_out(&st->timestamps, &timestamp, 1);
169 		/* when there is no timestamp, put timestamp as 0 */
170 		if (result == 0)
171 			timestamp = 0;
172 
173 		result = iio_push_to_buffers_with_timestamp(indio_dev, data,
174 							    timestamp);
175 		if (result)
176 			goto flush_fifo;
177 		fifo_count -= bytes_per_datum;
178 	}
179 
180 end_session:
181 	mutex_unlock(&indio_dev->mlock);
182 	iio_trigger_notify_done(indio_dev->trig);
183 
184 	return IRQ_HANDLED;
185 
186 flush_fifo:
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);
191 
192 	return IRQ_HANDLED;
193 }
194