| Message ID | 20200219143958.3548-9-jmaneyrol@invensense.com (mailing list archive) |
|---|---|
| State | New, archived |
| Headers | show |
| Series | Rework sensors engines and power management | expand |
On Wed, 19 Feb 2020 15:39:53 +0100 Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> 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@invensense.com> 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;
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;
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@invensense.com> --- 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(-)