lsm303dlh: balancing regulator calls for other displays

When either AV8100 or AB8500 displays are enabled, the devices
now call enable/disable in balanced manner.

ST-Ericsson ID : ER324883
Change-Id: Ic1bf7ebada025ce1004ced5671a2b916c1c0e31a
Signed-off-by: Chethan Krishna N <chethan.krishna@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/16531
Reviewed-by: Preetham Rao K <preetham.rao@stericsson.com>
Tested-by: Preetham Rao K <preetham.rao@stericsson.com>
Reviewed-by: Srinidhi KASAGAR <srinidhi.kasagar@stericsson.com>

2 files changed, 203 insertions, 122 deletions

diff --git a/drivers/hwmon/lsm303dlh_a.c b/drivers/hwmon/lsm303dlh_a.c
index 29d313c7fc7..30f9da73a71 100644
--- a/drivers/hwmon/lsm303dlh_a.c
+++ b/drivers/hwmon/lsm303dlh_a.c
@@ -138,6 +138,11 @@
 /* Multiple byte transfer enable */
 #define MULTIPLE_I2C_TR 0x80
 
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
 /* Range -2048 to 2047 */
 struct lsm303dlh_a_t {
 	short	x;
@@ -175,7 +180,7 @@ struct lsm303dlh_a_data {
 	unsigned char interrupt_duration[2];
 	unsigned char interrupt_threshold[2];
 	struct early_suspend early_suspend;
-	int regulator_enabled;
+	int device_status;
 };
 
 #ifdef CONFIG_HAS_EARLYSUSPEND
@@ -208,8 +213,12 @@ static int lsm303dlh_a_do_suspend(struct lsm303dlh_a_data *ddata)
 {
 	int ret;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF)
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		mutex_unlock(&ddata->lock);
 		return 0;
+	}
 
 #ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
 	disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
@@ -217,12 +226,15 @@ static int lsm303dlh_a_do_suspend(struct lsm303dlh_a_data *ddata)
 #endif
 
 	ret = lsm303dlh_a_write(ddata, CTRL_REG1,
-		LSM303DLH_A_MODE_OFF, "CONTROL");
+			LSM303DLH_A_MODE_OFF, "CONTROL");
 
-	if (ddata->regulator && ddata->regulator_enabled == 1) {
+	if (ddata->regulator)
 		regulator_disable(ddata->regulator);
-		ddata->regulator_enabled = 0;
-	}
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+
 	return ret;
 }
 
@@ -234,19 +246,28 @@ static int lsm303dlh_a_restore(struct lsm303dlh_a_data *ddata)
 	unsigned char context = (shifted_mode | shifted_rate);
 	int ret = 0;
 
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
 	/* in correct mode, no need to change it */
-	if (ddata->mode == LSM303DLH_A_MODE_OFF)
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		ddata->device_status = DEVICE_OFF;
+		mutex_unlock(&ddata->lock);
 		return 0;
+	} else
+		ddata->device_status = DEVICE_ON;
 
 #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 && ddata->regulator_enabled == 0) {
+	if (ddata->regulator)
 		regulator_enable(ddata->regulator);
-		ddata->regulator_enabled = 1;
-	}
 
 	/* BDU should be enabled by default/recommened */
 	reg = ddata->range;
@@ -254,7 +275,7 @@ static int lsm303dlh_a_restore(struct lsm303dlh_a_data *ddata)
 	context |= LSM303DLH_A_CR1_AXIS_ENABLE;
 
 	ret = lsm303dlh_a_write(ddata, CTRL_REG1, context,
-				"CTRL_REG1");
+			"CTRL_REG1");
 	if (ret < 0)
 		goto fail;
 
@@ -306,6 +327,7 @@ static int lsm303dlh_a_restore(struct lsm303dlh_a_data *ddata)
 		goto fail;
 
 fail:
+	mutex_unlock(&ddata->lock);
 	return ret;
 }
 
@@ -356,14 +378,14 @@ static ssize_t lsm303dlh_a_show_data(struct device *dev,
 	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
 	int ret = 0;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
 		return ret;
 	}
 
