1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * STMicroelectronics st_lsm6dsx FIFO buffer library driver
4 *
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.
16 *
17 * LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR/ISM330DHCX: The FIFO buffer can be
18 * configured to store data from gyroscope and accelerometer. Each sample
19 * is queued with a tag (1B) indicating data source (gyroscope, accelerometer,
20 * hw timer).
21 *
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
26 *
27 * Copyright 2016 STMicroelectronics Inc.
28 *
29 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
30 * Denis Ciocca <denis.ciocca@st.com>
31 */
32 #include <linux/module.h>
33 #include <linux/interrupt.h>
34 #include <linux/irq.h>
35 #include <linux/iio/kfifo_buf.h>
36 #include <linux/iio/iio.h>
37 #include <linux/iio/buffer.h>
38 #include <linux/regmap.h>
39 #include <linux/bitfield.h>
40
41 #include <linux/platform_data/st_sensors_pdata.h>
42
43 #include "st_lsm6dsx.h"
44
45 #define ST_LSM6DSX_REG_HLACTIVE_ADDR 0x12
46 #define ST_LSM6DSX_REG_HLACTIVE_MASK BIT(5)
47 #define ST_LSM6DSX_REG_PP_OD_ADDR 0x12
48 #define ST_LSM6DSX_REG_PP_OD_MASK BIT(4)
49 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
50 #define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
51 #define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
52 #define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
53 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
54 #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78
55 #define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
56
57 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
58
59 #define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */
60 #define ST_LSM6DSX_TS_RESET_VAL 0xaa
61
62 struct st_lsm6dsx_decimator_entry {
63 u8 decimator;
64 u8 val;
65 };
66
67 enum st_lsm6dsx_fifo_tag {
68 ST_LSM6DSX_GYRO_TAG = 0x01,
69 ST_LSM6DSX_ACC_TAG = 0x02,
70 ST_LSM6DSX_TS_TAG = 0x04,
71 ST_LSM6DSX_EXT0_TAG = 0x0f,
72 ST_LSM6DSX_EXT1_TAG = 0x10,
73 ST_LSM6DSX_EXT2_TAG = 0x11,
74 };
75
76 static const
77 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
78 { 0, 0x0 },
79 { 1, 0x1 },
80 { 2, 0x2 },
81 { 3, 0x3 },
82 { 4, 0x4 },
83 { 8, 0x5 },
84 { 16, 0x6 },
85 { 32, 0x7 },
86 };
87
st_lsm6dsx_get_decimator_val(u8 val)88 static int st_lsm6dsx_get_decimator_val(u8 val)
89 {
90 const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
91 int i;
92
93 for (i = 0; i < max_size; i++)
94 if (st_lsm6dsx_decimator_table[i].decimator == val)
95 break;
96
97 return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
98 }
99
st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw * hw,u16 * max_odr,u16 * min_odr)100 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
101 u16 *max_odr, u16 *min_odr)
102 {
103 struct st_lsm6dsx_sensor *sensor;
104 int i;
105
106 *max_odr = 0, *min_odr = ~0;
107 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
108 if (!hw->iio_devs[i])
109 continue;
110
111 sensor = iio_priv(hw->iio_devs[i]);
112
113 if (!(hw->enable_mask & BIT(sensor->id)))
114 continue;
115
116 *max_odr = max_t(u16, *max_odr, sensor->odr);
117 *min_odr = min_t(u16, *min_odr, sensor->odr);
118 }
119 }
120
st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw * hw)121 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
122 {
123 u16 max_odr, min_odr, sip = 0, ts_sip = 0;
124 const struct st_lsm6dsx_reg *ts_dec_reg;
125 struct st_lsm6dsx_sensor *sensor;
126 int err = 0, i;
127 u8 data;
128
129 st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
130
131 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
132 const struct st_lsm6dsx_reg *dec_reg;
133
134 if (!hw->iio_devs[i])
135 continue;
136
137 sensor = iio_priv(hw->iio_devs[i]);
138 /* update fifo decimators and sample in pattern */
139 if (hw->enable_mask & BIT(sensor->id)) {
140 sensor->sip = sensor->odr / min_odr;
141 sensor->decimator = max_odr / sensor->odr;
142 data = st_lsm6dsx_get_decimator_val(sensor->decimator);
143 } else {
144 sensor->sip = 0;
145 sensor->decimator = 0;
146 data = 0;
147 }
148 ts_sip = max_t(u16, ts_sip, sensor->sip);
149
150 dec_reg = &hw->settings->decimator[sensor->id];
151 if (dec_reg->addr) {
152 int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
153
154 err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
155 dec_reg->mask,
156 val);
157 if (err < 0)
158 return err;
159 }
160 sip += sensor->sip;
161 }
162 hw->sip = sip + ts_sip;
163 hw->ts_sip = ts_sip;
164
165 /*
166 * update hw ts decimator if necessary. Decimator for hw timestamp
167 * is always 1 or 0 in order to have a ts sample for each data
168 * sample in FIFO
169 */
170 ts_dec_reg = &hw->settings->ts_settings.decimator;
171 if (ts_dec_reg->addr) {
172 int val, ts_dec = !!hw->ts_sip;
173
174 val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
175 err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
176 ts_dec_reg->mask, val);
177 }
178 return err;
179 }
180
st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw * hw,enum st_lsm6dsx_fifo_mode fifo_mode)181 int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
182 enum st_lsm6dsx_fifo_mode fifo_mode)
183 {
184 unsigned int data;
185 int err;
186
187 data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
188 err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
189 ST_LSM6DSX_FIFO_MODE_MASK, data);
190 if (err < 0)
191 return err;
192
193 hw->fifo_mode = fifo_mode;
194
195 return 0;
196 }
197
st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor * sensor,bool enable)198 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
199 bool enable)
200 {
201 struct st_lsm6dsx_hw *hw = sensor->hw;
202 const struct st_lsm6dsx_reg *batch_reg;
203 u8 data;
204
205 batch_reg = &hw->settings->batch[sensor->id];
206 if (batch_reg->addr) {
207 int val;
208
209 if (enable) {
210 int err;
211
212 err = st_lsm6dsx_check_odr(sensor, sensor->odr,
213 &data);
214 if (err < 0)
215 return err;
216 } else {
217 data = 0;
218 }
219 val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
220 return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
221 batch_reg->mask, val);
222 } else {
223 data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
224 return st_lsm6dsx_update_bits_locked(hw,
225 ST_LSM6DSX_REG_FIFO_MODE_ADDR,
226 ST_LSM6DSX_FIFO_ODR_MASK,
227 FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
228 data));
229 }
230 }
231
st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor * sensor,u16 watermark)232 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
233 {
234 u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask;
235 struct st_lsm6dsx_hw *hw = sensor->hw;
236 struct st_lsm6dsx_sensor *cur_sensor;
237 int i, err, data;
238 __le16 wdata;
239
240 if (!hw->sip)
241 return 0;
242
243 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
244 if (!hw->iio_devs[i])
245 continue;
246
247 cur_sensor = iio_priv(hw->iio_devs[i]);
248
249 if (!(hw->enable_mask & BIT(cur_sensor->id)))
250 continue;
251
252 cur_watermark = (cur_sensor == sensor) ? watermark
253 : cur_sensor->watermark;
254
255 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
256 }
257
258 fifo_watermark = max_t(u16, fifo_watermark, hw->sip);
259 fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;
260 fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
261
262 mutex_lock(&hw->page_lock);
263 err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
264 &data);
265 if (err < 0)
266 goto out;
267
268 fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
269 fifo_watermark = ((data << 8) & ~fifo_th_mask) |
270 (fifo_watermark & fifo_th_mask);
271
272 wdata = cpu_to_le16(fifo_watermark);
273 err = regmap_bulk_write(hw->regmap,
274 hw->settings->fifo_ops.fifo_th.addr,
275 &wdata, sizeof(wdata));
276 out:
277 mutex_unlock(&hw->page_lock);
278 return err;
279 }
280
st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw * hw)281 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
282 {
283 struct st_lsm6dsx_sensor *sensor;
284 int i, err;
285
286 /* reset hw ts counter */
287 err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
288 ST_LSM6DSX_TS_RESET_VAL);
289 if (err < 0)
290 return err;
291
292 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
293 if (!hw->iio_devs[i])
294 continue;
295
296 sensor = iio_priv(hw->iio_devs[i]);
297 /*
298 * store enable buffer timestamp as reference for
299 * hw timestamp
300 */
301 sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
302 }
303 return 0;
304 }
305
306 /*
307 * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
308 * in order to avoid a kmalloc for each bus access
309 */
st_lsm6dsx_read_block(struct st_lsm6dsx_hw * hw,u8 addr,u8 * data,unsigned int data_len,unsigned int max_word_len)310 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
311 u8 *data, unsigned int data_len,
312 unsigned int max_word_len)
313 {
314 unsigned int word_len, read_len = 0;
315 int err;
316
317 while (read_len < data_len) {
318 word_len = min_t(unsigned int, data_len - read_len,
319 max_word_len);
320 err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
321 word_len);
322 if (err < 0)
323 return err;
324 read_len += word_len;
325 }
326 return 0;
327 }
328
329 #define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
330 sizeof(s64)) + sizeof(s64))
331 /**
332 * st_lsm6dsx_read_fifo() - hw FIFO read routine
333 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
334 *
335 * Read samples from the hw FIFO and push them to IIO buffers.
336 *
337 * Return: Number of bytes read from the FIFO
338 */
st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw * hw)339 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
340 {
341 u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
342 u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
343 int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
344 struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
345 u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
346 u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
347 bool reset_ts = false;
348 __le16 fifo_status;
349 s64 ts = 0;
350
351 err = st_lsm6dsx_read_locked(hw,
352 hw->settings->fifo_ops.fifo_diff.addr,
353 &fifo_status, sizeof(fifo_status));
354 if (err < 0) {
355 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
356 err);
357 return err;
358 }
359
360 if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
361 return 0;
362
363 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
364 ST_LSM6DSX_CHAN_SIZE;
365 fifo_len = (fifo_len / pattern_len) * pattern_len;
366
367 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
368 gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
369
370 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
371 err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
372 hw->buff, pattern_len,
373 ST_LSM6DSX_MAX_WORD_LEN);
374 if (err < 0) {
375 dev_err(hw->dev,
376 "failed to read pattern from fifo (err=%d)\n",
377 err);
378 return err;
379 }
380
381 /*
382 * Data are written to the FIFO with a specific pattern
383 * depending on the configured ODRs. The first sequence of data
384 * stored in FIFO contains the data of all enabled sensors
385 * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
386 * depending on the value of the decimation factor set for each
387 * sensor.
388 *
389 * Supposing the FIFO is storing data from gyroscope and
390 * accelerometer at different ODRs:
391 * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
392 * Since the gyroscope ODR is twice the accelerometer one, the
393 * following pattern is repeated every 9 samples:
394 * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
395 */
396 gyro_sip = gyro_sensor->sip;
397 acc_sip = acc_sensor->sip;
398 ts_sip = hw->ts_sip;
399 offset = 0;
400
401 while (acc_sip > 0 || gyro_sip > 0) {
402 if (gyro_sip > 0) {
403 memcpy(gyro_buff, &hw->buff[offset],
404 ST_LSM6DSX_SAMPLE_SIZE);
405 offset += ST_LSM6DSX_SAMPLE_SIZE;
406 }
407 if (acc_sip > 0) {
408 memcpy(acc_buff, &hw->buff[offset],
409 ST_LSM6DSX_SAMPLE_SIZE);
410 offset += ST_LSM6DSX_SAMPLE_SIZE;
411 }
412
413 if (ts_sip-- > 0) {
414 u8 data[ST_LSM6DSX_SAMPLE_SIZE];
415
416 memcpy(data, &hw->buff[offset], sizeof(data));
417 /*
418 * hw timestamp is 3B long and it is stored
419 * in FIFO using 6B as 4th FIFO data set
420 * according to this schema:
421 * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
422 */
423 ts = data[1] << 16 | data[0] << 8 | data[3];
424 /*
425 * check if hw timestamp engine is going to
426 * reset (the sensor generates an interrupt
427 * to signal the hw timestamp will reset in
428 * 1.638s)
429 */
430 if (!reset_ts && ts >= 0xff0000)
431 reset_ts = true;
432 ts *= ST_LSM6DSX_TS_SENSITIVITY;
433
434 offset += ST_LSM6DSX_SAMPLE_SIZE;
435 }
436
437 if (gyro_sip-- > 0)
438 iio_push_to_buffers_with_timestamp(
439 hw->iio_devs[ST_LSM6DSX_ID_GYRO],
440 gyro_buff, gyro_sensor->ts_ref + ts);
441 if (acc_sip-- > 0)
442 iio_push_to_buffers_with_timestamp(
443 hw->iio_devs[ST_LSM6DSX_ID_ACC],
444 acc_buff, acc_sensor->ts_ref + ts);
445 }
446 }
447
448 if (unlikely(reset_ts)) {
449 err = st_lsm6dsx_reset_hw_ts(hw);
450 if (err < 0) {
451 dev_err(hw->dev, "failed to reset hw ts (err=%d)\n",
452 err);
453 return err;
454 }
455 }
456 return read_len;
457 }
458
459 static int
st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw * hw,u8 tag,u8 * data,s64 ts)460 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
461 u8 *data, s64 ts)
462 {
463 struct st_lsm6dsx_sensor *sensor;
464 struct iio_dev *iio_dev;
465
466 /*
467 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
468 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
469 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
470 * channel
471 */
472 switch (tag) {
473 case ST_LSM6DSX_GYRO_TAG:
474 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
475 break;
476 case ST_LSM6DSX_ACC_TAG:
477 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
478 break;
479 case ST_LSM6DSX_EXT0_TAG:
480 if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
481 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
482 else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
483 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
484 else
485 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
486 break;
487 case ST_LSM6DSX_EXT1_TAG:
488 if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
489 (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
490 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
491 else
492 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
493 break;
494 case ST_LSM6DSX_EXT2_TAG:
495 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
496 break;
497 default:
498 return -EINVAL;
499 }
500
501 sensor = iio_priv(iio_dev);
502 iio_push_to_buffers_with_timestamp(iio_dev, data,
503 ts + sensor->ts_ref);
504
505 return 0;
506 }
507
508 /**
509 * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine
510 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
511 *
512 * Read samples from the hw FIFO and push them to IIO buffers.
513 *
514 * Return: Number of bytes read from the FIFO
515 */
st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw * hw)516 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
517 {
518 u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
519 u16 fifo_len, fifo_diff_mask;
520 u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE], tag;
521 bool reset_ts = false;
522 int i, err, read_len;
523 __le16 fifo_status;
524 s64 ts = 0;
525
526 err = st_lsm6dsx_read_locked(hw,
527 hw->settings->fifo_ops.fifo_diff.addr,
528 &fifo_status, sizeof(fifo_status));
529 if (err < 0) {
530 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
531 err);
532 return err;
533 }
534
535 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
536 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
537 ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
538 if (!fifo_len)
539 return 0;
540
541 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
542 err = st_lsm6dsx_read_block(hw,
543 ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
544 hw->buff, pattern_len,
545 ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
546 if (err < 0) {
547 dev_err(hw->dev,
548 "failed to read pattern from fifo (err=%d)\n",
549 err);
550 return err;
551 }
552
553 for (i = 0; i < pattern_len;
554 i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
555 memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
556 ST_LSM6DSX_SAMPLE_SIZE);
557
558 tag = hw->buff[i] >> 3;
559 if (tag == ST_LSM6DSX_TS_TAG) {
560 /*
561 * hw timestamp is 4B long and it is stored
562 * in FIFO according to this schema:
563 * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
564 * B3 = ts[31:24]
565 */
566 ts = le32_to_cpu(*((__le32 *)iio_buff));
567 /*
568 * check if hw timestamp engine is going to
569 * reset (the sensor generates an interrupt
570 * to signal the hw timestamp will reset in
571 * 1.638s)
572 */
573 if (!reset_ts && ts >= 0xffff0000)
574 reset_ts = true;
575 ts *= ST_LSM6DSX_TS_SENSITIVITY;
576 } else {
577 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
578 ts);
579 }
580 }
581 }
582
583 if (unlikely(reset_ts)) {
584 err = st_lsm6dsx_reset_hw_ts(hw);
585 if (err < 0)
586 return err;
587 }
588 return read_len;
589 }
590
st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw * hw)591 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
592 {
593 int err;
594
595 mutex_lock(&hw->fifo_lock);
596
597 hw->settings->fifo_ops.read_fifo(hw);
598 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
599
600 mutex_unlock(&hw->fifo_lock);
601
602 return err;
603 }
604
st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor * sensor,bool enable)605 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
606 {
607 struct st_lsm6dsx_hw *hw = sensor->hw;
608 int err;
609
610 mutex_lock(&hw->conf_lock);
611
612 if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
613 err = st_lsm6dsx_flush_fifo(hw);
614 if (err < 0)
615 goto out;
616 }
617
618 if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
619 sensor->id == ST_LSM6DSX_ID_EXT1 ||
620 sensor->id == ST_LSM6DSX_ID_EXT2) {
621 err = st_lsm6dsx_shub_set_enable(sensor, enable);
622 if (err < 0)
623 goto out;
624 } else {
625 err = st_lsm6dsx_sensor_set_enable(sensor, enable);
626 if (err < 0)
627 goto out;
628
629 err = st_lsm6dsx_set_fifo_odr(sensor, enable);
630 if (err < 0)
631 goto out;
632 }
633
634 err = st_lsm6dsx_update_decimators(hw);
635 if (err < 0)
636 goto out;
637
638 err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
639 if (err < 0)
640 goto out;
641
642 if (hw->enable_mask) {
643 /* reset hw ts counter */
644 err = st_lsm6dsx_reset_hw_ts(hw);
645 if (err < 0)
646 goto out;
647
648 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
649 }
650
651 out:
652 mutex_unlock(&hw->conf_lock);
653
654 return err;
655 }
656
st_lsm6dsx_handler_irq(int irq,void * private)657 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
658 {
659 struct st_lsm6dsx_hw *hw = private;
660
661 return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
662 }
663
st_lsm6dsx_handler_thread(int irq,void * private)664 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
665 {
666 struct st_lsm6dsx_hw *hw = private;
667 int count;
668
669 mutex_lock(&hw->fifo_lock);
670 count = hw->settings->fifo_ops.read_fifo(hw);
671 mutex_unlock(&hw->fifo_lock);
672
673 return count ? IRQ_HANDLED : IRQ_NONE;
674 }
675
st_lsm6dsx_buffer_preenable(struct iio_dev * iio_dev)676 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
677 {
678 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
679 struct st_lsm6dsx_hw *hw = sensor->hw;
680
681 if (!hw->settings->fifo_ops.update_fifo)
682 return -ENOTSUPP;
683
684 return hw->settings->fifo_ops.update_fifo(sensor, true);
685 }
686
st_lsm6dsx_buffer_postdisable(struct iio_dev * iio_dev)687 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
688 {
689 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
690 struct st_lsm6dsx_hw *hw = sensor->hw;
691
692 if (!hw->settings->fifo_ops.update_fifo)
693 return -ENOTSUPP;
694
695 return hw->settings->fifo_ops.update_fifo(sensor, false);
696 }
697
698 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
699 .preenable = st_lsm6dsx_buffer_preenable,
700 .postdisable = st_lsm6dsx_buffer_postdisable,
701 };
702
st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw * hw)703 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
704 {
705 struct device_node *np = hw->dev->of_node;
706 struct st_sensors_platform_data *pdata;
707 struct iio_buffer *buffer;
708 unsigned long irq_type;
709 bool irq_active_low;
710 int i, err;
711
712 irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
713
714 switch (irq_type) {
715 case IRQF_TRIGGER_HIGH:
716 case IRQF_TRIGGER_RISING:
717 irq_active_low = false;
718 break;
719 case IRQF_TRIGGER_LOW:
720 case IRQF_TRIGGER_FALLING:
721 irq_active_low = true;
722 break;
723 default:
724 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
725 return -EINVAL;
726 }
727
728 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
729 ST_LSM6DSX_REG_HLACTIVE_MASK,
730 FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
731 irq_active_low));
732 if (err < 0)
733 return err;
734
735 pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
736 if ((np && of_property_read_bool(np, "drive-open-drain")) ||
737 (pdata && pdata->open_drain)) {
738 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
739 ST_LSM6DSX_REG_PP_OD_MASK,
740 FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
741 1));
742 if (err < 0)
743 return err;
744
745 irq_type |= IRQF_SHARED;
746 }
747
748 err = devm_request_threaded_irq(hw->dev, hw->irq,
749 st_lsm6dsx_handler_irq,
750 st_lsm6dsx_handler_thread,
751 irq_type | IRQF_ONESHOT,
752 "lsm6dsx", hw);
753 if (err) {
754 dev_err(hw->dev, "failed to request trigger irq %d\n",
755 hw->irq);
756 return err;
757 }
758
759 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
760 if (!hw->iio_devs[i])
761 continue;
762
763 buffer = devm_iio_kfifo_allocate(hw->dev);
764 if (!buffer)
765 return -ENOMEM;
766
767 iio_device_attach_buffer(hw->iio_devs[i], buffer);
768 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
769 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
770 }
771
772 return 0;
773 }
774
