drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

   1 /*
   2 * Copyright (C) 2012 Invensense, Inc.
   3 *
   4 * This software is licensed under the terms of the GNU General Public
   5 * License version 2, as published by the Free Software Foundation, and
   6 * may be copied, distributed, and modified under those terms.
   7 *
   8 * This program is distributed in the hope that it will be useful,
   9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11 * GNU General Public License for more details.
  12 */
  13
  14 #include <linux/module.h>
  15 #include <linux/slab.h>
  16 #include <linux/err.h>
  17 #include <linux/delay.h>
  18 #include <linux/sysfs.h>
  19 #include <linux/jiffies.h>
  20 #include <linux/irq.h>
  21 #include <linux/interrupt.h>
  22 #include <linux/poll.h>
  23 #include <linux/math64.h>
  24 #include <asm/unaligned.h>
  25 #include "inv_mpu_iio.h"
  26
  27 /**
  28  *  inv_mpu6050_update_period() - Update chip internal period estimation
  29  *
  30  *  @st:                driver state
  31  *  @timestamp:         the interrupt timestamp
  32  *  @nb:                number of data set in the fifo
  33  *
  34  *  This function uses interrupt timestamps to estimate the chip period and
  35  *  to choose the data timestamp to come.
  36  */
  37 static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
  38                                       s64 timestamp, size_t nb)
  39 {
  40         /* Period boundaries for accepting timestamp */
  41         const s64 period_min =
  42                 (NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100;
  43         const s64 period_max =
  44                 (NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100;
  45         const s32 divider = INV_MPU6050_FREQ_DIVIDER(st);
  46         s64 delta, interval;
  47         bool use_it_timestamp = false;
  48
  49         if (st->it_timestamp == 0) {
  50                 /* not initialized, forced to use it_timestamp */
  51                 use_it_timestamp = true;
  52         } else if (nb == 1) {
  53                 /*
  54                  * Validate the use of it timestamp by checking if interrupt
  55                  * has been delayed.
  56                  * nb > 1 means interrupt was delayed for more than 1 sample,
  57                  * so it's obviously not good.
  58                  * Compute the chip period between 2 interrupts for validating.
  59                  */
  60                 delta = div_s64(timestamp - st->it_timestamp, divider);
  61                 if (delta > period_min && delta < period_max) {
  62                         /* update chip period and use it timestamp */
  63                         st->chip_period = (st->chip_period + delta) / 2;
  64                         use_it_timestamp = true;
  65                 }
  66         }
  67
  68         if (use_it_timestamp) {
  69                 /*
  70                  * Manage case of multiple samples in the fifo (nb > 1):
  71                  * compute timestamp corresponding to the first sample using
  72                  * estimated chip period.
  73                  */
  74                 interval = (nb - 1) * st->chip_period * divider;
  75                 st->data_timestamp = timestamp - interval;
  76         }
  77
  78         /* save it timestamp */
  79         st->it_timestamp = timestamp;
  80 }
  81
  82 /**
  83  *  inv_mpu6050_get_timestamp() - Return the current data timestamp
  84  *
  85  *  @st:                driver state
  86  *  @return:            current data timestamp
  87  *
  88  *  This function returns the current data timestamp and prepares for next one.
  89  */
  90 static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
  91 {
  92         s64 ts;
  93
  94         /* return current data timestamp and increment */
  95         ts = st->data_timestamp;
  96         st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st);
  97
  98         return ts;
  99 }
 100
 101 int inv_reset_fifo(struct iio_dev *indio_dev)
 102 {
 103         int result;
 104         u8 d;
 105         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 106
 107         /* reset it timestamp validation */
 108         st->it_timestamp = 0;
 109
 110         /* disable interrupt */
 111         result = regmap_write(st->map, st->reg->int_enable, 0);
 112         if (result) {
 113                 dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
 114                         result);
 115                 return result;
 116         }
 117         /* disable the sensor output to FIFO */
 118         result = regmap_write(st->map, st->reg->fifo_en, 0);
 119         if (result)
 120                 goto reset_fifo_fail;
 121         /* disable fifo reading */
 122         result = regmap_write(st->map, st->reg->user_ctrl,
 123                               st->chip_config.user_ctrl);
 124         if (result)
 125                 goto reset_fifo_fail;
 126
 127         /* reset FIFO*/
 128         d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
 129         result = regmap_write(st->map, st->reg->user_ctrl, d);
 130         if (result)
 131                 goto reset_fifo_fail;
 132
 133         /* enable interrupt */
 134         if (st->chip_config.accl_fifo_enable ||
 135             st->chip_config.gyro_fifo_enable) {
 136                 result = regmap_write(st->map, st->reg->int_enable,
 137                                       INV_MPU6050_BIT_DATA_RDY_EN);
 138                 if (result)
 139                         return result;
 140         }
 141         /* enable FIFO reading */
 142         d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
 143         result = regmap_write(st->map, st->reg->user_ctrl, d);
 144         if (result)
 145                 goto reset_fifo_fail;
 146         /* enable sensor output to FIFO */
 147         d = 0;
 148         if (st->chip_config.gyro_fifo_enable)
 149                 d |= INV_MPU6050_BITS_GYRO_OUT;
 150         if (st->chip_config.accl_fifo_enable)
 151                 d |= INV_MPU6050_BIT_ACCEL_OUT;
 152         result = regmap_write(st->map, st->reg->fifo_en, d);
 153         if (result)
 154                 goto reset_fifo_fail;
 155
 156         return 0;
 157
 158 reset_fifo_fail:
 159         dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
 160         result = regmap_write(st->map, st->reg->int_enable,
 161                               INV_MPU6050_BIT_DATA_RDY_EN);
 162
 163         return result;
 164 }
 165
 166 /**
 167  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
 168  */
 169 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 170 {
 171         struct iio_poll_func *pf = p;
 172         struct iio_dev *indio_dev = pf->indio_dev;
 173         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 174         size_t bytes_per_datum;
 175         int result;
 176         u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
 177         u16 fifo_count;
 178         s64 timestamp;
 179         int int_status;
 180         size_t i, nb;
 181
 182         mutex_lock(&st->lock);
 183
 184         /* ack interrupt and check status */
 185         result = regmap_read(st->map, st->reg->int_status, &int_status);
 186         if (result) {
 187                 dev_err(regmap_get_device(st->map),
 188                         "failed to ack interrupt\n");
 189                 goto flush_fifo;
 190         }
 191         /* handle fifo overflow by reseting fifo */
 192         if (int_status & INV_MPU6050_BIT_FIFO_OVERFLOW_INT)
 193                 goto flush_fifo;
 194         if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) {
 195                 dev_warn(regmap_get_device(st->map),
 196                         "spurious interrupt with status 0x%x\n", int_status);
 197                 goto end_session;
 198         }
 199
 200         if (!(st->chip_config.accl_fifo_enable |
 201                 st->chip_config.gyro_fifo_enable))
 202                 goto end_session;
 203         bytes_per_datum = 0;
 204         if (st->chip_config.accl_fifo_enable)
 205                 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 206
 207         if (st->chip_config.gyro_fifo_enable)
 208                 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 209
 210         /*
 211          * read fifo_count register to know how many bytes are inside the FIFO
 212          * right now
 213          */
 214         result = regmap_bulk_read(st->map, st->reg->fifo_count_h, data,
 215                                   INV_MPU6050_FIFO_COUNT_BYTE);
 216         if (result)
 217                 goto end_session;
 218         fifo_count = get_unaligned_be16(&data[0]);
 219         /* compute and process all complete datum */
 220         nb = fifo_count / bytes_per_datum;
 221         inv_mpu6050_update_period(st, pf->timestamp, nb);
 222         for (i = 0; i < nb; ++i) {
 223                 result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
 224                                           data, bytes_per_datum);
 225                 if (result)
 226                         goto flush_fifo;
 227                 /* skip first samples if needed */
 228                 if (st->skip_samples) {
 229                         st->skip_samples--;
 230                         continue;
 231                 }
 232                 timestamp = inv_mpu6050_get_timestamp(st);
 233                 iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
 234         }
 235
 236 end_session:
 237         mutex_unlock(&st->lock);
 238         iio_trigger_notify_done(indio_dev->trig);
 239
 240         return IRQ_HANDLED;
 241
 242 flush_fifo:
 243         /* Flush HW and SW FIFOs. */
 244         inv_reset_fifo(indio_dev);
 245         mutex_unlock(&st->lock);
 246         iio_trigger_notify_done(indio_dev->trig);
 247
 248         return IRQ_HANDLED;
 249 }