2 files changed, 24 insertions, 0 deletions

diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 40de56d6da0b..de833b1eadb9 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -1653,6 +1653,29 @@ void sdhci_enable_clk(struct sdhci_host *host, u16 clk)
                 udelay(10);
         }
 
+        if (host->version >= SDHCI_SPEC_410 && host->v4_mode) {
+                clk |= SDHCI_CLOCK_PLL_EN;
+                clk &= ~SDHCI_CLOCK_INT_STABLE;
+                sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+
+                /* Wait max 150 ms */
+                timeout = ktime_add_ms(ktime_get(), 150);
+                while (1) {
+                        bool timedout = ktime_after(ktime_get(), timeout);
+
+                        clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+                        if (clk & SDHCI_CLOCK_INT_STABLE)
+                                break;
+                        if (timedout) {
+                                pr_err("%s: PLL clock never stabilised.\n",
+                                       mmc_hostname(host->mmc));
+                                sdhci_dumpregs(host);
+                                return;
+                        }
+                        udelay(10);
+                }
+        }
+
         clk |= SDHCI_CLOCK_CARD_EN;
         sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
 }
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 902f855efe8f..929310e869a3 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -114,6 +114,7 @@
 #define  SDHCI_DIV_HI_MASK      0x300
 #define  SDHCI_PROG_CLOCK_MODE  0x0020
 #define  SDHCI_CLOCK_CARD_EN    0x0004
+#define  SDHCI_CLOCK_PLL_EN     0x0008
 #define  SDHCI_CLOCK_INT_STABLE 0x0002
 #define  SDHCI_CLOCK_INT_EN     0x0001