-	mutex_lock(&ddata->lock);
-
 	ret = lsm303dlh_a_readdata(ddata);
 
 	if (ret < 0) {
@@ -439,18 +461,19 @@ static ssize_t lsm303dlh_a_store_interrupt_control(struct device *dev,
 	unsigned long val;
 	int error;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
-		return count;
-	}
-
 	error = strict_strtoul(buf, 0, &val);
 	if (error)
 		return error;
 
 	mutex_lock(&ddata->lock);
 
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
 	ddata->interrupt_control = val;
 
 	error = lsm303dlh_a_write(ddata, CTRL_REG3, val, "CTRL_REG3");
@@ -512,18 +535,19 @@ static ssize_t lsm303dlh_a_store_interrupt_configure(struct device *dev,
 	unsigned long val;
 	int error;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
-		return count;
-	}
-
 	error = strict_strtoul(buf, 0, &val);
 	if (error)
 		return error;
 
 	mutex_lock(&ddata->lock);
 
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+	    mutex_unlock(&ddata->lock);
+		return count;
+	}
+
 	ddata->interrupt_configure[ddata->interrupt_channel] = val;
 
 	if (ddata->interrupt_channel == 0x0)
@@ -561,18 +585,20 @@ static ssize_t lsm303dlh_a_store_interrupt_duration(struct device *dev,
 	unsigned long val;
 	int error;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
-		return count;
-	}
-
 	error = strict_strtoul(buf, 0, &val);
 	if (error)
 		return error;
 
 	mutex_lock(&ddata->lock);
 
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
 	ddata->interrupt_duration[ddata->interrupt_channel] = val;
 
 	if (ddata->interrupt_channel == 0x0)
@@ -612,18 +638,19 @@ static ssize_t lsm303dlh_a_store_interrupt_threshold(struct device *dev,
 	unsigned long val;
 	int error;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
-		return count;
-	}
-
 	error = strict_strtoul(buf, 0, &val);
 	if (error)
 		return error;
 
 	mutex_lock(&ddata->lock);
 
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
 	ddata->interrupt_threshold[ddata->interrupt_channel] = val;
 
 	if (ddata->interrupt_channel == 0x0)
@@ -661,11 +688,6 @@ static ssize_t lsm303dlh_a_store_range(struct device *dev,
 	unsigned long bdu_enabled_val;
 	int error;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
-		return count;
-	}
 
 	error = strict_strtol(buf, 0, &val);
 	if (error)
