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