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
  34 #include <linux/platform_data/st_sensors_pdata.h>
  35
  36 #include "st_lsm6dsx.h"
  37
  38 #define ST_LSM6DSX_REG_FIFO_THL_ADDR            0x06
  39 #define ST_LSM6DSX_REG_FIFO_THH_ADDR            0x07
  40 #define ST_LSM6DSX_FIFO_TH_MASK                 GENMASK(11, 0)
  41 #define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR        0x08
  42 #define ST_LSM6DSX_REG_HLACTIVE_ADDR            0x12
  43 #define ST_LSM6DSX_REG_HLACTIVE_MASK            BIT(5)
  44 #define ST_LSM6DSX_REG_PP_OD_ADDR               0x12
  45 #define ST_LSM6DSX_REG_PP_OD_MASK               BIT(4)
  46 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR           0x0a
  47 #define ST_LSM6DSX_FIFO_MODE_MASK               GENMASK(2, 0)
  48 #define ST_LSM6DSX_FIFO_ODR_MASK                GENMASK(6, 3)
  49 #define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR          0x3a
  50 #define ST_LSM6DSX_FIFO_DIFF_MASK               GENMASK(11, 0)
  51 #define ST_LSM6DSX_FIFO_EMPTY_MASK              BIT(12)
  52 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR           0x3e
  53
  54 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL             0x08
  55
  56 struct st_lsm6dsx_decimator_entry {
  57         u8 decimator;
  58         u8 val;
  59 };
  60
  61 static const
  62 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
  63         {  0, 0x0 },
  64         {  1, 0x1 },
  65         {  2, 0x2 },
  66         {  3, 0x3 },
  67         {  4, 0x4 },
  68         {  8, 0x5 },
  69         { 16, 0x6 },
  70         { 32, 0x7 },
  71 };
  72
  73 static int st_lsm6dsx_get_decimator_val(u8 val)
  74 {
  75         const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
  76         int i;
  77
  78         for (i = 0; i < max_size; i++)
  79                 if (st_lsm6dsx_decimator_table[i].decimator == val)
  80                         break;
  81
  82         return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
  83 }
  84
  85 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
  86                                        u16 *max_odr, u16 *min_odr)
  87 {
  88         struct st_lsm6dsx_sensor *sensor;
  89         int i;
  90
  91         *max_odr = 0, *min_odr = ~0;
  92         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
  93                 sensor = iio_priv(hw->iio_devs[i]);
  94
  95                 if (!(hw->enable_mask & BIT(sensor->id)))
  96                         continue;
  97
  98                 *max_odr = max_t(u16, *max_odr, sensor->odr);
  99                 *min_odr = min_t(u16, *min_odr, sensor->odr);
 100         }
 101 }
 102
 103 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 104 {
 105         struct st_lsm6dsx_sensor *sensor;
 106         u16 max_odr, min_odr, sip = 0;
 107         int err, i;
 108         u8 data;
 109
 110         st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
 111
 112         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 113                 sensor = iio_priv(hw->iio_devs[i]);
 114
 115                 /* update fifo decimators and sample in pattern */
 116                 if (hw->enable_mask & BIT(sensor->id)) {
 117                         sensor->sip = sensor->odr / min_odr;
 118                         sensor->decimator = max_odr / sensor->odr;
 119                         data = st_lsm6dsx_get_decimator_val(sensor->decimator);
 120                 } else {
 121                         sensor->sip = 0;
 122                         sensor->decimator = 0;
 123                         data = 0;
 124                 }
 125
 126                 err = st_lsm6dsx_write_with_mask(hw,
 127                                         ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR,
 128                                         sensor->decimator_mask, data);
 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         u8 data;
 143         int err;
 144
 145         switch (fifo_mode) {
 146         case ST_LSM6DSX_FIFO_BYPASS:
 147                 data = fifo_mode;
 148                 break;
 149         case ST_LSM6DSX_FIFO_CONT:
 150                 data = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
 151                         __ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
 152                 break;
 153         default:
 154                 return -EINVAL;
 155         }
 156
 157         err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
 158                             sizeof(data), &data);
 159         if (err < 0)
 160                 return err;
 161
 162         hw->fifo_mode = fifo_mode;
 163
 164         return 0;
 165 }
 166
 167 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
 168 {
 169         u16 fifo_watermark = ~0, cur_watermark, sip = 0;
 170         struct st_lsm6dsx_hw *hw = sensor->hw;
 171         struct st_lsm6dsx_sensor *cur_sensor;
 172         __le16 wdata;
 173         int i, err;
 174         u8 data;
 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 * ST_LSM6DSX_SAMPLE_DEPTH;
 195
 196         mutex_lock(&hw->lock);
 197
 198         err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_THH_ADDR,
 199                            sizeof(data), &data);
 200         if (err < 0)
 201                 goto out;
 202
 203         fifo_watermark = ((data << 8) & ~ST_LSM6DSX_FIFO_TH_MASK) |
 204                          (fifo_watermark & ST_LSM6DSX_FIFO_TH_MASK);
 205
 206         wdata = cpu_to_le16(fifo_watermark);
 207         err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
 208                             sizeof(wdata), (u8 *)&wdata);
 209 out:
 210         mutex_unlock(&hw->lock);
 211
 212         return err < 0 ? err : 0;
 213 }
 214
 215 /**
 216  * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DS3H-LSM6DSL-LSM6DSM read FIFO routine
 217  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
 218  *
 219  * Read samples from the hw FIFO and push them to IIO buffers.
 220  *
 221  * Return: Number of bytes read from the FIFO
 222  */
 223 static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 224 {
 225         u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
 226         int err, acc_sip, gyro_sip, read_len, samples, offset;
 227         struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 228         s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
 229         u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
 230         u8 buff[pattern_len];
 231         __le16 fifo_status;
 232
 233         err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
 234                            sizeof(fifo_status), (u8 *)&fifo_status);
 235         if (err < 0)
 236                 return err;
 237
 238         if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
 239                 return 0;
 240
 241         fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
 242                    ST_LSM6DSX_CHAN_SIZE;
 243         samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
 244         fifo_len = (fifo_len / pattern_len) * pattern_len;
 245
 246         /*
 247          * compute delta timestamp between two consecutive samples
 248          * in order to estimate queueing time of data generated
 249          * by the sensor
 250          */
 251         acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 252         acc_ts = acc_sensor->ts - acc_sensor->delta_ts;
 253         acc_delta_ts = div_s64(acc_sensor->delta_ts * acc_sensor->decimator,
 254                                samples);
 255
 256         gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
 257         gyro_ts = gyro_sensor->ts - gyro_sensor->delta_ts;
 258         gyro_delta_ts = div_s64(gyro_sensor->delta_ts * gyro_sensor->decimator,
 259                                 samples);
 260
 261         for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 262                 err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
 263                                    sizeof(buff), buff);
 264                 if (err < 0)
 265                         return err;
 266
 267                 /*
 268                  * Data are written to the FIFO with a specific pattern
 269                  * depending on the configured ODRs. The first sequence of data
 270                  * stored in FIFO contains the data of all enabled sensors
 271                  * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
 272                  * depending on the value of the decimation factor set for each
 273                  * sensor.
 274                  *
 275                  * Supposing the FIFO is storing data from gyroscope and
 276                  * accelerometer at different ODRs:
 277                  *   - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
 278                  * Since the gyroscope ODR is twice the accelerometer one, the
 279                  * following pattern is repeated every 9 samples:
 280                  *   - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
 281                  */
 282                 gyro_sip = gyro_sensor->sip;
 283                 acc_sip = acc_sensor->sip;
 284                 offset = 0;
 285
 286                 while (acc_sip > 0 || gyro_sip > 0) {
 287                         if (gyro_sip-- > 0) {
 288                                 memcpy(iio_buff, &buff[offset],
 289                                        ST_LSM6DSX_SAMPLE_SIZE);
 290                                 iio_push_to_buffers_with_timestamp(
 291                                         hw->iio_devs[ST_LSM6DSX_ID_GYRO],
 292                                         iio_buff, gyro_ts);
 293                                 offset += ST_LSM6DSX_SAMPLE_SIZE;
 294                                 gyro_ts += gyro_delta_ts;
 295                         }
 296
 297                         if (acc_sip-- > 0) {
 298                                 memcpy(iio_buff, &buff[offset],
 299                                        ST_LSM6DSX_SAMPLE_SIZE);
 300                                 iio_push_to_buffers_with_timestamp(
 301                                         hw->iio_devs[ST_LSM6DSX_ID_ACC],
 302                                         iio_buff, acc_ts);
 303                                 offset += ST_LSM6DSX_SAMPLE_SIZE;
 304                                 acc_ts += acc_delta_ts;
 305                         }
 306                 }
 307         }
 308
 309         return read_len;
 310 }
 311
 312 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 313 {
 314         int err;
 315
 316         mutex_lock(&hw->fifo_lock);
 317
 318         st_lsm6dsx_read_fifo(hw);
 319         err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
 320
 321         mutex_unlock(&hw->fifo_lock);
 322
 323         return err;
 324 }
 325
 326 static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 327 {
 328         struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 329         struct st_lsm6dsx_hw *hw = sensor->hw;
 330         int err;
 331
 332         if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
 333                 err = st_lsm6dsx_flush_fifo(hw);
 334                 if (err < 0)
 335                         return err;
 336         }
 337
 338         if (enable) {
 339                 err = st_lsm6dsx_sensor_enable(sensor);
 340                 if (err < 0)
 341                         return err;
 342         } else {
 343                 err = st_lsm6dsx_sensor_disable(sensor);
 344                 if (err < 0)
 345                         return err;
 346         }
 347
 348         err = st_lsm6dsx_update_decimators(hw);
 349         if (err < 0)
 350                 return err;
 351
 352         err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
 353         if (err < 0)
 354                 return err;
 355
 356         if (hw->enable_mask) {
 357                 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
 358                 if (err < 0)
 359                         return err;
 360
 361                 /*
 362                  * store enable buffer timestamp as reference to compute
 363                  * first delta timestamp
 364                  */
 365                 sensor->ts = iio_get_time_ns(iio_dev);
 366         }
 367
 368         return 0;
 369 }
 370
 371 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 372 {
 373         struct st_lsm6dsx_hw *hw = private;
 374         struct st_lsm6dsx_sensor *sensor;
 375         int i;
 376
 377         if (!hw->sip)
 378                 return IRQ_NONE;
 379
 380         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 381                 sensor = iio_priv(hw->iio_devs[i]);
 382
 383                 if (sensor->sip > 0) {
 384                         s64 timestamp;
 385
 386                         timestamp = iio_get_time_ns(hw->iio_devs[i]);
 387                         sensor->delta_ts = timestamp - sensor->ts;
 388                         sensor->ts = timestamp;
 389                 }
 390         }
 391
 392         return IRQ_WAKE_THREAD;
 393 }
 394
 395 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 396 {
 397         struct st_lsm6dsx_hw *hw = private;
 398         int fifo_len = 0, len;
 399
 400         /*
 401          * If we are using edge IRQs, new samples can arrive while
 402          * processing current interrupt since there are no hw
 403          * guarantees the irq line stays "low" long enough to properly
 404          * detect the new interrupt. In this case the new sample will
 405          * be missed.
 406          * Polling FIFO status register allow us to read new
 407          * samples even if the interrupt arrives while processing
 408          * previous data and the timeslot where the line is "low" is
 409          * too short to be properly detected.
 410          */
 411         do {
 412                 mutex_lock(&hw->fifo_lock);
 413                 len = st_lsm6dsx_read_fifo(hw);
 414                 mutex_unlock(&hw->fifo_lock);
 415
 416                 if (len > 0)
 417                         fifo_len += len;
 418         } while (len > 0);
 419
 420         return fifo_len ? IRQ_HANDLED : IRQ_NONE;
 421 }
 422
 423 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
 424 {
 425         return st_lsm6dsx_update_fifo(iio_dev, true);
 426 }
 427
 428 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
 429 {
 430         return st_lsm6dsx_update_fifo(iio_dev, false);
 431 }
 432
 433 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
 434         .preenable = st_lsm6dsx_buffer_preenable,
 435         .postdisable = st_lsm6dsx_buffer_postdisable,
 436 };
 437
 438 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 439 {
 440         struct device_node *np = hw->dev->of_node;
 441         struct st_sensors_platform_data *pdata;
 442         struct iio_buffer *buffer;
 443         unsigned long irq_type;
 444         bool irq_active_low;
 445         int i, err;
 446
 447         irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
 448
 449         switch (irq_type) {
 450         case IRQF_TRIGGER_HIGH:
 451         case IRQF_TRIGGER_RISING:
 452                 irq_active_low = false;
 453                 break;
 454         case IRQF_TRIGGER_LOW:
 455         case IRQF_TRIGGER_FALLING:
 456                 irq_active_low = true;
 457                 break;
 458         default:
 459                 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
 460                 return -EINVAL;
 461         }
 462
 463         err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_HLACTIVE_ADDR,
 464                                          ST_LSM6DSX_REG_HLACTIVE_MASK,
 465                                          irq_active_low);
 466         if (err < 0)
 467                 return err;
 468
 469         pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
 470         if ((np && of_property_read_bool(np, "drive-open-drain")) ||
 471             (pdata && pdata->open_drain)) {
 472                 err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_PP_OD_ADDR,
 473                                                  ST_LSM6DSX_REG_PP_OD_MASK, 1);
 474                 if (err < 0)
 475                         return err;
 476
 477                 irq_type |= IRQF_SHARED;
 478         }
 479
 480         err = devm_request_threaded_irq(hw->dev, hw->irq,
 481                                         st_lsm6dsx_handler_irq,
 482                                         st_lsm6dsx_handler_thread,
 483                                         irq_type | IRQF_ONESHOT,
 484                                         "lsm6dsx", hw);
 485         if (err) {
 486                 dev_err(hw->dev, "failed to request trigger irq %d\n",
 487                         hw->irq);
 488                 return err;
 489         }
 490
 491         for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 492                 buffer = devm_iio_kfifo_allocate(hw->dev);
 493                 if (!buffer)
 494                         return -ENOMEM;
 495
 496                 iio_device_attach_buffer(hw->iio_devs[i], buffer);
 497                 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
 498                 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
 499         }
 500
 501         return 0;
 502 }