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 batch_reg = &hw->settings->batch[sensor->id];
229 if (batch_reg->addr) {
230 int val;
231
232 if (enable) {
233 int err;
234
235 err = st_lsm6dsx_check_odr(sensor, sensor->hwfifo_odr_mHz,
236 &data);
237 if (err < 0)
238 return err;
239 } else {
240 data = 0;
241 }
242 val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
243 return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
244 batch_reg->mask, val);
245 } else {
246 data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
247 return st_lsm6dsx_update_bits_locked(hw,
248 ST_LSM6DSX_REG_FIFO_MODE_ADDR,
249 ST_LSM6DSX_FIFO_ODR_MASK,
250 FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
251 data));
252 }
253 }
254
st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor * sensor,u16 watermark)255 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
256 {
257 u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask;
258 struct st_lsm6dsx_hw *hw = sensor->hw;
259 struct st_lsm6dsx_sensor *cur_sensor;
260 int i, err, data;
261 __le16 wdata;
262
263 if (!hw->sip)
264 return 0;
265
266 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
267 if (!hw->iio_devs[i])
268 continue;
269
270 cur_sensor = iio_priv(hw->iio_devs[i]);
271
272 if (!(hw->enable_mask & BIT(cur_sensor->id)))
273 continue;
274
275 cur_watermark = (cur_sensor == sensor) ? watermark
276 : cur_sensor->watermark;
277
278 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
279 }
280
281 fifo_watermark = max_t(u16, fifo_watermark, hw->sip);
282 fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;
283 fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
284
285 mutex_lock(&hw->page_lock);
286 err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
287 &data);
288 if (err < 0)
289 goto out;
290
291 fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
292 fifo_watermark = ((data << 8) & ~fifo_th_mask) |
293 (fifo_watermark & fifo_th_mask);
294
295 wdata = cpu_to_le16(fifo_watermark);
296 err = regmap_bulk_write(hw->regmap,
297 hw->settings->fifo_ops.fifo_th.addr,
298 &wdata, sizeof(wdata));
299 out:
300 mutex_unlock(&hw->page_lock);
301 return err;
302 }
303
st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw * hw)304 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
305 {
306 struct st_lsm6dsx_sensor *sensor;
307 int i, err;
308
309 /* reset hw ts counter */
310 err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
311 ST_LSM6DSX_TS_RESET_VAL);
312 if (err < 0)
313 return err;
314
315 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
316 if (!hw->iio_devs[i])
317 continue;
318
319 sensor = iio_priv(hw->iio_devs[i]);
320 /*
321 * store enable buffer timestamp as reference for
322 * hw timestamp
323 */
324 sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
325 }
326 return 0;
327 }
328
st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw * hw)329 int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw)
330 {
331 int err;
332
333 /* reset hw ts counter */
334 err = st_lsm6dsx_reset_hw_ts(hw);
335 if (err < 0)
336 return err;
337
338 return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
339 }
340
341 /*
342 * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
343 * in order to avoid a kmalloc for each bus access
344 */
st_lsm6dsx_read_block(struct st_lsm6dsx_hw * hw,u8 addr,u8 * data,unsigned int data_len,unsigned int max_word_len)345 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
346 u8 *data, unsigned int data_len,
347 unsigned int max_word_len)
348 {
349 unsigned int word_len, read_len = 0;
350 int err;
351
352 while (read_len < data_len) {
353 word_len = min_t(unsigned int, data_len - read_len,
354 max_word_len);
355 err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
356 word_len);
357 if (err < 0)
358 return err;
359 read_len += word_len;
360 }
361 return 0;
362 }
363
364 #define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
365 sizeof(s64)) + sizeof(s64))
366 /**
367 * st_lsm6dsx_read_fifo() - hw FIFO read routine
368 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
369 *
370 * Read samples from the hw FIFO and push them to IIO buffers.
371 *
372 * Return: Number of bytes read from the FIFO
373 */
st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw * hw)374 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
375 {
376 struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
377 int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
378 u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
379 u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
380 bool reset_ts = false;
381 __le16 fifo_status;
382 s64 ts = 0;
383
384 err = st_lsm6dsx_read_locked(hw,
385 hw->settings->fifo_ops.fifo_diff.addr,
386 &fifo_status, sizeof(fifo_status));
387 if (err < 0) {
388 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
389 err);
390 return err;
391 }
392
393 if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
394 return 0;
395
396 if (!pattern_len)
397 pattern_len = ST_LSM6DSX_SAMPLE_SIZE;
398
399 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
400 ST_LSM6DSX_CHAN_SIZE;
401 fifo_len = (fifo_len / pattern_len) * pattern_len;
402
403 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
404 gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
405 if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
406 ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
407
408 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
409 err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
410 hw->buff, pattern_len,
411 ST_LSM6DSX_MAX_WORD_LEN);
412 if (err < 0) {
413 dev_err(hw->dev,
414 "failed to read pattern from fifo (err=%d)\n",
415 err);
416 return err;
417 }
418
419 /*
420 * Data are written to the FIFO with a specific pattern
421 * depending on the configured ODRs. The first sequence of data
422 * stored in FIFO contains the data of all enabled sensors
423 * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
424 * depending on the value of the decimation factor set for each
425 * sensor.
426 *
427 * Supposing the FIFO is storing data from gyroscope and
428 * accelerometer at different ODRs:
429 * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
430 * Since the gyroscope ODR is twice the accelerometer one, the
431 * following pattern is repeated every 9 samples:
432 * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
433 */
434 ext_sip = ext_sensor ? ext_sensor->sip : 0;
435 gyro_sip = gyro_sensor->sip;
436 acc_sip = acc_sensor->sip;
437 ts_sip = hw->ts_sip;
438 offset = 0;
439 sip = 0;
440
441 while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
442 if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
443 memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels,
444 &hw->buff[offset],
445 sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels));
446 offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels);
447 }
448 if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
449 memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels,
450 &hw->buff[offset],
451 sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels));
452 offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels);
453 }
454 if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
455 memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels,
456 &hw->buff[offset],
457 sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels));
458 offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels);
459 }
460
461 if (ts_sip-- > 0) {
462 u8 data[ST_LSM6DSX_SAMPLE_SIZE];
463
464 memcpy(data, &hw->buff[offset], sizeof(data));
465 /*
466 * hw timestamp is 3B long and it is stored
467 * in FIFO using 6B as 4th FIFO data set
468 * according to this schema:
469 * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
470 */
471 ts = data[1] << 16 | data[0] << 8 | data[3];
472 /*
473 * check if hw timestamp engine is going to
474 * reset (the sensor generates an interrupt
475 * to signal the hw timestamp will reset in
476 * 1.638s)
477 */
478 if (!reset_ts && ts >= 0xff0000)
479 reset_ts = true;
480 ts *= hw->ts_gain;
481
482 offset += ST_LSM6DSX_SAMPLE_SIZE;
483 }
484
485 if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
486 /*
487 * We need to discards gyro samples during
488 * filters settling time
489 */
490 if (gyro_sensor->samples_to_discard > 0)
491 gyro_sensor->samples_to_discard--;
492 else
493 iio_push_to_buffers_with_timestamp(
494 hw->iio_devs[ST_LSM6DSX_ID_GYRO],
495 &hw->scan[ST_LSM6DSX_ID_GYRO],
496 gyro_sensor->ts_ref + ts);
497 gyro_sip--;
498 }
499 if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
500 /*
501 * We need to discards accel samples during
502 * filters settling time
503 */
504 if (acc_sensor->samples_to_discard > 0)
505 acc_sensor->samples_to_discard--;
506 else
507 iio_push_to_buffers_with_timestamp(
508 hw->iio_devs[ST_LSM6DSX_ID_ACC],
509 &hw->scan[ST_LSM6DSX_ID_ACC],
510 acc_sensor->ts_ref + ts);
511 acc_sip--;
512 }
513 if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
514 iio_push_to_buffers_with_timestamp(
515 hw->iio_devs[ST_LSM6DSX_ID_EXT0],
516 &hw->scan[ST_LSM6DSX_ID_EXT0],
517 ext_sensor->ts_ref + ts);
518 ext_sip--;
519 }
520 sip++;
521 }
522 }
523
524 if (unlikely(reset_ts)) {
525 err = st_lsm6dsx_reset_hw_ts(hw);
526 if (err < 0) {
527 dev_err(hw->dev, "failed to reset hw ts (err=%d)\n",
528 err);
529 return err;
530 }
531 }
532 return read_len;
533 }
534
535 #define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd
536 static int
st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw * hw,u8 tag,u8 * data,s64 ts)537 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
538 u8 *data, s64 ts)
539 {
540 s16 val = le16_to_cpu(*(__le16 *)data);
541 struct st_lsm6dsx_sensor *sensor;
542 struct iio_dev *iio_dev;
543
544 /* invalid sample during bootstrap phase */
545 if (val >= ST_LSM6DSX_INVALID_SAMPLE)
546 return -EINVAL;
547
548 /*
549 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
550 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
551 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
552 * channel
553 */
554 switch (tag) {
555 case ST_LSM6DSX_GYRO_TAG:
556 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
557 break;
558 case ST_LSM6DSX_ACC_TAG:
559 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
560 break;
561 case ST_LSM6DSX_EXT0_TAG:
562 if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
563 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
564 else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
565 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
566 else
567 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
568 break;
569 case ST_LSM6DSX_EXT1_TAG:
570 if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
571 (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
572 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
573 else
574 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
575 break;
576 case ST_LSM6DSX_EXT2_TAG:
577 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
578 break;
579 default:
580 return -EINVAL;
581 }
582
583 sensor = iio_priv(iio_dev);
584 iio_push_to_buffers_with_timestamp(iio_dev, data,
585 ts + sensor->ts_ref);
586
587 return 0;
588 }
589
590 /**
591 * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine
592 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
593 *
594 * Read samples from the hw FIFO and push them to IIO buffers.
595 *
596 * Return: Number of bytes read from the FIFO
597 */
st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw * hw)598 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
599 {
600 u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
601 u16 fifo_len, fifo_diff_mask;
602 /*
603 * Alignment needed as this can ultimately be passed to a
604 * call to iio_push_to_buffers_with_timestamp() which
605 * must be passed a buffer that is aligned to 8 bytes so
606 * as to allow insertion of a naturally aligned timestamp.
607 */
608 u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE] __aligned(8);
609 u8 tag;
610 bool reset_ts = false;
611 int i, err, read_len;
612 __le16 fifo_status;
613 s64 ts = 0;
614
615 err = st_lsm6dsx_read_locked(hw,
616 hw->settings->fifo_ops.fifo_diff.addr,
617 &fifo_status, sizeof(fifo_status));
618 if (err < 0) {
619 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
620 err);
621 return err;
622 }
623
624 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
625 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
626 ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
627 if (!fifo_len)
628 return 0;
629
630 if (!pattern_len)
631 pattern_len = ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
632
633 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
634 err = st_lsm6dsx_read_block(hw,
635 ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
636 hw->buff, pattern_len,
637 ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
638 if (err < 0) {
639 dev_err(hw->dev,
640 "failed to read pattern from fifo (err=%d)\n",
641 err);
642 return err;
643 }
644
645 for (i = 0; i < pattern_len;
646 i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
647 memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
648 ST_LSM6DSX_SAMPLE_SIZE);
649
650 tag = hw->buff[i] >> 3;
651 if (tag == ST_LSM6DSX_TS_TAG) {
652 /*
653 * hw timestamp is 4B long and it is stored
654 * in FIFO according to this schema:
655 * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
656 * B3 = ts[31:24]
657 */
658 ts = le32_to_cpu(*((__le32 *)iio_buff));
659 /*
660 * check if hw timestamp engine is going to
661 * reset (the sensor generates an interrupt
662 * to signal the hw timestamp will reset in
663 * 1.638s)
664 */
665 if (!reset_ts && ts >= 0xffff0000)
666 reset_ts = true;
667 ts *= hw->ts_gain;
668 } else {
669 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
670 ts);
671 }
672 }
673 }
674
675 if (unlikely(reset_ts)) {
676 err = st_lsm6dsx_reset_hw_ts(hw);
677 if (err < 0)
678 return err;
679 }
680 return read_len;
681 }
682
st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw * hw)683 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
684 {
685 int err;
686
687 if (!hw->settings->fifo_ops.read_fifo)
688 return -ENOTSUPP;
689
690 mutex_lock(&hw->fifo_lock);
691
692 hw->settings->fifo_ops.read_fifo(hw);
693 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
694
695 mutex_unlock(&hw->fifo_lock);
696
697 return err;
698 }
699
700 static void
st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor * sensor)701 st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor)
702 {
703 const struct st_lsm6dsx_samples_to_discard *data;
704 struct st_lsm6dsx_hw *hw = sensor->hw;
705 int i;
706
707 if (sensor->id != ST_LSM6DSX_ID_GYRO &&
708 sensor->id != ST_LSM6DSX_ID_ACC)
709 return;
710
711 /* check if drdy mask is supported in hw */
712 if (hw->settings->drdy_mask.addr)
713 return;
714
715 data = &hw->settings->samples_to_discard[sensor->id];
716 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
717 if (data->val[i].milli_hz == sensor->hwfifo_odr_mHz) {
718 sensor->samples_to_discard = data->val[i].samples;
719 return;
720 }
721 }
722 }
723
st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor * sensor,bool enable)724 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
725 {
726 struct st_lsm6dsx_hw *hw = sensor->hw;
727 u8 fifo_mask;
728 int err;
729
730 mutex_lock(&hw->conf_lock);
731
732 if (enable)
733 fifo_mask = hw->fifo_mask | BIT(sensor->id);
734 else
735 fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
736
737 if (hw->fifo_mask) {
738 err = st_lsm6dsx_flush_fifo(hw);
739 if (err < 0)
740 goto out;
741 }
742
743 if (enable)
744 st_lsm6dsx_update_samples_to_discard(sensor);
745
746 err = st_lsm6dsx_device_set_enable(sensor, enable);
747 if (err < 0)
748 goto out;
749
750 err = st_lsm6dsx_set_fifo_odr(sensor, enable);
751 if (err < 0)
752 goto out;
753
754 err = st_lsm6dsx_update_decimators(hw);
755 if (err < 0)
756 goto out;
757
758 err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
759 if (err < 0)
760 goto out;
761
762 if (fifo_mask) {
763 err = st_lsm6dsx_resume_fifo(hw);
764 if (err < 0)
765 goto out;
766 }
767
768 hw->fifo_mask = fifo_mask;
769
770 out:
771 mutex_unlock(&hw->conf_lock);
772
773 return err;
774 }
775
st_lsm6dsx_buffer_preenable(struct iio_dev * iio_dev)776 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
777 {
778 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
779 struct st_lsm6dsx_hw *hw = sensor->hw;
780
781 if (!hw->settings->fifo_ops.update_fifo)
782 return -ENOTSUPP;
783
784 return hw->settings->fifo_ops.update_fifo(sensor, true);
785 }
786
st_lsm6dsx_buffer_postdisable(struct iio_dev * iio_dev)787 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
788 {
789 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
790 struct st_lsm6dsx_hw *hw = sensor->hw;
791
792 if (!hw->settings->fifo_ops.update_fifo)
793 return -ENOTSUPP;
794
795 return hw->settings->fifo_ops.update_fifo(sensor, false);
796 }
797
798 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
799 .preenable = st_lsm6dsx_buffer_preenable,
800 .postdisable = st_lsm6dsx_buffer_postdisable,
801 };
802
st_lsm6dsx_hwfifo_odr_show(struct device * dev,struct device_attribute * attr,char * buf)803 static ssize_t st_lsm6dsx_hwfifo_odr_show(struct device *dev,
804 struct device_attribute *attr, char *buf)
805 {
806 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev));
807
808 return sysfs_emit(buf, "%d.%03d\n", sensor->hwfifo_odr_mHz / 1000,
809 sensor->hwfifo_odr_mHz % 1000);
810 }
811
st_lsm6dsx_hwfifo_odr_store(struct device * dev,struct device_attribute * attr,const char * buf,size_t len)812 static ssize_t st_lsm6dsx_hwfifo_odr_store(struct device *dev,
813 struct device_attribute *attr,
814 const char *buf, size_t len)
815 {
816 struct iio_dev *iio_dev = dev_to_iio_dev(dev);
817 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
818 int integer, milli;
819 int ret;
820 u32 hwfifo_odr;
821 u8 data;
822
823 if (!iio_device_claim_direct(iio_dev))
824 return -EBUSY;
825
826 ret = iio_str_to_fixpoint(buf, 100, &integer, &milli);
827 if (ret)
828 goto out;
829
830 hwfifo_odr = integer * 1000 + milli;
831 ret = st_lsm6dsx_check_odr(sensor, hwfifo_odr, &data);
832 if (ret < 0)
833 goto out;
834
835 hwfifo_odr = ret;
836
837 /* the batch data rate must not exceed the sensor output data rate */
838 if (hwfifo_odr <= sensor->odr)
839 sensor->hwfifo_odr_mHz = hwfifo_odr;
840 else
841 ret = -EINVAL;
842
843 out:
844 iio_device_release_direct(iio_dev);
845
846 return ret < 0 ? ret : len;
847 }
848
849 static IIO_DEV_ATTR_SAMP_FREQ(0664, st_lsm6dsx_hwfifo_odr_show, st_lsm6dsx_hwfifo_odr_store);
850
851 static const struct iio_dev_attr *st_lsm6dsx_buffer_attrs[] = {
852 &iio_dev_attr_sampling_frequency,
853 NULL
854 };
855
st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw * hw)856 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
857 {
858 int i, ret;
859
860 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
861 if (!hw->iio_devs[i])
862 continue;
863
864 ret = devm_iio_kfifo_buffer_setup_ext(hw->dev, hw->iio_devs[i],
865 &st_lsm6dsx_buffer_ops,
866 st_lsm6dsx_buffer_attrs);
867 if (ret)
868 return ret;
869 }
870
871 return 0;
872 }
873