1// SPDX-License-Identifier: GPL-2.0-only
2/*
3* Copyright (C) 2012 Invensense, Inc.
4*/
5
6#include <linux/module.h>
7#include <linux/slab.h>
8#include <linux/err.h>
9#include <linux/delay.h>
10#include <linux/sysfs.h>
11#include <linux/jiffies.h>
12#include <linux/irq.h>
13#include <linux/interrupt.h>
14#include <linux/poll.h>
15#include <linux/math64.h>
16
17#include <linux/iio/common/inv_sensors_timestamp.h>
18
19#include "inv_mpu_iio.h"
20
21static int inv_reset_fifo(struct iio_dev *indio_dev)
22{
23	int result;
24	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
25
26	/* disable fifo and reenable it */
27	inv_mpu6050_prepare_fifo(st, false);
28	result = inv_mpu6050_prepare_fifo(st, true);
29	if (result)
30		goto reset_fifo_fail;
31
32	return 0;
33
34reset_fifo_fail:
35	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
36	return regmap_update_bits(st->map, st->reg->int_enable,
37			INV_MPU6050_BIT_DATA_RDY_EN, INV_MPU6050_BIT_DATA_RDY_EN);
38}
39
40/*
41 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
42 */
43irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
44{
45	struct iio_poll_func *pf = p;
46	struct iio_dev *indio_dev = pf->indio_dev;
47	struct inv_mpu6050_state *st = iio_priv(indio_dev);
48	size_t bytes_per_datum;
49	int result;
50	u16 fifo_count;
51	u32 fifo_period;
52	s64 timestamp;
53	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
54	size_t i, nb;
55
56	mutex_lock(&st->lock);
57
58	if (!(st->chip_config.accl_fifo_enable |
59		st->chip_config.gyro_fifo_enable |
60		st->chip_config.magn_fifo_enable))
61		goto end_session;
62	bytes_per_datum = 0;
63	if (st->chip_config.accl_fifo_enable)
64		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
65
66	if (st->chip_config.gyro_fifo_enable)
67		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
68
69	if (st->chip_config.temp_fifo_enable)
70		bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
71
72	if (st->chip_config.magn_fifo_enable)
73		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
74
75	/*
76	 * read fifo_count register to know how many bytes are inside the FIFO
77	 * right now
78	 */
79	result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
80				  st->data, INV_MPU6050_FIFO_COUNT_BYTE);
81	if (result)
82		goto end_session;
83	fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
84
85	/*
86	 * Handle fifo overflow by resetting fifo.
87	 * Reset if there is only 3 data set free remaining to mitigate
88	 * possible delay between reading fifo count and fifo data.
89	 */
90	nb = 3 * bytes_per_datum;
91	if (fifo_count >= st->hw->fifo_size - nb) {
92		dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
93		goto flush_fifo;
94	}
95
96	/* compute and process only all complete datum */
97	nb = fifo_count / bytes_per_datum;
98	fifo_count = nb * bytes_per_datum;
99	if (nb == 0)
100		goto end_session;
101	/* Each FIFO data contains all sensors, so same number for FIFO and sensor data */
102	fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
103	inv_sensors_timestamp_interrupt(&st->timestamp, 1, pf->timestamp);
104	inv_sensors_timestamp_apply_odr(&st->timestamp, fifo_period, 1, 0);
105
106	/* clear internal data buffer for avoiding kernel data leak */
107	memset(data, 0, sizeof(data));
108
109	/* read all data once and process every samples */
110	result = regmap_noinc_read(st->map, st->reg->fifo_r_w, st->data, fifo_count);
111	if (result)
112		goto flush_fifo;
113	for (i = 0; i < nb; ++i) {
114		/* skip first samples if needed */
115		if (st->skip_samples) {
116			st->skip_samples--;
117			continue;
118		}
119		memcpy(data, &st->data[i * bytes_per_datum], bytes_per_datum);
120		timestamp = inv_sensors_timestamp_pop(&st->timestamp);
121		iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
122	}
123
124end_session:
125	mutex_unlock(&st->lock);
126	iio_trigger_notify_done(indio_dev->trig);
127
128	return IRQ_HANDLED;
129
130flush_fifo:
131	/* Flush HW and SW FIFOs. */
132	inv_reset_fifo(indio_dev);
133	mutex_unlock(&st->lock);
134	iio_trigger_notify_done(indio_dev->trig);
135
136	return IRQ_HANDLED;
137}
138