drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

   1 /*
   2  * STMicroelectronics st_lsm6dsx FIFO buffer library driver
   3  *
   4  * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM: The FIFO buffer can be configured
   5  * to store data from gyroscope and accelerometer. Samples are queued
   6  * without any tag according to a specific pattern based on 'FIFO data sets'
   7  * (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  * FIFO supported modes:
  16  *  - BYPASS: FIFO disabled
  17  *  - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
  18  *    restarts from the beginning and the oldest sample is overwritten
  19  *
  20  * Copyright 2016 STMicroelectronics Inc.
  21  *
  22  * Lorenzo Bianconi <lorenzo.bianconi@st.com>
  23  * Denis Ciocca <denis.ciocca@st.com>
  24  *
  25  * Licensed under the GPL-2.
  26  */
  27 #include <linux/module.h>
  28 #include <linux/interrupt.h>
  29 #include <linux/irq.h>
  30 #include <linux/iio/kfifo_buf.h>
  31 #include <linux/iio/iio.h>
  32 #include <linux/iio/buffer.h>
  33 #include <linux/regmap.h>
  34 #include <linux/bitfield.h>
  35
  36 #include <linux/platform_data/st_sensors_pdata.h>
  37
  38 #include "st_lsm6dsx.h"
  39
  40 #define ST_LSM6DSX_REG_HLACTIVE_ADDR            0x12
  41 #define ST_LSM6DSX_REG_HLACTIVE_MASK            BIT(5)
  42 #define ST_LSM6DSX_REG_PP_OD_ADDR               0x12
  43 #define ST_LSM6DSX_REG_PP_OD_MASK               BIT(4)
  44 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR           0x0a
  45 #define ST_LSM6DSX_FIFO_MODE_MASK               GENMASK(2, 0)
  46 #define ST_LSM6DSX_FIFO_ODR_MASK                GENMASK(6, 3)
  47 #define ST_LSM6DSX_FIFO_EMPTY_MASK              BIT(12)
  48 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR           0x3e
  49
  50 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL             0x08
  51
  52 struct st_lsm6dsx_decimator_entry {
  53         u8 decimator;
  54         u8 val;
  55 };
  56
  57 static const
  58 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
  59         {  0, 0x0 },
  60         {  1, 0x1 },
  61         {  2, 0x2 },
  62         {  3, 0x3 },
  63         {  4, 0x4 },
  64         {  8, 0x5 },
  65         { 16, 0x6 },
  66         { 32, 0x7 },
  67 };
  68
  69 static int st_lsm6dsx_get_decimator_val(u8 val)
  70 {
  71         const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
  72         int i;
  73
  74         for (i = 0; i < max_size; i++)
  75                 if (st_lsm6dsx_decimator_table[i].decimator == val)
  76                         break;
  77
  78         return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
  79 }
  80
  81 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
  82                                        u16 *max_odr, u16 *min_odr)
  83 {
  84         struct st_lsm6dsx_sensor *sensor;
  85         int i;
  86
  87         *max_odr = 0, *min_odr = ~0;
  88         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
  89                 sensor = iio_priv(hw->iio_devs[i]);
  90
  91                 if (!(hw->enable_mask & BIT(sensor->id)))
  92                         continue;
  93
  94                 *max_odr = max_t(u16, *max_odr, sensor->odr);
  95                 *min_odr = min_t(u16, *min_odr, sensor->odr);
  96         }
  97 }
  98
  99 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 100 {
 101         struct st_lsm6dsx_sensor *sensor;
 102         u16 max_odr, min_odr, sip = 0;
 103         int err, i;
 104         u8 data;
 105
 106         st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
 107
 108         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 109                 const struct st_lsm6dsx_reg *dec_reg;
 110
 111                 sensor = iio_priv(hw->iio_devs[i]);
 112                 /* update fifo decimators and sample in pattern */
 113                 if (hw->enable_mask & BIT(sensor->id)) {
 114                         sensor->sip = sensor->odr / min_odr;
 115                         sensor->decimator = max_odr / sensor->odr;
 116                         data = st_lsm6dsx_get_decimator_val(sensor->decimator);
 117                 } else {
 118                         sensor->sip = 0;
 119                         sensor->decimator = 0;
 120                         data = 0;
 121                 }
 122
 123                 dec_reg = &hw->settings->decimator[sensor->id];
 124                 if (dec_reg->addr) {
 125                         int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
 126
 127                         err = regmap_update_bits(hw->regmap, dec_reg->addr,
 128                                                  dec_reg->mask, val);
 129                         if (err < 0)
 130                                 return err;
 131                 }
 132                 sip += sensor->sip;
 133         }
 134         hw->sip = sip;
 135
 136         return 0;
 137 }
 138
 139 int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
 140                              enum st_lsm6dsx_fifo_mode fifo_mode)
 141 {
 142         int err;
 143
 144         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
 145                                  ST_LSM6DSX_FIFO_MODE_MASK,
 146                                  FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK,
 147                                             fifo_mode));
 148         if (err < 0)
 149                 return err;
 150
 151         hw->fifo_mode = fifo_mode;
 152
 153         return 0;
 154 }
 155
 156 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
 157                                    bool enable)
 158 {
 159         struct st_lsm6dsx_hw *hw = sensor->hw;
 160         u8 data;
 161
 162         data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
 163         return regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
 164                                  ST_LSM6DSX_FIFO_ODR_MASK,
 165                                  FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, data));
 166 }
 167
 168 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
 169 {
 170         u16 fifo_watermark = ~0, cur_watermark, sip = 0, fifo_th_mask;
 171         struct st_lsm6dsx_hw *hw = sensor->hw;
 172         struct st_lsm6dsx_sensor *cur_sensor;
 173         int i, err, data;
 174         __le16 wdata;
 175
 176         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 177                 cur_sensor = iio_priv(hw->iio_devs[i]);
 178
 179                 if (!(hw->enable_mask & BIT(cur_sensor->id)))
 180                         continue;
 181
 182                 cur_watermark = (cur_sensor == sensor) ? watermark
 183                                                        : cur_sensor->watermark;
 184
 185                 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
 186                 sip += cur_sensor->sip;
 187         }
 188
 189         if (!sip)
 190                 return 0;
 191
 192         fifo_watermark = max_t(u16, fifo_watermark, sip);
 193         fifo_watermark = (fifo_watermark / sip) * sip;
 194         fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
 195
 196         err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
 197                           &data);
 198         if (err < 0)
 199                 return err;
 200
 201         fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
 202         fifo_watermark = ((data << 8) & ~fifo_th_mask) |
 203                          (fifo_watermark & fifo_th_mask);
 204
 205         wdata = cpu_to_le16(fifo_watermark);
 206         return regmap_bulk_write(hw->regmap,
 207                                  hw->settings->fifo_ops.fifo_th.addr,
 208                                  &wdata, sizeof(wdata));
 209 }
 210
 211 /*
 212  * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN in order to avoid
 213  * a kmalloc for each bus access
 214  */
 215 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 *data,
 216                                         unsigned int data_len)
 217 {
 218         unsigned int word_len, read_len = 0;
 219         int err;
 220
 221         while (read_len < data_len) {
 222                 word_len = min_t(unsigned int, data_len - read_len,
 223                                  ST_LSM6DSX_MAX_WORD_LEN);
 224                 err = regmap_bulk_read(hw->regmap,
 225                                        ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
 226                                        data + read_len, word_len);
 227                 if (err < 0)
 228                         return err;
 229                 read_len += word_len;
 230         }
 231         return 0;
 232 }
 233
 234 /**
 235  * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DS3H-LSM6DSL-LSM6DSM read FIFO routine
 236  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
 237  *
 238  * Read samples from the hw FIFO and push them to IIO buffers.
 239  *
 240  * Return: Number of bytes read from the FIFO
 241  */
 242 static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 243 {
 244         u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
 245         u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
 246         int err, acc_sip, gyro_sip, read_len, samples, offset;
 247         struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 248         s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
 249         u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
 250         __le16 fifo_status;
 251
 252         err = regmap_bulk_read(hw->regmap,
 253                                hw->settings->fifo_ops.fifo_diff.addr,
 254                                &fifo_status, sizeof(fifo_status));
 255         if (err < 0)
 256                 return err;
 257
 258         if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
 259                 return 0;
 260
 261         fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
 262                    ST_LSM6DSX_CHAN_SIZE;
 263         samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
 264         fifo_len = (fifo_len / pattern_len) * pattern_len;
 265
 266         /*
 267          * compute delta timestamp between two consecutive samples
 268          * in order to estimate queueing time of data generated
 269          * by the sensor
 270          */
 271         acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 272         acc_ts = acc_sensor->ts - acc_sensor->delta_ts;
 273         acc_delta_ts = div_s64(acc_sensor->delta_ts * acc_sensor->decimator,
 274                                samples);
 275
 276         gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
 277         gyro_ts = gyro_sensor->ts - gyro_sensor->delta_ts;
 278         gyro_delta_ts = div_s64(gyro_sensor->delta_ts * gyro_sensor->decimator,
 279                                 samples);
 280
 281         for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 282                 err = st_lsm6dsx_read_block(hw, hw->buff, pattern_len);
 283                 if (err < 0)
 284                         return err;
 285
 286                 /*
 287                  * Data are written to the FIFO with a specific pattern
 288                  * depending on the configured ODRs. The first sequence of data
 289                  * stored in FIFO contains the data of all enabled sensors
 290                  * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
 291                  * depending on the value of the decimation factor set for each
 292                  * sensor.
 293                  *
 294                  * Supposing the FIFO is storing data from gyroscope and
 295                  * accelerometer at different ODRs:
 296                  *   - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
 297                  * Since the gyroscope ODR is twice the accelerometer one, the
 298                  * following pattern is repeated every 9 samples:
 299                  *   - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
 300                  */
 301                 gyro_sip = gyro_sensor->sip;
 302                 acc_sip = acc_sensor->sip;
 303                 offset = 0;
 304
 305                 while (acc_sip > 0 || gyro_sip > 0) {
 306                         if (gyro_sip-- > 0) {
 307                                 memcpy(iio_buff, &hw->buff[offset],
 308                                        ST_LSM6DSX_SAMPLE_SIZE);
 309                                 iio_push_to_buffers_with_timestamp(
 310                                         hw->iio_devs[ST_LSM6DSX_ID_GYRO],
 311                                         iio_buff, gyro_ts);
 312                                 offset += ST_LSM6DSX_SAMPLE_SIZE;
 313                                 gyro_ts += gyro_delta_ts;
 314                         }
 315
 316                         if (acc_sip-- > 0) {
 317                                 memcpy(iio_buff, &hw->buff[offset],
 318                                        ST_LSM6DSX_SAMPLE_SIZE);
 319                                 iio_push_to_buffers_with_timestamp(
 320                                         hw->iio_devs[ST_LSM6DSX_ID_ACC],
 321                                         iio_buff, acc_ts);
 322                                 offset += ST_LSM6DSX_SAMPLE_SIZE;
 323                                 acc_ts += acc_delta_ts;
 324                         }
 325                 }
 326         }
 327
 328         return read_len;
 329 }
 330
 331 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 332 {
 333         int err;
 334
 335         mutex_lock(&hw->fifo_lock);
 336
 337         st_lsm6dsx_read_fifo(hw);
 338         err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
 339
 340         mutex_unlock(&hw->fifo_lock);
 341
 342         return err;
 343 }
 344
 345 static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 346 {
 347         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 348         struct st_lsm6dsx_hw *hw = sensor->hw;
 349         int err;
 350
 351         mutex_lock(&hw->conf_lock);
 352
 353         if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
 354                 err = st_lsm6dsx_flush_fifo(hw);
 355                 if (err < 0)
 356                         goto out;
 357         }
 358
 359         if (enable) {
 360                 err = st_lsm6dsx_sensor_enable(sensor);
 361                 if (err < 0)
 362                         goto out;
 363         } else {
 364                 err = st_lsm6dsx_sensor_disable(sensor);
 365                 if (err < 0)
 366                         goto out;
 367         }
 368
 369         err = st_lsm6dsx_set_fifo_odr(sensor, enable);
 370         if (err < 0)
 371                 goto out;
 372
 373         err = st_lsm6dsx_update_decimators(hw);
 374         if (err < 0)
 375                 goto out;
 376
 377         err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
 378         if (err < 0)
 379                 goto out;
 380
 381         if (hw->enable_mask) {
 382                 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
 383                 if (err < 0)
 384                         goto out;
 385
 386                 /*
 387                  * store enable buffer timestamp as reference to compute
 388                  * first delta timestamp
 389                  */
 390                 sensor->ts = iio_get_time_ns(iio_dev);
 391         }
 392
 393 out:
 394         mutex_unlock(&hw->conf_lock);
 395
 396         return err;
 397 }
 398
 399 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 400 {
 401         struct st_lsm6dsx_hw *hw = private;
 402         struct st_lsm6dsx_sensor *sensor;
 403         int i;
 404
 405         if (!hw->sip)
 406                 return IRQ_NONE;
 407
 408         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 409                 sensor = iio_priv(hw->iio_devs[i]);
 410
 411                 if (sensor->sip > 0) {
 412                         s64 timestamp;
 413
 414                         timestamp = iio_get_time_ns(hw->iio_devs[i]);
 415                         sensor->delta_ts = timestamp - sensor->ts;
 416                         sensor->ts = timestamp;
 417                 }
 418         }
 419
 420         return IRQ_WAKE_THREAD;
 421 }
 422
 423 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 424 {
 425         struct st_lsm6dsx_hw *hw = private;
 426         int count;
 427
 428         mutex_lock(&hw->fifo_lock);
 429         count = st_lsm6dsx_read_fifo(hw);
 430         mutex_unlock(&hw->fifo_lock);
 431
 432         return !count ? IRQ_NONE : IRQ_HANDLED;
 433 }
 434
 435 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
 436 {
 437         return st_lsm6dsx_update_fifo(iio_dev, true);
 438 }
 439
 440 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
 441 {
 442         return st_lsm6dsx_update_fifo(iio_dev, false);
 443 }
 444
 445 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
 446         .preenable = st_lsm6dsx_buffer_preenable,
 447         .postdisable = st_lsm6dsx_buffer_postdisable,
 448 };
 449
 450 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 451 {
 452         struct device_node *np = hw->dev->of_node;
 453         struct st_sensors_platform_data *pdata;
 454         struct iio_buffer *buffer;
 455         unsigned long irq_type;
 456         bool irq_active_low;
 457         int i, err;
 458
 459         irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
 460
 461         switch (irq_type) {
 462         case IRQF_TRIGGER_HIGH:
 463         case IRQF_TRIGGER_RISING:
 464                 irq_active_low = false;
 465                 break;
 466         case IRQF_TRIGGER_LOW:
 467         case IRQF_TRIGGER_FALLING:
 468                 irq_active_low = true;
 469                 break;
 470         default:
 471                 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
 472                 return -EINVAL;
 473         }
 474
 475         err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
 476                                  ST_LSM6DSX_REG_HLACTIVE_MASK,
 477                                  FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
 478                                             irq_active_low));
 479         if (err < 0)
 480                 return err;
 481
 482         pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
 483         if ((np && of_property_read_bool(np, "drive-open-drain")) ||
 484             (pdata && pdata->open_drain)) {
 485                 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
 486                                          ST_LSM6DSX_REG_PP_OD_MASK,
 487                                          FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
 488                                                     1));
 489                 if (err < 0)
 490                         return err;
 491
 492                 irq_type |= IRQF_SHARED;
 493         }
 494
 495         err = devm_request_threaded_irq(hw->dev, hw->irq,
 496                                         st_lsm6dsx_handler_irq,
 497                                         st_lsm6dsx_handler_thread,
 498                                         irq_type | IRQF_ONESHOT,
 499                                         "lsm6dsx", hw);
 500         if (err) {
 501                 dev_err(hw->dev, "failed to request trigger irq %d\n",
 502                         hw->irq);
 503                 return err;
 504         }
 505
 506         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 507                 buffer = devm_iio_kfifo_allocate(hw->dev);
 508                 if (!buffer)
 509                         return -ENOMEM;
 510
 511                 iio_device_attach_buffer(hw->iio_devs[i], buffer);
 512                 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
 513                 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
 514         }
 515
 516         return 0;
 517 }