Merge git://www.denx.de/git/u-boot

Conflicts:

	CREDITS

25 files changed, 1382 insertions, 745 deletions

diff --git a/cpu/ppc4xx/405gp_pci.c b/cpu/ppc4xx/405gp_pci.c
index 7134355..282e7a1 100644
--- a/cpu/ppc4xx/405gp_pci.c
+++ b/cpu/ppc4xx/405gp_pci.c
@@ -77,11 +77,21 @@
 #include <asm/processor.h>
 #include <pci.h>
 
+#ifdef CONFIG_PCI
+
 DECLARE_GLOBAL_DATA_PTR;
 
-#if defined(CONFIG_405GP) || defined(CONFIG_405EP)
+/*
+ * Board-specific pci initialization
+ * Platform code can reimplement pci_pre_init() if needed
+ */
+int __pci_pre_init(struct pci_controller *hose)
+{
+	return 1;
+}
+int pci_pre_init(struct pci_controller *hose) __attribute__((weak, alias("__pci_pre_init")));
 
-#ifdef CONFIG_PCI
+#if defined(CONFIG_405GP) || defined(CONFIG_405EP)
 
 #if defined(CONFIG_PMC405)
 ushort pmc405_pci_subsys_deviceid(void);
@@ -191,6 +201,13 @@ void pci_405gp_init(struct pci_controller *hose)
 	if (hose->pci_fb)
 		pciauto_region_init(hose->pci_fb);
 
+	/* Let board change/modify hose & do initial checks */
+	if (pci_pre_init (hose) == 0) {
+		printf("PCI: Board-specific initialization failed.\n");
+		printf("PCI: Configuration aborted.\n");
+		return;
+	}
+
 	pci_register_hose(hose);
 
 	/*--------------------------------------------------------------------------+
@@ -380,7 +397,7 @@ void pci_405gp_setup_vga(struct pci_controller *hose, pci_dev_t dev,
 	pci_hose_write_config_dword(hose, dev, PCI_COMMAND, cmdstat);
 }
 
-#if !(defined(CONFIG_PIP405) || defined (CONFIG_MIP405)) && !(defined (CONFIG_SOLIDCARD3))
+#if !(defined(CONFIG_PIP405) || defined (CONFIG_MIP405)) && !(defined (CONFIG_SC3))
 
 /*
  *As is these functs get called out of flash Not a horrible
@@ -416,19 +433,17 @@ void pci_init_board(void)
 
 #endif
 
-#endif /* CONFIG_PCI */
-
 #endif /* CONFIG_405GP */
 
 /*-----------------------------------------------------------------------------+
  * CONFIG_440
  *-----------------------------------------------------------------------------*/
-#if defined(CONFIG_440) && defined(CONFIG_PCI)
+#if defined(CONFIG_440)
 
 static struct pci_controller ppc440_hose = {0};
 
 
-void pci_440_init (struct pci_controller *hose)
+int pci_440_init (struct pci_controller *hose)
 {
 	int reg_num = 0;
 
@@ -444,7 +459,7 @@ void pci_440_init (struct pci_controller *hose)
 	if ((strap & SDR0_SDSTP1_PISE_MASK) == 0) {
 		printf("PCI: SDR0_STRP1[PISE] not set.\n");
 		printf("PCI: Configuration aborted.\n");
-		return;
+		return -1;
 	}
 #elif defined(CONFIG_440GP)
 	unsigned long strap;
@@ -453,7 +468,7 @@ void pci_440_init (struct pci_controller *hose)
 	if ((strap & CPC0_STRP1_PISE_MASK) == 0) {
 		printf("PCI: CPC0_STRP1[PISE] not set.\n");
 		printf("PCI: Configuration aborted.\n");
-		return;
+		return -1;
 	}
 #endif
 #endif /* CONFIG_DISABLE_PISE_TEST */
@@ -462,7 +477,7 @@ void pci_440_init (struct pci_controller *hose)
 	 * PCI controller init
 	 *--------------------------------------------------------------------------*/
 	hose->first_busno = 0;
-	hose->last_busno = 0xff;
+	hose->last_busno = 0;
 
 	/* PCI I/O space */
 	pci_set_region(hose->regions + reg_num++,
@@ -496,14 +511,12 @@ void pci_440_init (struct pci_controller *hose)
 
 	pci_setup_indirect(hose, PCIX0_CFGADR, PCIX0_CFGDATA);
 
-#if defined(CFG_PCI_PRE_INIT)
 	/* Let board change/modify hose & do initial checks */
 	if (pci_pre_init (hose) == 0) {
 		printf("PCI: Board-specific initialization failed.\n");
 		printf("PCI: Configuration aborted.\n");
-		return;
+		return -1;
 	}
-#endif
 
 	pci_register_hose( hose );
 
@@ -565,14 +578,18 @@ void pci_440_init (struct pci_controller *hose)
 #endif
 		hose->last_busno = pci_hose_scan(hose);
 	}
+	return hose->last_busno;
 }
 
 void pci_init_board(void)
 {
-	pci_440_init (&ppc440_hose);
+	int busno;
+
+	busno = pci_440_init (&ppc440_hose);
 #if defined(CONFIG_440SPE)
-	pcie_setup_hoses();
+	pcie_setup_hoses(busno + 1);
 #endif
 }
 
-#endif /* CONFIG_440 & CONFIG_PCI */
+#endif /* CONFIG_440 */
+#endif /* CONFIG_PCI */
diff --git a/cpu/ppc4xx/440spe_pcie.c b/cpu/ppc4xx/440spe_pcie.c
index d6c4be5..158f1c5 100644
--- a/cpu/ppc4xx/440spe_pcie.c
+++ b/cpu/ppc4xx/440spe_pcie.c
@@ -1,5 +1,5 @@
 /*
- * (C) Copyright 2006
+ * (C) Copyright 2006 - 2007
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
  *
  * Copyright (c) 2005 Cisco Systems.  All rights reserved.
@@ -40,19 +40,81 @@ enum {
 	LNKW_X8			= 0x8
 };
 
+static u8* pcie_get_base(struct pci_controller *hose, unsigned int devfn)
+{
+	u8 *base = (u8*)hose->cfg_data;
+
+	/* use local configuration space for the first bus */
+	if (PCI_BUS(devfn) == 0) {
+		if (hose->cfg_data == (u8*)CFG_PCIE0_CFGBASE)
+			base = (u8*)CFG_PCIE0_XCFGBASE;
+		if (hose->cfg_data == (u8*)CFG_PCIE1_CFGBASE)
+			base = (u8*)CFG_PCIE1_XCFGBASE;
+		if (hose->cfg_data == (u8*)CFG_PCIE2_CFGBASE)
+			base = (u8*)CFG_PCIE2_XCFGBASE;
+	}
+
+	return base;
+}
+
+static void pcie_dmer_disable(void)
+{
+	mtdcr (DCRN_PEGPL_CFG(DCRN_PCIE0_BASE),
+		mfdcr (DCRN_PEGPL_CFG(DCRN_PCIE0_BASE)) | GPL_DMER_MASK_DISA);
+	mtdcr (DCRN_PEGPL_CFG(DCRN_PCIE1_BASE),
+		mfdcr (DCRN_PEGPL_CFG(DCRN_PCIE1_BASE)) | GPL_DMER_MASK_DISA);
+	mtdcr (DCRN_PEGPL_CFG(DCRN_PCIE2_BASE),
+		mfdcr (DCRN_PEGPL_CFG(DCRN_PCIE2_BASE)) | GPL_DMER_MASK_DISA);
+}
+
+static void pcie_dmer_enable(void)
+{
+	mtdcr (DCRN_PEGPL_CFG (DCRN_PCIE0_BASE),
+		mfdcr (DCRN_PEGPL_CFG(DCRN_PCIE0_BASE)) & ~GPL_DMER_MASK_DISA);
+	mtdcr (DCRN_PEGPL_CFG (DCRN_PCIE1_BASE),
+		mfdcr (DCRN_PEGPL_CFG(DCRN_PCIE1_BASE)) & ~GPL_DMER_MASK_DISA);
+	mtdcr (DCRN_PEGPL_CFG (DCRN_PCIE2_BASE),
+		mfdcr (DCRN_PEGPL_CFG(DCRN_PCIE2_BASE)) & ~GPL_DMER_MASK_DISA);
+}
+
 static int pcie_read_config(struct pci_controller *hose, unsigned int devfn,
 	int offset, int len, u32 *val) {
 
+	u8 *address;
 	*val = 0;
+
 	/*
-	 * 440SPE implements only one function per port
+	 * Bus numbers are relative to hose->first_busno
 	 */
-	if (!((PCI_FUNC(devfn) == 0) && (PCI_DEV(devfn) == 1)))
+	devfn -= PCI_BDF(hose->first_busno, 0, 0);
+
+	/*
+	 * NOTICE: configuration space ranges are currenlty mapped only for
+	 * the first 16 buses, so such limit must be imposed. In case more
+	 * buses are required the TLB settings in board/amcc/<board>/init.S
+	 * need to be altered accordingly (one bus takes 1 MB of memory space).
+	 */
+	if (PCI_BUS(devfn) >= 16)
 		return 0;
 
-	devfn = PCI_BDF(0,0,0);
+	/*
+	 * Only single device/single function is supported for the primary and
+	 * secondary buses of the 440SPe host bridge.
+	 */
+	if ((!((PCI_FUNC(devfn) == 0) && (PCI_DEV(devfn) == 0))) &&
+		((PCI_BUS(devfn) == 0) || (PCI_BUS(devfn) == 1)))
+		return 0;
+		
+	address = pcie_get_base(hose, devfn);
 	offset += devfn << 4;
 
+	/*
+	 * Reading from configuration space of non-existing device can
+	 * generate transaction errors. For the read duration we suppress
+	 * assertion of machine check exceptions to avoid those.
+	 */
+	pcie_dmer_disable ();
+
 	switch (len) {
 	case 1:
 		*val = in_8(hose->cfg_data + offset);
@@ -61,24 +123,43 @@ static int pcie_read_config(struct pci_controller *hose, unsigned int devfn,
 		*val = in_le16((u16 *)(hose->cfg_data + offset));
 		break;
 	default:
-		*val = in_le32((u32 *)(hose->cfg_data + offset));
+		*val = in_le32((u32*)(hose->cfg_data + offset));
 		break;
 	}
+
+	pcie_dmer_enable ();
+
 	return 0;
 }
 
 static int pcie_write_config(struct pci_controller *hose, unsigned int devfn,
 	int offset, int len, u32 val) {
 
+	u8 *address;
+	
+	/*
+	 * Bus numbers are relative to hose->first_busno
+	 */
+	devfn -= PCI_BDF(hose->first_busno, 0, 0);
+	
 	/*
-	 * 440SPE implements only one function per port
+	 * Same constraints as in pcie_read_config().
 	 */
-	if (!((PCI_FUNC(devfn) == 0) && (PCI_DEV(devfn) == 1)))
+	if (PCI_BUS(devfn) >= 16)
 		return 0;
 
-	devfn = PCI_BDF(0,0,0);
+	if ((!((PCI_FUNC(devfn) == 0) && (PCI_DEV(devfn) == 0))) &&
+		((PCI_BUS(devfn) == 0) || (PCI_BUS(devfn) == 1)))
+		return 0;
+	
+	address = pcie_get_base(hose, devfn);
 	offset += devfn << 4;
 
+	/*
+	 * Suppress MCK exceptions, similar to pcie_read_config()
+	 */
+	pcie_dmer_disable ();
+
 	switch (len) {
 	case 1:
 		out_8(hose->cfg_data + offset, val);
@@ -90,6 +171,9 @@ static int pcie_write_config(struct pci_controller *hose, unsigned int devfn,
 		out_le32((u32 *)(hose->cfg_data + offset), val);
 		break;
 	}
+
+	pcie_dmer_enable ();
+
 	return 0;
 }
 
@@ -98,7 +182,7 @@ int pcie_read_config_byte(struct pci_controller *hose,pci_dev_t dev,int offset,u
 	u32 v;
 	int rv;
 
-	rv =  pcie_read_config(hose, dev, offset, 1, &v);
+	rv = pcie_read_config(hose, dev, offset, 1, &v);
 	*val = (u8)v;
 	return rv;
 }
@@ -755,12 +839,12 @@ void ppc440spe_setup_pcie_rootpoint(struct pci_controller *hose, int port)
 	volatile void *rmbase = NULL;
 
 	pci_set_ops(hose,
-		    pcie_read_config_byte,
-		    pcie_read_config_word,
-		    pcie_read_config_dword,
-		    pcie_write_config_byte,
-		    pcie_write_config_word,
-		    pcie_write_config_dword);
+		pcie_read_config_byte,
+		pcie_read_config_word,
+		pcie_read_config_dword,
+		pcie_write_config_byte,
+		pcie_write_config_word,
+		pcie_write_config_dword);
 
 	switch (port) {
 	case 0:
@@ -842,6 +926,29 @@ void ppc440spe_setup_pcie_rootpoint(struct pci_controller *hose, int port)
 		 in_le16((u16 *)(mbase + PCI_COMMAND)) |
 		 PCI_COMMAND_IO | PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER);
 	printf("PCIE:%d successfully set as rootpoint\n",port);
+	
+	/* Set Device and Vendor Id */
+	switch (port) {
+	case 0:
+		out_le16(mbase + 0x200, 0xaaa0);
+		out_le16(mbase + 0x202, 0xbed0);
+		break;
+	case 1:
+		out_le16(mbase + 0x200, 0xaaa1);
+		out_le16(mbase + 0x202, 0xbed1);
+		break;
+	case 2:
+		out_le16(mbase + 0x200, 0xaaa2);
+		out_le16(mbase + 0x202, 0xbed2);
+		break;
+	default:
+		out_le16(mbase + 0x200, 0xaaa3);
+		out_le16(mbase + 0x202, 0xbed3);
+	}
+
+	/* Set Class Code to PCI-PCI bridge and Revision Id to 1 */
+	out_le32(mbase + 0x208, 0x06040001);
+
 }
 
 int ppc440spe_setup_pcie_endpoint(struct pci_controller *hose, int port)
@@ -919,8 +1026,8 @@ int ppc440spe_setup_pcie_endpoint(struct pci_controller *hose, int port)
 
 	/* Enable I/O, Mem, and Busmaster cycles */
 	out_le16((u16 *)(mbase + PCI_COMMAND),
-		 in_le16((u16 *)(mbase + PCI_COMMAND)) |
-		 PCI_COMMAND_IO | PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER);
+		in_le16((u16 *)(mbase + PCI_COMMAND)) |
+		PCI_COMMAND_IO | PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER);
 	out_le16(mbase + 0x200,0xcaad);			/* Setting vendor ID */
 	out_le16(mbase + 0x202,0xfeed);			/* Setting device ID */
 	attempts = 10;
diff --git a/cpu/ppc4xx/440spe_pcie.h b/cpu/ppc4xx/440spe_pcie.h
index 2becc77..38745eb 100644
--- a/cpu/ppc4xx/440spe_pcie.h
+++ b/cpu/ppc4xx/440spe_pcie.h
@@ -38,6 +38,7 @@
 #define DCRN_PEGPL_REGBAL(base)		(base + 0x13)
 #define DCRN_PEGPL_REGMSK(base)		(base + 0x14)
 #define DCRN_PEGPL_SPECIAL(base)	(base + 0x15)
+#define DCRN_PEGPL_CFG(base)		(base + 0x16)
 
 /*
  * System DCRs (SDRs)
@@ -145,8 +146,8 @@
 #define PECFG_PIMEN		0x33c
 #define PECFG_PIM0LAL		0x340
 #define PECFG_PIM0LAH		0x344
-#define PECFG_PIM1LAL     	0x348
-#define PECFG_PIM1LAH     	0x34c
+#define PECFG_PIM1LAL		0x348
+#define PECFG_PIM1LAH		0x34c
 #define PECFG_PIM01SAL		0x350
 #define PECFG_PIM01SAH		0x354
 
@@ -161,6 +162,8 @@
 	mtdcr(DCRN_SDR0_CFGADDR, offset); \
 	mtdcr(DCRN_SDR0_CFGDATA,data);})
 
+#define GPL_DMER_MASK_DISA	0x02000000
+
 int ppc440spe_init_pcie(void);
 int ppc440spe_init_pcie_rootport(int port);
 void yucca_setup_pcie_fpga_rootpoint(int port);
diff --git a/cpu/ppc4xx/44x_spd_ddr.c b/cpu/ppc4xx/44x_spd_ddr.c
index 10b4c18..4a4c6f2 100644
--- a/cpu/ppc4xx/44x_spd_ddr.c
+++ b/cpu/ppc4xx/44x_spd_ddr.c
@@ -20,7 +20,7 @@
  * Jun Gu, Artesyn Technology, jung@artesyncp.com
  * Support for AMCC 440 based on OpenBIOS draminit.c from IBM.
  *
- * (C) Copyright 2005
+ * (C) Copyright 2005-2007
  * Stefan Roese, DENX Software Engineering, sr@denx.de.
  *
  * See file CREDITS for list of people who contributed to this
@@ -42,6 +42,11 @@
  * MA 02111-1307 USA
  */
 
+/* define DEBUG for debugging output (obviously ;-)) */
+#if 0
+#define DEBUG
+#endif
+
 #include <common.h>
 #include <asm/processor.h>
 #include <i2c.h>
@@ -65,6 +70,15 @@
 
 #define ONE_BILLION	1000000000
 
+/*
+ * Board-specific Platform code can reimplement spd_ddr_init_hang () if needed
+ */
+void __spd_ddr_init_hang (void)
+{
+	hang ();
+}
+void spd_ddr_init_hang (void) __attribute__((weak, alias("__spd_ddr_init_hang")));
+
 /*-----------------------------------------------------------------------------
   |  Memory Controller Options 0
   +-----------------------------------------------------------------------------*/
@@ -246,25 +260,6 @@
 #define MY_TLB_WORD2_I_ENABLE	TLB_WORD2_I_ENABLE	/* disable caching on SDRAM */
 #endif
 
