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 468 st_lsm6dsx_shub_set_enable(sensor, false); 469 470 if (err < 0) 471 return err; 472 473 switch (len) { 474 case 2: 475 *val = (s16)le16_to_cpu(*((__le16 *)data)); 476 break; 477 default: 478 return -EINVAL; 479 } 480 481 return IIO_VAL_INT; 482 } 483 484 static int 485 st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev, 486 struct iio_chan_spec const *ch, 487 int *val, int *val2, long mask) 488 { 489 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); 490 int ret; 491 492 switch (mask) { 493 case IIO_CHAN_INFO_RAW: 494 ret = iio_device_claim_direct_mode(iio_dev); 495 if (ret) 496 break; 497 498 ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val); 499 iio_device_release_direct_mode(iio_dev); 500 break; 501 case IIO_CHAN_INFO_SAMP_FREQ: 502 *val = sensor->odr / 1000; 503 *val2 = (sensor->odr % 1000) * 1000; 504 ret = IIO_VAL_INT_PLUS_MICRO; 505 break; 506 case IIO_CHAN_INFO_SCALE: 507 *val = 0; 508 *val2 = sensor->gain; 509 ret = IIO_VAL_INT_PLUS_MICRO; 510 break; 511 default: 512 ret = -EINVAL; 513 break; 514 } 515 516 return ret; 517 } 518 519 static int 520 st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev, 521 struct iio_chan_spec const *chan, 522 int val, int val2, long mask) 523 { 524 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); 525 int err; 526 527 err = iio_device_claim_direct_mode(iio_dev); 528 if (err) 529 return err; 530 531 switch (mask) { 532 case IIO_CHAN_INFO_SAMP_FREQ: { 533 u16 data; 534 535 val = val * 1000 + val2 / 1000; 536 err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data); 537 if (!err) 538 sensor->odr = val; 539 break; 540 } 541 default: 542 err = -EINVAL; 543 break; 544 } 545 546 iio_device_release_direct_mode(iio_dev); 547 548 return err; 549 } 550 551 static ssize_t 552 st_lsm6dsx_shub_sampling_freq_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->odr_table.odr_len; i++) { 562 u32 val = settings->odr_table.odr_avl[i].milli_hz; 563 564 len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ", 565 val / 1000, val % 1000); 566 } 567 buf[len - 1] = '\n'; 568 569 return len; 570 } 571 572 static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev, 573 struct device_attribute *attr, 574 char *buf) 575 { 576 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); 577 const struct st_lsm6dsx_ext_dev_settings *settings; 578 int i, len = 0; 579 580 settings = sensor->ext_info.settings; 581 for (i = 0; i < settings->fs_table.fs_len; i++) 582 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", 583 settings->fs_table.fs_avl[i].gain); 584 buf[len - 1] = '\n'; 585 586 return len; 587 } 588 589 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail); 590 static IIO_DEVICE_ATTR(in_scale_available, 0444, 591 st_lsm6dsx_shub_scale_avail, NULL, 0); 592 static struct attribute *st_lsm6dsx_ext_attributes[] = { 593 &iio_dev_attr_sampling_frequency_available.dev_attr.attr, 594 &iio_dev_attr_in_scale_available.dev_attr.attr, 595 NULL, 596 }; 597 598 static const struct attribute_group st_lsm6dsx_ext_attribute_group = { 599 .attrs = st_lsm6dsx_ext_attributes, 600 }; 601 602 static const struct iio_info st_lsm6dsx_ext_info = { 603 .attrs = &st_lsm6dsx_ext_attribute_group, 604 .read_raw = st_lsm6dsx_shub_read_raw, 605 .write_raw = st_lsm6dsx_shub_write_raw, 606 .hwfifo_set_watermark = st_lsm6dsx_set_watermark, 607 }; 608 609 static struct iio_dev * 610 st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw, 611 enum st_lsm6dsx_sensor_id id, 612 const struct st_lsm6dsx_ext_dev_settings *info, 613 u8 i2c_addr, const char *name) 614 { 615 struct iio_chan_spec *ext_channels; 616 struct st_lsm6dsx_sensor *sensor; 617 struct iio_dev *iio_dev; 618 619 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); 620 if (!iio_dev) 621 return NULL; 622 623 iio_dev->modes = INDIO_DIRECT_MODE; 624 iio_dev->dev.parent = hw->dev; 625 iio_dev->info = &st_lsm6dsx_ext_info; 626 627 sensor = iio_priv(iio_dev); 628 sensor->id = id; 629 sensor->hw = hw; 630 sensor->odr = info->odr_table.odr_avl[0].milli_hz; 631 sensor->gain = info->fs_table.fs_avl[0].gain; 632 sensor->ext_info.settings = info; 633 sensor->ext_info.addr = i2c_addr; 634 sensor->watermark = 1; 635 636 switch (info->id) { 637 case ST_LSM6DSX_ID_MAGN: { 638 const struct iio_chan_spec magn_channels[] = { 639 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr, 640 IIO_MOD_X, 0), 641 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2, 642 IIO_MOD_Y, 1), 643 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4, 644 IIO_MOD_Z, 2), 645 IIO_CHAN_SOFT_TIMESTAMP(3), 646 }; 647 648 ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels), 649 GFP_KERNEL); 650 if (!ext_channels) 651 return NULL; 652 653 memcpy(ext_channels, magn_channels, sizeof(magn_channels)); 654 iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks; 655 iio_dev->channels = ext_channels; 656 iio_dev->num_channels = ARRAY_SIZE(magn_channels); 657 658 scnprintf(sensor->name, sizeof(sensor->name), "%s_magn", 659 name); 660 break; 661 } 662 default: 663 return NULL; 664 } 665 iio_dev->name = sensor->name; 666 667 return iio_dev; 668 } 669 670 static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor) 671 { 672 const struct st_lsm6dsx_ext_dev_settings *settings; 673 int err; 674 675 settings = sensor->ext_info.settings; 676 if (settings->bdu.addr) { 677 err = st_lsm6dsx_shub_write_with_mask(sensor, 678 settings->bdu.addr, 679 settings->bdu.mask, 1); 680 if (err < 0) 681 return err; 682 } 683 684 if (settings->temp_comp.addr) { 685 err = st_lsm6dsx_shub_write_with_mask(sensor, 686 settings->temp_comp.addr, 687 settings->temp_comp.mask, 1); 688 if (err < 0) 689 return err; 690 } 691 692 if (settings->off_canc.addr) { 693 err = st_lsm6dsx_shub_write_with_mask(sensor, 694 settings->off_canc.addr, 695 settings->off_canc.mask, 1); 696 if (err < 0) 697 return err; 698 } 699 700 return 0; 701 } 702 703 static int 704 st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr, 705 const struct st_lsm6dsx_ext_dev_settings *settings) 706 { 707 const struct st_lsm6dsx_shub_settings *hub_settings; 708 u8 config[3], data, slv_addr, slv_config = 0; 709 const struct st_lsm6dsx_reg *aux_sens; 710 struct st_lsm6dsx_sensor *sensor; 711 bool found = false; 712 int i, err; 713 714 sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); 715 hub_settings = &hw->settings->shub_settings; 716 aux_sens = &hw->settings->shub_settings.aux_sens; 717 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); 718 /* do not overwrite aux_sens */ 719 if (slv_addr + 2 == aux_sens->addr) 720 slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask); 721 722 for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) { 723 if (!settings->i2c_addr[i]) 724 continue; 725 726 /* read wai slave register */ 727 config[0] = (settings->i2c_addr[i] << 1) | 0x1; 728 config[1] = settings->wai.addr; 729 config[2] = 0x1 | slv_config; 730 731 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, 732 sizeof(config)); 733 if (err < 0) 734 return err; 735 736 err = st_lsm6dsx_shub_master_enable(sensor, true); 737 if (err < 0) 738 return err; 739 740 st_lsm6dsx_shub_wait_complete(hw); 741 742 err = st_lsm6dsx_shub_read_output(hw, &data, sizeof(data)); 743 744 st_lsm6dsx_shub_master_enable(sensor, false); 745 746 if (err < 0) 747 return err; 748 749 if (data != settings->wai.val) 750 continue; 751 752 *i2c_addr = settings->i2c_addr[i]; 753 found = true; 754 break; 755 } 756 757 /* reset SLV0 channel */ 758 config[0] = hub_settings->pause; 759 config[1] = 0; 760 config[2] = slv_config; 761 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, 762 sizeof(config)); 763 if (err < 0) 764 return err; 765 766 return found ? 0 : -ENODEV; 767 } 768 769 int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name) 770 { 771 enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0; 772 struct st_lsm6dsx_sensor *sensor; 773 int err, i, num_ext_dev = 0; 774 u8 i2c_addr = 0; 775 776 for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) { 777 err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr, 778 &st_lsm6dsx_ext_dev_table[i]); 779 if (err == -ENODEV) 780 continue; 781 else if (err < 0) 782 return err; 783 784 hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id, 785 &st_lsm6dsx_ext_dev_table[i], 786 i2c_addr, name); 787 if (!hw->iio_devs[id]) 788 return -ENOMEM; 789 790 sensor = iio_priv(hw->iio_devs[id]); 791 err = st_lsm6dsx_shub_init_device(sensor); 792 if (err < 0) 793 return err; 794 795 if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev) 796 break; 797 id++; 798 } 799 800 return 0; 801 } 802