On Wed, 19 Feb 2020 15:39:53 +0100
Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx> wrote:
> 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: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
Applied to the togreg branch of iio.git and pushed out as testing for
the autobuilders to play with it.
Thanks,
Jonathan
> ---
> drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 264 +++++++++++++-----
> drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 24 +-
> drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c | 12 +-
> drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h | 2 +-
> drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 91 +++---
> 5 files changed, 262 insertions(+), 131 deletions(-)
>
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index 63cdde20df7e..a51efc4c941b 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -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 = ®_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 = ®_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 = ®_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 = ®_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 = ®_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 = ®_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 = ®_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 = ®_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 = ®_set_icm20602,
> - .config = &chip_config_6050,
> + .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 = ®_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;
> +
> + 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);
> +}
> +
> +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;
> + }
> +
> + return 0;
> +}
>
> - mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK;
> +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;
> }
>
> - 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;
> + /* update user_crtl for driving magnetometer */
> + if (mask & INV_MPU6050_SENSOR_MAGN) {
> + user_ctrl = st->chip_config.user_ctrl;
> + if (en)
> + user_ctrl |= INV_MPU6050_BIT_I2C_MST_EN;
> + else
> + 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;
> }
>
> - result = regmap_read(st->map, st->reg->pwr_mgmt_2, &d);
> - if (result)
> - return result;
> - if (en)
> - d &= ~mask;
> - else
> - d |= mask;
> - result = regmap_write(st->map, st->reg->pwr_mgmt_2, d);
> - if (result)
> - return result;
> + /* 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;
> + }
>
> - 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;
> + /* 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) {
> + 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;
>
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index 17f1f6a15f95..a578789c9210 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -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);
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
> index 607104a2631e..3f402fa56d95 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
> @@ -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;
>
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
> index b41bd0578478..f7ad50ca6c1b 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
> @@ -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_ */
> 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;
