iio: imu: inv_mpu6050: factorize fifo enable/disable

Rework fifo enable/disable in a separate 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_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index a578789c9210fb8921ad2bb719dfd916998b86b0..e328c98e362c0d932813f93cb562cf773205c798 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -435,7 +435,7 @@ enum inv_mpu6050_clock_sel_e {
 
 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p);
 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type);
-int inv_reset_fifo(struct iio_dev *indio_dev);
+int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable);
 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
                              unsigned int mask);
 int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val);

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index d7397705974ecd5f202450d966bfcd03d1876dc7..9511e4715e2c1680b01fb6970ec96ee71a8346c6 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -90,60 +90,14 @@ static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
        return ts;
 }
 
-int inv_reset_fifo(struct iio_dev *indio_dev)
+static int inv_reset_fifo(struct iio_dev *indio_dev)
 {
        int result;
-       u8 d;
        struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 
-       /* reset it timestamp validation */
-       st->it_timestamp = 0;
-
-       /* disable interrupt */
-       result = regmap_write(st->map, st->reg->int_enable, 0);
-       if (result) {
-               dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
-                       result);
-               return result;
-       }
-       /* disable the sensor output to FIFO */
-       result = regmap_write(st->map, st->reg->fifo_en, 0);
-       if (result)
-               goto reset_fifo_fail;
-       /* disable fifo reading */
-       result = regmap_write(st->map, st->reg->user_ctrl,
-                             st->chip_config.user_ctrl);
-       if (result)
-               goto reset_fifo_fail;
-
-       /* reset FIFO*/
-       d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
-       result = regmap_write(st->map, st->reg->user_ctrl, d);
-       if (result)
-               goto reset_fifo_fail;
-
-       /* enable interrupt */
-       result = regmap_write(st->map, st->reg->int_enable,
-                             INV_MPU6050_BIT_DATA_RDY_EN);
-       if (result)
-               return result;
-
-       /* enable FIFO reading */
-       d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
-       result = regmap_write(st->map, st->reg->user_ctrl, d);
-       if (result)
-               goto reset_fifo_fail;
-       /* enable sensor output to FIFO */
-       d = 0;
-       if (st->chip_config.gyro_fifo_enable)
-               d |= INV_MPU6050_BITS_GYRO_OUT;
-       if (st->chip_config.accl_fifo_enable)
-               d |= INV_MPU6050_BIT_ACCEL_OUT;
-       if (st->chip_config.temp_fifo_enable)
-               d |= INV_MPU6050_BIT_TEMP_OUT;
-       if (st->chip_config.magn_fifo_enable)
-               d |= INV_MPU6050_BIT_SLAVE_0;
-       result = regmap_write(st->map, st->reg->fifo_en, d);
+       /* disable fifo and reenable it */
+       inv_mpu6050_prepare_fifo(st, false);
+       result = inv_mpu6050_prepare_fifo(st, true);
        if (result)
                goto reset_fifo_fail;
 

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index cfd7243159f65da719b32c4d572de37ff3979a6b..f53f50d08b9e64859e63d21c4fbd491d20a8b198 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -100,6 +100,54 @@ static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
        return skip_samples;
 }
 
+int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
+{
+       uint8_t d;
+       int ret;
+
+       if (enable) {
+               st->it_timestamp = 0;
+               /* reset FIFO */
+               d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
+               ret = regmap_write(st->map, st->reg->user_ctrl, d);
+               if (ret)
+                       return ret;
+               /* enable sensor output to FIFO */
+               d = 0;
+               if (st->chip_config.gyro_fifo_enable)
+                       d |= INV_MPU6050_BITS_GYRO_OUT;
+               if (st->chip_config.accl_fifo_enable)
+                       d |= INV_MPU6050_BIT_ACCEL_OUT;
+               if (st->chip_config.temp_fifo_enable)
+                       d |= INV_MPU6050_BIT_TEMP_OUT;
+               if (st->chip_config.magn_fifo_enable)
+                       d |= INV_MPU6050_BIT_SLAVE_0;
+               ret = regmap_write(st->map, st->reg->fifo_en, d);
+               if (ret)
+                       return ret;
+               /* enable FIFO reading */
+               d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
+               ret = regmap_write(st->map, st->reg->user_ctrl, d);
+               if (ret)
+                       return ret;
+               /* enable interrupt */
+               ret = regmap_write(st->map, st->reg->int_enable,
+                                  INV_MPU6050_BIT_DATA_RDY_EN);
+       } else {
+               ret = regmap_write(st->map, st->reg->int_enable, 0);
+               if (ret)
+                       return ret;
+               ret = regmap_write(st->map, st->reg->fifo_en, 0);
+               if (ret)
+                       return ret;
+               /* restore user_ctrl for disabling FIFO reading */
+               ret = regmap_write(st->map, st->reg->user_ctrl,
+                                  st->chip_config.user_ctrl);
+       }
+
+       return ret;
+}
+
 /**
  *  inv_mpu6050_set_enable() - enable chip functions.
  *  @indio_dev:        Device driver instance.
@@ -121,24 +169,13 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
                if (result)
                        goto error_power_off;
                st->skip_samples = inv_compute_skip_samples(st);
-               result = inv_reset_fifo(indio_dev);
+               result = inv_mpu6050_prepare_fifo(st, true);
                if (result)
                        goto error_sensors_off;
        } else {
-               result = regmap_write(st->map, st->reg->fifo_en, 0);
-               if (result)
-                       goto error_fifo_off;
-
-               result = regmap_write(st->map, st->reg->int_enable, 0);
-               if (result)
-                       goto error_fifo_off;
-
-               /* restore user_ctrl for disabling FIFO reading */
-               result = regmap_write(st->map, st->reg->user_ctrl,
-                                     st->chip_config.user_ctrl);
+               result = inv_mpu6050_prepare_fifo(st, false);
                if (result)
                        goto error_sensors_off;
-
                result = inv_mpu6050_switch_engine(st, false, scan);
                if (result)
                        goto error_power_off;
@@ -150,9 +187,6 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 
        return 0;
 
-error_fifo_off:
-       /* always restore user_ctrl to disable fifo properly */
-       regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
 error_sensors_off:
        inv_mpu6050_switch_engine(st, false, scan);
 error_power_off: