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_MAX_SLV_NUM                  3
  34 #define ST_LSM6DSX_SLV_ADDR(n, base)            ((base) + (n) * 3)
  35 #define ST_LSM6DSX_SLV_SUB_ADDR(n, base)        ((base) + 1 + (n) * 3)
  36 #define ST_LSM6DSX_SLV_CONFIG(n, base)          ((base) + 2 + (n) * 3)
  37
  38 #define ST_LS6DSX_READ_OP_MASK                  GENMASK(2, 0)
  39
  40 static const struct st_lsm6dsx_ext_dev_settings st_lsm6dsx_ext_dev_table[] = {
  41         /* LIS2MDL */
  42         {
  43                 .i2c_addr = { 0x1e },
  44                 .wai = {
  45                         .addr = 0x4f,
  46                         .val = 0x40,
  47                 },
  48                 .id = ST_LSM6DSX_ID_MAGN,
  49                 .odr_table = {
  50                         .reg = {
  51                                 .addr = 0x60,
  52                                 .mask = GENMASK(3, 2),
  53                         },
  54                         .odr_avl[0] = {  10000, 0x0 },
  55                         .odr_avl[1] = {  20000, 0x1 },
  56                         .odr_avl[2] = {  50000, 0x2 },
  57                         .odr_avl[3] = { 100000, 0x3 },
  58                         .odr_len = 4,
  59                 },
  60                 .fs_table = {
  61                         .fs_avl[0] = {
  62                                 .gain = 1500,
  63                                 .val = 0x0,
  64                         }, /* 1500 uG/LSB */
  65                         .fs_len = 1,
  66                 },
  67                 .temp_comp = {
  68                         .addr = 0x60,
  69                         .mask = BIT(7),
  70                 },
  71                 .pwr_table = {
  72                         .reg = {
  73                                 .addr = 0x60,
  74                                 .mask = GENMASK(1, 0),
  75                         },
  76                         .off_val = 0x2,
  77                         .on_val = 0x0,
  78                 },
  79                 .off_canc = {
  80                         .addr = 0x61,
  81                         .mask = BIT(1),
  82                 },
  83                 .bdu = {
  84                         .addr = 0x62,
  85                         .mask = BIT(4),
  86                 },
  87                 .out = {
  88                         .addr = 0x68,
  89                         .len = 6,
  90                 },
  91         },
  92 };
  93
  94 static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
  95 {
  96         struct st_lsm6dsx_sensor *sensor;
  97         u32 odr;
  98
  99         sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 100         odr = (hw->enable_mask & BIT(ST_LSM6DSX_ID_ACC)) ? sensor->odr : 12500;
 101         msleep((2000000U / odr) + 1);
 102 }
 103
 104 /**
 105  * st_lsm6dsx_shub_read_reg - read i2c controller register
 106  *
 107  * Read st_lsm6dsx i2c controller register
 108  */
 109 static int st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 addr,
 110                                     u8 *data, int len)
 111 {
 112         int err;
 113
 114         mutex_lock(&hw->page_lock);
 115
 116         err = st_lsm6dsx_set_page(hw, true);
 117         if (err < 0)
 118                 goto out;
 119
 120         err = regmap_bulk_read(hw->regmap, addr, data, len);
 121
 122         st_lsm6dsx_set_page(hw, false);
 123 out:
 124         mutex_unlock(&hw->page_lock);
 125
 126         return err;
 127 }
 128
 129 /**
 130  * st_lsm6dsx_shub_write_reg - write i2c controller register
 131  *
 132  * Write st_lsm6dsx i2c controller register
 133  */
 134 static int st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw *hw, u8 addr,
 135                                      u8 *data, int len)
 136 {
 137         int err;
 138
 139         mutex_lock(&hw->page_lock);
 140         err = st_lsm6dsx_set_page(hw, true);
 141         if (err < 0)
 142                 goto out;
 143
 144         err = regmap_bulk_write(hw->regmap, addr, data, len);
 145
 146         st_lsm6dsx_set_page(hw, false);
 147 out:
 148         mutex_unlock(&hw->page_lock);
 149
 150         return err;
 151 }
 152
 153 static int
 154 st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr,
 155                                     u8 mask, u8 val)
 156 {
 157         int err;
 158
 159         mutex_lock(&hw->page_lock);
 160         err = st_lsm6dsx_set_page(hw, true);
 161         if (err < 0)
 162                 goto out;
 163
 164         err = regmap_update_bits(hw->regmap, addr, mask, val);
 165
 166         st_lsm6dsx_set_page(hw, false);
 167 out:
 168         mutex_unlock(&hw->page_lock);
 169
 170         return err;
 171 }
 172
 173 static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,
 174                                          bool enable)
 175 {
 176         const struct st_lsm6dsx_shub_settings *hub_settings;
 177         struct st_lsm6dsx_hw *hw = sensor->hw;
 178         unsigned int data;
 179         int err;
 180
 181         /* enable acc sensor as trigger */
 182         err = st_lsm6dsx_sensor_set_enable(sensor, enable);
 183         if (err < 0)
 184                 return err;
 185
 186         mutex_lock(&hw->page_lock);
 187
 188         hub_settings = &hw->settings->shub_settings;
 189         err = st_lsm6dsx_set_page(hw, true);
 190         if (err < 0)
 191                 goto out;
 192
 193         data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask);
 194         err = regmap_update_bits(hw->regmap, hub_settings->master_en.addr,
 195                                  hub_settings->master_en.mask, data);
 196
 197         st_lsm6dsx_set_page(hw, false);
 198 out:
 199         mutex_unlock(&hw->page_lock);
 200
 201         return err;
 202 }
 203
 204 /**
 205  * st_lsm6dsx_shub_read - read data from slave device register
 206  *
 207  * Read data from slave device register. SLV0 is used for
 208  * one-shot read operation
 209  */
 210 static int
 211 st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 212                      u8 *data, int len)
 213 {
 214         const struct st_lsm6dsx_shub_settings *hub_settings;
 215         struct st_lsm6dsx_hw *hw = sensor->hw;
 216         u8 config[3], slv_addr;
 217         int err;
 218
 219         hub_settings = &hw->settings->shub_settings;
 220         slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
 221
 222         config[0] = (sensor->ext_info.addr << 1) | 1;
 223         config[1] = addr;
 224         config[2] = len & ST_LS6DSX_READ_OP_MASK;
 225
 226         err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 227                                         sizeof(config));
 228         if (err < 0)
 229                 return err;
 230
 231         err = st_lsm6dsx_shub_master_enable(sensor, true);
 232         if (err < 0)
 233                 return err;
 234
 235         st_lsm6dsx_shub_wait_complete(hw);
 236
 237         err = st_lsm6dsx_shub_read_reg(hw, hub_settings->shub_out, data,
 238                                        len & ST_LS6DSX_READ_OP_MASK);
 239
 240         st_lsm6dsx_shub_master_enable(sensor, false);
 241
 242         memset(config, 0, sizeof(config));
 243         return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 244                                          sizeof(config));
 245 }
 246
 247 /**
 248  * st_lsm6dsx_shub_write - write data to slave device register
 249  *
 250  * Write data from slave device register. SLV0 is used for
 251  * one-shot write operation
 252  */
 253 static int
 254 st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
 255                       u8 *data, int len)
 256 {
 257         const struct st_lsm6dsx_shub_settings *hub_settings;
 258         struct st_lsm6dsx_hw *hw = sensor->hw;
 259         u8 config[2], slv_addr;
 260         int err, i;
 261
 262         hub_settings = &hw->settings->shub_settings;
 263         if (hub_settings->wr_once.addr) {
 264                 unsigned int data;
 265
 266                 data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask);
 267                 err = st_lsm6dsx_shub_write_reg_with_mask(hw,
 268                         hub_settings->wr_once.addr,
 269                         hub_settings->wr_once.mask,
 270                         data);
 271                 if (err < 0)
 272                         return err;
 273         }
 274
 275         slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
 276         config[0] = sensor->ext_info.addr << 1;
 277         for (i = 0 ; i < len; i++) {
 278                 config[1] = addr + i;
 279
 280                 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 281                                                 sizeof(config));
 282                 if (err < 0)
 283                         return err;
 284
 285                 err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr,
 286                                                 &data[i], 1);
 287                 if (err < 0)
 288                         return err;
 289
 290                 err = st_lsm6dsx_shub_master_enable(sensor, true);
 291                 if (err < 0)
 292                         return err;
 293
 294                 st_lsm6dsx_shub_wait_complete(hw);
 295
 296                 st_lsm6dsx_shub_master_enable(sensor, false);
 297         }
 298
 299         memset(config, 0, sizeof(config));
 300         return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
 301 }
 302
 303 static int
 304 st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor *sensor,
 305                                 u8 addr, u8 mask, u8 val)
 306 {
 307         int err;
 308         u8 data;
 309
 310         err = st_lsm6dsx_shub_read(sensor, addr, &data, sizeof(data));
 311         if (err < 0)
 312                 return err;
 313
 314         data = ((data & ~mask) | (val << __ffs(mask) & mask));
 315
 316         return st_lsm6dsx_shub_write(sensor, addr, &data, sizeof(data));
 317 }
 318
 319 static int
 320 st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor *sensor,
 321                             u32 odr, u16 *val)
 322 {
 323         const struct st_lsm6dsx_ext_dev_settings *settings;
 324         int i;
 325
 326         settings = sensor->ext_info.settings;
 327         for (i = 0; i < settings->odr_table.odr_len; i++) {
 328                 if (settings->odr_table.odr_avl[i].milli_hz == odr)
 329                         break;
 330         }
 331
 332         if (i == settings->odr_table.odr_len)
 333                 return -EINVAL;
 334
 335         *val = settings->odr_table.odr_avl[i].val;
 336         return 0;
 337 }
 338
 339 static int
 340 st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u32 odr)
 341 {
 342         const struct st_lsm6dsx_ext_dev_settings *settings;
 343         u16 val;
 344         int err;
 345
 346         err = st_lsm6dsx_shub_get_odr_val(sensor, odr, &val);
 347         if (err < 0)
 348                 return err;
 349
 350         settings = sensor->ext_info.settings;
 351         return st_lsm6dsx_shub_write_with_mask(sensor,
 352                                                settings->odr_table.reg.addr,
 353                                                settings->odr_table.reg.mask,
 354                                                val);
 355 }
 356
 357 /* use SLV{1,2,3} for FIFO read operations */
 358 static int
 359 st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor,
 360                                 bool enable)
 361 {
 362         const struct st_lsm6dsx_shub_settings *hub_settings;
 363         const struct st_lsm6dsx_ext_dev_settings *settings;
 364         u8 config[9] = {}, enable_mask, slv_addr;
 365         struct st_lsm6dsx_hw *hw = sensor->hw;
 366         struct st_lsm6dsx_sensor *cur_sensor;
 367         int i, j = 0;
 368
 369         hub_settings = &hw->settings->shub_settings;
 370         if (enable)
 371                 enable_mask = hw->enable_mask | BIT(sensor->id);
 372         else
 373                 enable_mask = hw->enable_mask & ~BIT(sensor->id);
 374
 375         for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) {
 376                 if (!hw->iio_devs[i])
 377                         continue;
 378
 379                 cur_sensor = iio_priv(hw->iio_devs[i]);
 380                 if (!(enable_mask & BIT(cur_sensor->id)))
 381                         continue;
 382
 383                 settings = cur_sensor->ext_info.settings;
 384                 config[j] = (sensor->ext_info.addr << 1) | 1;
 385                 config[j + 1] = settings->out.addr;
 386                 config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) |
 387                                 hub_settings->batch_en;
 388                 j += 3;
 389         }
 390
 391         slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr);
 392         return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 393                                          sizeof(config));
 394 }
 395
 396 int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
 397 {
 398         const struct st_lsm6dsx_ext_dev_settings *settings;
 399         int err;
 400
 401         err = st_lsm6dsx_shub_config_channels(sensor, enable);
 402         if (err < 0)
 403                 return err;
 404
 405         settings = sensor->ext_info.settings;
 406         if (enable) {
 407                 err = st_lsm6dsx_shub_set_odr(sensor, sensor->odr);
 408                 if (err < 0)
 409                         return err;
 410         } else {
 411                 err = st_lsm6dsx_shub_write_with_mask(sensor,
 412                                         settings->odr_table.reg.addr,
 413                                         settings->odr_table.reg.mask, 0);
 414                 if (err < 0)
 415                         return err;
 416         }
 417
 418         if (settings->pwr_table.reg.addr) {
 419                 u8 val;
 420
 421                 val = enable ? settings->pwr_table.on_val
 422                              : settings->pwr_table.off_val;
 423                 err = st_lsm6dsx_shub_write_with_mask(sensor,
 424                                         settings->pwr_table.reg.addr,
 425                                         settings->pwr_table.reg.mask, val);
 426                 if (err < 0)
 427                         return err;
 428         }
 429
 430         return st_lsm6dsx_shub_master_enable(sensor, enable);
 431 }
 432
 433 static int
 434 st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor,
 435                              struct iio_chan_spec const *ch,
 436                              int *val)
 437 {
 438         int err, delay, len;
 439         u8 data[4];
 440
 441         err = st_lsm6dsx_shub_set_enable(sensor, true);
 442         if (err < 0)
 443                 return err;
 444
 445         delay = 1000000000 / sensor->odr;
 446         usleep_range(delay, 2 * delay);
 447
 448         len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3);
 449         err = st_lsm6dsx_shub_read(sensor, ch->address, data, len);
 450
 451         st_lsm6dsx_shub_set_enable(sensor, false);
 452
 453         if (err < 0)
 454                 return err;
 455
 456         switch (len) {
 457         case 2:
 458                 *val = (s16)le16_to_cpu(*((__le16 *)data));
 459                 break;
 460         default:
 461                 return -EINVAL;
 462         }
 463
 464         return IIO_VAL_INT;
 465 }
 466
 467 static int
 468 st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev,
 469                          struct iio_chan_spec const *ch,
 470                          int *val, int *val2, long mask)
 471 {
 472         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 473         int ret;
 474
 475         switch (mask) {
 476         case IIO_CHAN_INFO_RAW:
 477                 ret = iio_device_claim_direct_mode(iio_dev);
 478                 if (ret)
 479                         break;
 480
 481                 ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val);
 482                 iio_device_release_direct_mode(iio_dev);
 483                 break;
 484         case IIO_CHAN_INFO_SAMP_FREQ:
 485                 *val = sensor->odr / 1000;
 486                 *val2 = (sensor->odr % 1000) * 1000;
 487                 ret = IIO_VAL_INT_PLUS_MICRO;
 488                 break;
 489         case IIO_CHAN_INFO_SCALE:
 490                 *val = 0;
 491                 *val2 = sensor->gain;
 492                 ret = IIO_VAL_INT_PLUS_MICRO;
 493                 break;
 494         default:
 495                 ret = -EINVAL;
 496                 break;
 497         }
 498
 499         return ret;
 500 }
 501
 502 static int
 503 st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
 504                           struct iio_chan_spec const *chan,
 505                           int val, int val2, long mask)
 506 {
 507         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 508         int err;
 509
 510         err = iio_device_claim_direct_mode(iio_dev);
 511         if (err)
 512                 return err;
 513
 514         switch (mask) {
 515         case IIO_CHAN_INFO_SAMP_FREQ: {
 516                 u16 data;
 517
 518                 val = val * 1000 + val2 / 1000;
 519                 err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data);
 520                 if (!err)
 521                         sensor->odr = val;
 522                 break;
 523         }
 524         default:
 525                 err = -EINVAL;
 526                 break;
 527         }
 528
 529         iio_device_release_direct_mode(iio_dev);
 530
 531         return err;
 532 }
 533
 534 static ssize_t
 535 st_lsm6dsx_shub_sampling_freq_avail(struct device *dev,
 536                                     struct device_attribute *attr,
 537                                     char *buf)
 538 {
 539         struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
 540         const struct st_lsm6dsx_ext_dev_settings *settings;
 541         int i, len = 0;
 542
 543         settings = sensor->ext_info.settings;
 544         for (i = 0; i < settings->odr_table.odr_len; i++) {
 545                 u32 val = settings->odr_table.odr_avl[i].milli_hz;
 546
 547                 len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ",
 548                                  val / 1000, val % 1000);
 549         }
 550         buf[len - 1] = '\n';
 551
 552         return len;
 553 }
 554
 555 static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev,
 556                                            struct device_attribute *attr,
 557                                            char *buf)
 558 {
 559         struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
 560         const struct st_lsm6dsx_ext_dev_settings *settings;
 561         int i, len = 0;
 562
 563         settings = sensor->ext_info.settings;
 564         for (i = 0; i < settings->fs_table.fs_len; i++)
 565                 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
 566                                  settings->fs_table.fs_avl[i].gain);
 567         buf[len - 1] = '\n';
 568
 569         return len;
 570 }
 571
 572 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail);
 573 static IIO_DEVICE_ATTR(in_scale_available, 0444,
 574                        st_lsm6dsx_shub_scale_avail, NULL, 0);
 575 static struct attribute *st_lsm6dsx_ext_attributes[] = {
 576         &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 577         &iio_dev_attr_in_scale_available.dev_attr.attr,
 578         NULL,
 579 };
 580
 581 static const struct attribute_group st_lsm6dsx_ext_attribute_group = {
 582         .attrs = st_lsm6dsx_ext_attributes,
 583 };
 584
 585 static const struct iio_info st_lsm6dsx_ext_info = {
 586         .attrs = &st_lsm6dsx_ext_attribute_group,
 587         .read_raw = st_lsm6dsx_shub_read_raw,
 588         .write_raw = st_lsm6dsx_shub_write_raw,
 589         .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
 590 };
 591
 592 static struct iio_dev *
 593 st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw,
 594                              enum st_lsm6dsx_sensor_id id,
 595                              const struct st_lsm6dsx_ext_dev_settings *info,
 596                              u8 i2c_addr, const char *name)
 597 {
 598         struct iio_chan_spec *ext_channels;
 599         struct st_lsm6dsx_sensor *sensor;
 600         struct iio_dev *iio_dev;
 601
 602         iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
 603         if (!iio_dev)
 604                 return NULL;
 605
 606         iio_dev->modes = INDIO_DIRECT_MODE;
 607         iio_dev->dev.parent = hw->dev;
 608         iio_dev->info = &st_lsm6dsx_ext_info;
 609
 610         sensor = iio_priv(iio_dev);
 611         sensor->id = id;
 612         sensor->hw = hw;
 613         sensor->odr = info->odr_table.odr_avl[0].milli_hz;
 614         sensor->gain = info->fs_table.fs_avl[0].gain;
 615         sensor->ext_info.settings = info;
 616         sensor->ext_info.addr = i2c_addr;
 617         sensor->watermark = 1;
 618
 619         switch (info->id) {
 620         case ST_LSM6DSX_ID_MAGN: {
 621                 const struct iio_chan_spec magn_channels[] = {
 622                         ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr,
 623                                            IIO_MOD_X, 0),
 624                         ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2,
 625                                            IIO_MOD_Y, 1),
 626                         ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4,
 627                                            IIO_MOD_Z, 2),
 628                         IIO_CHAN_SOFT_TIMESTAMP(3),
 629                 };
 630
 631                 ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels),
 632                                             GFP_KERNEL);
 633                 if (!ext_channels)
 634                         return NULL;
 635
 636                 memcpy(ext_channels, magn_channels, sizeof(magn_channels));
 637                 iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
 638                 iio_dev->channels = ext_channels;
 639                 iio_dev->num_channels = ARRAY_SIZE(magn_channels);
 640
 641                 scnprintf(sensor->name, sizeof(sensor->name), "%s_magn",
 642                           name);
 643                 break;
 644         }
 645         default:
 646                 return NULL;
 647         }
 648         iio_dev->name = sensor->name;
 649
 650         return iio_dev;
 651 }
 652
 653 static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor)
 654 {
 655         const struct st_lsm6dsx_ext_dev_settings *settings;
 656         int err;
 657
 658         settings = sensor->ext_info.settings;
 659         if (settings->bdu.addr) {
 660                 err = st_lsm6dsx_shub_write_with_mask(sensor,
 661                                                       settings->bdu.addr,
 662                                                       settings->bdu.mask, 1);
 663                 if (err < 0)
 664                         return err;
 665         }
 666
 667         if (settings->temp_comp.addr) {
 668                 err = st_lsm6dsx_shub_write_with_mask(sensor,
 669                                         settings->temp_comp.addr,
 670                                         settings->temp_comp.mask, 1);
 671                 if (err < 0)
 672                         return err;
 673         }
 674
 675         if (settings->off_canc.addr) {
 676                 err = st_lsm6dsx_shub_write_with_mask(sensor,
 677                                         settings->off_canc.addr,
 678                                         settings->off_canc.mask, 1);
 679                 if (err < 0)
 680                         return err;
 681         }
 682
 683         return 0;
 684 }
 685
 686 static int
 687 st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 688                           const struct st_lsm6dsx_ext_dev_settings *settings)
 689 {
 690         const struct st_lsm6dsx_shub_settings *hub_settings;
 691         struct st_lsm6dsx_sensor *sensor;
 692         u8 config[3], data, slv_addr;
 693         bool found = false;
 694         int i, err;
 695
 696         hub_settings = &hw->settings->shub_settings;
 697         slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
 698         sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 699
 700         for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
 701                 if (!settings->i2c_addr[i])
 702                         continue;
 703
 704                 /* read wai slave register */
 705                 config[0] = (settings->i2c_addr[i] << 1) | 0x1;
 706                 config[1] = settings->wai.addr;
 707                 config[2] = 0x1;
 708
 709                 err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 710                                                 sizeof(config));
 711                 if (err < 0)
 712                         return err;
 713
 714                 err = st_lsm6dsx_shub_master_enable(sensor, true);
 715                 if (err < 0)
 716                         return err;
 717
 718                 st_lsm6dsx_shub_wait_complete(hw);
 719
 720                 err = st_lsm6dsx_shub_read_reg(hw,
 721                                                hub_settings->shub_out,
 722                                                &data, sizeof(data));
 723
 724                 st_lsm6dsx_shub_master_enable(sensor, false);
 725
 726                 if (err < 0)
 727                         return err;
 728
 729                 if (data != settings->wai.val)
 730                         continue;
 731
 732                 *i2c_addr = settings->i2c_addr[i];
 733                 found = true;
 734                 break;
 735         }
 736
 737         /* reset SLV0 channel */
 738         memset(config, 0, sizeof(config));
 739         err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 740                                         sizeof(config));
 741         if (err < 0)
 742                 return err;
 743
 744         return found ? 0 : -ENODEV;
 745 }
 746
 747 int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name)
 748 {
 749         enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0;
 750         struct st_lsm6dsx_sensor *sensor;
 751         int err, i, num_ext_dev = 0;
 752         u8 i2c_addr = 0;
 753
 754         for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) {
 755                 err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr,
 756                                         &st_lsm6dsx_ext_dev_table[i]);
 757                 if (err == -ENODEV)
 758                         continue;
 759                 else if (err < 0)
 760                         return err;
 761
 762                 hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id,
 763                                                 &st_lsm6dsx_ext_dev_table[i],
 764                                                 i2c_addr, name);
 765                 if (!hw->iio_devs[id])
 766                         return -ENOMEM;
 767
 768                 sensor = iio_priv(hw->iio_devs[id]);
 769                 err = st_lsm6dsx_shub_init_device(sensor);
 770                 if (err < 0)
 771                         return err;
 772
 773                 if (++num_ext_dev >= ST_LSM6DSX_MAX_SLV_NUM)
 774                         break;
 775                 id++;
 776         }
 777
 778         return 0;
 779 }