PPC405EP support added.

8 files changed, 404 insertions, 21 deletions

diff --git a/cpu/ppc4xx/405gp_enet.c b/cpu/ppc4xx/405gp_enet.c
index 49e9e424fd..af3c1b788b 100644
--- a/cpu/ppc4xx/405gp_enet.c
+++ b/cpu/ppc4xx/405gp_enet.c
@@ -80,7 +80,7 @@
 #include <malloc.h>
 #include "vecnum.h"
 
-#if defined(CONFIG_405GP) || defined(CONFIG_440)
+#if defined(CONFIG_405GP) || defined(CONFIG_440) || defined(CONFIG_405EP)
 
 #define EMAC_RESET_TIMEOUT 1000	/* 1000 ms reset timeout */
 #define PHY_AUTONEGOTIATE_TIMEOUT 2000	/* 2000 ms autonegotiate timeout */
@@ -705,7 +705,7 @@ void mal_err (unsigned long isr, unsigned long uic, unsigned long maldef,
 	mtdcr (maltxdeir, 0xC0000000);
 	mtdcr (malrxdeir, 0x80000000);
 
-#if 1	/*sr */
+#ifdef INFO_405_ENET
 	printf ("\nMAL error occured.... ISR = %lx UIC = = %lx  MAL_DEF = %lx  MAL_ERR= %lx \n",
 		isr, uic, maldef, mal_errr);
 #else
@@ -716,7 +716,7 @@ void mal_err (unsigned long isr, unsigned long uic, unsigned long maldef,
 	 */
 	printf ("M");			/* just to see something upon mal error */
 #endif
-#endif	/*sr */
+#endif
 
 	eth_init (bis_save);		/* start again... */
 }
diff --git a/cpu/ppc4xx/405gp_pci.c b/cpu/ppc4xx/405gp_pci.c
index fbd4d6db21..728952312e 100644
--- a/cpu/ppc4xx/405gp_pci.c
+++ b/cpu/ppc4xx/405gp_pci.c
@@ -78,7 +78,7 @@
 #include <asm/processor.h>
 #include <pci.h>
 
-#if defined(CONFIG_405GP)
+#if defined(CONFIG_405GP) || defined(CONFIG_405EP)
 
 #ifdef CONFIG_PCI
 
diff --git a/cpu/ppc4xx/cpu.c b/cpu/ppc4xx/cpu.c
index 8532d2873c..095a0aa38c 100644
--- a/cpu/ppc4xx/cpu.c
+++ b/cpu/ppc4xx/cpu.c
@@ -48,17 +48,24 @@ static int do_chip_reset( unsigned long sys0, unsigned long sys1 );
 
 int checkcpu (void)
 {
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_IOP480) || defined(CONFIG_440)
+#if defined(CONFIG_405GP) || \
+    defined(CONFIG_405CR) || \
+    defined(CONFIG_IOP480) || \
+    defined(CONFIG_440) || \
+    defined(CONFIG_405EP)
 	uint pvr = get_pvr();
 #endif
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_IOP480)
+#if defined(CONFIG_405GP) || \
+    defined(CONFIG_405CR) || \
+    defined(CONFIG_IOP480) || \
+    defined(CONFIG_405EP)
 	DECLARE_GLOBAL_DATA_PTR;
 
 	ulong clock = gd->cpu_clk;
 	char buf[32];
 #endif
 
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405EP)
 	PPC405_SYS_INFO sys_info;
 
 	puts ("CPU:   ");
@@ -75,6 +82,9 @@ int checkcpu (void)
 #if CONFIG_405CR
 	puts("IBM PowerPC 405CR Rev. ");
 #endif
+#if CONFIG_405EP
+	puts("IBM PowerPC 405EP Rev. ");
+#endif
 	switch (pvr) {
 	case PVR_405GP_RB:
 	case PVR_405GPR_RB:
@@ -98,6 +108,7 @@ int checkcpu (void)
 		putc('A');
 		break;
 	case PVR_405CR_RB:
+	case PVR_405EP_RB:
 		putc('B');
 		break;
 	default:
@@ -110,7 +121,7 @@ int checkcpu (void)
 	       sys_info.freqPLB / sys_info.pllOpbDiv / 1000000,
 	       sys_info.freqPLB / sys_info.pllExtBusDiv / 1000000);
 
