diff options
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 13 | ||||
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 35 |
2 files changed, 32 insertions, 16 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index e73c88c22e2f..5062fbe4d6c3 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -273,21 +273,21 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev) d = (INV_MPU6050_FSR_2000DPS << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); result = regmap_write(st->map, st->reg->gyro_config, d); if (result) - return result; + goto error_power_off; result = inv_mpu6050_set_lpf_regs(st, INV_MPU6050_FILTER_20HZ); if (result) - return result; + goto error_power_off; d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1; result = regmap_write(st->map, st->reg->sample_rate_div, d); if (result) - return result; + goto error_power_off; d = (INV_MPU6050_FS_02G << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); result = regmap_write(st->map, st->reg->accl_config, d); if (result) - return result; + goto error_power_off; result = regmap_write(st->map, st->reg->int_pin_cfg, st->irq_mask); if (result) @@ -295,8 +295,11 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev) memcpy(&st->chip_config, hw_info[st->chip_type].config, sizeof(struct inv_mpu6050_chip_config)); - result = inv_mpu6050_set_power_itg(st, false); + return inv_mpu6050_set_power_itg(st, false); + +error_power_off: + inv_mpu6050_set_power_itg(st, false); return result; } diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c index b8c5584e4252..fc8843ce9606 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c @@ -53,45 +53,58 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) result = inv_mpu6050_switch_engine(st, true, INV_MPU6050_BIT_PWR_GYRO_STBY); if (result) - return 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) - return result; + goto error_gyro_off; } result = inv_reset_fifo(indio_dev); if (result) - return result; + goto error_accl_off; } else { result = regmap_write(st->map, st->reg->fifo_en, 0); if (result) - return result; + goto error_accl_off; result = regmap_write(st->map, st->reg->int_enable, 0); if (result) - return result; + goto error_accl_off; result = regmap_write(st->map, st->reg->user_ctrl, 0); if (result) - return result; + goto error_accl_off; result = inv_mpu6050_switch_engine(st, false, - INV_MPU6050_BIT_PWR_GYRO_STBY); + INV_MPU6050_BIT_PWR_ACCL_STBY); if (result) - return result; + goto error_accl_off; result = inv_mpu6050_switch_engine(st, false, - INV_MPU6050_BIT_PWR_ACCL_STBY); + INV_MPU6050_BIT_PWR_GYRO_STBY); if (result) - return result; + goto error_gyro_off; + result = inv_mpu6050_set_power_itg(st, false); if (result) - return result; + goto error_power_off; } return 0; + +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_power_off: + inv_mpu6050_set_power_itg(st, false); + return result; } /** |