Merge commit drm-intel-fixes into topic/ppgtt

[firefly-linux-kernel-4.4.55.git] / drivers / mmc / host / sdhci-of-esdhc.c

diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index e328252ebf2a7f684d0756dbfe1c1b20935d05ab..0b249970b1197fcc92af6e63184c72cf7c406286 100644 (file)
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -199,6 +199,14 @@ static unsigned int esdhc_of_get_min_clock(struct sdhci_host *host)
 
 static void esdhc_of_set_clock(struct sdhci_host *host, unsigned int clock)
 {
+
+       int pre_div = 2;
+       int div = 1;
+       u32 temp;
+
+       if (clock == 0)
+               goto out;
+
        /* Workaround to reduce the clock frequency for p1010 esdhc */
        if (of_find_compatible_node(NULL, NULL, "fsl,p1010-esdhc")) {
                if (clock > 20000000)
@@ -207,8 +215,31 @@ static void esdhc_of_set_clock(struct sdhci_host *host, unsigned int clock)
                        clock -= 5000000;
        }
 
-       /* Set the clock */
-       esdhc_set_clock(host, clock, host->max_clk);
+       temp = sdhci_readl(host, ESDHC_SYSTEM_CONTROL);
+       temp &= ~(ESDHC_CLOCK_IPGEN | ESDHC_CLOCK_HCKEN | ESDHC_CLOCK_PEREN
+               | ESDHC_CLOCK_MASK);
+       sdhci_writel(host, temp, ESDHC_SYSTEM_CONTROL);
+
+       while (host->max_clk / pre_div / 16 > clock && pre_div < 256)
+               pre_div *= 2;
+
+       while (host->max_clk / pre_div / div > clock && div < 16)
+               div++;
+
+       dev_dbg(mmc_dev(host->mmc), "desired SD clock: %d, actual: %d\n",
+               clock, host->max_clk / pre_div / div);
+
+       pre_div >>= 1;
+       div--;
+
+       temp = sdhci_readl(host, ESDHC_SYSTEM_CONTROL);
+       temp |= (ESDHC_CLOCK_IPGEN | ESDHC_CLOCK_HCKEN | ESDHC_CLOCK_PEREN
+               | (div << ESDHC_DIVIDER_SHIFT)
+               | (pre_div << ESDHC_PREDIV_SHIFT));
+       sdhci_writel(host, temp, ESDHC_SYSTEM_CONTROL);
+       mdelay(1);
+out:
+       host->clock = clock;
 }
 
 #ifdef CONFIG_PM