1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31#include <linux/module.h>
32#include <linux/interrupt.h>
33#include <linux/irq.h>
34#include <linux/iio/kfifo_buf.h>
35#include <linux/iio/iio.h>
36#include <linux/iio/buffer.h>
37#include <linux/regmap.h>
38#include <linux/bitfield.h>
39
40#include <linux/platform_data/st_sensors_pdata.h>
41
42#include "st_lsm6dsx.h"
43
44#define ST_LSM6DSX_REG_HLACTIVE_ADDR 0x12
45#define ST_LSM6DSX_REG_HLACTIVE_MASK BIT(5)
46#define ST_LSM6DSX_REG_PP_OD_ADDR 0x12
47#define ST_LSM6DSX_REG_PP_OD_MASK BIT(4)
48#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
49#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
50#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
51#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
52#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
53#define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78
54#define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
55
56#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
57
58#define ST_LSM6DSX_TS_SENSITIVITY 25000UL
59#define ST_LSM6DSX_TS_RESET_VAL 0xaa
60
61struct st_lsm6dsx_decimator_entry {
62 u8 decimator;
63 u8 val;
64};
65
66enum st_lsm6dsx_fifo_tag {
67 ST_LSM6DSX_GYRO_TAG = 0x01,
68 ST_LSM6DSX_ACC_TAG = 0x02,
69 ST_LSM6DSX_TS_TAG = 0x04,
70 ST_LSM6DSX_EXT0_TAG = 0x0f,
71 ST_LSM6DSX_EXT1_TAG = 0x10,
72 ST_LSM6DSX_EXT2_TAG = 0x11,
73};
74
75static const
76struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
77 { 0, 0x0 },
78 { 1, 0x1 },
79 { 2, 0x2 },
80 { 3, 0x3 },
81 { 4, 0x4 },
82 { 8, 0x5 },
83 { 16, 0x6 },
84 { 32, 0x7 },
85};
86
87static int st_lsm6dsx_get_decimator_val(u8 val)
88{
89 const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
90 int i;
91
92 for (i = 0; i < max_size; i++)
93 if (st_lsm6dsx_decimator_table[i].decimator == val)
94 break;
95
96 return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
97}
98
99static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
100 u16 *max_odr, u16 *min_odr)
101{
102 struct st_lsm6dsx_sensor *sensor;
103 int i;
104
105 *max_odr = 0, *min_odr = ~0;
106 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
107 if (!hw->iio_devs[i])
108 continue;
109
110 sensor = iio_priv(hw->iio_devs[i]);
111
112 if (!(hw->enable_mask & BIT(sensor->id)))
113 continue;
114
115 *max_odr = max_t(u16, *max_odr, sensor->odr);
116 *min_odr = min_t(u16, *min_odr, sensor->odr);
117 }
118}
119
120static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
121{
122 u16 max_odr, min_odr, sip = 0, ts_sip = 0;
123 const struct st_lsm6dsx_reg *ts_dec_reg;
124 struct st_lsm6dsx_sensor *sensor;
125 int err = 0, i;
126 u8 data;
127
128 st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
129
130 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
131 const struct st_lsm6dsx_reg *dec_reg;
132
133 if (!hw->iio_devs[i])
134 continue;
135
136 sensor = iio_priv(hw->iio_devs[i]);
137
138 if (hw->enable_mask & BIT(sensor->id)) {
139 sensor->sip = sensor->odr / min_odr;
140 sensor->decimator = max_odr / sensor->odr;
141 data = st_lsm6dsx_get_decimator_val(sensor->decimator);
142 } else {
143 sensor->sip = 0;
144 sensor->decimator = 0;
145 data = 0;
146 }
147 ts_sip = max_t(u16, ts_sip, sensor->sip);
148
149 dec_reg = &hw->settings->decimator[sensor->id];
150 if (dec_reg->addr) {
151 int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
152
153 err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
154 dec_reg->mask,
155 val);
156 if (err < 0)
157 return err;
158 }
159 sip += sensor->sip;
160 }
161 hw->sip = sip + ts_sip;
162 hw->ts_sip = ts_sip;
163
164
165
166
167
168
169 ts_dec_reg = &hw->settings->ts_settings.decimator;
170 if (ts_dec_reg->addr) {
171 int val, ts_dec = !!hw->ts_sip;
172
173 val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
174 err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
175 ts_dec_reg->mask, val);
176 }
177 return err;
178}
179
180int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
181 enum st_lsm6dsx_fifo_mode fifo_mode)
182{
183 unsigned int data;
184 int err;
185
186 data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
187 err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
188 ST_LSM6DSX_FIFO_MODE_MASK, data);
189 if (err < 0)
190 return err;
191
192 hw->fifo_mode = fifo_mode;
193
194 return 0;
195}
196
197static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
198 bool enable)
199{
200 struct st_lsm6dsx_hw *hw = sensor->hw;
201 const struct st_lsm6dsx_reg *batch_reg;
202 u8 data;
203
204 batch_reg = &hw->settings->batch[sensor->id];
205 if (batch_reg->addr) {
206 int val;
207
208 if (enable) {
209 int err;
210
211 err = st_lsm6dsx_check_odr(sensor, sensor->odr,
212 &data);
213 if (err < 0)
214 return err;
215 } else {
216 data = 0;
217 }
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);
221 } else {
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,
227 data));
228 }
229}
230
231int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
232{
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;
236 int i, err, data;
237 __le16 wdata;
238
239 if (!hw->sip)
240 return 0;
241
242 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
243 if (!hw->iio_devs[i])
244 continue;
245
246 cur_sensor = iio_priv(hw->iio_devs[i]);
247
248 if (!(hw->enable_mask & BIT(cur_sensor->id)))
249 continue;
250
251 cur_watermark = (cur_sensor == sensor) ? watermark
252 : cur_sensor->watermark;
253
254 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
255 }
256
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;
260
261 mutex_lock(&hw->page_lock);
262 err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
263 &data);
264 if (err < 0)
265 goto out;
266
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);
270
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));
275out:
276 mutex_unlock(&hw->page_lock);
277 return err;
278}
279
280static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
281{
282 struct st_lsm6dsx_sensor *sensor;
283 int i, err;
284
285
286 err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
287 ST_LSM6DSX_TS_RESET_VAL);
288 if (err < 0)
289 return err;
290
291 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
292 if (!hw->iio_devs[i])
293 continue;
294
295 sensor = iio_priv(hw->iio_devs[i]);
296
297
298
299
300 sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
301 }
302 return 0;
303}
304
305
306
307
308
309static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
310 u8 *data, unsigned int data_len,
311 unsigned int max_word_len)
312{
313 unsigned int word_len, read_len = 0;
314 int err;
315
316 while (read_len < data_len) {
317 word_len = min_t(unsigned int, data_len - read_len,
318 max_word_len);
319 err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
320 word_len);
321 if (err < 0)
322 return err;
323 read_len += word_len;
324 }
325 return 0;
326}
327
328#define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
329 sizeof(s64)) + sizeof(s64))
330
331
332
333
334
335
336
337
338int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
339{
340 u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
341 u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
342 int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
343 struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
344 u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
345 u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
346 bool reset_ts = false;
347 __le16 fifo_status;
348 s64 ts = 0;
349
350 err = st_lsm6dsx_read_locked(hw,
351 hw->settings->fifo_ops.fifo_diff.addr,
352 &fifo_status, sizeof(fifo_status));
353 if (err < 0) {
354 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
355 err);
356 return err;
357 }
358
359 if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
360 return 0;
361
362 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
363 ST_LSM6DSX_CHAN_SIZE;
364 fifo_len = (fifo_len / pattern_len) * pattern_len;
365
366 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
367 gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
368
369 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
370 err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
371 hw->buff, pattern_len,
372 ST_LSM6DSX_MAX_WORD_LEN);
373 if (err < 0) {
374 dev_err(hw->dev,
375 "failed to read pattern from fifo (err=%d)\n",
376 err);
377 return err;
378 }
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395 gyro_sip = gyro_sensor->sip;
396 acc_sip = acc_sensor->sip;
397 ts_sip = hw->ts_sip;
398 offset = 0;
399
400 while (acc_sip > 0 || gyro_sip > 0) {
401 if (gyro_sip > 0) {
402 memcpy(gyro_buff, &hw->buff[offset],
403 ST_LSM6DSX_SAMPLE_SIZE);
404 offset += ST_LSM6DSX_SAMPLE_SIZE;
405 }
406 if (acc_sip > 0) {
407 memcpy(acc_buff, &hw->buff[offset],
408 ST_LSM6DSX_SAMPLE_SIZE);
409 offset += ST_LSM6DSX_SAMPLE_SIZE;
410 }
411
412 if (ts_sip-- > 0) {
413 u8 data[ST_LSM6DSX_SAMPLE_SIZE];
414
415 memcpy(data, &hw->buff[offset], sizeof(data));
416
417
418
419
420
421
422 ts = data[1] << 16 | data[0] << 8 | data[3];
423
424
425
426
427
428
429 if (!reset_ts && ts >= 0xff0000)
430 reset_ts = true;
431 ts *= ST_LSM6DSX_TS_SENSITIVITY;
432
433 offset += ST_LSM6DSX_SAMPLE_SIZE;
434 }
435
436 if (gyro_sip-- > 0)
437 iio_push_to_buffers_with_timestamp(
438 hw->iio_devs[ST_LSM6DSX_ID_GYRO],
439 gyro_buff, gyro_sensor->ts_ref + ts);
440 if (acc_sip-- > 0)
441 iio_push_to_buffers_with_timestamp(
442 hw->iio_devs[ST_LSM6DSX_ID_ACC],
443 acc_buff, acc_sensor->ts_ref + ts);
444 }
445 }
446
447 if (unlikely(reset_ts)) {
448 err = st_lsm6dsx_reset_hw_ts(hw);
449 if (err < 0) {
450 dev_err(hw->dev, "failed to reset hw ts (err=%d)\n",
451 err);
452 return err;
453 }
454 }
455 return read_len;
456}
457
458static int
459st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
460 u8 *data, s64 ts)
461{
462 struct st_lsm6dsx_sensor *sensor;
463 struct iio_dev *iio_dev;
464
465
466
467
468
469
470
471 switch (tag) {
472 case ST_LSM6DSX_GYRO_TAG:
473 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
474 break;
475 case ST_LSM6DSX_ACC_TAG:
476 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
477 break;
478 case ST_LSM6DSX_EXT0_TAG:
479 if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
480 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
481 else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
482 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
483 else
484 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
485 break;
486 case ST_LSM6DSX_EXT1_TAG:
487 if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
488 (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
489 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
490 else
491 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
492 break;
493 case ST_LSM6DSX_EXT2_TAG:
494 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
495 break;
496 default:
497 return -EINVAL;
498 }
499
500 sensor = iio_priv(iio_dev);
501 iio_push_to_buffers_with_timestamp(iio_dev, data,
502 ts + sensor->ts_ref);
503
504 return 0;
505}
506
507
508
509
510
511
512
513
514
515int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
516{
517 u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
518 u16 fifo_len, fifo_diff_mask;
519 u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE], tag;
520 bool reset_ts = false;
521 int i, err, read_len;
522 __le16 fifo_status;
523 s64 ts = 0;
524
525 err = st_lsm6dsx_read_locked(hw,
526 hw->settings->fifo_ops.fifo_diff.addr,
527 &fifo_status, sizeof(fifo_status));
528 if (err < 0) {
529 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
530 err);
531 return err;
532 }
533
534 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
535 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
536 ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
537 if (!fifo_len)
538 return 0;
539
540 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
541 err = st_lsm6dsx_read_block(hw,
542 ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
543 hw->buff, pattern_len,
544 ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
545 if (err < 0) {
546 dev_err(hw->dev,
547 "failed to read pattern from fifo (err=%d)\n",
548 err);
549 return err;
550 }
551
552 for (i = 0; i < pattern_len;
553 i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
554 memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
555 ST_LSM6DSX_SAMPLE_SIZE);
556
557 tag = hw->buff[i] >> 3;
558 if (tag == ST_LSM6DSX_TS_TAG) {
559
560
561
562
563
564
565 ts = le32_to_cpu(*((__le32 *)iio_buff));
566
567
568
569
570
571
572 if (!reset_ts && ts >= 0xffff0000)
573 reset_ts = true;
574 ts *= ST_LSM6DSX_TS_SENSITIVITY;
575 } else {
576 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
577 ts);
578 }
579 }
580 }
581
582 if (unlikely(reset_ts)) {
583 err = st_lsm6dsx_reset_hw_ts(hw);
584 if (err < 0)
585 return err;
586 }
587 return read_len;
588}
589
590int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
591{
592 int err;
593
594 mutex_lock(&hw->fifo_lock);
595
596 hw->settings->fifo_ops.read_fifo(hw);
597 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
598
599 mutex_unlock(&hw->fifo_lock);
600
601 return err;
602}
603
604static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
605{
606 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
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
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
651out:
652 mutex_unlock(&hw->conf_lock);
653
654 return err;
655}
656
657static 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
664static 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_NONE : IRQ_HANDLED;
674}
675
676static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
677{
678 return st_lsm6dsx_update_fifo(iio_dev, true);
679}
680
681static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
682{
683 return st_lsm6dsx_update_fifo(iio_dev, false);
684}
685
686static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
687 .preenable = st_lsm6dsx_buffer_preenable,
688 .postdisable = st_lsm6dsx_buffer_postdisable,
689};
690
691int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
692{
693 struct device_node *np = hw->dev->of_node;
694 struct st_sensors_platform_data *pdata;
695 struct iio_buffer *buffer;
696 unsigned long irq_type;
697 bool irq_active_low;
698 int i, err;
699
700 irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
701
702 switch (irq_type) {
703 case IRQF_TRIGGER_HIGH:
704 case IRQF_TRIGGER_RISING:
705 irq_active_low = false;
706 break;
707 case IRQF_TRIGGER_LOW:
708 case IRQF_TRIGGER_FALLING:
709 irq_active_low = true;
710 break;
711 default:
712 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
713 return -EINVAL;
714 }
715
716 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
717 ST_LSM6DSX_REG_HLACTIVE_MASK,
718 FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
719 irq_active_low));
720 if (err < 0)
721 return err;
722
723 pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
724 if ((np && of_property_read_bool(np, "drive-open-drain")) ||
725 (pdata && pdata->open_drain)) {
726 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
727 ST_LSM6DSX_REG_PP_OD_MASK,
728 FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
729 1));
730 if (err < 0)
731 return err;
732
733 irq_type |= IRQF_SHARED;
734 }
735
736 err = devm_request_threaded_irq(hw->dev, hw->irq,
737 st_lsm6dsx_handler_irq,
738 st_lsm6dsx_handler_thread,
739 irq_type | IRQF_ONESHOT,
740 "lsm6dsx", hw);
741 if (err) {
742 dev_err(hw->dev, "failed to request trigger irq %d\n",
743 hw->irq);
744 return err;
745 }
746
747 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
748 if (!hw->iio_devs[i])
749 continue;
750
751 buffer = devm_iio_kfifo_allocate(hw->dev);
752 if (!buffer)
753 return -ENOMEM;
754
755 iio_device_attach_buffer(hw->iio_devs[i], buffer);
756 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
757 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
758 }
759
760 return 0;
761}
762