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