[PATCH v2 3/3] iio: accel: icm42370: Add FIFO buffer functionality

Add support for the hardware FIFO buffer to support high data rate
capture. This includes necessary IIO buffer setup ops, watermark
management and FIFO packet decoding. The accelerometer has 2.25kB FIFO
size. The buffer can be handled via sysfs.

Signed-off-by: Kanak Shilledar <[email protected]>
---
 drivers/iio/accel/Kconfig               |   1 +
 drivers/iio/accel/Makefile              |   1 +
 drivers/iio/accel/inv_icm42370.h        |   6 +
 drivers/iio/accel/inv_icm42370_buffer.c | 496 ++++++++++++++++++++++++++++++++
 drivers/iio/accel/inv_icm42370_buffer.h | 100 +++++++
 drivers/iio/accel/inv_icm42370_core.c   | 206 ++++++++++++-
 6 files changed, 809 insertions(+), 1 deletion(-)

diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index 3ac2534c1c76d..a8dc5692db9d9 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -491,6 +491,7 @@ config INV_ICM42370
 	tristate
 	select IIO_BUFFER
 	select IIO_INV_SENSORS_TIMESTAMP
+	select IIO_KFIFO_BUF
 
 config INV_ICM42370_I2C
 	tristate "InvenSense ICM-42370-P I2C driver"
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index 6750b03edf518..939a23793735e 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -52,6 +52,7 @@ obj-$(CONFIG_IIO_KX022A_SPI)	+= kionix-kx022a-spi.o
 
 obj-$(CONFIG_INV_ICM42370) += inv-icm42370.o
 inv-icm42370-y += inv_icm42370_core.o
+inv-icm42370-y += inv_icm42370_buffer.o
 obj-$(CONFIG_INV_ICM42370_I2C) += inv_icm42370_i2c.o
 
 obj-$(CONFIG_KXCJK1013) += kxcjk-1013.o
diff --git a/drivers/iio/accel/inv_icm42370.h b/drivers/iio/accel/inv_icm42370.h
index a4a822f04355f..5df5876169cc9 100644
--- a/drivers/iio/accel/inv_icm42370.h
+++ b/drivers/iio/accel/inv_icm42370.h
@@ -17,6 +17,7 @@
 #include <linux/iio/common/inv_sensors_timestamp.h>
 #include <linux/iio/iio.h>
 
+#include "inv_icm42370_buffer.h"
 
 #define INV_ICM42370_SENSOR_CONF_INIT { -1, -1, -1, -1 }
 
@@ -45,6 +46,8 @@
 #define INV_ICM42370_REG_INT_STATUS 0x3A
 #define INV_ICM42370_REG_TEMP_CONFIG0 0x34
 #define INV_ICM42370_REG_INTF_CONFIG0 0x35
+#define INV_ICM42370_REG_FIFO_COUNT 0x3D
+#define INV_ICM42370_REG_FIFO_DATA 0x3F
 #define INV_ICM42370_REG_WHO_AM_I 0x75
 #define INV_ICM42370_REG_BLK_SEL_W 0x79
 #define INV_ICM42370_REG_MADDR_W 0x7A
