iio: imu: inv_mpu6050: clean read raw by factorizing out raw data

Factorize reading channel data in its own function.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 45f57f734e3b1b903bbd8a6c3a28b73c54400a8f..79b44fd42c137ab77f8707e9f35d58e46e97651b 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -332,6 +332,67 @@ 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,
@@ -342,63 +403,10 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev,
 
        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;
-       }
        case IIO_CHAN_INFO_SCALE:
                switch (chan->type) {
                case IIO_ANGL_VEL: