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