ST_LSM6DSX_ID_EXT0,
ST_LSM6DSX_ID_EXT1,
ST_LSM6DSX_ID_EXT2,
+ ST_LSM6DSX_ID_FUSION,
ST_LSM6DSX_ID_MAX
};
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.
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 {
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
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
#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,
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 = {
},
},
},
+ .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 = {
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)
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * STMicroelectronics st_lsm6dsx IMU sensor fusion
+ *
+ * Copyright 2026 BayLibre, SAS
+ */
+
+#include <linux/cleanup.h>
+#include <linux/errno.h>
+#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_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;
+}
projects / thirdparty / linux.git / commitdiff
