From 305f655d059d68d0e2973294eb9139eaba22e83c Mon Sep 17 00:00:00 2001 From: Jonathan Cameron Date: Mon, 17 Feb 2025 14:01:31 +0000 Subject: [PATCH] iio: accel: kx022a: Factor out guts of write_raw() to allow direct returns Create a new utility function for the actions taken when direct mode is held. This allows for direct returns, simplifying the code flow. Cc: Matti Vaittinen Reviewed-by: Matti Vaittinen Reviewed-by: David Lechner Link: https://patch.msgid.link/20250217140135.896574-5-jic23@kernel.org Signed-off-by: Jonathan Cameron --- drivers/iio/accel/kionix-kx022a.c | 104 ++++++++++++++++-------------- 1 file changed, 54 insertions(+), 50 deletions(-) diff --git a/drivers/iio/accel/kionix-kx022a.c b/drivers/iio/accel/kionix-kx022a.c index 3a56ab00791a..727e007c5fc1 100644 --- a/drivers/iio/accel/kionix-kx022a.c +++ b/drivers/iio/accel/kionix-kx022a.c @@ -510,12 +510,63 @@ static int kx022a_write_raw_get_fmt(struct iio_dev *idev, } } +static int __kx022a_write_raw(struct iio_dev *idev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct kx022a_data *data = iio_priv(idev); + int ret, n; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + n = ARRAY_SIZE(kx022a_accel_samp_freq_table); + + while (n--) + if (val == kx022a_accel_samp_freq_table[n][0] && + val2 == kx022a_accel_samp_freq_table[n][1]) + break; + if (n < 0) + return -EINVAL; + + ret = kx022a_turn_off_lock(data); + if (ret) + return ret; + + ret = regmap_update_bits(data->regmap, + data->chip_info->odcntl, + KX022A_MASK_ODR, n); + data->odr_ns = kx022a_odrs[n]; + kx022a_turn_on_unlock(data); + return ret; + case IIO_CHAN_INFO_SCALE: + n = data->chip_info->scale_table_size / 2; + + while (n-- > 0) + if (val == data->chip_info->scale_table[n][0] && + val2 == data->chip_info->scale_table[n][1]) + break; + if (n < 0) + return -EINVAL; + + ret = kx022a_turn_off_lock(data); + if (ret) + return ret; + + ret = regmap_update_bits(data->regmap, data->chip_info->cntl, + KX022A_MASK_GSEL, + n << KX022A_GSEL_SHIFT); + kx022a_turn_on_unlock(data); + return ret; + default: + return -EINVAL; + } +} + static int kx022a_write_raw(struct iio_dev *idev, struct iio_chan_spec const *chan, int val, int val2, long mask) { - struct kx022a_data *data = iio_priv(idev); - int ret, n; + int ret; /* * We should not allow changing scale or frequency when FIFO is running @@ -530,55 +581,8 @@ static int kx022a_write_raw(struct iio_dev *idev, if (ret) return ret; - switch (mask) { - case IIO_CHAN_INFO_SAMP_FREQ: - n = ARRAY_SIZE(kx022a_accel_samp_freq_table); + ret = __kx022a_write_raw(idev, chan, val, val2, mask); - while (n--) - if (val == kx022a_accel_samp_freq_table[n][0] && - val2 == kx022a_accel_samp_freq_table[n][1]) - break; - if (n < 0) { - ret = -EINVAL; - goto unlock_out; - } - ret = kx022a_turn_off_lock(data); - if (ret) - break; - - ret = regmap_update_bits(data->regmap, - data->chip_info->odcntl, - KX022A_MASK_ODR, n); - data->odr_ns = kx022a_odrs[n]; - kx022a_turn_on_unlock(data); - break; - case IIO_CHAN_INFO_SCALE: - n = data->chip_info->scale_table_size / 2; - - while (n-- > 0) - if (val == data->chip_info->scale_table[n][0] && - val2 == data->chip_info->scale_table[n][1]) - break; - if (n < 0) { - ret = -EINVAL; - goto unlock_out; - } - - ret = kx022a_turn_off_lock(data); - if (ret) - break; - - ret = regmap_update_bits(data->regmap, data->chip_info->cntl, - KX022A_MASK_GSEL, - n << KX022A_GSEL_SHIFT); - kx022a_turn_on_unlock(data); - break; - default: - ret = -EINVAL; - break; - } - -unlock_out: iio_device_release_direct_mode(idev); return ret;