diff mbox series

[v2,10/13] iio: imu: inv_mpu6050: factorize fifo enable/disable

Message ID 20200219143958.3548-11-jmaneyrol@invensense.com (mailing list archive)
State New, archived
Headers show
Series Rework sensors engines and power management | expand

Commit Message

Jean-Baptiste Maneyrol Feb. 19, 2020, 2:39 p.m. UTC 
Rework fifo enable/disable in a separate function.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +-
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 54 ++-------------
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 66 ++++++++++++++-----
 3 files changed, 55 insertions(+), 67 deletions(-)

Comments

Jonathan Cameron Feb. 21, 2020, 11:37 a.m. UTC | #1 
On Wed, 19 Feb 2020 15:39:55 +0100
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:

> Rework fifo enable/disable in a separate function.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>

Applied.

Thanks,

Jonathan

> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 54 ++-------------
>  drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 66 ++++++++++++++-----
>  3 files changed, 55 insertions(+), 67 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index a578789c9210..e328c98e362c 100644
> --- 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 d7397705974e..9511e4715e2c 100644
> --- 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 cfd7243159f6..f53f50d08b9e 100644
> --- 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:
diff mbox series

Patch

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index a578789c9210..e328c98e362c 100644
--- 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 d7397705974e..9511e4715e2c 100644
--- 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 cfd7243159f6..f53f50d08b9e 100644
--- 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: