drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c

   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 }