Add Invensense ICM42607

From: Chris Morgan <[email protected]>

Add devicetree binding for the Invensense ICM42607 and Invensense ICM42607P inertial measurement unit. This unit is a combined accelerometer, gyroscope, and thermometer available via I2C or SPI.

This device is functionally very similar to the icm42600 series with a very different register layout, however unlike the icm42600 driver we require a interrupt-names property be present.

Quoted 5 lines · Chris Morgan

From: Chris Morgan <[email protected]>

Add devicetree binding for the Invensense ICM42607 and Invensense ICM42607P inertial measurement unit. This unit is a combined accelerometer, gyroscope, and thermometer available via I2C or SPI.

Implement feedback from v3. Subject is still incorrect.

Quoted 4 lines

This device is functionally very similar to the icm42600 series with a very different register layout, however unlike the icm42600 driver we require a interrupt-names property be present.

Why do we require it? Who needs it?

Best regards,
Krzysztof

Quoted 9 lines · Krzysztof Kozlowski

Quoted 5 lines · Chris Morgan

From: Chris Morgan <[email protected]>

Add devicetree binding for the Invensense ICM42607 and Invensense ICM42607P inertial measurement unit. This unit is a combined accelerometer, gyroscope, and thermometer available via I2C or SPI.

Implement feedback from v3. Subject is still incorrect.

I'm sorry, that was dropped due to an oversight. I'll correct it.

Quoted 5 lines

Quoted 4 lines

This device is functionally very similar to the icm42600 series with a very different register layout, however unlike the icm42600 driver we require a interrupt-names property be present.

Quoted 2 lines

Why do we require it? Who needs it?

The driver this was based off of, the icm42600 driver, has a fallback to select the 1st available interrupt in the event of the named

interrupt not existing. I was told not to do that here so the named interrupt is a requirement for the icm42607.

Quoted 9 lines · Krzysztof Kozlowski

Quoted 5 lines · Chris Morgan

From: Chris Morgan <[email protected]>

Add devicetree binding for the Invensense ICM42607 and Invensense ICM42607P inertial measurement unit. This unit is a combined accelerometer, gyroscope, and thermometer available via I2C or SPI.

Implement feedback from v3. Subject is still incorrect.

I'm sorry, that was dropped due to an oversight. I'll correct it.

Quoted 5 lines

Quoted 4 lines

This device is functionally very similar to the icm42600 series with a very different register layout, however unlike the icm42600 driver we require a interrupt-names property be present.

Quoted 2 lines

Why do we require it? Who needs it?

The driver this was based off of, the icm42600 driver, has a fallback to select the 1st available interrupt in the event of the named interrupt not existing. I was told not to do that here so the named interrupt is a requirement for the icm42607.

Quoted 4 lines

Best regards,
Krzysztof

Thank you,
Chris

From: Chris Morgan <[email protected]>

Add the core component of a new inv_icm42607 driver. This includes a few setup functions and the full register definition in the header file.

Signed-off-by Chris Morgan <[email protected]>

---
 drivers/iio/imu/inv_icm42607/inv_icm42607.h   | 336 ++++++++++++++++++
 .../iio/imu/inv_icm42607/inv_icm42607_core.c  | 206 +++++++++++
 2 files changed, 542 insertions(+)
 create mode 100644 drivers/iio/imu/inv_icm42607/inv_icm42607.h
 create mode 100644 drivers/iio/imu/inv_icm42607/inv_icm42607_core.c

drivers/iio/imu/inv_icm42607/inv_icm42607_core.c new file +28 -0

new file mode 100644
index 000000000000..4801ece28058
 
+static int inv_icm42607_set_conf(struct inv_icm42607_state *st,
+				 const struct inv_icm42607_conf *conf)
+{
+	unsigned int val;
+	int ret;
+
+	val = FIELD_PREP(INV_ICM42607_PWR_MGMT0_GYRO_MODE_MASK, conf->gyro.mode);
+	val |= FIELD_PREP(INV_ICM42607_PWR_MGMT0_ACCEL_MODE_MASK, conf->accel.mode);
+	/*
+	 * No temperature enable reg in datasheet, but BSP driver selected RC
+	 * oscillator clock in LP mode when temperature was disabled.
+	 */
+	if (!conf->temp_en)
+		val |= INV_ICM42607_PWR_MGMT0_ACCEL_LP_CLK_SEL;
+	ret = regmap_write(st->map, INV_ICM42607_REG_PWR_MGMT0, val);
+	if (ret)
+		return ret;
+
+	val = FIELD_PREP(INV_ICM42607_GYRO_CONFIG0_FS_SEL_MASK, conf->gyro.fs);
+	val |= FIELD_PREP(INV_ICM42607_GYRO_CONFIG0_ODR_MASK, conf->gyro.odr);
+	ret = regmap_write(st->map, INV_ICM42607_REG_GYRO_CONFIG0, val);
+	if (ret)
+		return ret;
+
+	val = FIELD_PREP(INV_ICM42607_ACCEL_CONFIG0_FS_SEL_MASK, conf->accel.fs);
+	val |= FIELD_PREP(INV_ICM42607_ACCEL_CONFIG0_ODR_MASK, conf->accel.odr);
+	ret = regmap_write(st->map, INV_ICM42607_REG_ACCEL_CONFIG0, val);
+	if (ret)

drivers/iio/imu/inv_icm42607/inv_icm42607_core.c new file +36 -0

+		return ret;
+
+	st->conf = *conf;
+
+	return 0;
+}
+
+/**
+ *  inv_icm42607_setup() - check and setup chip
+ *  @st:	driver internal state
+ *  @bus_setup:	callback for setting up bus specific registers
+ *
+ *  Returns 0 on success, a negative error code otherwise.
+ */
+static int inv_icm42607_setup(struct inv_icm42607_state *st,
+			      inv_icm42607_bus_setup bus_setup)
+{
+	const struct device *dev = regmap_get_device(st->map);
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(st->map, INV_ICM42607_REG_WHOAMI, &val);
+	if (ret)
+		return ret;
+
+	if (val != st->hw->whoami)
+		dev_warn(dev, "Unknown whoami %#02x expected %#02x (%s)\n",
+			 val, st->hw->whoami, st->hw->name);
+
+	ret = regmap_write(st->map, INV_ICM42607_REG_SIGNAL_PATH_RESET,
+			   INV_ICM42607_SIGNAL_PATH_RESET_SOFT_RESET);
+	if (ret)
+		return ret;
+
+	fsleep(INV_ICM42607_RESET_TIME_MS * 1000);
+

drivers/iio/imu/inv_icm42607/inv_icm42607_core.c new file +22 -0

+	if (ret)
+		return ret;
+
+	ret = bus_setup(st);
+	if (ret)
+		return ret;
+
+	ret = regmap_set_bits(st->map, INV_ICM42607_REG_INTF_CONFIG0,
+			      INV_ICM42607_INTF_CONFIG0_SENSOR_DATA_ENDIAN);
+	if (ret)
+		return ret;
+
+	ret = regmap_update_bits(st->map, INV_ICM42607_REG_INTF_CONFIG1,
+				 INV_ICM42607_INTF_CONFIG1_CLKSEL_MASK,
+				 INV_ICM42607_INTF_CONFIG1_CLKSEL_PLL);
+	if (ret)
+		return ret;
+
+	return inv_icm42607_set_conf(st, st->hw->conf);
+}
 
