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