iio: imu: inv_mpu6050: new timestamp mechanism

Check validity of interrupt timestamps by computing time between
2 interrupts. If it matches the chip frequency modulo 4%, it is
used as the data timestamp and also for estimating the chip
frequency measured from the system. Otherwise timestamp is
computed using the estimated chip frequency.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index f50477c360e27b3c463a4adde395862c61f01642..de68e83fc52d51d7973536a0c55c1e0c8b473617 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -295,6 +295,13 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
        memcpy(&st->chip_config, hw_info[st->chip_type].config,
               sizeof(struct inv_mpu6050_chip_config));
 
+       /*
+        * Internal chip period is 1ms (1kHz).
+        * Let's use at the beginning the theorical value before measuring
+        * with interrupt timestamps.
+        */
+       st->chip_period = NSEC_PER_MSEC;
+
        return inv_mpu6050_set_power_itg(st, false);
 
 error_power_off:

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 6bc80ac9d120339b7174a302bc769a3beeece659..de8391693e17611f27c2c2dc5620a6b31773a174 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -125,6 +125,9 @@ struct inv_mpu6050_hw {
  *  @map               regmap pointer.
  *  @irq               interrupt number.
  *  @irq_mask          the int_pin_cfg mask to configure interrupt type.
+ *  @chip_period:      chip internal period estimation (~1kHz).
+ *  @it_timestamp:     timestamp from previous interrupt.
+ *  @data_timestamp:   timestamp for next data sample.
  */
 struct inv_mpu6050_state {
        struct mutex lock;
@@ -142,6 +145,9 @@ struct inv_mpu6050_state {
        int irq;
        u8 irq_mask;
        unsigned skip_samples;
+       s64 chip_period;
+       s64 it_timestamp;
+       s64 data_timestamp;
 };
 
 /*register and associated bit definition*/
@@ -223,6 +229,8 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_LATCH_INT_EN       0x20
 #define INV_MPU6050_BIT_BYPASS_EN      0x2
 
+/* Allowed timestamp period jitter in percent */
+#define INV_MPU6050_TS_PERIOD_JITTER   4
 
 /* init parameters */
 #define INV_MPU6050_INIT_FIFO_RATE           50

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 7a4aaed830446bea6145d7c264de0ff67dfba761..548e042f7b5bde8b508675b23f2070a78bef14a9 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -20,15 +20,93 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/poll.h>
+#include <linux/math64.h>
 #include <asm/unaligned.h>
 #include "inv_mpu_iio.h"
 
+/**
+ *  inv_mpu6050_update_period() - Update chip internal period estimation
+ *
+ *  @st:               driver state
+ *  @timestamp:                the interrupt timestamp
+ *  @nb:               number of data set in the fifo
+ *
+ *  This function uses interrupt timestamps to estimate the chip period and
+ *  to choose the data timestamp to come.
+ */
+static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
+                                     s64 timestamp, size_t nb)
+{
+       /* Period boundaries for accepting timestamp */
+       const s64 period_min =
+               (NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100;
+       const s64 period_max =
+               (NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100;
+       const s32 divider = INV_MPU6050_FREQ_DIVIDER(st);
+       s64 delta, interval;
+       bool use_it_timestamp = false;
+
+       if (st->it_timestamp == 0) {
+               /* not initialized, forced to use it_timestamp */
+               use_it_timestamp = true;
+       } else if (nb == 1) {
+               /*
+                * Validate the use of it timestamp by checking if interrupt
+                * has been delayed.
+                * nb > 1 means interrupt was delayed for more than 1 sample,
+                * so it's obviously not good.
+                * Compute the chip period between 2 interrupts for validating.
+                */
+               delta = div_s64(timestamp - st->it_timestamp, divider);
+               if (delta > period_min && delta < period_max) {
+                       /* update chip period and use it timestamp */
+                       st->chip_period = (st->chip_period + delta) / 2;
+                       use_it_timestamp = true;
+               }
+       }
+
+       if (use_it_timestamp) {
+               /*
+                * Manage case of multiple samples in the fifo (nb > 1):
+                * compute timestamp corresponding to the first sample using
+                * estimated chip period.
+                */
+               interval = (nb - 1) * st->chip_period * divider;
+               st->data_timestamp = timestamp - interval;
+       }
+
+       /* save it timestamp */
+       st->it_timestamp = timestamp;
+}
+
+/**
+ *  inv_mpu6050_get_timestamp() - Return the current data timestamp
+ *
+ *  @st:               driver state
+ *  @return:           current data timestamp
+ *
+ *  This function returns the current data timestamp and prepares for next one.
+ */
+static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
+{
+       s64 ts;
+
+       /* return current data timestamp and increment */
+       ts = st->data_timestamp;
+       st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st);
+
+       return ts;
+}
+
 int inv_reset_fifo(struct iio_dev *indio_dev)
 {
        int result;
        u8 d;
        struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 
+       /* reset it timestamp validation */
+       st->it_timestamp = 0;
+
        /* disable interrupt */
        result = regmap_write(st->map, st->reg->int_enable, 0);
        if (result) {
@@ -97,7 +175,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
        int result;
        u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
        u16 fifo_count;
-       s64 timestamp = pf->timestamp;
+       s64 timestamp;
        int int_status;
        size_t i, nb;
 
@@ -140,6 +218,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
        fifo_count = get_unaligned_be16(&data[0]);
        /* compute and process all complete datum */
        nb = fifo_count / bytes_per_datum;
+       inv_mpu6050_update_period(st, pf->timestamp, nb);
        for (i = 0; i < nb; ++i) {
                result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
                                          data, bytes_per_datum);
@@ -150,6 +229,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
                        st->skip_samples--;
                        continue;
                }
+               timestamp = inv_mpu6050_get_timestamp(st);
                iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
        }