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 err = odr; 549 goto release; 550 } 551 552 sensor->ext_info.slv_odr = val; 553 sensor->odr = odr; 554 } 555 break; 556 } 557 default: 558 err = -EINVAL; 559 break; 560 } 561 562 release: 563 iio_device_release_direct_mode(iio_dev); 564 565 return err; 566 } 567 568 static ssize_t 569 st_lsm6dsx_shub_sampling_freq_avail(struct device *dev, 570 struct device_attribute *attr, 571 char *buf) 572 { 573 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); 574 const struct st_lsm6dsx_ext_dev_settings *settings; 575 int i, len = 0; 576 577 settings = sensor->ext_info.settings; 578 for (i = 0; i < settings->odr_table.odr_len; i++) { 579 u32 val = settings->odr_table.odr_avl[i].milli_hz; 580 581 len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ", 582 val / 1000, val % 1000); 583 } 584 buf[len - 1] = '\n'; 585 586 return len; 587 } 588 589 static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev, 590 struct device_attribute *attr, 591 char *buf) 592 { 593 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); 594 const struct st_lsm6dsx_ext_dev_settings *settings; 595 int i, len = 0; 596 597 settings = sensor->ext_info.settings; 598 for (i = 0; i < settings->fs_table.fs_len; i++) 599 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", 600 settings->fs_table.fs_avl[i].gain); 601 buf[len - 1] = '\n'; 602 603 return len; 604 } 605 606 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail); 607 static IIO_DEVICE_ATTR(in_scale_available, 0444, 608 st_lsm6dsx_shub_scale_avail, NULL, 0); 609 static struct attribute *st_lsm6dsx_ext_attributes[] = { 610 &iio_dev_attr_sampling_frequency_available.dev_attr.attr, 611 &iio_dev_attr_in_scale_available.dev_attr.attr, 612 NULL, 613 }; 614 615 static const struct attribute_group st_lsm6dsx_ext_attribute_group = { 616 .attrs = st_lsm6dsx_ext_attributes, 617 }; 618 619 static const struct iio_info st_lsm6dsx_ext_info = { 620 .attrs = &st_lsm6dsx_ext_attribute_group, 621 .read_raw = st_lsm6dsx_shub_read_raw, 622 .write_raw = st_lsm6dsx_shub_write_raw, 623 .hwfifo_set_watermark = st_lsm6dsx_set_watermark, 624 }; 625 626 static struct iio_dev * 627 st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw, 628 enum st_lsm6dsx_sensor_id id, 629 const struct st_lsm6dsx_ext_dev_settings *info, 630 u8 i2c_addr, const char *name) 631 { 632 enum st_lsm6dsx_sensor_id ref_id = ST_LSM6DSX_ID_ACC; 633 struct iio_chan_spec *ext_channels; 634 struct st_lsm6dsx_sensor *sensor; 635 struct iio_dev *iio_dev; 636 637 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); 638 if (!iio_dev) 639 return NULL; 640 641 iio_dev->modes = INDIO_DIRECT_MODE; 642 iio_dev->dev.parent = hw->dev; 643 iio_dev->info = &st_lsm6dsx_ext_info; 644 645 sensor = iio_priv(iio_dev); 646 sensor->id = id; 647 sensor->hw = hw; 648 sensor->odr = hw->settings->odr_table[ref_id].odr_avl[0].milli_hz; 649 sensor->ext_info.slv_odr = info->odr_table.odr_avl[0].milli_hz; 650 sensor->gain = info->fs_table.fs_avl[0].gain; 651 sensor->ext_info.settings = info; 652 sensor->ext_info.addr = i2c_addr; 653 sensor->watermark = 1; 654 655 switch (info->id) { 656 case ST_LSM6DSX_ID_MAGN: { 657 const struct iio_chan_spec magn_channels[] = { 658 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr, 659 IIO_MOD_X, 0), 660 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2, 661 IIO_MOD_Y, 1), 662 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4, 663 IIO_MOD_Z, 2), 664 IIO_CHAN_SOFT_TIMESTAMP(3), 665 }; 666 667 ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels), 668 GFP_KERNEL); 669 if (!ext_channels) 670 return NULL; 671 672 memcpy(ext_channels, magn_channels, sizeof(magn_channels)); 673 iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks; 674 iio_dev->channels = ext_channels; 675 iio_dev->num_channels = ARRAY_SIZE(magn_channels); 676 677 scnprintf(sensor->name, sizeof(sensor->name), "%s_magn", 678 name); 679 break; 680 } 681 default: 682 return NULL; 683 } 684 iio_dev->name = sensor->name; 685 686 return iio_dev; 687 } 688 689 static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor) 690 { 691 const struct st_lsm6dsx_ext_dev_settings *settings; 692 int err; 693 694 settings = sensor->ext_info.settings; 695 if (settings->bdu.addr) { 696 err = st_lsm6dsx_shub_write_with_mask(sensor, 697 settings->bdu.addr, 698 settings->bdu.mask, 1); 699 if (err < 0) 700 return err; 701 } 702 703 if (settings->temp_comp.addr) { 704 err = st_lsm6dsx_shub_write_with_mask(sensor, 705 settings->temp_comp.addr, 706 settings->temp_comp.mask, 1); 707 if (err < 0) 708 return err; 709 } 710 711 if (settings->off_canc.addr) { 712 err = st_lsm6dsx_shub_write_with_mask(sensor, 713 settings->off_canc.addr, 714 settings->off_canc.mask, 1); 715 if (err < 0) 716 return err; 717 } 718 719 return 0; 720 } 721 722 static int 723 st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr, 724 const struct st_lsm6dsx_ext_dev_settings *settings) 725 { 726 const struct st_lsm6dsx_shub_settings *hub_settings; 727 u8 config[3], data, slv_addr, slv_config = 0; 728 const struct st_lsm6dsx_reg *aux_sens; 729 struct st_lsm6dsx_sensor *sensor; 730 bool found = false; 731 int i, err; 732 733 sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); 734 hub_settings = &hw->settings->shub_settings; 735 aux_sens = &hw->settings->shub_settings.aux_sens; 736 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); 737 /* do not overwrite aux_sens */ 738 if (slv_addr + 2 == aux_sens->addr) 739 slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask); 740 741 for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) { 742 if (!settings->i2c_addr[i]) 743 continue; 744 745 /* read wai slave register */ 746 config[0] = (settings->i2c_addr[i] << 1) | 0x1; 747 config[1] = settings->wai.addr; 748 config[2] = 0x1 | slv_config; 749 750 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, 751 sizeof(config)); 752 if (err < 0) 753 return err; 754 755 err = st_lsm6dsx_shub_master_enable(sensor, true); 756 if (err < 0) 757 return err; 758 759 st_lsm6dsx_shub_wait_complete(hw); 760 761 err = st_lsm6dsx_shub_read_output(hw, &data, sizeof(data)); 762 763 st_lsm6dsx_shub_master_enable(sensor, false); 764 765 if (err < 0) 766 return err; 767 768 if (data != settings->wai.val) 769 continue; 770 771 *i2c_addr = settings->i2c_addr[i]; 772 found = true; 773 break; 774 } 775 776 /* reset SLV0 channel */ 777 config[0] = hub_settings->pause; 778 config[1] = 0; 779 config[2] = slv_config; 780 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, 781 sizeof(config)); 782 if (err < 0) 783 return err; 784 785 return found ? 0 : -ENODEV; 786 } 787 788 int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name) 789 { 790 enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0; 791 struct st_lsm6dsx_sensor *sensor; 792 int err, i, num_ext_dev = 0; 793 u8 i2c_addr = 0; 794 795 for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) { 796 err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr, 797 &st_lsm6dsx_ext_dev_table[i]); 798 if (err == -ENODEV) 799 continue; 800 else if (err < 0) 801 return err; 802 803 hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id, 804 &st_lsm6dsx_ext_dev_table[i], 805 i2c_addr, name); 806 if (!hw->iio_devs[id]) 807 return -ENOMEM; 808 809 sensor = iio_priv(hw->iio_devs[id]); 810 err = st_lsm6dsx_shub_init_device(sensor); 811 if (err < 0) 812 return err; 813 814 if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev) 815 break; 816 id++; 817 } 818 819 return 0; 820 } 821