drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

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