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/LSM6DSOX/ASM330LHH/LSM6DSR: The FIFO buffer can be configured to
  17  * store data from gyroscope and accelerometer. Each sample is queued with
  18  * a tag (1B) 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 <[email protected]>
  28  * Denis Ciocca <[email protected]>
  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() - tagged hw FIFO read 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         u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE], tag;
 521         bool reset_ts = false;
 522         int i, err, read_len;
 523         __le16 fifo_status;
 524         s64 ts = 0;
 525
 526         err = st_lsm6dsx_read_locked(hw,
 527                                      hw->settings->fifo_ops.fifo_diff.addr,
 528                                      &fifo_status, sizeof(fifo_status));
 529         if (err < 0) {
 530                 dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
 531                         err);
 532                 return err;
 533         }
 534
 535         fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
 536         fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
 537                    ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
 538         if (!fifo_len)
 539                 return 0;
 540
 541         for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 542                 err = st_lsm6dsx_read_block(hw,
 543                                             ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
 544                                             hw->buff, pattern_len,
 545                                             ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
 546                 if (err < 0) {
 547                         dev_err(hw->dev,
 548                                 "failed to read pattern from fifo (err=%d)\n",
 549                                 err);
 550                         return err;
 551                 }
 552
 553                 for (i = 0; i < pattern_len;
 554                      i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
 555                         memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
 556                                ST_LSM6DSX_SAMPLE_SIZE);
 557
 558                         tag = hw->buff[i] >> 3;
 559                         if (tag == ST_LSM6DSX_TS_TAG) {
 560                                 /*
 561                                  * hw timestamp is 4B long and it is stored
 562                                  * in FIFO according to this schema:
 563                                  * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
 564                                  * B3 = ts[31:24]
 565                                  */
 566                                 ts = le32_to_cpu(*((__le32 *)iio_buff));
 567                                 /*
 568                                  * check if hw timestamp engine is going to
 569                                  * reset (the sensor generates an interrupt
 570                                  * to signal the hw timestamp will reset in
 571                                  * 1.638s)
 572                                  */
 573                                 if (!reset_ts && ts >= 0xffff0000)
 574                                         reset_ts = true;
 575                                 ts *= ST_LSM6DSX_TS_SENSITIVITY;
 576                         } else {
 577                                 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
 578                                                             ts);
 579                         }
 580                 }
 581         }
 582
 583         if (unlikely(reset_ts)) {
 584                 err = st_lsm6dsx_reset_hw_ts(hw);
 585                 if (err < 0)
 586                         return err;
 587         }
 588         return read_len;
 589 }
 590
 591 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 592 {
 593         int err;
 594
 595         mutex_lock(&hw->fifo_lock);
 596
 597         hw->settings->fifo_ops.read_fifo(hw);
 598         err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
 599
 600         mutex_unlock(&hw->fifo_lock);
 601
 602         return err;
 603 }
 604
 605 static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 606 {
 607         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 608         struct st_lsm6dsx_hw *hw = sensor->hw;
 609         int err;
 610
 611         mutex_lock(&hw->conf_lock);
 612
 613         if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
 614                 err = st_lsm6dsx_flush_fifo(hw);
 615                 if (err < 0)
 616                         goto out;
 617         }
 618
 619         if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
 620             sensor->id == ST_LSM6DSX_ID_EXT1 ||
 621             sensor->id == ST_LSM6DSX_ID_EXT2) {
 622                 err = st_lsm6dsx_shub_set_enable(sensor, enable);
 623                 if (err < 0)
 624                         goto out;
 625         } else {
 626                 err = st_lsm6dsx_sensor_set_enable(sensor, enable);
 627                 if (err < 0)
 628                         goto out;
 629
 630                 err = st_lsm6dsx_set_fifo_odr(sensor, enable);
 631                 if (err < 0)
 632                         goto out;
 633         }
 634
 635         err = st_lsm6dsx_update_decimators(hw);
 636         if (err < 0)
 637                 goto out;
 638
 639         err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
 640         if (err < 0)
 641                 goto out;
 642
 643         if (hw->enable_mask) {
 644                 /* reset hw ts counter */
 645                 err = st_lsm6dsx_reset_hw_ts(hw);
 646                 if (err < 0)
 647                         goto out;
 648
 649                 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
 650         }
 651
 652 out:
 653         mutex_unlock(&hw->conf_lock);
 654
 655         return err;
 656 }
 657
 658 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 659 {
 660         struct st_lsm6dsx_hw *hw = private;
 661
 662         return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
 663 }
 664
 665 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 666 {
 667         struct st_lsm6dsx_hw *hw = private;
 668         int count;
 669
 670         mutex_lock(&hw->fifo_lock);
 671         count = hw->settings->fifo_ops.read_fifo(hw);
 672         mutex_unlock(&hw->fifo_lock);
 673
 674         return !count ? IRQ_NONE : IRQ_HANDLED;
 675 }
 676
 677 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
 678 {
 679         return st_lsm6dsx_update_fifo(iio_dev, true);
 680 }
 681
 682 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
 683 {
 684         return st_lsm6dsx_update_fifo(iio_dev, false);
 685 }
 686
 687 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
 688         .preenable = st_lsm6dsx_buffer_preenable,
 689         .postdisable = st_lsm6dsx_buffer_postdisable,
 690 };
 691
 692 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 693 {
 694         struct device_node *np = hw->dev->of_node;
 695         struct st_sensors_platform_data *pdata;
 696         struct iio_buffer *buffer;
 697         unsigned long irq_type;
 698         bool irq_active_low;
 699         int i, err;
 700
 701         irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
 702
 703         switch (irq_type) {
 704         case IRQF_TRIGGER_HIGH:
 705         case IRQF_TRIGGER_RISING:
 706                 irq_active_low = false;
 707                 break;
 708         case IRQF_TRIGGER_LOW:
 709         case IRQF_TRIGGER_FALLING:
 710                 irq_active_low = true;
 711                 break;
 712         default:
 713                 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
 714                 return -EINVAL;
 715         }
 716
 717         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
 718                                  ST_LSM6DSX_REG_HLACTIVE_MASK,
 719                                  FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
 720                                             irq_active_low));
 721         if (err < 0)
 722                 return err;
 723
 724         pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
 725         if ((np && of_property_read_bool(np, "drive-open-drain")) ||
 726             (pdata && pdata->open_drain)) {
 727                 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
 728                                          ST_LSM6DSX_REG_PP_OD_MASK,
 729                                          FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
 730                                                     1));
 731                 if (err < 0)
 732                         return err;
 733
 734                 irq_type |= IRQF_SHARED;
 735         }
 736
 737         err = devm_request_threaded_irq(hw->dev, hw->irq,
 738                                         st_lsm6dsx_handler_irq,
 739                                         st_lsm6dsx_handler_thread,
 740                                         irq_type | IRQF_ONESHOT,
 741                                         "lsm6dsx", hw);
 742         if (err) {
 743                 dev_err(hw->dev, "failed to request trigger irq %d\n",
 744                         hw->irq);
 745                 return err;
 746         }
 747
 748         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 749                 if (!hw->iio_devs[i])
 750                         continue;
 751
 752                 buffer = devm_iio_kfifo_allocate(hw->dev);
 753                 if (!buffer)
 754                         return -ENOMEM;
 755
 756                 iio_device_attach_buffer(hw->iio_devs[i], buffer);
 757                 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
 758                 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
 759         }
 760
 761         return 0;
 762 }