diff options
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 2 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 71 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 7 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 2 |
4 files changed, 72 insertions, 10 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h index bd366c6e282a..3cd520bdec46 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -366,6 +366,7 @@ enum st_lsm6dsx_fifo_mode { * @hw: Pointer to instance of struct st_lsm6dsx_hw. * @gain: Configured sensor sensitivity. * @odr: Output data rate of the sensor [mHz]. + * hwfifo_odr_mHz: Batch data rate for hardware FIFO [mHz] * @samples_to_discard: Number of samples to discard for filters settling time. * @watermark: Sensor watermark level. * @decimator: Sensor decimation factor. @@ -380,6 +381,7 @@ struct st_lsm6dsx_sensor { u32 gain; u32 odr; + u32 hwfifo_odr_mHz; u16 samples_to_discard; u16 watermark; diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c index 8a9d2593576a..55d877745575 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -56,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> @@ -105,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) @@ -136,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; } @@ -231,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; @@ -713,7 +714,7 @@ st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor) 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->odr) { + if (data->val[i].milli_hz == sensor->hwfifo_odr_mHz) { sensor->samples_to_discard = data->val[i].samples; return; } @@ -799,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; @@ -807,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; } diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c index d8cb4b0218d5..88939625ace4 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -1847,10 +1847,12 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, val = val * 1000 + val2 / 1000; val = st_lsm6dsx_check_odr(sensor, val, &data); - if (val < 0) + if (val < 0) { err = val; - else + } else { sensor->odr = val; + sensor->hwfifo_odr_mHz = val; + } break; } default: @@ -2384,6 +2386,7 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, sensor->id = id; sensor->hw = hw; sensor->odr = hw->settings->odr_table[id].odr_avl[0].milli_hz; + sensor->hwfifo_odr_mHz = sensor->odr; sensor->gain = hw->settings->fs_table[id].fs_avl[0].gain; sensor->watermark = 1; diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c index 3c5e65dc0f97..d6a1eeb151ca 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c @@ -640,6 +640,7 @@ __st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev, sensor->ext_info.slv_odr = val; sensor->odr = odr; + sensor->hwfifo_odr_mHz = odr; return 0; } case IIO_CHAN_INFO_SCALE: @@ -746,6 +747,7 @@ st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw, sensor->id = id; sensor->hw = hw; sensor->odr = hw->settings->odr_table[ref_id].odr_avl[0].milli_hz; + sensor->hwfifo_odr_mHz = sensor->odr; sensor->ext_info.slv_odr = info->odr_table.odr_avl[0].milli_hz; sensor->gain = info->fs_table.fs_avl[0].gain; sensor->ext_info.settings = info; |