Commit 92e7407a authored by Jean-Baptiste Maneyrol's avatar Jean-Baptiste Maneyrol Committed by Jonathan Cameron
Browse files

iio: imu: inv_mpu6050: rewrite power and engine management 



Rewrite clock management to use automatic clock switching
present since MPU6500.
Sensors engine management can now turn on or off a batch of
sensors which simplifies usage a lot.
Temperature sensor is now turned on/off depending on usage.

Signed-off-by: default avatarJean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent 398da994

drivers/iio/imu/inv_mpu6050/inv_mpu_core.c

+195 −69
Original line number Diff line number Diff line
@@ -99,9 +99,31 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = { 
};



static const struct inv_mpu6050_chip_config chip_config_6050 = {

	.clk = INV_CLK_INTERNAL,

	.fsr = INV_MPU6050_FSR_2000DPS,

	.lpf = INV_MPU6050_FILTER_20HZ,

	.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50),

	.gyro_en = true,

	.accl_en = true,

	.temp_en = true,

	.magn_en = false,

	.gyro_fifo_enable = false,

	.accl_fifo_enable = false,

	.temp_fifo_enable = false,

	.magn_fifo_enable = false,

	.accl_fs = INV_MPU6050_FS_02G,

	.user_ctrl = 0,

};



static const struct inv_mpu6050_chip_config chip_config_6500 = {

	.clk = INV_CLK_PLL,

	.fsr = INV_MPU6050_FSR_2000DPS,

	.lpf = INV_MPU6050_FILTER_20HZ,

	.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50),

	.gyro_en = true,

	.accl_en = true,

	.temp_en = true,

	.magn_en = false,

	.gyro_fifo_enable = false,

	.accl_fifo_enable = false,

	.temp_fifo_enable = false,
@@ -124,7 +146,7 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_MPU6500_WHOAMI_VALUE,

		.name = "MPU6500",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 512,

		.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},

	},
@@ -132,7 +154,7 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_MPU6515_WHOAMI_VALUE,

		.name = "MPU6515",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 512,

		.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},

	},
@@ -156,7 +178,7 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_MPU9250_WHOAMI_VALUE,

		.name = "MPU9250",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 512,

		.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},

	},
@@ -164,7 +186,7 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_MPU9255_WHOAMI_VALUE,

		.name = "MPU9255",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 512,

		.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},

	},
@@ -172,7 +194,7 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_ICM20608_WHOAMI_VALUE,

		.name = "ICM20608",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 512,

		.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},

	},
@@ -180,7 +202,7 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_ICM20609_WHOAMI_VALUE,

		.name = "ICM20609",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 4 * 1024,

		.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},

	},
@@ -188,7 +210,7 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_ICM20689_WHOAMI_VALUE,

		.name = "ICM20689",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 4 * 1024,

		.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},

	},
@@ -196,15 +218,15 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_ICM20602_WHOAMI_VALUE,

		.name = "ICM20602",

		.reg = &reg_set_icm20602,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 1008,

		.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},

	},

	{

		.whoami = INV_ICM20690_WHOAMI_VALUE,

		.name = "ICM20690",

		.reg = &reg_set_icm20602,

		.config = &chip_config_6050,

		.reg = &reg_set_6500,

		.config = &chip_config_6500,

		.fifo_size = 1024,

		.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},

	},
@@ -212,61 +234,162 @@ static const struct inv_mpu6050_hw hw_info[] = { 
		.whoami = INV_IAM20680_WHOAMI_VALUE,

		.name = "IAM20680",

		.reg = &reg_set_6500,

		.config = &chip_config_6050,

		.config = &chip_config_6500,

		.fifo_size = 512,

		.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},

	},

};



int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)

static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep,

					int clock, int temp_dis)

