From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
Add wakeup from suspend for WoM when enabled and put accel in
low-power mode when suspended. Requires rewriting pwr_mgmt_1
register handling and factorize out accel LPF settings.
Use a low-power rate similar to the chip sampling rate but always
lower for a best match of the sampling rate while saving power.
Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
---
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 160 ++++++++++++++++-----
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 14 ++
2 files changed, 141 insertions(+), 33 deletions(-)
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index d2544c758815..db02dd50c0bc 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -287,7 +287,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
};
static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep,
- int clock, int temp_dis)
+ bool cycle, int clock, int temp_dis)
{
u8 val;
@@ -301,6 +301,8 @@ static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep
val |= INV_MPU6050_BIT_TEMP_DIS;
if (sleep)
val |= INV_MPU6050_BIT_SLEEP;
+ if (cycle)
+ val |= INV_MPU6050_BIT_CYCLE;
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);
@@ -316,7 +318,7 @@ static int inv_mpu6050_clock_switch(struct inv_mpu6050_state *st,
case INV_MPU6000:
case INV_MPU9150:
/* old chips: switch clock manually */
- ret = inv_mpu6050_pwr_mgmt_1_write(st, false, clock, -1);
+ ret = inv_mpu6050_pwr_mgmt_1_write(st, false, false, clock, -1);
if (ret)
return ret;
st->chip_config.clk = clock;
@@ -358,7 +360,7 @@ int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
/* turn on/off temperature sensor */
if (mask & INV_MPU6050_SENSOR_TEMP) {
- ret = inv_mpu6050_pwr_mgmt_1_write(st, false, -1, !en);
+ ret = inv_mpu6050_pwr_mgmt_1_write(st, false, false, -1, !en);
if (ret)
return ret;
st->chip_config.temp_en = en;
@@ -465,7 +467,7 @@ static int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st,
{
int result;
- result = inv_mpu6050_pwr_mgmt_1_write(st, !power_on, -1, -1);
+ result = inv_mpu6050_pwr_mgmt_1_write(st, !power_on, false, -1, -1);
if (result)
return result;
@@ -495,22 +497,9 @@ static int inv_mpu6050_set_gyro_fsr(struct inv_mpu6050_state *st,
return regmap_write(st->map, st->reg->gyro_config, data);
}
-/*
- * inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
- *
- * MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
- * MPU6500 and above have a dedicated register for accelerometer
- */
-static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
- enum inv_mpu6050_filter_e val)
+static int inv_mpu6050_set_accel_lpf_regs(struct inv_mpu6050_state *st,
+ enum inv_mpu6050_filter_e val)
{
- int result;
-
- result = regmap_write(st->map, st->reg->lpf, val);
- if (result)
- return result;
-
- /* set accel lpf */
switch (st->chip_type) {
case INV_MPU6050:
case INV_MPU6000:
@@ -529,6 +518,25 @@ static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
return regmap_write(st->map, st->reg->accel_lpf, val);
}
+/*
+ * inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
+ *
+ * MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
+ * MPU6500 and above have a dedicated register for accelerometer
+ */
+static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
+ enum inv_mpu6050_filter_e val)
+{
+ int result;
+
+ result = regmap_write(st->map, st->reg->lpf, val);
+ if (result)
+ return result;
+
+ /* set accel lpf */
+ return inv_mpu6050_set_accel_lpf_regs(st, val);
+}
+
/*
* inv_mpu6050_init_config() - Initialize hardware, disable FIFO.
*
@@ -921,6 +929,69 @@ static int inv_mpu6050_set_wom_int(struct inv_mpu6050_state *st, bool on)
INV_MPU6500_BIT_WOM_INT_EN, val);
}
+static int inv_mpu6050_set_wom_odr(struct inv_mpu6050_state *st, unsigned int rate)
+{
+ static const unsigned int hz[] = {500, 250, 125, 62, 31, 16, 8, 4};
+ static const unsigned int d[] = {
+ INV_MPU6050_LPOSC_500HZ, INV_MPU6050_LPOSC_250HZ,
+ INV_MPU6050_LPOSC_125HZ, INV_MPU6050_LPOSC_62HZ,
+ INV_MPU6050_LPOSC_31HZ, INV_MPU6050_LPOSC_16HZ,
+ INV_MPU6050_LPOSC_8HZ, INV_MPU6050_LPOSC_4HZ,
+ };
+ unsigned int data, i;
+
+ switch (st->chip_type) {
+ case INV_ICM20609:
+ case INV_ICM20689:
+ case INV_ICM20600:
+ case INV_ICM20602:
+ case INV_ICM20690:
+ /* nothing to do */
+ return 0;
+ default:
+ break;
+ }
+
+ data = INV_MPU6050_LPOSC_4HZ;
+ for (i = 0; i < ARRAY_SIZE(hz); ++i) {
+ if (rate >= hz[i]) {
+ data = d[i];
+ break;
+ }
+ }
+
+ dev_dbg(regmap_get_device(st->map), "lp_odr: 0x%x\n", data);
+ return regmap_write(st->map, INV_MPU6500_REG_LP_ODR, data);
+}
+
+static int inv_mpu6050_set_wom_lp(struct inv_mpu6050_state *st, bool on)
+{
+ int result;
+
+ if (on) {
+ /* set WoM low power ODR */
+ result = inv_mpu6050_set_wom_odr(st,
+ INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider));
+ if (result)
+ return result;
+ /* disable accel low pass filter */
+ result = inv_mpu6050_set_accel_lpf_regs(st, INV_MPU6050_FILTER_NOLPF);
+ if (result)
+ return result;
+ /* set cycle mode */
+ result = inv_mpu6050_pwr_mgmt_1_write(st, false, true, -1, -1);
+ } else {
+ /* disable cycle mode */
+ result = inv_mpu6050_pwr_mgmt_1_write(st, false, false, -1, -1);
+ if (result)
+ return result;
+ /* restore accel low pass filter */
+ result = inv_mpu6050_set_accel_lpf_regs(st, st->chip_config.lpf);
+ }
+
+ return result;
+}
+
static int inv_mpu6050_set_wom_threshold(struct inv_mpu6050_state *st, unsigned int value)
{
struct device *pdev = regmap_get_device(st->map);
@@ -1786,6 +1857,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
irq_type);
return -EINVAL;
}
+ device_set_wakeup_capable(dev, true);
st->vdd_supply = devm_regulator_get(dev, "vdd");
if (IS_ERR(st->vdd_supply))
@@ -1951,16 +2023,27 @@ static int inv_mpu_resume(struct device *dev)
{
struct iio_dev *indio_dev = dev_get_drvdata(dev);
struct inv_mpu6050_state *st = iio_priv(indio_dev);
+ bool wakeup;
int result;
mutex_lock(&st->lock);
- result = inv_mpu_core_enable_regulator_vddio(st);
- if (result)
- goto out_unlock;
- result = inv_mpu6050_set_power_itg(st, true);
- if (result)
- goto out_unlock;
+ wakeup = device_may_wakeup(dev) && st->chip_config.wom_en;
+
+ if (wakeup) {
+ enable_irq(st->irq);
+ disable_irq_wake(st->irq);
+ result = inv_mpu6050_set_wom_lp(st, false);
+ if (result)
+ goto out_unlock;
+ } else {
+ result = inv_mpu_core_enable_regulator_vddio(st);
+ if (result)
+ goto out_unlock;
+ result = inv_mpu6050_set_power_itg(st, true);
+ if (result)
+ goto out_unlock;
+ }
pm_runtime_disable(dev);
pm_runtime_set_active(dev);
@@ -1970,7 +2053,7 @@ static int inv_mpu_resume(struct device *dev)
if (result)
goto out_unlock;
- if (st->chip_config.wom_en) {
+ if (st->chip_config.wom_en && !wakeup) {
result = inv_mpu6050_set_wom_int(st, true);
if (result)
goto out_unlock;
@@ -1989,6 +2072,7 @@ static int inv_mpu_suspend(struct device *dev)
{
struct iio_dev *indio_dev = dev_get_drvdata(dev);
struct inv_mpu6050_state *st = iio_priv(indio_dev);
+ bool wakeup;
int result;
mutex_lock(&st->lock);
@@ -2005,13 +2089,15 @@ static int inv_mpu_suspend(struct device *dev)
goto out_unlock;
}
- if (st->chip_config.wom_en) {
+ wakeup = device_may_wakeup(dev) && st->chip_config.wom_en;
+
+ if (st->chip_config.wom_en && !wakeup) {
result = inv_mpu6050_set_wom_int(st, false);
if (result)
goto out_unlock;
}
- if (st->chip_config.accl_en)
+ if (st->chip_config.accl_en && !wakeup)
st->suspended_sensors |= INV_MPU6050_SENSOR_ACCL;
if (st->chip_config.gyro_en)
st->suspended_sensors |= INV_MPU6050_SENSOR_GYRO;
@@ -2019,17 +2105,25 @@ static int inv_mpu_suspend(struct device *dev)
st->suspended_sensors |= INV_MPU6050_SENSOR_TEMP;
if (st->chip_config.magn_en)
st->suspended_sensors |= INV_MPU6050_SENSOR_MAGN;
- if (st->chip_config.wom_en)
+ if (st->chip_config.wom_en && !wakeup)
st->suspended_sensors |= INV_MPU6050_SENSOR_WOM;
result = inv_mpu6050_switch_engine(st, false, st->suspended_sensors);
if (result)
goto out_unlock;
- result = inv_mpu6050_set_power_itg(st, false);
- if (result)
- goto out_unlock;
+ if (wakeup) {
+ result = inv_mpu6050_set_wom_lp(st, true);
+ if (result)
+ goto out_unlock;
+ enable_irq_wake(st->irq);
+ disable_irq(st->irq);
+ } else {
+ result = inv_mpu6050_set_power_itg(st, false);
+ if (result)
+ goto out_unlock;
+ inv_mpu_core_disable_regulator_vddio(st);
+ }
- inv_mpu_core_disable_regulator_vddio(st);
out_unlock:
mutex_unlock(&st->lock);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 9be67cebbd49..751e2b72aebc 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -302,6 +302,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_CYCLE 0x20
#define INV_MPU6050_BIT_TEMP_DIS 0x08
#define INV_MPU6050_BIT_CLK_MASK 0x7
@@ -333,6 +334,7 @@ struct inv_mpu6050_state {
/* mpu6500 registers */
#define INV_MPU6500_REG_ACCEL_CONFIG_2 0x1D
#define INV_ICM20689_BITS_FIFO_SIZE_MAX 0xC0
+#define INV_MPU6500_REG_LP_ODR 0x1E
#define INV_MPU6500_REG_WOM_THRESHOLD 0x1F
#define INV_MPU6500_REG_ACCEL_INTEL_CTRL 0x69
#define INV_MPU6500_BIT_ACCEL_INTEL_EN BIT(7)
@@ -449,6 +451,18 @@ enum inv_mpu6050_filter_e {
NUM_MPU6050_FILTER
};
+enum inv_mpu6050_lposc_e {
+ INV_MPU6050_LPOSC_4HZ = 4,
+ INV_MPU6050_LPOSC_8HZ,
+ INV_MPU6050_LPOSC_16HZ,
+ INV_MPU6050_LPOSC_31HZ,
+ INV_MPU6050_LPOSC_62HZ,
+ INV_MPU6050_LPOSC_125HZ,
+ INV_MPU6050_LPOSC_250HZ,
+ INV_MPU6050_LPOSC_500HZ,
+ NUM_MPU6050_LPOSC,
+};
+
/* IIO attribute address */
enum INV_MPU6050_IIO_ATTR_ADDR {
ATTR_GYRO_MATRIX,
--
2.34.1
