[8/8] iio: imu: inv_mpu6050: add fifo support for magnetometer data
diff mbox series

Message ID 20190829151801.13014-9-jmaneyrol@invensense.com
State New
Headers show
Series
  • add magnetometer support for MPU925x
Related show

Commit Message

Jean-Baptiste Maneyrol Aug. 29, 2019, 3:18 p.m. UTC
Put read magnetometer data by mpu inside the fifo.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c    |  1 +
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 14 ++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 90 +++++++++++++++++--
 4 files changed, 94 insertions(+), 13 deletions(-)

Comments

Jonathan Cameron Sept. 8, 2019, 12:07 p.m. UTC | #1
On Thu, 29 Aug 2019 15:18:46 +0000
Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote:

> Put read magnetometer data by mpu inside the fifo.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>

Inline...

> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c    |  1 +
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 14 ++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 90 +++++++++++++++++--
>  4 files changed, 94 insertions(+), 13 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index d08cec6a8a7a..d4a321fd03a9 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -105,6 +105,7 @@ static const struct inv_mpu6050_chip_config chip_config_6050 = {
>  	.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE),
>  	.gyro_fifo_enable = false,
>  	.accl_fifo_enable = false,
> +	.magn_fifo_enable = false,
>  	.accl_fs = INV_MPU6050_FS_02G,
>  	.user_ctrl = 0,
>  };
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index cfc11cb0a36c..48ed66e39cda 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -86,6 +86,7 @@ enum inv_devices {
>   *  @accl_fs:		accel full scale range.
>   *  @accl_fifo_enable:	enable accel data output
>   *  @gyro_fifo_enable:	enable gyro data output
> + *  @magn_fifo_enable:	enable magn data output
>   *  @divider:		chip sample rate divider (sample rate divider - 1)
>   */
>  struct inv_mpu6050_chip_config {
> @@ -94,6 +95,7 @@ struct inv_mpu6050_chip_config {
>  	unsigned int accl_fs:2;
>  	unsigned int accl_fifo_enable:1;
>  	unsigned int gyro_fifo_enable:1;
> +	unsigned int magn_fifo_enable:1;
>  	u8 divider;
>  	u8 user_ctrl;
>  };
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 5f9a5de0bab4..f0a5350c1848 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -14,6 +14,7 @@
>  #include <linux/poll.h>
>  #include <linux/math64.h>
>  #include <asm/unaligned.h>
> +

Clean this stuff up - should not be in this patch.


>  #include "inv_mpu_iio.h"
>  
>  /**
> @@ -124,7 +125,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  
>  	/* enable interrupt */
>  	if (st->chip_config.accl_fifo_enable ||
> -	    st->chip_config.gyro_fifo_enable) {
> +	    st->chip_config.gyro_fifo_enable ||
> +	    st->chip_config.magn_fifo_enable) {
>  		result = regmap_write(st->map, st->reg->int_enable,
>  				      INV_MPU6050_BIT_DATA_RDY_EN);
>  		if (result)
> @@ -141,6 +143,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  		d |= INV_MPU6050_BITS_GYRO_OUT;
>  	if (st->chip_config.accl_fifo_enable)
>  		d |= INV_MPU6050_BIT_ACCEL_OUT;
> +	if (st->chip_config.magn_fifo_enable)
> +		d |= INV_MPU6050_BIT_SLAVE_0;
>  	result = regmap_write(st->map, st->reg->fifo_en, d);
>  	if (result)
>  		goto reset_fifo_fail;
> @@ -190,17 +194,19 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	}
>  
>  	if (!(st->chip_config.accl_fifo_enable |
> -		st->chip_config.gyro_fifo_enable))
> +		st->chip_config.gyro_fifo_enable |
> +		st->chip_config.magn_fifo_enable))
>  		goto end_session;
> +
>  	bytes_per_datum = 0;
>  	if (st->chip_config.accl_fifo_enable)
>  		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
> -
I don't want to see white space changes in a patch doing something else.
Better to just be consistent and carry on with the blank line for your new
case.

