summaryrefslogtreecommitdiffstats
path: root/drivers/iio/common/inv_sensors
diff options
context:
space:
mode:
authorDaniel Baumann <daniel.baumann@progress-linux.org>2024-04-11 08:27:49 +0000
committerDaniel Baumann <daniel.baumann@progress-linux.org>2024-04-11 08:27:49 +0000
commitace9429bb58fd418f0c81d4c2835699bddf6bde6 (patch)
treeb2d64bc10158fdd5497876388cd68142ca374ed3 /drivers/iio/common/inv_sensors
parentInitial commit. (diff)
downloadlinux-ace9429bb58fd418f0c81d4c2835699bddf6bde6.tar.xz
linux-ace9429bb58fd418f0c81d4c2835699bddf6bde6.zip
Adding upstream version 6.6.15.upstream/6.6.15
Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>
Diffstat (limited to 'drivers/iio/common/inv_sensors')
-rw-r--r--drivers/iio/common/inv_sensors/Kconfig7
-rw-r--r--drivers/iio/common/inv_sensors/Makefile6
-rw-r--r--drivers/iio/common/inv_sensors/inv_sensors_timestamp.c194
3 files changed, 207 insertions, 0 deletions
diff --git a/drivers/iio/common/inv_sensors/Kconfig b/drivers/iio/common/inv_sensors/Kconfig
new file mode 100644
index 0000000000..28815fb431
--- /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 0000000000..dcf39f2491
--- /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_sensors_timestamp.o
diff --git a/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c b/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c
new file mode 100644
index 0000000000..03823ee57f
--- /dev/null
+++ b/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c
@@ -0,0 +1,194 @@
+// 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_sensors_timestamp.h>
+
+/* compute jitter, min and max following jitter in per mille */
+#define INV_SENSORS_TIMESTAMP_JITTER(_val, _jitter) \
+ (div_s64((_val) * (_jitter), 1000))
+#define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \
+ (((_val) * (1000 - (_jitter))) / 1000)
+#define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \
+ (((_val) * (1000 + (_jitter))) / 1000)
+
+/* Add a new value inside an accumulator and update the estimate value */
+static void inv_update_acc(struct inv_sensors_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_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
+ const struct inv_sensors_timestamp_chip *chip)
+{
+ memset(ts, 0, sizeof(*ts));
+
+ /* save chip parameters and compute min and max clock period */
+ ts->chip = *chip;
+ ts->min_period = INV_SENSORS_TIMESTAMP_MIN(chip->clock_period, chip->jitter);
+ ts->max_period = INV_SENSORS_TIMESTAMP_MAX(chip->clock_period, chip->jitter);
+
+ /* current multiplier and period values after reset */
+ ts->mult = chip->init_period / chip->clock_period;
+ ts->period = chip->init_period;
+
+ /* use theoretical value for chip period */
+ inv_update_acc(&ts->chip_period, chip->clock_period);
+}
+EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);
+
+int inv_sensors_timestamp_update_odr(struct inv_sensors_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 / ts->chip.clock_period;
+
+ return 0;
+}
+EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);
+
+static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult)
+{
+ uint32_t period_min, period_max;
+
+ /* check that period is acceptable */
+ period_min = ts->min_period * mult;
+ period_max = ts->max_period * mult;
+ if (period > period_min && period < period_max)
+ return true;
+ else
+ return false;
+}
+
+static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
+ uint32_t mult, uint32_t period)
+{
+ uint32_t new_chip_period;
+
+ if (!inv_validate_period(ts, 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_sensors_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 = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter);
+ if (delta > jitter)
+ adjust = jitter;
+ else if (delta < -jitter)
+ adjust = -jitter;
+ else
+ adjust = 0;
+
+ ts->timestamp += adjust;
+}
+
+void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ size_t sensor_nb, int64_t timestamp)
+{
+ struct inv_sensors_timestamp_interval *it;
+ int64_t delta, interval;
+ const uint32_t fifo_mult = fifo_period / ts->chip.clock_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_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);
+
+void inv_sensors_timestamp_apply_odr(struct inv_sensors_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 / ts->chip.clock_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_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense sensors timestamp module");
+MODULE_LICENSE("GPL");