1 files changed, 66 insertions, 35 deletions

diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 284ab0063156..3fa78d477a41 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -246,6 +246,65 @@ static int omap_hsmmc_get_cover_state(struct device *dev)
 
 #ifdef CONFIG_REGULATOR
 
+static int omap_hsmmc_enable_supply(struct mmc_host *mmc, int vdd)
+{
+	int ret;
+
+	if (mmc->supply.vmmc) {
+		ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, vdd);
+		if (ret)
+			return ret;
+	}
+
+	/* Enable interface voltage rail, if needed */
+	if (mmc->supply.vqmmc) {
+		ret = regulator_enable(mmc->supply.vqmmc);
+		if (ret) {
+			dev_err(mmc_dev(mmc), "vmmc_aux reg enable failed\n");
+			goto err_vqmmc;
+		}
+	}
+
+	return 0;
+
+err_vqmmc:
+	if (mmc->supply.vmmc)
+		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
+
+	return ret;
+}
+
+static int omap_hsmmc_disable_supply(struct mmc_host *mmc)
+{
+	int ret;
+	int status;
+
+	if (mmc->supply.vqmmc) {
+		ret = regulator_disable(mmc->supply.vqmmc);
+		if (ret) {
+			dev_err(mmc_dev(mmc), "vmmc_aux reg disable failed\n");
+			return ret;
+		}
+	}
+
+	if (mmc->supply.vmmc) {
+		ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
+		if (ret)
+			goto err_set_ocr;
+	}
+
+	return 0;
+
+err_set_ocr:
+	if (mmc->supply.vqmmc) {
+		status = regulator_enable(mmc->supply.vqmmc);
+		if (status)
+			dev_err(mmc_dev(mmc), "vmmc_aux re-enable failed\n");
+	}
+
+	return ret;
+}
+
 static int omap_hsmmc_set_power(struct device *dev, int power_on, int vdd)
 {
 	struct omap_hsmmc_host *host =
@@ -291,36 +350,13 @@ static int omap_hsmmc_set_power(struct device *dev, int power_on, int vdd)
 	 * chips/cards need an interface voltage rail too.
 	 */
 	if (power_on) {
-		if (mmc->supply.vmmc) {
-			ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, vdd);
-			if (ret)
-				return ret;
-		}
-
-		/* Enable interface voltage rail, if needed */
-		if (mmc->supply.vqmmc) {
-			ret = regulator_enable(mmc->supply.vqmmc);
-			if (ret) {
-				dev_err(dev, "vmmc_aux reg enable failed\n");
-				goto err_set_vqmmc;
-			}
-		}
+		ret = omap_hsmmc_enable_supply(mmc, vdd);
+		if (ret)
+			return ret;
 	} else {
-		/* Shut down the rail */
-		if (mmc->supply.vqmmc) {
-			ret = regulator_disable(mmc->supply.vqmmc);
-			if (ret) {
-				dev_err(dev, "vmmc_aux reg disable failed\n");
-				return ret;
-			}
-		}
-
-		if (mmc->supply.vmmc) {
-			/* Then proceed to shut down the local regulator */
-			ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
-			if (ret)
-				return ret;
-		}
+		ret = omap_hsmmc_disable_supply(mmc);
+		if (ret)
+			return ret;
 	}
 
 	if (host->pbias) {
@@ -350,12 +386,7 @@ static int omap_hsmmc_set_power(struct device *dev, int power_on, int vdd)
 	return 0;
 
 err_set_voltage:
-	if (mmc->supply.vqmmc)
-		regulator_disable(mmc->supply.vqmmc);
-
-err_set_vqmmc:
-	if (mmc->supply.vmmc)
-		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
+	omap_hsmmc_disable_supply(mmc);
 
 	return ret;
 }