drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c

   1 /*
   2  * STMicroelectronics st_lsm6dsx sensor driver
   3  *
   4  * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
   5  * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
   6  * interface standard output.
   7  * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
   8  * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
   9  * +-125/+-245/+-500/+-1000/+-2000 dps
  10  * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
  11  * allowing dynamic batching of sensor data.
  12  *
  13  * Supported sensors:
  14  * - LSM6DS3:
  15  *   - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
  16  *   - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
  17  *   - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
  18  *   - FIFO size: 8KB
  19  *
  20  * - LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC:
  21  *   - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
  22  *   - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
  23  *   - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
  24  *   - FIFO size: 4KB
  25  *
  26  * - LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR
  27  *   - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
  28  *   - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
  29  *   - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
  30  *   - FIFO size: 3KB
  31  *
  32  * Copyright 2016 STMicroelectronics Inc.
  33  *
  34  * Lorenzo Bianconi <lorenzo.bianconi@st.com>
  35  * Denis Ciocca <denis.ciocca@st.com>
  36  *
  37  * Licensed under the GPL-2.
  38  */
  39
  40 #include <linux/kernel.h>
  41 #include <linux/module.h>
  42 #include <linux/delay.h>
  43 #include <linux/iio/iio.h>
  44 #include <linux/iio/sysfs.h>
  45 #include <linux/pm.h>
  46 #include <linux/regmap.h>
  47 #include <linux/bitfield.h>
  48
  49 #include <linux/platform_data/st_sensors_pdata.h>
  50
  51 #include "st_lsm6dsx.h"
  52
  53 #define ST_LSM6DSX_REG_INT1_ADDR                0x0d
  54 #define ST_LSM6DSX_REG_INT2_ADDR                0x0e
  55 #define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK        BIT(3)
  56 #define ST_LSM6DSX_REG_WHOAMI_ADDR              0x0f
  57 #define ST_LSM6DSX_REG_RESET_ADDR               0x12
  58 #define ST_LSM6DSX_REG_RESET_MASK               BIT(0)
  59 #define ST_LSM6DSX_REG_BOOT_MASK                BIT(7)
  60 #define ST_LSM6DSX_REG_BDU_ADDR                 0x12
  61 #define ST_LSM6DSX_REG_BDU_MASK                 BIT(6)
  62 #define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR        0x13
  63 #define ST_LSM6DSX_REG_INT2_ON_INT1_MASK        BIT(5)
  64
  65 #define ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR         0x28
  66 #define ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR         0x2a
  67 #define ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR         0x2c
  68
  69 #define ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR        0x22
  70 #define ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR        0x24
  71 #define ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR        0x26
  72
  73 static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
  74         [ST_LSM6DSX_ID_ACC] = {
  75                 .reg = {
  76                         .addr = 0x10,
  77                         .mask = GENMASK(7, 4),
  78                 },
  79                 .odr_avl[0] = {  13, 0x01 },
  80                 .odr_avl[1] = {  26, 0x02 },
  81                 .odr_avl[2] = {  52, 0x03 },
  82                 .odr_avl[3] = { 104, 0x04 },
  83                 .odr_avl[4] = { 208, 0x05 },
  84                 .odr_avl[5] = { 416, 0x06 },
  85         },
  86         [ST_LSM6DSX_ID_GYRO] = {
  87                 .reg = {
  88                         .addr = 0x11,
  89                         .mask = GENMASK(7, 4),
  90                 },
  91                 .odr_avl[0] = {  13, 0x01 },
  92                 .odr_avl[1] = {  26, 0x02 },
  93                 .odr_avl[2] = {  52, 0x03 },
  94                 .odr_avl[3] = { 104, 0x04 },
  95                 .odr_avl[4] = { 208, 0x05 },
  96                 .odr_avl[5] = { 416, 0x06 },
  97         }
  98 };
  99
 100 static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = {
 101         [ST_LSM6DSX_ID_ACC] = {
 102                 .reg = {
 103                         .addr = 0x10,
 104                         .mask = GENMASK(3, 2),
 105                 },
 106                 .fs_avl[0] = {  IIO_G_TO_M_S_2(61), 0x0 },
 107                 .fs_avl[1] = { IIO_G_TO_M_S_2(122), 0x2 },
 108                 .fs_avl[2] = { IIO_G_TO_M_S_2(244), 0x3 },
 109                 .fs_avl[3] = { IIO_G_TO_M_S_2(488), 0x1 },
 110         },
 111         [ST_LSM6DSX_ID_GYRO] = {
 112                 .reg = {
 113                         .addr = 0x11,
 114                         .mask = GENMASK(3, 2),
 115                 },
 116                 .fs_avl[0] = {  IIO_DEGREE_TO_RAD(8750), 0x0 },
 117                 .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500), 0x1 },
 118                 .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000), 0x2 },
 119                 .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000), 0x3 },
 120         }
 121 };
 122
 123 static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 124         {
 125                 .wai = 0x69,
 126                 .max_fifo_size = 1365,
 127                 .id = {
 128                         [0] = ST_LSM6DS3_ID,
 129                 },
 130                 .decimator = {
 131                         [ST_LSM6DSX_ID_ACC] = {
 132                                 .addr = 0x08,
 133                                 .mask = GENMASK(2, 0),
 134                         },
 135                         [ST_LSM6DSX_ID_GYRO] = {
 136                                 .addr = 0x08,
 137                                 .mask = GENMASK(5, 3),
 138                         },
 139                 },
 140                 .fifo_ops = {
 141                         .read_fifo = st_lsm6dsx_read_fifo,
 142                         .fifo_th = {
 143                                 .addr = 0x06,
 144                                 .mask = GENMASK(11, 0),
 145                         },
 146                         .fifo_diff = {
 147                                 .addr = 0x3a,
 148                                 .mask = GENMASK(11, 0),
 149                         },
 150                         .th_wl = 3, /* 1LSB = 2B */
 151                 },
 152                 .ts_settings = {
 153                         .timer_en = {
 154                                 .addr = 0x58,
 155                                 .mask = BIT(7),
 156                         },
 157                         .hr_timer = {
 158                                 .addr = 0x5c,
 159                                 .mask = BIT(4),
 160                         },
 161                         .fifo_en = {
 162                                 .addr = 0x07,
 163                                 .mask = BIT(7),
 164                         },
 165                         .decimator = {
 166                                 .addr = 0x09,
 167                                 .mask = GENMASK(5, 3),
 168                         },
 169                 },
 170         },
 171         {
 172                 .wai = 0x69,
 173                 .max_fifo_size = 682,
 174                 .id = {
 175                         [0] = ST_LSM6DS3H_ID,
 176                 },
 177                 .decimator = {
 178                         [ST_LSM6DSX_ID_ACC] = {
 179                                 .addr = 0x08,
 180                                 .mask = GENMASK(2, 0),
 181                         },
 182                         [ST_LSM6DSX_ID_GYRO] = {
 183                                 .addr = 0x08,
 184                                 .mask = GENMASK(5, 3),
 185                         },
 186                 },
 187                 .fifo_ops = {
 188                         .read_fifo = st_lsm6dsx_read_fifo,
 189                         .fifo_th = {
 190                                 .addr = 0x06,
 191                                 .mask = GENMASK(11, 0),
 192                         },
 193                         .fifo_diff = {
 194                                 .addr = 0x3a,
 195                                 .mask = GENMASK(11, 0),
 196                         },
 197                         .th_wl = 3, /* 1LSB = 2B */
 198                 },
 199                 .ts_settings = {
 200                         .timer_en = {
 201                                 .addr = 0x58,
 202                                 .mask = BIT(7),
 203                         },
 204                         .hr_timer = {
 205                                 .addr = 0x5c,
 206                                 .mask = BIT(4),
 207                         },
 208                         .fifo_en = {
 209                                 .addr = 0x07,
 210                                 .mask = BIT(7),
 211                         },
 212                         .decimator = {
 213                                 .addr = 0x09,
 214                                 .mask = GENMASK(5, 3),
 215                         },
 216                 },
 217         },
 218         {
 219                 .wai = 0x6a,
 220                 .max_fifo_size = 682,
 221                 .id = {
 222                         [0] = ST_LSM6DSL_ID,
 223                         [1] = ST_LSM6DSM_ID,
 224                         [2] = ST_ISM330DLC_ID,
 225                 },
 226                 .decimator = {
 227                         [ST_LSM6DSX_ID_ACC] = {
 228                                 .addr = 0x08,
 229                                 .mask = GENMASK(2, 0),
 230                         },
 231                         [ST_LSM6DSX_ID_GYRO] = {
 232                                 .addr = 0x08,
 233                                 .mask = GENMASK(5, 3),
 234                         },
 235                 },
 236                 .fifo_ops = {
 237                         .read_fifo = st_lsm6dsx_read_fifo,
 238                         .fifo_th = {
 239                                 .addr = 0x06,
 240                                 .mask = GENMASK(10, 0),
 241                         },
 242                         .fifo_diff = {
 243                                 .addr = 0x3a,
 244                                 .mask = GENMASK(10, 0),
 245                         },
 246                         .th_wl = 3, /* 1LSB = 2B */
 247                 },
 248                 .ts_settings = {
 249                         .timer_en = {
 250                                 .addr = 0x19,
 251                                 .mask = BIT(5),
 252                         },
 253                         .hr_timer = {
 254                                 .addr = 0x5c,
 255                                 .mask = BIT(4),
 256                         },
 257                         .fifo_en = {
 258                                 .addr = 0x07,
 259                                 .mask = BIT(7),
 260                         },
 261                         .decimator = {
 262                                 .addr = 0x09,
 263                                 .mask = GENMASK(5, 3),
 264                         },
 265                 },
 266         },
 267         {
 268                 .wai = 0x6c,
 269                 .max_fifo_size = 512,
 270                 .id = {
 271                         [0] = ST_LSM6DSO_ID,
 272                         [1] = ST_LSM6DSOX_ID,
 273                 },
 274                 .batch = {
 275                         [ST_LSM6DSX_ID_ACC] = {
 276                                 .addr = 0x09,
 277                                 .mask = GENMASK(3, 0),
 278                         },
 279                         [ST_LSM6DSX_ID_GYRO] = {
 280                                 .addr = 0x09,
 281                                 .mask = GENMASK(7, 4),
 282                         },
 283                 },
 284                 .fifo_ops = {
 285                         .read_fifo = st_lsm6dsx_read_tagged_fifo,
 286                         .fifo_th = {
 287                                 .addr = 0x07,
 288                                 .mask = GENMASK(8, 0),
 289                         },
 290                         .fifo_diff = {
 291                                 .addr = 0x3a,
 292                                 .mask = GENMASK(8, 0),
 293                         },
 294                         .th_wl = 1,
 295                 },
 296                 .ts_settings = {
 297                         .timer_en = {
 298                                 .addr = 0x19,
 299                                 .mask = BIT(5),
 300                         },
 301                         .decimator = {
 302                                 .addr = 0x0a,
 303                                 .mask = GENMASK(7, 6),
 304                         },
 305                 },
 306                 .shub_settings = {
 307                         .page_mux = {
 308                                 .addr = 0x01,
 309                                 .mask = BIT(6),
 310                         },
 311                         .master_en = {
 312                                 .addr = 0x14,
 313                                 .mask = BIT(2),
 314                         },
 315                         .pullup_en = {
 316                                 .addr = 0x14,
 317                                 .mask = BIT(3),
 318                         },
 319                         .aux_sens = {
 320                                 .addr = 0x14,
 321                                 .mask = GENMASK(1, 0),
 322                         },
 323                         .wr_once = {
 324                                 .addr = 0x14,
 325                                 .mask = BIT(6),
 326                         },
 327                         .shub_out = 0x02,
 328                         .slv0_addr = 0x15,
 329                         .dw_slv0_addr = 0x21,
 330                         .batch_en = BIT(3),
 331                 }
 332         },
 333         {
 334                 .wai = 0x6b,
 335                 .max_fifo_size = 512,
 336                 .id = {
 337                         [0] = ST_ASM330LHH_ID,
 338                 },
 339                 .batch = {
 340                         [ST_LSM6DSX_ID_ACC] = {
 341                                 .addr = 0x09,
 342                                 .mask = GENMASK(3, 0),
 343                         },
 344                         [ST_LSM6DSX_ID_GYRO] = {
 345                                 .addr = 0x09,
 346                                 .mask = GENMASK(7, 4),
 347                         },
 348                 },
 349                 .fifo_ops = {
 350                         .read_fifo = st_lsm6dsx_read_tagged_fifo,
 351                         .fifo_th = {
 352                                 .addr = 0x07,
 353                                 .mask = GENMASK(8, 0),
 354                         },
 355                         .fifo_diff = {
 356                                 .addr = 0x3a,
 357                                 .mask = GENMASK(8, 0),
 358                         },
 359                         .th_wl = 1,
 360                 },
 361                 .ts_settings = {
 362                         .timer_en = {
 363                                 .addr = 0x19,
 364                                 .mask = BIT(5),
 365                         },
 366                         .decimator = {
 367                                 .addr = 0x0a,
 368                                 .mask = GENMASK(7, 6),
 369                         },
 370                 },
 371         },
 372         {
 373                 .wai = 0x6b,
 374                 .max_fifo_size = 512,
 375                 .id = {
 376                         [0] = ST_LSM6DSR_ID,
 377                 },
 378                 .batch = {
 379                         [ST_LSM6DSX_ID_ACC] = {
 380                                 .addr = 0x09,
 381                                 .mask = GENMASK(3, 0),
 382                         },
 383                         [ST_LSM6DSX_ID_GYRO] = {
 384                                 .addr = 0x09,
 385                                 .mask = GENMASK(7, 4),
 386                         },
 387                 },
 388                 .fifo_ops = {
 389                         .read_fifo = st_lsm6dsx_read_tagged_fifo,
 390                         .fifo_th = {
 391                                 .addr = 0x07,
 392                                 .mask = GENMASK(8, 0),
 393                         },
 394                         .fifo_diff = {
 395                                 .addr = 0x3a,
 396                                 .mask = GENMASK(8, 0),
 397                         },
 398                         .th_wl = 1,
 399                 },
 400                 .ts_settings = {
 401                         .timer_en = {
 402                                 .addr = 0x19,
 403                                 .mask = BIT(5),
 404                         },
 405                         .decimator = {
 406                                 .addr = 0x0a,
 407                                 .mask = GENMASK(7, 6),
 408                         },
 409                 },
 410                 .shub_settings = {
 411                         .page_mux = {
 412                                 .addr = 0x01,
 413                                 .mask = BIT(6),
 414                         },
 415                         .master_en = {
 416                                 .addr = 0x14,
 417                                 .mask = BIT(2),
 418                         },
 419                         .pullup_en = {
 420                                 .addr = 0x14,
 421                                 .mask = BIT(3),
 422                         },
 423                         .aux_sens = {
 424                                 .addr = 0x14,
 425                                 .mask = GENMASK(1, 0),
 426                         },
 427                         .wr_once = {
 428                                 .addr = 0x14,
 429                                 .mask = BIT(6),
 430                         },
 431                         .shub_out = 0x02,
 432                         .slv0_addr = 0x15,
 433                         .dw_slv0_addr = 0x21,
 434                         .batch_en = BIT(3),
 435                 }
 436         },
 437 };
 438
 439 static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
 440         ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
 441                            IIO_MOD_X, 0),
 442         ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
 443                            IIO_MOD_Y, 1),
 444         ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
 445                            IIO_MOD_Z, 2),
 446         IIO_CHAN_SOFT_TIMESTAMP(3),
 447 };
 448
 449 static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
 450         ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
 451                            IIO_MOD_X, 0),
 452         ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
 453                            IIO_MOD_Y, 1),
 454         ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
 455                            IIO_MOD_Z, 2),
 456         IIO_CHAN_SOFT_TIMESTAMP(3),
 457 };
 458
 459 int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable)
 460 {
 461         const struct st_lsm6dsx_shub_settings *hub_settings;
 462         unsigned int data;
 463         int err;
 464
 465         hub_settings = &hw->settings->shub_settings;
 466         data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->page_mux.mask);
 467         err = regmap_update_bits(hw->regmap, hub_settings->page_mux.addr,
 468                                  hub_settings->page_mux.mask, data);
 469         usleep_range(100, 150);
 470
 471         return err;
 472 }
 473
 474 static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
 475 {
 476         int err, i, j, data;
 477
 478         for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
 479                 for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) {
 480                         if (id == st_lsm6dsx_sensor_settings[i].id[j])
 481                                 break;
 482                 }
 483                 if (j < ST_LSM6DSX_MAX_ID)
 484                         break;
 485         }
 486
 487         if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
 488                 dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
 489                 return -ENODEV;
 490         }
 491
 492         err = regmap_read(hw->regmap, ST_LSM6DSX_REG_WHOAMI_ADDR, &data);
 493         if (err < 0) {
 494                 dev_err(hw->dev, "failed to read whoami register\n");
 495                 return err;
 496         }
 497
 498         if (data != st_lsm6dsx_sensor_settings[i].wai) {
 499                 dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
 500                 return -ENODEV;
 501         }
 502
 503         hw->settings = &st_lsm6dsx_sensor_settings[i];
 504
 505         return 0;
 506 }
 507
 508 static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
 509                                      u32 gain)
 510 {
 511         struct st_lsm6dsx_hw *hw = sensor->hw;
 512         const struct st_lsm6dsx_reg *reg;
 513         unsigned int data;
 514         int i, err;
 515         u8 val;
 516
 517         for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
 518                 if (st_lsm6dsx_fs_table[sensor->id].fs_avl[i].gain == gain)
 519                         break;
 520
 521         if (i == ST_LSM6DSX_FS_LIST_SIZE)
 522                 return -EINVAL;
 523
 524         val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val;
 525         reg = &st_lsm6dsx_fs_table[sensor->id].reg;
 526         data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);
 527         err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);
 528         if (err < 0)
 529                 return err;
 530
 531         sensor->gain = gain;
 532
 533         return 0;
 534 }
 535
 536 int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val)
 537 {
 538         int i;
 539
 540         for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
 541                 /*
 542                  * ext devices can run at different odr respect to
 543                  * accel sensor
 544                  */
 545                 if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz >= odr)
 546                         break;
 547
 548         if (i == ST_LSM6DSX_ODR_LIST_SIZE)
 549                 return -EINVAL;
 550
 551         *val = st_lsm6dsx_odr_table[sensor->id].odr_avl[i].val;
 552
 553         return 0;
 554 }
 555
 556 static u16 st_lsm6dsx_check_odr_dependency(struct st_lsm6dsx_hw *hw, u16 odr,
 557                                            enum st_lsm6dsx_sensor_id id)
 558 {
 559         struct st_lsm6dsx_sensor *ref = iio_priv(hw->iio_devs[id]);
 560
 561         if (odr > 0) {
 562                 if (hw->enable_mask & BIT(id))
 563                         return max_t(u16, ref->odr, odr);
 564                 else
 565                         return odr;
 566         } else {
 567                 return (hw->enable_mask & BIT(id)) ? ref->odr : 0;
 568         }
 569 }
 570
 571 static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 req_odr)
 572 {
 573         struct st_lsm6dsx_sensor *ref_sensor = sensor;
 574         struct st_lsm6dsx_hw *hw = sensor->hw;
 575         const struct st_lsm6dsx_reg *reg;
 576         unsigned int data;
 577         u8 val = 0;
 578         int err;
 579
 580         switch (sensor->id) {
 581         case ST_LSM6DSX_ID_EXT0:
 582         case ST_LSM6DSX_ID_EXT1:
 583         case ST_LSM6DSX_ID_EXT2:
 584         case ST_LSM6DSX_ID_ACC: {
 585                 u16 odr;
 586                 int i;
 587
 588                 /*
 589                  * i2c embedded controller relies on the accelerometer sensor as
 590                  * bus read/write trigger so we need to enable accel device
 591                  * at odr = max(accel_odr, ext_odr) in order to properly
 592                  * communicate with i2c slave devices
 593                  */
 594                 ref_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 595                 for (i = ST_LSM6DSX_ID_ACC; i < ST_LSM6DSX_ID_MAX; i++) {
 596                         if (!hw->iio_devs[i] || i == sensor->id)
 597                                 continue;
 598
 599                         odr = st_lsm6dsx_check_odr_dependency(hw, req_odr, i);
 600                         if (odr != req_odr)
 601                                 /* device already configured */
 602                                 return 0;
 603                 }
 604                 break;
 605         }
 606         default:
 607                 break;
 608         }
 609
 610         if (req_odr > 0) {
 611                 err = st_lsm6dsx_check_odr(ref_sensor, req_odr, &val);
 612                 if (err < 0)
 613                         return err;
 614         }
 615
 616         reg = &st_lsm6dsx_odr_table[ref_sensor->id].reg;
 617         data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);
 618         return st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);
 619 }
 620
 621 int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor,
 622                                  bool enable)
 623 {
 624         struct st_lsm6dsx_hw *hw = sensor->hw;
 625         u16 odr = enable ? sensor->odr : 0;
 626         int err;
 627
 628         err = st_lsm6dsx_set_odr(sensor, odr);
 629         if (err < 0)
 630                 return err;
 631
 632         if (enable)
 633                 hw->enable_mask |= BIT(sensor->id);
 634         else
 635                 hw->enable_mask &= ~BIT(sensor->id);
 636
 637         return 0;
 638 }
 639
 640 static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
 641                                    u8 addr, int *val)
 642 {
 643         struct st_lsm6dsx_hw *hw = sensor->hw;
 644         int err, delay;
 645         __le16 data;
 646
 647         err = st_lsm6dsx_sensor_set_enable(sensor, true);
 648         if (err < 0)
 649                 return err;
 650
 651         delay = 1000000 / sensor->odr;
 652         usleep_range(delay, 2 * delay);
 653
 654         err = st_lsm6dsx_read_locked(hw, addr, &data, sizeof(data));
 655         if (err < 0)
 656                 return err;
 657
 658         st_lsm6dsx_sensor_set_enable(sensor, false);
 659
 660         *val = (s16)le16_to_cpu(data);
 661
 662         return IIO_VAL_INT;
 663 }
 664
 665 static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev,
 666                                struct iio_chan_spec const *ch,
 667                                int *val, int *val2, long mask)
 668 {
 669         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 670         int ret;
 671
 672         switch (mask) {
 673         case IIO_CHAN_INFO_RAW:
 674                 ret = iio_device_claim_direct_mode(iio_dev);
 675                 if (ret)
 676                         break;
 677
 678                 ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
 679                 iio_device_release_direct_mode(iio_dev);
 680                 break;
 681         case IIO_CHAN_INFO_SAMP_FREQ:
 682                 *val = sensor->odr;
 683                 ret = IIO_VAL_INT;
 684                 break;
 685         case IIO_CHAN_INFO_SCALE:
 686                 *val = 0;
 687                 *val2 = sensor->gain;
 688                 ret = IIO_VAL_INT_PLUS_MICRO;
 689                 break;
 690         default:
 691                 ret = -EINVAL;
 692                 break;
 693         }
 694
 695         return ret;
 696 }
 697
 698 static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
 699                                 struct iio_chan_spec const *chan,
 700                                 int val, int val2, long mask)
 701 {
 702         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 703         int err;
 704
 705         err = iio_device_claim_direct_mode(iio_dev);
 706         if (err)
 707                 return err;
 708
 709         switch (mask) {
 710         case IIO_CHAN_INFO_SCALE:
 711                 err = st_lsm6dsx_set_full_scale(sensor, val2);
 712                 break;
 713         case IIO_CHAN_INFO_SAMP_FREQ: {
 714                 u8 data;
 715
 716                 err = st_lsm6dsx_check_odr(sensor, val, &data);
 717                 if (!err)
 718                         sensor->odr = val;
 719                 break;
 720         }
 721         default:
 722                 err = -EINVAL;
 723                 break;
 724         }
 725
 726         iio_device_release_direct_mode(iio_dev);
 727
 728         return err;
 729 }
 730
 731 int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
 732 {
 733         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 734         struct st_lsm6dsx_hw *hw = sensor->hw;
 735         int err;
 736
 737         if (val < 1 || val > hw->settings->max_fifo_size)
 738                 return -EINVAL;
 739
 740         mutex_lock(&hw->conf_lock);
 741
 742         err = st_lsm6dsx_update_watermark(sensor, val);
 743
 744         mutex_unlock(&hw->conf_lock);
 745
 746         if (err < 0)
 747                 return err;
 748
 749         sensor->watermark = val;
 750
 751         return 0;
 752 }
 753
 754 static ssize_t
 755 st_lsm6dsx_sysfs_sampling_frequency_avail(struct device *dev,
 756                                           struct device_attribute *attr,
 757                                           char *buf)
 758 {
 759         struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
 760         enum st_lsm6dsx_sensor_id id = sensor->id;
 761         int i, len = 0;
 762
 763         for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
 764                 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
 765                                  st_lsm6dsx_odr_table[id].odr_avl[i].hz);
 766         buf[len - 1] = '\n';
 767
 768         return len;
 769 }
 770
 771 static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *dev,
 772                                             struct device_attribute *attr,
 773                                             char *buf)
 774 {
 775         struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
 776         enum st_lsm6dsx_sensor_id id = sensor->id;
 777         int i, len = 0;
 778
 779         for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
 780                 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
 781                                  st_lsm6dsx_fs_table[id].fs_avl[i].gain);
 782         buf[len - 1] = '\n';
 783
 784         return len;
 785 }
 786
 787 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avail);
 788 static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
 789                        st_lsm6dsx_sysfs_scale_avail, NULL, 0);
 790 static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
 791                        st_lsm6dsx_sysfs_scale_avail, NULL, 0);
 792
 793 static struct attribute *st_lsm6dsx_acc_attributes[] = {
 794         &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 795         &iio_dev_attr_in_accel_scale_available.dev_attr.attr,
 796         NULL,
 797 };
 798
 799 static const struct attribute_group st_lsm6dsx_acc_attribute_group = {
 800         .attrs = st_lsm6dsx_acc_attributes,
 801 };
 802
 803 static const struct iio_info st_lsm6dsx_acc_info = {
 804         .attrs = &st_lsm6dsx_acc_attribute_group,
 805         .read_raw = st_lsm6dsx_read_raw,
 806         .write_raw = st_lsm6dsx_write_raw,
 807         .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
 808 };
 809
 810 static struct attribute *st_lsm6dsx_gyro_attributes[] = {
 811         &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 812         &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
 813         NULL,
 814 };
 815
 816 static const struct attribute_group st_lsm6dsx_gyro_attribute_group = {
 817         .attrs = st_lsm6dsx_gyro_attributes,
 818 };
 819
 820 static const struct iio_info st_lsm6dsx_gyro_info = {
 821         .attrs = &st_lsm6dsx_gyro_attribute_group,
 822         .read_raw = st_lsm6dsx_read_raw,
 823         .write_raw = st_lsm6dsx_write_raw,
 824         .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
 825 };
 826
 827 static int st_lsm6dsx_of_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin)
 828 {
 829         struct device_node *np = hw->dev->of_node;
 830
 831         if (!np)
 832                 return -EINVAL;
 833
 834         return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
 835 }
 836
 837 static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
 838 {
 839         int err = 0, drdy_pin;
 840
 841         if (st_lsm6dsx_of_get_drdy_pin(hw, &drdy_pin) < 0) {
 842                 struct st_sensors_platform_data *pdata;
 843                 struct device *dev = hw->dev;
 844
 845                 pdata = (struct st_sensors_platform_data *)dev->platform_data;
 846                 drdy_pin = pdata ? pdata->drdy_int_pin : 1;
 847         }
 848
 849         switch (drdy_pin) {
 850         case 1:
 851                 *drdy_reg = ST_LSM6DSX_REG_INT1_ADDR;
 852                 break;
 853         case 2:
 854                 *drdy_reg = ST_LSM6DSX_REG_INT2_ADDR;
 855                 break;
 856         default:
 857                 dev_err(hw->dev, "unsupported data ready pin\n");
 858                 err = -EINVAL;
 859                 break;
 860         }
 861
 862         return err;
 863 }
 864
 865 static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
 866 {
 867         const struct st_lsm6dsx_shub_settings *hub_settings;
 868         struct device_node *np = hw->dev->of_node;
 869         struct st_sensors_platform_data *pdata;
 870         unsigned int data;
 871         int err = 0;
 872
 873         hub_settings = &hw->settings->shub_settings;
 874
 875         pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
 876         if ((np && of_property_read_bool(np, "st,pullups")) ||
 877             (pdata && pdata->pullups)) {
 878                 err = st_lsm6dsx_set_page(hw, true);
 879                 if (err < 0)
 880                         return err;
 881
 882                 data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->pullup_en.mask);
 883                 err = regmap_update_bits(hw->regmap,
 884                                          hub_settings->pullup_en.addr,
 885                                          hub_settings->pullup_en.mask, data);
 886
 887                 st_lsm6dsx_set_page(hw, false);
 888
 889                 if (err < 0)
 890                         return err;
 891         }
 892
 893         if (hub_settings->aux_sens.addr) {
 894                 /* configure aux sensors */
 895                 err = st_lsm6dsx_set_page(hw, true);
 896                 if (err < 0)
 897                         return err;
 898
 899                 data = ST_LSM6DSX_SHIFT_VAL(3, hub_settings->aux_sens.mask);
 900                 err = regmap_update_bits(hw->regmap,
 901                                          hub_settings->aux_sens.addr,
 902                                          hub_settings->aux_sens.mask, data);
 903
 904                 st_lsm6dsx_set_page(hw, false);
 905         }
 906
 907         return err;
 908 }
 909
 910 static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw)
 911 {
 912         const struct st_lsm6dsx_hw_ts_settings *ts_settings;
 913         int err, val;
 914
 915         ts_settings = &hw->settings->ts_settings;
 916         /* enable hw timestamp generation if necessary */
 917         if (ts_settings->timer_en.addr) {
 918                 val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->timer_en.mask);
 919                 err = regmap_update_bits(hw->regmap,
 920                                          ts_settings->timer_en.addr,
 921                                          ts_settings->timer_en.mask, val);
 922                 if (err < 0)
 923                         return err;
 924         }
 925
 926         /* enable high resolution for hw ts timer if necessary */
 927         if (ts_settings->hr_timer.addr) {
 928                 val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->hr_timer.mask);
 929                 err = regmap_update_bits(hw->regmap,
 930                                          ts_settings->hr_timer.addr,
 931                                          ts_settings->hr_timer.mask, val);
 932                 if (err < 0)
 933                         return err;
 934         }
 935
 936         /* enable ts queueing in FIFO if necessary */
 937         if (ts_settings->fifo_en.addr) {
 938                 val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->fifo_en.mask);
 939                 err = regmap_update_bits(hw->regmap,
 940                                          ts_settings->fifo_en.addr,
 941                                          ts_settings->fifo_en.mask, val);
 942                 if (err < 0)
 943                         return err;
 944         }
 945         return 0;
 946 }
 947
 948 static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
 949 {
 950         u8 drdy_int_reg;
 951         int err;
 952
 953         /* device sw reset */
 954         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
 955                                  ST_LSM6DSX_REG_RESET_MASK,
 956                                  FIELD_PREP(ST_LSM6DSX_REG_RESET_MASK, 1));
 957         if (err < 0)
 958                 return err;
 959
 960         msleep(50);
 961
 962         /* reload trimming parameter */
 963         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
 964                                  ST_LSM6DSX_REG_BOOT_MASK,
 965                                  FIELD_PREP(ST_LSM6DSX_REG_BOOT_MASK, 1));
 966         if (err < 0)
 967                 return err;
 968
 969         msleep(50);
 970
 971         /* enable Block Data Update */
 972         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_BDU_ADDR,
 973                                  ST_LSM6DSX_REG_BDU_MASK,
 974                                  FIELD_PREP(ST_LSM6DSX_REG_BDU_MASK, 1));
 975         if (err < 0)
 976                 return err;
 977
 978         /* enable FIFO watermak interrupt */
 979         err = st_lsm6dsx_get_drdy_reg(hw, &drdy_int_reg);
 980         if (err < 0)
 981                 return err;
 982
 983         err = regmap_update_bits(hw->regmap, drdy_int_reg,
 984                                  ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK,
 985                                  FIELD_PREP(ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK,
 986                                             1));
 987         if (err < 0)
 988                 return err;
 989
 990         err = st_lsm6dsx_init_shub(hw);
 991         if (err < 0)
 992                 return err;
 993
 994         return st_lsm6dsx_init_hw_timer(hw);
 995 }
 996
 997 static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
 998                                                enum st_lsm6dsx_sensor_id id,
 999                                                const char *name)
