drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

   1 /*
   2  * STMicroelectronics st_lsm6dsx FIFO buffer library driver
   3  *
   4  * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC: The FIFO buffer can be
   5  * configured to store data from gyroscope and accelerometer. Samples are
   6  * queued without any tag according to a specific pattern based on
   7  * 'FIFO data sets' (6 bytes each):
   8  *  - 1st data set is reserved for gyroscope data
   9  *  - 2nd data set is reserved for accelerometer data
  10  * The FIFO pattern changes depending on the ODRs and decimation factors
  11  * assigned to the FIFO data sets. The first sequence of data stored in FIFO
  12  * buffer contains the data of all the enabled FIFO data sets
  13  * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
  14  * value of the decimation factor and ODR set for each FIFO data set.
  15  *
  16  * LSM6DSO/ASM330LHH: The FIFO buffer can be configured to store data
  17  * from gyroscope and accelerometer. Each sample is queued with a tag (1B)
  18  * indicating data source (gyroscope, accelerometer, hw timer).
  19  *
  20  * FIFO supported modes:
  21  *  - BYPASS: FIFO disabled
  22  *  - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
  23  *    restarts from the beginning and the oldest sample is overwritten
  24  *
  25  * Copyright 2016 STMicroelectronics Inc.
  26  *
  27  * Lorenzo Bianconi <lorenzo.bianconi@st.com>
  28  * Denis Ciocca <denis.ciocca@st.com>
  29  *
  30  * Licensed under the GPL-2.
  31  */
  32 #include <linux/module.h>
  33 #include <linux/interrupt.h>
  34 #include <linux/irq.h>
  35 #include <linux/iio/kfifo_buf.h>
  36 #include <linux/iio/iio.h>
  37 #include <linux/iio/buffer.h>
  38 #include <linux/regmap.h>
  39 #include <linux/bitfield.h>
  40
  41 #include <linux/platform_data/st_sensors_pdata.h>
  42
  43 #include "st_lsm6dsx.h"
  44
  45 #define ST_LSM6DSX_REG_HLACTIVE_ADDR            0x12
  46 #define ST_LSM6DSX_REG_HLACTIVE_MASK            BIT(5)
  47 #define ST_LSM6DSX_REG_PP_OD_ADDR               0x12
  48 #define ST_LSM6DSX_REG_PP_OD_MASK               BIT(4)
  49 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR           0x0a
  50 #define ST_LSM6DSX_FIFO_MODE_MASK               GENMASK(2, 0)
  51 #define ST_LSM6DSX_FIFO_ODR_MASK                GENMASK(6, 3)
  52 #define ST_LSM6DSX_FIFO_EMPTY_MASK              BIT(12)
  53 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR           0x3e
  54 #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR        0x78
  55 #define ST_LSM6DSX_REG_TS_RESET_ADDR            0x42
  56
  57 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL             0x08
  58
  59 #define ST_LSM6DSX_TS_SENSITIVITY               25000UL /* 25us */
  60 #define ST_LSM6DSX_TS_RESET_VAL                 0xaa
  61
  62 struct st_lsm6dsx_decimator_entry {
  63         u8 decimator;
  64         u8 val;
  65 };
  66
  67 enum st_lsm6dsx_fifo_tag {
  68         ST_LSM6DSX_GYRO_TAG = 0x01,
  69         ST_LSM6DSX_ACC_TAG = 0x02,
  70         ST_LSM6DSX_TS_TAG = 0x04,
  71         ST_LSM6DSX_EXT0_TAG = 0x0f,
  72         ST_LSM6DSX_EXT1_TAG = 0x10,
  73         ST_LSM6DSX_EXT2_TAG = 0x11,
  74 };
  75
  76 static const
  77 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
  78         {  0, 0x0 },
  79         {  1, 0x1 },
  80         {  2, 0x2 },
  81         {  3, 0x3 },
  82         {  4, 0x4 },
  83         {  8, 0x5 },
  84         { 16, 0x6 },
  85         { 32, 0x7 },
  86 };
  87
  88 static int st_lsm6dsx_get_decimator_val(u8 val)
  89 {
  90         const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
  91         int i;
  92
  93         for (i = 0; i < max_size; i++)
  94                 if (st_lsm6dsx_decimator_table[i].decimator == val)
  95                         break;
  96
  97         return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
  98 }
  99
 100 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
 101                                        u16 *max_odr, u16 *min_odr)
 102 {
 103         struct st_lsm6dsx_sensor *sensor;
 104         int i;
 105
 106         *max_odr = 0, *min_odr = ~0;
 107         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 108                 if (!hw->iio_devs[i])
 109                         continue;
 110
 111                 sensor = iio_priv(hw->iio_devs[i]);
 112
 113                 if (!(hw->enable_mask & BIT(sensor->id)))
 114                         continue;
 115
 116                 *max_odr = max_t(u16, *max_odr, sensor->odr);
 117                 *min_odr = min_t(u16, *min_odr, sensor->odr);
 118         }
 119 }
 120
 121 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 122 {
 123         u16 max_odr, min_odr, sip = 0, ts_sip = 0;
 124         const struct st_lsm6dsx_reg *ts_dec_reg;
 125         struct st_lsm6dsx_sensor *sensor;
 126         int err = 0, i;
 127         u8 data;
 128
 129         st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
 130
 131         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 132                 const struct st_lsm6dsx_reg *dec_reg;
 133
 134                 if (!hw->iio_devs[i])
 135                         continue;
 136
 137                 sensor = iio_priv(hw->iio_devs[i]);
 138                 /* update fifo decimators and sample in pattern */
 139                 if (hw->enable_mask & BIT(sensor->id)) {
 140                         sensor->sip = sensor->odr / min_odr;
 141                         sensor->decimator = max_odr / sensor->odr;
 142                         data = st_lsm6dsx_get_decimator_val(sensor->decimator);
 143                 } else {
 144                         sensor->sip = 0;
 145                         sensor->decimator = 0;
 146                         data = 0;
 147                 }
 148                 ts_sip = max_t(u16, ts_sip, sensor->sip);
 149
 150                 dec_reg = &hw->settings->decimator[sensor->id];
 151                 if (dec_reg->addr) {
 152                         int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
 153
 154                         err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
 155                                                             dec_reg->mask,
 156                                                             val);
 157                         if (err < 0)
 158                                 return err;
 159                 }
 160                 sip += sensor->sip;
 161         }
 162         hw->sip = sip + ts_sip;
 163         hw->ts_sip = ts_sip;
 164
 165         /*
 166          * update hw ts decimator if necessary. Decimator for hw timestamp
 167          * is always 1 or 0 in order to have a ts sample for each data
 168          * sample in FIFO
 169          */
 170         ts_dec_reg = &hw->settings->ts_settings.decimator;
 171         if (ts_dec_reg->addr) {
 172                 int val, ts_dec = !!hw->ts_sip;
 173
 174                 val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
 175                 err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
 176                                                     ts_dec_reg->mask, val);
 177         }
 178         return err;
 179 }
 180
 181 int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
 182                              enum st_lsm6dsx_fifo_mode fifo_mode)
 183 {
 184         unsigned int data;
 185         int err;
 186
 187         data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
 188         err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
 189                                             ST_LSM6DSX_FIFO_MODE_MASK, data);
 190         if (err < 0)
 191                 return err;
 192
 193         hw->fifo_mode = fifo_mode;
 194
 195         return 0;
 196 }
 197
 198 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
 199                                    bool enable)
 200 {
 201         struct st_lsm6dsx_hw *hw = sensor->hw;
 202         const struct st_lsm6dsx_reg *batch_reg;
 203         u8 data;
 204
 205         batch_reg = &hw->settings->batch[sensor->id];
 206         if (batch_reg->addr) {
 207                 int val;
 208
 209                 if (enable) {
 210                         int err;
 211
 212                         err = st_lsm6dsx_check_odr(sensor, sensor->odr,
 213                                                    &data);
 214                         if (err < 0)
 215                                 return err;
 216                 } else {
 217                         data = 0;
 218                 }
 219                 val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
 220                 return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
 221                                                      batch_reg->mask, val);
 222         } else {
 223                 data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
 224                 return st_lsm6dsx_update_bits_locked(hw,
 225                                         ST_LSM6DSX_REG_FIFO_MODE_ADDR,
 226                                         ST_LSM6DSX_FIFO_ODR_MASK,
 227                                         FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
 228                                                    data));
 229         }
 230 }
 231
 232 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
 233 {
 234         u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask;
 235         struct st_lsm6dsx_hw *hw = sensor->hw;
 236         struct st_lsm6dsx_sensor *cur_sensor;
 237         int i, err, data;
 238         __le16 wdata;
 239
 240         if (!hw->sip)
 241                 return 0;
 242
 243         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 244                 if (!hw->iio_devs[i])
 245                         continue;
 246
 247                 cur_sensor = iio_priv(hw->iio_devs[i]);
 248
 249                 if (!(hw->enable_mask & BIT(cur_sensor->id)))
 250                         continue;
 251
 252                 cur_watermark = (cur_sensor == sensor) ? watermark
 253                                                        : cur_sensor->watermark;
 254
 255                 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
 256         }
 257
 258         fifo_watermark = max_t(u16, fifo_watermark, hw->sip);
 259         fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;
 260         fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
 261
 262         mutex_lock(&hw->page_lock);
 263         err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
 264                           &data);
 265         if (err < 0)
 266                 goto out;
 267
 268         fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
 269         fifo_watermark = ((data << 8) & ~fifo_th_mask) |
 270                          (fifo_watermark & fifo_th_mask);
 271
 272         wdata = cpu_to_le16(fifo_watermark);
 273         err = regmap_bulk_write(hw->regmap,
 274                                 hw->settings->fifo_ops.fifo_th.addr,
 275                                 &wdata, sizeof(wdata));
 276 out:
 277         mutex_unlock(&hw->page_lock);
 278         return err;
 279 }
 280
 281 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
 282 {
 283         struct st_lsm6dsx_sensor *sensor;
 284         int i, err;
 285
 286         /* reset hw ts counter */
 287         err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
 288                                       ST_LSM6DSX_TS_RESET_VAL);
 289         if (err < 0)
 290                 return err;
 291
 292         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 293                 if (!hw->iio_devs[i])
 294                         continue;
 295
 296                 sensor = iio_priv(hw->iio_devs[i]);
 297                 /*
 298                  * store enable buffer timestamp as reference for
 299                  * hw timestamp
 300                  */
 301                 sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
 302         }
 303         return 0;
 304 }
 305
 306 /*
 307  * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
 308  * in order to avoid a kmalloc for each bus access
 309  */
 310 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
 311                                         u8 *data, unsigned int data_len,
 312                                         unsigned int max_word_len)
 313 {
 314         unsigned int word_len, read_len = 0;
 315         int err;
 316
 317         while (read_len < data_len) {
 318                 word_len = min_t(unsigned int, data_len - read_len,
 319                                  max_word_len);
 320                 err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
 321                                              word_len);
 322                 if (err < 0)
 323                         return err;
 324                 read_len += word_len;
 325         }
 326         return 0;
 327 }
 328
 329 #define ST_LSM6DSX_IIO_BUFF_SIZE        (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
 330                                                sizeof(s64)) + sizeof(s64))
 331 /**
 332  * st_lsm6dsx_read_fifo() - hw FIFO read routine
 333  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
 334  *
 335  * Read samples from the hw FIFO and push them to IIO buffers.
 336  *
 337  * Return: Number of bytes read from the FIFO
 338  */
 339 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 340 {
 341         u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
 342         u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
 343         int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
 344         struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 345         u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 346         u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 347         bool reset_ts = false;
 348         __le16 fifo_status;
 349         s64 ts = 0;
 350
 351         err = st_lsm6dsx_read_locked(hw,
 352                                      hw->settings->fifo_ops.fifo_diff.addr,
 353                                      &fifo_status, sizeof(fifo_status));
 354         if (err < 0) {
 355                 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
 356                         err);
 357                 return err;
 358         }
 359
 360         if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
 361                 return 0;
 362
 363         fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
 364                    ST_LSM6DSX_CHAN_SIZE;
 365         fifo_len = (fifo_len / pattern_len) * pattern_len;
 366
 367         acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 368         gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
 369
 370         for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 371                 err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
 372                                             hw->buff, pattern_len,
 373                                             ST_LSM6DSX_MAX_WORD_LEN);
 374                 if (err < 0) {
 375                         dev_err(hw->dev,
 376                                 "failed to read pattern from fifo (err=%d)\n",
 377                                 err);
 378                         return err;
 379                 }
 380
 381                 /*
 382                  * Data are written to the FIFO with a specific pattern
 383                  * depending on the configured ODRs. The first sequence of data
 384                  * stored in FIFO contains the data of all enabled sensors
 385                  * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
 386                  * depending on the value of the decimation factor set for each
 387                  * sensor.
 388                  *
 389                  * Supposing the FIFO is storing data from gyroscope and
 390                  * accelerometer at different ODRs:
 391                  *   - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
 392                  * Since the gyroscope ODR is twice the accelerometer one, the
 393                  * following pattern is repeated every 9 samples:
 394                  *   - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
 395                  */
 396                 gyro_sip = gyro_sensor->sip;
 397                 acc_sip = acc_sensor->sip;
 398                 ts_sip = hw->ts_sip;
 399                 offset = 0;
 400
 401                 while (acc_sip > 0 || gyro_sip > 0) {
 402                         if (gyro_sip > 0) {
 403                                 memcpy(gyro_buff, &hw->buff[offset],
 404                                        ST_LSM6DSX_SAMPLE_SIZE);
 405                                 offset += ST_LSM6DSX_SAMPLE_SIZE;
 406                         }
 407                         if (acc_sip > 0) {
 408                                 memcpy(acc_buff, &hw->buff[offset],
 409                                        ST_LSM6DSX_SAMPLE_SIZE);
 410                                 offset += ST_LSM6DSX_SAMPLE_SIZE;
 411                         }
 412
 413                         if (ts_sip-- > 0) {
 414                                 u8 data[ST_LSM6DSX_SAMPLE_SIZE];
 415
 416                                 memcpy(data, &hw->buff[offset], sizeof(data));
 417                                 /*
 418                                  * hw timestamp is 3B long and it is stored
 419                                  * in FIFO using 6B as 4th FIFO data set
 420                                  * according to this schema:
 421                                  * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
 422                                  */
 423                                 ts = data[1] << 16 | data[0] << 8 | data[3];
 424                                 /*
 425                                  * check if hw timestamp engine is going to
 426                                  * reset (the sensor generates an interrupt
 427                                  * to signal the hw timestamp will reset in
 428                                  * 1.638s)
 429                                  */
 430                                 if (!reset_ts && ts >= 0xff0000)
 431                                         reset_ts = true;
 432                                 ts *= ST_LSM6DSX_TS_SENSITIVITY;
 433
 434                                 offset += ST_LSM6DSX_SAMPLE_SIZE;
 435                         }
 436
 437                         if (gyro_sip-- > 0)
 438                                 iio_push_to_buffers_with_timestamp(
 439                                         hw->iio_devs[ST_LSM6DSX_ID_GYRO],
 440                                         gyro_buff, gyro_sensor->ts_ref + ts);
 441                         if (acc_sip-- > 0)
 442                                 iio_push_to_buffers_with_timestamp(
 443                                         hw->iio_devs[ST_LSM6DSX_ID_ACC],
 444                                         acc_buff, acc_sensor->ts_ref + ts);
 445                 }
 446         }
 447
 448         if (unlikely(reset_ts)) {
 449                 err = st_lsm6dsx_reset_hw_ts(hw);
 450                 if (err < 0) {
 451                         dev_err(hw->dev, "failed to reset hw ts (err=%d)\n",
 452                                 err);
 453                         return err;
 454                 }
 455         }
 456         return read_len;
 457 }
 458
 459 static int
 460 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
 461                             u8 *data, s64 ts)
 462 {
 463         struct st_lsm6dsx_sensor *sensor;
 464         struct iio_dev *iio_dev;
 465
 466         /*
 467          * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
 468          * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
 469          * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
 470          * channel
 471          */
 472         switch (tag) {
 473         case ST_LSM6DSX_GYRO_TAG:
 474                 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
 475                 break;
 476         case ST_LSM6DSX_ACC_TAG:
 477                 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
 478                 break;
 479         case ST_LSM6DSX_EXT0_TAG:
 480                 if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
 481                         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
 482                 else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
 483                         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
 484                 else
 485                         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
 486                 break;
 487         case ST_LSM6DSX_EXT1_TAG:
 488                 if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
 489                     (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
 490                         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
 491                 else
 492                         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
 493                 break;
 494         case ST_LSM6DSX_EXT2_TAG:
 495                 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
 496                 break;
 497         default:
 498                 return -EINVAL;
 499         }
 500
 501         sensor = iio_priv(iio_dev);
 502         iio_push_to_buffers_with_timestamp(iio_dev, data,
 503                                            ts + sensor->ts_ref);
 504
 505         return 0;
 506 }
 507
 508 /**
 509  * st_lsm6dsx_read_tagged_fifo() - LSM6DSO/ASM330LHH read FIFO routine
 510  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
 511  *
 512  * Read samples from the hw FIFO and push them to IIO buffers.
 513  *
 514  * Return: Number of bytes read from the FIFO
 515  */
 516 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
 517 {
 518         u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
 519         u16 fifo_len, fifo_diff_mask;
 520         struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 521         u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE], tag;
 522         bool reset_ts = false;
 523         int i, err, read_len;
 524         __le16 fifo_status;
 525         s64 ts = 0;
 526
 527         err = st_lsm6dsx_read_locked(hw,
 528                                      hw->settings->fifo_ops.fifo_diff.addr,
 529                                      &fifo_status, sizeof(fifo_status));
 530         if (err < 0) {
 531                 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
 532                         err);
 533                 return err;
 534         }
 535
 536         fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
 537         fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
 538                    ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
 539         if (!fifo_len)
 540                 return 0;
 541
 542         acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 543         gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
 544
 545         for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 546                 err = st_lsm6dsx_read_block(hw,
 547                                             ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
 548                                             hw->buff, pattern_len,
 549                                             ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
 550                 if (err < 0) {
 551                         dev_err(hw->dev,
 552                                 "failed to read pattern from fifo (err=%d)\n",
 553                                 err);
 554                         return err;
 555                 }
 556
 557                 for (i = 0; i < pattern_len;
 558                      i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
 559                         memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
 560                                ST_LSM6DSX_SAMPLE_SIZE);
 561
 562                         tag = hw->buff[i] >> 3;
 563                         if (tag == ST_LSM6DSX_TS_TAG) {
 564                                 /*
 565                                  * hw timestamp is 4B long and it is stored
 566                                  * in FIFO according to this schema:
 567                                  * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
 568                                  * B3 = ts[31:24]
 569                                  */
 570                                 ts = le32_to_cpu(*((__le32 *)iio_buff));
 571                                 /*
 572                                  * check if hw timestamp engine is going to
 573                                  * reset (the sensor generates an interrupt
 574                                  * to signal the hw timestamp will reset in
 575                                  * 1.638s)
 576                                  */
 577                                 if (!reset_ts && ts >= 0xffff0000)
 578                                         reset_ts = true;
 579                                 ts *= ST_LSM6DSX_TS_SENSITIVITY;
 580                         } else {
 581                                 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
 582                                                             ts);
 583                         }
 584                 }
 585         }
 586
 587         if (unlikely(reset_ts)) {
 588                 err = st_lsm6dsx_reset_hw_ts(hw);
 589                 if (err < 0)
 590                         return err;
 591         }
 592         return read_len;
 593 }
 594
 595 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 596 {
 597         int err;
 598
 599         mutex_lock(&hw->fifo_lock);
 600
 601         hw->settings->fifo_ops.read_fifo(hw);
 602         err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
 603
 604         mutex_unlock(&hw->fifo_lock);
 605
 606         return err;
 607 }
 608
 609 static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 610 {
 611         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 612         struct st_lsm6dsx_hw *hw = sensor->hw;
 613         int err;
 614
 615         mutex_lock(&hw->conf_lock);
 616
 617         if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
 618                 err = st_lsm6dsx_flush_fifo(hw);
 619                 if (err < 0)
 620                         goto out;
 621         }
 622
 623         if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
 624             sensor->id == ST_LSM6DSX_ID_EXT1 ||
 625             sensor->id == ST_LSM6DSX_ID_EXT2) {
 626                 err = st_lsm6dsx_shub_set_enable(sensor, enable);
 627                 if (err < 0)
 628                         goto out;
 629         } else {
 630                 err = st_lsm6dsx_sensor_set_enable(sensor, enable);
 631                 if (err < 0)
 632                         goto out;
 633
 634                 err = st_lsm6dsx_set_fifo_odr(sensor, enable);
 635                 if (err < 0)
 636                         goto out;
 637         }
 638
 639         err = st_lsm6dsx_update_decimators(hw);
 640         if (err < 0)
 641                 goto out;
 642
 643         err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
 644         if (err < 0)
 645                 goto out;
 646
 647         if (hw->enable_mask) {
 648                 /* reset hw ts counter */
 649                 err = st_lsm6dsx_reset_hw_ts(hw);
 650                 if (err < 0)
 651                         goto out;
 652
 653                 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
 654         }
 655
 656 out:
 657         mutex_unlock(&hw->conf_lock);
 658
 659         return err;
 660 }
 661
 662 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 663 {
 664         struct st_lsm6dsx_hw *hw = private;
 665
 666         return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
 667 }
 668
 669 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 670 {
 671         struct st_lsm6dsx_hw *hw = private;
 672         int count;
 673
 674         mutex_lock(&hw->fifo_lock);
 675         count = hw->settings->fifo_ops.read_fifo(hw);
 676         mutex_unlock(&hw->fifo_lock);
 677
 678         return !count ? IRQ_NONE : IRQ_HANDLED;
 679 }
 680
 681 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
 682 {
 683         return st_lsm6dsx_update_fifo(iio_dev, true);
 684 }
 685
 686 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
 687 {
 688         return st_lsm6dsx_update_fifo(iio_dev, false);
 689 }
 690
 691 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
 692         .preenable = st_lsm6dsx_buffer_preenable,
 693         .postdisable = st_lsm6dsx_buffer_postdisable,
 694 };
 695
 696 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 697 {
 698         struct device_node *np = hw->dev->of_node;
 699         struct st_sensors_platform_data *pdata;
 700         struct iio_buffer *buffer;
 701         unsigned long irq_type;
 702         bool irq_active_low;
 703         int i, err;
 704
 705         irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
 706
 707         switch (irq_type) {
 708         case IRQF_TRIGGER_HIGH:
 709         case IRQF_TRIGGER_RISING:
 710                 irq_active_low = false;
 711                 break;
 712         case IRQF_TRIGGER_LOW:
 713         case IRQF_TRIGGER_FALLING:
 714                 irq_active_low = true;
 715                 break;
 716         default:
 717                 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
 718                 return -EINVAL;
 719         }
 720
 721         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
 722                                  ST_LSM6DSX_REG_HLACTIVE_MASK,
 723                                  FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
 724                                             irq_active_low));
 725         if (err < 0)
 726                 return err;
 727
 728         pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
 729         if ((np && of_property_read_bool(np, "drive-open-drain")) ||
 730             (pdata && pdata->open_drain)) {
 731                 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
 732                                          ST_LSM6DSX_REG_PP_OD_MASK,
 733                                          FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
 734                                                     1));
 735                 if (err < 0)
 736                         return err;
 737
 738                 irq_type |= IRQF_SHARED;
 739         }
 740
 741         err = devm_request_threaded_irq(hw->dev, hw->irq,
 742                                         st_lsm6dsx_handler_irq,
 743                                         st_lsm6dsx_handler_thread,
 744                                         irq_type | IRQF_ONESHOT,
 745                                         "lsm6dsx", hw);
 746         if (err) {
 747                 dev_err(hw->dev, "failed to request trigger irq %d\n",
 748                         hw->irq);
 749                 return err;
 750         }
 751
 752         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 753                 if (!hw->iio_devs[i])
 754                         continue;
 755
 756                 buffer = devm_iio_kfifo_allocate(hw->dev);
 757                 if (!buffer)
 758                         return -ENOMEM;
 759
 760                 iio_device_attach_buffer(hw->iio_devs[i], buffer);
 761                 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
 762                 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
 763         }
 764
 765         return 0;
 766 }