iio: move inv_icm42600 timestamp module in common

Create new inv_sensors common modules and move inv_icm42600
timestamp module inside. This module will be used by IMUs and
also in the future by other chips.

Modify inv_icm42600 driver to use timestamp module and do some
headers cleanup.

Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>
Reviewed-by: Andy Shevchenko <andy.shevchenko@gmail.com>
Link: https://lore.kernel.org/r/20230606162147.79667-3-inv.git-commit@tdk.com
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

8 files changed, 13 insertions, 273 deletions

diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
index 50cbcfcb6cf1..f56b0816cc4d 100644
--- a/drivers/iio/imu/inv_icm42600/Kconfig
+++ b/drivers/iio/imu/inv_icm42600/Kconfig
@@ -3,6 +3,7 @@
 config INV_ICM42600
 	tristate
 	select IIO_BUFFER
+	select IIO_INV_SENSORS_TIMESTAMP
 
 config INV_ICM42600_I2C
 	tristate "InvenSense ICM-426xx I2C driver"
diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
index 291714d9aa54..0f49f6df3647 100644
--- a/drivers/iio/imu/inv_icm42600/Makefile
+++ b/drivers/iio/imu/inv_icm42600/Makefile
@@ -6,7 +6,6 @@ inv-icm42600-y += inv_icm42600_gyro.o
 inv-icm42600-y += inv_icm42600_accel.o
 inv-icm42600-y += inv_icm42600_temp.o
 inv-icm42600-y += inv_icm42600_buffer.o
-inv-icm42600-y += inv_icm42600_timestamp.o
 
 obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
 inv-icm42600-i2c-y += inv_icm42600_i2c.o
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index c3f433ad3af6..c2591101645a 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -10,14 +10,15 @@
 #include <linux/regmap.h>
 #include <linux/delay.h>
 #include <linux/math64.h>
-#include <linux/iio/iio.h>
+
 #include <linux/iio/buffer.h>
+#include <linux/iio/common/inv_icm42600_timestamp.h>
+#include <linux/iio/iio.h>
 #include <linux/iio/kfifo_buf.h>
 
 #include "inv_icm42600.h"
 #include "inv_icm42600_temp.h"
 #include "inv_icm42600_buffer.h"
