[PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices

[Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>]

Use triggered buffer by parsing FIFO data read in device trigger.
Support hwfifo watermark by multiplexing gyro and accel settings.
Support hwfifo flush.

Simply use interrupt timestamp first.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_icm42600/Kconfig          |   3 +-
 drivers/iio/imu/inv_icm42600/Makefile         |   1 +
 drivers/iio/imu/inv_icm42600/inv_icm42600.h   |   8 +
 .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 183 +++++++++
 .../imu/inv_icm42600/inv_icm42600_buffer.c    | 353 ++++++++++++++++++
 .../imu/inv_icm42600/inv_icm42600_buffer.h    | 162 ++++++++
 .../iio/imu/inv_icm42600/inv_icm42600_core.c  |  23 ++
 .../iio/imu/inv_icm42600/inv_icm42600_gyro.c  | 183 +++++++++
 .../imu/inv_icm42600/inv_icm42600_trigger.c   |  15 +-
 9 files changed, 928 insertions(+), 3 deletions(-)
 create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
 create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h

diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
index 7b3eaeb2aa4a..8c0969319c49 100644
--- a/drivers/iio/imu/inv_icm42600/Kconfig
+++ b/drivers/iio/imu/inv_icm42600/Kconfig
@@ -2,7 +2,8 @@
 
 config INV_ICM42600
 	tristate
-	select IIO_TRIGGER
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
 
 config INV_ICM42600_I2C
 	tristate "InvenSense ICM-426xx I2C driver"
diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
index e1f2aacbe888..d6732118010c 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_trigger.o
+inv-icm42600-y += inv_icm42600_buffer.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 175c1f67faee..947ca4dd245b 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -15,6 +15,8 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/trigger.h>
 
+#include "inv_icm42600_buffer.h"
+
 enum inv_icm42600_chip {
 	INV_CHIP_ICM42600,
 	INV_CHIP_ICM42602,
@@ -124,6 +126,7 @@ struct inv_icm42600_suspended {
  *  @indio_gyro:	gyroscope IIO device.
  *  @indio_accel:	accelerometer IIO device.
  *  @trigger:		device internal interrupt trigger
+ *  @fifo:		FIFO management structure.
  */
 struct inv_icm42600_state {
 	struct mutex lock;
@@ -138,6 +141,7 @@ struct inv_icm42600_state {
 	struct iio_dev *indio_gyro;
 	struct iio_dev *indio_accel;
 	struct iio_trigger *trigger;
+	struct inv_icm42600_fifo fifo;
 };
 
 /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -378,8 +382,12 @@ 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_accel_init(struct inv_icm42600_state *st);
 
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
+
 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 74dac5f283d4..4206be54d057 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -10,9 +10,13 @@
 #include <linux/regmap.h>
 #include <linux/delay.h>
 #include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "inv_icm42600.h"
 #include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
 
 #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info)		\
 	{								\
@@ -46,6 +50,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[] = {
@@ -61,8 +66,100 @@ 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 */
+struct inv_icm42600_accel_buffer {
+	struct inv_icm42600_fifo_sensor_data accel;
+	int8_t temp;
+	int64_t timestamp;
+};
+
+#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS				\
+	(BIT(INV_ICM42600_ACCEL_SCAN_X) |				\
+	BIT(INV_ICM42600_ACCEL_SCAN_Y) |				\
+	BIT(INV_ICM42600_ACCEL_SCAN_Z))
+
+#define INV_ICM42600_SCAN_MASK_TEMP	BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
+
+static const unsigned long inv_icm42600_accel_scan_masks[] = {
+	/* 3-axis accel + temperature */
+	INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
+	0,
+};
+
+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);
+	const size_t fifo_nb = st->fifo.nb.total;
+	int ret;
+
+	/* exit if no sample */
+	if (fifo_nb == 0)
+		goto out;
+
+	ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
+	if (ret)
+		dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
+			ret);
+
+out:
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+/* enable accelerometer sensor and FIFO write */
+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_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+	unsigned int fifo_en = 0;
+	unsigned int sleep_temp = 0;
+	unsigned int sleep_accel = 0;
+	unsigned int sleep;
+	int ret;
+
+	mutex_lock(&st->lock);
+
+	if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
+		/* enable temp sensor */
+		ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
+		if (ret)
+			goto out_unlock;
+		fifo_en |= INV_ICM42600_SENSOR_TEMP;
+	}
+
+	if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
+		/* enable accel sensor */
+		conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+		ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
+		if (ret)
+			goto out_unlock;
+		fifo_en |= INV_ICM42600_SENSOR_ACCEL;
+	}
+
+	/* update data FIFO write and FIFO watermark */
+	ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
+	if (ret)
+		goto out_unlock;
+	ret = inv_icm42600_buffer_update_watermark(st);
+
+out_unlock:
+	mutex_unlock(&st->lock);
+	/* sleep maximum required time */
+	if (sleep_accel > sleep_temp)
+		sleep = sleep_accel;
+	else
+		sleep = sleep_temp;
+	if (sleep)
+		msleep(sleep);
+	return ret;
+}
+
 static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
 					  struct iio_chan_spec const *chan,
 					  int16_t *val)
