diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile index 0f49f6df3647b..291714d9aa54f 100644 --- a/drivers/iio/imu/inv_icm42600/Makefile +++ b/drivers/iio/imu/inv_icm42600/Makefile @@ -6,6 +6,7 @@ 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.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h index 7b52d92739c33..c0f5059b13b31 100644 --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h @@ -126,6 +126,7 @@ struct inv_icm42600_suspended { * @indio_accel: accelerometer IIO device. * @buffer: data transfer buffer aligned for DMA. * @fifo: FIFO management structure. + * @timestamp: interrupt timestamps. */ struct inv_icm42600_state { struct mutex lock; @@ -141,6 +142,10 @@ struct inv_icm42600_state { struct iio_dev *indio_accel; uint8_t buffer[2] ____cacheline_aligned; struct inv_icm42600_fifo fifo; + struct { + int64_t gyro; + int64_t accel; + } timestamp; }; /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */ diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c index fbc029bd6e726..3441b0d61c5d5 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) \ { \ @@ -50,6 +51,7 @@ enum inv_icm42600_accel_scan { INV_ICM42600_ACCEL_SCAN_Y, INV_ICM42600_ACCEL_SCAN_Z, INV_ICM42600_ACCEL_SCAN_TEMP, + INV_ICM42600_ACCEL_SCAN_TIMESTAMP, }; static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = { @@ -65,15 +67,17 @@ 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), }; /* - * IIO buffer data: size must be a power of 2 - * 8 bytes: 6 bytes acceleration and 2 bytes temperature + * IIO buffer data: size must be a power of 2 and timestamp aligned + * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp */ struct inv_icm42600_accel_buffer { struct inv_icm42600_fifo_sensor_data accel; int16_t temp; + int64_t timestamp __aligned(8); }; #define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \ @@ -94,6 +98,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev, const unsigned long *scan_mask) { struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; unsigned int fifo_en = 0; unsigned int sleep_temp = 0; @@ -121,6 +126,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev, } /* update data FIFO write */ + inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0); ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); if (ret) goto out_unlock; @@ -301,9 +307,11 @@ static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st, return IIO_VAL_INT_PLUS_MICRO; } -static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st, +static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev, int val, int val2) { + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); struct device *dev = regmap_get_device(st->map); unsigned int idx; struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; @@ -322,6 +330,11 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st, pm_runtime_get_sync(dev); mutex_lock(&st->lock); + ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), + iio_buffer_enabled(indio_dev)); + if (ret) + goto out_unlock; + ret = inv_icm42600_set_accel_conf(st, &conf, NULL); if (ret) goto out_unlock; @@ -611,7 +624,7 @@ static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev, iio_device_release_direct_mode(indio_dev); return ret; case IIO_CHAN_INFO_SAMP_FREQ: - return inv_icm42600_accel_write_odr(st, val, val2); + return inv_icm42600_accel_write_odr(indio_dev, val, val2); case IIO_CHAN_INFO_CALIBBIAS: ret = iio_device_claim_direct_mode(indio_dev); if (ret) @@ -694,6 +707,7 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) { struct device *dev = regmap_get_device(st->map); const char *name; + struct inv_icm42600_timestamp *ts; struct iio_dev *indio_dev; struct iio_buffer *buffer; int ret; @@ -702,7 +716,7 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) if (!name) return ERR_PTR(-ENOMEM); - indio_dev = devm_iio_device_alloc(dev, 0); + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts)); if (!indio_dev) return ERR_PTR(-ENOMEM); @@ -710,6 +724,9 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) if (!buffer) return ERR_PTR(-ENOMEM); + ts = iio_priv(indio_dev); + inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr)); + iio_device_set_drvdata(indio_dev, st); indio_dev->name = name; indio_dev->info = &inv_icm42600_accel_info; @@ -731,14 +748,17 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) 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 = iio_priv(indio_dev); ssize_t i, size; + unsigned int no; const void *accel, *gyro, *timestamp; const int8_t *temp; unsigned int odr; + int64_t ts_val; struct inv_icm42600_accel_buffer buffer; /* 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); /* quit if error or FIFO is empty */ @@ -749,12 +769,18 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev) if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) continue; + /* update odr */ + if (odr & INV_ICM42600_SENSOR_ACCEL) + inv_icm42600_timestamp_apply_odr(ts, st->fifo.period, + st->fifo.nb.total, no); + /* buffer is copied to userspace, zeroing it to avoid any data leak */ memset(&buffer, 0, sizeof(buffer)); memcpy(&buffer.accel, accel, sizeof(buffer.accel)); /* convert 8 bits FIFO temperature in high resolution format */ buffer.temp = temp ? (*temp * 64) : 0; - iio_push_to_buffers(indio_dev, &buffer); + ts_val = inv_icm42600_timestamp_pop(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 e58e6f0c5698d..99576b2c171f4 100644 --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c @@ -13,6 +13,7 @@ #include #include "inv_icm42600.h" +#include "inv_icm42600_timestamp.h" #include "inv_icm42600_buffer.h" /* FIFO header: 1 byte */ @@ -374,6 +375,7 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev) struct device *dev = regmap_get_device(st->map); unsigned int sensor; unsigned int *watermark; + struct inv_icm42600_timestamp *ts; struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; unsigned int sleep_temp = 0; unsigned int sleep_sensor = 0; @@ -383,9 +385,11 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev) if (indio_dev == st->indio_gyro) { sensor = INV_ICM42600_SENSOR_GYRO; watermark = &st->fifo.watermark.gyro; + ts = iio_priv(st->indio_gyro); } else if (indio_dev == st->indio_accel) { sensor = INV_ICM42600_SENSOR_ACCEL; watermark = &st->fifo.watermark.accel; + ts = iio_priv(st->indio_accel); } else { return -EINVAL; } @@ -413,6 +417,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev) if (!st->fifo.on) ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp); + inv_icm42600_timestamp_reset(ts); + out_unlock: mutex_unlock(&st->lock); @@ -498,17 +504,26 @@ int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st, int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) { + struct inv_icm42600_timestamp *ts; int ret; if (st->fifo.nb.total == 0) return 0; + /* handle gyroscope timestamp and FIFO data parsing */ + ts = iio_priv(st->indio_gyro); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total, + st->fifo.nb.gyro, st->timestamp.gyro); if (st->fifo.nb.gyro > 0) { ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); if (ret) return ret; } + /* handle accelerometer timestamp and FIFO data parsing */ + ts = iio_priv(st->indio_accel); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total, + st->fifo.nb.accel, st->timestamp.accel); if (st->fifo.nb.accel > 0) { ret = inv_icm42600_accel_parse_fifo(st->indio_accel); if (ret) @@ -521,8 +536,13 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st, unsigned int count) { + struct inv_icm42600_timestamp *ts; + int64_t gyro_ts, accel_ts; int ret; + gyro_ts = iio_get_time_ns(st->indio_gyro); + accel_ts = iio_get_time_ns(st->indio_accel); + ret = inv_icm42600_buffer_fifo_read(st, count); if (ret) return ret; @@ -531,12 +551,20 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st, return 0; if (st->fifo.nb.gyro > 0) { + ts = iio_priv(st->indio_gyro); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, + st->fifo.nb.total, st->fifo.nb.gyro, + gyro_ts); ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); if (ret) return ret; } if (st->fifo.nb.accel > 0) { + ts = iio_priv(st->indio_accel); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, + st->fifo.nb.total, st->fifo.nb.accel, + accel_ts); ret = inv_icm42600_accel_parse_fifo(st->indio_accel); if (ret) return ret; diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c index 9e1f6e65fd451..8bd77185ccb71 100644 --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c @@ -19,6 +19,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[] = { { @@ -412,6 +413,16 @@ static int inv_icm42600_setup(struct inv_icm42600_state *st, return inv_icm42600_set_conf(st, hw->conf); } +static irqreturn_t inv_icm42600_irq_timestamp(int irq, void *_data) +{ + struct inv_icm42600_state *st = _data; + + st->timestamp.gyro = iio_get_time_ns(st->indio_gyro); + st->timestamp.accel = iio_get_time_ns(st->indio_accel); + + return IRQ_WAKE_THREAD; +} + static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data) { struct inv_icm42600_state *st = _data; @@ -495,7 +506,7 @@ static int inv_icm42600_irq_init(struct inv_icm42600_state *st, int irq, if (ret) return ret; - return devm_request_threaded_irq(dev, irq, NULL, + return devm_request_threaded_irq(dev, irq, inv_icm42600_irq_timestamp, inv_icm42600_irq_handler, irq_type, "inv_icm42600", st); } @@ -617,6 +628,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq, if (ret) return ret; + ret = inv_icm42600_timestamp_setup(st); + if (ret) + return ret; + ret = inv_icm42600_buffer_init(st); if (ret) return ret; diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c index 3b1f7594b60bf..aee7b9ff4bf4f 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) \ { \ @@ -50,6 +51,7 @@ enum inv_icm42600_gyro_scan { INV_ICM42600_GYRO_SCAN_Y, INV_ICM42600_GYRO_SCAN_Z, INV_ICM42600_GYRO_SCAN_TEMP, + INV_ICM42600_GYRO_SCAN_TIMESTAMP, }; static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = { @@ -65,15 +67,17 @@ 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), }; /* - * IIO buffer data: size must be a power of 2 - * 8 bytes: 6 bytes angular velocity and 2 bytes temperature + * IIO buffer data: size must be a power of 2 and timestamp aligned + * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp */ struct inv_icm42600_gyro_buffer { struct inv_icm42600_fifo_sensor_data gyro; int16_t temp; + int64_t timestamp __aligned(8); }; #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \ @@ -94,6 +98,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev, const unsigned long *scan_mask) { struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; unsigned int fifo_en = 0; unsigned int sleep_gyro = 0; @@ -121,6 +126,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev, } /* update data FIFO write */ + inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0); ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); if (ret) goto out_unlock; @@ -313,9 +319,11 @@ static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st, return IIO_VAL_INT_PLUS_MICRO; } -static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st, +static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev, int val, int val2) { + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); struct device *dev = regmap_get_device(st->map); unsigned int idx; struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; @@ -334,6 +342,11 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st, pm_runtime_get_sync(dev); mutex_lock(&st->lock); + ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), + iio_buffer_enabled(indio_dev)); + if (ret) + goto out_unlock; + ret = inv_icm42600_set_gyro_conf(st, &conf, NULL); if (ret) goto out_unlock; @@ -622,7 +635,7 @@ static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev, iio_device_release_direct_mode(indio_dev); return ret; case IIO_CHAN_INFO_SAMP_FREQ: - return inv_icm42600_gyro_write_odr(st, val, val2); + return inv_icm42600_gyro_write_odr(indio_dev, val, val2); case IIO_CHAN_INFO_CALIBBIAS: ret = iio_device_claim_direct_mode(indio_dev); if (ret) @@ -705,6 +718,7 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st) { struct device *dev = regmap_get_device(st->map); const char *name; + struct inv_icm42600_timestamp *ts; struct iio_dev *indio_dev; struct iio_buffer *buffer; int ret; @@ -713,7 +727,7 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st) if (!name) return ERR_PTR(-ENOMEM); - indio_dev = devm_iio_device_alloc(dev, 0); + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts)); if (!indio_dev) return ERR_PTR(-ENOMEM); @@ -721,6 +735,9 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st) if (!buffer) return ERR_PTR(-ENOMEM); + ts = iio_priv(indio_dev); + inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr)); + iio_device_set_drvdata(indio_dev, st); indio_dev->name = name; indio_dev->info = &inv_icm42600_gyro_info; @@ -742,14 +759,17 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st) 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 = iio_priv(indio_dev); ssize_t i, size; + unsigned int no; const void *accel, *gyro, *timestamp; const int8_t *temp; unsigned int odr; + int64_t ts_val; struct inv_icm42600_gyro_buffer buffer; /* 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); /* quit if error or FIFO is empty */ @@ -760,12 +780,18 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev) if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) continue; + /* update odr */ + if (odr & INV_ICM42600_SENSOR_GYRO) + inv_icm42600_timestamp_apply_odr(ts, st->fifo.period, + st->fifo.nb.total, no); + /* buffer is copied to userspace, zeroing it to avoid any data leak */ memset(&buffer, 0, sizeof(buffer)); memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro)); /* convert 8 bits FIFO temperature in high resolution format */ buffer.temp = temp ? (*temp * 64) : 0; - iio_push_to_buffers(indio_dev, &buffer); + ts_val = inv_icm42600_timestamp_pop(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 0000000000000..7f2dc41f807bb --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c @@ -0,0 +1,195 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#include +#include +#include + +#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) + +/* 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_setup(struct inv_icm42600_state *st) +{ + unsigned int val; + + /* 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); +} + +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_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); + + 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; + if (it->lo != 0) { + /* compute period: delta time divided by number of samples */ + period = div_s64(delta, fifo_nb); + valid = inv_compute_chip_period(ts, fifo_mult, period); + /* update sensor period if chip internal period is updated */ + if (valid) + ts->period = ts->mult * ts->chip_period.val; + } + + /* 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) { + /* compute measured fifo_period */ + fifo_period = fifo_mult * ts->chip_period.val; + /* delta time between last sample and last interrupt */ + delta = it->lo - ts->timestamp; + /* if there are multiple samples, go back to first one */ + 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; + 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; + + 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 new file mode 100644 index 0000000000000..4e4f331d4fe4b --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h @@ -0,0 +1,85 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#ifndef INV_ICM42600_TIMESTAMP_H_ +#define INV_ICM42600_TIMESTAMP_H_ + +#include + +struct inv_icm42600_state; + +/** + * 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_setup(struct inv_icm42600_state *st); + +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