On Thu, 7 May 2020 16:42:20 +0200
Jean-Baptiste Maneyrol <jmaney...@invensense.com> wrote:
> Add a timestamp channel with processed value that returns full
> precision 20 bits timestamp.
>
> Add a timestamping mechanism for buffer that provides accurate
> event timestamps when using watermark. This mechanism estimates
> device internal clock by comparing FIFO interrupts delta time and
> corresponding device elapsed time computed by parsing FIFO data.
Yikes. That is complex. Hmm. I always get lost in the maths in these
timestamp patches so my review may be a little superficial.
However a bit more in the way of docs would be good here.
The sysfs read of timestamp is unusual and I'm not really sure what it is for.
The delays in a sysfs read of that value are likely to be enough that it's
that useful for anything.
Also, do we make use of the timestamps within the fifo records?
J
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaney...@invensense.com>
> ---
> drivers/iio/imu/inv_icm42600/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 10 +-
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 32 ++-
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 28 +-
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 6 +
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 32 ++-
> .../imu/inv_icm42600/inv_icm42600_timestamp.c | 246 ++++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_timestamp.h | 82 ++++++
> 8 files changed, 421 insertions(+), 16 deletions(-)
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
>
> diff --git a/drivers/iio/imu/inv_icm42600/Makefile
> b/drivers/iio/imu/inv_icm42600/Makefile
> index d6732118010c..1197b545a682 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -7,6 +7,7 @@ inv-icm42600-y += inv_icm42600_accel.o
> inv-icm42600-y += inv_icm42600_temp.o
> inv-icm42600-y += inv_icm42600_trigger.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.h
> b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> index 947ca4dd245b..e15eddafe009 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -16,6 +16,7 @@
> #include <linux/iio/trigger.h>
>
> #include "inv_icm42600_buffer.h"
> +#include "inv_icm42600_timestamp.h"
>
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> @@ -127,6 +128,7 @@ struct inv_icm42600_suspended {
> * @indio_accel: accelerometer IIO device.
> * @trigger: device internal interrupt trigger
> * @fifo: FIFO management structure.
> + * @timestamp: timestamp management structure.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -142,6 +144,10 @@ struct inv_icm42600_state {
> struct iio_dev *indio_accel;
> struct iio_trigger *trigger;
> struct inv_icm42600_fifo fifo;
> + struct {
> + struct inv_icm42600_timestamp gyro;
> + struct inv_icm42600_timestamp accel;
> + } timestamp;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB
> */
> @@ -382,11 +388,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int
> chip, int irq,
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> -int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev);
>
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> -int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
>
> int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> int irq_type);
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 4206be54d057..ac140c824c03 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> @@ -17,6 +17,7 @@
> #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)
> \
> { \
> @@ -66,7 +67,7 @@ static const struct iio_chan_spec
> inv_icm42600_accel_channels[] = {
> INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
> inv_icm42600_accel_ext_infos),
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
> - IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
> + INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
> };
>
> /* IIO buffer data */
> @@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_accel_handler(int irq,
> void *_data)
> struct iio_poll_func *pf = _data;
> struct iio_dev *indio_dev = pf->indio_dev;
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.accel;
> const size_t fifo_nb = st->fifo.nb.total;
> + const size_t accel_nb = st->fifo.nb.accel;
> + const uint32_t fifo_period = st->fifo.period;
> int ret;
>
> /* exit if no sample */
> if (fifo_nb == 0)
> goto out;
>
> - ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
> + inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, accel_nb,
> + pf->timestamp);
> +
> + ret = inv_icm42600_accel_parse_fifo(indio_dev);
> if (ret)
> dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
> ret);
> @@ -143,6 +150,7 @@ static int inv_icm42600_accel_update_scan_mode(struct
> iio_dev *indio_dev,
> }
>
> /* update data FIFO write and FIFO watermark */
> + inv_icm42600_timestamp_apply_odr(&st->timestamp.accel, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -347,6 +355,7 @@ static int inv_icm42600_accel_write_odr(struct
> inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
> ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> + inv_icm42600_timestamp_update_odr(&st->timestamp.accel, conf.odr);
> inv_icm42600_buffer_update_fifo_period(st);
> inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
> @@ -502,6 +511,9 @@ static int inv_icm42600_accel_read_raw(struct iio_dev
> *indio_dev,
> case IIO_TEMP:
> return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
> mask);
> + case IIO_TIMESTAMP:
> + return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
> + val2, mask);
> default:
> return -EINVAL;
> }
> @@ -694,13 +706,18 @@ int inv_icm42600_accel_init(struct inv_icm42600_state
> *st)
> return devm_iio_device_register(dev, st->indio_accel);
> }
>
> -int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
> {
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.accel;
> + const size_t fifo_nb = st->fifo.nb.total;
> const size_t accel_nb = st->fifo.nb.accel;
> + const uint32_t fifo_period = st->fifo.period;
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *temp, *timestamp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_accel_buffer buffer;
>
> /* exit if no accel sample */
> @@ -708,7 +725,7 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev
> *indio_dev, int64_t ts)
> return 0;
>
> /* parse all fifo packets */
> - for (i = 0; i < st->fifo.count; i += size) {
> + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
> size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> &accel, &gyro, &temp, ×tamp, &odr);
> dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
> @@ -721,9 +738,14 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev
> *indio_dev, int64_t ts)
> dev_dbg(regmap_get_device(st->map), "skip accel
> data\n");
> continue;
> }
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_ACCEL)
> + inv_icm42600_timestamp_apply_odr(ts, fifo_period,
> + fifo_nb, no);
> memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> - iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + ts_val = inv_icm42600_timestamp_get(ts);
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
> }
>
> return 0;
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> index b428abdc92ee..bea4c9810da7 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -15,6 +15,7 @@
> #include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_timestamp.h"
> #include "inv_icm42600_buffer.h"
>
> void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
> @@ -194,14 +195,17 @@ static int inv_icm42600_buffer_postdisable(struct
> iio_dev *indio_dev)
> unsigned int *watermark;
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> unsigned int sleep = 0;
> + struct inv_icm42600_timestamp *ts;
> int ret;
>
> if (indio_dev == st->indio_gyro) {
> sensor = INV_ICM42600_SENSOR_GYRO;
> watermark = &st->fifo.watermark.gyro;
> + ts = &st->timestamp.gyro;
> } else if (indio_dev == st->indio_accel) {
> sensor = INV_ICM42600_SENSOR_ACCEL;
> watermark = &st->fifo.watermark.accel;
> + ts = &st->timestamp.accel;
> } else {
> return -EINVAL;
> }
> @@ -223,6 +227,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev
> *indio_dev)
> else
> ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
>
> + inv_icm42600_timestamp_reset(ts);
> +
> out_unlock:
> mutex_unlock(&st->lock);
> if (sleep)
> @@ -316,11 +322,25 @@ int inv_icm42600_buffer_hwfifo_flush(struct
> inv_icm42600_state *st,
> if (st->fifo.nb.total == 0)
> return 0;
>
> - ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
> - if (ret)
> - return ret;
> + if (st->fifo.nb.gyro > 0) {
> + inv_icm42600_timestamp_interrupt(&st->timestamp.gyro,
> + st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.gyro, ts_gyro);
> + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> + if (ret)
> + return ret;
> + }
>
> - return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
> + if (st->fifo.nb.accel > 0) {
> + inv_icm42600_timestamp_interrupt(&st->timestamp.accel,
> + st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.accel, ts_accel);
> + ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> + if (ret)
> + return ret;
> + }
> +
> + return ret;
> }
>
> int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> index 689089065ff9..563c4348de01 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -15,6 +15,7 @@
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_buffer.h"
> +#include "inv_icm42600_timestamp.h"
>
> static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
> {
> @@ -516,6 +517,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int
> chip, int irq,
> if (ret)
> return ret;
>
> + /* initialize timestamping */
> + ret = inv_icm42600_timestamp_init(st);
> + if (ret)
> + return ret;
> +
> /* setup FIFO buffer */
> ret = inv_icm42600_buffer_init(st);
> if (ret)
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> index dafb104abc77..1245588170bd 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -17,6 +17,7 @@
> #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) \
> { \
> @@ -66,7 +67,7 @@ static const struct iio_chan_spec
> inv_icm42600_gyro_channels[] = {
> INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
> inv_icm42600_gyro_ext_infos),
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
> - IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
Interrupt timestamp was pretty much garbage, so I'd just not register that in
the
first place. Introduce the timestamp for the first time in this patch.
> + INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
> };
>
> /* IIO buffer data */
> @@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_gyro_handler(int irq,
> void *_data)
> struct iio_poll_func *pf = _data;
> struct iio_dev *indio_dev = pf->indio_dev;
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.gyro;
> const size_t fifo_nb = st->fifo.nb.total;
> + const size_t gyro_nb = st->fifo.nb.gyro;
> + const uint32_t fifo_period = st->fifo.period;
> int ret;
>
> /* exit if no sample */
> if (fifo_nb == 0)
> goto out;
>
> - ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
> + inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, gyro_nb,
> + pf->timestamp);
> +
> + ret = inv_icm42600_gyro_parse_fifo(indio_dev);
> if (ret)
> dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
> ret);
> @@ -143,6 +150,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct
> iio_dev *indio_dev,
> }
>
> /* update data FIFO write and FIFO watermark */
> + inv_icm42600_timestamp_apply_odr(&st->timestamp.gyro, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -359,6 +367,7 @@ static int inv_icm42600_gyro_write_odr(struct
> inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
> ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> + inv_icm42600_timestamp_update_odr(&st->timestamp.gyro, conf.odr);
> inv_icm42600_buffer_update_fifo_period(st);
> inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
> @@ -514,6 +523,9 @@ static int inv_icm42600_gyro_read_raw(struct iio_dev
> *indio_dev,
> case IIO_TEMP:
> return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
> mask);
> + case IIO_TIMESTAMP:
> + return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
> + val2, mask);
> default:
> return -EINVAL;
> }
> @@ -706,13 +718,18 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state
> *st)
> return devm_iio_device_register(dev, st->indio_gyro);
> }
>
> -int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
> {
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.gyro;
> + const size_t fifo_nb = st->fifo.nb.total;
> const size_t gyro_nb = st->fifo.nb.gyro;
> + const uint32_t fifo_period = st->fifo.period;
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *temp, *timestamp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_gyro_buffer buffer;
>
> /* exit if no gyro sample */
> @@ -720,7 +737,7 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev
> *indio_dev, int64_t ts)
> return 0;
>
> /* parse all fifo packets */
> - for (i = 0; i < st->fifo.count; i += size) {
> + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
> size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> &accel, &gyro, &temp, ×tamp, &odr);
> dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
> @@ -733,9 +750,14 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev
> *indio_dev, int64_t ts)
> dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
> continue;
> }
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_GYRO)
> + inv_icm42600_timestamp_apply_odr(ts, fifo_period,
> + fifo_nb, no);
> memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> - iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + ts_val = inv_icm42600_timestamp_get(ts);
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
> }
>
> return 0;
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> new file mode 100644
> index 000000000000..79cf777e2e84
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> @@ -0,0 +1,246 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/math64.h>
> +#include <linux/iio/iio.h>
> +
> +#include "inv_icm42600.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)
> +
> +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;
> +
> + 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);
> +}
> +
> +static int inv_icm42600_timestamp_read(struct inv_icm42600_state *st,
> + uint32_t *ts)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + __le32 raw;
> + int ret;
> +
> + pm_runtime_get_sync(dev);
> + mutex_lock(&st->lock);
> +
> + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> + INV_ICM42600_SIGNAL_PATH_RESET_TMST_STROBE);
> + if (ret)
> + goto exit;
> +
> + /* Read timestamp value 3 registers little-endian */
> + raw = 0;
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TMSTVAL, &raw, 3);
> + if (ret)
> + goto exit;
> +
> + *ts = (uint32_t)le32_to_cpu(raw);
> +exit:
> + mutex_unlock(&st->lock);
> + pm_runtime_mark_last_busy(dev);
> + pm_runtime_put_autosuspend(dev);
> + return ret;
> +}
> +
> +int inv_icm42600_timestamp_read_raw(struct iio_dev *indio_dev,
> + struct iio_chan_spec const *chan,
> + int *val, int *val2, long mask)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + uint32_t ts;
> + int ret;
> +
> + if (chan->type != IIO_TIMESTAMP)
> + return -EINVAL;
> +
> + switch (mask) {
> + case IIO_CHAN_INFO_PROCESSED:
> + ret = iio_device_claim_direct_mode(indio_dev);
> + if (ret)
> + return ret;
> + ret = inv_icm42600_timestamp_read(st, &ts);
> + iio_device_release_direct_mode(indio_dev);
Unusual to expose it as a readable channel. Why would you want to do this?
> + if (ret)
> + return ret;
> + *val = ts * 1000;
> + return IIO_VAL_INT;
> + default:
> + return -EINVAL;
> + }
> +}
> +
> +static void inv_icm42600_init_ts(struct inv_icm42600_timestamp *ts,
> + struct device *dev, uint32_t period)
> +{
> + /* initial odr for sensor is 1kHz */
> + const uint32_t default_period = 1000000;
> +
> + ts->dev = dev;
> + ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> + ts->period = default_period;
> +
> + /* use theoretical value for chip period */
> + inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
> +}
> +
> +int inv_icm42600_timestamp_init(struct inv_icm42600_state *st)
> +{
> + unsigned int val;
> +
> + inv_icm42600_init_ts(&st->timestamp.gyro, regmap_get_device(st->map),
> + inv_icm42600_odr_to_period(st->conf.gyro.odr));
> + inv_icm42600_init_ts(&st->timestamp.accel, regmap_get_device(st->map),
> + inv_icm42600_odr_to_period(st->conf.accel.odr));
> +
> + /* enable timestamp register */
> + val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN |
> + INV_ICM42600_TMST_CONFIG_TMST_EN;
> + return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG,
> + INV_ICM42600_TMST_CONFIG_MASK, val);
> +}
> +
> +void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + int odr)
> +{
> + uint32_t period;
> +
> + period = inv_icm42600_odr_to_period(odr);
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> + dev_dbg(ts->dev, "ts new mult: %u\n", ts->new_mult);
> +}
> +
> +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 time interval between interrupts 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_compute_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);
> +
> + dev_dbg(ts->dev, "ts chip period: %u - val %u\n", new_chip_period,
> + ts->chip_period.val);
> +
> + return true;
> +}
> +
> +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;
> + int32_t m;
> + 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;
> + dev_dbg(ts->dev, "ts it: %lld - %lld - %lld\n", it->lo, it->up, delta);
> + if (it->lo != 0) {
> + period = div_s64(delta, fifo_nb);
> + valid = inv_compute_chip_period(ts, fifo_mult, period);
> + if (valid)
> + ts->period = ts->mult * ts->chip_period.val;
> + }
> +
> + /* no previous data, compute theoritical value from interrupt */
> + if (ts->timestamp == 0) {
> + interval = (int64_t)ts->period * (int64_t)sensor_nb;
> + ts->timestamp = it->up - interval;
> + dev_dbg(ts->dev, "ts start: %lld\n", ts->timestamp);
> + return;
> + }
> +
> + /* if interrupt interval is valid, sync with interrupt timestamp */
> + if (valid) {
> + /* compute real fifo_period */
> + fifo_period = fifo_mult * ts->chip_period.val;
> + delta = it->lo - ts->timestamp;
> + while (delta >= (fifo_period * 3 / 2))
> + delta -= fifo_period;
> + /* compute maximal adjustment value */
> + m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period;
> + if (delta > m)
> + delta = m;
> + else if (delta < -m)
> + delta = -m;
> + dev_dbg(ts->dev, "ts adj: %lld\n", delta);
> + ts->timestamp += delta;
> + }
> +}
> +
> +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;
> +
> + ts->mult = ts->new_mult;
> + ts->period = ts->mult * ts->chip_period.val;
> + dev_dbg(ts->dev, "ts mult: %u\n", ts->mult);
> +
> + if (ts->timestamp != 0) {
> + /* compute real fifo period */
> + fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + fifo_period = fifo_mult * ts->chip_period.val;
> + /* compute timestamp from current interrupt after ODR change */
> + interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
> + ts->timestamp = ts->it.up - interval;
> + dev_dbg(ts->dev, "ts new: %lld\n", ts->timestamp);
> + }
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
> b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
> new file mode 100644
> index 000000000000..c865e1a9a7c8
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
> @@ -0,0 +1,82 @@
> +/* SPDX-License-Identifier: GPL-2.0-or-later */
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#ifndef INV_ICM42600_TIMESTAMP_H_
> +#define INV_ICM42600_TIMESTAMP_H_
> +
> +#include <linux/device.h>
> +#include <linux/iio/iio.h>
> +
> +struct inv_icm42600_state;
> +
> +struct inv_icm42600_timestamp_interval {
> + int64_t lo;
> + int64_t up;
> +};
> +
> +struct inv_icm42600_timestamp_acc {
> + uint32_t val;
> + size_t idx;
> + uint32_t values[32];
> +};
> +
> +struct inv_icm42600_timestamp {
> + struct device *dev;
> + 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;
> +};
> +
> +#define INV_ICM42600_TIMESTAMP_CHAN(_index) \
> + { \
> + .type = IIO_TIMESTAMP, \
> + .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), \
> + .scan_index = _index, \
> + .scan_type = { \
> + .sign = 's', \
> + .realbits = 64, \
> + .storagebits = 64, \
> + }, \
> + }
> +
> +int inv_icm42600_timestamp_read_raw(struct iio_dev *indio_dev,
> + struct iio_chan_spec const *chan,
> + int *val, int *val2, long mask);
> +
> +int inv_icm42600_timestamp_init(struct inv_icm42600_state *st);
> +
> +void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + int odr);
> +
> +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_get(struct inv_icm42600_timestamp *ts)
Perhaps timestamp_pop? Kind of indicates that you are destructively
retrieving a timetamp.
> +{
> + ts->timestamp += ts->period;
> + dev_dbg(ts->dev, "ts: %lld\n", ts->timestamp);
> +
> + 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