@@ -250,6 +347,8 @@ 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_buffer_update_fifo_period(st);
+	inv_icm42600_buffer_update_watermark(st);
 	mutex_unlock(&st->lock);
 
 	pm_runtime_mark_last_busy(dev);
@@ -512,12 +611,51 @@ static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
 	}
 }
 
+static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
+						   unsigned int val)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	mutex_lock(&st->lock);
+
+	st->fifo.watermark.accel = val;
+	ret = inv_icm42600_buffer_update_watermark(st);
+
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
+static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
+					   unsigned int count)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	if (count == 0)
+		return 0;
+
+	mutex_lock(&st->lock);
+
+	ret = inv_icm42600_buffer_hwfifo_flush(st, count);
+	if (!ret)
+		ret = st->fifo.nb.accel;
+
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
 static const struct iio_info inv_icm42600_accel_info = {
 	.read_raw = inv_icm42600_accel_read_raw,
 	.read_avail = inv_icm42600_accel_read_avail,
 	.write_raw = inv_icm42600_accel_write_raw,
 	.write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
 	.debugfs_reg_access = inv_icm42600_debugfs_reg,
+	.update_scan_mode = inv_icm42600_accel_update_scan_mode,
+	.hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
+	.hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
 };
 
 int inv_icm42600_accel_init(struct inv_icm42600_state *st)
@@ -525,6 +663,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
 	struct device *dev = regmap_get_device(st->map);
 	const char *name;
 	struct iio_dev *indio_dev;
+	int ret;
 
 	name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
 	if (!name)
@@ -541,7 +680,51 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = inv_icm42600_accel_channels;
 	indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
+	indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
+
+	ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+					      inv_icm42600_accel_handler,
+					      &inv_icm42600_buffer_ops);
+	if (ret)
+		return ret;
+
+	indio_dev->trig = iio_trigger_get(st->trigger);
 
 	st->indio_accel = indio_dev;
 	return devm_iio_device_register(dev, st->indio_accel);
 }
