root/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

DEFINITIONS

1. st_lsm6dsx_get_decimator_val
2. st_lsm6dsx_get_max_min_odr
3. st_lsm6dsx_update_decimators
4. st_lsm6dsx_set_fifo_mode
5. st_lsm6dsx_set_fifo_odr
6. st_lsm6dsx_update_watermark
7. st_lsm6dsx_reset_hw_ts
8. st_lsm6dsx_read_block
9. st_lsm6dsx_read_fifo
10. st_lsm6dsx_push_tagged_data
11. st_lsm6dsx_read_tagged_fifo
12. st_lsm6dsx_flush_fifo
13. st_lsm6dsx_update_fifo
14. st_lsm6dsx_handler_irq
15. st_lsm6dsx_handler_thread
16. st_lsm6dsx_buffer_preenable
17. st_lsm6dsx_buffer_postdisable
18. st_lsm6dsx_fifo_setup

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * STMicroelectronics st_lsm6dsx FIFO buffer library driver
   4  *
   5  * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC/LSM6DS3TR-C:
   6  * The FIFO buffer can be configured to store data from gyroscope and
   7  * accelerometer. Samples are queued without any tag according to a
   8  * specific pattern based on 'FIFO data sets' (6 bytes each):
   9  *  - 1st data set is reserved for gyroscope data
  10  *  - 2nd data set is reserved for accelerometer data
  11  * The FIFO pattern changes depending on the ODRs and decimation factors
  12  * assigned to the FIFO data sets. The first sequence of data stored in FIFO
  13  * buffer contains the data of all the enabled FIFO data sets
  14  * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
  15  * value of the decimation factor and ODR set for each FIFO data set.
  16  *
  17  * LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR/ISM330DHCX: The FIFO buffer can be
  18  * configured to store data from gyroscope and accelerometer. Each sample
  19  * is queued with a tag (1B) indicating data source (gyroscope, accelerometer,
  20  * hw timer).
  21  *
  22  * FIFO supported modes:
  23  *  - BYPASS: FIFO disabled
  24  *  - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
  25  *    restarts from the beginning and the oldest sample is overwritten
  26  *
  27  * Copyright 2016 STMicroelectronics Inc.
  28  *
  29  * Lorenzo Bianconi <lorenzo.bianconi@st.com>
  30  * Denis Ciocca <denis.ciocca@st.com>
  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 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 606 {
 607         struct st_lsm6dsx_hw *hw = sensor->hw;
 608         int err;
 609 
 610         mutex_lock(&hw->conf_lock);
 611 
 612         if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
 613                 err = st_lsm6dsx_flush_fifo(hw);
 614                 if (err < 0)
 615                         goto out;
 616         }
 617 
 618         if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
 619             sensor->id == ST_LSM6DSX_ID_EXT1 ||
 620             sensor->id == ST_LSM6DSX_ID_EXT2) {
 621                 err = st_lsm6dsx_shub_set_enable(sensor, enable);
 622                 if (err < 0)
 623                         goto out;
 624         } else {
 625                 err = st_lsm6dsx_sensor_set_enable(sensor, enable);
 626                 if (err < 0)
 627                         goto out;
 628 
 629                 err = st_lsm6dsx_set_fifo_odr(sensor, enable);
 630                 if (err < 0)
 631                         goto out;
 632         }
 633 
 634         err = st_lsm6dsx_update_decimators(hw);
 635         if (err < 0)
 636                 goto out;
 637 
 638         err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
 639         if (err < 0)
 640                 goto out;
 641 
 642         if (hw->enable_mask) {
 643                 /* reset hw ts counter */
 644                 err = st_lsm6dsx_reset_hw_ts(hw);
 645                 if (err < 0)
 646                         goto out;
 647 
 648                 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
 649         }
 650 
 651 out:
 652         mutex_unlock(&hw->conf_lock);
 653 
 654         return err;
 655 }
 656 
 657 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 658 {
 659         struct st_lsm6dsx_hw *hw = private;
 660 
 661         return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
 662 }
 663 
 664 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 665 {
 666         struct st_lsm6dsx_hw *hw = private;
 667         int count;
 668 
 669         mutex_lock(&hw->fifo_lock);
 670         count = hw->settings->fifo_ops.read_fifo(hw);
 671         mutex_unlock(&hw->fifo_lock);
 672 
 673         return count ? IRQ_HANDLED : IRQ_NONE;
 674 }
 675 
 676 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
 677 {
 678         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 679         struct st_lsm6dsx_hw *hw = sensor->hw;
 680 
 681         if (!hw->settings->fifo_ops.update_fifo)
 682                 return -ENOTSUPP;
 683 
 684         return hw->settings->fifo_ops.update_fifo(sensor, true);
 685 }
 686 
 687 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
 688 {
 689         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 690         struct st_lsm6dsx_hw *hw = sensor->hw;
 691 
 692         if (!hw->settings->fifo_ops.update_fifo)
 693                 return -ENOTSUPP;
 694 
 695         return hw->settings->fifo_ops.update_fifo(sensor, false);
 696 }
 697 
 698 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
 699         .preenable = st_lsm6dsx_buffer_preenable,
 700         .postdisable = st_lsm6dsx_buffer_postdisable,
 701 };
 702 
 703 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 704 {
 705         struct device_node *np = hw->dev->of_node;
 706         struct st_sensors_platform_data *pdata;
 707         struct iio_buffer *buffer;
 708         unsigned long irq_type;
 709         bool irq_active_low;
 710         int i, err;
 711 
 712         irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
 713 
 714         switch (irq_type) {
 715         case IRQF_TRIGGER_HIGH:
 716         case IRQF_TRIGGER_RISING:
 717                 irq_active_low = false;
 718                 break;
 719         case IRQF_TRIGGER_LOW:
 720         case IRQF_TRIGGER_FALLING:
 721                 irq_active_low = true;
 722                 break;
 723         default:
 724                 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
 725                 return -EINVAL;
 726         }
 727 
 728         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
 729                                  ST_LSM6DSX_REG_HLACTIVE_MASK,
 730                                  FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
 731                                             irq_active_low));
 732         if (err < 0)
 733                 return err;
 734 
 735         pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
 736         if ((np && of_property_read_bool(np, "drive-open-drain")) ||
 737             (pdata && pdata->open_drain)) {
 738                 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
 739                                          ST_LSM6DSX_REG_PP_OD_MASK,
 740                                          FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
 741                                                     1));
 742                 if (err < 0)
 743                         return err;
 744 
 745                 irq_type |= IRQF_SHARED;
 746         }
 747 
 748         err = devm_request_threaded_irq(hw->dev, hw->irq,
 749                                         st_lsm6dsx_handler_irq,
 750                                         st_lsm6dsx_handler_thread,
 751                                         irq_type | IRQF_ONESHOT,
 752                                         "lsm6dsx", hw);
 753         if (err) {
 754                 dev_err(hw->dev, "failed to request trigger irq %d\n",
 755                         hw->irq);
 756                 return err;
 757         }
 758 
 759         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 760                 if (!hw->iio_devs[i])
 761                         continue;
 762 
 763                 buffer = devm_iio_kfifo_allocate(hw->dev);
 764                 if (!buffer)
 765                         return -ENOMEM;
 766 
 767                 iio_device_attach_buffer(hw->iio_devs[i], buffer);
 768                 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
 769                 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
 770         }
 771 
 772         return 0;
 773 }