From 92e7407ab1bc5024eb467d5788aace0ceff78a9d Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Maneyrol Date: Wed, 19 Feb 2020 15:39:53 +0100 Subject: [PATCH] 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: Jean-Baptiste Maneyrol Signed-off-by: Jonathan Cameron --- 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 63cdde2..a51efc4 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 17f1f6a..a578789 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 607104a..3f402fa 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 b41bd05..f7ad50c 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 5199fe7..cfd7243 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; -- 2.7.4