918 lines · c
1/*2 * STMicroelectronics st_lsm6dsx i2c controller driver3 *4 * i2c controller embedded in lsm6dx series can connect up to four5 * slave devices using accelerometer sensor as trigger for i2c6 * read/write operations. Current implementation relies on SLV0 channel7 * for slave configuration and SLV{1,2,3} to read data and push them into8 * the hw FIFO9 *10 * Copyright (C) 2018 Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>11 *12 * Permission to use, copy, modify, and/or distribute this software for any13 * purpose with or without fee is hereby granted, provided that the above14 * copyright notice and this permission notice appear in all copies.15 *16 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES17 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF18 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR19 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES20 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN21 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF22 * 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 39static 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 /* LIS3MDL */92 {93 .i2c_addr = { 0x1e },94 .wai = {95 .addr = 0x0f,96 .val = 0x3d,97 },98 .id = ST_LSM6DSX_ID_MAGN,99 .odr_table = {100 .reg = {101 .addr = 0x20,102 .mask = GENMASK(4, 2),103 },104 .odr_avl[0] = { 1000, 0x0 },105 .odr_avl[1] = { 2000, 0x1 },106 .odr_avl[2] = { 3000, 0x2 },107 .odr_avl[3] = { 5000, 0x3 },108 .odr_avl[4] = { 10000, 0x4 },109 .odr_avl[5] = { 20000, 0x5 },110 .odr_avl[6] = { 40000, 0x6 },111 .odr_avl[7] = { 80000, 0x7 },112 .odr_len = 8,113 },114 .fs_table = {115 .reg = {116 .addr = 0x21,117 .mask = GENMASK(6, 5),118 },119 .fs_avl[0] = {120 .gain = 146,121 .val = 0x00,122 }, /* 4000 uG/LSB */123 .fs_avl[1] = {124 .gain = 292,125 .val = 0x01,126 }, /* 8000 uG/LSB */127 .fs_avl[2] = {128 .gain = 438,129 .val = 0x02,130 }, /* 12000 uG/LSB */131 .fs_avl[3] = {132 .gain = 584,133 .val = 0x03,134 }, /* 16000 uG/LSB */135 .fs_len = 4,136 },137 .pwr_table = {138 .reg = {139 .addr = 0x22,140 .mask = GENMASK(1, 0),141 },142 .off_val = 0x2,143 .on_val = 0x0,144 },145 .bdu = {146 .addr = 0x24,147 .mask = BIT(6),148 },149 .out = {150 .addr = 0x28,151 .len = 6,152 },153 },154};155 156static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)157{158 struct st_lsm6dsx_sensor *sensor;159 u32 odr, timeout;160 161 sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);162 odr = (hw->enable_mask & BIT(ST_LSM6DSX_ID_ACC)) ? sensor->odr : 12500;163 /* set 10ms as minimum timeout for i2c slave configuration */164 timeout = max_t(u32, 2000000U / odr + 1, 10);165 msleep(timeout);166}167 168/*169 * st_lsm6dsx_shub_read_output - read i2c controller register170 *171 * Read st_lsm6dsx i2c controller register172 */173int st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data, int len)174{175 const struct st_lsm6dsx_shub_settings *hub_settings;176 int err;177 178 mutex_lock(&hw->page_lock);179 180 hub_settings = &hw->settings->shub_settings;181 if (hub_settings->shub_out.sec_page) {182 err = st_lsm6dsx_set_page(hw, true);183 if (err < 0)184 goto out;185 }186 187 err = regmap_bulk_read(hw->regmap, hub_settings->shub_out.addr,188 data, len);189 190 if (hub_settings->shub_out.sec_page)191 st_lsm6dsx_set_page(hw, false);192out:193 mutex_unlock(&hw->page_lock);194 195 return err;196}197 198/*199 * st_lsm6dsx_shub_write_reg - write i2c controller register200 *201 * Write st_lsm6dsx i2c controller register202 */203static int st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw *hw, u8 addr,204 u8 *data, int len)205{206 int err;207 208 mutex_lock(&hw->page_lock);209 err = st_lsm6dsx_set_page(hw, true);210 if (err < 0)211 goto out;212 213 err = regmap_bulk_write(hw->regmap, addr, data, len);214 215 st_lsm6dsx_set_page(hw, false);216out:217 mutex_unlock(&hw->page_lock);218 219 return err;220}221 222static int223st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr,224 u8 mask, u8 val)225{226 int err;227 228 mutex_lock(&hw->page_lock);229 err = st_lsm6dsx_set_page(hw, true);230 if (err < 0)231 goto out;232 233 err = regmap_update_bits(hw->regmap, addr, mask, val);234 235 st_lsm6dsx_set_page(hw, false);236out:237 mutex_unlock(&hw->page_lock);238 239 return err;240}241 242static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,243 bool enable)244{245 const struct st_lsm6dsx_shub_settings *hub_settings;246 struct st_lsm6dsx_hw *hw = sensor->hw;247 unsigned int data;248 int err;249 250 /* enable acc sensor as trigger */251 err = st_lsm6dsx_sensor_set_enable(sensor, enable);252 if (err < 0)253 return err;254 255 mutex_lock(&hw->page_lock);256 257 hub_settings = &hw->settings->shub_settings;258 if (hub_settings->master_en.sec_page) {259 err = st_lsm6dsx_set_page(hw, true);260 if (err < 0)261 goto out;262 }263 264 data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask);265 err = regmap_update_bits(hw->regmap, hub_settings->master_en.addr,266 hub_settings->master_en.mask, data);267 268 if (hub_settings->master_en.sec_page)269 st_lsm6dsx_set_page(hw, false);270out:271 mutex_unlock(&hw->page_lock);272 273 return err;274}275 276/*277 * st_lsm6dsx_shub_read - read data from slave device register278 *279 * Read data from slave device register. SLV0 is used for280 * one-shot read operation281 */282static int283st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,284 u8 *data, int len)285{286 const struct st_lsm6dsx_shub_settings *hub_settings;287 u8 config[3], slv_addr, slv_config = 0;288 struct st_lsm6dsx_hw *hw = sensor->hw;289 const struct st_lsm6dsx_reg *aux_sens;290 int err;291 292 hub_settings = &hw->settings->shub_settings;293 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);294 aux_sens = &hw->settings->shub_settings.aux_sens;295 /* do not overwrite aux_sens */296 if (slv_addr + 2 == aux_sens->addr)297 slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);298 299 config[0] = (sensor->ext_info.addr << 1) | 1;300 config[1] = addr;301 config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;302 303 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,304 sizeof(config));305 if (err < 0)306 return err;307 308 err = st_lsm6dsx_shub_master_enable(sensor, true);309 if (err < 0)310 return err;311 312 st_lsm6dsx_shub_wait_complete(hw);313 314 err = st_lsm6dsx_shub_read_output(hw, data,315 len & ST_LS6DSX_READ_OP_MASK);316 if (err < 0)317 return err;318 319 st_lsm6dsx_shub_master_enable(sensor, false);320 321 config[0] = hub_settings->pause;322 config[1] = 0;323 config[2] = slv_config;324 return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,325 sizeof(config));326}327 328/*329 * st_lsm6dsx_shub_write - write data to slave device register330 *331 * Write data from slave device register. SLV0 is used for332 * one-shot write operation333 */334static int335st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,336 u8 *data, int len)337{338 const struct st_lsm6dsx_shub_settings *hub_settings;339 struct st_lsm6dsx_hw *hw = sensor->hw;340 u8 config[2], slv_addr;341 int err, i;342 343 hub_settings = &hw->settings->shub_settings;344 if (hub_settings->wr_once.addr) {345 unsigned int data;346 347 data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask);348 err = st_lsm6dsx_shub_write_reg_with_mask(hw,349 hub_settings->wr_once.addr,350 hub_settings->wr_once.mask,351 data);352 if (err < 0)353 return err;354 }355 356 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);357 config[0] = sensor->ext_info.addr << 1;358 for (i = 0 ; i < len; i++) {359 config[1] = addr + i;360 361 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,362 sizeof(config));363 if (err < 0)364 return err;365 366 err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr,367 &data[i], 1);368 if (err < 0)369 return err;370 371 err = st_lsm6dsx_shub_master_enable(sensor, true);372 if (err < 0)373 return err;374 375 st_lsm6dsx_shub_wait_complete(hw);376 377 st_lsm6dsx_shub_master_enable(sensor, false);378 }379 380 config[0] = hub_settings->pause;381 config[1] = 0;382 return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));383}384 385static int386st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor *sensor,387 u8 addr, u8 mask, u8 val)388{389 int err;390 u8 data;391 392 err = st_lsm6dsx_shub_read(sensor, addr, &data, sizeof(data));393 if (err < 0)394 return err;395 396 data = ((data & ~mask) | (val << __ffs(mask) & mask));397 398 return st_lsm6dsx_shub_write(sensor, addr, &data, sizeof(data));399}400 401static int402st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor *sensor,403 u32 odr, u16 *val)404{405 const struct st_lsm6dsx_ext_dev_settings *settings;406 int i;407 408 settings = sensor->ext_info.settings;409 for (i = 0; i < settings->odr_table.odr_len; i++) {410 if (settings->odr_table.odr_avl[i].milli_hz == odr)411 break;412 }413 414 if (i == settings->odr_table.odr_len)415 return -EINVAL;416 417 *val = settings->odr_table.odr_avl[i].val;418 return 0;419}420 421static int422st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u32 odr)423{424 const struct st_lsm6dsx_ext_dev_settings *settings;425 u16 val;426 int err;427 428 err = st_lsm6dsx_shub_get_odr_val(sensor, odr, &val);429 if (err < 0)430 return err;431 432 settings = sensor->ext_info.settings;433 return st_lsm6dsx_shub_write_with_mask(sensor,434 settings->odr_table.reg.addr,435 settings->odr_table.reg.mask,436 val);437}438 439/* use SLV{1,2,3} for FIFO read operations */440static int441st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor,442 bool enable)443{444 const struct st_lsm6dsx_shub_settings *hub_settings;445 const struct st_lsm6dsx_ext_dev_settings *settings;446 u8 config[9] = {}, enable_mask, slv_addr;447 struct st_lsm6dsx_hw *hw = sensor->hw;448 struct st_lsm6dsx_sensor *cur_sensor;449 int i, j = 0;450 451 hub_settings = &hw->settings->shub_settings;452 if (enable)453 enable_mask = hw->enable_mask | BIT(sensor->id);454 else455 enable_mask = hw->enable_mask & ~BIT(sensor->id);456 457 for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) {458 if (!hw->iio_devs[i])459 continue;460 461 cur_sensor = iio_priv(hw->iio_devs[i]);462 if (!(enable_mask & BIT(cur_sensor->id)))463 continue;464 465 settings = cur_sensor->ext_info.settings;466 config[j] = (sensor->ext_info.addr << 1) | 1;467 config[j + 1] = settings->out.addr;468 config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) |469 hub_settings->batch_en;470 j += 3;471 }472 473 slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr);474 return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,475 sizeof(config));476}477 478int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)479{480 const struct st_lsm6dsx_ext_dev_settings *settings;481 int err;482 483 err = st_lsm6dsx_shub_config_channels(sensor, enable);484 if (err < 0)485 return err;486 487 settings = sensor->ext_info.settings;488 if (enable) {489 err = st_lsm6dsx_shub_set_odr(sensor,490 sensor->ext_info.slv_odr);491 if (err < 0)492 return err;493 } else {494 err = st_lsm6dsx_shub_write_with_mask(sensor,495 settings->odr_table.reg.addr,496 settings->odr_table.reg.mask, 0);497 if (err < 0)498 return err;499 }500 501 if (settings->pwr_table.reg.addr) {502 u8 val;503 504 val = enable ? settings->pwr_table.on_val505 : settings->pwr_table.off_val;506 err = st_lsm6dsx_shub_write_with_mask(sensor,507 settings->pwr_table.reg.addr,508 settings->pwr_table.reg.mask, val);509 if (err < 0)510 return err;511 }512 513 return st_lsm6dsx_shub_master_enable(sensor, enable);514}515 516static int517st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor,518 struct iio_chan_spec const *ch,519 int *val)520{521 int err, delay, len;522 u8 data[4];523 524 err = st_lsm6dsx_shub_set_enable(sensor, true);525 if (err < 0)526 return err;527 528 delay = 1000000000 / sensor->ext_info.slv_odr;529 usleep_range(delay, 2 * delay);530 531 len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3);532 err = st_lsm6dsx_shub_read(sensor, ch->address, data, len);533 if (err < 0)534 return err;535 536 err = st_lsm6dsx_shub_set_enable(sensor, false);537 if (err < 0)538 return err;539 540 switch (len) {541 case 2:542 *val = (s16)le16_to_cpu(*((__le16 *)data));543 break;544 default:545 return -EINVAL;546 }547 548 return IIO_VAL_INT;549}550 551static int552st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev,553 struct iio_chan_spec const *ch,554 int *val, int *val2, long mask)555{556 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);557 int ret;558 559 switch (mask) {560 case IIO_CHAN_INFO_RAW:561 ret = iio_device_claim_direct_mode(iio_dev);562 if (ret)563 break;564 565 ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val);566 iio_device_release_direct_mode(iio_dev);567 break;568 case IIO_CHAN_INFO_SAMP_FREQ:569 *val = sensor->ext_info.slv_odr / 1000;570 *val2 = (sensor->ext_info.slv_odr % 1000) * 1000;571 ret = IIO_VAL_INT_PLUS_MICRO;572 break;573 case IIO_CHAN_INFO_SCALE:574 *val = 0;575 *val2 = sensor->gain;576 ret = IIO_VAL_INT_PLUS_MICRO;577 break;578 default:579 ret = -EINVAL;580 break;581 }582 583 return ret;584}585 586static int587st_lsm6dsx_shub_set_full_scale(struct st_lsm6dsx_sensor *sensor,588 u32 gain)589{590 const struct st_lsm6dsx_fs_table_entry *fs_table;591 int i, err;592 593 fs_table = &sensor->ext_info.settings->fs_table;594 if (!fs_table->reg.addr)595 return -ENOTSUPP;596 597 for (i = 0; i < fs_table->fs_len; i++) {598 if (fs_table->fs_avl[i].gain == gain)599 break;600 }601 602 if (i == fs_table->fs_len)603 return -EINVAL;604 605 err = st_lsm6dsx_shub_write_with_mask(sensor, fs_table->reg.addr,606 fs_table->reg.mask,607 fs_table->fs_avl[i].val);608 if (err < 0)609 return err;610 611 sensor->gain = gain;612 613 return 0;614}615 616static int617st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,618 struct iio_chan_spec const *chan,619 int val, int val2, long mask)620{621 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);622 int err;623 624 err = iio_device_claim_direct_mode(iio_dev);625 if (err)626 return err;627 628 switch (mask) {629 case IIO_CHAN_INFO_SAMP_FREQ: {630 u16 data;631 632 val = val * 1000 + val2 / 1000;633 err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data);634 if (!err) {635 struct st_lsm6dsx_hw *hw = sensor->hw;636 struct st_lsm6dsx_sensor *ref_sensor;637 u8 odr_val;638 int odr;639 640 ref_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);641 odr = st_lsm6dsx_check_odr(ref_sensor, val, &odr_val);642 if (odr < 0) {643 err = odr;644 goto release;645 }646 647 sensor->ext_info.slv_odr = val;648 sensor->odr = odr;649 }650 break;651 }652 case IIO_CHAN_INFO_SCALE:653 err = st_lsm6dsx_shub_set_full_scale(sensor, val2);654 break;655 default:656 err = -EINVAL;657 break;658 }659 660release:661 iio_device_release_direct_mode(iio_dev);662 663 return err;664}665 666static ssize_t667st_lsm6dsx_shub_sampling_freq_avail(struct device *dev,668 struct device_attribute *attr,669 char *buf)670{671 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));672 const struct st_lsm6dsx_ext_dev_settings *settings;673 int i, len = 0;674 675 settings = sensor->ext_info.settings;676 for (i = 0; i < settings->odr_table.odr_len; i++) {677 u32 val = settings->odr_table.odr_avl[i].milli_hz;678 679 len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ",680 val / 1000, val % 1000);681 }682 buf[len - 1] = '\n';683 684 return len;685}686 687static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev,688 struct device_attribute *attr,689 char *buf)690{691 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));692 const struct st_lsm6dsx_ext_dev_settings *settings;693 int i, len = 0;694 695 settings = sensor->ext_info.settings;696 for (i = 0; i < settings->fs_table.fs_len; i++)697 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",698 settings->fs_table.fs_avl[i].gain);699 buf[len - 1] = '\n';700 701 return len;702}703 704static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail);705static IIO_DEVICE_ATTR(in_scale_available, 0444,706 st_lsm6dsx_shub_scale_avail, NULL, 0);707static struct attribute *st_lsm6dsx_shub_attributes[] = {708 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,709 &iio_dev_attr_in_scale_available.dev_attr.attr,710 NULL,711};712 713static const struct attribute_group st_lsm6dsx_shub_attribute_group = {714 .attrs = st_lsm6dsx_shub_attributes,715};716 717static const struct iio_info st_lsm6dsx_shub_info = {718 .attrs = &st_lsm6dsx_shub_attribute_group,719 .read_raw = st_lsm6dsx_shub_read_raw,720 .write_raw = st_lsm6dsx_shub_write_raw,721 .hwfifo_set_watermark = st_lsm6dsx_set_watermark,722};723 724static struct iio_dev *725st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw,726 enum st_lsm6dsx_sensor_id id,727 const struct st_lsm6dsx_ext_dev_settings *info,728 u8 i2c_addr, const char *name)729{730 enum st_lsm6dsx_sensor_id ref_id = ST_LSM6DSX_ID_ACC;731 struct iio_chan_spec *ext_channels;732 struct st_lsm6dsx_sensor *sensor;733 struct iio_dev *iio_dev;734 735 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));736 if (!iio_dev)737 return NULL;738 739 iio_dev->modes = INDIO_DIRECT_MODE;740 iio_dev->info = &st_lsm6dsx_shub_info;741 742 sensor = iio_priv(iio_dev);743 sensor->id = id;744 sensor->hw = hw;745 sensor->odr = hw->settings->odr_table[ref_id].odr_avl[0].milli_hz;746 sensor->ext_info.slv_odr = info->odr_table.odr_avl[0].milli_hz;747 sensor->gain = info->fs_table.fs_avl[0].gain;748 sensor->ext_info.settings = info;749 sensor->ext_info.addr = i2c_addr;750 sensor->watermark = 1;751 752 switch (info->id) {753 case ST_LSM6DSX_ID_MAGN: {754 const struct iio_chan_spec magn_channels[] = {755 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr,756 IIO_MOD_X, 0),757 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2,758 IIO_MOD_Y, 1),759 ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4,760 IIO_MOD_Z, 2),761 IIO_CHAN_SOFT_TIMESTAMP(3),762 };763 764 ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels),765 GFP_KERNEL);766 if (!ext_channels)767 return NULL;768 769 memcpy(ext_channels, magn_channels, sizeof(magn_channels));770 iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;771 iio_dev->channels = ext_channels;772 iio_dev->num_channels = ARRAY_SIZE(magn_channels);773 774 scnprintf(sensor->name, sizeof(sensor->name), "%s_magn",775 name);776 break;777 }778 default:779 return NULL;780 }781 iio_dev->name = sensor->name;782 783 return iio_dev;784}785 786static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor)787{788 const struct st_lsm6dsx_ext_dev_settings *settings;789 int err;790 791 settings = sensor->ext_info.settings;792 if (settings->bdu.addr) {793 err = st_lsm6dsx_shub_write_with_mask(sensor,794 settings->bdu.addr,795 settings->bdu.mask, 1);796 if (err < 0)797 return err;798 }799 800 if (settings->temp_comp.addr) {801 err = st_lsm6dsx_shub_write_with_mask(sensor,802 settings->temp_comp.addr,803 settings->temp_comp.mask, 1);804 if (err < 0)805 return err;806 }807 808 if (settings->off_canc.addr) {809 err = st_lsm6dsx_shub_write_with_mask(sensor,810 settings->off_canc.addr,811 settings->off_canc.mask, 1);812 if (err < 0)813 return err;814 }815 816 return 0;817}818 819static int820st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,821 const struct st_lsm6dsx_ext_dev_settings *settings)822{823 const struct st_lsm6dsx_shub_settings *hub_settings;824 u8 config[3], data, slv_addr, slv_config = 0;825 const struct st_lsm6dsx_reg *aux_sens;826 struct st_lsm6dsx_sensor *sensor;827 bool found = false;828 int i, err;829 830 sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);831 hub_settings = &hw->settings->shub_settings;832 aux_sens = &hw->settings->shub_settings.aux_sens;833 slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);834 /* do not overwrite aux_sens */835 if (slv_addr + 2 == aux_sens->addr)836 slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);837 838 for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {839 if (!settings->i2c_addr[i])840 continue;841 842 /* read wai slave register */843 config[0] = (settings->i2c_addr[i] << 1) | 0x1;844 config[1] = settings->wai.addr;845 config[2] = 0x1 | slv_config;846 847 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,848 sizeof(config));849 if (err < 0)850 return err;851 852 err = st_lsm6dsx_shub_master_enable(sensor, true);853 if (err < 0)854 return err;855 856 st_lsm6dsx_shub_wait_complete(hw);857 858 err = st_lsm6dsx_shub_read_output(hw, &data, sizeof(data));859 860 st_lsm6dsx_shub_master_enable(sensor, false);861 862 if (err < 0)863 return err;864 865 if (data != settings->wai.val)866 continue;867 868 *i2c_addr = settings->i2c_addr[i];869 found = true;870 break;871 }872 873 /* reset SLV0 channel */874 config[0] = hub_settings->pause;875 config[1] = 0;876 config[2] = slv_config;877 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,878 sizeof(config));879 if (err < 0)880 return err;881 882 return found ? 0 : -ENODEV;883}884 885int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name)886{887 enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0;888 struct st_lsm6dsx_sensor *sensor;889 int err, i, num_ext_dev = 0;890 u8 i2c_addr = 0;891 892 for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) {893 err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr,894 &st_lsm6dsx_ext_dev_table[i]);895 if (err == -ENODEV)896 continue;897 else if (err < 0)898 return err;899 900 hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id,901 &st_lsm6dsx_ext_dev_table[i],902 i2c_addr, name);903 if (!hw->iio_devs[id])904 return -ENOMEM;905 906 sensor = iio_priv(hw->iio_devs[id]);907 err = st_lsm6dsx_shub_init_device(sensor);908 if (err < 0)909 return err;910 911 if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev)912 break;913 id++;914 }915 916 return 0;917}918