@@ -199,6 +202,7 @@ enum inv_icm42370_accel_scan {
 	INV_ICM42370_ACCEL_SCAN_Y,
 	INV_ICM42370_ACCEL_SCAN_Z,
 	INV_ICM42370_ACCEL_SCAN_TEMP,
+	INV_ICM42370_ACCEL_SCAN_TIMESTAMP,
 };
 
 enum inv_icm42370_sensor_mode {
@@ -286,6 +290,7 @@ struct inv_icm42370_conf {
  * @indio_accel:	accelerometer IIO device.
  * @timestamp:		interrupt timestamp.
  * @orientation:	sensor chip orientation relative to main hardware.
+ * @fifo:		FIFO state and configuration.
  * @chip:		chip identifier.
  * @conf:		chip sensors configurations.
  * @filter:		sensor filter.
@@ -303,6 +308,7 @@ struct inv_icm42370_data {
 	struct iio_dev *indio_accel;
 	s64 timestamp;
 	struct iio_mount_matrix orientation;
+	struct inv_icm42370_fifo fifo;
 	enum inv_icm42370_chip chip;
 	struct inv_icm42370_conf conf;
 	enum inv_icm42370_filter filter;
diff --git a/drivers/iio/accel/inv_icm42370_buffer.c b/drivers/iio/accel/inv_icm42370_buffer.c
new file mode 100644
index 0000000000000..c6a4d313c92c5
--- /dev/null
+++ b/drivers/iio/accel/inv_icm42370_buffer.c
@@ -0,0 +1,496 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ * Copyright (C) 2026 Axis Communications AB
+ */
+
+#include <linux/minmax.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+
+#include <linux/iio/common/inv_sensors_timestamp.h>
+#include <linux/iio/iio.h>
+
+#include "inv_icm42370.h"
+#include "inv_icm42370_buffer.h"
+
+/* FIFO header: 1 byte */
+#define INV_ICM42370_FIFO_HEADER_MSG BIT(7)
+#define INV_ICM42370_FIFO_HEADER_ACCEL BIT(6)
+#define INV_ICM42370_FIFO_HEADER_ODR_ACCEL BIT(1)
+
+struct inv_icm42370_fifo_packet_1 {
+	u8 header;
+	struct inv_icm42370_fifo_sensor_data data;
+	s8 temp;
+} __packed;
+
+#define INV_ICM42370_FIFO_PACKET_1_SIZE 8
+
+ssize_t inv_icm42370_fifo_decode_packet(const void *packet, const void **accel,
+					const s8 **temp, const void **timestamp,
+					unsigned int *odr)
+{
+	const struct inv_icm42370_fifo_packet_1 *pack1 = packet;
+	u8 header = *((const u8 *)packet);
+
+	/* FIFO empty */
+	if (header & INV_ICM42370_FIFO_HEADER_MSG) {
+		*accel = NULL;
+		*temp = NULL;
+		*timestamp = NULL;
+		*odr = 0;
+		return 0;
+	}
+
+	/* ODR change flag */
+	*odr = 0;
+	if (header & INV_ICM42370_FIFO_HEADER_ODR_ACCEL)
+		*odr |= INV_ICM42370_SENSOR_ACCEL;
+
+	/* With TMST_EN disabled, all packets are Packet 1 (8 bytes) */
+	if (header & INV_ICM42370_FIFO_HEADER_ACCEL) {
+		*accel = &pack1->data;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42370_FIFO_PACKET_1_SIZE;
+	}
+
+	/* invalid or unsupported packet format */
+	return -EINVAL;
+}
+
+void inv_icm42370_buffer_update_fifo_period(struct inv_icm42370_data *data)
+{
+	u32 period_accel;
+
+	if (data->fifo.en & INV_ICM42370_SENSOR_ACCEL)
+		period_accel = inv_icm42370_odr_to_period(data->conf.odr);
+	else
+		period_accel = U32_MAX;
+
+	data->fifo.period = period_accel;
+}
+
+int inv_icm42370_buffer_set_fifo_en(struct inv_icm42370_data *data,
+				    unsigned int fifo_en)
+{
+	u8 mask, val, regval;
+	int ret;
+
+	/* update only FIFO EN bits */
+	mask = INV_ICM42370_FIFO_CONFIG5_ACCEL_EN;
+
+	val = 0;
+	if (fifo_en & INV_ICM42370_SENSOR_ACCEL)
+		val |= INV_ICM42370_FIFO_CONFIG5_ACCEL_EN;
+
+	ret = inv_icm42370_mreg_read(data, INV_ICM42370_MREG1,
+				     INV_ICM42370_REG_FIFO_CONFIG5, &regval);
+	if (ret)
+		return ret;
+
+	/* clear the mask bits and set the new values */
+	regval &= ~mask;
+	regval |= val;
+
+	ret = inv_icm42370_mreg_write(data, INV_ICM42370_MREG1,
+				      INV_ICM42370_REG_FIFO_CONFIG5, regval);
+	if (ret)
+		return ret;
+
+	data->fifo.en = fifo_en;
+	inv_icm42370_buffer_update_fifo_period(data);
+
+	return 0;
+}
+
+static size_t inv_icm42370_get_packet_size(unsigned int fifo_en)
+{
+	/*
+	 * With TMST_EN disabled, the device always produces Packet 1
+	 * (8 bytes: 1 header + 6 accel + 1 temp).
+	 */
+	return INV_ICM42370_FIFO_PACKET_1_SIZE;
+}
+
+static unsigned int inv_icm42370_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_ICM42370_FIFO_WATERMARK_MAX)
+		wm_size = INV_ICM42370_FIFO_WATERMARK_MAX;
+
+	wm = wm_size / packet_size;
+
+	return wm;
+}
+
+/**
+ * inv_icm42370_buffer_update_watermark - update watermark FIFO threshold
+ * @data:	driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ *
+ * FIFO watermark threshold is computed based on the required
+ * watermark values set for accel sensor.
+ */
+int inv_icm42370_buffer_update_watermark(struct inv_icm42370_data *data)
+{
+	size_t packet_size, wm_size;
+	unsigned int wm, watermark;
+	bool restore;
+	__le16 raw_wm;
+	int ret;
+
+	packet_size = inv_icm42370_get_packet_size(data->fifo.en);
+
+	/* compute sensors latency, depending on sensor watermark and odr */
+	wm = inv_icm42370_wm_truncate(data->fifo.watermark.accel, packet_size);
+
+	/* 0 value for watermark means that the sensor is turned off */
+	if (wm == 0)
+		return 0;
+
+	watermark = wm;
+	data->fifo.watermark.eff_accel = wm;
+
+	/* compute watermark value in bytes */
+	wm_size = watermark * packet_size;
+
+	/* changing FIFO watermark requires to turn off watermark interrupt */
+	ret = regmap_update_bits_check(
+		data->map, INV_ICM42370_REG_INT_SOURCE0,
+		INV_ICM42370_INT_SOURCE0_FIFO_THS_INT1_EN, 0, &restore);
+	if (ret)
+		return ret;
+
+	raw_wm = INV_ICM42370_FIFO_WATERMARK_VAL(wm_size);
+	memcpy(data->buffer, &raw_wm, sizeof(raw_wm));
+	ret = regmap_bulk_write(data->map, INV_ICM42370_REG_FIFO_WATERMARK,
+				data->buffer, sizeof(raw_wm));
+	if (ret)
+		return ret;
+
+	/* restore watermark interrupt */
+	if (restore) {
+		ret = regmap_set_bits(
+			data->map, INV_ICM42370_REG_INT_SOURCE0,
+			INV_ICM42370_INT_SOURCE0_FIFO_THS_INT1_EN);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int inv_icm42370_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42370_data *data = iio_priv(indio_dev);
+	struct device *dev = regmap_get_device(data->map);
+	struct inv_sensors_timestamp *ts = &data->ts;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(dev);
+	if (ret)
+		return ret;
+
+	guard(mutex)(&data->lock);
+
+	inv_sensors_timestamp_reset(ts);
+
+	return 0;
+}
+
+static int inv_icm42370_buffer_postenable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42370_data *data = iio_priv(indio_dev);
+	int ret;
+
+	guard(mutex)(&data->lock);
+
+	/* Exit if FIFO is already on. */
+	if (data->fifo.on) {
+		data->fifo.on++;
+		return 0;
+	}
+
+	ret = regmap_write(data->map, INV_ICM42370_REG_SIGNAL_PATH_RESET,
+			   INV_ICM42370_SIGNAL_PATH_RESET_FIFO_FLUSH);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(data->map, INV_ICM42370_REG_FIFO_CONFIG1,
+			   INV_ICM42370_FIFO_CONFIG_STREAM);
+	if (ret)
+		return ret;
+
+	/* when FIFO_CONFIG_STREAM bit is set FIFO is enabled, so
+	 * increase the count
+	 */
+	data->fifo.on++;
+
+	ret = regmap_bulk_read(data->map, INV_ICM42370_REG_FIFO_COUNT, data->buffer,
+			       2);
+	if (ret)
+		return ret;
+
+	ret = regmap_set_bits(data->map, INV_ICM42370_REG_INT_SOURCE0,
+			      INV_ICM42370_INT_SOURCE0_FIFO_THS_INT1_EN);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int inv_icm42370_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42370_data *data = iio_priv(indio_dev);
+	int ret;
+
+	guard(mutex)(&data->lock);
+
+	/* Exit if there are several sensors using the FIFO. */
+	if (data->fifo.on > 1) {
+		data->fifo.on--;
+		return 0;
+	}
+
+	/* set FIFO in bypass mode */
+	ret = regmap_write(data->map, INV_ICM42370_REG_FIFO_CONFIG1,
+			   INV_ICM42370_FIFO_CONFIG_BYPASS);
+	if (ret)
+		return ret;
+
+	/* when FIFO is bypassed it gets disabled, so reduce the
+	 * count
+	 */
+	data->fifo.on--;
+
+	/* flush FIFO data */
+	ret = regmap_write(data->map, INV_ICM42370_REG_SIGNAL_PATH_RESET,
+			   INV_ICM42370_SIGNAL_PATH_RESET_FIFO_FLUSH);
+	if (ret)
+		return ret;
+
+	/* disable FIFO threshold interrupt */
+	ret = regmap_clear_bits(data->map, INV_ICM42370_REG_INT_SOURCE0,
+				INV_ICM42370_INT_SOURCE0_FIFO_THS_INT1_EN);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int inv_icm42370_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42370_data *data = iio_priv(indio_dev);
+	struct inv_sensors_timestamp *ts = &data->ts;
+	struct device *dev = regmap_get_device(data->map);
+	unsigned int sensor;
+	unsigned int *watermark;
+	struct inv_icm42370_conf conf = INV_ICM42370_SENSOR_CONF_INIT;
+	unsigned int sleep_temp = 0;
+	unsigned int sleep_sensor = 0;
+	unsigned int sleep;
+	int ret;
+
+	if (indio_dev == data->indio_accel) {
+		sensor = INV_ICM42370_SENSOR_ACCEL;
+		watermark = &data->fifo.watermark.accel;
+	} else {
+		return -EINVAL;
+	}
+
+	guard(mutex)(&data->lock);
+
+	inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
+
+	ret = inv_icm42370_buffer_set_fifo_en(data, data->fifo.en & ~sensor);
+	if (ret)
+		goto out_unlock;
+
+	*watermark = 0;
+	ret = inv_icm42370_buffer_update_watermark(data);
+	if (ret)
+		goto out_unlock;
+
+	conf.mode = INV_ICM42370_SENSOR_MODE_OFF;
+	ret = inv_icm42370_set_accel_conf(data, &conf, &sleep_sensor);
+	if (ret)
+		goto out_unlock;
+
+out_unlock:
+	/* sleep maximum required time */
+	sleep = max(sleep_sensor, sleep_temp);
+	if (sleep)
+		msleep(sleep);
+
+	pm_runtime_put_autosuspend(dev);
+
+	return ret;
+}
+
+const struct iio_buffer_setup_ops inv_icm42370_buffer_ops = {
+	.preenable = inv_icm42370_buffer_preenable,
+	.postenable = inv_icm42370_buffer_postenable,
+	.predisable = inv_icm42370_buffer_predisable,
+	.postdisable = inv_icm42370_buffer_postdisable,
+};
+
+int inv_icm42370_buffer_fifo_read(struct inv_icm42370_data *data,
+				  unsigned int max)
+{
+	const ssize_t packet_size = sizeof(struct inv_icm42370_fifo_packet_1);
+	__be16 *raw_fifo_count;
+	size_t fifo_nb, i;
+	ssize_t size;
+	const void *accel, *timestamp;
+	const s8 *temp;
+	unsigned int odr;
+	int ret;
+
+	/* reset all samples counters */
+	data->fifo.count = 0;
+	data->fifo.nb.accel = 0;
+	data->fifo.nb.total = 0;
+
+	raw_fifo_count = (__be16 *)data->buffer;
+	ret = regmap_bulk_read(data->map, INV_ICM42370_REG_FIFO_COUNT,
+			       raw_fifo_count, sizeof(*raw_fifo_count));
+	if (ret)
+		return ret;
+
+	/* Check and limit number of samples if requested. */
+	fifo_nb = le16_to_cpup(raw_fifo_count);
+	if (fifo_nb == 0)
+		return 0;
+	if (max > 0 && fifo_nb > max)
+		fifo_nb = max;
+
+	/*
+	 * Read all FIFO data into the internal buffer, clamping the
+	 * device-reported count to the buffer capacity.
+	 */
+	data->fifo.count = min(fifo_nb * packet_size, INV_ICM42370_FIFO_SIZE_MAX);
+	ret = regmap_noinc_read(data->map, INV_ICM42370_REG_FIFO_DATA,
+				data->fifo.data, data->fifo.count);
+	if (ret == -EOPNOTSUPP || ret == -EFBIG) {
+		/* Read full fifo is not supported, read samples one by one. */
+		ret = 0;
+		for (i = 0; i < data->fifo.count && ret == 0; i += packet_size)
+			ret = regmap_noinc_read(data->map, INV_ICM42370_REG_FIFO_DATA,
+						&data->fifo.data[i], packet_size);
+	}
+	if (ret)
+		return ret;
+
+	for (i = 0; i < data->fifo.count; i += size) {
+		size = inv_icm42370_fifo_decode_packet(
+			&data->fifo.data[i], &accel, &temp, &timestamp, &odr);
+		if (size <= 0)
+			/* No more sample in buffer */
+			break;
+		if (accel && inv_icm42370_fifo_is_data_valid(accel))
+			data->fifo.nb.accel++;
+		data->fifo.nb.total++;
+	}
+
+	return 0;
+}
+
+int inv_icm42370_buffer_fifo_parse(struct inv_icm42370_data *data)
+{
+	struct inv_sensors_timestamp *ts;
+	int ret;
+
+	if (data->fifo.nb.total == 0)
+		return 0;
+
+	/* handle accelerometer timestamp and FIFO data parsing */
+	if (data->fifo.nb.accel > 0) {
+		ts = &data->ts;
+		inv_sensors_timestamp_interrupt(
+			ts, data->fifo.watermark.eff_accel, data->timestamp);
+		ret = inv_icm42370_accel_parse_fifo(data->indio_accel);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+int inv_icm42370_buffer_hwfifo_flush(struct inv_icm42370_data *data,
+				     unsigned int count)
+{
+	struct inv_sensors_timestamp *ts;
+	s64 accel_ts;
+	int ret;
+
+	accel_ts = iio_get_time_ns(data->indio_accel);
+
+	ret = inv_icm42370_buffer_fifo_read(data, count);
+	if (ret)
+		return ret;
+
+	if (data->fifo.nb.total == 0)
+		return 0;
+
+	if (data->fifo.nb.accel > 0) {
+		ts = &data->ts;
+		inv_sensors_timestamp_interrupt(ts, data->fifo.nb.accel,
+						accel_ts);
+		ret = inv_icm42370_accel_parse_fifo(data->indio_accel);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+int inv_icm42370_buffer_init(struct inv_icm42370_data *data)
+{
+	unsigned int val;
+	u8 regval;
+	int ret;
+
+	data->fifo.watermark.eff_accel = 1;
+
+	/* watermark should be set to a non-zero value before enabling interrupts */
+	data->fifo.watermark.accel = 1;
+	ret = inv_icm42370_buffer_update_watermark(data);
+	if (ret)
+		return ret;
+
+	/*
+	 * Default FIFO configuration (bits 6 to 5)
+	 * - FIFO count in bytes
+	 * - FIFO count in big endian
+	 */
+	val = INV_ICM42370_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
+	ret = regmap_update_bits(data->map, INV_ICM42370_REG_INTF_CONFIG0,
+				 GENMASK(6, 5), val);
+	if (ret)
+		return ret;
+
+	/*
+	 * Enable FIFO partial read interrupt.
+	 * Disable all FIFO EN bits.
+	 */
+	ret = inv_icm42370_mreg_read(data, INV_ICM42370_MREG1,
+				     INV_ICM42370_REG_FIFO_CONFIG5, &regval);
+	if (ret)
+		return ret;
+
+	regval &= ~(GENMASK(6, 5) | GENMASK(3, 0));
+	regval |= INV_ICM42370_FIFO_CONFIG5_WM_GT_TH;
+	regval |= INV_ICM42370_FIFO_CONFIG5_RESUME_PARTIAL_RD;
+
+	return inv_icm42370_mreg_write(data, INV_ICM42370_MREG1,
+				       INV_ICM42370_REG_FIFO_CONFIG5, regval);
+}
diff --git a/drivers/iio/accel/inv_icm42370_buffer.h b/drivers/iio/accel/inv_icm42370_buffer.h
new file mode 100644
index 0000000000000..d43a4209a5640
--- /dev/null
+++ b/drivers/iio/accel/inv_icm42370_buffer.h
@@ -0,0 +1,100 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ * Copyright (C) 2026 Axis Communications AB
+ */
+
+#ifndef INV_ICM42370_BUFFER_H_
+#define INV_ICM42370_BUFFER_H_
+
+#include <linux/kernel.h>
+#include <linux/bits.h>
+
+#define INV_ICM42370_SENSOR_ACCEL	BIT(1)
+#define INV_ICM42370_SENSOR_TEMP	BIT(2)
+
+/* 2.25kB buffer size */
+#define INV_ICM42370_FIFO_SIZE_MAX	0x900
+#define INV_ICM42370_FIFO_DATA_INVALID		-32768
+
+struct inv_icm42370_data;
+
+/**
+ * struct inv_icm42370_fifo - FIFO state variables
+ * @on:		reference counter for FIFO on.
+ * @en:		bits field of INV_ICM42370_SENSOR_* for FIFO EN bits.
+ * @period:	FIFO internal period.
+ * @watermark:	watermark configuration values for accel.
+ * @count:	number of bytes in the FIFO data buffer.
+ * @nb:		accel and total samples in the FIFO data buffer.
+ * @data:	FIFO data buffer aligned for DMA.
+ */
+struct inv_icm42370_fifo {
+	struct {
+		size_t accel;
+		size_t total;
+	} nb;
+	struct {
+		unsigned int accel;
+		unsigned int eff_accel;
+	} watermark;
+	unsigned int on;
+	unsigned int en;
+	size_t count;
+	u32 period;
+	u8 *data __aligned(IIO_DMA_MINALIGN);
+};
+
+/* FIFO data packet */
+struct inv_icm42370_fifo_sensor_data {
+	__be16 x;
+	__be16 y;
+	__be16 z;
+} __packed;
+
+static inline s16 inv_icm42370_fifo_get_sensor_data(__be16 d)
+{
+	return be16_to_cpu(d);
+}
+
+static inline bool
+inv_icm42370_fifo_is_data_valid(const struct inv_icm42370_fifo_sensor_data *s)
+{
+	s16 x, y, z;
+
+	x = inv_icm42370_fifo_get_sensor_data(s->x);
+	y = inv_icm42370_fifo_get_sensor_data(s->y);
+	z = inv_icm42370_fifo_get_sensor_data(s->z);
+
+	if (x == INV_ICM42370_FIFO_DATA_INVALID &&
+	    y == INV_ICM42370_FIFO_DATA_INVALID &&
+	    z == INV_ICM42370_FIFO_DATA_INVALID)
+		return false;
+
+	return true;
+}
+
+ssize_t inv_icm42370_fifo_decode_packet(const void *packet, const void **accel,
+					const s8 **temp, const void **timestamp,
+					unsigned int *odr);
+
+extern const struct iio_buffer_setup_ops inv_icm42370_buffer_ops;
+
+int inv_icm42370_buffer_init(struct inv_icm42370_data *data);
+
+void inv_icm42370_buffer_update_fifo_period(struct inv_icm42370_data *data);
+
+int inv_icm42370_buffer_set_fifo_en(struct inv_icm42370_data *data,
+				    unsigned int fifo_en);
+
+int inv_icm42370_buffer_update_watermark(struct inv_icm42370_data *data);
+
+int inv_icm42370_buffer_fifo_read(struct inv_icm42370_data *data,
+				  unsigned int max);
+
+int inv_icm42370_buffer_fifo_parse(struct inv_icm42370_data *data);
+
+int inv_icm42370_buffer_hwfifo_flush(struct inv_icm42370_data *data,
+				     unsigned int count);
+
+#endif
diff --git a/drivers/iio/accel/inv_icm42370_core.c b/drivers/iio/accel/inv_icm42370_core.c
index 6266362e83f6a..b1713a2f533c8 100644
--- a/drivers/iio/accel/inv_icm42370_core.c
+++ b/drivers/iio/accel/inv_icm42370_core.c
@@ -20,10 +20,25 @@
 #include <linux/types.h>
 #include <linux/units.h>
 
+#include <linux/iio/buffer.h>
 #include <linux/iio/common/inv_sensors_timestamp.h>
 #include <linux/iio/iio.h>
+#include <linux/iio/kfifo_buf.h>
 
 #include "inv_icm42370.h"
+#include "inv_icm42370_buffer.h"
+
+#define INV_ICM42370_SCAN_MASK_ACCEL_3AXIS				\
+	(BIT(INV_ICM42370_ACCEL_SCAN_X) |				\
+	BIT(INV_ICM42370_ACCEL_SCAN_Y) |				\
+	BIT(INV_ICM42370_ACCEL_SCAN_Z))
+
+#define INV_ICM42370_SCAN_MASK_TEMP	BIT(INV_ICM42370_ACCEL_SCAN_TEMP)
+
+static bool inv_icm42370_is_noinc_reg(struct device *dev, unsigned int reg)
+{
+	return reg == INV_ICM42370_REG_FIFO_DATA;
+}
 
 #define INV_ICM42370_SUSPEND_DELAY_MS 2000
 
@@ -32,6 +47,7 @@ const struct regmap_config inv_icm42370_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 	.max_register = 0x7E,
+	.readable_noinc_reg = inv_icm42370_is_noinc_reg,
 };
 EXPORT_SYMBOL_NS_GPL(inv_icm42370_regmap_config, "IIO_ICM42370");
 
@@ -57,6 +73,7 @@ static const struct iio_chan_spec inv_icm42370_accel_channels[] = {
 	INV_ICM42370_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42370_ACCEL_SCAN_Z,
 				inv_icm42370_accel_ext_infos),
 	INV_ICM42370_TEMP_CHAN(INV_ICM42370_ACCEL_SCAN_TEMP),
+	IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42370_ACCEL_SCAN_TIMESTAMP),
 };
 
 /* IIO format int + nano */
@@ -75,6 +92,18 @@ static const int inv_icm42370_accel_scale[] = {
 	[2 * INV_ICM42370_ACCEL_FS_2G + 1] = 598550,
 };
 
+/*
+ * IIO buffer layout: must match channel scan types.
+ * Accel: 3 x s16 BE (6 bytes), Temp: 1 x s16 native (2 bytes) = 8 bytes data.
+ * Timestamp: s64 at 8-byte aligned offset.
+ */
+struct inv_icm42370_accel_buffer {
+	struct inv_icm42370_fifo_sensor_data accel;
+	s16 temp;
+
+	s64 timestamp __aligned(8);
+};
+
 /**
  *  inv_icm42370_odr_to_period() - map ODR to Period
  *
@@ -431,6 +460,7 @@ static irqreturn_t inv_icm42370_irq_timestamp(int irq, void *_data)
 static irqreturn_t inv_icm42370_irq_handler(int irq, void *_data)
 {
 	struct inv_icm42370_data *data = _data;
+	struct device *dev = regmap_get_device(data->map);
 	unsigned int status;
 	int ret;
 
@@ -440,6 +470,21 @@ static irqreturn_t inv_icm42370_irq_handler(int irq, void *_data)
 	if (ret)
 		return IRQ_HANDLED;
 
+	if (status & INV_ICM42370_INT_STATUS_FIFO_FULL)
+		dev_warn_ratelimited(dev, "FIFO full data lost!\n");
+
+	if (status & (INV_ICM42370_INT_STATUS_FIFO_THS |
+		      INV_ICM42370_INT_STATUS_FIFO_FULL)) {
+		ret = inv_icm42370_buffer_fifo_read(data, 0);
+		if (ret) {
+			dev_err_ratelimited(dev, "FIFO read error %d\n", ret);
+			return IRQ_HANDLED;
+		}
+		ret = inv_icm42370_buffer_fifo_parse(data);
+		if (ret)
+			dev_err_ratelimited(dev, "FIFO parsing error %d\n", ret);
+	}
+
 	return IRQ_HANDLED;
 }
 
@@ -505,6 +550,27 @@ static int inv_icm42370_irq_init(struct inv_icm42370_data *data, int irq,
 					 "inv_icm42370", data);
 }
 
+static int inv_icm42370_timestamp_setup(struct inv_icm42370_data *data)
+{
+	u8 val;
+	int ret;
+
+	ret = inv_icm42370_mreg_read(data, INV_ICM42370_MREG1,
+				     INV_ICM42370_REG_TMST_CONFIG1, &val);
+	if (ret)
+		return ret;
+
+	/*
+	 * Disable FIFO timestamp to produce 8-byte Packet 1.
+	 * Host-side timestamps are interpolated from the IRQ timestamp
+	 * and ODR period via inv_sensors_timestamp.
+	 */
+	val &= ~INV_ICM42370_TMST_CONFIG_TMST_EN;
+
+	return inv_icm42370_mreg_write(data, INV_ICM42370_MREG1,
+				       INV_ICM42370_REG_TMST_CONFIG1, val);
+}
+
 /*
  * Calibration bias values, IIO range format int + micro.
  * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
@@ -990,6 +1056,8 @@ static int inv_icm42370_accel_write_odr(struct iio_dev *indio_dev, int val,
 	if (ret)
 		return ret;
 
+	inv_icm42370_buffer_update_fifo_period(data);
+
 	return 0;
 }
 
@@ -1039,6 +1107,40 @@ static int inv_icm42370_accel_write_raw(struct iio_dev *indio_dev,
 	}
 }
 
+/* enable accelerometer sensor and FIFO write */
+static int inv_icm42370_accel_update_scan_mode(struct iio_dev *indio_dev,
+					       const unsigned long *scan_mask)
+{
+	struct inv_icm42370_data *st = iio_priv(indio_dev);
+	struct inv_icm42370_conf conf = INV_ICM42370_SENSOR_CONF_INIT;
+	unsigned int fifo_en = 0;
+	unsigned int sleep_accel = 0;
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	if (*scan_mask & INV_ICM42370_SCAN_MASK_ACCEL_3AXIS) {
+		/* enable accel sensor */
+		conf.mode = st->conf.mode;
+		conf.filter = st->filter;
+
+		ret = inv_icm42370_set_accel_conf(st, &conf, &sleep_accel);
+		if (ret)
+			goto out;
+
+		fifo_en |= INV_ICM42370_SENSOR_ACCEL;
+	}
+
+	/* update data FIFO write */
+	ret = inv_icm42370_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
+
+out:
+	if (sleep_accel)
+		msleep(sleep_accel);
+
+	return ret;
+}
+
 /**
  * inv_icm42370_accel_read_sensor() - internal function to read accelerometer sensor registers
  *
@@ -1161,15 +1263,54 @@ static int inv_icm42370_accel_read_raw(struct iio_dev *indio_dev,
 	}
 }
 
+static int inv_icm42370_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
+						   unsigned int val)
+{
+	struct inv_icm42370_data *st = iio_priv(indio_dev);
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	st->fifo.watermark.accel = val;
+	ret = inv_icm42370_buffer_update_watermark(st);
+
+	return ret;
+}
+
+static int inv_icm42370_accel_hwfifo_flush(struct iio_dev *indio_dev,
+					   unsigned int count)
+{
+	struct inv_icm42370_data *st = iio_priv(indio_dev);
+	int ret;
+
+	if (count == 0)
+		return 0;
+
+	mutex_lock(&st->lock);
+
+	ret = inv_icm42370_buffer_hwfifo_flush(st, count);
+	if (!ret)
+		ret = st->fifo.nb.accel;
+
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
 static const struct iio_info inv_icm42370_info = {
 	.read_raw = inv_icm42370_accel_read_raw,
 	.write_raw = inv_icm42370_accel_write_raw,
+	.update_scan_mode = inv_icm42370_accel_update_scan_mode,
+	.hwfifo_set_watermark = inv_icm42370_accel_hwfifo_set_watermark,
+	.hwfifo_flush_to_buffer = inv_icm42370_accel_hwfifo_flush,
 };
 
 struct iio_dev *inv_icm42370_accel_init(struct iio_dev *indio_dev,
 					struct inv_icm42370_data *data)
 {
+	struct device *dev = regmap_get_device(data->map);
 	struct inv_sensors_timestamp_chip ts_chip;
+	int ret;
 
 	data->scales = inv_icm42370_accel_scale;
 	data->scales_len = ARRAY_SIZE(inv_icm42370_accel_scale);
@@ -1187,13 +1328,64 @@ struct iio_dev *inv_icm42370_accel_init(struct iio_dev *indio_dev,
 
 	indio_dev->name = "inv_icm42370";
 	indio_dev->info = &inv_icm42370_info;
-	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->modes = INDIO_DIRECT_MODE | INDIO_ALL_BUFFER_MODES;
 	indio_dev->channels = inv_icm42370_accel_channels;
 	indio_dev->num_channels = ARRAY_SIZE(inv_icm42370_accel_channels);
 
+	ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
+					  &inv_icm42370_buffer_ops);
+	if (ret)
+		return ERR_PTR(ret);
+
 	return indio_dev;
 }
 
+int inv_icm42370_accel_parse_fifo(struct iio_dev *indio_dev)
+{
+	struct inv_icm42370_data *data = iio_priv(indio_dev);
+	struct inv_sensors_timestamp *ts = &data->ts;
+	ssize_t i, size;
+	unsigned int no;
+	const void *accel, *timestamp;
+	const s8 *temp;
+	unsigned int odr;
+	s64 ts_val;
+	struct inv_icm42370_accel_buffer buffer = {};
+
+	for (i = 0, no = 0; i < data->fifo.count; i += size, ++no) {
+		size = inv_icm42370_fifo_decode_packet(&data->fifo.data[i],
+				&accel, &temp, &timestamp, &odr);
+		if (size <= 0)
+			return size;
+
+		if (accel == NULL || !inv_icm42370_fifo_is_data_valid(accel))
+			continue;
+
+		if (odr & INV_ICM42370_SENSOR_ACCEL)
+			inv_sensors_timestamp_apply_odr(ts, data->fifo.period,
+							data->fifo.nb.total, no);
+
+		memcpy(&buffer.accel, accel, sizeof(buffer.accel));
+
+		/*
+		 * FIFO 8-bit temp has sensitivity ~2 LSB/°C.
+		 * Register 16-bit temp has sensitivity 128 LSB/°C.
+		 * Scale factor: 128 / 2 = 64.
+		 * This lets the IIO scale (1000/128) and offset (3200) work
+		 * correctly for both register reads and FIFO data.
+		 */
+		if (temp)
+			buffer.temp = (s16)*temp * 64;
+		else
+			buffer.temp = (s16)INV_ICM42370_DATA_INVALID;
+
+		ts_val = inv_sensors_timestamp_pop(ts);
+		iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
+	}
+
+	return 0;
+}
+
 /**
  * inv_icm42370_core_probe() - initialize and register the ICM-42370 device
  * @regmap:	register map for accessing the device's registers.
@@ -1242,6 +1434,10 @@ int inv_icm42370_core_probe(struct regmap *regmap, int chip, int irq,
 
 	data = iio_priv(indio_dev);
 
+	data->fifo.data = devm_kzalloc(dev, INV_ICM42370_FIFO_SIZE_MAX, GFP_KERNEL);
+	if (!data->fifo.data)
+		return -ENOMEM;
+
 	ret = devm_mutex_init(dev, &data->lock);
 	if (ret)
 		return dev_err_probe(dev, ret, "unable to initialize mutex\n");
@@ -1267,6 +1463,14 @@ int inv_icm42370_core_probe(struct regmap *regmap, int chip, int irq,
 		return ret;
 	}
 
+	ret = inv_icm42370_timestamp_setup(data);
+	if (ret)
+		return ret;
+
+	ret = inv_icm42370_buffer_init(data);
+	if (ret)
+		return ret;
+
 	data->indio_accel = inv_icm42370_accel_init(indio_dev, data);
 	if (IS_ERR(data->indio_accel))
 		return PTR_ERR(data->indio_accel);

-- 
2.43.0