1 files changed, 43 insertions, 55 deletions

diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index 5fd3b34274cc..109ce3dcf533 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -348,11 +348,8 @@ static int ab8500_regulator_set_mode(struct regulator_dev *rdev,
 				     unsigned int mode)
 {
 	int ret = 0;
-	u8 bank;
-	u8 reg;
-	u8 mask;
-	u8 val;
-	bool dmr = false; /* Dedicated mode register */
+	u8 bank, reg, mask, val;
+	bool lp_mode_req = false;
 	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
 
 	if (info == NULL) {
@@ -360,66 +357,54 @@ static int ab8500_regulator_set_mode(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
-	if (info->shared_mode) {
-		/*
-		 * Special case where mode is shared between two regulators.
-		 */
-		struct ab8500_shared_mode *sm = info->shared_mode;
-		mutex_lock(&shared_mode_mutex);
-
-		if (mode == REGULATOR_MODE_IDLE) {
-			sm->lp_mode_req = true; /* Low power mode requested */
-			if (!((sm->shared_regulator)->
-			      shared_mode->lp_mode_req)) {
-				mutex_unlock(&shared_mode_mutex);
-				return 0; /* Other regulator prevent LP mode */
-			}
-		} else {
-			sm->lp_mode_req = false;
-		}
-	}
-
 	if (info->mode_mask) {
-		/* Dedicated register for handling mode */
-
-		dmr = true;
-
-		switch (mode) {
-		case REGULATOR_MODE_NORMAL:
-			val = info->mode_val_normal;
-			break;
-		case REGULATOR_MODE_IDLE:
-			val = info->mode_val_idle;
-			break;
-		default:
-			ret = -EINVAL;
-			goto out_unlock;
-		}
-
 		bank = info->mode_bank;
 		reg = info->mode_reg;
 		mask = info->mode_mask;
 	} else {
-		/* Mode register same as enable register */
+		bank = info->update_bank;
+		reg = info->update_reg;
+		mask = info->update_mask;
+	}
+
+	if (info->shared_mode)
+		mutex_lock(&shared_mode_mutex);
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		if (info->shared_mode)
+			lp_mode_req = false;
 
-		switch (mode) {
-		case REGULATOR_MODE_NORMAL:
+		if (info->mode_mask)
+			val = info->mode_val_normal;
+		else
 			val = info->update_val_normal;
-			break;
-		case REGULATOR_MODE_IDLE:
-			val = info->update_val_idle;
-			break;
-		default:
-			ret = -EINVAL;
-			goto out_unlock;
+		break;
+	case REGULATOR_MODE_IDLE:
+		if (info->shared_mode) {
+			struct ab8500_regulator_info *shared_regulator;
+
+			shared_regulator = info->shared_mode->shared_regulator;
+			if (!shared_regulator->shared_mode->lp_mode_req) {
+				/* Other regulator prevent LP mode */
+				info->shared_mode->lp_mode_req = true;
+				goto out_unlock;
+			}
+
+			lp_mode_req = true;
 		}
 
-		bank = info->update_bank;
-		reg = info->update_reg;
-		mask = info->update_mask;
+		if (info->mode_mask)
+			val = info->mode_val_idle;
+		else
+			val = info->update_val_idle;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out_unlock;
 	}
 
-	if (dmr || ab8500_regulator_is_enabled(rdev)) {
+	if (info->mode_mask || ab8500_regulator_is_enabled(rdev)) {
 		ret = abx500_mask_and_set_register_interruptible(info->dev,
 			bank, reg, mask, val);
 		if (ret < 0) {
@@ -435,9 +420,12 @@ static int ab8500_regulator_set_mode(struct regulator_dev *rdev,
 			mask, val);
 	}
 
-	if (!dmr)
+	if (!info->mode_mask)
 		info->update_val = val;
 
+	if (info->shared_mode)
+		info->shared_mode->lp_mode_req = lp_mode_req;
+
 out_unlock:
 	if (info->shared_mode)
 		mutex_unlock(&shared_mode_mutex);