ppc4xx fix unstable 440EPx bootstrap options

[karo-tx-uboot.git] / cpu / ppc4xx / cpu_init.c

diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index ccd9993677d6f9514c09b487aa988b56efc4baf7..8a6e545c1aba8987fc3c651d7cfd22b8408108e2 100644 (file)
--- a/cpu/ppc4xx/cpu_init.c
+++ b/cpu/ppc4xx/cpu_init.c
@@ -111,17 +111,72 @@ void reconfigure_pll(u32 new_cpu_freq)
                        mtcpr(CPR0_SPCID, reg);
                        reset_needed = 1;
                }
+       }
+
+       /* Get current value of FWDVA.*/
+       mfcpr(CPR0_PLLD, reg);
+       temp = (reg & PLLD_FWDVA_MASK) >> 16;
 
-               /* Set reload inhibit so configuration will persist across
-                * processor resets */
+       /*
+        * Check to see if FWDVA has been set to value of 1. if it has we must
+        * modify it.
+        */
+       if (temp == 1) {
+               mfcpr(CPR0_PLLD, reg);
+               /* Get current value of fbdv.  */
+               temp = (reg & PLLD_FBDV_MASK) >> 24;
+               fbdv = temp ? temp : 32;
+               /* Get current value of lfbdv. */
+               temp = (reg & PLLD_LFBDV_MASK);
+               lfbdv = temp ? temp : 64;
+               /*
+                * Load register that contains current boot strapping option.
+                */
+               mfcpr(CPR0_ICFG, reg);
+               /* Shift strapping option into low 3 bits.*/
+               reg = (reg >> 28);
+
+               if ((reg == BOOT_STRAP_OPTION_A) || (reg == BOOT_STRAP_OPTION_B) ||
+                   (reg == BOOT_STRAP_OPTION_D) || (reg == BOOT_STRAP_OPTION_E)) {
+                       /*
+                        * Get current value of FWDVA. Assign current FWDVA to
+                        * new FWDVB.
+                        */
+                       mfcpr(CPR0_PLLD, reg);
+                       target_fwdvb = (reg & PLLD_FWDVA_MASK) >> 16;
+                       fwdvb = target_fwdvb ? target_fwdvb : 8;
+                       /*
+                        * Get current value of FWDVB. Assign current FWDVB to
+                        * new FWDVA.
+                        */
+                       target_fwdva = (reg & PLLD_FWDVB_MASK) >> 8;
+                       fwdva = target_fwdva ? target_fwdva : 16;
+                       /*
+                        * Update CPR0_PLLD with switched FWDVA and FWDVB.
+                        */
+                       reg &= ~(PLLD_FWDVA_MASK | PLLD_FWDVB_MASK |
+                               PLLD_FBDV_MASK | PLLD_LFBDV_MASK);
+                       reg |= ((fwdva == 16 ? 0 : fwdva) << 16) |
+                               ((fwdvb == 8 ? 0 : fwdvb) << 8) |
+                               ((fbdv == 32 ? 0 : fbdv) << 24) |
+                               (lfbdv == 64 ? 0 : lfbdv);
+                       mtcpr(CPR0_PLLD, reg);
+                       /* Acknowledge that a reset is required. */
+                       reset_needed = 1;
+               }
+       }
+
+       if (reset_needed) {
+               /*
+                * Set reload inhibit so configuration will persist across
+                * processor resets
+                */
                mfcpr(CPR0_ICFG, reg);
                reg &= ~CPR0_ICFG_RLI_MASK;
                reg |= 1 << 31;
                mtcpr(CPR0_ICFG, reg);
-       }
 
-       /* Reset processor if configuration changed */
-       if (reset_needed) {
+               /* Reset processor if configuration changed */
                __asm__ __volatile__ ("sync; isync");
                mtspr(SPRN_DBCR0, 0x20000000);
        }