1 // SPDX-License-Identifier: GPL-2.0-only
3 * STMicroelectronics st_lsm6dsx FIFO buffer library driver
5 * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC/LSM6DS3TR-C:
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.
17 * LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR/LSM6DSRX/ISM330DHCX/LSM6DST/LSM6DSOP:
18 * The FIFO buffer can be configured to store data from gyroscope and
19 * accelerometer. Each sample is queued with a tag (1B) indicating data
20 * source (gyroscope, accelerometer, hw timer).
22 * FIFO supported modes:
23 * - BYPASS: FIFO disabled
24 * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
25 * restarts from the beginning and the oldest sample is overwritten
27 * Copyright 2016 STMicroelectronics Inc.
29 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
30 * Denis Ciocca <denis.ciocca@st.com>
32 #include <linux/module.h>
33 #include <linux/iio/kfifo_buf.h>
34 #include <linux/iio/iio.h>
35 #include <linux/iio/buffer.h>
36 #include <linux/regmap.h>
37 #include <linux/bitfield.h>
39 #include <linux/platform_data/st_sensors_pdata.h>
41 #include "st_lsm6dsx.h"
43 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
44 #define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
45 #define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
46 #define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
47 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
48 #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78
49 #define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
51 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
53 #define ST_LSM6DSX_TS_RESET_VAL 0xaa
55 struct st_lsm6dsx_decimator_entry
{
60 enum st_lsm6dsx_fifo_tag
{
61 ST_LSM6DSX_GYRO_TAG
= 0x01,
62 ST_LSM6DSX_ACC_TAG
= 0x02,
63 ST_LSM6DSX_TS_TAG
= 0x04,
64 ST_LSM6DSX_EXT0_TAG
= 0x0f,
65 ST_LSM6DSX_EXT1_TAG
= 0x10,
66 ST_LSM6DSX_EXT2_TAG
= 0x11,
70 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table
[] = {
82 st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor
*sensor
, u32 max_odr
)
84 const int max_size
= ARRAY_SIZE(st_lsm6dsx_decimator_table
);
85 u32 decimator
= max_odr
/ sensor
->odr
;
89 decimator
= round_down(decimator
, 2);
91 for (i
= 0; i
< max_size
; i
++) {
92 if (st_lsm6dsx_decimator_table
[i
].decimator
== decimator
)
96 sensor
->decimator
= decimator
;
97 return i
== max_size
? 0 : st_lsm6dsx_decimator_table
[i
].val
;
100 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw
*hw
,
101 u32
*max_odr
, u32
*min_odr
)
103 struct st_lsm6dsx_sensor
*sensor
;
106 *max_odr
= 0, *min_odr
= ~0;
107 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
108 if (!hw
->iio_devs
[i
])
111 sensor
= iio_priv(hw
->iio_devs
[i
]);
113 if (!(hw
->enable_mask
& BIT(sensor
->id
)))
116 *max_odr
= max_t(u32
, *max_odr
, sensor
->odr
);
117 *min_odr
= min_t(u32
, *min_odr
, sensor
->odr
);
121 static u8
st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor
*sensor
, u32 min_odr
)
123 u8 sip
= sensor
->odr
/ min_odr
;
125 return sip
> 1 ? round_down(sip
, 2) : sip
;
128 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw
*hw
)
130 const struct st_lsm6dsx_reg
*ts_dec_reg
;
131 struct st_lsm6dsx_sensor
*sensor
;
132 u16 sip
= 0, ts_sip
= 0;
133 u32 max_odr
, min_odr
;
137 st_lsm6dsx_get_max_min_odr(hw
, &max_odr
, &min_odr
);
139 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
140 const struct st_lsm6dsx_reg
*dec_reg
;
142 if (!hw
->iio_devs
[i
])
145 sensor
= iio_priv(hw
->iio_devs
[i
]);
146 /* update fifo decimators and sample in pattern */
147 if (hw
->enable_mask
& BIT(sensor
->id
)) {
148 sensor
->sip
= st_lsm6dsx_get_sip(sensor
, min_odr
);
149 data
= st_lsm6dsx_get_decimator_val(sensor
, max_odr
);
154 ts_sip
= max_t(u16
, ts_sip
, sensor
->sip
);
156 dec_reg
= &hw
->settings
->decimator
[sensor
->id
];
158 int val
= ST_LSM6DSX_SHIFT_VAL(data
, dec_reg
->mask
);
160 err
= st_lsm6dsx_update_bits_locked(hw
, dec_reg
->addr
,
168 hw
->sip
= sip
+ ts_sip
;
172 * update hw ts decimator if necessary. Decimator for hw timestamp
173 * is always 1 or 0 in order to have a ts sample for each data
176 ts_dec_reg
= &hw
->settings
->ts_settings
.decimator
;
177 if (ts_dec_reg
->addr
) {
178 int val
, ts_dec
= !!hw
->ts_sip
;
180 val
= ST_LSM6DSX_SHIFT_VAL(ts_dec
, ts_dec_reg
->mask
);
181 err
= st_lsm6dsx_update_bits_locked(hw
, ts_dec_reg
->addr
,
182 ts_dec_reg
->mask
, val
);
187 static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw
*hw
,
188 enum st_lsm6dsx_fifo_mode fifo_mode
)
192 data
= FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK
, fifo_mode
);
193 return st_lsm6dsx_update_bits_locked(hw
, ST_LSM6DSX_REG_FIFO_MODE_ADDR
,
194 ST_LSM6DSX_FIFO_MODE_MASK
, data
);
197 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor
*sensor
,
200 struct st_lsm6dsx_hw
*hw
= sensor
->hw
;
201 const struct st_lsm6dsx_reg
*batch_reg
;
204 batch_reg
= &hw
->settings
->batch
[sensor
->id
];
205 if (batch_reg
->addr
) {
211 err
= st_lsm6dsx_check_odr(sensor
, sensor
->odr
,
218 val
= ST_LSM6DSX_SHIFT_VAL(data
, batch_reg
->mask
);
219 return st_lsm6dsx_update_bits_locked(hw
, batch_reg
->addr
,
220 batch_reg
->mask
, val
);
222 data
= hw
->enable_mask
? ST_LSM6DSX_MAX_FIFO_ODR_VAL
: 0;
223 return st_lsm6dsx_update_bits_locked(hw
,
224 ST_LSM6DSX_REG_FIFO_MODE_ADDR
,
225 ST_LSM6DSX_FIFO_ODR_MASK
,
226 FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK
,
231 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor
*sensor
, u16 watermark
)
233 u16 fifo_watermark
= ~0, cur_watermark
, fifo_th_mask
;
234 struct st_lsm6dsx_hw
*hw
= sensor
->hw
;
235 struct st_lsm6dsx_sensor
*cur_sensor
;
242 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
243 if (!hw
->iio_devs
[i
])
246 cur_sensor
= iio_priv(hw
->iio_devs
[i
]);
248 if (!(hw
->enable_mask
& BIT(cur_sensor
->id
)))
251 cur_watermark
= (cur_sensor
== sensor
) ? watermark
252 : cur_sensor
->watermark
;
254 fifo_watermark
= min_t(u16
, fifo_watermark
, cur_watermark
);
257 fifo_watermark
= max_t(u16
, fifo_watermark
, hw
->sip
);
258 fifo_watermark
= (fifo_watermark
/ hw
->sip
) * hw
->sip
;
259 fifo_watermark
= fifo_watermark
* hw
->settings
->fifo_ops
.th_wl
;
261 mutex_lock(&hw
->page_lock
);
262 err
= regmap_read(hw
->regmap
, hw
->settings
->fifo_ops
.fifo_th
.addr
+ 1,
267 fifo_th_mask
= hw
->settings
->fifo_ops
.fifo_th
.mask
;
268 fifo_watermark
= ((data
<< 8) & ~fifo_th_mask
) |
269 (fifo_watermark
& fifo_th_mask
);
271 wdata
= cpu_to_le16(fifo_watermark
);
272 err
= regmap_bulk_write(hw
->regmap
,
273 hw
->settings
->fifo_ops
.fifo_th
.addr
,
274 &wdata
, sizeof(wdata
));
276 mutex_unlock(&hw
->page_lock
);
280 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw
*hw
)
282 struct st_lsm6dsx_sensor
*sensor
;
285 /* reset hw ts counter */
286 err
= st_lsm6dsx_write_locked(hw
, ST_LSM6DSX_REG_TS_RESET_ADDR
,
287 ST_LSM6DSX_TS_RESET_VAL
);
291 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
292 if (!hw
->iio_devs
[i
])
295 sensor
= iio_priv(hw
->iio_devs
[i
]);
297 * store enable buffer timestamp as reference for
300 sensor
->ts_ref
= iio_get_time_ns(hw
->iio_devs
[i
]);
305 int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw
*hw
)
309 /* reset hw ts counter */
310 err
= st_lsm6dsx_reset_hw_ts(hw
);
314 return st_lsm6dsx_set_fifo_mode(hw
, ST_LSM6DSX_FIFO_CONT
);
318 * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
319 * in order to avoid a kmalloc for each bus access
321 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw
*hw
, u8 addr
,
322 u8
*data
, unsigned int data_len
,
323 unsigned int max_word_len
)
325 unsigned int word_len
, read_len
= 0;
328 while (read_len
< data_len
) {
329 word_len
= min_t(unsigned int, data_len
- read_len
,
331 err
= st_lsm6dsx_read_locked(hw
, addr
, data
+ read_len
,
335 read_len
+= word_len
;
340 #define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
341 sizeof(s64)) + sizeof(s64))
343 * st_lsm6dsx_read_fifo() - hw FIFO read routine
344 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
346 * Read samples from the hw FIFO and push them to IIO buffers.
348 * Return: Number of bytes read from the FIFO
350 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw
*hw
)
352 struct st_lsm6dsx_sensor
*acc_sensor
, *gyro_sensor
, *ext_sensor
= NULL
;
353 int err
, sip
, acc_sip
, gyro_sip
, ts_sip
, ext_sip
, read_len
, offset
;
354 u16 fifo_len
, pattern_len
= hw
->sip
* ST_LSM6DSX_SAMPLE_SIZE
;
355 u16 fifo_diff_mask
= hw
->settings
->fifo_ops
.fifo_diff
.mask
;
356 bool reset_ts
= false;
360 err
= st_lsm6dsx_read_locked(hw
,
361 hw
->settings
->fifo_ops
.fifo_diff
.addr
,
362 &fifo_status
, sizeof(fifo_status
));
364 dev_err(hw
->dev
, "failed to read fifo status (err=%d)\n",
369 if (fifo_status
& cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK
))
372 fifo_len
= (le16_to_cpu(fifo_status
) & fifo_diff_mask
) *
373 ST_LSM6DSX_CHAN_SIZE
;
374 fifo_len
= (fifo_len
/ pattern_len
) * pattern_len
;
376 acc_sensor
= iio_priv(hw
->iio_devs
[ST_LSM6DSX_ID_ACC
]);
377 gyro_sensor
= iio_priv(hw
->iio_devs
[ST_LSM6DSX_ID_GYRO
]);
378 if (hw
->iio_devs
[ST_LSM6DSX_ID_EXT0
])
379 ext_sensor
= iio_priv(hw
->iio_devs
[ST_LSM6DSX_ID_EXT0
]);
381 for (read_len
= 0; read_len
< fifo_len
; read_len
+= pattern_len
) {
382 err
= st_lsm6dsx_read_block(hw
, ST_LSM6DSX_REG_FIFO_OUTL_ADDR
,
383 hw
->buff
, pattern_len
,
384 ST_LSM6DSX_MAX_WORD_LEN
);
387 "failed to read pattern from fifo (err=%d)\n",
393 * Data are written to the FIFO with a specific pattern
394 * depending on the configured ODRs. The first sequence of data
395 * stored in FIFO contains the data of all enabled sensors
396 * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
397 * depending on the value of the decimation factor set for each
400 * Supposing the FIFO is storing data from gyroscope and
401 * accelerometer at different ODRs:
402 * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
403 * Since the gyroscope ODR is twice the accelerometer one, the
404 * following pattern is repeated every 9 samples:
405 * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
407 ext_sip
= ext_sensor
? ext_sensor
->sip
: 0;
408 gyro_sip
= gyro_sensor
->sip
;
409 acc_sip
= acc_sensor
->sip
;
414 while (acc_sip
> 0 || gyro_sip
> 0 || ext_sip
> 0) {
415 if (gyro_sip
> 0 && !(sip
% gyro_sensor
->decimator
)) {
416 memcpy(hw
->scan
[ST_LSM6DSX_ID_GYRO
].channels
,
418 sizeof(hw
->scan
[ST_LSM6DSX_ID_GYRO
].channels
));
419 offset
+= sizeof(hw
->scan
[ST_LSM6DSX_ID_GYRO
].channels
);
421 if (acc_sip
> 0 && !(sip
% acc_sensor
->decimator
)) {
422 memcpy(hw
->scan
[ST_LSM6DSX_ID_ACC
].channels
,
424 sizeof(hw
->scan
[ST_LSM6DSX_ID_ACC
].channels
));
425 offset
+= sizeof(hw
->scan
[ST_LSM6DSX_ID_ACC
].channels
);
427 if (ext_sip
> 0 && !(sip
% ext_sensor
->decimator
)) {
428 memcpy(hw
->scan
[ST_LSM6DSX_ID_EXT0
].channels
,
430 sizeof(hw
->scan
[ST_LSM6DSX_ID_EXT0
].channels
));
431 offset
+= sizeof(hw
->scan
[ST_LSM6DSX_ID_EXT0
].channels
);
435 u8 data
[ST_LSM6DSX_SAMPLE_SIZE
];
437 memcpy(data
, &hw
->buff
[offset
], sizeof(data
));
439 * hw timestamp is 3B long and it is stored
440 * in FIFO using 6B as 4th FIFO data set
441 * according to this schema:
442 * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
444 ts
= data
[1] << 16 | data
[0] << 8 | data
[3];
446 * check if hw timestamp engine is going to
447 * reset (the sensor generates an interrupt
448 * to signal the hw timestamp will reset in
451 if (!reset_ts
&& ts
>= 0xff0000)
455 offset
+= ST_LSM6DSX_SAMPLE_SIZE
;
458 if (gyro_sip
> 0 && !(sip
% gyro_sensor
->decimator
)) {
459 iio_push_to_buffers_with_timestamp(
460 hw
->iio_devs
[ST_LSM6DSX_ID_GYRO
],
461 &hw
->scan
[ST_LSM6DSX_ID_GYRO
],
462 gyro_sensor
->ts_ref
+ ts
);
465 if (acc_sip
> 0 && !(sip
% acc_sensor
->decimator
)) {
466 iio_push_to_buffers_with_timestamp(
467 hw
->iio_devs
[ST_LSM6DSX_ID_ACC
],
468 &hw
->scan
[ST_LSM6DSX_ID_ACC
],
469 acc_sensor
->ts_ref
+ ts
);
472 if (ext_sip
> 0 && !(sip
% ext_sensor
->decimator
)) {
473 iio_push_to_buffers_with_timestamp(
474 hw
->iio_devs
[ST_LSM6DSX_ID_EXT0
],
475 &hw
->scan
[ST_LSM6DSX_ID_EXT0
],
476 ext_sensor
->ts_ref
+ ts
);
483 if (unlikely(reset_ts
)) {
484 err
= st_lsm6dsx_reset_hw_ts(hw
);
486 dev_err(hw
->dev
, "failed to reset hw ts (err=%d)\n",
494 #define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd
496 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw
*hw
, u8 tag
,
499 s16 val
= le16_to_cpu(*(__le16
*)data
);
500 struct st_lsm6dsx_sensor
*sensor
;
501 struct iio_dev
*iio_dev
;
503 /* invalid sample during bootstrap phase */
504 if (val
>= ST_LSM6DSX_INVALID_SAMPLE
)
508 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
509 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
510 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
514 case ST_LSM6DSX_GYRO_TAG
:
515 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_GYRO
];
517 case ST_LSM6DSX_ACC_TAG
:
518 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_ACC
];
520 case ST_LSM6DSX_EXT0_TAG
:
521 if (hw
->enable_mask
& BIT(ST_LSM6DSX_ID_EXT0
))
522 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_EXT0
];
523 else if (hw
->enable_mask
& BIT(ST_LSM6DSX_ID_EXT1
))
524 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_EXT1
];
526 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_EXT2
];
528 case ST_LSM6DSX_EXT1_TAG
:
529 if ((hw
->enable_mask
& BIT(ST_LSM6DSX_ID_EXT0
)) &&
530 (hw
->enable_mask
& BIT(ST_LSM6DSX_ID_EXT1
)))
531 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_EXT1
];
533 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_EXT2
];
535 case ST_LSM6DSX_EXT2_TAG
:
536 iio_dev
= hw
->iio_devs
[ST_LSM6DSX_ID_EXT2
];
542 sensor
= iio_priv(iio_dev
);
543 iio_push_to_buffers_with_timestamp(iio_dev
, data
,
544 ts
+ sensor
->ts_ref
);
550 * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine
551 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
553 * Read samples from the hw FIFO and push them to IIO buffers.
555 * Return: Number of bytes read from the FIFO
557 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw
*hw
)
559 u16 pattern_len
= hw
->sip
* ST_LSM6DSX_TAGGED_SAMPLE_SIZE
;
560 u16 fifo_len
, fifo_diff_mask
;
562 * Alignment needed as this can ultimately be passed to a
563 * call to iio_push_to_buffers_with_timestamp() which
564 * must be passed a buffer that is aligned to 8 bytes so
565 * as to allow insertion of a naturally aligned timestamp.
567 u8 iio_buff
[ST_LSM6DSX_IIO_BUFF_SIZE
] __aligned(8);
569 bool reset_ts
= false;
570 int i
, err
, read_len
;
574 err
= st_lsm6dsx_read_locked(hw
,
575 hw
->settings
->fifo_ops
.fifo_diff
.addr
,
576 &fifo_status
, sizeof(fifo_status
));
578 dev_err(hw
->dev
, "failed to read fifo status (err=%d)\n",
583 fifo_diff_mask
= hw
->settings
->fifo_ops
.fifo_diff
.mask
;
584 fifo_len
= (le16_to_cpu(fifo_status
) & fifo_diff_mask
) *
585 ST_LSM6DSX_TAGGED_SAMPLE_SIZE
;
589 for (read_len
= 0; read_len
< fifo_len
; read_len
+= pattern_len
) {
590 err
= st_lsm6dsx_read_block(hw
,
591 ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR
,
592 hw
->buff
, pattern_len
,
593 ST_LSM6DSX_MAX_TAGGED_WORD_LEN
);
596 "failed to read pattern from fifo (err=%d)\n",
601 for (i
= 0; i
< pattern_len
;
602 i
+= ST_LSM6DSX_TAGGED_SAMPLE_SIZE
) {
603 memcpy(iio_buff
, &hw
->buff
[i
+ ST_LSM6DSX_TAG_SIZE
],
604 ST_LSM6DSX_SAMPLE_SIZE
);
606 tag
= hw
->buff
[i
] >> 3;
607 if (tag
== ST_LSM6DSX_TS_TAG
) {
609 * hw timestamp is 4B long and it is stored
610 * in FIFO according to this schema:
611 * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
614 ts
= le32_to_cpu(*((__le32
*)iio_buff
));
616 * check if hw timestamp engine is going to
617 * reset (the sensor generates an interrupt
618 * to signal the hw timestamp will reset in
621 if (!reset_ts
&& ts
>= 0xffff0000)
625 st_lsm6dsx_push_tagged_data(hw
, tag
, iio_buff
,
631 if (unlikely(reset_ts
)) {
632 err
= st_lsm6dsx_reset_hw_ts(hw
);
639 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw
*hw
)
643 if (!hw
->settings
->fifo_ops
.read_fifo
)
646 mutex_lock(&hw
->fifo_lock
);
648 hw
->settings
->fifo_ops
.read_fifo(hw
);
649 err
= st_lsm6dsx_set_fifo_mode(hw
, ST_LSM6DSX_FIFO_BYPASS
);
651 mutex_unlock(&hw
->fifo_lock
);
656 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor
*sensor
, bool enable
)
658 struct st_lsm6dsx_hw
*hw
= sensor
->hw
;
662 mutex_lock(&hw
->conf_lock
);
665 fifo_mask
= hw
->fifo_mask
| BIT(sensor
->id
);
667 fifo_mask
= hw
->fifo_mask
& ~BIT(sensor
->id
);
670 err
= st_lsm6dsx_flush_fifo(hw
);
675 if (sensor
->id
== ST_LSM6DSX_ID_EXT0
||
676 sensor
->id
== ST_LSM6DSX_ID_EXT1
||
677 sensor
->id
== ST_LSM6DSX_ID_EXT2
) {
678 err
= st_lsm6dsx_shub_set_enable(sensor
, enable
);
682 err
= st_lsm6dsx_sensor_set_enable(sensor
, enable
);
687 err
= st_lsm6dsx_set_fifo_odr(sensor
, enable
);
691 err
= st_lsm6dsx_update_decimators(hw
);
695 err
= st_lsm6dsx_update_watermark(sensor
, sensor
->watermark
);
700 err
= st_lsm6dsx_resume_fifo(hw
);
705 hw
->fifo_mask
= fifo_mask
;
708 mutex_unlock(&hw
->conf_lock
);
713 static int st_lsm6dsx_buffer_preenable(struct iio_dev
*iio_dev
)
715 struct st_lsm6dsx_sensor
*sensor
= iio_priv(iio_dev
);
716 struct st_lsm6dsx_hw
*hw
= sensor
->hw
;
718 if (!hw
->settings
->fifo_ops
.update_fifo
)
721 return hw
->settings
->fifo_ops
.update_fifo(sensor
, true);
724 static int st_lsm6dsx_buffer_postdisable(struct iio_dev
*iio_dev
)
726 struct st_lsm6dsx_sensor
*sensor
= iio_priv(iio_dev
);
727 struct st_lsm6dsx_hw
*hw
= sensor
->hw
;
729 if (!hw
->settings
->fifo_ops
.update_fifo
)
732 return hw
->settings
->fifo_ops
.update_fifo(sensor
, false);
735 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops
= {
736 .preenable
= st_lsm6dsx_buffer_preenable
,
737 .postdisable
= st_lsm6dsx_buffer_postdisable
,
740 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw
*hw
)
742 struct iio_buffer
*buffer
;
745 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
746 if (!hw
->iio_devs
[i
])
749 buffer
= devm_iio_kfifo_allocate(hw
->dev
);
753 iio_device_attach_buffer(hw
->iio_devs
[i
], buffer
);
754 hw
->iio_devs
[i
]->modes
|= INDIO_BUFFER_SOFTWARE
;
755 hw
->iio_devs
[i
]->setup_ops
= &st_lsm6dsx_buffer_ops
;