@@ -676,6 +698,13 @@ static ssize_t lsm303dlh_a_store_range(struct device *dev,
 
 	mutex_lock(&ddata->lock);
 
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
 	ddata->range = val;
 	ddata->range <<= LSM303DLH_A_CR4_FS_BIT;
 
@@ -736,28 +765,40 @@ static ssize_t lsm303dlh_a_store_mode(struct device *dev,
 	if (error)
 		return error;
 
+	mutex_lock(&ddata->lock);
+
 	/* not in correct range */
 
-	if (val < LSM303DLH_A_MODE_OFF || val > LSM303DLH_A_MODE_LP_10)
+	if (val < LSM303DLH_A_MODE_OFF || val > LSM303DLH_A_MODE_LP_10) {
+		mutex_unlock(&ddata->lock);
 		return -EINVAL;
+	}
+
+	if (ddata->device_status == DEVICE_SUSPENDED &&
+			val == LSM303DLH_A_MODE_OFF) {
+		ddata->mode = val;
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
 
 	/*  if same mode as existing, return */
-	if (ddata->mode == val)
+	if (ddata->mode == val) {
+		mutex_unlock(&ddata->lock);
 		return 0;
+	}
 
 	/* turn on the supplies if already off */
 	if (ddata->regulator && ddata->mode == LSM303DLH_A_MODE_OFF
-			&& ddata->regulator_enabled == 0) {
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
 		regulator_enable(ddata->regulator);
-		ddata->regulator_enabled = 1;
+		ddata->device_status = DEVICE_ON;
 #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
 	}
 
-	mutex_lock(&ddata->lock);
-
 	data = lsm303dlh_a_read(ddata, CTRL_REG1, "CTRL_REG1");
 
 	data &= ~LSM303DLH_A_CR1_PM_MASK;
@@ -768,16 +809,14 @@ static ssize_t lsm303dlh_a_store_mode(struct device *dev,
 
 	error = lsm303dlh_a_write(ddata, CTRL_REG1, data, "CTRL_REG1");
 	if (error < 0) {
-		if (ddata->regulator && ddata->regulator_enabled == 1) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
 			regulator_disable(ddata->regulator);
-			ddata->regulator_enabled = 0;
+			ddata->device_status = DEVICE_OFF;
 		}
 		mutex_unlock(&ddata->lock);
 		return error;
 	}
 
-	mutex_unlock(&ddata->lock);
-
 	if (val == LSM303DLH_A_MODE_OFF) {
 #ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
 		disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
@@ -792,11 +831,12 @@ static ssize_t lsm303dlh_a_store_mode(struct device *dev,
 		ddata->range = LSM303DLH_A_RANGE_2G;
 		ddata->shift_adjust = SHIFT_ADJ_2G;
 
-		if (ddata->regulator && ddata->regulator_enabled == 1) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
 			regulator_disable(ddata->regulator);
-			ddata->regulator_enabled = 0;
+			ddata->device_status = DEVICE_OFF;
 		}
 	}
+	mutex_unlock(&ddata->lock);
 
 	return count;
 }
@@ -821,12 +861,6 @@ static ssize_t lsm303dlh_a_store_rate(struct device *dev,
 	unsigned char data;
 	int error;
 
-	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
-		return count;
-	}
-
 	error = strict_strtol(buf, 0, &val);
 	if (error)
 		return error;
@@ -836,6 +870,13 @@ static ssize_t lsm303dlh_a_store_rate(struct device *dev,
 
 	mutex_lock(&ddata->lock);
 
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
 	data = lsm303dlh_a_read(ddata, CTRL_REG1, "CTRL_REG1");
 
 	data &= ~LSM303DLH_A_CR1_DR_MASK;
@@ -974,7 +1015,7 @@ static int __devinit lsm303dlh_a_probe(struct i2c_client *client,
 	ddata->range = LSM303DLH_A_RANGE_2G;
 	ddata->sleep_wake = LSM303DLH_A_SLEEPWAKE_DISABLE;
 	ddata->shift_adjust = SHIFT_ADJ_2G;
-	ddata->regulator_enabled = 0;
+	ddata->device_status = DEVICE_OFF;
 	dev_set_name(&client->dev, ddata->pdata.name_a);
 
 	ddata->regulator = regulator_get(&client->dev, "v-accel");
@@ -984,9 +1025,9 @@ static int __devinit lsm303dlh_a_probe(struct i2c_client *client,
 		ddata->regulator = NULL;
 	}
 
-	if (ddata->regulator_enabled == 0 && ddata->regulator) {
+	if (ddata->regulator) {
 		regulator_enable(ddata->regulator);
-		ddata->regulator_enabled = 1;
+		ddata->device_status = DEVICE_ON;
 	}
 
 	ret = lsm303dlh_a_read(ddata, WHO_AM_I, "WHO_AM_I");
@@ -1085,9 +1126,9 @@ static int __devinit lsm303dlh_a_probe(struct i2c_client *client,
 	register_early_suspend(&ddata->early_suspend);
 #endif
 
-	if (ddata->regulator_enabled == 1 && ddata->regulator) {
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
 		regulator_disable(ddata->regulator);
-		ddata->regulator_enabled = 0;
+		ddata->device_status = DEVICE_OFF;
 	}
 	return ret;
 
@@ -1102,10 +1143,10 @@ err_input_alloc_failed:
 	input_free_device(ddata->input_dev);
 #endif
 exit_free_regulator:
-	if (ddata->regulator_enabled == 1 && ddata->regulator) {
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
 		regulator_disable(ddata->regulator);
 		regulator_put(ddata->regulator);
-		ddata->regulator_enabled = 0;
+		ddata->device_status = DEVICE_OFF;
 	}
 err_op_failed:
 	kfree(ddata);
@@ -1129,10 +1170,10 @@ static int __devexit lsm303dlh_a_remove(struct i2c_client *client)
 	/* safer to make device off */
 	if (ddata->mode != LSM303DLH_A_MODE_OFF) {
 		lsm303dlh_a_write(ddata, CTRL_REG1, 0, "CONTROL");
-		if (ddata->regulator && ddata->regulator_enabled == 1) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
 			regulator_disable(ddata->regulator);
 			regulator_put(ddata->regulator);
-			ddata->regulator_enabled = 0;
+			ddata->device_status = DEVICE_OFF;
 		}
 	}
 
diff --git a/drivers/hwmon/lsm303dlh_m.c b/drivers/hwmon/lsm303dlh_m.c
index 2cc6423a95b..2db50b6cba1 100644
--- a/drivers/hwmon/lsm303dlh_m.c
+++ b/drivers/hwmon/lsm303dlh_m.c
@@ -121,6 +121,11 @@
 /* Multiple byte transfer enable */
 #define MULTIPLE_I2C_TR 0x80
 
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
 /*
  * magnetometer local data
  */
@@ -141,7 +146,7 @@ struct lsm303dlh_m_data {
 	unsigned char rate;
 	unsigned char range;
 	struct early_suspend early_suspend;
-	int regulator_enabled;
+	int device_status;
 };
 
 #ifdef CONFIG_HAS_EARLYSUSPEND
@@ -165,39 +170,55 @@ static int lsm303dlh_m_write(struct lsm303dlh_m_data *ddata,
 static int lsm303dlh_m_do_suspend(struct lsm303dlh_m_data *ddata)
 {
 	int ret;
-	if (ddata->mode == LSM303DLH_M_MODE_SLEEP)
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		mutex_unlock(&ddata->lock);
 		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;
 
-	if (ddata->regulator && ddata->regulator_enabled == 1) {
+	if (ddata->regulator)
 		regulator_disable(ddata->regulator);
-		ddata->regulator_enabled = 0;
-	}
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+
 	return ret;
 }
 
 static int lsm303dlh_m_restore(struct lsm303dlh_m_data *ddata)
 {
 	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
 	/* in correct mode, no need to change it */
-	if (ddata->mode == LSM303DLH_M_MODE_SLEEP)
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		ddata->device_status = DEVICE_OFF;
+		mutex_unlock(&ddata->lock);
 		return 0;
+	} else
+		 ddata->device_status = DEVICE_ON;
 
 #ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
 	enable_irq(gpio_to_irq(ddata->pdata.irq_m));
 #endif
 
-	if (ddata->regulator_enabled == 0 && ddata->regulator) {
+	if (ddata->regulator)
 		regulator_enable(ddata->regulator);
-		ddata->regulator_enabled = 1;
-	}
 
 	ret = lsm303dlh_m_write(ddata, CRB_REG_M, ddata->range, "SET RANGE");
 
@@ -215,6 +236,7 @@ static int lsm303dlh_m_restore(struct lsm303dlh_m_data *ddata)
 		goto fail;
 
 fail:
+	mutex_unlock(&ddata->lock);
 	return ret;
 }
 
@@ -251,17 +273,17 @@ static ssize_t lsm303dlh_m_store_rate(struct device *dev,
 	unsigned char data;
 	int error;
 
-	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
-		return count;
-	}
-
 	error = strict_strtoul(buf, 0, &val);
 	if (error)
 		return error;
 
 	mutex_lock(&ddata->lock);
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
 
 	data = ((val << LSM303DLH_M_CRA_DO_BIT) & LSM303DLH_M_CRA_DO_MASK);
 	ddata->rate = data;
@@ -330,15 +352,16 @@ static ssize_t lsm303dlh_m_values(struct device *dev,
 	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
 	int ret = 0;
 
-	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
-		dev_info(&ddata->client->dev,
-				"device is switched off,make it ON using MODE");
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
 		return ret;
 	}
 
-	mutex_lock(&ddata->lock);
-
 	ret = lsm303dlh_m_xyz_read(ddata);
+
 	if (ret < 0) {
 		mutex_unlock(&ddata->lock);
 		return -EINVAL;
@@ -419,19 +442,22 @@ static ssize_t lsm303dlh_m_store_range(struct device *dev,
 	short xy_gain;
 	short z_gain;
 	unsigned long range;
-
 	int error;
 
+	error = strict_strtoul(buf, 0, &range);
+
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
 	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
 		dev_info(&ddata->client->dev,
 				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
 		return count;
 	}
 
-	error = strict_strtoul(buf, 0, &range);
-	if (error)
-		return error;
-
 	switch (range) {
 	case LSM303DLH_M_RANGE_1_3G:
 		xy_gain = XY_GAIN_1_3;
@@ -465,8 +491,6 @@ static ssize_t lsm303dlh_m_store_range(struct device *dev,
 		return -EINVAL;
 	}
 
-	mutex_lock(&ddata->lock);
-
 	ddata->gain[ddata->pdata.axis_map_x] = xy_gain;
 	ddata->gain[ddata->pdata.axis_map_y] = xy_gain;
 	ddata->gain[ddata->pdata.axis_map_z] = z_gain;
@@ -507,30 +531,44 @@ static ssize_t lsm303dlh_m_store_mode(struct device *dev,
 	if (error)
 		return error;
 
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_SUSPENDED &&
+			mode == LSM303DLH_M_MODE_SLEEP) {
+		ddata->mode = (mode >> LSM303DLH_M_MR_MD_BIT);
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
 	/*  if same mode as existing, return */
-	if (ddata->mode == mode)
+	if (ddata->mode == mode) {
+		mutex_unlock(&ddata->lock);
 		return 0;
+	}
 
 	/* turn on the supplies if already off */
 	if (ddata->mode == LSM303DLH_M_MODE_SLEEP && ddata->regulator
-			&& ddata->regulator_enabled == 0) {
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
 		regulator_enable(ddata->regulator);
-		ddata->regulator_enabled = 1;
+		ddata->device_status = DEVICE_ON;
 
 #ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
-	enable_irq(gpio_to_irq(ddata->pdata.irq_m));
+		enable_irq(gpio_to_irq(ddata->pdata.irq_m));
 #endif
 	}
 
-	mutex_lock(&ddata->lock);
-
 	error = lsm303dlh_m_set_mode(ddata, mode);
 
 	ddata->mode = (mode >> LSM303DLH_M_MR_MD_BIT);
-	mutex_unlock(&ddata->lock);
-
-	if (error < 0)
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
 		return error;
+	}
 
 	if (mode == LSM303DLH_M_MODE_SLEEP) {
 
@@ -550,11 +588,12 @@ static ssize_t lsm303dlh_m_store_mode(struct device *dev,
 		ddata->gain[ddata->pdata.axis_map_y] = XY_GAIN_1_3;
 		ddata->gain[ddata->pdata.axis_map_z] = Z_GAIN_1_3;
 
-		if (ddata->regulator_enabled == 1 && ddata->regulator) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
 			regulator_disable(ddata->regulator);
-			ddata->regulator_enabled = 0;
+			ddata->device_status = DEVICE_OFF;
 		}
 	}
+	mutex_unlock(&ddata->lock);
 
 	return count;
 }
@@ -612,7 +651,7 @@ static int __devinit lsm303dlh_m_probe(struct i2c_client *client,
 	ddata->gain[ddata->pdata.axis_map_x] = XY_GAIN_1_3;
 	ddata->gain[ddata->pdata.axis_map_y] = XY_GAIN_1_3;
 	ddata->gain[ddata->pdata.axis_map_z] = Z_GAIN_1_3;
-	ddata->regulator_enabled = 0;
+	ddata->device_status = DEVICE_OFF;
 	dev_set_name(&client->dev, ddata->pdata.name_m);
 	ddata->regulator = regulator_get(&client->dev, "v-mag");
 
@@ -622,9 +661,9 @@ static int __devinit lsm303dlh_m_probe(struct i2c_client *client,
 		ddata->regulator = NULL;
 	}
 
-	if (ddata->regulator && ddata->regulator_enabled == 0) {
+	if (ddata->regulator) {
 		regulator_enable(ddata->regulator);
-		ddata->regulator_enabled = 1;
+		ddata->device_status = DEVICE_ON;
 	}
 
 	ret = lsm303dlh_m_read_multi(ddata, IRA_REG_M, 3, version, "IRA_REG_M");
@@ -688,10 +727,11 @@ static int __devinit lsm303dlh_m_probe(struct i2c_client *client,
 	register_early_suspend(&ddata->early_suspend);
 #endif
 
-	if (ddata->regulator && ddata->regulator_enabled == 1) {
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
 		regulator_disable(ddata->regulator);
-		ddata->regulator_enabled = 0;
+		ddata->device_status = DEVICE_OFF;
 	}
+
 	return ret;
 
 #ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
@@ -701,13 +741,13 @@ err_input_register_failed:
 	input_free_device(ddata->input_dev);
 #endif
 exit_free_regulator:
-	if (ddata->regulator && ddata->regulator_enabled == 1) {
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
 		regulator_disable(ddata->regulator);
 		regulator_put(ddata->regulator);
-		ddata->regulator_enabled = 0;
+		ddata->device_status = DEVICE_OFF;
 	}
 err_op_failed:
-	dev_err(&client->dev, " lsm303dlh_m_probe failed %x", ret);
+	dev_err(&client->dev, "lsm303dlh_m_probe failed %x", ret);
 	kfree(ddata);
 	return ret;
 }
@@ -728,10 +768,10 @@ static int __devexit lsm303dlh_m_remove(struct i2c_client *client)
 	/* safer to make device off */
 	if (ddata->mode != LSM303DLH_M_MODE_SLEEP) {
 		lsm303dlh_m_set_mode(ddata, LSM303DLH_M_MODE_SLEEP);
-		if (ddata->regulator && ddata->regulator_enabled == 1) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
 			regulator_disable(ddata->regulator);
 			regulator_put(ddata->regulator);
-			ddata->regulator_enabled = 0;
+			ddata->device_status = DEVICE_OFF;
 		}
 	}