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