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