[ANDROID]:lsm303dlh:Disable accelerometer/magnetometer devices during backlight off

Disabled the lsm303dlh accelerometer and magnetometer devices
vaux1 regulator during backlight off by implementing early
suspend.

ST-Ericsson Id: ER318551
Change-Id: If145751b31399dfa4474da9b8b33f680575e1c82
Signed-off-by: Chethan Krishna N <chethan.krishna@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/13052
Reviewed-by: QATOOLS
Reviewed-by: Jonas ABERG <jonas.aberg@stericsson.com>

2 files changed, 143 insertions, 50 deletions

diff --git a/drivers/hwmon/lsm303dlh_a.c b/drivers/hwmon/lsm303dlh_a.c
index 31282ff6e36..2ff1e7d23a8 100644
--- a/drivers/hwmon/lsm303dlh_a.c
+++ b/drivers/hwmon/lsm303dlh_a.c
@@ -37,6 +37,7 @@
 #endif
 
 #include <linux/lsm303dlh.h>
+#include <linux/earlysuspend.h>
 #include <linux/regulator/consumer.h>
 
  /* lsm303dlh accelerometer registers */
@@ -172,8 +173,14 @@ struct lsm303dlh_a_data {
 	unsigned char interrupt_configure[2];
 	unsigned char interrupt_duration[2];
 	unsigned char interrupt_threshold[2];
+	struct early_suspend early_suspend;
 };
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lsm303dlh_a_early_suspend(struct early_suspend *data);
+static void lsm303dlh_a_late_resume(struct early_suspend *data);
+#endif
+
 static int lsm303dlh_a_write(struct lsm303dlh_a_data *ddata, u8 reg,
 		u8 val, char *msg)
 {
@@ -195,6 +202,25 @@ static int lsm303dlh_a_read(struct lsm303dlh_a_data *ddata, u8 reg, char *msg)
 	return ret;
 }
 