-const unsigned long test[NUMMEMTESTS][NUMMEMWORDS] = {
-	{0x00000000, 0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000,
-	 0xFFFFFFFF, 0xFFFFFFFF},
-	{0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000, 0xFFFFFFFF, 0xFFFFFFFF,
-	 0x00000000, 0x00000000},
-	{0xAAAAAAAA, 0xAAAAAAAA, 0x55555555, 0x55555555, 0xAAAAAAAA, 0xAAAAAAAA,
-	 0x55555555, 0x55555555},
-	{0x55555555, 0x55555555, 0xAAAAAAAA, 0xAAAAAAAA, 0x55555555, 0x55555555,
-	 0xAAAAAAAA, 0xAAAAAAAA},
-	{0xA5A5A5A5, 0xA5A5A5A5, 0x5A5A5A5A, 0x5A5A5A5A, 0xA5A5A5A5, 0xA5A5A5A5,
-	 0x5A5A5A5A, 0x5A5A5A5A},
-	{0x5A5A5A5A, 0x5A5A5A5A, 0xA5A5A5A5, 0xA5A5A5A5, 0x5A5A5A5A, 0x5A5A5A5A,
-	 0xA5A5A5A5, 0xA5A5A5A5},
-	{0xAA55AA55, 0xAA55AA55, 0x55AA55AA, 0x55AA55AA, 0xAA55AA55, 0xAA55AA55,
-	 0x55AA55AA, 0x55AA55AA},
-	{0x55AA55AA, 0x55AA55AA, 0xAA55AA55, 0xAA55AA55, 0x55AA55AA, 0x55AA55AA,
-	 0xAA55AA55, 0xAA55AA55}
-};
-
 /* bank_parms is used to sort the bank sizes by descending order */
 struct bank_param {
 	unsigned long cr;
@@ -274,50 +269,40 @@ struct bank_param {
 typedef struct bank_param BANKPARMS;
 
 #ifdef CFG_SIMULATE_SPD_EEPROM
-extern unsigned char cfg_simulate_spd_eeprom[128];
+extern const unsigned char cfg_simulate_spd_eeprom[128];
 #endif
-void program_tlb(u32 start, u32 size, u32 tlb_word2_i_value);
 
-unsigned char spd_read(uchar chip, uint addr);
-
-void get_spd_info(unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks);
-
-void check_mem_type
-(unsigned long* dimm_populated,
- unsigned char* iic0_dimm_addr,
- unsigned long	num_dimm_banks);
-
-void check_volt_type
-(unsigned long* dimm_populated,
- unsigned char* iic0_dimm_addr,
- unsigned long	num_dimm_banks);
-
-void program_cfg0(unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks);
-
-void program_cfg1(unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks);
-
-void program_rtr (unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks);
-
-void program_tr0 (unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks);
-
-void program_tr1 (void);
-
-void program_ecc (unsigned long	 num_bytes);
+static unsigned char spd_read(uchar chip, uint addr);
+static void get_spd_info(unsigned long *dimm_populated,
+			 unsigned char *iic0_dimm_addr,
+			 unsigned long num_dimm_banks);
+static void check_mem_type(unsigned long *dimm_populated,
+			   unsigned char *iic0_dimm_addr,
+			   unsigned long num_dimm_banks);
+static void check_volt_type(unsigned long *dimm_populated,
+			    unsigned char *iic0_dimm_addr,
+			    unsigned long num_dimm_banks);
+static void program_cfg0(unsigned long *dimm_populated,
+			 unsigned char *iic0_dimm_addr,
+			 unsigned long  num_dimm_banks);
+static void program_cfg1(unsigned long *dimm_populated,
+			 unsigned char *iic0_dimm_addr,
+			 unsigned long num_dimm_banks);
+static void program_rtr(unsigned long *dimm_populated,
+			unsigned char *iic0_dimm_addr,
+			unsigned long num_dimm_banks);
+static void program_tr0(unsigned long *dimm_populated,
+			unsigned char *iic0_dimm_addr,
+			unsigned long num_dimm_banks);
+static void program_tr1(void);
+
+#ifdef CONFIG_DDR_ECC
+static void program_ecc(unsigned long num_bytes);
+#endif
 
-unsigned
-long  program_bxcr(unsigned long* dimm_populated,
-		   unsigned char* iic0_dimm_addr,
-		   unsigned long  num_dimm_banks);
+static unsigned long program_bxcr(unsigned long *dimm_populated,
+				  unsigned char *iic0_dimm_addr,
+				  unsigned long num_dimm_banks);
 
 /*
  * This function is reading data from the DIMM module EEPROM over the SPD bus
@@ -328,7 +313,6 @@ long  program_bxcr(unsigned long* dimm_populated,
  * BUG: Don't handle ECC memory
  * BUG: A few values in the TR register is currently hardcoded
  */
-
 long int spd_sdram(void) {
 	unsigned char iic0_dimm_addr[] = SPD_EEPROM_ADDRESS;
 	unsigned long dimm_populated[sizeof(iic0_dimm_addr)];
@@ -397,7 +381,7 @@ long int spd_sdram(void) {
 
 #ifdef CONFIG_PROG_SDRAM_TLB /* this define should eventually be removed */
 	/* and program tlb entries for this size (dynamic) */
-	program_tlb(0, total_size, MY_TLB_WORD2_I_ENABLE);
+	program_tlb(0, 0, total_size, MY_TLB_WORD2_I_ENABLE);
 #endif
 
 	/*
@@ -421,9 +405,8 @@ long int spd_sdram(void) {
 	 */
 	while (1) {
 		mfsdram(mem_mcsts, mcsts);
-		if ((mcsts & SDRAM_MCSTS_MRSC) != 0) {
+		if ((mcsts & SDRAM_MCSTS_MRSC) != 0)
 			break;
-		}
 	}
 
 	/*
@@ -431,14 +414,17 @@ long int spd_sdram(void) {
 	 */
 	program_tr1();
 
+#ifdef CONFIG_DDR_ECC
 	/*
-	 * if ECC is enabled, initialize parity bits
+	 * If ecc is enabled, initialize the parity bits.
 	 */
+	program_ecc(total_size);
+#endif
 
 	return total_size;
 }
 
-unsigned char spd_read(uchar chip, uint addr)
+static unsigned char spd_read(uchar chip, uint addr)
 {
 	unsigned char data[2];
 
@@ -460,9 +446,9 @@ unsigned char spd_read(uchar chip, uint addr)
 	return 0;
 }
 
-void get_spd_info(unsigned long*   dimm_populated,
-		  unsigned char*   iic0_dimm_addr,
-		  unsigned long	   num_dimm_banks)
+static void get_spd_info(unsigned long *dimm_populated,
+			 unsigned char *iic0_dimm_addr,
+			 unsigned long num_dimm_banks)
 {
 	unsigned long dimm_num;
 	unsigned long dimm_found;
@@ -480,26 +466,22 @@ void get_spd_info(unsigned long*   dimm_populated,
 		if ((num_of_bytes != 0) && (total_size != 0)) {
 			dimm_populated[dimm_num] = TRUE;
 			dimm_found = TRUE;
-#if 0
-			printf("DIMM slot %lu: populated\n", dimm_num);
-#endif
+			debug("DIMM slot %lu: populated\n", dimm_num);
 		} else {
 			dimm_populated[dimm_num] = FALSE;
-#if 0
-			printf("DIMM slot %lu: Not populated\n", dimm_num);
-#endif
+			debug("DIMM slot %lu: Not populated\n", dimm_num);
 		}
 	}
 
 	if (dimm_found == FALSE) {
 		printf("ERROR - No memory installed. Install a DDR-SDRAM DIMM.\n\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 }
 
-void check_mem_type(unsigned long*   dimm_populated,
-		    unsigned char*   iic0_dimm_addr,
-		    unsigned long    num_dimm_banks)
+static void check_mem_type(unsigned long *dimm_populated,
+			   unsigned char *iic0_dimm_addr,
+			   unsigned long num_dimm_banks)
 {
 	unsigned long dimm_num;
 	unsigned char dimm_type;
@@ -509,26 +491,23 @@ void check_mem_type(unsigned long*   dimm_populated,
 			dimm_type = spd_read(iic0_dimm_addr[dimm_num], 2);
 			switch (dimm_type) {
 			case 7:
-#if 0
-				printf("DIMM slot %lu: DDR SDRAM detected\n", dimm_num);
-#endif
+				debug("DIMM slot %lu: DDR SDRAM detected\n", dimm_num);
 				break;
 			default:
 				printf("ERROR: Unsupported DIMM detected in slot %lu.\n",
 				       dimm_num);
 				printf("Only DDR SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 		}
 	}
 }
 
-
-void check_volt_type(unsigned long*   dimm_populated,
-		     unsigned char*   iic0_dimm_addr,
-		     unsigned long    num_dimm_banks)
+static void check_volt_type(unsigned long *dimm_populated,
+			    unsigned char *iic0_dimm_addr,
+			    unsigned long num_dimm_banks)
 {
 	unsigned long dimm_num;
 	unsigned long voltage_type;
@@ -539,20 +518,18 @@ void check_volt_type(unsigned long*   dimm_populated,
 			if (voltage_type != 0x04) {
 				printf("ERROR: DIMM %lu with unsupported voltage level.\n",
 				       dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 			} else {
-#if 0
-				printf("DIMM %lu voltage level supported.\n", dimm_num);
-#endif
+				debug("DIMM %lu voltage level supported.\n", dimm_num);
 			}
 			break;
 		}
 	}
 }
 
-void program_cfg0(unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks)
+static void program_cfg0(unsigned long *dimm_populated,
+			 unsigned char *iic0_dimm_addr,
+			 unsigned long num_dimm_banks)
 {
 	unsigned long dimm_num;
 	unsigned long cfg0;
@@ -612,7 +589,7 @@ void program_cfg0(unsigned long* dimm_populated,
 				printf("WARNING: DIMM with datawidth of %lu bits.\n",
 				       data_width);
 				printf("Only DIMMs with 32 or 64 bit datawidths supported.\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 			break;
 		}
@@ -640,9 +617,9 @@ void program_cfg0(unsigned long* dimm_populated,
 	mtsdram(mem_cfg0, cfg0);
 }
 
-void program_cfg1(unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks)
+static void program_cfg1(unsigned long *dimm_populated,
+			 unsigned char *iic0_dimm_addr,
+			 unsigned long num_dimm_banks)
 {
 	unsigned long cfg1;
 	mfsdram(mem_cfg1, cfg1);
@@ -658,9 +635,9 @@ void program_cfg1(unsigned long* dimm_populated,
 	mtsdram(mem_cfg1, cfg1);
 }
 
-void program_rtr (unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks)
+static void program_rtr(unsigned long *dimm_populated,
+			unsigned char *iic0_dimm_addr,
+			unsigned long num_dimm_banks)
 {
 	unsigned long dimm_num;
 	unsigned long bus_period_x_10;
@@ -676,7 +653,6 @@ void program_rtr (unsigned long* dimm_populated,
 	get_sys_info(&sys_info);
 	bus_period_x_10 = ONE_BILLION / (sys_info.freqPLB / 10);
 
-
 	for (dimm_num = 0;  dimm_num < num_dimm_banks; dimm_num++) {
 		if (dimm_populated[dimm_num] == TRUE) {
 			refresh_rate_type = 0x7F & spd_read(iic0_dimm_addr[dimm_num], 12);
@@ -719,9 +695,9 @@ void program_rtr (unsigned long* dimm_populated,
 	mtsdram(mem_rtr, sdram_rtr);
 }
 
-void program_tr0 (unsigned long* dimm_populated,
-		  unsigned char* iic0_dimm_addr,
-		  unsigned long	 num_dimm_banks)
+static void program_tr0(unsigned long *dimm_populated,
+			 unsigned char *iic0_dimm_addr,
+			 unsigned long num_dimm_banks)
 {
 	unsigned long dimm_num;
 	unsigned long tr0;
@@ -801,7 +777,7 @@ void program_tr0 (unsigned long* dimm_populated,
 				if ((tcyc_reg & 0x0F) >= 10) {
 					printf("ERROR: Tcyc incorrect for DIMM in slot %lu\n",
 					       dimm_num);
-					hang();
+					spd_ddr_init_hang ();
 				}
 
 				cycle_time_ns_x_10[cas_index] =
@@ -881,7 +857,7 @@ void program_tr0 (unsigned long* dimm_populated,
 		printf("ERROR: No supported CAS latency with the installed DIMMs.\n");
 		printf("Only CAS latencies of 2.0, 2.5, and 3.0 are supported.\n");
 		printf("Make sure the PLB speed is within the supported range.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	/*
@@ -1001,13 +977,74 @@ void program_tr0 (unsigned long* dimm_populated,
 		break;
 	}
 
-#if 0
-	printf("tr0: %x\n", tr0);
-#endif
+	debug("tr0: %x\n", tr0);
 	mtsdram(mem_tr0, tr0);
 }
 
-void program_tr1 (void)
+static int short_mem_test(void)
+{
+	unsigned long i, j;
+	unsigned long bxcr_num;
+	unsigned long *membase;
+	const unsigned long test[NUMMEMTESTS][NUMMEMWORDS] = {
+		{0x00000000, 0x00000000, 0xFFFFFFFF, 0xFFFFFFFF,
+		 0x00000000, 0x00000000, 0xFFFFFFFF, 0xFFFFFFFF},
+		{0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000,
+		 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000},
+		{0xAAAAAAAA, 0xAAAAAAAA, 0x55555555, 0x55555555,
+		 0xAAAAAAAA, 0xAAAAAAAA, 0x55555555, 0x55555555},
+		{0x55555555, 0x55555555, 0xAAAAAAAA, 0xAAAAAAAA,
+		 0x55555555, 0x55555555, 0xAAAAAAAA, 0xAAAAAAAA},
+		{0xA5A5A5A5, 0xA5A5A5A5, 0x5A5A5A5A, 0x5A5A5A5A,
+		 0xA5A5A5A5, 0xA5A5A5A5, 0x5A5A5A5A, 0x5A5A5A5A},
+		{0x5A5A5A5A, 0x5A5A5A5A, 0xA5A5A5A5, 0xA5A5A5A5,
+		 0x5A5A5A5A, 0x5A5A5A5A, 0xA5A5A5A5, 0xA5A5A5A5},
+		{0xAA55AA55, 0xAA55AA55, 0x55AA55AA, 0x55AA55AA,
+		 0xAA55AA55, 0xAA55AA55, 0x55AA55AA, 0x55AA55AA},
+		{0x55AA55AA, 0x55AA55AA, 0xAA55AA55, 0xAA55AA55,
+		 0x55AA55AA, 0x55AA55AA, 0xAA55AA55, 0xAA55AA55}};
+
+	for (bxcr_num = 0; bxcr_num < MAXBXCR; bxcr_num++) {
+		mtdcr(memcfga, mem_b0cr + (bxcr_num << 2));
+		if ((mfdcr(memcfgd) & SDRAM_BXCR_SDBE) == SDRAM_BXCR_SDBE) {
+			/* Bank is enabled */
+			membase = (unsigned long*)
+				(mfdcr(memcfgd) & SDRAM_BXCR_SDBA_MASK);
+
+			/*
+			 * Run the short memory test
+			 */
+			for (i = 0; i < NUMMEMTESTS; i++) {
+				for (j = 0; j < NUMMEMWORDS; j++) {
+					/* printf("bank enabled base:%x\n", &membase[j]); */
+					membase[j] = test[i][j];
+					ppcDcbf((unsigned long)&(membase[j]));
+				}
+
+				for (j = 0; j < NUMMEMWORDS; j++) {
+					if (membase[j] != test[i][j]) {
+						ppcDcbf((unsigned long)&(membase[j]));
+						return 0;
+					}
+					ppcDcbf((unsigned long)&(membase[j]));
+				}
+
+				if (j < NUMMEMWORDS)
+					return 0;
+			}
+
+			/*
+			 * see if the rdclt value passed
+			 */
+			if (i < NUMMEMTESTS)
+				return 0;
+		}
+	}
+
+	return 1;
+}
+
+static void program_tr1(void)
 {
 	unsigned long tr0;
 	unsigned long tr1;
@@ -1015,8 +1052,7 @@ void program_tr1 (void)
 	unsigned long ecc_temp;
 	unsigned long dlycal;
 	unsigned long dly_val;
-	unsigned long i, j, k;
-	unsigned long bxcr_num;
+	unsigned long k;
 	unsigned long max_pass_length;
 	unsigned long current_pass_length;
 	unsigned long current_fail_length;
@@ -1029,7 +1065,6 @@ void program_tr1 (void)
 	unsigned char window_found;
 	unsigned char fail_found;
 	unsigned char pass_found;
-	unsigned long * membase;
 	PPC440_SYS_INFO sys_info;
 
 	/*
@@ -1079,55 +1114,16 @@ void program_tr1 (void)
 	window_found = FALSE;
 	fail_found = FALSE;
 	pass_found = FALSE;
-#ifdef DEBUG
-	printf("Starting memory test ");
-#endif
+	debug("Starting memory test ");
+
 	for (k = 0; k < NUMHALFCYCLES; k++) {
-		for (rdclt = 0; rdclt < dly_val; rdclt++)  {
+		for (rdclt = 0; rdclt < dly_val; rdclt++) {
 			/*
 			 * Set the timing reg for the test.
 			 */
 			mtsdram(mem_tr1, (tr1 | SDRAM_TR1_RDCT_ENCODE(rdclt)));
 
-			for (bxcr_num = 0; bxcr_num < MAXBXCR; bxcr_num++) {
-				mtdcr(memcfga, mem_b0cr + (bxcr_num<<2));
-				if ((mfdcr(memcfgd) & SDRAM_BXCR_SDBE) == SDRAM_BXCR_SDBE) {
-					/* Bank is enabled */
-					membase = (unsigned long*)
-						(mfdcr(memcfgd) & SDRAM_BXCR_SDBA_MASK);
-
-					/*
-					 * Run the short memory test
-					 */
-					for (i = 0; i < NUMMEMTESTS; i++) {
-						for (j = 0; j < NUMMEMWORDS; j++) {
-							membase[j] = test[i][j];
-							ppcDcbf((unsigned long)&(membase[j]));
-						}
-
-						for (j = 0; j < NUMMEMWORDS; j++) {
-							if (membase[j] != test[i][j]) {
-								ppcDcbf((unsigned long)&(membase[j]));
-								break;
-							}
-							ppcDcbf((unsigned long)&(membase[j]));
-						}
-
-						if (j < NUMMEMWORDS) {
-							break;
-						}
-					}
-
-					/*
-					 * see if the rdclt value passed
-					 */
-					if (i < NUMMEMTESTS) {
-						break;
-					}
-				}
-			}
-
-			if (bxcr_num == MAXBXCR) {
+			if (short_mem_test()) {
 				if (fail_found == TRUE) {
 					pass_found = TRUE;
 					if (current_pass_length == 0) {
@@ -1157,9 +1153,8 @@ void program_tr1 (void)
 				}
 			}
 		}
-#ifdef DEBUG
-		printf(".");
-#endif
+		debug(".");
+
 		if (window_found == TRUE) {
 			break;
 		}
@@ -1167,16 +1162,14 @@ void program_tr1 (void)
 		tr1 = tr1 ^ SDRAM_TR1_RDCD_MASK;
 		rdclt_offset += dly_val;
 	}
-#ifdef DEBUG
-	printf("\n");
-#endif
+	debug("\n");
 
 	/*
 	 * make sure we find the window
 	 */
 	if (window_found == FALSE) {
 		printf("ERROR: Cannot determine a common read delay.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	/*
@@ -1218,18 +1211,17 @@ void program_tr1 (void)
 	}
 	tr1 |= SDRAM_TR1_RDCT_ENCODE(rdclt_average);
 
-#if 0
-	printf("tr1: %x\n", tr1);
-#endif
+	debug("tr1: %x\n", tr1);
+
 	/*
 	 * program SDRAM Timing Register 1 TR1
 	 */
 	mtsdram(mem_tr1, tr1);
 }
 
-unsigned long program_bxcr(unsigned long* dimm_populated,
-			   unsigned char* iic0_dimm_addr,
-			   unsigned long  num_dimm_banks)
+static unsigned long program_bxcr(unsigned long *dimm_populated,
+				  unsigned char *iic0_dimm_addr,
+				  unsigned long num_dimm_banks)
 {
 	unsigned long dimm_num;
 	unsigned long bank_base_addr;
@@ -1262,8 +1254,8 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 #ifdef CONFIG_BAMBOO
 	/*
 	 * This next section is hardware dependent and must be programmed
-	 * to match the hardware.  For bammboo, the following holds...
-	 * 1. SDRAM0_B0CR: Bank 0 of dimm 0 ctrl_bank_num : 0
+	 * to match the hardware.  For bamboo, the following holds...
+	 * 1. SDRAM0_B0CR: Bank 0 of dimm 0 ctrl_bank_num : 0 (soldered onboard)
 	 * 2. SDRAM0_B1CR: Bank 0 of dimm 1 ctrl_bank_num : 1
 	 * 3. SDRAM0_B2CR: Bank 1 of dimm 1 ctrl_bank_num : 1
 	 * 4. SDRAM0_B3CR: Bank 0 of dimm 2 ctrl_bank_num : 3
@@ -1273,10 +1265,12 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 	ctrl_bank_num[1] = 1;
 	ctrl_bank_num[2] = 3;
 #else
+	/*
+	 * Ocotea, Ebony and the other IBM/AMCC eval boards have
+	 * 2 DIMM slots with each max 2 banks
+	 */
 	ctrl_bank_num[0] = 0;
-	ctrl_bank_num[1] = 1;
-	ctrl_bank_num[2] = 2;
-	ctrl_bank_num[3] = 3;
+	ctrl_bank_num[1] = 2;
 #endif
 
 	/*
@@ -1290,6 +1284,8 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 			num_col_addr = spd_read(iic0_dimm_addr[dimm_num], 4);
 			num_banks    = spd_read(iic0_dimm_addr[dimm_num], 5);
 			bank_size_id = spd_read(iic0_dimm_addr[dimm_num], 31);
+			debug("DIMM%d: row=%d col=%d banks=%d\n", dimm_num,
+			      num_row_addr, num_col_addr, num_banks);
 
 			/*
 			 * Set the SDRAM0_BxCR regs
@@ -1323,7 +1319,7 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 				printf("ERROR: Unsupported value for the banksize: %d.\n",
 				       bank_size_id);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 
 			switch (num_col_addr) {
@@ -1345,7 +1341,7 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 				printf("ERROR: Unsupported value for number of "
 				       "column addresses: %d.\n", num_col_addr);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 
 			/*
@@ -1353,11 +1349,14 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 			 */
 			cr |= SDRAM_BXCR_SDBE;
 
- 			for (i = 0; i < num_banks; i++) {
-				bank_parms[ctrl_bank_num[dimm_num]+i+dimm_num].bank_size_bytes =
-					(4 * 1024 * 1024) * bank_size_id;
-				bank_parms[ctrl_bank_num[dimm_num]+i+dimm_num].cr = cr;
- 			}
+			for (i = 0; i < num_banks; i++) {
+				bank_parms[ctrl_bank_num[dimm_num]+i].bank_size_bytes =
+					(4 << 20) * bank_size_id;
+				bank_parms[ctrl_bank_num[dimm_num]+i].cr = cr;
+				debug("DIMM%d-bank %d (SDRAM0_B%dCR): bank_size_bytes=%d\n",
+				      dimm_num, i, ctrl_bank_num[dimm_num]+i,
+				      bank_parms[ctrl_bank_num[dimm_num]+i].bank_size_bytes);
+			}
 		}
 	}
 
@@ -1400,13 +1399,15 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 				bank_parms[sorted_bank_num[bx_cr_num]].cr;
 			mtdcr(memcfgd, temp);
 			bank_base_addr += bank_parms[sorted_bank_num[bx_cr_num]].bank_size_bytes;
+			debug("SDRAM0_B%dCR=0x%08lx\n", sorted_bank_num[bx_cr_num], temp);
 		}
 	}
 
 	return(bank_base_addr);
 }
 
-void program_ecc (unsigned long	 num_bytes)
+#ifdef CONFIG_DDR_ECC
+static void program_ecc(unsigned long num_bytes)
 {
 	unsigned long bank_base_addr;
 	unsigned long current_address;
@@ -1425,14 +1426,12 @@ void program_ecc (unsigned long	 num_bytes)
 	bank_base_addr = CFG_SDRAM_BASE;
 
 	if ((cfg0 & SDRAM_CFG0_MCHK_MASK) != SDRAM_CFG0_MCHK_NON) {
-		mtsdram(mem_cfg0, (cfg0 & ~SDRAM_CFG0_MCHK_MASK) |
-			SDRAM_CFG0_MCHK_GEN);
+		mtsdram(mem_cfg0, (cfg0 & ~SDRAM_CFG0_MCHK_MASK) | SDRAM_CFG0_MCHK_GEN);
 
-		if ((cfg0 & SDRAM_CFG0_DMWD_MASK) == SDRAM_CFG0_DMWD_32) {
+		if ((cfg0 & SDRAM_CFG0_DMWD_MASK) == SDRAM_CFG0_DMWD_32)
 			address_increment = 4;
-		} else {
+		else
 			address_increment = 8;
-		}
 
 		current_address = (unsigned long)(bank_base_addr);
 		end_address = (unsigned long)(bank_base_addr) + num_bytes;
@@ -1446,4 +1445,5 @@ void program_ecc (unsigned long	 num_bytes)
 			SDRAM_CFG0_MCHK_CHK);
 	}
 }
+#endif /* CONFIG_DDR_ECC */
 #endif /* CONFIG_SPD_EEPROM */
diff --git a/cpu/ppc4xx/44x_spd_ddr2.c b/cpu/ppc4xx/44x_spd_ddr2.c
index 2ecd3e4..67ba5bd 100644
--- a/cpu/ppc4xx/44x_spd_ddr2.c
+++ b/cpu/ppc4xx/44x_spd_ddr2.c
@@ -58,8 +58,8 @@
 #define SDRAM_DDR2	2
 #define SDRAM_NONE	0
 
-#define MAXDIMMS 	2
-#define MAXRANKS 	4
+#define MAXDIMMS	2
+#define MAXRANKS	4
 #define MAXBXCF		4
 #define MAX_SPD_BYTES	256   /* Max number of bytes on the DIMM's SPD EEPROM */
 
@@ -109,7 +109,7 @@
 /* Defines for the Read Cycle Delay test */
 #define NUMMEMTESTS	8
 #define NUMMEMWORDS	8
-#define NUMLOOPS	256		/* memory test loops */
+#define NUMLOOPS	64		/* memory test loops */
 
 #undef CONFIG_ECC_ERROR_RESET		/* test-only: see description below, at check_ecc() */
 
@@ -129,6 +129,36 @@
 #define MY_TLB_WORD2_I_ENABLE	TLB_WORD2_I_ENABLE	/* disable caching on SDRAM */
 #endif
 
+/*
+ * Board-specific Platform code can reimplement spd_ddr_init_hang () if needed
+ */
+void __spd_ddr_init_hang (void)
+{
+	hang ();
+}
+void spd_ddr_init_hang (void) __attribute__((weak, alias("__spd_ddr_init_hang")));
+
+/*
+ * To provide an interface for board specific config values in this common
+ * DDR setup code, we implement he "weak" default functions here. They return
+ * the default value back to the caller.
+ *
+ * Please see include/configs/yucca.h for an example fora board specific
+ * implementation.
+ */
+u32 __ddr_wrdtr(u32 default_val)
+{
+	return default_val;
+}
+u32 ddr_wrdtr(u32) __attribute__((weak, alias("__ddr_wrdtr")));
+
+u32 __ddr_clktr(u32 default_val)
+{
+	return default_val;
+}
+u32 ddr_clktr(u32) __attribute__((weak, alias("__ddr_clktr")));
+
+
 /* Private Structure Definitions */
 
 /* enum only to ease code for cas latency setting */
@@ -144,7 +174,6 @@ typedef enum ddr_cas_id {
  * Prototypes
  *-----------------------------------------------------------------------------*/
 static unsigned long sdram_memsize(void);
-void program_tlb(u32 start, u32 size, u32 tlb_word2_i_value);
 static void get_spd_info(unsigned long *dimm_populated,
 			 unsigned char *iic0_dimm_addr,
 			 unsigned long num_dimm_banks);
@@ -206,9 +235,7 @@ static void	test(void);
 #else
 static void	DQS_calibration_process(void);
 #endif
-#if defined(DEBUG)
 static void ppc440sp_sdram_register_dump(void);
-#endif
 int do_reset (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[]);
 void dcbz_area(u32 start_address, u32 num_bytes);
 void dflush(void);
@@ -459,13 +486,14 @@ long int initdram(int board_type)
 	 *-----------------------------------------------------------------*/
 	mfsdram(SDRAM_WRDTR, val);
 	mtsdram(SDRAM_WRDTR, (val & ~(SDRAM_WRDTR_LLWP_MASK | SDRAM_WRDTR_WTR_MASK)) |
-		(SDRAM_WRDTR_LLWP_1_CYC | SDRAM_WRDTR_WTR_90_DEG_ADV));
+		ddr_wrdtr(SDRAM_WRDTR_LLWP_1_CYC | SDRAM_WRDTR_WTR_90_DEG_ADV));
 
 	/*------------------------------------------------------------------
 	 * Set the SDRAM Clock Timing Register
 	 *-----------------------------------------------------------------*/
 	mfsdram(SDRAM_CLKTR, val);
-	mtsdram(SDRAM_CLKTR, (val & ~SDRAM_CLKTR_CLKP_MASK) | SDRAM_CLKTR_CLKP_0_DEG);
+	mtsdram(SDRAM_CLKTR, (val & ~SDRAM_CLKTR_CLKP_MASK) |
+		ddr_clktr(SDRAM_CLKTR_CLKP_0_DEG));
 
 	/*------------------------------------------------------------------
 	 * Program the BxCF registers.
@@ -524,7 +552,12 @@ long int initdram(int board_type)
 	dram_size = sdram_memsize();
 
 	/* and program tlb entries for this size (dynamic) */
-	program_tlb(0, dram_size, MY_TLB_WORD2_I_ENABLE);
+
+	/*
+	 * Program TLB entries with caches enabled, for best performace
+	 * while auto-calibrating and ECC generation
+	 */
+	program_tlb(0, 0, dram_size, 0);
 
 	/*------------------------------------------------------------------
 	 * DQS calibration.
@@ -535,12 +568,18 @@ long int initdram(int board_type)
 	/*------------------------------------------------------------------
 	 * If ecc is enabled, initialize the parity bits.
 	 *-----------------------------------------------------------------*/
-	program_ecc(dimm_populated, iic0_dimm_addr, num_dimm_banks, MY_TLB_WORD2_I_ENABLE);
+	program_ecc(dimm_populated, iic0_dimm_addr, num_dimm_banks, 0);
 #endif
 
-#ifdef DEBUG
+	/*
+	 * Now after initialization (auto-calibration and ECC generation)
+	 * remove the TLB entries with caches enabled and program again with
+	 * desired cache functionality
+	 */
+	remove_tlb(0, dram_size);
+	program_tlb(0, 0, dram_size, MY_TLB_WORD2_I_ENABLE);
+
 	ppc440sp_sdram_register_dump();
-#endif
 
 	return dram_size;
 }
@@ -578,11 +617,10 @@ static void get_spd_info(unsigned long *dimm_populated,
 
 	if (dimm_found == FALSE) {
 		printf("ERROR - No memory installed. Install a DDR-SDRAM DIMM.\n\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 }
 
-#ifdef CONFIG_ADD_RAM_INFO
 void board_add_ram_info(int use_default)
 {
 	PPC440_SYS_INFO board_cfg;
@@ -603,7 +641,6 @@ void board_add_ram_info(int use_default)
 	val = (val & SDRAM_MMODE_DCL_MASK) >> 4;
 	printf(", CL%d)", val);
 }
-#endif
 
 /*------------------------------------------------------------------
  * For the memory DIMMs installed, this routine verifies that they
@@ -625,42 +662,42 @@ static void check_mem_type(unsigned long *dimm_populated,
 				       "slot %d.\n", (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 2:
 				printf("ERROR: EDO DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 3:
 				printf("ERROR: Pipelined Nibble DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 4:
 				printf("ERROR: SDRAM DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 5:
 				printf("ERROR: Multiplexed ROM DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 6:
 				printf("ERROR: SGRAM DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 7:
 				debug("DIMM slot %d: DDR1 SDRAM detected\n", dimm_num);
@@ -675,7 +712,7 @@ static void check_mem_type(unsigned long *dimm_populated,
 				       (unsigned int)dimm_num);
 				printf("Only DDR1 and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 		}
@@ -685,7 +722,7 @@ static void check_mem_type(unsigned long *dimm_populated,
 		    && (dimm_populated[dimm_num]   != SDRAM_NONE)
 		    && (dimm_populated[dimm_num-1] != dimm_populated[dimm_num])) {
 			printf("ERROR: DIMM's DDR1 and DDR2 type can not be mixed.\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 }
@@ -760,7 +797,7 @@ static void check_frequency(unsigned long *dimm_populated,
 				       (unsigned int)(calc_cycle_time*10));
 				printf("Replace the DIMM, or change DDR frequency via "
 				       "strapping bits.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 		}
 	}
@@ -792,7 +829,7 @@ static void check_rank_number(unsigned long *dimm_populated,
 				       "slot %d is not supported.\n", dimm_rank, dimm_num);
 				printf("Only %d ranks are supported for all DIMM.\n", MAXRANKS);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			} else
 				total_rank += dimm_rank;
 		}
@@ -801,7 +838,7 @@ static void check_rank_number(unsigned long *dimm_populated,
 			       "for all slots.\n", (unsigned int)total_rank);
 			printf("Only %d ranks are supported for all DIMM.\n", MAXRANKS);
 			printf("Remove one of the DIMM modules.\n\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 }
@@ -826,28 +863,28 @@ static void check_voltage_type(unsigned long *dimm_populated,
 				printf("This DIMM is 5.0 Volt/TTL.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x01:
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("This DIMM is LVTTL.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x02:
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("This DIMM is 1.5 Volt.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x03:
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("This DIMM is 3.3 Volt/TTL.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x04:
 				/* 2.5 Voltage only for DDR1 */
@@ -859,7 +896,7 @@ static void check_voltage_type(unsigned long *dimm_populated,
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 		}
@@ -1002,13 +1039,13 @@ static void program_copt1(unsigned long *dimm_populated,
 	if ((dimm_populated[0] != SDRAM_NONE) && (dimm_populated[1] != SDRAM_NONE)) {
 		if (buf0 != buf1) {
 			printf("ERROR: DIMM's buffered/unbuffered, registered, clocking don't match.\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 
 	if ((dimm_64bit == TRUE) && (dimm_32bit == TRUE)) {
 		printf("ERROR: Cannot mix 32 bit and 64 bit DDR-SDRAM DIMMs together.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 	else if ((dimm_64bit == TRUE) && (dimm_32bit == FALSE)) {
 		mcopt1 |= SDRAM_MCOPT1_DMWD_64;
@@ -1016,7 +1053,7 @@ static void program_copt1(unsigned long *dimm_populated,
 		mcopt1 |= SDRAM_MCOPT1_DMWD_32;
 	} else {
 		printf("ERROR: Please install only 32 or 64 bit DDR-SDRAM DIMMs.\n\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	if (ecc_enabled == TRUE)
@@ -1117,7 +1154,8 @@ static void program_codt(unsigned long *dimm_populated,
 				modt3 = 0x00000000;
 			}
 			if (total_rank == 4) {
-				codt |= CALC_ODT_R(0) | CALC_ODT_R(1) | CALC_ODT_R(2) | CALC_ODT_R(3);
+				codt |= CALC_ODT_R(0) | CALC_ODT_R(1) |
+					CALC_ODT_R(2) | CALC_ODT_R(3);
 				modt0 = CALC_ODT_RW(2);
 				modt1 = 0x00000000;
 				modt2 = CALC_ODT_RW(0);
@@ -1204,7 +1242,7 @@ static void program_initplr(unsigned long *dimm_populated,
 			break;
 		default:
 			printf("ERROR: ucode error on selected_cas value %d", selected_cas);
-			hang();
+			spd_ddr_init_hang ();
 			break;
 		}
 
@@ -1236,7 +1274,7 @@ static void program_initplr(unsigned long *dimm_populated,
 			break;
 		default:
 			printf("ERROR: write recovery not support (%d)", write_recovery);
-			hang();
+			spd_ddr_init_hang ();
 			break;
 		}
 #else
@@ -1254,7 +1292,7 @@ static void program_initplr(unsigned long *dimm_populated,
 			ods = ODS_REDUCED;
 		} else {
 			printf("ERROR: Unsupported number of DIMM's (%d)", total_dimm);
-			hang();
+			spd_ddr_init_hang ();
 		}
 
 		mr = CMD_EMR | SELECT_MR | BURST_LEN_4 | wr | cas;
@@ -1279,7 +1317,7 @@ static void program_initplr(unsigned long *dimm_populated,
 		mtsdram(SDRAM_INITPLR13, 0x80800000 | emr);		/* EMR OCD Exit */
 	} else {
 		printf("ERROR: ucode error as unknown DDR type in program_initplr");
-		hang();
+		spd_ddr_init_hang ();
 	}
 }
 
@@ -1384,7 +1422,7 @@ static void program_mode(unsigned long *dimm_populated,
 					} else {
 						printf("ERROR: SPD reported Tcyc is incorrect for DIMM "
 						       "in slot %d\n", (unsigned int)dimm_num);
-						hang();
+						spd_ddr_init_hang ();
 					}
 				} else {
 					/* Convert from hex to decimal */
@@ -1521,7 +1559,7 @@ static void program_mode(unsigned long *dimm_populated,
 			printf("ERROR: Cannot find a supported CAS latency with the installed DIMMs.\n");
 			printf("Only DIMMs DDR1 with CAS latencies of 2.0, 2.5, and 3.0 are supported.\n");
 			printf("Make sure the PLB speed is within the supported range of the DIMMs.\n\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	} else { /* DDR2 */
 		debug("cas_3_0_available=%d\n", cas_3_0_available);
@@ -1544,7 +1582,7 @@ static void program_mode(unsigned long *dimm_populated,
 			       cas_3_0_available, cas_4_0_available, cas_5_0_available);
 			printf("sdram_freq=%d cycle3=%d cycle4=%d cycle5=%d\n\n",
 			       sdram_freq, cycle_3_0_clk, cycle_4_0_clk, cycle_5_0_clk);
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 
@@ -1653,7 +1691,7 @@ static void program_rtr(unsigned long *dimm_populated,
 				printf("ERROR: DIMM %d unsupported refresh rate/type.\n",
 				       (unsigned int)dimm_num);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 
@@ -2061,7 +2099,7 @@ static void program_bxcf(unsigned long *dimm_populated,
 					printf("ERROR: Unsupported value for number of "
 					       "column addresses: %d.\n", (unsigned int)num_col_addr);
 					printf("Replace the DIMM module with a supported DIMM.\n\n");
-					hang();
+					spd_ddr_init_hang ();
 				}
 			}
 
@@ -2143,7 +2181,7 @@ static void program_memory_queue(unsigned long *dimm_populated,
 				printf("ERROR: Unsupported value for the banksize: %d.\n",
 				       (unsigned int)rank_size_id);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 
 			if ((dimm_populated[dimm_num] != SDRAM_NONE) && (dimm_num == 1))
@@ -2688,7 +2726,8 @@ calibration_loop:
 		printf("\nERROR: Cannot determine a common read delay for the "
 		       "DIMM(s) installed.\n");
 		debug("%s[%d] ERROR : \n", __FUNCTION__,__LINE__);
-		hang();
+		ppc440sp_sdram_register_dump();
+		spd_ddr_init_hang ();
 	}
 
 	blank_string(strlen(str));
@@ -2844,7 +2883,7 @@ static void test(void)
 	if (window_found == FALSE) {
 		printf("ERROR: Cannot determine a common read delay for the "
 		       "DIMM(s) installed.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	/*------------------------------------------------------------------
@@ -3013,5 +3052,9 @@ static void ppc440sp_sdram_register_dump(void)
 	dcr_data = mfdcr(SDRAM_R3BAS);
 	printf("        MQ3_B0BAS       = 0x%08X\n", dcr_data);
 }
+#else
+static void ppc440sp_sdram_register_dump(void)
+{
+}
 #endif
 #endif /* CONFIG_SPD_EEPROM */
diff --git a/cpu/ppc4xx/4xx_enet.c b/cpu/ppc4xx/4xx_enet.c
index 1200d02..cc8e734 100644
--- a/cpu/ppc4xx/4xx_enet.c
+++ b/cpu/ppc4xx/4xx_enet.c
@@ -94,9 +94,9 @@
  * network support enabled.
  * Remark: CONFIG_405 describes Xilinx PPC405 FPGA without EMAC controller!
  */
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && !defined(CONFIG_405) && !defined(CONFIG_IOP480)
+#if defined(CONFIG_CMD_NET) && !defined(CONFIG_405) && !defined(CONFIG_IOP480)
 
-#if !(defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII))
+#if !(defined(CONFIG_MII) || defined(CONFIG_CMD_MII))
 #error "CONFIG_MII has to be defined!"
 #endif
 
@@ -1415,10 +1415,8 @@ static void enet_rcv (struct eth_device *dev, unsigned long malisr)
 			if ((MAL_RX_CTRL_EMPTY & hw_p->rx[i].ctrl)
 			    || (loop_count >= NUM_RX_BUFF))
 				break;
+
 			loop_count++;
-			hw_p->rx_slot++;
-			if (NUM_RX_BUFF == hw_p->rx_slot)
-				hw_p->rx_slot = 0;
 			handled++;
 			data_len = (unsigned long) hw_p->rx[i].data_len;	/* Get len */
 			if (data_len) {
@@ -1468,6 +1466,10 @@ static void enet_rcv (struct eth_device *dev, unsigned long malisr)
 				if (NUM_RX_BUFF == hw_p->rx_i_index)
 					hw_p->rx_i_index = 0;
 
+				hw_p->rx_slot++;
+				if (NUM_RX_BUFF == hw_p->rx_slot)
+					hw_p->rx_slot = 0;
+
 				/*  AS.HARNOIS
 				 * free receive buffer only when
 				 * buffer has been handled (eth_rx)
@@ -1683,7 +1685,7 @@ int ppc_4xx_eth_initialize (bd_t * bis)
 #endif
 
 #if defined(CONFIG_NET_MULTI)
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 		miiphy_register (dev->name,
 				 emac4xx_miiphy_read, emac4xx_miiphy_write);
 #endif
@@ -1724,7 +1726,7 @@ int eth_rx(void)
 
 int emac4xx_miiphy_initialize (bd_t * bis)
 {
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 	miiphy_register ("ppc_4xx_eth0",
 			 emac4xx_miiphy_read, emac4xx_miiphy_write);
 #endif
@@ -1733,4 +1735,4 @@ int emac4xx_miiphy_initialize (bd_t * bis)
 }
 #endif /* !defined(CONFIG_NET_MULTI) */
 
-#endif /* #if (CONFIG_COMMANDS & CFG_CMD_NET) */
+#endif
diff --git a/cpu/ppc4xx/Makefile b/cpu/ppc4xx/Makefile
index 4068b53..af9da5b 100644
--- a/cpu/ppc4xx/Makefile
+++ b/cpu/ppc4xx/Makefile
@@ -27,12 +27,12 @@ LIB	= $(obj)lib$(CPU).a
 
 START	= start.o resetvec.o kgdb.o
 SOBJS	= dcr.o
-COBJS	= 405gp_pci.o 4xx_enet.o \
+COBJS	= 405gp_pci.o 440spe_pcie.o 4xx_enet.o \
 	  bedbug_405.o commproc.o \
 	  cpu.o cpu_init.o gpio.o i2c.o interrupts.o \
 	  miiphy.o ndfc.o sdram.o serial.o \
 	  40x_spd_sdram.o 44x_spd_ddr.o 44x_spd_ddr2.o speed.o \
-	  tlb.o traps.o usb_ohci.o usbdev.o \
+	  tlb.o traps.o usb_ohci.o usb.o usbdev.o \
 	  440spe_pcie.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
diff --git a/cpu/ppc4xx/bedbug_405.c b/cpu/ppc4xx/bedbug_405.c
index a3c2119..5ef5607 100644
--- a/cpu/ppc4xx/bedbug_405.c
+++ b/cpu/ppc4xx/bedbug_405.c
@@ -10,7 +10,7 @@
 #include <bedbug/regs.h>
 #include <bedbug/ppc.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG) && defined(CONFIG_4xx)
+#if defined(CONFIG_CMD_BEDBUG) && defined(CONFIG_4xx)
 
 #define MAX_BREAK_POINTS 4
 
diff --git a/cpu/ppc4xx/config.mk b/cpu/ppc4xx/config.mk
index 119e061..4fd5108 100644
--- a/cpu/ppc4xx/config.mk
+++ b/cpu/ppc4xx/config.mk
@@ -22,5 +22,13 @@
 #
 
 PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -fno-strict-aliasing
+PLATFORM_CPPFLAGS += -DCONFIG_4xx -ffixed-r2 -ffixed-r29 -mstring -msoft-float
 
-PLATFORM_CPPFLAGS += -DCONFIG_4xx -ffixed-r2 -ffixed-r29 -mstring -Wa,-m405 -mcpu=405 -msoft-float
+cfg=$(shell grep configs $(OBJTREE)/include/config.h | sed 's/.*<\(configs.*\)>/\1/')
+is440=$(shell grep CONFIG_440 $(TOPDIR)/include/$(cfg))
+
+ifneq (,$(findstring CONFIG_440,$(is440)))
+PLATFORM_CPPFLAGS += -Wa,-m440 -mcpu=440
+else
+PLATFORM_CPPFLAGS += -Wa,-m405 -mcpu=405
+endif
diff --git a/cpu/ppc4xx/cpu.c b/cpu/ppc4xx/cpu.c
index 8e6bc84..c07bc0c 100644
--- a/cpu/ppc4xx/cpu.c
+++ b/cpu/ppc4xx/cpu.c
@@ -139,6 +139,7 @@ static char *bootstrap_str[] = {
 	"Reserved",
 	"I2C (Addr 0x50)",
 };
+static char bootstrap_char[] = { 'A', 'B', 'C', 'B', 'D', 'E', 'x', 'F' };
 #endif
 
 #if defined(CONFIG_440SP) || defined(CONFIG_440SPE)
@@ -149,6 +150,7 @@ static char *bootstrap_str[] = {
 	"I2C (Addr 0x54)",
 	"I2C (Addr 0x50)",
 };
+static char bootstrap_char[] = { 'A', 'B', 'C', 'D'};
 #endif
 
 #if defined(CONFIG_440EP) || defined(CONFIG_440GR)
@@ -163,6 +165,7 @@ static char *bootstrap_str[] = {
 	"PCI",
 	"I2C (Addr 0x52)",
 };
+static char bootstrap_char[] = { 'A', 'B', 'C', 'D', 'E', 'G', 'F', 'H' };
 #endif
 
 #if defined(CONFIG_440EPX) || defined(CONFIG_440GRX)
@@ -177,6 +180,7 @@ static char *bootstrap_str[] = {
 	"PCI",
 	"I2C (Addr 0x52)",
 };
+static char bootstrap_char[] = { 'A', 'B', 'C', 'D', 'E', 'G', 'F', 'H' };
 #endif
 
 #if defined(CONFIG_405EZ)
@@ -199,6 +203,8 @@ static char *bootstrap_str[] = {
 	"SPI (slow)",
 	"I2C (Addr 0x50)",
 };
+static char bootstrap_char[] = { 'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', \
+				 'I', 'x', 'K', 'L', 'M', 'N', 'O', 'P' };
 #endif
 
 #if defined(SDR0_PINSTP_SHIFT)
@@ -427,7 +433,7 @@ int checkcpu (void)
 	printf ("       I2C boot EEPROM %sabled\n", i2c_bootrom_enabled() ? "en" : "dis");
 #endif	/* I2C_BOOTROM */
 #if defined(SDR0_PINSTP_SHIFT)
-	printf ("       Bootstrap Option %c - ", (char)bootstrap_option() + 'A');
+	printf ("       Bootstrap Option %c - ", bootstrap_char[bootstrap_option()]);
 	printf ("Boot ROM Location %s\n", bootstrap_str[bootstrap_option()]);
 #endif	/* SDR0_PINSTP_SHIFT */
 
diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index 66e8637..351da36 100644
--- a/cpu/ppc4xx/cpu_init.c
+++ b/cpu/ppc4xx/cpu_init.c
@@ -153,7 +153,7 @@ cpu_init_f (void)
 	 */
 	asm volatile("	bl	0f"		::: "lr");
 	asm volatile("0:	mflr	3"		::: "r3");
-	asm volatile("	addi 	4, 0, 14"	::: "r4");
+	asm volatile("	addi	4, 0, 14"	::: "r4");
 	asm volatile("	mtctr	4"		::: "ctr");
 	asm volatile("1:	icbt	0, 3");
 	asm volatile("	addi	3, 3, 32"	::: "r3");
@@ -211,6 +211,8 @@ cpu_init_f (void)
 	val = mfspr(tcr);
 #if defined(CONFIG_440EP) || defined(CONFIG_440GR)
 	val |= 0xb8000000;      /* generate system reset after 1.34 seconds */
+#elif defined(CONFIG_440EPX)
+	val |= 0xb0000000;      /* generate system reset after 1.34 seconds */
 #else
 	val |= 0xf0000000;      /* generate system reset after 2.684 seconds */
 #endif
diff --git a/cpu/ppc4xx/dcr.S b/cpu/ppc4xx/dcr.S
index 7102364..93465a3 100644
--- a/cpu/ppc4xx/dcr.S
+++ b/cpu/ppc4xx/dcr.S
@@ -22,7 +22,7 @@
  */
 #include <config.h>
 
-#if defined(CONFIG_4xx) && defined(CFG_CMD_SETGETDCR)
+#if defined(CONFIG_4xx) && defined(CONFIG_CMD_SETGETDCR)
 
 #include <ppc4xx.h>
 
@@ -195,4 +195,4 @@ set_dcr:
 		blr				/* Return to calling function */
 .Lfe4:		.size	set_dcr,.Lfe4-set_dcr
 /* end set_dcr() */
-#endif /* CONFIG_4xx & CFG_CMD_SETGETDCR */
+#endif
diff --git a/cpu/ppc4xx/gpio.c b/cpu/ppc4xx/gpio.c
index dd84e58..50f2fdf 100644
--- a/cpu/ppc4xx/gpio.c
+++ b/cpu/ppc4xx/gpio.c
@@ -103,6 +103,18 @@ void gpio_write_bit(int pin, int val)
 		out32(GPIO0_OR + offs, in32(GPIO0_OR + offs) & ~GPIO_VAL(pin));
 }
 
+int gpio_read_out_bit(int pin)
+{
+	u32 offs = 0;
+
+	if (pin >= GPIO_MAX) {
+		offs = 0x100;
+		pin -= GPIO_MAX;
+	}
+
+	return (in32(GPIO0_OR + offs) & GPIO_VAL(pin) ? 1 : 0);
+}
+
 #if defined(CFG_440_GPIO_TABLE)
 void gpio_set_chip_configuration(void)
 {
@@ -157,14 +169,42 @@ void gpio_set_chip_configuration(void)
 				switch (gpio_tab[gpio_core][i].alt_nb) {
 				case GPIO_SEL:
 					if (gpio_core == GPIO0) {
-						reg = in32(GPIO0_TCR) | (0x80000000 >> (j));
+						/*
+						 * Setup output value
+						 * 1 -> high level
+						 * 0 -> low level
+						 * else -> don't touch
+						 */
+						reg = in32(GPIO0_OR);
+						if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_1)
+							reg |= (0x80000000 >> (i));
+						else if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_0)
+							reg &= ~(0x80000000 >> (i));
+						out32(GPIO0_OR, reg);
+
+						reg = in32(GPIO0_TCR) | (0x80000000 >> (i));
 						out32(GPIO0_TCR, reg);
 					}
 
+#ifdef GPIO1
 					if (gpio_core == GPIO1) {
-						reg = in32(GPIO1_TCR) | (0x80000000 >> (j));
+						/*
+						 * Setup output value
+						 * 1 -> high level
+						 * 0 -> low level
+						 * else -> don't touch
+						 */
+						reg = in32(GPIO1_OR);
+						if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_1)
+							reg |= (0x80000000 >> (i));
+						else if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_0)
+							reg &= ~(0x80000000 >> (i));
+						out32(GPIO1_OR, reg);
+
+						reg = in32(GPIO1_TCR) | (0x80000000 >> (i));
 						out32(GPIO1_TCR, reg);
 					}
+#endif /* GPIO1 */
 
 					reg = in32(GPIO_OS(core_add+offs))
 						& ~(GPIO_MASK >> (j*2));
diff --git a/cpu/ppc4xx/interrupts.c b/cpu/ppc4xx/interrupts.c
index c5a9f02..ca565cc 100644
--- a/cpu/ppc4xx/interrupts.c
+++ b/cpu/ppc4xx/interrupts.c
@@ -628,7 +628,7 @@ void timer_interrupt_cpu (struct pt_regs *regs)
 
 /****************************************************************************/
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 
 /*******************************************************************************
  *
@@ -698,4 +698,4 @@ do_irqinfo(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 
 	return 0;
 }
-#endif  /* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/ppc4xx/kgdb.S b/cpu/ppc4xx/kgdb.S
index be28340..8c4bbf2 100644
--- a/cpu/ppc4xx/kgdb.S
+++ b/cpu/ppc4xx/kgdb.S
@@ -34,7 +34,7 @@
 #include <asm/cache.h>
 #include <asm/mmu.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
  /*
  * cache flushing routines for kgdb
  */
@@ -75,4 +75,4 @@ kgdb_flush_cache_range:
 	SYNC
 	blr
 
-#endif /* CFG_CMD_KGDB */
+#endif
diff --git a/cpu/ppc4xx/ndfc.c b/cpu/ppc4xx/ndfc.c
index b198ff4..3984577 100644
--- a/cpu/ppc4xx/ndfc.c
+++ b/cpu/ppc4xx/ndfc.c
@@ -3,7 +3,7 @@
  *   Platform independend driver for NDFC (NanD Flash Controller)
  *   integrated into EP440 cores
  *
- * (C) Copyright 2006
+ * (C) Copyright 2006-2007
  * Stefan Roese, DENX Software Engineering, sr@denx.de.
  *
  * Based on original work by
@@ -31,14 +31,17 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY) && \
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY) && \
 	(defined(CONFIG_440EP) || defined(CONFIG_440GR) ||	     \
-	 defined(CONFIG_440EPX) || defined(CONFIG_440GRX))
+	 defined(CONFIG_440EPX) || defined(CONFIG_440GRX) ||	     \
+	 defined(CONFIG_405EZ))
 
 #include <nand.h>
 #include <linux/mtd/ndfc.h>
+#include <linux/mtd/nand_ecc.h>
 #include <asm/processor.h>
-#include <ppc440.h>
+#include <asm/io.h>
+#include <ppc4xx.h>
 
 static u8 hwctl = 0;
 
@@ -69,11 +72,11 @@ static void ndfc_write_byte(struct mtd_info *mtdinfo, u_char byte)
 	ulong base = (ulong) this->IO_ADDR_W & 0xfffffffc;
 
 	if (hwctl & 0x1)
-		out8(base + NDFC_CMD, byte);
+		out_8((u8 *)(base + NDFC_CMD), byte);
 	else if (hwctl & 0x2)
-		out8(base + NDFC_ALE, byte);
+		out_8((u8 *)(base + NDFC_ALE), byte);
 	else
-		out8(base + NDFC_DATA, byte);
+		out_8((u8 *)(base + NDFC_DATA), byte);
 }
 
 static u_char ndfc_read_byte(struct mtd_info *mtdinfo)
@@ -81,7 +84,7 @@ static u_char ndfc_read_byte(struct mtd_info *mtdinfo)
 	struct nand_chip *this = mtdinfo->priv;
 	ulong base = (ulong) this->IO_ADDR_W & 0xfffffffc;
 
-	return (in8(base + NDFC_DATA));
+	return (in_8((u8 *)(base + NDFC_DATA)));
 }
 
 static int ndfc_dev_ready(struct mtd_info *mtdinfo)
@@ -89,17 +92,41 @@ static int ndfc_dev_ready(struct mtd_info *mtdinfo)
 	struct nand_chip *this = mtdinfo->priv;
 	ulong base = (ulong) this->IO_ADDR_W & 0xfffffffc;
 
-	while (!(in32(base + NDFC_STAT) & NDFC_STAT_IS_READY))
+	while (!(in_be32((u32 *)(base + NDFC_STAT)) & NDFC_STAT_IS_READY))
 		;
 
 	return 1;
 }
 
-#ifndef CONFIG_NAND_SPL
-/*
- * Don't use these speedup functions in NAND boot image, since the image
- * has to fit into 4kByte.
- */
+static void ndfc_enable_hwecc(struct mtd_info *mtdinfo, int mode)
+{
+	struct nand_chip *this = mtdinfo->priv;
+	ulong base = (ulong) this->IO_ADDR_W & 0xfffffffc;
+	u32 ccr;
+
+	ccr = in_be32((u32 *)(base + NDFC_CCR));
+	ccr |= NDFC_CCR_RESET_ECC;
+	out_be32((u32 *)(base + NDFC_CCR), ccr);
+}
+
+static int ndfc_calculate_ecc(struct mtd_info *mtdinfo,
+			      const u_char *dat, u_char *ecc_code)
+{
+	struct nand_chip *this = mtdinfo->priv;
+	ulong base = (ulong) this->IO_ADDR_W & 0xfffffffc;
+	u32 ecc;
+	u8 *p = (u8 *)&ecc;
+
+	ecc = in_be32((u32 *)(base + NDFC_ECC));
+
+	/* The NDFC uses Smart Media (SMC) bytes order
+	 */
+	ecc_code[0] = p[2];
+	ecc_code[1] = p[1];
+	ecc_code[2] = p[3];
+
+	return 0;
+}
 
 /*
  * Speedups for buffer read/write/verify
@@ -115,9 +142,14 @@ static void ndfc_read_buf(struct mtd_info *mtdinfo, uint8_t *buf, int len)
 	uint32_t *p = (uint32_t *) buf;
 
 	for (;len > 0; len -= 4)
-		*p++ = in32(base + NDFC_DATA);
+		*p++ = in_be32((u32 *)(base + NDFC_DATA));
 }
 
+#ifndef CONFIG_NAND_SPL
+/*
+ * Don't use these speedup functions in NAND boot image, since the image
+ * has to fit into 4kByte.
+ */
 static void ndfc_write_buf(struct mtd_info *mtdinfo, const uint8_t *buf, int len)
 {
 	struct nand_chip *this = mtdinfo->priv;
@@ -125,7 +157,7 @@ static void ndfc_write_buf(struct mtd_info *mtdinfo, const uint8_t *buf, int len
 	uint32_t *p = (uint32_t *) buf;
 
 	for (; len > 0; len -= 4)
-		out32(base + NDFC_DATA, *p++);
+		out_be32((u32 *)(base + NDFC_DATA), *p++);
 }
 
 static int ndfc_verify_buf(struct mtd_info *mtdinfo, const uint8_t *buf, int len)
@@ -135,7 +167,7 @@ static int ndfc_verify_buf(struct mtd_info *mtdinfo, const uint8_t *buf, int len
 	uint32_t *p = (uint32_t *) buf;
 
 	for (; len > 0; len -= 4)
-		if (*p++ != in32(base + NDFC_DATA))
+		if (*p++ != in_be32((u32 *)(base + NDFC_DATA)))
 			return -1;
 
 	return 0;
@@ -152,8 +184,8 @@ void board_nand_select_device(struct nand_chip *nand, int chip)
 	ulong base = (ulong)nand->IO_ADDR_W & 0xfffffffc;
 
 	/* Set NandFlash Core Configuration Register */
-	/* 1col x 2 rows */
-	out32(base + NDFC_CCR, 0x00000000 | (cs << 24));
+	/* 1 col x 2 rows */
+	out_be32((u32 *)(base + NDFC_CCR), 0x00000000 | (cs << 24));
 }
 
 int board_nand_init(struct nand_chip *nand)
@@ -161,23 +193,25 @@ int board_nand_init(struct nand_chip *nand)
 	int cs = (ulong)nand->IO_ADDR_W & 0x00000003;
 	ulong base = (ulong)nand->IO_ADDR_W & 0xfffffffc;
 
-	nand->eccmode = NAND_ECC_SOFT;
-
 	nand->hwcontrol  = ndfc_hwcontrol;
 	nand->read_byte  = ndfc_read_byte;
+	nand->read_buf   = ndfc_read_buf;
 	nand->write_byte = ndfc_write_byte;
 	nand->dev_ready  = ndfc_dev_ready;
 
+	nand->eccmode = NAND_ECC_HW3_256;
+	nand->enable_hwecc = ndfc_enable_hwecc;
+	nand->calculate_ecc = ndfc_calculate_ecc;
+	nand->correct_data = nand_correct_data;
+
 #ifndef CONFIG_NAND_SPL
 	nand->write_buf  = ndfc_write_buf;
-	nand->read_buf   = ndfc_read_buf;
 	nand->verify_buf = ndfc_verify_buf;
 #else
 	/*
 	 * Setup EBC (CS0 only right now)
 	 */
-	mtdcr(ebccfga, xbcfg);
-	mtdcr(ebccfgd, 0xb8400000);
+	mtebc(EBC0_CFG, 0xb8400000);
 
 	mtebc(pb0cr, CFG_EBC_PB0CR);
 	mtebc(pb0ap, CFG_EBC_PB0AP);
@@ -187,7 +221,7 @@ int board_nand_init(struct nand_chip *nand)
 	 * Select required NAND chip in NDFC
 	 */
 	board_nand_select_device(nand, cs);
-	out32(base + NDFC_BCFG0 + (cs << 2), 0x80002222);
+	out_be32((u32 *)(base + NDFC_BCFG0 + (cs << 2)), 0x80002222);
 	return 0;
 }
 
diff --git a/cpu/ppc4xx/sdram.c b/cpu/ppc4xx/sdram.c
index d520cd3..2724d91 100644
--- a/cpu/ppc4xx/sdram.c
+++ b/cpu/ppc4xx/sdram.c
@@ -187,14 +187,14 @@ void sdram_init(void)
 		/*
 		 * Disable memory controller.
 		 */
-		mtsdram0(mem_mcopt1, 0x00000000);
+		mtsdram(mem_mcopt1, 0x00000000);
 
 		/*
 		 * Set MB0CF for bank 0.
 		 */
-		mtsdram0(mem_mb0cf, mb0cf[i].reg);
-		mtsdram0(mem_sdtr1, sdtr1);
-		mtsdram0(mem_rtr, compute_rtr(speed, mb0cf[i].rows, 64));
+		mtsdram(mem_mb0cf, mb0cf[i].reg);
+		mtsdram(mem_sdtr1, sdtr1);
+		mtsdram(mem_rtr, compute_rtr(speed, mb0cf[i].rows, 64));
 
 		udelay(200);
 
@@ -203,14 +203,34 @@ void sdram_init(void)
 		 * Set DC_EN to '1' and BRD_PRF to '01' for 16 byte PLB Burst
 		 * read/prefetch.
 		 */
-		mtsdram0(mem_mcopt1, 0x80800000);
+		mtsdram(mem_mcopt1, 0x80800000);
 
 		udelay(10000);
 
 		if (get_ram_size(0, mb0cf[i].size) == mb0cf[i].size) {
 			/*
-			 * OK, size detected -> all done
+			 * OK, size detected.  Enable second bank if
+			 * defined (assumes same type as bank 0)
 			 */
+#ifdef CONFIG_SDRAM_BANK1
+			u32 b1cr = mb0cf[i].size | mb0cf[i].reg;
+
+			mtsdram(mem_mcopt1, 0x00000000);
+			mtsdram(mem_mb1cf, b1cr); /* SDRAM0_B1CR */
+			mtsdram(mem_mcopt1, 0x80800000);
+			udelay(10000);
+
+			/*
+			 * Check if 2nd bank is really available.
+			 * If the size not equal to the size of the first
+			 * bank, then disable the 2nd bank completely.
+			 */
+			if (get_ram_size((long *)mb0cf[i].size, mb0cf[i].size) !=
+			    mb0cf[i].size) {
+				mtsdram(mem_mb1cf, 0);
+				mtsdram(mem_mcopt1, 0);
+			}
+#endif
 			return;
 		}
 	}
diff --git a/cpu/ppc4xx/sdram.h b/cpu/ppc4xx/sdram.h
index 62b5442..4fb9b1a 100644
--- a/cpu/ppc4xx/sdram.h
+++ b/cpu/ppc4xx/sdram.h
@@ -29,8 +29,6 @@
 
 #include <config.h>
 
-#define mtsdram0(reg, data)  mtdcr(memcfga,reg);mtdcr(memcfgd,data)
-
 #define ONE_BILLION	1000000000
 
 struct sdram_conf_s {
diff --git a/cpu/ppc4xx/serial.c b/cpu/ppc4xx/serial.c
index e62dd9d..60712b1 100644
--- a/cpu/ppc4xx/serial.c
+++ b/cpu/ppc4xx/serial.c
@@ -448,12 +448,17 @@ static void serial_divs (int baudrate, unsigned long *pudiv,
 	unsigned long i;
 	unsigned long est;		/* current estimate */
 	unsigned long plloutb;
+	unsigned long cpr_pllc;
 	u32 reg;
 
+	/* check the pll feedback source */
+	mfcpr(cprpllc, cpr_pllc);
+
 	get_sys_info(&sysinfo);
 
-	plloutb = ((CONFIG_SYS_CLK_FREQ * sysinfo.pllFwdDiv * sysinfo.pllFbkDiv)
-		   / sysinfo.pllFwdDivB);
+	plloutb = ((CONFIG_SYS_CLK_FREQ * ((cpr_pllc & PLLC_SRC_MASK) ?
+		sysinfo.pllFwdDivB : sysinfo.pllFwdDiv) * sysinfo.pllFbkDiv) /
+		sysinfo.pllFwdDivB);
 	udiv = 256;			/* Assume lowest possible serial clk */
 	div = plloutb / (16 * baudrate); /* total divisor */
 	umin = (plloutb / get_OPB_freq()) << 1;	/* 2 x OPB divisor */
@@ -843,7 +848,7 @@ int serial_buffered_tstc (void)
 
 #endif	/* CONFIG_SERIAL_SOFTWARE_FIFO */
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 /*
   AS HARNOIS : according to CONFIG_KGDB_SER_INDEX kgdb uses serial port
   number 0 or number 1
@@ -949,7 +954,7 @@ void kgdb_interruptible (int yes)
 	return;
 }
 #endif	/* (CONFIG_KGDB_SER_INDEX & 2) */
-#endif	/* CFG_CMD_KGDB */
+#endif
 
 
 #if defined(CONFIG_SERIAL_MULTI)
diff --git a/cpu/ppc4xx/speed.c b/cpu/ppc4xx/speed.c
index 028b11a..da5330a 100644
--- a/cpu/ppc4xx/speed.c
+++ b/cpu/ppc4xx/speed.c
@@ -771,6 +771,7 @@ ulong get_PCI_freq (void)
 void get_sys_info (PPC405_SYS_INFO * sysInfo)
 {
 	unsigned long cpr_plld;
+	unsigned long cpr_pllc;
 	unsigned long cpr_primad;
 	unsigned long sysClkPeriodPs = ONE_BILLION / (CONFIG_SYS_CLK_FREQ/1000);
 	unsigned long primad_cpudv;
@@ -780,6 +781,7 @@ void get_sys_info (PPC405_SYS_INFO * sysInfo)
 	 * Read PLL Mode registers
 	 */
 	mfcpr(cprplld, cpr_plld);
+	mfcpr(cprpllc, cpr_pllc);
 
 	/*
 	 * Determine forward divider A
@@ -787,20 +789,18 @@ void get_sys_info (PPC405_SYS_INFO * sysInfo)
 	sysInfo->pllFwdDiv = ((cpr_plld & PLLD_FWDVA_MASK) >> 16);
 
 	/*
-	 * Determine forward divider B (should be equal to A)
+	 * Determine forward divider B
 	 */
 	sysInfo->pllFwdDivB = ((cpr_plld & PLLD_FWDVB_MASK) >> 8);
-	if (sysInfo->pllFwdDivB == 0) {
+	if (sysInfo->pllFwdDivB == 0)
 		sysInfo->pllFwdDivB = 8;
-	}
 
 	/*
 	 * Determine FBK_DIV.
 	 */
 	sysInfo->pllFbkDiv = ((cpr_plld & PLLD_FBDV_MASK) >> 24);
-	if (sysInfo->pllFbkDiv == 0) {
+	if (sysInfo->pllFbkDiv == 0)
 		sysInfo->pllFbkDiv = 256;
-	}
 
 	/*
 	 * Read CPR_PRIMAD register
@@ -810,30 +810,30 @@ void get_sys_info (PPC405_SYS_INFO * sysInfo)
 	 * Determine PLB_DIV.
 	 */
 	sysInfo->pllPlbDiv = ((cpr_primad & PRIMAD_PLBDV_MASK) >> 16);
-	if (sysInfo->pllPlbDiv == 0) {
+	if (sysInfo->pllPlbDiv == 0)
 		sysInfo->pllPlbDiv = 16;
-	}
 
 	/*
 	 * Determine EXTBUS_DIV.
 	 */
 	sysInfo->pllExtBusDiv = (cpr_primad & PRIMAD_EBCDV_MASK);
-	if (sysInfo->pllExtBusDiv == 0) {
+	if (sysInfo->pllExtBusDiv == 0)
 		sysInfo->pllExtBusDiv = 16;
-	}
 
 	/*
 	 * Determine OPB_DIV.
 	 */
 	sysInfo->pllOpbDiv = ((cpr_primad & PRIMAD_OPBDV_MASK) >> 8);
-	if (sysInfo->pllOpbDiv == 0) {
+	if (sysInfo->pllOpbDiv == 0)
 		sysInfo->pllOpbDiv = 16;
-	}
 
 	/*
 	 * Determine the M factor
 	 */
-	m = sysInfo->pllFbkDiv * sysInfo->pllFwdDivB;
+	if (cpr_pllc & PLLC_SRC_MASK)
+		m = sysInfo->pllFbkDiv * sysInfo->pllFwdDivB;
+	else
+		m = sysInfo->pllFbkDiv * sysInfo->pllFwdDiv;
 
 	/*
 	 * Determine VCO clock frequency
@@ -845,16 +845,17 @@ void get_sys_info (PPC405_SYS_INFO * sysInfo)
 	 * Determine CPU clock frequency
 	 */
 	primad_cpudv = ((cpr_primad & PRIMAD_CPUDV_MASK) >> 24);
-	if (primad_cpudv == 0) {
+	if (primad_cpudv == 0)
 		primad_cpudv = 16;
-	}
 
-	sysInfo->freqProcessor = (CONFIG_SYS_CLK_FREQ * sysInfo->pllFbkDiv) / primad_cpudv;
+	sysInfo->freqProcessor = (CONFIG_SYS_CLK_FREQ * m) /
+		sysInfo->pllFwdDiv / primad_cpudv;
 
 	/*
 	 * Determine PLB clock frequency
 	 */
-	sysInfo->freqPLB = (CONFIG_SYS_CLK_FREQ * sysInfo->pllFbkDiv) / sysInfo->pllPlbDiv;
+	sysInfo->freqPLB = (CONFIG_SYS_CLK_FREQ * m) /
+		sysInfo->pllFwdDiv / sysInfo->pllPlbDiv;
 }
 
 /********************************************
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index a96083c..9626b65 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -22,26 +22,27 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  * MA 02111-1307 USA
  */
-/*------------------------------------------------------------------------------+ */
-/* */
-/*	 This source code has been made available to you by IBM on an AS-IS */
-/*	 basis.	 Anyone receiving this source is licensed under IBM */
-/*	 copyrights to use it in any way he or she deems fit, including */
-/*	 copying it, modifying it, compiling it, and redistributing it either */
-/*	 with or without modifications.	 No license under IBM patents or */
-/*	 patent applications is to be implied by the copyright license. */
-/* */
-/*	 Any user of this software should understand that IBM cannot provide */
-/*	 technical support for this software and will not be responsible for */
-/*	 any consequences resulting from the use of this software. */
-/* */
-/*	 Any person who transfers this source code or any derivative work */
-/*	 must include the IBM copyright notice, this paragraph, and the */
-/*	 preceding two paragraphs in the transferred software. */
-/* */
-/*	 COPYRIGHT   I B M   CORPORATION 1995 */
-/*	 LICENSED MATERIAL  -  PROGRAM PROPERTY OF I B M */
-/*------------------------------------------------------------------------------- */
+/*------------------------------------------------------------------------------+
+ *
+ *	 This source code has been made available to you by IBM on an AS-IS
+ *	 basis.	 Anyone receiving this source is licensed under IBM
+ *	 copyrights to use it in any way he or she deems fit, including
+ *	 copying it, modifying it, compiling it, and redistributing it either
+ *	 with or without modifications.	 No license under IBM patents or
+ *	 patent applications is to be implied by the copyright license.
+ *
+ *	 Any user of this software should understand that IBM cannot provide
+ *	 technical support for this software and will not be responsible for
+ *	 any consequences resulting from the use of this software.
+ *
+ *	 Any person who transfers this source code or any derivative work
+ *	 must include the IBM copyright notice, this paragraph, and the
+ *	 preceding two paragraphs in the transferred software.
+ *
+ *	 COPYRIGHT   I B M   CORPORATION 1995
+ *	 LICENSED MATERIAL  -  PROGRAM PROPERTY OF I B M
+ *-------------------------------------------------------------------------------
+ */
 
 /*  U-Boot - Startup Code for AMCC 4xx PowerPC based Embedded Boards
  *
@@ -59,7 +60,6 @@
  *  address and (s)dram will be positioned at address 0
  */
 #include <config.h>
-#include <mpc8xx.h>
 #include <ppc4xx.h>
 #include <version.h>
 
@@ -110,6 +110,13 @@
 # endif
 #endif /* CFG_INIT_DCACHE_CS */
 
+#define function_prolog(func_name)	.text; \
+					.align 2; \
+					.globl func_name; \
+					func_name:
+#define function_epilog(func_name)	.type func_name,@function; \
+					.size func_name,.-func_name
+
 /* We don't want the  MMU yet.
 */
 #undef	MSR_KERNEL
@@ -148,7 +155,9 @@
 	 * NAND U-Boot image is started from offset 0
 	 */
 	.text
+#if defined(CONFIG_440)
 	bl	reconfig_tlb0
+#endif
 	GET_GOT
 	bl	cpu_init_f	/* run low-level CPU init code	   (from Flash) */
 	bl	board_init_f
@@ -285,11 +294,13 @@ skip_debug_init:
 	mtspr	ivor7,r1	/* Floating point unavailable */
 	li	r1,0x0c00
 	mtspr	ivor8,r1	/* System call */
-	li	r1,0x1000
-	mtspr	ivor10,r1	/* Decrementer (PIT for 440) */
-	li	r1,0x1400
-	mtspr	ivor13,r1	/* Data TLB error */
+	li	r1,0x0a00
+	mtspr	ivor9,r1	/* Auxiliary Processor unavailable */
+	li	r1,0x0900
+	mtspr	ivor10,r1	/* Decrementer */
 	li	r1,0x1300
+	mtspr	ivor13,r1	/* Data TLB error */
+	li	r1,0x1400
 	mtspr	ivor14,r1	/* Instr TLB error */
 	li	r1,0x2000
 	mtspr	ivor15,r1	/* Debug */
@@ -388,8 +399,9 @@ rsttlb:	tlbwe	r0,r1,0x0000	/* Invalidate all entries (V=0)*/
 2:
 
 #if defined(CONFIG_NAND_SPL)
+#if defined(CONFIG_440EPX) || defined(CONFIG_440GRX)
 	/*
-	 * Enable internal SRAM
+	 * Enable internal SRAM (only on 440EPx/GRx, 440EP/GR have no OCM)
 	 */
 	lis	r2,0x7fff
 	ori	r2,r2,0xffff
@@ -399,6 +411,45 @@ rsttlb:	tlbwe	r0,r1,0x0000	/* Invalidate all entries (V=0)*/
 	mfdcr	r1,isram0_pmeg
 	and	r1,r1,r2		/* Disable pwr mgmt */
 	mtdcr	isram0_pmeg,r1
+#endif
+#if defined(CONFIG_440EP)
+	/*
+	 * On 440EP with no internal SRAM, we setup SDRAM very early
+	 * and copy the NAND_SPL to SDRAM and jump to it
+	 */
+	/* Clear Dcache to use as RAM */
+	addis	r3,r0,CFG_INIT_RAM_ADDR@h
+	ori	r3,r3,CFG_INIT_RAM_ADDR@l
+	addis	r4,r0,CFG_INIT_RAM_END@h
+	ori	r4,r4,CFG_INIT_RAM_END@l
+	rlwinm. r5,r4,0,27,31
+	rlwinm	r5,r4,27,5,31
+	beq	..d_ran3
+	addi	r5,r5,0x0001
+..d_ran3:
+	mtctr	r5
+..d_ag3:
+	dcbz	r0,r3
+	addi	r3,r3,32
+	bdnz	..d_ag3
+	/*----------------------------------------------------------------*/
+	/* Setup the stack in internal SRAM */
+	/*----------------------------------------------------------------*/
+	lis	r1,CFG_INIT_RAM_ADDR@h
+	ori	r1,r1,CFG_INIT_SP_OFFSET@l
+	li	r0,0
+	stwu	r0,-4(r1)
+	stwu	r0,-4(r1)		/* Terminate call chain */
+
+	stwu	r1,-8(r1)		/* Save back chain and move SP */
+	lis	r0,RESET_VECTOR@h	/* Address of reset vector */
+	ori	r0,r0, RESET_VECTOR@l
+	stwu	r1,-8(r1)		/* Save back chain and move SP */
+	stw	r0,+12(r1)		/* Save return addr (underflow vect) */
+	sync
+	bl	early_sdram_init
+	sync
+#endif /* CONFIG_440EP */
 
 	/*
 	 * Copy SPL from cache into internal SRAM
@@ -429,7 +480,7 @@ spl_loop:
 start_ram:
 	sync
 	isync
-#endif
+#endif /* CONFIG_NAND_SPL */
 
 	bl	3f
 	b	_start
@@ -454,11 +505,81 @@ version_string:
 	.ascii " (", __DATE__, " - ", __TIME__, ")"
 	.ascii CONFIG_IDENT_STRING, "\0"
 
-/*
- * Maybe this should be moved somewhere else because the current
- * location (0x100) is where the CriticalInput Execption should be.
- */
 	. = EXC_OFF_SYS_RESET
+	.globl	_start_of_vectors
+_start_of_vectors:
+
+/* Critical input. */
+	CRIT_EXCEPTION(0x100, CritcalInput, UnknownException)
+
+#ifdef CONFIG_440
+/* Machine check */
+	MCK_EXCEPTION(0x200, MachineCheck, MachineCheckException)
+#else
+	CRIT_EXCEPTION(0x200, MachineCheck, MachineCheckException)
+#endif /* CONFIG_440 */
+
+/* Data Storage exception. */
+	STD_EXCEPTION(0x300, DataStorage, UnknownException)
+
+/* Instruction Storage exception. */
+	STD_EXCEPTION(0x400, InstStorage, UnknownException)
+
+/* External Interrupt exception. */
+	STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
+
+/* Alignment exception. */
+	. = 0x600
+Alignment:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	mfspr	r4,DAR
+	stw	r4,_DAR(r21)
+	mfspr	r5,DSISR
+	stw	r5,_DSISR(r21)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_Alignment:
+	.long	AlignmentException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
+
+/* Program check exception */
+	. = 0x700
+ProgramCheck:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_ProgramCheck:
+	.long	ProgramCheckException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
+
+#ifdef CONFIG_440
+	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
+	STD_EXCEPTION(0x900, Decrementer, DecrementerPITException)
+	STD_EXCEPTION(0xa00, APU, UnknownException)
+#endif
+	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
+
+#ifdef CONFIG_440
+	STD_EXCEPTION(0x1300, DataTLBError, UnknownException)
+	STD_EXCEPTION(0x1400, InstructionTLBError, UnknownException)
+#else
+	STD_EXCEPTION(0x1000, PIT, DecrementerPITException)
+	STD_EXCEPTION(0x1100, InstructionTLBMiss, UnknownException)
+	STD_EXCEPTION(0x1200, DataTLBMiss, UnknownException)
+#endif
+	CRIT_EXCEPTION(0x2000, DebugBreakpoint, DebugException )
+
+	.globl	_end_of_vectors
+_end_of_vectors:
+	. = _START_OFFSET
 #endif
 	.globl	_start
 _start:
@@ -768,22 +889,22 @@ _start:
 	 */
 	lis	r3,CFG_OCM_DATA_ADDR@h	/* OCM location */
 	ori	r3,r3,CFG_OCM_DATA_ADDR@l
-	ori	r3,r3,0x8270	/* 32K Offset, 16K for Bank 1, R/W/Enable */
+	ori	r3,r3,0x0270		/* 16K for Bank 1, R/W/Enable */
 	mtdcr	ocmplb3cr1,r3		/* Set PLB Access */
 	ori	r3,r3,0x4000		/* Add 0x4000 for bank 2 */
 	mtdcr	ocmplb3cr2,r3		/* Set PLB Access */
 	isync
 
-	lis	r3,CFG_OCM_DATA_ADDR@h  /* OCM location */
+	lis	r3,CFG_OCM_DATA_ADDR@h	/* OCM location */
 	ori	r3,r3,CFG_OCM_DATA_ADDR@l
-	ori	r3,r3,0x0270            /* 16K for Bank 1, R/W/Enable */
-	mtdcr	ocmdscr1, r3            /* Set Data Side */
-	mtdcr	ocmiscr1, r3            /* Set Instruction Side */
+	ori	r3,r3,0x0270		/* 16K for Bank 1, R/W/Enable */
+	mtdcr	ocmdscr1, r3		/* Set Data Side */
+	mtdcr	ocmiscr1, r3		/* Set Instruction Side */
 	ori	r3,r3,0x4000		/* Add 0x4000 for bank 2 */
-	mtdcr	ocmdscr2, r3            /* Set Data Side */
-	mtdcr	ocmiscr2, r3            /* Set Instruction Side */
-	addis	r3,0,0x0800             /* OCM Data Parity Disable - 1 Wait State */
-	mtdcr	ocmdsisdpc,r4
+	mtdcr	ocmdscr2, r3		/* Set Data Side */
+	mtdcr	ocmiscr2, r3		/* Set Instruction Side */
+	addis	r3,0,0x0800		/* OCM Data Parity Disable - 1 Wait State */
+	mtdcr	ocmdsisdpc,r3
 
 	isync
 #else /* CONFIG_405EZ */
@@ -801,7 +922,7 @@ _start:
 	mtdcr	ocmdscntl, r4		/* set data-side IRAM config */
 	isync
 
-	lis	r3,CFG_OCM_DATA_ADDR@h  /* OCM location */
+	lis	r3,CFG_OCM_DATA_ADDR@h	/* OCM location */
 	ori	r3,r3,CFG_OCM_DATA_ADDR@l
 	mtdcr	ocmdsarc, r3
 	addis	r4, 0, 0xC000		/* OCM data area enabled */
@@ -810,6 +931,38 @@ _start:
 #endif /* CONFIG_405EZ */
 #endif
 
+#ifdef CONFIG_NAND_SPL
+	/*
+	 * Copy SPL from cache into internal SRAM
+	 */
+	li	r4,(CFG_NAND_BOOT_SPL_SIZE >> 2) - 1
+	mtctr	r4
+	lis	r2,CFG_NAND_BOOT_SPL_SRC@h
+	ori	r2,r2,CFG_NAND_BOOT_SPL_SRC@l
+	lis	r3,CFG_NAND_BOOT_SPL_DST@h
+	ori	r3,r3,CFG_NAND_BOOT_SPL_DST@l
+spl_loop:
+	lwzu	r4,4(r2)
+	stwu	r4,4(r3)
+	bdnz	spl_loop
+
+	/*
+	 * Jump to code in RAM
+	 */
+	bl	00f
+00:	mflr	r10
+	lis	r3,(CFG_NAND_BOOT_SPL_SRC - CFG_NAND_BOOT_SPL_DST)@h
+	ori	r3,r3,(CFG_NAND_BOOT_SPL_SRC - CFG_NAND_BOOT_SPL_DST)@l
+	sub	r10,r10,r3
+	addi	r10,r10,28
+	mtlr	r10
+	blr
+
+start_ram:
+	sync
+	isync
+#endif /* CONFIG_NAND_SPL */
+
 	/*----------------------------------------------------------------------- */
 	/* Setup temporary stack in DCACHE or OCM if needed for SDRAM SPD. */
 	/*----------------------------------------------------------------------- */
@@ -920,119 +1073,22 @@ _start:
 	stw	r0, +12(r1)		/* Save return addr (underflow vect) */
 #endif /* !(CFG_INIT_DCACHE_CS	|| !CFG_TEM_STACK_OCM) */
 
+#ifdef CONFIG_NAND_SPL
+	bl	nand_boot		/* will not return */
+#else
 	GET_GOT			/* initialize GOT access			*/
 
 	bl	cpu_init_f	/* run low-level CPU init code	   (from Flash) */
 
 	/* NEVER RETURNS! */
 	bl	board_init_f	/* run first part of init code (from Flash)	*/
+#endif /* CONFIG_NAND_SPL */
 
 #endif	/* CONFIG_405GP || CONFIG_405CR || CONFIG_405 || CONFIG_405EP */
 	/*----------------------------------------------------------------------- */
 
 
 #ifndef CONFIG_NAND_SPL
-/*****************************************************************************/
-	.globl	_start_of_vectors
-_start_of_vectors:
-
-#if 0
-/*TODO Fixup _start above so we can do this*/
-/* Critical input. */
-	CRIT_EXCEPTION(0x100, CritcalInput, CritcalInputException)
-#endif
-
-/* Machine check */
-	CRIT_EXCEPTION(0x200, MachineCheck, MachineCheckException)
-
-/* Data Storage exception. */
-	STD_EXCEPTION(0x300, DataStorage, UnknownException)
-
-/* Instruction Storage exception. */
-	STD_EXCEPTION(0x400, InstStorage, UnknownException)
-
-/* External Interrupt exception. */
-	STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
-
-/* Alignment exception. */
-	. = 0x600
-Alignment:
-	EXCEPTION_PROLOG
-	mfspr	r4,DAR
-	stw	r4,_DAR(r21)
-	mfspr	r5,DSISR
-	stw	r5,_DSISR(r21)
-	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
-
-/* Program check exception */
-	. = 0x700
-ProgramCheck:
-	EXCEPTION_PROLOG
-	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
-
-	/* No FPU on MPC8xx.  This exception is not supposed to happen.
-	*/
-	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
-
-	/* I guess we could implement decrementer, and may have
-	 * to someday for timekeeping.
-	 */
-	STD_EXCEPTION(0x900, Decrementer, timer_interrupt)
-	STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
-	STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
-	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
-	STD_EXCEPTION(0xd00, SingleStep, UnknownException)
-
-	STD_EXCEPTION(0xe00, Trap_0e, UnknownException)
-	STD_EXCEPTION(0xf00, Trap_0f, UnknownException)
-
-	/* On the MPC8xx, this is a software emulation interrupt.  It occurs
-	 * for all unimplemented and illegal instructions.
-	 */
-	STD_EXCEPTION(0x1000, PIT, PITException)
-
-	STD_EXCEPTION(0x1100, InstructionTLBMiss, UnknownException)
-	STD_EXCEPTION(0x1200, DataTLBMiss, UnknownException)
-	STD_EXCEPTION(0x1300, InstructionTLBError, UnknownException)
-	STD_EXCEPTION(0x1400, DataTLBError, UnknownException)
-
-	STD_EXCEPTION(0x1500, Reserved5, UnknownException)
-	STD_EXCEPTION(0x1600, Reserved6, UnknownException)
-	STD_EXCEPTION(0x1700, Reserved7, UnknownException)
-	STD_EXCEPTION(0x1800, Reserved8, UnknownException)
-	STD_EXCEPTION(0x1900, Reserved9, UnknownException)
-	STD_EXCEPTION(0x1a00, ReservedA, UnknownException)
-	STD_EXCEPTION(0x1b00, ReservedB, UnknownException)
-
-	STD_EXCEPTION(0x1c00, DataBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1d00, InstructionBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1e00, PeripheralBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1f00, DevPortBreakpoint, UnknownException)
-
-	CRIT_EXCEPTION(0x2000, DebugBreakpoint, DebugException )
-
-	.globl	_end_of_vectors
-_end_of_vectors:
-
-
-	. = 0x2100
-
 /*
  * This code finishes saving the registers to the exception frame
  * and jumps to the appropriate handler for the exception.
@@ -1048,28 +1104,12 @@ transfer_to_handler:
 	SAVE_4GPRS(8, r21)
 	SAVE_8GPRS(12, r21)
 	SAVE_8GPRS(24, r21)
-#if 0
-	andi.	r23,r23,MSR_PR
-	mfspr	r23,SPRG3		/* if from user, fix up tss.regs */
-	beq	2f
-	addi	r24,r1,STACK_FRAME_OVERHEAD
-	stw	r24,PT_REGS(r23)
-2:	addi	r2,r23,-TSS		/* set r2 to current */
-	tovirt(r2,r2,r23)
-#endif
 	mflr	r23
 	andi.	r24,r23,0x3f00		/* get vector offset */
 	stw	r24,TRAP(r21)
 	li	r22,0
 	stw	r22,RESULT(r21)
 	mtspr	SPRG2,r22		/* r1 is now kernel sp */
-#if 0
-	addi	r24,r2,TASK_STRUCT_SIZE /* check for kernel stack overflow */
-	cmplw	0,r1,r2
-	cmplw	1,r1,r24
-	crand	1,1,4
-	bgt	stack_ovf		/* if r2 < r1 < r2+TASK_STRUCT_SIZE */
-#endif
 	lwz	r24,0(r23)		/* virtual address of handler */
 	lwz	r23,4(r23)		/* where to go when done */
 	mtspr	SRR0,r24
@@ -1130,34 +1170,72 @@ crit_return:
 	REST_GPR(31, r1)
 	lwz	r2,_NIP(r1)	/* Restore environment */
 	lwz	r0,_MSR(r1)
-	mtspr	990,r2		/* SRR2 */
-	mtspr	991,r0		/* SRR3 */
+	mtspr	csrr0,r2
+	mtspr	csrr1,r0
 	lwz	r0,GPR0(r1)
 	lwz	r2,GPR2(r1)
 	lwz	r1,GPR1(r1)
 	SYNC
 	rfci
-#endif /* CONFIG_NAND_SPL */
 
-/* Cache functions.
-*/
-invalidate_icache:
-	iccci	r0,r0			/* for 405, iccci invalidates the */
-	blr				/*   entire I cache */
+#ifdef CONFIG_440
+mck_return:
+	mfmsr	r28		/* Disable interrupts */
+	li	r4,0
+	ori	r4,r4,MSR_EE
+	andc	r28,r28,r4
+	SYNC			/* Some chip revs need this... */
+	mtmsr	r28
+	SYNC
+	lwz	r2,_CTR(r1)
+	lwz	r0,_LINK(r1)
+	mtctr	r2
+	mtlr	r0
+	lwz	r2,_XER(r1)
+	lwz	r0,_CCR(r1)
+	mtspr	XER,r2
+	mtcrf	0xFF,r0
+	REST_10GPRS(3, r1)
+	REST_10GPRS(13, r1)
+	REST_8GPRS(23, r1)
+	REST_GPR(31, r1)
+	lwz	r2,_NIP(r1)	/* Restore environment */
+	lwz	r0,_MSR(r1)
+	mtspr	mcsrr0,r2
+	mtspr	mcsrr1,r0
+	lwz	r0,GPR0(r1)
+	lwz	r2,GPR2(r1)
+	lwz	r1,GPR1(r1)
+	SYNC
+	rfmci
+#endif /* CONFIG_440 */
 
-invalidate_dcache:
-	addi	r6,0,0x0000		/* clear GPR 6 */
-	/* Do loop for # of dcache congruence classes. */
-	lis	r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@ha	/* TBS for large sized cache */
-	ori	r7, r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@l
-					/* NOTE: dccci invalidates both */
-	mtctr	r7			/* ways in the D cache */
-..dcloop:
-	dccci	0,r6			/* invalidate line */
-	addi	r6,r6, CFG_CACHELINE_SIZE /* bump to next line */
-	bdnz	..dcloop
+
+/*
+ * Cache functions.
+ *
+ * NOTE: currently the 440s run with dcache _disabled_ once relocated to DRAM,
+ * although for some cache-ralated calls stubs have to be provided to satisfy
+ * symbols resolution.
+ * Icache-related functions are used in POST framework.
+ *
+ */
+#ifdef CONFIG_440
+       .globl  dcache_disable
+       .globl  icache_disable
+       .globl  icache_enable
+dcache_disable:
+icache_disable:
+icache_enable:
 	blr
 
+	.globl	dcache_status
+	.globl	icache_status
+dcache_status:
+icache_status:
+	mr	r3,  0
+	blr
+#else
 flush_dcache:
 	addis	r9,r0,0x0002		/* set mask for EE and CE msr bits */
 	ori	r9,r9,0x8000
@@ -1236,42 +1314,13 @@ dcache_status:
 	mfdccr	r3
 	srwi	r3, r3, 31	/* >>31 => select bit 0 */
 	blr
+#endif
 
 	.globl get_pvr
 get_pvr:
 	mfspr	r3, PVR
 	blr
 
-#if !defined(CONFIG_440)
-	.globl wr_pit
-wr_pit:
-	mtspr	pit, r3
-	blr
-#endif
-
-	.globl wr_tcr
-wr_tcr:
-	mtspr	tcr, r3
-	blr
-
-/*------------------------------------------------------------------------------- */
-/* Function:	 in8 */
-/* Description:	 Input 8 bits */
-/*------------------------------------------------------------------------------- */
-	.globl	in8
-in8:
-	lbz	r3,0x0000(r3)
-	blr
-
-/*------------------------------------------------------------------------------- */
-/* Function:	 out8 */
-/* Description:	 Output 8 bits */
-/*------------------------------------------------------------------------------- */
-	.globl	out8
-out8:
-	stb	r4,0x0000(r3)
-	blr
-
 /*------------------------------------------------------------------------------- */
 /* Function:	 out16 */
 /* Description:	 Output 16 bits */
@@ -1291,15 +1340,6 @@ out16r:
 	blr
 
 /*------------------------------------------------------------------------------- */
-/* Function:	 out32 */
-/* Description:	 Output 32 bits */
-/*------------------------------------------------------------------------------- */
-	.globl	out32
-out32:
-	stw	r4,0x0000(r3)
-	blr
-
-/*------------------------------------------------------------------------------- */
 /* Function:	 out32r */
 /* Description:	 Byte reverse and output 32 bits */
 /*------------------------------------------------------------------------------- */
@@ -1327,15 +1367,6 @@ in16r:
 	blr
 
 /*------------------------------------------------------------------------------- */
-/* Function:	 in32 */
-/* Description:	 Input 32 bits */
-/*------------------------------------------------------------------------------- */
-	.globl	in32
-in32:
-	lwz	3,0x0000(3)
-	blr
-
-/*------------------------------------------------------------------------------- */
 /* Function:	 in32r */
 /* Description:	 Input 32 bits and byte reverse */
 /*------------------------------------------------------------------------------- */
@@ -1377,9 +1408,6 @@ ppcSync:
 	sync
 	blr
 
-/*------------------------------------------------------------------------------*/
-
-#ifndef CONFIG_NAND_SPL
 /*
  * void relocate_code (addr_sp, gd, addr_moni)
  *
@@ -1490,7 +1518,7 @@ relocate_code:
  * initialization, now running from RAM.
  */
 
-	addi	r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
+	addi	r0, r10, in_ram - _start + _START_OFFSET
 	mtlr	r0
 	blr				/* NEVER RETURNS! */
 
@@ -1560,7 +1588,7 @@ clear_bss:
 	 */
 	.globl	trap_init
 trap_init:
-	lwz	r7, GOT(_start)
+	lwz	r7, GOT(_start_of_vectors)
 	lwz	r8, GOT(_end_of_vectors)
 
 	li	r9, 0x100		/* reset vector always at 0x100 */
@@ -1580,35 +1608,48 @@ trap_init:
 	/*
 	 * relocate `hdlr' and `int_return' entries
 	 */
-	li	r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
-	li	r8, Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_MachineCheck - _start + _START_OFFSET
+	li	r8, Alignment - _start + _START_OFFSET
 2:
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
+	addi	r7, r7, 0x100		/* next exception vector */
 	cmplw	0, r7, r8
 	blt	2b
 
-	li	r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_Alignment - _start + _START_OFFSET
 	bl	trap_reloc
 
-	li	r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_ProgramCheck - _start + _START_OFFSET
 	bl	trap_reloc
 
-	li	r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
-	li	r8, SystemCall - _start + EXC_OFF_SYS_RESET
-3:
+#ifdef CONFIG_440
+	li	r7, .L_FPUnavailable - _start + _START_OFFSET
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
-	blt	3b
 
-	li	r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
-	li	r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
-4:
+	li	r7, .L_Decrementer - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_APU - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_InstructionTLBError - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_DataTLBError - _start + _START_OFFSET
+	bl	trap_reloc
+#else /* CONFIG_440 */
+	li	r7, .L_PIT - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_InstructionTLBMiss - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_DataTLBMiss - _start + _START_OFFSET
+	bl	trap_reloc
+#endif /* CONFIG_440 */
+
+	li	r7, .L_DebugBreakpoint - _start + _START_OFFSET
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
-	blt	4b
 
 #if !defined(CONFIG_440)
 	addi	r7,r0,0x1000		/* set ME bit (Machine Exceptions) */
@@ -1644,8 +1685,105 @@ trap_reloc:
 	stw	r0, 4(r7)
 
 	blr
+
+#if defined(CONFIG_440)
+/*----------------------------------------------------------------------------+
+| dcbz_area.
++----------------------------------------------------------------------------*/
+	function_prolog(dcbz_area)
+	rlwinm. r5,r4,0,27,31
+	rlwinm	r5,r4,27,5,31
+	beq	..d_ra2
+	addi	r5,r5,0x0001
+..d_ra2:mtctr	r5
+..d_ag2:dcbz	r0,r3
+	addi	r3,r3,32
+	bdnz	..d_ag2
+	sync
+	blr
+	function_epilog(dcbz_area)
+
+/*----------------------------------------------------------------------------+
+| dflush.  Assume 32K at vector address is cachable.
++----------------------------------------------------------------------------*/
+	function_prolog(dflush)
+	mfmsr	r9
+	rlwinm	r8,r9,0,15,13
+	rlwinm	r8,r8,0,17,15
+	mtmsr	r8
+	addi	r3,r0,0x0000
+	mtspr	dvlim,r3
+	mfspr	r3,ivpr
+	addi	r4,r0,1024
+	mtctr	r4
+..dflush_loop:
+	lwz	r6,0x0(r3)
+	addi	r3,r3,32
+	bdnz	..dflush_loop
+	addi	r3,r3,-32
+	mtctr	r4
+..ag:	dcbf	r0,r3
+	addi	r3,r3,-32
+	bdnz	..ag
+	sync
+	mtmsr	r9
+	blr
+	function_epilog(dflush)
+#endif /* CONFIG_440 */
 #endif /* CONFIG_NAND_SPL */
 
+/*------------------------------------------------------------------------------- */
+/* Function:	 in8 */
+/* Description:	 Input 8 bits */
+/*------------------------------------------------------------------------------- */
+	.globl	in8
+in8:
+	lbz	r3,0x0000(r3)
+	blr
+
+/*------------------------------------------------------------------------------- */
+/* Function:	 out8 */
+/* Description:	 Output 8 bits */
+/*------------------------------------------------------------------------------- */
+	.globl	out8
+out8:
+	stb	r4,0x0000(r3)
+	blr
+
+/*------------------------------------------------------------------------------- */
+/* Function:	 out32 */
+/* Description:	 Output 32 bits */
+/*------------------------------------------------------------------------------- */
+	.globl	out32
+out32:
+	stw	r4,0x0000(r3)
+	blr
+
+/*------------------------------------------------------------------------------- */
+/* Function:	 in32 */
+/* Description:	 Input 32 bits */
+/*------------------------------------------------------------------------------- */
+	.globl	in32
+in32:
+	lwz	3,0x0000(3)
+	blr
+
+invalidate_icache:
+	iccci	r0,r0			/* for 405, iccci invalidates the */
+	blr				/*   entire I cache */
+
+invalidate_dcache:
+	addi	r6,0,0x0000		/* clear GPR 6 */
+	/* Do loop for # of dcache congruence classes. */
+	lis	r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@ha	/* TBS for large sized cache */
+	ori	r7, r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@l
+					/* NOTE: dccci invalidates both */
+	mtctr	r7			/* ways in the D cache */
+..dcloop:
+	dccci	0,r6			/* invalidate line */
+	addi	r6,r6, CFG_CACHELINE_SIZE /* bump to next line */
+	bdnz	..dcloop
+	blr
 
 /**************************************************************************/
 /* PPC405EP specific stuff						  */
@@ -1732,28 +1870,6 @@ ppc405ep_init:
 	mtdcr	ebccfgd,r3
 #endif
 
-#ifndef CFG_CPC0_PCI
-	li	r3,CPC0_PCI_HOST_CFG_EN
-#ifdef CONFIG_BUBINGA
-	/*
-	!-----------------------------------------------------------------------
-	! Check FPGA for PCI internal/external arbitration
-	!   If board is set to internal arbitration, update cpc0_pci
-	!-----------------------------------------------------------------------
-	*/
-	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
-#else /* CFG_CPC0_PCI */
-	li	r3,CFG_CPC0_PCI
-#endif /* CFG_CPC0_PCI */
-..pci_cfg_set:
-	mtdcr	CPC0_PCI, r3		 /* Enable internal arbiter*/
-
 	/*
 	!-----------------------------------------------------------------------
 	! Check to see if chip is in bypass mode.
@@ -1809,11 +1925,50 @@ ppc405ep_init:
 ..no_pllset:
 #endif /* CONFIG_BUBINGA */
 
+#ifdef CONFIG_TAIHU
+	mfdcr	r4, CPC0_BOOT
+	andi.	r5, r4, CPC0_BOOT_SEP@l
+	bne	strap_1			/* serial eeprom present */
+	addis	r5,0,CPLD_REG0_ADDR@h
+	ori	r5,r5,CPLD_REG0_ADDR@l
+	andi.	r5, r5, 0x10
+	bne	_pci_66mhz
+#endif /* CONFIG_TAIHU */
+
+#if defined(CONFIG_ZEUS)
+	mfdcr	r4, CPC0_BOOT
+	andi.	r5, r4, CPC0_BOOT_SEP@l
+	bne	strap_1  	/* serial eeprom present */
+	lis	r3,0x0000
+	addi	r3,r3,0x3030
+	lis	r4,0x8042
+	addi	r4,r4,0x223e
+	b	1f
+strap_1:
+	mfdcr	r3, CPC0_PLLMR0
+	mfdcr	r4, CPC0_PLLMR1
+	b	1f
+#endif
+
 	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	   /* */
 
+#ifdef CONFIG_TAIHU
+	b	1f
+_pci_66mhz:
+	addis	r3,0,PLLMR0_DEFAULT_PCI66@h
+	ori	r3,r3,PLLMR0_DEFAULT_PCI66@l
+	addis	r4,0,PLLMR1_DEFAULT_PCI66@h
+	ori	r4,r4,PLLMR1_DEFAULT_PCI66@l
+	b	1f
+strap_1:
+	mfdcr	r3, CPC0_PLLMR0
+	mfdcr	r4, CPC0_PLLMR1
+#endif /* CONFIG_TAIHU */
+
+1:
 	b	pll_write		  /* Write the CPC0_PLLMR with new value */
 
 pll_done:
@@ -1892,13 +2047,6 @@ pll_wait:
 #endif /* CONFIG_405EP */
 
 #if defined(CONFIG_440)
-#define function_prolog(func_name)	.text; \
-					.align 2; \
-					.globl func_name; \
-					func_name:
-#define function_epilog(func_name)	.type func_name,@function; \
-					.size func_name,.-func_name
-
 /*----------------------------------------------------------------------------+
 | mttlb3.
 +----------------------------------------------------------------------------*/
@@ -1946,47 +2094,4 @@ pll_wait:
 	TLBRE(3,3,0)
 	blr
 	function_epilog(mftlb1)
-
-/*----------------------------------------------------------------------------+
-| dcbz_area.
-+----------------------------------------------------------------------------*/
-	function_prolog(dcbz_area)
-	rlwinm. r5,r4,0,27,31
-	rlwinm	r5,r4,27,5,31
-	beq	..d_ra2
-	addi	r5,r5,0x0001
-..d_ra2:mtctr	r5
-..d_ag2:dcbz	r0,r3
-	addi	r3,r3,32
-	bdnz	..d_ag2
-	sync
-	blr
-	function_epilog(dcbz_area)
-
-/*----------------------------------------------------------------------------+
-| dflush.  Assume 32K at vector address is cachable.
-+----------------------------------------------------------------------------*/
-	function_prolog(dflush)
-	mfmsr	r9
-	rlwinm	r8,r9,0,15,13
-	rlwinm	r8,r8,0,17,15
-	mtmsr	r8
-	addi	r3,r0,0x0000
-	mtspr	dvlim,r3
-	mfspr	r3,ivpr
-	addi	r4,r0,1024
-	mtctr	r4
-..dflush_loop:
-	lwz	r6,0x0(r3)
-	addi	r3,r3,32
-	bdnz	..dflush_loop
-	addi	r3,r3,-32
-	mtctr	r4
-..ag:	dcbf	r0,r3
-	addi	r3,r3,-32
-	bdnz	..ag
-	sync
-	mtmsr	r9
-	blr
-	function_epilog(dflush)
 #endif /* CONFIG_440 */
diff --git a/cpu/ppc4xx/tlb.c b/cpu/ppc4xx/tlb.c
index 50344a4..098694c 100644
--- a/cpu/ppc4xx/tlb.c
+++ b/cpu/ppc4xx/tlb.c
@@ -25,7 +25,6 @@
 
 #if defined(CONFIG_440)
 
-#include <ppc4xx.h>
 #include <ppc440.h>
 #include <asm/io.h>
 #include <asm/mmu.h>
@@ -36,7 +35,69 @@ typedef struct region {
 	unsigned long tlb_word2_i_value;
 } region_t;
 
-static int add_tlb_entry(unsigned long base_addr,
+void remove_tlb(u32 vaddr, u32 size)
+{
+	int i;
+	u32 tlb_word0_value;
+	u32 tlb_vaddr;
+	u32 tlb_size = 0;
+
+	/* First, find the index of a TLB entry not being used */
+	for (i=0; i<PPC4XX_TLB_SIZE; i++) {
+		tlb_word0_value = mftlb1(i);
+		tlb_vaddr = TLB_WORD0_EPN_DECODE(tlb_word0_value);
+		if (((tlb_word0_value & TLB_WORD0_V_MASK) == TLB_WORD0_V_ENABLE) &&
+		    (tlb_vaddr >= vaddr)) {
+			/*
+			 * TLB is enabled and start address is lower or equal
+			 * than the area we are looking for. Now we only have
+			 * to check the size/end address for a match.
+			 */
+			switch (tlb_word0_value & TLB_WORD0_SIZE_MASK) {
+			case TLB_WORD0_SIZE_1KB:
+				tlb_size = 1 << 10;
+				break;
+			case TLB_WORD0_SIZE_4KB:
+				tlb_size = 4 << 10;
+				break;
+			case TLB_WORD0_SIZE_16KB:
+				tlb_size = 16 << 10;
+				break;
+			case TLB_WORD0_SIZE_64KB:
+				tlb_size = 64 << 10;
+				break;
+			case TLB_WORD0_SIZE_256KB:
+				tlb_size = 256 << 10;
+				break;
+			case TLB_WORD0_SIZE_1MB:
+				tlb_size = 1 << 20;
+				break;
+			case TLB_WORD0_SIZE_16MB:
+				tlb_size = 16 << 20;
+				break;
+			case TLB_WORD0_SIZE_256MB:
+				tlb_size = 256 << 20;
+				break;
+			}
+
+			/*
+			 * Now check the end-address if it's in the range
+			 */
+			if ((tlb_vaddr + tlb_size - 1) <= (vaddr + size - 1))
+				/*
+				 * Found a TLB in the range.
+				 * Disable it by writing 0 to tlb0 word.
+				 */
+				mttlb1(i, 0);
+		}
+	}
+
+	/* Execute an ISYNC instruction so that the new TLB entry takes effect */
+	asm("isync");
+}
+
+static int add_tlb_entry(unsigned long phys_addr,
+			 unsigned long virt_addr,
 			 unsigned long tlb_word0_size_value,
 			 unsigned long tlb_word2_i_value)
 {
@@ -55,9 +116,9 @@ static int add_tlb_entry(unsigned long base_addr,
 		return -1;
 
 	/* Second, create the TLB entry */
-	tlb_word0_value = TLB_WORD0_EPN_ENCODE(base_addr) | TLB_WORD0_V_ENABLE |
+	tlb_word0_value = TLB_WORD0_EPN_ENCODE(virt_addr) | TLB_WORD0_V_ENABLE |
 		TLB_WORD0_TS_0 | tlb_word0_size_value;
-	tlb_word1_value = TLB_WORD1_RPN_ENCODE(base_addr) | TLB_WORD1_ERPN_ENCODE(0);
+	tlb_word1_value = TLB_WORD1_RPN_ENCODE(phys_addr) | TLB_WORD1_ERPN_ENCODE(0);
 	tlb_word2_value = TLB_WORD2_U0_DISABLE | TLB_WORD2_U1_DISABLE |
 		TLB_WORD2_U2_DISABLE | TLB_WORD2_U3_DISABLE |
 		TLB_WORD2_W_DISABLE | tlb_word2_i_value |
@@ -81,7 +142,9 @@ static int add_tlb_entry(unsigned long base_addr,
 	return 0;
 }
 
-static void program_tlb_addr(unsigned long base_addr, unsigned long mem_size,
+static void program_tlb_addr(unsigned long phys_addr,
+			     unsigned long virt_addr,
+			     unsigned long mem_size,
 			     unsigned long tlb_word2_i_value)
 {
 	int rc;
@@ -91,70 +154,86 @@ static void program_tlb_addr(unsigned long base_addr, unsigned long mem_size,
 	while (mem_size != 0) {
 		rc = 0;
 		/* Add the TLB entries in to map the region. */
-		if (((base_addr & TLB_256MB_ALIGN_MASK) == base_addr) &&
+		if (((phys_addr & TLB_256MB_ALIGN_MASK) == phys_addr) &&
 		    (mem_size >= TLB_256MB_SIZE)) {
 			/* Add a 256MB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_256MB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_256MB, tlb_i)) == 0) {
 				mem_size -= TLB_256MB_SIZE;
-				base_addr += TLB_256MB_SIZE;
+				phys_addr += TLB_256MB_SIZE;
+				virt_addr += TLB_256MB_SIZE;
 			}
-		} else if (((base_addr & TLB_16MB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_16MB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_16MB_SIZE)) {
 			/* Add a 16MB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_16MB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_16MB, tlb_i)) == 0) {
 				mem_size -= TLB_16MB_SIZE;
-				base_addr += TLB_16MB_SIZE;
+				phys_addr += TLB_16MB_SIZE;
+				virt_addr += TLB_16MB_SIZE;
 			}
-		} else if (((base_addr & TLB_1MB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_1MB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_1MB_SIZE)) {
 			/* Add a 1MB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_1MB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_1MB, tlb_i)) == 0) {
 				mem_size -= TLB_1MB_SIZE;
-				base_addr += TLB_1MB_SIZE;
+				phys_addr += TLB_1MB_SIZE;
+				virt_addr += TLB_1MB_SIZE;
 			}
-		} else if (((base_addr & TLB_256KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_256KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_256KB_SIZE)) {
 			/* Add a 256KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_256KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_256KB, tlb_i)) == 0) {
 				mem_size -= TLB_256KB_SIZE;
-				base_addr += TLB_256KB_SIZE;
+				phys_addr += TLB_256KB_SIZE;
+				virt_addr += TLB_256KB_SIZE;
 			}
-		} else if (((base_addr & TLB_64KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_64KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_64KB_SIZE)) {
 			/* Add a 64KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_64KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_64KB, tlb_i)) == 0) {
 				mem_size -= TLB_64KB_SIZE;
-				base_addr += TLB_64KB_SIZE;
+				phys_addr += TLB_64KB_SIZE;
+				virt_addr += TLB_64KB_SIZE;
 			}
-		} else if (((base_addr & TLB_16KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_16KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_16KB_SIZE)) {
 			/* Add a 16KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_16KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_16KB, tlb_i)) == 0) {
 				mem_size -= TLB_16KB_SIZE;
-				base_addr += TLB_16KB_SIZE;
+				phys_addr += TLB_16KB_SIZE;
+				virt_addr += TLB_16KB_SIZE;
 			}
-		} else if (((base_addr & TLB_4KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_4KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_4KB_SIZE)) {
 			/* Add a 4KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_4KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_4KB, tlb_i)) == 0) {
 				mem_size -= TLB_4KB_SIZE;
-				base_addr += TLB_4KB_SIZE;
+				phys_addr += TLB_4KB_SIZE;
+				virt_addr += TLB_4KB_SIZE;
 			}
-		} else if (((base_addr & TLB_1KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_1KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_1KB_SIZE)) {
 			/* Add a 1KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_1KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_1KB, tlb_i)) == 0) {
 				mem_size -= TLB_1KB_SIZE;
-				base_addr += TLB_1KB_SIZE;
+				phys_addr += TLB_1KB_SIZE;
+				virt_addr += TLB_1KB_SIZE;
 			}
 		} else {
 			printf("ERROR: no TLB size exists for the base address 0x%0X.\n",
-				base_addr);
+				phys_addr);
 		}
 
 		if (rc != 0)
 			printf("ERROR: no TLB entries available for the base addr 0x%0X.\n",
-				base_addr);
+				phys_addr);
 	}
 
 	return;
@@ -166,16 +245,16 @@ static void program_tlb_addr(unsigned long base_addr, unsigned long mem_size,
  * Common usage for boards with SDRAM DIMM modules to dynamically
  * configure the TLB's for the SDRAM
  */
-void program_tlb(u32 start, u32 size, u32 tlb_word2_i_value)
+void program_tlb(u32 phys_addr, u32 virt_addr, u32 size, u32 tlb_word2_i_value)
 {
 	region_t region_array;
 
-	region_array.base = start;
+	region_array.base = phys_addr;
 	region_array.size = size;
 	region_array.tlb_word2_i_value = tlb_word2_i_value;	/* en-/disable cache */
 
 	/* Call the routine to add in the tlb entries for the memory regions */
-	program_tlb_addr(region_array.base, region_array.size,
+	program_tlb_addr(region_array.base, virt_addr, region_array.size,
 			 region_array.tlb_word2_i_value);
 
 	return;
diff --git a/cpu/ppc4xx/traps.c b/cpu/ppc4xx/traps.c
index 6aecca2..38b6f89 100644
--- a/cpu/ppc4xx/traps.c
+++ b/cpu/ppc4xx/traps.c
@@ -36,7 +36,9 @@
 #include <command.h>
 #include <asm/processor.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+DECLARE_GLOBAL_DATA_PTR;
+
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
@@ -45,8 +47,7 @@ extern unsigned long search_exception_table(unsigned long);
 
 /* THIS NEEDS CHANGING to use the board info structure.
  */
-#define END_OF_MEM	0x00400000
-
+#define END_OF_MEM	(gd->bd->bi_memstart + gd->bd->bi_memsize)
 
 static __inline__ void set_tsr(unsigned long val)
 {
@@ -77,7 +78,7 @@ static __inline__ unsigned long get_esr(void)
 #define ESR_DIZ 0x00400000
 #define ESR_U0F 0x00008000
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -110,7 +111,7 @@ void show_regs(struct pt_regs * regs)
 {
 	int i;
 
-	printf("NIP: %08lX XER: %08lX LR: %08lX REGS: %p TRAP: %04lx DAR: %08lX\n",
+	printf("NIP: %08lX XER: %08lX LR: %08lX REGS: %p TRAP: %04lx DEAR: %08lX\n",
 	       regs->nip, regs->xer, regs->link, regs, regs->trap, regs->dar);
 	printf("MSR: %08lx EE: %01x PR: %01x FP: %01x ME: %01x IR/DR: %01x%01x\n",
 	       regs->msr, regs->msr&MSR_EE ? 1 : 0, regs->msr&MSR_PR ? 1 : 0,
@@ -120,14 +121,12 @@ void show_regs(struct pt_regs * regs)
 
 	printf("\n");
 	for (i = 0;  i < 32;  i++) {
-		if ((i % 8) == 0)
-		{
+		if ((i % 8) == 0) {
 			printf("GPR%02d: ", i);
 		}
 
 		printf("%08lX ", regs->gpr[i]);
-		if ((i % 8) == 7)
-		{
+		if ((i % 8) == 7) {
 			printf("\n");
 		}
 	}
@@ -139,48 +138,166 @@ _exception(int signr, struct pt_regs *regs)
 {
 	show_regs(regs);
 	print_backtrace((unsigned long *)regs->gpr[1]);
-	panic("Exception in kernel pc %lx signal %d",regs->nip,signr);
+	panic("Exception");
 }
 
 void
 MachineCheckException(struct pt_regs *regs)
 {
-	unsigned long fixup;
+	unsigned long fixup, val;
+#if defined(CONFIG_440EPX) || defined(CONFIG_440GRX)
+	u32 value2;
+	int corr_ecc = 0;
+	int uncorr_ecc = 0;
+#endif
 
-	/* Probing PCI using config cycles cause this exception
-	 * when a device is not present.  Catch it and return to
-	 * the PCI exception handler.
-	 */
 	if ((fixup = search_exception_table(regs->nip)) != 0) {
 		regs->nip = fixup;
+		val = mfspr(MCSR);
+		/* Clear MCSR */
+		mtspr(SPRN_MCSR, val);
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
 
-	printf("Machine check in kernel mode.\n");
+	printf("Machine Check Exception.\n");
 	printf("Caused by (from msr): ");
-	printf("regs %p ",regs);
-	switch( regs->msr & 0x000F0000) {
-	case (0x80000000>>12):
-		printf("Machine check signal - probably due to mm fault\n"
-		       "with mmu off\n");
+	printf("regs %p ", regs);
+
+	val = get_esr();
+
+#if !defined(CONFIG_440)
+	if (val& ESR_IMCP) {
+		printf("Instruction");
+		mtspr(ESR, val & ~ESR_IMCP);
+	} else {
+		printf("Data");
+	}
+	printf(" machine check.\n");
+
+#elif defined(CONFIG_440)
+	if (val& ESR_IMCP){
+		printf("Instruction Synchronous Machine Check exception\n");
+		mtspr(SPRN_ESR, val & ~ESR_IMCP);
+	} else {
+		val = mfspr(MCSR);
+		if (val & MCSR_IB)
+			printf("Instruction Read PLB Error\n");
+		if (val & MCSR_DRB)
+			printf("Data Read PLB Error\n");
+		if (val & MCSR_DWB)
+			printf("Data Write PLB Error\n");
+		if (val & MCSR_TLBP)
+			printf("TLB Parity Error\n");
+		if (val & MCSR_ICP){
+			/*flush_instruction_cache(); */
+			printf("I-Cache Parity Error\n");
+		}
+		if (val & MCSR_DCSP)
+			printf("D-Cache Search Parity Error\n");
+		if (val & MCSR_DCFP)
+			printf("D-Cache Flush Parity Error\n");
+		if (val & MCSR_IMPE)
+			printf("Machine Check exception is imprecise\n");
+
+		/* Clear MCSR */
+		mtspr(SPRN_MCSR, val);
+	}
+#if defined(CONFIG_440EPX) || defined(CONFIG_440GRX)
+	mfsdram(DDR0_00, val) ;
+	printf("DDR0: DDR0_00 %p\n", val);
+	val = (val >> 16) & 0xff;
+	if (val & 0x80)
+		printf("DDR0: At least one interrupt active\n");
+	if (val & 0x40)
+		printf("DDR0: DRAM initialization complete.\n");
+	if (val & 0x20) {
+		printf("DDR0: Multiple uncorrectable ECC events.\n");
+		uncorr_ecc = 1;
+	}
+	if (val & 0x10) {
+		printf("DDR0: Single uncorrectable ECC event.\n");
+		uncorr_ecc = 1;
+	}
+	if (val & 0x08) {
+		printf("DDR0: Multiple correctable ECC events.\n");
+		corr_ecc = 1;
+	}
+	if (val & 0x04) {
+		printf("DDR0: Single correctable ECC event.\n");
+		corr_ecc = 1;
+	}
+	if (val & 0x02)
+		printf("Multiple accesses outside the defined"
+		       " physical memory space detected\n");
+	if (val & 0x01)
+		printf("DDR0: Single access outside the defined"
+		       " physical memory space detected.\n");
+
+	mfsdram(DDR0_01, val);
+	val = (val >> 8) & 0x7;
+	switch (val ) {
+	case 0:
+		printf("DDR0: Write Out-of-Range command\n");
+		break;
+	case 1:
+		printf("DDR0: Read Out-of-Range command\n");
 		break;
-	case (0x80000000>>13):
-		printf("Transfer error ack signal\n");
+	case 2:
+		printf("DDR0: Masked write Out-of-Range command\n");
 		break;
-	case (0x80000000>>14):
-		printf("Data parity signal\n");
+	case 4:
+		printf("DDR0: Wrap write Out-of-Range command\n");
 		break;
-	case (0x80000000>>15):
-		printf("Address parity signal\n");
+	case 5:
+		printf("DDR0: Wrap read Out-of-Range command\n");
 		break;
 	default:
-		printf("Unknown values in msr\n");
+		mfsdram(DDR0_01, value2);
+		printf("DDR0: No DDR0 error know 0x%x %p\n", val, value2);
 	}
+	mfsdram(DDR0_23, val);
+	if (((val >> 16) & 0xff) && corr_ecc)
+		printf("DDR0: Syndrome for correctable ECC event 0x%x\n",
+		       (val >> 16) & 0xff);
+	mfsdram(DDR0_23, val);
+	if (((val >> 8) & 0xff) && uncorr_ecc)
+		printf("DDR0: Syndrome for uncorrectable ECC event 0x%x\n",
+		       (val >> 8) & 0xff);
+	mfsdram(DDR0_33, val);
+	if (val)
+		printf("DDR0: Address of command that caused an "
+		       "Out-of-Range interrupt %p\n", val);
+	mfsdram(DDR0_34, val);
+	if (val && uncorr_ecc)
+		printf("DDR0: Address of uncorrectable ECC event %p\n", val);
+	mfsdram(DDR0_35, val);
+	if (val && uncorr_ecc)
+		printf("DDR0: Address of uncorrectable ECC event %p\n", val);
+	mfsdram(DDR0_36, val);
+	if (val && uncorr_ecc)
+		printf("DDR0: Data of uncorrectable ECC event 0x%08x\n", val);
+	mfsdram(DDR0_37, val);
+	if (val && uncorr_ecc)
+		printf("DDR0: Data of uncorrectable ECC event 0x%08x\n", val);
+	mfsdram(DDR0_38, val);
+	if (val && corr_ecc)
+		printf("DDR0: Address of correctable ECC event %p\n", val);
+	mfsdram(DDR0_39, val);
+	if (val && corr_ecc)
+		printf("DDR0: Address of correctable ECC event %p\n", val);
+	mfsdram(DDR0_40, val);
+	if (val && corr_ecc)
+		printf("DDR0: Data of correctable ECC event 0x%08x\n", val);
+	mfsdram(DDR0_41, val);
+	if (val && corr_ecc)
+		printf("DDR0: Data of correctable ECC event 0x%08x\n", val);
+#endif /* CONFIG_440EPX */
+#endif /* CONFIG_440 */
 	show_regs(regs);
 	print_backtrace((unsigned long *)regs->gpr[1]);
 	panic("machine check");
@@ -189,7 +306,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -204,7 +321,7 @@ ProgramCheckException(struct pt_regs *regs)
 {
 	long esr_val;
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -224,7 +341,7 @@ ProgramCheckException(struct pt_regs *regs)
 }
 
 void
-PITException(struct pt_regs *regs)
+DecrementerPITException(struct pt_regs *regs)
 {
 	/*
 	 * Reset PIT interrupt
@@ -241,7 +358,7 @@ PITException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -256,7 +373,7 @@ DebugException(struct pt_regs *regs)
 {
 	printf("Debugger trap at @ %lx\n", regs->nip );
 	show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 	do_bedbug_breakpoint( regs );
 #endif
 }
@@ -272,17 +389,17 @@ addr_probe(uint *addr)
 
 	__asm__ __volatile__(			\
 		"1:	lwz %0,0(%1)\n"		\
-						"	eieio\n"		\
-						"	li %0,0\n"		\
-						"2:\n"				\
-						".section .fixup,\"ax\"\n"	\
-						"3:	li %0,-1\n"		\
-						"	b 2b\n"			\
-						".section __ex_table,\"a\"\n"	\
-						"	.align 2\n"		\
-						"	.long 1b,3b\n"		\
-						".text"				\
-						: "=r" (retval) : "r"(addr));
+		"	eieio\n"		\
+		"	li %0,0\n"		\
+		"2:\n"				\
+		".section .fixup,\"ax\"\n"	\
+		"3:	li %0,-1\n"		\
+		"	b 2b\n"			\
+		".section __ex_table,\"a\"\n"	\
+		"	.align 2\n"		\
+		"	.long 1b,3b\n"		\
+		".text"				\
+		: "=r" (retval) : "r"(addr));
 
 	return (retval);
 #endif
diff --git a/cpu/ppc4xx/usb.c b/cpu/ppc4xx/usb.c
new file mode 100644
index 0000000..272ed8c
--- /dev/null
+++ b/cpu/ppc4xx/usb.c
@@ -0,0 +1,50 @@
+/*
+ * (C) Copyright 2007
+ * Markus Klotzbuecher, DENX Software Engineering <mk@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#if defined(CONFIG_USB_OHCI_NEW) && defined(CFG_USB_OHCI_CPU_INIT)
+
+#include "usbdev.h"
+
+int usb_cpu_init(void)
+{
+
+#if defined(CONFIG_440EP) || defined(CONFIG_440EPX)
+	usb_dev_init();
+#endif
+
+	return 0;
+}
+
+int usb_cpu_stop(void)
+{
+	return 0;
+}
+
+int usb_cpu_init_fail(void)
+{
+	return 0;
+}
+
+#endif /* defined(CONFIG_USB_OHCI) && defined(CFG_USB_OHCI_CPU_INIT) */
diff --git a/cpu/ppc4xx/usbdev.c b/cpu/ppc4xx/usbdev.c
index 6140d2a..5924a6c 100644
--- a/cpu/ppc4xx/usbdev.c
+++ b/cpu/ppc4xx/usbdev.c
@@ -3,7 +3,7 @@
 #include <common.h>
 #include <asm/processor.h>
 
-#if (defined(CONFIG_440EP) || defined(CONFIG_440EPX)) && (CONFIG_COMMANDS & CFG_CMD_USB)
+#if (defined(CONFIG_440EP) || defined(CONFIG_440EPX)) && defined(CONFIG_CMD_USB)
 
 #include <usb.h>
 #include "usbdev.h"