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

98 files changed, 8369 insertions, 2038 deletions

diff --git a/cpu/arm1136/config.mk b/cpu/arm1136/config.mk
index e39e7741c1..6ab0dd35a8 100644
--- a/cpu/arm1136/config.mk
+++ b/cpu/arm1136/config.mk
@@ -31,4 +31,5 @@ PLATFORM_CPPFLAGS += -march=armv5
 #
 # =========================================================================
 PLATFORM_CPPFLAGS +=$(call cc-option,-mapcs-32,-mabi=apcs-gnu)
+PLATFORM_CPPFLAGS +=$(call cc-option,-mno-thumb-interwork,)
 PLATFORM_RELFLAGS +=$(call cc-option,-mshort-load-bytes,$(call cc-option,-malignment-traps,))
diff --git a/cpu/arm720t/serial.c b/cpu/arm720t/serial.c
index 15c54af08b..27eb73ad88 100644
--- a/cpu/arm720t/serial.c
+++ b/cpu/arm720t/serial.c
@@ -129,8 +129,6 @@ serial_puts (const char *s)
 
 void serial_setbrg (void)
 {
-	DECLARE_GLOBAL_DATA_PTR;
-
 	unsigned short divisor = 0;
 
 	switch (gd->baudrate) {
diff --git a/cpu/arm920t/at91rm9200/Makefile b/cpu/arm920t/at91rm9200/Makefile
index eaabad26a0..ab4c52c8fb 100644
--- a/cpu/arm920t/at91rm9200/Makefile
+++ b/cpu/arm920t/at91rm9200/Makefile
@@ -26,7 +26,7 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(SOC).a
 
 COBJS	= bcm5221.o dm9161.o ether.o i2c.o interrupts.o \
-	  lxt972.o serial.o usb.o
+	  lxt972.o serial.o usb.o spi.o
 SOBJS	= lowlevel_init.o
 
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
diff --git a/cpu/arm920t/at91rm9200/dm9161.c b/cpu/arm920t/at91rm9200/dm9161.c
index 968f653081..1beb6e8ba1 100644
--- a/cpu/arm920t/at91rm9200/dm9161.c
+++ b/cpu/arm920t/at91rm9200/dm9161.c
@@ -95,7 +95,7 @@ UCHAR dm9161_GetLinkSpeed (AT91PS_EMAC p_mac)
 		return TRUE;
 	}
 
-	if ((stat1 & DM9161_100BASE_T4_HD) && (stat2 & DM9161_100HDX)) {
+	if ((stat1 & DM9161_100BASE_TX_HD) && (stat2 & DM9161_100HDX)) {
 		/*set MII for 100BaseTX and Half Duplex  */
 		p_mac->EMAC_CFG = (p_mac->EMAC_CFG &
 				~(AT91C_EMAC_SPD | AT91C_EMAC_FD))
@@ -140,7 +140,7 @@ UCHAR dm9161_InitPhy (AT91PS_EMAC p_mac)
 	at91rm9200_EmacReadPhy (p_mac, DM9161_MDINTR, &IntValue);
 	/* set FDX, SPD, Link, INTR masks */
 	IntValue |= (DM9161_FDX_MASK | DM9161_SPD_MASK |
-		     DM9161_LINK_MASK | DM9161_INTR_MASK);
+			DM9161_LINK_MASK | DM9161_INTR_MASK);
 	at91rm9200_EmacWritePhy (p_mac, DM9161_MDINTR, &IntValue);
 	at91rm9200_EmacDisableMDIO (p_mac);
 
@@ -174,10 +174,11 @@ UCHAR dm9161_AutoNegotiate (AT91PS_EMAC p_mac, int *status)
 	if (!at91rm9200_EmacWritePhy (p_mac, DM9161_BMCR, &value))
 		return FALSE;
 
-	/* Set the Auto_negotiation Advertisement Register */
-	/* MII advertising for Next page, 100BaseTxFD and HD, 10BaseTFD and HD, IEEE 802.3 */
+	/* Set the Auto_negotiation Advertisement Register	*/
+	/* MII advertising for Next page, 100BaseTxFD and HD,	*/
+	/* 10BaseTFD and HD, IEEE 802.3 */
 	PhyAnar = DM9161_NP | DM9161_TX_FDX | DM9161_TX_HDX |
-		  DM9161_10_FDX | DM9161_10_HDX | DM9161_AN_IEEE_802_3;
+			DM9161_10_FDX | DM9161_10_HDX | DM9161_AN_IEEE_802_3;
 	if (!at91rm9200_EmacWritePhy (p_mac, DM9161_ANAR, &PhyAnar))
 		return FALSE;
 
diff --git a/cpu/arm920t/at91rm9200/spi.c b/cpu/arm920t/at91rm9200/spi.c
new file mode 100644
index 0000000000..265d18525f
--- /dev/null
+++ b/cpu/arm920t/at91rm9200/spi.c
@@ -0,0 +1,151 @@
+/* Driver for ATMEL DataFlash support
+ * Author : Hamid Ikdoumi (Atmel)
+ *
+ * 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 <config.h>
+#include <common.h>
+#include <asm/hardware.h>
+
+#ifdef CONFIG_HAS_DATAFLASH
+#include <dataflash.h>
+
+#define AT91C_SPI_CLK	10000000	/* Max Value = 10MHz to be compliant to
+					the Continuous Array Read function */
+
+/* AC Characteristics */
+/* DLYBS = tCSS = 250ns min and DLYBCT = tCSH = 250ns */
+#define DATAFLASH_TCSS	(0xC << 16)
+#define DATAFLASH_TCHS	(0x1 << 24)
+
+#define AT91C_TIMEOUT_WRDY		200000
+#define AT91C_SPI_PCS0_SERIAL_DATAFLASH	0xE	/* Chip Select 0: NPCS0%1110 */
+#define AT91C_SPI_PCS3_DATAFLASH_CARD	0x7	/* Chip Select 3: NPCS3%0111 */
+
+/*-------------------------------------------------------------------*/
+/*	SPI DataFlash Init					     */
+/*-------------------------------------------------------------------*/
+void AT91F_SpiInit(void)
+{
+	/* Configure PIOs */
+	AT91C_BASE_PIOA->PIO_ASR =
+		AT91C_PA3_NPCS0 | AT91C_PA4_NPCS1 | AT91C_PA1_MOSI |
+		AT91C_PA5_NPCS2 | AT91C_PA6_NPCS3 | AT91C_PA0_MISO |
+		AT91C_PA2_SPCK;
+	AT91C_BASE_PIOA->PIO_PDR =
+		AT91C_PA3_NPCS0 | AT91C_PA4_NPCS1 | AT91C_PA1_MOSI |
+		AT91C_PA5_NPCS2 | AT91C_PA6_NPCS3 | AT91C_PA0_MISO |
+		AT91C_PA2_SPCK;
+	/* Enable CLock */
+	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_SPI;
+
+	/* Reset the SPI */
+	AT91C_BASE_SPI->SPI_CR = AT91C_SPI_SWRST;
+
+	/* Configure SPI in Master Mode with No CS selected !!! */
+	AT91C_BASE_SPI->SPI_MR =
+		AT91C_SPI_MSTR | AT91C_SPI_MODFDIS | AT91C_SPI_PCS;
+
+	/* Configure CS0 and CS3 */
+	*(AT91C_SPI_CSR + 0) =
+		AT91C_SPI_CPOL | (AT91C_SPI_DLYBS & DATAFLASH_TCSS) |
+		(AT91C_SPI_DLYBCT & DATAFLASH_TCHS) |
+		((AT91C_MASTER_CLOCK / (2*AT91C_SPI_CLK)) << 8);
+
+	*(AT91C_SPI_CSR + 3) =
+		AT91C_SPI_CPOL | (AT91C_SPI_DLYBS & DATAFLASH_TCSS) |
+		(AT91C_SPI_DLYBCT & DATAFLASH_TCHS) |
+		((AT91C_MASTER_CLOCK / (2*AT91C_SPI_CLK)) << 8);
+}
+
+void AT91F_SpiEnable(int cs)
+{
+	switch(cs) {
+	case 0:	/* Configure SPI CS0 for Serial DataFlash AT45DBxx */
+		AT91C_BASE_SPI->SPI_MR &= 0xFFF0FFFF;
+		AT91C_BASE_SPI->SPI_MR |=
+			((AT91C_SPI_PCS0_SERIAL_DATAFLASH<<16) &
+				AT91C_SPI_PCS);
+		break;
+	case 3:	/* Configure SPI CS3 for Serial DataFlash Card */
+		/* Set up PIO SDC_TYPE to switch on DataFlash Card */
+		/* and not MMC/SDCard */
+		AT91C_BASE_PIOB->PIO_PER =
+			AT91C_PIO_PB7;	/* Set in PIO mode */
+		AT91C_BASE_PIOB->PIO_OER =
+			AT91C_PIO_PB7;	/* Configure in output */
+		/* Clear Output */
+		AT91C_BASE_PIOB->PIO_CODR = AT91C_PIO_PB7;
+		/* Configure PCS */
+		AT91C_BASE_SPI->SPI_MR &= 0xFFF0FFFF;
+		AT91C_BASE_SPI->SPI_MR |=
+			((AT91C_SPI_PCS3_DATAFLASH_CARD<<16) & AT91C_SPI_PCS);
+		break;
+	}
+
+	/* SPI_Enable */
+	AT91C_BASE_SPI->SPI_CR = AT91C_SPI_SPIEN; }
+
+/*---------------------------------------------------------------------------*/
+/* \fn    AT91F_SpiWrite						     */
+/* \brief Set the PDC registers for a transfert				     */
+/*---------------------------------------------------------------------------*/
+unsigned int AT91F_SpiWrite ( AT91PS_DataflashDesc pDesc )
+{
+	unsigned int timeout;
+
+	pDesc->state = BUSY;
+
+	AT91C_BASE_SPI->SPI_PTCR = AT91C_PDC_TXTDIS + AT91C_PDC_RXTDIS;
+
+	/* Initialize the Transmit and Receive Pointer */
+	AT91C_BASE_SPI->SPI_RPR = (unsigned int)pDesc->rx_cmd_pt ;
+	AT91C_BASE_SPI->SPI_TPR = (unsigned int)pDesc->tx_cmd_pt ;
+
+	/* Intialize the Transmit and Receive Counters */
+	AT91C_BASE_SPI->SPI_RCR = pDesc->rx_cmd_size;
+	AT91C_BASE_SPI->SPI_TCR = pDesc->tx_cmd_size;
+
+	if ( pDesc->tx_data_size != 0 ) {
+		/* Initialize the Next Transmit and Next Receive Pointer */
+		AT91C_BASE_SPI->SPI_RNPR = (unsigned int)pDesc->rx_data_pt ;
+		AT91C_BASE_SPI->SPI_TNPR = (unsigned int)pDesc->tx_data_pt ;
+
+		/* Intialize the Next Transmit and Next Receive Counters */
+		AT91C_BASE_SPI->SPI_RNCR = pDesc->rx_data_size ;
+		AT91C_BASE_SPI->SPI_TNCR = pDesc->tx_data_size ;
+	}
+
+	/* arm simple, non interrupt dependent timer */
+	reset_timer_masked();
+	timeout = 0;
+
+	AT91C_BASE_SPI->SPI_PTCR = AT91C_PDC_TXTEN + AT91C_PDC_RXTEN;
+	while(!(AT91C_BASE_SPI->SPI_SR & AT91C_SPI_RXBUFF) &&
+		((timeout = get_timer_masked() ) < CFG_SPI_WRITE_TOUT));
+	AT91C_BASE_SPI->SPI_PTCR = AT91C_PDC_TXTDIS + AT91C_PDC_RXTDIS;
+	pDesc->state = IDLE;
+
+	if (timeout >= CFG_SPI_WRITE_TOUT){
+		printf("Error Timeout\n\r");
+		return DATAFLASH_ERROR;
+	}
+
+	return DATAFLASH_OK;
+}
+#endif
diff --git a/cpu/arm920t/start.S b/cpu/arm920t/start.S
index 346f0d09ea..b9c364bc67 100644
--- a/cpu/arm920t/start.S
+++ b/cpu/arm920t/start.S
@@ -27,7 +27,9 @@
 
 #include <config.h>
 #include <version.h>
-
+#if	defined(CONFIG_AT91RM9200DK) || defined(CONFIG_AT91RM9200EK) || defined(CONFIG_AT91RM9200DF)
+#include	<led.h>
+#endif
 
 /*
  *************************************************************************
@@ -116,6 +118,69 @@ reset:
 	orr	r0,r0,#0xd3
 	msr	cpsr,r0
 
+#if	CONFIG_AT91RM9200
+#if	defined(CONFIG_AT91RM9200DK) || defined(CONFIG_AT91RM9200EK) || defined(CONFIG_AT91RM9200DF)
+	bl LED_init
+	bl red_LED_on
+#endif
+
+#ifdef CONFIG_BOOTBINFUNC
+/* code based on entry.S from ATMEL */
+#define AT91C_BASE_CKGR 0xFFFFFC20
+#define CKGR_MOR 0
+	/* Get the CKGR Base Address */
+	ldr     r1, =AT91C_BASE_CKGR
+
+/* Main oscillator Enable register	APMC_MOR : Enable main oscillator , OSCOUNT = 0xFF */
+/*	ldr 	r0, = AT91C_CKGR_MOSCEN:OR:AT91C_CKGR_OSCOUNT */
+	ldr 	r0, =0x0000FF01
+	str     r0, [r1, #CKGR_MOR]
+	/* Add loop to compensate Main Oscillator startup time */
+	ldr 	r0, =0x00000010
+LoopOsc:
+	subs    r0, r0, #1
+	bhi     LoopOsc
+	/* scratch stack */
+	ldr 	r1, =0x00204000
+	/* Insure word alignment */
+	bic     r1, r1, #3
+	/* Init stack SYS	 */
+	mov     sp, r1
+	/*
+	 * This does a lot more than just set up the memory, which
+	 * is why it's called lowlevelinit
+	 */
+	bl	lowlevelinit /* in memsetup.S */
+	bl	icache_enable;
+	/* ------------------------------------
+	 * Read/modify/write CP15 control register
+	 * -------------------------------------
+	 * read cp15 control register (cp15 r1) in r0
+	 * ------------------------------------
+	 */
+	mrc     p15, 0, r0, c1, c0, 0
+	/* Reset bit :Little Endian end fast bus mode */
+	ldr     r3, =0xC0000080
+	/* Set bit :Asynchronous clock mode, Not Fast Bus */
+	ldr     r4, =0xC0000000
+	bic     r0, r0, r3
+	orr     r0, r0, r4
+	/* write r0 in cp15 control register (cp15 r1) */
+	mcr     p15, 0, r0, c1, c0, 0
+#endif /* CONFIG_BOOTBINFUNC */
+	/*
+	 * relocate exeception table
+	 */
+	ldr	r0, =_start
+	ldr	r1, =0x0
+	mov	r2, #16
+copyex:
+	subs	r2, r2, #1
+	ldr	r3, [r0], #4
+	str	r3, [r1], #4
+	bne	copyex
+#endif
+
 /* turn off the watchdog */
 #if defined(CONFIG_S3C2400)
 # define pWTCON		0x15300000
@@ -160,6 +225,26 @@ reset:
 	bl	cpu_init_crit
 #endif
 
+#ifdef	CONFIG_AT91RM9200
+#ifdef CONFIG_BOOTBINFUNC
+relocate:				/* relocate U-Boot to RAM	    */
+	adr	r0, _start		/* r0 <- current position of code   */
+	ldr	r1, _TEXT_BASE		/* test if we run from flash or RAM */
+	cmp     r0, r1                  /* don't reloc during debug         */
+	beq     stack_setup
+
+	ldr	r2, _armboot_start
+	ldr	r3, _bss_start
+	sub	r2, r3, r2		/* r2 <- size of armboot            */
+	add	r2, r0, r2		/* r2 <- source end address         */
+
+copy_loop:
+	ldmia	r0!, {r3-r10}		/* copy from source address [r0]    */
+	stmia	r1!, {r3-r10}		/* copy to   target address [r1]    */
+	cmp	r0, r2			/* until source end addreee [r2]    */
+	ble	copy_loop
+#endif /* CONFIG_BOOTBINFUNC */
+#else
 #ifndef CONFIG_SKIP_RELOCATE_UBOOT
 relocate:				/* relocate U-Boot to RAM	    */
 	adr	r0, _start		/* r0 <- current position of code   */
@@ -178,7 +263,7 @@ copy_loop:
 	cmp	r0, r2			/* until source end addreee [r2]    */
 	ble	copy_loop
 #endif	/* CONFIG_SKIP_RELOCATE_UBOOT */
-
+#endif
 	/* Set up the stack						    */
 stack_setup:
 	ldr	r0, _TEXT_BASE		/* upper 128 KiB: relocated uboot   */
@@ -262,7 +347,11 @@ cpu_init_crit:
 	 * find a lowlevel_init.S in your board directory.
 	 */
 	mov	ip, lr
+#if	defined(CONFIG_AT91RM9200DK) || defined(CONFIG_AT91RM9200EK) || defined(CONFIG_AT91RM9200DF)
+
+#else
 	bl	lowlevel_init
+#endif
 	mov	lr, ip
 	mov	pc, lr
 #endif /* CONFIG_SKIP_LOWLEVEL_INIT */
diff --git a/cpu/arm926ejs/davinci/Makefile b/cpu/arm926ejs/davinci/Makefile
new file mode 100644
index 0000000000..0f77f402ed
--- /dev/null
+++ b/cpu/arm926ejs/davinci/Makefile
@@ -0,0 +1,49 @@
+#
+# (C) Copyright 2000-2006
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+#
+# 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 $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(SOC).a
+
+COBJS	= timer.o ether.o lxt972.o dp83848.o i2c.o nand.o
+SOBJS	= lowlevel_init.o reset.o
+
+SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS) $(SOBJS))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(LIB)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/arm926ejs/davinci/dp83848.c b/cpu/arm926ejs/davinci/dp83848.c
new file mode 100644
index 0000000000..5719845b34
--- /dev/null
+++ b/cpu/arm926ejs/davinci/dp83848.c
@@ -0,0 +1,156 @@
+/*
+ * National Semiconductor DP83848 PHY Driver for TI DaVinci
+ * (TMS320DM644x) based boards.
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * --------------------------------------------------------
+ *
+ * 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>
+#include <net.h>
+#include <dp83848.h>
+#include <asm/arch/emac_defs.h>
+
+#ifdef CONFIG_DRIVER_TI_EMAC
+
+#ifdef CONFIG_CMD_NET
+
+int dp83848_is_phy_connected(int phy_addr)
+{
+	u_int16_t	id1, id2;
+
+	if (!dm644x_eth_phy_read(phy_addr, DP83848_PHYID1_REG, &id1))
+		return(0);
+	if (!dm644x_eth_phy_read(phy_addr, DP83848_PHYID2_REG, &id2))
+		return(0);
+
+	if ((id1 == DP83848_PHYID1_OUI) && (id2 == DP83848_PHYID2_OUI))
+		return(1);
+
+	return(0);
+}
+
+int dp83848_get_link_speed(int phy_addr)
+{
+	u_int16_t		tmp;
+	volatile emac_regs*	emac = (emac_regs *)EMAC_BASE_ADDR;
+
+	if (!dm644x_eth_phy_read(phy_addr, DP83848_STAT_REG, &tmp))
+		return(0);
+
+	if (!(tmp & DP83848_LINK_STATUS))	/* link up? */
+		return(0);
+
+	if (!dm644x_eth_phy_read(phy_addr, DP83848_PHY_STAT_REG, &tmp))
+		return(0);
+
+	/* Speed doesn't matter, there is no setting for it in EMAC... */
+	if (tmp & DP83848_SPEED) {
+		if (tmp & DP83848_DUPLEX) {
+			/* set DM644x EMAC for Full Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE | EMAC_MACCONTROL_FULLDUPLEX_ENABLE;
+		} else {
+			/*set DM644x EMAC for Half Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE;
+		}
+
+		return(1);
+	} else {
+		if (tmp & DP83848_DUPLEX) {
+			/* set DM644x EMAC for Full Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE | EMAC_MACCONTROL_FULLDUPLEX_ENABLE;
+		} else {
+			/*set DM644x EMAC for Half Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE;
+		}
+
+		return(1);
+	}
+
+	return(0);
+}
+
+
+int dp83848_init_phy(int phy_addr)
+{
+	int	ret = 1;
+
+	if (!dp83848_get_link_speed(phy_addr)) {
+		/* Try another time */
+		udelay(100000);
+		ret = dp83848_get_link_speed(phy_addr);
+	}
+
+	/* Disable PHY Interrupts */
+	dm644x_eth_phy_write(phy_addr, DP83848_PHY_INTR_CTRL_REG, 0);
+
+	return(ret);
+}
+
+
+int dp83848_auto_negotiate(int phy_addr)
+{
+	u_int16_t	tmp;
+
+
+	if (!dm644x_eth_phy_read(phy_addr, DP83848_CTL_REG, &tmp))
+		return(0);
+
+	/* Restart Auto_negotiation  */
+	tmp &= ~DP83848_AUTONEG;	/* remove autonegotiation enable */
+	tmp |= DP83848_ISOLATE;		/* Electrically isolate PHY */
+	dm644x_eth_phy_write(phy_addr, DP83848_CTL_REG, tmp);
+
+	/* Set the Auto_negotiation Advertisement Register
+	 * MII advertising for Next page, 100BaseTxFD and HD,
+	 * 10BaseTFD and HD, IEEE 802.3
+	 */
+	tmp = DP83848_NP | DP83848_TX_FDX | DP83848_TX_HDX |
+	 	DP83848_10_FDX | DP83848_10_HDX | DP83848_AN_IEEE_802_3;
+	dm644x_eth_phy_write(phy_addr, DP83848_ANA_REG, tmp);
+
+
+	/* Read Control Register */
+	if (!dm644x_eth_phy_read(phy_addr, DP83848_CTL_REG, &tmp))
+		return(0);
+
+	tmp |= DP83848_SPEED_SELECT | DP83848_AUTONEG | DP83848_DUPLEX_MODE;
+	dm644x_eth_phy_write(phy_addr, DP83848_CTL_REG, tmp);
+
+	/* Restart Auto_negotiation  */
+	tmp |= DP83848_RESTART_AUTONEG;
+	dm644x_eth_phy_write(phy_addr, DP83848_CTL_REG, tmp);
+
+	/*check AutoNegotiate complete */
+	udelay(10000);
+	if (!dm644x_eth_phy_read(phy_addr, DP83848_STAT_REG, &tmp))
+		return(0);
+
+	if (!(tmp & DP83848_AUTONEG_COMP))
+		return(0);
+
+	return (dp83848_get_link_speed(phy_addr));
+}
+
+#endif	/* CONFIG_CMD_NET */
+
+#endif	/* CONFIG_DRIVER_ETHER */
diff --git a/cpu/arm926ejs/davinci/ether.c b/cpu/arm926ejs/davinci/ether.c
new file mode 100644
index 0000000000..766bc7d66c
--- /dev/null
+++ b/cpu/arm926ejs/davinci/ether.c
@@ -0,0 +1,650 @@
+/*
+ * Ethernet driver for TI TMS320DM644x (DaVinci) chips.
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * Parts shamelessly stolen from TI's dm644x_emac.c. Original copyright
+ * follows:
+ *
+ * ----------------------------------------------------------------------------
+ *
+ * dm644x_emac.c
+ *
+ * TI DaVinci (DM644X) EMAC peripheral driver source for DV-EVM
+ *
+ * Copyright (C) 2005 Texas Instruments.
+ *
+ * ----------------------------------------------------------------------------
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ * ----------------------------------------------------------------------------
+
+ * Modifications:
+ * ver. 1.0: Sep 2005, Anant Gole - Created EMAC version for uBoot.
+ * ver  1.1: Nov 2005, Anant Gole - Extended the RX logic for multiple descriptors
+ *
+ */
+#include <common.h>
+#include <command.h>
+#include <net.h>
+#include <miiphy.h>
+#include <asm/arch/emac_defs.h>
+
+#ifdef CONFIG_DRIVER_TI_EMAC
+
+#ifdef CONFIG_CMD_NET
+
+unsigned int	emac_dbg = 0;
+#define debug_emac(fmt,args...)	if (emac_dbg) printf(fmt,##args)
+
+/* Internal static functions */
+static int dm644x_eth_hw_init (void);
+static int dm644x_eth_open (void);
+static int dm644x_eth_close (void);
+static int dm644x_eth_send_packet (volatile void *packet, int length);
+static int dm644x_eth_rcv_packet (void);
+static void dm644x_eth_mdio_enable(void);
+
+static int gen_init_phy(int phy_addr);
+static int gen_is_phy_connected(int phy_addr);
+static int gen_get_link_speed(int phy_addr);
+static int gen_auto_negotiate(int phy_addr);
+
+/* Wrappers exported to the U-Boot proper */
+int eth_hw_init(void)
+{
+	return(dm644x_eth_hw_init());
+}
+
+int eth_init(bd_t * bd)
+{
+	return(dm644x_eth_open());
+}
+
+void eth_halt(void)
+{
+	dm644x_eth_close();
+}
+
+int eth_send(volatile void *packet, int length)
+{
+	return(dm644x_eth_send_packet(packet, length));
+}
+
+int eth_rx(void)
+{
+	return(dm644x_eth_rcv_packet());
+}
+
+void eth_mdio_enable(void)
+{
+	dm644x_eth_mdio_enable();
+}
+/* End of wrappers */
+
+
+static u_int8_t dm644x_eth_mac_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+/*
+ * This function must be called before emac_open() if you want to override
+ * the default mac address.
+ */
+void dm644x_eth_set_mac_addr(const u_int8_t *addr)
+{
+	int i;
+
+	for (i = 0; i < sizeof (dm644x_eth_mac_addr); i++) {
+		dm644x_eth_mac_addr[i] = addr[i];
+	}
+}
+
+/* EMAC Addresses */
+static volatile emac_regs	*adap_emac = (emac_regs *)EMAC_BASE_ADDR;
+static volatile ewrap_regs	*adap_ewrap = (ewrap_regs *)EMAC_WRAPPER_BASE_ADDR;
+static volatile mdio_regs	*adap_mdio = (mdio_regs *)EMAC_MDIO_BASE_ADDR;
+
+/* EMAC descriptors */
+static volatile emac_desc	*emac_rx_desc = (emac_desc *)(EMAC_WRAPPER_RAM_ADDR + EMAC_RX_DESC_BASE);
+static volatile emac_desc	*emac_tx_desc = (emac_desc *)(EMAC_WRAPPER_RAM_ADDR + EMAC_TX_DESC_BASE);
+static volatile emac_desc	*emac_rx_active_head = 0;
+static volatile emac_desc	*emac_rx_active_tail = 0;
+static int			emac_rx_queue_active = 0;
+
+/* Receive packet buffers */
+static unsigned char		emac_rx_buffers[EMAC_MAX_RX_BUFFERS * (EMAC_MAX_ETHERNET_PKT_SIZE + EMAC_PKT_ALIGN)];
+
+/* PHY address for a discovered PHY (0xff - not found) */
+static volatile u_int8_t	active_phy_addr = 0xff;
+
+phy_t				phy;
+
+static void dm644x_eth_mdio_enable(void)
+{
+	u_int32_t	clkdiv;
+
+	clkdiv = (EMAC_MDIO_BUS_FREQ / EMAC_MDIO_CLOCK_FREQ) - 1;
+
+	adap_mdio->CONTROL = (clkdiv & 0xff) |
+		MDIO_CONTROL_ENABLE |
+		MDIO_CONTROL_FAULT |
+		MDIO_CONTROL_FAULT_ENABLE;
+
+	while (adap_mdio->CONTROL & MDIO_CONTROL_IDLE) {;}
+}
+
+/*
+ * Tries to find an active connected PHY. Returns 1 if address if found.
+ * If no active PHY (or more than one PHY) found returns 0.
+ * Sets active_phy_addr variable.
+ */
+static int dm644x_eth_phy_detect(void)
+{
+	u_int32_t	phy_act_state;
+	int		i;
+
+	active_phy_addr = 0xff;
+
+	if ((phy_act_state = adap_mdio->ALIVE) == 0)
+		return(0);				/* No active PHYs */
+
+	debug_emac("dm644x_eth_phy_detect(), ALIVE = 0x%08x\n", phy_act_state);
+
+	for (i = 0; i < 32; i++) {
+		if (phy_act_state & (1 << i)) {
+			if (phy_act_state & ~(1 << i))
+				return(0);		/* More than one PHY */
+			else {
+				active_phy_addr = i;
+				return(1);
+			}
+		}
+	}
+
+	return(0);	/* Just to make GCC happy */
+}
+
+
+/* Read a PHY register via MDIO inteface. Returns 1 on success, 0 otherwise */
+int dm644x_eth_phy_read(u_int8_t phy_addr, u_int8_t reg_num, u_int16_t *data)
+{
+	int	tmp;
+
+	while (adap_mdio->USERACCESS0 & MDIO_USERACCESS0_GO) {;}
+
+	adap_mdio->USERACCESS0 = MDIO_USERACCESS0_GO |
+				MDIO_USERACCESS0_WRITE_READ |
+				((reg_num & 0x1f) << 21) |
+				((phy_addr & 0x1f) << 16);
+
+	/* Wait for command to complete */
+	while ((tmp = adap_mdio->USERACCESS0) & MDIO_USERACCESS0_GO) {;}
+
+	if (tmp & MDIO_USERACCESS0_ACK) {
+		*data = tmp & 0xffff;
+		return(1);
+	}
+
+	*data = -1;
+	return(0);
+}
+
+/* Write to a PHY register via MDIO inteface. Blocks until operation is complete. */
+int dm644x_eth_phy_write(u_int8_t phy_addr, u_int8_t reg_num, u_int16_t data)
+{
+
+	while (adap_mdio->USERACCESS0 & MDIO_USERACCESS0_GO) {;}
+
+	adap_mdio->USERACCESS0 = MDIO_USERACCESS0_GO |
+				MDIO_USERACCESS0_WRITE_WRITE |
+				((reg_num & 0x1f) << 21) |
+				((phy_addr & 0x1f) << 16) |
+				(data & 0xffff);
+
+	/* Wait for command to complete */
+	while (adap_mdio->USERACCESS0 & MDIO_USERACCESS0_GO) {;}
+
+	return(1);
+}
+
+/* PHY functions for a generic PHY */
+static int gen_init_phy(int phy_addr)
+{
+	int	ret = 1;
+
+	if (gen_get_link_speed(phy_addr)) {
+		/* Try another time */
+		ret = gen_get_link_speed(phy_addr);
+	}
+
+	return(ret);
+}
+
+static int gen_is_phy_connected(int phy_addr)
+{
+	u_int16_t	dummy;
+
+	return(dm644x_eth_phy_read(phy_addr, PHY_PHYIDR1, &dummy));
+}
+
+static int gen_get_link_speed(int phy_addr)
+{
+	u_int16_t	tmp;
+
+	if (dm644x_eth_phy_read(phy_addr, MII_STATUS_REG, &tmp) && (tmp & 0x04))
+		return(1);
+
+	return(0);
+}
+
+static int gen_auto_negotiate(int phy_addr)
+{
+	u_int16_t	tmp;
+
+	if (!dm644x_eth_phy_read(phy_addr, PHY_BMCR, &tmp))
+		return(0);
+
+	/* Restart Auto_negotiation  */
+	tmp |= PHY_BMCR_AUTON;
+	dm644x_eth_phy_write(phy_addr, PHY_BMCR, tmp);
+
+	/*check AutoNegotiate complete */
+	udelay (10000);
+	if (!dm644x_eth_phy_read(phy_addr, PHY_BMSR, &tmp))
+		return(0);
+
+	if (!(tmp & PHY_BMSR_AUTN_COMP))
+		return(0);
+
+	return(gen_get_link_speed(phy_addr));
+}
+/* End of generic PHY functions */
+
+
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
+static int dm644x_mii_phy_read(char *devname, unsigned char addr, unsigned char reg, unsigned short *value)
+{
+	return(dm644x_eth_phy_read(addr, reg, value) ? 0 : 1);
+}
+
+static int dm644x_mii_phy_write(char *devname, unsigned char addr, unsigned char reg, unsigned short value)
+{
+	return(dm644x_eth_phy_write(addr, reg, value) ? 0 : 1);
+}
+
+int dm644x_eth_miiphy_initialize(bd_t *bis)
+{
+	miiphy_register(phy.name, dm644x_mii_phy_read, dm644x_mii_phy_write);
+
+	return(1);
+}
+#endif
+
+/*
+ * This function initializes the emac hardware. It does NOT initialize
+ * EMAC modules power or pin multiplexors, that is done by board_init()
+ * much earlier in bootup process. Returns 1 on success, 0 otherwise.
+ */
+static int dm644x_eth_hw_init(void)
+{
+	u_int32_t	phy_id;
+	u_int16_t	tmp;
+	int		i;
+
+	dm644x_eth_mdio_enable();
+
+	for (i = 0; i < 256; i++) {
+		if (adap_mdio->ALIVE)
+			break;
+		udelay(10);
+	}
+
+	if (i >= 256) {
+		printf("No ETH PHY detected!!!\n");
+		return(0);
+	}
+
+	/* Find if a PHY is connected and get it's address */
+	if (!dm644x_eth_phy_detect())
+		return(0);
+
+	/* Get PHY ID and initialize phy_ops for a detected PHY */
+	if (!dm644x_eth_phy_read(active_phy_addr, PHY_PHYIDR1, &tmp)) {
+		active_phy_addr = 0xff;
+		return(0);
+	}
+
+	phy_id = (tmp << 16) & 0xffff0000;
+
+	if (!dm644x_eth_phy_read(active_phy_addr, PHY_PHYIDR2, &tmp)) {
+		active_phy_addr = 0xff;
+		return(0);
+	}
+
+	phy_id |= tmp & 0x0000ffff;
+
+	switch (phy_id) {
+		case PHY_LXT972:
+			sprintf(phy.name, "LXT972 @ 0x%02x", active_phy_addr);
+			phy.init = lxt972_init_phy;
+			phy.is_phy_connected = lxt972_is_phy_connected;
+			phy.get_link_speed = lxt972_get_link_speed;
+			phy.auto_negotiate = lxt972_auto_negotiate;
+			break;
+		case PHY_DP83848:
+			sprintf(phy.name, "DP83848 @ 0x%02x", active_phy_addr);
+			phy.init = dp83848_init_phy;
+			phy.is_phy_connected = dp83848_is_phy_connected;
+			phy.get_link_speed = dp83848_get_link_speed;
+			phy.auto_negotiate = dp83848_auto_negotiate;
+			break;
+		default:
+			sprintf(phy.name, "GENERIC @ 0x%02x", active_phy_addr);
+			phy.init = gen_init_phy;
+			phy.is_phy_connected = gen_is_phy_connected;
+			phy.get_link_speed = gen_get_link_speed;
+			phy.auto_negotiate = gen_auto_negotiate;
+	}
+
+	return(1);
+}
+
+
+/* Eth device open */
+static int dm644x_eth_open(void)
+{
+	dv_reg_p		addr;
+	u_int32_t		clkdiv, cnt;
+	volatile emac_desc	*rx_desc;
+
+	debug_emac("+ emac_open\n");
+
+	/* Reset EMAC module and disable interrupts in wrapper */
+	adap_emac->SOFTRESET = 1;
+	while (adap_emac->SOFTRESET != 0) {;}
+	adap_ewrap->EWCTL = 0;
+	for (cnt = 0; cnt < 5; cnt++) {
+		clkdiv = adap_ewrap->EWCTL;
+	}
+
+	rx_desc = emac_rx_desc;
+
+	adap_emac->TXCONTROL = 0x01;
+	adap_emac->RXCONTROL = 0x01;
+
+	/* Set MAC Addresses & Init multicast Hash to 0 (disable any multicast receive) */
+	/* Using channel 0 only - other channels are disabled */
+	adap_emac->MACINDEX = 0;
+	adap_emac->MACADDRHI =
+		(dm644x_eth_mac_addr[3] << 24) |
+		(dm644x_eth_mac_addr[2] << 16) |
+		(dm644x_eth_mac_addr[1] << 8)  |
+		(dm644x_eth_mac_addr[0]);
+	adap_emac->MACADDRLO =
+		(dm644x_eth_mac_addr[5] << 8) |
+		(dm644x_eth_mac_addr[4]);
+
+	adap_emac->MACHASH1 = 0;
+	adap_emac->MACHASH2 = 0;
+
+	/* Set source MAC address - REQUIRED */
+	adap_emac->MACSRCADDRHI =
+		(dm644x_eth_mac_addr[3] << 24) |
+		(dm644x_eth_mac_addr[2] << 16) |
+		(dm644x_eth_mac_addr[1] << 8)  |
+		(dm644x_eth_mac_addr[0]);
+	adap_emac->MACSRCADDRLO =
+		(dm644x_eth_mac_addr[4] << 8) |
+		(dm644x_eth_mac_addr[5]);
+
+	/* Set DMA 8 TX / 8 RX Head pointers to 0 */
+	addr = &adap_emac->TX0HDP;
+	for(cnt = 0; cnt < 16; cnt++)
+		*addr++ = 0;
+
+	addr = &adap_emac->RX0HDP;
+	for(cnt = 0; cnt < 16; cnt++)
+		*addr++ = 0;
+
+	/* Clear Statistics (do this before setting MacControl register) */
+	addr = &adap_emac->RXGOODFRAMES;
+	for(cnt = 0; cnt < EMAC_NUM_STATS; cnt++)
+		*addr++ = 0;
+
+	/* No multicast addressing */
+	adap_emac->MACHASH1 = 0;
+	adap_emac->MACHASH2 = 0;
+
+	/* Create RX queue and set receive process in place */
+	emac_rx_active_head = emac_rx_desc;
+	for (cnt = 0; cnt < EMAC_MAX_RX_BUFFERS; cnt++) {
+		rx_desc->next = (u_int32_t)(rx_desc + 1);
+		rx_desc->buffer = &emac_rx_buffers[cnt * (EMAC_MAX_ETHERNET_PKT_SIZE + EMAC_PKT_ALIGN)];
+		rx_desc->buff_off_len = EMAC_MAX_ETHERNET_PKT_SIZE;
+		rx_desc->pkt_flag_len = EMAC_CPPI_OWNERSHIP_BIT;
+		rx_desc++;
+	}
+
+	/* Set the last descriptor's "next" parameter to 0 to end the RX desc list */
+	rx_desc--;
+	rx_desc->next = 0;
+	emac_rx_active_tail = rx_desc;
+	emac_rx_queue_active = 1;
+
+	/* Enable TX/RX */
+	adap_emac->RXMAXLEN = EMAC_MAX_ETHERNET_PKT_SIZE;
+	adap_emac->RXBUFFEROFFSET = 0;
+
+	/* No fancy configs - Use this for promiscous for debug - EMAC_RXMBPENABLE_RXCAFEN_ENABLE */
+	adap_emac->RXMBPENABLE = EMAC_RXMBPENABLE_RXBROADEN;
+
+	/* Enable ch 0 only */
+	adap_emac->RXUNICASTSET = 0x01;
+
+	/* Enable MII interface and Full duplex mode */
+	adap_emac->MACCONTROL = (EMAC_MACCONTROL_MIIEN_ENABLE | EMAC_MACCONTROL_FULLDUPLEX_ENABLE);
+
+	/* Init MDIO & get link state */
+	clkdiv = (EMAC_MDIO_BUS_FREQ / EMAC_MDIO_CLOCK_FREQ) - 1;
+	adap_mdio->CONTROL = ((clkdiv & 0xff) | MDIO_CONTROL_ENABLE | MDIO_CONTROL_FAULT);
+
+	if (!phy.get_link_speed(active_phy_addr))
+		return(0);
+
+	/* Start receive process */
+	adap_emac->RX0HDP = (u_int32_t)emac_rx_desc;
+
+	debug_emac("- emac_open\n");
+
+	return(1);
+}
+
+/* EMAC Channel Teardown */
+static void dm644x_eth_ch_teardown(int ch)
+{
+	dv_reg		dly = 0xff;
+	dv_reg		cnt;
+
+	debug_emac("+ emac_ch_teardown\n");
+
+	if (ch == EMAC_CH_TX) {
+		/* Init TX channel teardown */
+		adap_emac->TXTEARDOWN = 1;
+		for(cnt = 0; cnt != 0xfffffffc; cnt = adap_emac->TX0CP) {
+			/* Wait here for Tx teardown completion interrupt to occur
+			 * Note: A task delay can be called here to pend rather than
+			 * occupying CPU cycles - anyway it has been found that teardown
+			 * takes very few cpu cycles and does not affect functionality */
+			 dly--;
+			 udelay(1);
+			 if (dly == 0)
+			 	break;
+		}
+		adap_emac->TX0CP = cnt;
+		adap_emac->TX0HDP = 0;
+	} else {
+		/* Init RX channel teardown */
+		adap_emac->RXTEARDOWN = 1;
+		for(cnt = 0; cnt != 0xfffffffc; cnt = adap_emac->RX0CP) {
+			/* Wait here for Rx teardown completion interrupt to occur
+			 * Note: A task delay can be called here to pend rather than
+			 * occupying CPU cycles - anyway it has been found that teardown
+			 * takes very few cpu cycles and does not affect functionality */
+			 dly--;
+			 udelay(1);
+			 if (dly == 0)
+			 	break;
+		}
+		adap_emac->RX0CP = cnt;
+		adap_emac->RX0HDP = 0;
+	}
+
+	debug_emac("- emac_ch_teardown\n");
+}
+
+/* Eth device close */
+static int dm644x_eth_close(void)
+{
+	debug_emac("+ emac_close\n");
+
+	dm644x_eth_ch_teardown(EMAC_CH_TX);	/* TX Channel teardown */
+	dm644x_eth_ch_teardown(EMAC_CH_RX);	/* RX Channel teardown */
+
+	/* Reset EMAC module and disable interrupts in wrapper */
+	adap_emac->SOFTRESET = 1;
+	adap_ewrap->EWCTL = 0;
+
+	debug_emac("- emac_close\n");
+	return(1);
+}
+
+static int tx_send_loop = 0;
+
+/*
+ * This function sends a single packet on the network and returns
+ * positive number (number of bytes transmitted) or negative for error
+ */
+static int dm644x_eth_send_packet(volatile void *packet, int length)
+{
+	int ret_status = -1;
+	tx_send_loop = 0;
+
+	/* Return error if no link */
+	if (!phy.get_link_speed(active_phy_addr))
+	{
+		printf("WARN: emac_send_packet: No link\n");
+		return (ret_status);
+	}
+
+	/* Check packet size and if < EMAC_MIN_ETHERNET_PKT_SIZE, pad it up */
+	if (length < EMAC_MIN_ETHERNET_PKT_SIZE)
+	{
+		length = EMAC_MIN_ETHERNET_PKT_SIZE;
+	}
+
+	/* Populate the TX descriptor */
+	emac_tx_desc->next         = 0;
+	emac_tx_desc->buffer       = (u_int8_t *)packet;
+	emac_tx_desc->buff_off_len = (length & 0xffff);
+	emac_tx_desc->pkt_flag_len = ((length & 0xffff) |
+			EMAC_CPPI_SOP_BIT |
+			EMAC_CPPI_OWNERSHIP_BIT |
+			EMAC_CPPI_EOP_BIT);
+	/* Send the packet */
+	adap_emac->TX0HDP = (unsigned int)emac_tx_desc;
+
+	/* Wait for packet to complete or link down */
+	while (1) {
+	        if (!phy.get_link_speed(active_phy_addr)) {
+	        	dm644x_eth_ch_teardown(EMAC_CH_TX);
+	        	return (ret_status);
+	        }
+	        if (adap_emac->TXINTSTATRAW & 0x01) {
+	        	ret_status = length;
+	        	break;
+		}
+	        tx_send_loop++;
+	}
+
+	return(ret_status);
+}
+
+/*
+ * This function handles receipt of a packet from the network
+ */
+static int dm644x_eth_rcv_packet(void)
+{
+	volatile emac_desc	*rx_curr_desc;
+	volatile emac_desc	*curr_desc;
+	volatile emac_desc	*tail_desc;
+	int			status, ret = -1;
+
+	rx_curr_desc = emac_rx_active_head;
+	status = rx_curr_desc->pkt_flag_len;
+	if ((rx_curr_desc) && ((status & EMAC_CPPI_OWNERSHIP_BIT) == 0)) {
+	        if (status & EMAC_CPPI_RX_ERROR_FRAME) {
+	        	/* Error in packet - discard it and requeue desc */
+	        	printf("WARN: emac_rcv_pkt: Error in packet\n");
+		} else {
+			NetReceive(rx_curr_desc->buffer, (rx_curr_desc->buff_off_len & 0xffff));
+			ret = rx_curr_desc->buff_off_len & 0xffff;
+	        }
+
+	        /* Ack received packet descriptor */
+	        adap_emac->RX0CP = (unsigned int)rx_curr_desc;
+	        curr_desc = rx_curr_desc;
+	        emac_rx_active_head = (volatile emac_desc *)rx_curr_desc->next;
+
+	        if (status & EMAC_CPPI_EOQ_BIT) {
+	        	if (emac_rx_active_head) {
+	        		adap_emac->RX0HDP = (unsigned int)emac_rx_active_head;
+			} else {
+				emac_rx_queue_active = 0;
+				printf("INFO:emac_rcv_packet: RX Queue not active\n");
+			}
+		}
+
+		/* Recycle RX descriptor */
+		rx_curr_desc->buff_off_len = EMAC_MAX_ETHERNET_PKT_SIZE;
+		rx_curr_desc->pkt_flag_len = EMAC_CPPI_OWNERSHIP_BIT;
+		rx_curr_desc->next = 0;
+
+		if (emac_rx_active_head == 0) {
+			printf("INFO: emac_rcv_pkt: active queue head = 0\n");
+			emac_rx_active_head = curr_desc;
+			emac_rx_active_tail = curr_desc;
+			if (emac_rx_queue_active != 0) {
+				adap_emac->RX0HDP = (unsigned int)emac_rx_active_head;
+				printf("INFO: emac_rcv_pkt: active queue head = 0, HDP fired\n");
+				emac_rx_queue_active = 1;
+			}
+		} else {
+			tail_desc = emac_rx_active_tail;
+			emac_rx_active_tail = curr_desc;
+			tail_desc->next = (unsigned int)curr_desc;
+			status = tail_desc->pkt_flag_len;
+			if (status & EMAC_CPPI_EOQ_BIT) {
+				adap_emac->RX0HDP = (unsigned int)curr_desc;
+				status &= ~EMAC_CPPI_EOQ_BIT;
+				tail_desc->pkt_flag_len = status;
+			}
+		}
+		return(ret);
+	}
+	return(0);
+}
+
+#endif /* CONFIG_CMD_NET */
+
+#endif /* CONFIG_DRIVER_TI_EMAC */
diff --git a/cpu/arm926ejs/davinci/i2c.c b/cpu/arm926ejs/davinci/i2c.c
new file mode 100644
index 0000000000..af9dc034c2
--- /dev/null
+++ b/cpu/arm926ejs/davinci/i2c.c
@@ -0,0 +1,351 @@
+/*
+ * TI DaVinci (TMS320DM644x) I2C driver.
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * --------------------------------------------------------
+ *
+ * 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>
+
+#ifdef CONFIG_DRIVER_DAVINCI_I2C
+
+#include <i2c.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/i2c_defs.h>
+
+#define CHECK_NACK() \
+	do {\
+		if (tmp & (I2C_TIMEOUT | I2C_STAT_NACK)) {\
+			REG(I2C_CON) = 0;\
+			return(1);\
+		}\
+	} while (0)
+
+
+static int wait_for_bus(void)
+{
+	int	stat, timeout;
+
+	REG(I2C_STAT) = 0xffff;
+
+	for (timeout = 0; timeout < 10; timeout++) {
+		if (!((stat = REG(I2C_STAT)) & I2C_STAT_BB)) {
+			REG(I2C_STAT) = 0xffff;
+			return(0);
+		}
+
+		REG(I2C_STAT) = stat;
+		udelay(50000);
+	}
+
+	REG(I2C_STAT) = 0xffff;
+	return(1);
+}
+
+
+static int poll_i2c_irq(int mask)
+{
+	int	stat, timeout;
+
+	for (timeout = 0; timeout < 10; timeout++) {
+		udelay(1000);
+		stat = REG(I2C_STAT);
+		if (stat & mask) {
+			return(stat);
+		}
+	}
+
+	REG(I2C_STAT) = 0xffff;
+	return(stat | I2C_TIMEOUT);
+}
+
+
+void flush_rx(void)
+{
+	int	dummy;
+
+	while (1) {
+		if (!(REG(I2C_STAT) & I2C_STAT_RRDY))
+			break;
+
+		dummy = REG(I2C_DRR);
+		REG(I2C_STAT) = I2C_STAT_RRDY;
+		udelay(1000);
+	}
+}
+
+
+void i2c_init(int speed, int slaveadd)
+{
+	u_int32_t	div, psc;
+
+	if (REG(I2C_CON) & I2C_CON_EN) {
+		REG(I2C_CON) = 0;
+		udelay (50000);
+	}
+
+	psc = 2;
+	div = (CFG_HZ_CLOCK / ((psc + 1) * speed)) - 10;	/* SCLL + SCLH */
+	REG(I2C_PSC) = psc;			/* 27MHz / (2 + 1) = 9MHz */
+	REG(I2C_SCLL) = (div * 50) / 100;	/* 50% Duty */
+	REG(I2C_SCLH) = div - REG(I2C_SCLL);
+
+	REG(I2C_OA) = slaveadd;
+	REG(I2C_CNT) = 0;
+
+	/* Interrupts must be enabled or I2C module won't work */
+	REG(I2C_IE) = I2C_IE_SCD_IE | I2C_IE_XRDY_IE |
+		I2C_IE_RRDY_IE | I2C_IE_ARDY_IE | I2C_IE_NACK_IE;
+
+	/* Now enable I2C controller (get it out of reset) */
+	REG(I2C_CON) = I2C_CON_EN;
+
+	udelay(1000);
+}
+
+
+int i2c_probe(u_int8_t chip)
+{
+	int	rc = 1;
+
+	if (chip == REG(I2C_OA)) {
+		return(rc);
+	}
+
+	REG(I2C_CON) = 0;
+	if (wait_for_bus()) {return(1);}
+
+	/* try to read one byte from current (or only) address */
+	REG(I2C_CNT) = 1;
+	REG(I2C_SA) = chip;
+	REG(I2C_CON) = (I2C_CON_EN | I2C_CON_MST | I2C_CON_STT | I2C_CON_STP);
+	udelay (50000);
+
+	if (!(REG(I2C_STAT) & I2C_STAT_NACK)) {
+		rc = 0;
+		flush_rx();
+		REG(I2C_STAT) = 0xffff;
+	} else {
+		REG(I2C_STAT) = 0xffff;
+		REG(I2C_CON) |= I2C_CON_STP;
+		udelay(20000);
+		if (wait_for_bus()) {return(1);}
+	}
+
+	flush_rx();
+	REG(I2C_STAT) = 0xffff;
+	REG(I2C_CNT) = 0;
+	return(rc);
+}
+
+
+int i2c_read(u_int8_t chip, u_int32_t addr, int alen, u_int8_t *buf, int len)
+{
+	u_int32_t	tmp;
+	int		i;
+
+	if ((alen < 0) || (alen > 2)) {
+		printf("%s(): bogus address length %x\n", __FUNCTION__, alen);
+		return(1);
+	}
+
+	if (wait_for_bus()) {return(1);}
+
+	if (alen != 0) {
+		/* Start address phase */
+		tmp = I2C_CON_EN | I2C_CON_MST | I2C_CON_STT | I2C_CON_TRX;
+		REG(I2C_CNT) = alen;
+		REG(I2C_SA) = chip;
+		REG(I2C_CON) = tmp;
+
+		tmp = poll_i2c_irq(I2C_STAT_XRDY | I2C_STAT_NACK);
+
+		CHECK_NACK();
+
+		switch (alen) {
+			case 2:
+				/* Send address MSByte */
+				if (tmp & I2C_STAT_XRDY) {
+					REG(I2C_DXR) = (addr >> 8) & 0xff;
+				} else {
+					REG(I2C_CON) = 0;
+					return(1);
+				}
+
+				tmp = poll_i2c_irq(I2C_STAT_XRDY | I2C_STAT_NACK);
+
+				CHECK_NACK();
+				/* No break, fall through */
+			case 1:
+				/* Send address LSByte */
+				if (tmp & I2C_STAT_XRDY) {
+					REG(I2C_DXR) = addr & 0xff;
+				} else {
+					REG(I2C_CON) = 0;
+					return(1);
+				}
+
+				tmp = poll_i2c_irq(I2C_STAT_XRDY | I2C_STAT_NACK | I2C_STAT_ARDY);
+
+				CHECK_NACK();
+
+				if (!(tmp & I2C_STAT_ARDY)) {
+					REG(I2C_CON) = 0;
+					return(1);
+				}
+		}
+	}
+
+	/* Address phase is over, now read 'len' bytes and stop */
+	tmp = I2C_CON_EN | I2C_CON_MST | I2C_CON_STT | I2C_CON_STP;
+	REG(I2C_CNT) = len & 0xffff;
+	REG(I2C_SA) = chip;
+	REG(I2C_CON) = tmp;
+
+	for (i = 0; i < len; i++) {
+		tmp = poll_i2c_irq(I2C_STAT_RRDY | I2C_STAT_NACK | I2C_STAT_ROVR);
+
+		CHECK_NACK();
+
+		if (tmp & I2C_STAT_RRDY) {
+			buf[i] = REG(I2C_DRR);
+		} else {
+			REG(I2C_CON) = 0;
+			return(1);
+		}
+	}
+
+	tmp = poll_i2c_irq(I2C_STAT_SCD | I2C_STAT_NACK);
+
+	CHECK_NACK();
+
+	if (!(tmp & I2C_STAT_SCD)) {
+		REG(I2C_CON) = 0;
+		return(1);
+	}
+
+	flush_rx();
+	REG(I2C_STAT) = 0xffff;
+	REG(I2C_CNT) = 0;
+	REG(I2C_CON) = 0;
+
+	return(0);
+}
+
+
+int i2c_write(u_int8_t chip, u_int32_t addr, int alen, u_int8_t *buf, int len)
+{
+	u_int32_t	tmp;
+	int		i;
+
+	if ((alen < 0) || (alen > 2)) {
+		printf("%s(): bogus address length %x\n", __FUNCTION__, alen);
+		return(1);
+	}
+	if (len < 0) {
+		printf("%s(): bogus length %x\n", __FUNCTION__, len);
+		return(1);
+	}
+
+	if (wait_for_bus()) {return(1);}
+
+	/* Start address phase */
+	tmp = I2C_CON_EN | I2C_CON_MST | I2C_CON_STT | I2C_CON_TRX | I2C_CON_STP;
+	REG(I2C_CNT) = (alen == 0) ? len & 0xffff : (len & 0xffff) + alen;
+	REG(I2C_SA) = chip;
+	REG(I2C_CON) = tmp;
+
+	switch (alen) {
+		case 2:
+			/* Send address MSByte */
+			tmp = poll_i2c_irq(I2C_STAT_XRDY | I2C_STAT_NACK);
+
+			CHECK_NACK();
+
+			if (tmp & I2C_STAT_XRDY) {
+				REG(I2C_DXR) = (addr >> 8) & 0xff;
+			} else {
+				REG(I2C_CON) = 0;
+				return(1);
+			}
+			/* No break, fall through */
+		case 1:
+			/* Send address LSByte */
+			tmp = poll_i2c_irq(I2C_STAT_XRDY | I2C_STAT_NACK);
+
+			CHECK_NACK();
+
+			if (tmp & I2C_STAT_XRDY) {
+				REG(I2C_DXR) = addr & 0xff;
+			} else {
+				REG(I2C_CON) = 0;
+				return(1);
+			}
+	}
+
+	for (i = 0; i < len; i++) {
+		tmp = poll_i2c_irq(I2C_STAT_XRDY | I2C_STAT_NACK);
+
+		CHECK_NACK();
+
+		if (tmp & I2C_STAT_XRDY) {
+			REG(I2C_DXR) = buf[i];
+		} else {
+			return(1);
+		}
+	}
+
+	tmp = poll_i2c_irq(I2C_STAT_SCD | I2C_STAT_NACK);
+
+	CHECK_NACK();
+
+	if (!(tmp & I2C_STAT_SCD)) {
+		REG(I2C_CON) = 0;
+		return(1);
+	}
+
+	flush_rx();
+	REG(I2C_STAT) = 0xffff;
+	REG(I2C_CNT) = 0;
+	REG(I2C_CON) = 0;
+
+	return(0);
+}
+
+
+u_int8_t i2c_reg_read(u_int8_t chip, u_int8_t reg)
+{
+	u_int8_t	tmp;
+
+	i2c_read(chip, reg, 1, &tmp, 1);
+	return(tmp);
+}
+
+
+void i2c_reg_write(u_int8_t chip, u_int8_t reg, u_int8_t val)
+{
+	u_int8_t	tmp;
+
+	i2c_write(chip, reg, 1, &tmp, 1);
+}
+
+#endif /* CONFIG_DRIVER_DAVINCI_I2C */
diff --git a/cpu/arm926ejs/davinci/lowlevel_init.S b/cpu/arm926ejs/davinci/lowlevel_init.S
new file mode 100644
index 0000000000..a87c112eca
--- /dev/null
+++ b/cpu/arm926ejs/davinci/lowlevel_init.S
@@ -0,0 +1,707 @@
+/*
+ * Low-level board setup code for TI DaVinci SoC based boards.
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * Partially based on TI sources, original copyrights follow:
+ */
+
+/*
+ * Board specific setup info
+ *
+ * (C) Copyright 2003
+ * Texas Instruments, <www.ti.com>
+ * Kshitij Gupta <Kshitij@ti.com>
+ *
+ * Modified for OMAP 1610 H2 board by Nishant Kamat, Jan 2004
+ *
+ * Modified for OMAP 5912 OSK board by Rishi Bhattacharya, Apr 2004
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * Modified for DV-EVM board by Rishi Bhattacharya, Apr 2005
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * Modified for DV-EVM board by Swaminathan S, Nov 2005
+ * 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 <config.h>
+
+.globl	lowlevel_init
+lowlevel_init:
+
+	/*-------------------------------------------------------*
+	 * Mask all IRQs by setting all bits in the EINT default *
+	 *-------------------------------------------------------*/
+	mov	r1, $0
+	ldr	r0, =EINT_ENABLE0
+	str	r1, [r0]
+	ldr	r0, =EINT_ENABLE1
+	str	r1, [r0]
+
+	/*------------------------------------------------------*
+	 * Put the GEM in reset					*
+	 *------------------------------------------------------*/
+
+	/* Put the GEM in reset */
+	ldr	r8, PSC_GEM_FLAG_CLEAR
+	ldr	r6, MDCTL_GEM
+	ldr	r7, [r6]
+	and	r7, r7, r8
+	str	r7, [r6]
+
+	/* Enable the Power Domain Transition Command */
+	ldr	r6, PTCMD
+	ldr	r7, [r6]
+	orr	r7, r7, $0x02
+	str	r7, [r6]
+
+	/* Check for Transition Complete(PTSTAT) */
+checkStatClkStopGem:
+	ldr	r6, PTSTAT
+	ldr	r7, [r6]
+	ands	r7, r7, $0x02
+	bne	checkStatClkStopGem
+
+	/* Check for GEM Reset Completion */
+checkGemStatClkStop:
+	ldr	r6, MDSTAT_GEM
+	ldr	r7, [r6]
+	ands	r7, r7, $0x100
+	bne	checkGemStatClkStop
+
+	/* Do this for enabling a WDT initiated reset this is a workaround
+	   for a chip bug.  Not required under normal situations */
+	ldr	r6, P1394
+	mov	r10, $0
+	str	r10, [r6]
+
+	/*------------------------------------------------------*
+	 * Enable L1 & L2 Memories in Fast mode                 *
+	 *------------------------------------------------------*/
+	ldr	r6, DFT_ENABLE
+	mov	r10, $0x01
+	str	r10, [r6]
+
+	ldr	r6, MMARG_BRF0
+	ldr	r10, MMARG_BRF0_VAL
+	str	r10, [r6]
+
+	ldr	r6, DFT_ENABLE
+	mov	r10, $0
+	str	r10, [r6]
+
+	/*------------------------------------------------------*
+	 * DDR2 PLL Initialization			    	*
+	 *------------------------------------------------------*/
+
+	/* Select the Clock Mode Depending on the Value written in the Boot Table by the run script */
+	mov	r10, $0
+	ldr	r6, PLL2_CTL
+	ldr	r7, PLL_CLKSRC_MASK
+	ldr	r8, [r6]
+	and	r8, r8, r7
+	mov	r9, r10, lsl $8
+	orr	r8, r8, r9
+	str	r8, [r6]
+
+	/* Select the PLLEN source */
+	ldr	r7, PLL_ENSRC_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Bypass the PLL */
+	ldr	r7, PLL_BYPASS_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Wait for few cycles to allow PLLEN Mux switch properly to bypass Clock */
+	mov	r10, $0x20
+WaitPPL2Loop:
+	subs	r10, r10, $1
+	bne	WaitPPL2Loop
+
+	/* Reset the PLL */
+	ldr	r7, PLL_RESET_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Power up the PLL */
+	ldr	r7, PLL_PWRUP_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Enable the PLL from Disable Mode */
+	ldr	r7, PLL_DISABLE_ENABLE_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Program the PLL Multiplier */
+	ldr	r6, PLL2_PLLM
+	mov	r2, $0x17	/* 162 MHz */
+	str	r2, [r6]
+
+	/* Program the PLL2 Divisor Value */
+	ldr	r6, PLL2_DIV2
+	mov	r3, $0x01
+	str	r3, [r6]
+
+	/* Program the PLL2 Divisor Value */
+	ldr	r6, PLL2_DIV1
+	mov	r4, $0x0b	/* 54 MHz */
+	str	r4, [r6]
+
+	/* PLL2 DIV2 MMR */
+	ldr	r8, PLL2_DIV_MASK
+	ldr	r6, PLL2_DIV2
+	ldr	r9, [r6]
+	and	r8, r8, r9
+	mov	r9, $0x01
+	mov	r9, r9, lsl $15
+	orr	r8, r8, r9
+	str	r8, [r6]
+
+	/* Program the GOSET bit to take new divider values */
+	ldr	r6, PLL2_PLLCMD
+	ldr	r7, [r6]
+	orr	r7, r7, $0x01
+	str	r7, [r6]
+
+	/* Wait for Done */
+	ldr	r6, PLL2_PLLSTAT
+doneLoop_0:
+	ldr	r7, [r6]
+	ands	r7, r7, $0x01
+	bne	doneLoop_0
+
+	/* PLL2 DIV1 MMR */
+	ldr	r8, PLL2_DIV_MASK
+	ldr	r6, PLL2_DIV1
+	ldr	r9, [r6]
+	and	r8, r8, r9
+	mov	r9, $0x01
+	mov	r9, r9, lsl $15
+	orr	r8, r8, r9
+	str	r8, [r6]
+
+	/* Program the GOSET bit to take new divider values */
+	ldr	r6, PLL2_PLLCMD
+	ldr	r7, [r6]
+	orr	r7, r7, $0x01
+	str	r7, [r6]
+
+	/* Wait for Done */
+	ldr	r6, PLL2_PLLSTAT
+doneLoop:
+	ldr	r7, [r6]
+	ands	r7, r7, $0x01
+	bne	doneLoop
+
+	/* Wait for PLL to Reset Properly */
+	mov	r10, $0x218
+ResetPPL2Loop:
+	subs	r10, r10, $1
+	bne	ResetPPL2Loop
+
+	/* Bring PLL out of Reset */
+	ldr	r6, PLL2_CTL
+	ldr	r8, [r6]
+	orr	r8, r8, $0x08
+	str	r8, [r6]
+
+	/* Wait for PLL to Lock */
+	ldr	r10, PLL_LOCK_COUNT
+PLL2Lock:
+	subs	r10, r10, $1
+	bne	PLL2Lock
+
+	/* Enable the PLL */
+	ldr	r6, PLL2_CTL
+	ldr	r8, [r6]
+	orr	r8, r8, $0x01
+	str	r8, [r6]
+
+	/*------------------------------------------------------*
+	 * Issue Soft Reset to DDR Module			*
+	 *------------------------------------------------------*/
+
+	/* Shut down the DDR2 LPSC Module */
+	ldr	r8, PSC_FLAG_CLEAR
+	ldr	r6, MDCTL_DDR2
+	ldr	r7, [r6]
+	and	r7, r7, r8
+	orr	r7, r7, $0x03
+	str	r7, [r6]
+
+	/* Enable the Power Domain Transition Command */
+	ldr	r6, PTCMD
+	ldr	r7, [r6]
+	orr	r7, r7, $0x01
+	str	r7, [r6]
+
+	/* Check for Transition Complete(PTSTAT) */
+checkStatClkStop:
+	ldr	r6, PTSTAT
+	ldr	r7, [r6]
+	ands	r7, r7, $0x01
+	bne	checkStatClkStop
+
+	/* Check for DDR2 Controller Enable Completion */
+checkDDRStatClkStop:
+	ldr	r6, MDSTAT_DDR2
+	ldr	r7, [r6]
+	and	r7, r7, $0x1f
+	cmp	r7, $0x03
+	bne	checkDDRStatClkStop
+
+	/*------------------------------------------------------*
+	 * Program DDR2 MMRs for 162MHz Setting			*
+	 *------------------------------------------------------*/
+
+	/* Program PHY Control Register */
+	ldr	r6, DDRCTL
+	ldr	r7, DDRCTL_VAL
+	str	r7, [r6]
+
+	/* Program SDRAM Bank Config Register */
+	ldr	r6, SDCFG
+	ldr	r7, SDCFG_VAL
+	str	r7, [r6]
+
+	/* Program SDRAM TIM-0 Config Register */
+	ldr	r6, SDTIM0
+	ldr	r7, SDTIM0_VAL_162MHz
+	str	r7, [r6]
+
+	/* Program SDRAM TIM-1 Config Register */
+	ldr	r6, SDTIM1
+	ldr	r7, SDTIM1_VAL_162MHz
+	str	r7, [r6]
+
+	/* Program the SDRAM Bank Config Control Register */
+	ldr	r10, MASK_VAL
+	ldr	r8, SDCFG
+	ldr	r9, SDCFG_VAL
+	and	r9, r9, r10
+	str	r9, [r8]
+
+	/* Program SDRAM SDREF Config Register */
+	ldr	r6, SDREF
+	ldr	r7, SDREF_VAL
+	str	r7, [r6]
+
+	/*------------------------------------------------------*
+	 * Issue Soft Reset to DDR Module			*
+	 *------------------------------------------------------*/
+
+	/* Issue a Dummy DDR2 read/write */
+	ldr	r8, DDR2_START_ADDR
+	ldr	r7, DUMMY_VAL
+	str	r7, [r8]
+	ldr	r7, [r8]
+
+	/* Shut down the DDR2 LPSC Module */
+	ldr	r8, PSC_FLAG_CLEAR
+	ldr	r6, MDCTL_DDR2
+	ldr	r7, [r6]
+	and	r7, r7, r8
+	orr	r7, r7, $0x01
+	str	r7, [r6]
+
+	/* Enable the Power Domain Transition Command */
+	ldr	r6, PTCMD
+	ldr	r7, [r6]
+	orr	r7, r7, $0x01
+	str	r7, [r6]
+
+	/* Check for Transition Complete(PTSTAT) */
+checkStatClkStop2:
+	ldr	r6, PTSTAT
+	ldr	r7, [r6]
+	ands	r7, r7, $0x01
+	bne	checkStatClkStop2
+
+	/* Check for DDR2 Controller Enable Completion */
+checkDDRStatClkStop2:
+	ldr	r6, MDSTAT_DDR2
+	ldr	r7, [r6]
+	and	r7, r7, $0x1f
+	cmp	r7, $0x01
+	bne	checkDDRStatClkStop2
+
+	/*------------------------------------------------------*
+	 * Turn DDR2 Controller Clocks On			*
+	 *------------------------------------------------------*/
+
+	/* Enable the DDR2 LPSC Module */
+	ldr	r6, MDCTL_DDR2
+	ldr	r7, [r6]
+	orr	r7, r7, $0x03
+	str	r7, [r6]
+
+	/* Enable the Power Domain Transition Command */
+	ldr	r6, PTCMD
+	ldr	r7, [r6]
+	orr	r7, r7, $0x01
+	str	r7, [r6]
+
+	/* Check for Transition Complete(PTSTAT) */
+checkStatClkEn2:
+	ldr	r6, PTSTAT
+	ldr	r7, [r6]
+	ands	r7, r7, $0x01
+	bne	checkStatClkEn2
+
+	/* Check for DDR2 Controller Enable Completion */
+checkDDRStatClkEn2:
+	ldr	r6, MDSTAT_DDR2
+	ldr	r7, [r6]
+	and	r7, r7, $0x1f
+	cmp	r7, $0x03
+	bne	checkDDRStatClkEn2
+
+	/*  DDR Writes and Reads */
+	ldr	r6, CFGTEST
+	mov	r3, $0x01
+	str	r3, [r6]
+
+	/*------------------------------------------------------*
+	 * System PLL Initialization				*
+	 *------------------------------------------------------*/
+
+	/* Select the Clock Mode Depending on the Value written in the Boot Table by the run script */
+	mov	r2, $0
+	ldr	r6, PLL1_CTL
+	ldr	r7, PLL_CLKSRC_MASK
+	ldr	r8, [r6]
+	and	r8, r8, r7
+	mov	r9, r2, lsl $8
+	orr	r8, r8, r9
+	str	r8, [r6]
+
+	/* Select the PLLEN source */
+	ldr	r7, PLL_ENSRC_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Bypass the PLL */
+	ldr	r7, PLL_BYPASS_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Wait for few cycles to allow PLLEN Mux switch properly to bypass Clock */
+	mov	r10, $0x20
+
+WaitLoop:
+	subs	r10, r10, $1
+	bne	WaitLoop
+
+	/* Reset the PLL */
+	ldr	r7, PLL_RESET_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Disable the PLL */
+	orr	r8, r8, $0x10
+	str	r8, [r6]
+
+	/* Power up the PLL */
+	ldr	r7, PLL_PWRUP_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Enable the PLL from Disable Mode */
+	ldr	r7, PLL_DISABLE_ENABLE_MASK
+	and	r8, r8, r7
+	str	r8, [r6]
+
+	/* Program the PLL Multiplier */
+	ldr	r6, PLL1_PLLM
+	mov	r3, $0x15	/* For 594MHz */
+	str	r3, [r6]
+
+	/* Wait for PLL to Reset Properly */
+	mov	r10, $0xff
+
+ResetLoop:
+	subs	r10, r10, $1
+	bne	ResetLoop
+
+	/* Bring PLL out of Reset */
+	ldr	r6, PLL1_CTL
+	orr	r8, r8, $0x08
+	str	r8, [r6]
+
+	/* Wait for PLL to Lock */
+	ldr	r10, PLL_LOCK_COUNT
+
+PLL1Lock:
+	subs	r10, r10, $1
+	bne	PLL1Lock
+
+	/* Enable the PLL */
+	orr	r8, r8, $0x01
+	str	r8, [r6]
+
+	nop
+	nop
+	nop
+	nop
+
+	/*------------------------------------------------------*
+	 * AEMIF configuration for NOR Flash (double check)     *
+	 *------------------------------------------------------*/
+	ldr	r0, _PINMUX0
+	ldr	r1, _DEV_SETTING
+	str	r1, [r0]
+
+	ldr	r0, WAITCFG
+	ldr	r1, WAITCFG_VAL
+	ldr	r2, [r0]
+	orr	r2, r2, r1
+	str	r2, [r0]
+
+	ldr	r0, ACFG3
+	ldr	r1, ACFG3_VAL
+	ldr	r2, [r0]
+	and	r1, r2, r1
+	str	r1, [r0]
+
+	ldr	r0, ACFG4
+	ldr	r1, ACFG4_VAL
+	ldr	r2, [r0]
+	and	r1, r2, r1
+	str	r1, [r0]
+
+	ldr	r0, ACFG5
+	ldr	r1, ACFG5_VAL
+	ldr	r2, [r0]
+	and	r1, r2, r1
+	str	r1, [r0]
+
+	/*--------------------------------------*
+	 * VTP manual Calibration               *
+	 *--------------------------------------*/
+	ldr	r0, VTPIOCR
+	ldr	r1, VTP_MMR0
+	str	r1, [r0]
+
+	ldr	r0, VTPIOCR
+	ldr	r1, VTP_MMR1
+	str	r1, [r0]
+
+	/* Wait for 33 VTP CLK cycles.  VRP operates at 27 MHz */
+	ldr	r10, VTP_LOCK_COUNT
+VTPLock:
+	subs	r10, r10, $1
+	bne	VTPLock
+
+	ldr	r6, DFT_ENABLE
+	mov	r10, $0x01
+	str	r10, [r6]
+
+	ldr	r6, DDRVTPR
+	ldr	r7, [r6]
+	and	r7, r7, $0x1f
+	and	r8, r7, $0x3e0
+	orr	r8, r7, r8
+	ldr	r7, VTP_RECAL
+	orr	r8, r7, r8
+	ldr	r7, VTP_EN
+	orr	r8, r7, r8
+	str	r8, [r0]
+
+
+	/* Wait for 33 VTP CLK cycles.  VRP operates at 27 MHz */
+	ldr	r10, VTP_LOCK_COUNT
+VTP1Lock:
+	subs	r10, r10, $1
+	bne	VTP1Lock
+
+	ldr	r1, [r0]
+	ldr	r2, VTP_MASK
+	and	r2, r1, r2
+	str	r2, [r0]
+
+	ldr	r6, DFT_ENABLE
+	mov	r10, $0
+	str	r10, [r6]
+
+	/*
+	 * Call board-specific lowlevel init.
+ 	 * That MUST be present and THAT returns
+	 * back to arch calling code with "mov pc, lr."
+	 */
+	b	dv_board_init
+
+.ltorg
+
+_PINMUX0:
+	.word	0x01c40000		/* Device Configuration Registers */
+_PINMUX1:
+	.word	0x01c40004		/* Device Configuration Registers */
+
+_DEV_SETTING:
+	.word	0x00000c1f
+
+WAITCFG:
+	.word	0x01e00004
+WAITCFG_VAL:
+	.word	0
+ACFG3:
+	.word	0x01e00014
+ACFG3_VAL:
+	.word	0x3ffffffd
+ACFG4:
+	.word	0x01e00018
+ACFG4_VAL:
+	.word	0x3ffffffd
+ACFG5:
+	.word	0x01e0001c
+ACFG5_VAL:
+	.word	0x3ffffffd
+
+MDCTL_DDR2:
+	.word	0x01c41a34
+MDSTAT_DDR2:
+	.word	0x01c41834
+
+PTCMD:
+	.word	0x01c41120
+PTSTAT:
+	.word	0x01c41128
+
+EINT_ENABLE0:
+	.word	0x01c48018
+EINT_ENABLE1:
+	.word	0x01c4801c
+
+PSC_FLAG_CLEAR:
+	.word	0xffffffe0
+PSC_GEM_FLAG_CLEAR:
+	.word	0xfffffeff
+
+/* DDR2 MMR & CONFIGURATION VALUES, 162 MHZ clock */
+DDRCTL:
+	.word	0x200000e4
+DDRCTL_VAL:
+	.word	0x50006405
+SDREF:
+	.word	0x2000000c
+SDREF_VAL:
+	.word	0x000005c3
+SDCFG:
+	.word	0x20000008
+SDCFG_VAL:
+#ifdef	DDR_4BANKS
+	.word	0x00178622
+#elif defined DDR_8BANKS
+	.word	0x00178632
+#else
+#error "Unknown DDR configuration!!!"
+#endif
+SDTIM0:
+	.word	0x20000010
+SDTIM0_VAL_162MHz:
+	.word	0x28923211
+SDTIM1:
+	.word	0x20000014
+SDTIM1_VAL_162MHz:
+	.word	0x0016c722
+VTPIOCR:
+	.word	0x200000f0	/* VTP IO Control register */
+DDRVTPR:
+	.word	0x01c42030	/* DDR VPTR MMR */
+VTP_MMR0:
+	.word	0x201f
+VTP_MMR1:
+	.word	0xa01f
+DFT_ENABLE:
+	.word	0x01c4004c
+VTP_LOCK_COUNT:
+	.word	0x5b0
+VTP_MASK:
+	.word	0xffffdfff
+VTP_RECAL:
+	.word	0x40000
+VTP_EN:
+	.word	0x02000
+CFGTEST:
+	.word	0x80010000
+MASK_VAL:
+	.word	0x00000fff
+
+/* GEM Power Up & LPSC Control Register */
+MDCTL_GEM:
+	.word	0x01c41a9c
+MDSTAT_GEM:
+	.word	0x01c4189c
+
+/* For WDT reset chip bug */
+P1394:
+	.word	0x01c41a20
+
+PLL_CLKSRC_MASK:
+	.word	0xfffffeff	/* Mask the Clock Mode bit */
+PLL_ENSRC_MASK:
+	.word	0xffffffdf	/* Select the PLLEN source */
+PLL_BYPASS_MASK:
+	.word	0xfffffffe	/* Put the PLL in BYPASS */
+PLL_RESET_MASK:
+	.word	0xfffffff7	/* Put the PLL in Reset Mode */
+PLL_PWRUP_MASK:
+	.word	0xfffffffd	/* PLL Power up Mask Bit  */
+PLL_DISABLE_ENABLE_MASK:
+	.word	0xffffffef	/* Enable the PLL from Disable */
+PLL_LOCK_COUNT:
+	.word	0x2000
+
+/* PLL1-SYSTEM PLL MMRs */
+PLL1_CTL:
+	.word	0x01c40900
+PLL1_PLLM:
+	.word	0x01c40910
+
+/* PLL2-SYSTEM PLL MMRs */
+PLL2_CTL:
+	.word	0x01c40d00
+PLL2_PLLM:
+	.word	0x01c40d10
+PLL2_DIV1:
+	.word	0x01c40d18
+PLL2_DIV2:
+	.word	0x01c40d1c
+PLL2_PLLCMD:
+	.word	0x01c40d38
+PLL2_PLLSTAT:
+	.word	0x01c40d3c
+PLL2_DIV_MASK:
+	.word	0xffff7fff
+
+MMARG_BRF0:
+	.word	0x01c42010	/* BRF margin mode 0 (R/W)*/
+MMARG_BRF0_VAL:
+	.word	0x00444400
+
+DDR2_START_ADDR:
+	.word	0x80000000
+DUMMY_VAL:
+	.word	0xa55aa55a
diff --git a/cpu/arm926ejs/davinci/lxt972.c b/cpu/arm926ejs/davinci/lxt972.c
new file mode 100644
index 0000000000..6eeb6e5ee9
--- /dev/null
+++ b/cpu/arm926ejs/davinci/lxt972.c
@@ -0,0 +1,142 @@
+/*
+ * Intel LXT971/LXT972 PHY Driver for TI DaVinci
+ * (TMS320DM644x) based boards.
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * --------------------------------------------------------
+ *
+ * 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>
+#include <net.h>
+#include <lxt971a.h>
+#include <asm/arch/emac_defs.h>
+
+#ifdef CONFIG_DRIVER_TI_EMAC
+
+#ifdef CONFIG_CMD_NET
+
+int lxt972_is_phy_connected(int phy_addr)
+{
+	u_int16_t	id1, id2;
+
+	if (!dm644x_eth_phy_read(phy_addr, PHY_COMMON_ID1, &id1))
+		return(0);
+	if (!dm644x_eth_phy_read(phy_addr, PHY_COMMON_ID2, &id2))
+		return(0);
+
+	if ((id1 == (0x0013)) && ((id2  & 0xfff0) == 0x78e0))
+		return(1);
+
+	return(0);
+}
+
+int lxt972_get_link_speed(int phy_addr)
+{
+	u_int16_t		stat1, tmp;
+	volatile emac_regs*	emac = (emac_regs *)EMAC_BASE_ADDR;
+
+	if (!dm644x_eth_phy_read(phy_addr, PHY_LXT971_STAT2, &stat1))
+		return(0);
+
+	if (!(stat1 & PHY_LXT971_STAT2_LINK))	/* link up? */
+		return(0);
+
+	if (!dm644x_eth_phy_read(phy_addr, PHY_LXT971_DIG_CFG, &tmp))
+		return(0);
+
+	tmp |= PHY_LXT971_DIG_CFG_MII_DRIVE;
+
+	dm644x_eth_phy_write(phy_addr, PHY_LXT971_DIG_CFG, tmp);
+	/* Read back */
+	if (!dm644x_eth_phy_read(phy_addr, PHY_LXT971_DIG_CFG, &tmp))
+		return(0);
+
+
+	/* Speed doesn't matter, there is no setting for it in EMAC... */
+	if (stat1 & PHY_LXT971_STAT2_100BTX) {
+		if (stat1 & PHY_LXT971_STAT2_DUPLEX_MODE) {
+			/* set DM644x EMAC for Full Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE | EMAC_MACCONTROL_FULLDUPLEX_ENABLE;
+		} else {
+			/*set DM644x EMAC for Half Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE;
+		}
+
+		return(1);
+	} else {
+		if (stat1 & PHY_LXT971_STAT2_DUPLEX_MODE) {
+			/* set DM644x EMAC for Full Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE | EMAC_MACCONTROL_FULLDUPLEX_ENABLE;
+		} else {
+			/*set DM644x EMAC for Half Duplex  */
+			emac->MACCONTROL = EMAC_MACCONTROL_MIIEN_ENABLE;
+		}
+
+		return(1);
+	}
+
+	return(0);
+}
+
+
+int lxt972_init_phy(int phy_addr)
+{
+	int	ret = 1;
+
+	if (!lxt972_get_link_speed(phy_addr)) {
+		/* Try another time */
+		ret = lxt972_get_link_speed(phy_addr);
+	}
+
+	/* Disable PHY Interrupts */
+	dm644x_eth_phy_write(phy_addr, PHY_LXT971_INT_ENABLE, 0);
+
+	return(ret);
+}
+
+
+int lxt972_auto_negotiate(int phy_addr)
+{
+	u_int16_t	tmp;
+
+
+	if (!dm644x_eth_phy_read(phy_addr, PHY_COMMON_CTRL, &tmp))
+		return(0);
+
+	/* Restart Auto_negotiation  */
+	tmp |= PHY_COMMON_CTRL_RES_AUTO;
+	dm644x_eth_phy_write(phy_addr, PHY_COMMON_CTRL, tmp);
+
+	/*check AutoNegotiate complete */
+	udelay (10000);
+	if (!dm644x_eth_phy_read(phy_addr, PHY_COMMON_STAT, &tmp))
+		return(0);
+
+	if (!(tmp & PHY_COMMON_STAT_AN_COMP))
+		return(0);
+
+	return (lxt972_get_link_speed(phy_addr));
+}
+
+#endif	/* CONFIG_CMD_NET */
+
+#endif	/* CONFIG_DRIVER_ETHER */
diff --git a/cpu/arm926ejs/davinci/nand.c b/cpu/arm926ejs/davinci/nand.c
new file mode 100644
index 0000000000..127be9fcd4
--- /dev/null
+++ b/cpu/arm926ejs/davinci/nand.c
@@ -0,0 +1,389 @@
+/*
+ * NAND driver for TI DaVinci based boards.
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * Based on Linux DaVinci NAND driver by TI. Original copyright follows:
+ */
+
+/*
+ *
+ * linux/drivers/mtd/nand/nand_davinci.c
+ *
+ * NAND Flash Driver
+ *
+ * Copyright (C) 2006 Texas Instruments.
+ *
+ * ----------------------------------------------------------------------------
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ * ----------------------------------------------------------------------------
+ *
+ *  Overview:
+ *   This is a device driver for the NAND flash device found on the
+ *   DaVinci board which utilizes the Samsung k9k2g08 part.
+ *
+ Modifications:
+ ver. 1.0: Feb 2005, Vinod/Sudhakar
+ -
+ *
+ */
+
+#include <common.h>
+
+#ifdef CFG_USE_NAND
+#if !defined(CFG_NAND_LEGACY)
+
+#include <nand.h>
+#include <asm/arch/nand_defs.h>
+#include <asm/arch/emif_defs.h>
+
+extern struct nand_chip nand_dev_desc[CFG_MAX_NAND_DEVICE];
+
+static void nand_davinci_hwcontrol(struct mtd_info *mtd, int cmd)
+{
+	struct		nand_chip *this = mtd->priv;
+	u_int32_t	IO_ADDR_W = (u_int32_t)this->IO_ADDR_W;
+
+	IO_ADDR_W &= ~(MASK_ALE|MASK_CLE);
+
+	switch (cmd) {
+		case NAND_CTL_SETCLE:
+			IO_ADDR_W |= MASK_CLE;
+			break;
+		case NAND_CTL_SETALE:
+			IO_ADDR_W |= MASK_ALE;
+			break;
+	}
+
+	this->IO_ADDR_W = (void *)IO_ADDR_W;
+}
+
+/* Set WP on deselect, write enable on select */
+static void nand_davinci_select_chip(struct mtd_info *mtd, int chip)
+{
+#define GPIO_SET_DATA01	0x01c67018
+#define GPIO_CLR_DATA01	0x01c6701c
+#define GPIO_NAND_WP	(1 << 4)
+#ifdef SONATA_BOARD_GPIOWP
+	if (chip < 0) {
+		REG(GPIO_CLR_DATA01) |= GPIO_NAND_WP;
+	} else {
+		REG(GPIO_SET_DATA01) |= GPIO_NAND_WP;
+	}
+#endif
+}
+
+#ifdef CFG_NAND_HW_ECC
+#ifdef CFG_NAND_LARGEPAGE
+static struct nand_oobinfo davinci_nand_oobinfo = {
+	.useecc = MTD_NANDECC_AUTOPLACE,
+	.eccbytes = 12,
+	.eccpos = {8, 9, 10, 24, 25, 26, 40, 41, 42, 56, 57, 58},
+	.oobfree = { {2, 6}, {12, 12}, {28, 12}, {44, 12}, {60, 4} }
+};
+#elif defined(CFG_NAND_SMALLPAGE)
+static struct nand_oobinfo davinci_nand_oobinfo = {
+	.useecc = MTD_NANDECC_AUTOPLACE,
+	.eccbytes = 3,
+	.eccpos = {0, 1, 2},
+	.oobfree = { {6, 2}, {8, 8} }
+};
+#else
+#error "Either CFG_NAND_LARGEPAGE or CFG_NAND_SMALLPAGE must be defined!"
+#endif
+
+static void nand_davinci_enable_hwecc(struct mtd_info *mtd, int mode)
+{
+	emifregs	emif_addr;
+	int		dummy;
+
+	emif_addr = (emifregs)DAVINCI_ASYNC_EMIF_CNTRL_BASE;
+
+	dummy = emif_addr->NANDF1ECC;
+	dummy = emif_addr->NANDF2ECC;
+	dummy = emif_addr->NANDF3ECC;
+	dummy = emif_addr->NANDF4ECC;
+
+	emif_addr->NANDFCR |= (1 << 8);
+}
+
+static u_int32_t nand_davinci_readecc(struct mtd_info *mtd, u_int32_t region)
+{
+	u_int32_t	ecc = 0;
+	emifregs	emif_base_addr;
+
+	emif_base_addr = (emifregs)DAVINCI_ASYNC_EMIF_CNTRL_BASE;
+
+	if (region == 1)
+		ecc = emif_base_addr->NANDF1ECC;
+	else if (region == 2)
+		ecc = emif_base_addr->NANDF2ECC;
+	else if (region == 3)
+		ecc = emif_base_addr->NANDF3ECC;
+	else if (region == 4)
+		ecc = emif_base_addr->NANDF4ECC;
+
+	return(ecc);
+}
+
+static int nand_davinci_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u_char *ecc_code)
+{
+	u_int32_t		tmp;
+	int			region, n;
+	struct nand_chip	*this = mtd->priv;
+
+	n = (this->eccmode == NAND_ECC_HW12_2048) ? 4 : 1;
+
+	region = 1;
+	while (n--) {
+		tmp = nand_davinci_readecc(mtd, region);
+		*ecc_code++ = tmp;
+		*ecc_code++ = tmp >> 16;
+		*ecc_code++ = ((tmp >> 8) & 0x0f) | ((tmp >> 20) & 0xf0);
+		region++;
+	}
+	return(0);
+}
+
+static void nand_davinci_gen_true_ecc(u_int8_t *ecc_buf)
+{
+	u_int32_t	tmp = ecc_buf[0] | (ecc_buf[1] << 16) | ((ecc_buf[2] & 0xf0) << 20) | ((ecc_buf[2] & 0x0f) << 8);
+
+	ecc_buf[0] = ~(P64o(tmp) | P64e(tmp) | P32o(tmp) | P32e(tmp) | P16o(tmp) | P16e(tmp) | P8o(tmp) | P8e(tmp));
+	ecc_buf[1] = ~(P1024o(tmp) | P1024e(tmp) | P512o(tmp) | P512e(tmp) | P256o(tmp) | P256e(tmp) | P128o(tmp) | P128e(tmp));
+	ecc_buf[2] = ~( P4o(tmp) | P4e(tmp) | P2o(tmp) | P2e(tmp) | P1o(tmp) | P1e(tmp) | P2048o(tmp) | P2048e(tmp));
+}
+
+static int nand_davinci_compare_ecc(u_int8_t *ecc_nand, u_int8_t *ecc_calc, u_int8_t *page_data)
+{
+	u_int32_t	i;
+	u_int8_t	tmp0_bit[8], tmp1_bit[8], tmp2_bit[8];
+	u_int8_t	comp0_bit[8], comp1_bit[8], comp2_bit[8];
+	u_int8_t	ecc_bit[24];
+	u_int8_t	ecc_sum = 0;
+	u_int8_t	find_bit = 0;
+	u_int32_t	find_byte = 0;
+	int		is_ecc_ff;
+
+	is_ecc_ff = ((*ecc_nand == 0xff) && (*(ecc_nand + 1) == 0xff) && (*(ecc_nand + 2) == 0xff));
+
+	nand_davinci_gen_true_ecc(ecc_nand);
+	nand_davinci_gen_true_ecc(ecc_calc);
+
+	for (i = 0; i <= 2; i++) {
+		*(ecc_nand + i) = ~(*(ecc_nand + i));
+		*(ecc_calc + i) = ~(*(ecc_calc + i));
+	}
+
+	for (i = 0; i < 8; i++) {
+		tmp0_bit[i] = *ecc_nand % 2;
+		*ecc_nand = *ecc_nand / 2;
+	}
+
+	for (i = 0; i < 8; i++) {
+		tmp1_bit[i] = *(ecc_nand + 1) % 2;
+		*(ecc_nand + 1) = *(ecc_nand + 1) / 2;
+	}
+
+	for (i = 0; i < 8; i++) {
+		tmp2_bit[i] = *(ecc_nand + 2) % 2;
+		*(ecc_nand + 2) = *(ecc_nand + 2) / 2;
+	}
+
+	for (i = 0; i < 8; i++) {
+		comp0_bit[i] = *ecc_calc % 2;
+		*ecc_calc = *ecc_calc / 2;
+	}
+
+	for (i = 0; i < 8; i++) {
+		comp1_bit[i] = *(ecc_calc + 1) % 2;
+		*(ecc_calc + 1) = *(ecc_calc + 1) / 2;
+	}
+
+	for (i = 0; i < 8; i++) {
+		comp2_bit[i] = *(ecc_calc + 2) % 2;
+		*(ecc_calc + 2) = *(ecc_calc + 2) / 2;
+	}
+
+	for (i = 0; i< 6; i++)
+		ecc_bit[i] = tmp2_bit[i + 2] ^ comp2_bit[i + 2];
+
+	for (i = 0; i < 8; i++)
+		ecc_bit[i + 6] = tmp0_bit[i] ^ comp0_bit[i];
+
+	for (i = 0; i < 8; i++)
+		ecc_bit[i + 14] = tmp1_bit[i] ^ comp1_bit[i];
+
+	ecc_bit[22] = tmp2_bit[0] ^ comp2_bit[0];
+	ecc_bit[23] = tmp2_bit[1] ^ comp2_bit[1];
+
+	for (i = 0; i < 24; i++)
+		ecc_sum += ecc_bit[i];
+
+	switch (ecc_sum) {
+		case 0:
+			/* Not reached because this function is not called if
+			   ECC values are equal */
+			return 0;
+		case 1:
+			/* Uncorrectable error */
+			DEBUG (MTD_DEBUG_LEVEL0, "ECC UNCORRECTED_ERROR 1\n");
+			return(-1);
+		case 12:
+			/* Correctable error */
+			find_byte = (ecc_bit[23] << 8) +
+				(ecc_bit[21] << 7) +
+				(ecc_bit[19] << 6) +
+				(ecc_bit[17] << 5) +
+				(ecc_bit[15] << 4) +
+				(ecc_bit[13] << 3) +
+				(ecc_bit[11] << 2) +
+				(ecc_bit[9]  << 1) +
+				ecc_bit[7];
+
+			find_bit = (ecc_bit[5] << 2) + (ecc_bit[3] << 1) + ecc_bit[1];
+
+			DEBUG (MTD_DEBUG_LEVEL0, "Correcting single bit ECC error at offset: %d, bit: %d\n", find_byte, find_bit);
+
+			page_data[find_byte] ^= (1 << find_bit);
+
+			return(0);
+		default:
+			if (is_ecc_ff) {
+				if (ecc_calc[0] == 0 && ecc_calc[1] == 0 && ecc_calc[2] == 0)
+					return(0);
+			}
+			DEBUG (MTD_DEBUG_LEVEL0, "UNCORRECTED_ERROR default\n");
+			return(-1);
+	}
+}
+
+static int nand_davinci_correct_data(struct mtd_info *mtd, u_char *dat, u_char *read_ecc, u_char *calc_ecc)
+{
+	struct nand_chip	*this;
+	int			block_count = 0, i, rc;
+
+	this = mtd->priv;
+	block_count = (this->eccmode == NAND_ECC_HW12_2048) ? 4 : 1;
+	for (i = 0; i < block_count; i++) {
+		if (memcmp(read_ecc, calc_ecc, 3) != 0) {
+			rc = nand_davinci_compare_ecc(read_ecc, calc_ecc, dat);
+			if (rc < 0) {
+				return(rc);
+			}
+		}
+		read_ecc += 3;
+		calc_ecc += 3;
+		dat += 512;
+	}
+	return(0);
+}
+#endif
+
+static int nand_davinci_dev_ready(struct mtd_info *mtd)
+{
+	emifregs	emif_addr;
+
+	emif_addr = (emifregs)DAVINCI_ASYNC_EMIF_CNTRL_BASE;
+
+	return(emif_addr->NANDFSR & 0x1);
+}
+
+static int nand_davinci_waitfunc(struct mtd_info *mtd, struct nand_chip *this, int state)
+{
+	while(!nand_davinci_dev_ready(mtd)) {;}
+	*NAND_CE0CLE = NAND_STATUS;
+	return(*NAND_CE0DATA);
+}
+
+static void nand_flash_init(void)
+{
+	u_int32_t	acfg1 = 0x3ffffffc;
+	u_int32_t	acfg2 = 0x3ffffffc;
+	u_int32_t	acfg3 = 0x3ffffffc;
+	u_int32_t	acfg4 = 0x3ffffffc;
+	emifregs	emif_regs;
+
+	/*------------------------------------------------------------------*
+	 *  NAND FLASH CHIP TIMEOUT @ 459 MHz                               *
+	 *                                                                  *
+	 *  AEMIF.CLK freq   = PLL1/6 = 459/6 = 76.5 MHz                    *
+	 *  AEMIF.CLK period = 1/76.5 MHz = 13.1 ns                         *
+	 *                                                                  *
+	 *------------------------------------------------------------------*/
+	 acfg1 = 0
+	 	| (0 << 31 )	/* selectStrobe */
+	 	| (0 << 30 )	/* extWait */
+	 	| (1 << 26 )	/* writeSetup	10 ns */
+	 	| (3 << 20 )	/* writeStrobe	40 ns */
+	 	| (1 << 17 )	/* writeHold	10 ns */
+	 	| (1 << 13 )	/* readSetup	10 ns */
+	 	| (5 << 7 )	/* readStrobe	60 ns */
+	 	| (1 << 4 )	/* readHold	10 ns */
+	 	| (3 << 2 )	/* turnAround	?? ns */
+	 	| (0 << 0 )	/* asyncSize	8-bit bus */
+	 	;
+
+	emif_regs = (emifregs)DAVINCI_ASYNC_EMIF_CNTRL_BASE;
+
+	emif_regs->AWCCR |= 0x10000000;
+	emif_regs->AB1CR = acfg1;	/* 0x08244128 */;
+	emif_regs->AB2CR = acfg2;
+	emif_regs->AB3CR = acfg3;
+	emif_regs->AB4CR = acfg4;
+	emif_regs->NANDFCR = 0x00000101;
+}
+
+int board_nand_init(struct nand_chip *nand)
+{
+	nand->IO_ADDR_R   = (void  __iomem *)NAND_CE0DATA;
+	nand->IO_ADDR_W   = (void  __iomem *)NAND_CE0DATA;
+	nand->chip_delay  = 0;
+	nand->select_chip = nand_davinci_select_chip;
+#ifdef CFG_NAND_USE_FLASH_BBT
+	nand->options	  = NAND_USE_FLASH_BBT;
+#endif
+#ifdef CFG_NAND_HW_ECC
+#ifdef CFG_NAND_LARGEPAGE
+	nand->eccmode     = NAND_ECC_HW12_2048;
+#elif defined(CFG_NAND_SMALLPAGE)
+	nand->eccmode     = NAND_ECC_HW3_512;
+#else
+#error "Either CFG_NAND_LARGEPAGE or CFG_NAND_SMALLPAGE must be defined!"
+#endif
+	nand->autooob	  = &davinci_nand_oobinfo;
+	nand->calculate_ecc = nand_davinci_calculate_ecc;
+	nand->correct_data  = nand_davinci_correct_data;
+	nand->enable_hwecc  = nand_davinci_enable_hwecc;
+#else
+	nand->eccmode     = NAND_ECC_SOFT;
+#endif
+
+	/* Set address of hardware control function */
+	nand->hwcontrol = nand_davinci_hwcontrol;
+
+	nand->dev_ready = nand_davinci_dev_ready;
+	nand->waitfunc = nand_davinci_waitfunc;
+
+	nand_flash_init();
+
+	return(0);
+}
+
+#else
+#error "U-Boot legacy NAND support not available for DaVinci chips"
+#endif
+#endif	/* CFG_USE_NAND */
diff --git a/cpu/arm926ejs/davinci/reset.S b/cpu/arm926ejs/davinci/reset.S
new file mode 100644
index 0000000000..a687d44035
--- /dev/null
+++ b/cpu/arm926ejs/davinci/reset.S
@@ -0,0 +1,77 @@
+/*
+ * Processor reset using WDT for TI TMS320DM644x SoC.
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * -----------------------------------------------------
+ *
+ * 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
+ */
+
+.globl reset_cpu
+reset_cpu:
+	ldr	r0, WDT_TGCR
+	mov	r1, $0x08
+	str	r1, [r0]
+	ldr	r1, [r0]
+	orr	r1, r1, $0x03
+	str	r1, [r0]
+	mov	r1, $0
+	ldr	r0, WDT_TIM12
+	str	r1, [r0]
+	ldr	r0, WDT_TIM34
+	str	r1, [r0]
+	ldr	r0, WDT_PRD12
+	str	r1, [r0]
+	ldr	r0, WDT_PRD34
+	str	r1, [r0]
+	ldr	r0, WDT_TCR
+	ldr	r1, [r0]
+	orr	r1, r1, $0x40
+	str	r1, [r0]
+	ldr	r0, WDT_WDTCR
+	ldr	r1, [r0]
+	orr	r1, r1, $0x4000
+	str	r1, [r0]
+	ldr	r1, WDTCR_VAL1
+	str	r1, [r0]
+	ldr	r1, WDTCR_VAL2
+	str	r1, [r0]
+	nop
+	nop
+	nop
+	nop
+reset_cpu_loop:
+	b	reset_cpu_loop
+
+WDT_TGCR:
+	.word	0x01c21c24
+WDT_TIM12:
+	.word	0x01c21c10
+WDT_TIM34:
+	.word	0x01c21c14
+WDT_PRD12:
+	.word	0x01c21c18
+WDT_PRD34:
+	.word	0x01c21c1c
+WDT_TCR:
+	.word	0x01c21c20
+WDT_WDTCR:
+	.word	0x01c21c28
+WDTCR_VAL1:
+	.word	0xa5c64000
+WDTCR_VAL2:
+	.word	0xda7e4000
diff --git a/cpu/arm926ejs/davinci/timer.c b/cpu/arm926ejs/davinci/timer.c
new file mode 100644
index 0000000000..c6b1dda51f
--- /dev/null
+++ b/cpu/arm926ejs/davinci/timer.c
@@ -0,0 +1,165 @@
+/*
+ * (C) Copyright 2003
+ * Texas Instruments <www.ti.com>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Marius Groeger <mgroeger@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Alex Zuepke <azu@sysgo.de>
+ *
+ * (C) Copyright 2002-2004
+ * Gary Jennejohn, DENX Software Engineering, <gj@denx.de>
+ *
+ * (C) Copyright 2004
+ * Philippe Robin, ARM Ltd. <philippe.robin@arm.com>
+ *
+ * Copyright (C) 2007 Sergey Kubushyn <ksi@koi8.net>
+ *
+ * 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>
+#include <arm926ejs.h>
+
+typedef volatile struct {
+	u_int32_t	pid12;
+	u_int32_t	emumgt_clksped;
+	u_int32_t	gpint_en;
+	u_int32_t	gpdir_dat;
+	u_int32_t	tim12;
+	u_int32_t	tim34;
+	u_int32_t	prd12;
+	u_int32_t	prd34;
+	u_int32_t	tcr;
+	u_int32_t	tgcr;
+	u_int32_t	wdtcr;
+	u_int32_t	tlgc;
+	u_int32_t	tlmr;
+} davinci_timer;
+
+davinci_timer		*timer = (davinci_timer *)CFG_TIMERBASE;
+
+#define TIMER_LOAD_VAL	(CFG_HZ_CLOCK / CFG_HZ)
+#define READ_TIMER	timer->tim34
+
+static ulong timestamp;
+static ulong lastinc;
+
+int timer_init(void)
+{
+	/* We are using timer34 in unchained 32-bit mode, full speed */
+	timer->tcr = 0x0;
+	timer->tgcr = 0x0;
+	timer->tgcr = 0x06;
+	timer->tim34 = 0x0;
+	timer->prd34 = TIMER_LOAD_VAL;
+	lastinc = 0;
+	timer->tcr = 0x80 << 16;
+	timestamp = 0;
+
+	return(0);
+}
+
+void reset_timer(void)
+{
+	reset_timer_masked();
+}
+
+ulong get_timer(ulong base)
+{
+	return(get_timer_masked() - base);
+}
+
+void set_timer(ulong t)
+{
+	timestamp = t;
+}
+
+void udelay(unsigned long usec)
+{
+	udelay_masked(usec);
+}
+
+void reset_timer_masked(void)
+{
+	lastinc = READ_TIMER;
+	timestamp = 0;
+}
+
+ulong get_timer_raw(void)
+{
+	ulong now = READ_TIMER;
+
+	if (now >= lastinc) {
+		/* normal mode */
+		timestamp += now - lastinc;
+	} else {
+		/* overflow ... */
+		timestamp += now + TIMER_LOAD_VAL - lastinc;
+	}
+	lastinc = now;
+	return timestamp;
+}
+
+ulong get_timer_masked(void)
+{
+	return(get_timer_raw() / TIMER_LOAD_VAL);
+}
+
+void udelay_masked(unsigned long usec)
+{
+	ulong tmo;
+	ulong endtime;
+	signed long diff;
+
+	tmo = CFG_HZ_CLOCK / 1000;
+	tmo *= usec;
+	tmo /= 1000;
+
+	endtime = get_timer_raw() + tmo;
+
+	do {
+		ulong now = get_timer_raw();
+		diff = endtime - now;
+	} while (diff >= 0);
+}
+
+/*
+ * This function is derived from PowerPC code (read timebase as long long).
+ * On ARM it just returns the timer value.
+ */
+unsigned long long get_ticks(void)
+{
+	return(get_timer(0));
+}
+
+/*
+ * This function is derived from PowerPC code (timebase clock frequency).
+ * On ARM it returns the number of timer ticks per second.
+ */
+ulong get_tbclk(void)
+{
+	ulong tbclk;
+
+	tbclk = CFG_HZ;
+	return(tbclk);
+}
diff --git a/cpu/at32ap/atmel_mci.c b/cpu/at32ap/atmel_mci.c
index 9f62c0f14b..cf48be10ba 100644
--- a/cpu/at32ap/atmel_mci.c
+++ b/cpu/at32ap/atmel_mci.c
@@ -56,6 +56,7 @@
 #define MMC_DEFAULT_RCA		1
 
 static unsigned int mmc_rca;
+static int mmc_card_is_sd;
 static block_dev_desc_t mmc_blkdev;
 
 block_dev_desc_t *mmc_get_dev(int dev)
@@ -82,7 +83,9 @@ static void mci_set_mode(unsigned long hz, unsigned long blklen)
 
 	blklen &= 0xfffc;
 	mmci_writel(MR, (MMCI_BF(CLKDIV, clkdiv)
-			 | MMCI_BF(BLKLEN, blklen)));
+			 | MMCI_BF(BLKLEN, blklen)
+			 | MMCI_BIT(RDPROOF)
+			 | MMCI_BIT(WRPROOF)));
 }
 
 #define RESP_NO_CRC	1
@@ -225,7 +228,7 @@ mmc_bread(int dev, unsigned long start, lbaint_t blkcnt,
 				*buffer++ = data;
 				wordcount++;
 			}
-		} while(wordcount < (512 / 4));
+		} while(wordcount < (mmc_blkdev.blksz / 4));
 
 		pr_debug("mmc: read %u words, waiting for BLKE\n", wordcount);
 
@@ -243,7 +246,7 @@ out:
 
 fail:
 	mmc_cmd(MMC_CMD_SEND_STATUS, mmc_rca << 16, &card_status, R1 | NCR);
-	printf("mmc: bread failed, card status = ", card_status);
+	printf("mmc: bread failed, card status = %08x\n", card_status);
 	goto out;
 }
 
@@ -371,6 +374,7 @@ static int sd_init_card(struct mmc_cid *cid, int verbose)
 	mmc_rca = resp[0] >> 16;
 	if (verbose)
 		printf("SD Card detected (RCA %u)\n", mmc_rca);
+	mmc_card_is_sd = 1;
 	return 0;
 }
 
@@ -405,10 +409,64 @@ static int mmc_init_card(struct mmc_cid *cid, int verbose)
 	return ret;
 }
 
+static void mci_set_data_timeout(struct mmc_csd *csd)
+{
+	static const unsigned int dtomul_to_shift[] = {
+		0, 4, 7, 8, 10, 12, 16, 20,
+	};
+	static const unsigned int taac_exp[] = {
+		1, 10, 100, 1000, 10000, 100000, 1000000, 10000000,
+	};
+	static const unsigned int taac_mant[] = {
+		0,  10, 12, 13, 15, 60, 25, 30,
+		35, 40, 45, 50, 55, 60, 70, 80,
+	};
+	unsigned int timeout_ns, timeout_clks;
+	unsigned int e, m;
+	unsigned int dtocyc, dtomul;
+	unsigned int shift;
+	u32 dtor;
+
+	e = csd->taac & 0x07;
+	m = (csd->taac >> 3) & 0x0f;
+
+	timeout_ns = (taac_exp[e] * taac_mant[m] + 9) / 10;
+	timeout_clks = csd->nsac * 100;
+
+	timeout_clks += (((timeout_ns + 9) / 10)
+			 * ((CFG_MMC_CLK_PP + 99999) / 100000) + 9999) / 10000;
+	if (!mmc_card_is_sd)
+		timeout_clks *= 10;
+	else
+		timeout_clks *= 100;
+
+	dtocyc = timeout_clks;
+	dtomul = 0;
+	while (dtocyc > 15 && dtomul < 8) {
+		dtomul++;
+		shift = dtomul_to_shift[dtomul];
+		dtocyc = (timeout_clks + (1 << shift) - 1) >> shift;
+	}
+
+	if (dtomul >= 8) {
+		dtomul = 7;
+		dtocyc = 15;
+		puts("Warning: Using maximum data timeout\n");
+	}
+
+	dtor = (MMCI_BF(DTOMUL, dtomul)
+		| MMCI_BF(DTOCYC, dtocyc));
+	mmci_writel(DTOR, dtor);
+
+	printf("mmc: Using %u cycles data timeout (DTOR=0x%x)\n",
+	       dtocyc << shift, dtor);
+}
+
 int mmc_init(int verbose)
 {
 	struct mmc_cid cid;
 	struct mmc_csd csd;
+	unsigned int max_blksz;
 	int ret;
 
 	/* Initialize controller */
@@ -418,6 +476,8 @@ int mmc_init(int verbose)
 	mmci_writel(IDR, ~0UL);
 	mci_set_mode(CFG_MMC_CLK_OD, MMC_DEFAULT_BLKLEN);
 
+	mmc_card_is_sd = 0;
+
 	ret = sd_init_card(&cid, verbose);
 	if (ret) {
 		mmc_rca = MMC_DEFAULT_RCA;
@@ -433,6 +493,8 @@ int mmc_init(int verbose)
 	if (verbose)
 		mmc_dump_csd(&csd);
 
+	mci_set_data_timeout(&csd);
+
 	/* Initialize the blockdev structure */
 	mmc_blkdev.if_type = IF_TYPE_MMC;
 	mmc_blkdev.part_type = PART_TYPE_DOS;
@@ -444,7 +506,17 @@ int mmc_init(int verbose)
 		sizeof(mmc_blkdev.product));
 	sprintf((char *)mmc_blkdev.revision, "%x %x",
 		cid.prv >> 4, cid.prv & 0x0f);
-	mmc_blkdev.blksz = 1 << csd.read_bl_len;
+
+	/*
+	 * If we can't use 512 byte blocks, refuse to deal with the
+	 * card. Tons of code elsewhere seems to depend on this.
+	 */
+	max_blksz = 1 << csd.read_bl_len;
+	if (max_blksz < 512 || (max_blksz > 512 && !csd.read_bl_partial)) {
+		printf("Card does not support 512 byte reads, aborting.\n");
+		return -ENODEV;
+	}
+	mmc_blkdev.blksz = 512;
 	mmc_blkdev.lba = (csd.c_size + 1) * (1 << (csd.c_size_mult + 2));
 
 	mci_set_mode(CFG_MMC_CLK_PP, mmc_blkdev.blksz);
diff --git a/cpu/at32ap/atmel_mci.h b/cpu/at32ap/atmel_mci.h
index 0ffbc4fd09..5b4f5c99b6 100644
--- a/cpu/at32ap/atmel_mci.h
+++ b/cpu/at32ap/atmel_mci.h
@@ -57,6 +57,10 @@
 #define MMCI_CLKDIV_SIZE			8
 #define MMCI_PWSDIV_OFFSET			8
 #define MMCI_PWSDIV_SIZE			3
+#define MMCI_RDPROOF_OFFSET			11
+#define MMCI_RDPROOF_SIZE			1
+#define MMCI_WRPROOF_OFFSET			12
+#define MMCI_WRPROOF_SIZE			1
 #define MMCI_PDCPADV_OFFSET			14
 #define MMCI_PDCPADV_SIZE			1
 #define MMCI_PDCMODE_OFFSET			15
diff --git a/cpu/at32ap/interrupts.c b/cpu/at32ap/interrupts.c
index c9e04993c7..bef1f30d79 100644
--- a/cpu/at32ap/interrupts.c
+++ b/cpu/at32ap/interrupts.c
@@ -20,8 +20,8 @@
  * MA 02111-1307 USA
  */
 #include <common.h>
+#include <div64.h>
 
-#include <asm/div64.h>
 #include <asm/errno.h>
 #include <asm/io.h>
 #include <asm/processor.h>
diff --git a/cpu/bf533/serial.c b/cpu/bf533/serial.c
index 6cab5daac7..8ac6e3ff64 100644
--- a/cpu/bf533/serial.c
+++ b/cpu/bf533/serial.c
@@ -76,7 +76,6 @@ void calc_baud(void)
 void serial_setbrg(void)
 {
 	int i;
-	DECLARE_GLOBAL_DATA_PTR;
 
 	calc_baud();
 
diff --git a/cpu/bf537/i2c.c b/cpu/bf537/i2c.c
index 3b0d026e0d..0daba63b68 100644
--- a/cpu/bf537/i2c.c
+++ b/cpu/bf537/i2c.c
@@ -22,6 +22,8 @@
 #include <i2c.h>
 #include <asm/io.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 #define bfin_read16(addr) ({ unsigned __v; \
 			__asm__ __volatile__ (\
 			"%0 = w[%1] (z);\n\t"\
@@ -68,7 +70,6 @@
 
 #ifdef DEBUG_I2C
 #define PRINTD(fmt,args...)	do {	\
-	DECLARE_GLOBAL_DATA_PTR;	\
 	if (gd->have_console)		\
 		printf(fmt ,##args);	\
 	} while (0)
diff --git a/cpu/bf537/serial.c b/cpu/bf537/serial.c
index e04d08a0e7..f7a2483ffb 100644
--- a/cpu/bf537/serial.c
+++ b/cpu/bf537/serial.c
@@ -52,6 +52,8 @@
 #include <asm/io.h>
 #include "serial.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
 unsigned long pll_div_fact;
 
 void calc_baud(void)
@@ -74,7 +76,6 @@ void calc_baud(void)
 void serial_setbrg(void)
 {
 	int i;
-	DECLARE_GLOBAL_DATA_PTR;
 
 	calc_baud();
 
diff --git a/cpu/bf561/serial.c b/cpu/bf561/serial.c
index 7f5c695361..bc5a4f5726 100644
--- a/cpu/bf561/serial.c
+++ b/cpu/bf561/serial.c
@@ -52,6 +52,8 @@
 #include "serial.h"
 #include <asm/io.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 unsigned long pll_div_fact;
 
 void calc_baud(void)
@@ -74,7 +76,6 @@ void calc_baud(void)
 void serial_setbrg(void)
 {
 	int i;
-	DECLARE_GLOBAL_DATA_PTR;
 
 	calc_baud();
 
diff --git a/cpu/mcf523x/Makefile b/cpu/mcf523x/Makefile
new file mode 100644
index 0000000000..d0e9b4550f
--- /dev/null
+++ b/cpu/mcf523x/Makefile
@@ -0,0 +1,48 @@
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@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 $(TOPDIR)/config.mk
+
+# CFLAGS += -DET_DEBUG
+
+LIB	= lib$(CPU).a
+
+START	= start.o
+COBJS	= cpu.o speed.o cpu_init.o interrupts.o
+
+SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(START) $(LIB)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/mcf523x/config.mk b/cpu/mcf523x/config.mk
new file mode 100644
index 0000000000..ba324a894a
--- /dev/null
+++ b/cpu/mcf523x/config.mk
@@ -0,0 +1,27 @@
+#
+# (C) Copyright 2003 Josef Baumgartner <josef.baumgartner@telex.de>
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@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
+#
+
+PLATFORM_RELFLAGS += -ffixed-d7 -msep-data
+PLATFORM_CPPFLAGS += -m5307 -fPIC
diff --git a/cpu/mcf523x/cpu.c b/cpu/mcf523x/cpu.c
new file mode 100644
index 0000000000..f0d954b3fb
--- /dev/null
+++ b/cpu/mcf523x/cpu.c
@@ -0,0 +1,109 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <watchdog.h>
+#include <command.h>
+
+#include <asm/immap.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
+{
+	volatile ccm_t *ccm = (ccm_t *) MMAP_CCM;
+
+	ccm->rcr = CCM_RCR_SOFTRST;
+	/* we don't return! */
+	return 0;
+};
+
+int checkcpu(void)
+{
+	volatile ccm_t *ccm = (ccm_t *) MMAP_CCM;
+	u16 msk;
+	u16 id = 0;
+	u8 ver;
+
+	puts("CPU:   ");
+	msk = (ccm->cir >> 6);
+	ver = (ccm->cir & 0x003f);
+	switch (msk) {
+	case 0x31:
+		id = 5235;
+		break;
+	}
+
+	if (id) {
+		printf("Freescale MCF%d (Mask:%01x Version:%x)\n", id, msk,
+		       ver);
+		printf("       CPU CLK %d Mhz BUS CLK %d Mhz\n",
+		       (int)(gd->cpu_clk / 1000000),
+		       (int)(gd->bus_clk / 1000000));
+	}
+
+	return 0;
+};
+
+#if defined(CONFIG_WATCHDOG)
+/* Called by macro WATCHDOG_RESET */
+void watchdog_reset(void)
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
+
+	wdp->sr = 0x5555;	/* Count register */
+	asm("nop");
+	wdp->sr = 0xAAAA;	/* Count register */
+}
+
+int watchdog_disable(void)
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
+
+	/* UserManual, once the wdog is disabled, wdog cannot be re-enabled */
+	wdp->cr |= WTM_WCR_HALTED;	/* halted watchdog timer */
+
+	puts("WATCHDOG:disabled\n");
+	return (0);
+}
+
+int watchdog_init(void)
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
+	u32 wdog_module = 0;
+
+	/* set timeout and enable watchdog */
+	wdog_module = ((CFG_CLK / CFG_HZ) * CONFIG_WATCHDOG_TIMEOUT);
+	wdog_module |= (wdog_module / 8192);
+	wdp->mr = wdog_module;
+
+	wdp->cr = WTM_WCR_EN;
+	puts("WATCHDOG:enabled\n");
+
+	return (0);
+}
+#endif				/* CONFIG_WATCHDOG */
diff --git a/cpu/mcf523x/cpu_init.c b/cpu/mcf523x/cpu_init.c
new file mode 100644
index 0000000000..55c9cd356d
--- /dev/null
+++ b/cpu/mcf523x/cpu_init.c
@@ -0,0 +1,145 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * (C) Copyright 2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <watchdog.h>
+
+#include <asm/immap.h>
+
+/*
+ * Breath some life into the CPU...
+ *
+ * Set up the memory map,
+ * initialize a bunch of registers,
+ * initialize the UPM's
+ */
+void cpu_init_f(void)
+{
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
+	volatile fbcs_t *fbcs = (fbcs_t *) MMAP_FBCS;
+	volatile wdog_t *wdog = (wdog_t *) MMAP_WDOG;
+	volatile scm_t *scm = (scm_t *) MMAP_SCM;
+
+	/* watchdog is enabled by default - disable the watchdog */
+#ifndef CONFIG_WATCHDOG
+	wdog->cr = 0;
+#endif
+
+	scm->rambar = (CFG_INIT_RAM_ADDR | SCM_RAMBAR_BDE);
+
+	/* Port configuration */
+	gpio->par_cs = 0;
+
+#if (defined(CFG_CS0_BASE) && defined(CFG_CS0_MASK) && defined(CFG_CS0_CTRL))
+	fbcs->csar0 = CFG_CS0_BASE;
+	fbcs->cscr0 = CFG_CS0_CTRL;
+	fbcs->csmr0 = CFG_CS0_MASK;
+#endif
+
+#if (defined(CFG_CS1_BASE) && defined(CFG_CS1_MASK) && defined(CFG_CS1_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS_CS1;
+	fbcs->csar1 = CFG_CS1_BASE;
+	fbcs->cscr1 = CFG_CS1_CTRL;
+	fbcs->csmr1 = CFG_CS1_MASK;
+#endif
+
+#if (defined(CFG_CS2_BASE) && defined(CFG_CS2_MASK) && defined(CFG_CS2_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS_CS2;
+	fbcs->csar2 = CFG_CS2_BASE;
+	fbcs->cscr2 = CFG_CS2_CTRL;
+	fbcs->csmr2 = CFG_CS2_MASK;
+#endif
+
+#if (defined(CFG_CS3_BASE) && defined(CFG_CS3_MASK) && defined(CFG_CS3_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS_CS3;
+	fbcs->csar3 = CFG_CS3_BASE;
+	fbcs->cscr3 = CFG_CS3_CTRL;
+	fbcs->csmr3 = CFG_CS3_MASK;
+#endif
+
+#if (defined(CFG_CS4_BASE) && defined(CFG_CS4_MASK) && defined(CFG_CS4_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS_CS4;
+	fbcs->csar4 = CFG_CS4_BASE;
+	fbcs->cscr4 = CFG_CS4_CTRL;
+	fbcs->csmr4 = CFG_CS4_MASK;
+#endif
+
+#if (defined(CFG_CS5_BASE) && defined(CFG_CS5_MASK) && defined(CFG_CS5_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS_CS5;
+	fbcs->csar5 = CFG_CS5_BASE;
+	fbcs->cscr5 = CFG_CS5_CTRL;
+	fbcs->csmr5 = CFG_CS5_MASK;
+#endif
+
+#if (defined(CFG_CS6_BASE) && defined(CFG_CS6_MASK) && defined(CFG_CS6_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS_CS6;
+	fbcs->csar6 = CFG_CS6_BASE;
+	fbcs->cscr6 = CFG_CS6_CTRL;
+	fbcs->csmr6 = CFG_CS6_MASK;
+#endif
+
+#if (defined(CFG_CS7_BASE) && defined(CFG_CS7_MASK) && defined(CFG_CS7_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS_CS7;
+	fbcs->csar7 = CFG_CS7_BASE;
+	fbcs->cscr7 = CFG_CS7_CTRL;
+	fbcs->csmr7 = CFG_CS7_MASK;
+#endif
+
+#ifdef CONFIG_FSL_I2C
+	gpio->par_feci2c &= ~(GPIO_PAR_FECI2C_SCL_MASK | GPIO_PAR_FECI2C_SDA_MASK);
+	gpio->par_feci2c |= (GPIO_PAR_FECI2C_SCL_I2CSCL | GPIO_PAR_FECI2C_SDA_I2CSDA);
+#endif
+
+	icache_enable();
+}
+
+/*
+ * initialize higher level parts of CPU like timers
+ */
+int cpu_init_r(void)
+{
+	return (0);
+}
+
+void uart_port_conf(void)
+{
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
+
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		gpio->par_uart = (GPIO_PAR_UART_U0RXD | GPIO_PAR_UART_U0TXD);
+		break;
+	case 1:
+		gpio->par_uart =
+			(GPIO_PAR_UART_U1RXD_U1RXD | GPIO_PAR_UART_U1TXD_U1TXD);
+		break;
+	case 2:
+		gpio->par_timer = (GPIO_PAR_UART_U2RXD | GPIO_PAR_UART_U2TXD);
+		break;
+	}
+}
diff --git a/cpu/mcf523x/interrupts.c b/cpu/mcf523x/interrupts.c
new file mode 100644
index 0000000000..125c53b1bc
--- /dev/null
+++ b/cpu/mcf523x/interrupts.c
@@ -0,0 +1,49 @@
+/*
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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
+ */
+
+/* CPU specific interrupt routine */
+#include <common.h>
+#include <asm/immap.h>
+
+int interrupt_init(void)
+{
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
+
+	/* Make sure all interrupts are disabled */
+	intp->imrl0 |= 0x1;
+
+	enable_interrupts();
+	return 0;
+}
+
+#if defined(CONFIG_MCFTMR)
+void dtimer_intr_setup(void)
+{
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
+
+	intp->icr0[CFG_TMRINTR_NO] = CFG_TMRINTR_PRI;
+	intp->imrl0 &= ~INTC_IPRL_INT0;
+	intp->imrl0 &= ~CFG_TMRINTR_MASK;
+}
+#endif
diff --git a/cpu/mcf523x/speed.c b/cpu/mcf523x/speed.c
new file mode 100644
index 0000000000..247d3188bb
--- /dev/null
+++ b/cpu/mcf523x/speed.c
@@ -0,0 +1,49 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <asm/processor.h>
+
+#include <asm/immap.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+/*
+ * get_clocks() fills in gd->cpu_clock and gd->bus_clk
+ */
+int get_clocks(void)
+{
+	volatile pll_t *pll = (volatile pll_t *)(MMAP_PLL);
+
+	pll->syncr = PLL_SYNCR_MFD(1);
+
+	while (!(pll->synsr & PLL_SYNSR_LOCK));
+
+	gd->bus_clk = CFG_CLK;
+	gd->cpu_clk = (gd->bus_clk * 2);
+
+	return (0);
+}
diff --git a/cpu/mcf523x/start.S b/cpu/mcf523x/start.S
new file mode 100644
index 0000000000..2bd603db66
--- /dev/null
+++ b/cpu/mcf523x/start.S
@@ -0,0 +1,340 @@
+/*
+ * Copyright (C) 2003	Josef Baumgartner <josef.baumgartner@telex.de>
+ * Based on code from Bernhard Kuhn <bkuhn@metrowerks.com>
+ *
+ * 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 <config.h>
+#include "version.h"
+
+#ifndef	 CONFIG_IDENT_STRING
+#define	 CONFIG_IDENT_STRING ""
+#endif
+
+#define _START	_start
+#define _FAULT	_fault
+
+#define SAVE_ALL						\
+	move.w	#0x2700,%sr;		/* disable intrs */	\
+	subl	#60,%sp;		/* space for 15 regs */ \
+	moveml	%d0-%d7/%a0-%a6,%sp@;
+
+#define RESTORE_ALL						\
+	moveml	%sp@,%d0-%d7/%a0-%a6;				\
+	addl	#60,%sp;		/* space for 15 regs */ \
+	rte;
+
+.text
+/*
+ *	Vector table. This is used for initial platform startup.
+ *	These vectors are to catch any un-intended traps.
+ */
+_vectors:
+
+INITSP:		.long	0x00000000	/* Initial SP	*/
+INITPC:		.long	_START	/* Initial PC 		*/
+vector02:	.long	_FAULT	/* Access Error		*/
+vector03:	.long	_FAULT	/* Address Error	*/
+vector04:	.long	_FAULT	/* Illegal Instruction	*/
+vector05:	.long	_FAULT	/* Reserved		*/
+vector06:	.long	_FAULT	/* Reserved		*/
+vector07:	.long	_FAULT	/* Reserved		*/
+vector08:	.long	_FAULT	/* Privilege Violation	*/
+vector09:	.long	_FAULT	/* Trace		*/
+vector0A:	.long	_FAULT	/* Unimplemented A-Line	*/
+vector0B:	.long	_FAULT	/* Unimplemented F-Line	*/
+vector0C:	.long	_FAULT	/* Debug Interrupt	*/
+vector0D:	.long	_FAULT	/* Reserved		*/
+vector0E:	.long	_FAULT	/* Format Error		*/
+vector0F:	.long	_FAULT	/* Unitialized Int.	*/
+
+/* Reserved */
+vector10_17:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector18:	.long	_FAULT	/* Spurious Interrupt	*/
+vector19:	.long	_FAULT	/* Autovector Level 1	*/
+vector1A:	.long	_FAULT	/* Autovector Level 2	*/
+vector1B:	.long	_FAULT	/* Autovector Level 3	*/
+vector1C:	.long	_FAULT	/* Autovector Level 4	*/
+vector1D:	.long	_FAULT	/* Autovector Level 5	*/
+vector1E:	.long	_FAULT	/* Autovector Level 6	*/
+vector1F:	.long	_FAULT	/* Autovector Level 7	*/
+
+/* TRAP #0 - #15 */
+vector20_2F:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+/* Reserved	*/
+vector30_3F:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector64_127:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector128_191:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector192_255:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+	.text
+
+	.globl	_start
+_start:
+	nop
+	nop
+	move.w #0x2700,%sr	/* Mask off Interrupt */
+
+	/* Set vector base register at the beginning of the Flash */
+	move.l	#CFG_FLASH_BASE, %d0
+	movec	%d0, %VBR
+
+	move.l	#(CFG_INIT_RAM_ADDR + CFG_INIT_RAM_CTRL), %d0
+	movec	%d0, %RAMBAR1
+
+	/* invalidate and disable cache */
+	move.l	#0x01000000, %d0		/* Invalidate cache cmd */
+	movec	%d0, %CACR			/* Invalidate cache */
+	nop
+	move.l	#0, %d0
+	movec	%d0, %ACR0
+	movec	%d0, %ACR1
+
+	/* initialize general use internal ram */
+	move.l #0, %d0
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a2
+	move.l %d0, (%a1)
+	move.l %d0, (%a2)
+
+	/* set stackpointer to end of internal ram to get some stackspace for the
+	   first c-code */
+	move.l	#(CFG_INIT_RAM_ADDR + CFG_INIT_SP_OFFSET), %sp
+	clr.l %sp@-
+
+	move.l #__got_start, %a5	/* put relocation table address to a5 */
+
+	bsr cpu_init_f			/* run low-level CPU init code (from flash) */
+	bsr board_init_f		/* run low-level board init code (from flash) */
+
+	/* board_init_f() does not return */
+
+/*------------------------------------------------------------------------------*/
+
+/*
+ * void relocate_code (addr_sp, gd, addr_moni)
+ *
+ * This "function" does not return, instead it continues in RAM
+ * after relocating the monitor code.
+ *
+ * r3 = dest
+ * r4 = src
+ * r5 = length in bytes
+ * r6 = cachelinesize
+ */
+	.globl	relocate_code
+relocate_code:
+	link.w %a6,#0
+	move.l 8(%a6), %sp		/* set new stack pointer */
+
+	move.l 12(%a6), %d0		/* Save copy of Global Data pointer */
+	move.l 16(%a6), %a0		/* Save copy of Destination Address */
+
+	move.l #CFG_MONITOR_BASE, %a1
+	move.l #__init_end, %a2
+	move.l %a0, %a3
+
+	/* copy the code to RAM */
+1:
+	move.l (%a1)+, (%a3)+
+	cmp.l  %a1,%a2
+	bgt.s	 1b
+
+/*
+ * We are done. Do not return, instead branch to second part of board
+ * initialization, now running from RAM.
+ */
+	move.l	%a0, %a1
+	add.l	#(in_ram - CFG_MONITOR_BASE), %a1
+	jmp	(%a1)
+
+in_ram:
+
+clear_bss:
+	/*
+	 * Now clear BSS segment
+	 */
+	move.l	%a0, %a1
+	add.l	#(_sbss - CFG_MONITOR_BASE),%a1
+	move.l	%a0, %d1
+	add.l	#(_ebss - CFG_MONITOR_BASE),%d1
+6:
+	clr.l	(%a1)+
+	cmp.l	%a1,%d1
+	bgt.s	6b
+
+	/*
+	 * fix got table in RAM
+	 */
+	move.l	%a0, %a1
+	add.l	#(__got_start - CFG_MONITOR_BASE),%a1
+	move.l	%a1,%a5		/* * fix got pointer register a5 */
+
+	move.l	%a0, %a2
+	add.l	#(__got_end - CFG_MONITOR_BASE),%a2
+
+7:
+	move.l	(%a1),%d1
+	sub.l	#_start,%d1
+	add.l	%a0,%d1
+	move.l	%d1,(%a1)+
+	cmp.l	%a2, %a1
+	bne	7b
+
+	/* calculate relative jump to board_init_r in ram */
+	move.l %a0, %a1
+	add.l #(board_init_r - CFG_MONITOR_BASE), %a1
+
+	/* set parameters for board_init_r */
+	move.l %a0,-(%sp)		/* dest_addr */
+	move.l %d0,-(%sp)		/* gd */
+	jsr	(%a1)
+
+/*------------------------------------------------------------------------------*/
+/* exception code */
+	.globl _fault
+_fault:
+	jmp _fault
+	.globl	_exc_handler
+
+_exc_handler:
+	SAVE_ALL
+	movel	%sp,%sp@-
+	bsr exc_handler
+	addql	#4,%sp
+	RESTORE_ALL
+
+	.globl	_int_handler
+_int_handler:
+	SAVE_ALL
+	movel	%sp,%sp@-
+	bsr int_handler
+	addql	#4,%sp
+	RESTORE_ALL
+
+/*------------------------------------------------------------------------------*/
+/* cache functions */
+	.globl	icache_enable
+icache_enable:
+	move.l	#0x01000000, %d0		/* Invalidate cache cmd */
+	movec	%d0, %CACR			/* Invalidate cache */
+	nop
+	move.l	#(CFG_SDRAM_BASE + 0xc000), %d0	/* Setup cache mask */
+	movec	%d0, %ACR0			/* Enable cache */
+	move.l	#(CFG_FLASH_BASE + 0xc000), %d0	/* Setup cache mask */
+	movec	%d0, %ACR1			/* Enable cache */
+
+	move.l	#0x80400100, %d0		/* Setup cache mask */
+	movec	%d0, %CACR			/* Enable cache */
+	nop
+
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	moveq	#1, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	icache_disable
+icache_disable:
+	move.l	#0x00000100, %d0		/* Setup cache mask */
+	movec	%d0, %CACR			/* Disable cache */
+	clr.l	%d0				/* Setup cache mask */
+	movec	%d0, %ACR0
+	movec	%d0, %ACR1
+
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	moveq	#0, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	icache_status
+icache_status:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	move.l	(%a1), %d0
+	rts
+
+	.globl	icache_invalid
+icache_invalid:
+	move.l	#0x80600100, %d0		/* Setup cache mask */
+	movec	%d0, %CACR			/* Enable cache */
+	nop
+	rts
+
+	.globl	dcache_enable
+dcache_enable:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a1
+	moveq	#1, %d0
+	move.l	%d0, (%a1)
+	rts
+
+    /* No dcache, just a dummy function */
+	.globl	dcache_disable
+dcache_disable:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a1
+	moveq	#0, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	dcache_status
+dcache_status:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a1
+	move.l	(%a1), %d0
+	rts
+
+/*------------------------------------------------------------------------------*/
+
+	.globl	version_string
+version_string:
+	.ascii U_BOOT_VERSION
+	.ascii " (", __DATE__, " - ", __TIME__, ")"
+	.ascii CONFIG_IDENT_STRING, "\0"
diff --git a/cpu/mcf52x2/Makefile b/cpu/mcf52x2/Makefile
index 70d57cf609..937cdd0584 100644
--- a/cpu/mcf52x2/Makefile
+++ b/cpu/mcf52x2/Makefile
@@ -27,8 +27,8 @@ include $(TOPDIR)/config.mk
 
 LIB	= $(obj)lib$(CPU).a
 
-START	=
-COBJS	= serial.o interrupts.o cpu.o speed.o cpu_init.o fec.o
+START	= start.o
+COBJS	= interrupts.o cpu.o speed.o cpu_init.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/mcf52x2/cpu.c b/cpu/mcf52x2/cpu.c
index ce59d39cfa..71ea408aa5 100644
--- a/cpu/mcf52x2/cpu.c
+++ b/cpu/mcf52x2/cpu.c
@@ -28,33 +28,15 @@
 #include <common.h>
 #include <watchdog.h>
 #include <command.h>
+#include <asm/immap.h>
 
 #ifdef  CONFIG_M5271
-#include <asm/immap_5271.h>
-#include <asm/m5271.h>
-#endif
-
-#ifdef	CONFIG_M5272
-#include <asm/immap_5272.h>
-#include <asm/m5272.h>
-#endif
-
-#ifdef	CONFIG_M5282
-#include <asm/m5282.h>
-#include <asm/immap_5282.h>
-#endif
-
-#ifdef	CONFIG_M5249
-#include <asm/m5249.h>
-#endif
-
-#ifdef	CONFIG_M5271
 /*
  * Both MCF5270 and MCF5271 are members of the MPC5271 family. Try to
  * determine which one we are running on, based on the Chip Identification
  * Register (CIR).
  */
-int checkcpu (void)
+int checkcpu(void)
 {
 	char buf[32];
 	unsigned short cir;	/* Chip Identification Register */
@@ -80,156 +62,194 @@ int checkcpu (void)
 
 	if (cpu_model)
 		printf("CPU:   Freescale ColdFire MCF%s rev. %hu, at %s MHz\n",
-			cpu_model, prn, strmhz(buf, CFG_CLK));
+		       cpu_model, prn, strmhz(buf, CFG_CLK));
 	else
 		printf("CPU:   Unknown - Freescale ColdFire MCF5271 family"
-			" (PIN: 0x%x) rev. %hu, at %s MHz\n",
-			pin, prn, strmhz(buf, CFG_CLK));
+		       " (PIN: 0x%x) rev. %hu, at %s MHz\n",
+		       pin, prn, strmhz(buf, CFG_CLK));
 
 	return 0;
 }
 
-int do_reset (cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[]) {
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
+{
 	mbar_writeByte(MCF_RCM_RCR,
-			MCF_RCM_RCR_SOFTRST | MCF_RCM_RCR_FRCRSTOUT);
+		       MCF_RCM_RCR_SOFTRST | MCF_RCM_RCR_FRCRSTOUT);
 	return 0;
 };
 
 #if defined(CONFIG_WATCHDOG)
-void watchdog_reset (void)
+void watchdog_reset(void)
 {
 	mbar_writeShort(MCF_WTM_WSR, 0x5555);
 	mbar_writeShort(MCF_WTM_WSR, 0xAAAA);
 }
 
-int watchdog_disable (void)
+int watchdog_disable(void)
 {
 	mbar_writeShort(MCF_WTM_WCR, 0);
 	return (0);
 }
 
-int watchdog_init (void)
+int watchdog_init(void)
 {
 	mbar_writeShort(MCF_WTM_WCR, MCF_WTM_WCR_EN);
 	return (0);
 }
-#endif /* #ifdef CONFIG_WATCHDOG */
+#endif				/* #ifdef CONFIG_WATCHDOG */
 
 #endif
 
 #ifdef	CONFIG_M5272
-int do_reset (cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[]) {
-	volatile wdog_t * wdp = (wdog_t *)(CFG_MBAR + MCFSIM_WRRR);
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
 
 	wdp->wdog_wrrr = 0;
-	udelay (1000);
+	udelay(1000);
 
 	/* enable watchdog, set timeout to 0 and wait */
 	wdp->wdog_wrrr = 1;
-	while (1);
+	while (1) ;
 
 	/* we don't return! */
 	return 0;
 };
 
-int checkcpu(void) {
-	ulong *dirp = (ulong *)(CFG_MBAR + MCFSIM_DIR);
+int checkcpu(void)
+{
+	volatile sysctrl_t *sysctrl = (sysctrl_t *) (MMAP_CFG);
 	uchar msk;
-	char  *suf;
+	char *suf;
 
-	puts ("CPU:   ");
-	msk = (*dirp > 28) & 0xf;
+	puts("CPU:   ");
+	msk = (sysctrl->sc_dir > 28) & 0xf;
 	switch (msk) {
-		case 0x2: suf = "1K75N"; break;
-		case 0x4: suf = "3K75N"; break;
-		default:
-			suf = NULL;
-			printf ("Freescale MCF5272 (Mask:%01x)\n", msk);
-			break;
-		}
+	case 0x2:
+		suf = "1K75N";
+		break;
+	case 0x4:
+		suf = "3K75N";
+		break;
+	default:
+		suf = NULL;
+		printf("Freescale MCF5272 (Mask:%01x)\n", msk);
+		break;
+	}
 
 	if (suf)
-		printf ("Freescale MCF5272 %s\n", suf);
+		printf("Freescale MCF5272 %s\n", suf);
 	return 0;
 };
 
 #if defined(CONFIG_WATCHDOG)
 /* Called by macro WATCHDOG_RESET */
-void watchdog_reset (void)
+void watchdog_reset(void)
 {
-	volatile immap_t * regp = (volatile immap_t *)CFG_MBAR;
-	regp->wdog_reg.wdog_wcr = 0;
+	volatile wdog_t *wdt = (volatile wdog_t *)(MMAP_WDOG);
+	wdt->wdog_wcr = 0;
 }
 
-int watchdog_disable (void)
+int watchdog_disable(void)
 {
-	volatile immap_t *regp = (volatile immap_t *)CFG_MBAR;
+	volatile wdog_t *wdt = (volatile wdog_t *)(MMAP_WDOG);
 
-	regp->wdog_reg.wdog_wcr = 0;	/* reset watchdog counter */
-	regp->wdog_reg.wdog_wirr = 0;	/* disable watchdog interrupt */
-	regp->wdog_reg.wdog_wrrr = 0;	/* disable watchdog timer */
+	wdt->wdog_wcr = 0;	/* reset watchdog counter */
+	wdt->wdog_wirr = 0;	/* disable watchdog interrupt */
+	wdt->wdog_wrrr = 0;	/* disable watchdog timer */
 
-	puts ("WATCHDOG:disabled\n");
+	puts("WATCHDOG:disabled\n");
 	return (0);
 }
 
-int watchdog_init (void)
+int watchdog_init(void)
 {
-	volatile immap_t *regp = (volatile immap_t *)CFG_MBAR;
+	volatile wdog_t *wdt = (volatile wdog_t *)(MMAP_WDOG);
 
-	regp->wdog_reg.wdog_wirr = 0;	/* disable watchdog interrupt */
+	wdt->wdog_wirr = 0;	/* disable watchdog interrupt */
 
 	/* set timeout and enable watchdog */
-	regp->wdog_reg.wdog_wrrr = ((CONFIG_WATCHDOG_TIMEOUT * CFG_HZ) / (32768 * 1000)) - 1;
-	regp->wdog_reg.wdog_wcr = 0;	/* reset watchdog counter */
+	wdt->wdog_wrrr =
+	    ((CONFIG_WATCHDOG_TIMEOUT * CFG_HZ) / (32768 * 1000)) - 1;
+	wdt->wdog_wcr = 0;	/* reset watchdog counter */
 
-	puts ("WATCHDOG:enabled\n");
+	puts("WATCHDOG:enabled\n");
 	return (0);
 }
-#endif /* #ifdef CONFIG_WATCHDOG */
-
-#endif /* #ifdef CONFIG_M5272 */
+#endif				/* #ifdef CONFIG_WATCHDOG */
 
+#endif				/* #ifdef CONFIG_M5272 */
 
 #ifdef	CONFIG_M5282
-int checkcpu (void)
+int checkcpu(void)
 {
 	unsigned char resetsource = MCFRESET_RSR;
 
-	printf ("CPU:   Freescale Coldfire MCF5282 (PIN: %2.2x REV: %2.2x)\n",
-		MCFCCM_CIR>>8,MCFCCM_CIR & MCFCCM_CIR_PRN_MASK);
-	printf ("Reset:%s%s%s%s%s%s%s\n",
-		(resetsource & MCFRESET_RSR_LOL)  ? " Loss of Lock"	: "",
-		(resetsource & MCFRESET_RSR_LOC)  ? " Loss of Clock"	: "",
-		(resetsource & MCFRESET_RSR_EXT)  ? " External"		: "",
-		(resetsource & MCFRESET_RSR_POR)  ? " Power On"		: "",
-		(resetsource & MCFRESET_RSR_WDR)  ? " Watchdog"		: "",
-		(resetsource & MCFRESET_RSR_SOFT) ? " Software"		: "",
-		(resetsource & MCFRESET_RSR_LVD)  ? " Low Voltage"	: ""
-	);
+	printf("CPU:   Freescale Coldfire MCF5282 (PIN: %2.2x REV: %2.2x)\n",
+	       MCFCCM_CIR >> 8, MCFCCM_CIR & MCFCCM_CIR_PRN_MASK);
+	printf("Reset:%s%s%s%s%s%s%s\n",
+	       (resetsource & MCFRESET_RSR_LOL) ? " Loss of Lock" : "",
+	       (resetsource & MCFRESET_RSR_LOC) ? " Loss of Clock" : "",
+	       (resetsource & MCFRESET_RSR_EXT) ? " External" : "",
+	       (resetsource & MCFRESET_RSR_POR) ? " Power On" : "",
+	       (resetsource & MCFRESET_RSR_WDR) ? " Watchdog" : "",
+	       (resetsource & MCFRESET_RSR_SOFT) ? " Software" : "",
+	       (resetsource & MCFRESET_RSR_LVD) ? " Low Voltage" : "");
 	return 0;
 }
 
-int do_reset (cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[])
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
 {
 	MCFRESET_RCR = MCFRESET_RCR_SOFTRST;
 	return 0;
 };
 #endif
 
-#ifdef CONFIG_M5249 /* test-only: todo... */
-int checkcpu (void)
+#ifdef CONFIG_M5249
+int checkcpu(void)
 {
 	char buf[32];
 
-	printf ("CPU:   Freescale Coldfire MCF5249 at %s MHz\n", strmhz(buf, CFG_CLK));
+	printf("CPU:   Freescale Coldfire MCF5249 at %s MHz\n",
+	       strmhz(buf, CFG_CLK));
 	return 0;
 }
 
-int do_reset (cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[]) {
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
+{
 	/* enable watchdog, set timeout to 0 and wait */
 	mbar_writeByte(MCFSIM_SYPCR, 0xc0);
-	while (1);
+	while (1) ;
+
+	/* we don't return! */
+	return 0;
+};
+#endif
+
+#ifdef CONFIG_M5253
+int checkcpu(void)
+{
+	char buf[32];
+
+	unsigned char resetsource = mbar_readLong(SIM_RSR);
+	printf("CPU:   Freescale Coldfire MCF5253 at %s MHz\n",
+	       strmhz(buf, CFG_CLK));
+
+	if ((resetsource & SIM_RSR_HRST) || (resetsource & SIM_RSR_SWTR)) {
+		printf("Reset:%s%s\n",
+		       (resetsource & SIM_RSR_HRST) ? " Hardware/ System Reset"
+		       : "",
+		       (resetsource & SIM_RSR_SWTR) ? " Software Watchdog" :
+		       "");
+	}
+	return 0;
+}
+
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
+{
+	/* enable watchdog, set timeout to 0 and wait */
+	mbar_writeByte(SIM_SYPCR, 0xc0);
+	while (1) ;
 
 	/* we don't return! */
 	return 0;
diff --git a/cpu/mcf52x2/cpu_init.c b/cpu/mcf52x2/cpu_init.c
index 1748ea9d9b..458b85ef14 100644
--- a/cpu/mcf52x2/cpu_init.c
+++ b/cpu/mcf52x2/cpu_init.c
@@ -6,6 +6,10 @@
  * (C) Copyright 2005
  * BuS Elektronik GmbH & Co. KG <esw@bus-elektronik.de>
  *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ * Hayden Fraser (Hayden.Fraser@freescale.com)
+ *
  * See file CREDITS for list of people who contributed to this
  * project.
  *
@@ -27,28 +31,78 @@
 
 #include <common.h>
 #include <watchdog.h>
+#include <asm/immap.h>
 
-#ifdef	CONFIG_M5271
-#include <asm/m5271.h>
-#include <asm/immap_5271.h>
-#endif
+#if defined(CONFIG_M5253)
+/*
+ * Breath some life into the CPU...
+ *
+ * Set up the memory map,
+ * initialize a bunch of registers,
+ * initialize the UPM's
+ */
+void cpu_init_f(void)
+{
+	mbar_writeByte(MCFSIM_MPARK, 0x40);	/* 5249 Internal Core takes priority over DMA */
+	mbar_writeByte(MCFSIM_SYPCR, 0x00);
+	mbar_writeByte(MCFSIM_SWIVR, 0x0f);
+	mbar_writeByte(MCFSIM_SWSR, 0x00);
+	mbar_writeByte(MCFSIM_SWDICR, 0x00);
+	mbar_writeByte(MCFSIM_TIMER1ICR, 0x00);
+	mbar_writeByte(MCFSIM_TIMER2ICR, 0x88);
+	mbar_writeByte(MCFSIM_I2CICR, 0x00);
+	mbar_writeByte(MCFSIM_UART1ICR, 0x00);
+	mbar_writeByte(MCFSIM_UART2ICR, 0x00);
+	mbar_writeByte(MCFSIM_ICR6, 0x00);
+	mbar_writeByte(MCFSIM_ICR7, 0x00);
+	mbar_writeByte(MCFSIM_ICR8, 0x00);
+	mbar_writeByte(MCFSIM_ICR9, 0x00);
+	mbar_writeByte(MCFSIM_QSPIICR, 0x00);
 
-#ifdef	CONFIG_M5272
-#include <asm/m5272.h>
-#include <asm/immap_5272.h>
-#endif
+	mbar2_writeLong(MCFSIM_GPIO_INT_EN, 0x00000080);
+	mbar2_writeByte(MCFSIM_INTBASE, 0x40);	/* Base interrupts at 64 */
+	mbar2_writeByte(MCFSIM_SPURVEC, 0x00);
 
-#ifdef	CONFIG_M5282
-#include <asm/m5282.h>
-#include <asm/immap_5282.h>
-#endif
+	/*mbar2_writeLong(MCFSIM_IDECONFIG1, 0x00000020); */ /* Enable a 1 cycle pre-drive cycle on CS1 */
 
-#ifdef	CONFIG_M5249
-#include <asm/m5249.h>
-#endif
+	/*
+	 *  Setup chip selects...
+	 */
+
+	mbar_writeShort(MCFSIM_CSAR1, CFG_CSAR1);
+	mbar_writeShort(MCFSIM_CSCR1, CFG_CSCR1);
+	mbar_writeLong(MCFSIM_CSMR1, CFG_CSMR1);
+
+	mbar_writeShort(MCFSIM_CSAR0, CFG_CSAR0);
+	mbar_writeShort(MCFSIM_CSCR0, CFG_CSCR0);
+	mbar_writeLong(MCFSIM_CSMR0, CFG_CSMR0);
+
+	/* enable instruction cache now */
+	icache_enable();
+}
+
+/*initialize higher level parts of CPU like timers */
+int cpu_init_r(void)
+{
+	return (0);
+}
+
+void uart_port_conf(void)
+{
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		break;
+	case 1:
+		break;
+	case 2:
+		break;
+	}
+}
+#endif				/* #if defined(CONFIG_M5253) */
 
 #if defined(CONFIG_M5271)
-void cpu_init_f (void)
+void cpu_init_f(void)
 {
 #ifndef CONFIG_WATCHDOG
 	/* Disable the watchdog if we aren't using it */
@@ -58,25 +112,35 @@ void cpu_init_f (void)
 	/* Set clockspeed to 100MHz */
 	mbar_writeShort(MCF_FMPLL_SYNCR,
 			MCF_FMPLL_SYNCR_MFD(0) | MCF_FMPLL_SYNCR_RFD(0));
-	while (!mbar_readByte(MCF_FMPLL_SYNSR) & MCF_FMPLL_SYNSR_LOCK);
-
-	/* Enable UART pins */
-	mbar_writeShort(MCF_GPIO_PAR_UART, MCF_GPIO_PAR_UART_U0TXD |
-			MCF_GPIO_PAR_UART_U0RXD |
-			MCF_GPIO_PAR_UART_U1RXD_UART1 |
-			MCF_GPIO_PAR_UART_U1TXD_UART1);
-
-	/* Enable Ethernet pins */
-	mbar_writeByte(MCF_GPIO_PAR_FECI2C, CFG_FECI2C);
+	while (!mbar_readByte(MCF_FMPLL_SYNSR) & MCF_FMPLL_SYNSR_LOCK) ;
 }
 
 /*
  * initialize higher level parts of CPU like timers
  */
-int cpu_init_r	(void)
+int cpu_init_r(void)
 {
 	return (0);
 }
+
+void uart_port_conf(void)
+{
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		mbar_writeShort(MCF_GPIO_PAR_UART, MCF_GPIO_PAR_UART_U0TXD |
+				MCF_GPIO_PAR_UART_U0RXD);
+		break;
+	case 1:
+		mbar_writeShort(MCF_GPIO_PAR_UART,
+				MCF_GPIO_PAR_UART_U1RXD_UART1 |
+				MCF_GPIO_PAR_UART_U1TXD_UART1);
+		break;
+	case 2:
+		mbar_writeShort(MCF_GPIO_PAR_UART, 0x3000);
+		break;
+	}
+}
 #endif
 
 #if defined(CONFIG_M5272)
@@ -87,69 +151,68 @@ int cpu_init_r	(void)
  * initialize a bunch of registers,
  * initialize the UPM's
  */
-void cpu_init_f (void)
+void cpu_init_f(void)
 {
 	/* if we come from RAM we assume the CPU is
 	 * already initialized.
 	 */
 #ifndef CONFIG_MONITOR_IS_IN_RAM
-	volatile immap_t *regp = (immap_t *)CFG_MBAR;
-
-	volatile unsigned char	*mbar;
-	mbar = (volatile unsigned char *) CFG_MBAR;
+	volatile sysctrl_t *sysctrl = (sysctrl_t *) (CFG_MBAR);
+	volatile gpio_t *gpio = (gpio_t *) (MMAP_GPIO);
+	volatile csctrl_t *csctrl = (csctrl_t *) (MMAP_FBCS);
 
-	regp->sysctrl_reg.sc_scr = CFG_SCR;
-	regp->sysctrl_reg.sc_spr = CFG_SPR;
+	sysctrl->sc_scr = CFG_SCR;
+	sysctrl->sc_spr = CFG_SPR;
 
 	/* Setup Ports: */
-	regp->gpio_reg.gpio_pacnt = CFG_PACNT;
-	regp->gpio_reg.gpio_paddr = CFG_PADDR;
-	regp->gpio_reg.gpio_padat = CFG_PADAT;
-	regp->gpio_reg.gpio_pbcnt = CFG_PBCNT;
-	regp->gpio_reg.gpio_pbddr = CFG_PBDDR;
-	regp->gpio_reg.gpio_pbdat = CFG_PBDAT;
-	regp->gpio_reg.gpio_pdcnt = CFG_PDCNT;
+	gpio->gpio_pacnt = CFG_PACNT;
+	gpio->gpio_paddr = CFG_PADDR;
+	gpio->gpio_padat = CFG_PADAT;
+	gpio->gpio_pbcnt = CFG_PBCNT;
+	gpio->gpio_pbddr = CFG_PBDDR;
+	gpio->gpio_pbdat = CFG_PBDAT;
+	gpio->gpio_pdcnt = CFG_PDCNT;
 
 	/* Memory Controller: */
-	regp->csctrl_reg.cs_br0 = CFG_BR0_PRELIM;
-	regp->csctrl_reg.cs_or0 = CFG_OR0_PRELIM;
+	csctrl->cs_br0 = CFG_BR0_PRELIM;
+	csctrl->cs_or0 = CFG_OR0_PRELIM;
 
 #if (defined(CFG_OR1_PRELIM) && defined(CFG_BR1_PRELIM))
-	regp->csctrl_reg.cs_br1 = CFG_BR1_PRELIM;
-	regp->csctrl_reg.cs_or1 = CFG_OR1_PRELIM;
+	csctrl->cs_br1 = CFG_BR1_PRELIM;
+	csctrl->cs_or1 = CFG_OR1_PRELIM;
 #endif
 
 #if defined(CFG_OR2_PRELIM) && defined(CFG_BR2_PRELIM)
-	regp->csctrl_reg.cs_br2 = CFG_BR2_PRELIM;
-	regp->csctrl_reg.cs_or2 = CFG_OR2_PRELIM;
+	csctrl->cs_br2 = CFG_BR2_PRELIM;
+	csctrl->cs_or2 = CFG_OR2_PRELIM;
 #endif
 
 #if defined(CFG_OR3_PRELIM) && defined(CFG_BR3_PRELIM)
-	regp->csctrl_reg.cs_br3 = CFG_BR3_PRELIM;
-	regp->csctrl_reg.cs_or3 = CFG_OR3_PRELIM;
+	csctrl->cs_br3 = CFG_BR3_PRELIM;
+	csctrl->cs_or3 = CFG_OR3_PRELIM;
 #endif
 
 #if defined(CFG_OR4_PRELIM) && defined(CFG_BR4_PRELIM)
-	regp->csctrl_reg.cs_br4 = CFG_BR4_PRELIM;
-	regp->csctrl_reg.cs_or4 = CFG_OR4_PRELIM;
+	csctrl->cs_br4 = CFG_BR4_PRELIM;
+	csctrl->cs_or4 = CFG_OR4_PRELIM;
 #endif
 
 #if defined(CFG_OR5_PRELIM) && defined(CFG_BR5_PRELIM)
-	regp->csctrl_reg.cs_br5 = CFG_BR5_PRELIM;
-	regp->csctrl_reg.cs_or5 = CFG_OR5_PRELIM;
+	csctrl->cs_br5 = CFG_BR5_PRELIM;
+	csctrl->cs_or5 = CFG_OR5_PRELIM;
 #endif
 
 #if defined(CFG_OR6_PRELIM) && defined(CFG_BR6_PRELIM)
-	regp->csctrl_reg.cs_br6 = CFG_BR6_PRELIM;
-	regp->csctrl_reg.cs_or6 = CFG_OR6_PRELIM;
+	csctrl->cs_br6 = CFG_BR6_PRELIM;
+	csctrl->cs_or6 = CFG_OR6_PRELIM;
 #endif
 
 #if defined(CFG_OR7_PRELIM) && defined(CFG_BR7_PRELIM)
-	regp->csctrl_reg.cs_br7 = CFG_BR7_PRELIM;
-	regp->csctrl_reg.cs_or7 = CFG_OR7_PRELIM;
+	csctrl->cs_br7 = CFG_BR7_PRELIM;
+	csctrl->cs_or7 = CFG_OR7_PRELIM;
 #endif
 
-#endif /* #ifndef CONFIG_MONITOR_IS_IN_RAM */
+#endif				/* #ifndef CONFIG_MONITOR_IS_IN_RAM */
 
 	/* enable instruction cache now */
 	icache_enable();
@@ -159,14 +222,30 @@ void cpu_init_f (void)
 /*
  * initialize higher level parts of CPU like timers
  */
-int cpu_init_r	(void)
+int cpu_init_r(void)
 {
 	return (0);
 }
-#endif /* #if defined(CONFIG_M5272) */
 
+void uart_port_conf(void)
+{
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
 
-#ifdef	CONFIG_M5282
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		gpio->gpio_pbcnt &= ~(GPIO_PBCNT_PB0MSK | GPIO_PBCNT_PB1MSK);
+		gpio->gpio_pbcnt |= (GPIO_PBCNT_URT0_TXD | GPIO_PBCNT_URT0_RXD);
+		break;
+	case 1:
+		gpio->gpio_pdcnt &= ~(GPIO_PDCNT_PD1MSK | GPIO_PDCNT_PD4MSK);
+		gpio->gpio_pdcnt |= (GPIO_PDCNT_URT1_RXD | GPIO_PDCNT_URT1_TXD);
+		break;
+	}
+}
+#endif				/* #if defined(CONFIG_M5272) */
+
+#if defined(CONFIG_M5282)
 /*
  * Breath some life into the CPU...
  *
@@ -174,7 +253,7 @@ int cpu_init_r	(void)
  * initialize a bunch of registers,
  * initialize the UPM's
  */
-void cpu_init_f (void)
+void cpu_init_f(void)
 {
 #ifndef CONFIG_WATCHDOG
 	/* disable watchdog if we aren't using it */
@@ -183,7 +262,11 @@ void cpu_init_f (void)
 
 #ifndef CONFIG_MONITOR_IS_IN_RAM
 	/* Set speed /PLL */
-	MCFCLOCK_SYNCR =  MCFCLOCK_SYNCR_MFD(CFG_MFD) | MCFCLOCK_SYNCR_RFD(CFG_RFD);
+	MCFCLOCK_SYNCR =
+	    MCFCLOCK_SYNCR_MFD(CFG_MFD) | MCFCLOCK_SYNCR_RFD(CFG_RFD);
+	while (!(MCFCLOCK_SYNSR & MCFCLOCK_SYNSR_LOCK)) ;
+
+	MCFGPIO_PBCDPAR = 0xc0;
 
 	/* Set up the GPIO ports */
 #ifdef CFG_PEPAR
@@ -228,29 +311,28 @@ void cpu_init_f (void)
     defined(CFG_CS0_WIDTH) & defined(CFG_CS0_RO) & \
 	defined(CFG_CS0_WS)
 
-	MCFCSM_CSAR0 =	(CFG_CS0_BASE >> 16) & 0xFFFF;
-
-	#if (CFG_CS0_WIDTH == 8)
-		#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_8
-	#elif (CFG_CS0_WIDTH == 16)
-		#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_16
-	#elif (CFG_CS0_WIDTH == 32)
-		#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_32
-	#else
-		#error	"CFG_CS0_WIDTH: Fault - wrong bus with for CS0"
-	#endif
-	MCFCSM_CSCR0 =	MCFCSM_CSCR_WS(CFG_CS0_WS)
-			|CFG_CS0_PS
-			|MCFCSM_CSCR_AA;
-
-	#if (CFG_CS0_RO != 0)
-		MCFCSM_CSMR0 =	MCFCSM_CSMR_BAM(CFG_CS0_SIZE-1)
-				|MCFCSM_CSMR_WP|MCFCSM_CSMR_V;
- 	#else
-		MCFCSM_CSMR0 =	MCFCSM_CSMR_BAM(CFG_CS0_SIZE-1)|MCFCSM_CSMR_V;
-	#endif
+	MCFCSM_CSAR0 = (CFG_CS0_BASE >> 16) & 0xFFFF;
+
+#if (CFG_CS0_WIDTH == 8)
+#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_8
+#elif (CFG_CS0_WIDTH == 16)
+#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_16
+#elif (CFG_CS0_WIDTH == 32)
+#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_32
+#else
+#error	"CFG_CS0_WIDTH: Fault - wrong bus with for CS0"
+#endif
+	MCFCSM_CSCR0 = MCFCSM_CSCR_WS(CFG_CS0_WS)
+	    | CFG_CS0_PS | MCFCSM_CSCR_AA;
+
+#if (CFG_CS0_RO != 0)
+	MCFCSM_CSMR0 = MCFCSM_CSMR_BAM(CFG_CS0_SIZE - 1)
+	    | MCFCSM_CSMR_WP | MCFCSM_CSMR_V;
 #else
-	#waring "Chip Select 0 are not initialized/used"
+	MCFCSM_CSMR0 = MCFCSM_CSMR_BAM(CFG_CS0_SIZE - 1) | MCFCSM_CSMR_V;
+#endif
+#else
+#waring "Chip Select 0 are not initialized/used"
 #endif
 
 #if defined(CFG_CS1_BASE) & defined(CFG_CS1_SIZE) & \
@@ -259,29 +341,27 @@ void cpu_init_f (void)
 
 	MCFCSM_CSAR1 = (CFG_CS1_BASE >> 16) & 0xFFFF;
 
-	#if (CFG_CS1_WIDTH == 8)
-		#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_8
-	#elif (CFG_CS1_WIDTH == 16)
-		#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_16
-	#elif (CFG_CS1_WIDTH == 32)
-		#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_32
-	#else
-		#error	"CFG_CS1_WIDTH: Fault - wrong bus with for CS1"
-	#endif
-	MCFCSM_CSCR1 =	MCFCSM_CSCR_WS(CFG_CS1_WS)
-			|CFG_CS1_PS
-			|MCFCSM_CSCR_AA;
-
-	#if (CFG_CS1_RO != 0)
-		MCFCSM_CSMR1 =	MCFCSM_CSMR_BAM(CFG_CS1_SIZE-1)
-				|MCFCSM_CSMR_WP
-				|MCFCSM_CSMR_V;
- 	#else
-		MCFCSM_CSMR1 =	MCFCSM_CSMR_BAM(CFG_CS1_SIZE-1)
-				|MCFCSM_CSMR_V;
-	#endif
+#if (CFG_CS1_WIDTH == 8)
+#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_8
+#elif (CFG_CS1_WIDTH == 16)
+#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_16
+#elif (CFG_CS1_WIDTH == 32)
+#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_32
+#else
+#error	"CFG_CS1_WIDTH: Fault - wrong bus with for CS1"
+#endif
+	MCFCSM_CSCR1 = MCFCSM_CSCR_WS(CFG_CS1_WS)
+	    | CFG_CS1_PS | MCFCSM_CSCR_AA;
+
+#if (CFG_CS1_RO != 0)
+	MCFCSM_CSMR1 = MCFCSM_CSMR_BAM(CFG_CS1_SIZE - 1)
+	    | MCFCSM_CSMR_WP | MCFCSM_CSMR_V;
+#else
+	MCFCSM_CSMR1 = MCFCSM_CSMR_BAM(CFG_CS1_SIZE - 1)
+	    | MCFCSM_CSMR_V;
+#endif
 #else
-	#warning "Chip Select 1 are not initialized/used"
+#warning "Chip Select 1 are not initialized/used"
 #endif
 
 #if defined(CFG_CS2_BASE) & defined(CFG_CS2_SIZE) & \
@@ -290,29 +370,27 @@ void cpu_init_f (void)
 
 	MCFCSM_CSAR2 = (CFG_CS2_BASE >> 16) & 0xFFFF;
 
-	#if (CFG_CS2_WIDTH == 8)
-		#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_8
-	#elif (CFG_CS2_WIDTH == 16)
-		#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_16
-	#elif (CFG_CS2_WIDTH == 32)
-		#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_32
-	#else
-		#error	"CFG_CS2_WIDTH: Fault - wrong bus with for CS2"
-	#endif
-	MCFCSM_CSCR2 =	MCFCSM_CSCR_WS(CFG_CS2_WS)
-			|CFG_CS2_PS
-			|MCFCSM_CSCR_AA;
-
-	#if (CFG_CS2_RO != 0)
-		MCFCSM_CSMR2 =	MCFCSM_CSMR_BAM(CFG_CS2_SIZE-1)
-				|MCFCSM_CSMR_WP
-				|MCFCSM_CSMR_V;
- 	#else
-		MCFCSM_CSMR2 =	MCFCSM_CSMR_BAM(CFG_CS2_SIZE-1)
-				|MCFCSM_CSMR_V;
-	#endif
+#if (CFG_CS2_WIDTH == 8)
+#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_8
+#elif (CFG_CS2_WIDTH == 16)
+#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_16
+#elif (CFG_CS2_WIDTH == 32)
+#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_32
 #else
-	#warning "Chip Select 2 are not initialized/used"
+#error	"CFG_CS2_WIDTH: Fault - wrong bus with for CS2"
+#endif
+	MCFCSM_CSCR2 = MCFCSM_CSCR_WS(CFG_CS2_WS)
+	    | CFG_CS2_PS | MCFCSM_CSCR_AA;
+
+#if (CFG_CS2_RO != 0)
+	MCFCSM_CSMR2 = MCFCSM_CSMR_BAM(CFG_CS2_SIZE - 1)
+	    | MCFCSM_CSMR_WP | MCFCSM_CSMR_V;
+#else
+	MCFCSM_CSMR2 = MCFCSM_CSMR_BAM(CFG_CS2_SIZE - 1)
+	    | MCFCSM_CSMR_V;
+#endif
+#else
+#warning "Chip Select 2 are not initialized/used"
 #endif
 
 #if defined(CFG_CS3_BASE) & defined(CFG_CS3_SIZE) & \
@@ -321,32 +399,30 @@ void cpu_init_f (void)
 
 	MCFCSM_CSAR3 = (CFG_CS3_BASE >> 16) & 0xFFFF;
 
-	#if (CFG_CS3_WIDTH == 8)
-		#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_8
-	#elif (CFG_CS3_WIDTH == 16)
-		#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_16
-	#elif (CFG_CS3_WIDTH == 32)
-		#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_32
-	#else
-		#error	"CFG_CS3_WIDTH: Fault - wrong bus with for CS1"
-	#endif
-	MCFCSM_CSCR3 =	MCFCSM_CSCR_WS(CFG_CS3_WS)
-			|CFG_CS3_PS
-			|MCFCSM_CSCR_AA;
-
-	#if (CFG_CS3_RO != 0)
-		MCFCSM_CSMR3 =	MCFCSM_CSMR_BAM(CFG_CS3_SIZE-1)
-				|MCFCSM_CSMR_WP
-				|MCFCSM_CSMR_V;
- 	#else
-		MCFCSM_CSMR3 =	MCFCSM_CSMR_BAM(CFG_CS3_SIZE-1)
-				|MCFCSM_CSMR_V;
-	#endif
+#if (CFG_CS3_WIDTH == 8)
+#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_8
+#elif (CFG_CS3_WIDTH == 16)
+#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_16
+#elif (CFG_CS3_WIDTH == 32)
+#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_32
 #else
-	#warning "Chip Select 3 are not initialized/used"
+#error	"CFG_CS3_WIDTH: Fault - wrong bus with for CS1"
 #endif
+	MCFCSM_CSCR3 = MCFCSM_CSCR_WS(CFG_CS3_WS)
+	    | CFG_CS3_PS | MCFCSM_CSCR_AA;
 
-#endif /* CONFIG_MONITOR_IS_IN_RAM */
+#if (CFG_CS3_RO != 0)
+	MCFCSM_CSMR3 = MCFCSM_CSMR_BAM(CFG_CS3_SIZE - 1)
+	    | MCFCSM_CSMR_WP | MCFCSM_CSMR_V;
+#else
+	MCFCSM_CSMR3 = MCFCSM_CSMR_BAM(CFG_CS3_SIZE - 1)
+	    | MCFCSM_CSMR_V;
+#endif
+#else
+#warning "Chip Select 3 are not initialized/used"
+#endif
+
+#endif				/* CONFIG_MONITOR_IS_IN_RAM */
 
 	/* defer enabling cache until boot (see do_go) */
 	/* icache_enable(); */
@@ -355,10 +431,29 @@ void cpu_init_f (void)
 /*
  * initialize higher level parts of CPU like timers
  */
-int cpu_init_r	(void)
+int cpu_init_r(void)
 {
 	return (0);
 }
+
+void uart_port_conf(void)
+{
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		MCFGPIO_PUAPAR &= 0xFc;
+		MCFGPIO_PUAPAR |= 0x03;
+		break;
+	case 1:
+		MCFGPIO_PUAPAR &= 0xF3;
+		MCFGPIO_PUAPAR |= 0x0C;
+		break;
+	case 2:
+		MCFGPIO_PASPAR &= 0xFF0F;
+		MCFGPIO_PASPAR |= 0x00A0;
+		break;
+	}
+}
 #endif
 
 #if defined(CONFIG_M5249)
@@ -369,33 +464,13 @@ int cpu_init_r	(void)
  * initialize a bunch of registers,
  * initialize the UPM's
  */
-void cpu_init_f (void)
+void cpu_init_f(void)
 {
-#ifndef CFG_PLL_BYPASS
-	/*
-	 *  Setup the PLL to run at the specified speed
-	 *
-	 */
-	volatile unsigned long cpll = mbar2_readLong(MCFSIM_PLLCR);
-	unsigned long pllcr;
-#ifdef CFG_FAST_CLK
-	pllcr = 0x925a3100;			  /* ~140MHz clock (PLL bypass = 0) */
-#else
-	pllcr = 0x135a4140;			  /* ~72MHz clock (PLL bypass = 0) */
-#endif
-	cpll = cpll & 0xfffffffe;		  /* Set PLL bypass mode = 0 (PSTCLK = crystal) */
-	mbar2_writeLong(MCFSIM_PLLCR, cpll);	  /* Set the PLL to bypass mode (PSTCLK = crystal) */
-	mbar2_writeLong(MCFSIM_PLLCR, pllcr);	  /* set the clock speed */
-	pllcr ^= 0x00000001;			  /* Set pll bypass to 1 */
-	mbar2_writeLong(MCFSIM_PLLCR, pllcr);	  /* Start locking (pll bypass = 1) */
-	udelay(0x20);				  /* Wait for a lock ... */
-#endif /* #ifndef CFG_PLL_BYPASS */
-
 	/*
 	 *  NOTE: by setting the GPIO_FUNCTION registers, we ensure that the UART pins
-	 *	  (UART0: gpio 30,27, UART1: gpio 31, 28) will be used as UART pins
-	 *	  which is their primary function.
-	 *	  ~Jeremy
+	 *        (UART0: gpio 30,27, UART1: gpio 31, 28) will be used as UART pins
+	 *        which is their primary function.
+	 *        ~Jeremy
 	 */
 	mbar2_writeLong(MCFSIM_GPIO_FUNC, CFG_GPIO_FUNC);
 	mbar2_writeLong(MCFSIM_GPIO1_FUNC, CFG_GPIO1_FUNC);
@@ -411,7 +486,7 @@ void cpu_init_f (void)
 	 *    ~Jeremy
 	 *
 	 */
-	mbar_writeByte(MCFSIM_MPARK, 0x30);    /* 5249 Internal Core takes priority over DMA */
+	mbar_writeByte(MCFSIM_MPARK, 0x30);	/* 5249 Internal Core takes priority over DMA */
 	mbar_writeByte(MCFSIM_SYPCR, 0x00);
 	mbar_writeByte(MCFSIM_SWIVR, 0x0f);
 	mbar_writeByte(MCFSIM_SWSR, 0x00);
@@ -431,7 +506,7 @@ void cpu_init_f (void)
 	mbar2_writeLong(MCFSIM_GPIO_INT_EN, 0x00000080);
 	mbar2_writeByte(MCFSIM_INTBASE, 0x40);	/* Base interrupts at 64 */
 	mbar2_writeByte(MCFSIM_SPURVEC, 0x00);
-	mbar2_writeLong(MCFSIM_IDECONFIG1, 0x00000020);	 /* Enable a 1 cycle pre-drive cycle on CS1 */
+	mbar2_writeLong(MCFSIM_IDECONFIG1, 0x00000020);	/* Enable a 1 cycle pre-drive cycle on CS1 */
 
 	/* Setup interrupt priorities for gpio7 */
 	/* mbar2_writeLong(MCFSIM_INTLEV5, 0x70000000); */
@@ -459,8 +534,19 @@ void cpu_init_f (void)
 /*
  * initialize higher level parts of CPU like timers
  */
-int cpu_init_r	(void)
+int cpu_init_r(void)
 {
 	return (0);
 }
-#endif /* #if defined(CONFIG_M5249) */
+
+void uart_port_conf(void)
+{
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		break;
+	case 1:
+		break;
+	}
+}
+#endif				/* #if defined(CONFIG_M5249) */
diff --git a/cpu/mcf52x2/fec.c b/cpu/mcf52x2/fec.c
deleted file mode 100644
index ef9c7984ca..0000000000
--- a/cpu/mcf52x2/fec.c
+++ /dev/null
@@ -1,605 +0,0 @@
-/*
- * (C) Copyright 2000-2004
- * Wolfgang Denk, DENX Software Engineering, wd@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>
-#include <malloc.h>
-#include <asm/fec.h>
-
-#ifdef  CONFIG_M5271
-#include <asm/m5271.h>
-#include <asm/immap_5271.h>
-#endif
-
-#ifdef	CONFIG_M5272
-#include <asm/m5272.h>
-#include <asm/immap_5272.h>
-#endif
-
-#ifdef	CONFIG_M5282
-#include <asm/m5282.h>
-#include <asm/immap_5282.h>
-#endif
-
-#include <net.h>
-#include <command.h>
-
-#ifdef	CONFIG_M5272
-#define FEC_ADDR		(CFG_MBAR + 0x840)
-#endif
-#if defined(CONFIG_M5282) || defined(CONFIG_M5271)
-#define FEC_ADDR 		(CFG_MBAR + 0x1000)
-#endif
-
-#undef	ET_DEBUG
-#undef	MII_DEBUG
-
-#if defined(CONFIG_CMD_NET) && defined(FEC_ENET)
-
-#ifdef CFG_DISCOVER_PHY
-#include <miiphy.h>
-static void mii_discover_phy (void);
-#endif
-
-/* Ethernet Transmit and Receive Buffers */
-#define DBUF_LENGTH  1520
-
-#define TX_BUF_CNT 2
-
-#define TOUT_LOOP 100
-
-#define PKT_MAXBUF_SIZE		1518
-#define PKT_MINBUF_SIZE		64
-#define PKT_MAXBLR_SIZE		1520
-
-
-static char txbuf[DBUF_LENGTH];
-
-static uint rxIdx;		/* index of the current RX buffer */
-static uint txIdx;		/* index of the current TX buffer */
-
-/*
-  * FEC Ethernet Tx and Rx buffer descriptors allocated at the
-  *  immr->udata_bd address on Dual-Port RAM
-  * Provide for Double Buffering
-  */
-
-typedef volatile struct CommonBufferDescriptor {
-	cbd_t rxbd[PKTBUFSRX];	/* Rx BD */
-	cbd_t txbd[TX_BUF_CNT];	/* Tx BD */
-} RTXBD;
-
-static RTXBD *rtx = NULL;
-
-int eth_send (volatile void *packet, int length)
-{
-	int j, rc;
-	volatile fec_t *fecp = (fec_t *) (FEC_ADDR);
-
-	/* section 16.9.23.3
-	 * Wait for ready
-	 */
-	j = 0;
-	while ((rtx->txbd[txIdx].cbd_sc & BD_ENET_TX_READY)
-	       && (j < TOUT_LOOP)) {
-		udelay (1);
-		j++;
-	}
-	if (j >= TOUT_LOOP) {
-		printf ("TX not ready\n");
-	}
-
-	rtx->txbd[txIdx].cbd_bufaddr = (uint) packet;
-	rtx->txbd[txIdx].cbd_datlen = length;
-	rtx->txbd[txIdx].cbd_sc |= BD_ENET_TX_READY | BD_ENET_TX_LAST;
-
-	/* Activate transmit Buffer Descriptor polling */
-	fecp->fec_x_des_active = 0x01000000;	/* Descriptor polling active    */
-
-	j = 0;
-	while ((rtx->txbd[txIdx].cbd_sc & BD_ENET_TX_READY)
-	       && (j < TOUT_LOOP)) {
-		udelay (1);
-		j++;
-	}
-	if (j >= TOUT_LOOP) {
-		printf ("TX timeout\n");
-	}
-#ifdef ET_DEBUG
-	printf ("%s[%d] %s: cycles: %d    status: %x  retry cnt: %d\n",
-		__FILE__, __LINE__, __FUNCTION__, j, rtx->txbd[txIdx].cbd_sc,
-		(rtx->txbd[txIdx].cbd_sc & 0x003C) >> 2);
-#endif
-
-	/* return only status bits */ ;
-	rc = (rtx->txbd[txIdx].cbd_sc & BD_ENET_TX_STATS);
-
-	txIdx = (txIdx + 1) % TX_BUF_CNT;
-
-	return rc;
-}
-
-int eth_rx (void)
-{
-	int length;
-	volatile fec_t *fecp = (fec_t *) FEC_ADDR;
-
-	for (;;) {
-		/* section 16.9.23.2 */
-		if (rtx->rxbd[rxIdx].cbd_sc & BD_ENET_RX_EMPTY) {
-			length = -1;
-			break;	/* nothing received - leave for() loop */
-		}
-
-		length = rtx->rxbd[rxIdx].cbd_datlen;
-
-		if (rtx->rxbd[rxIdx].cbd_sc & 0x003f) {
-#ifdef ET_DEBUG
-			printf ("%s[%d] err: %x\n",
-				__FUNCTION__, __LINE__,
-				rtx->rxbd[rxIdx].cbd_sc);
-#endif
-		} else {
-			/* Pass the packet up to the protocol layers. */
-			NetReceive (NetRxPackets[rxIdx], length - 4);
-		}
-
-		/* Give the buffer back to the FEC. */
-		rtx->rxbd[rxIdx].cbd_datlen = 0;
-
-		/* wrap around buffer index when necessary */
-		if ((rxIdx + 1) >= PKTBUFSRX) {
-			rtx->rxbd[PKTBUFSRX - 1].cbd_sc =
-				(BD_ENET_RX_WRAP | BD_ENET_RX_EMPTY);
-			rxIdx = 0;
-		} else {
-			rtx->rxbd[rxIdx].cbd_sc = BD_ENET_RX_EMPTY;
-			rxIdx++;
-		}
-
-		/* Try to fill Buffer Descriptors */
-		fecp->fec_r_des_active = 0x01000000;	/* Descriptor polling active    */
-	}
-
-	return length;
-}
-
-/**************************************************************
- *
- * FEC Ethernet Initialization Routine
- *
- *************************************************************/
-#define FEC_ECNTRL_ETHER_EN	0x00000002
-#define FEC_ECNTRL_RESET	0x00000001
-
-#define FEC_RCNTRL_BC_REJ	0x00000010
-#define FEC_RCNTRL_PROM		0x00000008
-#define FEC_RCNTRL_MII_MODE	0x00000004
-#define FEC_RCNTRL_DRT		0x00000002
-#define FEC_RCNTRL_LOOP		0x00000001
-
-#define FEC_TCNTRL_FDEN		0x00000004
-#define FEC_TCNTRL_HBC		0x00000002
-#define FEC_TCNTRL_GTS		0x00000001
-
-#define	FEC_RESET_DELAY		50000
-
-int eth_init (bd_t * bd)
-{
-#ifndef CFG_ENET_BD_BASE
-	DECLARE_GLOBAL_DATA_PTR;
-#endif
-	int i;
-	volatile fec_t *fecp = (fec_t *) (FEC_ADDR);
-
-	/* Whack a reset.
-	 * A delay is required between a reset of the FEC block and
-	 * initialization of other FEC registers because the reset takes
-	 * some time to complete. If you don't delay, subsequent writes
-	 * to FEC registers might get killed by the reset routine which is
-	 * still in progress.
-	 */
-	fecp->fec_ecntrl = FEC_ECNTRL_RESET;
-	for (i = 0;
-	     (fecp->fec_ecntrl & FEC_ECNTRL_RESET) && (i < FEC_RESET_DELAY);
-	     ++i) {
-		udelay (1);
-	}
-	if (i == FEC_RESET_DELAY) {
-		printf ("FEC_RESET_DELAY timeout\n");
-		return 0;
-	}
-
-	/* We use strictly polling mode only
-	 */
-	fecp->fec_imask = 0;
-
-	/* Clear any pending interrupt */
-	fecp->fec_ievent = 0xffffffff;
-
-	/* Set station address   */
-#define ea bd->bi_enetaddr
-	fecp->fec_addr_low = (ea[0] << 24) | (ea[1] << 16) |
-		(ea[2] << 8) | (ea[3]);
-	fecp->fec_addr_high = (ea[4] << 24) | (ea[5] << 16);
-#ifdef ET_DEBUG
-	printf ("Eth Addrs: %02x:%02x:%02x:%02x:%02x:%02x\n",
-		ea[0], ea[1], ea[2], ea[3], ea[4], ea[5]);
-#endif
-#undef ea
-
-#ifdef CONFIG_M5271
-	/* Clear multicast address hash table
-	 */
-	fecp->fec_ghash_table_high = 0;
-	fecp->fec_ghash_table_low = 0;
-
-	/* Clear individual address hash table
-	 */
-	fecp->fec_ihash_table_high = 0;
-	fecp->fec_ihash_table_low = 0;
-#else
-	/* Clear multicast address hash table
-	 */
-#ifdef	CONFIG_M5282
-	fecp->fec_ihash_table_high = 0;
-	fecp->fec_ihash_table_low = 0;
-#else
-	fecp->fec_hash_table_high = 0;
-	fecp->fec_hash_table_low = 0;
-#endif
-#endif
-
-	/* Set maximum receive buffer size.
-	 */
-	fecp->fec_r_buff_size = PKT_MAXBLR_SIZE;
-
-	/*
-	 * Setup Buffers and Buffer Desriptors
-	 */
-	rxIdx = 0;
-	txIdx = 0;
-
-	if (!rtx) {
-#ifdef CFG_ENET_BD_BASE
-		rtx = (RTXBD *) CFG_ENET_BD_BASE;
-#else
-		rtx = (RTXBD *) (CFG_MONITOR_BASE+gd->reloc_off -
-				 (((PKTBUFSRX+TX_BUF_CNT)*+sizeof(cbd_t)
-				  +0xFF)
-				  & ~0xFF)
-				);
-		debug("set ENET_DB_BASE to %lX\n",(long) rtx);
-#endif
-	}
-
-	/*
-	 * Setup Receiver Buffer Descriptors (13.14.24.18)
-	 * Settings:
-	 *     Empty, Wrap
-	 */
-	for (i = 0; i < PKTBUFSRX; i++) {
-		rtx->rxbd[i].cbd_sc = BD_ENET_RX_EMPTY;
-		rtx->rxbd[i].cbd_datlen = 0;	/* Reset */
-		rtx->rxbd[i].cbd_bufaddr = (uint) NetRxPackets[i];
-	}
-	rtx->rxbd[PKTBUFSRX - 1].cbd_sc |= BD_ENET_RX_WRAP;
-
-	/*
-	 * Setup Ethernet Transmitter Buffer Descriptors (13.14.24.19)
-	 * Settings:
-	 *    Last, Tx CRC
-	 */
-	for (i = 0; i < TX_BUF_CNT; i++) {
-		rtx->txbd[i].cbd_sc = BD_ENET_TX_LAST | BD_ENET_TX_TC;
-		rtx->txbd[i].cbd_datlen = 0;	/* Reset */
-		rtx->txbd[i].cbd_bufaddr = (uint) (&txbuf[0]);
-	}
-	rtx->txbd[TX_BUF_CNT - 1].cbd_sc |= BD_ENET_TX_WRAP;
-
-	/* Set receive and transmit descriptor base
-	 */
-	fecp->fec_r_des_start = (unsigned int) (&rtx->rxbd[0]);
-	fecp->fec_x_des_start = (unsigned int) (&rtx->txbd[0]);
-
-	/* Enable MII mode
-	 */
-
-#if 0	/* Full duplex mode */
-	fecp->fec_r_cntrl = FEC_RCNTRL_MII_MODE;
-	fecp->fec_x_cntrl = FEC_TCNTRL_FDEN;
-#else	/* Half duplex mode */
-	fecp->fec_r_cntrl = (PKT_MAXBUF_SIZE << 16); /* set max frame length */
-	fecp->fec_r_cntrl |= FEC_RCNTRL_MII_MODE | FEC_RCNTRL_DRT;
-	fecp->fec_x_cntrl = 0;
-#endif
-	/* Set MII speed */
-	fecp->fec_mii_speed = (((CFG_CLK / 2) / (2500000 / 10)) + 5) / 10;
-	fecp->fec_mii_speed *= 2;
-
-	/* Configure port B for MII.
-	 */
-	/* port initialization was already made in cpu_init_f() */
-
-	/* Now enable the transmit and receive processing
-	 */
-	fecp->fec_ecntrl = FEC_ECNTRL_ETHER_EN;
-
-#ifdef CFG_DISCOVER_PHY
-	/* wait for the PHY to wake up after reset */
-	mii_discover_phy ();
-#endif
-
-	/* And last, try to fill Rx Buffer Descriptors */
-	fecp->fec_r_des_active = 0x01000000;	/* Descriptor polling active    */
-
-	return 1;
-}
-
-void eth_halt (void)
-{
-	volatile fec_t *fecp = (fec_t *) FEC_ADDR;
-
-	fecp->fec_ecntrl = 0;
-}
-
-
-#if defined(CFG_DISCOVER_PHY) || defined(CONFIG_CMD_MII)
-
-static int phyaddr = -1;	/* didn't find a PHY yet */
-static uint phytype;
-
-/* Make MII read/write commands for the FEC.
-*/
-
-#define mk_mii_read(ADDR, REG)	(0x60020000 | ((ADDR << 23) | \
-						(REG & 0x1f) << 18))
-
-#define mk_mii_write(ADDR, REG, VAL)	(0x50020000 | ((ADDR << 23) | \
-						(REG & 0x1f) << 18) | \
-						(VAL & 0xffff))
-
-/* Interrupt events/masks.
-*/
-#define FEC_ENET_HBERR	((uint)0x80000000)	/* Heartbeat error */
-#define FEC_ENET_BABR	((uint)0x40000000)	/* Babbling receiver */
-#define FEC_ENET_BABT	((uint)0x20000000)	/* Babbling transmitter */
-#define FEC_ENET_GRA	((uint)0x10000000)	/* Graceful stop complete */
-#define FEC_ENET_TXF	((uint)0x08000000)	/* Full frame transmitted */
-#define FEC_ENET_TXB	((uint)0x04000000)	/* A buffer was transmitted */
-#define FEC_ENET_RXF	((uint)0x02000000)	/* Full frame received */
-#define FEC_ENET_RXB	((uint)0x01000000)	/* A buffer was received */
-#define FEC_ENET_MII	((uint)0x00800000)	/* MII interrupt */
-#define FEC_ENET_EBERR	((uint)0x00400000)	/* SDMA bus error */
-
-/* PHY identification
- */
-#define PHY_ID_LXT970		0x78100000	/* LXT970 */
-#define PHY_ID_LXT971		0x001378e0	/* LXT971 and 972 */
-#define PHY_ID_82555		0x02a80150	/* Intel 82555 */
-#define PHY_ID_QS6612		0x01814400	/* QS6612 */
-#define PHY_ID_AMD79C784	0x00225610	/* AMD 79C784 */
-#define PHY_ID_LSI80225		0x0016f870	/* LSI 80225 */
-#define PHY_ID_LSI80225B	0x0016f880	/* LSI 80225/B */
-
-/* send command to phy using mii, wait for result */
-static uint mii_send (uint mii_cmd)
-{
-	uint mii_reply;
-	volatile fec_t *ep = (fec_t *) (FEC_ADDR);
-
-	ep->fec_mii_data = mii_cmd;	/* command to phy */
-
-	/* wait for mii complete */
-	while (!(ep->fec_ievent & FEC_ENET_MII));	/* spin until done */
-	mii_reply = ep->fec_mii_data;	/* result from phy */
-	ep->fec_ievent = FEC_ENET_MII;	/* clear MII complete */
-#ifdef ET_DEBUG
-	printf ("%s[%d] %s: sent=0x%8.8x, reply=0x%8.8x\n",
-		__FILE__, __LINE__, __FUNCTION__, mii_cmd, mii_reply);
-#endif
-	return (mii_reply & 0xffff);	/* data read from phy */
-}
-#endif
-
-#if defined(CFG_DISCOVER_PHY)
-static void mii_discover_phy (void)
-{
-#define MAX_PHY_PASSES 11
-	uint phyno;
-	int pass;
-
-	phyaddr = -1;		/* didn't find a PHY yet */
-	for (pass = 1; pass <= MAX_PHY_PASSES && phyaddr < 0; ++pass) {
-		if (pass > 1) {
-			/* PHY may need more time to recover from reset.
-			 * The LXT970 needs 50ms typical, no maximum is
-			 * specified, so wait 10ms before try again.
-			 * With 11 passes this gives it 100ms to wake up.
-			 */
-			udelay (10000);	/* wait 10ms */
-		}
-		for (phyno = 1; phyno < 32 && phyaddr < 0; ++phyno) {
-			phytype = mii_send (mk_mii_read (phyno, PHY_PHYIDR1));
-#ifdef ET_DEBUG
-			printf ("PHY type 0x%x pass %d type ", phytype, pass);
-#endif
-			if (phytype != 0xffff) {
-				phyaddr = phyno;
-				phytype <<= 16;
-				phytype |= mii_send (mk_mii_read (phyno,
-								  PHY_PHYIDR2));
-
-#ifdef ET_DEBUG
-				printf ("PHY @ 0x%x pass %d type ", phyno,
-					pass);
-				switch (phytype & 0xfffffff0) {
-				case PHY_ID_LXT970:
-					printf ("LXT970\n");
-					break;
-				case PHY_ID_LXT971:
-					printf ("LXT971\n");
-					break;
-				case PHY_ID_82555:
-					printf ("82555\n");
-					break;
-				case PHY_ID_QS6612:
-					printf ("QS6612\n");
-					break;
-				case PHY_ID_AMD79C784:
-					printf ("AMD79C784\n");
-					break;
-				case PHY_ID_LSI80225B:
-					printf ("LSI L80225/B\n");
-					break;
-				default:
-					printf ("0x%08x\n", phytype);
-					break;
-				}
-#endif
-			}
-		}
-	}
-	if (phyaddr < 0) {
-		printf ("No PHY device found.\n");
-	}
-}
-#endif /* CFG_DISCOVER_PHY */
-
-#if defined(CONFIG_CMD_MII) && !defined(CONFIG_BITBANGMII)
-
-static int mii_init_done = 0;
-
-/****************************************************************************
- * mii_init -- Initialize the MII for MII command without ethernet
- * This function is a subset of eth_init
- ****************************************************************************
- */
-void mii_init (void)
-{
-	volatile fec_t *fecp = (fec_t *) (FEC_ADDR);
-
-	int i;
-
-	if (mii_init_done != 0) {
-		return;
-	}
-
-	/* Whack a reset.
-	 * A delay is required between a reset of the FEC block and
-	 * initialization of other FEC registers because the reset takes
-	 * some time to complete. If you don't delay, subsequent writes
-	 * to FEC registers might get killed by the reset routine which is
-	 * still in progress.
-	 */
-
-	fecp->fec_ecntrl = FEC_ECNTRL_RESET;
-	for (i = 0;
-	     (fecp->fec_ecntrl & FEC_ECNTRL_RESET) && (i < FEC_RESET_DELAY);
-	     ++i) {
-		udelay (1);
-	}
-	if (i == FEC_RESET_DELAY) {
-		printf ("FEC_RESET_DELAY timeout\n");
-		return;
-	}
-
-	/* We use strictly polling mode only
-	 */
-	fecp->fec_imask = 0;
-
-	/* Clear any pending interrupt
-	 */
-	fecp->fec_ievent = 0xffffffff;
-
-	/* Set MII speed */
-	fecp->fec_mii_speed = 0x0e;
-
-	/* Configure port B for MII.
-	 */
-	/* port initialization was already made in cpu_init_f() */
-
-	/* Now enable the transmit and receive processing */
-	fecp->fec_ecntrl = FEC_ECNTRL_ETHER_EN;
-
-	mii_init_done = 1;
-}
-
-/*****************************************************************************
- * Read and write a MII PHY register, routines used by MII Utilities
- *
- * FIXME: These routines are expected to return 0 on success, but mii_send
- *	  does _not_ return an error code. Maybe 0xFFFF means error, i.e.
- *	  no PHY connected...
- *	  For now always return 0.
- * FIXME: These routines only work after calling eth_init() at least once!
- *	  Otherwise they hang in mii_send() !!! Sorry!
- *****************************************************************************/
-
-int mcf52x2_miiphy_read (char *devname, unsigned char addr,
-		unsigned char reg, unsigned short *value)
-{
-	short rdreg;		/* register working value */
-
-#ifdef MII_DEBUG
-	printf ("miiphy_read(0x%x) @ 0x%x = ", reg, addr);
-#endif
-	rdreg = mii_send (mk_mii_read (addr, reg));
-
-	*value = rdreg;
-
-#ifdef MII_DEBUG
-	printf ("0x%04x\n", *value);
-#endif
-
-	return 0;
-}
-
-int mcf52x2_miiphy_write (char *devname, unsigned char addr,
-		unsigned char reg, unsigned short value)
-{
-	short rdreg;		/* register working value */
-
-#ifdef MII_DEBUG
-	printf ("miiphy_write(0x%x) @ 0x%x = ", reg, addr);
-#endif
-
-	rdreg = mii_send (mk_mii_write (addr, reg, value));
-
-#ifdef MII_DEBUG
-	printf ("0x%04x\n", value);
-#endif
-
-	return 0;
-}
-#endif
-#endif
-
-int mcf52x2_miiphy_initialize(bd_t *bis)
-{
-#if defined(CONFIG_CMD_NET) && defined(FEC_ENET)
-#if defined(CONFIG_CMD_MII) && !defined(CONFIG_BITBANGMII)
-	miiphy_register("mcf52x2phy", mcf52x2_miiphy_read, mcf52x2_miiphy_write);
-#endif
-#endif
-	return 0;
-}
diff --git a/cpu/mcf52x2/interrupts.c b/cpu/mcf52x2/interrupts.c
index 116747ad3a..2ccbde587e 100644
--- a/cpu/mcf52x2/interrupts.c
+++ b/cpu/mcf52x2/interrupts.c
@@ -1,9 +1,10 @@
 /*
- * (C) Copyright 2003 Josef Baumgartner <josef.baumgartner@telex.de>
- *
  * (C) Copyright 2000-2004
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
  *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
  * See file CREDITS for list of people who contributed to this
  * project.
  *
@@ -26,168 +27,75 @@
 #include <common.h>
 #include <watchdog.h>
 #include <asm/processor.h>
-
-#ifdef	CONFIG_M5271
-#include <asm/m5271.h>
-#include <asm/immap_5271.h>
-#endif
+#include <asm/immap.h>
 
 #ifdef	CONFIG_M5272
-#include <asm/m5272.h>
-#include <asm/immap_5272.h>
-#endif
-
-#ifdef	CONFIG_M5282
-#include <asm/m5282.h>
-#include <asm/immap_5282.h>
-#endif
-
-#ifdef	CONFIG_M5249
-#include <asm/m5249.h>
-#endif
-
-
-#define	NR_IRQS		31
-
-/*
- * Interrupt vector functions.
- */
-struct interrupt_action {
-	interrupt_handler_t *handler;
-	void *arg;
-};
-
-static struct interrupt_action irq_vecs[NR_IRQS];
-
-static __inline__ unsigned short get_sr (void)
+int interrupt_init(void)
 {
-	unsigned short sr;
-
-	asm volatile ("move.w %%sr,%0":"=r" (sr):);
+	volatile intctrl_t *intp = (intctrl_t *) (MMAP_INTC);
 
-	return sr;
-}
-
-static __inline__ void set_sr (unsigned short sr)
-{
-	asm volatile ("move.w %0,%%sr"::"r" (sr));
-}
-
-/************************************************************************/
-/*
- * Install and free an interrupt handler
- */
-void irq_install_handler (int vec, interrupt_handler_t * handler, void *arg)
-{
-#ifdef	CONFIG_M5272
-	volatile intctrl_t *intp = (intctrl_t *) (CFG_MBAR + MCFSIM_ICR1);
-#endif
-	int vec_base = 0;
-
-#ifdef	CONFIG_M5272
-	vec_base = intp->int_pivr & 0xe0;
-#endif
-
-	if ((vec < vec_base) || (vec > vec_base + NR_IRQS)) {
-		printf ("irq_install_handler: wrong interrupt vector %d\n",
-			vec);
-		return;
-	}
-
-	irq_vecs[vec - vec_base].handler = handler;
-	irq_vecs[vec - vec_base].arg = arg;
-}
-
-void irq_free_handler (int vec)
-{
-#ifdef	CONFIG_M5272
-	volatile intctrl_t *intp = (intctrl_t *) (CFG_MBAR + MCFSIM_ICR1);
-#endif
-	int vec_base = 0;
-
-#ifdef	CONFIG_M5272
-	vec_base = intp->int_pivr & 0xe0;
-#endif
+	/* disable all external interrupts */
+	intp->int_icr1 = 0x88888888;
+	intp->int_icr2 = 0x88888888;
+	intp->int_icr3 = 0x88888888;
+	intp->int_icr4 = 0x88888888;
+	intp->int_pitr = 0x00000000;
+	/* initialize vector register */
+	intp->int_pivr = 0x40;
 
-	if ((vec < vec_base) || (vec > vec_base + NR_IRQS)) {
-		return;
-	}
+	enable_interrupts();
 
-	irq_vecs[vec - vec_base].handler = NULL;
-	irq_vecs[vec - vec_base].arg = NULL;
+	return 0;
 }
 
-void enable_interrupts (void)
+#if defined(CONFIG_MCFTMR)
+void dtimer_intr_setup(void)
 {
-	unsigned short sr;
+	volatile intctrl_t *intp = (intctrl_t *) (CFG_INTR_BASE);
 
-	sr = get_sr ();
-	set_sr (sr & ~0x0700);
+	intp->int_icr1 &= ~INT_ICR1_TMR3MASK;
+	intp->int_icr1 |= CFG_TMRINTR_PRI;
 }
+#endif				/* CONFIG_MCFTMR */
+#endif				/* CONFIG_M5272 */
 
-int disable_interrupts (void)
+#if defined(CONFIG_M5282) || defined(CONFIG_M5271)
+int interrupt_init(void)
 {
-	unsigned short sr;
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
 
-	sr = get_sr ();
-	set_sr (sr | 0x0700);
+	/* Make sure all interrupts are disabled */
+	intp->imrl0 |= 0x1;
 
-	return ((sr & 0x0700) == 0);	/* return TRUE, if interrupts were enabled before */
+	enable_interrupts();
+	return 0;
 }
 
-void int_handler (struct pt_regs *fp)
+#if defined(CONFIG_MCFTMR)
+void dtimer_intr_setup(void)
 {
-#ifdef	CONFIG_M5272
-	volatile intctrl_t *intp = (intctrl_t *) (CFG_MBAR + MCFSIM_ICR1);
-#endif
-	int vec, vec_base = 0;
-
-	vec = (fp->vector >> 2) & 0xff;
-#ifdef	CONFIG_M5272
-	vec_base = intp->int_pivr & 0xe0;
-#endif
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
 
-	if (irq_vecs[vec - vec_base].handler != NULL) {
-		irq_vecs[vec -
-			 vec_base].handler (irq_vecs[vec - vec_base].arg);
-	} else {
-		printf ("\nBogus External Interrupt Vector %d\n", vec);
-	}
+	intp->icr0[CFG_TMRINTR_NO] = CFG_TMRINTR_PRI;
+	intp->imrl0 &= ~0xFFFFFFFE;
+	intp->imrl0 &= ~CFG_TMRINTR_MASK;
 }
+#endif				/* CONFIG_MCFTMR */
+#endif				/* CONFIG_M5282 | CONFIG_M5271 */
 
-
-#ifdef	CONFIG_M5272
-int interrupt_init (void)
+#if defined(CONFIG_M5249) || defined(CONFIG_M5253)
+int interrupt_init(void)
 {
-	volatile intctrl_t *intp = (intctrl_t *) (CFG_MBAR + MCFSIM_ICR1);
-
-	/* disable all external interrupts */
-	intp->int_icr1 = 0x88888888;
-	intp->int_icr2 = 0x88888888;
-	intp->int_icr3 = 0x88888888;
-	intp->int_icr4 = 0x88888888;
-	intp->int_pitr = 0x00000000;
-	/* initialize vector register */
-	intp->int_pivr = 0x40;
+	enable_interrupts();
 
-	enable_interrupts ();
-
-	return 0;
-}
-#endif
-
-#if defined(CONFIG_M5282) || defined(CONFIG_M5271)
-int interrupt_init (void)
-{
 	return 0;
 }
-#endif
 
-#ifdef	CONFIG_M5249
-int interrupt_init (void)
+#if defined(CONFIG_MCFTMR)
+void dtimer_intr_setup(void)
 {
-	enable_interrupts ();
-
-	return 0;
+	mbar_writeLong(MCFSIM_IMR, mbar_readLong(MCFSIM_IMR) & ~0x00000400);
+	mbar_writeByte(MCFSIM_TIMER2ICR, CFG_TMRINTR_PRI);
 }
-#endif
+#endif				/* CONFIG_MCFTMR */
+#endif				/* CONFIG_M5249 || CONFIG_M5253 */
diff --git a/cpu/mcf52x2/serial.c b/cpu/mcf52x2/serial.c
deleted file mode 100644
index 8be09e34fe..0000000000
--- a/cpu/mcf52x2/serial.c
+++ /dev/null
@@ -1,215 +0,0 @@
-/*
- * (C) Copyright 2000-2004
- * Wolfgang Denk, DENX Software Engineering, wd@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>
-#include <command.h>
-#include <watchdog.h>
-
-#include <asm/mcfuart.h>
-
-#ifdef CONFIG_M5271
-#include <asm/m5271.h>
-#endif
-
-#ifdef CONFIG_M5272
-#include <asm/m5272.h>
-#endif
-
-#ifdef CONFIG_M5282
-#include <asm/m5282.h>
-#endif
-
-#ifdef CONFIG_M5249
-#include <asm/m5249.h>
-#endif
-
-DECLARE_GLOBAL_DATA_PTR;
-
-#if defined(CONFIG_M5249) || defined(CONFIG_M5271)
-#define DoubleClock(a) ((double)(CFG_CLK/2) / 32.0 / (double)(a))
-#else
-#define DoubleClock(a) ((double)(CFG_CLK) / 32.0 / (double)(a))
-#endif
-
-void rs_serial_setbaudrate(int port,int baudrate)
-{
-#if defined(CONFIG_M5272) || defined(CONFIG_M5249) || defined(CONFIG_M5271)
-	volatile unsigned char	*uartp;
-# ifndef CONFIG_M5271
-	double fraction;
-# endif
-	double clock;
-
-	if (port == 0)
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
-	else
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE2);
-
-	clock = DoubleClock(baudrate);	/* Set baud above */
-
-	uartp[MCFUART_UBG1] = (((int)clock >> 8) & 0xff);	/* set msb baud */
-	uartp[MCFUART_UBG2] = ((int)clock & 0xff);		/* set lsb baud */
-
-# ifndef CONFIG_M5271
-	fraction = ((clock - (int)clock) * 16.0) + 0.5;
-	uartp[MCFUART_UFPD] = ((int)fraction & 0xf);	/* set baud fraction adjust */
-# endif
-#endif
-
-#if  defined(CONFIG_M5282)
-	volatile unsigned char	*uartp;
-	long clock;
-
-	switch (port) {
-	case 1:
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE2);
-		break;
-	case 2:
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE3);
-		break;
-	default:
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
-	}
-
-	clock = (long) CFG_CLK / ((long) 32 * baudrate);	/* Set baud above */
-
-	uartp[MCFUART_UBG1] = (((int)clock >> 8) & 0xff);	/* set msb baud */
-	uartp[MCFUART_UBG2] = ((int) clock & 0xff);		/* set lsb baud */
-
-#endif
-};
-
-void rs_serial_init (int port, int baudrate)
-{
-	volatile unsigned char *uartp;
-
-	/*
-	 *      Reset UART, get it into known state...
-	 */
-	switch (port) {
-	case 1:
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE2);
-		break;
-#if  defined(CONFIG_M5282)
-	case 2:
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE3);
-		break;
-#endif
-	default:
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
-	}
-
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETTX;	/* reset TX */
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETRX;	/* reset RX */
-
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETMRPTR;	/* reset MR pointer */
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETERR;	/* reset Error pointer */
-
-	/*
-	 * Set port for CONSOLE_BAUD_RATE, 8 data bits, 1 stop bit, no parity.
-	 */
-	uartp[MCFUART_UMR] = MCFUART_MR1_PARITYNONE | MCFUART_MR1_CS8;
-	uartp[MCFUART_UMR] = MCFUART_MR2_STOP1;
-
-	/* Mask UART interrupts */
-	uartp[MCFUART_UIMR] = 0;
-
-	/* Set clock Select Register: Tx/Rx clock is timer */
-	uartp[MCFUART_UCSR] = MCFUART_UCSR_RXCLKTIMER | MCFUART_UCSR_TXCLKTIMER;
-
-	rs_serial_setbaudrate (port, baudrate);
-
-	/* Enable Tx/Rx */
-	uartp[MCFUART_UCR] = MCFUART_UCR_RXENABLE | MCFUART_UCR_TXENABLE;
-
-	return;
-}
-
-/****************************************************************************/
-/*
- *	Output a single character, using UART polled mode.
- *	This is used for console output.
- */
-
-void rs_put_char(char ch)
-{
-	volatile unsigned char	*uartp;
-	int			i;
-
-	uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
-
-	for (i = 0; (i < 0x10000); i++) {
-		if (uartp[MCFUART_USR] & MCFUART_USR_TXREADY)
-			break;
-	}
-	uartp[MCFUART_UTB] = ch;
-	return;
-}
-
-int rs_is_char(void)
-{
-	volatile unsigned char	*uartp;
-
-	uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
-	return((uartp[MCFUART_USR] & MCFUART_USR_RXREADY) ? 1 : 0);
-}
-
-int rs_get_char(void)
-{
-	volatile unsigned char	*uartp;
-
-	uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
-	return(uartp[MCFUART_URB]);
-}
-
-void serial_setbrg(void) {
-	rs_serial_setbaudrate(0,gd->bd->bi_baudrate);
-}
-
-int serial_init(void) {
-	rs_serial_init(0,gd->baudrate);
-	return 0;
-}
-
-
-void serial_putc(const char c) {
-	if (c == '\n')
-		serial_putc ('\r');
-	rs_put_char(c);
-}
-
-void serial_puts (const char *s) {
-	while (*s)
-		serial_putc(*s++);
-}
-
-int serial_getc(void) {
-	while(!rs_is_char())
-		WATCHDOG_RESET();
-
-	return rs_get_char();
-}
-
-int serial_tstc() {
-	return rs_is_char();
-}
diff --git a/cpu/mcf52x2/speed.c b/cpu/mcf52x2/speed.c
index ac860b2c67..bc1e20023b 100644
--- a/cpu/mcf52x2/speed.c
+++ b/cpu/mcf52x2/speed.c
@@ -2,6 +2,9 @@
  * (C) Copyright 2003
  * Josef Baumgartner <josef.baumgartner@telex.de>
  *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * Hayden Fraser (Hayden.Fraser@freescale.com)
+ *
  * See file CREDITS for list of people who contributed to this
  * project.
  *
@@ -23,6 +26,7 @@
 
 #include <common.h>
 #include <asm/processor.h>
+#include <asm/immap.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -31,8 +35,37 @@ DECLARE_GLOBAL_DATA_PTR;
  */
 int get_clocks (void)
 {
-	gd->cpu_clk = CFG_CLK;
+#if defined(CONFIG_M5249) || defined(CONFIG_M5253)
+	volatile unsigned long cpll = mbar2_readLong(MCFSIM_PLLCR);
+	unsigned long pllcr;
+
+#ifndef CFG_PLL_BYPASS
+
 #ifdef CONFIG_M5249
+	/* Setup the PLL to run at the specified speed */
+#ifdef CFG_FAST_CLK
+	pllcr = 0x925a3100;	/* ~140MHz clock (PLL bypass = 0) */
+#else
+	pllcr = 0x135a4140;	/* ~72MHz clock (PLL bypass = 0) */
+#endif
+#endif				/* CONFIG_M5249 */
+
+#ifdef CONFIG_M5253
+	pllcr = CFG_PLLCR;
+#endif				/* CONFIG_M5253 */
+
+	cpll = cpll & 0xfffffffe;	/* Set PLL bypass mode = 0 (PSTCLK = crystal) */
+	mbar2_writeLong(MCFSIM_PLLCR, cpll);	/* Set the PLL to bypass mode (PSTCLK = crystal) */
+	mbar2_writeLong(MCFSIM_PLLCR, pllcr);	/* set the clock speed */
+	pllcr ^= 0x00000001;	/* Set pll bypass to 1 */
+	mbar2_writeLong(MCFSIM_PLLCR, pllcr);	/* Start locking (pll bypass = 1) */
+	udelay(0x20);		/* Wait for a lock ... */
+#endif				/* #ifndef CFG_PLL_BYPASS */
+
+#endif				/* CONFIG_M5249 || CONFIG_M5253 */
+
+	gd->cpu_clk = CFG_CLK;
+#if defined(CONFIG_M5249) || defined(CONFIG_M5253)
 	gd->bus_clk = gd->cpu_clk / 2;
 #else
 	gd->bus_clk = gd->cpu_clk;
diff --git a/cpu/mcf52x2/start.S b/cpu/mcf52x2/start.S
index 7c9a7d2d2b..686e2a5333 100644
--- a/cpu/mcf52x2/start.S
+++ b/cpu/mcf52x2/start.S
@@ -121,7 +121,7 @@ _start:
 	nop
 	move.w #0x2700,%sr
 
-#if defined(CONFIG_M5272) || defined(CONFIG_M5249)
+#if defined(CONFIG_M5272) || defined(CONFIG_M5249) || defined(CONFIG_M5253)
 	move.l	#(CFG_MBAR + 1), %d0		/* set MBAR address + valid flag */
 	move.c	%d0, %MBAR
 
@@ -133,7 +133,7 @@ _start:
 
 	move.l	#(CFG_INIT_RAM_ADDR + 1), %d0
 	movec	%d0, %RAMBAR0
-#endif /* #if defined(CONFIG_M5272) || defined(CONFIG_M5249) */
+#endif /* CONFIG_M5272 || CONFIG_M5249 || CONFIG_M5253 */
 
 #if defined(CONFIG_M5282) || defined(CONFIG_M5271)
 	/* Initialize IPSBAR */
@@ -159,7 +159,7 @@ _copy_flash:
 
 _flashbar_setup:
 	/* Initialize FLASHBAR: locate internal Flash and validate it */
-	move.l	#(CFG_INT_FLASH_BASE + 0x21), %d0
+	move.l	#(CFG_INT_FLASH_BASE + CFG_INT_FLASH_ENABLE), %d0
 	movec	%d0, %RAMBAR0
 	jmp _after_flashbar_copy.L	/* Force jump to absolute address */
 _flashbar_setup_end:
@@ -167,7 +167,7 @@ _flashbar_setup_end:
 _after_flashbar_copy:
 #else
 	/* Setup code to initialize FLASHBAR, if start from external Memory */
-	move.l	#(CFG_INT_FLASH_BASE + 0x21), %d0
+	move.l	#(CFG_INT_FLASH_BASE + CFG_INT_FLASH_ENABLE), %d0
 	movec	%d0, %RAMBAR0
 #endif /* (TEXT_BASE == CFG_INT_FLASH_BASE) */
 
@@ -326,10 +326,10 @@ clear_bss:
 	/* set parameters for board_init_r */
 	move.l %a0,-(%sp)		/* dest_addr */
 	move.l %d0,-(%sp)		/* gd */
-	#if defined(DEBUG) && (TEXT_BASE != CFG_INT_FLASH_BASE) && \
-	    defined(CFG_HALT_BEFOR_RAM_JUMP)
- 		halt
-	#endif
+#if defined(DEBUG) && (TEXT_BASE != CFG_INT_FLASH_BASE) && \
+    defined(CFG_HALT_BEFOR_RAM_JUMP)
+	halt
+#endif
 	jsr	(%a1)
 
 /*------------------------------------------------------------------------------*/
@@ -356,6 +356,24 @@ _int_handler:
 
 /*------------------------------------------------------------------------------*/
 /* cache functions */
+#ifdef	CONFIG_M5271
+	.globl	icache_enable
+icache_enable:
+	move.l	#0x01000000, %d0		/* Invalidate cache cmd */
+	movec	%d0, %CACR			/* Invalidate cache */
+	move.l	#(CFG_SDRAM_BASE + 0xc000), %d0	/* Setup cache mask */
+	movec	%d0, %ACR0			/* Enable cache */
+
+	move.l	#0x80000200, %d0		/* Setup cache mask */
+	movec	%d0, %CACR			/* Enable cache */
+	nop
+
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	moveq	#1, %d0
+	move.l	%d0, (%a1)
+	rts
+#endif
+
 #ifdef	CONFIG_M5272
 	.globl	icache_enable
 icache_enable:
@@ -389,7 +407,7 @@ icache_state_access_1:
 	rts
 #endif
 
-#ifdef CONFIG_M5249
+#if defined(CONFIG_M5249) || defined(CONFIG_M5253)
 	.globl	icache_enable
 icache_enable:
 	/*
@@ -426,13 +444,29 @@ icache_state_access_2:
 	.globl	icache_status
 icache_status:
 icache_state_access_3:
-	move.l	icache_state, %d0
+	move.l	#(icache_state), %a0
+	move.l	(%a0), %d0
 	rts
 
 	.data
 icache_state:
 	.long	0	/* cache is diabled on inirialization */
 
+	.globl	dcache_enable
+dcache_enable:
+	/* dummy function */
+	rts
+
+	.globl	dcache_disable
+dcache_disable:
+	/* dummy function */
+	rts
+
+	.globl	dcache_status
+dcache_status:
+	/* dummy function */
+	rts
+
 /*------------------------------------------------------------------------------*/
 
 	.globl	version_string
diff --git a/cpu/mcf532x/Makefile b/cpu/mcf532x/Makefile
new file mode 100644
index 0000000000..6790d90f27
--- /dev/null
+++ b/cpu/mcf532x/Makefile
@@ -0,0 +1,48 @@
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@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 $(TOPDIR)/config.mk
+
+# CFLAGS += -DET_DEBUG
+
+LIB	= lib$(CPU).a
+
+START	=
+COBJS	= cpu.o speed.o cpu_init.o interrupts.o
+
+SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(START) $(LIB)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/mcf532x/config.mk b/cpu/mcf532x/config.mk
new file mode 100644
index 0000000000..ba324a894a
--- /dev/null
+++ b/cpu/mcf532x/config.mk
@@ -0,0 +1,27 @@
+#
+# (C) Copyright 2003 Josef Baumgartner <josef.baumgartner@telex.de>
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@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
+#
+
+PLATFORM_RELFLAGS += -ffixed-d7 -msep-data
+PLATFORM_CPPFLAGS += -m5307 -fPIC
diff --git a/cpu/mcf532x/cpu.c b/cpu/mcf532x/cpu.c
new file mode 100644
index 0000000000..2f62e956cc
--- /dev/null
+++ b/cpu/mcf532x/cpu.c
@@ -0,0 +1,119 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <watchdog.h>
+#include <command.h>
+
+#include <asm/immap.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
+
+	wdp->cr = 0;
+	udelay(1000);
+
+	/* enable watchdog, set timeout to 0 and wait */
+	wdp->cr = WTM_WCR_EN;
+	while (1) ;
+
+	/* we don't return! */
+	return 0;
+};
+
+int checkcpu(void)
+{
+	volatile ccm_t *ccm = (ccm_t *) MMAP_CCM;
+	u16 msk;
+	u16 id = 0;
+	u8 ver;
+
+	puts("CPU:   ");
+	msk = (ccm->cir >> 6);
+	ver = (ccm->cir & 0x003f);
+	switch (msk) {
+	case 0x54:
+		id = 5329;
+		break;
+	case 0x59:
+		id = 5328;
+		break;
+	case 0x61:
+		id = 5327;
+		break;
+	}
+
+	if (id) {
+		printf("Freescale MCF%d (Mask:%01x Version:%x)\n", id, msk,
+		       ver);
+		printf("       CPU CLK %d Mhz BUS CLK %d Mhz\n",
+		       (int)(gd->cpu_clk / 1000000),
+		       (int)(gd->bus_clk / 1000000));
+	}
+
+	return 0;
+};
+
+#if defined(CONFIG_WATCHDOG)
+/* Called by macro WATCHDOG_RESET */
+void watchdog_reset(void)
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
+
+	wdp->sr = 0x5555;	/* Count register */
+}
+
+int watchdog_disable(void)
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
+
+	/* UserManual, once the wdog is disabled, wdog cannot be re-enabled */
+	wdp->cr |= WTM_WCR_HALTED;	/* halted watchdog timer */
+
+	puts("WATCHDOG:disabled\n");
+	return (0);
+}
+
+int watchdog_init(void)
+{
+	volatile wdog_t *wdp = (wdog_t *) (MMAP_WDOG);
+	u32 wdog_module = 0;
+
+	/* set timeout and enable watchdog */
+	wdog_module = ((CFG_CLK / 1000) * CONFIG_WATCHDOG_TIMEOUT);
+	wdog_module |= (wdog_module / 8192);
+	wdp->mr = wdog_module;
+
+	wdp->cr = WTM_WCR_EN;
+	puts("WATCHDOG:enabled\n");
+
+	return (0);
+}
+#endif				/* CONFIG_WATCHDOG */
diff --git a/cpu/mcf532x/cpu_init.c b/cpu/mcf532x/cpu_init.c
new file mode 100644
index 0000000000..93086f74bf
--- /dev/null
+++ b/cpu/mcf532x/cpu_init.c
@@ -0,0 +1,141 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * (C) Copyright 2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <watchdog.h>
+
+#include <asm/immap.h>
+
+/*
+ * Breath some life into the CPU...
+ *
+ * Set up the memory map,
+ * initialize a bunch of registers,
+ * initialize the UPM's
+ */
+void cpu_init_f(void)
+{
+	volatile scm1_t *scm1 = (scm1_t *) MMAP_SCM1;
+	volatile scm2_t *scm2 = (scm2_t *) MMAP_SCM2;
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
+	volatile fbcs_t *fbcs = (fbcs_t *) MMAP_FBCS;
+	volatile wdog_t *wdog = (wdog_t *) MMAP_WDOG;
+
+	/* watchdog is enabled by default - disable the watchdog */
+#ifndef CONFIG_WATCHDOG
+	wdog->cr = 0;
+#endif
+
+	scm1->mpr0 = 0x77777777;
+	scm2->pacra = 0;
+	scm2->pacrb = 0;
+	scm2->pacrc = 0;
+	scm2->pacrd = 0;
+	scm2->pacre = 0;
+	scm2->pacrf = 0;
+	scm2->pacrg = 0;
+	scm1->pacrh = 0;
+
+	/* Port configuration */
+	gpio->par_cs = 0;
+
+#if (defined(CFG_CS0_BASE) && defined(CFG_CS0_MASK) && defined(CFG_CS0_CTRL))
+	fbcs->csar0 = CFG_CS0_BASE;
+	fbcs->cscr0 = CFG_CS0_CTRL;
+	fbcs->csmr0 = CFG_CS0_MASK;
+#endif
+
+#if (defined(CFG_CS1_BASE) && defined(CFG_CS1_MASK) && defined(CFG_CS1_CTRL))
+	/* Latch chipselect */
+	gpio->par_cs |= GPIO_PAR_CS1;
+	fbcs->csar1 = CFG_CS1_BASE;
+	fbcs->cscr1 = CFG_CS1_CTRL;
+	fbcs->csmr1 = CFG_CS1_MASK;
+#endif
+
+#if (defined(CFG_CS2_BASE) && defined(CFG_CS2_MASK) && defined(CFG_CS2_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS2;
+	fbcs->csar2 = CFG_CS2_BASE;
+	fbcs->cscr2 = CFG_CS2_CTRL;
+	fbcs->csmr2 = CFG_CS2_MASK;
+#endif
+
+#if (defined(CFG_CS3_BASE) && defined(CFG_CS3_MASK) && defined(CFG_CS3_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS3;
+	fbcs->csar3 = CFG_CS3_BASE;
+	fbcs->cscr3 = CFG_CS3_CTRL;
+	fbcs->csmr3 = CFG_CS3_MASK;
+#endif
+
+#if (defined(CFG_CS4_BASE) && defined(CFG_CS4_MASK) && defined(CFG_CS4_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS4;
+	fbcs->csar4 = CFG_CS4_BASE;
+	fbcs->cscr4 = CFG_CS4_CTRL;
+	fbcs->csmr4 = CFG_CS4_MASK;
+#endif
+
+#if (defined(CFG_CS5_BASE) && defined(CFG_CS5_MASK) && defined(CFG_CS5_CTRL))
+	gpio->par_cs |= GPIO_PAR_CS5;
+	fbcs->csar5 = CFG_CS5_BASE;
+	fbcs->cscr5 = CFG_CS5_CTRL;
+	fbcs->csmr5 = CFG_CS5_MASK;
+#endif
+
+#ifdef CONFIG_FSL_I2C
+	gpio->par_feci2c = GPIO_PAR_FECI2C_SCL_SCL | GPIO_PAR_FECI2C_SDA_SDA;
+#endif
+
+	icache_enable();
+}
+
+/*
+ * initialize higher level parts of CPU like timers
+ */
+int cpu_init_r(void)
+{
+	return (0);
+}
+
+void uart_port_conf(void)
+{
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
+
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		gpio->par_uart = (GPIO_PAR_UART_TXD0 | GPIO_PAR_UART_RXD0);
+		break;
+	case 1:
+		gpio->par_uart =
+		    (GPIO_PAR_UART_TXD1(3) | GPIO_PAR_UART_RXD1(3));
+		break;
+	case 2:
+		gpio->par_timer &= 0x0F;
+		gpio->par_timer |= (GPIO_PAR_TIN3_URXD2 | GPIO_PAR_TIN2_UTXD2);
+		break;
+	}
+}
diff --git a/cpu/mcf532x/interrupts.c b/cpu/mcf532x/interrupts.c
new file mode 100644
index 0000000000..ff50d7ddfb
--- /dev/null
+++ b/cpu/mcf532x/interrupts.c
@@ -0,0 +1,49 @@
+/*
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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
+ */
+
+/* CPU specific interrupt routine */
+#include <common.h>
+#include <asm/immap.h>
+
+int interrupt_init(void)
+{
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
+
+	/* Make sure all interrupts are disabled */
+	intp->imrh0 |= 0xFFFFFFFF;
+	intp->imrl0 |= 0xFFFFFFFF;
+
+	enable_interrupts();
+	return 0;
+}
+
+#if defined(CONFIG_MCFTMR)
+void dtimer_intr_setup(void)
+{
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
+
+	intp->icr0[CFG_TMRINTR_NO] = CFG_TMRINTR_PRI;
+	intp->imrh0 &= ~CFG_TMRINTR_MASK;
+}
+#endif
diff --git a/cpu/mcf532x/speed.c b/cpu/mcf532x/speed.c
new file mode 100644
index 0000000000..001b9f42d6
--- /dev/null
+++ b/cpu/mcf532x/speed.c
@@ -0,0 +1,216 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <asm/processor.h>
+
+#include <asm/immap.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/* PLL min/max specifications */
+#define MAX_FVCO	500000	/* KHz */
+#define MAX_FSYS	80000	/* KHz */
+#define MIN_FSYS	58333	/* KHz */
+#define FREF		16000	/* KHz */
+#define MAX_MFD		135	/* Multiplier */
+#define MIN_MFD		88	/* Multiplier */
+#define BUSDIV		6	/* Divider */
+/*
+ * Low Power Divider specifications
+ */
+#define MIN_LPD		(1 << 0)	/* Divider (not encoded) */
+#define MAX_LPD		(1 << 15)	/* Divider (not encoded) */
+#define DEFAULT_LPD	(1 << 1)	/* Divider (not encoded) */
+
+/*
+ * Get the value of the current system clock
+ *
+ * Parameters:
+ *  none
+ *
+ * Return Value:
+ *  The current output system frequency
+ */
+int get_sys_clock(void)
+{
+	volatile ccm_t *ccm = (volatile ccm_t *)(MMAP_CCM);
+	volatile pll_t *pll = (volatile pll_t *)(MMAP_PLL);
+	int divider;
+
+	/* Test to see if device is in LIMP mode */
+	if (ccm->misccr & CCM_MISCCR_LIMP) {
+		divider = ccm->cdr & CCM_CDR_LPDIV(0xF);
+		return (FREF / (2 << divider));
+	} else {
+		return ((FREF * pll->pfdr) / (BUSDIV * 4));
+	}
+}
+
+/*
+ * Initialize the Low Power Divider circuit
+ *
+ * Parameters:
+ *  div     Desired system frequency divider
+ *
+ * Return Value:
+ *  The resulting output system frequency
+ */
+int clock_limp(int div)
+{
+	volatile ccm_t *ccm = (volatile ccm_t *)(MMAP_CCM);
+	u32 temp;
+
+	/* Check bounds of divider */
+	if (div < MIN_LPD)
+		div = MIN_LPD;
+	if (div > MAX_LPD)
+		div = MAX_LPD;
+
+	/* Save of the current value of the SSIDIV so we don't overwrite the value */
+	temp = (ccm->cdr & CCM_CDR_SSIDIV(0xF));
+
+	/* Apply the divider to the system clock */
+	ccm->cdr = (CCM_CDR_LPDIV(div) | CCM_CDR_SSIDIV(temp));
+
+	ccm->misccr |= CCM_MISCCR_LIMP;
+
+	return (FREF / (3 * (1 << div)));
+}
+
+/*
+ * Exit low power LIMP mode
+ *
+ * Parameters:
+ *  div     Desired system frequency divider
+ *
+ * Return Value:
+ *  The resulting output system frequency
+ */
+int clock_exit_limp(void)
+{
+	volatile ccm_t *ccm = (volatile ccm_t *)(MMAP_CCM);
+	int fout;
+
+	/* Exit LIMP mode */
+	ccm->misccr &= (~CCM_MISCCR_LIMP);
+
+	/* Wait for PLL to lock */
+	while (!(ccm->misccr & CCM_MISCCR_PLL_LOCK)) ;
+
+	fout = get_sys_clock();
+
+	return fout;
+}
+
+/* Initialize the PLL
+ *
+ * Parameters:
+ *  fref    PLL reference clock frequency in KHz
+ *  fsys    Desired PLL output frequency in KHz
+ *  flags   Operating parameters
+ *
+ * Return Value:
+ *  The resulting output system frequency
+ */
+int clock_pll(int fsys, int flags)
+{
+	volatile u32 *sdram_workaround = (volatile u32 *)(MMAP_SDRAM + 0x80);
+	volatile pll_t *pll = (volatile pll_t *)(MMAP_PLL);
+	int fref, temp, fout, mfd;
+	u32 i;
+
+	fref = FREF;
+
+	if (fsys == 0) {
+		/* Return current PLL output */
+		mfd = pll->pfdr;
+
+		return (fref * mfd / (BUSDIV * 4));
+	}
+
+	/* Check bounds of requested system clock */
+	if (fsys > MAX_FSYS)
+		fsys = MAX_FSYS;
+
+	if (fsys < MIN_FSYS)
+		fsys = MIN_FSYS;
+
+	/* Multiplying by 100 when calculating the temp value,
+	   and then dividing by 100 to calculate the mfd allows
+	   for exact values without needing to include floating
+	   point libraries. */
+	temp = (100 * fsys) / fref;
+	mfd = (4 * BUSDIV * temp) / 100;
+
+	/* Determine the output frequency for selected values */
+	fout = ((fref * mfd) / (BUSDIV * 4));
+
+	/*
+	 * Check to see if the SDRAM has already been initialized.
+	 * If it has then the SDRAM needs to be put into self refresh
+	 * mode before reprogramming the PLL.
+	 */
+
+	/*
+	 * Initialize the PLL to generate the new system clock frequency.
+	 * The device must be put into LIMP mode to reprogram the PLL.
+	 */
+
+	/* Enter LIMP mode */
+	clock_limp(DEFAULT_LPD);
+
+	/* Reprogram PLL for desired fsys */
+	pll->podr = (PLL_PODR_CPUDIV(BUSDIV / 3) | PLL_PODR_BUSDIV(BUSDIV));
+
+	pll->pfdr = mfd;
+
+	/* Exit LIMP mode */
+	clock_exit_limp();
+
+	/*
+	 * Return the SDRAM to normal operation if it is in use.
+	 */
+
+	/* software workaround for SDRAM opeartion after exiting LIMP mode errata */
+	*sdram_workaround = CFG_SDRAM_BASE;
+
+	/* wait for DQS logic to relock */
+	for (i = 0; i < 0x200; i++) ;
+
+	return fout;
+}
+
+/*
+ * get_clocks() fills in gd->cpu_clock and gd->bus_clk
+ */
+int get_clocks(void)
+{
+	gd->bus_clk = clock_pll(CFG_CLK / 1000, 0) * 1000;
+	gd->cpu_clk = (gd->bus_clk * 3);
+	return (0);
+}
diff --git a/cpu/mcf532x/start.S b/cpu/mcf532x/start.S
new file mode 100644
index 0000000000..5cc1c87cdd
--- /dev/null
+++ b/cpu/mcf532x/start.S
@@ -0,0 +1,335 @@
+/*
+ * Copyright (C) 2003	Josef Baumgartner <josef.baumgartner@telex.de>
+ * Based on code from Bernhard Kuhn <bkuhn@metrowerks.com>
+ *
+ * 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 <config.h>
+#include "version.h"
+
+#ifndef	 CONFIG_IDENT_STRING
+#define	 CONFIG_IDENT_STRING ""
+#endif
+
+#define _START	_start
+#define _FAULT	_fault
+
+#define SAVE_ALL						\
+	move.w	#0x2700,%sr;		/* disable intrs */	\
+	subl	#60,%sp;		/* space for 15 regs */ \
+	moveml	%d0-%d7/%a0-%a6,%sp@;
+
+#define RESTORE_ALL						\
+	moveml	%sp@,%d0-%d7/%a0-%a6;				\
+	addl	#60,%sp;		/* space for 15 regs */ \
+	rte;
+
+.text
+/*
+ *	Vector table. This is used for initial platform startup.
+ *	These vectors are to catch any un-intended traps.
+ */
+_vectors:
+
+INITSP:		.long	0x00000000	/* Initial SP	*/
+INITPC:		.long	_START	/* Initial PC 		*/
+vector02:	.long	_FAULT	/* Access Error		*/
+vector03:	.long	_FAULT	/* Address Error	*/
+vector04:	.long	_FAULT	/* Illegal Instruction	*/
+vector05:	.long	_FAULT	/* Reserved		*/
+vector06:	.long	_FAULT	/* Reserved		*/
+vector07:	.long	_FAULT	/* Reserved		*/
+vector08:	.long	_FAULT	/* Privilege Violation	*/
+vector09:	.long	_FAULT	/* Trace		*/
+vector0A:	.long	_FAULT	/* Unimplemented A-Line	*/
+vector0B:	.long	_FAULT	/* Unimplemented F-Line	*/
+vector0C:	.long	_FAULT	/* Debug Interrupt	*/
+vector0D:	.long	_FAULT	/* Reserved		*/
+vector0E:	.long	_FAULT	/* Format Error		*/
+vector0F:	.long	_FAULT	/* Unitialized Int.	*/
+
+/* Reserved */
+vector10_17:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector18:	.long	_FAULT	/* Spurious Interrupt	*/
+vector19:	.long	_FAULT	/* Autovector Level 1	*/
+vector1A:	.long	_FAULT	/* Autovector Level 2	*/
+vector1B:	.long	_FAULT	/* Autovector Level 3	*/
+vector1C:	.long	_FAULT	/* Autovector Level 4	*/
+vector1D:	.long	_FAULT	/* Autovector Level 5	*/
+vector1E:	.long	_FAULT	/* Autovector Level 6	*/
+vector1F:	.long	_FAULT	/* Autovector Level 7	*/
+
+/* TRAP #0 - #15 */
+vector20_2F:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+/* Reserved	*/
+vector30_3F:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector64_127:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector128_191:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector192_255:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+	.text
+
+	.globl	_start
+_start:
+	nop
+	nop
+	move.w #0x2700,%sr	/* Mask off Interrupt */
+
+	/* Set vector base register at the beginning of the Flash */
+	move.l	#CFG_FLASH_BASE, %d0
+	movec	%d0, %VBR
+
+	move.l	#(CFG_INIT_RAM_ADDR + CFG_INIT_RAM_CTRL), %d0
+	movec	%d0, %RAMBAR0
+
+	/* invalidate and disable cache */
+	move.l	#0x01000000, %d0		/* Invalidate cache cmd */
+	movec	%d0, %CACR			/* Invalidate cache */
+	move.l	#0, %d0
+	movec	%d0, %ACR0
+	movec	%d0, %ACR1
+
+	/* initialize general use internal ram */
+	move.l #0, %d0
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a2
+	move.l %d0, (%a1)
+	move.l %d0, (%a2)
+
+	/* set stackpointer to end of internal ram to get some stackspace for the
+	   first c-code */
+	move.l	#(CFG_INIT_RAM_ADDR + CFG_INIT_SP_OFFSET), %sp
+	clr.l %sp@-
+
+	move.l #__got_start, %a5	/* put relocation table address to a5 */
+
+	bsr cpu_init_f			/* run low-level CPU init code (from flash) */
+	bsr board_init_f		/* run low-level board init code (from flash) */
+
+	/* board_init_f() does not return */
+
+/*------------------------------------------------------------------------------*/
+
+/*
+ * void relocate_code (addr_sp, gd, addr_moni)
+ *
+ * This "function" does not return, instead it continues in RAM
+ * after relocating the monitor code.
+ *
+ * r3 = dest
+ * r4 = src
+ * r5 = length in bytes
+ * r6 = cachelinesize
+ */
+	.globl	relocate_code
+relocate_code:
+	link.w %a6,#0
+	move.l 8(%a6), %sp		/* set new stack pointer */
+
+	move.l 12(%a6), %d0		/* Save copy of Global Data pointer */
+	move.l 16(%a6), %a0		/* Save copy of Destination Address */
+
+	move.l #CFG_MONITOR_BASE, %a1
+	move.l #__init_end, %a2
+	move.l %a0, %a3
+
+	/* copy the code to RAM */
+1:
+	move.l (%a1)+, (%a3)+
+	cmp.l  %a1,%a2
+	bgt.s	 1b
+
+/*
+ * We are done. Do not return, instead branch to second part of board
+ * initialization, now running from RAM.
+ */
+	move.l	%a0, %a1
+	add.l	#(in_ram - CFG_MONITOR_BASE), %a1
+	jmp	(%a1)
+
+in_ram:
+
+clear_bss:
+	/*
+	 * Now clear BSS segment
+	 */
+	move.l	%a0, %a1
+	add.l	#(_sbss - CFG_MONITOR_BASE),%a1
+	move.l	%a0, %d1
+	add.l	#(_ebss - CFG_MONITOR_BASE),%d1
+6:
+	clr.l	(%a1)+
+	cmp.l	%a1,%d1
+	bgt.s	6b
+
+	/*
+	 * fix got table in RAM
+	 */
+	move.l	%a0, %a1
+	add.l	#(__got_start - CFG_MONITOR_BASE),%a1
+	move.l	%a1,%a5		/* * fix got pointer register a5 */
+
+	move.l	%a0, %a2
+	add.l	#(__got_end - CFG_MONITOR_BASE),%a2
+
+7:
+	move.l	(%a1),%d1
+	sub.l	#_start,%d1
+	add.l	%a0,%d1
+	move.l	%d1,(%a1)+
+	cmp.l	%a2, %a1
+	bne	7b
+
+	/* calculate relative jump to board_init_r in ram */
+	move.l %a0, %a1
+	add.l #(board_init_r - CFG_MONITOR_BASE), %a1
+
+	/* set parameters for board_init_r */
+	move.l %a0,-(%sp)		/* dest_addr */
+	move.l %d0,-(%sp)		/* gd */
+	jsr	(%a1)
+
+/*------------------------------------------------------------------------------*/
+/* exception code */
+	.globl _fault
+_fault:
+	jmp _fault
+	.globl	_exc_handler
+
+_exc_handler:
+	SAVE_ALL
+	movel	%sp,%sp@-
+	bsr exc_handler
+	addql	#4,%sp
+	RESTORE_ALL
+
+	.globl	_int_handler
+_int_handler:
+	SAVE_ALL
+	movel	%sp,%sp@-
+	bsr int_handler
+	addql	#4,%sp
+	RESTORE_ALL
+
+/*------------------------------------------------------------------------------*/
+/* cache functions */
+	.globl	icache_enable
+icache_enable:
+	move.l	#0x01000000, %d0		/* Invalidate cache cmd */
+	movec	%d0, %CACR			/* Invalidate cache */
+	move.l	#(CFG_SDRAM_BASE + 0xc000 + ((CFG_SDRAM_SIZE & 0x1fe0) << 11)), %d0
+	movec	%d0, %ACR0			/* Enable cache */
+
+	move.l	#0x80000200, %d0		/* Setup cache mask */
+	movec	%d0, %CACR			/* Enable cache */
+	nop
+
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	moveq	#1, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	icache_disable
+icache_disable:
+	move.l	#0x01000000, %d0		/* Setup cache mask */
+	movec	%d0, %CACR			/* Disable cache */
+	clr.l	%d0				/* Setup cache mask */
+	movec	%d0, %ACR0
+	movec	%d0, %ACR1
+
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	moveq	#0, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	icache_status
+icache_status:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-8), %a1
+	move.l	(%a1), %d0
+	rts
+
+	.globl	icache_invalid
+icache_invalid:
+	move.l	#0x81000200, %d0		/* Setup cache mask */
+	movec	%d0, %CACR			/* Enable cache */
+	rts
+
+	.globl	dcache_enable
+dcache_enable:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a1
+	moveq	#1, %d0
+	move.l	%d0, (%a1)
+	rts
+
+    /* No dcache, just a dummy function */
+	.globl	dcache_disable
+dcache_disable:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a1
+	moveq	#0, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	dcache_status
+dcache_status:
+	move.l #(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-4), %a1
+	move.l	(%a1), %d0
+	rts
+
+/*------------------------------------------------------------------------------*/
+
+	.globl	version_string
+version_string:
+	.ascii U_BOOT_VERSION
+	.ascii " (", __DATE__, " - ", __TIME__, ")"
+	.ascii CONFIG_IDENT_STRING, "\0"
diff --git a/cpu/mcf5445x/Makefile b/cpu/mcf5445x/Makefile
new file mode 100644
index 0000000000..26ec29895e
--- /dev/null
+++ b/cpu/mcf5445x/Makefile
@@ -0,0 +1,48 @@
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@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 $(TOPDIR)/config.mk
+
+# CFLAGS += -DET_DEBUG
+
+LIB	= lib$(CPU).a
+
+START	= start.o
+COBJS	= cpu.o speed.o cpu_init.o interrupts.o pci.o
+
+SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(START) $(LIB)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/mcf5445x/config.mk b/cpu/mcf5445x/config.mk
new file mode 100644
index 0000000000..d0c72fb6b0
--- /dev/null
+++ b/cpu/mcf5445x/config.mk
@@ -0,0 +1,27 @@
+#
+# (C) Copyright 2003 Josef Baumgartner <josef.baumgartner@telex.de>
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@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
+#
+
+PLATFORM_RELFLAGS += -ffixed-d7 -msep-data
+PLATFORM_CPPFLAGS += -m5407 -fPIC
diff --git a/cpu/mcf5445x/cpu.c b/cpu/mcf5445x/cpu.c
new file mode 100644
index 0000000000..e601b8949b
--- /dev/null
+++ b/cpu/mcf5445x/cpu.c
@@ -0,0 +1,97 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <watchdog.h>
+#include <command.h>
+
+#include <asm/immap.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
+{
+	volatile rcm_t *rcm = (rcm_t *) (MMAP_RCM);
+	udelay(1000);
+	rcm->rcr |= RCM_RCR_SOFTRST;
+
+	/* we don't return! */
+	return 0;
+};
+
+int checkcpu(void)
+{
+	volatile ccm_t *ccm = (ccm_t *) MMAP_CCM;
+	u16 msk;
+	u16 id = 0;
+	u8 ver;
+
+	puts("CPU:   ");
+	msk = (ccm->cir >> 6);
+	ver = (ccm->cir & 0x003f);
+	switch (msk) {
+	case 0x48:
+		id = 54455;
+		break;
+	case 0x49:
+		id = 54454;
+		break;
+	case 0x4a:
+		id = 54453;
+		break;
+	case 0x4b:
+		id = 54452;
+		break;
+	case 0x4d:
+		id = 54451;
+		break;
+	case 0x4f:
+		id = 54450;
+		break;
+	}
+
+	if (id) {
+		printf("Freescale MCF%d (Mask:%01x Version:%x)\n", id, msk,
+		       ver);
+		printf("       CPU CLK %d Mhz BUS CLK %d Mhz FLB CLK %d Mhz\n",
+		       (int)(gd->cpu_clk / 1000000),
+		       (int)(gd->bus_clk / 1000000),
+		       (int)(gd->flb_clk / 1000000));
+#ifdef CONFIG_PCI
+		printf("       PCI CLK %d Mhz INP CLK %d Mhz VCO CLK %d Mhz\n",
+		       (int)(gd->pci_clk / 1000000),
+		       (int)(gd->inp_clk / 1000000),
+		       (int)(gd->vco_clk / 1000000));
+#else
+		printf("       INP CLK %d Mhz VCO CLK %d Mhz\n",
+		       (int)(gd->inp_clk / 1000000),
+		       (int)(gd->vco_clk / 1000000));
+#endif
+	}
+
+	return 0;
+}
diff --git a/cpu/mcf5445x/cpu_init.c b/cpu/mcf5445x/cpu_init.c
new file mode 100644
index 0000000000..6622eeea98
--- /dev/null
+++ b/cpu/mcf5445x/cpu_init.c
@@ -0,0 +1,140 @@
+/*
+ *
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * (C) Copyright 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <watchdog.h>
+
+#include <asm/immap.h>
+#include <asm/rtc.h>
+
+/*
+ * Breath some life into the CPU...
+ *
+ * Set up the memory map,
+ * initialize a bunch of registers,
+ * initialize the UPM's
+ */
+void cpu_init_f(void)
+{
+	volatile scm1_t *scm1 = (scm1_t *) MMAP_SCM1;
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
+	volatile fbcs_t *fbcs = (fbcs_t *) MMAP_FBCS;
+
+	scm1->mpr = 0x77777777;
+	scm1->pacra = 0;
+	scm1->pacrb = 0;
+	scm1->pacrc = 0;
+	scm1->pacrd = 0;
+	scm1->pacre = 0;
+	scm1->pacrf = 0;
+	scm1->pacrg = 0;
+
+	/* FlexBus */
+	gpio->par_be =
+	    GPIO_PAR_BE_BE3_BE3 | GPIO_PAR_BE_BE2_BE2 | GPIO_PAR_BE_BE1_BE1 |
+	    GPIO_PAR_BE_BE0_BE0;
+	gpio->par_fbctl =
+	    GPIO_PAR_FBCTL_OE | GPIO_PAR_FBCTL_TA_TA | GPIO_PAR_FBCTL_RW_RW |
+	    GPIO_PAR_FBCTL_TS_TS;
+
+#if (defined(CFG_CS0_BASE) && defined(CFG_CS0_MASK) && defined(CFG_CS0_CTRL))
+	fbcs->csar0 = CFG_CS0_BASE;
+	fbcs->cscr0 = CFG_CS0_CTRL;
+	fbcs->csmr0 = CFG_CS0_MASK;
+#endif
+
+#if (defined(CFG_CS1_BASE) && defined(CFG_CS1_MASK) && defined(CFG_CS1_CTRL))
+	/* Latch chipselect */
+	fbcs->csar1 = CFG_CS1_BASE;
+	fbcs->cscr1 = CFG_CS1_CTRL;
+	fbcs->csmr1 = CFG_CS1_MASK;
+#endif
+
+#if (defined(CFG_CS2_BASE) && defined(CFG_CS2_MASK) && defined(CFG_CS2_CTRL))
+	fbcs->csar2 = CFG_CS2_BASE;
+	fbcs->cscr2 = CFG_CS2_CTRL;
+	fbcs->csmr2 = CFG_CS2_MASK;
+#endif
+
+#if (defined(CFG_CS3_BASE) && defined(CFG_CS3_MASK) && defined(CFG_CS3_CTRL))
+	fbcs->csar3 = CFG_CS3_BASE;
+	fbcs->cscr3 = CFG_CS3_CTRL;
+	fbcs->csmr3 = CFG_CS3_MASK;
+#endif
+
+#if (defined(CFG_CS4_BASE) && defined(CFG_CS4_MASK) && defined(CFG_CS4_CTRL))
+	fbcs->csar4 = CFG_CS4_BASE;
+	fbcs->cscr4 = CFG_CS4_CTRL;
+	fbcs->csmr4 = CFG_CS4_MASK;
+#endif
+
+#if (defined(CFG_CS5_BASE) && defined(CFG_CS5_MASK) && defined(CFG_CS5_CTRL))
+	fbcs->csar5 = CFG_CS5_BASE;
+	fbcs->cscr5 = CFG_CS5_CTRL;
+	fbcs->csmr5 = CFG_CS5_MASK;
+#endif
+
+#ifdef CONFIG_FSL_I2C
+	gpio->par_feci2c = GPIO_PAR_FECI2C_SCL_SCL | GPIO_PAR_FECI2C_SDA_SDA;
+#endif
+
+	icache_enable();
+}
+
+/*
+ * initialize higher level parts of CPU like timers
+ */
+int cpu_init_r(void)
+{
+#ifdef CONFIG_MCFTMR
+	volatile rtc_t *rtc = (volatile rtc_t *)(CFG_MCFRTC_BASE);
+	volatile rtcex_t *rtcex = (volatile rtcex_t *)&rtc->extended;
+	u32 oscillator = CFG_RTC_OSCILLATOR;
+
+	rtcex->gocu = (CFG_RTC_OSCILLATOR >> 16) & 0xFFFF;
+	rtcex->gocl = CFG_RTC_OSCILLATOR & 0xFFFF;
+#endif
+
+	return (0);
+}
+
+void uart_port_conf(void)
+{
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
+
+	/* Setup Ports: */
+	switch (CFG_UART_PORT) {
+	case 0:
+		gpio->par_uart =
+		    (GPIO_PAR_UART_U0TXD_U0TXD | GPIO_PAR_UART_U0RXD_U0RXD);
+		break;
+	case 1:
+		gpio->par_uart =
+		    (GPIO_PAR_UART_U1TXD_U1TXD | GPIO_PAR_UART_U1RXD_U1RXD);
+		break;
+	}
+}
diff --git a/cpu/mcf5445x/interrupts.c b/cpu/mcf5445x/interrupts.c
new file mode 100644
index 0000000000..9572a7bc32
--- /dev/null
+++ b/cpu/mcf5445x/interrupts.c
@@ -0,0 +1,52 @@
+/*
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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
+ */
+
+/* CPU specific interrupt routine */
+#include <common.h>
+#include <asm/immap.h>
+
+int interrupt_init(void)
+{
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
+
+	/* Make sure all interrupts are disabled */
+	intp->imrh0 |= 0xFFFFFFFF;
+	intp->imrl0 |= 0xFFFFFFFF;
+
+	enable_interrupts();
+	return 0;
+}
+
+#if defined(CONFIG_MCFTMR)
+void dtimer_intr_setup(void)
+{
+	volatile int0_t *intp = (int0_t *) (CFG_INTR_BASE);
+
+	intp->icr0[CFG_TMRINTR_NO] = CFG_TMRINTR_PRI;
+	intp->imrh0 &= ~CFG_TMRINTR_MASK;
+}
+#endif
diff --git a/cpu/mcf5445x/pci.c b/cpu/mcf5445x/pci.c
new file mode 100644
index 0000000000..8ace53630f
--- /dev/null
+++ b/cpu/mcf5445x/pci.c
@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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
+ */
+
+/*
+ * PCI Configuration space access support
+ */
+#include <common.h>
+#include <pci.h>
+#include <asm/io.h>
+#include <asm/immap.h>
+
+#if defined(CONFIG_PCI)
+/* System RAM mapped over PCI */
+#define CFG_PCI_SYS_MEM_BUS	CFG_SDRAM_BASE
+#define CFG_PCI_SYS_MEM_PHYS	CFG_SDRAM_BASE
+#define CFG_PCI_SYS_MEM_SIZE	(1024 * 1024 * 1024)
+
+#define cfg_read(val, addr, type, op)		*val = op((type)(addr));
+#define cfg_write(val, addr, type, op)		op((type *)(addr), (val));
+
+#define PCI_OP(rw, size, type, op, mask)				\
+int pci_##rw##_cfg_##size(struct pci_controller *hose,			\
+	pci_dev_t dev, int offset, type val)				\
+{									\
+	u32 addr = 0;							\
+	u16 cfg_type = 0;						\
+	addr = ((offset & 0xfc) | cfg_type | (dev)  | 0x80000000);	\
+	out_be32(hose->cfg_addr, addr);					\
+	__asm__ __volatile__("nop");					\
+	cfg_##rw(val, hose->cfg_data + (offset & mask), type, op);	\
+	out_be32(hose->cfg_addr, addr & 0x7fffffff);			\
+	__asm__ __volatile__("nop");					\
+	return 0;							\
+}
+
+PCI_OP(read, byte, u8 *, in_8, 3)
+PCI_OP(read, word, u16 *, in_le16, 2)
+PCI_OP(write, byte, u8, out_8, 3)
+PCI_OP(write, word, u16, out_le16, 2)
+PCI_OP(write, dword, u32, out_le32, 0)
+
+int pci_read_cfg_dword(struct pci_controller *hose, pci_dev_t dev,
+		       int offset, u32 * val)
+{
+	u32 addr;
+	u32 tmpv;
+	u32 mask = 2;		/* word access */
+	/* Read lower 16 bits */
+	addr = ((offset & 0xfc) | (dev) | 0x80000000);
+	out_be32(hose->cfg_addr, addr);
+	__asm__ __volatile__("nop");
+	*val = (u32) in_le16((u16 *) (hose->cfg_data + (offset & mask)));
+	out_be32(hose->cfg_addr, addr & 0x7fffffff);
+	__asm__ __volatile__("nop");
+
+	/* Read upper 16 bits */
+	offset += 2;
+	addr = ((offset & 0xfc) | 1 | (dev) | 0x80000000);
+	out_be32(hose->cfg_addr, addr);
+	__asm__ __volatile__("nop");
+	tmpv = (u32) in_le16((u16 *) (hose->cfg_data + (offset & mask)));
+	out_be32(hose->cfg_addr, addr & 0x7fffffff);
+	__asm__ __volatile__("nop");
+
+	/* combine results into dword value */
+	*val = (tmpv << 16) | *val;
+
+	return 0;
+}
+
+void pci_mcf5445x_init(struct pci_controller *hose)
+{
+	volatile pci_t *pci = (volatile pci_t *)MMAP_PCI;
+	volatile pciarb_t *pciarb = (volatile pciarb_t *)MMAP_PCIARB;
+	volatile gpio_t *gpio = (gpio_t *) MMAP_GPIO;
+	u32 barEn = 0;
+
+	pciarb->acr = 0x001f001f;
+
+	/* Set PCIGNT1, PCIREQ1, PCIREQ0/PCIGNTIN, PCIGNT0/PCIREQOUT,
+	   PCIREQ2, PCIGNT2 */
+	gpio->par_pci =
+	    GPIO_PAR_PCI_GNT3_GNT3 | GPIO_PAR_PCI_GNT2 | GPIO_PAR_PCI_GNT1 |
+	    GPIO_PAR_PCI_GNT0 | GPIO_PAR_PCI_REQ3_REQ3 | GPIO_PAR_PCI_REQ2 |
+	    GPIO_PAR_PCI_REQ1 | GPIO_PAR_PCI_REQ0;
+
+	pci->tcr1 |= PCI_TCR1_P;
+
+	/* Initiator windows */
+	pci->iw0btar = CFG_PCI_MEM_PHYS;
+	pci->iw1btar = CFG_PCI_IO_PHYS;
+	pci->iw2btar = CFG_PCI_CFG_PHYS;
+
+	pci->iwcr =
+	    PCI_IWCR_W0C_EN | PCI_IWCR_W1C_EN | PCI_IWCR_W1C_IO |
+	    PCI_IWCR_W2C_EN | PCI_IWCR_W2C_IO;
+
+	/* Enable bus master and mem access */
+	pci->scr = PCI_SCR_MW | PCI_SCR_B | PCI_SCR_M;
+
+	/* Cache line size and master latency */
+	pci->cr1 = PCI_CR1_CLS(8) | PCI_CR1_LTMR(0xFF);
+	pci->cr2 = 0;
+
+#ifdef CFG_PCI_BAR0
+	pci->bar0 = PCI_BAR_BAR0(CFG_PCI_BAR0);
+	pci->tbatr0 = CFG_PCI_TBATR0 | PCI_TBATR_EN;
+	barEn |= PCI_TCR1_B0E;
+#endif
+#ifdef CFG_PCI_BAR1
+	pci->bar1 = PCI_BAR_BAR1(CFG_PCI_BAR1);
+	pci->tbatr1 = CFG_PCI_TBATR1 | PCI_TBATR_EN;
+	barEn |= PCI_TCR1_B1E;
+#endif
+#ifdef CFG_PCI_BAR2
+	pci->bar2 = PCI_BAR_BAR2(CFG_PCI_BAR2);
+	pci->tbatr2 = CFG_PCI_TBATR2 | PCI_TBATR_EN;
+	barEn |= PCI_TCR1_B2E;
+#endif
+#ifdef CFG_PCI_BAR3
+	pci->bar3 = PCI_BAR_BAR3(CFG_PCI_BAR3);
+	pci->tbatr3 = CFG_PCI_TBATR3 | PCI_TBATR_EN;
+	barEn |= PCI_TCR1_B3E;
+#endif
+#ifdef CFG_PCI_BAR4
+	pci->bar4 = PCI_BAR_BAR4(CFG_PCI_BAR4);
+	pci->tbatr4 = CFG_PCI_TBATR4 | PCI_TBATR_EN;
+	barEn |= PCI_TCR1_B4E;
+#endif
+#ifdef CFG_PCI_BAR5
+	pci->bar5 = PCI_BAR_BAR5(CFG_PCI_BAR5);
+	pci->tbatr5 = CFG_PCI_TBATR5 | PCI_TBATR_EN;
+	barEn |= PCI_TCR1_B5E;
+#endif
+
+	pci->tcr2 = barEn;
+
+	/* Deassert reset bit */
+	pci->gscr &= ~PCI_GSCR_PR;
+	udelay(1000);
+
+	/* Enable PCI bus master support */
+	hose->first_busno = 0;
+	hose->last_busno = 0xff;
+
+	pci_set_region(hose->regions + 0, CFG_PCI_MEM_BUS, CFG_PCI_MEM_PHYS,
+		       CFG_PCI_MEM_SIZE, PCI_REGION_MEM);
+
+	pci_set_region(hose->regions + 1, CFG_PCI_IO_BUS, CFG_PCI_IO_PHYS,
+		       CFG_PCI_IO_SIZE, PCI_REGION_IO);
+
+	pci_set_region(hose->regions + 2, CFG_PCI_SYS_MEM_BUS,
+		       CFG_PCI_SYS_MEM_PHYS, CFG_PCI_SYS_MEM_SIZE,
+		       PCI_REGION_MEM | PCI_REGION_MEMORY);
+
+	hose->region_count = 3;
+
+	hose->cfg_addr = &(pci->car);
+	hose->cfg_data = (volatile unsigned char *)CFG_PCI_CFG_BUS;
+
+	pci_set_ops(hose, pci_read_cfg_byte, pci_read_cfg_word,
+		    pci_read_cfg_dword, pci_write_cfg_byte, pci_write_cfg_word,
+		    pci_write_cfg_dword);
+
+	/* Hose scan */
+	pci_register_hose(hose);
+	hose->last_busno = pci_hose_scan(hose);
+}
+#endif				/* CONFIG_PCI */
diff --git a/cpu/mcf5445x/speed.c b/cpu/mcf5445x/speed.c
new file mode 100644
index 0000000000..967becfdd0
--- /dev/null
+++ b/cpu/mcf5445x/speed.c
@@ -0,0 +1,186 @@
+/*
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * 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>
+#include <asm/processor.h>
+
+#include <asm/immap.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/*
+ * Low Power Divider specifications
+ */
+#define CLOCK_LPD_MIN		(1 << 0)	/* Divider (decoded) */
+#define CLOCK_LPD_MAX		(1 << 15)	/* Divider (decoded) */
+
+#define CLOCK_PLL_FVCO_MAX	540000000
+#define CLOCK_PLL_FVCO_MIN	300000000
+
+#define CLOCK_PLL_FSYS_MAX	266666666
+#define CLOCK_PLL_FSYS_MIN	100000000
+#define MHZ			1000000
+
+void clock_enter_limp(int lpdiv)
+{
+	volatile ccm_t *ccm = (volatile ccm_t *)MMAP_CCM;
+	int i, j;
+
+	/* Check bounds of divider */
+	if (lpdiv < CLOCK_LPD_MIN)
+		lpdiv = CLOCK_LPD_MIN;
+	if (lpdiv > CLOCK_LPD_MAX)
+		lpdiv = CLOCK_LPD_MAX;
+
+	/* Round divider down to nearest power of two */
+	for (i = 0, j = lpdiv; j != 1; j >>= 1, i++) ;
+
+	/* Apply the divider to the system clock */
+	ccm->cdr = (ccm->cdr & 0xF0FF) | CCM_CDR_LPDIV(i);
+
+	/* Enable Limp Mode */
+	ccm->misccr |= CCM_MISCCR_LIMP;
+}
+
+/*
+ * brief   Exit Limp mode
+ * warning The PLL should be set and locked prior to exiting Limp mode
+ */
+void clock_exit_limp(void)
+{
+	volatile ccm_t *ccm = (volatile ccm_t *)MMAP_CCM;
+	volatile pll_t *pll = (volatile pll_t *)MMAP_PLL;
+
+	/* Exit Limp mode */
+	ccm->misccr &= ~CCM_MISCCR_LIMP;
+
+	/* Wait for the PLL to lock */
+	while (!(pll->psr & PLL_PSR_LOCK)) ;
+}
+
+/*
+ * get_clocks() fills in gd->cpu_clock and gd->bus_clk
+ */
+int get_clocks(void)
+{
+	volatile ccm_t *ccm = (volatile ccm_t *)MMAP_CCM;
+	volatile pll_t *pll = (volatile pll_t *)MMAP_PLL;
+	volatile u8 *cpld = (volatile u8 *)(CFG_CS2_BASE + 3);
+	volatile u8 *fpga = (volatile u8 *)(CFG_CS3_BASE + 14);
+	int pllmult_nopci[] = { 20, 10, 24, 18, 12, 6, 16, 8 };
+	int pllmult_pci[] = { 12, 6, 16, 8 };
+	int vco, bPci, temp, fbtemp, pcrvalue;
+	int *pPllmult = NULL;
+	u16 fbpll_mask;
+	u8 cpldmode;
+
+	/* To determine PCI is present or not */
+	if (((ccm->ccr & CCM_CCR_360_FBCONFIG_MASK) == 0x00e0) ||
+	    ((ccm->ccr & CCM_CCR_360_FBCONFIG_MASK) == 0x0060)) {
+		pPllmult = &pllmult_pci[0];
+		fbpll_mask = 3;
+		bPci = 1;
+	} else {
+		pPllmult = &pllmult_nopci[0];
+		fbpll_mask = 7;
+#ifdef CONFIG_PCI
+		gd->pci_clk = 0;
+#endif
+		bPci = 0;
+	}
+
+#ifdef CONFIG_M54455EVB
+	/* Temporary place here, belongs in board/freescale/... */
+	/* Temporary read from CCR- fixed fb issue, must be the same clock
+	   as pci or input clock, causing cpld/fpga read inconsistancy */
+	fbtemp = pPllmult[ccm->ccr & fbpll_mask];
+
+	/* Break down into small pieces, code still in flex bus */
+	pcrvalue = pll->pcr & 0xFFFFF0FF;
+	temp = fbtemp - 1;
+	pcrvalue |= PLL_PCR_OUTDIV3(temp);
+
+	pll->pcr = pcrvalue;
+
+	cpldmode = *cpld & 0x03;
+	if (cpldmode == 0) {
+		/* RCON mode */
+		vco = pPllmult[ccm->rcon & fbpll_mask] * CFG_INPUT_CLKSRC;
+
+		if ((vco < CLOCK_PLL_FVCO_MIN) || (vco > CLOCK_PLL_FVCO_MAX)) {
+			/* invaild range, re-set in PCR */
+			int temp = ((pll->pcr & PLL_PCR_OUTDIV2_MASK) >> 4) + 1;
+			int i, j, bus;
+
+			j = (pll->pcr & 0xFF000000) >> 24;
+			for (i = j; i < 0xFF; i++) {
+				vco = i * CFG_INPUT_CLKSRC;
+				if (vco >= CLOCK_PLL_FVCO_MIN) {
+					bus = vco / temp;
+					if (bus <= CLOCK_PLL_FSYS_MIN - MHZ)
+						continue;
+					else
+						break;
+				}
+			}
+			pcrvalue = pll->pcr & 0x00FF00FF;
+			fbtemp = ((i - 1) << 8) | ((i - 1) << 12);
+			pcrvalue |= ((i << 24) | fbtemp);
+
+			pll->pcr = pcrvalue;
+		}
+		gd->vco_clk = vco;	/* Vco clock */
+	} else if (cpldmode == 2) {
+		/* Normal mode */
+		vco = pPllmult[ccm->ccr & fbpll_mask] * CFG_INPUT_CLKSRC;
+		gd->vco_clk = vco;	/* Vco clock */
+	} else if (cpldmode == 3) {
+		/* serial mode */
+	}
+#endif				/* CONFIG_M54455EVB */
+
+	if ((ccm->ccr & CCM_MISCCR_LIMP) == CCM_MISCCR_LIMP) {
+		/* Limp mode */
+	} else {
+		gd->inp_clk = CFG_INPUT_CLKSRC;	/* Input clock */
+
+		temp = (pll->pcr & PLL_PCR_OUTDIV1_MASK) + 1;
+		gd->cpu_clk = vco / temp;	/* cpu clock */
+
+		temp = ((pll->pcr & PLL_PCR_OUTDIV2_MASK) >> 4) + 1;
+		gd->bus_clk = vco / temp;	/* bus clock */
+
+		temp = ((pll->pcr & PLL_PCR_OUTDIV3_MASK) >> 8) + 1;
+		gd->flb_clk = vco / temp;	/* FlexBus clock */
+
+#ifdef CONFIG_PCI
+		if (bPci) {
+			temp = ((pll->pcr & PLL_PCR_OUTDIV4_MASK) >> 12) + 1;
+			gd->pci_clk = vco / temp;	/* PCI clock */
+		}
+#endif
+	}
+
+	return (0);
+}
diff --git a/cpu/mcf5445x/start.S b/cpu/mcf5445x/start.S
new file mode 100644
index 0000000000..cd989ab626
--- /dev/null
+++ b/cpu/mcf5445x/start.S
@@ -0,0 +1,388 @@
+/*
+ * Copyright (C) 2003	Josef Baumgartner <josef.baumgartner@telex.de>
+ * Based on code from Bernhard Kuhn <bkuhn@metrowerks.com>
+ *
+ * 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 <config.h>
+#include "version.h"
+
+#ifndef	 CONFIG_IDENT_STRING
+#define	 CONFIG_IDENT_STRING ""
+#endif
+
+/* last three long word reserved for cache status */
+#define CACR_STATUS	(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END-12)
+#define ICACHE_STATUS	(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END- 8)
+#define DCACHE_STATUS	(CFG_INIT_RAM_ADDR+CFG_INIT_RAM_END- 4)
+
+#define _START	_start
+#define _FAULT	_fault
+
+#define SAVE_ALL						\
+	move.w	#0x2700,%sr;		/* disable intrs */	\
+	subl	#60,%sp;		/* space for 15 regs */ \
+	moveml	%d0-%d7/%a0-%a6,%sp@;
+
+#define RESTORE_ALL						\
+	moveml	%sp@,%d0-%d7/%a0-%a6;				\
+	addl	#60,%sp;		/* space for 15 regs */ \
+	rte;
+
+.text
+/*
+ *	Vector table. This is used for initial platform startup.
+ *	These vectors are to catch any un-intended traps.
+ */
+_vectors:
+
+INITSP:		.long	0x00000000	/* Initial SP	*/
+INITPC:		.long	_START	/* Initial PC 		*/
+vector02:	.long	_FAULT	/* Access Error		*/
+vector03:	.long	_FAULT	/* Address Error	*/
+vector04:	.long	_FAULT	/* Illegal Instruction	*/
+vector05:	.long	_FAULT	/* Reserved		*/
+vector06:	.long	_FAULT	/* Reserved		*/
+vector07:	.long	_FAULT	/* Reserved		*/
+vector08:	.long	_FAULT	/* Privilege Violation	*/
+vector09:	.long	_FAULT	/* Trace		*/
+vector0A:	.long	_FAULT	/* Unimplemented A-Line	*/
+vector0B:	.long	_FAULT	/* Unimplemented F-Line	*/
+vector0C:	.long	_FAULT	/* Debug Interrupt	*/
+vector0D:	.long	_FAULT	/* Reserved		*/
+vector0E:	.long	_FAULT	/* Format Error		*/
+vector0F:	.long	_FAULT	/* Unitialized Int.	*/
+
+/* Reserved */
+vector10_17:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector18:	.long	_FAULT	/* Spurious Interrupt	*/
+vector19:	.long	_FAULT	/* Autovector Level 1	*/
+vector1A:	.long	_FAULT	/* Autovector Level 2	*/
+vector1B:	.long	_FAULT	/* Autovector Level 3	*/
+vector1C:	.long	_FAULT	/* Autovector Level 4	*/
+vector1D:	.long	_FAULT	/* Autovector Level 5	*/
+vector1E:	.long	_FAULT	/* Autovector Level 6	*/
+vector1F:	.long	_FAULT	/* Autovector Level 7	*/
+
+/* TRAP #0 - #15 */
+vector20_2F:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+/* Reserved	*/
+vector30_3F:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector64_127:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector128_191:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+vector192_255:
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+.long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+
+	.text
+
+	.globl	_start
+_start:
+	nop
+	nop
+	move.w #0x2700,%sr		/* Mask off Interrupt */
+
+	/* Set vector base register at the beginning of the Flash */
+	move.l	#CFG_FLASH_BASE, %d0
+	movec	%d0, %VBR
+
+	move.l	#(CFG_INIT_RAM_ADDR + CFG_INIT_RAM_CTRL), %d0
+	movec	%d0, %RAMBAR0
+
+	/* initialize general use internal ram */
+	move.l #0, %d0
+	move.l #(CACR_STATUS), %a1	/* CACR */
+	move.l #(ICACHE_STATUS), %a2	/* icache */
+	move.l #(DCACHE_STATUS), %a3	/* dcache */
+	move.l %d0, (%a1)
+	move.l %d0, (%a2)
+	move.l %d0, (%a3)
+
+	/* invalidate and disable cache */
+	move.l	#0x01004100, %d0	/* Invalidate cache cmd */
+	movec	%d0, %CACR		/* Invalidate cache */
+	move.l	#0, %d0
+	movec	%d0, %ACR0
+	movec	%d0, %ACR1
+	movec	%d0, %ACR2
+	movec	%d0, %ACR3
+
+	/* set stackpointer to end of internal ram to get some stackspace for
+	   the first c-code */
+	move.l	#(CFG_INIT_RAM_ADDR + CFG_INIT_SP_OFFSET), %sp
+	clr.l %sp@-
+
+	move.l #__got_start, %a5	/* put relocation table address to a5 */
+
+	bsr cpu_init_f			/* run low-level CPU init code (from flash) */
+	bsr board_init_f		/* run low-level board init code (from flash) */
+
+	/* board_init_f() does not return */
+
+/*------------------------------------------------------------------------------*/
+
+/*
+ * void relocate_code (addr_sp, gd, addr_moni)
+ *
+ * This "function" does not return, instead it continues in RAM
+ * after relocating the monitor code.
+ *
+ * r3 = dest
+ * r4 = src
+ * r5 = length in bytes
+ * r6 = cachelinesize
+ */
+	.globl	relocate_code
+relocate_code:
+	link.w %a6,#0
+	move.l 8(%a6), %sp		/* set new stack pointer */
+
+	move.l 12(%a6), %d0		/* Save copy of Global Data pointer */
+	move.l 16(%a6), %a0		/* Save copy of Destination Address */
+
+	move.l #CFG_MONITOR_BASE, %a1
+	move.l #__init_end, %a2
+	move.l %a0, %a3
+
+	/* copy the code to RAM */
+1:
+	move.l (%a1)+, (%a3)+
+	cmp.l  %a1,%a2
+	bgt.s	 1b
+
+/*
+ * We are done. Do not return, instead branch to second part of board
+ * initialization, now running from RAM.
+ */
+	move.l	%a0, %a1
+	add.l	#(in_ram - CFG_MONITOR_BASE), %a1
+	jmp	(%a1)
+
+in_ram:
+
+clear_bss:
+	/*
+	 * Now clear BSS segment
+	 */
+	move.l	%a0, %a1
+	add.l	#(_sbss - CFG_MONITOR_BASE),%a1
+	move.l	%a0, %d1
+	add.l	#(_ebss - CFG_MONITOR_BASE),%d1
+6:
+	clr.l	(%a1)+
+	cmp.l	%a1,%d1
+	bgt.s	6b
+
+	/*
+	 * fix got table in RAM
+	 */
+	move.l	%a0, %a1
+	add.l	#(__got_start - CFG_MONITOR_BASE),%a1
+	move.l	%a1,%a5			/* * fix got pointer register a5 */
+
+	move.l	%a0, %a2
+	add.l	#(__got_end - CFG_MONITOR_BASE),%a2
+
+7:
+	move.l	(%a1),%d1
+	sub.l	#_start,%d1
+	add.l	%a0,%d1
+	move.l	%d1,(%a1)+
+	cmp.l	%a2, %a1
+	bne	7b
+
+	/* calculate relative jump to board_init_r in ram */
+	move.l %a0, %a1
+	add.l #(board_init_r - CFG_MONITOR_BASE), %a1
+
+	/* set parameters for board_init_r */
+	move.l %a0,-(%sp)		/* dest_addr */
+	move.l %d0,-(%sp)		/* gd */
+	jsr	(%a1)
+
+/*------------------------------------------------------------------------------*/
+/* exception code */
+	.globl _fault
+_fault:
+	jmp _fault
+	.globl	_exc_handler
+
+_exc_handler:
+	SAVE_ALL
+	movel	%sp,%sp@-
+	bsr exc_handler
+	addql	#4,%sp
+	RESTORE_ALL
+
+	.globl	_int_handler
+_int_handler:
+	SAVE_ALL
+	movel	%sp,%sp@-
+	bsr int_handler
+	addql	#4,%sp
+	RESTORE_ALL
+
+/*------------------------------------------------------------------------------*/
+/* cache functions */
+	.globl	icache_enable
+icache_enable:
+	move.l #(CACR_STATUS), %a1	/* read CACR Status */
+	move.l	(%a1), %d1
+
+	move.l	#0x00040100, %d0	/* Invalidate icache */
+	or.l	%d1, %d0
+	movec	%d0, %CACR
+
+	move.l	#(CFG_SDRAM_BASE + 0xc000), %d0	/* Setup icache */
+	movec	%d0, %ACR2
+
+	or.l	#0x00088400, %d1	/* Enable bcache and icache */
+	movec	%d1, %CACR
+
+	move.l #(ICACHE_STATUS), %a1
+	moveq	#1, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	icache_disable
+icache_disable:
+	move.l #(CACR_STATUS), %a1	/* read CACR Status */
+	move.l	(%a1), %d0
+
+	and.l	#0xFFF77BFF, %d0
+	or.l	#0x00040100, %d0	/* Setup cache mask */
+	movec	%d0, %CACR		/* Invalidate icache */
+	clr.l	%d0
+	movec	%d0, %ACR2
+	movec	%d0, %ACR3
+
+	move.l #(ICACHE_STATUS), %a1
+	moveq	#0, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	icache_status
+icache_status:
+	move.l #(ICACHE_STATUS), %a1
+	move.l	(%a1), %d0
+	rts
+
+	.globl	icache_invalid
+icache_invalid:
+	move.l #(CACR_STATUS), %a1	/* read CACR Status */
+	move.l	(%a1), %d0
+
+	or.l	#0x00040100, %d0	/* Invalidate icache */
+	movec	%d0, %CACR		/* Enable and invalidate cache */
+	rts
+
+	.globl	dcache_enable
+dcache_enable:
+	move.l #(CACR_STATUS), %a1	/* read CACR Status */
+	move.l	(%a1), %d1
+
+	move.l	#0x01000000, %d0
+	or.l	%d1, %d0
+	movec	%d0, %CACR		/* Invalidate dcache */
+
+	move.l  #(CFG_SDRAM_BASE + 0xc000), %d0
+	movec	%d0, %ACR0
+	move.l  #0, %d0
+	movec	%d0, %ACR1
+
+	or.l	#0x80000000, %d1	/* Enable bcache and icache */
+	movec	%d1, %CACR
+
+	move.l #(DCACHE_STATUS), %a1
+	moveq	#1, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	dcache_disable
+dcache_disable:
+	move.l #(CACR_STATUS), %a1	/* read CACR Status */
+	move.l	(%a1), %d0
+
+	and.l	#0x7FFFFFFF, %d0
+	or.l	#0x01000000, %d0	/* Setup cache mask */
+	movec	%d0, %CACR		/* Disable dcache */
+	clr.l	%d0
+	movec	%d0, %ACR0
+	movec	%d0, %ACR1
+
+	move.l #(DCACHE_STATUS), %a1
+	moveq	#0, %d0
+	move.l	%d0, (%a1)
+	rts
+
+	.globl	dcache_invalid
+dcache_invalid:
+	move.l #(CACR_STATUS), %a1	/* read CACR Status */
+	move.l	(%a1), %d0
+
+	or.l	#0x01000000, %d0	/* Setup cache mask */
+	movec	%d0, %CACR		/* Enable and invalidate cache */
+	rts
+
+	.globl	dcache_status
+dcache_status:
+	move.l #(DCACHE_STATUS), %a1
+	move.l	(%a1), %d0
+	rts
+
+/*------------------------------------------------------------------------------*/
+
+	.globl	version_string
+version_string:
+	.ascii U_BOOT_VERSION
+	.ascii " (", __DATE__, " - ", __TIME__, ")"
+	.ascii CONFIG_IDENT_STRING, "\0"
diff --git a/cpu/mpc512x/config.mk b/cpu/mpc512x/config.mk
index 8a07c5a3b6..3259d53a13 100644
--- a/cpu/mpc512x/config.mk
+++ b/cpu/mpc512x/config.mk
@@ -19,7 +19,7 @@
 # Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 # MA 02111-1307 USA
 #
-PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi
+PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -mrelocatable
 
 PLATFORM_CPPFLAGS += -DCONFIG_MPC512X -DCONFIG_E300 \
 			-ffixed-r2 -ffixed-r29 -msoft-float -mcpu=603e
diff --git a/cpu/mpc512x/cpu.c b/cpu/mpc512x/cpu.c
index 3be565ad03..accae6e066 100644
--- a/cpu/mpc512x/cpu.c
+++ b/cpu/mpc512x/cpu.c
@@ -42,7 +42,7 @@ int checkcpu (void)
 	u32 spridr = immr->sysconf.spridr;
 	char buf[32];
 
-	puts("CPU: ");
+	puts ("CPU:   ");
 
 	switch (spridr & 0xffff0000) {
 	case SPR_5121E:
diff --git a/cpu/mpc512x/fec.c b/cpu/mpc512x/fec.c
index 1c87a53859..675b7a2e09 100644
--- a/cpu/mpc512x/fec.c
+++ b/cpu/mpc512x/fec.c
@@ -17,10 +17,10 @@ DECLARE_GLOBAL_DATA_PTR;
 
 #define DEBUG 0
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
 	defined(CONFIG_MPC512x_FEC)
 
-#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
 
@@ -32,6 +32,9 @@ int fec512x_miiphy_read(char *devname, uint8 phyAddr, uint8 regAddr, uint16 * re
 int fec512x_miiphy_write(char *devname, uint8 phyAddr, uint8 regAddr, uint16 data);
 int mpc512x_fec_init_phy(struct eth_device *dev, bd_t * bis);
 
+static uchar rx_buff[FEC_BUFFER_SIZE];
+static int rx_buff_idx = 0;
+
 /********************************************************************/
 #if (DEBUG & 0x2)
 static void mpc512x_fec_phydump (char *devname)
@@ -234,8 +237,8 @@ static int mpc512x_fec_init (struct eth_device *dev, bd_t * bis)
 	/* Set Opcode/Pause Duration Register */
 	fec->eth->op_pause = 0x00010020;
 
-	/* Frame length=1518; MII mode */
-	fec->eth->r_cntrl = 0x05ee000c;
+	/* Frame length=1522; MII mode */
+	fec->eth->r_cntrl = (FEC_MAX_FRAME_LEN << 16) | 0x24;
 
 	/* Half-duplex, heartbeat disabled */
 	fec->eth->x_cntrl = 0x00000000;
@@ -245,7 +248,7 @@ static int mpc512x_fec_init (struct eth_device *dev, bd_t * bis)
 
 	/* Setup recv fifo start and buff size */
 	fec->eth->r_fstart = 0x500;
-	fec->eth->r_buff_size = 0x5e0;
+	fec->eth->r_buff_size = FEC_BUFFER_SIZE;
 
 	/* Setup BD base addresses */
 	fec->eth->r_des_start = (uint32)fec->bdBase->rbd;
@@ -520,8 +523,7 @@ static int mpc512x_fec_recv (struct eth_device *dev)
 	mpc512x_fec_priv *fec = (mpc512x_fec_priv *)dev->priv;
 	volatile FEC_RBD *pRbd = &fec->bdBase->rbd[fec->rbdIndex];
 	unsigned long ievent;
-	int frame_length, len = 0;
-	uchar buff[FEC_MAX_PKT_SIZE];
+	int frame_length = 0;
 
 #if (DEBUG & 0x1)
 	printf ("mpc512x_fec_recv %d Start...\n", fec->rbdIndex);
@@ -555,31 +557,37 @@ static int mpc512x_fec_recv (struct eth_device *dev)
 	}
 
 	if (!(pRbd->status & FEC_RBD_EMPTY)) {
-		if ((pRbd->status & FEC_RBD_LAST) &&
-			!(pRbd->status & FEC_RBD_ERR) &&
+		if (!(pRbd->status & FEC_RBD_ERR) &&
 			((pRbd->dataLength - 4) > 14)) {
 
 			/*
 			 * Get buffer size
 			 */
-			frame_length = pRbd->dataLength - 4;
-
+			if (pRbd->status & FEC_RBD_LAST)
+				frame_length = pRbd->dataLength - 4;
+			else
+				frame_length = pRbd->dataLength;
 #if (DEBUG & 0x20)
 			{
 				int i;
-				printf ("recv data hdr:");
+				printf ("recv data length 0x%08x data hdr: ",
+					pRbd->dataLength);
 				for (i = 0; i < 14; i++)
 					printf ("%x ", *((uint8*)pRbd->dataPointer + i));
 				printf("\n");
 			}
 #endif
-
 			/*
 			 *  Fill the buffer and pass it to upper layers
 			 */
-			memcpy (buff, (void*)pRbd->dataPointer, frame_length);
-			NetReceive ((uchar*)buff, frame_length);
-			len = frame_length;
+			memcpy (&rx_buff[rx_buff_idx], (void*)pRbd->dataPointer,
+				frame_length - rx_buff_idx);
+			rx_buff_idx = frame_length;
+
+			if (pRbd->status & FEC_RBD_LAST) {
+				NetReceive ((uchar*)rx_buff, frame_length);
+				rx_buff_idx = 0;
+			}
 		}
 
 		/*
@@ -590,7 +598,7 @@ static int mpc512x_fec_recv (struct eth_device *dev)
 
 	/* Try to fill Buffer Descriptors */
 	fec->eth->r_des_active = 0x01000000;	/* Descriptor polling active */
-	return len;
+	return frame_length;
 }
 
 /********************************************************************/
@@ -626,7 +634,7 @@ int mpc512x_fec_initialize (bd_t * bis)
 	sprintf (dev->name, "FEC ETHERNET");
 	eth_register (dev);
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 	miiphy_register (dev->name,
 			fec512x_miiphy_read, fec512x_miiphy_write);
 #endif
diff --git a/cpu/mpc512x/fec.h b/cpu/mpc512x/fec.h
index d2d877aa52..9c38502870 100644
--- a/cpu/mpc512x/fec.h
+++ b/cpu/mpc512x/fec.h
@@ -164,10 +164,13 @@ typedef enum {
 #define FEC_RBD_NUM		32	/* The user can adjust this value */
 
 /* packet size limit */
-#define FEC_MAX_PKT_SIZE	1536
+#define FEC_MAX_FRAME_LEN	1522	/* recommended default value */
+
+/* Buffer size must be evenly divisible by 16 */
+#define FEC_BUFFER_SIZE		((FEC_MAX_FRAME_LEN + 0x10) & (~0xf))
 
 typedef struct {
-	uint8 frame[FEC_MAX_PKT_SIZE];
+	uint8 frame[FEC_BUFFER_SIZE];
 } mpc512x_frame;
 
 typedef struct {
diff --git a/cpu/mpc512x/traps.c b/cpu/mpc512x/traps.c
index 40281a2cbb..8455c92761 100644
--- a/cpu/mpc512x/traps.c
+++ b/cpu/mpc512x/traps.c
@@ -106,7 +106,7 @@ MachineCheckException (struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#ifdef CONFIG_CMD_KGDB
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -144,7 +144,7 @@ MachineCheckException (struct pt_regs *regs)
 void
 AlignmentException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#ifdef CONFIG_CMD_KGDB
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -156,7 +156,7 @@ AlignmentException (struct pt_regs *regs)
 void
 ProgramCheckException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#ifdef CONFIG_CMD_KGDB
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -168,7 +168,7 @@ ProgramCheckException (struct pt_regs *regs)
 void
 SoftEmuException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#ifdef CONFIG_CMD_KGDB
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -181,7 +181,7 @@ SoftEmuException (struct pt_regs *regs)
 void
 UnknownException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#ifdef CONFIG_CMD_KGDB
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -190,7 +190,7 @@ UnknownException (struct pt_regs *regs)
 	_exception (0, regs);
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#ifdef CONFIG_CMD_BEDBUG
 extern void do_bedbug_breakpoint (struct pt_regs *);
 #endif
 
@@ -199,7 +199,7 @@ DebugException (struct pt_regs *regs)
 {
 	printf ("Debugger trap at @ %lx\n", regs->nip );
 	show_regs (regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#ifdef CONFIG_CMD_BEDBUG
 	do_bedbug_breakpoint (regs);
 #endif
 }
diff --git a/cpu/mpc5xxx/cpu.c b/cpu/mpc5xxx/cpu.c
index 1eac2bbfbe..7f16b92a6c 100644
--- a/cpu/mpc5xxx/cpu.c
+++ b/cpu/mpc5xxx/cpu.c
@@ -29,10 +29,12 @@
 #include <watchdog.h>
 #include <command.h>
 #include <mpc5xxx.h>
+#include <asm/io.h>
 #include <asm/processor.h>
 
-#if defined(CONFIG_OF_FLAT_TREE)
-#include <ft_build.h>
+#if defined(CONFIG_OF_LIBFDT)
+#include <libfdt.h>
+#include <libfdt_env.h>
 #endif
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -111,29 +113,43 @@ unsigned long get_tbclk (void)
 
 /* ------------------------------------------------------------------------- */
 
-#ifdef CONFIG_OF_FLAT_TREE
-void
-ft_cpu_setup(void *blob, bd_t *bd)
+#ifdef CONFIG_OF_LIBFDT
+static void do_fixup(void *fdt, const char *node, const char *prop,
+		     const void *val, int len, int create)
 {
-	u32 *p;
-	int len;
-
-	/* Core XLB bus frequency */
-	p = ft_get_prop(blob, "/cpus/" OF_CPU "/bus-frequency", &len);
-	if (p != NULL)
-		*p = cpu_to_be32(bd->bi_busfreq);
-
-	/* SOC peripherals use the IPB bus frequency */
-	p = ft_get_prop(blob, "/" OF_SOC "/bus-frequency", &len);
-	if (p != NULL)
-		*p = cpu_to_be32(bd->bi_ipbfreq);
-
-	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@3000/mac-address", &len);
-	if (p != NULL)
-		memcpy(p, bd->bi_enetaddr, 6);
-
-	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@3000/local-mac-address", &len);
-	if (p != NULL)
-		memcpy(p, bd->bi_enetaddr, 6);
+#if defined(DEBUG)
+	int i;
+	debug("Updating property '%s/%s' = ", node, prop);
+	for (i = 0; i < len; i++)
+		debug(" %.2x", *(u8*)(val+i));
+	debug("\n");
+#endif
+	int rc = fdt_find_and_setprop(fdt, node, prop, val, len, create);
+	if (rc)
+		printf("Unable to update property %s:%s, err=%s\n",
+		       node, prop, fdt_strerror(rc));
+}
+
+static void do_fixup_u32(void *fdt, const char *node, const char *prop,
+			 u32 val, int create)
+{
+	val = cpu_to_fdt32(val);
+	do_fixup(fdt, node, prop, &val, sizeof(val), create);
+}
+
+void ft_cpu_setup(void *blob, bd_t *bd)
+{
+	int div = in_8((void*)CFG_MBAR + 0x204) & 0x0020 ? 8 : 4;
+	char * cpu_path = "/cpus/" OF_CPU;
+	char * eth_path = "/" OF_SOC "/ethernet@3000";
+
+	do_fixup_u32(blob, cpu_path, "timebase-frequency", OF_TBCLK, 1);
+	do_fixup_u32(blob, cpu_path, "bus-frequency", bd->bi_busfreq, 1);
+	do_fixup_u32(blob, cpu_path, "clock-frequency", bd->bi_intfreq, 1);
+	do_fixup_u32(blob, "/" OF_SOC, "bus-frequency", bd->bi_ipbfreq, 1);
+	do_fixup_u32(blob, "/" OF_SOC, "system-frequency",
+			bd->bi_busfreq*div, 1);
+	do_fixup(blob, eth_path, "mac-address", bd->bi_enetaddr, 6, 0);
+	do_fixup(blob, eth_path, "local-mac-address", bd->bi_enetaddr, 6, 0);
 }
 #endif
diff --git a/cpu/mpc5xxx/ide.c b/cpu/mpc5xxx/ide.c
index 087ddac109..df5b4acd0e 100644
--- a/cpu/mpc5xxx/ide.c
+++ b/cpu/mpc5xxx/ide.c
@@ -54,11 +54,19 @@ int ide_preinit (void)
 	/* All sample codes do that... */
 	*(vu_long *) MPC5XXX_ATA_SHARE_COUNT = 0;
 
+#if defined(CONFIG_UC101)
+	/* Configure and reset host */
+	*(vu_long *) MPC5XXX_ATA_HOST_CONFIG =
+		MPC5xxx_ATA_HOSTCONF_SMR | MPC5xxx_ATA_HOSTCONF_FR;
+	udelay (10);
+	*(vu_long *) MPC5XXX_ATA_HOST_CONFIG = 0;
+#else
 	/* Configure and reset host */
 	*(vu_long *) MPC5XXX_ATA_HOST_CONFIG = MPC5xxx_ATA_HOSTCONF_IORDY |
 		MPC5xxx_ATA_HOSTCONF_SMR | MPC5xxx_ATA_HOSTCONF_FR;
 	udelay (10);
 	*(vu_long *) MPC5XXX_ATA_HOST_CONFIG = MPC5xxx_ATA_HOSTCONF_IORDY;
+#endif
 
 	/* Disable prefetch on Commbus */
 	psdma->PtdCntrl |= 1;
diff --git a/cpu/mpc5xxx/usb.c b/cpu/mpc5xxx/usb.c
index ce709fc652..ed467ab3b8 100644
--- a/cpu/mpc5xxx/usb.c
+++ b/cpu/mpc5xxx/usb.c
@@ -27,7 +27,7 @@
 
 #include <mpc5xxx.h>
 
-int usb_cpu_init()
+int usb_cpu_init(void)
 {
 	/* Set the USB Clock						     */
 	*(vu_long *)MPC5XXX_CDM_48_FDC = CONFIG_USB_CLOCK;
@@ -41,12 +41,12 @@ int usb_cpu_init()
 	return 0;
 }
 
-int usb_cpu_stop()
+int usb_cpu_stop(void)
 {
 	return 0;
 }
 
-int usb_cpu_init_fail()
+int usb_cpu_init_fail(void)
 {
 	return 0;
 }
diff --git a/cpu/mpc83xx/Makefile b/cpu/mpc83xx/Makefile
index bb96f774fe..2329970051 100644
--- a/cpu/mpc83xx/Makefile
+++ b/cpu/mpc83xx/Makefile
@@ -29,7 +29,7 @@ LIB	= $(obj)lib$(CPU).a
 
 START	= start.o
 COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o \
-	  spd_sdram.o qe_io.o pci.o
+	  spd_sdram.o ecc.o qe_io.o pci.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/mpc83xx/cpu.c b/cpu/mpc83xx/cpu.c
index 841fe82428..e634f0a25b 100644
--- a/cpu/mpc83xx/cpu.c
+++ b/cpu/mpc83xx/cpu.c
@@ -33,15 +33,12 @@
 #include <asm/processor.h>
 #if defined(CONFIG_OF_FLAT_TREE)
 #include <ft_build.h>
-#endif
-#if defined(CONFIG_OF_LIBFDT)
+#elif defined(CONFIG_OF_LIBFDT)
 #include <libfdt.h>
-#include <libfdt_env.h>
 #endif
 
 DECLARE_GLOBAL_DATA_PTR;
 
-
 int checkcpu(void)
 {
 	volatile immap_t *immr;
@@ -113,12 +110,14 @@ int checkcpu(void)
 	case SPR_8360E_REV11:
 	case SPR_8360E_REV12:
 	case SPR_8360E_REV20:
+	case SPR_8360E_REV21:
 		puts("MPC8360E, ");
 		break;
 	case SPR_8360_REV10:
 	case SPR_8360_REV11:
 	case SPR_8360_REV12:
 	case SPR_8360_REV20:
+	case SPR_8360_REV21:
 		puts("MPC8360, ");
 		break;
 	case SPR_8323E_REV10:
@@ -150,7 +149,8 @@ int checkcpu(void)
 		puts("MPC8313E, ");
 		break;
 	default:
-		puts("Rev: Unknown revision number.\nWarning: Unsupported cpu revision!\n");
+		printf("Rev: Unknown revision number:%08x\n"
+			"Warning: Unsupported cpu revision!\n",spridr);
 		return 0;
 	}
 
@@ -329,178 +329,237 @@ void watchdog_reset (void)
 /*
  * "Setter" functions used to add/modify FDT entries.
  */
-static int fdt_set_eth0(void *fdt, int nodeoffset, const char *name, bd_t *bd)
+static int fdt_set_eth0(void *blob, int nodeoffset, const char *name, bd_t *bd)
 {
-	/*
-	 * Fix it up if it exists, don't create it if it doesn't exist.
-	 */
-	if (fdt_get_property(fdt, nodeoffset, name, 0)) {
-		return fdt_setprop(fdt, nodeoffset, name, bd->bi_enetaddr, 6);
+	/* Fix it up if it exists, don't create it if it doesn't exist */
+	if (fdt_get_property(blob, nodeoffset, name, 0)) {
+		return fdt_setprop(blob, nodeoffset, name, bd->bi_enetaddr, 6);
 	}
-	return -FDT_ERR_NOTFOUND;
+	return 0;
 }
 #ifdef CONFIG_HAS_ETH1
 /* second onboard ethernet port */
-static int fdt_set_eth1(void *fdt, int nodeoffset, const char *name, bd_t *bd)
+static int fdt_set_eth1(void *blob, int nodeoffset, const char *name, bd_t *bd)
 {
-	/*
-	 * Fix it up if it exists, don't create it if it doesn't exist.
-	 */
-	if (fdt_get_property(fdt, nodeoffset, name, 0)) {
-		return fdt_setprop(fdt, nodeoffset, name, bd->bi_enet1addr, 6);
+	/* Fix it up if it exists, don't create it if it doesn't exist */
+	if (fdt_get_property(blob, nodeoffset, name, 0)) {
+		return fdt_setprop(blob, nodeoffset, name, bd->bi_enet1addr, 6);
 	}
-	return -FDT_ERR_NOTFOUND;
+	return 0;
 }
 #endif
 #ifdef CONFIG_HAS_ETH2
 /* third onboard ethernet port */
-static int fdt_set_eth2(void *fdt, int nodeoffset, const char *name, bd_t *bd)
+static int fdt_set_eth2(void *blob, int nodeoffset, const char *name, bd_t *bd)
 {
-	/*
-	 * Fix it up if it exists, don't create it if it doesn't exist.
-	 */
-	if (fdt_get_property(fdt, nodeoffset, name, 0)) {
-		return fdt_setprop(fdt, nodeoffset, name, bd->bi_enet2addr, 6);
+	/* Fix it up if it exists, don't create it if it doesn't exist */
+	if (fdt_get_property(blob, nodeoffset, name, 0)) {
+		return fdt_setprop(blob, nodeoffset, name, bd->bi_enet2addr, 6);
 	}
-	return -FDT_ERR_NOTFOUND;
+	return 0;
 }
 #endif
 #ifdef CONFIG_HAS_ETH3
 /* fourth onboard ethernet port */
-static int fdt_set_eth3(void *fdt, int nodeoffset, const char *name, bd_t *bd)
+static int fdt_set_eth3(void *blob, int nodeoffset, const char *name, bd_t *bd)
 {
-	/*
-	 * Fix it up if it exists, don't create it if it doesn't exist.
-	 */
-	if (fdt_get_property(fdt, nodeoffset, name, 0)) {
-		return fdt_setprop(fdt, nodeoffset, name, bd->bi_enet3addr, 6);
+	/* Fix it up if it exists, don't create it if it doesn't exist */
+	if (fdt_get_property(blob, nodeoffset, name, 0)) {
+		return fdt_setprop(blob, nodeoffset, name, bd->bi_enet3addr, 6);
 	}
-	return -FDT_ERR_NOTFOUND;
+	return 0;
 }
 #endif
 
-static int fdt_set_busfreq(void *fdt, int nodeoffset, const char *name, bd_t *bd)
+static int fdt_set_busfreq(void *blob, int nodeoffset, const char *name, bd_t *bd)
 {
 	u32  tmp;
-	/*
-	 * Create or update the property.
-	 */
+	/* Create or update the property */
 	tmp = cpu_to_be32(bd->bi_busfreq);
-	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+	return fdt_setprop(blob, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+static int fdt_set_tbfreq(void *blob, int nodeoffset, const char *name, bd_t *bd)
+{
+	u32  tmp;
+	/* Create or update the property */
+	tmp = cpu_to_be32(OF_TBCLK);
+	return fdt_setprop(blob, nodeoffset, name, &tmp, sizeof(tmp));
 }
 
+
+static int fdt_set_clockfreq(void *blob, int nodeoffset, const char *name, bd_t *bd)
+{
+	u32  tmp;
+	/* Create or update the property */
+	tmp = cpu_to_be32(gd->core_clk);
+	return fdt_setprop(blob, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+#ifdef CONFIG_QE
+static int fdt_set_qe_busfreq(void *blob, int nodeoffset, const char *name, bd_t *bd)
+{
+	u32  tmp;
+	/* Create or update the property */
+	tmp = cpu_to_be32(gd->qe_clk);
+	return fdt_setprop(blob, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+static int fdt_set_qe_brgfreq(void *blob, int nodeoffset, const char *name, bd_t *bd)
+{
+	u32  tmp;
+	/* Create or update the property */
+	tmp = cpu_to_be32(gd->brg_clk);
+	return fdt_setprop(blob, nodeoffset, name, &tmp, sizeof(tmp));
+}
+#endif
+
 /*
- * Fixups to the fdt.  If "create" is TRUE, the node is created
- * unconditionally.  If "create" is FALSE, the node is updated
- * only if it already exists.
+ * Fixups to the fdt.
  */
 static const struct {
 	char *node;
 	char *prop;
-	int (*set_fn)(void *fdt, int nodeoffset, const char *name, bd_t *bd);
+	int (*set_fn)(void *blob, int nodeoffset, const char *name, bd_t *bd);
 } fixup_props[] = {
 	{	"/cpus/" OF_CPU,
-		 "bus-frequency",
+		"timebase-frequency",
+		fdt_set_tbfreq
+	},
+	{	"/cpus/" OF_CPU,
+		"bus-frequency",
 		fdt_set_busfreq
 	},
-	{	"/cpus/" OF_SOC,
+	{	"/cpus/" OF_CPU,
+		"clock-frequency",
+		fdt_set_clockfreq
+	},
+	{	"/" OF_SOC,
 		"bus-frequency",
 		fdt_set_busfreq
 	},
-	{	"/" OF_SOC "/serial@4500/",
+	{	"/" OF_SOC "/serial@4500",
 		"clock-frequency",
 		fdt_set_busfreq
 	},
-	{	"/" OF_SOC "/serial@4600/",
+	{	"/" OF_SOC "/serial@4600",
 		"clock-frequency",
 		fdt_set_busfreq
 	},
 #ifdef CONFIG_TSEC1
-	{	"/" OF_SOC "/ethernet@24000,
+	{	"/" OF_SOC "/ethernet@24000",
 		"mac-address",
 		fdt_set_eth0
 	},
-	{	"/" OF_SOC "/ethernet@24000,
+	{	"/" OF_SOC "/ethernet@24000",
 		"local-mac-address",
 		fdt_set_eth0
 	},
 #endif
 #ifdef CONFIG_TSEC2
-	{	"/" OF_SOC "/ethernet@25000,
+	{	"/" OF_SOC "/ethernet@25000",
 		"mac-address",
 		fdt_set_eth1
 	},
-	{	"/" OF_SOC "/ethernet@25000,
+	{	"/" OF_SOC "/ethernet@25000",
 		"local-mac-address",
 		fdt_set_eth1
 	},
 #endif
+#ifdef CONFIG_QE
+	{	"/" OF_QE,
+		"brg-frequency",
+		fdt_set_qe_brgfreq
+	},
+	{	"/" OF_QE,
+		"bus-frequency",
+		fdt_set_qe_busfreq
+	},
 #ifdef CONFIG_UEC_ETH1
 #if CFG_UEC1_UCC_NUM == 0  /* UCC1 */
-	{	"/" OF_QE "/ucc@2000/mac-address",
+	{	"/" OF_QE "/ucc@2000",
 		"mac-address",
 		fdt_set_eth0
 	},
-	{	"/" OF_QE "/ucc@2000/mac-address",
+	{	"/" OF_QE "/ucc@2000",
 		"local-mac-address",
 		fdt_set_eth0
 	},
 #elif CFG_UEC1_UCC_NUM == 2  /* UCC3 */
-	{	"/" OF_QE "/ucc@2200/mac-address",
+	{	"/" OF_QE "/ucc@2200",
 		"mac-address",
 		fdt_set_eth0
 	},
-	{	"/" OF_QE "/ucc@2200/mac-address",
+	{	"/" OF_QE "/ucc@2200",
 		"local-mac-address",
 		fdt_set_eth0
 	},
 #endif
-#endif
+#endif /* CONFIG_UEC_ETH1 */
 #ifdef CONFIG_UEC_ETH2
 #if CFG_UEC2_UCC_NUM == 1  /* UCC2 */
-	{	"/" OF_QE "/ucc@3000/mac-address",
+	{	"/" OF_QE "/ucc@3000",
 		"mac-address",
 		fdt_set_eth1
 	},
-	{	"/" OF_QE "/ucc@3000/mac-address",
+	{	"/" OF_QE "/ucc@3000",
 		"local-mac-address",
 		fdt_set_eth1
 	},
-#elif CFG_UEC1_UCC_NUM == 3  /* UCC4 */
-	{	"/" OF_QE "/ucc@3200/mac-address",
+#elif CFG_UEC2_UCC_NUM == 3  /* UCC4 */
+	{	"/" OF_QE "/ucc@3200",
 		"mac-address",
 		fdt_set_eth1
 	},
-	{	"/" OF_QE "/ucc@3200/mac-address",
+	{	"/" OF_QE "/ucc@3200",
 		"local-mac-address",
 		fdt_set_eth1
 	},
 #endif
-#endif
+#endif /* CONFIG_UEC_ETH2 */
+#endif /* CONFIG_QE */
 };
 
 void
 ft_cpu_setup(void *blob, bd_t *bd)
 {
-	int  nodeoffset;
-	int  err;
-	int  j;
+	int nodeoffset;
+	int err;
+	int j;
+	int tmp[2];
 
 	for (j = 0; j < (sizeof(fixup_props) / sizeof(fixup_props[0])); j++) {
-		nodeoffset = fdt_path_offset(fdt, fixup_props[j].node);
+		nodeoffset = fdt_find_node_by_path(blob, fixup_props[j].node);
 		if (nodeoffset >= 0) {
-			err = (*fixup_props[j].set_fn)(blob, nodeoffset, fixup_props[j].prop, bd);
+			err = fixup_props[j].set_fn(blob, nodeoffset,
+						    fixup_props[j].prop, bd);
 			if (err < 0)
-				printf("set_fn/libfdt: %s %s returned %s\n",
-					fixup_props[j].node,
-					fixup_props[j].prop,
-					fdt_strerror(err));
+				debug("Problem setting %s = %s: %s\n",
+				      fixup_props[j].node, fixup_props[j].prop,
+				      fdt_strerror(err));
+		} else {
+			debug("Couldn't find %s: %s\n",
+			      fixup_props[j].node, fdt_strerror(nodeoffset));
 		}
 	}
-}
-#endif
 
-#if defined(CONFIG_OF_FLAT_TREE)
+	/* update, or add and update /memory node */
+	nodeoffset = fdt_find_node_by_path(blob, "/memory");
+	if (nodeoffset < 0) {
+		nodeoffset = fdt_add_subnode(blob, 0, "memory");
+		if (nodeoffset < 0)
+			debug("failed to add /memory node: %s\n",
+			      fdt_strerror(nodeoffset));
+	}
+	if (nodeoffset >= 0) {
+		fdt_setprop(blob, nodeoffset, "device_type",
+			    "memory", sizeof("memory"));
+		tmp[0] = cpu_to_be32(bd->bi_memstart);
+		tmp[1] = cpu_to_be32(bd->bi_memsize);
+		fdt_setprop(blob, nodeoffset, "reg", tmp, sizeof(tmp));
+	}
+}
+#elif defined(CONFIG_OF_FLAT_TREE)
 void
 ft_cpu_setup(void *blob, bd_t *bd)
 {
diff --git a/cpu/mpc83xx/cpu_init.c b/cpu/mpc83xx/cpu_init.c
index 3ac91619c4..722497966a 100644
--- a/cpu/mpc83xx/cpu_init.c
+++ b/cpu/mpc83xx/cpu_init.c
@@ -83,20 +83,30 @@ void cpu_init_f (volatile immap_t * im)
 	im->sysconf.spcr = (im->sysconf.spcr & ~SPCR_TSEC2EP) | (CFG_SPCR_TSEC2EP << SPCR_TSEC2EP_SHIFT);
 #endif
 
-#ifdef CONFIG_MPC834X
 #ifdef CFG_SCCR_TSEC1CM
 	/* TSEC1 clock mode */
 	im->clk.sccr = (im->clk.sccr & ~SCCR_TSEC1CM) | (CFG_SCCR_TSEC1CM << SCCR_TSEC1CM_SHIFT);
 #endif
+
 #ifdef CFG_SCCR_TSEC2CM
 	/* TSEC2 & I2C1 clock mode */
 	im->clk.sccr = (im->clk.sccr & ~SCCR_TSEC2CM) | (CFG_SCCR_TSEC2CM << SCCR_TSEC2CM_SHIFT);
 #endif
+
+#ifdef CFG_SCCR_TSEC1ON
+	/* TSEC1 clock switch */
+	im->clk.sccr = (im->clk.sccr & ~SCCR_TSEC1ON) | (CFG_SCCR_TSEC1ON << SCCR_TSEC1ON_SHIFT);
+#endif
+
+#ifdef CFG_SCCR_TSEC2ON
+	/* TSEC2 clock switch */
+	im->clk.sccr = (im->clk.sccr & ~SCCR_TSEC2ON) | (CFG_SCCR_TSEC2ON << SCCR_TSEC2ON_SHIFT);
+#endif
+
 #ifdef CFG_SCCR_USBMPHCM
 	/* USB MPH clock mode */
 	im->clk.sccr = (im->clk.sccr & ~SCCR_USBMPHCM) | (CFG_SCCR_USBMPHCM << SCCR_USBMPHCM_SHIFT);
 #endif
-#endif /* CONFIG_MPC834X */
 
 #ifdef CFG_SCCR_PCICM
 	/* PCI & DMA clock mode */
@@ -247,3 +257,39 @@ int cpu_init_r (void)
 #endif
 	return 0;
 }
+
+/*
+ * Figure out the cause of the reset
+ */
+int prt_83xx_rsr(void)
+{
+	static struct {
+		ulong mask;
+		char *desc;
+	} bits[] = {
+		{
+		RSR_SWSR, "Software Soft"}, {
+		RSR_SWHR, "Software Hard"}, {
+		RSR_JSRS, "JTAG Soft"}, {
+		RSR_CSHR, "Check Stop"}, {
+		RSR_SWRS, "Software Watchdog"}, {
+		RSR_BMRS, "Bus Monitor"}, {
+		RSR_SRS,  "External/Internal Soft"}, {
+		RSR_HRS,  "External/Internal Hard"}
+	};
+	static int n = sizeof bits / sizeof bits[0];
+	ulong rsr = gd->reset_status;
+	int i;
+	char *sep;
+
+	puts("Reset Status:");
+
+	sep = " ";
+	for (i = 0; i < n; i++)
+		if (rsr & bits[i].mask) {
+			printf("%s%s", sep, bits[i].desc);
+			sep = ", ";
+		}
+	puts("\n\n");
+	return 0;
+}
diff --git a/cpu/mpc83xx/ecc.c b/cpu/mpc83xx/ecc.c
new file mode 100644
index 0000000000..6f13094243
--- /dev/null
+++ b/cpu/mpc83xx/ecc.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright (C) 2007 Freescale Semiconductor, Inc.
+ *
+ * Dave Liu <daveliu@freescale.com>
+ * based on the contribution of Marian Balakowicz <m8@semihalf.com>
+ *
+ * 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.
+ */
+
+#include <common.h>
+#include <mpc83xx.h>
+#include <command.h>
+
+#if defined(CONFIG_DDR_ECC) && defined(CONFIG_DDR_ECC_CMD)
+void ecc_print_status(void)
+{
+	volatile immap_t *immap = (immap_t *) CFG_IMMR;
+	volatile ddr83xx_t *ddr = &immap->ddr;
+
+	printf("\nECC mode: %s\n\n",
+	       (ddr->sdram_cfg & SDRAM_CFG_ECC_EN) ? "ON" : "OFF");
+
+	/* Interrupts */
+	printf("Memory Error Interrupt Enable:\n");
+	printf("  Multiple-Bit Error Interrupt Enable: %d\n",
+	       (ddr->err_int_en & ECC_ERR_INT_EN_MBEE) ? 1 : 0);
+	printf("  Single-Bit Error Interrupt Enable: %d\n",
+	       (ddr->err_int_en & ECC_ERR_INT_EN_SBEE) ? 1 : 0);
+	printf("  Memory Select Error Interrupt Enable: %d\n\n",
+	       (ddr->err_int_en & ECC_ERR_INT_EN_MSEE) ? 1 : 0);
+
+	/* Error disable */
+	printf("Memory Error Disable:\n");
+	printf("  Multiple-Bit Error Disable: %d\n",
+	       (ddr->err_disable & ECC_ERROR_DISABLE_MBED) ? 1 : 0);
+	printf("  Sinle-Bit Error Disable: %d\n",
+	       (ddr->err_disable & ECC_ERROR_DISABLE_SBED) ? 1 : 0);
+	printf("  Memory Select Error Disable: %d\n\n",
+	       (ddr->err_disable & ECC_ERROR_DISABLE_MSED) ? 1 : 0);
+
+	/* Error injection */
+	printf("Memory Data Path Error Injection Mask High/Low: %08lx %08lx\n",
+	       ddr->data_err_inject_hi, ddr->data_err_inject_lo);
+
+	printf("Memory Data Path Error Injection Mask ECC:\n");
+	printf("  ECC Mirror Byte: %d\n",
+	       (ddr->ecc_err_inject & ECC_ERR_INJECT_EMB) ? 1 : 0);
+	printf("  ECC Injection Enable: %d\n",
+	       (ddr->ecc_err_inject & ECC_ERR_INJECT_EIEN) ? 1 : 0);
+	printf("  ECC Error Injection Mask: 0x%02x\n\n",
+	       ddr->ecc_err_inject & ECC_ERR_INJECT_EEIM);
+
+	/* SBE counter/threshold */
+	printf("Memory Single-Bit Error Management (0..255):\n");
+	printf("  Single-Bit Error Threshold: %d\n",
+	       (ddr->err_sbe & ECC_ERROR_MAN_SBET) >> ECC_ERROR_MAN_SBET_SHIFT);
+	printf("  Single-Bit Error Counter: %d\n\n",
+	       (ddr->err_sbe & ECC_ERROR_MAN_SBEC) >> ECC_ERROR_MAN_SBEC_SHIFT);
+
+	/* Error detect */
+	printf("Memory Error Detect:\n");
+	printf("  Multiple Memory Errors: %d\n",
+	       (ddr->err_detect & ECC_ERROR_DETECT_MME) ? 1 : 0);
+	printf("  Multiple-Bit Error: %d\n",
+	       (ddr->err_detect & ECC_ERROR_DETECT_MBE) ? 1 : 0);
+	printf("  Single-Bit Error: %d\n",
+	       (ddr->err_detect & ECC_ERROR_DETECT_SBE) ? 1 : 0);
+	printf("  Memory Select Error: %d\n\n",
+	       (ddr->err_detect & ECC_ERROR_DETECT_MSE) ? 1 : 0);
+
+	/* Capture data */
+	printf("Memory Error Address Capture: 0x%08lx\n", ddr->capture_address);
+	printf("Memory Data Path Read Capture High/Low: %08lx %08lx\n",
+	       ddr->capture_data_hi, ddr->capture_data_lo);
+	printf("Memory Data Path Read Capture ECC: 0x%02x\n\n",
+	       ddr->capture_ecc & CAPTURE_ECC_ECE);
+
+	printf("Memory Error Attributes Capture:\n");
+	printf(" Data Beat Number: %d\n",
+	       (ddr->capture_attributes & ECC_CAPT_ATTR_BNUM) >>
+	       ECC_CAPT_ATTR_BNUM_SHIFT);
+	printf("  Transaction Size: %d\n",
+	       (ddr->capture_attributes & ECC_CAPT_ATTR_TSIZ) >>
+	       ECC_CAPT_ATTR_TSIZ_SHIFT);
+	printf("  Transaction Source: %d\n",
+	       (ddr->capture_attributes & ECC_CAPT_ATTR_TSRC) >>
+	       ECC_CAPT_ATTR_TSRC_SHIFT);
+	printf("  Transaction Type: %d\n",
+	       (ddr->capture_attributes & ECC_CAPT_ATTR_TTYP) >>
+	       ECC_CAPT_ATTR_TTYP_SHIFT);
+	printf("  Error Information Valid: %d\n\n",
+	       ddr->capture_attributes & ECC_CAPT_ATTR_VLD);
+}
+
+int do_ecc(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	volatile immap_t *immap = (immap_t *) CFG_IMMR;
+	volatile ddr83xx_t *ddr = &immap->ddr;
+	volatile u32 val;
+	u64 *addr;
+	u32 count;
+	register u64 *i;
+	u32 ret[2];
+	u32 pattern[2];
+	u32 writeback[2];
+
+	/* The pattern is written into memory to generate error */
+	pattern[0] = 0xfedcba98UL;
+	pattern[1] = 0x76543210UL;
+
+	/* After injecting error, re-initialize the memory with the value */
+	writeback[0] = 0x01234567UL;
+	writeback[1] = 0x89abcdefUL;
+
+	if (argc > 4) {
+		printf("Usage:\n%s\n", cmdtp->usage);
+		return 1;
+	}
+
+	if (argc == 2) {
+		if (strcmp(argv[1], "status") == 0) {
+			ecc_print_status();
+			return 0;
+		} else if (strcmp(argv[1], "captureclear") == 0) {
+			ddr->capture_address = 0;
+			ddr->capture_data_hi = 0;
+			ddr->capture_data_lo = 0;
+			ddr->capture_ecc = 0;
+			ddr->capture_attributes = 0;
+			return 0;
+		}
+	}
+	if (argc == 3) {
+		if (strcmp(argv[1], "sbecnt") == 0) {
+			val = simple_strtoul(argv[2], NULL, 10);
+			if (val > 255) {
+				printf("Incorrect Counter value, "
+				       "should be 0..255\n");
+				return 1;
+			}
+
+			val = (val << ECC_ERROR_MAN_SBEC_SHIFT);
+			val |= (ddr->err_sbe & ECC_ERROR_MAN_SBET);
+
+			ddr->err_sbe = val;
+			return 0;
+		} else if (strcmp(argv[1], "sbethr") == 0) {
+			val = simple_strtoul(argv[2], NULL, 10);
+			if (val > 255) {
+				printf("Incorrect Counter value, "
+				       "should be 0..255\n");
+				return 1;
+			}
+
+			val = (val << ECC_ERROR_MAN_SBET_SHIFT);
+			val |= (ddr->err_sbe & ECC_ERROR_MAN_SBEC);
+
+			ddr->err_sbe = val;
+			return 0;
+		} else if (strcmp(argv[1], "errdisable") == 0) {
+			val = ddr->err_disable;
+
+			if (strcmp(argv[2], "+sbe") == 0) {
+				val |= ECC_ERROR_DISABLE_SBED;
+			} else if (strcmp(argv[2], "+mbe") == 0) {
+				val |= ECC_ERROR_DISABLE_MBED;
+			} else if (strcmp(argv[2], "+mse") == 0) {
+				val |= ECC_ERROR_DISABLE_MSED;
+			} else if (strcmp(argv[2], "+all") == 0) {
+				val |= (ECC_ERROR_DISABLE_SBED |
+					ECC_ERROR_DISABLE_MBED |
+					ECC_ERROR_DISABLE_MSED);
+			} else if (strcmp(argv[2], "-sbe") == 0) {
+				val &= ~ECC_ERROR_DISABLE_SBED;
+			} else if (strcmp(argv[2], "-mbe") == 0) {
+				val &= ~ECC_ERROR_DISABLE_MBED;
+			} else if (strcmp(argv[2], "-mse") == 0) {
+				val &= ~ECC_ERROR_DISABLE_MSED;
+			} else if (strcmp(argv[2], "-all") == 0) {
+				val &= ~(ECC_ERROR_DISABLE_SBED |
+					 ECC_ERROR_DISABLE_MBED |
+					 ECC_ERROR_DISABLE_MSED);
+			} else {
+				printf("Incorrect err_disable field\n");
+				return 1;
+			}
+
+			ddr->err_disable = val;
+			__asm__ __volatile__("sync");
+			__asm__ __volatile__("isync");
+			return 0;
+		} else if (strcmp(argv[1], "errdetectclr") == 0) {
+			val = ddr->err_detect;
+
+			if (strcmp(argv[2], "mme") == 0) {
+				val |= ECC_ERROR_DETECT_MME;
+			} else if (strcmp(argv[2], "sbe") == 0) {
+				val |= ECC_ERROR_DETECT_SBE;
+			} else if (strcmp(argv[2], "mbe") == 0) {
+				val |= ECC_ERROR_DETECT_MBE;
+			} else if (strcmp(argv[2], "mse") == 0) {
+				val |= ECC_ERROR_DETECT_MSE;
+			} else if (strcmp(argv[2], "all") == 0) {
+				val |= (ECC_ERROR_DETECT_MME |
+					ECC_ERROR_DETECT_MBE |
+					ECC_ERROR_DETECT_SBE |
+					ECC_ERROR_DETECT_MSE);
+			} else {
+				printf("Incorrect err_detect field\n");
+				return 1;
+			}
+
+			ddr->err_detect = val;
+			return 0;
+		} else if (strcmp(argv[1], "injectdatahi") == 0) {
+			val = simple_strtoul(argv[2], NULL, 16);
+
+			ddr->data_err_inject_hi = val;
+			return 0;
+		} else if (strcmp(argv[1], "injectdatalo") == 0) {
+			val = simple_strtoul(argv[2], NULL, 16);
+
+			ddr->data_err_inject_lo = val;
+			return 0;
+		} else if (strcmp(argv[1], "injectecc") == 0) {
+			val = simple_strtoul(argv[2], NULL, 16);
+			if (val > 0xff) {
+				printf("Incorrect ECC inject mask, "
+				       "should be 0x00..0xff\n");
+				return 1;
+			}
+			val |= (ddr->ecc_err_inject & ~ECC_ERR_INJECT_EEIM);
+
+			ddr->ecc_err_inject = val;
+			return 0;
+		} else if (strcmp(argv[1], "inject") == 0) {
+			val = ddr->ecc_err_inject;
+
+			if (strcmp(argv[2], "en") == 0)
+				val |= ECC_ERR_INJECT_EIEN;
+			else if (strcmp(argv[2], "dis") == 0)
+				val &= ~ECC_ERR_INJECT_EIEN;
+			else
+				printf("Incorrect command\n");
+
+			ddr->ecc_err_inject = val;
+			__asm__ __volatile__("sync");
+			__asm__ __volatile__("isync");
+			return 0;
+		} else if (strcmp(argv[1], "mirror") == 0) {
+			val = ddr->ecc_err_inject;
+
+			if (strcmp(argv[2], "en") == 0)
+				val |= ECC_ERR_INJECT_EMB;
+			else if (strcmp(argv[2], "dis") == 0)
+				val &= ~ECC_ERR_INJECT_EMB;
+			else
+				printf("Incorrect command\n");
+
+			ddr->ecc_err_inject = val;
+			return 0;
+		}
+	}
+	if (argc == 4) {
+		if (strcmp(argv[1], "testdw") == 0) {
+			addr = (u64 *) simple_strtoul(argv[2], NULL, 16);
+			count = simple_strtoul(argv[3], NULL, 16);
+
+			if ((u32) addr % 8) {
+				printf("Address not alligned on "
+				       "double word boundary\n");
+				return 1;
+			}
+			disable_interrupts();
+
+			for (i = addr; i < addr + count; i++) {
+
+				/* enable injects */
+				ddr->ecc_err_inject |= ECC_ERR_INJECT_EIEN;
+				__asm__ __volatile__("sync");
+				__asm__ __volatile__("isync");
+
+				/* write memory location injecting errors */
+				ppcDWstore((u32 *) i, pattern);
+				__asm__ __volatile__("sync");
+
+				/* disable injects */
+				ddr->ecc_err_inject &= ~ECC_ERR_INJECT_EIEN;
+				__asm__ __volatile__("sync");
+				__asm__ __volatile__("isync");
+
+				/* read data, this generates ECC error */
+				ppcDWload((u32 *) i, ret);
+				__asm__ __volatile__("sync");
+
+				/* re-initialize memory, double word write the location again,
+				 * generates new ECC code this time */
+				ppcDWstore((u32 *) i, writeback);
+				__asm__ __volatile__("sync");
+			}
+			enable_interrupts();
+			return 0;
+		}
+		if (strcmp(argv[1], "testword") == 0) {
+			addr = (u64 *) simple_strtoul(argv[2], NULL, 16);
+			count = simple_strtoul(argv[3], NULL, 16);
+
+			if ((u32) addr % 8) {
+				printf("Address not alligned on "
+				       "double word boundary\n");
+				return 1;
+			}
+			disable_interrupts();
+
+			for (i = addr; i < addr + count; i++) {
+
+				/* enable injects */
+				ddr->ecc_err_inject |= ECC_ERR_INJECT_EIEN;
+				__asm__ __volatile__("sync");
+				__asm__ __volatile__("isync");
+
+				/* write memory location injecting errors */
+				*(u32 *) i = 0xfedcba98UL;
+				__asm__ __volatile__("sync");
+
+				/* sub double word write,
+				 * bus will read-modify-write,
+				 * generates ECC error */
+				*((u32 *) i + 1) = 0x76543210UL;
+				__asm__ __volatile__("sync");
+
+				/* disable injects */
+				ddr->ecc_err_inject &= ~ECC_ERR_INJECT_EIEN;
+				__asm__ __volatile__("sync");
+				__asm__ __volatile__("isync");
+
+				/* re-initialize memory,
+				 * double word write the location again,
+				 * generates new ECC code this time */
+				ppcDWstore((u32 *) i, writeback);
+				__asm__ __volatile__("sync");
+			}
+			enable_interrupts();
+			return 0;
+		}
+	}
+	printf("Usage:\n%s\n", cmdtp->usage);
+	return 1;
+}
+
+U_BOOT_CMD(ecc, 4, 0, do_ecc,
+	   "ecc     - support for DDR ECC features\n",
+	   "status              - print out status info\n"
+	   "ecc captureclear        - clear capture regs data\n"
+	   "ecc sbecnt <val>        - set Single-Bit Error counter\n"
+	   "ecc sbethr <val>        - set Single-Bit Threshold\n"
+	   "ecc errdisable <flag>   - clear/set disable Memory Error Disable, flag:\n"
+	   "  [-|+]sbe - Single-Bit Error\n"
+	   "  [-|+]mbe - Multiple-Bit Error\n"
+	   "  [-|+]mse - Memory Select Error\n"
+	   "  [-|+]all - all errors\n"
+	   "ecc errdetectclr <flag> - clear Memory Error Detect, flag:\n"
+	   "  mme - Multiple Memory Errors\n"
+	   "  sbe - Single-Bit Error\n"
+	   "  mbe - Multiple-Bit Error\n"
+	   "  mse - Memory Select Error\n"
+	   "  all - all errors\n"
+	   "ecc injectdatahi <hi>  - set Memory Data Path Error Injection Mask High\n"
+	   "ecc injectdatalo <lo>  - set Memory Data Path Error Injection Mask Low\n"
+	   "ecc injectecc <ecc>    - set ECC Error Injection Mask\n"
+	   "ecc inject <en|dis>    - enable/disable error injection\n"
+	   "ecc mirror <en|dis>    - enable/disable mirror byte\n"
+	   "ecc testdw <addr> <cnt>  - test mem region with double word access:\n"
+	   "  - enables injects\n"
+	   "  - writes pattern injecting errors with double word access\n"
+	   "  - disables injects\n"
+	   "  - reads pattern back with double word access, generates error\n"
+	   "  - re-inits memory\n"
+	   "ecc testword <addr> <cnt>  - test mem region with word access:\n"
+	   "  - enables injects\n"
+	   "  - writes pattern injecting errors with word access\n"
+	   "  - writes pattern with word access, generates error\n"
+	   "  - disables injects\n" "  - re-inits memory");
+#endif
diff --git a/cpu/mpc83xx/pci.c b/cpu/mpc83xx/pci.c
index 785d6129da..5675afe971 100644
--- a/cpu/mpc83xx/pci.c
+++ b/cpu/mpc83xx/pci.c
@@ -25,7 +25,13 @@
 
 #include <common.h>
 #include <pci.h>
+
+#if defined(CONFIG_OF_LIBFDT)
+#include <libfdt.h>
+#elif defined(CONFIG_OF_FLAT_TREE)
 #include <ft_build.h>
+#endif
+
 #include <asm/mpc8349_pci.h>
 
 #ifdef CONFIG_83XX_GENERIC_PCI
@@ -163,7 +169,44 @@ void mpc83xx_pci_init(int num_buses, struct pci_region **reg, int warmboot)
 		pci_init_bus(i, reg[i]);
 }
 
-#ifdef CONFIG_OF_FLAT_TREE
+#if defined(CONFIG_OF_LIBFDT)
+void ft_pci_setup(void *blob, bd_t *bd)
+{
+	int nodeoffset;
+	int err;
+	int tmp[2];
+
+	if (pci_num_buses < 1)
+		return;
+
+	nodeoffset = fdt_find_node_by_path(blob, "/" OF_SOC "/pci@8500");
+	if (nodeoffset >= 0) {
+		tmp[0] = cpu_to_be32(pci_hose[0].first_busno);
+		tmp[1] = cpu_to_be32(pci_hose[0].last_busno);
+		err = fdt_setprop(blob, nodeoffset, "bus-range",
+				  tmp, sizeof(tmp));
+
+		tmp[0] = cpu_to_be32(gd->pci_clk);
+		err = fdt_setprop(blob, nodeoffset, "clock-frequency",
+				  tmp, sizeof(tmp[0]));
+	}
+
+	if (pci_num_buses < 2)
+		return;
+
+	nodeoffset = fdt_find_node_by_path(blob, "/" OF_SOC "/pci@8600");
+	if (nodeoffset >= 0) {
+		tmp[0] = cpu_to_be32(pci_hose[0].first_busno);
+		tmp[1] = cpu_to_be32(pci_hose[0].last_busno);
+		err = fdt_setprop(blob, nodeoffset, "bus-range",
+				  tmp, sizeof(tmp));
+
+		tmp[0] = cpu_to_be32(gd->pci_clk);
+		err = fdt_setprop(blob, nodeoffset, "clock-frequency",
+				  tmp, sizeof(tmp[0]));
+	}
+}
+#elif CONFIG_OF_FLAT_TREE
 void ft_pci_setup(void *blob, bd_t *bd)
 {
 	u32 *p;
diff --git a/cpu/mpc83xx/spd_sdram.c b/cpu/mpc83xx/spd_sdram.c
index 647813f68d..ee2d0385e4 100644
--- a/cpu/mpc83xx/spd_sdram.c
+++ b/cpu/mpc83xx/spd_sdram.c
@@ -34,6 +34,30 @@
 #include <asm/mmu.h>
 #include <spd_sdram.h>
 
+void board_add_ram_info(int use_default)
+{
+	volatile immap_t *immap = (immap_t *) CFG_IMMR;
+	volatile ddr83xx_t *ddr = &immap->ddr;
+
+	printf(" (DDR%d", ((ddr->sdram_cfg & SDRAM_CFG_SDRAM_TYPE_MASK)
+			   >> SDRAM_CFG_SDRAM_TYPE_SHIFT) - 1);
+
+	if (ddr->sdram_cfg & SDRAM_CFG_32_BE)
+		puts(", 32-bit");
+	else
+		puts(", 64-bit");
+
+	if (ddr->sdram_cfg & SDRAM_CFG_ECC_EN)
+		puts(", ECC on)");
+	else
+		puts(", ECC off)");
+
+#if defined(CFG_LB_SDRAM) && defined(CFG_LBC_SDRAM_SIZE)
+	puts("\nSDRAM: ");
+	print_size (CFG_LBC_SDRAM_SIZE * 1024 * 1024, " (local bus)");
+#endif
+}
+
 #ifdef CONFIG_SPD_EEPROM
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -109,7 +133,7 @@ long int spd_sdram()
 	unsigned int n_ranks;
 	unsigned int odt_rd_cfg, odt_wr_cfg;
 	unsigned char twr_clk, twtr_clk;
-	unsigned char sdram_type;
+	unsigned int sdram_type;
 	unsigned int memsize;
 	unsigned int law_size;
 	unsigned char caslat, caslat_ctrl;
@@ -137,7 +161,7 @@ long int spd_sdram()
 #endif
 	/* Check the memory type */
 	if (spd.mem_type != SPD_MEMTYPE_DDR && spd.mem_type != SPD_MEMTYPE_DDR2) {
-		printf("DDR: Module mem type is %02X\n", spd.mem_type);
+		debug("DDR: Module mem type is %02X\n", spd.mem_type);
 		return 0;
 	}
 
@@ -574,18 +598,21 @@ long int spd_sdram()
 
 	/* Check DIMM data bus width */
 	if (spd.dataw_lsb == 0x20) {
-		burstlen = 0x03; /* 32 bit data bus, burst len is 8 */
-		printf("\n   DDR DIMM: data bus width is 32 bit");
+		if (spd.mem_type == SPD_MEMTYPE_DDR)
+			burstlen = 0x03; /* 32 bit data bus, burst len is 8 */
+		else
+			burstlen = 0x02; /* 32 bit data bus, burst len is 4 */
+		debug("\n   DDR DIMM: data bus width is 32 bit");
 	} else {
 		burstlen = 0x02; /* Others act as 64 bit bus, burst len is 4 */
-		printf("\n   DDR DIMM: data bus width is 64 bit");
+		debug("\n   DDR DIMM: data bus width is 64 bit");
 	}
 
 	/* Is this an ECC DDR chip? */
 	if (spd.config == 0x02)
-		printf(" with ECC\n");
+		debug(" with ECC\n");
 	else
-		printf(" without ECC\n");
+		debug(" without ECC\n");
 
 	/* Burst length is always 4 for 64 bit data bus, 8 for 32 bit data bus,
 	   Burst type is sequential
@@ -715,23 +742,27 @@ long int spd_sdram()
 	 * sdram_cfg[13] = 0 (8_BE =0, 4-beat bursts)
 	 */
 	if (spd.mem_type == SPD_MEMTYPE_DDR)
-		sdram_type = 2;
+		sdram_type = SDRAM_CFG_SDRAM_TYPE_DDR1;
 	else
-		sdram_type = 3;
+		sdram_type = SDRAM_CFG_SDRAM_TYPE_DDR2;
 
 	sdram_cfg = (0
-		     | (1 << 31)			/* DDR enable */
-		     | (1 << 30)			/* Self refresh */
-		     | (sdram_type << 24)		/* SDRAM type */
+		     | SDRAM_CFG_MEM_EN		/* DDR enable */
+		     | SDRAM_CFG_SREN		/* Self refresh */
+		     | sdram_type		/* SDRAM type */
 		     );
 
 	/* sdram_cfg[3] = RD_EN - registered DIMM enable */
 	if (spd.mod_attr & 0x02)
-		sdram_cfg |= 0x10000000;
+		sdram_cfg |= SDRAM_CFG_RD_EN;
 
 	/* The DIMM is 32bit width */
-	if (spd.dataw_lsb == 0x20)
-		sdram_cfg |= 0x000C0000;
+	if (spd.dataw_lsb == 0x20) {
+		if (spd.mem_type == SPD_MEMTYPE_DDR)
+			sdram_cfg |= SDRAM_CFG_32_BE | SDRAM_CFG_8_BE;
+		if (spd.mem_type == SPD_MEMTYPE_DDR2)
+			sdram_cfg |= SDRAM_CFG_32_BE;
+	}
 
 	ddrc_ecc_enable = 0;
 
@@ -751,7 +782,7 @@ long int spd_sdram()
 	debug("DDR:err_disable=0x%08x\n", ddr->err_disable);
 	debug("DDR:err_sbe=0x%08x\n", ddr->err_sbe);
 #endif
-	printf("   DDRC ECC mode: %s\n", ddrc_ecc_enable ? "ON":"OFF");
+	debug("   DDRC ECC mode: %s\n", ddrc_ecc_enable ? "ON":"OFF");
 
 #if defined(CONFIG_DDR_2T_TIMING)
 	/*
diff --git a/cpu/mpc83xx/speed.c b/cpu/mpc83xx/speed.c
index bf30616548..cba57fadb9 100644
--- a/cpu/mpc83xx/speed.c
+++ b/cpu/mpc83xx/speed.c
@@ -351,6 +351,7 @@ int get_clocks(void)
 	gd->qe_clk = qe_clk;
 	gd->brg_clk = brg_clk;
 #endif
+	gd->pci_clk = pci_sync_in;
 	gd->cpu_clk = gd->core_clk;
 	gd->bus_clk = gd->csb_clk;
 	return 0;
diff --git a/cpu/mpc85xx/Makefile b/cpu/mpc85xx/Makefile
index ff67dcdd35..32091fa4e1 100644
--- a/cpu/mpc85xx/Makefile
+++ b/cpu/mpc85xx/Makefile
@@ -30,7 +30,7 @@ LIB	= $(obj)lib$(CPU).a
 
 START	= start.o resetvec.o
 COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o \
-	  pci.o serial_scc.o commproc.o ether_fcc.o spd_sdram.o
+	  pci.o serial_scc.o commproc.o ether_fcc.o spd_sdram.o qe_io.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/mpc85xx/cpu.c b/cpu/mpc85xx/cpu.c
index 1d791c9b9b..08e04685f5 100644
--- a/cpu/mpc85xx/cpu.c
+++ b/cpu/mpc85xx/cpu.c
@@ -280,7 +280,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	if (p != NULL)
 		*p = cpu_to_be32(clock);
 
-#if defined(CONFIG_TSEC1)
+#if defined(CONFIG_HAS_ETH0)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@24000/mac-address", &len);
 	if (p)
 		memcpy(p, bd->bi_enetaddr, 6);
@@ -308,6 +308,17 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@26000/local-mac-address", &len);
 	if (p)
 		memcpy(p, bd->bi_enet2addr, 6);
+
+#ifdef CONFIG_UEC_ETH
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2000/mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet2addr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2000/local-mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet2addr, 6);
+
+#endif
 #endif
 
 #if defined(CONFIG_HAS_ETH3)
@@ -318,6 +329,17 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@27000/local-mac-address", &len);
 	if (p)
 		memcpy(p, bd->bi_enet3addr, 6);
+
+#ifdef CONFIG_UEC_ETH
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3000/mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet3addr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3000/local-mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet3addr, 6);
+
+#endif
 #endif
 
 }
diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 9517146ed2..79ad20c91a 100644
--- a/cpu/mpc85xx/cpu_init.c
+++ b/cpu/mpc85xx/cpu_init.c
@@ -1,4 +1,6 @@
 /*
+ * Copyright 2007 Freescale Semiconductor.
+ *
  * (C) Copyright 2003 Motorola Inc.
  * Modified by Xianghua Xiao, X.Xiao@motorola.com
  *
@@ -32,6 +34,29 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+#ifdef CONFIG_QE
+extern qe_iop_conf_t qe_iop_conf_tab[];
+extern void qe_config_iopin(u8 port, u8 pin, int dir,
+				int open_drain, int assign);
+extern void qe_init(uint qe_base);
+extern void qe_reset(void);
+
+static void config_qe_ioports(void)
+{
+	u8      port, pin;
+	int     dir, open_drain, assign;
+	int     i;
+
+	for (i = 0; qe_iop_conf_tab[i].assign != QE_IOP_TAB_END; i++) {
+		port		= qe_iop_conf_tab[i].port;
+		pin		= qe_iop_conf_tab[i].pin;
+		dir		= qe_iop_conf_tab[i].dir;
+		open_drain	= qe_iop_conf_tab[i].open_drain;
+		assign		= qe_iop_conf_tab[i].assign;
+		qe_config_iopin(port, pin, dir, open_drain, assign);
+	}
+}
+#endif
 
 #ifdef CONFIG_CPM2
 static void config_8560_ioports (volatile immap_t * immr)
@@ -133,15 +158,18 @@ void cpu_init_f (void)
 #endif
 
 	/* now restrict to preliminary range */
+	/* if cs1 is already set via debugger, leave cs0/cs1 alone */
+	if (! memctl->br1 & 1) {
 #if defined(CFG_BR0_PRELIM) && defined(CFG_OR0_PRELIM)
-	memctl->br0 = CFG_BR0_PRELIM;
-	memctl->or0 = CFG_OR0_PRELIM;
+		memctl->br0 = CFG_BR0_PRELIM;
+		memctl->or0 = CFG_OR0_PRELIM;
 #endif
 
 #if defined(CFG_BR1_PRELIM) && defined(CFG_OR1_PRELIM)
-	memctl->or1 = CFG_OR1_PRELIM;
-	memctl->br1 = CFG_BR1_PRELIM;
+		memctl->or1 = CFG_OR1_PRELIM;
+		memctl->br1 = CFG_BR1_PRELIM;
 #endif
+	}
 
 #if defined(CFG_BR2_PRELIM) && defined(CFG_OR2_PRELIM)
 	memctl->or2 = CFG_OR2_PRELIM;
@@ -176,6 +204,11 @@ void cpu_init_f (void)
 #if defined(CONFIG_CPM2)
 	m8560_cpm_reset();
 #endif
+#ifdef CONFIG_QE
+	/* Config QE ioports */
+	config_qe_ioports();
+#endif
+
 }
 
 
@@ -185,16 +218,25 @@ void cpu_init_f (void)
  * The newer 8548, etc, parts have twice as much cache, but
  * use the same bit-encoding as the older 8555, etc, parts.
  *
- * FIXME: Use PVR_VER(pvr) == 1 test here instead of SVR_VER()?
  */
 
 int cpu_init_r(void)
 {
+#if defined(CONFIG_CLEAR_LAW0) || defined(CONFIG_L2_CACHE)
+	volatile immap_t    *immap = (immap_t *)CFG_IMMR;
+#endif
+#ifdef CONFIG_CLEAR_LAW0
+	volatile ccsr_local_ecm_t *ecm = &immap->im_local_ecm;
+
+	/* clear alternate boot location LAW (used for sdram, or ddr bank) */
+	ecm->lawar0 = 0;
+#endif
+
 #if defined(CONFIG_L2_CACHE)
-	volatile immap_t *immap = (immap_t *)CFG_IMMR;
 	volatile ccsr_l2cache_t *l2cache = &immap->im_l2cache;
 	volatile uint cache_ctl;
 	uint svr, ver;
+	uint l2srbar;
 
 	svr = get_svr();
 	ver = SVR_VER(svr);
@@ -204,33 +246,55 @@ int cpu_init_r(void)
 
 	switch (cache_ctl & 0x30000000) {
 	case 0x20000000:
-		if (ver == SVR_8548 || ver == SVR_8548_E) {
+		if (ver == SVR_8548 || ver == SVR_8548_E ||
+		    ver == SVR_8544 || ver == SVR_8568_E) {
 			printf ("L2 cache 512KB:");
+			/* set L2E=1, L2I=1, & L2SRAM=0 */
+			cache_ctl = 0xc0000000;
 		} else {
 			printf ("L2 cache 256KB:");
+			/* set L2E=1, L2I=1, & L2BLKSZ=2 (256 Kbyte) */
+			cache_ctl = 0xc8000000;
 		}
 		break;
-	case 0x00000000:
 	case 0x10000000:
+		printf ("L2 cache 256KB:");
+		if (ver == SVR_8544 || ver == SVR_8544_E) {
+			cache_ctl = 0xc0000000; /* set L2E=1, L2I=1, & L2SRAM=0 */
+		}
+		break;
 	case 0x30000000:
+	case 0x00000000:
 	default:
 		printf ("L2 cache unknown size (0x%08x)\n", cache_ctl);
 		return -1;
 	}
 
-	asm("msync;isync");
-	l2cache->l2ctl = 0x68000000; /* invalidate */
-	cache_ctl = l2cache->l2ctl;
-	asm("msync;isync");
-
-	l2cache->l2ctl = 0xa8000000; /* enable 256KB L2 cache */
-	cache_ctl = l2cache->l2ctl;
-	asm("msync;isync");
-
-	printf(" enabled\n");
+	if (l2cache->l2ctl & 0x80000000) {
+		printf(" already enabled.");
+		l2srbar = l2cache->l2srbar0;
+#ifdef CFG_INIT_L2_ADDR
+		if (l2cache->l2ctl & 0x00010000 && l2srbar >= CFG_FLASH_BASE) {
+			l2srbar = CFG_INIT_L2_ADDR;
+			l2cache->l2srbar0 = l2srbar;
+			printf("  Moving to 0x%08x", CFG_INIT_L2_ADDR);
+		}
+#endif /* CFG_INIT_L2_ADDR */
+		puts("\n");
+	} else {
+		asm("msync;isync");
+		l2cache->l2ctl = cache_ctl; /* invalidate & enable */
+		asm("msync;isync");
+		printf(" enabled\n");
+	}
 #else
 	printf("L2 cache: disabled\n");
 #endif
+#ifdef CONFIG_QE
+	uint qe_base = CFG_IMMR + 0x00080000; /* QE immr base */
+	qe_init(qe_base);
+	qe_reset();
+#endif
 
 	return 0;
 }
diff --git a/cpu/mpc85xx/interrupts.c b/cpu/mpc85xx/interrupts.c
index dc246dca02..bf737d6228 100644
--- a/cpu/mpc85xx/interrupts.c
+++ b/cpu/mpc85xx/interrupts.c
@@ -89,6 +89,39 @@ int interrupt_init (void)
 	mtspr(SPRN_TCR, TCR_PIE);
 	set_dec (decrementer_count);
 	set_msr (get_msr () | MSR_EE);
+
+#ifdef CONFIG_INTERRUPTS
+	volatile ccsr_pic_t *pic = &immr->im_pic;
+
+	pic->iivpr1 = 0x810002;	/* 50220 enable ecm interrupts */
+	debug("iivpr1@%x = %x\n",&pic->iivpr1, pic->iivpr1);
+
+	pic->iivpr2 = 0x810002;	/* 50240 enable ddr interrupts */
+	debug("iivpr2@%x = %x\n",&pic->iivpr2, pic->iivpr2);
+
+	pic->iivpr3 = 0x810003;	/* 50260 enable lbc interrupts */
+	debug("iivpr3@%x = %x\n",&pic->iivpr3, pic->iivpr3);
+
+#ifdef CONFIG_PCI1
+	pic->iivpr8 = 0x810008;	/* enable pci1 interrupts */
+	debug("iivpr8@%x = %x\n",&pic->iivpr8, pic->iivpr8);
+#endif
+#if defined(CONFIG_PCI2) || defined(CONFIG_PCIE2)
+	pic->iivpr9 = 0x810009;	/* enable pci1 interrupts */
+	debug("iivpr9@%x = %x\n",&pic->iivpr9, pic->iivpr9);
+#endif
+#ifdef CONFIG_PCIE1
+	pic->iivpr10 = 0x81000a;	/* enable pcie1 interrupts */
+	debug("iivpr10@%x = %x\n",&pic->iivpr10, pic->iivpr10);
+#endif
+#ifdef CONFIG_PCIE3
+	pic->iivpr11 = 0x81000b;	/* enable pcie3 interrupts */
+	debug("iivpr11@%x = %x\n",&pic->iivpr11, pic->iivpr11);
+#endif
+
+	pic->ctpr=0;		/* 40080 clear current task priority register */
+#endif
+
 	return (0);
 }
 
diff --git a/cpu/mpc85xx/pci.c b/cpu/mpc85xx/pci.c
index 3c1a323aad..db09e45fbc 100644
--- a/cpu/mpc85xx/pci.c
+++ b/cpu/mpc85xx/pci.c
@@ -142,7 +142,7 @@ pci_mpc85xx_init(struct pci_controller *board_hose)
 		u8 header_type;
 
 		pci_hose_read_config_byte(hose,
-					  PCI_BDF(0,17,0),
+					  PCI_BDF(0,BRIDGE_ID,0),
 					  PCI_HEADER_TYPE,
 					  &header_type);
 	}
diff --git a/cpu/mpc85xx/qe_io.c b/cpu/mpc85xx/qe_io.c
new file mode 100644
index 0000000000..8878bc5319
--- /dev/null
+++ b/cpu/mpc85xx/qe_io.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2006 Freescale Semiconductor, Inc.
+ *
+ * Dave Liu <daveliu@freescale.com>
+ * based on source code of Shlomi Gridish
+ *
+ * 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"
+#include "asm/errno.h"
+#include "asm/io.h"
+#include "asm/immap_85xx.h"
+
+#if defined(CONFIG_QE)
+#define	NUM_OF_PINS	32
+void qe_config_iopin(u8 port, u8 pin, int dir, int open_drain, int assign)
+{
+	u32			pin_2bit_mask;
+	u32			pin_2bit_dir;
+	u32			pin_2bit_assign;
+	u32			pin_1bit_mask;
+	u32			tmp_val;
+	volatile immap_t	*im = (volatile immap_t *)CFG_IMMR;
+	volatile par_io_t	*par_io = (volatile par_io_t *)
+						&(im->im_gur.qe_par_io);
+
+	/* Caculate pin location and 2bit mask and dir */
+	pin_2bit_mask = (u32)(0x3 << (NUM_OF_PINS-(pin%(NUM_OF_PINS/2)+1)*2));
+	pin_2bit_dir = (u32)(dir << (NUM_OF_PINS-(pin%(NUM_OF_PINS/2)+1)*2));
+
+	/* Setup the direction */
+	tmp_val = (pin > (NUM_OF_PINS/2) - 1) ? \
+		in_be32(&par_io[port].cpdir2) :
+		in_be32(&par_io[port].cpdir1);
+
+	if (pin > (NUM_OF_PINS/2) -1) {
+		out_be32(&par_io[port].cpdir2, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cpdir2, pin_2bit_dir | tmp_val);
+	} else {
+		out_be32(&par_io[port].cpdir1, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cpdir1, pin_2bit_dir | tmp_val);
+	}
+
+	/* Calculate pin location for 1bit mask */
+	pin_1bit_mask = (u32)(1 << (NUM_OF_PINS - (pin+1)));
+
+	/* Setup the open drain */
+	tmp_val = in_be32(&par_io[port].cpodr);
+	if (open_drain)
+		out_be32(&par_io[port].cpodr, pin_1bit_mask | tmp_val);
+	else
+		out_be32(&par_io[port].cpodr, ~pin_1bit_mask & tmp_val);
+
+	/* Setup the assignment */
+	tmp_val = (pin > (NUM_OF_PINS/2) - 1) ?
+		in_be32(&par_io[port].cppar2):
+		in_be32(&par_io[port].cppar1);
+	pin_2bit_assign = (u32)(assign
+				<< (NUM_OF_PINS - (pin%(NUM_OF_PINS/2)+1)*2));
+
+	/* Clear and set 2 bits mask */
+	if (pin > (NUM_OF_PINS/2) - 1) {
+		out_be32(&par_io[port].cppar2, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cppar2, pin_2bit_assign | tmp_val);
+	} else {
+		out_be32(&par_io[port].cppar1, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cppar1, pin_2bit_assign | tmp_val);
+	}
+}
+
+#endif /* CONFIG_QE */
diff --git a/cpu/mpc85xx/spd_sdram.c b/cpu/mpc85xx/spd_sdram.c
index 3777f49adc..5dc223a53e 100644
--- a/cpu/mpc85xx/spd_sdram.c
+++ b/cpu/mpc85xx/spd_sdram.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004 Freescale Semiconductor.
+ * Copyright 2004, 2007 Freescale Semiconductor.
  * (C) Copyright 2003 Motorola Inc.
  * Xianghua Xiao (X.Xiao@motorola.com)
  *
@@ -173,11 +173,10 @@ spd_sdram(void)
 {
 	volatile immap_t *immap = (immap_t *)CFG_IMMR;
 	volatile ccsr_ddr_t *ddr = &immap->im_ddr;
-	volatile ccsr_gur_t *gur = &immap->im_gur;
 	spd_eeprom_t spd;
 	unsigned int n_ranks;
 	unsigned int rank_density;
-	unsigned int odt_rd_cfg, odt_wr_cfg;
+	unsigned int odt_rd_cfg, odt_wr_cfg, ba_bits;
 	unsigned int odt_cfg, mode_odt_enable;
 	unsigned int refresh_clk;
 #ifdef MPC85xx_DDR_SDRAM_CLK_CNTL
@@ -189,7 +188,7 @@ spd_sdram(void)
 	unsigned int max_data_rate, effective_data_rate;
 	unsigned int busfreq;
 	unsigned sdram_cfg;
-	unsigned int memsize;
+	unsigned int memsize = 0;
 	unsigned char caslat, caslat_ctrl;
 	unsigned int trfc, trfc_clk, trfc_low, trfc_high;
 	unsigned int trcd_clk;
@@ -204,6 +203,46 @@ spd_sdram(void)
 	unsigned int mode_caslat;
 	unsigned char sdram_type;
 	unsigned char d_init;
+	unsigned int bnds;
+
+	/*
+	 * Skip configuration if already configured.
+	 * memsize is determined from last configured chip select.
+	 */
+	if (ddr->cs0_config & 0x80000000) {
+		debug(" cs0 already configured, bnds=%x\n",ddr->cs0_bnds);
+		bnds = 0xfff & ddr->cs0_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4;
+		}
+	}
+	if (ddr->cs1_config & 0x80000000) {
+		debug(" cs1 already configured, bnds=%x\n",ddr->cs1_bnds);
+		bnds = 0xfff & ddr->cs1_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4; /* assume ordered bnds */
+		}
+	}
+	if (ddr->cs2_config & 0x80000000) {
+		debug(" cs2 already configured, bnds=%x\n",ddr->cs2_bnds);
+		bnds = 0xfff & ddr->cs2_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4;
+		}
+	}
+	if (ddr->cs3_config & 0x80000000) {
+		debug(" cs3 already configured, bnds=%x\n",ddr->cs3_bnds);
+		bnds = 0xfff & ddr->cs3_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4;
+		}
+	}
+
+	if (memsize) {
+		printf("       Reusing current %dMB configuration\n",memsize);
+		memsize = setup_laws_and_tlbs(memsize);
+		return memsize << 20;
+	}
 
 	/*
 	 * Read SPD information.
@@ -262,6 +301,7 @@ spd_sdram(void)
 		return 0;
 	}
 
+#ifdef CONFIG_MPC8548
 	/*
 	 * Adjust DDR II IO voltage biasing.
 	 * Only 8548 rev 1 needs the fix
@@ -269,9 +309,11 @@ spd_sdram(void)
 	if ((SVR_VER(get_svr()) == SVR_8548_E) &&
 			(SVR_MJREV(get_svr()) == 1) &&
 			(spd.mem_type == SPD_MEMTYPE_DDR2)) {
+		volatile ccsr_gur_t *gur = &immap->im_gur;
 		gur->ddrioovcr = (0x80000000	/* Enable */
 				  | 0x10000000);/* VSEL to 1.8V */
 	}
+#endif
 
 	/*
 	 * Determine the size of each Rank in bytes.
@@ -299,9 +341,14 @@ spd_sdram(void)
 #endif
 	}
 
+	ba_bits = 0;
+	if (spd.nbanks == 0x8)
+		ba_bits = 1;
+
 	ddr->cs0_config = ( 1 << 31
 			    | (odt_rd_cfg << 20)
 			    | (odt_wr_cfg << 16)
+			    | (ba_bits << 14)
 			    | (spd.nrow_addr - 12) << 8
 			    | (spd.ncol_addr - 8) );
 	debug("\n");
@@ -645,13 +692,10 @@ spd_sdram(void)
 	 */
 	cpo = 0;
 	if (spd.mem_type == SPD_MEMTYPE_DDR2) {
-		if (effective_data_rate == 266 || effective_data_rate == 333) {
+		if (effective_data_rate <= 333) {
 			cpo = 0x7;		/* READ_LAT + 5/4 */
-		} else if (effective_data_rate == 400) {
-			cpo = 0x9;		/* READ_LAT + 7/4 */
 		} else {
-			/* Pure speculation */
-			cpo = 0xb;
+			cpo = 0x9;		/* READ_LAT + 7/4 */
 		}
 	}
 
@@ -858,7 +902,12 @@ spd_sdram(void)
 	if (spd.mem_type == SPD_MEMTYPE_DDR)
 		clk_adjust = 0x6;
 	else
+#ifdef CONFIG_MPC8568
+		/* Empirally setting clk_adjust */
+		clk_adjust = 0x6;
+#else
 		clk_adjust = 0x7;
+#endif
 
 	ddr->sdram_clk_cntl = (0
 			       | 0x80000000
diff --git a/cpu/mpc85xx/start.S b/cpu/mpc85xx/start.S
index 77c155c5bd..2c98c2ad8a 100644
--- a/cpu/mpc85xx/start.S
+++ b/cpu/mpc85xx/start.S
@@ -1,7 +1,6 @@
 /*
- * Copyright 2004 Freescale Semiconductor.
+ * Copyright 2004, 2007 Freescale Semiconductor.
  * Copyright (C) 2003  Motorola,Inc.
- * Xianghua Xiao<X.Xiao@motorola.com>
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -46,7 +45,7 @@
 #endif
 
 #undef	MSR_KERNEL
-#define MSR_KERNEL ( MSR_ME  )	/* Machine Check */
+#define MSR_KERNEL ( MSR_ME )	/* Machine Check */
 
 /*
  * Set up GOT: Global Offset Table
@@ -80,110 +79,37 @@
  *
  */
 
-    .section .bootpg,"ax"
-    .globl _start_e500
+	.section .bootpg,"ax"
+	.globl _start_e500
 
 _start_e500:
-	mfspr	r0, PVR
-	lis	r1, PVR_85xx_REV1@h
-	ori	r1, r1, PVR_85xx_REV1@l
-	cmpw	r0, r1
-	bne	1f
 
-	/* Semi-bogus errata fixup for Rev 1 */
-	li	r0,0x2000
-	mtspr	977,r0
+/* clear registers/arrays not reset by hardware */
 
-	/*
-	 * Before invalidating MMU L1/L2, read TLB1 Entry 0 and then
-	 * write it back immediately to fixup a Rev 1 bug (Errata CPU4)
-	 * for this initial TLB1 entry 0, otherwise the TLB1 entry 0
-	 * will be invalidated (incorrectly).
-	 */
-	lis	r2,0x1000
-	mtspr	MAS0,r2
-	tlbre
-	tlbwe
-	isync
-
-1:
-	/*
-	 * Clear and set up some registers.
-	 * Note: Some registers need strict synchronization by
-	 * sync/mbar/msync/isync when being "mtspr".
-	 * BookE: isync before PID,tlbivax,tlbwe
-	 * BookE: isync after MSR,PID; msync_isync after tlbivax & tlbwe
-	 * E500:  msync,isync before L1CSR0
-	 * E500:  isync after BBEAR,BBTAR,BUCSR,DBCR0,DBCR1,HID0,HID1,
-	 *	  L1CSR0, L1CSR1, MAS[0,1,2,3,4,6],MMUCSR0, PID[0,1,2],
-	 *	  SPEFCSR
-	 */
-
-	/* invalidate d-cache */
-	mfspr	r0,L1CSR0
-	ori	r0,r0,0x0002
-	msync
-	isync
-	mtspr	L1CSR0,r0
-	isync
-
-	/* disable d-cache */
-	li	r0,0x0
-	mtspr	L1CSR0,r0
-
-	/* invalidate i-cache */
-	mfspr	r0,L1CSR1
-	ori	r0,r0,0x0002
-	mtspr	L1CSR1,r0
-	isync
-
-	/* disable i-cache */
-	li	r0,0x0
-	mtspr	L1CSR1,r0
-	isync
-
-	/* clear registers */
-	li	r0,0
-	mtspr	SRR0,r0
-	mtspr	SRR1,r0
-	mtspr	CSRR0,r0
-	mtspr	CSRR1,r0
-	mtspr	MCSRR0,r0
-	mtspr	MCSRR1,r0
-
-	mtspr	ESR,r0
-	mtspr	MCSR,r0
-	mtspr	DEAR,r0
-
-	/* not needed and conflicts with some debuggers */
-	/* mtspr	DBCR0,r0 */
-	mtspr	DBCR1,r0
-	mtspr	DBCR2,r0
-	/* not needed and conflicts with some debuggers */
-	/* mtspr	IAC1,r0 */
-	/* mtspr	IAC2,r0 */
-	mtspr	DAC1,r0
-	mtspr	DAC2,r0
+	/* L1 */
+	li	r0,2
+	mtspr	L1CSR0,r0	/* invalidate d-cache */
+	mtspr	L1CSR1,r0 	/* invalidate i-cache */
 
 	mfspr	r1,DBSR
 	mtspr	DBSR,r1		/* Clear all valid bits */
 
-	mtspr	PID0,r0
-	mtspr	PID1,r0
-	mtspr	PID2,r0
-	mtspr	TCR,r0
+	/*
+	 *	Enable L1 Caches early
+	 *
+	 */
 
-	mtspr	BUCSR,r0	/* disable branch prediction */
-	mtspr	MAS4,r0
-	mtspr	MAS6,r0
-#if defined(CONFIG_ENABLE_36BIT_PHYS)
-	mtspr	MAS7,r0
-#endif
+	lis	r2,L1CSR0_CPE@H	/* enable parity */
+	ori	r2,r2,L1CSR0_DCE
+	mtspr	L1CSR0,r2	/* enable L1 Dcache */
 	isync
+	mtspr	L1CSR1,r2	/* enable L1 Icache */
+	isync
+	msync
 
 	/* Setup interrupt vectors */
 	lis	r1,TEXT_BASE@h
-	mtspr IVPR, r1
+	mtspr	IVPR,r1
 
 	li	r1,0x0100
 	mtspr	IVOR0,r1	/* 0: Critical input */
@@ -217,26 +143,6 @@ _start_e500:
 	li	r1,0x0f00
 	mtspr	IVOR15,r1	/* 15: Debug */
 
-	/*
-	 * Invalidate MMU L1/L2
-	 *
-	 * Note: There is a fixup earlier for Errata CPU4 on
-	 * Rev 1 parts that must precede this MMU invalidation.
-	 */
-	li	r2, 0x001e
-	mtspr	MMUCSR0, r2
-	isync
-
-	/*
-	 * Invalidate all TLB0 entries.
-	 */
-	li	r3,4
-	li	r4,0
-	tlbivax r4,r3
-	/*
-	 * To avoid REV1 Errata CPU6 issues, make sure
-	 * the instruction following tlbivax is not a store.
-	 */
 
 	/*
 	 * After reset, CCSRBAR is located at CFG_CCSRBAR_DEFAULT, i.e.
@@ -254,14 +160,14 @@ _start_e500:
 	lwzu	r4,0(r5)	/* how many TLB1 entries we actually use */
 	mtctr	r4
 
-0:	lwzu	r0,4(r5)
-	lwzu	r1,4(r5)
-	lwzu	r2,4(r5)
-	lwzu	r3,4(r5)
-	mtspr	MAS0,r0
-	mtspr	MAS1,r1
-	mtspr	MAS2,r2
-	mtspr	MAS3,r3
+0:	lwzu	r6,4(r5)
+	lwzu	r7,4(r5)
+	lwzu	r8,4(r5)
+	lwzu	r9,4(r5)
+	mtspr	MAS0,r6
+	mtspr	MAS1,r7
+	mtspr	MAS2,r8
+	mtspr	MAS3,r9
 	isync
 	msync
 	tlbwe
@@ -271,22 +177,22 @@ _start_e500:
 1:
 #if (CFG_CCSRBAR_DEFAULT != CFG_CCSRBAR)
 	/* Special sequence needed to update CCSRBAR itself */
-	lis	r4, CFG_CCSRBAR_DEFAULT@h
-	ori	r4, r4, CFG_CCSRBAR_DEFAULT@l
+	lis	r4,CFG_CCSRBAR_DEFAULT@h
+	ori	r4,r4,CFG_CCSRBAR_DEFAULT@l
 
-	lis	r5, CFG_CCSRBAR@h
-	ori	r5, r5, CFG_CCSRBAR@l
+	lis	r5,CFG_CCSRBAR@h
+	ori	r5,r5,CFG_CCSRBAR@l
 	srwi	r6,r5,12
-	stw	r6, 0(r4)
+	stw	r6,0(r4)
 	isync
 
-	lis	r5, 0xffff
+	lis	r5,0xffff
 	ori	r5,r5,0xf000
-	lwz	r5, 0(r5)
+	lwz	r5,0(r5)
 	isync
 
-	lis	r3, CFG_CCSRBAR@h
-	lwz	r5, CFG_CCSRBAR@l(r3)
+	lis	r3,CFG_CCSRBAR@h
+	lwz	r5,CFG_CCSRBAR@l(r3)
 	isync
 #endif
 
@@ -300,8 +206,8 @@ _start_e500:
 	lwzu	r5,0(r6)	/* how many windows we actually use */
 	mtctr	r5
 
-	li	r2,0x0c28	/* the first pair is reserved for boot-over-rio-or-pci */
-	li	r1,0x0c30
+	li	r2,0x0c28	/* the first pair is reserved for */
+	li	r1,0x0c30	/* boot-over-rio-or-pci */
 
 0:	lwzu	r4,4(r6)
 	lwzu	r3,4(r6)
@@ -311,31 +217,6 @@ _start_e500:
 	addi	r1,r1,0x0020
 	bdnz	0b
 
-	/* Jump out the last 4K page and continue to 'normal' start */
-1:	bl	3f
-	b	_start
-
-3:	li	r0,0
-	mtspr	SRR1,r0		/* Keep things disabled for now */
-	mflr	r1
-	mtspr	SRR0,r1
-	rfi
-
-/*
- * r3 - 1st arg to board_init(): IMMP pointer
- * r4 - 2nd arg to board_init(): boot flag
- */
-	.text
-	.long	0x27051956		/* U-BOOT Magic Number			*/
-	.globl	version_string
-version_string:
-	.ascii U_BOOT_VERSION
-	.ascii " (", __DATE__, " - ", __TIME__, ")"
-	.ascii CONFIG_IDENT_STRING, "\0"
-
-	. = EXC_OFF_SYS_RESET
-	.globl	_start
-_start:
 	/* Clear and set up some registers. */
 	li	r0,0x0000
 	lis	r1,0xffff
@@ -354,17 +235,14 @@ _start:
 
 	/* Enable Time Base and Select Time Base Clock */
 	lis	r0,HID0_EMCP@h		/* Enable machine check */
-	ori	r0,r0,0x4000		/* time base is processor clock */
 #if defined(CONFIG_ENABLE_36BIT_PHYS)
-	ori	r0,r0,0x0080		/* enable MAS7 updates */
+	ori	r0,r0,(HID0_TBEN|HID0_ENMAS7)@l	/* Enable Timebase & MAS7 */
+#else
+	ori	r0,r0,HID0_TBEN@l	/* enable Timebase */
 #endif
 	mtspr	HID0,r0
 
-#if defined(CONFIG_ADDR_STREAMING)
-	li	r0,0x3000
-#else
-	li	r0,0x1000
-#endif
+	li	r0,(HID1_ASTME|HID1_ABE)@l	/* Addr streaming & broadcast */
 	mtspr	HID1,r0
 
 	/* Enable Branch Prediction */
@@ -383,37 +261,53 @@ _start:
 #endif
 
 /* L1 DCache is used for initial RAM */
-	mfspr	r2, L1CSR0
-	ori	r2, r2, 0x0003
-	oris	r2, r2, 0x0001
-	mtspr	L1CSR0, r2	/* enable/invalidate L1 Dcache */
-	isync
 
 	/* Allocate Initial RAM in data cache.
 	 */
-	lis	r3, CFG_INIT_RAM_ADDR@h
-	ori	r3, r3, CFG_INIT_RAM_ADDR@l
-	li	r2, 512 /* 512*32=16K */
+	lis	r3,CFG_INIT_RAM_ADDR@h
+	ori	r3,r3,CFG_INIT_RAM_ADDR@l
+	li	r2,512 /* 512*32=16K */
 	mtctr	r2
-	li	r0, 0
+	li	r0,0
 1:
-	dcbz	r0, r3
-	dcbtls	0,r0, r3
-	addi	r3, r3, 32
+	dcbz	r0,r3
+	dcbtls	0,r0,r3
+	addi	r3,r3,32
 	bdnz	1b
 
-#ifndef CFG_RAMBOOT
+	/* Jump out the last 4K page and continue to 'normal' start */
+#ifdef CFG_RAMBOOT
+	bl	3f
+	b	_start_cont
+#else
 	/* Calculate absolute address in FLASH and jump there		*/
 	/*--------------------------------------------------------------*/
-	lis	r3, CFG_MONITOR_BASE@h
-	ori	r3, r3, CFG_MONITOR_BASE@l
-	addi	r3, r3, in_flash - _start + EXC_OFF_SYS_RESET
+	lis	r3,CFG_MONITOR_BASE@h
+	ori	r3,r3,CFG_MONITOR_BASE@l
+	addi	r3,r3,_start_cont - _start + _START_OFFSET
 	mtlr	r3
-	blr
+#endif
 
-in_flash:
-#endif	/* CFG_RAMBOOT */
+3:	li	r0,0
+	mtspr	SRR1,r0		/* Keep things disabled for now */
+	mflr	r1
+	mtspr	SRR0,r1
+	rfi
+	isync
 
+	.text
+	.globl	_start
+_start:
+	.long	0x27051956		/* U-BOOT Magic Number */
+	.globl	version_string
+version_string:
+	.ascii U_BOOT_VERSION
+	.ascii " (", __DATE__, " - ", __TIME__, ")"
+	.ascii CONFIG_IDENT_STRING, "\0"
+
+	.align	4
+	.globl	_start_cont
+_start_cont:
 	/* Setup the stack in initial RAM,could be L2-as-SRAM or L1 dcache*/
 	lis	r1,CFG_INIT_RAM_ADDR@h
 	ori	r1,r1,CFG_INIT_SP_OFFSET@l
@@ -424,26 +318,24 @@ in_flash:
 
 	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
+	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) */
 
 	GET_GOT
 	bl	cpu_init_f
-	bl	icache_enable
 	bl	board_init_f
 	isync
 
-/* --FIXME-- machine check with MCSRRn and rfmci */
-
+	. = EXC_OFF_SYS_RESET
 	.globl	_start_of_vectors
 _start_of_vectors:
-#if 0
+
 /* Critical input. */
-	CRIT_EXCEPTION(0x0100, CritcalInput, CritcalInputException)
-#endif
-/* Machine check --FIXME-- Should be MACH_EXCEPTION */
-	CRIT_EXCEPTION(0x0200, MachineCheck, MachineCheckException)
+	CRIT_EXCEPTION(0x0100, CriticalInput, CritcalInputException)
+
+/* Machine check */
+	MCK_EXCEPTION(0x200, MachineCheck, MachineCheckException)
 
 /* Data Storage exception. */
 	STD_EXCEPTION(0x0300, DataStorage, UnknownException)
@@ -452,7 +344,7 @@ _start_of_vectors:
 	STD_EXCEPTION(0x0400, InstStorage, UnknownException)
 
 /* External Interrupt exception. */
-	STD_EXCEPTION(0x0500, ExtInterrupt, UnknownException)
+	STD_EXCEPTION(0x0500, ExtInterrupt, ExtIntException)
 
 /* Alignment exception. */
 	. = 0x0600
@@ -469,8 +361,8 @@ Alignment:
 	mtlr	r6
 	blrl
 .L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	.long	AlignmentException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
 
 /* Program check exception */
 	. = 0x0700
@@ -483,8 +375,8 @@ ProgramCheck:
 	mtlr	r6
 	blrl
 .L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	.long	ProgramCheckException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
 
 	/* No FPU on MPC85xx.  This exception is not supposed to happen.
 	*/
@@ -496,23 +388,23 @@ ProgramCheck:
  * r3-... arguments
  */
 SystemCall:
-	addis	r11,r0,0		/* get functions table addr */
-	ori	r11,r11,0		/* Note: this code is patched in trap_init */
-	addis	r12,r0,0		/* get number of functions */
+	addis	r11,r0,0	/* get functions table addr */
+	ori	r11,r11,0	/* Note: this code is patched in trap_init */
+	addis	r12,r0,0	/* get number of functions */
 	ori	r12,r12,0
 
-	cmplw	0, r0, r12
+	cmplw	0,r0,r12
 	bge	1f
 
-	rlwinm	r0,r0,2,0,31		/* fn_addr = fn_tbl[r0] */
+	rlwinm	r0,r0,2,0,31	/* fn_addr = fn_tbl[r0] */
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r20,0xd00-4		/* Get stack pointer */
+	li	r20,0xd00-4	/* Get stack pointer */
 	lwz	r12,0(r20)
-	subi	r12,r12,12		/* Adjust stack pointer */
+	subi	r12,r12,12	/* Adjust stack pointer */
 	li	r0,0xc00+_end_back-SystemCall
-	cmplw	0, r0, r12		/* Check stack overflow */
+	cmplw	0,r0,r12	/* Check stack overflow */
 	bgt	1f
 	stw	r12,0(r20)
 
@@ -570,7 +462,7 @@ _end_back:
 _end_of_vectors:
 
 
-	. = 0x2100
+	. = . + (0x100 - ( . & 0xff ))	/* align for debug */
 
 /*
  * This code finishes saving the registers to the exception frame
@@ -655,26 +547,58 @@ 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	SPRN_CSRR0,r2
+	mtspr	SPRN_CSRR1,r0
 	lwz	r0,GPR0(r1)
 	lwz	r2,GPR2(r1)
 	lwz	r1,GPR1(r1)
 	SYNC
 	rfci
 
+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	SPRN_MCSRR0,r2
+	mtspr	SPRN_MCSRR1,r0
+	lwz	r0,GPR0(r1)
+	lwz	r2,GPR2(r1)
+	lwz	r1,GPR1(r1)
+	SYNC
+	rfmci
+
 /* Cache functions.
 */
 invalidate_icache:
 	mfspr	r0,L1CSR1
-	ori	r0,r0,0x0002
+	ori	r0,r0,L1CSR1_ICFI
+	msync
+	isync
 	mtspr	L1CSR1,r0
 	isync
-	blr				/*   entire I cache */
+	blr				/* entire I cache */
 
 invalidate_dcache:
 	mfspr	r0,L1CSR0
-	ori	r0,r0,0x0002
+	ori	r0,r0,L1CSR0_DCFI
 	msync
 	isync
 	mtspr	L1CSR0,r0
@@ -697,9 +621,9 @@ icache_enable:
 	.globl	icache_disable
 icache_disable:
 	mfspr	r0,L1CSR1
-	lis	r1,0xfffffffe@h
-	ori	r1,r1,0xfffffffe@l
-	and	r0,r0,r1
+	lis	r3,0
+	ori	r3,r3,L1CSR1_ICE
+	andc	r0,r0,r3
 	mtspr	L1CSR1,r0
 	isync
 	blr
@@ -707,7 +631,7 @@ icache_disable:
 	.globl	icache_status
 icache_status:
 	mfspr	r3,L1CSR1
-	andi.	r3,r3,1
+	andi.	r3,r3,L1CSR1_ICE
 	blr
 
 	.globl	dcache_enable
@@ -727,12 +651,10 @@ dcache_enable:
 
 	.globl	dcache_disable
 dcache_disable:
-	mfspr	r0,L1CSR0
-	lis	r1,0xfffffffe@h
-	ori	r1,r1,0xfffffffe@l
-	and	r0,r0,r1
-	msync
-	isync
+	mfspr	r3,L1CSR0
+	lis	r4,0
+	ori	r4,r4,L1CSR0_DCE
+	andc	r3,r3,r4
 	mtspr	L1CSR0,r0
 	isync
 	blr
@@ -740,27 +662,27 @@ dcache_disable:
 	.globl	dcache_status
 dcache_status:
 	mfspr	r3,L1CSR0
-	andi.	r3,r3,1
+	andi.	r3,r3,L1CSR0_DCE
 	blr
 
 	.globl get_pir
 get_pir:
-	mfspr	r3, PIR
+	mfspr	r3,PIR
 	blr
 
 	.globl get_pvr
 get_pvr:
-	mfspr	r3, PVR
+	mfspr	r3,PVR
 	blr
 
 	.globl get_svr
 get_svr:
-	mfspr	r3, SVR
+	mfspr	r3,SVR
 	blr
 
 	.globl wr_tcr
 wr_tcr:
-	mtspr	TCR, r3
+	mtspr	TCR,r3
 	blr
 
 /*------------------------------------------------------------------------------- */
@@ -913,16 +835,16 @@ ppcSync:
  */
 	.globl	relocate_code
 relocate_code:
-	mr	r1,  r3		/* Set new stack pointer		*/
-	mr	r9,  r4		/* Save copy of Init Data pointer	*/
-	mr	r10, r5		/* Save copy of Destination Address	*/
+	mr	r1,r3		/* Set new stack pointer		*/
+	mr	r9,r4		/* Save copy of Init Data pointer	*/
+	mr	r10,r5		/* Save copy of Destination Address	*/
 
-	mr	r3,  r5				/* Destination Address	*/
-	lis	r4, CFG_MONITOR_BASE@h		/* Source      Address	*/
-	ori	r4, r4, CFG_MONITOR_BASE@l
+	mr	r3,r5				/* Destination Address	*/
+	lis	r4,CFG_MONITOR_BASE@h		/* Source      Address	*/
+	ori	r4,r4,CFG_MONITOR_BASE@l
 	lwz	r5,GOT(__init_end)
 	sub	r5,r5,r4
-	li	r6, CFG_CACHELINE_SIZE		/* Cache Line Size	*/
+	li	r6,CFG_CACHELINE_SIZE		/* Cache Line Size	*/
 
 	/*
 	 * Fix GOT pointer:
@@ -931,12 +853,12 @@ relocate_code:
 	 *
 	 * Offset:
 	 */
-	sub	r15, r10, r4
+	sub	r15,r10,r4
 
 	/* First our own GOT */
-	add	r14, r14, r15
+	add	r14,r14,r15
 	/* the the one used by the C code */
-	add	r30, r30, r15
+	add	r30,r30,r15
 
 	/*
 	 * Now relocate code
@@ -997,10 +919,10 @@ 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! */
-
+	.globl	in_ram
 in_ram:
 
 	/*
@@ -1044,19 +966,19 @@ clear_bss:
 	lwz	r3,GOT(__bss_start)
 	lwz	r4,GOT(_end)
 
-	cmplw	0, r3, r4
+	cmplw	0,r3,r4
 	beq	6f
 
-	li	r0, 0
+	li	r0,0
 5:
-	stw	r0, 0(r3)
-	addi	r3, r3, 4
-	cmplw	0, r3, r4
+	stw	r0,0(r3)
+	addi	r3,r3,4
+	cmplw	0,r3,r4
 	bne	5b
 6:
 
-	mr	r3, r9		/* Init Data pointer		*/
-	mr	r4, r10		/* Destination Address		*/
+	mr	r3,r9		/* Init Data pointer		*/
+	mr	r4,r10		/* Destination Address		*/
 	bl	board_init_r
 
 	/*
@@ -1067,52 +989,54 @@ clear_bss:
 	 */
 	.globl	trap_init
 trap_init:
-	lwz	r7, GOT(_start)
-	lwz	r8, GOT(_end_of_vectors)
+	lwz	r7,GOT(_start_of_vectors)
+	lwz	r8,GOT(_end_of_vectors)
 
-	li	r9, 0x100		/* reset vector always at 0x100 */
+	li	r9,0x100		/* reset vector always at 0x100 */
 
-	cmplw	0, r7, r8
+	cmplw	0,r7,r8
 	bgelr				/* return if r7>=r8 - just in case */
 
 	mflr	r4			/* save link register		*/
 1:
-	lwz	r0, 0(r7)
-	stw	r0, 0(r9)
-	addi	r7, r7, 4
-	addi	r9, r9, 4
-	cmplw	0, r7, r8
+	lwz	r0,0(r7)
+	stw	r0,0(r9)
+	addi	r7,r7,4
+	addi	r9,r9,4
+	cmplw	0,r7,r8
 	bne	1b
 
 	/*
 	 * relocate `hdlr' and `int_return' entries
 	 */
-	li	r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_CriticalInput - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_DataStorage - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_MachineCheck - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_InstStorage - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_DataStorage - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_ExtInterrupt - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_InstStorage - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_ExtInterrupt - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_Alignment - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_ProgramCheck - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_Decrementer - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_FPUnavailable - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_IntervalTimer - _start + EXC_OFF_SYS_RESET
-	li	r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_Decrementer - _start + _START_OFFSET
+	bl	trap_reloc
+	li	r7,.L_IntervalTimer - _start + _START_OFFSET
+	li	r8,_end_of_vectors - _start + _START_OFFSET
 2:
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
+	addi	r7,r7,0x100		/* next exception vector	*/
+	cmplw	0,r7,r8
 	blt	2b
 
 	lis	r7,0x0
-	mtspr	IVPR, r7
+	mtspr	IVPR,r7
 
 	mtlr	r4			/* restore link register	*/
 	blr
@@ -1121,13 +1045,13 @@ trap_init:
 	 * Function: relocate entries for one exception vector
 	 */
 trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
+	lwz	r0,0(r7)		/* hdlr ...			*/
+	add	r0,r0,r3		/*  ... += dest_addr		*/
+	stw	r0,0(r7)
 
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
+	lwz	r0,4(r7)		/* int_return ...		*/
+	add	r0,r0,r3		/*  ... += dest_addr		*/
+	stw	r0,4(r7)
 
 	blr
 
@@ -1135,13 +1059,13 @@ trap_reloc:
 .globl unlock_ram_in_cache
 unlock_ram_in_cache:
 	/* invalidate the INIT_RAM section */
-	lis	r3, (CFG_INIT_RAM_ADDR & ~31)@h
-	ori	r3, r3, (CFG_INIT_RAM_ADDR & ~31)@l
-	li	r2,512
-	mtctr	r2
-1:	icbi	r0, r3
-	dcbi	r0, r3
-	addi	r3, r3, 32
+	lis	r3,(CFG_INIT_RAM_ADDR & ~31)@h
+	ori	r3,r3,(CFG_INIT_RAM_ADDR & ~31)@l
+	li	r4,512
+	mtctr	r4
+1:	icbi	r0,r3
+	dcbi	r0,r3
+	addi	r3,r3,32
 	bdnz	1b
 	sync			/* Wait for all icbi to complete on bus */
 	isync
diff --git a/cpu/mpc85xx/traps.c b/cpu/mpc85xx/traps.c
index 9cd621c3ac..efc80c7aee 100644
--- a/cpu/mpc85xx/traps.c
+++ b/cpu/mpc85xx/traps.c
@@ -1,6 +1,7 @@
 /*
  * linux/arch/ppc/kernel/traps.c
  *
+ * Copyright 2007 Freescale Semiconductor.
  * Copyright (C) 2003 Motorola
  * Modified by Xianghua Xiao(x.xiao@motorola.com)
  *
@@ -145,10 +146,13 @@ CritcalInputException(struct pt_regs *regs)
 	panic("Critical Input Exception");
 }
 
+int machinecheck_count = 0;
+int machinecheck_error = 0;
 void
 MachineCheckException(struct pt_regs *regs)
 {
 	unsigned long fixup;
+	unsigned int mcsr, mcsrr0, mcsrr1, mcar;
 
 	/* Probing PCI using config cycles cause this exception
 	 * when a device is not present.  Catch it and return to
@@ -159,34 +163,62 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
+	mcsrr0 = mfspr(SPRN_MCSRR0);
+	mcsrr1 = mfspr(SPRN_MCSRR1);
+	mcsr = mfspr(SPRN_MCSR);
+	mcar = mfspr(SPRN_MCAR);
+
+	machinecheck_count++;
+	machinecheck_error=1;
+
 #if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
 
 	printf("Machine check in kernel mode.\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");
-		break;
-	case (0x80000000>>13):
-		printf("Transfer error ack signal\n");
-		break;
-	case (0x80000000>>14):
-		printf("Data parity signal\n");
-		break;
-	case (0x80000000>>15):
-		printf("Address parity signal\n");
-		break;
-	default:
-		printf("Unknown values in msr\n");
-	}
+	printf("Caused by (from mcsr): ");
+	printf("mcsr = 0x%08x\n", mcsr);
+	if (mcsr & 0x80000000)
+		printf("Machine check input pin\n");
+	if (mcsr & 0x40000000)
+		printf("Instruction cache parity error\n");
+	if (mcsr & 0x20000000)
+		printf("Data cache push parity error\n");
+	if (mcsr & 0x10000000)
+		printf("Data cache parity error\n");
+	if (mcsr & 0x00000080)
+		printf("Bus instruction address error\n");
+	if (mcsr & 0x00000040)
+		printf("Bus Read address error\n");
+	if (mcsr & 0x00000020)
+		printf("Bus Write address error\n");
+	if (mcsr & 0x00000010)
+		printf("Bus Instruction data bus error\n");
+	if (mcsr & 0x00000008)
+		printf("Bus Read data bus error\n");
+	if (mcsr & 0x00000004)
+		printf("Bus Write bus error\n");
+	if (mcsr & 0x00000002)
+		printf("Bus Instruction parity error\n");
+	if (mcsr & 0x00000001)
+		printf("Bus Read parity error\n");
+
 	show_regs(regs);
+	printf("MCSR=0x%08x \tMCSRR0=0x%08x \nMCSRR1=0x%08x \tMCAR=0x%08x\n",
+	       mcsr, mcsrr0, mcsrr1, mcar);
 	print_backtrace((unsigned long *)regs->gpr[1]);
-	panic("machine check");
+	if (machinecheck_count > 10) {
+		panic("machine check count too high\n");
+	}
+
+	if (machinecheck_count > 1) {
+		regs->nip += 4; /* skip offending instruction */
+		printf("Skipping current instr, Returning to 0x%08x\n",
+		       regs->nip);
+	} else {
+		printf("Returning back to 0x%08x\n",regs->nip);
+	}
 }
 
 void
@@ -253,6 +285,33 @@ UnknownException(struct pt_regs *regs)
 	       regs->nip, regs->msr, regs->trap);
 	_exception(0, regs);
 }
+void
+ExtIntException(struct pt_regs *regs)
+{
+	volatile immap_t *immap = (immap_t *)CFG_IMMR;
+	volatile ccsr_pic_t *pic = &immap->im_pic;
+	uint vect;
+
+#if defined(CONFIG_CMD_KGDB)
+	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+		return;
+#endif
+
+	printf("External Interrupt Exception at PC: %lx, SR: %lx, vector=%lx",
+	       regs->nip, regs->msr, regs->trap);
+	vect = pic->iack0;
+	printf(" irq IACK0@%05x=%d\n",&pic->iack0,vect);
+	show_regs(regs);
+	print_backtrace((unsigned long *)regs->gpr[1]);
+	machinecheck_count++;
+#ifdef EXTINT_NOSKIP
+	printf("Returning back to 0x%08x\n",regs->nip);
+#else
+	regs->nip += 4; /* skip offending instruction */
+	printf("Skipping current instr, Returning to 0x%08x\n",regs->nip);
+#endif
+
+}
 
 void
 DebugException(struct pt_regs *regs)
diff --git a/cpu/mpc86xx/cpu_init.c b/cpu/mpc86xx/cpu_init.c
index 4673d05e71..4f8956e0af 100644
--- a/cpu/mpc86xx/cpu_init.c
+++ b/cpu/mpc86xx/cpu_init.c
@@ -29,6 +29,8 @@
 #include <common.h>
 #include <mpc86xx.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 /*
  * Breathe some life into the CPU...
  *
@@ -38,7 +40,6 @@
 
 void cpu_init_f(void)
 {
-	DECLARE_GLOBAL_DATA_PTR;
 	volatile immap_t    *immap = (immap_t *)CFG_IMMR;
 	volatile ccsr_lbc_t *memctl = &immap->im_lbc;
 
@@ -104,8 +105,8 @@ void cpu_init_f(void)
 	/* enable the timebase bit in HID0 */
 	set_hid0(get_hid0() | 0x4000000);
 
-	/* enable SYNCBE | ABE bits in  HID1 */
-	set_hid1(get_hid1() | 0x00000C00);
+	/* enable EMCP, SYNCBE | ABE bits in HID1 */
+	set_hid1(get_hid1() | 0x80000C00);
 }
 
 /*
diff --git a/cpu/mpc86xx/interrupts.c b/cpu/mpc86xx/interrupts.c
index 08e0675fee..d9f634fdab 100644
--- a/cpu/mpc86xx/interrupts.c
+++ b/cpu/mpc86xx/interrupts.c
@@ -8,7 +8,7 @@
  * (C) Copyright 2003 Motorola Inc. (MPC85xx port)
  * Xianghua Xiao (X.Xiao@motorola.com)
  *
- * (C) Copyright 2004 Freescale Semiconductor. (MPC86xx Port)
+ * (C) Copyright 2004, 2007 Freescale Semiconductor. (MPC86xx Port)
  * Jeff Brown
  * Srikanth Srinivasan (srikanth.srinivasan@freescale.com)
  *
@@ -80,25 +80,10 @@ int interrupt_init(void)
 {
 	int ret;
 
-	/*
-	 * The IRQ0 on Rev 2 is pulled high (low in Rev 1.x) to
-	 * implement PEX10 errata.  As INT is active high, it
-	 * will cause core to take 0x500 interrupt.
-	 *
-	 * Due to the PIC's default pass through mode, as soon
-	 * as interrupts are enabled (MSR[EE] = 1), an interrupt
-	 * will be taken and u-boot will hang.  This is due to a
-	 * hardware change (per an errata fix) on new revisions
-	 * of the board with Rev 2.x parts.
-	 *
-	 * Setting the PIC to mixed mode prevents the hang.
-	 */
-	if ((get_svr() & 0xf0) == 0x20) {
-		volatile immap_t *immr = (immap_t *)CFG_IMMR;
-		immr->im_pic.gcr = MPC86xx_PICGCR_RST;
-		while (immr->im_pic.gcr & MPC86xx_PICGCR_RST);
-		immr->im_pic.gcr = MPC86xx_PICGCR_MODE;
-	}
+	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	immr->im_pic.gcr = MPC86xx_PICGCR_RST;
+	while (immr->im_pic.gcr & MPC86xx_PICGCR_RST);
+	immr->im_pic.gcr = MPC86xx_PICGCR_MODE;
 
 	/* call cpu specific function from $(CPU)/interrupts.c */
 	ret = interrupt_init_cpu(&decrementer_count);
@@ -119,6 +104,30 @@ int interrupt_init(void)
 	      get_msr(),
 	      get_dec());
 
+#ifdef CONFIG_INTERRUPTS
+	volatile ccsr_pic_t *pic = &immr->im_pic;
+
+	pic->iivpr1 = 0x810001;	/* 50220 enable mcm interrupts */
+	debug("iivpr1@%x = %x\n", &pic->iivpr1, pic->iivpr1);
+
+	pic->iivpr2 = 0x810002;	/* 50240 enable ddr interrupts */
+	debug("iivpr2@%x = %x\n", &pic->iivpr2, pic->iivpr2);
+
+	pic->iivpr3 = 0x810003;	/* 50260 enable lbc interrupts */
+	debug("iivpr3@%x = %x\n", &pic->iivpr3, pic->iivpr3);
+
+#if defined(CONFIG_PCI1) || defined(CONFIG_PCIE1)
+	pic->iivpr8 = 0x810008;	/* enable pcie1 interrupts */
+	debug("iivpr8@%x = %x\n", &pic->iivpr8, pic->iivpr8);
+#endif
+#if defined(CONFIG_PCI2) || defined(CONFIG_PCIE2)
+	pic->iivpr9 = 0x810009;	/* enable pcie2 interrupts */
+	debug("iivpr9@%x = %x\n", &pic->iivpr9, pic->iivpr9);
+#endif
+
+	pic->ctpr = 0;	/* 40080 clear current task priority register */
+#endif
+
 	return 0;
 }
 
@@ -158,8 +167,6 @@ void timer_interrupt(struct pt_regs *regs)
 
 	timestamp++;
 
-	ppcDcbf((unsigned long)&timestamp);
-
 	/* Restore Decrementer Count */
 	set_dec(decrementer_count);
 
diff --git a/cpu/mpc86xx/speed.c b/cpu/mpc86xx/speed.c
index 312ca12827..23161ca8cb 100644
--- a/cpu/mpc86xx/speed.c
+++ b/cpu/mpc86xx/speed.c
@@ -29,6 +29,7 @@
 #include <mpc86xx.h>
 #include <asm/processor.h>
 
+DECLARE_GLOBAL_DATA_PTR;
 
 void get_sys_info(sys_info_t *sysInfo)
 {
@@ -96,7 +97,6 @@ void get_sys_info(sys_info_t *sysInfo)
 
 int get_clocks(void)
 {
-	DECLARE_GLOBAL_DATA_PTR;
 	sys_info_t sys_info;
 
 	get_sys_info(&sys_info);
diff --git a/cpu/mpc86xx/start.S b/cpu/mpc86xx/start.S
index 412745bdae..c83310a333 100644
--- a/cpu/mpc86xx/start.S
+++ b/cpu/mpc86xx/start.S
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004 Freescale Semiconductor.
+ * Copyright 2004, 2007 Freescale Semiconductor.
  * Srikanth Srinivasan <srikanth.srinivaan@freescale.com>
  *
  * See file CREDITS for list of people who contributed to this
@@ -44,11 +44,9 @@
 #define CONFIG_IDENT_STRING ""
 #endif
 
-/* We don't want the  MMU yet.
-*/
-#undef	MSR_KERNEL
-/* Machine Check and Recoverable Interr. */
-#define MSR_KERNEL ( MSR_ME | MSR_RI )
+/*
+ * Need MSR_DR | MSR_IR enabled to access I/O (printf) in exceptions
+ */
 
 /*
  * Set up GOT: Global Offset Table
@@ -195,17 +193,21 @@ boot_warm:
 	bl	secondary_cpu_setup
 #endif
 
+1:
+#ifdef CFG_RAMBOOT
 	/* disable everything */
-1:	li	r0, 0
+	li	r0, 0
 	mtspr	HID0, r0
 	sync
 	mtmsr	0
+#endif
+
 	bl	invalidate_bats
 	sync
 
 #ifdef CFG_L2
 	/* init the L2 cache */
-	addis	r3, r0, L2_INIT@h
+	lis	r3, L2_INIT@h
 	ori	r3, r3, L2_INIT@l
 	mtspr	l2cr, r3
 	/* invalidate the L2 cache */
@@ -241,69 +243,9 @@ in_flash:
 	bl	setup_ccsrbar
 #endif
 
-
-	/* -- MPC8641 Rev 1.0 MCM Errata fixups -- */
-
-	/* skip fixups if not Rev 1.0 */
-	mfspr	r4, SVR
-	rlwinm	r4,r4,0,24,31
-	cmpwi	r4,0x10
-	bne	1f
-
-	lis	r3,MCM_ABCR@ha
-	lwz	r4,MCM_ABCR@l(r3)	/* ABCR -> r4 */
-
-	/* set ABCR[A_STRM_CNT] = 0 */
-	rlwinm	r4,r4,0,0,29
-
-	/* set ABCR[ARB_POLICY] to 0x1 (round-robin) */
-	addi	r0,r0,1
-	rlwimi	r4,r0,12,18,19
-
-	stw	r4,MCM_ABCR@l(r3)	/* r4 -> ABCR */
-	sync
-
-	/* Set DBCR[ERD_DIS] */
-	lis	r3,MCM_DBCR@ha
-	lwz	r4,MCM_DBCR@l(r3)
-	oris	r4, r4, 0x4000
-	stw	r4,MCM_DBCR@l(r3)
-	sync
-1:
 	/* setup the law entries */
 	bl	law_entry
 	sync
-
-
-#if (EMULATOR_RUN == 1)
-	/* On the emulator we want to adjust these ASAP */
-	/* otherwise things are sloooow */
-	/* Setup OR0 (LALE FIX)*/
-	lis	r3, CFG_CCSRBAR@h
-	ori	r3, r3, 0x5004
-	li	r4, 0x0FF3
-	stw	r4, 0(r3)
-	sync
-
-	/* Setup LCRR */
-	lis	r3, CFG_CCSRBAR@h
-	ori	r3, r3, 0x50D4
-	lis	r4, 0x8000
-	ori	r4, r4, 0x0002
-	stw	r4, 0(r3)
-	sync
-#endif
-#if 1
-	/* make sure timer enabled in guts register too */
-	lis	r3, CFG_CCSRBAR@h
-	oris	r3,r3, 0xE
-	ori	r3,r3,0x0070
-	lwz	r4, 0(r3)
-	lis	r5,0xFFFC
-	ori	r5,r5,0x5FFF
-	and	r4,r4,r5
-	stw	r4,0(r3)
-#endif
 	/*
 	 * Cache must be enabled here for stack-in-cache trick.
 	 * This means we need to enable the BATS.
@@ -346,8 +288,6 @@ in_flash:
 
 #ifdef	RUN_DIAG
 
-	/* Sri:	 Code to run the diagnostic automatically */
-
 	/* Load PX_AUX register address in r4 */
 	lis	r4, 0xf810
 	ori	r4, r4, 0x6
@@ -392,6 +332,7 @@ diag_done:
 	.globl	invalidate_bats
 invalidate_bats:
 
+	li	r0, 0
 	/* invalidate BATs */
 	mtspr	IBAT0U, r0
 	mtspr	IBAT1U, r0
@@ -1040,6 +981,7 @@ trap_init:
 	mfmsr	r7
 	li	r8,MSR_IP
 	andc	r7,r7,r8
+	ori	r7,r7,MSR_ME		/* Enable Machine Check */
 	mtmsr	r7
 
 	mtlr	r4			/* restore link register	*/
@@ -1224,8 +1166,9 @@ secondary_cpu_setup:
 	sync
 	isync
 
-	/*SYNCBE|ABE in HID1*/
+	/* MCP|SYNCBE|ABE in HID1 */
 	mfspr	r4, HID1
+	oris	r4, r4, 0x8000
 	ori	r4, r4, 0x0C00
 	mtspr	HID1, r4
 	sync
diff --git a/cpu/mpc86xx/traps.c b/cpu/mpc86xx/traps.c
index fab1975834..04c2e1331b 100644
--- a/cpu/mpc86xx/traps.c
+++ b/cpu/mpc86xx/traps.c
@@ -34,6 +34,8 @@
 #include <command.h>
 #include <asm/processor.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 #if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
@@ -50,8 +52,6 @@ extern unsigned long search_exception_table(unsigned long);
 void
 print_backtrace(unsigned long *sp)
 {
-	DECLARE_GLOBAL_DATA_PTR;
-
 	int cnt = 0;
 	unsigned long i;
 
@@ -130,8 +130,11 @@ MachineCheckException(struct pt_regs *regs)
 	printf("Machine check in kernel mode.\n");
 	printf("Caused by (from msr): ");
 	printf("regs %p ", regs);
-	switch (regs->msr & 0x000F0000) {
-	case (0x80000000 >> 12):
+	switch ( regs->msr & 0x001F0000) {
+	case (0x80000000>>11):
+		printf("MSS error. MSSSR0: %08x\n", mfspr(SPRN_MSSSR0));
+		break;
+	case (0x80000000>>12):
 		printf("Machine check signal - probably due to mm fault\n"
 		       "with mmu off\n");
 		break;
@@ -209,6 +212,7 @@ UnknownException(struct pt_regs *regs)
 	if (debugger_exception_handler && (*debugger_exception_handler) (regs))
 		return;
 #endif
+	printf("UnknownException regs@%x\n", regs);
 	printf("Bad trap at PC: %lx, SR: %lx, vector=%lx\n",
 	       regs->nip, regs->msr, regs->trap);
 	_exception(0, regs);
diff --git a/cpu/nios/cpu.c b/cpu/nios/cpu.c
index d2bb2c09d1..5519e82780 100644
--- a/cpu/nios/cpu.c
+++ b/cpu/nios/cpu.c
@@ -34,7 +34,7 @@ int checkcpu (void)
 
 	/* Get cpu version info */
 	val = rdctl (CTL_CPU_ID);
-	printf ("CPU: ");
+	puts ("CPU:   ");
 	printf ("%s", (val & 0x00008000) ? "Nios-16 " : "Nios-32 ");
 	rev_major = (val>>12) & 0x07;
 	rev_minor = (val>>4) & 0x0ff;
diff --git a/cpu/ppc4xx/405gp_pci.c b/cpu/ppc4xx/405gp_pci.c
index 28379298b7..282e7a1ba4 100644
--- a/cpu/ppc4xx/405gp_pci.c
+++ b/cpu/ppc4xx/405gp_pci.c
@@ -443,7 +443,7 @@ void pci_init_board(void)
 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;
 
@@ -459,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;
@@ -468,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 */
@@ -477,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++,
@@ -515,7 +515,7 @@ void pci_440_init (struct pci_controller *hose)
 	if (pci_pre_init (hose) == 0) {
 		printf("PCI: Board-specific initialization failed.\n");
 		printf("PCI: Configuration aborted.\n");
-		return;
+		return -1;
 	}
 
 	pci_register_hose( hose );
@@ -578,13 +578,16 @@ 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
 }
 
diff --git a/cpu/ppc4xx/440spe_pcie.c b/cpu/ppc4xx/440spe_pcie.c
index d6c4be5f1a..158f1c5595 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 2becc77722..38745eb797 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 6d6fba1802..4a4c6f29ed 100644
--- a/cpu/ppc4xx/44x_spd_ddr.c
+++ b/cpu/ppc4xx/44x_spd_ddr.c
@@ -269,9 +269,8 @@ 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 phys_addr, u32 virt_addr, u32 size, u32 tlb_word2_i_value);
 
 static unsigned char spd_read(uchar chip, uint addr);
 static void get_spd_info(unsigned long *dimm_populated,
diff --git a/cpu/ppc4xx/44x_spd_ddr2.c b/cpu/ppc4xx/44x_spd_ddr2.c
index 5fef27b984..67ba5bdef2 100644
--- a/cpu/ppc4xx/44x_spd_ddr2.c
+++ b/cpu/ppc4xx/44x_spd_ddr2.c
@@ -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() */
 
@@ -138,6 +138,26 @@ void __spd_ddr_init_hang (void)
 }
 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 */
 
@@ -154,7 +174,6 @@ typedef enum ddr_cas_id {
  * Prototypes
  *-----------------------------------------------------------------------------*/
 static unsigned long sdram_memsize(void);
-void program_tlb(u32 phys_addr, u32 virt_addr, 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);
@@ -216,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);
@@ -469,17 +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);
-#ifdef CFG_44x_DDR2_CKTR_180
-	mtsdram(SDRAM_CLKTR, (val & ~SDRAM_CLKTR_CLKP_MASK) | SDRAM_CLKTR_CLKP_180_DEG_ADV);
-#else
-	mtsdram(SDRAM_CLKTR, (val & ~SDRAM_CLKTR_CLKP_MASK) | SDRAM_CLKTR_CLKP_0_DEG);
-#endif
+	mtsdram(SDRAM_CLKTR, (val & ~SDRAM_CLKTR_CLKP_MASK) |
+		ddr_clktr(SDRAM_CLKTR_CLKP_0_DEG));
 
 	/*------------------------------------------------------------------
 	 * Program the BxCF registers.
@@ -538,7 +552,12 @@ long int initdram(int board_type)
 	dram_size = sdram_memsize();
 
 	/* and program tlb entries for this size (dynamic) */
-	program_tlb(0, 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.
@@ -549,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;
 }
@@ -596,7 +621,6 @@ static void get_spd_info(unsigned long *dimm_populated,
 	}
 }
 
-#ifdef CONFIG_ADD_RAM_INFO
 void board_add_ram_info(int use_default)
 {
 	PPC440_SYS_INFO board_cfg;
@@ -617,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
@@ -2703,6 +2726,7 @@ calibration_loop:
 		printf("\nERROR: Cannot determine a common read delay for the "
 		       "DIMM(s) installed.\n");
 		debug("%s[%d] ERROR : \n", __FUNCTION__,__LINE__);
+		ppc440sp_sdram_register_dump();
 		spd_ddr_init_hang ();
 	}
 
@@ -3028,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 d78279171e..cc8e7346da 100644
--- a/cpu/ppc4xx/4xx_enet.c
+++ b/cpu/ppc4xx/4xx_enet.c
@@ -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)
diff --git a/cpu/ppc4xx/gpio.c b/cpu/ppc4xx/gpio.c
index 5235203ea2..50f2fdf113 100644
--- a/cpu/ppc4xx/gpio.c
+++ b/cpu/ppc4xx/gpio.c
@@ -186,6 +186,7 @@ void gpio_set_chip_configuration(void)
 						out32(GPIO0_TCR, reg);
 					}
 
+#ifdef GPIO1
 					if (gpio_core == GPIO1) {
 						/*
 						 * Setup output value
@@ -193,16 +194,17 @@ void gpio_set_chip_configuration(void)
 						 * 0 -> low level
 						 * else -> don't touch
 						 */
-						reg = in32(GPIO0_OR);
+						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(GPIO0_OR, reg);
+						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/sdram.c b/cpu/ppc4xx/sdram.c
index d520cd3ff4..2724d91f0f 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 62b5442f3b..4fb9b1ae14 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 3f67136be5..60712b151e 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 */
diff --git a/cpu/ppc4xx/speed.c b/cpu/ppc4xx/speed.c
index 028b11af89..da5330a360 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 8ecaaea4d9..9626b65c88 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -1870,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.
@@ -1947,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:
diff --git a/cpu/ppc4xx/tlb.c b/cpu/ppc4xx/tlb.c
index 049a785495..098694caf4 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,6 +35,67 @@ typedef struct region {
 	unsigned long tlb_word2_i_value;
 } region_t;
 
+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,
diff --git a/cpu/ppc4xx/traps.c b/cpu/ppc4xx/traps.c
index 899cdbd1f4..38b6f89555 100644
--- a/cpu/ppc4xx/traps.c
+++ b/cpu/ppc4xx/traps.c
@@ -147,14 +147,15 @@ MachineCheckException(struct pt_regs *regs)
 	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;
 	}
 
@@ -214,14 +215,22 @@ MachineCheckException(struct pt_regs *regs)
 		printf("DDR0: At least one interrupt active\n");
 	if (val & 0x40)
 		printf("DDR0: DRAM initialization complete.\n");
-	if (val & 0x20)
+	if (val & 0x20) {
 		printf("DDR0: Multiple uncorrectable ECC events.\n");
-	if (val & 0x10)
+		uncorr_ecc = 1;
+	}
+	if (val & 0x10) {
 		printf("DDR0: Single uncorrectable ECC event.\n");
-	if (val & 0x08)
+		uncorr_ecc = 1;
+	}
+	if (val & 0x08) {
 		printf("DDR0: Multiple correctable ECC events.\n");
-	if (val & 0x04)
+		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");
@@ -252,11 +261,11 @@ MachineCheckException(struct pt_regs *regs)
 		printf("DDR0: No DDR0 error know 0x%x %p\n", val, value2);
 	}
 	mfsdram(DDR0_23, val);
-	if ( (val >> 16) & 0xff)
+	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)
+	if (((val >> 8) & 0xff) && uncorr_ecc)
 		printf("DDR0: Syndrome for uncorrectable ECC event 0x%x\n",
 		       (val >> 8) & 0xff);
 	mfsdram(DDR0_33, val);
@@ -264,28 +273,28 @@ MachineCheckException(struct pt_regs *regs)
 		printf("DDR0: Address of command that caused an "
 		       "Out-of-Range interrupt %p\n", val);
 	mfsdram(DDR0_34, val);
-	if (val)
+	if (val && uncorr_ecc)
 		printf("DDR0: Address of uncorrectable ECC event %p\n", val);
 	mfsdram(DDR0_35, val);
-	if (val)
+	if (val && uncorr_ecc)
 		printf("DDR0: Address of uncorrectable ECC event %p\n", val);
 	mfsdram(DDR0_36, val);
-	if (val)
+	if (val && uncorr_ecc)
 		printf("DDR0: Data of uncorrectable ECC event 0x%08x\n", val);
 	mfsdram(DDR0_37, val);
-	if (val)
+	if (val && uncorr_ecc)
 		printf("DDR0: Data of uncorrectable ECC event 0x%08x\n", val);
 	mfsdram(DDR0_38, val);
-	if (val)
+	if (val && corr_ecc)
 		printf("DDR0: Address of correctable ECC event %p\n", val);
 	mfsdram(DDR0_39, val);
-	if (val)
+	if (val && corr_ecc)
 		printf("DDR0: Address of correctable ECC event %p\n", val);
 	mfsdram(DDR0_40, val);
-	if (val)
+	if (val && corr_ecc)
 		printf("DDR0: Data of correctable ECC event 0x%08x\n", val);
 	mfsdram(DDR0_41, val);
-	if (val)
+	if (val && corr_ecc)
 		printf("DDR0: Data of correctable ECC event 0x%08x\n", val);
 #endif /* CONFIG_440EPX */
 #endif /* CONFIG_440 */
diff --git a/cpu/ppc4xx/usb.c b/cpu/ppc4xx/usb.c
index 2837b37c58..272ed8c15e 100644
--- a/cpu/ppc4xx/usb.c
+++ b/cpu/ppc4xx/usb.c
@@ -27,7 +27,7 @@
 
 #include "usbdev.h"
 
-int usb_cpu_init()
+int usb_cpu_init(void)
 {
 
 #if defined(CONFIG_440EP) || defined(CONFIG_440EPX)
@@ -37,12 +37,12 @@ int usb_cpu_init()
 	return 0;
 }
 
-int usb_cpu_stop()
+int usb_cpu_stop(void)
 {
 	return 0;
 }
 
-int usb_cpu_init_fail()
+int usb_cpu_init_fail(void)
 {
 	return 0;
 }
diff --git a/cpu/pxa/serial.c b/cpu/pxa/serial.c
index cb3a478990..51e7f65887 100644
--- a/cpu/pxa/serial.c
+++ b/cpu/pxa/serial.c
@@ -30,11 +30,28 @@
 
 #include <common.h>
 #include <watchdog.h>
+#include <serial.h>
 #include <asm/arch/pxa-regs.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
-void serial_setbrg (void)
+#define FFUART	0
+#define BTUART	1
+#define STUART	2
+
+#ifndef CONFIG_SERIAL_MULTI
+#if defined (CONFIG_FFUART)
+#define UART_INDEX	FFUART
+#elif defined (CONFIG_BTUART)
+#define UART_INDEX	BTUART
+#elif defined (CONFIG_STUART)
+#define UART_INDEX	STUART
+#else
+#error "Bad: you didn't configure serial ..."
+#endif
+#endif
+
+void pxa_setbrg_dev (unsigned int uart_index)
 {
 	unsigned int quot = 0;
 
@@ -53,63 +70,68 @@ void serial_setbrg (void)
 	else
 		hang ();
 
-#ifdef CONFIG_FFUART
+	switch (uart_index) {
+		case FFUART:
 #ifdef CONFIG_CPU_MONAHANS
-	CKENA |= CKENA_22_FFUART;
+			CKENA |= CKENA_22_FFUART;
 #else
-	CKEN |= CKEN6_FFUART;
+			CKEN |= CKEN6_FFUART;
 #endif /* CONFIG_CPU_MONAHANS */
 
-	FFIER = 0;					/* Disable for now */
-	FFFCR = 0;					/* No fifos enabled */
+			FFIER = 0;	/* Disable for now */
+			FFFCR = 0;	/* No fifos enabled */
 
-	/* set baud rate */
-	FFLCR = LCR_WLS0 | LCR_WLS1 | LCR_DLAB;
-	FFDLL = quot & 0xff;
-	FFDLH = quot >> 8;
-	FFLCR = LCR_WLS0 | LCR_WLS1;
+			/* set baud rate */
+			FFLCR = LCR_WLS0 | LCR_WLS1 | LCR_DLAB;
+			FFDLL = quot & 0xff;
+			FFDLH = quot >> 8;
+			FFLCR = LCR_WLS0 | LCR_WLS1;
 
-	FFIER = IER_UUE;			/* Enable FFUART */
+			FFIER = IER_UUE;	/* Enable FFUART */
+		break;
 
-#elif defined(CONFIG_BTUART)
+		case BTUART:
 #ifdef CONFIG_CPU_MONAHANS
-	CKENA |= CKENA_21_BTUART;
+			CKENA |= CKENA_21_BTUART;
 #else
-	CKEN |= CKEN7_BTUART;
+			CKEN |= CKEN7_BTUART;
 #endif /*  CONFIG_CPU_MONAHANS */
 
-	BTIER = 0;
-	BTFCR = 0;
+			BTIER = 0;
+			BTFCR = 0;
 
-	/* set baud rate */
-	BTLCR = LCR_DLAB;
-	BTDLL = quot & 0xff;
-	BTDLH = quot >> 8;
-	BTLCR = LCR_WLS0 | LCR_WLS1;
+			/* set baud rate */
+			BTLCR = LCR_DLAB;
+			BTDLL = quot & 0xff;
+			BTDLH = quot >> 8;
+			BTLCR = LCR_WLS0 | LCR_WLS1;
 
-	BTIER = IER_UUE;			/* Enable BFUART */
+			BTIER = IER_UUE;	/* Enable BFUART */
 
-#elif defined(CONFIG_STUART)
+		break;
+
+		case STUART:
 #ifdef CONFIG_CPU_MONAHANS
-	CKENA |= CKENA_23_STUART;
+			CKENA |= CKENA_23_STUART;
 #else
-	CKEN |= CKEN5_STUART;
+			CKEN |= CKEN5_STUART;
 #endif /* CONFIG_CPU_MONAHANS */
 
-	STIER = 0;
-	STFCR = 0;
+			STIER = 0;
+			STFCR = 0;
 
-	/* set baud rate */
-	STLCR = LCR_DLAB;
-	STDLL = quot & 0xff;
-	STDLH = quot >> 8;
-	STLCR = LCR_WLS0 | LCR_WLS1;
+			/* set baud rate */
+			STLCR = LCR_DLAB;
+			STDLL = quot & 0xff;
+			STDLH = quot >> 8;
+			STLCR = LCR_WLS0 | LCR_WLS1;
 
-	STIER = IER_UUE;			/* Enable STUART */
+			STIER = IER_UUE;			/* Enable STUART */
+			break;
 
-#else
-#error "Bad: you didn't configure serial ..."
-#endif
+		default:
+			hang();
+	}
 }
 
 
@@ -118,9 +140,9 @@ void serial_setbrg (void)
  * are always 8 data bits, no parity, 1 stop bit, no start bits.
  *
  */
-int serial_init (void)
+int pxa_init_dev (unsigned int uart_index)
 {
-	serial_setbrg ();
+	pxa_setbrg_dev (uart_index);
 
 	return (0);
 }
@@ -129,26 +151,32 @@ int serial_init (void)
 /*
  * Output a single byte to the serial port.
  */
-void serial_putc (const char c)
-{
-#ifdef CONFIG_FFUART
-	/* wait for room in the tx FIFO on FFUART */
-	while ((FFLSR & LSR_TEMT) == 0)
-		WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
-	FFTHR = c;
-#elif defined(CONFIG_BTUART)
-	while ((BTLSR & LSR_TEMT ) == 0 )
-		WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
-	BTTHR = c;
-#elif defined(CONFIG_STUART)
-	while ((STLSR & LSR_TEMT ) == 0 )
-		WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
-	STTHR = c;
-#endif
+void pxa_putc_dev (unsigned int uart_index,const char c)
+{
+	switch (uart_index) {
+		case FFUART:
+		/* wait for room in the tx FIFO on FFUART */
+			while ((FFLSR & LSR_TEMT) == 0)
+				WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
+			FFTHR = c;
+			break;
+
+		case BTUART:
+			while ((BTLSR & LSR_TEMT ) == 0 )
+				WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
+			BTTHR = c;
+			break;
+
+		case STUART:
+			while ((STLSR & LSR_TEMT ) == 0 )
+				WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
+			STTHR = c;
+			break;
+	}
 
 	/* If \n, also do \r */
 	if (c == '\n')
-		serial_putc ('\r');
+		pxa_putc_dev (uart_index,'\r');
 }
 
 /*
@@ -156,15 +184,17 @@ void serial_putc (const char c)
  * otherwise. When the function is succesfull, the character read is
  * written into its argument c.
  */
-int serial_tstc (void)
-{
-#ifdef CONFIG_FFUART
-	return FFLSR & LSR_DR;
-#elif defined(CONFIG_BTUART)
-	return BTLSR & LSR_DR;
-#elif defined(CONFIG_STUART)
-	return STLSR & LSR_DR;
-#endif
+int pxa_tstc_dev (unsigned int uart_index)
+{
+	switch (uart_index) {
+		case FFUART:
+			return FFLSR & LSR_DR;
+		case BTUART:
+			return BTLSR & LSR_DR;
+		case STUART:
+			return STLSR & LSR_DR;
+	}
+	return -1;
 }
 
 /*
@@ -172,27 +202,184 @@ int serial_tstc (void)
  * otherwise. When the function is succesfull, the character read is
  * written into its argument c.
  */
-int serial_getc (void)
-{
-#ifdef CONFIG_FFUART
-	while (!(FFLSR & LSR_DR))
-		WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
-	return (char) FFRBR & 0xff;
-#elif defined(CONFIG_BTUART)
-	while (!(BTLSR & LSR_DR))
-		WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
-	return (char) BTRBR & 0xff;
-#elif defined(CONFIG_STUART)
-	while (!(STLSR & LSR_DR))
-		WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
-	return (char) STRBR & 0xff;
-#endif
+int pxa_getc_dev (unsigned int uart_index)
+{
+	switch (uart_index) {
+		case FFUART:
+			while (!(FFLSR & LSR_DR))
+			WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
+			return (char) FFRBR & 0xff;
+
+		case BTUART:
+			while (!(BTLSR & LSR_DR))
+			WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
+			return (char) BTRBR & 0xff;
+		case STUART:
+			while (!(STLSR & LSR_DR))
+			WATCHDOG_RESET ();	/* Reset HW Watchdog, if needed */
+			return (char) STRBR & 0xff;
+	}
+	return -1;
 }
 
 void
-serial_puts (const char *s)
+pxa_puts_dev (unsigned int uart_index,const char *s)
 {
 	while (*s) {
-		serial_putc (*s++);
+		pxa_putc_dev (uart_index,*s++);
 	}
 }
+
+#if defined (CONFIG_FFUART)
+static int ffuart_init(void)
+{
+	return pxa_init_dev(FFUART);
+}
+
+static void ffuart_setbrg(void)
+{
+	return pxa_setbrg_dev(FFUART);
+}
+
+static void ffuart_putc(const char c)
+{
+	return pxa_putc_dev(FFUART,c);
+}
+
+static void ffuart_puts(const char *s)
+{
+	return pxa_puts_dev(FFUART,s);
+}
+
+static int ffuart_getc(void)
+{
+	return pxa_getc_dev(FFUART);
+}
+
+static int ffuart_tstc(void)
+{
+	return pxa_tstc_dev(FFUART);
+}
+
+struct serial_device serial_ffuart_device =
+{
+	"serial_ffuart",
+	"PXA",
+	ffuart_init,
+	ffuart_setbrg,
+	ffuart_getc,
+	ffuart_tstc,
+	ffuart_putc,
+	ffuart_puts,
+};
+#endif
+
+#if defined (CONFIG_BTUART)
+static int btuart_init(void)
+{
+	return pxa_init_dev(BTUART);
+}
+
+static void btuart_setbrg(void)
+{
+	return pxa_setbrg_dev(BTUART);
+}
+
+static void btuart_putc(const char c)
+{
+	return pxa_putc_dev(BTUART,c);
+}
+
+static void btuart_puts(const char *s)
+{
+	return pxa_puts_dev(BTUART,s);
+}
+
+static int btuart_getc(void)
+{
+	return pxa_getc_dev(BTUART);
+}
+
+static int btuart_tstc(void)
+{
+	return pxa_tstc_dev(BTUART);
+}
+
+struct serial_device serial_btuart_device =
+{
+	"serial_btuart",
+	"PXA",
+	btuart_init,
+	btuart_setbrg,
+	btuart_getc,
+	btuart_tstc,
+	btuart_putc,
+	btuart_puts,
+};
+#endif
+
+#if defined (CONFIG_STUART)
+static int stuart_init(void)
+{
+	return pxa_init_dev(STUART);
+}
+
+static void stuart_setbrg(void)
+{
+	return pxa_setbrg_dev(STUART);
+}
+
+static void stuart_putc(const char c)
+{
+	return pxa_putc_dev(STUART,c);
+}
+
+static void stuart_puts(const char *s)
+{
+	return pxa_puts_dev(STUART,s);
+}
+
+static int stuart_getc(void)
+{
+	return pxa_getc_dev(STUART);
+}
+
+static int stuart_tstc(void)
+{
+	return pxa_tstc_dev(STUART);
+}
+
+struct serial_device serial_stuart_device =
+{
+	"serial_stuart",
+	"PXA",
+	stuart_init,
+	stuart_setbrg,
+	stuart_getc,
+	stuart_tstc,
+	stuart_putc,
+	stuart_puts,
+};
+#endif
+
+
+#ifndef CONFIG_SERIAL_MULTI
+inline int serial_init(void) {
+	return (pxa_init_dev(UART_INDEX));
+}
+void serial_setbrg(void) {
+	pxa_setbrg_dev(UART_INDEX);
+}
+int serial_getc(void) {
+	return(pxa_getc_dev(UART_INDEX));
+}
+int serial_tstc(void) {
+	return(pxa_tstc_dev(UART_INDEX));
+}
+void serial_putc(const char c) {
+	pxa_putc_dev(UART_INDEX,c);
+}
+void serial_puts(const char *s) {
+	pxa_puts_dev(UART_INDEX,s);
+}
+#endif	/* CONFIG_SERIAL_MULTI */