+static int lsm303dlh_a_do_suspend(struct lsm303dlh_a_data *ddata)
+{
+	int ret;
+	if (ddata->mode == LSM303DLH_A_MODE_OFF)
+		return 0;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+
+	ret = lsm303dlh_a_write(ddata, CTRL_REG1,
+		LSM303DLH_A_MODE_OFF, "CONTROL");
+
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+	return ret;
+}
+
 static int lsm303dlh_a_restore(struct lsm303dlh_a_data *ddata)
 {
 	unsigned char reg;
@@ -203,6 +229,18 @@ static int lsm303dlh_a_restore(struct lsm303dlh_a_data *ddata)
 	unsigned char context = (shifted_mode | shifted_rate);
 	int ret = 0;
 
+	/* in correct mode, no need to change it */
+	if (ddata->mode == LSM303DLH_A_MODE_OFF)
+		return 0;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	enable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	enable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
 	/* BDU should be enabled by default/recommened */
 	reg = ddata->range;
 	reg |= LSM303DLH_A_CR4_BDU_MASK;
@@ -1023,6 +1061,14 @@ static int __devinit lsm303dlh_a_probe(struct i2c_client *client,
 
 #endif
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+			EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = lsm303dlh_a_early_suspend;
+	ddata->early_suspend.resume = lsm303dlh_a_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
 	return ret;
 
 #ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
@@ -1074,7 +1120,7 @@ static int __devexit lsm303dlh_a_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
 static int lsm303dlh_a_suspend(struct device *dev)
 {
 	struct lsm303dlh_a_data *ddata;
@@ -1082,19 +1128,7 @@ static int lsm303dlh_a_suspend(struct device *dev)
 
 	ddata = dev_get_drvdata(dev);
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF)
-		return 0;
-
-#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
-	disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
-	disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
-#endif
-
-	ret = lsm303dlh_a_write(ddata, CTRL_REG1,
-		LSM303DLH_A_MODE_OFF, "CONTROL");
-
-	if (ddata->regulator)
-		regulator_disable(ddata->regulator);
+	ret = lsm303dlh_a_do_suspend(ddata);
 
 	return ret;
 }
@@ -1106,18 +1140,6 @@ static int lsm303dlh_a_resume(struct device *dev)
 
 	ddata = dev_get_drvdata(dev);
 
-	/* in correct mode, no need to change it */
-	if (ddata->mode == LSM303DLH_A_MODE_OFF)
-		return 0;
-
-#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
-	enable_irq(gpio_to_irq(ddata->pdata.irq_a1));
-	enable_irq(gpio_to_irq(ddata->pdata.irq_a2));
-#endif
-
-	if (ddata->regulator)
-		regulator_enable(ddata->regulator);
-
 	ret	= lsm303dlh_a_restore(ddata);
 
 	if (ret < 0)
@@ -1130,6 +1152,27 @@ static const struct dev_pm_ops lsm303dlh_a_dev_pm_ops = {
 	.suspend = lsm303dlh_a_suspend,
 	.resume  = lsm303dlh_a_resume,
 };
+#else
+static void lsm303dlh_a_early_suspend(struct early_suspend *data)
+{
+	struct lsm303dlh_a_data *ddata =
+		container_of(data, struct lsm303dlh_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_a_do_suspend(ddata);
+}
+
+static void lsm303dlh_a_late_resume(struct early_suspend *data)
+{
+	struct lsm303dlh_a_data *ddata =
+		container_of(data, struct lsm303dlh_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_a_restore(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"lsm303dlh_a late resume failed\n");
+}
 #endif /* CONFIG_PM */
 
 static const struct i2c_device_id lsm303dlh_a_id[] = {
@@ -1143,7 +1186,7 @@ static struct i2c_driver lsm303dlh_a_driver = {
 	.id_table	= lsm303dlh_a_id,
 	.driver = {
 		.name = "lsm303dlh_a",
-	#ifdef CONFIG_PM
+	#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
 		.pm	=	&lsm303dlh_a_dev_pm_ops,
 	#endif
 	},
diff --git a/drivers/hwmon/lsm303dlh_m.c b/drivers/hwmon/lsm303dlh_m.c
index 58b27656d02..2500e8fd027 100644
--- a/drivers/hwmon/lsm303dlh_m.c
+++ b/drivers/hwmon/lsm303dlh_m.c
@@ -38,6 +38,7 @@
 
 #include <linux/lsm303dlh.h>
 #include <linux/regulator/consumer.h>
+#include <linux/earlysuspend.h>
 #include <linux/kernel.h>
 
 /* lsm303dlh magnetometer registers */
@@ -139,8 +140,14 @@ struct lsm303dlh_m_data {
 	unsigned char mode;
 	unsigned char rate;
 	unsigned char range;
+	struct early_suspend early_suspend;
 };
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lsm303dlh_m_early_suspend(struct early_suspend *data);
+static void lsm303dlh_m_late_resume(struct early_suspend *data);
+#endif
+
 static int lsm303dlh_m_set_mode(struct lsm303dlh_m_data *ddata,
 		unsigned char mode);
 static int lsm303dlh_m_write(struct lsm303dlh_m_data *ddata,
@@ -154,9 +161,36 @@ static int lsm303dlh_m_write(struct lsm303dlh_m_data *ddata,
 	return ret;
 }
 
+static int lsm303dlh_m_do_suspend(struct lsm303dlh_m_data *ddata)
+{
+	int ret;
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP)
+		return 0;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	disable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+	ret = lsm303dlh_m_set_mode(ddata, LSM303DLH_M_MODE_SLEEP);
+	if (ret < 0)
+		return ret;
+
+	regulator_disable(ddata->regulator);
+	return ret;
+}
+
 static int lsm303dlh_m_restore(struct lsm303dlh_m_data *ddata)
 {
 	int ret = 0;
+	/* in correct mode, no need to change it */
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP)
+		return 0;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	enable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+	regulator_enable(ddata->regulator);
 
 	ret = lsm303dlh_m_write(ddata, CRB_REG_M, ddata->range, "SET RANGE");
 
@@ -631,6 +665,13 @@ static int __devinit lsm303dlh_m_probe(struct i2c_client *client,
 	disable_irq(gpio_to_irq(ddata->pdata.irq_m));
 #endif
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+				EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = lsm303dlh_m_early_suspend;
+	ddata->early_suspend.resume = lsm303dlh_m_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
 	return ret;
 
 #ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
@@ -676,7 +717,7 @@ static int __devexit lsm303dlh_m_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
 static int lsm303dlh_m_suspend(struct device *dev)
 {
 	struct lsm303dlh_m_data *ddata;
@@ -684,18 +725,11 @@ static int lsm303dlh_m_suspend(struct device *dev)
 
 	ddata = dev_get_drvdata(dev);
 
-	if (ddata->mode == LSM303DLH_M_MODE_SLEEP)
-		return 0;
-
-#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
-	disable_irq(gpio_to_irq(ddata->pdata.irq_m));
-#endif
-
-	ret = lsm303dlh_m_set_mode(ddata, LSM303DLH_M_MODE_SLEEP);
+	ret = lsm303dlh_m_do_suspend(ddata);
 	if (ret < 0)
-		return ret;
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device");
 
-	regulator_disable(ddata->regulator);
 	return ret;
 }
 
@@ -706,20 +740,11 @@ static int lsm303dlh_m_resume(struct device *dev)
 
 	ddata = dev_get_drvdata(dev);
 
-	/* in correct mode, no need to change it */
-	if (ddata->mode == LSM303DLH_M_MODE_SLEEP)
-		return 0;
-
-#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
-	enable_irq(gpio_to_irq(ddata->pdata.irq_m));
-#endif
-
-	regulator_enable(ddata->regulator);
-
 	ret = lsm303dlh_m_restore(ddata);
 
 	if (ret < 0)
-		return ret;
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device");
 
 	return ret;
 }
@@ -727,6 +752,31 @@ static const struct dev_pm_ops lsm303dlh_m_dev_pm_ops = {
 	.suspend = lsm303dlh_m_suspend,
 	.resume  = lsm303dlh_m_resume,
 };
+#else
+static void lsm303dlh_m_early_suspend(struct early_suspend *data)
+{
+	struct lsm303dlh_m_data *ddata =
+		container_of(data, struct lsm303dlh_m_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_m_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device");
+}
+
+static void lsm303dlh_m_late_resume(struct early_suspend *data)
+{
+	struct lsm303dlh_m_data *ddata =
+		container_of(data, struct lsm303dlh_m_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_m_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"lsm303dlh_m late resume failed\n");
+}
 #endif /* CONFIG_PM */
 
 static const struct i2c_device_id lsm303dlh_m_id[] = {
@@ -740,7 +790,7 @@ static struct i2c_driver lsm303dlh_m_driver = {
 	.id_table	= lsm303dlh_m_id,
 	.driver = {
 		.name = "lsm303dlh_m",
-	#ifdef CONFIG_PM
+	#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
 		.pm = &lsm303dlh_m_dev_pm_ops,
 	#endif
 	},