diff --git a/drivers/staging/iio/imu/adis16400_core.c b/drivers/staging/iio/imu/adis16400_core.c index cb6622578e9b..7114de94f412 100644 --- a/drivers/staging/iio/imu/adis16400_core.c +++ b/drivers/staging/iio/imu/adis16400_core.c @@ -242,33 +242,32 @@ static ssize_t adis16400_read_frequency(struct device *dev, static const unsigned adis16400_3db_divisors[] = { [0] = 2, /* Special case */ - [1] = 5, - [2] = 10, - [3] = 50, - [4] = 200, + [1] = 6, + [2] = 12, + [3] = 25, + [4] = 50, + [5] = 100, + [6] = 200, + [7] = 200, /* Not a valid setting */ }; static int adis16400_set_filter(struct iio_dev *indio_dev, int sps, int val) { int i, ret; u16 val16; - for (i = ARRAY_SIZE(adis16400_3db_divisors) - 1; i >= 0; i--) - if (sps/adis16400_3db_divisors[i] > val) - break; - if (i == -1) - ret = -EINVAL; - else { - ret = adis16400_spi_read_reg_16(indio_dev, - ADIS16400_SENS_AVG, - &val16); - if (ret < 0) - goto error_ret; - ret = adis16400_spi_write_reg_16(indio_dev, - ADIS16400_SENS_AVG, - (val16 & ~0x03) | i); + for (i = ARRAY_SIZE(adis16400_3db_divisors) - 1; i >= 1; i--) { + if (sps / adis16400_3db_divisors[i] >= val) + break; } -error_ret: + + ret = adis16400_spi_read_reg_16(indio_dev, ADIS16400_SENS_AVG, + &val16); + if (ret < 0) + return ret; + + ret = adis16400_spi_write_reg_16(indio_dev, ADIS16400_SENS_AVG, + (val16 & ~0x07) | i); return ret; } @@ -653,9 +652,9 @@ static int adis16400_read_raw(struct iio_dev *indio_dev, mutex_unlock(&indio_dev->mlock); return ret; } - val16 = st->variant->get_freq(indio_dev); - if (ret > 0) - *val = ret/adis16400_3db_divisors[val16 & 0x03]; + ret = st->variant->get_freq(indio_dev); + if (ret >= 0) + *val = ret / adis16400_3db_divisors[val16 & 0x07]; *val2 = 0; mutex_unlock(&indio_dev->mlock); if (ret < 0)