-#if CONFIG_405GP
+#if defined(CONFIG_405GP)
 	if (mfdcr(strap) & PSR_PCI_ASYNC_EN)
 		printf("           PCI async ext clock used, ");
 	else
@@ -120,15 +131,27 @@ int checkcpu (void)
 		printf("internal PCI arbiter enabled\n");
 	else
 		printf("external PCI arbiter enabled\n");
+#elif defined(CONFIG_405EP)
+	if (mfdcr(cpc0_boot) & CPC0_BOOT_SEP)
+		printf("           IIC Boot EEPROM enabled\n");
+	else
+		printf("           IIC Boot EEPROM disabled\n");
+	printf("           PCI async ext clock used, ");
+	if (mfdcr(cpc0_pci) & CPC0_PCI_ARBIT_EN)
+		printf("internal PCI arbiter enabled\n");
+	else
+		printf("external PCI arbiter enabled\n");
 #endif
 
+#if defined(CONFIG_405EP)
+	printf("           16 kB I-Cache 16 kB D-Cache");
+#else
 	if ((pvr | 0x00000001) == PVR_405GPR_RB) {
 		printf("           16 kB I-Cache 16 kB D-Cache");
 	} else {
 		printf("           16 kB I-Cache 8 kB D-Cache");
 	}
-
-
+#endif
 #endif  /* defined(CONFIG_405GP) || defined(CONFIG_405CR) */
 
 #ifdef CONFIG_IOP480
@@ -213,7 +236,10 @@ unsigned long get_tbclk (void)
 	get_sys_info(&sys_info);
 	return (sys_info.freqProcessor);
 
-#elif defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405)
+#elif defined(CONFIG_405GP) || \
+      defined(CONFIG_405CR) || \
+      defined(CONFIG_405) || \
+      defined(CONFIG_405EP)
 
 	PPC405_SYS_INFO sys_info;
 
diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index 1d149bc608..ac34092d8b 100644
--- a/cpu/ppc4xx/cpu_init.c
+++ b/cpu/ppc4xx/cpu_init.c
@@ -40,6 +40,24 @@
 void
 cpu_init_f (void)
 {
+#if defined(CONFIG_405EP)
+	/*
+	 * GPIO0 setup (select GPIO or alternate function)
+	 */
+	out32(GPIO0_OSRH, CFG_GPIO0_OSRH);   /* output select */
+	out32(GPIO0_OSRL, CFG_GPIO0_OSRL);
+	out32(GPIO0_ISR1H, CFG_GPIO0_ISR1H); /* input select */
+	out32(GPIO0_ISR1L, CFG_GPIO0_ISR1L);
+	out32(GPIO0_TSRH, CFG_GPIO0_TSRH);   /* three-state select */
+	out32(GPIO0_TSRL, CFG_GPIO0_TSRL);
+	out32(GPIO0_TCR, CFG_GPIO0_TCR);     /* enable output driver for outputs */
+
+	/*
+	 * Set EMAC noise filter bits
+	 */
+	mtdcr(cpc0_epctl, CPC0_EPRCSR_E0NFE | CPC0_EPRCSR_E1NFE);
+#endif /* CONFIG_405EP */
+
 	/*
 	 * External Bus Controller (EBC) Setup
 	 */
@@ -119,12 +137,14 @@ cpu_init_f (void)
  */
 int cpu_init_r (void)
 {
-#ifdef CONFIG_405GP
+#if defined(CONFIG_405GP)  || defined(CONFIG_405EP)
 	DECLARE_GLOBAL_DATA_PTR;
 
 	bd_t *bd = gd->bd;
 	unsigned long reg;
+#if defined(CONFIG_405GP)
 	uint pvr = get_pvr();
+#endif
 
 	/*
 	 * Write Ethernetaddress into on-chip register
@@ -145,6 +165,7 @@ int cpu_init_r (void)
 	reg |= bd->bi_enetaddr[5];
 	out32 (EMAC_IAL, reg);
 
+#if defined(CONFIG_405GP)
 	/*
 	 * Set edge conditioning circuitry on PPC405GPr
 	 * for compatibility to existing PPC405GP designs.
@@ -152,7 +173,7 @@ int cpu_init_r (void)
 	if ((pvr & 0xfffffff0) == (PVR_405GPR_RB & 0xfffffff0)) {
 		mtdcr(ecr, 0x60606000);
 	}
-
-#endif  /* CONFIG_405GP */
+#endif  /* defined(CONFIG_405GP) */
+#endif  /* defined(CONFIG_405GP) || defined(CONFIG_405EP) */
 	return (0);
 }
diff --git a/cpu/ppc4xx/miiphy.c b/cpu/ppc4xx/miiphy.c
index 51ba47111b..c5a90a9f21 100644
--- a/cpu/ppc4xx/miiphy.c
+++ b/cpu/ppc4xx/miiphy.c
@@ -48,7 +48,7 @@
 #include <405_mal.h>
 #include <miiphy.h>
 
-#if defined(CONFIG_405GP) || defined(CONFIG_440)
+#if defined(CONFIG_405GP) || defined(CONFIG_440) || defined(CONFIG_405EP)
 
 
 /***********************************************************/
diff --git a/cpu/ppc4xx/spd_sdram.c b/cpu/ppc4xx/spd_sdram.c
index 9c1cac5d0e..76aee2e0e4 100644
--- a/cpu/ppc4xx/spd_sdram.c
+++ b/cpu/ppc4xx/spd_sdram.c
@@ -118,9 +118,11 @@ long int spd_sdram(int(read_spd)(uint addr))
 	int bank_cnt;
 
 	int sdram0_pmit=0x07c00000;
+#ifndef CONFIG_405EP /* not on PPC405EP */
 	int sdram0_besr0=-1;
 	int sdram0_besr1=-1;
 	int sdram0_eccesr=-1;
+#endif
 	int sdram0_ecccfg;
 
 	int sdram0_rtr=0;
@@ -153,8 +155,46 @@ long int spd_sdram(int(read_spd)(uint addr))
 	 * Calculate the bus period, we do it this
 	 * way to minimize stack utilization.
 	 */
+#ifndef CONFIG_405EP
     	tmp = (mfdcr(pllmd) >> (31-6)) & 0xf;	/* get FBDV bits */
 	tmp = CONFIG_SYS_CLK_FREQ * tmp;	/* get plb freq */
+#else
+	{
+		unsigned long freqCPU;
+		unsigned long pllmr0;
+		unsigned long pllmr1;
+		unsigned long pllFbkDiv;
+		unsigned long pllPlbDiv;
+		unsigned long pllmr0_ccdv;
+
+		/*
+		 * Read PLL Mode registers
+		 */
+		pllmr0 = mfdcr (cpc0_pllmr0);
+		pllmr1 = mfdcr (cpc0_pllmr1);
+
+		pllFbkDiv = ((pllmr1 & PLLMR1_FBMUL_MASK) >> 20);
+		if (pllFbkDiv == 0) {
+			pllFbkDiv = 16;
+		}
+		pllPlbDiv = ((pllmr0 & PLLMR0_CPU_TO_PLB_MASK) >> 16) + 1;
+
+		/*
+		 * Determine CPU clock frequency
+		 */
+		pllmr0_ccdv = ((pllmr0 & PLLMR0_CPU_DIV_MASK) >> 20) + 1;
+		if (pllmr1 & PLLMR1_SSCS_MASK) {
+			freqCPU = (CONFIG_SYS_CLK_FREQ * pllFbkDiv) / pllmr0_ccdv;
+		} else {
+			freqCPU = CONFIG_SYS_CLK_FREQ / pllmr0_ccdv;
+		}
+
+		/*
+		 * Determine PLB clock frequency
+		 */
+		tmp = freqCPU / pllPlbDiv;
+	}
+#endif
 	bus_period = sdram_HZ_to_ns(tmp);	/* get sdram speed */
 
      	/* Make shure we are using SDRAM */
@@ -414,8 +454,12 @@ long int spd_sdram(int(read_spd)(uint addr))
 	sdram0_cfg = 0;
 	mtsdram0( mem_mcopt1, sdram0_cfg );
 
+#ifndef CONFIG_405EP /* not on PPC405EP */
 	mtsdram0( mem_besra , sdram0_besr0 );
 	mtsdram0( mem_besrb , sdram0_besr1 );
+	mtsdram0( mem_ecccf , sdram0_ecccfg );
+	mtsdram0( mem_eccerr, sdram0_eccesr );
+#endif
 	mtsdram0( mem_rtr   , sdram0_rtr );
 	mtsdram0( mem_pmit  , sdram0_pmit );
 	mtsdram0( mem_mb0cf , sdram0_b0cr );
@@ -423,8 +467,6 @@ long int spd_sdram(int(read_spd)(uint addr))
 	mtsdram0( mem_mb2cf , sdram0_b2cr );
 	mtsdram0( mem_mb3cf , sdram0_b3cr );
 	mtsdram0( mem_sdtr1 , sdram0_tr );
-	mtsdram0( mem_ecccf , sdram0_ecccfg );
-	mtsdram0( mem_eccerr, sdram0_eccesr );
 
 	/* SDRAM have a power on delay,  500 micro should do */
 	udelay(500);
diff --git a/cpu/ppc4xx/speed.c b/cpu/ppc4xx/speed.c
index f075e3a047..fdefbb6877 100644
--- a/cpu/ppc4xx/speed.c
+++ b/cpu/ppc4xx/speed.c
@@ -257,11 +257,132 @@ void get_sys_info (sys_info_t * sysInfo) {
 
 }
 
+#elif defined(CONFIG_405EP)
+void get_sys_info (PPC405_SYS_INFO * sysInfo)
+{
+	unsigned long pllmr0;
+	unsigned long pllmr1;
+	unsigned long sysClkPeriodPs = ONE_BILLION / (CONFIG_SYS_CLK_FREQ / 1000);
+	unsigned long m;
+	unsigned long pllmr0_ccdv;
+
+	/*
+	 * Read PLL Mode registers
+	 */
+	pllmr0 = mfdcr (cpc0_pllmr0);
+	pllmr1 = mfdcr (cpc0_pllmr1);
+
+	/*
+	 * Determine forward divider A
+	 */
+	sysInfo->pllFwdDiv = 8 - ((pllmr1 & PLLMR1_FWDVA_MASK) >> 16);
+
+	/*
+	 * Determine forward divider B (should be equal to A)
+	 */
+	sysInfo->pllFwdDivB = 8 - ((pllmr1 & PLLMR1_FWDVB_MASK) >> 12);
+
+	/*
+	 * Determine FBK_DIV.
+	 */
+	sysInfo->pllFbkDiv = ((pllmr1 & PLLMR1_FBMUL_MASK) >> 20);
+	if (sysInfo->pllFbkDiv == 0) {
+		sysInfo->pllFbkDiv = 16;
+	}
+
+	/*
+	 * Determine PLB_DIV.
+	 */
+	sysInfo->pllPlbDiv = ((pllmr0 & PLLMR0_CPU_TO_PLB_MASK) >> 16) + 1;
+
+	/*
+	 * Determine PCI_DIV.
+	 */
+	sysInfo->pllPciDiv = (pllmr0 & PLLMR0_PCI_TO_PLB_MASK) + 1;
+
+	/*
+	 * Determine EXTBUS_DIV.
+	 */
+	sysInfo->pllExtBusDiv = ((pllmr0 & PLLMR0_EXB_TO_PLB_MASK) >> 8) + 2;
+
+	/*
+	 * Determine OPB_DIV.
+	 */
+	sysInfo->pllOpbDiv = ((pllmr0 & PLLMR0_OPB_TO_PLB_MASK) >> 12) + 1;
+
+	/*
+	 * Determine the M factor
+	 */
+	m = sysInfo->pllFbkDiv * sysInfo->pllFwdDivB;
+
+	/*
+	 * Determine VCO clock frequency
+	 */
+	sysInfo->freqVCOMhz = (1000000 * m) / sysClkPeriodPs;
+
+	/*
+	 * Determine CPU clock frequency
+	 */
+	pllmr0_ccdv = ((pllmr0 & PLLMR0_CPU_DIV_MASK) >> 20) + 1;
+	if (pllmr1 & PLLMR1_SSCS_MASK) {
+		sysInfo->freqProcessor = (CONFIG_SYS_CLK_FREQ * sysInfo->pllFbkDiv)
+			/ pllmr0_ccdv;
+	} else {
+		sysInfo->freqProcessor = CONFIG_SYS_CLK_FREQ / pllmr0_ccdv;
+	}
+
+	/*
+	 * Determine PLB clock frequency
+	 */
+	sysInfo->freqPLB = sysInfo->freqProcessor / sysInfo->pllPlbDiv;
+
+	if (!((sysInfo->freqVCOMhz >= VCO_MIN) && (sysInfo->freqVCOMhz <= VCO_MAX))) {
+		printf ("\nInvalid VCO frequency calculated :  %ld MHz \a\n",
+			sysInfo->freqVCOMhz);
+		printf ("It must be between %d-%d MHz \a\n", VCO_MIN, VCO_MAX);
+		printf ("PLL Mode reg 0           :  %8.8lx\a\n", pllmr0);
+		printf ("PLL Mode reg 1           :  %8.8lx\a\n", pllmr1);
+		hang ();
+	}
+}
+
+
+/********************************************
+ * get_OPB_freq
+ * return OPB bus freq in Hz
+ *********************************************/
+ulong get_OPB_freq (void)
+{
+	ulong val = 0;
+
+	PPC405_SYS_INFO sys_info;
+
+	get_sys_info (&sys_info);
+	val = sys_info.freqPLB / sys_info.pllOpbDiv;
+
+	return val;
+}
+
+
+/********************************************
+ * get_PCI_freq
+ * return PCI bus freq in Hz
+ *********************************************/
+ulong get_PCI_freq (void)
+{
+	ulong val;
+	PPC405_SYS_INFO sys_info;
+
+	get_sys_info (&sys_info);
+	val = sys_info.freqPLB / sys_info.pllPciDiv;
+	return val;
+}
+
 #endif
 
 int get_clocks (void)
 {
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_440) || defined(CONFIG_405)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_440) || defined(CONFIG_405) || defined(CONFIG_405EP)
 	DECLARE_GLOBAL_DATA_PTR;
 
 	sys_info_t sys_info;
