linux/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
<<
>>
Prefs
   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: The FIFO buffer can be configured to store data from gyroscope and
  17 * accelerometer. Each sample is queued with a tag (1B) indicating data source
  18 * (gyroscope, accelerometer, hw timer).
  19 *
  20 * FIFO supported modes:
  21 *  - BYPASS: FIFO disabled
  22 *  - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
  23 *    restarts from the beginning and the oldest sample is overwritten
  24 *
  25 * Copyright 2016 STMicroelectronics Inc.
  26 *
  27 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
  28 * Denis Ciocca <denis.ciocca@st.com>
  29 *
  30 * Licensed under the GPL-2.
  31 */
  32#include <linux/module.h>
  33#include <linux/interrupt.h>
  34#include <linux/irq.h>
  35#include <linux/iio/kfifo_buf.h>
  36#include <linux/iio/iio.h>
  37#include <linux/iio/buffer.h>
  38#include <linux/regmap.h>
  39#include <linux/bitfield.h>
  40
  41#include <linux/platform_data/st_sensors_pdata.h>
  42
  43#include "st_lsm6dsx.h"
  44
  45#define ST_LSM6DSX_REG_HLACTIVE_ADDR            0x12
  46#define ST_LSM6DSX_REG_HLACTIVE_MASK            BIT(5)
  47#define ST_LSM6DSX_REG_PP_OD_ADDR               0x12
  48#define ST_LSM6DSX_REG_PP_OD_MASK               BIT(4)
  49#define ST_LSM6DSX_REG_FIFO_MODE_ADDR           0x0a
  50#define ST_LSM6DSX_FIFO_MODE_MASK               GENMASK(2, 0)
  51#define ST_LSM6DSX_FIFO_ODR_MASK                GENMASK(6, 3)
  52#define ST_LSM6DSX_FIFO_EMPTY_MASK              BIT(12)
  53#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR           0x3e
  54#define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR        0x78
  55#define ST_LSM6DSX_REG_TS_RESET_ADDR            0x42
  56
  57#define ST_LSM6DSX_MAX_FIFO_ODR_VAL             0x08
  58
  59#define ST_LSM6DSX_TS_SENSITIVITY               25000UL /* 25us */
  60#define ST_LSM6DSX_TS_RESET_VAL                 0xaa
  61
  62struct st_lsm6dsx_decimator_entry {
  63        u8 decimator;
  64        u8 val;
  65};
  66
  67enum 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
  76static const
  77struct 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
  88static 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
 100static 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
 121static 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
 181int 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
 198static 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
 232int 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));
 276out:
 277        mutex_unlock(&hw->page_lock);
 278        return err;
 279}
 280
 281static 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 */
 310static 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 */
 339int 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
 459static int
 460st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
 461                            u8 *data, s64 ts)
 462{
 463        struct st_lsm6dsx_sensor *sensor;
 464        struct iio_dev *iio_dev;
 465
 466        /*
 467         * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
 468         * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
 469         * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
 470         * channel
 471         */
 472        switch (tag) {
 473        case ST_LSM6DSX_GYRO_TAG:
 474                iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
 475                break;
 476        case ST_LSM6DSX_ACC_TAG:
 477                iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
 478                break;
 479        case ST_LSM6DSX_EXT0_TAG:
 480                if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
 481                        iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
 482                else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
 483                        iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
 484                else
 485                        iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
 486                break;
 487        case ST_LSM6DSX_EXT1_TAG:
 488                if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
 489                    (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
 490                        iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
 491                else
 492                        iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
 493                break;
 494        case ST_LSM6DSX_EXT2_TAG:
 495                iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
 496                break;
 497        default:
 498                return -EINVAL;
 499        }
 500
 501        sensor = iio_priv(iio_dev);
 502        iio_push_to_buffers_with_timestamp(iio_dev, data,
 503                                           ts + sensor->ts_ref);
 504
 505        return 0;
 506}
 507
 508/**
 509 * st_lsm6dsx_read_tagged_fifo() - LSM6DSO read FIFO routine
 510 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
 511 *
 512 * Read samples from the hw FIFO and push them to IIO buffers.
 513 *
 514 * Return: Number of bytes read from the FIFO
 515 */
 516int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
 517{
 518        u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
 519        u16 fifo_len, fifo_diff_mask;
 520        struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 521        u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE], tag;
 522        bool reset_ts = false;
 523        int i, err, read_len;
 524        __le16 fifo_status;
 525        s64 ts = 0;
 526
 527        err = st_lsm6dsx_read_locked(hw,
 528                                     hw->settings->fifo_ops.fifo_diff.addr,
 529                                     &fifo_status, sizeof(fifo_status));
 530        if (err < 0) {
 531                dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
 532                        err);
 533                return err;
 534        }
 535
 536        fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
 537        fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
 538                   ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
 539        if (!fifo_len)
 540                return 0;
 541
 542        acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 543        gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
 544
 545        for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 546                err = st_lsm6dsx_read_block(hw,
 547                                            ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
 548                                            hw->buff, pattern_len,
 549                                            ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
 550                if (err < 0) {
 551                        dev_err(hw->dev,
 552                                "failed to read pattern from fifo (err=%d)\n",
 553                                err);
 554                        return err;
 555                }
 556
 557                for (i = 0; i < pattern_len;
 558                     i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
 559                        memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
 560                               ST_LSM6DSX_SAMPLE_SIZE);
 561
 562                        tag = hw->buff[i] >> 3;
 563                        if (tag == ST_LSM6DSX_TS_TAG) {
 564                                /*
 565                                 * hw timestamp is 4B long and it is stored
 566                                 * in FIFO according to this schema:
 567                                 * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
 568                                 * B3 = ts[31:24]
 569                                 */
 570                                ts = le32_to_cpu(*((__le32 *)iio_buff));
 571                                /*
 572                                 * check if hw timestamp engine is going to
 573                                 * reset (the sensor generates an interrupt
 574                                 * to signal the hw timestamp will reset in
 575                                 * 1.638s)
 576                                 */
 577                                if (!reset_ts && ts >= 0xffff0000)
 578                                        reset_ts = true;
 579                                ts *= ST_LSM6DSX_TS_SENSITIVITY;
 580                        } else {
 581                                st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
 582                                                            ts);
 583                        }
 584                }
 585        }
 586
 587        if (unlikely(reset_ts)) {
 588                err = st_lsm6dsx_reset_hw_ts(hw);
 589                if (err < 0)
 590                        return err;
 591        }
 592        return read_len;
 593}
 594
 595int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 596{
 597        int err;
 598
 599        mutex_lock(&hw->fifo_lock);
 600
 601        hw->settings->fifo_ops.read_fifo(hw);
 602        err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
 603
 604        mutex_unlock(&hw->fifo_lock);
 605
 606        return err;
 607}
 608
 609static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 610{
 611        struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 612        struct st_lsm6dsx_hw *hw = sensor->hw;
 613        int err;
 614
 615        mutex_lock(&hw->conf_lock);
 616
 617        if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
 618                err = st_lsm6dsx_flush_fifo(hw);
 619                if (err < 0)
 620                        goto out;
 621        }
 622
 623        if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
 624            sensor->id == ST_LSM6DSX_ID_EXT1 ||
 625            sensor->id == ST_LSM6DSX_ID_EXT2) {
 626                err = st_lsm6dsx_shub_set_enable(sensor, enable);
 627                if (err < 0)
 628                        goto out;
 629        } else {
 630                err = st_lsm6dsx_sensor_set_enable(sensor, enable);
 631                if (err < 0)
 632                        goto out;
 633
 634                err = st_lsm6dsx_set_fifo_odr(sensor, enable);
 635                if (err < 0)
 636                        goto out;
 637        }
 638
 639        err = st_lsm6dsx_update_decimators(hw);
 640        if (err < 0)
 641                goto out;
 642
 643        err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
 644        if (err < 0)
 645                goto out;
 646
 647        if (hw->enable_mask) {
 648                /* reset hw ts counter */
 649                err = st_lsm6dsx_reset_hw_ts(hw);
 650                if (err < 0)
 651                        goto out;
 652
 653                err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
 654        }
 655
 656out:
 657        mutex_unlock(&hw->conf_lock);
 658
 659        return err;
 660}
 661
 662static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 663{
 664        struct st_lsm6dsx_hw *hw = private;
 665
 666        return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
 667}
 668
 669static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 670{
 671        struct st_lsm6dsx_hw *hw = private;
 672        int count;
 673
 674        mutex_lock(&hw->fifo_lock);
 675        count = hw->settings->fifo_ops.read_fifo(hw);
 676        mutex_unlock(&hw->fifo_lock);
 677
 678        return !count ? IRQ_NONE : IRQ_HANDLED;
 679}
 680
 681static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
 682{
 683        return st_lsm6dsx_update_fifo(iio_dev, true);
 684}
 685
 686static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
 687{
 688        return st_lsm6dsx_update_fifo(iio_dev, false);
 689}
 690
 691static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
 692        .preenable = st_lsm6dsx_buffer_preenable,
 693        .postdisable = st_lsm6dsx_buffer_postdisable,
 694};
 695
 696int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 697{
 698        struct device_node *np = hw->dev->of_node;
 699        struct st_sensors_platform_data *pdata;
 700        struct iio_buffer *buffer;
 701        unsigned long irq_type;
 702        bool irq_active_low;
 703        int i, err;
 704
 705        irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
 706
 707        switch (irq_type) {
 708        case IRQF_TRIGGER_HIGH:
 709        case IRQF_TRIGGER_RISING:
 710                irq_active_low = false;
 711                break;
 712        case IRQF_TRIGGER_LOW:
 713        case IRQF_TRIGGER_FALLING:
 714                irq_active_low = true;
 715                break;
 716        default:
 717                dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
 718                return -EINVAL;
 719        }
 720
 721        err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
 722                                 ST_LSM6DSX_REG_HLACTIVE_MASK,
 723                                 FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
 724                                            irq_active_low));
 725        if (err < 0)
 726                return err;
 727
 728        pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
 729        if ((np && of_property_read_bool(np, "drive-open-drain")) ||
 730            (pdata && pdata->open_drain)) {
 731                err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
 732                                         ST_LSM6DSX_REG_PP_OD_MASK,
 733                                         FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
 734                                                    1));
 735                if (err < 0)
 736                        return err;
 737
 738                irq_type |= IRQF_SHARED;
 739        }
 740
 741        err = devm_request_threaded_irq(hw->dev, hw->irq,
 742                                        st_lsm6dsx_handler_irq,
 743                                        st_lsm6dsx_handler_thread,
 744                                        irq_type | IRQF_ONESHOT,
 745                                        "lsm6dsx", hw);
 746        if (err) {
 747                dev_err(hw->dev, "failed to request trigger irq %d\n",
 748                        hw->irq);
 749                return err;
 750        }
 751
 752        for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 753                if (!hw->iio_devs[i])
 754                        continue;
 755
 756                buffer = devm_iio_kfifo_allocate(hw->dev);
 757                if (!buffer)
 758                        return -ENOMEM;
 759
 760                iio_device_attach_buffer(hw->iio_devs[i], buffer);
 761                hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
 762                hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
 763        }
 764
 765        return 0;
 766}
 767