1000 {
1001         struct st_lsm6dsx_sensor *sensor;
1002         struct iio_dev *iio_dev;
1003
1004         iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
1005         if (!iio_dev)
1006                 return NULL;
1007
1008         iio_dev->modes = INDIO_DIRECT_MODE;
1009         iio_dev->dev.parent = hw->dev;
1010         iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
1011
1012         sensor = iio_priv(iio_dev);
1013         sensor->id = id;
1014         sensor->hw = hw;
1015         sensor->odr = st_lsm6dsx_odr_table[id].odr_avl[0].hz;
1016         sensor->gain = st_lsm6dsx_fs_table[id].fs_avl[0].gain;
1017         sensor->watermark = 1;
1018
1019         switch (id) {
1020         case ST_LSM6DSX_ID_ACC:
1021                 iio_dev->channels = st_lsm6dsx_acc_channels;
1022                 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels);
1023                 iio_dev->info = &st_lsm6dsx_acc_info;
1024
1025                 scnprintf(sensor->name, sizeof(sensor->name), "%s_accel",
1026                           name);
1027                 break;
1028         case ST_LSM6DSX_ID_GYRO:
1029                 iio_dev->channels = st_lsm6dsx_gyro_channels;
1030                 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels);
1031                 iio_dev->info = &st_lsm6dsx_gyro_info;
1032
1033                 scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro",
1034                           name);
1035                 break;
1036         default:
1037                 return NULL;
1038         }
1039         iio_dev->name = sensor->name;
1040
1041         return iio_dev;
1042 }
1043
1044 int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
1045                      struct regmap *regmap)
1046 {
1047         const struct st_lsm6dsx_shub_settings *hub_settings;
1048         struct st_lsm6dsx_hw *hw;
1049         int i, err;
1050
1051         hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
1052         if (!hw)
1053                 return -ENOMEM;
1054
1055         dev_set_drvdata(dev, (void *)hw);
1056
1057         mutex_init(&hw->fifo_lock);
1058         mutex_init(&hw->conf_lock);
1059         mutex_init(&hw->page_lock);
1060
1061         hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL);
1062         if (!hw->buff)
1063                 return -ENOMEM;
1064
1065         hw->dev = dev;
1066         hw->irq = irq;
1067         hw->regmap = regmap;
1068
1069         err = st_lsm6dsx_check_whoami(hw, hw_id);
1070         if (err < 0)
1071                 return err;
1072
1073         for (i = 0; i < ST_LSM6DSX_ID_EXT0; i++) {
1074                 hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name);
1075                 if (!hw->iio_devs[i])
1076                         return -ENOMEM;
1077         }
1078
1079         err = st_lsm6dsx_init_device(hw);
1080         if (err < 0)
1081                 return err;
1082
1083         hub_settings = &hw->settings->shub_settings;
1084         if (hub_settings->master_en.addr) {
1085                 err = st_lsm6dsx_shub_probe(hw, name);
1086                 if (err < 0)
1087                         return err;
1088         }
1089
1090         if (hw->irq > 0) {
1091                 err = st_lsm6dsx_fifo_setup(hw);
1092                 if (err < 0)
1093                         return err;
1094         }
1095
1096         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
1097                 if (!hw->iio_devs[i])
1098                         continue;
1099
1100                 err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
1101                 if (err)
1102                         return err;
1103         }
1104
1105         return 0;
1106 }
1107 EXPORT_SYMBOL(st_lsm6dsx_probe);
1108
1109 static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
1110 {
1111         struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
1112         struct st_lsm6dsx_sensor *sensor;
1113         const struct st_lsm6dsx_reg *reg;
1114         unsigned int data;
1115         int i, err = 0;
1116
1117         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
1118                 if (!hw->iio_devs[i])
1119                         continue;
1120
1121                 sensor = iio_priv(hw->iio_devs[i]);
1122                 if (!(hw->enable_mask & BIT(sensor->id)))
1123                         continue;
1124
1125                 reg = &st_lsm6dsx_odr_table[sensor->id].reg;
1126                 data = ST_LSM6DSX_SHIFT_VAL(0, reg->mask);
1127                 err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask,
1128                                                     data);
1129                 if (err < 0)
1130                         return err;
1131         }
1132
1133         if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
1134                 err = st_lsm6dsx_flush_fifo(hw);
1135
1136         return err;
1137 }
1138
1139 static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
1140 {
1141         struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
1142         struct st_lsm6dsx_sensor *sensor;
1143         int i, err = 0;
1144
1145         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
1146                 if (!hw->iio_devs[i])
1147                         continue;
1148
1149                 sensor = iio_priv(hw->iio_devs[i]);
1150                 if (!(hw->enable_mask & BIT(sensor->id)))
1151                         continue;
1152
1153                 err = st_lsm6dsx_set_odr(sensor, sensor->odr);
1154                 if (err < 0)
1155                         return err;
1156         }
1157
1158         if (hw->enable_mask)
1159                 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
1160
1161         return err;
1162 }
1163
1164 const struct dev_pm_ops st_lsm6dsx_pm_ops = {
1165         SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsx_suspend, st_lsm6dsx_resume)
1166 };
1167 EXPORT_SYMBOL(st_lsm6dsx_pm_ops);
1168
1169 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
1170 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
1171 MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver");
1172 MODULE_LICENSE("GPL v2");