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 constdiff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 5b28a3ffcc3d..9a259e04755f 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);
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..11ab246ef9d6
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c
@@ -0,0 +1,220 @@
+// 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);
+ if (err < 0) {
+ st_lsm6dsx_sf_set_page(hw, false);
+ return err;
+ }
+
+ return st_lsm6dsx_sf_set_page(hw, false);
+}
+
+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;
+ }
+
+ err = regmap_assign_bits(hw->regmap, settings->fifo_enable.addr,
+ settings->fifo_enable.mask, enable);
+ if (err < 0)
+ goto out;
+
+ return st_lsm6dsx_sf_set_page(hw, false);
+
+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;
+
+ settings = &sensor->hw->settings->sf_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_sf_get_odr_val(settings, odr_mHz, &odr_val);
+ if (err)
+ return err;
+
+ sensor->hwfifo_odr_mHz = 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 = 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