1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * STMicroelectronics st_lsm6dsx FIFO buffer library driver
4 *
5 * Pattern FIFO:
6 * The FIFO buffer can be configured to store data from gyroscope and
7 * accelerometer. Samples are queued without any tag according to a
8 * specific pattern based on 'FIFO data sets' (6 bytes each):
9 * - 1st data set is reserved for gyroscope data
10 * - 2nd data set is reserved for accelerometer data
11 * The FIFO pattern changes depending on the ODRs and decimation factors
12 * assigned to the FIFO data sets. The first sequence of data stored in FIFO
13 * buffer contains the data of all the enabled FIFO data sets
14 * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
15 * value of the decimation factor and ODR set for each FIFO data set.
16 *
17 * Supported devices:
18 * - ISM330DLC
19 * - LSM6DS3
20 * - LSM6DS3H
21 * - LSM6DS3TR-C
22 * - LSM6DSL
23 * - LSM6DSM
24 *
25 * Tagged FIFO:
26 * The FIFO buffer can be configured to store data from gyroscope and
27 * accelerometer. Each sample is queued with a tag (1B) indicating data
28 * source (gyroscope, accelerometer, hw timer).
29 *
30 * Supported devices:
31 * - ASM330LHB
32 * - ASM330LHH
33 * - ASM330LHHX
34 * - ASM330LHHXG1
35 * - ISM330DHCX
36 * - LSM6DSO
37 * - LSM6DSOP
38 * - LSM6DSOX
39 * - LSM6DSR
40 * - LSM6DSRX
41 * - LSM6DST
42 * - LSM6DSTX
43 * - LSM6DSV
44 *
45 * FIFO supported modes:
46 * - BYPASS: FIFO disabled
47 * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
48 * restarts from the beginning and the oldest sample is overwritten
49 *
50 * Copyright 2016 STMicroelectronics Inc.
51 *
52 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
53 * Denis Ciocca <denis.ciocca@st.com>
54 */
55 #include <linux/module.h>
56 #include <linux/iio/kfifo_buf.h>
57 #include <linux/iio/iio.h>
58 #include <linux/iio/buffer.h>
59 #include <linux/iio/sysfs.h>
60 #include <linux/regmap.h>
61 #include <linux/bitfield.h>
62
63 #include <linux/platform_data/st_sensors_pdata.h>
64
65 #include "st_lsm6dsx.h"
66
67 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
68 #define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
69 #define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
70 #define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
71 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
72 #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78
73 #define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
74
75 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
76
77 #define ST_LSM6DSX_TS_RESET_VAL 0xaa
78
79 struct st_lsm6dsx_decimator_entry {
80 u8 decimator;
81 u8 val;
82 };
83
84 enum st_lsm6dsx_fifo_tag {
85 ST_LSM6DSX_GYRO_TAG = 0x01,
86 ST_LSM6DSX_ACC_TAG = 0x02,
87 ST_LSM6DSX_TS_TAG = 0x04,
88 ST_LSM6DSX_EXT0_TAG = 0x0f,
89 ST_LSM6DSX_EXT1_TAG = 0x10,
90 ST_LSM6DSX_EXT2_TAG = 0x11,
91 };
92
93 static const
94 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
95 { 0, 0x0 },
96 { 1, 0x1 },
97 { 2, 0x2 },
98 { 3, 0x3 },
99 { 4, 0x4 },
100 { 8, 0x5 },
101 { 16, 0x6 },
102 { 32, 0x7 },
103 };
104
105 static int
st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor * sensor,u32 max_odr)106 st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
107 {
108 const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
109 u32 decimator = max_odr / sensor->hwfifo_odr_mHz;
110 int i;
111
112 if (decimator > 1)
113 decimator = round_down(decimator, 2);
114
115 for (i = 0; i < max_size; i++) {
116 if (st_lsm6dsx_decimator_table[i].decimator == decimator)
117 break;
118 }
119
120 sensor->decimator = decimator;
121 return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
122 }
123
st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw * hw,u32 * max_odr,u32 * min_odr)124 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
125 u32 *max_odr, u32 *min_odr)
126 {
127 struct st_lsm6dsx_sensor *sensor;
128 int i;
129
130 *max_odr = 0, *min_odr = ~0;
131 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
132 if (!hw->iio_devs[i])
133 continue;
134
135 sensor = iio_priv(hw->iio_devs[i]);
136
137 if (!(hw->enable_mask & BIT(sensor->id)))
138 continue;
139
140 *max_odr = max(*max_odr, sensor->hwfifo_odr_mHz);
141 *min_odr = min(*min_odr, sensor->hwfifo_odr_mHz);
142 }
143 }
144
st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor * sensor,u32 min_odr)145 static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr)
146 {
147 u8 sip = sensor->hwfifo_odr_mHz / min_odr;
148
149 return sip > 1 ? round_down(sip, 2) : sip;
150 }
151
st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw * hw)152 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
153 {
154 const struct st_lsm6dsx_reg *ts_dec_reg;
155 struct st_lsm6dsx_sensor *sensor;
156 u16 sip = 0, ts_sip = 0;
157 u32 max_odr, min_odr;
158 int err = 0, i;
159 u8 data;
160
161 st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
162
163 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
164 const struct st_lsm6dsx_reg *dec_reg;
165
166 if (!hw->iio_devs[i])
167 continue;
168
169 sensor = iio_priv(hw->iio_devs[i]);
170 /* update fifo decimators and sample in pattern */
171 if (hw->enable_mask & BIT(sensor->id)) {
172 sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr);
173 data = st_lsm6dsx_get_decimator_val(sensor, max_odr);
174 } else {
175 sensor->sip = 0;
176 data = 0;
177 }
178 ts_sip = max_t(u16, ts_sip, sensor->sip);
179
180 dec_reg = &hw->settings->decimator[sensor->id];
181 if (dec_reg->addr) {
182 int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
183
184 err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
185 dec_reg->mask,
186 val);
187 if (err < 0)
188 return err;
189 }
190 sip += sensor->sip;
191 }
192 hw->sip = sip + ts_sip;
193 hw->ts_sip = ts_sip;
194
195 /*
196 * update hw ts decimator if necessary. Decimator for hw timestamp
197 * is always 1 or 0 in order to have a ts sample for each data
198 * sample in FIFO
199 */
200 ts_dec_reg = &hw->settings->ts_settings.decimator;
201 if (ts_dec_reg->addr) {
202 int val, ts_dec = !!hw->ts_sip;
203
204 val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
205 err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
206 ts_dec_reg->mask, val);
207 }
208 return err;
209 }
210
st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw * hw,enum st_lsm6dsx_fifo_mode fifo_mode)211 static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
212 enum st_lsm6dsx_fifo_mode fifo_mode)
213 {
214 unsigned int data;
215
216 data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
217 return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
218 ST_LSM6DSX_FIFO_MODE_MASK, data);
219 }
220
st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor * sensor,bool enable)221 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
222 bool enable)
223 {
224 struct st_lsm6dsx_hw *hw = sensor->hw;
225 const struct st_lsm6dsx_reg *batch_reg;
226 u8 data;
227
228 /* Only internal sensors have a FIFO ODR configuration register. */
229 if (sensor->id >= ARRAY_SIZE(hw->settings->batch))
230 return 0;
231
232 batch_reg = &hw->settings->batch[sensor->id];
233 if (batch_reg->addr) {
234 int val;
235
236 if (enable) {
237 int err;
238
239 err = st_lsm6dsx_check_odr(sensor, sensor->hwfifo_odr_mHz,
240 &data);
241 if (err < 0)
242 return err;
243 } else {
244 data = 0;
245 }
246 val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
247 return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
248 batch_reg->mask, val);
249 } else {
250 data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
251 return st_lsm6dsx_update_bits_locked(hw,
252 ST_LSM6DSX_REG_FIFO_MODE_ADDR,
253 ST_LSM6DSX_FIFO_ODR_MASK,
254 FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
255 data));
256 }
257 }
258
st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor * sensor,u16 watermark)259 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
260 {
261 u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask;
262 struct st_lsm6dsx_hw *hw = sensor->hw;
263 struct st_lsm6dsx_sensor *cur_sensor;
264 int i, err, data;
265 __le16 wdata;
266
267 if (!hw->sip)
268 return 0;
269
270 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
271 if (!hw->iio_devs[i])
272 continue;
273
274 cur_sensor = iio_priv(hw->iio_devs[i]);
275
276 if (!(hw->enable_mask & BIT(cur_sensor->id)))
277 continue;
278
279 cur_watermark = (cur_sensor == sensor) ? watermark
280 : cur_sensor->watermark;
281
282 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
283 }
284
285 fifo_watermark = max_t(u16, fifo_watermark, hw->sip);
286 fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;
287 fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
288
289 mutex_lock(&hw->page_lock);
290 err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
291 &data);
292 if (err < 0)
293 goto out;
294
295 fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
296 fifo_watermark = ((data << 8) & ~fifo_th_mask) |
297 (fifo_watermark & fifo_th_mask);
298
299 wdata = cpu_to_le16(fifo_watermark);
300 err = regmap_bulk_write(hw->regmap,
301 hw->settings->fifo_ops.fifo_th.addr,
302 &wdata, sizeof(wdata));
303 out:
304 mutex_unlock(&hw->page_lock);
305 return err;
306 }
307
st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw * hw)308 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
309 {
310 struct st_lsm6dsx_sensor *sensor;
311 int i, err;
312
313 /* reset hw ts counter */
314 err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
315 ST_LSM6DSX_TS_RESET_VAL);
316 if (err < 0)
317 return err;
318
319 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
320 if (!hw->iio_devs[i])
321 continue;
322
323 sensor = iio_priv(hw->iio_devs[i]);
324 /*
325 * store enable buffer timestamp as reference for
326 * hw timestamp
327 */
328 sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
329 }
330 return 0;
331 }
332
st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw * hw)333 int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw)
334 {
335 int err;
336
337 /* reset hw ts counter */
338 err = st_lsm6dsx_reset_hw_ts(hw);
339 if (err < 0)
340 return err;
341
342 return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
343 }
344
345 /*
346 * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
347 * in order to avoid a kmalloc for each bus access
348 */
st_lsm6dsx_read_block(struct st_lsm6dsx_hw * hw,u8 addr,u8 * data,unsigned int data_len,unsigned int max_word_len)349 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
350 u8 *data, unsigned int data_len,
351 unsigned int max_word_len)
352 {
353 unsigned int word_len, read_len = 0;
354 int err;
355
356 while (read_len < data_len) {
357 word_len = min_t(unsigned int, data_len - read_len,
358 max_word_len);
359 err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
360 word_len);
361 if (err < 0)
362 return err;
363 read_len += word_len;
364 }
365 return 0;
366 }
367
368 #define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
369 sizeof(s64)) + sizeof(s64))
370 /**
371 * st_lsm6dsx_read_fifo() - hw FIFO read routine
372 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
373 *
374 * Read samples from the hw FIFO and push them to IIO buffers.
375 *
376 * Return: Number of bytes read from the FIFO
377 */
st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw * hw)378 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
379 {
380 struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
381 int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
382 u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
383 u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
384 bool reset_ts = false;
385 __le16 fifo_status;
386 s64 ts = 0;
387
388 err = st_lsm6dsx_read_locked(hw,
389 hw->settings->fifo_ops.fifo_diff.addr,
390 &fifo_status, sizeof(fifo_status));
391 if (err < 0) {
392 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
393 err);
394 return err;
395 }
396
397 if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
398 return 0;
399
400 if (!pattern_len)
401 pattern_len = ST_LSM6DSX_SAMPLE_SIZE;
402
403 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
404 ST_LSM6DSX_CHAN_SIZE;
405 fifo_len = (fifo_len / pattern_len) * pattern_len;
406
407 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
408 gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
409 if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
410 ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
411
412 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
413 err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
414 hw->buff, pattern_len,
415 ST_LSM6DSX_MAX_WORD_LEN);
416 if (err < 0) {
417 dev_err(hw->dev,
418 "failed to read pattern from fifo (err=%d)\n",
419 err);
420 return err;
421 }
422
423 /*
424 * Data are written to the FIFO with a specific pattern
425 * depending on the configured ODRs. The first sequence of data
426 * stored in FIFO contains the data of all enabled sensors
427 * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
428 * depending on the value of the decimation factor set for each
429 * sensor.
430 *
431 * Supposing the FIFO is storing data from gyroscope and
432 * accelerometer at different ODRs:
433 * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
434 * Since the gyroscope ODR is twice the accelerometer one, the
435 * following pattern is repeated every 9 samples:
436 * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
437 */
438 ext_sip = ext_sensor ? ext_sensor->sip : 0;
439 gyro_sip = gyro_sensor->sip;
440 acc_sip = acc_sensor->sip;
441 ts_sip = hw->ts_sip;
442 offset = 0;
443 sip = 0;
444
445 while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
446 if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
447 memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels,
448 &hw->buff[offset],
449 sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels));
450 offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels);
451 }
452 if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
453 memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels,
454 &hw->buff[offset],
455 sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels));
456 offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels);
457 }
458 if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
459 memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels,
460 &hw->buff[offset],
461 sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels));
462 offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels);
463 }
464
465 if (ts_sip-- > 0) {
466 u8 data[ST_LSM6DSX_SAMPLE_SIZE];
467
468 memcpy(data, &hw->buff[offset], sizeof(data));
469 /*
470 * hw timestamp is 3B long and it is stored
471 * in FIFO using 6B as 4th FIFO data set
472 * according to this schema:
473 * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
474 */
475 ts = data[1] << 16 | data[0] << 8 | data[3];
476 /*
477 * check if hw timestamp engine is going to
478 * reset (the sensor generates an interrupt
479 * to signal the hw timestamp will reset in
480 * 1.638s)
481 */
482 if (!reset_ts && ts >= 0xff0000)
483 reset_ts = true;
484 ts *= hw->ts_gain;
485
486 offset += ST_LSM6DSX_SAMPLE_SIZE;
487 }
488
489 if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
490 /*
491 * We need to discards gyro samples during
492 * filters settling time
493 */
494 if (gyro_sensor->samples_to_discard > 0)
495 gyro_sensor->samples_to_discard--;
496 else
497 iio_push_to_buffers_with_timestamp(
498 hw->iio_devs[ST_LSM6DSX_ID_GYRO],
499 &hw->scan[ST_LSM6DSX_ID_GYRO],
500 gyro_sensor->ts_ref + ts);
501 gyro_sip--;
502 }
503 if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
504 /*
505 * We need to discards accel samples during
506 * filters settling time
507 */
508 if (acc_sensor->samples_to_discard > 0)
509 acc_sensor->samples_to_discard--;
510 else
511 iio_push_to_buffers_with_timestamp(
512 hw->iio_devs[ST_LSM6DSX_ID_ACC],
513 &hw->scan[ST_LSM6DSX_ID_ACC],
514 acc_sensor->ts_ref + ts);
515 acc_sip--;
516 }
517 if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
518 iio_push_to_buffers_with_timestamp(
519 hw->iio_devs[ST_LSM6DSX_ID_EXT0],
520 &hw->scan[ST_LSM6DSX_ID_EXT0],
521 ext_sensor->ts_ref + ts);
522 ext_sip--;
523 }
524 sip++;
525 }
526 }
527
528 if (unlikely(reset_ts)) {
529 err = st_lsm6dsx_reset_hw_ts(hw);
530 if (err < 0) {
531 dev_err(hw->dev, "failed to reset hw ts (err=%d)\n",
532 err);
533 return err;
534 }
535 }
536 return read_len;
537 }
538
539 #define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd
540 static int
st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw * hw,u8 tag,u8 * data,s64 ts)541 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
542 u8 *data, s64 ts)
543 {
544 s16 val = le16_to_cpu(*(__le16 *)data);
545 struct st_lsm6dsx_sensor *sensor;
546 struct iio_dev *iio_dev;
547
548 /* invalid sample during bootstrap phase */
549 if (val >= ST_LSM6DSX_INVALID_SAMPLE)
550 return -EINVAL;
551
552 /*
553 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
554 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
555 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
556 * channel
557 */
558 switch (tag) {
559 case ST_LSM6DSX_GYRO_TAG:
560 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
561 break;
562 case ST_LSM6DSX_ACC_TAG:
563 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
564 break;
565 case ST_LSM6DSX_EXT0_TAG:
566 if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
567 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
568 else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
569 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
570 else
571 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
572 break;
573 case ST_LSM6DSX_EXT1_TAG:
574 if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
575 (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
576 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
577 else
578 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
579 break;
580 case ST_LSM6DSX_EXT2_TAG:
581 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
582 break;
583 default:
584 return -EINVAL;
585 }
586
587 sensor = iio_priv(iio_dev);
588 iio_push_to_buffers_with_timestamp(iio_dev, data,
589 ts + sensor->ts_ref);
590
591 return 0;
592 }
593
594 /**
595 * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine
596 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
597 *
598 * Read samples from the hw FIFO and push them to IIO buffers.
599 *
600 * Return: Number of bytes read from the FIFO
601 */
st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw * hw)602 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
603 {
604 u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
605 u16 fifo_len, fifo_diff_mask;
606 /*
607 * Alignment needed as this can ultimately be passed to a
608 * call to iio_push_to_buffers_with_timestamp() which
609 * must be passed a buffer that is aligned to 8 bytes so
610 * as to allow insertion of a naturally aligned timestamp.
611 */
612 u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE] __aligned(8);
613 u8 tag;
614 bool reset_ts = false;
615 int i, err, read_len;
616 __le16 fifo_status;
617 s64 ts = 0;
618
619 err = st_lsm6dsx_read_locked(hw,
620 hw->settings->fifo_ops.fifo_diff.addr,
621 &fifo_status, sizeof(fifo_status));
622 if (err < 0) {
623 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
624 err);
625 return err;
626 }
627
628 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
629 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
630 ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
631 if (!fifo_len)
632 return 0;
633
634 if (!pattern_len)
635 pattern_len = ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
636
637 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
638 err = st_lsm6dsx_read_block(hw,
639 ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
640 hw->buff, pattern_len,
641 ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
642 if (err < 0) {
643 dev_err(hw->dev,
644 "failed to read pattern from fifo (err=%d)\n",
645 err);
646 return err;
647 }
648
649 for (i = 0; i < pattern_len;
650 i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
651 memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
652 ST_LSM6DSX_SAMPLE_SIZE);
653
654 tag = hw->buff[i] >> 3;
655 if (tag == ST_LSM6DSX_TS_TAG) {
656 /*
657 * hw timestamp is 4B long and it is stored
658 * in FIFO according to this schema:
659 * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
660 * B3 = ts[31:24]
661 */
662 ts = le32_to_cpu(*((__le32 *)iio_buff));
663 /*
664 * check if hw timestamp engine is going to
665 * reset (the sensor generates an interrupt
666 * to signal the hw timestamp will reset in
667 * 1.638s)
668 */
669 if (!reset_ts && ts >= 0xffff0000)
670 reset_ts = true;
671 ts *= hw->ts_gain;
672 } else {
673 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
674 ts);
675 }
676 }
677 }
678
679 if (unlikely(reset_ts)) {
680 err = st_lsm6dsx_reset_hw_ts(hw);
681 if (err < 0)
682 return err;
683 }
684 return read_len;
685 }
686
st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw * hw)687 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
688 {
689 int err;
690
691 if (!hw->settings->fifo_ops.read_fifo)
692 return -ENOTSUPP;
693
694 mutex_lock(&hw->fifo_lock);
695
696 hw->settings->fifo_ops.read_fifo(hw);
697 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
698
699 mutex_unlock(&hw->fifo_lock);
700
701 return err;
702 }
703
704 static void
st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor * sensor)705 st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor)
706 {
707 const struct st_lsm6dsx_samples_to_discard *data;
708 struct st_lsm6dsx_hw *hw = sensor->hw;
709 int i;
710
711 if (sensor->id != ST_LSM6DSX_ID_GYRO &&
712 sensor->id != ST_LSM6DSX_ID_ACC)
713 return;
714
715 /* check if drdy mask is supported in hw */
716 if (hw->settings->drdy_mask.addr)
717 return;
718
719 data = &hw->settings->samples_to_discard[sensor->id];
720 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
721 if (data->val[i].milli_hz == sensor->hwfifo_odr_mHz) {
722 sensor->samples_to_discard = data->val[i].samples;
723 return;
724 }
725 }
726 }
727
st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor * sensor,bool enable)728 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
729 {
730 struct st_lsm6dsx_hw *hw = sensor->hw;
731 u8 fifo_mask;
732 int err;
733
734 mutex_lock(&hw->conf_lock);
735
736 if (enable)
737 fifo_mask = hw->fifo_mask | BIT(sensor->id);
738 else
739 fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
740
741 if (hw->fifo_mask) {
742 err = st_lsm6dsx_flush_fifo(hw);
743 if (err < 0)
744 goto out;
745 }
746
747 if (enable)
748 st_lsm6dsx_update_samples_to_discard(sensor);
749
750 err = st_lsm6dsx_device_set_enable(sensor, enable);
751 if (err < 0)
752 goto out;
753
754 err = st_lsm6dsx_set_fifo_odr(sensor, enable);
755 if (err < 0)
756 goto out;
757
758 err = st_lsm6dsx_update_decimators(hw);
759 if (err < 0)
760 goto out;
761
762 err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
763 if (err < 0)
764 goto out;
765
766 if (fifo_mask) {
767 err = st_lsm6dsx_resume_fifo(hw);
768 if (err < 0)
769 goto out;
770 }
771
772 hw->fifo_mask = fifo_mask;
773
774 out:
775 mutex_unlock(&hw->conf_lock);
776
777 return err;
778 }
779
st_lsm6dsx_buffer_preenable(struct iio_dev * iio_dev)780 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
781 {
782 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
783 struct st_lsm6dsx_hw *hw = sensor->hw;
784
785 if (!hw->settings->fifo_ops.update_fifo)
786 return -ENOTSUPP;
787
788 return hw->settings->fifo_ops.update_fifo(sensor, true);
789 }
790
st_lsm6dsx_buffer_postdisable(struct iio_dev * iio_dev)791 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
792 {
793 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
794 struct st_lsm6dsx_hw *hw = sensor->hw;
795
796 if (!hw->settings->fifo_ops.update_fifo)
797 return -ENOTSUPP;
798
799 return hw->settings->fifo_ops.update_fifo(sensor, false);
800 }
801
802 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
803 .preenable = st_lsm6dsx_buffer_preenable,
804 .postdisable = st_lsm6dsx_buffer_postdisable,
805 };
806
st_lsm6dsx_hwfifo_odr_show(struct device * dev,struct device_attribute * attr,char * buf)807 static ssize_t st_lsm6dsx_hwfifo_odr_show(struct device *dev,
808 struct device_attribute *attr, char *buf)
809 {
810 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev));
811
812 return sysfs_emit(buf, "%d.%03d\n", sensor->hwfifo_odr_mHz / 1000,
813 sensor->hwfifo_odr_mHz % 1000);
814 }
815
st_lsm6dsx_hwfifo_odr_store(struct device * dev,struct device_attribute * attr,const char * buf,size_t len)816 static ssize_t st_lsm6dsx_hwfifo_odr_store(struct device *dev,
817 struct device_attribute *attr,
818 const char *buf, size_t len)
819 {
820 struct iio_dev *iio_dev = dev_to_iio_dev(dev);
821 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
822 int integer, milli;
823 int ret;
824 u32 hwfifo_odr;
825 u8 data;
826
827 if (!iio_device_claim_direct(iio_dev))
828 return -EBUSY;
829
830 ret = iio_str_to_fixpoint(buf, 100, &integer, &milli);
831 if (ret)
832 goto out;
833
834 hwfifo_odr = integer * 1000 + milli;
835 ret = st_lsm6dsx_check_odr(sensor, hwfifo_odr, &data);
836 if (ret < 0)
837 goto out;
838
839 hwfifo_odr = ret;
840
841 /* the batch data rate must not exceed the sensor output data rate */
842 if (hwfifo_odr <= sensor->odr)
843 sensor->hwfifo_odr_mHz = hwfifo_odr;
844 else
845 ret = -EINVAL;
846
847 out:
848 iio_device_release_direct(iio_dev);
849
850 return ret < 0 ? ret : len;
851 }
852
853 static IIO_DEV_ATTR_SAMP_FREQ(0664, st_lsm6dsx_hwfifo_odr_show, st_lsm6dsx_hwfifo_odr_store);
854
855 static const struct iio_dev_attr *st_lsm6dsx_buffer_attrs[] = {
856 &iio_dev_attr_sampling_frequency,
857 NULL
858 };
859
st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw * hw)860 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
861 {
862 int i, ret;
863
864 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
865 const struct iio_dev_attr **attrs;
866
867 if (!hw->iio_devs[i])
868 continue;
869
870 /*
871 * For the accelerometer, allow setting FIFO sampling frequency
872 * values different from the sensor sampling frequency, which
873 * may be needed to keep FIFO data rate low while sampling
874 * acceleration data at high rates for accurate event detection.
875 */
876 attrs = i == ST_LSM6DSX_ID_ACC ? st_lsm6dsx_buffer_attrs : NULL;
877 ret = devm_iio_kfifo_buffer_setup_ext(hw->dev, hw->iio_devs[i],
878 &st_lsm6dsx_buffer_ops,
879 attrs);
880 if (ret)
881 return ret;
882 }
883
884 return 0;
885 }
886