>  	if (st->chip_config.gyro_fifo_enable)
>  		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
> -
>  	if (st->chip_type == INV_ICM20602)
>  		bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;
> +	if (st->chip_config.magn_fifo_enable)
> +		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
>  
>  	/*
>  	 * read fifo_count register to know how many bytes are inside the FIFO
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> index dd55e70b6f77..fa4928e589b9 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> @@ -5,7 +5,7 @@
>  
>  #include "inv_mpu_iio.h"
>  
> -static void inv_scan_query(struct iio_dev *indio_dev)
> +static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
>  {
>  	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
>  
> @@ -26,6 +26,52 @@ static void inv_scan_query(struct iio_dev *indio_dev)
>  			 indio_dev->active_scan_mask);
>  }
>  
> +#ifdef CONFIG_INV_MPU6050_MAGN
> +static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +
> +	st->chip_config.gyro_fifo_enable =
> +		test_bit(INV_MPU9X50_SCAN_GYRO_X,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_GYRO_Y,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_GYRO_Z,
> +			 indio_dev->active_scan_mask);
> +
> +	st->chip_config.accl_fifo_enable =
> +		test_bit(INV_MPU9X50_SCAN_ACCL_X,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_ACCL_Y,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_ACCL_Z,
> +			 indio_dev->active_scan_mask);
> +
> +	st->chip_config.magn_fifo_enable =
> +		test_bit(INV_MPU9X50_SCAN_MAGN_X,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_MAGN_Y,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_MAGN_Z,
> +			 indio_dev->active_scan_mask);
> +}
> +#endif
> +
> +static void inv_scan_query(struct iio_dev *indio_dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +
> +	switch (st->chip_type) {
> +#ifdef CONFIG_INV_MPU6050_MAGN
> +	case INV_MPU9250:
> +	case INV_MPU9255:
> +		return inv_scan_query_mpu9x50(indio_dev);
> +#endif
> +	default:
> +		return inv_scan_query_mpu6050(indio_dev);
> +	}
> +}
> +
>  /**
>   *  inv_mpu6050_set_enable() - enable chip functions.
>   *  @indio_dev:	Device driver instance.
> @@ -34,6 +80,11 @@ static void inv_scan_query(struct iio_dev *indio_dev)
>  static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  {
>  	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	uint8_t d;
> +	unsigned int gyro_skip = 0;
> +	unsigned int accl_skip = 0;
> +	unsigned int magn_skip = 0;
> +	unsigned int skip_samples;
>  	int result;
>  
>  	if (enable) {
> @@ -41,14 +92,13 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  		if (result)
>  			return result;
>  		inv_scan_query(indio_dev);
> -		st->skip_samples = 0;
>  		if (st->chip_config.gyro_fifo_enable) {
>  			result = inv_mpu6050_switch_engine(st, true,
>  					INV_MPU6050_BIT_PWR_GYRO_STBY);
>  			if (result)
>  				goto error_power_off;
>  			/* gyro first sample is out of specs, skip it */
> -			st->skip_samples = 1;
> +			gyro_skip = 1;
>  		}
>  		if (st->chip_config.accl_fifo_enable) {
>  			result = inv_mpu6050_switch_engine(st, true,
> @@ -56,22 +106,40 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  			if (result)
>  				goto error_gyro_off;
>  		}
> +		if (st->chip_config.magn_fifo_enable) {
> +			d = st->chip_config.user_ctrl |
> +					INV_MPU6050_BIT_I2C_MST_EN;
> +			result = regmap_write(st->map, st->reg->user_ctrl, d);
> +			if (result)
> +				goto error_accl_off;
> +			st->chip_config.user_ctrl = d;
> +			/* mag first sample is always not ready, skip it */
> +			magn_skip = 1;
> +		}
> +		/* compute first samples to skip */
> +		skip_samples = gyro_skip;
> +		if (accl_skip > skip_samples)
> +			skip_samples = accl_skip;
> +		if (magn_skip > skip_samples)
> +			skip_samples = magn_skip;
> +		st->skip_samples = skip_samples;
>  		result = inv_reset_fifo(indio_dev);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
>  	} else {
>  		result = regmap_write(st->map, st->reg->fifo_en, 0);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
>  
>  		result = regmap_write(st->map, st->reg->int_enable, 0);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
>  
> -		result = regmap_write(st->map, st->reg->user_ctrl,
> -				      st->chip_config.user_ctrl);
> +		d = st->chip_config.user_ctrl & ~INV_MPU6050_BIT_I2C_MST_EN;
> +		result = regmap_write(st->map, st->reg->user_ctrl, d);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
> +		st->chip_config.user_ctrl = d;
>  
>  		result = inv_mpu6050_switch_engine(st, false,
>  					INV_MPU6050_BIT_PWR_ACCL_STBY);
> @@ -90,6 +158,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  
>  	return 0;
>  
> +error_magn_off:
> +	/* always restore user_ctrl to disable fifo properly */
> +	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;
> +	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
>  error_accl_off:
>  	if (st->chip_config.accl_fifo_enable)
>  		inv_mpu6050_switch_engine(st, false,

Patch
diff mbox series

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index d08cec6a8a7a..d4a321fd03a9 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -105,6 +105,7 @@  static const struct inv_mpu6050_chip_config chip_config_6050 = {
 	.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE),
 	.gyro_fifo_enable = false,
 	.accl_fifo_enable = false,
+	.magn_fifo_enable = false,
 	.accl_fs = INV_MPU6050_FS_02G,
 	.user_ctrl = 0,
 };
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index cfc11cb0a36c..48ed66e39cda 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -86,6 +86,7 @@  enum inv_devices {
  *  @accl_fs:		accel full scale range.
  *  @accl_fifo_enable:	enable accel data output
  *  @gyro_fifo_enable:	enable gyro data output
+ *  @magn_fifo_enable:	enable magn data output
  *  @divider:		chip sample rate divider (sample rate divider - 1)
  */
 struct inv_mpu6050_chip_config {
@@ -94,6 +95,7 @@  struct inv_mpu6050_chip_config {
 	unsigned int accl_fs:2;
 	unsigned int accl_fifo_enable:1;
 	unsigned int gyro_fifo_enable:1;
+	unsigned int magn_fifo_enable:1;
 	u8 divider;
 	u8 user_ctrl;
 };
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 5f9a5de0bab4..f0a5350c1848 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -14,6 +14,7 @@ 
 #include <linux/poll.h>
 #include <linux/math64.h>
 #include <asm/unaligned.h>
+
 #include "inv_mpu_iio.h"
 
 /**
@@ -124,7 +125,8 @@  int inv_reset_fifo(struct iio_dev *indio_dev)
 
 	/* enable interrupt */
 	if (st->chip_config.accl_fifo_enable ||
-	    st->chip_config.gyro_fifo_enable) {
+	    st->chip_config.gyro_fifo_enable ||
+	    st->chip_config.magn_fifo_enable) {
 		result = regmap_write(st->map, st->reg->int_enable,
 				      INV_MPU6050_BIT_DATA_RDY_EN);
 		if (result)
@@ -141,6 +143,8 @@  int inv_reset_fifo(struct iio_dev *indio_dev)
 		d |= INV_MPU6050_BITS_GYRO_OUT;
 	if (st->chip_config.accl_fifo_enable)
 		d |= INV_MPU6050_BIT_ACCEL_OUT;
+	if (st->chip_config.magn_fifo_enable)
+		d |= INV_MPU6050_BIT_SLAVE_0;
 	result = regmap_write(st->map, st->reg->fifo_en, d);
 	if (result)
 		goto reset_fifo_fail;
@@ -190,17 +194,19 @@  irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	}
 
 	if (!(st->chip_config.accl_fifo_enable |
-		st->chip_config.gyro_fifo_enable))
+		st->chip_config.gyro_fifo_enable |
+		st->chip_config.magn_fifo_enable))
 		goto end_session;
+
 	bytes_per_datum = 0;
 	if (st->chip_config.accl_fifo_enable)
 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
-
 	if (st->chip_config.gyro_fifo_enable)
 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
-
 	if (st->chip_type == INV_ICM20602)
 		bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;
+	if (st->chip_config.magn_fifo_enable)
+		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
 
 	/*
 	 * read fifo_count register to know how many bytes are inside the FIFO
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index dd55e70b6f77..fa4928e589b9 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -5,7 +5,7 @@ 
 
 #include "inv_mpu_iio.h"
 
-static void inv_scan_query(struct iio_dev *indio_dev)
+static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
 {
 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 
@@ -26,6 +26,52 @@  static void inv_scan_query(struct iio_dev *indio_dev)
 			 indio_dev->active_scan_mask);
 }
 
+#ifdef CONFIG_INV_MPU6050_MAGN
+static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+	st->chip_config.gyro_fifo_enable =
+		test_bit(INV_MPU9X50_SCAN_GYRO_X,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_GYRO_Y,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_GYRO_Z,
+			 indio_dev->active_scan_mask);
+
+	st->chip_config.accl_fifo_enable =
+		test_bit(INV_MPU9X50_SCAN_ACCL_X,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_ACCL_Y,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_ACCL_Z,
+			 indio_dev->active_scan_mask);
+
+	st->chip_config.magn_fifo_enable =
+		test_bit(INV_MPU9X50_SCAN_MAGN_X,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_MAGN_Y,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_MAGN_Z,
+			 indio_dev->active_scan_mask);
+}
+#endif
+
+static void inv_scan_query(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+	switch (st->chip_type) {
+#ifdef CONFIG_INV_MPU6050_MAGN
+	case INV_MPU9250:
+	case INV_MPU9255:
+		return inv_scan_query_mpu9x50(indio_dev);
+#endif
+	default:
+		return inv_scan_query_mpu6050(indio_dev);
+	}
+}
+
 /**
  *  inv_mpu6050_set_enable() - enable chip functions.
  *  @indio_dev:	Device driver instance.
@@ -34,6 +80,11 @@  static void inv_scan_query(struct iio_dev *indio_dev)
 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 {
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	uint8_t d;
+	unsigned int gyro_skip = 0;
+	unsigned int accl_skip = 0;
+	unsigned int magn_skip = 0;
+	unsigned int skip_samples;
 	int result;
 
 	if (enable) {
@@ -41,14 +92,13 @@  static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (result)
 			return result;
 		inv_scan_query(indio_dev);
-		st->skip_samples = 0;
 		if (st->chip_config.gyro_fifo_enable) {
 			result = inv_mpu6050_switch_engine(st, true,
 					INV_MPU6050_BIT_PWR_GYRO_STBY);
 			if (result)
 				goto error_power_off;
 			/* gyro first sample is out of specs, skip it */
-			st->skip_samples = 1;
+			gyro_skip = 1;
 		}
 		if (st->chip_config.accl_fifo_enable) {
 			result = inv_mpu6050_switch_engine(st, true,
@@ -56,22 +106,40 @@  static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 			if (result)
 				goto error_gyro_off;
 		}
+		if (st->chip_config.magn_fifo_enable) {
+			d = st->chip_config.user_ctrl |
+					INV_MPU6050_BIT_I2C_MST_EN;
+			result = regmap_write(st->map, st->reg->user_ctrl, d);
+			if (result)
+				goto error_accl_off;
+			st->chip_config.user_ctrl = d;
+			/* mag first sample is always not ready, skip it */
+			magn_skip = 1;
+		}
+		/* compute first samples to skip */
+		skip_samples = gyro_skip;
+		if (accl_skip > skip_samples)
+			skip_samples = accl_skip;
+		if (magn_skip > skip_samples)
+			skip_samples = magn_skip;
+		st->skip_samples = skip_samples;
 		result = inv_reset_fifo(indio_dev);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
 	} else {
 		result = regmap_write(st->map, st->reg->fifo_en, 0);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
 
 		result = regmap_write(st->map, st->reg->int_enable, 0);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
 
-		result = regmap_write(st->map, st->reg->user_ctrl,
-				      st->chip_config.user_ctrl);
+		d = st->chip_config.user_ctrl & ~INV_MPU6050_BIT_I2C_MST_EN;
+		result = regmap_write(st->map, st->reg->user_ctrl, d);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
+		st->chip_config.user_ctrl = d;
 
 		result = inv_mpu6050_switch_engine(st, false,
 					INV_MPU6050_BIT_PWR_ACCL_STBY);
@@ -90,6 +158,10 @@  static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 
 	return 0;
 
+error_magn_off:
+	/* always restore user_ctrl to disable fifo properly */
+	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;
+	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
 error_accl_off:
 	if (st->chip_config.accl_fifo_enable)
 		inv_mpu6050_switch_engine(st, false,