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@xxxxxxxxxxxxxx>
---
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 7 ++
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 8 +++
drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 81 +++++++++++++++++++++-
3 files changed, 95 insertions(+), 1 deletion(-)
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 2c3e666aa970..93c64f30b615 100644
--- 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 6bc80ac9d120..de8391693e17 100644
--- 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 7a4aaed83044..6930af413397 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -23,12 +23,89 @@
#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 unsigned 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 = (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 +174,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 +217,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 +228,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);
}
--
2.17.0
--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
