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, &timestamp, &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),
+	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, &timestamp, &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);
+		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)
+{
+	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
