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#include <linux/module.h>
26#include <linux/regmap.h>
27#include <linux/iio/iio.h>
28#include <linux/iio/sysfs.h>
29#include <linux/bitfield.h>
30
31#include "st_lsm6dsx.h"
32
33#define ST_LSM6DSX_MAX_SLV_NUM 3
34#define ST_LSM6DSX_SLV_ADDR(n, base) ((base) + (n) * 3)
35#define ST_LSM6DSX_SLV_SUB_ADDR(n, base) ((base) + 1 + (n) * 3)
36#define ST_LSM6DSX_SLV_CONFIG(n, base) ((base) + 2 + (n) * 3)
37
38#define ST_LS6DSX_READ_OP_MASK GENMASK(2, 0)
39
40static const struct st_lsm6dsx_ext_dev_settings st_lsm6dsx_ext_dev_table[] = {
41
42 {
43 .i2c_addr = { 0x1e },
44 .wai = {
45 .addr = 0x4f,
46 .val = 0x40,
47 },
48 .id = ST_LSM6DSX_ID_MAGN,
49 .odr_table = {
50 .reg = {
51 .addr = 0x60,
52 .mask = GENMASK(3, 2),
53 },
54 .odr_avl[0] = { 10, 0x0 },
55 .odr_avl[1] = { 20, 0x1 },
56 .odr_avl[2] = { 50, 0x2 },
57 .odr_avl[3] = { 100, 0x3 },
58 },
59 .fs_table = {
60 .fs_avl[0] = {
61 .gain = 1500,
62 .val = 0x0,
63 },
64 },
65 .temp_comp = {
66 .addr = 0x60,
67 .mask = BIT(7),
68 },
69 .pwr_table = {
70 .reg = {
71 .addr = 0x60,
72 .mask = GENMASK(1, 0),
73 },
74 .off_val = 0x2,
75 .on_val = 0x0,
76 },
77 .off_canc = {
78 .addr = 0x61,
79 .mask = BIT(1),
80 },
81 .bdu = {
82 .addr = 0x62,
83 .mask = BIT(4),
84 },
85 .out = {
86 .addr = 0x68,
87 .len = 6,
88 },
89 },
90};
91
92static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
93{
94 struct st_lsm6dsx_sensor *sensor;
95
96 sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
97 msleep((2000U / sensor->odr) + 1);
98}
99
100
101
102
103
104
105static int st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 addr,
106 u8 *data, int len)
107{
108 int err;
109
110 mutex_lock(&hw->page_lock);
111
112 err = st_lsm6dsx_set_page(hw, true);
113 if (err < 0)
114 goto out;
115
116 err = regmap_bulk_read(hw->regmap, addr, data, len);
117
118 st_lsm6dsx_set_page(hw, false);
119out:
120 mutex_unlock(&hw->page_lock);
121
122 return err;
123}
124
125
126
127
128
129
130static int st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw *hw, u8 addr,
131 u8 *data, int len)
132{
133 int err;
134
135 mutex_lock(&hw->page_lock);
136 err = st_lsm6dsx_set_page(hw, true);
137 if (err < 0)
138 goto out;
139
140 err = regmap_bulk_write(hw->regmap, addr, data, len);
141
142 st_lsm6dsx_set_page(hw, false);
143out:
144 mutex_unlock(&hw->page_lock);
145
146 return err;
147}
148
149static int
150st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr,
151 u8 mask, u8 val)
152{
153 int err;
154
155 mutex_lock(&hw->page_lock);
156 err = st_lsm6dsx_set_page(hw, true);
157 if (err < 0)
158 goto out;
159
160 err = regmap_update_bits(hw->regmap, addr, mask, val);
161
162 st_lsm6dsx_set_page(hw, false);
163out:
164 mutex_unlock(&hw->page_lock);
165
166 return err;
167}
168
169static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,
170 bool enable)
171{
172 const struct st_lsm6dsx_shub_settings *hub_settings;
173 struct st_lsm6dsx_hw *hw = sensor->hw;
174 unsigned int data;
175 int err;
176
177
178 err = st_lsm6dsx_sensor_set_enable(sensor, enable);
179 if (err < 0)
180 return err;
181
182 mutex_lock(&hw->page_lock);
183
184 hub_settings = &hw->settings->shub_settings;
185 err = st_lsm6dsx_set_page(hw, true);
186 if (err < 0)
187 goto out;
188
189 data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask);
190 err = regmap_update_bits(hw->regmap, hub_settings->master_en.addr,
191 hub_settings->master_en.mask, data);
192
193 st_lsm6dsx_set_page(hw, false);
194out:
195 mutex_unlock(&hw->page_lock);
196
197 return err;
198}
199
200
201
202
203
204
205
206static int
207st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
208 u8 *data, int len)
209{
210 const struct st_lsm6dsx_shub_settings *hub_settings;
211 struct st_lsm6dsx_hw *hw = sensor->hw;
212 u8 config[3], slv_addr;
213 int err;
214
215 hub_settings = &hw->settings->shub_settings;
216 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
217
218 config[0] = (sensor->ext_info.addr << 1) | 1;
219 config[1] = addr;
220 config[2] = len & ST_LS6DSX_READ_OP_MASK;
221
222 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
223 sizeof(config));
224 if (err < 0)
225 return err;
226
227 err = st_lsm6dsx_shub_master_enable(sensor, true);
228 if (err < 0)
229 return err;
230
231 st_lsm6dsx_shub_wait_complete(hw);
232
233 err = st_lsm6dsx_shub_read_reg(hw, hub_settings->shub_out, data,
234 len & ST_LS6DSX_READ_OP_MASK);
235
236 st_lsm6dsx_shub_master_enable(sensor, false);
237
238 memset(config, 0, sizeof(config));
239 return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
240 sizeof(config));
241}
242
243
244
245
246
247
248
249static int
250st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
251 u8 *data, int len)
252{
253 const struct st_lsm6dsx_shub_settings *hub_settings;
254 struct st_lsm6dsx_hw *hw = sensor->hw;
255 u8 config[2], slv_addr;
256 int err, i;
257
258 hub_settings = &hw->settings->shub_settings;
259 if (hub_settings->wr_once.addr) {
260 unsigned int data;
261
262 data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask);
263 err = st_lsm6dsx_shub_write_reg_with_mask(hw,
264 hub_settings->wr_once.addr,
265 hub_settings->wr_once.mask,
266 data);
267 if (err < 0)
268 return err;
269 }
270
271 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
272 config[0] = sensor->ext_info.addr << 1;
273 for (i = 0 ; i < len; i++) {
274 config[1] = addr + i;
275
276 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
277 sizeof(config));
278 if (err < 0)
279 return err;
280
281 err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr,
282 &data[i], 1);
283 if (err < 0)
284 return err;
285
286 err = st_lsm6dsx_shub_master_enable(sensor, true);
287 if (err < 0)
288 return err;
289
290 st_lsm6dsx_shub_wait_complete(hw);
291
292 st_lsm6dsx_shub_master_enable(sensor, false);
293 }
294
295 memset(config, 0, sizeof(config));
296 return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
297}
298
299static int
300st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor *sensor,
301 u8 addr, u8 mask, u8 val)
302{
303 int err;
304 u8 data;
305
306 err = st_lsm6dsx_shub_read(sensor, addr, &data, sizeof(data));
307 if (err < 0)
308 return err;
309
310 data = ((data & ~mask) | (val << __ffs(mask) & mask));
311
312 return st_lsm6dsx_shub_write(sensor, addr, &data, sizeof(data));
313}
314
315static int
316st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor *sensor,
317 u16 odr, u16 *val)
318{
319 const struct st_lsm6dsx_ext_dev_settings *settings;
320 int i;
321
322 settings = sensor->ext_info.settings;
323 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
324 if (settings->odr_table.odr_avl[i].hz == odr)
325 break;
326
327 if (i == ST_LSM6DSX_ODR_LIST_SIZE)
328 return -EINVAL;
329
330 *val = settings->odr_table.odr_avl[i].val;
331 return 0;
332}
333
334static int
335st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
336{
337 const struct st_lsm6dsx_ext_dev_settings *settings;
338 u16 val;
339 int err;
340
341 err = st_lsm6dsx_shub_get_odr_val(sensor, odr, &val);
342 if (err < 0)
343 return err;
344
345 settings = sensor->ext_info.settings;
346 return st_lsm6dsx_shub_write_with_mask(sensor,
347 settings->odr_table.reg.addr,
348 settings->odr_table.reg.mask,
349 val);
350}
351
352
353static int
354st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor,
355 bool enable)
356{
357 const struct st_lsm6dsx_shub_settings *hub_settings;
358 const struct st_lsm6dsx_ext_dev_settings *settings;
359 u8 config[9] = {}, enable_mask, slv_addr;
360 struct st_lsm6dsx_hw *hw = sensor->hw;
361 struct st_lsm6dsx_sensor *cur_sensor;
362 int i, j = 0;
363
364 hub_settings = &hw->settings->shub_settings;
365 if (enable)
366 enable_mask = hw->enable_mask | BIT(sensor->id);
367 else
368 enable_mask = hw->enable_mask & ~BIT(sensor->id);
369
370 for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) {
371 if (!hw->iio_devs[i])
372 continue;
373
374 cur_sensor = iio_priv(hw->iio_devs[i]);
375 if (!(enable_mask & BIT(cur_sensor->id)))
376 continue;
377
378 settings = cur_sensor->ext_info.settings;
379 config[j] = (sensor->ext_info.addr << 1) | 1;
380 config[j + 1] = settings->out.addr;
381 config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) |
382 hub_settings->batch_en;
383 j += 3;
384 }
385
386 slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr);
387 return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
388 sizeof(config));
389}
390
391int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
392{
393 const struct st_lsm6dsx_ext_dev_settings *settings;
394 int err;
395
396 err = st_lsm6dsx_shub_config_channels(sensor, enable);
397 if (err < 0)
398 return err;
399
400 settings = sensor->ext_info.settings;
401 if (enable) {
402 err = st_lsm6dsx_shub_set_odr(sensor, sensor->odr);
403 if (err < 0)
404 return err;
405 } else {
406 err = st_lsm6dsx_shub_write_with_mask(sensor,
407 settings->odr_table.reg.addr,
408 settings->odr_table.reg.mask, 0);
409 if (err < 0)
410 return err;
411 }
412
413 if (settings->pwr_table.reg.addr) {
414 u8 val;
415
416 val = enable ? settings->pwr_table.on_val
417 : settings->pwr_table.off_val;
418 err = st_lsm6dsx_shub_write_with_mask(sensor,
419 settings->pwr_table.reg.addr,
420 settings->pwr_table.reg.mask, val);
421 if (err < 0)
422 return err;
423 }
424
425 return st_lsm6dsx_shub_master_enable(sensor, enable);
426}
427
428static int
429st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor,
430 struct iio_chan_spec const *ch,
431 int *val)
432{
433 int err, delay, len;
434 u8 data[4];
435
436 err = st_lsm6dsx_shub_set_enable(sensor, true);
437 if (err < 0)
438 return err;
439
440 delay = 1000000 / sensor->odr;
441 usleep_range(delay, 2 * delay);
442
443 len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3);
444 err = st_lsm6dsx_shub_read(sensor, ch->address, data, len);
445
446 st_lsm6dsx_shub_set_enable(sensor, false);
447
448 if (err < 0)
449 return err;
450
451 switch (len) {
452 case 2:
453 *val = (s16)le16_to_cpu(*((__le16 *)data));
454 break;
455 default:
456 return -EINVAL;
457 }
458
459 return IIO_VAL_INT;
460}
461
462static int
463st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev,
464 struct iio_chan_spec const *ch,
465 int *val, int *val2, long mask)
466{
467 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
468 int ret;
469
470 switch (mask) {
471 case IIO_CHAN_INFO_RAW:
472 ret = iio_device_claim_direct_mode(iio_dev);
473 if (ret)
474 break;
475
476 ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val);
477 iio_device_release_direct_mode(iio_dev);
478 break;
479 case IIO_CHAN_INFO_SAMP_FREQ:
480 *val = sensor->odr;
481 ret = IIO_VAL_INT;
482 break;
483 case IIO_CHAN_INFO_SCALE:
484 *val = 0;
485 *val2 = sensor->gain;
486 ret = IIO_VAL_INT_PLUS_MICRO;
487 break;
488 default:
489 ret = -EINVAL;
490 break;
491 }
492
493 return ret;
494}
495
496static int
497st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
498 struct iio_chan_spec const *chan,
499 int val, int val2, long mask)
500{
501 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
502 int err;
503
504 err = iio_device_claim_direct_mode(iio_dev);
505 if (err)
506 return err;
507
508 switch (mask) {
509 case IIO_CHAN_INFO_SAMP_FREQ: {
510 u16 data;
511
512 err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data);
513 if (!err)
514 sensor->odr = val;
515 break;
516 }
517 default:
518 err = -EINVAL;
519 break;
520 }
521
522 iio_device_release_direct_mode(iio_dev);
523
524 return err;
525}
526
527static ssize_t
528st_lsm6dsx_shub_sampling_freq_avail(struct device *dev,
529 struct device_attribute *attr,
530 char *buf)
531{
532 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
533 const struct st_lsm6dsx_ext_dev_settings *settings;
534 int i, len = 0;
535
536 settings = sensor->ext_info.settings;
537 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
538 u16 val = settings->odr_table.odr_avl[i].hz;
539
540 if (val > 0)
541 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
542 val);
543 }
544 buf[len - 1] = '\n';
545
546 return len;
547}
548
549static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev,
550 struct device_attribute *attr,
551 char *buf)
552{
553 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
554 const struct st_lsm6dsx_ext_dev_settings *settings;
555 int i, len = 0;
556
557 settings = sensor->ext_info.settings;
558 for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) {
559 u16 val = settings->fs_table.fs_avl[i].gain;
560
561 if (val > 0)
562 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
563 val);
564 }
565 buf[len - 1] = '\n';
566
567 return len;
568}
569
570static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail);
571static IIO_DEVICE_ATTR(in_scale_available, 0444,
572 st_lsm6dsx_shub_scale_avail, NULL, 0);
573static struct attribute *st_lsm6dsx_ext_attributes[] = {
574 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
575 &iio_dev_attr_in_scale_available.dev_attr.attr,
576 NULL,
577};
578
579static const struct attribute_group st_lsm6dsx_ext_attribute_group = {
580 .attrs = st_lsm6dsx_ext_attributes,
581};
582
583static const struct iio_info st_lsm6dsx_ext_info = {
584 .attrs = &st_lsm6dsx_ext_attribute_group,
585 .read_raw = st_lsm6dsx_shub_read_raw,
586 .write_raw = st_lsm6dsx_shub_write_raw,
587 .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
588};
589
590static struct iio_dev *
591st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw,
592 enum st_lsm6dsx_sensor_id id,
593 const struct st_lsm6dsx_ext_dev_settings *info,
594 u8 i2c_addr, const char *name)
595{
596 struct iio_chan_spec *ext_channels;
597 struct st_lsm6dsx_sensor *sensor;
598 struct iio_dev *iio_dev;
599
600 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
601 if (!iio_dev)
602 return NULL;
603
604 iio_dev->modes = INDIO_DIRECT_MODE;
605 iio_dev->dev.parent = hw->dev;
606 iio_dev->info = &st_lsm6dsx_ext_info;
607
608 sensor = iio_priv(iio_dev);
609 sensor->id = id;
610 sensor->hw = hw;
611 sensor->odr = info->odr_table.odr_avl[0].hz;
612 sensor->gain = info->fs_table.fs_avl[0].gain;
613 sensor->ext_info.settings = info;
614 sensor->ext_info.addr = i2c_addr;
615 sensor->watermark = 1;
616
617 switch (info->id) {
618 case ST_LSM6DSX_ID_MAGN: {
619 const struct iio_chan_spec magn_channels[] = {
620 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr,
621 IIO_MOD_X, 0),
622 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2,
623 IIO_MOD_Y, 1),
624 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4,
625 IIO_MOD_Z, 2),
626 IIO_CHAN_SOFT_TIMESTAMP(3),
627 };
628
629 ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels),
630 GFP_KERNEL);
631 if (!ext_channels)
632 return NULL;
633
634 memcpy(ext_channels, magn_channels, sizeof(magn_channels));
635 iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
636 iio_dev->channels = ext_channels;
637 iio_dev->num_channels = ARRAY_SIZE(magn_channels);
638
639 scnprintf(sensor->name, sizeof(sensor->name), "%s_magn",
640 name);
641 break;
642 }
643 default:
644 return NULL;
645 }
646 iio_dev->name = sensor->name;
647
648 return iio_dev;
649}
650
651static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor)
652{
653 const struct st_lsm6dsx_ext_dev_settings *settings;
654 int err;
655
656 settings = sensor->ext_info.settings;
657 if (settings->bdu.addr) {
658 err = st_lsm6dsx_shub_write_with_mask(sensor,
659 settings->bdu.addr,
660 settings->bdu.mask, 1);
661 if (err < 0)
662 return err;
663 }
664
665 if (settings->temp_comp.addr) {
666 err = st_lsm6dsx_shub_write_with_mask(sensor,
667 settings->temp_comp.addr,
668 settings->temp_comp.mask, 1);
669 if (err < 0)
670 return err;
671 }
672
673 if (settings->off_canc.addr) {
674 err = st_lsm6dsx_shub_write_with_mask(sensor,
675 settings->off_canc.addr,
676 settings->off_canc.mask, 1);
677 if (err < 0)
678 return err;
679 }
680
681 return 0;
682}
683
684static int
685st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
686 const struct st_lsm6dsx_ext_dev_settings *settings)
687{
688 const struct st_lsm6dsx_shub_settings *hub_settings;
689 struct st_lsm6dsx_sensor *sensor;
690 u8 config[3], data, slv_addr;
691 bool found = false;
692 int i, err;
693
694 hub_settings = &hw->settings->shub_settings;
695 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
696 sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
697
698 for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
699 if (!settings->i2c_addr[i])
700 continue;
701
702
703 config[0] = (settings->i2c_addr[i] << 1) | 0x1;
704 config[1] = settings->wai.addr;
705 config[2] = 0x1;
706
707 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
708 sizeof(config));
709 if (err < 0)
710 return err;
711
712 err = st_lsm6dsx_shub_master_enable(sensor, true);
713 if (err < 0)
714 return err;
715
716 st_lsm6dsx_shub_wait_complete(hw);
717
718 err = st_lsm6dsx_shub_read_reg(hw,
719 hub_settings->shub_out,
720 &data, sizeof(data));
721
722 st_lsm6dsx_shub_master_enable(sensor, false);
723
724 if (err < 0)
725 return err;
726
727 if (data != settings->wai.val)
728 continue;
729
730 *i2c_addr = settings->i2c_addr[i];
731 found = true;
732 break;
733 }
734
735
736 memset(config, 0, sizeof(config));
737 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
738 sizeof(config));
739 if (err < 0)
740 return err;
741
742 return found ? 0 : -ENODEV;
743}
744
745int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name)
746{
747 enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0;
748 struct st_lsm6dsx_sensor *sensor;
749 int err, i, num_ext_dev = 0;
750 u8 i2c_addr = 0;
751
752 for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) {
753 err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr,
754 &st_lsm6dsx_ext_dev_table[i]);
755 if (err == -ENODEV)
756 continue;
757 else if (err < 0)
758 return err;
759
760 hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id,
761 &st_lsm6dsx_ext_dev_table[i],
762 i2c_addr, name);
763 if (!hw->iio_devs[id])
764 return -ENOMEM;
765
766 sensor = iio_priv(hw->iio_devs[id]);
767 err = st_lsm6dsx_shub_init_device(sensor);
768 if (err < 0)
769 return err;
770
771 if (++num_ext_dev >= ST_LSM6DSX_MAX_SLV_NUM)
772 break;
773 id++;
774 }
775
776 return 0;
777}
778