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>

5 files changed, 210 insertions, 0 deletions

diff --git a/drivers/iio/common/Kconfig b/drivers/iio/common/Kconfig
index 0334b4954773..1ccb5ccf3706 100644
--- a/drivers/iio/common/Kconfig
+++ b/drivers/iio/common/Kconfig
@@ -5,6 +5,7 @@
 
 source "drivers/iio/common/cros_ec_sensors/Kconfig"
 source "drivers/iio/common/hid-sensors/Kconfig"
+source "drivers/iio/common/inv_sensors/Kconfig"
 source "drivers/iio/common/ms_sensors/Kconfig"
 source "drivers/iio/common/scmi_sensors/Kconfig"
 source "drivers/iio/common/ssp_sensors/Kconfig"
diff --git a/drivers/iio/common/Makefile b/drivers/iio/common/Makefile
index fad40e1e1718..d3e952239a62 100644
--- a/drivers/iio/common/Makefile
+++ b/drivers/iio/common/Makefile
@@ -10,6 +10,7 @@
 # When adding new entries keep the list in alphabetical order
 obj-y += cros_ec_sensors/
 obj-y += hid-sensors/
+obj-y += inv_sensors/
 obj-y += ms_sensors/
 obj-y += scmi_sensors/
 obj-y += ssp_sensors/
diff --git a/drivers/iio/common/inv_sensors/Kconfig b/drivers/iio/common/inv_sensors/Kconfig
new file mode 100644
index 000000000000..28815fb43157
--- /dev/null
+++ b/drivers/iio/common/inv_sensors/Kconfig
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# TDK-InvenSense sensors common library
+#
+
+config IIO_INV_SENSORS_TIMESTAMP
+	tristate
diff --git a/drivers/iio/common/inv_sensors/Makefile b/drivers/iio/common/inv_sensors/Makefile
new file mode 100644
index 000000000000..93bddb9356b8
--- /dev/null
+++ b/drivers/iio/common/inv_sensors/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for TDK-InvenSense sensors module.
+#
+
+obj-$(CONFIG_IIO_INV_SENSORS_TIMESTAMP) += inv_icm42600_timestamp.o
diff --git a/drivers/iio/common/inv_sensors/inv_icm42600_timestamp.c b/drivers/iio/common/inv_sensors/inv_icm42600_timestamp.c
new file mode 100644
index 000000000000..7cd80cdf0ee5
--- /dev/null
+++ b/drivers/iio/common/inv_sensors/inv_icm42600_timestamp.c
@@ -0,0 +1,195 @@
+// 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 <linux/module.h>
+
+#include <linux/iio/common/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);
+}
+EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);
+
+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;
+}
+EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);
+
+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);
+}
+EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);
+
+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;
+	}
+}
+EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense sensors timestamp module");
+MODULE_LICENSE("GPL");