Rewrite set_power_itg in a more clean way (failing now don't update
the reference counter) and fix usage in init function (setting
power on one time is sufficient to ensure chip is effectively on).
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
---
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 44 ++++++++++++++++++------------
1 file changed, 26 insertions(+), 18 deletions(-)
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 7d64be3..9f97f4f 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -185,26 +185,29 @@ int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)
int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on)
{
- int result = 0;
+ int result;
if (power_on) {
- if (!st->powerup_count)
+ if (st->powerup_count == 0) {
result = regmap_write(st->map, st->reg->pwr_mgmt_1, 0);
- if (!result)
- st->powerup_count++;
+ if (result)
+ return result;
+ usleep_range(INV_MPU6050_REG_UP_TIME_MIN,
+ INV_MPU6050_REG_UP_TIME_MAX);
+ }
+ st->powerup_count++;
} else {
- st->powerup_count--;
- if (!st->powerup_count)
+ if (st->powerup_count == 1) {
result = regmap_write(st->map, st->reg->pwr_mgmt_1,
INV_MPU6050_BIT_SLEEP);
+ if (result)
+ return result;
+ }
+ st->powerup_count--;
}
- if (result)
- return result;
-
- if (power_on)
- usleep_range(INV_MPU6050_REG_UP_TIME_MIN,
- INV_MPU6050_REG_UP_TIME_MAX);
+ dev_dbg(regmap_get_device(st->map), "set power %d, count=%u\n",
+ power_on, st->powerup_count);
return 0;
}
@@ -850,14 +853,11 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)
msleep(INV_MPU6050_POWER_UP_TIME);
/*
- * toggle power state. After reset, the sleep bit could be on
- * or off depending on the OTP settings. Toggling power would
+ * toggle power on. After reset, the sleep bit could be on
+ * or off depending on the OTP settings. Toggling power on
* make it in a definite state as well as making the hardware
* state align with the software state
*/
- result = inv_mpu6050_set_power_itg(st, false);
- if (result)
- return result;
result = inv_mpu6050_set_power_itg(st, true);
if (result)
return result;
@@ -865,13 +865,21 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)
result = inv_mpu6050_switch_engine(st, false,
INV_MPU6050_BIT_PWR_ACCL_STBY);
if (result)
- return result;
+ goto error_power_off;
result = inv_mpu6050_switch_engine(st, false,
INV_MPU6050_BIT_PWR_GYRO_STBY);
if (result)
+ goto error_power_off;
+
+ result = inv_mpu6050_set_power_itg(st, false);
+ if (result)
return result;
return 0;
+
+error_power_off:
+ inv_mpu6050_set_power_itg(st, false);
+ return result;
}
int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
--
2.7.4
--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
