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
27static void inv_clear_kfifo(struct inv_mpu6050_state *st)
28{
29	unsigned long flags;
30
31	/* take the spin lock sem to avoid interrupt kick in */
32	spin_lock_irqsave(&st->time_stamp_lock, flags);
33	kfifo_reset(&st->timestamps);
34	spin_unlock_irqrestore(&st->time_stamp_lock, flags);
35}
36
37int inv_reset_fifo(struct iio_dev *indio_dev)
38{
39	int result;
40	u8 d;
41	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
42
43	/* disable interrupt */
44	result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
45	if (result) {
46		dev_err(&st->client->dev, "int_enable failed %d\n", result);
47		return result;
48	}
49	/* disable the sensor output to FIFO */
50	result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
51	if (result)
52		goto reset_fifo_fail;
53	/* disable fifo reading */
54	result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
55	if (result)
56		goto reset_fifo_fail;
57
58	/* reset FIFO*/
59	result = inv_mpu6050_write_reg(st, 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 = inv_mpu6050_write_reg(st, 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 = inv_mpu6050_write_reg(st, 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 = inv_mpu6050_write_reg(st, st->reg->fifo_en, d);
87	if (result)
88		goto reset_fifo_fail;
89
90	return 0;
91
92reset_fifo_fail:
93	dev_err(&st->client->dev, "reset fifo failed %d\n", result);
94	result = inv_mpu6050_write_reg(st, 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 */
103irqreturn_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 */
120irqreturn_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 = i2c_smbus_read_i2c_block_data(st->client,
147				       st->reg->fifo_count_h,
148				       INV_MPU6050_FIFO_COUNT_BYTE, data);
149	if (result != INV_MPU6050_FIFO_COUNT_BYTE)
150		goto end_session;
151	fifo_count = be16_to_cpup((__be16 *)(&data[0]));
152	if (fifo_count < bytes_per_datum)
153		goto end_session;
154	/* fifo count can't be odd number, if it is odd, reset fifo*/
155	if (fifo_count & 1)
156		goto flush_fifo;
157	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
158		goto flush_fifo;
159	/* Timestamp mismatch. */
160	if (kfifo_len(&st->timestamps) >
161		fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
162			goto flush_fifo;
163	while (fifo_count >= bytes_per_datum) {
164		result = i2c_smbus_read_i2c_block_data(st->client,
165						       st->reg->fifo_r_w,
166						       bytes_per_datum, data);
167		if (result != bytes_per_datum)
168			goto flush_fifo;
169
170		result = kfifo_out(&st->timestamps, &timestamp, 1);
171		/* when there is no timestamp, put timestamp as 0 */
172		if (0 == result)
173			timestamp = 0;
174
175		result = iio_push_to_buffers_with_timestamp(indio_dev, data,
176			timestamp);
177		if (result)
178			goto flush_fifo;
179		fifo_count -= bytes_per_datum;
180	}
181
182end_session:
183	mutex_unlock(&indio_dev->mlock);
184	iio_trigger_notify_done(indio_dev->trig);
185
186	return IRQ_HANDLED;
187
188flush_fifo:
189	/* Flush HW and SW FIFOs. */
190	inv_reset_fifo(indio_dev);
191	mutex_unlock(&indio_dev->mlock);
192	iio_trigger_notify_done(indio_dev->trig);
193
194	return IRQ_HANDLED;
195}
196