+
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	const size_t accel_nb = st->fifo.nb.accel;
+	ssize_t i, size;
+	const void *accel, *gyro, *temp, *timestamp;
+	unsigned int odr;
+	struct inv_icm42600_accel_buffer buffer;
+
+	/* exit if no accel sample */
+	if (accel_nb == 0)
+		return 0;
+
+	/* parse all fifo packets */
+	for (i = 0; i < st->fifo.count; i += size) {
+		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",
+			size);
+		/* quit if error or FIFO is empty */
+		if (size <= 0)
+			return size;
+		/* skip packet if no accel data or data is invalid */
+		if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) {
+			dev_dbg(regmap_get_device(st->map), "skip accel data\n");
+			continue;
+		}
+		memcpy(&buffer.accel, accel, sizeof(buffer.accel));
+		memcpy(&buffer.temp, temp, sizeof(buffer.temp));
+		iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+	}
+
+	return 0;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
new file mode 100644
index 000000000000..b428abdc92ee
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -0,0 +1,353 @@
+// 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/delay.h>
+#include <linux/math64.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+
+#include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
+
+void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
+{
+	uint32_t period_gyro, period_accel, period;
+
+	if (st->fifo.en & INV_ICM42600_SENSOR_GYRO)
+		period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
+	else
+		period_gyro = U32_MAX;
+
+	if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL)
+		period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
+	else
+		period_accel = U32_MAX;
+
+	if (period_gyro <= period_accel)
+		period = period_gyro;
+	else
+		period = period_accel;
+
+	st->fifo.period = period;
+}
+
+int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
+				    unsigned int fifo_en)
+{
+	unsigned int mask, val;
+	int ret;
+
+	/* update only FIFO EN bits */
+	mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN |
+		INV_ICM42600_FIFO_CONFIG1_TEMP_EN |
+		INV_ICM42600_FIFO_CONFIG1_GYRO_EN |
+		INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
+
+	val = 0;
+	if (fifo_en & INV_ICM42600_SENSOR_GYRO)
+		val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN;
+	if (fifo_en & INV_ICM42600_SENSOR_ACCEL)
+		val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
+	if (fifo_en & INV_ICM42600_SENSOR_TEMP)
+		val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN;
+
+	ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
+				 mask, val);
+	if (ret)
+		return ret;
+
+	st->fifo.en = fifo_en;
+	inv_icm42600_buffer_update_fifo_period(st);
+
+	return 0;
+}
+
+static size_t inv_icm42600_get_packet_size(unsigned int fifo_en)
+{
+	size_t packet_size;
+
+	if ((fifo_en & INV_ICM42600_SENSOR_GYRO) &&
+			(fifo_en & INV_ICM42600_SENSOR_ACCEL))
+		packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
+	else
+		packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+
+	return packet_size;
+}
+
+static unsigned int inv_icm42600_wm_truncate(unsigned int watermark,
+					     size_t packet_size)
+{
+	size_t wm_size;
+	unsigned int wm;
+
+	wm_size = watermark * packet_size;
+	if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX)
+		wm_size = INV_ICM42600_FIFO_WATERMARK_MAX;
+
+	wm = wm_size / packet_size;
+
+	return wm;
+}
+
+int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
+{
+	size_t packet_size, wm_size;
+	unsigned int wm_gyro, wm_accel, watermark;
+	uint32_t period_gyro, period_accel, period;
+	int64_t latency_gyro, latency_accel, latency;
+	unsigned int mask, val;
+	bool restore;
+	__le16 raw_wm;
+	int ret;
+
+	packet_size = inv_icm42600_get_packet_size(st->fifo.en);
+
+	/* get minimal latency, depending on sensor watermark and odr */
+	wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro,
+					   packet_size);
+	wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel,
+					    packet_size);
+	period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
+	period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
+	latency_gyro = (int64_t)period_gyro * (int64_t)wm_gyro;
+	latency_accel = (int64_t)period_accel * (int64_t)wm_accel;
+	if (latency_gyro == 0) {
+		latency = latency_accel;
+		watermark = wm_accel;
+	} else if (latency_accel == 0) {
+		latency = latency_gyro;
+		watermark = wm_gyro;
+	} else {
+		/* compute the smallest latency that is a multiple of both */
+		if (latency_gyro <= latency_accel) {
+			latency = latency_gyro;
+			latency -= latency_accel % latency_gyro;
+		} else {
+			latency = latency_accel;
+			latency -= latency_gyro % latency_accel;
+		}
+		/* use the shortest period */
+		if (period_gyro <= period_accel)
+			period = period_gyro;
+		else
+			period = period_accel;
+		/* all this works because periods are multiple of each others */
+		watermark = div_s64(latency, period);
+		if (watermark < 1)
+			watermark = 1;
+	}
+	wm_size = watermark * packet_size;
+	dev_dbg(regmap_get_device(st->map), "watermark: %u (%zu)\n",
+		watermark, wm_size);
+
+	/* changing FIFO watermark requires to turn off watermark interrupt */
+	mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+	val = 0;
+	ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
+				       mask, val, &restore);
+	if (ret)
+		return ret;
+
+	raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
+	ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
+				&raw_wm, sizeof(raw_wm));
+	if (ret)
+		return ret;
+
+	/* restore watermark interrupt */
+	if (restore) {
+		mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+		val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+		ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+					 mask, val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+
+	pm_runtime_get_sync(dev);
+
+	return 0;
+}
+
+static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+	unsigned int sensor;
+	unsigned int *watermark;
+	struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+	unsigned int sleep = 0;
+	int ret;
+
+	if (indio_dev == st->indio_gyro) {
+		sensor = INV_ICM42600_SENSOR_GYRO;
+		watermark = &st->fifo.watermark.gyro;
+	} else if (indio_dev == st->indio_accel) {
+		sensor = INV_ICM42600_SENSOR_ACCEL;
+		watermark = &st->fifo.watermark.accel;
+	} else {
+		return -EINVAL;
+	}
+
+	mutex_lock(&st->lock);
+
+	ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
+	if (ret)
+		goto out_unlock;
+
+	*watermark = 0;
+	ret = inv_icm42600_buffer_update_watermark(st);
+	if (ret)
+		goto out_unlock;
+
+	conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
+	if (sensor == INV_ICM42600_SENSOR_GYRO)
+		ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep);
+	else
+		ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
+
+out_unlock:
+	mutex_unlock(&st->lock);
+	if (sleep)
+		msleep(sleep);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	return ret;
+}
+
+const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = {
+	.preenable = inv_icm42600_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = iio_triggered_buffer_predisable,
+	.postdisable = inv_icm42600_buffer_postdisable,
+};
+
+int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
+				  unsigned int max)
+{
+	struct device *dev = regmap_get_device(st->map);
+	__be16 raw_fifo_count;
+	size_t max_count;
+	ssize_t i, size;
+	const void *accel, *gyro, *temp, *timestamp;
+	unsigned int odr;
+	int ret;
+
+	/* reset all samples counters */
+	st->fifo.count = 0;
+	st->fifo.nb.gyro = 0;
+	st->fifo.nb.accel = 0;
+	st->fifo.nb.total = 0;
+
+	/* compute maximum FIFO read size */
+	if (max == 0)
+		max_count = sizeof(st->fifo.data);
+	else
+		max_count = max * inv_icm42600_get_packet_size(st->fifo.en);
+
+	/* read FIFO count value */
+	ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
+			       &raw_fifo_count, sizeof(raw_fifo_count));
+	if (ret)
+		return ret;
+	st->fifo.count = be16_to_cpu(raw_fifo_count);
+	dev_dbg(dev, "FIFO count = %zu\n", st->fifo.count);
+
+	/* check and sanitize FIFO count value */
+	if (st->fifo.count == 0)
+		return 0;
+	if (st->fifo.count > max_count)
+		st->fifo.count = max_count;
+
+	/* read all FIFO data in internal buffer */
+	ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA,
+				st->fifo.data, st->fifo.count);
+	if (ret)
+		return ret;
+
+	/* compute number of samples for each sensor */
+	for (i = 0; i < st->fifo.count; i += size) {
+		size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
+				&accel, &gyro, &temp, &timestamp, &odr);
+		if (size <= 0)
+			break;
+		if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro))
+			st->fifo.nb.gyro++;
+		if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel))
+			st->fifo.nb.accel++;
+		st->fifo.nb.total++;
+	}
+
+	return 0;
+}
+
+int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
+				     unsigned int count)
+{
+	int64_t ts_gyro, ts_accel;
+	int ret;
+
+	dev_dbg(regmap_get_device(st->map), "FIFO flush %u\n", count);
+
+	ts_gyro = iio_get_time_ns(st->indio_gyro);
+	ts_accel = iio_get_time_ns(st->indio_accel);
+	ret = inv_icm42600_buffer_fifo_read(st, count);
+	if (ret)
+		return ret;
+
+	if (st->fifo.nb.total == 0)
+		return 0;
+
+	ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
+	if (ret)
+		return ret;
+
+	return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
+}
+
+int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
+{
+	unsigned int mask, val;
+	int ret;
+
+	/*
+	 * Default FIFO configuration (bits 7 to 5)
+	 * - use invalid value
+	 * - FIFO count in bytes
+	 * - FIFO count in big endian
+	 */
+	mask = GENMASK(7, 5);
+	val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
+	ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+				 mask, val);
+	if (ret)
+		return ret;
+
+	/*
+	 * Enable FIFO partial read and continuous watermark interrupt.
+	 * Disable all FIFO EN bits.
+	 */
+	mask = GENMASK(6, 5) | GENMASK(3, 0);
+	val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD |
+	      INV_ICM42600_FIFO_CONFIG1_WM_GT_TH;
+	return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
+				  mask, val);
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
new file mode 100644
index 000000000000..74b91c0e664b
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
@@ -0,0 +1,162 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_BUFFER_H_
+#define INV_ICM42600_BUFFER_H_
+
+#include <linux/kernel.h>
+#include <linux/bits.h>
+
+struct inv_icm42600_state;
+
+#define INV_ICM42600_SENSOR_GYRO	BIT(0)
+#define INV_ICM42600_SENSOR_ACCEL	BIT(1)
+#define INV_ICM42600_SENSOR_TEMP	BIT(2)
+
+struct inv_icm42600_fifo {
+	unsigned int en;
+	uint32_t period;
+	struct {
+		unsigned int gyro;
+		unsigned int accel;
+	} watermark;
+	size_t count;
+	struct {
+		size_t gyro;
+		size_t accel;
+		size_t total;
+	} nb;
+	uint8_t data[2080];
+};
+
+/* FIFO header: 1 byte */
+#define INV_ICM42600_FIFO_HEADER_MSG		BIT(7)
+#define INV_ICM42600_FIFO_HEADER_ACCEL		BIT(6)
+#define INV_ICM42600_FIFO_HEADER_GYRO		BIT(5)
+#define INV_ICM42600_FIFO_HEADER_TMST_FSYNC	GENMASK(3, 2)
+#define INV_ICM42600_FIFO_HEADER_ODR_ACCEL	BIT(1)
+#define INV_ICM42600_FIFO_HEADER_ODR_GYRO	BIT(0)
+
+/* FIFO data packet */
+struct inv_icm42600_fifo_sensor_data {
+	__be16 x;
+	__be16 y;
+	__be16 z;
+} __packed;
+#define INV_ICM42600_FIFO_DATA_INVALID		-32768
+
+struct inv_icm42600_fifo_1sensor_packet {
+	uint8_t header;
+	struct inv_icm42600_fifo_sensor_data data;
+	int8_t temp;
+} __packed;
+#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE		8
+
+struct inv_icm42600_fifo_2sensors_packet {
+	uint8_t header;
+	struct inv_icm42600_fifo_sensor_data accel;
+	struct inv_icm42600_fifo_sensor_data gyro;
+	int8_t temp;
+	__be16 timestamp;
+} __packed;
+#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE		16
+
+static inline int16_t inv_icm42600_fifo_get_sensor_data(__be16 d)
+{
+	return be16_to_cpu(d);
+}
+
+static inline bool
+inv_icm42600_fifo_is_data_valid(const struct inv_icm42600_fifo_sensor_data *s)
+{
+	int16_t x, y, z;
+
+	x = inv_icm42600_fifo_get_sensor_data(s->x);
+	y = inv_icm42600_fifo_get_sensor_data(s->y);
+	z = inv_icm42600_fifo_get_sensor_data(s->z);
+
+	if (x == INV_ICM42600_FIFO_DATA_INVALID &&
+			y == INV_ICM42600_FIFO_DATA_INVALID &&
+			z == INV_ICM42600_FIFO_DATA_INVALID)
+		return false;
+
+	return true;
+}
+
+static inline ssize_t inv_icm42600_fifo_decode_packet(const void *packet,
+		const void **accel, const void **gyro, const void **temp,
+		const void **timestamp, unsigned int *odr)
+{
+	const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet;
+	const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet;
+	uint8_t header = *((const uint8_t *)packet);
+
+	/* FIFO empty */
+	if (header & INV_ICM42600_FIFO_HEADER_MSG) {
+		*accel = NULL;
+		*gyro = NULL;
+		*temp = NULL;
+		*timestamp = NULL;
+		*odr = 0;
+		return 0;
+	}
+
+	/* handle odr flags */
+	*odr = 0;
+	if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO)
+		*odr |= INV_ICM42600_SENSOR_GYRO;
+	if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL)
+		*odr |= INV_ICM42600_SENSOR_ACCEL;
+
+	/* accel + gyro */
+	if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) &&
+			(header & INV_ICM42600_FIFO_HEADER_GYRO)) {
+		*accel = &pack2->accel;
+		*gyro = &pack2->gyro;
+		*temp = &pack2->temp;
+		*timestamp = &pack2->timestamp;
+		return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
+	}
+
+	/* accel only */
+	if (header & INV_ICM42600_FIFO_HEADER_ACCEL) {
+		*accel = &pack1->data;
+		*gyro = NULL;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* gyro only */
+	if (header & INV_ICM42600_FIFO_HEADER_GYRO) {
+		*accel = NULL;
+		*gyro = &pack1->data;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* invalid packet if here */
+	return -EINVAL;
+}
+
+extern const struct iio_buffer_setup_ops inv_icm42600_buffer_ops;
+
+int inv_icm42600_buffer_init(struct inv_icm42600_state *st);
+
+void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st);
+
+int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
+				    unsigned int fifo_en);
+
+int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st);
+
+int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
+				  unsigned int max);
+
+int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
+				     unsigned int count);
+
+#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 1102c54396e3..689089065ff9 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -14,6 +14,7 @@
 #include <linux/iio/iio.h>
 
 #include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
 
 static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
 	{
@@ -515,6 +516,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
 	if (ret)
 		return ret;
 
+	/* setup FIFO buffer */
+	ret = inv_icm42600_buffer_init(st);
+	if (ret)
+		return ret;
+
 	/* setup interrupt trigger */
 	ret = inv_icm42600_trigger_init(st, irq, irq_type);
 	if (ret)
@@ -559,6 +565,16 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
 		goto out_unlock;
 	}
 
