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
27int 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
78reset_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
86static 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 */
99irqreturn_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 */
116irqreturn_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
178end_session:
179	mutex_unlock(&indio_dev->mlock);
180	iio_trigger_notify_done(indio_dev->trig);
181
182	return IRQ_HANDLED;
183
184flush_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