[PATCH 1/5] iio: imu: inv_mpu6050: replace timestamp fifo by generic timestamp

[PATCH 1/5] iio: imu: inv_mpu6050: replace timestamp fifo by generic timestamp

[Jean-Baptiste Maneyrol]

Using a fifo for storing timestamps is useless since the interrupt
is in one-shot mode, preventing the hard irq handler to be called
when the irq thread is running. Instead use the generic timestamp
function iio_pollfunc_store_time.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c |  6 +---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  | 10 -------
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 44 +-----------------------------
 3 files changed, 2 insertions(+), 58 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 43fba5f..1e7e750 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -20,8 +20,6 @@
 #include <linux/jiffies.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
-#include <linux/kfifo.h>
-#include <linux/spinlock.h>
 #include <linux/iio/iio.h>
 #include <linux/acpi.h>
 #include <linux/platform_device.h>
@@ -996,7 +994,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 	indio_dev->modes = INDIO_BUFFER_TRIGGERED;
 
 	result = devm_iio_triggered_buffer_setup(dev, indio_dev,
-						 inv_mpu6050_irq_handler,
+						 iio_pollfunc_store_time,
 						 inv_mpu6050_read_fifo,
 						 NULL);
 	if (result) {
@@ -1009,8 +1007,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		return result;
 	}
 
