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..767aadfe7061 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_FUSION, ST_LSM6DSX_ID_MAX }; @@ -301,6 +302,17 @@ enum st_lsm6dsx_ext_sensor_id { ST_LSM6DSX_ID_MAGN, }; +struct st_lsm6dsx_fusion_settings { + const struct iio_chan_spec *chan; + int chan_len; + struct st_lsm6dsx_reg odr_reg; + int odr_hz[ST_LSM6DSX_ODR_LIST_SIZE]; + int odr_len; + 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 +400,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_fusion_settings fusion_settings; }; enum st_lsm6dsx_fifo_mode { @@ -510,6 +523,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_fusion_probe(struct st_lsm6dsx_hw *hw, const char *name); +int st_lsm6dsx_fusion_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable); +int st_lsm6dsx_fusion_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 +580,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_FUSION: + return st_lsm6dsx_fusion_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 8b79ade8e7de..777d479f10c0 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_FUSION) + return st_lsm6dsx_fusion_set_odr(sensor, enable); return 0; + } batch_reg = &hw->settings->batch[sensor->id]; if (batch_reg->addr) { @@ -585,6 +589,9 @@ static int 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: + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_FUSION]; + 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..630e2cae6f19 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -94,6 +94,26 @@ #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_AXIS, \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .info_mask_shared_by_all_available = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = 0, \ + .scan_type = { \ + .format = IIO_SCAN_FORMAT_FLOAT, \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + .repeat = 3, \ + }, \ + .ext_info = st_lsm6dsx_ext_info, \ +} + static const struct iio_event_spec st_lsm6dsx_ev_motion[] = { { .type = IIO_EV_TYPE_THRESH, @@ -153,6 +173,11 @@ static const struct iio_chan_spec st_lsm6ds0_gyro_channels[] = { IIO_CHAN_SOFT_TIMESTAMP(3), }; +static const struct iio_chan_spec st_lsm6dsx_fusion_channels[] = { + ST_LSM6DSX_CHANNEL_ROT, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { { .reset = { @@ -1492,6 +1517,33 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { }, }, }, + .fusion_settings = { + .chan = st_lsm6dsx_fusion_channels, + .chan_len = ARRAY_SIZE(st_lsm6dsx_fusion_channels), + .odr_reg = { + .addr = 0x5e, + .mask = GENMASK(5, 3), + }, + .odr_hz[0] = 15, + .odr_hz[1] = 30, + .odr_hz[2] = 60, + .odr_hz[3] = 120, + .odr_hz[4] = 240, + .odr_hz[5] = 480, + .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->fusion_settings.chan) { + err = st_lsm6dsx_fusion_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..2e72c7ba94dd --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c @@ -0,0 +1,241 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsx IMU sensor fusion + * + * Copyright 2026 BayLibre, SAS + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsx.h" + +static int +st_lsm6dsx_fusion_get_odr_val(const struct st_lsm6dsx_fusion_settings *settings, + u32 odr_mHz, u8 *val) +{ + int odr_hz = odr_mHz / MILLI; + int i; + + for (i = 0; i < settings->odr_len; i++) { + if (settings->odr_hz[i] == odr_hz) + break; + } + if (i == settings->odr_len) + return -EINVAL; + + *val = i; + return 0; +} + +/** + * st_lsm6dsx_fusion_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_fusion_page_enable(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_reg *mux; + + mux = &hw->settings->fusion_settings.page_mux; + + return regmap_set_bits(hw->regmap, mux->addr, mux->mask); +} + +/** + * st_lsm6dsx_fusion_page_disable - Disable access to sensor fusion + * configuration registers. + * @hw: Sensor hardware instance. + * + * Return: 0 on success, negative value on error. + */ +static int st_lsm6dsx_fusion_page_disable(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_reg *mux; + + mux = &hw->settings->fusion_settings.page_mux; + + return regmap_clear_bits(hw->regmap, mux->addr, mux->mask); +} + +static int st_lsm6dsx_fusion_set_odr_locked(struct st_lsm6dsx_sensor *sensor, + bool enable) +{ + const struct st_lsm6dsx_fusion_settings *settings; + struct st_lsm6dsx_hw *hw = sensor->hw; + int err; + + settings = &hw->settings->fusion_settings; + if (enable) { + const struct st_lsm6dsx_reg *reg = &settings->odr_reg; + u8 odr_val; + u8 data; + + st_lsm6dsx_fusion_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) + return err; + } + + return regmap_assign_bits(hw->regmap, settings->fifo_enable.addr, + settings->fifo_enable.mask, enable); +} + +int st_lsm6dsx_fusion_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->fusion_settings.enable; + err = st_lsm6dsx_fusion_page_enable(hw); + if (err) + return err; + + err = regmap_assign_bits(hw->regmap, en_reg->addr, en_reg->mask, enable); + if (err) { + st_lsm6dsx_fusion_page_disable(hw); + return err; + } + + return st_lsm6dsx_fusion_page_disable(hw); +} + +int st_lsm6dsx_fusion_set_odr(struct st_lsm6dsx_sensor *sensor, bool enable) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + int err; + + guard(mutex)(&hw->page_lock); + + err = st_lsm6dsx_fusion_page_enable(hw); + if (err) + return err; + + err = st_lsm6dsx_fusion_set_odr_locked(sensor, enable); + if (err) { + st_lsm6dsx_fusion_page_disable(hw); + return err; + } + + return st_lsm6dsx_fusion_page_disable(hw); +} + +static int st_lsm6dsx_fusion_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_fusion_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_fusion_settings *settings; + int err; + + settings = &sensor->hw->settings->fusion_settings; + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u32 odr_mHz = val * MILLI + val2 * (MILLI / MICRO); + u8 odr_val; + + /* check that the requested frequency is supported */ + err = st_lsm6dsx_fusion_get_odr_val(settings, odr_mHz, &odr_val); + if (err) + return err; + + sensor->hwfifo_odr_mHz = odr_mHz; + return 0; + } + default: + return -EINVAL; + } +} + +static int st_lsm6dsx_fusion_read_avail(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + const int **vals, int *type, + int *length, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev); + const struct st_lsm6dsx_fusion_settings *settings; + + settings = &sensor->hw->settings->fusion_settings; + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + *vals = settings->odr_hz; + *type = IIO_VAL_INT; + *length = settings->odr_len; + return IIO_AVAIL_LIST; + default: + return -EINVAL; + } +} + +static const struct iio_info st_lsm6dsx_fusion_info = { + .read_raw = st_lsm6dsx_fusion_read_raw, + .read_avail = st_lsm6dsx_fusion_read_avail, + .write_raw = st_lsm6dsx_fusion_write_raw, + .hwfifo_set_watermark = st_lsm6dsx_set_watermark, +}; + +int st_lsm6dsx_fusion_probe(struct st_lsm6dsx_hw *hw, const char *name) +{ + const struct st_lsm6dsx_fusion_settings *settings; + struct st_lsm6dsx_sensor *sensor; + struct iio_dev *iio_dev; + int ret; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return -ENOMEM; + + settings = &hw->settings->fusion_settings; + sensor = iio_priv(iio_dev); + sensor->id = ST_LSM6DSX_ID_FUSION; + sensor->hw = hw; + sensor->hwfifo_odr_mHz = settings->odr_hz[0] * MILLI; + sensor->watermark = 1; + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->info = &st_lsm6dsx_fusion_info; + iio_dev->channels = settings->chan; + iio_dev->num_channels = settings->chan_len; + ret = snprintf(sensor->name, sizeof(sensor->name), "%s_fusion", name); + if (ret >= sizeof(sensor->name)) + return -E2BIG; + 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_FUSION] = iio_dev; + + return 0; +}