FSL LAW: Keep track of LAW allocations

[u-boot] / cpu / ppc4xx / cpu_init.c

diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index 2e0dd6f062fd44f9bc01c1e8593e7228dc34cd64..42eabfe568a5b6b0d48a5ebb8eb9bf435dcfe1f3 100644 (file)
--- a/cpu/ppc4xx/cpu_init.c
+++ b/cpu/ppc4xx/cpu_init.c
@@ -99,10 +99,107 @@ DECLARE_GLOBAL_DATA_PTR;
 # endif
 #endif /* CFG_INIT_DCACHE_CS */
 
+#ifndef CFG_PLL_RECONFIG
+#define CFG_PLL_RECONFIG       0
+#endif
+
+void reconfigure_pll(u32 new_cpu_freq)
+{
+#if defined(CONFIG_440EPX)
+       int     reset_needed = 0;
+       u32     reg, temp;
+       u32     prbdv0, target_prbdv0,                          /* CLK_PRIMBD */
+               fwdva, target_fwdva, fwdvb, target_fwdvb,       /* CLK_PLLD */
+               fbdv, target_fbdv, lfbdv, target_lfbdv,
+               perdv0, target_perdv0,                          /* CLK_PERD */
+               spcid0, target_spcid0;                          /* CLK_SPCID */
+
+       /* Reconfigure clocks if necessary.
+        * See PPC440EPx User's Manual, sections 8.2 and 14 */
+       if (new_cpu_freq == 667) {
+               target_prbdv0 = 2;
+               target_fwdva = 2;
+               target_fwdvb = 4;
+               target_fbdv = 20;
+               target_lfbdv = 1;
+               target_perdv0 = 4;
+               target_spcid0 = 4;
+
+               mfcpr(clk_primbd, reg);
+               temp = (reg & PRBDV_MASK) >> 24;
+               prbdv0 = temp ? temp : 8;
+               if (prbdv0 != target_prbdv0) {
+                       reg &= ~PRBDV_MASK;
+                       reg |= ((target_prbdv0 == 8 ? 0 : target_prbdv0) << 24);
+                       mtcpr(clk_primbd, reg);
+                       reset_needed = 1;
+               }
+
+               mfcpr(clk_plld, reg);
+
+               temp = (reg & PLLD_FWDVA_MASK) >> 16;
+               fwdva = temp ? temp : 16;
+
+               temp = (reg & PLLD_FWDVB_MASK) >> 8;
+               fwdvb = temp ? temp : 8;
+
+               temp = (reg & PLLD_FBDV_MASK) >> 24;
+               fbdv = temp ? temp : 32;
+
+               temp = (reg & PLLD_LFBDV_MASK);
+               lfbdv = temp ? temp : 64;
+
+               if (fwdva != target_fwdva || fbdv != target_fbdv || lfbdv != target_lfbdv) {
+                       reg &= ~(PLLD_FWDVA_MASK | PLLD_FWDVB_MASK |
+                                PLLD_FBDV_MASK | PLLD_LFBDV_MASK);
+                       reg |= ((target_fwdva == 16 ? 0 : target_fwdva) << 16) |
+                               ((target_fwdvb == 8 ? 0 : target_fwdvb) << 8) |
+                               ((target_fbdv == 32 ? 0 : target_fbdv) << 24) |
+                               (target_lfbdv == 64 ? 0 : target_lfbdv);
+                       mtcpr(clk_plld, reg);
+                       reset_needed = 1;
+               }
+
+               mfcpr(clk_perd, reg);
+               perdv0 = (reg & CPR0_PERD_PERDV0_MASK) >> 24;
+               if (perdv0 != target_perdv0) {
+                       reg &= ~CPR0_PERD_PERDV0_MASK;
+                       reg |= (target_perdv0 << 24);
+                       mtcpr(clk_perd, reg);
+                       reset_needed = 1;
+               }
+
+               mfcpr(clk_spcid, reg);
+               temp = (reg & CPR0_SPCID_SPCIDV0_MASK) >> 24;
+               spcid0 = temp ? temp : 4;
+               if (spcid0 != target_spcid0) {
+                       reg &= ~CPR0_SPCID_SPCIDV0_MASK;
+                       reg |= ((target_spcid0 == 4 ? 0 : target_spcid0) << 24);
+                       mtcpr(clk_spcid, reg);
+                       reset_needed = 1;
+               }
+
+               /* Set reload inhibit so configuration will persist across
+                * processor resets */
+               mfcpr(clk_icfg, reg);
+               reg &= ~CPR0_ICFG_RLI_MASK;
+               reg |= 1 << 31;
+               mtcpr(clk_icfg, reg);
+       }
+
+       /* Reset processor if configuration changed */
+       if (reset_needed) {
+               __asm__ __volatile__ ("sync; isync");
+               mtspr(dbcr0, 0x20000000);
+       }
+#endif
+}
+
 /*
  * Breath some life into the CPU...
  *
- * Set up the memory map,
+ * Reconfigure PLL if necessary,
+ * set up the memory map,
  * initialize a bunch of registers
  */
 void
@@ -111,6 +208,7 @@ cpu_init_f (void)
 #if defined(CONFIG_WATCHDOG)
        unsigned long val;
 #endif
+       reconfigure_pll(CFG_PLL_RECONFIG);
 
 #if (defined(CONFIG_405EP) || defined (CONFIG_405EX)) && !defined(CFG_4xx_GPIO_TABLE)
        /*
@@ -135,6 +233,7 @@ cpu_init_f (void)
 #if defined (CFG_GPIO0_TCR)
        out32(GPIO0_TCR, CFG_GPIO0_TCR);        /* enable output driver for outputs     */
 #endif
+#endif /* CONFIG_405EP ... && !CFG_4xx_GPIO_TABLE */
 
 #if defined (CONFIG_405EP)
        /*
@@ -147,7 +246,6 @@ cpu_init_f (void)
         */
        mtdcr(cpc0_pci, mfdcr(cpc0_pci) | CPC0_PCI_HOST_CFG_EN | CPC0_PCI_ARBIT_EN);
 #endif /* CONFIG_405EP */
-#endif /* CONFIG_405EP */
 
 #if defined(CFG_4xx_GPIO_TABLE)
        gpio_set_chip_configuration();
@@ -302,5 +400,6 @@ int cpu_init_r (void)
        }
 #endif  /* defined(CONFIG_405GP) */
 #endif  /* defined(CONFIG_405GP) || defined(CONFIG_405EP) */
+
        return (0);
 }