drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

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