From a521de4e5196bdb4b81c825d5ff3841513e92869 Mon Sep 17 00:00:00 2001
From: Johan Rudholm <johan.rudholm@stericsson.com>
Date: Fri, 2 Nov 2012 15:22:17 +0100
Subject: [PATCH] Clock and power control

---
 drivers/mmc/core/core.c  |   11 +++++------
 drivers/mmc/core/sd.c    |    6 ++----
 drivers/mmc/host/sdhci.c |   32 ++++++++++++++++++++++++++++++++
 3 files changed, 39 insertions(+), 10 deletions(-)

diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index dcfd451..e1e797a 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -1044,6 +1044,8 @@ u32 mmc_select_voltage(struct mmc_host *host, u32 ocr)
 	return ocr;
 }
 
+extern void clock_control(struct mmc_host *mmc, int on);
+
 int mmc_set_signal_voltage(struct mmc_host *host, int signal_voltage, bool cmd11)
 {
 	struct mmc_command cmd = {0};
@@ -1076,20 +1078,17 @@ int mmc_set_signal_voltage(struct mmc_host *host, int signal_voltage, bool cmd11
 		mmc_host_clk_hold(host);
 		clock = host->ios.clock;
 		if (cmd11) {
-			host->ios.clock = 0;
-			mmc_set_ios(host);
+			clock_control(host, 0);
 		}
 
 		err = host->ops->start_signal_voltage_switch(host, &host->ios);
 
 		if (err && cmd11) {
-			host->ios.clock = clock;
-			mmc_set_ios(host);
+			clock_control(host, 1);
 		} else if (cmd11) {
 			/* Stop clock for at least 5 ms according to spec */
 			mmc_delay(5);
-			host->ios.clock = clock;
-			mmc_set_ios(host);
+			clock_control(host, 1);
 
 			/* Wait for at least 1 ms according to spec */
 			mmc_delay(1);
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index dd4509f..8523c4f 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -720,6 +720,7 @@ struct device_type sd_type = {
 	.groups = sd_attr_groups,
 };
 
+extern void power_cycle(struct mmc_host *mmc);
 /*
  * Fetch CID from card.
  */
@@ -783,10 +784,7 @@ try_again:
 			pr_warning("%s: Signal voltage switch failed, "
 				"power cycling card (retries = %d)\n",
 				mmc_hostname(host), retries);
-			mmc_power_off(host);
-			/* Wait at least 1 ms according to spec */
-			mmc_delay(1);
-			mmc_power_up(host);
+			power_cycle(host);
 			retries--;
 			goto try_again;
 		} else if (err) {
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index f4b8b4d..bc29a04 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -1677,6 +1677,38 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 		return 0;
 }
 
+void power_cycle(struct mmc_host *mmc)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+	u8 pwr;
+
+	pwr = sdhci_readb(host, SDHCI_POWER_CONTROL);
+	pwr &= ~SDHCI_POWER_ON;
+	sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+
+	/* Wait for 1ms as per the spec */
+	usleep_range(1000, 1500);
+	pwr |= SDHCI_POWER_ON;
+	sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+}
+
+void clock_control(struct mmc_host *mmc, int on)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+	u16 clk;
+
+	if (on) {
+		clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+		clk |= SDHCI_CLOCK_CARD_EN;
+		sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+	} else {
+		/* Stop SDCLK */
+		clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+		clk &= ~SDHCI_CLOCK_CARD_EN;
+		sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+	}
+}
+
 static int sdhci_start_signal_voltage_switch(struct mmc_host *mmc,
 	struct mmc_ios *ios)
 {
-- 
1.7.10