ppc4xx: Fix 440EPx bug in reconfigure_pll()

This patch fixes a bug in reconfigure_pll(), where the detection of
the current bootstrap option is wrong. The ICS bits where incorrectly
shifted. This bug was found on the lwmon5 board, which uses bootstrap
option H (I2C bootstrap EEPROM).

Additionally a bit of code was moved into the if statement, since its
only used after later on. No need to run this code all the time.

Also, a few empty lines are added to make the code better readable.

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

arch/powerpc/cpu/ppc4xx/cpu_init.c

@@ -142,22 +142,28 @@ void reconfigure_pll(u32 new_cpu_freq)
 	 * 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);
+		/*
+		 * Strapping option bits (ICS) are already in correct position,
+		 * only masking needed.
+		 */
+		reg &= CPR0_ICFG_ICS_MASK;
 
 		if ((reg == BOOT_STRAP_OPTION_A) || (reg == BOOT_STRAP_OPTION_B) ||
 		    (reg == BOOT_STRAP_OPTION_D) || (reg == BOOT_STRAP_OPTION_E)) {
+			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;
+
 			/*
 			 * Get current value of FWDVA. Assign current FWDVA to
 			 * new FWDVB.
@@ -165,12 +171,14 @@ void reconfigure_pll(u32 new_cpu_freq)
 			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.
 			 */
@@ -181,6 +189,7 @@ void reconfigure_pll(u32 new_cpu_freq)
 				((fbdv == 32 ? 0 : fbdv) << 24) |
 				(lfbdv == 64 ? 0 : lfbdv);
 			mtcpr(CPR0_PLLD, reg);
+
 			/* Acknowledge that a reset is required. */
 			reset_needed = 1;
 		}

include/ppc440.h

@@ -1711,6 +1711,7 @@
 #if defined(CONFIG_440EP) || defined(CONFIG_440GR) || \
     defined(CONFIG_440EPX) || defined(CONFIG_440GRX)
 #define CPR0_ICFG_RLI_MASK	0x80000000
+#define CPR0_ICFG_ICS_MASK	0x00000007
 #define CPR0_SPCID_SPCIDV0_MASK	0x03000000
 #define CPR0_SPCID_SPCIDV0_DIV1	0x01000000
 #define CPR0_SPCID_SPCIDV0_DIV2	0x02000000