{

	unsigned int d, mgmt_1;

	int result;

	/*

	 * switch clock needs to be careful. Only when gyro is on, can

	 * clock source be switched to gyro. Otherwise, it must be set to

	 * internal clock

	 */

	if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) {

		result = regmap_read(st->map, st->reg->pwr_mgmt_1, &mgmt_1);

		if (result)

			return result;

	u8 val;



	if (clock < 0)

		clock = st->chip_config.clk;

	if (temp_dis < 0)

		temp_dis = !st->chip_config.temp_en;



	val = clock & INV_MPU6050_BIT_CLK_MASK;

	if (temp_dis)

		val |= INV_MPU6050_BIT_TEMP_DIS;

	if (sleep)

		val |= INV_MPU6050_BIT_SLEEP;



		mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK;

	dev_dbg(regmap_get_device(st->map), "pwr_mgmt_1: 0x%x\n", val);

	return regmap_write(st->map, st->reg->pwr_mgmt_1, val);

}



	if ((mask == INV_MPU6050_BIT_PWR_GYRO_STBY) && (!en)) {

		/*

		 * turning off gyro requires switch to internal clock first.

		 * Then turn off gyro engine

		 */

		mgmt_1 |= INV_CLK_INTERNAL;

		result = regmap_write(st->map, st->reg->pwr_mgmt_1, mgmt_1);

		if (result)

			return result;

static int inv_mpu6050_clock_switch(struct inv_mpu6050_state *st,

				    unsigned int clock)

{

	int ret;



	switch (st->chip_type) {

	case INV_MPU6050:

	case INV_MPU6000:

	case INV_MPU9150:

		/* old chips: switch clock manually */

		ret = inv_mpu6050_pwr_mgmt_1_write(st, false, clock, -1);

		if (ret)

			return ret;

		st->chip_config.clk = clock;

		break;

	default:

		/* automatic clock switching, nothing to do */

		break;

	}



	result = regmap_read(st->map, st->reg->pwr_mgmt_2, &d);

	if (result)

		return result;

	return 0;

}



int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,

			      unsigned int mask)

{

	unsigned int sleep;

	u8 pwr_mgmt2, user_ctrl;

	int ret;



	/* delete useless requests */

	if (mask & INV_MPU6050_SENSOR_ACCL && en == st->chip_config.accl_en)

		mask &= ~INV_MPU6050_SENSOR_ACCL;

	if (mask & INV_MPU6050_SENSOR_GYRO && en == st->chip_config.gyro_en)

		mask &= ~INV_MPU6050_SENSOR_GYRO;

	if (mask & INV_MPU6050_SENSOR_TEMP && en == st->chip_config.temp_en)

		mask &= ~INV_MPU6050_SENSOR_TEMP;

	if (mask & INV_MPU6050_SENSOR_MAGN && en == st->chip_config.magn_en)

		mask &= ~INV_MPU6050_SENSOR_MAGN;

	if (mask == 0)

		return 0;



	/* turn on/off temperature sensor */

	if (mask & INV_MPU6050_SENSOR_TEMP) {

		ret = inv_mpu6050_pwr_mgmt_1_write(st, false, -1, !en);

		if (ret)

			return ret;

		st->chip_config.temp_en = en;

	}



	/* update user_crtl for driving magnetometer */

	if (mask & INV_MPU6050_SENSOR_MAGN) {

		user_ctrl = st->chip_config.user_ctrl;

		if (en)

		d &= ~mask;

			user_ctrl |= INV_MPU6050_BIT_I2C_MST_EN;

		else

		d |= mask;

	result = regmap_write(st->map, st->reg->pwr_mgmt_2, d);

	if (result)

		return result;

			user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;

		ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);

		if (ret)

			return ret;

		st->chip_config.user_ctrl = user_ctrl;

		st->chip_config.magn_en = en;

	}



	/* manage accel & gyro engines */

	if (mask & (INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO)) {

		/* compute power management 2 current value */

		pwr_mgmt2 = 0;

		if (!st->chip_config.accl_en)

			pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY;

		if (!st->chip_config.gyro_en)

			pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY;



		/* update to new requested value */

		if (mask & INV_MPU6050_SENSOR_ACCL) {

			if (en)

				pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_ACCL_STBY;

			else

				pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY;

		}

		if (mask & INV_MPU6050_SENSOR_GYRO) {

			if (en)

				pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_GYRO_STBY;

			else

				pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY;

		}



		/* switch clock to internal when turning gyro off */

		if (mask & INV_MPU6050_SENSOR_GYRO && !en) {

			ret = inv_mpu6050_clock_switch(st, INV_CLK_INTERNAL);

			if (ret)

				return ret;

		}



		/* update sensors engine */

		dev_dbg(regmap_get_device(st->map), "pwr_mgmt_2: 0x%x\n",

			pwr_mgmt2);

		ret = regmap_write(st->map, st->reg->pwr_mgmt_2, pwr_mgmt2);

		if (ret)

			return ret;

		if (mask & INV_MPU6050_SENSOR_ACCL)

			st->chip_config.accl_en = en;

		if (mask & INV_MPU6050_SENSOR_GYRO)

			st->chip_config.gyro_en = en;



		/* compute required time to have sensors stabilized */

		sleep = 0;

		if (en) {

		/* Wait for output to stabilize */

		msleep(INV_MPU6050_TEMP_UP_TIME);

		if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) {

			/* switch internal clock to PLL */

			mgmt_1 |= INV_CLK_PLL;

			result = regmap_write(st->map,

					      st->reg->pwr_mgmt_1, mgmt_1);

			if (result)

				return result;

			if (mask & INV_MPU6050_SENSOR_ACCL) {

				if (sleep < INV_MPU6050_ACCEL_UP_TIME)

					sleep = INV_MPU6050_ACCEL_UP_TIME;

			}

			if (mask & INV_MPU6050_SENSOR_GYRO) {

				if (sleep < INV_MPU6050_GYRO_UP_TIME)

					sleep = INV_MPU6050_GYRO_UP_TIME;

			}

		} else {

			if (mask & INV_MPU6050_SENSOR_GYRO) {

				if (sleep < INV_MPU6050_GYRO_DOWN_TIME)

					sleep = INV_MPU6050_GYRO_DOWN_TIME;

			}

		}

		if (sleep)

			msleep(sleep);



		/* switch clock to PLL when turning gyro on */

		if (mask & INV_MPU6050_SENSOR_GYRO && en) {

			ret = inv_mpu6050_clock_switch(st, INV_CLK_PLL);

			if (ret)

				return ret;

		}

	}
