2 * STMicroelectronics st_lsm6dsx FIFO buffer library driver
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
19 * Copyright 2016 STMicroelectronics Inc.
21 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
22 * Denis Ciocca <denis.ciocca@st.com>
24 * Licensed under the GPL-2.
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>
33 #include "st_lsm6dsx.h"
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
47 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
49 struct st_lsm6dsx_decimator_entry
{
55 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table
[] = {
66 static int st_lsm6dsx_get_decimator_val(u8 val
)
68 const int max_size
= ARRAY_SIZE(st_lsm6dsx_decimator_table
);
71 for (i
= 0; i
< max_size
; i
++)
72 if (st_lsm6dsx_decimator_table
[i
].decimator
== val
)
75 return i
== max_size
? 0 : st_lsm6dsx_decimator_table
[i
].val
;
78 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw
*hw
,
79 u16
*max_odr
, u16
*min_odr
)
81 struct st_lsm6dsx_sensor
*sensor
;
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
]);
88 if (!(hw
->enable_mask
& BIT(sensor
->id
)))
91 *max_odr
= max_t(u16
, *max_odr
, sensor
->odr
);
92 *min_odr
= min_t(u16
, *min_odr
, sensor
->odr
);
96 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw
*hw
)
98 struct st_lsm6dsx_sensor
*sensor
;
99 u16 max_odr
, min_odr
, sip
= 0;
103 st_lsm6dsx_get_max_min_odr(hw
, &max_odr
, &min_odr
);
105 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
106 sensor
= iio_priv(hw
->iio_devs
[i
]);
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
);
115 sensor
->decimator
= 0;
119 err
= st_lsm6dsx_write_with_mask(hw
,
120 ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR
,
121 sensor
->decimator_mask
, data
);
132 static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw
*hw
,
133 enum st_lsm6dsx_fifo_mode fifo_mode
)
139 case ST_LSM6DSX_FIFO_BYPASS
:
142 case ST_LSM6DSX_FIFO_CONT
:
143 data
= (ST_LSM6DSX_MAX_FIFO_ODR_VAL
<<
144 __ffs(ST_LSM6DSX_FIFO_ODR_MASK
)) | fifo_mode
;
150 err
= hw
->tf
->write(hw
->dev
, ST_LSM6DSX_REG_FIFO_MODE_ADDR
,
151 sizeof(data
), &data
);
155 hw
->fifo_mode
= fifo_mode
;
160 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor
*sensor
, u16 watermark
)
162 u16 fifo_watermark
= ~0, cur_watermark
, sip
= 0;
163 struct st_lsm6dsx_hw
*hw
= sensor
->hw
;
164 struct st_lsm6dsx_sensor
*cur_sensor
;
169 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
170 cur_sensor
= iio_priv(hw
->iio_devs
[i
]);
172 if (!(hw
->enable_mask
& BIT(cur_sensor
->id
)))
175 cur_watermark
= (cur_sensor
== sensor
) ? watermark
176 : cur_sensor
->watermark
;
178 fifo_watermark
= min_t(u16
, fifo_watermark
, cur_watermark
);
179 sip
+= cur_sensor
->sip
;
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
;
189 mutex_lock(&hw
->lock
);
191 err
= hw
->tf
->read(hw
->dev
, ST_LSM6DSX_REG_FIFO_THH_ADDR
,
192 sizeof(data
), &data
);
196 fifo_watermark
= ((data
<< 8) & ~ST_LSM6DSX_FIFO_TH_MASK
) |
197 (fifo_watermark
& ST_LSM6DSX_FIFO_TH_MASK
);
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
);
203 mutex_unlock(&hw
->lock
);
205 return err
< 0 ? err
: 0;
209 * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
210 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
212 * Read samples from the hw FIFO and push them to IIO buffers.
214 * Return: Number of bytes read from the FIFO
216 static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw
*hw
)
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
];
226 err
= hw
->tf
->read(hw
->dev
, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR
,
227 sizeof(fifo_status
), (u8
*)&fifo_status
);
231 if (fifo_status
& cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK
))
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
;
240 * compute delta timestamp between two consecutive samples
241 * in order to estimate queueing time of data generated
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
,
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
,
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
,
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
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
275 gyro_sip
= gyro_sensor
->sip
;
276 acc_sip
= acc_sensor
->sip
;
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
],
286 offset
+= ST_LSM6DSX_SAMPLE_SIZE
;
287 gyro_ts
+= gyro_delta_ts
;
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
],
296 offset
+= ST_LSM6DSX_SAMPLE_SIZE
;
297 acc_ts
+= acc_delta_ts
;
305 static int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw
*hw
)
309 mutex_lock(&hw
->fifo_lock
);
311 st_lsm6dsx_read_fifo(hw
);
312 err
= st_lsm6dsx_set_fifo_mode(hw
, ST_LSM6DSX_FIFO_BYPASS
);
314 mutex_unlock(&hw
->fifo_lock
);
319 static int st_lsm6dsx_update_fifo(struct iio_dev
*iio_dev
, bool enable
)
321 struct st_lsm6dsx_sensor
*sensor
= iio_priv(iio_dev
);
322 struct st_lsm6dsx_hw
*hw
= sensor
->hw
;
325 if (hw
->fifo_mode
!= ST_LSM6DSX_FIFO_BYPASS
) {
326 err
= st_lsm6dsx_flush_fifo(hw
);
332 err
= st_lsm6dsx_sensor_enable(sensor
);
336 err
= st_lsm6dsx_sensor_disable(sensor
);
341 err
= st_lsm6dsx_update_decimators(hw
);
345 err
= st_lsm6dsx_update_watermark(sensor
, sensor
->watermark
);
349 if (hw
->enable_mask
) {
350 err
= st_lsm6dsx_set_fifo_mode(hw
, ST_LSM6DSX_FIFO_CONT
);
355 * store enable buffer timestamp as reference to compute
356 * first delta timestamp
358 sensor
->ts
= iio_get_time_ns(iio_dev
);
364 static irqreturn_t
st_lsm6dsx_handler_irq(int irq
, void *private)
366 struct st_lsm6dsx_hw
*hw
= (struct st_lsm6dsx_hw
*)private;
367 struct st_lsm6dsx_sensor
*sensor
;
373 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
374 sensor
= iio_priv(hw
->iio_devs
[i
]);
376 if (sensor
->sip
> 0) {
379 timestamp
= iio_get_time_ns(hw
->iio_devs
[i
]);
380 sensor
->delta_ts
= timestamp
- sensor
->ts
;
381 sensor
->ts
= timestamp
;
385 return IRQ_WAKE_THREAD
;
388 static irqreturn_t
st_lsm6dsx_handler_thread(int irq
, void *private)
390 struct st_lsm6dsx_hw
*hw
= (struct st_lsm6dsx_hw
*)private;
393 mutex_lock(&hw
->fifo_lock
);
394 count
= st_lsm6dsx_read_fifo(hw
);
395 mutex_unlock(&hw
->fifo_lock
);
397 return !count
? IRQ_NONE
: IRQ_HANDLED
;
400 static int st_lsm6dsx_buffer_preenable(struct iio_dev
*iio_dev
)
402 return st_lsm6dsx_update_fifo(iio_dev
, true);
405 static int st_lsm6dsx_buffer_postdisable(struct iio_dev
*iio_dev
)
407 return st_lsm6dsx_update_fifo(iio_dev
, false);
410 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops
= {
411 .preenable
= st_lsm6dsx_buffer_preenable
,
412 .postdisable
= st_lsm6dsx_buffer_postdisable
,
415 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw
*hw
)
417 struct iio_buffer
*buffer
;
418 unsigned long irq_type
;
421 irq_type
= irqd_get_trigger_type(irq_get_irq_data(hw
->irq
));
424 case IRQF_TRIGGER_HIGH
:
425 case IRQF_TRIGGER_RISING
:
428 dev_info(hw
->dev
, "mode %lx unsupported\n", irq_type
);
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
,
438 dev_err(hw
->dev
, "failed to request trigger irq %d\n",
443 for (i
= 0; i
< ST_LSM6DSX_ID_MAX
; i
++) {
444 buffer
= devm_iio_kfifo_allocate(hw
->dev
);
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
;