diff options
Diffstat (limited to 'drivers/iio/imu/st_lsm6dsx')
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 4 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 41 |
2 files changed, 28 insertions, 17 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c index e95982590373..c8e5cfd0ef0b 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -364,7 +364,7 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable) static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private) { - struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private; + struct st_lsm6dsx_hw *hw = private; struct st_lsm6dsx_sensor *sensor; int i; @@ -388,7 +388,7 @@ static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private) static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private) { - struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private; + struct st_lsm6dsx_hw *hw = private; int count; mutex_lock(&hw->fifo_lock); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c index c4332232ad71..f80a3d4ff977 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -308,32 +308,40 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor, return 0; } -static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) +static int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, + u8 *val) { - enum st_lsm6dsx_sensor_id id = sensor->id; - int i, err; - u8 val; + int i; for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) - if (st_lsm6dsx_odr_table[id].odr_avl[i].hz == odr) + if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz == odr) break; if (i == ST_LSM6DSX_ODR_LIST_SIZE) return -EINVAL; - val = st_lsm6dsx_odr_table[id].odr_avl[i].val; - err = st_lsm6dsx_write_with_mask(sensor->hw, - st_lsm6dsx_odr_table[id].reg.addr, - st_lsm6dsx_odr_table[id].reg.mask, - val); - if (err < 0) - return err; - + *val = st_lsm6dsx_odr_table[sensor->id].odr_avl[i].val; sensor->odr = odr; return 0; } +static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) +{ + enum st_lsm6dsx_sensor_id id = sensor->id; + int err; + u8 val; + + err = st_lsm6dsx_check_odr(sensor, odr, &val); + if (err < 0) + return err; + + return st_lsm6dsx_write_with_mask(sensor->hw, + st_lsm6dsx_odr_table[id].reg.addr, + st_lsm6dsx_odr_table[id].reg.mask, + val); +} + int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor) { int err; @@ -436,9 +444,12 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, case IIO_CHAN_INFO_SCALE: err = st_lsm6dsx_set_full_scale(sensor, val2); break; - case IIO_CHAN_INFO_SAMP_FREQ: - err = st_lsm6dsx_set_odr(sensor, val); + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_lsm6dsx_check_odr(sensor, val, &data); break; + } default: err = -EINVAL; break; |