@@ -279,7 +402,7 @@ int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on) 


	if (power_on) {

		if (!st->powerup_count) {

			result = regmap_write(st->map, st->reg->pwr_mgmt_1, 0);

			result = inv_mpu6050_pwr_mgmt_1_write(st, false, -1, -1);

			if (result)

				return result;

			usleep_range(INV_MPU6050_REG_UP_TIME_MIN,
@@ -288,8 +411,7 @@ int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on) 
		st->powerup_count++;

	} else {

		if (st->powerup_count == 1) {

			result = regmap_write(st->map, st->reg->pwr_mgmt_1,

					      INV_MPU6050_BIT_SLEEP);

			result = inv_mpu6050_pwr_mgmt_1_write(st, true, -1, -1);

			if (result)

				return result;

		}
@@ -451,33 +573,41 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev, 
	switch (chan->type) {

	case IIO_ANGL_VEL:

		result = inv_mpu6050_switch_engine(st, true,

				INV_MPU6050_BIT_PWR_GYRO_STBY);

				INV_MPU6050_SENSOR_GYRO);

		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);

				INV_MPU6050_SENSOR_GYRO);

		if (result)

			goto error_power_off;

		break;

	case IIO_ACCEL:

		result = inv_mpu6050_switch_engine(st, true,

				INV_MPU6050_BIT_PWR_ACCL_STBY);

				INV_MPU6050_SENSOR_ACCL);

		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);

				INV_MPU6050_SENSOR_ACCL);

		if (result)

			goto error_power_off;

		break;

	case IIO_TEMP:

		result = inv_mpu6050_switch_engine(st, true,

				INV_MPU6050_SENSOR_TEMP);

		if (result)

			goto error_power_off;

		/* wait for stablization */

		msleep(INV_MPU6050_SENSOR_UP_TIME);

		msleep(INV_MPU6050_TEMP_UP_TIME);

		ret = inv_mpu6050_sensor_show(st, st->reg->temperature,

					      IIO_MOD_X, val);

		result = inv_mpu6050_switch_engine(st, false,

				INV_MPU6050_SENSOR_TEMP);

		if (result)

			goto error_power_off;

		break;

	case IIO_MAGN:

		ret = inv_mpu_magn_read(st, chan->channel2, val);
@@ -1108,7 +1238,7 @@ static const struct iio_info mpu_info = { 
static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)

{

	int result;

	unsigned int regval;

	unsigned int regval, mask;

	int i;



	st->hw  = &hw_info[st->chip_type];
@@ -1174,13 +1304,9 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st) 
	result = inv_mpu6050_set_power_itg(st, true);

	if (result)

		return result;



	result = inv_mpu6050_switch_engine(st, false,

					   INV_MPU6050_BIT_PWR_ACCL_STBY);

	if (result)

		goto error_power_off;

	result = inv_mpu6050_switch_engine(st, false,

					   INV_MPU6050_BIT_PWR_GYRO_STBY);

	mask = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO |

			INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN;

	result = inv_mpu6050_switch_engine(st, false, mask);

	if (result)

		goto error_power_off;

drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h

+22 −2
Original line number Diff line number Diff line
@@ -83,11 +83,22 @@ enum inv_devices { 
	INV_NUM_PARTS

};



