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 .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 * st_lsm6dsx_shub_read_reg - read i2c controller register 105 * 106 * Read st_lsm6dsx i2c controller register 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 * st_lsm6dsx_shub_write_reg - write i2c controller register 130 * 131 * Write st_lsm6dsx i2c controller register 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 /* enable acc sensor as trigger */ 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 * st_lsm6dsx_shub_read - read data from slave device register 205 * 206 * Read data from slave device register. SLV0 is used for 207 * one-shot read operation 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 * st_lsm6dsx_shub_write - write data to slave device register 248 * 249 * Write data from slave device register. SLV0 is used for 250 * one-shot write operation 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 /* use SLV{1,2,3} for FIFO read operations */ 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 /* read wai slave register */ 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 /* reset SLV0 channel */ 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 } 777