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