-#include "inv_icm42600_timestamp.h"
 
 #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info)		\
 	{								\
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
index 32d7f8364230..ba4bb389a9fc 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -9,11 +9,12 @@
 #include <linux/pm_runtime.h>
 #include <linux/regmap.h>
 #include <linux/delay.h>
-#include <linux/iio/iio.h>
+
 #include <linux/iio/buffer.h>
+#include <linux/iio/common/inv_icm42600_timestamp.h>
+#include <linux/iio/iio.h>
 
 #include "inv_icm42600.h"
-#include "inv_icm42600_timestamp.h"
 #include "inv_icm42600_buffer.h"
 
 /* FIFO header: 1 byte */
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index c34735b05830..9d227b4776eb 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -15,11 +15,12 @@
 #include <linux/pm_runtime.h>
 #include <linux/property.h>
 #include <linux/regmap.h>
+
+#include <linux/iio/common/inv_icm42600_timestamp.h>
 #include <linux/iio/iio.h>
 
 #include "inv_icm42600.h"
 #include "inv_icm42600_buffer.h"
-#include "inv_icm42600_timestamp.h"
 
 static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
 	{
@@ -799,3 +800,4 @@ EXPORT_NS_GPL_DEV_PM_OPS(inv_icm42600_pm_ops, IIO_ICM42600) = {
 MODULE_AUTHOR("InvenSense, Inc.");
 MODULE_DESCRIPTION("InvenSense ICM-426xx device driver");
 MODULE_LICENSE("GPL");
+MODULE_IMPORT_NS(IIO_INV_SENSORS_TIMESTAMP);
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index 9d94a8518e3c..0ea3d8bf709d 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -10,14 +10,15 @@
 #include <linux/regmap.h>
 #include <linux/delay.h>
 #include <linux/math64.h>
-#include <linux/iio/iio.h>
+
 #include <linux/iio/buffer.h>
+#include <linux/iio/common/inv_icm42600_timestamp.h>
+#include <linux/iio/iio.h>
 #include <linux/iio/kfifo_buf.h>
 
 #include "inv_icm42600.h"
 #include "inv_icm42600_temp.h"
 #include "inv_icm42600_buffer.h"
-#include "inv_icm42600_timestamp.h"
 
 #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info)		\
 	{								\
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
deleted file mode 100644
index 3e305e7e9887..000000000000
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
+++ /dev/null
@@ -1,186 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0-or-later
-/*
- * Copyright (C) 2020 Invensense, Inc.
- */
-
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/math64.h>
-
-#include "inv_icm42600_timestamp.h"
-
-/* internal chip period is 32kHz, 31250ns */
-#define INV_ICM42600_TIMESTAMP_PERIOD		31250
-/* allow a jitter of +/- 2% */
-#define INV_ICM42600_TIMESTAMP_JITTER		2
-/* compute min and max periods accepted */
-#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p)		\
-	(((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100)
-#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p)		\
-	(((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
-
-/* Add a new value inside an accumulator and update the estimate value */
-static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
-{
-	uint64_t sum = 0;
-	size_t i;
-
-	acc->values[acc->idx++] = val;
-	if (acc->idx >= ARRAY_SIZE(acc->values))
-		acc->idx = 0;
-
-	/* compute the mean of all stored values, use 0 as empty slot */
-	for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
-		if (acc->values[i] == 0)
-			break;
-		sum += acc->values[i];
-	}
-
-	acc->val = div_u64(sum, i);
-}
-
-void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
-				 uint32_t period)
-{
-	/* initial odr for sensor after reset is 1kHz */
-	const uint32_t default_period = 1000000;
-
-	/* current multiplier and period values after reset */
-	ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
-	ts->period = default_period;
-	/* new set multiplier is the one from chip initialization */
-	ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
-
-	/* use theoretical value for chip period */
-	inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
-}
-
-int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
-				      uint32_t period, bool fifo)
-{
-	/* when FIFO is on, prevent odr change if one is already pending */
-	if (fifo && ts->new_mult != 0)
-		return -EAGAIN;
-
-	ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
-
-	return 0;
-}
-
-static bool inv_validate_period(uint32_t period, uint32_t mult)
-{
-	const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
-	uint32_t period_min, period_max;
-
-	/* check that period is acceptable */
-	period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult;
-	period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult;
-	if (period > period_min && period < period_max)
-		return true;
-	else
-		return false;
-}
-
-static bool inv_update_chip_period(struct inv_icm42600_timestamp *ts,
-				   uint32_t mult, uint32_t period)
-{
-	uint32_t new_chip_period;
-
-	if (!inv_validate_period(period, mult))
-		return false;
-
-	/* update chip internal period estimation */
-	new_chip_period = period / mult;
-	inv_update_acc(&ts->chip_period, new_chip_period);
-	ts->period = ts->mult * ts->chip_period.val;
-
-	return true;
-}
-
-static void inv_align_timestamp_it(struct inv_icm42600_timestamp *ts)
-{
-	int64_t delta, jitter;
-	int64_t adjust;
-
-	/* delta time between last sample and last interrupt */
-	delta = ts->it.lo - ts->timestamp;
-
-	/* adjust timestamp while respecting jitter */
-	jitter = div_s64((int64_t)ts->period * INV_ICM42600_TIMESTAMP_JITTER, 100);
-	if (delta > jitter)
-		adjust = jitter;
-	else if (delta < -jitter)
-		adjust = -jitter;
-	else
-		adjust = 0;
-
-	ts->timestamp += adjust;
-}
-
-void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
-				      uint32_t fifo_period, size_t fifo_nb,
-				      size_t sensor_nb, int64_t timestamp)
-{
-	struct inv_icm42600_timestamp_interval *it;
-	int64_t delta, interval;
-	const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
-	uint32_t period = ts->period;
-	bool valid = false;
-
-	if (fifo_nb == 0)
-		return;
-
-	/* update interrupt timestamp and compute chip and sensor periods */
-	it = &ts->it;
-	it->lo = it->up;
-	it->up = timestamp;
-	delta = it->up - it->lo;
-	if (it->lo != 0) {
-		/* compute period: delta time divided by number of samples */
-		period = div_s64(delta, fifo_nb);
-		valid = inv_update_chip_period(ts, fifo_mult, period);
-	}
-
-	/* no previous data, compute theoritical value from interrupt */
-	if (ts->timestamp == 0) {
-		/* elapsed time: sensor period * sensor samples number */
-		interval = (int64_t)ts->period * (int64_t)sensor_nb;
-		ts->timestamp = it->up - interval;
-		return;
-	}
-
-	/* if interrupt interval is valid, sync with interrupt timestamp */
-	if (valid)
-		inv_align_timestamp_it(ts);
-}
-
-void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
-				      uint32_t fifo_period, size_t fifo_nb,
-				      unsigned int fifo_no)
-{
-	int64_t interval;
-	uint32_t fifo_mult;
-
-	if (ts->new_mult == 0)
-		return;
-
-	/* update to new multiplier and update period */
-	ts->mult = ts->new_mult;
-	ts->new_mult = 0;
-	ts->period = ts->mult * ts->chip_period.val;
-
-	/*
-	 * After ODR change the time interval with the previous sample is
-	 * undertermined (depends when the change occures). So we compute the
-	 * timestamp from the current interrupt using the new FIFO period, the
-	 * total number of samples and the current sample numero.
-	 */
-	if (ts->timestamp != 0) {
-		/* compute measured fifo period */
-		fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
-		fifo_period = fifo_mult * ts->chip_period.val;
-		/* computes time interval between interrupt and this sample */
-		interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
-		ts->timestamp = ts->it.up - interval;
-	}
-}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
deleted file mode 100644
index b808a6da15e5..000000000000
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-or-later */
-/*
- * Copyright (C) 2020 Invensense, Inc.
- */
-
-#ifndef INV_ICM42600_TIMESTAMP_H_
-#define INV_ICM42600_TIMESTAMP_H_
-
-/**
- * struct inv_icm42600_timestamp_interval - timestamps interval
- * @lo:	interval lower bound
- * @up:	interval upper bound
- */
-struct inv_icm42600_timestamp_interval {
-	int64_t lo;
-	int64_t up;
-};
-
-/**
- * struct inv_icm42600_timestamp_acc - accumulator for computing an estimation
- * @val:	current estimation of the value, the mean of all values
- * @idx:	current index of the next free place in values table
- * @values:	table of all measured values, use for computing the mean
- */
-struct inv_icm42600_timestamp_acc {
-	uint32_t val;
-	size_t idx;
-	uint32_t values[32];
-};
-
-/**
- * struct inv_icm42600_timestamp - timestamp management states
- * @it:			interrupts interval timestamps
- * @timestamp:		store last timestamp for computing next data timestamp
- * @mult:		current internal period multiplier
- * @new_mult:		new set internal period multiplier (not yet effective)
- * @period:		measured current period of the sensor
- * @chip_period:	accumulator for computing internal chip period
- */
-struct inv_icm42600_timestamp {
-	struct inv_icm42600_timestamp_interval it;
-	int64_t timestamp;
-	uint32_t mult;
-	uint32_t new_mult;
-	uint32_t period;
-	struct inv_icm42600_timestamp_acc chip_period;
-};
-
-void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
-				 uint32_t period);
-
-int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
-				      uint32_t period, bool fifo);
-
-void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
-				      uint32_t fifo_period, size_t fifo_nb,
-				      size_t sensor_nb, int64_t timestamp);
-
-static inline int64_t
-inv_icm42600_timestamp_pop(struct inv_icm42600_timestamp *ts)
-{
-	ts->timestamp += ts->period;
-	return ts->timestamp;
-}
-
-void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
-				      uint32_t fifo_period, size_t fifo_nb,
-				      unsigned int fifo_no);
-
-static inline void
-inv_icm42600_timestamp_reset(struct inv_icm42600_timestamp *ts)
-{
-	const struct inv_icm42600_timestamp_interval interval_init = {0LL, 0LL};
-
-	ts->it = interval_init;
-	ts->timestamp = 0;
-}
-
-#endif