1 files changed, 39 insertions, 52 deletions

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index 5199fe790c30..cfd7243159f6 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -5,9 +5,10 @@
 
 #include "inv_mpu_iio.h"
 
-static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
+static unsigned int inv_scan_query_mpu6050(struct iio_dev *indio_dev)
 {
 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
+	unsigned int mask;
 
 	st->chip_config.gyro_fifo_enable =
 		test_bit(INV_MPU6050_SCAN_GYRO_X,
@@ -27,17 +28,28 @@ static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
 
 	st->chip_config.temp_fifo_enable =
 		test_bit(INV_MPU6050_SCAN_TEMP, indio_dev->active_scan_mask);
+
+	mask = 0;
+	if (st->chip_config.gyro_fifo_enable)
+		mask |= INV_MPU6050_SENSOR_GYRO;
+	if (st->chip_config.accl_fifo_enable)
+		mask |= INV_MPU6050_SENSOR_ACCL;
+	if (st->chip_config.temp_fifo_enable)
+		mask |= INV_MPU6050_SENSOR_TEMP;
+
+	return mask;
 }
 
-static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
+static unsigned int inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
 {
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	unsigned int mask;
 
-	inv_scan_query_mpu6050(indio_dev);
+	mask = inv_scan_query_mpu6050(indio_dev);
 
 	/* no magnetometer if i2c auxiliary bus is used */
 	if (st->magn_disabled)
-		return;
+		return mask;
 
 	st->chip_config.magn_fifo_enable =
 		test_bit(INV_MPU9X50_SCAN_MAGN_X,
@@ -46,9 +58,13 @@ static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
 			 indio_dev->active_scan_mask) ||
 		test_bit(INV_MPU9X50_SCAN_MAGN_Z,
 			 indio_dev->active_scan_mask);
+	if (st->chip_config.magn_fifo_enable)
+		mask |= INV_MPU6050_SENSOR_MAGN;
+
+	return mask;
 }
 
-static void inv_scan_query(struct iio_dev *indio_dev)
+static unsigned int inv_scan_query(struct iio_dev *indio_dev)
 {
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
 
@@ -92,62 +108,40 @@ static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
 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 scan;
 	int result;
 
+	scan = inv_scan_query(indio_dev);
+
 	if (enable) {
 		result = inv_mpu6050_set_power_itg(st, true);
 		if (result)
 			return result;
-		inv_scan_query(indio_dev);
-		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;
-		}
-		if (st->chip_config.accl_fifo_enable) {
-			result = inv_mpu6050_switch_engine(st, true,
-					INV_MPU6050_BIT_PWR_ACCL_STBY);
-			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;
-		}
+		result = inv_mpu6050_switch_engine(st, true, scan);
+		if (result)
+			goto error_power_off;
 		st->skip_samples = inv_compute_skip_samples(st);
 		result = inv_reset_fifo(indio_dev);
 		if (result)
-			goto error_magn_off;
+			goto error_sensors_off;
 	} else {
 		result = regmap_write(st->map, st->reg->fifo_en, 0);
 		if (result)
-			goto error_magn_off;
+			goto error_fifo_off;
 
 		result = regmap_write(st->map, st->reg->int_enable, 0);
 		if (result)
-			goto error_magn_off;
-
-		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_magn_off;
-		st->chip_config.user_ctrl = d;
+			goto error_fifo_off;
 
-		result = inv_mpu6050_switch_engine(st, false,
-					INV_MPU6050_BIT_PWR_ACCL_STBY);
+		/* restore user_ctrl for disabling FIFO reading */
+		result = regmap_write(st->map, st->reg->user_ctrl,
+				      st->chip_config.user_ctrl);
 		if (result)
-			goto error_accl_off;
+			goto error_sensors_off;
 
-		result = inv_mpu6050_switch_engine(st, false,
-					INV_MPU6050_BIT_PWR_GYRO_STBY);
+		result = inv_mpu6050_switch_engine(st, false, scan);
 		if (result)
-			goto error_gyro_off;
+			goto error_power_off;
 
 		result = inv_mpu6050_set_power_itg(st, false);
 		if (result)
@@ -156,18 +150,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 
 	return 0;
 
-error_magn_off:
+error_fifo_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,
-					  INV_MPU6050_BIT_PWR_ACCL_STBY);
-error_gyro_off:
-	if (st->chip_config.gyro_fifo_enable)
-		inv_mpu6050_switch_engine(st, false,
-					  INV_MPU6050_BIT_PWR_GYRO_STBY);
+error_sensors_off:
+	inv_mpu6050_switch_engine(st, false, scan);
 error_power_off:
 	inv_mpu6050_set_power_itg(st, false);
 	return result;