From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
Replace timestamping by the new common inv_sensors timestamp
module. The principle behind is the same but the implementation in
the new module is far better providing less jitter and a better
estimation.
Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
---
drivers/iio/imu/inv_mpu6050/Kconfig | 1 +
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 30 +++++--
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 18 ++--
drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 83 ++-----------------
drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 6 +-
5 files changed, 45 insertions(+), 93 deletions(-)
diff --git a/drivers/iio/imu/inv_mpu6050/Kconfig b/drivers/iio/imu/inv_mpu6050/Kconfig
index 64dd73dcc4ba..5f62e4fd475d 100644
--- a/drivers/iio/imu/inv_mpu6050/Kconfig
+++ b/drivers/iio/imu/inv_mpu6050/Kconfig
@@ -7,6 +7,7 @@ config INV_MPU6050_IIO
tristate
select IIO_BUFFER
select IIO_TRIGGERED_BUFFER
+ select IIO_INV_SENSORS_TIMESTAMP
config INV_MPU6050_I2C
tristate "Invensense MPU6050 devices (I2C)"
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 592a6e60b413..13086b569b90 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -12,12 +12,15 @@
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
-#include <linux/iio/iio.h>
#include <linux/acpi.h>
#include <linux/platform_device.h>
#include <linux/regulator/consumer.h>
#include <linux/pm.h>
#include <linux/pm_runtime.h>
+
+#include <linux/iio/common/inv_sensors_timestamp.h>
+#include <linux/iio/iio.h>
+
#include "inv_mpu_iio.h"
#include "inv_mpu_magn.h"
@@ -521,6 +524,7 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
int result;
u8 d;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
+ struct inv_sensors_timestamp_chip timestamp;
result = inv_mpu6050_set_gyro_fsr(st, st->chip_config.fsr);
if (result)
@@ -544,12 +548,12 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
if (result)
return result;
- /*
- * 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;
+ /* clock jitter is +/- 2% */
+ timestamp.clock_period = NSEC_PER_SEC / INV_MPU6050_INTERNAL_FREQ_HZ;
+ timestamp.jitter = 20;
+ timestamp.init_period =
+ NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
+ inv_sensors_timestamp_init(&st->timestamp, ×tamp);
/* magn chip init, noop if not present in the chip */
result = inv_mpu_magn_probe(st);
@@ -936,6 +940,8 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
const char *buf, size_t count)
{
int fifo_rate;
+ u32 fifo_period;
+ bool fifo_on;
u8 d;
int result;
struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -952,12 +958,21 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(fifo_rate);
/* compute back the fifo rate to handle truncation cases */
fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(d);
+ fifo_period = NSEC_PER_SEC / fifo_rate;
mutex_lock(&st->lock);
if (d == st->chip_config.divider) {
result = 0;
goto fifo_rate_fail_unlock;
}
+
+ fifo_on = st->chip_config.accl_fifo_enable ||
+ st->chip_config.gyro_fifo_enable ||
+ st->chip_config.magn_fifo_enable;
+ result = inv_sensors_timestamp_update_odr(&st->timestamp, fifo_period, fifo_on);
+ if (result)
+ goto fifo_rate_fail_unlock;
+
result = pm_runtime_resume_and_get(pdev);
if (result)
goto fifo_rate_fail_unlock;
@@ -1785,3 +1800,4 @@ EXPORT_NS_GPL_DEV_PM_OPS(inv_mpu_pmops, IIO_MPU6050) = {
MODULE_AUTHOR("Invensense Corporation");
MODULE_DESCRIPTION("Invensense device MPU6050 driver");
MODULE_LICENSE("GPL");
+MODULE_IMPORT_NS(IIO_INV_SENSORS_TIMESTAMP);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index b4ab2c397d0f..a51d103a57ca 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -9,15 +9,17 @@
#include <linux/i2c.h>
#include <linux/i2c-mux.h>
#include <linux/mutex.h>
-#include <linux/iio/iio.h>
-#include <linux/iio/buffer.h>
+#include <linux/platform_data/invensense_mpu6050.h>
#include <linux/regmap.h>
-#include <linux/iio/sysfs.h>
+
+#include <linux/iio/buffer.h>
+#include <linux/iio/common/inv_sensors_timestamp.h>
+#include <linux/iio/iio.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/trigger.h>
#include <linux/iio/triggered_buffer.h>
#include <linux/iio/trigger_consumer.h>
-#include <linux/platform_data/invensense_mpu6050.h>
+#include <linux/iio/sysfs.h>
/**
* struct inv_mpu6050_reg_map - Notable registers.
@@ -170,9 +172,7 @@ 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.
+ * @timestamp: timestamping module
* @vdd_supply: VDD voltage regulator for the chip.
* @vddio_supply I/O voltage regulator for the chip.
* @magn_disabled: magnetometer disabled for backward compatibility reason.
@@ -196,9 +196,7 @@ struct inv_mpu6050_state {
int irq;
u8 irq_mask;
unsigned skip_samples;
- s64 chip_period;
- s64 it_timestamp;
- s64 data_timestamp;
+ struct inv_sensors_timestamp timestamp;
struct regulator *vdd_supply;
struct regulator *vddio_supply;
bool magn_disabled;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 45c37525c2f1..d83f61a99504 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -13,81 +13,10 @@
#include <linux/interrupt.h>
#include <linux/poll.h>
#include <linux/math64.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;
- }
+#include <linux/iio/common/inv_sensors_timestamp.h>
- /* 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;
-}
+#include "inv_mpu_iio.h"
static int inv_reset_fifo(struct iio_dev *indio_dev)
{
@@ -121,6 +50,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
size_t bytes_per_datum;
int result;
u16 fifo_count;
+ u32 fifo_period;
s64 timestamp;
int int_status;
size_t i, nb;
@@ -177,7 +107,10 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
/* compute and process all complete datum */
nb = fifo_count / bytes_per_datum;
- inv_mpu6050_update_period(st, pf->timestamp, nb);
+ /* Each FIFO data contains all sensors, so same number for FIFO and sensor data */
+ fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
+ inv_sensors_timestamp_interrupt(&st->timestamp, fifo_period, nb, nb, pf->timestamp);
+ inv_sensors_timestamp_apply_odr(&st->timestamp, fifo_period, nb, 0);
for (i = 0; i < nb; ++i) {
result = regmap_noinc_read(st->map, st->reg->fifo_r_w,
st->data, bytes_per_datum);
@@ -188,7 +121,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
st->skip_samples--;
continue;
}
- timestamp = inv_mpu6050_get_timestamp(st);
+ timestamp = inv_sensors_timestamp_pop(&st->timestamp);
iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp);
}
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index 882546897255..676704f9151f 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -4,6 +4,9 @@
*/
#include <linux/pm_runtime.h>
+
+#include <linux/iio/common/inv_sensors_timestamp.h>
+
#include "inv_mpu_iio.h"
static unsigned int inv_scan_query_mpu6050(struct iio_dev *indio_dev)
@@ -106,7 +109,8 @@ int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
int ret;
if (enable) {
- st->it_timestamp = 0;
+ /* reset timestamping */
+ inv_sensors_timestamp_reset(&st->timestamp);
/* reset FIFO */
d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
ret = regmap_write(st->map, st->reg->user_ctrl, d);
--
2.34.1
