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 "inv_mpu_iio.h"
  17
  18 /**
  19  *  inv_mpu6050_update_period() - Update chip internal period estimation
  20  *
  21  *  @st:                driver state
  22  *  @timestamp:         the interrupt timestamp
  23  *  @nb:                number of data set in the fifo
  24  *
  25  *  This function uses interrupt timestamps to estimate the chip period and
  26  *  to choose the data timestamp to come.
  27  */
  28 static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
  29                                       s64 timestamp, size_t nb)
  30 {
  31         /* Period boundaries for accepting timestamp */
  32         const s64 period_min =
  33                 (NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100;
  34         const s64 period_max =
  35                 (NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100;
  36         const s32 divider = INV_MPU6050_FREQ_DIVIDER(st);
  37         s64 delta, interval;
  38         bool use_it_timestamp = false;
  39
  40         if (st->it_timestamp == 0) {
  41                 /* not initialized, forced to use it_timestamp */
  42                 use_it_timestamp = true;
  43         } else if (nb == 1) {
  44                 /*
  45                  * Validate the use of it timestamp by checking if interrupt
  46                  * has been delayed.
  47                  * nb > 1 means interrupt was delayed for more than 1 sample,
  48                  * so it's obviously not good.
  49                  * Compute the chip period between 2 interrupts for validating.
  50                  */
  51                 delta = div_s64(timestamp - st->it_timestamp, divider);
  52                 if (delta > period_min && delta < period_max) {
  53                         /* update chip period and use it timestamp */
  54                         st->chip_period = (st->chip_period + delta) / 2;
  55                         use_it_timestamp = true;
  56                 }
  57         }
  58
  59         if (use_it_timestamp) {
  60                 /*
  61                  * Manage case of multiple samples in the fifo (nb > 1):
  62                  * compute timestamp corresponding to the first sample using
  63                  * estimated chip period.
  64                  */
  65                 interval = (nb - 1) * st->chip_period * divider;
  66                 st->data_timestamp = timestamp - interval;
  67         }
  68
  69         /* save it timestamp */
  70         st->it_timestamp = timestamp;
  71 }
  72
  73 /**
  74  *  inv_mpu6050_get_timestamp() - Return the current data timestamp
  75  *
  76  *  @st:                driver state
  77  *  @return:            current data timestamp
  78  *
  79  *  This function returns the current data timestamp and prepares for next one.
  80  */
  81 static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
  82 {
  83         s64 ts;
  84
  85         /* return current data timestamp and increment */
  86         ts = st->data_timestamp;
  87         st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st);
  88
  89         return ts;
  90 }
  91
  92 static int inv_reset_fifo(struct iio_dev *indio_dev)
  93 {
  94         int result;
  95         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
  96
  97         /* disable fifo and reenable it */
  98         inv_mpu6050_prepare_fifo(st, false);
  99         result = inv_mpu6050_prepare_fifo(st, true);
 100         if (result)
 101                 goto reset_fifo_fail;
 102
 103         return 0;
 104
 105 reset_fifo_fail:
 106         dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
 107         result = regmap_write(st->map, st->reg->int_enable,
 108                               INV_MPU6050_BIT_DATA_RDY_EN);
 109
 110         return result;
 111 }
 112
 113 /*
 114  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
 115  */
 116 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 117 {
 118         struct iio_poll_func *pf = p;
 119         struct iio_dev *indio_dev = pf->indio_dev;
 120         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 121         size_t bytes_per_datum;
 122         int result;
 123         u16 fifo_count;
 124         s64 timestamp;
 125         int int_status;
 126         size_t i, nb;
 127
 128         mutex_lock(&st->lock);
 129
 130         /* ack interrupt and check status */
 131         result = regmap_read(st->map, st->reg->int_status, &int_status);
 132         if (result) {
 133                 dev_err(regmap_get_device(st->map),
 134                         "failed to ack interrupt\n");
 135                 goto flush_fifo;
 136         }
 137         if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
 138                 goto end_session;
 139
 140         if (!(st->chip_config.accl_fifo_enable |
 141                 st->chip_config.gyro_fifo_enable |
 142                 st->chip_config.magn_fifo_enable))
 143                 goto end_session;
 144         bytes_per_datum = 0;
 145         if (st->chip_config.accl_fifo_enable)
 146                 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 147
 148         if (st->chip_config.gyro_fifo_enable)
 149                 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 150
 151         if (st->chip_config.temp_fifo_enable)
 152                 bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
 153
 154         if (st->chip_config.magn_fifo_enable)
 155                 bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
 156
 157         /*
 158          * read fifo_count register to know how many bytes are inside the FIFO
 159          * right now
 160          */
 161         result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
 162                                   st->data, INV_MPU6050_FIFO_COUNT_BYTE);
 163         if (result)
 164                 goto end_session;
 165         fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
 166
 167         /*
 168          * Handle fifo overflow by resetting fifo.
 169          * Reset if there is only 3 data set free remaining to mitigate
 170          * possible delay between reading fifo count and fifo data.
 171          */
 172         nb = 3 * bytes_per_datum;
 173         if (fifo_count >= st->hw->fifo_size - nb) {
 174                 dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
 175                 goto flush_fifo;
 176         }
 177
 178         /* compute and process all complete datum */
 179         nb = fifo_count / bytes_per_datum;
 180         inv_mpu6050_update_period(st, pf->timestamp, nb);
 181         for (i = 0; i < nb; ++i) {
 182                 result = regmap_noinc_read(st->map, st->reg->fifo_r_w,
 183                                            st->data, bytes_per_datum);
 184                 if (result)
 185                         goto flush_fifo;
 186                 /* skip first samples if needed */
 187                 if (st->skip_samples) {
 188                         st->skip_samples--;
 189                         continue;
 190                 }
 191                 timestamp = inv_mpu6050_get_timestamp(st);
 192                 iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp);
 193         }
 194
 195 end_session:
 196         mutex_unlock(&st->lock);
 197         iio_trigger_notify_done(indio_dev->trig);
 198
 199         return IRQ_HANDLED;
 200
 201 flush_fifo:
 202         /* Flush HW and SW FIFOs. */
 203         inv_reset_fifo(indio_dev);
 204         mutex_unlock(&st->lock);
 205         iio_trigger_notify_done(indio_dev->trig);
 206
 207         return IRQ_HANDLED;
 208 }