diff --git a/drivers/iio/adc/ad7192.c b/drivers/iio/adc/ad7192.c index e96a5ae92375..785429900da8 100644 --- a/drivers/iio/adc/ad7192.c +++ b/drivers/iio/adc/ad7192.c @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -945,80 +946,82 @@ static int ad7192_read_raw(struct iio_dev *indio_dev, return -EINVAL; } +static int __ad7192_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, + int val2, + long mask) +{ + struct ad7192_state *st = iio_priv(indio_dev); + int i, div; + unsigned int tmp; + + guard(mutex)(&st->lock); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++) { + if (val2 != st->scale_avail[i][1]) + continue; + + tmp = st->conf; + st->conf &= ~AD7192_CONF_GAIN_MASK; + st->conf |= FIELD_PREP(AD7192_CONF_GAIN_MASK, i); + if (tmp == st->conf) + return 0; + ad_sd_write_reg(&st->sd, AD7192_REG_CONF, 3, st->conf); + ad7192_calibrate_all(st); + return 0; + } + return -EINVAL; + case IIO_CHAN_INFO_SAMP_FREQ: + if (!val) + return -EINVAL; + + div = st->fclk / (val * ad7192_get_f_order(st) * 1024); + if (div < 1 || div > 1023) + return -EINVAL; + + st->mode &= ~AD7192_MODE_RATE_MASK; + st->mode |= FIELD_PREP(AD7192_MODE_RATE_MASK, div); + ad_sd_write_reg(&st->sd, AD7192_REG_MODE, 3, st->mode); + ad7192_update_filter_freq_avail(st); + return 0; + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + return ad7192_set_3db_filter_freq(st, val, val2 / 1000); + case IIO_CHAN_INFO_OVERSAMPLING_RATIO: + for (i = 0; i < ARRAY_SIZE(st->oversampling_ratio_avail); i++) { + if (val != st->oversampling_ratio_avail[i]) + continue; + + tmp = st->mode; + st->mode &= ~AD7192_MODE_AVG_MASK; + st->mode |= FIELD_PREP(AD7192_MODE_AVG_MASK, i); + if (tmp == st->mode) + return 0; + ad_sd_write_reg(&st->sd, AD7192_REG_MODE, 3, st->mode); + ad7192_update_filter_freq_avail(st); + return 0; + } + return -EINVAL; + default: + return -EINVAL; + } +} + static int ad7192_write_raw(struct iio_dev *indio_dev, struct iio_chan_spec const *chan, int val, int val2, long mask) { - struct ad7192_state *st = iio_priv(indio_dev); - int ret, i, div; - unsigned int tmp; + int ret; ret = iio_device_claim_direct_mode(indio_dev); if (ret) return ret; - mutex_lock(&st->lock); - - switch (mask) { - case IIO_CHAN_INFO_SCALE: - ret = -EINVAL; - for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++) - if (val2 == st->scale_avail[i][1]) { - ret = 0; - tmp = st->conf; - st->conf &= ~AD7192_CONF_GAIN_MASK; - st->conf |= FIELD_PREP(AD7192_CONF_GAIN_MASK, i); - if (tmp == st->conf) - break; - ad_sd_write_reg(&st->sd, AD7192_REG_CONF, - 3, st->conf); - ad7192_calibrate_all(st); - break; - } - break; - case IIO_CHAN_INFO_SAMP_FREQ: - if (!val) { - ret = -EINVAL; - break; - } - - div = st->fclk / (val * ad7192_get_f_order(st) * 1024); - if (div < 1 || div > 1023) { - ret = -EINVAL; - break; - } - - st->mode &= ~AD7192_MODE_RATE_MASK; - st->mode |= FIELD_PREP(AD7192_MODE_RATE_MASK, div); - ad_sd_write_reg(&st->sd, AD7192_REG_MODE, 3, st->mode); - ad7192_update_filter_freq_avail(st); - break; - case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: - ret = ad7192_set_3db_filter_freq(st, val, val2 / 1000); - break; - case IIO_CHAN_INFO_OVERSAMPLING_RATIO: - ret = -EINVAL; - for (i = 0; i < ARRAY_SIZE(st->oversampling_ratio_avail); i++) - if (val == st->oversampling_ratio_avail[i]) { - ret = 0; - tmp = st->mode; - st->mode &= ~AD7192_MODE_AVG_MASK; - st->mode |= FIELD_PREP(AD7192_MODE_AVG_MASK, i); - if (tmp == st->mode) - break; - ad_sd_write_reg(&st->sd, AD7192_REG_MODE, - 3, st->mode); - break; - } - ad7192_update_filter_freq_avail(st); - break; - default: - ret = -EINVAL; - } - - mutex_unlock(&st->lock); + ret = __ad7192_write_raw(indio_dev, chan, val, val2, mask); iio_device_release_direct_mode(indio_dev);