+int inv_icm42607_core_probe(struct regmap *regmap,
+			    const struct inv_icm42607_hw *hw,

drivers/iio/imu/inv_icm42607/inv_icm42607_core.c new file +45 -0

+
+	ret = devm_mutex_init(dev, &st->lock);
+	if (ret)
+		return ret;
+
+	st->hw = hw;
+	st->map = regmap;
+	st->irq = irq;
+
+	ret = iio_read_mount_matrix(dev, &st->orientation);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to retrieve mounting matrix %d\n", ret);
+
+	ret = devm_regulator_get_enable(dev, "vdd");
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Failed to get vdd regulator\n");
+
+	st->vddio_supply = devm_regulator_get(dev, "vddio");
+	if (IS_ERR(st->vddio_supply))
+		return dev_err_probe(dev, PTR_ERR(st->vddio_supply),
+				     "Failed to get vddio regulator\n");
+
+	ret = inv_icm42607_enable_vddio_reg(st);
+	if (ret)
+		return ret;
+
+	ret = devm_add_action_or_reset(dev, inv_icm42607_disable_vddio_reg, st);
+	if (ret)
+		return ret;
+
+	/* Setup chip registers (includes WHOAMI check, reset check, bus setup) */
+	ret = inv_icm42607_setup(st, bus_setup);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(inv_icm42607_core_probe, "IIO_ICM42607");
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense ICM-42607x device driver");
+MODULE_LICENSE("GPL");
+MODULE_IMPORT_NS("IIO_INV_SENSORS_TIMESTAMP");

From: Chris Morgan <[email protected]>

Add I2C and SPI driver support for InvenSense ICM-42607 devices. Add necessary Kconfig and Makefile to allow building of (incomplete) driver.

Signed-off-by Chris Morgan <[email protected]>

From: Chris Morgan <[email protected]>

Add power management support for the ICM42607 device driver.

Signed-off-by Chris Morgan <[email protected]>

drivers/iio/imu/inv_icm42607/inv_icm42607_core.c +0 -0

index 3dd4cc8c6b1d..cfdce70ce25d 100644
 
 

drivers/iio/imu/inv_icm42607/inv_icm42607_core.c +21 -0

+	if (ret)
+		return ret;
+
+	ret = inv_icm42607_set_pwr_mgmt0(st, INV_ICM42607_SENSOR_MODE_OFF,
+					 INV_ICM42607_SENSOR_MODE_OFF,
+					 false, NULL);
+	if (ret)
+		return ret;
+
+	inv_icm42607_disable_vddio_reg(st);
+
+	return 0;
+}
 
