Message-ID: <aWjsWzo3PXHKsdJX@lore-desk>
Date: Thu, 15 Jan 2026 14:32:11 +0100
From: Lorenzo Bianconi <lorenzo@...nel.org>
To: Francesco Lavra <flavra@...libre.com>
Cc: Jonathan Cameron <jic23@...nel.org>,
	David Lechner <dlechner@...libre.com>,
	Nuno Sá <nuno.sa@...log.com>,
	Andy Shevchenko <andy@...nel.org>, linux-iio@...r.kernel.org,
	linux-kernel@...r.kernel.org
Subject: Re: [PATCH v2 3/3] iio: imu: st_lsm6dsx: Add support for rotation
 sensor

> 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.

Nice feature, thx for working on this. I think the patch is fine,
just a couple of Nits inline.

Acked-by: Lorenzo Bianconi <lorenzo@...nel.org>

> 
> Signed-off-by: Francesco Lavra <flavra@...libre.com>
> ---
>  drivers/iio/imu/st_lsm6dsx/Makefile           |   2 +-
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  26 ++-
>  .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    |  16 +-
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  |  58 +++++
>  .../iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c    | 212 ++++++++++++++++++
>  5 files changed, 307 insertions(+), 7 deletions(-)
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c
> 
> diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6dsx/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 := st_lsm6dsx_core.o st_lsm6dsx_buffer.o \
> -		st_lsm6dsx_shub.o
> +		st_lsm6dsx_shub.o st_lsm6dsx_fusion.o
>  
>  obj-$(CONFIG_IIO_ST_LSM6DSX) += st_lsm6dsx.o
>  obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) += st_lsm6dsx_i2c.o
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> index 07b1773c87bd..4173f670f7af 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
>  };
>  
> @@ -301,6 +302,15 @@ enum st_lsm6dsx_ext_sensor_id {
>  	ST_LSM6DSX_ID_MAGN,
>  };
>  
> +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;
>  };
>  
>  enum st_lsm6dsx_fifo_mode {
> @@ -510,6 +521,9 @@ int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, 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 enable);
>  int st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data, int len);
> +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);
>  
>  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 == ST_LSM6DSX_ID_EXT0 ||
> -	    sensor->id == ST_LSM6DSX_ID_EXT1 ||
> -	    sensor->id == 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);
> +	}
>  }
>  
>  static const
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> index cde29b2e6f34..1846b9f84c29 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 = 0x0f,
>  	ST_LSM6DSX_EXT1_TAG = 0x10,
>  	ST_LSM6DSX_EXT2_TAG = 0x11,
> +	ST_LSM6DSX_ROT_TAG = 0x13,
>  };
>  
>  static const
> @@ -226,8 +227,11 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
>  	u8 data;
>  
>  	/* Only internal sensors have a FIFO ODR configuration register. */
> -	if (sensor->id >= ARRAY_SIZE(hw->settings->batch))
> +	if (sensor->id >= ARRAY_SIZE(hw->settings->batch)) {
> +		if (sensor->id == ST_LSM6DSX_ID_SF)
> +			return st_lsm6dsx_sf_set_odr(sensor, enable);

Nit: please align this to the comment in patch 1/3

>  		return 0;
> +	}
>  
>  	batch_reg = &hw->settings->batch[sensor->id];
>  	if (batch_reg->addr) {
> @@ -580,6 +584,16 @@ st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
>  	case ST_LSM6DSX_EXT2_TAG:
>  		iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
>  		break;
> +	case ST_LSM6DSX_ROT_TAG:
> +		/*
> +		 * The sensor reports only the {X, Y, Z} elements of the
> +		 * quaternion vector; set the W value to 0 (it can be derived
> +		 * from the {X, Y, Z} values due to the property that the vector
> +		 * is normalized).
> +		 */
> +		*(u16 *)(data + ST_LSM6DSX_SAMPLE_SIZE) = 0;
> +		iio_dev = 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 dc0ae0e488ce..c21163a06a71 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 @@
>  
>  #define ST_LSM6DSX_REG_WHOAMI_ADDR		0x0f
>  
> +/* Raw values from the IMU are 16-bit half-precision floating-point numbers. */
> +#define ST_LSM6DSX_CHANNEL_ROT						\
> +{									\
> +	.type = IIO_ROT,						\
> +	.modified = 1,							\
> +	.channel2 = IIO_MOD_QUATERNION,					\
> +	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
> +	.scan_index = 0,						\
> +	.scan_type = {							\
> +		.sign = 'u',						\
> +		.realbits = 16,						\
> +		.storagebits = 16,					\
> +		.endianness = IIO_LE,					\
> +		.repeat = 4,						\
> +	},								\
> +	.ext_info = st_lsm6dsx_ext_info,				\
> +}
> +
>  static const struct iio_event_spec st_lsm6dsx_ev_motion[] = {
>  	{
>  		.type = IIO_EV_TYPE_THRESH,
> @@ -153,6 +171,11 @@ static const struct iio_chan_spec st_lsm6ds0_gyro_channels[] = {
>  	IIO_CHAN_SOFT_TIMESTAMP(3),
>  };
>  
> +static const struct iio_chan_spec st_lsm6dsx_sf_channels[] = {
> +	ST_LSM6DSX_CHANNEL_ROT,
> +	IIO_CHAN_SOFT_TIMESTAMP(1),
> +};
> +
>  static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  	{
>  		.reset = {
> @@ -1492,6 +1515,35 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
>  				},
>  			},
>  		},
> +		.sf_settings = {
> +			.chan = st_lsm6dsx_sf_channels,
> +			.chan_len = ARRAY_SIZE(st_lsm6dsx_sf_channels),
> +			.odr_table = {
> +				.reg = {
> +					.addr = 0x5e,
> +					.mask = GENMASK(5, 3),
> +				},
> +				.odr_avl[0] = {  15000, 0x00 },
> +				.odr_avl[1] = {  30000, 0x01 },
> +				.odr_avl[2] = {  60000, 0x02 },
> +				.odr_avl[3] = { 120000, 0x03 },
> +				.odr_avl[4] = { 240000, 0x04 },
> +				.odr_avl[5] = { 480000, 0x05 },
> +				.odr_len = 6,
> +			},
> +			.fifo_enable = {
> +				.addr = 0x44,
> +				.mask = BIT(1),
> +			},
> +			.page_mux = {
> +				.addr = 0x01,
> +				.mask = BIT(7),
> +			},
> +			.enable = {
> +				.addr = 0x04,
> +				.mask = BIT(1),
> +			},
> +		},
>  	},
>  	{
>  		.reset = {
> @@ -2899,6 +2951,12 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
>  			return err;
>  	}
>  
> +	if (hw->settings->sf_settings.chan) {
> +		err = st_lsm6dsx_sf_probe(hw, name);
> +		if (err)
> +			return err;
> +	}
> +
>  	if (hw->irq > 0) {
>  		err = st_lsm6dsx_irq_setup(hw);
>  		if (err < 0)
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c
> new file mode 100644
> index 000000000000..3594d97a98ff
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c
> @@ -0,0 +1,212 @@
> +// SPDX-License-Identifier: GPL-2.0-only
> +/*
> + * STMicroelectronics st_lsm6dsx IMU sensor fusion
> + */
> +
> +#include <linux/iio/iio.h>
> +#include <linux/iio/sysfs.h>
> +#include <linux/mutex.h>
> +#include <linux/regmap.h>
> +#include <linux/sprintf.h>
> +#include <linux/types.h>
> +#include <linux/units.h>
> +
> +#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 = 0; i < settings->odr_table.odr_len; i++) {
> +		if (settings->odr_table.odr_avl[i].milli_hz == odr)
> +			break;
> +	}
> +	if (i == settings->odr_table.odr_len)
> +		return -EINVAL;
> +
> +	*val = settings->odr_table.odr_avl[i].val;
> +	return 0;
> +}
> +
> +/**
> + * st_lsm6dsx_sf_set_page - Enable or disable access to sensor fusion
> + * configuration registers.
> + * @hw: Sensor hardware instance.
> + * @enable: True to enable access, false to disable access.
> + *
> + * Return: 0 on success, negative value on error.
> + */
> +static int st_lsm6dsx_sf_set_page(struct st_lsm6dsx_hw *hw, bool enable)
> +{
> +	const struct st_lsm6dsx_reg *mux = &hw->settings->sf_settings.page_mux;
> +
> +	return regmap_assign_bits(hw->regmap, mux->addr, mux->mask, enable);
> +}
> +
> +int st_lsm6dsx_sf_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
> +{
> +	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	const struct st_lsm6dsx_reg *en_reg;
> +	int err;
> +
> +	guard(mutex)(&hw->page_lock);
> +
> +	en_reg = &hw->settings->sf_settings.enable;
> +	err = st_lsm6dsx_sf_set_page(hw, true);
> +	if (err < 0)
> +		return err;
> +
> +	err = regmap_assign_bits(hw->regmap, en_reg->addr, en_reg->mask, enable);
> +	st_lsm6dsx_sf_set_page(hw, false);
> +
> +	return err;
> +}
> +
> +int st_lsm6dsx_sf_set_odr(struct st_lsm6dsx_sensor *sensor, bool enable)
> +{
> +	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	const struct st_lsm6dsx_sf_settings *settings;
> +	u8 data;
> +	int err;
> +
> +	guard(mutex)(&hw->page_lock);
> +
> +	err = st_lsm6dsx_sf_set_page(hw, true);
> +	if (err < 0)
> +		return err;
> +
> +	settings = &hw->settings->sf_settings;
> +	if (enable) {
> +		u8 odr_val;
> +		const struct st_lsm6dsx_reg *reg = &settings->odr_table.reg;
> +
> +		st_lsm6dsx_sf_get_odr_val(settings, sensor->hwfifo_odr_mHz,
> +					  &odr_val);
> +		data = ST_LSM6DSX_SHIFT_VAL(odr_val, reg->mask);
> +		err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
> +					 data);
> +		if (err < 0)
> +			goto out;
> +	}

Nit: new-line here.

> +	err = regmap_assign_bits(hw->regmap, settings->fifo_enable.addr,
> +				 settings->fifo_enable.mask, enable);
> +
> +out:
> +	st_lsm6dsx_sf_set_page(hw, false);
> +
> +	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 = iio_priv(iio_dev);
> +
> +	switch (mask) {
> +	case IIO_CHAN_INFO_SAMP_FREQ:
> +		*val = sensor->hwfifo_odr_mHz / MILLI;
> +		*val2 = (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 = iio_priv(iio_dev);
> +	const struct st_lsm6dsx_sf_settings *settings;
> +	int err;

I guess err = -EINVAL;

> +
> +	settings = &sensor->hw->settings->sf_settings;
> +	switch (mask) {
> +	case IIO_CHAN_INFO_SAMP_FREQ: {
> +		u32 odr_mHz;
> +		u8 odr_val;
> +
> +		odr_mHz = val * MILLI + val2 * MILLI / MICRO;
> +		err = st_lsm6dsx_sf_get_odr_val(settings, odr_mHz, &odr_val);
> +		if (err)
> +			return err;
> +
> +		sensor->hwfifo_odr_mHz = odr_mHz;
> +		return 0;

break;

> +	}
> +	default:
> +		return -EINVAL;

break;

> +	}

return err;

> +}
> +
> +static ssize_t st_lsm6dsx_sf_sampling_freq_avail(struct device *dev,
> +						 struct device_attribute *attr,
> +						 char *buf)
> +{
> +	struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev));
> +	const struct st_lsm6dsx_sf_settings *settings;
> +	int len = 0;
> +
> +	settings = &sensor->hw->settings->sf_settings;
> +	for (unsigned int i = 0; i < settings->odr_table.odr_len; i++) {
> +		u32 val = settings->odr_table.odr_avl[i].milli_hz;
> +
> +		len += sysfs_emit_at(buf, len, "%lu.%03lu ", val / MILLI,
> +				     val % MILLI);
> +	}
> +	buf[len - 1] = '\n';
> +
> +	return len;
> +}
> +
> +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sf_sampling_freq_avail);
> +static struct attribute *st_lsm6dsx_sf_attributes[] = {
> +	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
> +	NULL
> +};
> +
> +static const struct attribute_group st_lsm6dsx_sf_attribute_group = {
> +	.attrs = st_lsm6dsx_sf_attributes,
> +};
> +
> +static const struct iio_info st_lsm6dsx_sf_info = {
> +	.attrs = &st_lsm6dsx_sf_attribute_group,
> +	.read_raw = st_lsm6dsx_sf_read_raw,
> +	.write_raw = st_lsm6dsx_sf_write_raw,
> +	.hwfifo_set_watermark = 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 = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
> +	if (!iio_dev)
> +		return -ENOMEM;
> +
> +	settings = &hw->settings->sf_settings;
> +	sensor = iio_priv(iio_dev);
> +	sensor->id = ST_LSM6DSX_ID_SF;
> +	sensor->hw = hw;
> +	sensor->hwfifo_odr_mHz = settings->odr_table.odr_avl[0].milli_hz;
> +	sensor->watermark = 1;
> +	iio_dev->modes = INDIO_DIRECT_MODE;
> +	iio_dev->info = &st_lsm6dsx_sf_info;
> +	iio_dev->channels = settings->chan;
> +	iio_dev->num_channels = settings->chan_len;
> +	snprintf(sensor->name, sizeof(sensor->name), "%s_sf", name);
> +	iio_dev->name = 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] = iio_dev;
> +
> +	return 0;
> +}
> -- 
> 2.39.5
> 

Download attachment "signature.asc" of type "application/pgp-signature" (229 bytes)

Powered by blists - more mailing lists