summaryrefslogtreecommitdiff
path: root/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c')
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c160
1 files changed, 142 insertions, 18 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 7dd5205aea5b..55d877745575 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -2,7 +2,7 @@
/*
* STMicroelectronics st_lsm6dsx FIFO buffer library driver
*
- * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC/LSM6DS3TR-C:
+ * Pattern FIFO:
* The FIFO buffer can be configured to store data from gyroscope and
* accelerometer. Samples are queued without any tag according to a
* specific pattern based on 'FIFO data sets' (6 bytes each):
@@ -14,12 +14,34 @@
* (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
* value of the decimation factor and ODR set for each FIFO data set.
*
- * LSM6DSO/LSM6DSOX/ASM330LHH/ASM330LHHX/LSM6DSR/LSM6DSRX/ISM330DHCX/
- * LSM6DST/LSM6DSOP/LSM6DSTX/LSM6DSV:
+ * Supported devices:
+ * - ISM330DLC
+ * - LSM6DS3
+ * - LSM6DS3H
+ * - LSM6DS3TR-C
+ * - LSM6DSL
+ * - LSM6DSM
+ *
+ * Tagged FIFO:
* The FIFO buffer can be configured to store data from gyroscope and
* accelerometer. Each sample is queued with a tag (1B) indicating data
* source (gyroscope, accelerometer, hw timer).
*
+ * Supported devices:
+ * - ASM330LHB
+ * - ASM330LHH
+ * - ASM330LHHX
+ * - ASM330LHHXG1
+ * - ISM330DHCX
+ * - LSM6DSO
+ * - LSM6DSOP
+ * - LSM6DSOX
+ * - LSM6DSR
+ * - LSM6DSRX
+ * - LSM6DST
+ * - LSM6DSTX
+ * - LSM6DSV
+ *
* FIFO supported modes:
* - BYPASS: FIFO disabled
* - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
@@ -34,6 +56,7 @@
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
+#include <linux/iio/sysfs.h>
#include <linux/regmap.h>
#include <linux/bitfield.h>
@@ -83,7 +106,7 @@ static int
st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
{
const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
- u32 decimator = max_odr / sensor->odr;
+ u32 decimator = max_odr / sensor->hwfifo_odr_mHz;
int i;
if (decimator > 1)
@@ -114,14 +137,14 @@ static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
if (!(hw->enable_mask & BIT(sensor->id)))
continue;
- *max_odr = max_t(u32, *max_odr, sensor->odr);
- *min_odr = min_t(u32, *min_odr, sensor->odr);
+ *max_odr = max(*max_odr, sensor->hwfifo_odr_mHz);
+ *min_odr = min(*min_odr, sensor->hwfifo_odr_mHz);
}
}
static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr)
{
- u8 sip = sensor->odr / min_odr;
+ u8 sip = sensor->hwfifo_odr_mHz / min_odr;
return sip > 1 ? round_down(sip, 2) : sip;
}
@@ -209,7 +232,7 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
if (enable) {
int err;
- err = st_lsm6dsx_check_odr(sensor, sensor->odr,
+ err = st_lsm6dsx_check_odr(sensor, sensor->hwfifo_odr_mHz,
&data);
if (err < 0)
return err;
@@ -370,6 +393,9 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
return 0;
+ if (!pattern_len)
+ pattern_len = ST_LSM6DSX_SAMPLE_SIZE;
+
fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
ST_LSM6DSX_CHAN_SIZE;
fifo_len = (fifo_len / pattern_len) * pattern_len;
@@ -457,17 +483,31 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
}
if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
- iio_push_to_buffers_with_timestamp(
- hw->iio_devs[ST_LSM6DSX_ID_GYRO],
- &hw->scan[ST_LSM6DSX_ID_GYRO],
- gyro_sensor->ts_ref + ts);
+ /*
+ * We need to discards gyro samples during
+ * filters settling time
+ */
+ if (gyro_sensor->samples_to_discard > 0)
+ gyro_sensor->samples_to_discard--;
+ else
+ iio_push_to_buffers_with_timestamp(
+ hw->iio_devs[ST_LSM6DSX_ID_GYRO],
+ &hw->scan[ST_LSM6DSX_ID_GYRO],
+ gyro_sensor->ts_ref + ts);
gyro_sip--;
}
if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
- iio_push_to_buffers_with_timestamp(
- hw->iio_devs[ST_LSM6DSX_ID_ACC],
- &hw->scan[ST_LSM6DSX_ID_ACC],
- acc_sensor->ts_ref + ts);
+ /*
+ * We need to discards accel samples during
+ * filters settling time
+ */
+ if (acc_sensor->samples_to_discard > 0)
+ acc_sensor->samples_to_discard--;
+ else
+ iio_push_to_buffers_with_timestamp(
+ hw->iio_devs[ST_LSM6DSX_ID_ACC],
+ &hw->scan[ST_LSM6DSX_ID_ACC],
+ acc_sensor->ts_ref + ts);
acc_sip--;
}
if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
@@ -587,6 +627,9 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
if (!fifo_len)
return 0;
+ if (!pattern_len)
+ pattern_len = ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
+
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
err = st_lsm6dsx_read_block(hw,
ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
@@ -654,6 +697,30 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
return err;
}
+static void
+st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor)
+{
+ const struct st_lsm6dsx_samples_to_discard *data;
+ struct st_lsm6dsx_hw *hw = sensor->hw;
+ int i;
+
+ if (sensor->id != ST_LSM6DSX_ID_GYRO &&
+ sensor->id != ST_LSM6DSX_ID_ACC)
+ return;
+
+ /* check if drdy mask is supported in hw */
+ if (hw->settings->drdy_mask.addr)
+ return;
+
+ data = &hw->settings->samples_to_discard[sensor->id];
+ for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
+ if (data->val[i].milli_hz == sensor->hwfifo_odr_mHz) {
+ sensor->samples_to_discard = data->val[i].samples;
+ return;
+ }
+ }
+}
+
int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
{
struct st_lsm6dsx_hw *hw = sensor->hw;
@@ -673,6 +740,9 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
goto out;
}
+ if (enable)
+ st_lsm6dsx_update_samples_to_discard(sensor);
+
err = st_lsm6dsx_device_set_enable(sensor, enable);
if (err < 0)
goto out;
@@ -730,6 +800,59 @@ static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
.postdisable = st_lsm6dsx_buffer_postdisable,
};
+static ssize_t st_lsm6dsx_hwfifo_odr_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+ struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev));
+
+ return sysfs_emit(buf, "%d.%03d\n", sensor->hwfifo_odr_mHz / 1000,
+ sensor->hwfifo_odr_mHz % 1000);
+}
+
+static ssize_t st_lsm6dsx_hwfifo_odr_store(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t len)
+{
+ struct iio_dev *iio_dev = dev_to_iio_dev(dev);
+ struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
+ int integer, milli;
+ int ret;
+ u32 hwfifo_odr;
+ u8 data;
+
+ if (!iio_device_claim_direct(iio_dev))
+ return -EBUSY;
+
+ ret = iio_str_to_fixpoint(buf, 100, &integer, &milli);
+ if (ret)
+ goto out;
+
+ hwfifo_odr = integer * 1000 + milli;
+ ret = st_lsm6dsx_check_odr(sensor, hwfifo_odr, &data);
+ if (ret < 0)
+ goto out;
+
+ hwfifo_odr = ret;
+
+ /* the batch data rate must not exceed the sensor output data rate */
+ if (hwfifo_odr <= sensor->odr)
+ sensor->hwfifo_odr_mHz = hwfifo_odr;
+ else
+ ret = -EINVAL;
+
+out:
+ iio_device_release_direct(iio_dev);
+
+ return ret < 0 ? ret : len;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ(0664, st_lsm6dsx_hwfifo_odr_show, st_lsm6dsx_hwfifo_odr_store);
+
+static const struct iio_dev_attr *st_lsm6dsx_buffer_attrs[] = {
+ &iio_dev_attr_sampling_frequency,
+ NULL
+};
+
int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
{
int i, ret;
@@ -738,8 +861,9 @@ int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
if (!hw->iio_devs[i])
continue;
- ret = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i],
- &st_lsm6dsx_buffer_ops);
+ ret = devm_iio_kfifo_buffer_setup_ext(hw->dev, hw->iio_devs[i],
+ &st_lsm6dsx_buffer_ops,
+ st_lsm6dsx_buffer_attrs);
if (ret)
return ret;
}
generated by cgit (git 2.34.1) at 2025-12-26 09:47:19 +0000