1 /*
2  * STMicroelectronics st_lsm6dsx FIFO buffer library driver
3  *
4  * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data
5  * from gyroscope and accelerometer. Samples are queued without any tag
6  * according to a specific pattern based on 'FIFO data sets' (6 bytes each):
7  *  - 1st data set is reserved for gyroscope data
8  *  - 2nd data set is reserved for accelerometer data
9  * The FIFO pattern changes depending on the ODRs and decimation factors
10  * assigned to the FIFO data sets. The first sequence of data stored in FIFO
11  * buffer contains the data of all the enabled FIFO data sets
12  * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
13  * value of the decimation factor and ODR set for each FIFO data set.
14  * FIFO supported modes:
15  *  - BYPASS: FIFO disabled
16  *  - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
17  *    restarts from the beginning and the oldest sample is overwritten
18  *
19  * Copyright 2016 STMicroelectronics Inc.
20  *
21  * Lorenzo Bianconi <lorenzo.bianconi@st.com>
22  * Denis Ciocca <denis.ciocca@st.com>
23  *
24  * Licensed under the GPL-2.
25  */
26 #include <linux/module.h>
27 #include <linux/interrupt.h>
28 #include <linux/irq.h>
29 #include <linux/iio/kfifo_buf.h>
30 #include <linux/iio/iio.h>
31 #include <linux/iio/buffer.h>
32 
33 #include "st_lsm6dsx.h"
34 
35 #define ST_LSM6DSX_REG_FIFO_THL_ADDR		0x06
36 #define ST_LSM6DSX_REG_FIFO_THH_ADDR		0x07
37 #define ST_LSM6DSX_FIFO_TH_MASK			GENMASK(11, 0)
38 #define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR	0x08
39 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR		0x0a
40 #define ST_LSM6DSX_FIFO_MODE_MASK		GENMASK(2, 0)
41 #define ST_LSM6DSX_FIFO_ODR_MASK		GENMASK(6, 3)
42 #define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR		0x3a
43 #define ST_LSM6DSX_FIFO_DIFF_MASK		GENMASK(11, 0)
44 #define ST_LSM6DSX_FIFO_EMPTY_MASK		BIT(12)
45 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR		0x3e
46 
47 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL		0x08
48 
49 struct st_lsm6dsx_decimator_entry {
50 	u8 decimator;
51 	u8 val;
52 };
53 
54 static const
55 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
56 	{  0, 0x0 },
57 	{  1, 0x1 },
58 	{  2, 0x2 },
59 	{  3, 0x3 },
60 	{  4, 0x4 },
61 	{  8, 0x5 },
62 	{ 16, 0x6 },
63 	{ 32, 0x7 },
64 };
65 
66 static int st_lsm6dsx_get_decimator_val(u8 val)
67 {
68 	const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
69 	int i;
70 
71 	for (i = 0; i < max_size; i++)
72 		if (st_lsm6dsx_decimator_table[i].decimator == val)
73 			break;
74 
75 	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
76 }
77 
78 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
79 				       u16 *max_odr, u16 *min_odr)
80 {
81 	struct st_lsm6dsx_sensor *sensor;
82 	int i;
83 
84 	*max_odr = 0, *min_odr = ~0;
85 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
86 		sensor = iio_priv(hw->iio_devs[i]);
87 
88 		if (!(hw->enable_mask & BIT(sensor->id)))
89 			continue;
90 
91 		*max_odr = max_t(u16, *max_odr, sensor->odr);
92 		*min_odr = min_t(u16, *min_odr, sensor->odr);
93 	}
94 }
95 
96 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
97 {
98 	struct st_lsm6dsx_sensor *sensor;
99 	u16 max_odr, min_odr, sip = 0;
100 	int err, i;
101 	u8 data;
102 
103 	st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
104 
105 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
106 		sensor = iio_priv(hw->iio_devs[i]);
107 
108 		/* update fifo decimators and sample in pattern */
109 		if (hw->enable_mask & BIT(sensor->id)) {
110 			sensor->sip = sensor->odr / min_odr;
111 			sensor->decimator = max_odr / sensor->odr;
112 			data = st_lsm6dsx_get_decimator_val(sensor->decimator);
113 		} else {
114 			sensor->sip = 0;
115 			sensor->decimator = 0;
116 			data = 0;
117 		}
118 
119 		err = st_lsm6dsx_write_with_mask(hw,
120 					ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR,
121 					sensor->decimator_mask, data);
122 		if (err < 0)
123 			return err;
124 
125 		sip += sensor->sip;
126 	}
127 	hw->sip = sip;
128 
129 	return 0;
130 }
131 
132 static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
133 				    enum st_lsm6dsx_fifo_mode fifo_mode)
134 {
135 	u8 data;
136 	int err;
137 
138 	switch (fifo_mode) {
139 	case ST_LSM6DSX_FIFO_BYPASS:
140 		data = fifo_mode;
141 		break;
142 	case ST_LSM6DSX_FIFO_CONT:
143 		data = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
144 			__ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
145 		break;
146 	default:
147 		return -EINVAL;
148 	}
149 
150 	err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
151 			    sizeof(data), &data);
152 	if (err < 0)
153 		return err;
154 
155 	hw->fifo_mode = fifo_mode;
156 
157 	return 0;
158 }
159 
160 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
161 {
162 	u16 fifo_watermark = ~0, cur_watermark, sip = 0;
163 	struct st_lsm6dsx_hw *hw = sensor->hw;
164 	struct st_lsm6dsx_sensor *cur_sensor;
165 	__le16 wdata;
166 	int i, err;
167 	u8 data;
168 
169 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
170 		cur_sensor = iio_priv(hw->iio_devs[i]);
171 
172 		if (!(hw->enable_mask & BIT(cur_sensor->id)))
173 			continue;
174 
175 		cur_watermark = (cur_sensor == sensor) ? watermark
176 						       : cur_sensor->watermark;
177 
178 		fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
179 		sip += cur_sensor->sip;
180 	}
181 
182 	if (!sip)
183 		return 0;
184 
185 	fifo_watermark = max_t(u16, fifo_watermark, sip);
186 	fifo_watermark = (fifo_watermark / sip) * sip;
187 	fifo_watermark = fifo_watermark * ST_LSM6DSX_SAMPLE_DEPTH;
188 
189 	mutex_lock(&hw->lock);
190 
191 	err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_THH_ADDR,
192 			   sizeof(data), &data);
193 	if (err < 0)
194 		goto out;
195 
196 	fifo_watermark = ((data << 8) & ~ST_LSM6DSX_FIFO_TH_MASK) |
197 			 (fifo_watermark & ST_LSM6DSX_FIFO_TH_MASK);
198 
199 	wdata = cpu_to_le16(fifo_watermark);
200 	err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
201 			    sizeof(wdata), (u8 *)&wdata);
202 out:
203 	mutex_unlock(&hw->lock);
204 
205 	return err < 0 ? err : 0;
206 }
207 
208 /**
209  * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
210  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
211  *
212  * Read samples from the hw FIFO and push them to IIO buffers.
213  *
214  * Return: Number of bytes read from the FIFO
215  */
216 static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
217 {
218 	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
219 	int err, acc_sip, gyro_sip, read_len, samples, offset;
220 	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
221 	s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
222 	u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
223 	u8 buff[pattern_len];
224 	__le16 fifo_status;
225 
226 	err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
227 			   sizeof(fifo_status), (u8 *)&fifo_status);
228 	if (err < 0)
229 		return err;
230 
231 	if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
232 		return 0;
233 
234 	fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
235 		   ST_LSM6DSX_CHAN_SIZE;
236 	samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
237 	fifo_len = (fifo_len / pattern_len) * pattern_len;
238 
239 	/*
240 	 * compute delta timestamp between two consecutive samples
241 	 * in order to estimate queueing time of data generated
242 	 * by the sensor
243 	 */
244 	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
245 	acc_ts = acc_sensor->ts - acc_sensor->delta_ts;
246 	acc_delta_ts = div_s64(acc_sensor->delta_ts * acc_sensor->decimator,
247 			       samples);
248 
249 	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
250 	gyro_ts = gyro_sensor->ts - gyro_sensor->delta_ts;
251 	gyro_delta_ts = div_s64(gyro_sensor->delta_ts * gyro_sensor->decimator,
252 				samples);
253 
254 	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
255 		err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
256 				   sizeof(buff), buff);
257 		if (err < 0)
258 			return err;
259 
260 		/*
261 		 * Data are written to the FIFO with a specific pattern
262 		 * depending on the configured ODRs. The first sequence of data
263 		 * stored in FIFO contains the data of all enabled sensors
264 		 * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
265 		 * depending on the value of the decimation factor set for each
266 		 * sensor.
267 		 *
268 		 * Supposing the FIFO is storing data from gyroscope and
269 		 * accelerometer at different ODRs:
270 		 *   - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
271 		 * Since the gyroscope ODR is twice the accelerometer one, the
272 		 * following pattern is repeated every 9 samples:
273 		 *   - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
274 		 */
275 		gyro_sip = gyro_sensor->sip;
276 		acc_sip = acc_sensor->sip;
277 		offset = 0;
278 
279 		while (acc_sip > 0 || gyro_sip > 0) {
280 			if (gyro_sip-- > 0) {
281 				memcpy(iio_buff, &buff[offset],
282 				       ST_LSM6DSX_SAMPLE_SIZE);
283 				iio_push_to_buffers_with_timestamp(
284 					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
285 					iio_buff, gyro_ts);
286 				offset += ST_LSM6DSX_SAMPLE_SIZE;
287 				gyro_ts += gyro_delta_ts;
288 			}
289 
290 			if (acc_sip-- > 0) {
291 				memcpy(iio_buff, &buff[offset],
292 				       ST_LSM6DSX_SAMPLE_SIZE);
293 				iio_push_to_buffers_with_timestamp(
294 					hw->iio_devs[ST_LSM6DSX_ID_ACC],
295 					iio_buff, acc_ts);
296 				offset += ST_LSM6DSX_SAMPLE_SIZE;
297 				acc_ts += acc_delta_ts;
298 			}
299 		}
300 	}
301 
302 	return read_len;
303 }
304 
305 static int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
306 {
307 	int err;
308 
309 	mutex_lock(&hw->fifo_lock);
310 
311 	st_lsm6dsx_read_fifo(hw);
312 	err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
313 
314 	mutex_unlock(&hw->fifo_lock);
315 
316 	return err;
317 }
318 
319 static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
320 {
321 	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
322 	struct st_lsm6dsx_hw *hw = sensor->hw;
323 	int err;
324 
325 	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
326 		err = st_lsm6dsx_flush_fifo(hw);
327 		if (err < 0)
328 			return err;
329 	}
330 
331 	if (enable) {
332 		err = st_lsm6dsx_sensor_enable(sensor);
333 		if (err < 0)
334 			return err;
335 	} else {
336 		err = st_lsm6dsx_sensor_disable(sensor);
337 		if (err < 0)
338 			return err;
339 	}
340 
341 	err = st_lsm6dsx_update_decimators(hw);
342 	if (err < 0)
343 		return err;
344 
345 	err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
346 	if (err < 0)
347 		return err;
348 
349 	if (hw->enable_mask) {
350 		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
351 		if (err < 0)
352 			return err;
353 
354 		/*
355 		 * store enable buffer timestamp as reference to compute
356 		 * first delta timestamp
357 		 */
358 		sensor->ts = iio_get_time_ns(iio_dev);
359 	}
360 
361 	return 0;
362 }
363 
364 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
365 {
366 	struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
367 	struct st_lsm6dsx_sensor *sensor;
368 	int i;
369 
370 	if (!hw->sip)
371 		return IRQ_NONE;
372 
373 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
374 		sensor = iio_priv(hw->iio_devs[i]);
375 
376 		if (sensor->sip > 0) {
377 			s64 timestamp;
378 
379 			timestamp = iio_get_time_ns(hw->iio_devs[i]);
380 			sensor->delta_ts = timestamp - sensor->ts;
381 			sensor->ts = timestamp;
382 		}
383 	}
384 
385 	return IRQ_WAKE_THREAD;
386 }
387 
388 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
389 {
390 	struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
391 	int count;
392 
393 	mutex_lock(&hw->fifo_lock);
394 	count = st_lsm6dsx_read_fifo(hw);
395 	mutex_unlock(&hw->fifo_lock);
396 
397 	return !count ? IRQ_NONE : IRQ_HANDLED;
398 }
399 
400 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
401 {
402 	return st_lsm6dsx_update_fifo(iio_dev, true);
403 }
404 
405 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
406 {
407 	return st_lsm6dsx_update_fifo(iio_dev, false);
408 }
409 
410 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
411 	.preenable = st_lsm6dsx_buffer_preenable,
412 	.postdisable = st_lsm6dsx_buffer_postdisable,
413 };
414 
415 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
416 {
417 	struct iio_buffer *buffer;
418 	unsigned long irq_type;
419 	int i, err;
420 
421 	irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
422 
423 	switch (irq_type) {
424 	case IRQF_TRIGGER_HIGH:
425 	case IRQF_TRIGGER_RISING:
426 		break;
427 	default:
428 		dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
429 		return -EINVAL;
430 	}
431 
432 	err = devm_request_threaded_irq(hw->dev, hw->irq,
433 					st_lsm6dsx_handler_irq,
434 					st_lsm6dsx_handler_thread,
435 					irq_type | IRQF_ONESHOT,
436 					"lsm6dsx", hw);
437 	if (err) {
438 		dev_err(hw->dev, "failed to request trigger irq %d\n",
439 			hw->irq);
440 		return err;
441 	}
442 
443 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
444 		buffer = devm_iio_kfifo_allocate(hw->dev);
445 		if (!buffer)
446 			return -ENOMEM;
447 
448 		iio_device_attach_buffer(hw->iio_devs[i], buffer);
449 		hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
450 		hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
451 	}
452 
453 	return 0;
454 }
455