+	/* disable FIFO data streaming */
+	if (iio_buffer_enabled(st->indio_gyro) ||
+			iio_buffer_enabled(st->indio_accel)) {
+		/* set FIFO in bypass mode */
+		ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
+				   INV_ICM42600_FIFO_CONFIG_BYPASS);
+		if (ret)
+			goto out_unlock;
+	}
+
 	ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
 					 INV_ICM42600_SENSOR_MODE_OFF, false,
 					 NULL);
@@ -594,6 +610,13 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
 	if (ret)
 		goto out_unlock;
 
+	/* restore FIFO data streaming */
+	if (iio_buffer_enabled(st->indio_gyro) ||
+			iio_buffer_enabled(st->indio_accel)) {
+		ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
+				   INV_ICM42600_FIFO_CONFIG_STREAM);
+	}
+
 out_unlock:
 	mutex_unlock(&st->lock);
 	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 c0164ab2830e..dafb104abc77 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -10,9 +10,13 @@
 #include <linux/regmap.h>
 #include <linux/delay.h>
 #include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "inv_icm42600.h"
 #include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
 
 #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info)		\
 	{								\
@@ -46,6 +50,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[] = {
@@ -61,8 +66,100 @@ 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 */
+struct inv_icm42600_gyro_buffer {
+	struct inv_icm42600_fifo_sensor_data gyro;
+	int8_t temp;
+	int64_t timestamp;
+};
+
+#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS				\
+	(BIT(INV_ICM42600_GYRO_SCAN_X) |				\
+	BIT(INV_ICM42600_GYRO_SCAN_Y) |					\
+	BIT(INV_ICM42600_GYRO_SCAN_Z))
+
+#define INV_ICM42600_SCAN_MASK_TEMP	BIT(INV_ICM42600_GYRO_SCAN_TEMP)
+
+static const unsigned long inv_icm42600_gyro_scan_masks[] = {
+	/* 3-axis gyro + temperature */
+	INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
+	0,
+};
+
+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);
+	const size_t fifo_nb = st->fifo.nb.total;
+	int ret;
+
+	/* exit if no sample */
+	if (fifo_nb == 0)
+		goto out;
+
+	ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
+	if (ret)
+		dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
+			ret);
+
+out:
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+/* enable gyroscope sensor and FIFO write */
+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_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+	unsigned int fifo_en = 0;
+	unsigned int sleep_gyro = 0;
+	unsigned int sleep_temp = 0;
+	unsigned int sleep;
+	int ret;
+
+	mutex_lock(&st->lock);
+
+	if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
+		/* enable temp sensor */
+		ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
+		if (ret)
+			goto out_unlock;
+		fifo_en |= INV_ICM42600_SENSOR_TEMP;
+	}
+
+	if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
+		/* enable gyro sensor */
+		conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+		ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
+		if (ret)
+			goto out_unlock;
+		fifo_en |= INV_ICM42600_SENSOR_GYRO;
+	}
+
+	/* update data FIFO write and FIFO watermark */
+	ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
+	if (ret)
+		goto out_unlock;
+	ret = inv_icm42600_buffer_update_watermark(st);
+
+out_unlock:
+	mutex_unlock(&st->lock);
+	/* sleep maximum required time */
+	if (sleep_gyro > sleep_temp)
+		sleep = sleep_gyro;
+	else
+		sleep = sleep_temp;
+	if (sleep)
+		msleep(sleep);
+	return ret;
+}
+
 static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
 					 struct iio_chan_spec const *chan,
 					 int16_t *val)
