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 
21 static 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 
34 reset_fifo_fail:
35 	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
36 	result = regmap_write(st->map, st->reg->int_enable,
37 			      INV_MPU6050_BIT_DATA_RDY_EN);
38 
39 	return result;
40 }
41 
42 /*
43  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
44  */
45 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
46 {
47 	struct iio_poll_func *pf = p;
48 	struct iio_dev *indio_dev = pf->indio_dev;
49 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
50 	size_t bytes_per_datum;
51 	int result;
52 	u16 fifo_count;
53 	u32 fifo_period;
54 	s64 timestamp;
55 	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
56 	int int_status;
57 	size_t i, nb;
58 
59 	mutex_lock(&st->lock);
60 
61 	/* ack interrupt and check status */
62 	result = regmap_read(st->map, st->reg->int_status, &int_status);
63 	if (result) {
64 		dev_err(regmap_get_device(st->map),
65 			"failed to ack interrupt\n");
66 		goto flush_fifo;
67 	}
68 	if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
69 		goto end_session;
70 
71 	if (!(st->chip_config.accl_fifo_enable |
72 		st->chip_config.gyro_fifo_enable |
73 		st->chip_config.magn_fifo_enable))
74 		goto end_session;
75 	bytes_per_datum = 0;
76 	if (st->chip_config.accl_fifo_enable)
77 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
78 
79 	if (st->chip_config.gyro_fifo_enable)
80 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
81 
82 	if (st->chip_config.temp_fifo_enable)
83 		bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
84 
85 	if (st->chip_config.magn_fifo_enable)
86 		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
87 
88 	/*
89 	 * read fifo_count register to know how many bytes are inside the FIFO
90 	 * right now
91 	 */
92 	result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
93 				  st->data, INV_MPU6050_FIFO_COUNT_BYTE);
94 	if (result)
95 		goto end_session;
96 	fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
97 
98 	/*
99 	 * Handle fifo overflow by resetting fifo.
100 	 * Reset if there is only 3 data set free remaining to mitigate
101 	 * possible delay between reading fifo count and fifo data.
102 	 */
103 	nb = 3 * bytes_per_datum;
104 	if (fifo_count >= st->hw->fifo_size - nb) {
105 		dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
106 		goto flush_fifo;
107 	}
108 
109 	/* compute and process only all complete datum */
110 	nb = fifo_count / bytes_per_datum;
111 	fifo_count = nb * bytes_per_datum;
112 	if (nb == 0)
113 		goto end_session;
114 	/* Each FIFO data contains all sensors, so same number for FIFO and sensor data */
115 	fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
116 	inv_sensors_timestamp_interrupt(&st->timestamp, fifo_period, nb, nb, pf->timestamp);
117 	inv_sensors_timestamp_apply_odr(&st->timestamp, fifo_period, nb, 0);
118 
119 	/* clear internal data buffer for avoiding kernel data leak */
120 	memset(data, 0, sizeof(data));
121 
122 	/* read all data once and process every samples */
123 	result = regmap_noinc_read(st->map, st->reg->fifo_r_w, st->data, fifo_count);
124 	if (result)
125 		goto flush_fifo;
126 	for (i = 0; i < nb; ++i) {
127 		/* skip first samples if needed */
128 		if (st->skip_samples) {
129 			st->skip_samples--;
130 			continue;
131 		}
132 		memcpy(data, &st->data[i * bytes_per_datum], bytes_per_datum);
133 		timestamp = inv_sensors_timestamp_pop(&st->timestamp);
134 		iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
135 	}
136 
137 end_session:
138 	mutex_unlock(&st->lock);
139 	iio_trigger_notify_done(indio_dev->trig);
140 
141 	return IRQ_HANDLED;
142 
143 flush_fifo:
144 	/* Flush HW and SW FIFOs. */
145 	inv_reset_fifo(indio_dev);
146 	mutex_unlock(&st->lock);
147 	iio_trigger_notify_done(indio_dev->trig);
148 
149 	return IRQ_HANDLED;
150 }
151