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 
27 int inv_reset_fifo(struct iio_dev *indio_dev)
28 {
29 	int result;
30 	u8 d;
31 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
32 
33 	/* disable interrupt */
34 	result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
35 	if (result) {
36 		dev_err(&st->client->dev, "int_enable failed %d\n", result);
37 		return result;
38 	}
39 	/* disable the sensor output to FIFO */
40 	result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
41 	if (result)
42 		goto reset_fifo_fail;
43 	/* disable fifo reading */
44 	result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
45 	if (result)
46 		goto reset_fifo_fail;
47 
48 	/* reset FIFO*/
49 	result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
50 					INV_MPU6050_BIT_FIFO_RST);
51 	if (result)
52 		goto reset_fifo_fail;
53 	/* enable interrupt */
54 	if (st->chip_config.accl_fifo_enable ||
55 	    st->chip_config.gyro_fifo_enable) {
56 		result = inv_mpu6050_write_reg(st, st->reg->int_enable,
57 					INV_MPU6050_BIT_DATA_RDY_EN);
58 		if (result)
59 			return result;
60 	}
61 	/* enable FIFO reading and I2C master interface*/
62 	result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
63 					INV_MPU6050_BIT_FIFO_EN);
64 	if (result)
65 		goto reset_fifo_fail;
66 	/* enable sensor output to FIFO */
67 	d = 0;
68 	if (st->chip_config.gyro_fifo_enable)
69 		d |= INV_MPU6050_BITS_GYRO_OUT;
70 	if (st->chip_config.accl_fifo_enable)
71 		d |= INV_MPU6050_BIT_ACCEL_OUT;
72 	result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d);
73 	if (result)
74 		goto reset_fifo_fail;
75 
76 	return 0;
77 
78 reset_fifo_fail:
79 	dev_err(&st->client->dev, "reset fifo failed %d\n", result);
80 	result = inv_mpu6050_write_reg(st, st->reg->int_enable,
81 					INV_MPU6050_BIT_DATA_RDY_EN);
82 
83 	return result;
84 }
85 
86 static void inv_clear_kfifo(struct inv_mpu6050_state *st)
87 {
88 	unsigned long flags;
89 
90 	/* take the spin lock sem to avoid interrupt kick in */
91 	spin_lock_irqsave(&st->time_stamp_lock, flags);
92 	kfifo_reset(&st->timestamps);
93 	spin_unlock_irqrestore(&st->time_stamp_lock, flags);
94 }
95 
96 /**
97  * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
98  */
99 irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
100 {
101 	struct iio_poll_func *pf = p;
102 	struct iio_dev *indio_dev = pf->indio_dev;
103 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
104 	s64 timestamp;
105 
106 	timestamp = iio_get_time_ns();
107 	kfifo_in_spinlocked(&st->timestamps, &timestamp, 1,
108 				&st->time_stamp_lock);
109 
110 	return IRQ_WAKE_THREAD;
111 }
112 
113 /**
114  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
115  */
116 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
117 {
118 	struct iio_poll_func *pf = p;
119 	struct iio_dev *indio_dev = pf->indio_dev;
120 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
121 	size_t bytes_per_datum;
122 	int result;
123 	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
124 	u16 fifo_count;
125 	s64 timestamp;
126 
127 	mutex_lock(&indio_dev->mlock);
128 	if (!(st->chip_config.accl_fifo_enable |
129 		st->chip_config.gyro_fifo_enable))
130 		goto end_session;
131 	bytes_per_datum = 0;
132 	if (st->chip_config.accl_fifo_enable)
133 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
134 
135 	if (st->chip_config.gyro_fifo_enable)
136 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
137 
138 	/*
139 	 * read fifo_count register to know how many bytes inside FIFO
140 	 * right now
141 	 */
142 	result = i2c_smbus_read_i2c_block_data(st->client,
143 				       st->reg->fifo_count_h,
144 				       INV_MPU6050_FIFO_COUNT_BYTE, data);
145 	if (result != INV_MPU6050_FIFO_COUNT_BYTE)
146 		goto end_session;
147 	fifo_count = be16_to_cpup((__be16 *)(&data[0]));
148 	if (fifo_count < bytes_per_datum)
149 		goto end_session;
150 	/* fifo count can't be odd number, if it is odd, reset fifo*/
151 	if (fifo_count & 1)
152 		goto flush_fifo;
153 	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
154 		goto flush_fifo;
155 	/* Timestamp mismatch. */
156 	if (kfifo_len(&st->timestamps) >
157 		fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
158 			goto flush_fifo;
159 	while (fifo_count >= bytes_per_datum) {
160 		result = i2c_smbus_read_i2c_block_data(st->client,
161 						       st->reg->fifo_r_w,
162 						       bytes_per_datum, data);
163 		if (result != bytes_per_datum)
164 			goto flush_fifo;
165 
166 		result = kfifo_out(&st->timestamps, &timestamp, 1);
167 		/* when there is no timestamp, put timestamp as 0 */
168 		if (0 == result)
169 			timestamp = 0;
170 
171 		result = iio_push_to_buffers_with_timestamp(indio_dev, data,
172 			timestamp);
173 		if (result)
174 			goto flush_fifo;
175 		fifo_count -= bytes_per_datum;
176 	}
177 
178 end_session:
179 	mutex_unlock(&indio_dev->mlock);
180 	iio_trigger_notify_done(indio_dev->trig);
181 
182 	return IRQ_HANDLED;
183 
184 flush_fifo:
185 	/* Flush HW and SW FIFOs. */
186 	inv_reset_fifo(indio_dev);
187 	inv_clear_kfifo(st);
188 	mutex_unlock(&indio_dev->mlock);
189 	iio_trigger_notify_done(indio_dev->trig);
190 
191 	return IRQ_HANDLED;
192 }
193