Factorize reading channel data in its own function and use a single
return path at the end of the global read_raw function.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
---
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 150 ++++++++++++++++-------------
1 file changed, 84 insertions(+), 66 deletions(-)
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 5290e59..c46740f 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -332,72 +332,80 @@ static int inv_mpu6050_sensor_show(struct inv_mpu6050_state *st, int reg,
return IIO_VAL_INT;
}
+static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val)
+{
+ struct inv_mpu6050_state *st = iio_priv(indio_dev);
+ int result;
+ int ret = IIO_VAL_INT;
+
+ result = iio_device_claim_direct_mode(indio_dev);
+ if (result)
+ return result;
+ result = inv_mpu6050_set_power_itg(st, true);
+ if (result)
+ goto error_release;
+
+ switch (chan->type) {
+ case IIO_ANGL_VEL:
+ result = inv_mpu6050_switch_engine(st, true,
+ INV_MPU6050_BIT_PWR_GYRO_STBY);
+ if (result)
+ goto error_power_off;
+ ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
+ chan->channel2, val);
+ result = inv_mpu6050_switch_engine(st, false,
+ INV_MPU6050_BIT_PWR_GYRO_STBY);
+ if (result)
+ goto error_power_off;
+ break;
+ case IIO_ACCEL:
+ result = inv_mpu6050_switch_engine(st, true,
+ INV_MPU6050_BIT_PWR_ACCL_STBY);
+ if (result)
+ goto error_power_off;
+ ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
+ chan->channel2, val);
+ result = inv_mpu6050_switch_engine(st, false,
+ INV_MPU6050_BIT_PWR_ACCL_STBY);
+ if (result)
+ goto error_power_off;
+ break;
+ case IIO_TEMP:
+ /* wait for stablization */
+ msleep(INV_MPU6050_SENSOR_UP_TIME);
+ ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
+ IIO_MOD_X, val);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+error_power_off:
+ result |= inv_mpu6050_set_power_itg(st, false);
+error_release:
+ iio_device_release_direct_mode(indio_dev);
+ if (result)
+ return result;
+
+ return ret;
+}
+
static int inv_mpu6050_read_raw(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int *val, int *val2, long mask)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
- int ret = 0;
+ int ret;
switch (mask) {
case IIO_CHAN_INFO_RAW:
- {
- int result;
-
- ret = IIO_VAL_INT;
mutex_lock(&st->lock);
- result = iio_device_claim_direct_mode(indio_dev);
- if (result)
- goto error_read_raw_unlock;
- result = inv_mpu6050_set_power_itg(st, true);
- if (result)
- goto error_read_raw_release;
- switch (chan->type) {
- case IIO_ANGL_VEL:
- result = inv_mpu6050_switch_engine(st, true,
- INV_MPU6050_BIT_PWR_GYRO_STBY);
- if (result)
- goto error_read_raw_power_off;
- ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
- chan->channel2, val);
- result = inv_mpu6050_switch_engine(st, false,
- INV_MPU6050_BIT_PWR_GYRO_STBY);
- if (result)
- goto error_read_raw_power_off;
- break;
- case IIO_ACCEL:
- result = inv_mpu6050_switch_engine(st, true,
- INV_MPU6050_BIT_PWR_ACCL_STBY);
- if (result)
- goto error_read_raw_power_off;
- ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
- chan->channel2, val);
- result = inv_mpu6050_switch_engine(st, false,
- INV_MPU6050_BIT_PWR_ACCL_STBY);
- if (result)
- goto error_read_raw_power_off;
- break;
- case IIO_TEMP:
- /* wait for stablization */
- msleep(INV_MPU6050_SENSOR_UP_TIME);
- ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
- IIO_MOD_X, val);
- break;
- default:
- ret = -EINVAL;
- break;
- }
-error_read_raw_power_off:
- result |= inv_mpu6050_set_power_itg(st, false);
-error_read_raw_release:
- iio_device_release_direct_mode(indio_dev);
-error_read_raw_unlock:
+ ret = inv_mpu6050_read_channel_data(indio_dev, chan, val);
mutex_unlock(&st->lock);
- if (result)
- return result;
-
- return ret;
- }
+ break;
case IIO_CHAN_INFO_SCALE:
switch (chan->type) {
case IIO_ANGL_VEL:
@@ -405,27 +413,33 @@ static int inv_mpu6050_read_raw(struct iio_dev *indio_dev,
*val = 0;
*val2 = gyro_scale_6050[st->chip_config.fsr];
mutex_unlock(&st->lock);
- return IIO_VAL_INT_PLUS_NANO;
+ ret = IIO_VAL_INT_PLUS_NANO;
+ break;
case IIO_ACCEL:
mutex_lock(&st->lock);
*val = 0;
*val2 = accel_scale[st->chip_config.accl_fs];
mutex_unlock(&st->lock);
- return IIO_VAL_INT_PLUS_MICRO;
+ ret = IIO_VAL_INT_PLUS_MICRO;
+ break;
case IIO_TEMP:
*val = 0;
*val2 = INV_MPU6050_TEMP_SCALE;
- return IIO_VAL_INT_PLUS_MICRO;
+ ret = IIO_VAL_INT_PLUS_MICRO;
+ break;
default:
- return -EINVAL;
+ ret = -EINVAL;
+ break;
}
case IIO_CHAN_INFO_OFFSET:
switch (chan->type) {
case IIO_TEMP:
*val = INV_MPU6050_TEMP_OFFSET;
- return IIO_VAL_INT;
+ ret = IIO_VAL_INT;
+ break;
default:
- return -EINVAL;
+ ret = -EINVAL;
+ break;
}
case IIO_CHAN_INFO_CALIBBIAS:
switch (chan->type) {
@@ -434,19 +448,23 @@ static int inv_mpu6050_read_raw(struct iio_dev *indio_dev,
ret = inv_mpu6050_sensor_show(st, st->reg->gyro_offset,
chan->channel2, val);
mutex_unlock(&st->lock);
- return IIO_VAL_INT;
+ break;
case IIO_ACCEL:
mutex_lock(&st->lock);
ret = inv_mpu6050_sensor_show(st, st->reg->accl_offset,
chan->channel2, val);
mutex_unlock(&st->lock);
- return IIO_VAL_INT;
+ break;
default:
- return -EINVAL;
+ ret = -EINVAL;
+ break;
}
default:
- return -EINVAL;
+ ret = -EINVAL;
+ break;
}
+
+ return ret;
}
static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val)
--
1.9.1
--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