@@ -290,7 +411,7 @@ ulong get_bus_freq (ulong dummy)
 {
 	ulong val;
 
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405) || defined(CONFIG_440)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405) || defined(CONFIG_440) || defined(CONFIG_405EP)
 	sys_info_t sys_info;
 
 	get_sys_info (&sys_info);
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index c334f936b3..afe8635585 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -297,7 +297,7 @@ _start_440:
 	mflr	r1
 	mtspr	srr0,r1
 	rfi
-#endif
+#endif /* CONFIG_440 */
 
 /*
  * r3 - 1st arg to board_init(): IMMP pointer
@@ -504,7 +504,7 @@ _start:
 #endif	/* CONFIG_IOP480 */
 
 /*****************************************************************************/
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405) || defined(CONFIG_405EP)
 	/*----------------------------------------------------------------------- */
 	/* Clear and set up some registers. */
 	/*----------------------------------------------------------------------- */
@@ -551,6 +551,17 @@ _start:
 	bl	ext_bus_cntlr_init
 #endif
 
+#if defined(CONFIG_405EP)
+	/*----------------------------------------------------------------------- */
+	/* DMA Status, clear to come up clean */
+	/*----------------------------------------------------------------------- */
+        addis   r3,r0, 0xFFFF         /* Clear all existing DMA status */
+        ori     r3,r3, 0xFFFF
+        mtdcr   dmasr, r3
+
+	bl	ppc405ep_init         /* do ppc405ep specific init */
+#endif /* CONFIG_405EP */
+
 #if defined(CFG_OCM_DATA_ADDR) && defined(CFG_OCM_DATA_SIZE)
 	/********************************************************************
 	 * Setup OCM - On Chip Memory
@@ -693,6 +704,7 @@ _start:
 #endif	/* CONFIG_405GP || CONFIG_405CR */
 
 