+
+static int inv_icm42607_runtime_suspend(struct device *dev)
+{
+	struct inv_icm42607_state *st = dev_get_drvdata(dev);
+	int ret;
+
+	guard(mutex)(&st->lock);
+

From: Chris Morgan <[email protected]>

Add all FIFO parsing and reading functions to support inv_icm42607 hardware.

Signed-off-by Chris Morgan <[email protected]>

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +26 -0

new file mode 100644
index 000000000000..e065d60ac119
@@ -0,0 +1,483 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2026 InvenSense, Inc.
+ */
+
+#include <linux/delay.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/minmax.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/unaligned.h>
+
+#include <linux/iio/common/inv_sensors_timestamp.h>
+
+#include "inv_icm42607.h"
+#include "inv_icm42607_buffer.h"
+
+/* FIFO header: 1 byte */
+#define INV_ICM42607_FIFO_HEADER_MSG		BIT(7)
+#define INV_ICM42607_FIFO_HEADER_ACCEL		BIT(6)
+#define INV_ICM42607_FIFO_HEADER_GYRO		BIT(5)
+#define INV_ICM42607_FIFO_HEADER_TMST_FSYNC	GENMASK(3, 2)
+#define INV_ICM42607_FIFO_HEADER_ODR_ACCEL	BIT(1)
+#define INV_ICM42607_FIFO_HEADER_ODR_GYRO	BIT(0)

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +103 -0

+	u8 header = *((const u8 *)packet);
+
+	/* FIFO empty */
+	if (header & INV_ICM42607_FIFO_HEADER_MSG) {
+		*accel = NULL;
+		*gyro = NULL;
+		*temp = NULL;
+		*timestamp = NULL;
+		return 0;
+	}
+
+	/* accel + gyro */
+	if ((header & INV_ICM42607_FIFO_HEADER_ACCEL) &&
+	    (header & INV_ICM42607_FIFO_HEADER_GYRO)) {
+		*accel = &pack2->accel;
+		*gyro = &pack2->gyro;
+		*temp = &pack2->temp;
+		*timestamp = &pack2->timestamp;
+		return INV_ICM42607_FIFO_2SENSORS_PACKET_SIZE;
+	}
+
+	/* accel only */
+	if (header & INV_ICM42607_FIFO_HEADER_ACCEL) {
+		*accel = &pack1->data;
+		*gyro = NULL;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* gyro only */
+	if (header & INV_ICM42607_FIFO_HEADER_GYRO) {
+		*accel = NULL;
+		*gyro = &pack1->data;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* invalid packet if here */
+	return -EINVAL;
+}
 
+/**
+ * inv_icm42607_buffer_update_watermark - update watermark FIFO threshold
+ * @st:	driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ */
+int inv_icm42607_buffer_update_watermark(struct inv_icm42607_state *st)
+{
+	const struct device *dev = regmap_get_device(st->map);
+	unsigned int wm_gyro, wm_accel, watermark;
+	u32 latency_gyro, latency_accel, latency;
+	u32 period_gyro, period_accel;
+	size_t packet_size, wm_size;
+	__le16 raw_wm;
+	bool restore;
+	int ret;
+
+	packet_size = inv_icm42607_get_packet_size(st->fifo.en);
+
+	/* compute sensors latency, depending on sensor watermark and odr */
+	wm_gyro = inv_icm42607_wm_truncate(st->fifo.watermark.gyro, packet_size);
+	wm_accel = inv_icm42607_wm_truncate(st->fifo.watermark.accel, packet_size);
+	/* use us for odr to avoid overflow using 32 bits values */
+	period_gyro = inv_icm42607_odr_to_period(st->conf.gyro.odr) / 1000UL;
+	period_accel = inv_icm42607_odr_to_period(st->conf.accel.odr) / 1000UL;
+	latency_gyro = period_gyro * wm_gyro;
+	latency_accel = period_accel * wm_accel;
+
+	/* 0 value for watermark means that the sensor is turned off */
+	if (wm_gyro == 0 && wm_accel == 0)
+		return 0;
+
+	if (latency_gyro == 0) {
+		watermark = wm_accel;
+		st->fifo.watermark.eff_accel = wm_accel;
+	} else if (latency_accel == 0) {
+		watermark = wm_gyro;
+		st->fifo.watermark.eff_gyro = wm_gyro;
+	} else {
+		/* compute the smallest latency that is a multiple of both */
+		if (latency_gyro <= latency_accel)
+			latency = latency_gyro - (latency_accel % latency_gyro);
+		else
+			latency = latency_accel - (latency_gyro % latency_accel);
+		/* all this works because periods are multiple of each others */
+		watermark = latency / min(period_gyro, period_accel);
+		watermark = max(watermark, 1);
+		/* update effective watermark */
+		st->fifo.watermark.eff_gyro = max(latency / period_gyro, 1);
+		st->fifo.watermark.eff_accel = max(latency / period_accel, 1);
+	}
+
+	/* changing FIFO watermark requires to turn off watermark interrupt */
+	ret = regmap_update_bits_check(st->map, INV_ICM42607_REG_INT_SOURCE0,
+				       INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN,
+				       0, &restore);
+	if (ret)
+		return ret;
+
+	/* compute watermark value in bytes */
+	wm_size = watermark * packet_size;

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +27 -0

+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
 
+
+static int inv_icm42607_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	if (st->fifo.on > 1) {
+		st->fifo.on--;
+		return 0;
+	}
+
+	/* Set FIFO to 0 since iio core ignores teardown errors. */
+	st->fifo.on = 0;
+
+	/* set FIFO in bypass mode */
+	ret = regmap_write(st->map, INV_ICM42607_REG_FIFO_CONFIG1,
+			   INV_ICM42607_FIFO_CONFIG1_BYPASS);
+	if (ret)
+		return ret;

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +25 -0

+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int inv_icm42607_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+	unsigned int sensor;
+	unsigned int *watermark;
+	int ret;
+
+	if (indio_dev == st->indio_gyro) {
+		sensor = INV_ICM42607_SENSOR_GYRO;
+		watermark = &st->fifo.watermark.gyro;
+	} else if (indio_dev == st->indio_accel) {
+		sensor = INV_ICM42607_SENSOR_ACCEL;
+		watermark = &st->fifo.watermark.accel;
+	} else {
+		return -EINVAL;
+	}
+
+	mutex_lock(&st->lock);

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +31 -0

+	ret = inv_icm42607_buffer_update_watermark(st);
+	if (ret)
+		goto out_unlock;
+
+out_unlock:
+	mutex_unlock(&st->lock);
+
+	pm_runtime_put_autosuspend(dev);
+
+	return ret;
+}
 
+int inv_icm42607_buffer_fifo_read(struct inv_icm42607_state *st,
+				  unsigned int max)
+{
+	const void *accel, *gyro, *timestamp;
+	size_t i, max_count;
+	const s8 *temp;
+	ssize_t size;
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	/* 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);

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +32 -0

+
+	/* check and clamp FIFO count value */
+	if (st->fifo.count == 0)
+		return 0;
+
+	st->fifo.count = min(st->fifo.count, max_count);
+
+	/* read all FIFO data in internal buffer */
+	ret = regmap_noinc_read(st->map, INV_ICM42607_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_icm42607_fifo_decode_packet(&st->fifo.data[i],
+				&accel, &gyro, &temp, &timestamp);
+		/* Make sure the size is at least 1 valid packet. */
+		if (size < INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE)
+			break;
+		/* Error if we are going to overflow the buffer. */
+		if (i + size > st->fifo.count)
+			return -EIO;
+		if (gyro != NULL && inv_icm42607_fifo_is_data_valid(gyro))
+			st->fifo.nb.gyro++;
+		if (accel != NULL && inv_icm42607_fifo_is_data_valid(accel))
+			st->fifo.nb.accel++;
+		st->fifo.nb.total++;
+	}
+
+	return 0;
+}

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h new file +0 -0

new file mode 100644
index 000000000000..b77deb66f8bd

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h new file +19 -0

+
+#define INV_ICM42607_FIFO_DATA_INVALID		-32768
+
+static inline bool
+inv_icm42607_fifo_is_data_valid(const struct inv_icm42607_fifo_sensor_data *s)
+{
+	s16 x, y, z;
+
+	x = be16_to_cpu(s->x);
+	y = be16_to_cpu(s->y);
+	z = be16_to_cpu(s->z);
+
+	if (x == INV_ICM42607_FIFO_DATA_INVALID &&
+	    y == INV_ICM42607_FIFO_DATA_INVALID &&
+	    z == INV_ICM42607_FIFO_DATA_INVALID)
+		return false;
+
+	return true;
+}

Quoted 6 lines · On Fri, 29 May 2026 22:17:33 -0500 Chris Morgan <[email protected]> wrote:

From: Chris Morgan <[email protected]>

Add all FIFO parsing and reading functions to support inv_icm42607 hardware.

Signed-off-by Chris Morgan <[email protected]>

A few things inline.

J

Quoted 35 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +29 -0

new file mode 100644
index 000000000000..e065d60ac119
@@ -0,0 +1,483 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2026 InvenSense, Inc.
+ */
+
+#include <linux/delay.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/minmax.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/unaligned.h>
+
+#include <linux/iio/common/inv_sensors_timestamp.h>
+
+#include "inv_icm42607.h"
+#include "inv_icm42607_buffer.h"
+
+/* FIFO header: 1 byte */
+#define INV_ICM42607_FIFO_HEADER_MSG		BIT(7)
+#define INV_ICM42607_FIFO_HEADER_ACCEL		BIT(6)
+#define INV_ICM42607_FIFO_HEADER_GYRO		BIT(5)
+#define INV_ICM42607_FIFO_HEADER_TMST_FSYNC	GENMASK(3, 2)
+#define INV_ICM42607_FIFO_HEADER_ODR_ACCEL	BIT(1)
+#define INV_ICM42607_FIFO_HEADER_ODR_GYRO	BIT(0)
+
+struct inv_icm42607_fifo_1sensor_packet {
+	u8 header;

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 36

Quoted 7 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +7 -0

+	struct inv_icm42607_fifo_sensor_data data;
+	s8 temp;
+} __packed;
+
+struct inv_icm42607_fifo_2sensors_packet {
+	u8 header;
+	struct inv_icm42607_fifo_sensor_data accel;

Good example of the need for the inner structures to be packed

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 47

Quoted 12 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +11 -0

+	struct inv_icm42607_fifo_sensor_data gyro;
+	s8 temp;
+	__be16 timestamp;
+} __packed;
+
+ssize_t inv_icm42607_fifo_decode_packet(const void *packet, const void **accel,
+					const void **gyro, const int8_t **temp,
+					const void **timestamp)
+{
+	const struct inv_icm42607_fifo_1sensor_packet *pack1 = get_unaligned(&packet);
+	const struct inv_icm42607_fifo_2sensors_packet *pack2 = get_unaligned(&packet);

Hmm. Sashiko points out you are messing around with pointers here and it's not those but what the point to that we need to worry about alignment for.
You could memcpy that data into local structures.

Quoted 106 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +103 -0

+	u8 header = *((const u8 *)packet);
+
+	/* FIFO empty */
+	if (header & INV_ICM42607_FIFO_HEADER_MSG) {
+		*accel = NULL;
+		*gyro = NULL;
+		*temp = NULL;
+		*timestamp = NULL;
+		return 0;
+	}
+
+	/* accel + gyro */
+	if ((header & INV_ICM42607_FIFO_HEADER_ACCEL) &&
+	    (header & INV_ICM42607_FIFO_HEADER_GYRO)) {
+		*accel = &pack2->accel;
+		*gyro = &pack2->gyro;
+		*temp = &pack2->temp;
+		*timestamp = &pack2->timestamp;
+		return INV_ICM42607_FIFO_2SENSORS_PACKET_SIZE;
+	}
+
+	/* accel only */
+	if (header & INV_ICM42607_FIFO_HEADER_ACCEL) {
+		*accel = &pack1->data;
+		*gyro = NULL;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* gyro only */
+	if (header & INV_ICM42607_FIFO_HEADER_GYRO) {
+		*accel = NULL;
+		*gyro = &pack1->data;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* invalid packet if here */
+	return -EINVAL;
+}
 
+/**
+ * inv_icm42607_buffer_update_watermark - update watermark FIFO threshold
+ * @st:	driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ */
+int inv_icm42607_buffer_update_watermark(struct inv_icm42607_state *st)
+{
+	const struct device *dev = regmap_get_device(st->map);
+	unsigned int wm_gyro, wm_accel, watermark;
+	u32 latency_gyro, latency_accel, latency;
+	u32 period_gyro, period_accel;
+	size_t packet_size, wm_size;
+	__le16 raw_wm;
+	bool restore;
+	int ret;
+
+	packet_size = inv_icm42607_get_packet_size(st->fifo.en);
+
+	/* compute sensors latency, depending on sensor watermark and odr */
+	wm_gyro = inv_icm42607_wm_truncate(st->fifo.watermark.gyro, packet_size);
+	wm_accel = inv_icm42607_wm_truncate(st->fifo.watermark.accel, packet_size);
+	/* use us for odr to avoid overflow using 32 bits values */
+	period_gyro = inv_icm42607_odr_to_period(st->conf.gyro.odr) / 1000UL;
+	period_accel = inv_icm42607_odr_to_period(st->conf.accel.odr) / 1000UL;
+	latency_gyro = period_gyro * wm_gyro;
+	latency_accel = period_accel * wm_accel;
+
+	/* 0 value for watermark means that the sensor is turned off */
+	if (wm_gyro == 0 && wm_accel == 0)
+		return 0;
+
+	if (latency_gyro == 0) {
+		watermark = wm_accel;
+		st->fifo.watermark.eff_accel = wm_accel;
+	} else if (latency_accel == 0) {
+		watermark = wm_gyro;
+		st->fifo.watermark.eff_gyro = wm_gyro;
+	} else {
+		/* compute the smallest latency that is a multiple of both */
+		if (latency_gyro <= latency_accel)
+			latency = latency_gyro - (latency_accel % latency_gyro);
+		else
+			latency = latency_accel - (latency_gyro % latency_accel);
+		/* all this works because periods are multiple of each others */
+		watermark = latency / min(period_gyro, period_accel);
+		watermark = max(watermark, 1);
+		/* update effective watermark */
+		st->fifo.watermark.eff_gyro = max(latency / period_gyro, 1);
+		st->fifo.watermark.eff_accel = max(latency / period_accel, 1);
+	}
+
+	/* changing FIFO watermark requires to turn off watermark interrupt */
+	ret = regmap_update_bits_check(st->map, INV_ICM42607_REG_INT_SOURCE0,
+				       INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN,
+				       0, &restore);
+	if (ret)
+		return ret;
+
+	/* compute watermark value in bytes */
+	wm_size = watermark * packet_size;

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 230

Quoted 10 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +10 -0

+	raw_wm = INV_ICM42607_FIFO_WATERMARK_VAL(wm_size);
+	memcpy(st->buffer, &raw_wm, sizeof(raw_wm));
+	ret = regmap_bulk_write(st->map, INV_ICM42607_REG_FIFO_CONFIG2,
+				st->buffer, sizeof(raw_wm));
+	if (ret) {
+		dev_err(dev, "Unable to change watermark value: %d\n", ret);
+		if (restore)
+			regmap_update_bits(st->map, INV_ICM42607_REG_INT_SOURCE0,
+					   INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN,
+					   INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN);

set_bits.

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 238

Quoted 8 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +8 -0

+		return ret;
+	}
+
+	/* restore watermark interrupt */
+	if (restore) {
+		ret = regmap_update_bits(st->map, INV_ICM42607_REG_INT_SOURCE0,
+					 INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN,
+					 INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN);

set_bits

Quoted 28 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +27 -0

+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
 
+
+static int inv_icm42607_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	if (st->fifo.on > 1) {
+		st->fifo.on--;
+		return 0;
+	}
+
+	/* Set FIFO to 0 since iio core ignores teardown errors. */
+	st->fifo.on = 0;
+
+	/* set FIFO in bypass mode */
+	ret = regmap_write(st->map, INV_ICM42607_REG_FIFO_CONFIG1,
+			   INV_ICM42607_FIFO_CONFIG1_BYPASS);
+	if (ret)
+		return ret;

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 339

Quoted 10 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +10 -0

+
+	/* flush FIFO data */
+	ret = regmap_write(st->map, INV_ICM42607_REG_SIGNAL_PATH_RESET,
+			   INV_ICM42607_SIGNAL_PATH_RESET_FIFO_FLUSH);
+	if (ret)
+		return ret;
+
+	/* disable FIFO threshold interrupt */
+	ret = regmap_update_bits(st->map, INV_ICM42607_REG_INT_SOURCE0,
+				 INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN, 0);

regmap_clear_bits()

Quoted 30 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +30 -0

+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int inv_icm42607_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+	unsigned int sensor;
+	unsigned int *watermark;
+	int ret;
+
+	if (indio_dev == st->indio_gyro) {
+		sensor = INV_ICM42607_SENSOR_GYRO;
+		watermark = &st->fifo.watermark.gyro;
+	} else if (indio_dev == st->indio_accel) {
+		sensor = INV_ICM42607_SENSOR_ACCEL;
+		watermark = &st->fifo.watermark.accel;
+	} else {
+		return -EINVAL;
+	}
+
+	mutex_lock(&st->lock);
+
+	/*
+	 * FIFO enabled at update scan mode for accel or gyro, and
+	 * disabled here.
+	 */

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 374

Quoted 5 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +5 -0

+	ret = inv_icm42607_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
+	if (ret)
+		goto out_unlock;
+
+	*watermark = 0;

Add a comment on why this needs to be set to 0. Normally that only matters at all if the fifo is on, so I guess something unusual here?

Quoted 34 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +31 -0

+	ret = inv_icm42607_buffer_update_watermark(st);
+	if (ret)
+		goto out_unlock;
+
+out_unlock:
+	mutex_unlock(&st->lock);
+
+	pm_runtime_put_autosuspend(dev);
+
+	return ret;
+}
 
+int inv_icm42607_buffer_fifo_read(struct inv_icm42607_state *st,
+				  unsigned int max)
+{
+	const void *accel, *gyro, *timestamp;
+	size_t i, max_count;
+	const s8 *temp;
+	ssize_t size;
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	/* 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);

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 423

Quoted 10 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +10 -0

+	else
+		max_count = min((max * inv_icm42607_get_packet_size(st->fifo.en)),
+				sizeof(st->fifo.data));
+
+	/* read FIFO count value */
+	ret = regmap_bulk_read(st->map, INV_ICM42607_REG_FIFO_COUNTH,
+			       st->buffer, sizeof(u8) * 2);
+	if (ret)
+		return ret;
+	st->fifo.count = be16_to_cpup(st->buffer);

Might be ok to assume this is always a multiple of the scan size, but maybe sanity check it to keep sashiko happy and remove that assumption of atomic update.

Quoted 38 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +32 -0

+
+	/* check and clamp FIFO count value */
+	if (st->fifo.count == 0)
+		return 0;
+
+	st->fifo.count = min(st->fifo.count, max_count);
+
+	/* read all FIFO data in internal buffer */
+	ret = regmap_noinc_read(st->map, INV_ICM42607_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_icm42607_fifo_decode_packet(&st->fifo.data[i],
+				&accel, &gyro, &temp, &timestamp);
+		/* Make sure the size is at least 1 valid packet. */
+		if (size < INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE)
+			break;
+		/* Error if we are going to overflow the buffer. */
+		if (i + size > st->fifo.count)
+			return -EIO;
+		if (gyro != NULL && inv_icm42607_fifo_is_data_valid(gyro))
+			st->fifo.nb.gyro++;
+		if (accel != NULL && inv_icm42607_fifo_is_data_valid(accel))
+			st->fifo.nb.accel++;
+		st->fifo.nb.total++;
+	}
+
+	return 0;
+}

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h new file +0 -0

new file mode 100644
index 000000000000..b77deb66f8bd

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h line 51

Quoted 7 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h new file +7 -0

+
+/* FIFO data packet */
+struct inv_icm42607_fifo_sensor_data {
+	__be16 x;
+	__be16 y;
+	__be16 z;
+};

Sashiko is probably correct that this should be packed.
Makes not difference here but it's not aligned in the places it's embedded in other structs and the compiler seeing this will assume it is aligned.

Quoted 19 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h new file +19 -0

+
+#define INV_ICM42607_FIFO_DATA_INVALID		-32768
+
+static inline bool
+inv_icm42607_fifo_is_data_valid(const struct inv_icm42607_fifo_sensor_data *s)
+{
+	s16 x, y, z;
+
+	x = be16_to_cpu(s->x);
+	y = be16_to_cpu(s->y);
+	z = be16_to_cpu(s->z);
+
+	if (x == INV_ICM42607_FIFO_DATA_INVALID &&
+	    y == INV_ICM42607_FIFO_DATA_INVALID &&
+	    z == INV_ICM42607_FIFO_DATA_INVALID)
+		return false;
+
+	return true;
+}

Quoted 48 lines · Jonathan Cameron

Quoted 6 lines · On Fri, 29 May 2026 22:17:33 -0500 Chris Morgan <[email protected]> wrote:

From: Chris Morgan <[email protected]>

Add all FIFO parsing and reading functions to support inv_icm42607 hardware.

Signed-off-by Chris Morgan <[email protected]>

A few things inline.

J

Quoted 35 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +29 -0

new file mode 100644
index 000000000000..e065d60ac119
@@ -0,0 +1,483 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2026 InvenSense, Inc.
+ */
+
+#include <linux/delay.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/minmax.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/unaligned.h>
+
+#include <linux/iio/common/inv_sensors_timestamp.h>
+
+#include "inv_icm42607.h"
+#include "inv_icm42607_buffer.h"
+
+/* FIFO header: 1 byte */
+#define INV_ICM42607_FIFO_HEADER_MSG		BIT(7)
+#define INV_ICM42607_FIFO_HEADER_ACCEL		BIT(6)
+#define INV_ICM42607_FIFO_HEADER_GYRO		BIT(5)
+#define INV_ICM42607_FIFO_HEADER_TMST_FSYNC	GENMASK(3, 2)
+#define INV_ICM42607_FIFO_HEADER_ODR_ACCEL	BIT(1)
+#define INV_ICM42607_FIFO_HEADER_ODR_GYRO	BIT(0)
+
+struct inv_icm42607_fifo_1sensor_packet {
+	u8 header;

Quoted 8 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 36

Quoted 7 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +7 -0

+	struct inv_icm42607_fifo_sensor_data data;
+	s8 temp;
+} __packed;
+
+struct inv_icm42607_fifo_2sensors_packet {
+	u8 header;
+	struct inv_icm42607_fifo_sensor_data accel;  

Good example of the need for the inner structures to be packed

Are you saying I should set the inv_icm42607_fifo_sensor_data to be __packed? I think I was told in a previous patch not to do that, but I can add it back.

Quoted 17 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 47

Quoted 13 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +11 -0

+	struct inv_icm42607_fifo_sensor_data gyro;
+	s8 temp;
+	__be16 timestamp;
+} __packed;
+
+ssize_t inv_icm42607_fifo_decode_packet(const void *packet, const void **accel,
+					const void **gyro, const int8_t **temp,
+					const void **timestamp)
+{
+	const struct inv_icm42607_fifo_1sensor_packet *pack1 = get_unaligned(&packet);
+	const struct inv_icm42607_fifo_2sensors_packet *pack2 = get_unaligned(&packet);  

Hmm. Sashiko points out you are messing around with pointers here and it's not those but what the point to that we need to worry about alignment for.
You could memcpy that data into local structures.

I still suck at pointers, sorry. What exactly would you recommend here? memcpy the packet value into an allocated inv_icm42607_fifo_1sensor_packet or inv_icm42607_fifo_2sensors_packet?

Quoted 202 lines

Quoted 107 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +103 -0

+	u8 header = *((const u8 *)packet);
+
+	/* FIFO empty */
+	if (header & INV_ICM42607_FIFO_HEADER_MSG) {
+		*accel = NULL;
+		*gyro = NULL;
+		*temp = NULL;
+		*timestamp = NULL;
+		return 0;
+	}
+
+	/* accel + gyro */
+	if ((header & INV_ICM42607_FIFO_HEADER_ACCEL) &&
+	    (header & INV_ICM42607_FIFO_HEADER_GYRO)) {
+		*accel = &pack2->accel;
+		*gyro = &pack2->gyro;
+		*temp = &pack2->temp;
+		*timestamp = &pack2->timestamp;
+		return INV_ICM42607_FIFO_2SENSORS_PACKET_SIZE;
+	}
+
+	/* accel only */
+	if (header & INV_ICM42607_FIFO_HEADER_ACCEL) {
+		*accel = &pack1->data;
+		*gyro = NULL;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* gyro only */
+	if (header & INV_ICM42607_FIFO_HEADER_GYRO) {
+		*accel = NULL;
+		*gyro = &pack1->data;
+		*temp = &pack1->temp;
+		*timestamp = NULL;
+		return INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* invalid packet if here */
+	return -EINVAL;
+}  
 
+/**
+ * inv_icm42607_buffer_update_watermark - update watermark FIFO threshold
+ * @st:	driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ */
+int inv_icm42607_buffer_update_watermark(struct inv_icm42607_state *st)
+{
+	const struct device *dev = regmap_get_device(st->map);
+	unsigned int wm_gyro, wm_accel, watermark;
+	u32 latency_gyro, latency_accel, latency;
+	u32 period_gyro, period_accel;
+	size_t packet_size, wm_size;
+	__le16 raw_wm;
+	bool restore;
+	int ret;
+
+	packet_size = inv_icm42607_get_packet_size(st->fifo.en);
+
+	/* compute sensors latency, depending on sensor watermark and odr */
+	wm_gyro = inv_icm42607_wm_truncate(st->fifo.watermark.gyro, packet_size);
+	wm_accel = inv_icm42607_wm_truncate(st->fifo.watermark.accel, packet_size);
+	/* use us for odr to avoid overflow using 32 bits values */
+	period_gyro = inv_icm42607_odr_to_period(st->conf.gyro.odr) / 1000UL;
+	period_accel = inv_icm42607_odr_to_period(st->conf.accel.odr) / 1000UL;
+	latency_gyro = period_gyro * wm_gyro;
+	latency_accel = period_accel * wm_accel;
+
+	/* 0 value for watermark means that the sensor is turned off */
+	if (wm_gyro == 0 && wm_accel == 0)
+		return 0;
+
+	if (latency_gyro == 0) {
+		watermark = wm_accel;
+		st->fifo.watermark.eff_accel = wm_accel;
+	} else if (latency_accel == 0) {
+		watermark = wm_gyro;
+		st->fifo.watermark.eff_gyro = wm_gyro;
+	} else {
+		/* compute the smallest latency that is a multiple of both */
+		if (latency_gyro <= latency_accel)
+			latency = latency_gyro - (latency_accel % latency_gyro);
+		else
+			latency = latency_accel - (latency_gyro % latency_accel);
+		/* all this works because periods are multiple of each others */
+		watermark = latency / min(period_gyro, period_accel);
+		watermark = max(watermark, 1);
+		/* update effective watermark */
+		st->fifo.watermark.eff_gyro = max(latency / period_gyro, 1);
+		st->fifo.watermark.eff_accel = max(latency / period_accel, 1);
+	}
+
+	/* changing FIFO watermark requires to turn off watermark interrupt */
+	ret = regmap_update_bits_check(st->map, INV_ICM42607_REG_INT_SOURCE0,
+				       INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN,
+				       0, &restore);
+	if (ret)
+		return ret;
+
+	/* compute watermark value in bytes */
+	wm_size = watermark * packet_size;

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 230

Quoted 10 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +10 -0

+	raw_wm = INV_ICM42607_FIFO_WATERMARK_VAL(wm_size);
+	memcpy(st->buffer, &raw_wm, sizeof(raw_wm));
+	ret = regmap_bulk_write(st->map, INV_ICM42607_REG_FIFO_CONFIG2,
+				st->buffer, sizeof(raw_wm));
+	if (ret) {
+		dev_err(dev, "Unable to change watermark value: %d\n", ret);
+		if (restore)
+			regmap_update_bits(st->map, INV_ICM42607_REG_INT_SOURCE0,
+					   INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN,
+					   INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN);  

set_bits.

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 238

Quoted 8 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +8 -0

+		return ret;
+	}
+
+	/* restore watermark interrupt */
+	if (restore) {
+		ret = regmap_update_bits(st->map, INV_ICM42607_REG_INT_SOURCE0,
+					 INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN,
+					 INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN);  

set_bits

Quoted 28 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +27 -0

+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}  
 
+
+static int inv_icm42607_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	if (st->fifo.on > 1) {
+		st->fifo.on--;
+		return 0;
+	}
+
+	/* Set FIFO to 0 since iio core ignores teardown errors. */
+	st->fifo.on = 0;
+
+	/* set FIFO in bypass mode */
+	ret = regmap_write(st->map, INV_ICM42607_REG_FIFO_CONFIG1,
+			   INV_ICM42607_FIFO_CONFIG1_BYPASS);
+	if (ret)
+		return ret;

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 339

Quoted 10 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +10 -0

+
+	/* flush FIFO data */
+	ret = regmap_write(st->map, INV_ICM42607_REG_SIGNAL_PATH_RESET,
+			   INV_ICM42607_SIGNAL_PATH_RESET_FIFO_FLUSH);
+	if (ret)
+		return ret;
+
+	/* disable FIFO threshold interrupt */
+	ret = regmap_update_bits(st->map, INV_ICM42607_REG_INT_SOURCE0,
+				 INV_ICM42607_INT_SOURCE0_FIFO_THS_INT1_EN, 0);  

regmap_clear_bits()

Quoted 30 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +30 -0

+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int inv_icm42607_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+	unsigned int sensor;
+	unsigned int *watermark;
+	int ret;
+
+	if (indio_dev == st->indio_gyro) {
+		sensor = INV_ICM42607_SENSOR_GYRO;
+		watermark = &st->fifo.watermark.gyro;
+	} else if (indio_dev == st->indio_accel) {
+		sensor = INV_ICM42607_SENSOR_ACCEL;
+		watermark = &st->fifo.watermark.accel;
+	} else {
+		return -EINVAL;
+	}
+
+	mutex_lock(&st->lock);
+
+	/*
+	 * FIFO enabled at update scan mode for accel or gyro, and
+	 * disabled here.
+	 */

Quoted 8 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 374

Quoted 5 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +5 -0

+	ret = inv_icm42607_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
+	if (ret)
+		goto out_unlock;
+
+	*watermark = 0;  

Add a comment on why this needs to be set to 0. Normally that only matters at all if the fifo is on, so I guess something unusual here?

I think it's just resetting the watermark to 0 when shutting down, not 100% sure though.

Quoted 88 lines

Quoted 34 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +31 -0

+	ret = inv_icm42607_buffer_update_watermark(st);
+	if (ret)
+		goto out_unlock;
+
+out_unlock:
+	mutex_unlock(&st->lock);
+
+	pm_runtime_put_autosuspend(dev);
+
+	return ret;
+}  
 
+int inv_icm42607_buffer_fifo_read(struct inv_icm42607_state *st,
+				  unsigned int max)
+{
+	const void *accel, *gyro, *timestamp;
+	size_t i, max_count;
+	const s8 *temp;
+	ssize_t size;
+	int ret;
+
+	guard(mutex)(&st->lock);
+
+	/* 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);

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c line 423

Quoted 10 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +10 -0

+	else
+		max_count = min((max * inv_icm42607_get_packet_size(st->fifo.en)),
+				sizeof(st->fifo.data));
+
+	/* read FIFO count value */
+	ret = regmap_bulk_read(st->map, INV_ICM42607_REG_FIFO_COUNTH,
+			       st->buffer, sizeof(u8) * 2);
+	if (ret)
+		return ret;
+	st->fifo.count = be16_to_cpup(st->buffer);  

Might be ok to assume this is always a multiple of the scan size, but maybe sanity check it to keep sashiko happy and remove that assumption of atomic update.

Quoted 38 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.c new file +32 -0

+
+	/* check and clamp FIFO count value */
+	if (st->fifo.count == 0)
+		return 0;
+
+	st->fifo.count = min(st->fifo.count, max_count);
+
+	/* read all FIFO data in internal buffer */
+	ret = regmap_noinc_read(st->map, INV_ICM42607_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_icm42607_fifo_decode_packet(&st->fifo.data[i],
+				&accel, &gyro, &temp, &timestamp);
+		/* Make sure the size is at least 1 valid packet. */
+		if (size < INV_ICM42607_FIFO_1SENSOR_PACKET_SIZE)
+			break;
+		/* Error if we are going to overflow the buffer. */
+		if (i + size > st->fifo.count)
+			return -EIO;
+		if (gyro != NULL && inv_icm42607_fifo_is_data_valid(gyro))
+			st->fifo.nb.gyro++;
+		if (accel != NULL && inv_icm42607_fifo_is_data_valid(accel))
+			st->fifo.nb.accel++;
+		st->fifo.nb.total++;
+	}
+
+	return 0;
+}  

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h new file +0 -0

new file mode 100644
index 000000000000..b77deb66f8bd

Quoted 12 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h line 51

Quoted 7 lines

drivers/iio/imu/inv_icm42607/inv_icm42607_buffer.h new file +7 -0

+
+/* FIFO data packet */
+struct inv_icm42607_fifo_sensor_data {
+	__be16 x;
+	__be16 y;
+	__be16 z;
+};  

Sashiko is probably correct that this should be packed.
Makes not difference here but it's not aligned in the places it's embedded in other structs and the compiler seeing this will assume it is aligned.

So I should pack this instead of doing the "get_unaligned()" call?

Thank you.