1 /* 2 * STMicroelectronics st_lsm6dsx i2c controller driver 3 * 4 * i2c controller embedded in lsm6dx series can connect up to four 5 * slave devices using accelerometer sensor as trigger for i2c 6 * read/write operations. Current implementation relies on SLV0 channel 7 * for slave configuration and SLV{1,2,3} to read data and push them into 8 * the hw FIFO 9 * 10 * Copyright (C) 2018 Lorenzo Bianconi <lorenzo.bianconi83@gmail.com> 11 * 12 * Permission to use, copy, modify, and/or distribute this software for any 13 * purpose with or without fee is hereby granted, provided that the above 14 * copyright notice and this permission notice appear in all copies. 15 * 16 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 17 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 18 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 19 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 20 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 21 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 22 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 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 /* LIS2MDL */ 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 }, /* 1500 uG/LSB */ 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 92 static 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 * st_lsm6dsx_shub_read_reg - read i2c controller register 102 * 103 * Read st_lsm6dsx i2c controller register 104 */ 105 static 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); 119 out: 120 mutex_unlock(&hw->page_lock); 121 122 return err; 123 } 124 125 /** 126 * st_lsm6dsx_shub_write_reg - write i2c controller register 127 * 128 * Write st_lsm6dsx i2c controller register 129 */ 130 static 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); 143 out: 144 mutex_unlock(&hw->page_lock); 145 146 return err; 147 } 148 149 static int 150 st_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); 163 out: 164 mutex_unlock(&hw->page_lock); 165 166 return err; 167 } 168 169 static 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 /* enable acc sensor as trigger */ 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); 194 out: 195 mutex_unlock(&hw->page_lock); 196 197 return err; 198 } 199 200 /** 201 * st_lsm6dsx_shub_read - read data from slave device register 202 * 203 * Read data from slave device register. SLV0 is used for 204 * one-shot read operation 205 */ 206 static int 207 st_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 * st_lsm6dsx_shub_write - write data to slave device register 245 * 246 * Write data from slave device register. SLV0 is used for 247 * one-shot write operation 248 */ 249 static int 250 st_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 299 static int 300 st_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 315 static int 316 st_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 334 static int 335 st_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 /* use SLV{1,2,3} for FIFO read operations */ 353 static int 354 st_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 391 int 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 428 static int 429 st_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 462 static int 463 st_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 496 static int 497 st_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 527 static ssize_t 528 st_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 549 static 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 570 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail); 571 static IIO_DEVICE_ATTR(in_scale_available, 0444, 572 st_lsm6dsx_shub_scale_avail, NULL, 0); 573 static 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 579 static const struct attribute_group st_lsm6dsx_ext_attribute_group = { 580 .attrs = st_lsm6dsx_ext_attributes, 581 }; 582 583 static 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 590 static struct iio_dev * 591 st_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 651 static 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 684 static int 685 st_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 /* read wai slave register */ 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 /* reset SLV0 channel */ 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 745 int 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