From nobody Tue Apr 7 13:47:53 2026 Received: from mail-wr1-f66.google.com (mail-wr1-f66.google.com [209.85.221.66]) (using TLSv1.2 with cipher ECDHE-RSA-AES128-GCM-SHA256 (128/128 bits)) (No client certificate requested) by smtp.subspace.kernel.org (Postfix) with ESMTPS id 0C7AD38F93B for ; Wed, 25 Feb 2026 10:18:17 +0000 (UTC) Authentication-Results: smtp.subspace.kernel.org; arc=none smtp.client-ip=209.85.221.66 ARC-Seal: i=1; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1772014701; cv=none; b=OYIHqIW07ZxQIHqKWrCuSR9ZJ0KhgZgWvIQ6QAAZmMM67EsWCxCt4QIhXiVGsYsx+FtYBCaCtAcfJHeK2njX4AMIxE4pr70+4zZccaG7K0KLjjLMIr8Yor6HttPYorud0o0696OmWGHyzEwohUqedQxtW0H1uDQX7M2KF2Q/BWs= ARC-Message-Signature: i=1; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1772014701; c=relaxed/simple; bh=7Xc5iR2r3mSsEKkkNfZiPqHLw/poBRd/FgOqBlPCoYM=; h=From:To:Subject:Date:Message-Id:In-Reply-To:References: MIME-Version; b=Z9GN7RVXLSFxpuX3PZDDW9R7My2lL2l+q86LwGIqWD0ItKnTauR2ZpKSKzyux2ejPfMDfrW7+xfqN/DiXC7RXUirhpBRlC9o5fscf8Fz2B8/hICk0HNiT79VvdyXZWeuhs5L2FazLAoxgtkV1JdbM/pdYrWAoez6TqoViqMfAos= ARC-Authentication-Results: i=1; smtp.subspace.kernel.org; dmarc=none (p=none dis=none) header.from=baylibre.com; spf=pass smtp.mailfrom=baylibre.com; dkim=pass (2048-bit key) header.d=baylibre-com.20230601.gappssmtp.com header.i=@baylibre-com.20230601.gappssmtp.com header.b=GT7X5QXW; arc=none smtp.client-ip=209.85.221.66 Authentication-Results: smtp.subspace.kernel.org; dmarc=none (p=none dis=none) header.from=baylibre.com Authentication-Results: smtp.subspace.kernel.org; spf=pass smtp.mailfrom=baylibre.com Authentication-Results: smtp.subspace.kernel.org; dkim=pass (2048-bit key) header.d=baylibre-com.20230601.gappssmtp.com header.i=@baylibre-com.20230601.gappssmtp.com header.b="GT7X5QXW" Received: by mail-wr1-f66.google.com with SMTP id ffacd0b85a97d-4376c0bffc1so4871412f8f.0 for ; Wed, 25 Feb 2026 02:18:17 -0800 (PST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=baylibre-com.20230601.gappssmtp.com; s=20230601; t=1772014696; x=1772619496; darn=vger.kernel.org; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:from:to:cc:subject:date:message-id :reply-to; bh=28QWOw1vn6hlB7Z/6wxJ6t4X6YR/ayXcRdrKzUQeF9o=; b=GT7X5QXWa9aaRXkCw6DlWWuRH949YDLVBb4wN5K8sZ3NXKhcVlDb/S9qKOYnGSxMSl 6+19mu2N0q5SdEykI3MJOV6RKtHNRB+kECApy3c9+Dv7FxkZYxR+Ed3dV7Cn3A7I4iLV +VrnQ2F6LAocyJtxDywZZJyN1d1Nr8X3tnX/wAEBY8HoWLKr/YD6TtdtTZb4yohRYcuO yMSfORIDhVoZgCINwzJvCMz4+AZCtspOCHJys9hcs0cJ8JWYZJPaVGitVhsWtbEtWBCZ fCxs10BP28F4jX6iWCXt6Wd9CwArfH3ASQ9wGn15EKtJTe0eVtO3vHzG84gvDHYCsIQ/ v6fA== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20230601; t=1772014696; x=1772619496; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:to:from:x-gm-gg:x-gm-message-state:from:to :cc:subject:date:message-id:reply-to; bh=28QWOw1vn6hlB7Z/6wxJ6t4X6YR/ayXcRdrKzUQeF9o=; b=HhJt3ROkkmZPXlBEwAVD5m5hubL/YpnzIWP5nQZvUVSzSaHTLmJzhGjSaRSxIL886E DtUz1G4mGwFjjPdHR4+OisATHCYfheLVs9ZAogNKmpzX/aH+KTRO5fQQI1oaMvPvdR// heDDvef35WseoHcloEKWRqSixtIOMAdYN3C3HnUt5Sm/WkoqpNTDqGU9/0/d3j6hwYMb ScIyOm9JnszPJrgnYrEDHiteTEYLrdBrJQqLAr7SS1VGuoyinF2BbYSCObbn9IBUn868 moJBySG0MdSzkyyExsbfEPJ8bVskn9ACszft+tSu+BPwydwoI3/N3ogVxGX3ONj0xYD0 ojjg== X-Forwarded-Encrypted: i=1; AJvYcCXihIsB2WJm69h1A7N/DtJAxfES4oJb/FxHs5TRS93+dm0jUZS2oE9EZSQPBrMblAZEqHvSSy3qpeb0BOg=@vger.kernel.org X-Gm-Message-State: AOJu0YwcoPJLS4ROhA0+9UaFg5N9r2/aqsFWjFW7SUO6edWyAzPNaSc5 d8zZUTUEq7BY/9ka4Xqqd9IRBfQGY6NiyT7qxNy6tVByD5140a7RmvgaZ4qdhvEBVl0= X-Gm-Gg: ATEYQzyczui0JDPoML8prt2badycfy/8wEGQOniBEN5sFKlFn9H3kmNz3f+lTk7LV0F 3ADOJFzglp9yNcpfPsWqoi3azgVyYAFR/OUX3U7TWLTwBghDatvzPiGow0K4b/MWF6OxxYROUmt rhz6PiDADrzHRYPfzQnM++ht/1hpbu6T6mA3B6R/258VufOZLKoHyd1JSN2+TloKRigiBXCC6Ns wh+rtoJqb3u7br+E9q+tlQqEtA9cvZEHghJXZ69l2My4MeY+TUwhF08cZyKF9socdiVSG9/2Uy8 927QRjdk6GNWiPNVkNbxFwW0EnjcL7fx5onVmzfqJ4QnyuzabRSCmAZuzyZKo9NBakyer8a3KB4 KHKROBMLbe5/8uzaXNFjX9PG2r9wdG4UPXfFDsoPbjWjaaPHi7NzwHOEnvEdBB/iQwPigh3YsFl uW2rUu X-Received: by 2002:a05:6000:25c3:b0:439:936b:bfe7 with SMTP id ffacd0b85a97d-439936bc280mr419489f8f.45.1772014696210; Wed, 25 Feb 2026 02:18:16 -0800 (PST) Received: from localhost ([151.35.220.155]) by smtp.gmail.com with ESMTPSA id ffacd0b85a97d-43987f3ed03sm12516439f8f.16.2026.02.25.02.18.14 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Wed, 25 Feb 2026 02:18:15 -0800 (PST) From: Francesco Lavra To: Jonathan Cameron , David Lechner , =?UTF-8?q?Nuno=20S=C3=A1?= , Andy Shevchenko , Jonathan Corbet , Shuah Khan , Lorenzo Bianconi , linux-iio@vger.kernel.org, linux-doc@vger.kernel.org, linux-kernel@vger.kernel.org Subject: [PATCH v6 7/7] iio: imu: st_lsm6dsx: Add support for rotation sensor Date: Wed, 25 Feb 2026 11:18:13 +0100 Message-Id: <20260225101814.2368431-1-flavra@baylibre.com> X-Mailer: git-send-email 2.39.5 In-Reply-To: <20260225100421.2366864-1-flavra@baylibre.com> References: <20260225100421.2366864-1-flavra@baylibre.com> Precedence: bulk X-Mailing-List: linux-kernel@vger.kernel.org List-Id: List-Subscribe: List-Unsubscribe: MIME-Version: 1.0 X-Developer-Signature: v=1; a=openpgp-sha256; l=17523; i=flavra@baylibre.com; h=from:subject; bh=7Xc5iR2r3mSsEKkkNfZiPqHLw/poBRd/FgOqBlPCoYM=; b=owEB7QES/pANAwAKAe3xO3POlDZfAcsmYgBpnsxlPLZSbXdNj1LvL64OwyXTo2r/OxPgCDMri bvowPGzMfiJAbMEAAEKAB0WIQSGV4VPlTvcox7DFObt8TtzzpQ2XwUCaZ7MZQAKCRDt8TtzzpQ2 X+NWDACW7lTBTCvvFwKhCA2RlAr7pb8OFZ/y5KGWBv03crK7V2IlVjAyF2KrO6KdELlPX54rAg1 FoAURtaErUREbtM3rbKGdZK+1jmrZzbrJ1QYF3NbRDOUY1/AMhcFkUEMFXBYrDxZPywK6/xjtZv U9gXByh72FyHyL17h/oTZFmjJms9g3M+uRrNbn3Kr1Y/F0cu+oDbbZEbdmFsAasnw4P49ZUDILR 0wD5KxZypndfG7o9KNxSDUZxlRY68iTCV9YcZFC9kJ9ADXrCEPQOAZvE9aED1Z6dXYDWrtmLnT1 pk4jBl7mKbcPAhUr7T8dcA+G+/HJS2R/9z5aa3Q1tzrI1gQUdrPsxA/Wctqj4QnA9QSTU0HJql5 RF8/AJ6+SxjXcC61weg8FT46jcnxB7/M3m44vJOS0i7CxxaxG+JtOJtk1WysOI/nlRMgwDQ0wZH YQtji/SZTRgck6MCZsA4yQbCNzueavHP+U5ru+1IjYw7++dB/OtXFLNwd4bZotR5ZpnpI= X-Developer-Key: i=flavra@baylibre.com; a=openpgp; fpr=8657854F953BDCA31EC314E6EDF13B73CE94365F Content-Transfer-Encoding: quoted-printable Content-Type: text/plain; charset="utf-8" Some IMU chips in the LSM6DSX family have sensor fusion features that combine data from the accelerometer and gyroscope. One of these features generates rotation vector data and makes it available in the hardware FIFO as a quaternion (more specifically, the X, Y and Z components of the quaternion vector, expressed as 16-bit half-precision floating-point numbers). Add support for a new sensor instance that allows receiving sensor fusion data, by defining a new struct st_lsm6dsx_sf_settings (which contains chip-specific details for the sensor fusion functionality), and adding this struct as a new field in struct st_lsm6dsx_settings. In st_lsm6dsx_core.c, populate this new struct for the LSM6DSV and LSM6DSV16X chips, and add the logic to initialize an additional IIO device if this struct is populated for the hardware type being probed. Note: a new IIO device is being defined (as opposed to adding channels to an existing device) because the rate at which sensor fusion data is generated may not match the data rate from any of the existing devices. Tested on LSMDSV16X. Signed-off-by: Francesco Lavra Acked-by: Lorenzo Bianconi --- Documentation/iio/index.rst | 1 + Documentation/iio/st_lsm6dsx.rst | 44 ++++ drivers/iio/imu/st_lsm6dsx/Makefile | 2 +- drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 26 +- .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 9 +- drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 58 +++++ .../iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c | 235 ++++++++++++++++++ 7 files changed, 368 insertions(+), 7 deletions(-) create mode 100644 Documentation/iio/st_lsm6dsx.rst create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c diff --git a/Documentation/iio/index.rst b/Documentation/iio/index.rst index ba3e609c6a13..4e2ebe64f97f 100644 --- a/Documentation/iio/index.rst +++ b/Documentation/iio/index.rst @@ -39,3 +39,4 @@ Industrial I/O Kernel Drivers bno055 ep93xx_adc opt4060 + st_lsm6dsx diff --git a/Documentation/iio/st_lsm6dsx.rst b/Documentation/iio/st_lsm6ds= x.rst new file mode 100644 index 000000000000..7f11a0b7e5c0 --- /dev/null +++ b/Documentation/iio/st_lsm6dsx.rst @@ -0,0 +1,44 @@ +.. SPDX-License-Identifier: GPL-2.0-only + +=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D +ST LSM6DSX driver +=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D + +Device driver for STMicroelectronics LSM6DSx, LSM9DS1, ASM330, and ISM330 = series +of 6-axis inertial measurement units. The core module is called ``st_lsm6d= sx``, +and the transport-specific modules are called ``st_lsm6dsx_i2c``, +``st_lsm6dsx_spi``, and ``st_lsm6dsx_i3c``. + +IIO devices +=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D + +This driver instantiates multiple IIO devices: + +* accelerometer (IIO_ACCEL channel) +* gyroscope (IIO_ANGL_VEL channel) +* (optionally) magnetometer (IIO_MAGN channel), if the device has a second= ary + I2C interface connected to a slave sensor device (sensor hub functionali= ty) +* (optionally) sensor fusion (IIO_CUSTOM channel), which combines accelera= tion + and angular velocity data + +Sensor Fusion +------------- + +Some chips supported by this driver implement an internal algorithm that t= akes +input data from the accelerometer and gyroscope, and calculates the device +orientation in 3D space, which is then made available in the hardware FIFO. +Orientation is expressed in the form of a 4-dimensional quaternion vector,= whose +components are typically represented in an array as ``[x, y, z, w]``. +The sensor device outputs the ``[x, y, z]`` components of the quaternion, +expressed as half-precision (16-bit) floating-point numbers. + +The ``z`` component is not output by the device, but its value can be deri= ved +from the rest of the data, due to the following constraints: + +* the quaternion vector is normalized, i.e. :math:`x^2 + y^2 + z^2 + w^2 = =3D 1` +* the rotation angle :math:`\theta` remains within + :math:`[-180^\circ, 180^\circ]`, i.e. the ``w`` component is non-negativ= e: + :math:`w =3D \cos(\theta/2) \geq 0` + +These constraints allow the ``w`` value to be calculated from the other +components: :math:`w =3D \sqrt{1 - (x^2 + y^2 + z^2)}`. diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6d= sx/Makefile index 57cbcd67d64f..19a488254de3 100644 --- a/drivers/iio/imu/st_lsm6dsx/Makefile +++ b/drivers/iio/imu/st_lsm6dsx/Makefile @@ -1,6 +1,6 @@ # SPDX-License-Identifier: GPL-2.0-only st_lsm6dsx-y :=3D st_lsm6dsx_core.o st_lsm6dsx_buffer.o \ - st_lsm6dsx_shub.o + st_lsm6dsx_shub.o st_lsm6dsx_fusion.o =20 obj-$(CONFIG_IIO_ST_LSM6DSX) +=3D st_lsm6dsx.o obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) +=3D st_lsm6dsx_i2c.o diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_l= sm6dsx/st_lsm6dsx.h index 39d23b9e677c..2cd849ef0c9a 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -294,6 +294,7 @@ enum st_lsm6dsx_sensor_id { ST_LSM6DSX_ID_EXT0, ST_LSM6DSX_ID_EXT1, ST_LSM6DSX_ID_EXT2, + ST_LSM6DSX_ID_SF, ST_LSM6DSX_ID_MAX }; =20 @@ -301,6 +302,15 @@ enum st_lsm6dsx_ext_sensor_id { ST_LSM6DSX_ID_MAGN, }; =20 +struct st_lsm6dsx_sf_settings { + const struct iio_chan_spec *chan; + int chan_len; + struct st_lsm6dsx_odr_table_entry odr_table; + struct st_lsm6dsx_reg fifo_enable; + struct st_lsm6dsx_reg page_mux; + struct st_lsm6dsx_reg enable; +}; + /** * struct st_lsm6dsx_ext_dev_settings - i2c controller slave settings * @i2c_addr: I2c slave address list. @@ -388,6 +398,7 @@ struct st_lsm6dsx_settings { struct st_lsm6dsx_hw_ts_settings ts_settings; struct st_lsm6dsx_shub_settings shub_settings; struct st_lsm6dsx_event_settings event_settings; + struct st_lsm6dsx_sf_settings sf_settings; }; =20 enum st_lsm6dsx_fifo_mode { @@ -510,6 +521,9 @@ int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sens= or, u32 odr, u8 *val); int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name); int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enab= le); int st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data, int le= n); +int st_lsm6dsx_sf_probe(struct st_lsm6dsx_hw *hw, const char *name); +int st_lsm6dsx_sf_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable= ); +int st_lsm6dsx_sf_set_odr(struct st_lsm6dsx_sensor *sensor, bool enable); int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable); =20 static inline int @@ -564,12 +578,14 @@ st_lsm6dsx_get_mount_matrix(const struct iio_dev *iio= _dev, static inline int st_lsm6dsx_device_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable) { - if (sensor->id =3D=3D ST_LSM6DSX_ID_EXT0 || - sensor->id =3D=3D ST_LSM6DSX_ID_EXT1 || - sensor->id =3D=3D ST_LSM6DSX_ID_EXT2) + switch (sensor->id) { + case ST_LSM6DSX_ID_EXT0 ... ST_LSM6DSX_ID_EXT2: return st_lsm6dsx_shub_set_enable(sensor, enable); - - return st_lsm6dsx_sensor_set_enable(sensor, enable); + case ST_LSM6DSX_ID_SF: + return st_lsm6dsx_sf_set_enable(sensor, enable); + default: + return st_lsm6dsx_sensor_set_enable(sensor, enable); + } } =20 static const diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/i= mu/st_lsm6dsx/st_lsm6dsx_buffer.c index a6ee2da5a06c..6b9371423f2d 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -88,6 +88,7 @@ enum st_lsm6dsx_fifo_tag { ST_LSM6DSX_EXT0_TAG =3D 0x0f, ST_LSM6DSX_EXT1_TAG =3D 0x10, ST_LSM6DSX_EXT2_TAG =3D 0x11, + ST_LSM6DSX_ROT_TAG =3D 0x13, }; =20 static const @@ -226,8 +227,11 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_s= ensor *sensor, u8 data; =20 /* Only internal sensors have a FIFO ODR configuration register. */ - if (sensor->id >=3D ARRAY_SIZE(hw->settings->batch)) + if (sensor->id >=3D ARRAY_SIZE(hw->settings->batch)) { + if (sensor->id =3D=3D ST_LSM6DSX_ID_SF) + return st_lsm6dsx_sf_set_odr(sensor, enable); return 0; + } =20 batch_reg =3D &hw->settings->batch[sensor->id]; if (batch_reg->addr) { @@ -578,6 +582,9 @@ st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u= 8 tag, case ST_LSM6DSX_EXT2_TAG: iio_dev =3D hw->iio_devs[ST_LSM6DSX_ID_EXT2]; break; + case ST_LSM6DSX_ROT_TAG: + iio_dev =3D hw->iio_devs[ST_LSM6DSX_ID_SF]; + break; default: return -EINVAL; } diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu= /st_lsm6dsx/st_lsm6dsx_core.c index 450cb5b47346..5b336b62eb60 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -94,6 +94,24 @@ =20 #define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f =20 +/* Raw values from the IMU are 16-bit half-precision floating-point number= s. */ +#define ST_LSM6DSX_CHANNEL_ROT \ +{ \ + .type =3D IIO_CUSTOM, \ + .modified =3D 1, \ + .channel2 =3D IIO_MOD_QUATERNION, \ + .info_mask_shared_by_all =3D BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index =3D 0, \ + .scan_type =3D { \ + .format =3D 'f', \ + .realbits =3D 16, \ + .storagebits =3D 16, \ + .endianness =3D IIO_LE, \ + .repeat =3D 3, \ + }, \ + .ext_info =3D st_lsm6dsx_ext_info, \ +} + static const struct iio_event_spec st_lsm6dsx_ev_motion[] =3D { { .type =3D IIO_EV_TYPE_THRESH, @@ -153,6 +171,11 @@ static const struct iio_chan_spec st_lsm6ds0_gyro_chan= nels[] =3D { IIO_CHAN_SOFT_TIMESTAMP(3), }; =20 +static const struct iio_chan_spec st_lsm6dsx_sf_channels[] =3D { + ST_LSM6DSX_CHANNEL_ROT, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] =3D { { .reset =3D { @@ -1492,6 +1515,35 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_s= ensor_settings[] =3D { }, }, }, + .sf_settings =3D { + .chan =3D st_lsm6dsx_sf_channels, + .chan_len =3D ARRAY_SIZE(st_lsm6dsx_sf_channels), + .odr_table =3D { + .reg =3D { + .addr =3D 0x5e, + .mask =3D GENMASK(5, 3), + }, + .odr_avl[0] =3D { 15000, 0x00 }, + .odr_avl[1] =3D { 30000, 0x01 }, + .odr_avl[2] =3D { 60000, 0x02 }, + .odr_avl[3] =3D { 120000, 0x03 }, + .odr_avl[4] =3D { 240000, 0x04 }, + .odr_avl[5] =3D { 480000, 0x05 }, + .odr_len =3D 6, + }, + .fifo_enable =3D { + .addr =3D 0x44, + .mask =3D BIT(1), + }, + .page_mux =3D { + .addr =3D 0x01, + .mask =3D BIT(7), + }, + .enable =3D { + .addr =3D 0x04, + .mask =3D BIT(1), + }, + }, }, { .reset =3D { @@ -2899,6 +2951,12 @@ int st_lsm6dsx_probe(struct device *dev, int irq, in= t hw_id, return err; } =20 + if (hw->settings->sf_settings.chan) { + err =3D st_lsm6dsx_sf_probe(hw, name); + if (err) + return err; + } + if (hw->irq > 0) { err =3D st_lsm6dsx_irq_setup(hw); if (err < 0) diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c b/drivers/iio/i= mu/st_lsm6dsx/st_lsm6dsx_fusion.c new file mode 100644 index 000000000000..7033aaeba13e --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c @@ -0,0 +1,235 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsx IMU sensor fusion + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsx.h" + +static int +st_lsm6dsx_sf_get_odr_val(const struct st_lsm6dsx_sf_settings *settings, + u32 odr, u8 *val) +{ + int i; + + for (i =3D 0; i < settings->odr_table.odr_len; i++) { + if (settings->odr_table.odr_avl[i].milli_hz =3D=3D odr) + break; + } + if (i =3D=3D settings->odr_table.odr_len) + return -EINVAL; + + *val =3D settings->odr_table.odr_avl[i].val; + return 0; +} + +/** + * st_lsm6dsx_sf_page_enable - Enable access to sensor fusion configuration + * registers. + * @hw: Sensor hardware instance. + * + * Return: 0 on success, negative value on error. + */ +static int st_lsm6dsx_sf_page_enable(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_reg *mux =3D &hw->settings->sf_settings.page_mux; + + return regmap_set_bits(hw->regmap, mux->addr, mux->mask); +} + +/** + * st_lsm6dsx_sf_page_disable - Disable access to sensor fusion configurat= ion + * registers. + * @hw: Sensor hardware instance. + * + * Return: 0 on success, negative value on error. + */ +static int st_lsm6dsx_sf_page_disable(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_reg *mux =3D &hw->settings->sf_settings.page_mux; + + return regmap_clear_bits(hw->regmap, mux->addr, mux->mask); +} + +int st_lsm6dsx_sf_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable) +{ + struct st_lsm6dsx_hw *hw =3D sensor->hw; + const struct st_lsm6dsx_reg *en_reg; + int err; + + guard(mutex)(&hw->page_lock); + + en_reg =3D &hw->settings->sf_settings.enable; + err =3D st_lsm6dsx_sf_page_enable(hw); + if (err) + return err; + + err =3D regmap_assign_bits(hw->regmap, en_reg->addr, en_reg->mask, enable= ); + if (err) { + st_lsm6dsx_sf_page_disable(hw); + return err; + } + + return st_lsm6dsx_sf_page_disable(hw); +} + +int st_lsm6dsx_sf_set_odr(struct st_lsm6dsx_sensor *sensor, bool enable) +{ + const struct st_lsm6dsx_sf_settings *settings; + struct st_lsm6dsx_hw *hw =3D sensor->hw; + u8 data; + int err; + + guard(mutex)(&hw->page_lock); + + err =3D st_lsm6dsx_sf_page_enable(hw); + if (err) + return err; + + settings =3D &hw->settings->sf_settings; + if (enable) { + const struct st_lsm6dsx_reg *reg =3D &settings->odr_table.reg; + u8 odr_val; + + st_lsm6dsx_sf_get_odr_val(settings, sensor->hwfifo_odr_mHz, + &odr_val); + data =3D ST_LSM6DSX_SHIFT_VAL(odr_val, reg->mask); + err =3D regmap_update_bits(hw->regmap, reg->addr, reg->mask, + data); + if (err) + goto out; + } + + err =3D regmap_assign_bits(hw->regmap, settings->fifo_enable.addr, + settings->fifo_enable.mask, enable); + if (err) + goto out; + + return st_lsm6dsx_sf_page_disable(hw); + +out: + st_lsm6dsx_sf_page_disable(hw); + + return err; +} + +static int st_lsm6dsx_sf_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor =3D iio_priv(iio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + *val =3D sensor->hwfifo_odr_mHz / MILLI; + *val2 =3D (sensor->hwfifo_odr_mHz % MILLI) * (MICRO / MILLI); + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } +} + +static int st_lsm6dsx_sf_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor =3D iio_priv(iio_dev); + const struct st_lsm6dsx_sf_settings *settings; + int err; + + settings =3D &sensor->hw->settings->sf_settings; + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u32 odr_mHz =3D val * MILLI + val2 * (MILLI / MICRO); + u8 odr_val; + + /* check that the requested frequency is supported */ + err =3D st_lsm6dsx_sf_get_odr_val(settings, odr_mHz, &odr_val); + if (err) + return err; + + sensor->hwfifo_odr_mHz =3D odr_mHz; + return 0; + } + default: + return -EINVAL; + } +} + +static ssize_t st_lsm6dsx_sf_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor =3D iio_priv(dev_to_iio_dev(dev)); + const struct st_lsm6dsx_sf_settings *settings; + int len =3D 0; + + settings =3D &sensor->hw->settings->sf_settings; + for (unsigned int i =3D 0; i < settings->odr_table.odr_len; i++) { + u32 val =3D settings->odr_table.odr_avl[i].milli_hz; + + len +=3D sysfs_emit_at(buf, len, "%lu.%03lu ", val / MILLI, + val % MILLI); + } + buf[len - 1] =3D '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sf_sampling_freq_avail); +static struct attribute *st_lsm6dsx_sf_attributes[] =3D { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + NULL +}; + +static const struct attribute_group st_lsm6dsx_sf_attribute_group =3D { + .attrs =3D st_lsm6dsx_sf_attributes, +}; + +static const struct iio_info st_lsm6dsx_sf_info =3D { + .attrs =3D &st_lsm6dsx_sf_attribute_group, + .read_raw =3D st_lsm6dsx_sf_read_raw, + .write_raw =3D st_lsm6dsx_sf_write_raw, + .hwfifo_set_watermark =3D st_lsm6dsx_set_watermark, +}; + +int st_lsm6dsx_sf_probe(struct st_lsm6dsx_hw *hw, const char *name) +{ + const struct st_lsm6dsx_sf_settings *settings; + struct st_lsm6dsx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev =3D devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return -ENOMEM; + + settings =3D &hw->settings->sf_settings; + sensor =3D iio_priv(iio_dev); + sensor->id =3D ST_LSM6DSX_ID_SF; + sensor->hw =3D hw; + sensor->hwfifo_odr_mHz =3D settings->odr_table.odr_avl[0].milli_hz; + sensor->watermark =3D 1; + iio_dev->modes =3D INDIO_DIRECT_MODE; + iio_dev->info =3D &st_lsm6dsx_sf_info; + iio_dev->channels =3D settings->chan; + iio_dev->num_channels =3D settings->chan_len; + if (snprintf(sensor->name, sizeof(sensor->name), "%s_sf", name) >=3D + sizeof(sensor->name)) + return -E2BIG; + iio_dev->name =3D sensor->name; + + /* + * Put the IIO device pointer in the iio_devs array so that the caller + * can set up a buffer and register this IIO device. + */ + hw->iio_devs[ST_LSM6DSX_ID_SF] =3D iio_dev; + + return 0; +} --=20 2.39.5