drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

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