ppc4xx fix unstable 440EPx bootstrap options

440EPx fixed bootstrap options A, B, D, and E sets PLL FWDVA to a value = 1.
This results in the PLLOUTB being greater than the CPU clock frequency
resulting unstable 440EPx operation resulting in various software hang
conditions.

This patch reprograms the FWDVA satisfying the requirement of setting FWDVB
to a value greater than 1 while using one of the four deafult bootstrap options.

Signed-off-by: Rupjyoti Sarmah <rsarmah@amcc.com>
Acked-by : Victor Gallardo <vgallardo@appliedmicro.com>
Signed-off-by: Stefan Roese <sr@denx.de>

1 files changed, 60 insertions, 5 deletions

diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index ccd9993677..8a6e545c1a 100644
--- 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);
 	}