-	INIT_KFIFO(st->timestamps);
-	spin_lock_init(&st->time_stamp_lock);
 	result = devm_iio_device_register(dev, indio_dev);
 	if (result) {
 		dev_err(dev, "IIO register fail %d\n", result);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index c54da77..a92ddd4 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -12,8 +12,6 @@
 */
 #include <linux/i2c.h>
 #include <linux/i2c-mux.h>
-#include <linux/kfifo.h>
-#include <linux/spinlock.h>
 #include <linux/mutex.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
@@ -116,36 +114,30 @@ struct inv_mpu6050_hw {
 
 /*
  *  struct inv_mpu6050_state - Driver state variables.
- *  @TIMESTAMP_FIFO_SIZE: fifo size for timestamp.
  *  @lock:              Chip access lock.
  *  @trig:              IIO trigger.
  *  @chip_config:	Cached attribute information.
  *  @reg:		Map of important registers.
  *  @hw:		Other hardware-specific information.
  *  @chip_type:		chip type.
- *  @time_stamp_lock:	spin lock to time stamp.
  *  @plat_data:		platform data (deprecated in favor of @orientation).
  *  @orientation:	sensor chip orientation relative to main hardware.
- *  @timestamps:        kfifo queue to store time stamp.
  *  @map		regmap pointer.
  *  @irq		interrupt number.
  *  @irq_mask		the int_pin_cfg mask to configure interrupt type.
  */
 struct inv_mpu6050_state {
-#define TIMESTAMP_FIFO_SIZE 16
 	struct mutex lock;
 	struct iio_trigger  *trig;
 	struct inv_mpu6050_chip_config chip_config;
 	const struct inv_mpu6050_reg_map *reg;
 	const struct inv_mpu6050_hw *hw;
 	enum   inv_devices chip_type;
-	spinlock_t time_stamp_lock;
 	struct i2c_mux_core *muxc;
 	struct i2c_client *mux_client;
 	unsigned int powerup_count;
 	struct inv_mpu6050_platform_data plat_data;
 	struct iio_mount_matrix orientation;
-	DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE);
 	struct regmap *map;
 	int irq;
 	u8 irq_mask;
@@ -234,7 +226,6 @@ struct inv_mpu6050_state {
 
 /* init parameters */
 #define INV_MPU6050_INIT_FIFO_RATE           50
-#define INV_MPU6050_TIME_STAMP_TOR           5
 #define INV_MPU6050_MAX_FIFO_RATE            1000
 #define INV_MPU6050_MIN_FIFO_RATE            4
 #define INV_MPU6050_ONE_K_HZ                 1000
@@ -300,7 +291,6 @@ enum inv_mpu6050_clock_sel_e {
 	NUM_CLK
 };
 
-irqreturn_t inv_mpu6050_irq_handler(int irq, void *p);
 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p);
 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type);
 int inv_reset_fifo(struct iio_dev *indio_dev);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 1795418..5436d18 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -19,20 +19,9 @@
 #include <linux/jiffies.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
-#include <linux/kfifo.h>
 #include <linux/poll.h>
 #include "inv_mpu_iio.h"
 
-static void inv_clear_kfifo(struct inv_mpu6050_state *st)
-{
-	unsigned long flags;
-
-	/* take the spin lock sem to avoid interrupt kick in */
-	spin_lock_irqsave(&st->time_stamp_lock, flags);
-	kfifo_reset(&st->timestamps);
-	spin_unlock_irqrestore(&st->time_stamp_lock, flags);
-}
-
 int inv_reset_fifo(struct iio_dev *indio_dev)
 {
 	int result;
@@ -62,9 +51,6 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 	if (result)
 		goto reset_fifo_fail;
 
-	/* clear timestamps fifo */
-	inv_clear_kfifo(st);
-
 	/* enable interrupt */
 	if (st->chip_config.accl_fifo_enable ||
 	    st->chip_config.gyro_fifo_enable) {
@@ -99,23 +85,6 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 }
 
 /**
- * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
- */
-irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
-{
-	struct iio_poll_func *pf = p;
-	struct iio_dev *indio_dev = pf->indio_dev;
-	struct inv_mpu6050_state *st = iio_priv(indio_dev);
-	s64 timestamp;
-
-	timestamp = iio_get_time_ns(indio_dev);
-	kfifo_in_spinlocked(&st->timestamps, &timestamp, 1,
-			    &st->time_stamp_lock);
-
-	return IRQ_WAKE_THREAD;
-}
-
-/**
  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
  */
 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
@@ -127,7 +96,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;
+	s64 timestamp = pf->timestamp;
 	int int_status;
 
 	mutex_lock(&st->lock);
@@ -171,28 +140,17 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 		goto flush_fifo;
 	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
 		goto flush_fifo;
-	/* Timestamp mismatch. */
-	if (kfifo_len(&st->timestamps) >
-	    fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
-		goto flush_fifo;
 	do {
 		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
 					  data, bytes_per_datum);
 		if (result)
 			goto flush_fifo;
-
-		result = kfifo_out(&st->timestamps, &timestamp, 1);
-		/* when there is no timestamp, put timestamp as 0 */
-		if (result == 0)
-			timestamp = 0;
-
 		/* skip first samples if needed */
 		if (st->skip_samples)
 			st->skip_samples--;
 		else
 			iio_push_to_buffers_with_timestamp(indio_dev, data,
 							   timestamp);
-
 		fifo_count -= bytes_per_datum;
 	} while (fifo_count >= bytes_per_datum);
 
-- 
2.7.4

[PATCH 2/5] iio: imu: inv_mpu6050: switch to use sample rate divider

[Jean-Baptiste Maneyrol]

Instead of storing fifo rate in Hz, store the chip internal sample
rate divider. This will be more useful for timestamping. There
are both equivalent.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 15 +++++++++------
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  4 ++--
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 1e7e750..9e5c5e7 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -82,7 +82,7 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = {
 static const struct inv_mpu6050_chip_config chip_config_6050 = {
 	.fsr = INV_MPU6050_FSR_2000DPS,
 	.lpf = INV_MPU6050_FILTER_20HZ,
-	.fifo_rate = INV_MPU6050_INIT_FIFO_RATE,
+	.divider = (INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE) - 1,
 	.gyro_fifo_enable = false,
 	.accl_fifo_enable = false,
 	.accl_fs = INV_MPU6050_FS_02G,
@@ -628,7 +628,7 @@ static ssize_t
 inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
 			    const char *buf, size_t count)
 {
-	s32 fifo_rate;
+	int fifo_rate;
 	u8 d;
 	int result;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -644,8 +644,12 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
 	if (result)
 		return result;
 
+	/* compute freq divider and update fifo rate in case of truncation */
+	d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
+	fifo_rate = INV_MPU6050_ONE_K_HZ / (d + 1);
+
 	mutex_lock(&st->lock);
-	if (fifo_rate == st->chip_config.fifo_rate) {
+	if (d == st->chip_config.divider) {
 		result = 0;
 		goto fifo_rate_fail_unlock;
 	}
@@ -653,11 +657,10 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
 	if (result)
 		goto fifo_rate_fail_unlock;
 
-	d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
 	result = regmap_write(st->map, st->reg->sample_rate_div, d);
 	if (result)
 		goto fifo_rate_fail_power_off;
-	st->chip_config.fifo_rate = fifo_rate;
+	st->chip_config.divider = d;
 
 	result = inv_mpu6050_set_lpf(st, fifo_rate);
 	if (result)
@@ -685,7 +688,7 @@ inv_fifo_rate_show(struct device *dev, struct device_attribute *attr,
 	unsigned fifo_rate;
 
 	mutex_lock(&st->lock);
-	fifo_rate = st->chip_config.fifo_rate;
+	fifo_rate = INV_MPU6050_ONE_K_HZ / (st->chip_config.divider + 1);
 	mutex_unlock(&st->lock);
 
 	return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index a92ddd4..8d9044c 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -86,7 +86,7 @@ enum inv_devices {
  *  @accl_fs:		accel full scale range.
  *  @accl_fifo_enable:	enable accel data output
  *  @gyro_fifo_enable:	enable gyro data output
- *  @fifo_rate:		FIFO update rate.
+ *  @divider:		sample rate divider.
  */
 struct inv_mpu6050_chip_config {
 	unsigned int fsr:2;
@@ -94,7 +94,7 @@ struct inv_mpu6050_chip_config {
 	unsigned int accl_fs:2;
 	unsigned int accl_fifo_enable:1;
 	unsigned int gyro_fifo_enable:1;
-	u16 fifo_rate;
+	u8 divider;
 	u8 user_ctrl;
 };
 
-- 
2.7.4

[PATCH 3/5] iio: imu: inv_mpu6050: fix fifo count reading

[Jean-Baptiste Maneyrol]

Use unaligned access since buffer is a bytes table. Truncate fifo
count to read only complete datum.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 24 +++++++++++-------------
 1 file changed, 11 insertions(+), 13 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 5436d18..7724888 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -20,6 +20,7 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/poll.h>
+#include <asm/unaligned.h>
 #include "inv_mpu_iio.h"
 
 int inv_reset_fifo(struct iio_dev *indio_dev)
@@ -98,6 +99,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	u16 fifo_count;
 	s64 timestamp = pf->timestamp;
 	int int_status;
+	size_t i, nb;
 
 	mutex_lock(&st->lock);
 
@@ -132,27 +134,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 				  INV_MPU6050_FIFO_COUNT_BYTE);
 	if (result)
 		goto end_session;
-	fifo_count = be16_to_cpup((__be16 *)(&data[0]));
-	if (fifo_count < bytes_per_datum)
-		goto end_session;
-	/* fifo count can't be an odd number. If it is odd, reset the FIFO. */
-	if (fifo_count & 1)
-		goto flush_fifo;
+	fifo_count = get_unaligned_be16(&data[0]);
 	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
 		goto flush_fifo;
-	do {
+	/* compute and process all complete datum */
+	nb = fifo_count / bytes_per_datum;
+	for (i = 0; i < nb; ++i) {
 		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
 					  data, bytes_per_datum);
 		if (result)
 			goto flush_fifo;
 		/* skip first samples if needed */
-		if (st->skip_samples)
+		if (st->skip_samples) {
 			st->skip_samples--;
-		else
-			iio_push_to_buffers_with_timestamp(indio_dev, data,
-							   timestamp);
-		fifo_count -= bytes_per_datum;
-	} while (fifo_count >= bytes_per_datum);
+			continue;
+		}
+		iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
+	}
 
 end_session:
 	mutex_unlock(&st->lock);
-- 
2.7.4

[PATCH 4/5] iio: imu: inv_mpu6050: better fifo overflow handling

[Jean-Baptiste Maneyrol]

Use fifo overflow bit from int status rather than using an
arbitrary threshold.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  | 2 +-
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 5 +++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 8d9044c..09a7e1c 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -166,6 +166,7 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_REG_RAW_GYRO            0x43
 
 #define INV_MPU6050_REG_INT_STATUS          0x3A
+#define INV_MPU6050_BIT_FIFO_OVERFLOW_INT   0x10
 #define INV_MPU6050_BIT_RAW_DATA_RDY_INT    0x01
 
 #define INV_MPU6050_REG_USER_CTRL           0x6A
@@ -190,7 +191,6 @@ struct inv_mpu6050_state {
 
 #define INV_MPU6050_BYTES_PER_3AXIS_SENSOR   6
 #define INV_MPU6050_FIFO_COUNT_BYTE          2
-#define INV_MPU6050_FIFO_THRESHOLD           500
 
 /* mpu6500 registers */
 #define INV_MPU6500_REG_ACCEL_CONFIG_2      0x1D
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 7724888..7a4aaed 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -110,6 +110,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 			"failed to ack interrupt\n");
 		goto flush_fifo;
 	}
+	/* handle fifo overflow by reseting fifo */
+	if (int_status & INV_MPU6050_BIT_FIFO_OVERFLOW_INT)
+		goto flush_fifo;
 	if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) {
 		dev_warn(regmap_get_device(st->map),
 			"spurious interrupt with status 0x%x\n", int_status);
@@ -135,8 +138,6 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	if (result)
 		goto end_session;
 	fifo_count = get_unaligned_be16(&data[0]);
-	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
-		goto flush_fifo;
 	/* compute and process all complete datum */
 	nb = fifo_count / bytes_per_datum;
 	for (i = 0; i < nb; ++i) {
-- 
2.7.4

[PATCH 5/5] iio: imu: inv_mpu6050: new timestamp mechanism

[Jean-Baptiste Maneyrol]

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>
---
 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 | 78 +++++++++++++++++++++++++++++-
 3 files changed, 92 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 9e5c5e7..ade6294 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 09a7e1c..b78640c 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 7a4aaed..e06e509 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -23,12 +23,86 @@
 #include <asm/unaligned.h>
 #include "inv_mpu_iio.h"
 
+/**
+ *  inv_mpu6050_add_timestamp() - Add a new interrupt timestamp
+ *
+ *  @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_add_timestamp(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 = st->chip_config.divider + 1;
+	s64 val;
+	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.
+		 */
+		val = (timestamp - st->it_timestamp) / divider;
+		if (val > period_min && val < period_max) {
+			/* update chip period and use it timestamp */
+			st->chip_period = (st->chip_period + val) / 2;
+			use_it_timestamp = true;
+		}
+	}
+
+	if (use_it_timestamp) {
+		/* manage case of multiple samples in the fifo (nb > 1) */
+		val = (nb - 1) * st->chip_period * divider;
+		st->data_timestamp = timestamp - val;
+	}
+
+	/* 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)
+{
+	const unsigned divider = st->chip_config.divider + 1;
+	s64 ts;
+
+	/* return current data timestamp and increment */
+	ts = st->data_timestamp;
+	st->data_timestamp += st->chip_period * divider;
+
+	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 +171,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 +214,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_add_timestamp(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 +225,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.7.4

Re: [PATCH 2/5] iio: imu: inv_mpu6050: switch to use sample rate divider

[Martin Kelly]

On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
> Instead of storing fifo rate in Hz, store the chip internal sample
> rate divider. This will be more useful for timestamping. There
> are both equivalent.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
>   drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 15 +++++++++------
>   drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  4 ++--
>   2 files changed, 11 insertions(+), 8 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index 1e7e750..9e5c5e7 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -82,7 +82,7 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = {
>   static const struct inv_mpu6050_chip_config chip_config_6050 = {
>   	.fsr = INV_MPU6050_FSR_2000DPS,
>   	.lpf = INV_MPU6050_FILTER_20HZ,
> -	.fifo_rate = INV_MPU6050_INIT_FIFO_RATE,
> +	.divider = (INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE) - 1,
>   	.gyro_fifo_enable = false,
>   	.accl_fifo_enable = false,
>   	.accl_fs = INV_MPU6050_FS_02G,
> @@ -628,7 +628,7 @@ static ssize_t
>   inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
>   			    const char *buf, size_t count)
>   {
> -	s32 fifo_rate;
> +	int fifo_rate;
>   	u8 d;

Since the divider is a u8, I think the fifo_rate should also be unsigned 
(I think u8 would fit it, but unsigned int should be fine too).

>   	int result;
>   	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
> @@ -644,8 +644,12 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
>   	if (result)
>   		return result;
>   
> +	/* compute freq divider and update fifo rate in case of truncation */
> +	d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
> +	fifo_rate = INV_MPU6050_ONE_K_HZ / (d + 1);
> +

I don't quite understand why we need to set fifo_rate again. Could you 
explain?

>   	mutex_lock(&st->lock);
> -	if (fifo_rate == st->chip_config.fifo_rate) {
> +	if (d == st->chip_config.divider) {
>   		result = 0;
>   		goto fifo_rate_fail_unlock;
>   	}
> @@ -653,11 +657,10 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
>   	if (result)
>   		goto fifo_rate_fail_unlock;
>   
> -	d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
>   	result = regmap_write(st->map, st->reg->sample_rate_div, d);
>   	if (result)
>   		goto fifo_rate_fail_power_off;
> -	st->chip_config.fifo_rate = fifo_rate;
> +	st->chip_config.divider = d;
>   
>   	result = inv_mpu6050_set_lpf(st, fifo_rate);
>   	if (result)
> @@ -685,7 +688,7 @@ inv_fifo_rate_show(struct device *dev, struct device_attribute *attr,
>   	unsigned fifo_rate;
>   
>   	mutex_lock(&st->lock);
> -	fifo_rate = st->chip_config.fifo_rate;
> +	fifo_rate = INV_MPU6050_ONE_K_HZ / (st->chip_config.divider + 1);

Since we use this same line twice, I think it would be useful to put it 
in a function inv_calculate_fifo_rate(u8 divider) or similar.

>   	mutex_unlock(&st->lock);
>   
>   	return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate);
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index a92ddd4..8d9044c 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -86,7 +86,7 @@ enum inv_devices {
>    *  @accl_fs:		accel full scale range.
>    *  @accl_fifo_enable:	enable accel data output
>    *  @gyro_fifo_enable:	enable gyro data output
> - *  @fifo_rate:		FIFO update rate.
> + *  @divider:		sample rate divider.
>    */
>   struct inv_mpu6050_chip_config {
>   	unsigned int fsr:2;
> @@ -94,7 +94,7 @@ struct inv_mpu6050_chip_config {
>   	unsigned int accl_fs:2;
>   	unsigned int accl_fifo_enable:1;
>   	unsigned int gyro_fifo_enable:1;
> -	u16 fifo_rate;
> +	u8 divider;
>   	u8 user_ctrl;
>   };
>   
> 

Re: [PATCH 3/5] iio: imu: inv_mpu6050: fix fifo count reading

[Martin Kelly]

On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
> Use unaligned access since buffer is a bytes table. Truncate fifo
> count to read only complete datum.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
>   drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 24 +++++++++++-------------
>   1 file changed, 11 insertions(+), 13 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 5436d18..7724888 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -20,6 +20,7 @@
>   #include <linux/irq.h>
>   #include <linux/interrupt.h>
>   #include <linux/poll.h>
> +#include <asm/unaligned.h>
>   #include "inv_mpu_iio.h"
>   
>   int inv_reset_fifo(struct iio_dev *indio_dev)
> @@ -98,6 +99,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>   	u16 fifo_count;
>   	s64 timestamp = pf->timestamp;
>   	int int_status;
> +	size_t i, nb;
>   
>   	mutex_lock(&st->lock);
>   
> @@ -132,27 +134,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>   				  INV_MPU6050_FIFO_COUNT_BYTE);
>   	if (result)
>   		goto end_session;
> -	fifo_count = be16_to_cpup((__be16 *)(&data[0]));
> -	if (fifo_count < bytes_per_datum)
> -		goto end_session;
> -	/* fifo count can't be an odd number. If it is odd, reset the FIFO. */
> -	if (fifo_count & 1)
> -		goto flush_fifo;
> +	fifo_count = get_unaligned_be16(&data[0]);
>   	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
>   		goto flush_fifo;
> -	do {
> +	/* compute and process all complete datum */
> +	nb = fifo_count / bytes_per_datum;
> +	for (i = 0; i < nb; ++i) {
>   		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
>   					  data, bytes_per_datum);
>   		if (result)
>   			goto flush_fifo;
>   		/* skip first samples if needed */
> -		if (st->skip_samples)
> +		if (st->skip_samples) {
>   			st->skip_samples--;
> -		else
> -			iio_push_to_buffers_with_timestamp(indio_dev, data,
> -							   timestamp);
> -		fifo_count -= bytes_per_datum;
> -	} while (fifo_count >= bytes_per_datum);
> +			continue;
> +		}
> +		iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
> +	}
>   
>   end_session:
>   	mutex_unlock(&st->lock);
> 

Reviewed-by: Martin Kelly <mkelly@xevo.com>

Re: [PATCH 4/5] iio: imu: inv_mpu6050: better fifo overflow handling

[Martin Kelly]

On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
> Use fifo overflow bit from int status rather than using an
> arbitrary threshold.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
>   drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  | 2 +-
>   drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 5 +++--
>   2 files changed, 4 insertions(+), 3 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index 8d9044c..09a7e1c 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -166,6 +166,7 @@ struct inv_mpu6050_state {
>   #define INV_MPU6050_REG_RAW_GYRO            0x43
>   
>   #define INV_MPU6050_REG_INT_STATUS          0x3A
> +#define INV_MPU6050_BIT_FIFO_OVERFLOW_INT   0x10
>   #define INV_MPU6050_BIT_RAW_DATA_RDY_INT    0x01
>   
>   #define INV_MPU6050_REG_USER_CTRL           0x6A
> @@ -190,7 +191,6 @@ struct inv_mpu6050_state {
>   
>   #define INV_MPU6050_BYTES_PER_3AXIS_SENSOR   6
>   #define INV_MPU6050_FIFO_COUNT_BYTE          2
> -#define INV_MPU6050_FIFO_THRESHOLD           500
>   
>   /* mpu6500 registers */
>   #define INV_MPU6500_REG_ACCEL_CONFIG_2      0x1D
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 7724888..7a4aaed 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -110,6 +110,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>   			"failed to ack interrupt\n");
>   		goto flush_fifo;
>   	}
> +	/* handle fifo overflow by reseting fifo */
> +	if (int_status & INV_MPU6050_BIT_FIFO_OVERFLOW_INT)
> +		goto flush_fifo;
>   	if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) {
>   		dev_warn(regmap_get_device(st->map),
>   			"spurious interrupt with status 0x%x\n", int_status);
> @@ -135,8 +138,6 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>   	if (result)
>   		goto end_session;
>   	fifo_count = get_unaligned_be16(&data[0]);
> -	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
> -		goto flush_fifo;
>   	/* compute and process all complete datum */
>   	nb = fifo_count / bytes_per_datum;
>   	for (i = 0; i < nb; ++i) {
> 

Reviewed-by: Martin Kelly <mkelly@xevo.com>

Re: [PATCH 5/5] iio: imu: inv_mpu6050: new timestamp mechanism

[Martin Kelly]

On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
> 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>
> ---
>   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 | 78 +++++++++++++++++++++++++++++-
>   3 files changed, 92 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 9e5c5e7..ade6294 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 09a7e1c..b78640c 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 7a4aaed..e06e509 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -23,12 +23,86 @@
>   #include <asm/unaligned.h>
>   #include "inv_mpu_iio.h"
>   
> +/**
> + *  inv_mpu6050_add_timestamp() - Add a new interrupt timestamp
> + *
> + *  @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_add_timestamp(struct inv_mpu6050_state *st,
> +				      s64 timestamp, size_t nb)

I think it would be clearer if this were called something like 
inv_mpu6050_update_ts_estimate(), as we're not really attaching a 
timestamp to a sample here (what I first thought) but are just updating 
our estimation data.

> +{
> +	/* 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 = st->chip_config.divider + 1;

I think this variable is better named fifo_rate to avoid confusion with 
st->chip_config.divider (and if we refactor to calculate it with a 
function, we should call that function here).

> +	s64 val;
> +	bool use_it_timestamp = false;
> +
> +	if (st->it_timestamp == 0)  > +		/* not initialized, forced to use it timestamp */

nit: should probably be "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.
> +		 */
> +		val = (timestamp - st->it_timestamp) / divider;
> +		if (val > period_min && val < period_max) {
> +			/* update chip period and use it timestamp */
> +			st->chip_period = (st->chip_period + val) / 2;
> +			use_it_timestamp = true;
> +		}
> +	}
> +
> +	if (use_it_timestamp) {
> +		/* manage case of multiple samples in the fifo (nb > 1) */
> +		val = (nb - 1) * st->chip_period * divider;
> +		st->data_timestamp = timestamp - val;

Since this is complex code, I think naming "val" something more 
descriptive would be helpful for understanding. A would suggest a name 
like "gap_estimate", but others would work too.

> +	}
> +
> +	/* 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)
> +{
> +	const unsigned divider = st->chip_config.divider + 1;

Same here; I think this should be called fifo_rate.

> +	s64 ts;
> +
> +	/* return current data timestamp and increment */
> +	ts = st->data_timestamp;
> +	st->data_timestamp += st->chip_period * divider;
> +
> +	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 +171,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 +214,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_add_timestamp(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 +225,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);
>   	}
>   
> 

Re: [PATCH 2/5] iio: imu: inv_mpu6050: switch to use sample rate divider

[Jean-Baptiste Maneyrol]

On 15/05/2018 21:08, Martin Kelly wrote:
> CAUTION: This email originated from outside of the organization. Please 
> make sure the sender is who they say they are and do not click links or 
> open attachments unless you recognize the sender and know the content is 
> safe.
> 
> 
> On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
>> Instead of storing fifo rate in Hz, store the chip internal sample
>> rate divider. This will be more useful for timestamping. There
>> are both equivalent.
>>
>> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
>> ---
>>   drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 15 +++++++++------
>>   drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  4 ++--
>>   2 files changed, 11 insertions(+), 8 deletions(-)
>>
>> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c 
>> b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>> index 1e7e750..9e5c5e7 100644
>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>> @@ -82,7 +82,7 @@ static const struct inv_mpu6050_reg_map reg_set_6050 
>> = {
>>   static const struct inv_mpu6050_chip_config chip_config_6050 = {
>>       .fsr = INV_MPU6050_FSR_2000DPS,
>>       .lpf = INV_MPU6050_FILTER_20HZ,
>> -     .fifo_rate = INV_MPU6050_INIT_FIFO_RATE,
>> +     .divider = (INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE) - 1,
>>       .gyro_fifo_enable = false,
>>       .accl_fifo_enable = false,
>>       .accl_fs = INV_MPU6050_FS_02G,
>> @@ -628,7 +628,7 @@ static ssize_t
>>   inv_mpu6050_fifo_rate_store(struct device *dev, struct 
>> device_attribute *attr,
>>                           const char *buf, size_t count)
>>   {
>> -     s32 fifo_rate;
>> +     int fifo_rate;
>>       u8 d;
> 
> Since the divider is a u8, I think the fifo_rate should also be unsigned
> (I think u8 would fit it, but unsigned int should be fine too).
fifo_rate is taken from sysfs entry using kstrtoint which requires a 
signed int. The user can enter any number, even negative ones. Using a 
signed integer for reading the entry enables to catch negative number 
and return EINVAL for them.

> 
>>       int result;
>>       struct iio_dev *indio_dev = dev_to_iio_dev(dev);
>> @@ -644,8 +644,12 @@ inv_mpu6050_fifo_rate_store(struct device *dev, 
>> struct device_attribute *attr,
>>       if (result)
>>               return result;
>>
>> +     /* compute freq divider and update fifo rate in case of 
>> truncation */
>> +     d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
>> +     fifo_rate = INV_MPU6050_ONE_K_HZ / (d + 1);
>> +
> 
> I don't quite understand why we need to set fifo_rate again. Could you
> explain?
Since fifo_rate is an arbitrary number coming from userspace, it can be 
a rate that is not supported by the chip (not a divider of 1KHz). For 
dealing with this case, I compute first the divider using provided 
value. This computation will return a truncated number that is not 
providing necessary the fifo rate asked if it is not supported. So I 
compute back the fifo rate using this divider to store the real value.

> 
>>       mutex_lock(&st->lock);
>> -     if (fifo_rate == st->chip_config.fifo_rate) {
>> +     if (d == st->chip_config.divider) {
>>               result = 0;
>>               goto fifo_rate_fail_unlock;
>>       }
>> @@ -653,11 +657,10 @@ inv_mpu6050_fifo_rate_store(struct device *dev, 
>> struct device_attribute *attr,
>>       if (result)
>>               goto fifo_rate_fail_unlock;
>>
>> -     d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
>>       result = regmap_write(st->map, st->reg->sample_rate_div, d);
>>       if (result)
>>               goto fifo_rate_fail_power_off;
>> -     st->chip_config.fifo_rate = fifo_rate;
>> +     st->chip_config.divider = d;
>>
>>       result = inv_mpu6050_set_lpf(st, fifo_rate);
>>       if (result)
>> @@ -685,7 +688,7 @@ inv_fifo_rate_show(struct device *dev, struct 
>> device_attribute *attr,
>>       unsigned fifo_rate;
>>
>>       mutex_lock(&st->lock);
>> -     fifo_rate = st->chip_config.fifo_rate;
>> +     fifo_rate = INV_MPU6050_ONE_K_HZ / (st->chip_config.divider + 1);
> 
> Since we use this same line twice, I think it would be useful to put it
> in a function inv_calculate_fifo_rate(u8 divider) or similar.
That is indeed possible.

> 
>>       mutex_unlock(&st->lock);
>>
>>       return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate);
>> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h 
>> b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
>> index a92ddd4..8d9044c 100644
>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
>> @@ -86,7 +86,7 @@ enum inv_devices {
>>    *  @accl_fs:               accel full scale range.
>>    *  @accl_fifo_enable:      enable accel data output
>>    *  @gyro_fifo_enable:      enable gyro data output
>> - *  @fifo_rate:              FIFO update rate.
>> + *  @divider:                sample rate divider.
>>    */
>>   struct inv_mpu6050_chip_config {
>>       unsigned int fsr:2;
>> @@ -94,7 +94,7 @@ struct inv_mpu6050_chip_config {
>>       unsigned int accl_fs:2;
>>       unsigned int accl_fifo_enable:1;
>>       unsigned int gyro_fifo_enable:1;
>> -     u16 fifo_rate;
>> +     u8 divider;
>>       u8 user_ctrl;
>>   };
>>
>>

Re: [PATCH 5/5] iio: imu: inv_mpu6050: new timestamp mechanism

[Jean-Baptiste Maneyrol]

On 15/05/2018 21:31, Martin Kelly wrote:
> CAUTION: This email originated from outside of the organization. Please 
> make sure the sender is who they say they are and do not click links or 
> open attachments unless you recognize the sender and know the content is 
> safe.
> 
> 
> On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
>> 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>
>> ---
>>   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 | 78 
>> +++++++++++++++++++++++++++++-
>>   3 files changed, 92 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 9e5c5e7..ade6294 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 09a7e1c..b78640c 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 7a4aaed..e06e509 100644
>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
>> @@ -23,12 +23,86 @@
>>   #include <asm/unaligned.h>
>>   #include "inv_mpu_iio.h"
>>
>> +/**
>> + *  inv_mpu6050_add_timestamp() - Add a new interrupt timestamp
>> + *
>> + *  @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_add_timestamp(struct inv_mpu6050_state *st,
>> +                                   s64 timestamp, size_t nb)
> 
> I think it would be clearer if this were called something like
> inv_mpu6050_update_ts_estimate(), as we're not really attaching a
> timestamp to a sample here (what I first thought) but are just updating
> our estimation data.
In fact, we are adding a new timestamp for estimating the chip internal 
frequency. But a better name would be good I agree.

> 
>> +{
>> +     /* 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 = st->chip_config.divider + 1;
> 
> I think this variable is better named fifo_rate to avoid confusion with
> st->chip_config.divider (and if we refactor to calculate it with a
> function, we should call that function here).
This is not fifo rate, but the real frequency divider, which is chip 
divider + 1. Here fifo_rate = 1KHz / divider. If you have an idea about 
a better name, I'm interested.

> 
>> +     s64 val;
>> +     bool use_it_timestamp = false;
>> +
>> +     if (st->it_timestamp == 0)  > +         /* not initialized, 
>> forced to use it timestamp */
> 
> nit: should probably be "forced to use it_timestamp"
it_timestamp is the it (means interrupt) 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.
>> +              */
>> +             val = (timestamp - st->it_timestamp) / divider;
>> +             if (val > period_min && val < period_max) {
>> +                     /* update chip period and use it timestamp */
>> +                     st->chip_period = (st->chip_period + val) / 2;
>> +                     use_it_timestamp = true;
>> +             }
>> +     }
>> +
>> +     if (use_it_timestamp) {
>> +             /* manage case of multiple samples in the fifo (nb > 1) */
>> +             val = (nb - 1) * st->chip_period * divider;
>> +             st->data_timestamp = timestamp - val;
> 
> Since this is complex code, I think naming "val" something more
> descriptive would be helpful for understanding. A would suggest a name
> like "gap_estimate", but others would work too.
I can declare 2 variables with better names instead.

> 
>> +     }
>> +
>> +     /* 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)
>> +{
>> +     const unsigned divider = st->chip_config.divider + 1;
> 
> Same here; I think this should be called fifo_rate.
Real frequency divider like above.

> 
>> +     s64 ts;
>> +
>> +     /* return current data timestamp and increment */
>> +     ts = st->data_timestamp;
>> +     st->data_timestamp += st->chip_period * divider;
>> +
>> +     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 +171,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 +214,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_add_timestamp(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 +225,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);
>>       }
>>
>>

Re: [PATCH 2/5] iio: imu: inv_mpu6050: switch to use sample rate divider

[Martin Kelly]

On 05/16/2018 12:08 AM, Jean-Baptiste Maneyrol wrote:
> 
> 
> On 15/05/2018 21:08, Martin Kelly wrote:
>> CAUTION: This email originated from outside of the organization. 
>> Please make sure the sender is who they say they are and do not click 
>> links or open attachments unless you recognize the sender and know the 
>> content is safe.
>>
>>
>> On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
>>> Instead of storing fifo rate in Hz, store the chip internal sample
>>> rate divider. This will be more useful for timestamping. There
>>> are both equivalent.
>>>
>>> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
>>> ---
>>>   drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 15 +++++++++------
>>>   drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  4 ++--
>>>   2 files changed, 11 insertions(+), 8 deletions(-)
>>>
>>> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c 
>>> b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>> index 1e7e750..9e5c5e7 100644
>>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>> @@ -82,7 +82,7 @@ static const struct inv_mpu6050_reg_map 
>>> reg_set_6050 = {
>>>   static const struct inv_mpu6050_chip_config chip_config_6050 = {
>>>       .fsr = INV_MPU6050_FSR_2000DPS,
>>>       .lpf = INV_MPU6050_FILTER_20HZ,
>>> -     .fifo_rate = INV_MPU6050_INIT_FIFO_RATE,
>>> +     .divider = (INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE) 
>>> - 1,
>>>       .gyro_fifo_enable = false,
>>>       .accl_fifo_enable = false,
>>>       .accl_fs = INV_MPU6050_FS_02G,
>>> @@ -628,7 +628,7 @@ static ssize_t
>>>   inv_mpu6050_fifo_rate_store(struct device *dev, struct 
>>> device_attribute *attr,
>>>                           const char *buf, size_t count)
>>>   {
>>> -     s32 fifo_rate;
>>> +     int fifo_rate;
>>>       u8 d;
>>
>> Since the divider is a u8, I think the fifo_rate should also be unsigned
>> (I think u8 would fit it, but unsigned int should be fine too).
> fifo_rate is taken from sysfs entry using kstrtoint which requires a 
> signed int. The user can enter any number, even negative ones. Using a 
> signed integer for reading the entry enables to catch negative number 
> and return EINVAL for them.
> 

Yes, you are right. In this case we have a check for the range 4-1000, 
so the division will be safe. I suspect the compiler will generate a 
warning here about mixing types, but I think it's not turned on.

>>
>>>       int result;
>>>       struct iio_dev *indio_dev = dev_to_iio_dev(dev);
>>> @@ -644,8 +644,12 @@ inv_mpu6050_fifo_rate_store(struct device *dev, 
>>> struct device_attribute *attr,
>>>       if (result)
>>>               return result;
>>>
>>> +     /* compute freq divider and update fifo rate in case of 
>>> truncation */
>>> +     d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
>>> +     fifo_rate = INV_MPU6050_ONE_K_HZ / (d + 1);
>>> +
>>
>> I don't quite understand why we need to set fifo_rate again. Could you
>> explain?
> Since fifo_rate is an arbitrary number coming from userspace, it can be 
> a rate that is not supported by the chip (not a divider of 1KHz). For 
> dealing with this case, I compute first the divider using provided 
> value. This computation will return a truncated number that is not 
> providing necessary the fifo rate asked if it is not supported. So I 
> compute back the fifo rate using this divider to store the real value.
> 

Yes, makes sense. If the user gives an evenly divisible number, they 
will be the same, but if not, we need to calculate in the way you do.

>>
>>>       mutex_lock(&st->lock);
>>> -     if (fifo_rate == st->chip_config.fifo_rate) {
>>> +     if (d == st->chip_config.divider) {
>>>               result = 0;
>>>               goto fifo_rate_fail_unlock;
>>>       }
>>> @@ -653,11 +657,10 @@ inv_mpu6050_fifo_rate_store(struct device *dev, 
>>> struct device_attribute *attr,
>>>       if (result)
>>>               goto fifo_rate_fail_unlock;
>>>
>>> -     d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
>>>       result = regmap_write(st->map, st->reg->sample_rate_div, d);
>>>       if (result)
>>>               goto fifo_rate_fail_power_off;
>>> -     st->chip_config.fifo_rate = fifo_rate;
>>> +     st->chip_config.divider = d;
>>>
>>>       result = inv_mpu6050_set_lpf(st, fifo_rate);
>>>       if (result)
>>> @@ -685,7 +688,7 @@ inv_fifo_rate_show(struct device *dev, struct 
>>> device_attribute *attr,
>>>       unsigned fifo_rate;
>>>
>>>       mutex_lock(&st->lock);
>>> -     fifo_rate = st->chip_config.fifo_rate;
>>> +     fifo_rate = INV_MPU6050_ONE_K_HZ / (st->chip_config.divider + 1);
>>
>> Since we use this same line twice, I think it would be useful to put it
>> in a function inv_calculate_fifo_rate(u8 divider) or similar.
> That is indeed possible.
> 
>>
>>>       mutex_unlock(&st->lock);
>>>
>>>       return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate);
>>> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h 
>>> b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
>>> index a92ddd4..8d9044c 100644
>>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
>>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
>>> @@ -86,7 +86,7 @@ enum inv_devices {
>>>    *  @accl_fs:               accel full scale range.
>>>    *  @accl_fifo_enable:      enable accel data output
>>>    *  @gyro_fifo_enable:      enable gyro data output
>>> - *  @fifo_rate:              FIFO update rate.
>>> + *  @divider:                sample rate divider.
>>>    */
>>>   struct inv_mpu6050_chip_config {
>>>       unsigned int fsr:2;
>>> @@ -94,7 +94,7 @@ struct inv_mpu6050_chip_config {
>>>       unsigned int accl_fs:2;
>>>       unsigned int accl_fifo_enable:1;
>>>       unsigned int gyro_fifo_enable:1;
>>> -     u16 fifo_rate;
>>> +     u8 divider;
>>>       u8 user_ctrl;
>>>   };
>>>
>>>

Re: [PATCH 5/5] iio: imu: inv_mpu6050: new timestamp mechanism

[Martin Kelly]

On 05/16/2018 12:33 AM, Jean-Baptiste Maneyrol wrote:
> 
> 
> On 15/05/2018 21:31, Martin Kelly wrote:
>> CAUTION: This email originated from outside of the organization. 
>> Please make sure the sender is who they say they are and do not click 
>> links or open attachments unless you recognize the sender and know the 
>> content is safe.
>>
>>
>> On 05/14/2018 02:05 PM, Jean-Baptiste Maneyrol wrote:
>>> 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>
>>> ---
>>>   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 | 78 
>>> +++++++++++++++++++++++++++++-
>>>   3 files changed, 92 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 9e5c5e7..ade6294 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 09a7e1c..b78640c 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 7a4aaed..e06e509 100644
>>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
>>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
>>> @@ -23,12 +23,86 @@
>>>   #include <asm/unaligned.h>
>>>   #include "inv_mpu_iio.h"
>>>
>>> +/**
>>> + *  inv_mpu6050_add_timestamp() - Add a new interrupt timestamp
>>> + *
>>> + *  @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_add_timestamp(struct inv_mpu6050_state *st,
>>> +                                   s64 timestamp, size_t nb)
>>
>> I think it would be clearer if this were called something like
>> inv_mpu6050_update_ts_estimate(), as we're not really attaching a
>> timestamp to a sample here (what I first thought) but are just updating
>> our estimation data.
> In fact, we are adding a new timestamp for estimating the chip internal 
> frequency. But a better name would be good I agree.
> 
>>
>>> +{
>>> +     /* 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 = st->chip_config.divider + 1;
>>
>> I think this variable is better named fifo_rate to avoid confusion with
>> st->chip_config.divider (and if we refactor to calculate it with a
>> function, we should call that function here).
> This is not fifo rate, but the real frequency divider, which is chip 
> divider + 1. Here fifo_rate = 1KHz / divider. If you have an idea about 
> a better name, I'm interested.
> 

A few ideas:

- Rename st->chip_config.divider to st->chip_config.freq_divider (and 
set it to 1 higher). When setting it in the chip registers, subtract 1. 
This would avoid the repeated code of "st->chip_config.divider + 1" and 
make it clearer exactly what the divider is.

- Call the variable freq_divider (as opposed to chip_divider, say).

- Use a get_freq_divider() function to get the frequency divider from 
the st->chip_config.divider (the chip divider), with a comment in the 
function explaining how the two are 1 different from each other.

I think basically anything that lets the reader understand the 
relationship between the dividers without combing through the datasheet 
would be helpful.

>>
>>> +     s64 val;
>>> +     bool use_it_timestamp = false;
>>> +
>>> +     if (st->it_timestamp == 0)  > +         /* not initialized, 
>>> forced to use it timestamp */
>>
>> nit: should probably be "forced to use it_timestamp"
> it_timestamp is the it (means interrupt) timestamp.
> 

I was actually just referring to "it timestamp" vs "it_timestamp" (since 
"it_timestamp" is the variable). That said, as a small change, I think 
"int_timestamp" would be clearer than "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.
>>> +              */
>>> +             val = (timestamp - st->it_timestamp) / divider;
>>> +             if (val > period_min && val < period_max) {
>>> +                     /* update chip period and use it timestamp */
>>> +                     st->chip_period = (st->chip_period + val) / 2;
>>> +                     use_it_timestamp = true;
>>> +             }
>>> +     }
>>> +
>>> +     if (use_it_timestamp) {
>>> +             /* manage case of multiple samples in the fifo (nb > 1) */
>>> +             val = (nb - 1) * st->chip_period * divider;
>>> +             st->data_timestamp = timestamp - val;
>>
>> Since this is complex code, I think naming "val" something more
>> descriptive would be helpful for understanding. A would suggest a name
>> like "gap_estimate", but others would work too.
> I can declare 2 variables with better names instead.
> 

That sounds good.

>>
>>> +     }
>>> +
>>> +     /* 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)
>>> +{
>>> +     const unsigned divider = st->chip_config.divider + 1;
>>
>> Same here; I think this should be called fifo_rate.
> Real frequency divider like above.
> 
>>
>>> +     s64 ts;
>>> +
>>> +     /* return current data timestamp and increment */
>>> +     ts = st->data_timestamp;
>>> +     st->data_timestamp += st->chip_period * divider;
>>> +
>>> +     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 +171,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 +214,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_add_timestamp(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 +225,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);
>>>       }
>>>
>>>

Re: [PATCH 3/5] iio: imu: inv_mpu6050: fix fifo count reading

[Jonathan Cameron]

On Mon, 14 May 2018 23:05:53 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:

> Use unaligned access since buffer is a bytes table. Truncate fifo
> count to read only complete datum.

Hmm. Probably cheaper to enforce the alignment, but I suppose it
doesn't really matter.

Series looks fine except for the minor stuff you have already
said you'll address. I'll take a close look at V2 before applying.

Thanks,

Jonathan


> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 24 +++++++++++-------------
>  1 file changed, 11 insertions(+), 13 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 5436d18..7724888 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -20,6 +20,7 @@
>  #include <linux/irq.h>
>  #include <linux/interrupt.h>
>  #include <linux/poll.h>
> +#include <asm/unaligned.h>
>  #include "inv_mpu_iio.h"
>  
>  int inv_reset_fifo(struct iio_dev *indio_dev)
> @@ -98,6 +99,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	u16 fifo_count;
>  	s64 timestamp = pf->timestamp;
>  	int int_status;
> +	size_t i, nb;
>  
>  	mutex_lock(&st->lock);
>  
> @@ -132,27 +134,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  				  INV_MPU6050_FIFO_COUNT_BYTE);
>  	if (result)
>  		goto end_session;
> -	fifo_count = be16_to_cpup((__be16 *)(&data[0]));
> -	if (fifo_count < bytes_per_datum)
> -		goto end_session;
> -	/* fifo count can't be an odd number. If it is odd, reset the FIFO. */
> -	if (fifo_count & 1)
> -		goto flush_fifo;
> +	fifo_count = get_unaligned_be16(&data[0]);
>  	if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
>  		goto flush_fifo;
> -	do {
> +	/* compute and process all complete datum */
> +	nb = fifo_count / bytes_per_datum;
> +	for (i = 0; i < nb; ++i) {
>  		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
>  					  data, bytes_per_datum);
>  		if (result)
>  			goto flush_fifo;
>  		/* skip first samples if needed */
> -		if (st->skip_samples)
> +		if (st->skip_samples) {
>  			st->skip_samples--;
> -		else
> -			iio_push_to_buffers_with_timestamp(indio_dev, data,
> -							   timestamp);
> -		fifo_count -= bytes_per_datum;
> -	} while (fifo_count >= bytes_per_datum);
> +			continue;
> +		}
> +		iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
> +	}
>  
>  end_session:
>  	mutex_unlock(&st->lock);