+/*****************************************************************************/
 	.globl	_start_of_vectors
 _start_of_vectors:
 
@@ -1437,3 +1449,164 @@ trap_reloc:
 	stw	r0, 4(r7)
 
 	blr
+
+
+/**************************************************************************/
+/* PPC405EP specific stuff                                                */
+/**************************************************************************/
+#ifdef CONFIG_405EP
+ppc405ep_init:
+        /*
+        !-----------------------------------------------------------------------
+        ! Check FPGA for PCI internal/external arbitration
+        !   If board is set to internal arbitration, update cpc0_pci
+        !-----------------------------------------------------------------------
+	*/
+        addi    r3,0,CPC0_PCI_HOST_CFG_EN
+#ifdef CONFIG_BUBINGA405EP
+        addis   r5,r0,FPGA_REG1@h      /* set offset for FPGA_REG1 */
+        ori     r5,r5,FPGA_REG1@l
+        lbz     r5,0x0(r5)              /* read to get PCI arb selection */
+        andi.   r6,r5,FPGA_REG1_PCI_INT_ARB  /* using internal arbiter ?*/
+        beq     ..pci_cfg_set             /* if not set, then bypass reg write*/
+#endif
+        ori     r3,r3,CPC0_PCI_ARBIT_EN
+..pci_cfg_set:
+        mtdcr   CPC0_PCI, r3             /* Enable internal arbiter*/
+
+        /*
+        !-----------------------------------------------------------------------
+        ! Check to see if chip is in bypass mode.
+        ! If so, write stored CPC0_PLLMR0 and CPC0_PLLMR1 values and perform a
+        ! CPU reset   Otherwise, skip this step and keep going.
+        ! Note:  Running BIOS in bypass mode is not supported since PLB speed
+        !        will not be fast enough for the SDRAM (min 66MHz)
+        !-----------------------------------------------------------------------
+	*/
+        mfdcr   r5, CPC0_PLLMR1
+        rlwinm  r4,r5,1,0x1            /* get system clock source (SSCS) */
+        cmpi    cr0,0,r4,0x1
+
+        beq    pll_done                   /* if SSCS =b'1' then PLL has */
+                                          /* already been set */
+                                          /* and CPU has been reset */
+                                          /* so skip to next section */
+
+#ifdef CONFIG_BUBINGA405EP
+	/*
+        !-----------------------------------------------------------------------
+        ! Read NVRAM to get value to write in PLLMR.
+        ! If value has not been correctly saved, write default value
+        ! Default config values (assuming on-board 33MHz SYS_CLK) are above.
+        ! See CPU_DEFAULT_200 and CPU_DEFAULT_266 above.
+        !
+        ! WARNING:  This code assumes the first three words in the nvram_t
+        !           structure in openbios.h.  Changing the beginning of
+        !           the structure will break this code.
+        !
+        !-----------------------------------------------------------------------
+	*/
+        addis   r3,0,NVRAM_BASE@h
+        addi    r3,r3,NVRAM_BASE@l
+
+        lwz     r4, 0(r3)
+        addis   r5,0,NVRVFY1@h
+        addi    r5,r5,NVRVFY1@l
+        cmp     cr0,0,r4,r5            /* Compare 1st NVRAM Magic number*/
+        bne     ..no_pllset
+        addi    r3,r3,4
+        lwz     r4, 0(r3)
+        addis   r5,0,NVRVFY2@h
+        addi    r5,r5,NVRVFY2@l
+        cmp     cr0,0,r4,r5            /* Compare 2 NVRAM Magic number */
+        bne     ..no_pllset
+        addi    r3,r3,8                 /* Skip over conf_size */
+        lwz     r4, 4(r3)               /* Load PLLMR1 value from NVRAM */
+        lwz     r3, 0(r3)               /* Load PLLMR0 value from NVRAM */
+        rlwinm  r5,r4,1,0x1             /* get system clock source (SSCS) */
+        cmpi     cr0,0,r5,1             /* See if PLL is locked */
+        beq     pll_write
+..no_pllset:
+#endif /* CONFIG_BUBINGA405EP */
+
+        addis   r3,0,PLLMR0_DEFAULT@h       /* PLLMR0 default value */
+        ori     r3,r3,PLLMR0_DEFAULT@l     /* */
+        addis   r4,0,PLLMR1_DEFAULT@h       /* PLLMR1 default value */
+        ori     r4,r4,PLLMR1_DEFAULT@l     /* */
+
+        b       pll_write                 /* Write the CPC0_PLLMR with new value */
+
+pll_done:
+        /*
+        !-----------------------------------------------------------------------
+        ! Clear Soft Reset Register
+        ! This is needed to enable PCI if not booting from serial EPROM
+        !-----------------------------------------------------------------------
+		*/
+        addi    r3, 0, 0x0
+        mtdcr   CPC0_SRR, r3
+
+        addis    r3,0,0x0010
+        mtctr   r3
+pci_wait:
+        bdnz    pci_wait
+
+	blr				  /* return to main code */
+
+/*
+!-----------------------------------------------------------------------------
+! Function:     pll_write
+! Description:  Updates the value of the CPC0_PLLMR according to CMOS27E documentation
+!               That is:
+!                         1.  Pll is first disabled (de-activated by putting in bypass mode)
+!                         2.  PLL is reset
+!                         3.  Clock dividers are set while PLL is held in reset and bypassed
+!                         4.  PLL Reset is cleared
+!                         5.  Wait 100us for PLL to lock
+!                         6.  A core reset is performed
+! Input: r3 = Value to write to CPC0_PLLMR0
+! Input: r4 = Value to write to CPC0_PLLMR1
+! Output r3 = none
+!-----------------------------------------------------------------------------
+*/
+pll_write:
+        mfdcr  r5, CPC0_UCR
+        andis. r5,r5,0xFFFF
+        ori    r5,r5,0x0101              /* Stop the UART clocks */
+        mtdcr  CPC0_UCR,r5               /* Before changing PLL */
+
+        mfdcr  r5, CPC0_PLLMR1
+        rlwinm r5,r5,0,0x7FFFFFFF        /* Disable PLL */
+        mtdcr   CPC0_PLLMR1,r5
+        oris   r5,r5,0x4000              /* Set PLL Reset */
+        mtdcr   CPC0_PLLMR1,r5
+
+        mtdcr   CPC0_PLLMR0,r3           /* Set clock dividers */
+        rlwinm r5,r4,0,0x3FFFFFFF        /* Reset & Bypass new PLL dividers */
+        oris   r5,r5,0x4000              /* Set PLL Reset */
+        mtdcr   CPC0_PLLMR1,r5           /* Set clock dividers */
+        rlwinm r5,r5,0,0xBFFFFFFF        /* Clear PLL Reset */
+        mtdcr   CPC0_PLLMR1,r5
+
+		/*
+        ! Wait min of 100us for PLL to lock.
+        ! See CMOS 27E databook for more info.
+        ! At 200MHz, that means waiting 20,000 instructions
+		 */
+        addi    r3,0,20000              /* 2000 = 0x4e20 */
+        mtctr   r3
+pll_wait:
+        bdnz    pll_wait
+
+        oris   r5,r5,0x8000             /* Enable PLL */
+        mtdcr   CPC0_PLLMR1,r5          /* Engage */
+
+        /*
+         * Reset CPU to guarantee timings are OK
+         * Not sure if this is needed...
+ 	 */
+        addis r3,0,0x1000
+        mtspr dbcr0,r3               /* This will cause a CPU core reset, and */
+                                     /* execution will continue from the poweron */
+                                     /* vector of 0xfffffffc */
+#endif /* CONFIG_405EP */