/* chip sensors mask: accelerometer, gyroscope, temperature, magnetometer */

#define INV_MPU6050_SENSOR_ACCL		BIT(0)

#define INV_MPU6050_SENSOR_GYRO		BIT(1)

#define INV_MPU6050_SENSOR_TEMP		BIT(2)

#define INV_MPU6050_SENSOR_MAGN		BIT(3)



/**

 *  struct inv_mpu6050_chip_config - Cached chip configuration data.

 *  @clk:		selected chip clock

 *  @fsr:		Full scale range.

 *  @lpf:		Digital low pass filter frequency.

 *  @accl_fs:		accel full scale range.

 *  @accl_en:		accel engine enabled

 *  @gyro_en:		gyro engine enabled

 *  @temp_en:		temperature sensor enabled

 *  @magn_en:		magn engine (i2c master) enabled

 *  @accl_fifo_enable:	enable accel data output

 *  @gyro_fifo_enable:	enable gyro data output

 *  @temp_fifo_enable:	enable temp data output
@@ -95,9 +106,14 @@ enum inv_devices { 
 *  @divider:		chip sample rate divider (sample rate divider - 1)

 */

struct inv_mpu6050_chip_config {

	unsigned int clk:3;

	unsigned int fsr:2;

	unsigned int lpf:3;

	unsigned int accl_fs:2;

	unsigned int accl_en:1;

	unsigned int gyro_en:1;

	unsigned int temp_en:1;

	unsigned int magn_en:1;

	unsigned int accl_fifo_enable:1;

	unsigned int gyro_fifo_enable:1;

	unsigned int temp_fifo_enable:1;
@@ -262,6 +278,7 @@ struct inv_mpu6050_state { 
#define INV_MPU6050_REG_PWR_MGMT_1          0x6B

#define INV_MPU6050_BIT_H_RESET             0x80

#define INV_MPU6050_BIT_SLEEP               0x40

#define INV_MPU6050_BIT_TEMP_DIS            0x08

#define INV_MPU6050_BIT_CLK_MASK            0x7



#define INV_MPU6050_REG_PWR_MGMT_2          0x6C
@@ -292,7 +309,9 @@ struct inv_mpu6050_state { 
/* delay time in milliseconds */

#define INV_MPU6050_POWER_UP_TIME            100

#define INV_MPU6050_TEMP_UP_TIME             100

#define INV_MPU6050_SENSOR_UP_TIME           30

#define INV_MPU6050_ACCEL_UP_TIME            20

#define INV_MPU6050_GYRO_UP_TIME             35

#define INV_MPU6050_GYRO_DOWN_TIME           150



/* delay time in microseconds */

#define INV_MPU6050_REG_UP_TIME_MIN          5000
@@ -417,7 +436,8 @@ 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_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask);

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);

int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on);

int inv_mpu_acpi_create_mux_client(struct i2c_client *client);

drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c

+5 −7
Original line number Diff line number Diff line
@@ -316,9 +316,9 @@ int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st) 
 *

 * Returns 0 on success, a negative error code otherwise

 */

int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val)

int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val)

{

	unsigned int user_ctrl, status;

	unsigned int status;

	__be16 data;

	uint8_t addr;

	unsigned int freq_hz, period_ms;
@@ -350,16 +350,14 @@ int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val) 
		freq_hz = INV_MPU_MAGN_FREQ_HZ_MAX;

	period_ms = 1000 / freq_hz;



	/* start i2c master, wait for xfer, stop */

	user_ctrl = st->chip_config.user_ctrl | INV_MPU6050_BIT_I2C_MST_EN;

	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);

	ret = inv_mpu6050_switch_engine(st, true, INV_MPU6050_SENSOR_MAGN);

	if (ret)

		return ret;



	/* need to wait 2 periods + half-period margin */

	msleep(period_ms * 2 + period_ms / 2);

	user_ctrl = st->chip_config.user_ctrl;

	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);



	ret = inv_mpu6050_switch_engine(st, false, INV_MPU6050_SENSOR_MAGN);

	if (ret)

		return ret;

drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h

+1 −1
Original line number Diff line number Diff line
@@ -31,6 +31,6 @@ int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate); 


int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st);



int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val);

int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val);



#endif		/* INV_MPU_MAGN_H_ */

drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c

+39 −52
Original line number Diff line number Diff line
@@ -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);

		result = inv_mpu6050_switch_engine(st, true, scan);

		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;

		}

		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;

CRA Git | Maintained and supported by SUSTech CRA and CCSE