jz4760: do the MSC (ie SD) clocking setup when we change PLL0

Change-Id: Ia17b1d7069af507c3f029bcaed0f65e7e97df275

2 files changed, 13 insertions, 31 deletions

diff --git a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c
index 881c83066e..8da1313cf8 100644
--- a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c
@@ -689,12 +689,18 @@ void DMA_CALLBACK(DMA_SD_TX_CHANNEL1)(void)
 #endif                /* SD_DMA_INTERRUPT */
 #endif                /* SD_DMA_ENABLE */
 
+#ifndef HAVE_ADJUSTABLE_CPU_FREQ
+#define cpu_frequency __cpm_get_pllout2()
+#endif
+
 static inline unsigned int jz_sd_calc_clkrt(const int drive, unsigned int rate)
 {
-    unsigned int clkrt;
-    unsigned int clk_src = sd2_0[drive] ? SD_CLOCK_HIGH : SD_CLOCK_FAST;
+    unsigned int clkrt = 0;
+    unsigned int clk_src = cpu_frequency / __cpm_get_mscdiv(); /* MSC_CLK */
+
+    if (!sd2_0[drive] && rate > SD_CLOCK_FAST)
+        rate = SD_CLOCK_FAST;
 
-    clkrt = 0;
     while (rate < clk_src)
     {
         clkrt++;
@@ -703,33 +709,11 @@ static inline unsigned int jz_sd_calc_clkrt(const int drive, unsigned int rate)
     return clkrt;
 }
 
-#ifndef HAVE_ADJUSTABLE_CPU_FREQ
-#define cpu_frequency __cpm_get_pllout2()
-#endif
-
-void cpm_select_msc_clk(void)
-{
-    unsigned int div = cpu_frequency / SD_CLOCK_FAST;
-
-    if (div == 0)
-            div = 1;
-
-    if (div == __cpm_get_mscdiv())
-            return;
-
-    REG_CPM_MSCCDR = MSCCDR_MCS | (div - 1);
-    DEBUG("MSCCLK == %x\n", REG_CPM_MSCCDR);
-    __cpm_enable_pll_change();
-}
-
 /* Set the MMC clock frequency */
 static void jz_sd_set_clock(const int drive, unsigned int rate)
 {
     int clkrt;
 
-    /* select clock source from CPM */
-    cpm_select_msc_clk();
-
     clkrt = jz_sd_calc_clkrt(drive, rate);
     REG_MSC_CLKRT(MSC_CHN(drive)) = clkrt;
 
diff --git a/firmware/target/mips/ingenic_jz47xx/system-jz4760.c b/firmware/target/mips/ingenic_jz47xx/system-jz4760.c
index d94bea3f00..eab3ef64e6 100644
--- a/firmware/target/mips/ingenic_jz47xx/system-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/system-jz4760.c
@@ -491,12 +491,14 @@ static void pll0_init(unsigned int freq)
              | CPPCR0_PLLEN;             /* enable PLL */
 
     /*
-     * Init USB Host clock, pllout2 must be n*48MHz
-     * For JZ4760b UHC - River.
+     * Init USB Host clock, PLL0 must be multiple of 48MHz!
      */
     usbdiv = (cfcr & CPCCR_PCS) ? freq : (freq / 2);
     REG_CPM_UHCCDR = usbdiv / 48000000 - 1;
 
+    /* Init MSC clock; shoot for 48MHz base clock. */
+    REG_CPM_MSCCDR = MSCCDR_MCS | ((freq / 48000000) - 1);
+
     /* init PLL */
     REG_CPM_CPCCR = cfcr;
     REG_CPM_CPPCR0 = plcr1;
@@ -756,10 +758,7 @@ int system_memory_guard(int newmode)
     return 0;
 }
 
-
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
-void cpm_select_msc_clk(void);
-
 void set_cpu_frequency(long frequency)
 {
    if (frequency == cpu_frequency)
@@ -771,6 +770,5 @@ void set_cpu_frequency(long frequency)
 
    pll0_init(frequency);
    cpu_frequency = __cpm_get_pllout2();
-   cpm_select_msc_clk();
 }
 #endif