From nobody Sun Sep 14 00:34:55 2025 Received: from smtp.kernel.org (aws-us-west-2-korg-mail-1.web.codeaurora.org [10.30.226.201]) (using TLSv1.2 with cipher ECDHE-RSA-AES256-GCM-SHA384 (256/256 bits)) (No client certificate requested) by smtp.subspace.kernel.org (Postfix) with ESMTPS id A6A2126FDB2; Wed, 20 Aug 2025 14:24:45 +0000 (UTC) Authentication-Results: smtp.subspace.kernel.org; arc=none smtp.client-ip=10.30.226.201 ARC-Seal: i=1; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1755699885; cv=none; b=Ya4DmbvcojhEqWWTNBmwBJwep5HQ8iq/cRLXo41WqSyegYKQhwhkA2TRNMQZt15oWKyALTMMRQV40xCI792z5K5dvpP6JWKqDdQdHw/KvRh6LNRKRlLvR/2RYF8E1BDajWg2C4AynaE190eSmjnUuHqMnNYg/FCbbe8ekzRiYuc= ARC-Message-Signature: i=1; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1755699885; c=relaxed/simple; bh=iWuO/NUBOxPQ8PyB/hzHjmuwTU69Lpj2W6tXIe4ygQc=; h=From:Date:Subject:MIME-Version:Content-Type:Message-Id:References: In-Reply-To:To:Cc; b=trSSY8XlS8r9oytCsivmxwpVXCVa1yt6OeSQbkgWGvWsqkxdP6czYBIijM+g//WUg7WXsOX6AmgDdnx5k1wYZkMkYrDR7yu7gTgoz3eAepkA62LRZzmXkSbfEQw0c/Y6UQIPRqL72711hMrcQFoEDRB8A+6SEHcKaX3B5kdZELA= ARC-Authentication-Results: i=1; smtp.subspace.kernel.org; dkim=pass (2048-bit key) header.d=kernel.org header.i=@kernel.org header.b=atnPApvX; arc=none smtp.client-ip=10.30.226.201 Authentication-Results: smtp.subspace.kernel.org; dkim=pass (2048-bit key) header.d=kernel.org header.i=@kernel.org header.b="atnPApvX" Received: by smtp.kernel.org (Postfix) with ESMTPS id 7A135C113CF; Wed, 20 Aug 2025 14:24:45 +0000 (UTC) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=kernel.org; s=k20201202; t=1755699885; bh=iWuO/NUBOxPQ8PyB/hzHjmuwTU69Lpj2W6tXIe4ygQc=; h=From:Date:Subject:References:In-Reply-To:To:Cc:Reply-To:From; b=atnPApvXjKiMsLspWkoElX/U8MSKMYX33HO/IGQjRdct3Y6MBgFvlq+o1cJfTHQWt S9lfHTDGdApoczMX5KnudH4so5I9+/49NEbi1NwKCN7H6czGTIzwNuOPYdqa3TP16Z y+ZbcqWp3C6r6/lM3TpmO52eBdmSYlBYUUYzao0clpVdCGN7cNW3TcSNyTrOXKIbQy UHIxKP0Z5xeyRqgqKlj9M0fpUolvsKSTUZUy1TTSaZiS33eGMdNxOVzcI0K/XlBdhG U8J8lW0/iUe/shtIrJv9QQ9wK7Oe/9i+QNo9mUUeCXRAvH/4F6KiXq2nTwn9Y+4Qcn rlZsbEifF3b7Q== Received: from aws-us-west-2-korg-lkml-1.web.codeaurora.org (localhost.localdomain [127.0.0.1]) by smtp.lore.kernel.org (Postfix) with ESMTP id 71F8CCA0EE4; Wed, 20 Aug 2025 14:24:45 +0000 (UTC) From: Remi Buisson via B4 Relay Date: Wed, 20 Aug 2025 14:24:21 +0000 Subject: [PATCH v5 3/9] iio: imu: inv_icm45600: add buffer support in iio devices Precedence: bulk X-Mailing-List: linux-kernel@vger.kernel.org List-Id: List-Subscribe: List-Unsubscribe: MIME-Version: 1.0 Content-Type: text/plain; charset="utf-8" Content-Transfer-Encoding: quoted-printable Message-Id: <20250820-add_newport_driver-v5-3-2fc9f13dddee@tdk.com> References: <20250820-add_newport_driver-v5-0-2fc9f13dddee@tdk.com> In-Reply-To: <20250820-add_newport_driver-v5-0-2fc9f13dddee@tdk.com> To: Jonathan Cameron , David Lechner , =?utf-8?q?Nuno_S=C3=A1?= , Andy Shevchenko , Rob Herring , Krzysztof Kozlowski , Conor Dooley Cc: linux-kernel@vger.kernel.org, linux-iio@vger.kernel.org, devicetree@vger.kernel.org, Remi Buisson X-Mailer: b4 0.14.2 X-Developer-Signature: v=1; a=ed25519-sha256; t=1755699883; l=28197; i=remi.buisson@tdk.com; s=20250411; h=from:subject:message-id; bh=7sReBcTlSe5SKVNWkBxn46Owj7RIZjXaQG5ARzMBkII=; b=seZgtXloV0sofx8MlMdSykTICXYQgetKF1KmCd0ZzWyr0i7DXptMnxJfL9yIHxzCCaoqSAuFM O6xT89U7bscB2T7RqGBJvShrrhqpT4OJMpFL4MVVzOUh+LJrzBKtUXK X-Developer-Key: i=remi.buisson@tdk.com; a=ed25519; pk=yDVMi4C7RpXN4dififo42A7fDDt3THYzoZoNq9lUZuo= X-Endpoint-Received: by B4 Relay for remi.buisson@tdk.com/20250411 with auth_id=372 X-Original-From: Remi Buisson Reply-To: remi.buisson@tdk.com From: Remi Buisson Add FIFO control functions. Support hwfifo watermark by multiplexing gyro and accel settings. Support hwfifo flush. Signed-off-by: Remi Buisson --- drivers/iio/imu/inv_icm45600/Makefile | 1 + drivers/iio/imu/inv_icm45600/inv_icm45600.h | 7 + drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.c | 492 +++++++++++++++++= ++++ drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.h | 98 ++++ drivers/iio/imu/inv_icm45600/inv_icm45600_core.c | 158 ++++++- 5 files changed, 754 insertions(+), 2 deletions(-) diff --git a/drivers/iio/imu/inv_icm45600/Makefile b/drivers/iio/imu/inv_ic= m45600/Makefile index 4f442b61896e91647c7947a044949792bae06a30..72f95bc30d993e0ea16b97622f4= a041a09ec6559 100644 --- a/drivers/iio/imu/inv_icm45600/Makefile +++ b/drivers/iio/imu/inv_icm45600/Makefile @@ -2,3 +2,4 @@ =20 obj-$(CONFIG_INV_ICM45600) +=3D inv-icm45600.o inv-icm45600-y +=3D inv_icm45600_core.o +inv-icm45600-y +=3D inv_icm45600_buffer.o diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600.h b/drivers/iio/imu/= inv_icm45600/inv_icm45600.h index 94ef0ff3ccda85583101f2eaca3bc3771141505a..a8f5769a09c9f734a15d02d2ee9= 53fac649dc04f 100644 --- a/drivers/iio/imu/inv_icm45600/inv_icm45600.h +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600.h @@ -8,8 +8,11 @@ #include #include #include +#include #include =20 +#include "inv_icm45600_buffer.h" + #define INV_ICM45600_REG_BANK_MASK GENMASK(15, 8) #define INV_ICM45600_REG_ADDR_MASK GENMASK(7, 0) =20 @@ -90,6 +93,8 @@ struct inv_icm45600_sensor_conf { u8 filter; }; =20 +#define INV_ICM45600_SENSOR_CONF_KEEP_VALUES {U8_MAX, U8_MAX, U8_MAX, U8_M= AX, } + struct inv_icm45600_conf { struct inv_icm45600_sensor_conf gyro; struct inv_icm45600_sensor_conf accel; @@ -127,6 +132,7 @@ extern const struct inv_icm45600_chip_info inv_icm45689= _chip_info; * @indio_gyro: gyroscope IIO device. * @indio_accel: accelerometer IIO device. * @timestamp: interrupt timestamps. + * @fifo: FIFO management structure. * @buffer: data transfer buffer aligned for DMA. */ struct inv_icm45600_state { @@ -143,6 +149,7 @@ struct inv_icm45600_state { s64 gyro; s64 accel; } timestamp; + struct inv_icm45600_fifo fifo; union { u8 buff[2]; __le16 u16; diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.c b/drivers/i= io/imu/inv_icm45600/inv_icm45600_buffer.c new file mode 100644 index 0000000000000000000000000000000000000000..50fd21a24e34decfbe10426946a= 51c61353eb6a9 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.c @@ -0,0 +1,492 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* Copyright (C) 2025 Invensense, Inc. */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm45600_buffer.h" +#include "inv_icm45600.h" + +/* FIFO header: 1 byte */ +#define INV_ICM45600_FIFO_EXT_HEADER BIT(7) +#define INV_ICM45600_FIFO_HEADER_ACCEL BIT(6) +#define INV_ICM45600_FIFO_HEADER_GYRO BIT(5) +#define INV_ICM45600_FIFO_HEADER_HIGH_RES BIT(4) +#define INV_ICM45600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2) +#define INV_ICM45600_FIFO_HEADER_ODR_ACCEL BIT(1) +#define INV_ICM45600_FIFO_HEADER_ODR_GYRO BIT(0) + +struct inv_icm45600_fifo_1sensor_packet { + u8 header; + struct inv_icm45600_fifo_sensor_data data; + s8 temp; +} __packed; + +struct inv_icm45600_fifo_2sensors_packet { + u8 header; + struct inv_icm45600_fifo_sensor_data accel; + struct inv_icm45600_fifo_sensor_data gyro; + s8 temp; + __le16 timestamp; +} __packed; + +ssize_t inv_icm45600_fifo_decode_packet(const void *packet, + const struct inv_icm45600_fifo_sensor_data **accel, + const struct inv_icm45600_fifo_sensor_data **gyro, + const s8 **temp, + const __le16 **timestamp, unsigned int *odr) +{ + const struct inv_icm45600_fifo_1sensor_packet *pack1 =3D packet; + const struct inv_icm45600_fifo_2sensors_packet *pack2 =3D packet; + u8 header =3D *((const u8 *)packet); + + /* FIFO extended header */ + if (header & INV_ICM45600_FIFO_EXT_HEADER) { + /* Not yet supported */ + return 0; + } + + /* handle odr flags. */ + *odr =3D 0; + if (header & INV_ICM45600_FIFO_HEADER_ODR_GYRO) + *odr |=3D INV_ICM45600_SENSOR_GYRO; + if (header & INV_ICM45600_FIFO_HEADER_ODR_ACCEL) + *odr |=3D INV_ICM45600_SENSOR_ACCEL; + + /* Accel + Gyro data are present. */ + if ((header & INV_ICM45600_FIFO_HEADER_ACCEL) && + (header & INV_ICM45600_FIFO_HEADER_GYRO)) { + *accel =3D &pack2->accel; + *gyro =3D &pack2->gyro; + *temp =3D &pack2->temp; + *timestamp =3D &pack2->timestamp; + return sizeof(*pack2); + } + + /* Accel data only. */ + if (header & INV_ICM45600_FIFO_HEADER_ACCEL) { + *accel =3D &pack1->data; + *gyro =3D NULL; + *temp =3D &pack1->temp; + *timestamp =3D NULL; + return sizeof(*pack1); + } + + /* Gyro data only. */ + if (header & INV_ICM45600_FIFO_HEADER_GYRO) { + *accel =3D NULL; + *gyro =3D &pack1->data; + *temp =3D &pack1->temp; + *timestamp =3D NULL; + return sizeof(*pack1); + } + + /* Invalid packet if here. */ + return -EINVAL; +} + +void inv_icm45600_buffer_update_fifo_period(struct inv_icm45600_state *st) +{ + u32 period_gyro, period_accel; + + if (st->fifo.en & INV_ICM45600_SENSOR_GYRO) + period_gyro =3D inv_icm45600_odr_to_period(st->conf.gyro.odr); + else + period_gyro =3D U32_MAX; + + if (st->fifo.en & INV_ICM45600_SENSOR_ACCEL) + period_accel =3D inv_icm45600_odr_to_period(st->conf.accel.odr); + else + period_accel =3D U32_MAX; + + st->fifo.period =3D min(period_gyro, period_accel); +} + +int inv_icm45600_buffer_set_fifo_en(struct inv_icm45600_state *st, + unsigned int fifo_en) +{ + unsigned int mask, val; + int ret; + + /* Update only FIFO EN bits. */ + mask =3D INV_ICM45600_FIFO_CONFIG3_GYRO_EN | + INV_ICM45600_FIFO_CONFIG3_ACCEL_EN; + + val =3D 0; + if ((fifo_en & INV_ICM45600_SENSOR_GYRO) || (fifo_en & INV_ICM45600_SENSO= R_ACCEL)) + val =3D (INV_ICM45600_FIFO_CONFIG3_GYRO_EN | INV_ICM45600_FIFO_CONFIG3_A= CCEL_EN); + + ret =3D regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, mask, = val); + if (ret) + return ret; + + st->fifo.en =3D fifo_en; + inv_icm45600_buffer_update_fifo_period(st); + + return 0; +} + +static unsigned int inv_icm45600_wm_truncate(unsigned int watermark, size_= t packet_size, + unsigned int fifo_period) +{ + size_t watermark_max, grace_samples; + + /* Keep 20ms for processing FIFO. fifo_period is in ns */ + grace_samples =3D (20U * 1000000U) / fifo_period; + if (grace_samples < 1) + grace_samples =3D 1; + + watermark_max =3D INV_ICM45600_FIFO_SIZE_MAX / packet_size; + watermark_max -=3D grace_samples; + + return min(watermark, watermark_max); +} + +/** + * inv_icm45600_buffer_update_watermark - update watermark FIFO threshold + * @st: driver internal state + * + * Returns 0 on success, a negative error code otherwise. + * + * FIFO watermark threshold is computed based on the required watermark va= lues + * set for gyro and accel sensors. Since watermark is all about acceptable= data + * latency, use the smallest setting between the 2. It means choosing the + * smallest latency but this is not as simple as choosing the smallest wat= ermark + * value. Latency depends on watermark and ODR. It requires several steps: + * 1) compute gyro and accel latencies and choose the smallest value. + * 2) adapt the chosen latency so that it is a multiple of both gyro and a= ccel + * ones. Otherwise it is possible that you don't meet a requirement. (f= or + * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing t= he + * value of 4 will not meet accel latency requirement because 6 is not a + * multiple of 4. You need to use the value 2.) + * 3) Since all periods are multiple of each others, watermark is computed= by + * dividing this computed latency by the smallest period, which corresp= onds + * to the FIFO frequency. + */ +int inv_icm45600_buffer_update_watermark(struct inv_icm45600_state *st) +{ + const size_t packet_size =3D sizeof(struct inv_icm45600_fifo_2sensors_pac= ket); + unsigned int wm_gyro, wm_accel, watermark; + u32 period_gyro, period_accel, period; + u32 latency_gyro, latency_accel, latency; + + /* Compute sensors latency, depending on sensor watermark and odr. */ + wm_gyro =3D inv_icm45600_wm_truncate(st->fifo.watermark.gyro, packet_size, + st->fifo.period); + wm_accel =3D inv_icm45600_wm_truncate(st->fifo.watermark.accel, packet_si= ze, + st->fifo.period); + /* Use us for odr to avoid overflow using 32 bits values. */ + period_gyro =3D inv_icm45600_odr_to_period(st->conf.gyro.odr) / 1000UL; + period_accel =3D inv_icm45600_odr_to_period(st->conf.accel.odr) / 1000UL; + latency_gyro =3D period_gyro * wm_gyro; + latency_accel =3D period_accel * wm_accel; + + /* 0 value for watermark means that the sensor is turned off. */ + if (wm_gyro =3D=3D 0 && wm_accel =3D=3D 0) + return 0; + + if (latency_gyro =3D=3D 0) { + watermark =3D wm_accel; + st->fifo.watermark.eff_accel =3D wm_accel; + } else if (latency_accel =3D=3D 0) { + watermark =3D wm_gyro; + st->fifo.watermark.eff_gyro =3D wm_gyro; + } else { + /* Compute the smallest latency that is a multiple of both. */ + if (latency_gyro <=3D latency_accel) + latency =3D latency_gyro - (latency_accel % latency_gyro); + else + latency =3D latency_accel - (latency_gyro % latency_accel); + /* Use the shortest period. */ + period =3D min(period_gyro, period_accel); + /* All this works because periods are multiple of each others. */ + watermark =3D max(latency / period, 1); + /* Update effective watermark. */ + st->fifo.watermark.eff_gyro =3D max(latency / period_gyro, 1); + st->fifo.watermark.eff_accel =3D max(latency / period_accel, 1); + } + + + st->buffer.u16 =3D cpu_to_le16(watermark); + return regmap_bulk_write(st->map, INV_ICM45600_REG_FIFO_WATERMARK, + &st->buffer.u16, sizeof(st->buffer.u16)); +} + +static int inv_icm45600_buffer_preenable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st =3D iio_device_get_drvdata(indio_dev); + struct device *dev =3D regmap_get_device(st->map); + struct inv_icm45600_sensor_state *sensor_st =3D iio_priv(indio_dev); + struct inv_sensors_timestamp *ts =3D &sensor_st->ts; + int ret; + + ret =3D pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + guard(mutex)(&st->lock); + inv_sensors_timestamp_reset(ts); + + return 0; +} + +/* + * Update_scan_mode callback is turning sensors on and setting data FIFO e= nable + * bits. + */ +static int inv_icm45600_buffer_postenable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st =3D iio_device_get_drvdata(indio_dev); + unsigned int val; + int ret; + + guard(mutex)(&st->lock); + + /* Exit if FIFO is already on. */ + if (st->fifo.on) { + /* Increase FIFO on counter. */ + st->fifo.on++; + return 0; + } + + /* Flush all FIFO data. */ + ret =3D regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2, + INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH); + if (ret) + return ret; + + /* Set FIFO threshold and full interrupt. */ + ret =3D regmap_set_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0, + INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN | + INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN); + if (ret) + return ret; + + /* Set FIFO in streaming mode. */ + val =3D FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_STREAM); + ret =3D regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + + /* Enable writing sensor data to FIFO. */ + ret =3D regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + if (ret) + return ret; + + /* Increase FIFO on counter. */ + st->fifo.on++; + return 0; +} + +static int inv_icm45600_buffer_predisable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st =3D iio_device_get_drvdata(indio_dev); + unsigned int val; + int ret; + + guard(mutex)(&st->lock); + + /* Exit if there are several sensors using the FIFO. */ + if (st->fifo.on > 1) { + /* decrease FIFO on counter */ + st->fifo.on--; + return 0; + } + + /* Disable writing sensor data to FIFO. */ + ret =3D regmap_clear_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + if (ret) + return ret; + + /* Set FIFO in bypass mode. */ + val =3D FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS); + ret =3D regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + + /* Disable FIFO threshold and full interrupt. */ + ret =3D regmap_clear_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0, + INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN | + INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN); + if (ret) + return ret; + + /* Flush all FIFO data. */ + ret =3D regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2, + INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH); + if (ret) + return ret; + + /* Decrease FIFO on counter. */ + st->fifo.on--; + return 0; +} + +static int _inv_icm45600_buffer_postdisable(struct inv_icm45600_state *st, + unsigned int sensor, unsigned int *watermark, + unsigned int *sleep) +{ + struct inv_icm45600_sensor_conf conf =3D INV_ICM45600_SENSOR_CONF_KEEP_VA= LUES; + int ret; + + ret =3D inv_icm45600_buffer_set_fifo_en(st, st->fifo.en & ~sensor); + if (ret) + return ret; + + *watermark =3D 0; + ret =3D inv_icm45600_buffer_update_watermark(st); + if (ret) + return ret; + + conf.mode =3D INV_ICM45600_SENSOR_MODE_OFF; + if (sensor =3D=3D INV_ICM45600_SENSOR_GYRO) + ret =3D inv_icm45600_set_gyro_conf(st, &conf, sleep); + else + ret =3D inv_icm45600_set_accel_conf(st, &conf, sleep); + + return ret; +} + +static int inv_icm45600_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st =3D iio_device_get_drvdata(indio_dev); + struct device *dev =3D regmap_get_device(st->map); + unsigned int sensor; + unsigned int *watermark; + unsigned int sleep; + int ret; + + if (indio_dev =3D=3D st->indio_gyro) { + sensor =3D INV_ICM45600_SENSOR_GYRO; + watermark =3D &st->fifo.watermark.gyro; + } else if (indio_dev =3D=3D st->indio_accel) { + sensor =3D INV_ICM45600_SENSOR_ACCEL; + watermark =3D &st->fifo.watermark.accel; + } else { + return -EINVAL; + } + + scoped_guard(mutex, &st->lock) + ret =3D _inv_icm45600_buffer_postdisable(st, sensor, watermark, &sleep); + + /* Sleep required time. */ + if (sleep) + msleep(sleep); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +const struct iio_buffer_setup_ops inv_icm45600_buffer_ops =3D { + .preenable =3D inv_icm45600_buffer_preenable, + .postenable =3D inv_icm45600_buffer_postenable, + .predisable =3D inv_icm45600_buffer_predisable, + .postdisable =3D inv_icm45600_buffer_postdisable, +}; + +int inv_icm45600_buffer_fifo_read(struct inv_icm45600_state *st) +{ + const ssize_t packet_size =3D sizeof(struct inv_icm45600_fifo_2sensors_pa= cket); + __le16 *raw_fifo_count; + size_t fifo_nb, i; + ssize_t size; + const struct inv_icm45600_fifo_sensor_data *accel, *gyro; + const __le16 *timestamp; + const s8 *temp; + unsigned int odr; + int ret; + + /* Reset all samples counters. */ + st->fifo.count =3D 0; + st->fifo.nb.gyro =3D 0; + st->fifo.nb.accel =3D 0; + st->fifo.nb.total =3D 0; + + /* Read FIFO count value. */ + raw_fifo_count =3D &st->buffer.u16; + ret =3D regmap_bulk_read(st->map, INV_ICM45600_REG_FIFO_COUNT, + raw_fifo_count, sizeof(*raw_fifo_count)); + if (ret) + return ret; + fifo_nb =3D le16_to_cpup(raw_fifo_count); + + /* Check and limit number of samples if requested. */ + if (fifo_nb =3D=3D 0) + return 0; + + /* Try to read all FIFO data in internal buffer. */ + st->fifo.count =3D fifo_nb * packet_size; + ret =3D regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA, + st->fifo.data, st->fifo.count); + if (ret =3D=3D -ENOTSUPP || ret =3D=3D -EFBIG) { + /* Read full fifo is not supported, read samples one by one. */ + ret =3D 0; + for (i =3D 0; i < st->fifo.count && ret =3D=3D 0; i +=3D packet_size) + ret =3D regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA, + &st->fifo.data[i], packet_size); + } + if (ret) + return ret; + + for (i =3D 0; i < st->fifo.count; i +=3D size) { + size =3D inv_icm45600_fifo_decode_packet(&st->fifo.data[i], &accel, &gyr= o, + &temp, ×tamp, &odr); + if (size <=3D 0) + break; + if (gyro !=3D NULL && inv_icm45600_fifo_is_data_valid(gyro)) + st->fifo.nb.gyro++; + if (accel !=3D NULL && inv_icm45600_fifo_is_data_valid(accel)) + st->fifo.nb.accel++; + st->fifo.nb.total++; + } + + return 0; +} + +int inv_icm45600_buffer_init(struct inv_icm45600_state *st) +{ + int ret; + unsigned int val; + + st->fifo.watermark.eff_gyro =3D 1; + st->fifo.watermark.eff_accel =3D 1; + + /* Disable all FIFO EN bits. */ + ret =3D regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG3, 0); + if (ret) + return ret; + + /* Disable FIFO and set depth. */ + val =3D FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS); + val |=3D INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MAX; + ret =3D regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG0, val); + if (ret) + return ret; + + /* Enable only timestamp in fifo, disable compression. */ + ret =3D regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG4, + INV_ICM45600_FIFO_CONFIG4_TMST_FSYNC_EN); + if (ret) + return ret; + + /* Enable FIFO continuous watermark interrupt. */ + return regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2, + INV_ICM45600_REG_FIFO_CONFIG2_WM_GT_TH); +} diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.h b/drivers/i= io/imu/inv_icm45600/inv_icm45600_buffer.h new file mode 100644 index 0000000000000000000000000000000000000000..da463461b5f2708014126f868fa= 6008db0520a4e --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.h @@ -0,0 +1,98 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* Copyright (C) 2025 Invensense, Inc. */ + +#ifndef INV_ICM45600_BUFFER_H_ +#define INV_ICM45600_BUFFER_H_ + +#include +#include +#include + +struct inv_icm45600_state; + +#define INV_ICM45600_SENSOR_GYRO BIT(0) +#define INV_ICM45600_SENSOR_ACCEL BIT(1) +#define INV_ICM45600_SENSOR_TEMP BIT(2) + +/** + * struct inv_icm45600_fifo - FIFO state variables + * @on: reference counter for FIFO on. + * @en: bits field of INV_ICM45600_SENSOR_* for FIFO EN bits. + * @period: FIFO internal period. + * @watermark: watermark configuration values for accel and gyro. + * @count: number of bytes in the FIFO data buffer. + * @nb: gyro, accel and total samples in the FIFO data buffer. + * @data: FIFO data buffer aligned for DMA (8kB) + */ +struct inv_icm45600_fifo { + unsigned int on; + unsigned int en; + u32 period; + struct { + unsigned int gyro; + unsigned int accel; + unsigned int eff_gyro; + unsigned int eff_accel; + } watermark; + size_t count; + struct { + size_t gyro; + size_t accel; + size_t total; + } nb; + u8 *data; +}; + +/* FIFO data packet */ +struct inv_icm45600_fifo_sensor_data { + __le16 x; + __le16 y; + __le16 z; +} __packed; +#define INV_ICM45600_DATA_INVALID S16_MIN + +static inline s16 inv_icm45600_fifo_get_sensor_data(__le16 d) +{ + return le16_to_cpu(d); +} + +static inline bool +inv_icm45600_fifo_is_data_valid(const struct inv_icm45600_fifo_sensor_data= *s) +{ + s16 x, y, z; + + x =3D inv_icm45600_fifo_get_sensor_data(s->x); + y =3D inv_icm45600_fifo_get_sensor_data(s->y); + z =3D inv_icm45600_fifo_get_sensor_data(s->z); + + if (x =3D=3D INV_ICM45600_DATA_INVALID && + y =3D=3D INV_ICM45600_DATA_INVALID && + z =3D=3D INV_ICM45600_DATA_INVALID) + return false; + + return true; +} + +ssize_t inv_icm45600_fifo_decode_packet(const void *packet, + const struct inv_icm45600_fifo_sensor_data **accel, + const struct inv_icm45600_fifo_sensor_data **gyro, + const s8 **temp, + const __le16 **timestamp, unsigned int *odr); + +extern const struct iio_buffer_setup_ops inv_icm45600_buffer_ops; + +int inv_icm45600_buffer_init(struct inv_icm45600_state *st); + +void inv_icm45600_buffer_update_fifo_period(struct inv_icm45600_state *st); + +int inv_icm45600_buffer_set_fifo_en(struct inv_icm45600_state *st, + unsigned int fifo_en); + +int inv_icm45600_buffer_update_watermark(struct inv_icm45600_state *st); + +int inv_icm45600_buffer_fifo_read(struct inv_icm45600_state *st); + +int inv_icm45600_buffer_hwfifo_flush(struct inv_icm45600_state *st, + unsigned int count); + +#endif diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_core.c b/drivers/iio= /imu/inv_icm45600/inv_icm45600_core.c index 4b8f3dad8984cfa6642b1b4c94acbb0674084f3f..1657daf7c2d0445d6d10658f7f5= b0f16f1623d69 100644 --- a/drivers/iio/imu/inv_icm45600/inv_icm45600_core.c +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_core.c @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include #include @@ -15,6 +17,7 @@ #include #include =20 +#include "inv_icm45600_buffer.h" #include "inv_icm45600.h" =20 static int inv_icm45600_ireg_read(struct regmap *map, unsigned int reg, @@ -510,6 +513,94 @@ static int inv_icm45600_setup(struct inv_icm45600_stat= e *st, return inv_icm45600_set_conf(st, chip_info->conf); } =20 +static irqreturn_t inv_icm45600_irq_timestamp(int irq, void *_data) +{ + struct inv_icm45600_state *st =3D _data; + + st->timestamp.gyro =3D iio_get_time_ns(st->indio_gyro); + st->timestamp.accel =3D iio_get_time_ns(st->indio_accel); + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t inv_icm45600_irq_handler(int irq, void *_data) +{ + struct inv_icm45600_state *st =3D _data; + struct device *dev =3D regmap_get_device(st->map); + unsigned int mask, status; + int ret; + + guard(mutex)(&st->lock); + + ret =3D regmap_read(st->map, INV_ICM45600_REG_INT_STATUS, &status); + if (ret) + return IRQ_HANDLED; + + /* Read the FIFO data. */ + mask =3D INV_ICM45600_INT_STATUS_FIFO_THS | INV_ICM45600_INT_STATUS_FIFO_= FULL; + if (status & mask) { + ret =3D inv_icm45600_buffer_fifo_read(st); + if (ret) { + dev_err(dev, "FIFO read error %d\n", ret); + return IRQ_HANDLED; + } + } + + /* FIFO full warning. */ + if (status & INV_ICM45600_INT_STATUS_FIFO_FULL) + dev_warn(dev, "FIFO full possible data lost!\n"); + + return IRQ_HANDLED; +} + +/** + * inv_icm45600_irq_init() - initialize int pin and interrupt handler + * @st: driver internal state + * @irq: irq number + * @irq_type: irq trigger type + * @open_drain: true if irq is open drain, false for push-pull + * + * Returns 0 on success, a negative error code otherwise. + */ +static int inv_icm45600_irq_init(struct inv_icm45600_state *st, int irq, + int irq_type, bool open_drain) +{ + struct device *dev =3D regmap_get_device(st->map); + unsigned int val; + int ret; + + /* Configure INT1 interrupt: default is active low on edge. */ + switch (irq_type) { + case IRQF_TRIGGER_RISING: + case IRQF_TRIGGER_HIGH: + val =3D INV_ICM45600_INT1_CONFIG2_ACTIVE_HIGH; + break; + default: + val =3D INV_ICM45600_INT1_CONFIG2_ACTIVE_LOW; + break; + } + + switch (irq_type) { + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_HIGH: + val |=3D INV_ICM45600_INT1_CONFIG2_LATCHED; + break; + default: + break; + } + + if (!open_drain) + val |=3D INV_ICM45600_INT1_CONFIG2_PUSH_PULL; + + ret =3D regmap_write(st->map, INV_ICM45600_REG_INT1_CONFIG2, val); + if (ret) + return ret; + + return devm_request_threaded_irq(dev, irq, inv_icm45600_irq_timestamp, + inv_icm45600_irq_handler, irq_type | IRQF_ONESHOT, + "inv_icm45600", st); +} + static int inv_icm45600_timestamp_setup(struct inv_icm45600_state *st) { /* Enable timestamps. */ @@ -549,8 +640,20 @@ int inv_icm45600_core_probe(struct regmap *regmap, con= st struct inv_icm45600_chi struct fwnode_handle *fwnode; struct inv_icm45600_state *st; struct regmap *regmap_custom; + int irq, irq_type; + bool open_drain; int ret; =20 + /* Get INT1 only supported interrupt. */ + fwnode =3D dev_fwnode(dev); + irq =3D fwnode_irq_get_byname(fwnode, "int1"); + if (irq < 0) + return dev_err_probe(dev, irq, "Missing int1 interrupt\n"); + + irq_type =3D irq_get_trigger_type(irq); + + open_drain =3D device_property_read_bool(dev, "drive-open-drain"); + regmap_custom =3D devm_regmap_init(dev, &inv_icm45600_regmap_bus, regmap, &inv_icm45600_regmap_config); if (IS_ERR(regmap_custom)) @@ -562,6 +665,10 @@ int inv_icm45600_core_probe(struct regmap *regmap, con= st struct inv_icm45600_chi =20 dev_set_drvdata(dev, st); =20 + st->fifo.data =3D devm_kzalloc(dev, 8192, GFP_KERNEL); + if (!st->fifo.data) + return dev_err_probe(dev, -ENOMEM, "Cannot allocate fifo memory\n"); + ret =3D devm_mutex_init(dev, &st->lock); if (ret) return ret; @@ -599,6 +706,14 @@ int inv_icm45600_core_probe(struct regmap *regmap, con= st struct inv_icm45600_chi if (ret) return ret; =20 + ret =3D inv_icm45600_buffer_init(st); + if (ret) + return ret; + + ret =3D inv_icm45600_irq_init(st, irq, irq_type, open_drain); + if (ret) + return ret; + /* Setup runtime power management. */ ret =3D devm_pm_runtime_set_active_enabled(dev); if (ret) @@ -623,6 +738,23 @@ static int inv_icm45600_suspend(struct device *dev) int ret; =20 scoped_guard(mutex, &st->lock) { + /* Disable FIFO data streaming. */ + if (st->fifo.on) { + unsigned int val; + + /* Clear FIFO_CONFIG3_IF_EN before changing the FIFO configuration */ + ret =3D regmap_clear_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + if (ret) + return ret; + val =3D FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS); + ret =3D regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + } + /* Save sensors states */ st->suspended.gyro =3D st->conf.gyro.mode; st->suspended.accel =3D st->conf.accel.mode; @@ -644,10 +776,32 @@ static int inv_icm45600_resume(struct device *dev) if (ret) return ret; =20 - scoped_guard(mutex, &st->lock) + scoped_guard(mutex, &st->lock) { /* Restore sensors state. */ ret =3D inv_icm45600_set_pwr_mgmt0(st, st->suspended.gyro, - st->suspended.accel, NULL); + st->suspended.accel, NULL); + if (ret) + return ret; + + /* Restore FIFO data streaming. */ + if (st->fifo.on) { + struct inv_icm45600_sensor_state *gyro_st =3D iio_priv(st->indio_gyro); + struct inv_icm45600_sensor_state *accel_st =3D iio_priv(st->indio_accel= ); + unsigned int val; + + inv_sensors_timestamp_reset(&gyro_st->ts); + inv_sensors_timestamp_reset(&accel_st->ts); + val =3D FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_STREAM); + ret =3D regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + /* FIFO_CONFIG3_IF_EN must only be set at end of FIFO the configuration= */ + ret =3D regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + } + } =20 return ret; } --=20 2.34.1