1 files changed, 34 insertions, 17 deletions

diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 8741d0f5146a..031141a7c87e 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -38,6 +38,33 @@
 
 static unsigned int fmax = 515633;
 
+/*
+ * This must be called with host->lock held
+ */
+static void mmci_set_clkreg(struct mmci_host *host, unsigned int desired)
+{
+        u32 clk = 0;
+
+        if (desired) {
+                if (desired >= host->mclk) {
+                        clk = MCI_CLK_BYPASS;
+                        host->cclk = host->mclk;
+                } else {
+                        clk = host->mclk / (2 * desired) - 1;
+                        if (clk >= 256)
+                                clk = 255;
+                        host->cclk = host->mclk / (2 * (clk + 1));
+                }
+                if (host->hw_designer == 0x80)
+                        clk |= MCI_FCEN; /* Bug fix in ST IP block */
+                clk |= MCI_CLK_ENABLE;
+                /* This hasn't proven to be worthwhile */
+                /* clk |= MCI_CLK_PWRSAVE; */
+        }
+
+        writel(clk, host->base + MMCICLOCK);
+}
+
 static void
 mmci_request_end(struct mmci_host *host, struct mmc_request *mrq)
 {
@@ -419,22 +446,8 @@ static void mmci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
         struct mmci_host *host = mmc_priv(mmc);
-        u32 clk = 0, pwr = 0;
-
-        if (ios->clock) {
-                if (ios->clock >= host->mclk) {
-                        clk = MCI_CLK_BYPASS;
-                        host->cclk = host->mclk;
-                } else {
-                        clk = host->mclk / (2 * ios->clock) - 1;
-                        if (clk >= 256)
-                                clk = 255;
-                        host->cclk = host->mclk / (2 * (clk + 1));
-                }
-                if (host->hw_designer == AMBA_VENDOR_ST)
-                        clk |= MCI_FCEN; /* Bug fix in ST IP block */
-                clk |= MCI_CLK_ENABLE;
-        }
+        u32 pwr = 0;
+        unsigned long flags;
 
         if (host->plat->translate_vdd)
                 pwr |= host->plat->translate_vdd(mmc_dev(mmc), ios->vdd);
@@ -465,12 +478,16 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
                 }
         }
 
-        writel(clk, host->base + MMCICLOCK);
+        spin_lock_irqsave(&host->lock, flags);
+
+        mmci_set_clkreg(host, ios->clock);
 
         if (host->pwr != pwr) {
                 host->pwr = pwr;
                 writel(pwr, host->base + MMCIPOWER);
         }
+
+        spin_unlock_irqrestore(&host->lock, flags);
 }
 
 static int mmci_get_ro(struct mmc_host *mmc)