@@ -262,6 +359,8 @@ 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_buffer_update_fifo_period(st);
+	inv_icm42600_buffer_update_watermark(st);
 	mutex_unlock(&st->lock);
 
 	pm_runtime_mark_last_busy(dev);
@@ -524,12 +623,51 @@ static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
 	}
 }
 
+static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
+						  unsigned int val)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	mutex_lock(&st->lock);
+
+	st->fifo.watermark.gyro = val;
+	ret = inv_icm42600_buffer_update_watermark(st);
+
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
+static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
+					  unsigned int count)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	if (count == 0)
+		return 0;
+
+	mutex_lock(&st->lock);
+
+	ret = inv_icm42600_buffer_hwfifo_flush(st, count);
+	if (!ret)
+		ret = st->fifo.nb.gyro;
+
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
 static const struct iio_info inv_icm42600_gyro_info = {
 	.read_raw = inv_icm42600_gyro_read_raw,
 	.read_avail = inv_icm42600_gyro_read_avail,
 	.write_raw = inv_icm42600_gyro_write_raw,
 	.write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
 	.debugfs_reg_access = inv_icm42600_debugfs_reg,
+	.update_scan_mode = inv_icm42600_gyro_update_scan_mode,
+	.hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
+	.hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
 };
 
 int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
