[PATCH 4/4] iio: imu: inv_mpu6050: add WoM suspend wakeup with low-power mode

[inv.git-commit@tdk.com]

From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>

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@tdk.com>
---
 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