ppc4xx: Reconfigure PLL for 667MHz processor for PPC440EPx

On PPC440EPx without a bootstrap I2C EEPROM, the PLL can be reconfigured
after startup to change the speed of the clocks. This patch adds the
option CFG_PLL_RECONFIG. If this option is set to 667, the CPU
initialization code will reconfigure the PLL to run the system with a CPU
frequency of 667MHz and PLB frequency of 166MHz, without the need for an
external EEPROM.

Signed-off-by: Mike Nuss <mike@terascala.com>
Acked-by: Stefan Roese <sr@denx.de>

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)
 	/*