@@ -537,6 +675,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
 	struct device *dev = regmap_get_device(st->map);
 	const char *name;
 	struct iio_dev *indio_dev;
+	int ret;
 
 	name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
 	if (!name)
@@ -553,7 +692,51 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = inv_icm42600_gyro_channels;
 	indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
+	indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
+
+	ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+					      inv_icm42600_gyro_handler,
+					      &inv_icm42600_buffer_ops);
+	if (ret)
+		return ret;
+
+	indio_dev->trig = iio_trigger_get(st->trigger);
 
 	st->indio_gyro = indio_dev;
 	return devm_iio_device_register(dev, st->indio_gyro);
 }
+
+int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	const size_t gyro_nb = st->fifo.nb.gyro;
+	ssize_t i, size;
+	const void *accel, *gyro, *temp, *timestamp;
+	unsigned int odr;
+	struct inv_icm42600_gyro_buffer buffer;
+
+	/* exit if no gyro sample */
+	if (gyro_nb == 0)
+		return 0;
+
+	/* parse all fifo packets */
+	for (i = 0; i < st->fifo.count; i += size) {
+		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",
+			size);
+		/* quit if error or FIFO is empty */
+		if (size <= 0)
+			return size;
+		/* skip packet if no gyro data or data is invalid */
+		if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) {
+			dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
+			continue;
+		}
+		memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
+		memcpy(&buffer.temp, temp, sizeof(buffer.temp));
+		iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+	}
+
+	return 0;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
index 7a5e76305f0b..5667e0204722 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
@@ -13,6 +13,7 @@
 #include <linux/iio/trigger_consumer.h>
 
 #include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
 
 static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
 {
@@ -45,8 +46,18 @@ static irqreturn_t inv_icm42600_trigger_int_handler(int irq, void *_data)
 		dev_warn(dev, "FIFO full data lost!\n");
 
 	/* FIFO threshold reached */
-	if (status & INV_ICM42600_INT_STATUS_FIFO_THS)
-		iio_trigger_poll_chained(st->trigger);
+	if (status & INV_ICM42600_INT_STATUS_FIFO_THS) {
+		ret = inv_icm42600_buffer_fifo_read(st, 0);
+		if (ret)
+			dev_err(dev, "FIFO read error %d\n", ret);
+	} else {
+		st->fifo.count = 0;
+		st->fifo.nb.gyro = 0;
+		st->fifo.nb.accel = 0;
+		st->fifo.nb.total = 0;
+	}
+
+	iio_trigger_poll_chained(st->trigger);
 
 out_unlock:
 	mutex_unlock(&st->lock);
-- 
2.17.1