diff options
Diffstat (limited to 'drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c')
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 71 |
1 files changed, 63 insertions, 8 deletions
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; } |