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 #include "inv_mpu_iio.h"
17 
18 /**
19  *  inv_mpu6050_update_period() - Update chip internal period estimation
20  *
21  *  @st:		driver state
22  *  @timestamp:		the interrupt timestamp
23  *  @nb:		number of data set in the fifo
24  *
25  *  This function uses interrupt timestamps to estimate the chip period and
26  *  to choose the data timestamp to come.
27  */
inv_mpu6050_update_period(struct inv_mpu6050_state * st,s64 timestamp,size_t nb)28 static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
29 				      s64 timestamp, size_t nb)
30 {
31 	/* Period boundaries for accepting timestamp */
32 	const s64 period_min =
33 		(NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100;
34 	const s64 period_max =
35 		(NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100;
36 	const s32 divider = INV_MPU6050_FREQ_DIVIDER(st);
37 	s64 delta, interval;
38 	bool use_it_timestamp = false;
39 
40 	if (st->it_timestamp == 0) {
41 		/* not initialized, forced to use it_timestamp */
42 		use_it_timestamp = true;
43 	} else if (nb == 1) {
44 		/*
45 		 * Validate the use of it timestamp by checking if interrupt
46 		 * has been delayed.
47 		 * nb > 1 means interrupt was delayed for more than 1 sample,
48 		 * so it's obviously not good.
49 		 * Compute the chip period between 2 interrupts for validating.
50 		 */
51 		delta = div_s64(timestamp - st->it_timestamp, divider);
52 		if (delta > period_min && delta < period_max) {
53 			/* update chip period and use it timestamp */
54 			st->chip_period = (st->chip_period + delta) / 2;
55 			use_it_timestamp = true;
56 		}
57 	}
58 
59 	if (use_it_timestamp) {
60 		/*
61 		 * Manage case of multiple samples in the fifo (nb > 1):
62 		 * compute timestamp corresponding to the first sample using
63 		 * estimated chip period.
64 		 */
65 		interval = (nb - 1) * st->chip_period * divider;
66 		st->data_timestamp = timestamp - interval;
67 	}
68 
69 	/* save it timestamp */
70 	st->it_timestamp = timestamp;
71 }
72 
73 /**
74  *  inv_mpu6050_get_timestamp() - Return the current data timestamp
75  *
76  *  @st:		driver state
77  *  @return:		current data timestamp
78  *
79  *  This function returns the current data timestamp and prepares for next one.
80  */
inv_mpu6050_get_timestamp(struct inv_mpu6050_state * st)81 static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
82 {
83 	s64 ts;
84 
85 	/* return current data timestamp and increment */
86 	ts = st->data_timestamp;
87 	st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st);
88 
89 	return ts;
90 }
91 
inv_reset_fifo(struct iio_dev * indio_dev)92 static int inv_reset_fifo(struct iio_dev *indio_dev)
93 {
94 	int result;
95 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
96 
97 	/* disable fifo and reenable it */
98 	inv_mpu6050_prepare_fifo(st, false);
99 	result = inv_mpu6050_prepare_fifo(st, true);
100 	if (result)
101 		goto reset_fifo_fail;
102 
103 	return 0;
104 
105 reset_fifo_fail:
106 	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
107 	result = regmap_write(st->map, st->reg->int_enable,
108 			      INV_MPU6050_BIT_DATA_RDY_EN);
109 
110 	return result;
111 }
112 
113 /*
114  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
115  */
inv_mpu6050_read_fifo(int irq,void * p)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 	u16 fifo_count;
124 	s64 timestamp;
125 	int int_status;
126 	size_t i, nb;
127 
128 	mutex_lock(&st->lock);
129 
130 	/* ack interrupt and check status */
131 	result = regmap_read(st->map, st->reg->int_status, &int_status);
132 	if (result) {
133 		dev_err(regmap_get_device(st->map),
134 			"failed to ack interrupt\n");
135 		goto flush_fifo;
136 	}
137 	if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
138 		goto end_session;
139 
140 	if (!(st->chip_config.accl_fifo_enable |
141 		st->chip_config.gyro_fifo_enable |
142 		st->chip_config.magn_fifo_enable))
143 		goto end_session;
144 	bytes_per_datum = 0;
145 	if (st->chip_config.accl_fifo_enable)
146 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
147 
148 	if (st->chip_config.gyro_fifo_enable)
149 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
150 
151 	if (st->chip_config.temp_fifo_enable)
152 		bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
153 
154 	if (st->chip_config.magn_fifo_enable)
155 		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
156 
157 	/*
158 	 * read fifo_count register to know how many bytes are inside the FIFO
159 	 * right now
160 	 */
161 	result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
162 				  st->data, INV_MPU6050_FIFO_COUNT_BYTE);
163 	if (result)
164 		goto end_session;
165 	fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
166 
167 	/*
168 	 * Handle fifo overflow by resetting fifo.
169 	 * Reset if there is only 3 data set free remaining to mitigate
170 	 * possible delay between reading fifo count and fifo data.
171 	 */
172 	nb = 3 * bytes_per_datum;
173 	if (fifo_count >= st->hw->fifo_size - nb) {
174 		dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
175 		goto flush_fifo;
176 	}
177 
178 	/* compute and process all complete datum */
179 	nb = fifo_count / bytes_per_datum;
180 	inv_mpu6050_update_period(st, pf->timestamp, nb);
181 	for (i = 0; i < nb; ++i) {
182 		result = regmap_noinc_read(st->map, st->reg->fifo_r_w,
183 					   st->data, bytes_per_datum);
184 		if (result)
185 			goto flush_fifo;
186 		/* skip first samples if needed */
187 		if (st->skip_samples) {
188 			st->skip_samples--;
189 			continue;
190 		}
191 		timestamp = inv_mpu6050_get_timestamp(st);
192 		iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp);
193 	}
194 
195 end_session:
196 	mutex_unlock(&st->lock);
197 	iio_trigger_notify_done(indio_dev->trig);
198 
199 	return IRQ_HANDLED;
200 
201 flush_fifo:
202 	/* Flush HW and SW FIFOs. */
203 	inv_reset_fifo(indio_dev);
204 	mutex_unlock(&st->lock);
205 	iio_trigger_notify_done(indio_dev->trig);
206 
207 	return IRQ_HANDLED;
208 }
209