Sync'd u-boot-net with mainline

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

Conflicts:

	drivers/bcm570x.c
	drivers/tigon3.c

145 files changed, 9334 insertions, 3153 deletions

diff --git a/cpu/74xx_7xx/kgdb.S b/cpu/74xx_7xx/kgdb.S
index e838513c1d..4f231228c2 100644
--- a/cpu/74xx_7xx/kgdb.S
+++ b/cpu/74xx_7xx/kgdb.S
@@ -31,7 +31,7 @@
 #include <asm/cache.h>
 #include <asm/mmu.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 
  /*
  * cache flushing routines for kgdb
@@ -74,4 +74,4 @@ kgdb_flush_cache_range:
 	SYNC
 	blr
 
-#endif /* CFG_CMD_KGDB */
+#endif
diff --git a/cpu/74xx_7xx/start.S b/cpu/74xx_7xx/start.S
index 11430388f5..b5834b91e3 100644
--- a/cpu/74xx_7xx/start.S
+++ b/cpu/74xx_7xx/start.S
@@ -125,7 +125,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -143,7 +143,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
diff --git a/cpu/74xx_7xx/traps.c b/cpu/74xx_7xx/traps.c
index 50c5eeb483..b066227696 100644
--- a/cpu/74xx_7xx/traps.c
+++ b/cpu/74xx_7xx/traps.c
@@ -40,7 +40,7 @@
 DECLARE_GLOBAL_DATA_PTR;
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
@@ -133,7 +133,7 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -166,7 +166,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -181,7 +181,7 @@ ProgramCheckException(struct pt_regs *regs)
 	unsigned char *p = regs ? (unsigned char *)(regs->nip) : NULL;
 	int i, j;
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -204,7 +204,7 @@ ProgramCheckException(struct pt_regs *regs)
 void
 SoftEmuException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -217,7 +217,7 @@ SoftEmuException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
diff --git a/cpu/arm720t/lpc2292/Makefile b/cpu/arm720t/lpc2292/Makefile
new file mode 100644
index 0000000000..240f1e3b3b
--- /dev/null
+++ b/cpu/arm720t/lpc2292/Makefile
@@ -0,0 +1,50 @@
+#
+# (C) Copyright 2000-2007
+# 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
+
+LIB	= $(obj)lib$(SOC).a
+
+COBJS	= flash.o mmc.o mmc_hw.o spi.o
+SOBJS	= $(obj)iap_entry.o
+
+SRCS	:= $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS))
+
+all:	$(obj).depend $(LIB)
+
+$(LIB):	$(OBJS) $(SOBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS) $(SOBJS)
+
+# this MUST be compiled as thumb code!
+$(SOBJS):
+	$(CC) $(AFLAGS) -march=armv4t -c -o $(SOBJS) iap_entry.S
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/arm720t/lpc2292/flash.c b/cpu/arm720t/lpc2292/flash.c
new file mode 100644
index 0000000000..e5c869722d
--- /dev/null
+++ b/cpu/arm720t/lpc2292/flash.c
@@ -0,0 +1,249 @@
+/*
+ * (C) Copyright 2006 Embedded Artists AB <www.embeddedartists.com>
+ *
+ * Modified to remove all but the IAP-command related code by
+ * Gary Jennejohn <garyj@denx.de>
+ *
+ * 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/arch/hardware.h>
+
+/* IAP commands use 32 bytes at the top of CPU internal sram, we
+   use 512 bytes below that */
+#define COPY_BUFFER_LOCATION 0x40003de0
+
+#define IAP_LOCATION 0x7ffffff1
+#define IAP_CMD_PREPARE 50
+#define IAP_CMD_COPY 51
+#define IAP_CMD_ERASE 52
+#define IAP_CMD_CHECK 53
+#define IAP_CMD_ID 54
+#define IAP_CMD_VERSION 55
+#define IAP_CMD_COMPARE 56
+
+#define IAP_RET_CMD_SUCCESS 0
+
+static unsigned long command[5];
+static unsigned long result[2];
+
+extern void iap_entry(unsigned long * command, unsigned long * result);
+
+/*-----------------------------------------------------------------------
+ *
+ */
+static int get_flash_sector(flash_info_t * info, ulong flash_addr)
+{
+	int i;
+
+	for(i = 1; i < (info->sector_count); i++) {
+		if (flash_addr < (info->start[i]))
+			break;
+	}
+
+	return (i-1);
+}
+
+/*-----------------------------------------------------------------------
+ * This function assumes that flash_addr is aligned on 512 bytes boundary
+ * in flash. This function also assumes that prepare have been called
+ * for the sector in question.
+ */
+int lpc2292_copy_buffer_to_flash(flash_info_t * info, ulong flash_addr)
+{
+	int first_sector;
+	int last_sector;
+
+	first_sector = get_flash_sector(info, flash_addr);
+	last_sector = get_flash_sector(info, flash_addr + 512 - 1);
+
+	/* prepare sectors for write */
+	command[0] = IAP_CMD_PREPARE;
+	command[1] = first_sector;
+	command[2] = last_sector;
+	iap_entry(command, result);
+	if (result[0] != IAP_RET_CMD_SUCCESS) {
+		printf("IAP prepare failed\n");
+		return ERR_PROG_ERROR;
+	}
+
+	command[0] = IAP_CMD_COPY;
+	command[1] = flash_addr;
+	command[2] = COPY_BUFFER_LOCATION;
+	command[3] = 512;
+	command[4] = CFG_SYS_CLK_FREQ >> 10;
+	iap_entry(command, result);
+	if (result[0] != IAP_RET_CMD_SUCCESS) {
+		printf("IAP copy failed\n");
+		return 1;
+	}
+
+	return 0;
+}
+
+/*-----------------------------------------------------------------------
+ */
+
+int lpc2292_flash_erase (flash_info_t * info, int s_first, int s_last)
+{
+	int flag;
+	int prot;
+	int sect;
+
+	prot = 0;
+	for (sect = s_first; sect <= s_last; ++sect) {
+		if (info->protect[sect]) {
+			prot++;
+		}
+	}
+	if (prot)
+		return ERR_PROTECTED;
+
+
+	flag = disable_interrupts();
+
+	printf ("Erasing %d sectors starting at sector %2d.\n"
+	"This make take some time ... ",
+	s_last - s_first + 1, s_first);
+
+	command[0] = IAP_CMD_PREPARE;
+	command[1] = s_first;
+	command[2] = s_last;
+	iap_entry(command, result);
+	if (result[0] != IAP_RET_CMD_SUCCESS) {
+		printf("IAP prepare failed\n");
+		return ERR_PROTECTED;
+	}
+
+	command[0] = IAP_CMD_ERASE;
+	command[1] = s_first;
+	command[2] = s_last;
+	command[3] = CFG_SYS_CLK_FREQ >> 10;
+	iap_entry(command, result);
+	if (result[0] != IAP_RET_CMD_SUCCESS) {
+		printf("IAP erase failed\n");
+		return ERR_PROTECTED;
+	}
+
+	if (flag)
+		enable_interrupts();
+
+	return ERR_OK;
+}
+
+int lpc2292_write_buff (flash_info_t * info, uchar * src, ulong addr,
+			ulong cnt)
+{
+	int first_copy_size;
+	int last_copy_size;
+	int first_block;
+	int last_block;
+	int nbr_mid_blocks;
+	uchar memmap_value;
+	ulong i;
+	uchar* src_org;
+	uchar* dst_org;
+	int ret = ERR_OK;
+
+	src_org = src;
+	dst_org = (uchar*)addr;
+
+	first_block = addr / 512;
+	last_block = (addr + cnt) / 512;
+	nbr_mid_blocks = last_block - first_block - 1;
+
+	first_copy_size = 512 - (addr % 512);
+	last_copy_size = (addr + cnt) % 512;
+
+	debug("\ncopy first block: (1) %lX -> %lX 0x200 bytes, "
+		"(2) %lX -> %lX 0x%X bytes, (3) %lX -> %lX 0x200 bytes\n",
+	(ulong)(first_block * 512),
+	(ulong)COPY_BUFFER_LOCATION,
+	(ulong)src,
+	(ulong)(COPY_BUFFER_LOCATION + 512 - first_copy_size),
+	first_copy_size,
+	(ulong)COPY_BUFFER_LOCATION,
+	(ulong)(first_block * 512));
+
+	/* copy first block */
+	memcpy((void*)COPY_BUFFER_LOCATION,
+		(void*)(first_block * 512), 512);
+	memcpy((void*)(COPY_BUFFER_LOCATION + 512 - first_copy_size),
+		src, first_copy_size);
+	lpc2292_copy_buffer_to_flash(info, first_block * 512);
+	src += first_copy_size;
+	addr += first_copy_size;
+
+	/* copy middle blocks */
+	for (i = 0; i < nbr_mid_blocks; i++) {
+		debug("copy middle block: %lX -> %lX 512 bytes, "
+		"%lX -> %lX 512 bytes\n",
+		(ulong)src,
+		(ulong)COPY_BUFFER_LOCATION,
+		(ulong)COPY_BUFFER_LOCATION,
+		(ulong)addr);
+
+		memcpy((void*)COPY_BUFFER_LOCATION, src, 512);
+		lpc2292_copy_buffer_to_flash(info, addr);
+		src += 512;
+		addr += 512;
+	}
+
+
+	if (last_copy_size > 0) {
+		debug("copy last block: (1) %lX -> %lX 0x200 bytes, "
+		"(2) %lX -> %lX 0x%X bytes, (3) %lX -> %lX x200 bytes\n",
+		(ulong)(last_block * 512),
+		(ulong)COPY_BUFFER_LOCATION,
+		(ulong)src,
+		(ulong)(COPY_BUFFER_LOCATION),
+		last_copy_size,
+		(ulong)COPY_BUFFER_LOCATION,
+		(ulong)addr);
+
+		/* copy last block */
+		memcpy((void*)COPY_BUFFER_LOCATION,
+			(void*)(last_block * 512), 512);
+		memcpy((void*)COPY_BUFFER_LOCATION,
+			src, last_copy_size);
+		lpc2292_copy_buffer_to_flash(info, addr);
+	}
+
+	/* verify write */
+	memmap_value = GET8(MEMMAP);
+
+	disable_interrupts();
+
+	PUT8(MEMMAP, 01);		/* we must make sure that initial 64
+							   bytes are taken from flash when we
+							   do the compare */
+
+	for (i = 0; i < cnt; i++) {
+		if (*dst_org != *src_org){
+			printf("Write failed. Byte %lX differs\n", i);
+			ret = ERR_PROG_ERROR;
+			break;
+		}
+		dst_org++;
+		src_org++;
+	}
+
+	PUT8(MEMMAP, memmap_value);
+	enable_interrupts();
+
+	return ret;
+}
diff --git a/cpu/arm720t/lpc2292/iap_entry.S b/cpu/arm720t/lpc2292/iap_entry.S
new file mode 100644
index 0000000000..c31d5190bd
--- /dev/null
+++ b/cpu/arm720t/lpc2292/iap_entry.S
@@ -0,0 +1,7 @@
+IAP_ADDRESS:	.word	0x7FFFFFF1
+
+.globl iap_entry
+iap_entry:
+	ldr	r2, IAP_ADDRESS
+	bx	r2
+	mov	pc, lr
diff --git a/cpu/arm720t/lpc2292/mmc.c b/cpu/arm720t/lpc2292/mmc.c
new file mode 100644
index 0000000000..fd7f149b66
--- /dev/null
+++ b/cpu/arm720t/lpc2292/mmc.c
@@ -0,0 +1,157 @@
+/*
+ * 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 <mmc.h>
+#include <asm/errno.h>
+#include <asm/arch/hardware.h>
+#include <part.h>
+#include <fat.h>
+#include "mmc_hw.h"
+#include <asm/arch/spi.h>
+
+#ifdef CONFIG_MMC
+
+#undef MMC_DEBUG
+
+static block_dev_desc_t mmc_dev;
+
+/* these are filled out by a call to mmc_hw_get_parameters */
+static int hw_size;		/* in kbytes */
+static int hw_nr_sects;
+static int hw_sect_size;	/* in bytes */
+
+block_dev_desc_t * mmc_get_dev(int dev)
+{
+	return (block_dev_desc_t *)(&mmc_dev);
+}
+
+unsigned long mmc_block_read(int dev,
+			     unsigned long start,
+			     lbaint_t blkcnt,
+			     void *buffer)
+{
+	unsigned long rc = 0;
+	unsigned char *p = (unsigned char *)buffer;
+	unsigned long i;
+	unsigned long addr = start;
+
+#ifdef MMC_DEBUG
+	printf("mmc_block_read: start=%lu, blkcnt=%lu\n", start,
+		 (unsigned long)blkcnt);
+#endif
+
+	for(i = 0; i < (unsigned long)blkcnt; i++) {
+#ifdef MMC_DEBUG
+		printf("mmc_read_sector: addr=%lu, buffer=%p\n", addr, p);
+#endif
+		(void)mmc_read_sector(addr, p);
+		rc++;
+		addr++;
+		p += hw_sect_size;
+	}
+
+	return rc;
+}
+
+/*-----------------------------------------------------------------------------
+ * Read hardware paramterers (sector size, size, number of sectors)
+ */
+static int mmc_hw_get_parameters(void)
+{
+	unsigned char csddata[16];
+	unsigned int sizemult;
+	unsigned int size;
+
+	mmc_read_csd(csddata);
+	hw_sect_size = 1<<(csddata[5] & 0x0f);
+	size = ((csddata[6]&0x03)<<10)+(csddata[7]<<2)+(csddata[8]&0xc0);
+	sizemult = ((csddata[10] & 0x80)>>7)+((csddata[9] & 0x03)<<1);
+	hw_nr_sects = (size+1)*(1<<(sizemult+2));
+	hw_size = hw_nr_sects*hw_sect_size/1024;
+
+#ifdef MMC_DEBUG
+	printf("mmc_hw_get_parameters: hw_sect_size=%d, hw_nr_sects=%d, "
+		 "hw_size=%d\n", hw_sect_size, hw_nr_sects, hw_size);
+#endif
+
+	return 0;
+}
+
+int mmc_init(int verbose)
+{
+	int ret = -ENODEV;
+
+	if (verbose)
+		printf("mmc_init\n");
+
+	spi_init();
+	/* this meeds to be done twice */
+	mmc_hw_init();
+	udelay(1000);
+	mmc_hw_init();
+
+	mmc_hw_get_parameters();
+
+	mmc_dev.if_type = IF_TYPE_MMC;
+	mmc_dev.part_type = PART_TYPE_DOS;
+	mmc_dev.dev = 0;
+	mmc_dev.lun = 0;
+	mmc_dev.type = 0;
+	mmc_dev.blksz = hw_sect_size;
+	mmc_dev.lba = hw_nr_sects;
+	sprintf((char*)mmc_dev.vendor, "Unknown vendor");
+	sprintf((char*)mmc_dev.product, "Unknown product");
+	sprintf((char*)mmc_dev.revision, "N/A");
+	mmc_dev.removable = 0;	/* should be true??? */
+	mmc_dev.block_read = mmc_block_read;
+
+	fat_register_device(&mmc_dev, 1);
+
+	ret = 0;
+
+	return ret;
+}
+
+int mmc_write(uchar * src, ulong dst, int size)
+{
+#ifdef MMC_DEBUG
+	printf("mmc_write: src=%p, dst=%lu, size=%u\n", src, dst, size);
+#endif
+	/* Since mmc2info always returns 0 this function will never be called */
+	return 0;
+}
+
+int mmc_read(ulong src, uchar * dst, int size)
+{
+#ifdef MMC_DEBUG
+	printf("mmc_read: src=%lu, dst=%p, size=%u\n", src, dst, size);
+#endif
+	/* Since mmc2info always returns 0 this function will never be called */
+	return 0;
+}
+
+int mmc2info(ulong addr)
+{
+	/* This function is used by cmd_cp to determine if source or destination
+	 address resides on MMC-card or not. We do not support copy to and from
+	 MMC-card so we always return 0. */
+	return 0;
+}
+
+#endif /* CONFIG_MMC */
diff --git a/cpu/arm720t/lpc2292/mmc_hw.c b/cpu/arm720t/lpc2292/mmc_hw.c
new file mode 100644
index 0000000000..b4dc4a6e2f
--- /dev/null
+++ b/cpu/arm720t/lpc2292/mmc_hw.c
@@ -0,0 +1,233 @@
+/*
+    This code was original written by Ulrich Radig and modified by
+    Embedded Artists AB (www.embeddedartists.com).
+
+    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/arch/hardware.h>
+#include <asm/arch/spi.h>
+
+#define MMC_Enable() PUT32(IO1CLR, 1l << 22)
+#define MMC_Disable() PUT32(IO1SET, 1l << 22)
+#define mmc_spi_cfg() spi_set_clock(8); spi_set_cfg(0, 1, 0);
+
+static unsigned char Write_Command_MMC (unsigned char *CMD);
+static void MMC_Read_Block(unsigned char *CMD, unsigned char *Buffer,
+		    unsigned short int Bytes);
+
+/* initialize the hardware */
+int mmc_hw_init(void)
+{
+	unsigned long a;
+	unsigned short int Timeout = 0;
+	unsigned char b;
+	unsigned char CMD[] = {0x40, 0x00, 0x00, 0x00, 0x00, 0x95};
+
+	/* set-up GPIO and SPI */
+	(*((volatile unsigned long *)PINSEL2)) &= ~(1l << 3); /* clear bit 3 */
+	(*((volatile unsigned long *)IO1DIR)) |= (1l << 22); /* set bit 22 (output) */
+
+	MMC_Disable();
+
+	spi_lock();
+	spi_set_clock(248);
+	spi_set_cfg(0, 1, 0);
+	MMC_Enable();
+
+	/* waste some time */
+	for(a=0; a < 20000; a++)
+		asm("nop");
+
+	/* Put the MMC/SD-card into SPI-mode */
+	for (b = 0; b < 10; b++) /* Sends min 74+ clocks to the MMC/SD-card */
+		spi_write(0xff);
+
+	/* Sends command CMD0 to MMC/SD-card */
+	while (Write_Command_MMC(CMD) != 1) {
+		if (Timeout++ > 200) {
+			MMC_Disable();
+			spi_unlock();
+			return(1); /* Abort with command 1 (return 1) */
+		}
+	}
+	/* Sends Command CMD1 an MMC/SD-card */
+	Timeout = 0;
+	CMD[0] = 0x41;/* Command 1 */
+	CMD[5] = 0xFF;
+
+	while (Write_Command_MMC(CMD) != 0) {
+		if (Timeout++ > 200) {
+			MMC_Disable();
+			spi_unlock();
+			return (2); /* Abort with command 2 (return 2) */
+		}
+	}
+
+	MMC_Disable();
+	spi_unlock();
+
+	return 0;
+}
+
+/* ############################################################################
+   Sends a command to the MMC/SD-card
+   ######################################################################### */
+static unsigned char Write_Command_MMC (unsigned char *CMD)
+{
+	unsigned char a, tmp = 0xff;
+	unsigned short int Timeout = 0;
+
+	MMC_Disable();
+	spi_write(0xFF);
+	MMC_Enable();
+
+	for (a = 0; a < 0x06; a++)
+		spi_write(*CMD++);
+
+	while (tmp == 0xff) {
+		tmp = spi_read();
+		if (Timeout++ > 5000)
+		  break;
+	}
+
+	return (tmp);
+}
+
+/* ############################################################################
+   Routine to read the CID register from the MMC/SD-card (16 bytes)
+   ######################################################################### */
+void MMC_Read_Block(unsigned char *CMD, unsigned char *Buffer, unsigned short
+	int Bytes)
+{
+	unsigned short int a;
+
+	spi_lock();
+	mmc_spi_cfg();
+	MMC_Enable();
+
+	if (Write_Command_MMC(CMD) != 0) {
+		MMC_Disable();
+		spi_unlock();
+		return;
+	}
+
+	while (spi_read() != 0xfe) {};
+	for (a = 0; a < Bytes; a++)
+		*Buffer++ = spi_read();
+
+	/* Read the CRC-byte */
+	spi_read(); /* CRC - byte is discarded */
+	spi_read(); /* CRC - byte is discarded */
+	/* set MMC_Chip_Select to high (MMC/SD-card Inaktiv) */
+	MMC_Disable();
+	spi_unlock();
+
+	return;
+}
+
+/* ############################################################################
+   Routine to read a block (512 bytes) from the MMC/SD-card
+   ######################################################################### */
+unsigned char mmc_read_sector (unsigned long addr,unsigned char *Buffer)
+{
+	/* Command 16 to read aBlocks from the MMC/SD - caed */
+	unsigned char CMD[] = {0x51,0x00,0x00,0x00,0x00,0xFF};
+
+	/* The addres on the MMC/SD-card is in bytes,
+	addr is transformed from blocks to bytes and the result is
+	placed into the command */
+
+	addr = addr << 9; /* addr = addr * 512 */
+
+	CMD[1] = ((addr & 0xFF000000) >> 24);
+	CMD[2] = ((addr & 0x00FF0000) >> 16);
+	CMD[3] = ((addr & 0x0000FF00) >> 8 );
+
+	MMC_Read_Block(CMD, Buffer, 512);
+
+	return (0);
+}
+
+/* ############################################################################
+   Routine to write a block (512 byte) to the MMC/SD-card
+   ######################################################################### */
+unsigned char mmc_write_sector (unsigned long addr,unsigned char *Buffer)
+{
+	unsigned char tmp, a;
+	unsigned short int b;
+	/* Command 24 to write a block to the MMC/SD - card */
+	unsigned char CMD[] = {0x58, 0x00, 0x00, 0x00, 0x00, 0xFF};
+
+	/* The addres on the MMC/SD-card is in bytes,
+	addr is transformed from blocks to bytes and the result is
+	placed into the command */
+
+	addr = addr << 9; /* addr = addr * 512 */
+
+	CMD[1] = ((addr & 0xFF000000) >> 24);
+	CMD[2] = ((addr & 0x00FF0000) >> 16);
+	CMD[3] = ((addr & 0x0000FF00) >> 8 );
+
+	spi_lock();
+	mmc_spi_cfg();
+	MMC_Enable();
+
+	/* Send command CMD24 to the MMC/SD-card (Write 1 Block/512 Bytes) */
+	tmp = Write_Command_MMC(CMD);
+	if (tmp != 0) {
+		MMC_Disable();
+		spi_unlock();
+		return(tmp);
+	}
+
+	/* Do a short delay and send a clock-pulse to the MMC/SD-card */
+	for (a = 0; a < 100; a++)
+		spi_read();
+
+	/* Send a start byte to the MMC/SD-card */
+	spi_write(0xFE);
+
+	/* Write the block (512 bytes) to the MMC/SD-card */
+	for (b = 0; b < 512; b++)
+		spi_write(*Buffer++);
+
+	/* write the CRC-Byte */
+	spi_write(0xFF); /* write a dummy CRC */
+	spi_write(0xFF); /* CRC code is not used */
+
+	/* Wait for MMC/SD-card busy */
+	while (spi_read() != 0xff) {};
+
+	/* set MMC_Chip_Select to high (MMC/SD-card inactive) */
+	MMC_Disable();
+	spi_unlock();
+	return (0);
+}
+
+/* #########################################################################
+   Routine to read the CSD register from the MMC/SD-card (16 bytes)
+   ######################################################################### */
+unsigned char mmc_read_csd (unsigned char *Buffer)
+{
+	/* Command to read the CSD register */
+	unsigned char CMD[] = {0x49, 0x00, 0x00, 0x00, 0x00, 0xFF};
+
+	MMC_Read_Block(CMD, Buffer, 16);
+
+	return (0);
+}
diff --git a/cpu/arm720t/lpc2292/mmc_hw.h b/cpu/arm720t/lpc2292/mmc_hw.h
new file mode 100644
index 0000000000..3687dbf696
--- /dev/null
+++ b/cpu/arm720t/lpc2292/mmc_hw.h
@@ -0,0 +1,29 @@
+/*
+    This module implements a linux character device driver for the 24c256 chip.
+    Copyright (C) 2006  Embedded Artists AB (www.embeddedartists.com)
+
+    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
+*/
+
+#ifndef _MMC_HW_
+#define _MMC_HW_
+
+unsigned char mmc_read_csd(unsigned char *Buffer);
+unsigned char mmc_read_sector (unsigned long addr,
+			       unsigned char *Buffer);
+unsigned char mmc_write_sector (unsigned long addr,unsigned char *Buffer);
+int mmc_hw_init(void);
+
+#endif /* _MMC_HW_ */
diff --git a/cpu/arm720t/lpc2292/spi.c b/cpu/arm720t/lpc2292/spi.c
new file mode 100644
index 0000000000..d296bdac68
--- /dev/null
+++ b/cpu/arm720t/lpc2292/spi.c
@@ -0,0 +1,40 @@
+/*
+    This module implements an interface to the SPI on the lpc22xx.
+    Copyright (C) 2006  Embedded Artists AB (www.embeddedartists.com)
+
+    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/errno.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/spi.h>
+
+unsigned long spi_flags;
+unsigned char spi_idle = 0x00;
+
+int spi_init(void)
+{
+	unsigned long pinsel0_value;
+
+	/* activate spi pins */
+	pinsel0_value = GET32(PINSEL0);
+	pinsel0_value &= ~(0xFFl << 8);
+	pinsel0_value |= (0x55l << 8);
+	PUT32(PINSEL0, pinsel0_value);
+
+	return 0;
+}
diff --git a/cpu/arm920t/at91rm9200/Makefile b/cpu/arm920t/at91rm9200/Makefile
index 8d4e478fb5..eaabad26a0 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_ohci.o
+	  lxt972.o serial.o usb.o
 SOBJS	= lowlevel_init.o
 
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
diff --git a/cpu/arm920t/at91rm9200/bcm5221.c b/cpu/arm920t/at91rm9200/bcm5221.c
index 6db143562c..b52c615864 100644
--- a/cpu/arm920t/at91rm9200/bcm5221.c
+++ b/cpu/arm920t/at91rm9200/bcm5221.c
@@ -32,7 +32,7 @@
 
 #ifdef CONFIG_DRIVER_ETHER
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 /*
  * Name:
@@ -227,6 +227,6 @@ unsigned char bcm5221_AutoNegotiate (AT91PS_EMAC p_mac, int *status)
 	return FALSE;
 }
 
-#endif	/* CONFIG_COMMANDS & CFG_CMD_NET */
+#endif
 
 #endif	/* CONFIG_DRIVER_ETHER */
diff --git a/cpu/arm920t/at91rm9200/dm9161.c b/cpu/arm920t/at91rm9200/dm9161.c
index 4b13c237c7..968f653081 100644
--- a/cpu/arm920t/at91rm9200/dm9161.c
+++ b/cpu/arm920t/at91rm9200/dm9161.c
@@ -27,7 +27,7 @@
 
 #ifdef CONFIG_DRIVER_ETHER
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 /*
  * Name:
@@ -220,6 +220,6 @@ UCHAR dm9161_AutoNegotiate (AT91PS_EMAC p_mac, int *status)
 	return FALSE;
 }
 
-#endif	/* CONFIG_COMMANDS & CFG_CMD_NET */
+#endif
 
 #endif	/* CONFIG_DRIVER_ETHER */
diff --git a/cpu/arm920t/at91rm9200/ether.c b/cpu/arm920t/at91rm9200/ether.c
index 67008d0b91..c8f56aa52d 100644
--- a/cpu/arm920t/at91rm9200/ether.c
+++ b/cpu/arm920t/at91rm9200/ether.c
@@ -50,7 +50,7 @@ typedef struct {
 
 #ifdef CONFIG_DRIVER_ETHER
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 /* alignment as per Errata #11 (64 bytes) is insufficient! */
 rbf_t rbfdt[RBF_FRAMEMAX] __attribute((aligned(512)));
@@ -265,7 +265,7 @@ void eth_halt (void)
 {
 };
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 int  at91rm9200_miiphy_read(char *devname, unsigned char addr,
 		unsigned char reg, unsigned short * value)
 {
@@ -284,16 +284,16 @@ int  at91rm9200_miiphy_write(char *devname, unsigned char addr,
 	return 0;
 }
 
-#endif	/* defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII) */
+#endif
 
 int at91rm9200_miiphy_initialize(bd_t *bis)
 {
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 	miiphy_register("at91rm9200phy", at91rm9200_miiphy_read, at91rm9200_miiphy_write);
 #endif
 	return 0;
 }
 
-#endif	/* CONFIG_COMMANDS & CFG_CMD_NET */
+#endif
 
 #endif	/* CONFIG_DRIVER_ETHER */
diff --git a/cpu/arm920t/at91rm9200/lxt972.c b/cpu/arm920t/at91rm9200/lxt972.c
index f12c59c158..4edcc9a1b9 100644
--- a/cpu/arm920t/at91rm9200/lxt972.c
+++ b/cpu/arm920t/at91rm9200/lxt972.c
@@ -33,7 +33,7 @@
 
 #ifdef CONFIG_DRIVER_ETHER
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 /*
  * Name:
@@ -186,6 +186,6 @@ UCHAR lxt972_AutoNegotiate (AT91PS_EMAC p_mac, int *status)
 	return (lxt972_GetLinkSpeed (p_mac));
 }
 
-#endif	/* CONFIG_COMMANDS & CFG_CMD_NET */
+#endif
 
 #endif	/* CONFIG_DRIVER_ETHER */
diff --git a/cpu/arm920t/at91rm9200/usb.c b/cpu/arm920t/at91rm9200/usb.c
new file mode 100644
index 0000000000..366262e4cc
--- /dev/null
+++ b/cpu/arm920t/at91rm9200/usb.c
@@ -0,0 +1,53 @@
+/*
+ * (C) Copyright 2006
+ * DENX Software Engineering <mk@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#if defined(CONFIG_USB_OHCI_NEW) && defined(CFG_USB_OHCI_CPU_INIT)
+# ifdef CONFIG_AT91RM9200
+
+#include <asm/arch/hardware.h>
+
+int usb_cpu_init()
+{
+	/* Enable USB host clock. */
+	*AT91C_PMC_SCER = AT91C_PMC_UHP;	/* 48MHz clock enabled for UHP */
+	*AT91C_PMC_PCER = 1 << AT91C_ID_UHP;	/* Peripheral Clock Enable Register */
+	return 0;
+}
+
+int usb_cpu_stop()
+{
+	/* Initialization failed */
+	*AT91C_PMC_PCDR = 1 << AT91C_ID_UHP;	/* Peripheral Clock Disable Register */
+	*AT91C_PMC_SCDR = AT91C_PMC_UHP;	/* 48MHz clock disabled for UHP */
+	return 0;
+}
+
+int usb_cpu_init_fail()
+{
+	usb_cpu_stop();
+}
+
+# endif /* CONFIG_AT91RM9200 */
+#endif /* defined(CONFIG_USB_OHCI) && defined(CFG_USB_OHCI_CPU_INIT) */
diff --git a/cpu/arm920t/at91rm9200/usb_ohci.c b/cpu/arm920t/at91rm9200/usb_ohci.c
deleted file mode 100644
index 5b2c56cffc..0000000000
--- a/cpu/arm920t/at91rm9200/usb_ohci.c
+++ /dev/null
@@ -1,1635 +0,0 @@
-/*
- * URB OHCI HCD (Host Controller Driver) for USB on the AT91RM9200.
- *
- * (C) Copyright 2003
- * Gary Jennejohn, DENX Software Engineering <gj@denx.de>
- *
- * Note: Much of this code has been derived from Linux 2.4
- * (C) Copyright 1999 Roman Weissgaerber <weissg@vienna.at>
- * (C) Copyright 2000-2002 David Brownell
- *
- * Modified for the MP2USB by (C) Copyright 2005 Eric Benard
- * ebenard@eukrea.com - based on s3c24x0's driver
- *
- * 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
- *
- */
-/*
- * IMPORTANT NOTES
- * 1 - you MUST define LITTLEENDIAN in the configuration file for the
- *     board or this driver will NOT work!
- * 2 - this driver is intended for use with USB Mass Storage Devices
- *     (BBB) ONLY. There is NO support for Interrupt or Isochronous pipes!
- * 3 - when running on a PQFP208 AT91RM9200, define CONFIG_AT91C_PQFP_UHPBUG
- *     to activate workaround for bug #41 or this driver will NOT work!
- */
-
-#include <common.h>
-/* #include <pci.h> no PCI on the S3C24X0 */
-
-#ifdef CONFIG_USB_OHCI
-
-#include <asm/arch/hardware.h>
-
-#include <malloc.h>
-#include <usb.h>
-#include "usb_ohci.h"
-
-#define OHCI_USE_NPS		/* force NoPowerSwitching mode */
-#undef OHCI_VERBOSE_DEBUG	/* not always helpful */
-
-/* For initializing controller (mask in an HCFS mode too) */
-#define OHCI_CONTROL_INIT \
-	(OHCI_CTRL_CBSR & 0x3) | OHCI_CTRL_IE | OHCI_CTRL_PLE
-
-#define readl(a) (*((vu_long *)(a)))
-#define writel(a, b) (*((vu_long *)(b)) = ((vu_long)a))
-
-#define min_t(type,x,y) ({ type __x = (x); type __y = (y); __x < __y ? __x: __y; })
-
-#undef DEBUG
-#ifdef DEBUG
-#define dbg(format, arg...) printf("DEBUG: " format "\n", ## arg)
-#else
-#define dbg(format, arg...) do {} while(0)
-#endif /* DEBUG */
-#define err(format, arg...) printf("ERROR: " format "\n", ## arg)
-#undef SHOW_INFO
-#ifdef SHOW_INFO
-#define info(format, arg...) printf("INFO: " format "\n", ## arg)
-#else
-#define info(format, arg...) do {} while(0)
-#endif
-
-#define m16_swap(x) swap_16(x)
-#define m32_swap(x) swap_32(x)
-
-/* global ohci_t */
-static ohci_t gohci;
-/* this must be aligned to a 256 byte boundary */
-struct ohci_hcca ghcca[1];
-/* a pointer to the aligned storage */
-struct ohci_hcca *phcca;
-/* this allocates EDs for all possible endpoints */
-struct ohci_device ohci_dev;
-/* urb_priv */
-urb_priv_t urb_priv;
-/* RHSC flag */
-int got_rhsc;
-/* device which was disconnected */
-struct usb_device *devgone;
-
-/*-------------------------------------------------------------------------*/
-
-/* AMD-756 (D2 rev) reports corrupt register contents in some cases.
- * The erratum (#4) description is incorrect.  AMD's workaround waits
- * till some bits (mostly reserved) are clear; ok for all revs.
- */
-#define OHCI_QUIRK_AMD756 0xabcd
-#define read_roothub(hc, register, mask) ({ \
-	u32 temp = readl (&hc->regs->roothub.register); \
-	if (hc->flags & OHCI_QUIRK_AMD756) \
-		while (temp & mask) \
-			temp = readl (&hc->regs->roothub.register); \
-	temp; })
-
-static u32 roothub_a (struct ohci *hc)
-	{ return read_roothub (hc, a, 0xfc0fe000); }
-static inline u32 roothub_b (struct ohci *hc)
-	{ return readl (&hc->regs->roothub.b); }
-static inline u32 roothub_status (struct ohci *hc)
-	{ return readl (&hc->regs->roothub.status); }
-static u32 roothub_portstatus (struct ohci *hc, int i)
-	{ return read_roothub (hc, portstatus [i], 0xffe0fce0); }
-
-
-/* forward declaration */
-static int hc_interrupt (void);
-static void
-td_submit_job (struct usb_device * dev, unsigned long pipe, void * buffer,
-	int transfer_len, struct devrequest * setup, urb_priv_t * urb, int interval);
-
-/*-------------------------------------------------------------------------*
- * URB support functions
- *-------------------------------------------------------------------------*/
-
-/* free HCD-private data associated with this URB */
-
-static void urb_free_priv (urb_priv_t * urb)
-{
-	int		i;
-	int		last;
-	struct td	* td;
-
-	last = urb->length - 1;
-	if (last >= 0) {
-		for (i = 0; i <= last; i++) {
-			td = urb->td[i];
-			if (td) {
-				td->usb_dev = NULL;
-				urb->td[i] = NULL;
-			}
-		}
-	}
-}
-
-/*-------------------------------------------------------------------------*/
-
-#ifdef DEBUG
-static int sohci_get_current_frame_number (struct usb_device * dev);
-
-/* debug| print the main components of an URB
- * small: 0) header + data packets 1) just header */
-
-static void pkt_print (struct usb_device * dev, unsigned long pipe, void * buffer,
-	int transfer_len, struct devrequest * setup, char * str, int small)
-{
-	urb_priv_t * purb = &urb_priv;
-
-	dbg("%s URB:[%4x] dev:%2d,ep:%2d-%c,type:%s,len:%d/%d stat:%#lx",
-			str,
-			sohci_get_current_frame_number (dev),
-			usb_pipedevice (pipe),
-			usb_pipeendpoint (pipe),
-			usb_pipeout (pipe)? 'O': 'I',
-			usb_pipetype (pipe) < 2? (usb_pipeint (pipe)? "INTR": "ISOC"):
-				(usb_pipecontrol (pipe)? "CTRL": "BULK"),
-			purb->actual_length,
-			transfer_len, dev->status);
-#ifdef	OHCI_VERBOSE_DEBUG
-	if (!small) {
-		int i, len;
-
-		if (usb_pipecontrol (pipe)) {
-			printf (__FILE__ ": cmd(8):");
-			for (i = 0; i < 8 ; i++)
-				printf (" %02x", ((__u8 *) setup) [i]);
-			printf ("\n");
-		}
-		if (transfer_len > 0 && buffer) {
-			printf (__FILE__ ": data(%d/%d):",
-				purb->actual_length,
-				transfer_len);
-			len = usb_pipeout (pipe)?
-					transfer_len: purb->actual_length;
-			for (i = 0; i < 16 && i < len; i++)
-				printf (" %02x", ((__u8 *) buffer) [i]);
-			printf ("%s\n", i < len? "...": "");
-		}
-	}
-#endif
-}
-
-/* just for debugging; prints non-empty branches of the int ed tree inclusive iso eds*/
-void ep_print_int_eds (ohci_t *ohci, char * str) {
-	int i, j;
-	 __u32 * ed_p;
-	for (i= 0; i < 32; i++) {
-		j = 5;
-		ed_p = &(ohci->hcca->int_table [i]);
-		if (*ed_p == 0)
-		    continue;
-		printf (__FILE__ ": %s branch int %2d(%2x):", str, i, i);
-		while (*ed_p != 0 && j--) {
-			ed_t *ed = (ed_t *)m32_swap(ed_p);
-			printf (" ed: %4x;", ed->hwINFO);
-			ed_p = &ed->hwNextED;
-		}
-		printf ("\n");
-	}
-}
-
-static void ohci_dump_intr_mask (char *label, __u32 mask)
-{
-	dbg ("%s: 0x%08x%s%s%s%s%s%s%s%s%s",
-		label,
-		mask,
-		(mask & OHCI_INTR_MIE) ? " MIE" : "",
-		(mask & OHCI_INTR_OC) ? " OC" : "",
-		(mask & OHCI_INTR_RHSC) ? " RHSC" : "",
-		(mask & OHCI_INTR_FNO) ? " FNO" : "",
-		(mask & OHCI_INTR_UE) ? " UE" : "",
-		(mask & OHCI_INTR_RD) ? " RD" : "",
-		(mask & OHCI_INTR_SF) ? " SF" : "",
-		(mask & OHCI_INTR_WDH) ? " WDH" : "",
-		(mask & OHCI_INTR_SO) ? " SO" : ""
-		);
-}
-
-static void maybe_print_eds (char *label, __u32 value)
-{
-	ed_t *edp = (ed_t *)value;
-
-	if (value) {
-		dbg ("%s %08x", label, value);
-		dbg ("%08x", edp->hwINFO);
-		dbg ("%08x", edp->hwTailP);
-		dbg ("%08x", edp->hwHeadP);
-		dbg ("%08x", edp->hwNextED);
-	}
-}
-
-static char * hcfs2string (int state)
-{
-	switch (state) {
-		case OHCI_USB_RESET:	return "reset";
-		case OHCI_USB_RESUME:	return "resume";
-		case OHCI_USB_OPER:	return "operational";
-		case OHCI_USB_SUSPEND:	return "suspend";
-	}
-	return "?";
-}
-
-/* dump control and status registers */
-static void ohci_dump_status (ohci_t *controller)
-{
-	struct ohci_regs	*regs = controller->regs;
-	__u32			temp;
-
-	temp = readl (&regs->revision) & 0xff;
-	if (temp != 0x10)
-		dbg ("spec %d.%d", (temp >> 4), (temp & 0x0f));
-
-	temp = readl (&regs->control);
-	dbg ("control: 0x%08x%s%s%s HCFS=%s%s%s%s%s CBSR=%d", temp,
-		(temp & OHCI_CTRL_RWE) ? " RWE" : "",
-		(temp & OHCI_CTRL_RWC) ? " RWC" : "",
-		(temp & OHCI_CTRL_IR) ? " IR" : "",
-		hcfs2string (temp & OHCI_CTRL_HCFS),
-		(temp & OHCI_CTRL_BLE) ? " BLE" : "",
-		(temp & OHCI_CTRL_CLE) ? " CLE" : "",
-		(temp & OHCI_CTRL_IE) ? " IE" : "",
-		(temp & OHCI_CTRL_PLE) ? " PLE" : "",
-		temp & OHCI_CTRL_CBSR
-		);
-
-	temp = readl (&regs->cmdstatus);
-	dbg ("cmdstatus: 0x%08x SOC=%d%s%s%s%s", temp,
-		(temp & OHCI_SOC) >> 16,
-		(temp & OHCI_OCR) ? " OCR" : "",
-		(temp & OHCI_BLF) ? " BLF" : "",
-		(temp & OHCI_CLF) ? " CLF" : "",
-		(temp & OHCI_HCR) ? " HCR" : ""
-		);
-
-	ohci_dump_intr_mask ("intrstatus", readl (&regs->intrstatus));
-	ohci_dump_intr_mask ("intrenable", readl (&regs->intrenable));
-
-	maybe_print_eds ("ed_periodcurrent", readl (&regs->ed_periodcurrent));
-
-	maybe_print_eds ("ed_controlhead", readl (&regs->ed_controlhead));
-	maybe_print_eds ("ed_controlcurrent", readl (&regs->ed_controlcurrent));
-
-	maybe_print_eds ("ed_bulkhead", readl (&regs->ed_bulkhead));
-	maybe_print_eds ("ed_bulkcurrent", readl (&regs->ed_bulkcurrent));
-
-	maybe_print_eds ("donehead", readl (&regs->donehead));
-}
-
-static void ohci_dump_roothub (ohci_t *controller, int verbose)
-{
-	__u32			temp, ndp, i;
-
-	temp = roothub_a (controller);
-	ndp = (temp & RH_A_NDP);
-#ifdef CONFIG_AT91C_PQFP_UHPBUG
-	ndp = (ndp == 2) ? 1:0;
-#endif
-	if (verbose) {
-		dbg ("roothub.a: %08x POTPGT=%d%s%s%s%s%s NDP=%d", temp,
-			((temp & RH_A_POTPGT) >> 24) & 0xff,
-			(temp & RH_A_NOCP) ? " NOCP" : "",
-			(temp & RH_A_OCPM) ? " OCPM" : "",
-			(temp & RH_A_DT) ? " DT" : "",
-			(temp & RH_A_NPS) ? " NPS" : "",
-			(temp & RH_A_PSM) ? " PSM" : "",
-			ndp
-			);
-		temp = roothub_b (controller);
-		dbg ("roothub.b: %08x PPCM=%04x DR=%04x",
-			temp,
-			(temp & RH_B_PPCM) >> 16,
-			(temp & RH_B_DR)
-			);
-		temp = roothub_status (controller);
-		dbg ("roothub.status: %08x%s%s%s%s%s%s",
-			temp,
-			(temp & RH_HS_CRWE) ? " CRWE" : "",
-			(temp & RH_HS_OCIC) ? " OCIC" : "",
-			(temp & RH_HS_LPSC) ? " LPSC" : "",
-			(temp & RH_HS_DRWE) ? " DRWE" : "",
-			(temp & RH_HS_OCI) ? " OCI" : "",
-			(temp & RH_HS_LPS) ? " LPS" : ""
-			);
-	}
-
-	for (i = 0; i < ndp; i++) {
-		temp = roothub_portstatus (controller, i);
-		dbg ("roothub.portstatus [%d] = 0x%08x%s%s%s%s%s%s%s%s%s%s%s%s",
-			i,
-			temp,
-			(temp & RH_PS_PRSC) ? " PRSC" : "",
-			(temp & RH_PS_OCIC) ? " OCIC" : "",
-			(temp & RH_PS_PSSC) ? " PSSC" : "",
-			(temp & RH_PS_PESC) ? " PESC" : "",
-			(temp & RH_PS_CSC) ? " CSC" : "",
-
-			(temp & RH_PS_LSDA) ? " LSDA" : "",
-			(temp & RH_PS_PPS) ? " PPS" : "",
-			(temp & RH_PS_PRS) ? " PRS" : "",
-			(temp & RH_PS_POCI) ? " POCI" : "",
-			(temp & RH_PS_PSS) ? " PSS" : "",
-
-			(temp & RH_PS_PES) ? " PES" : "",
-			(temp & RH_PS_CCS) ? " CCS" : ""
-			);
-	}
-}
-
-static void ohci_dump (ohci_t *controller, int verbose)
-{
-	dbg ("OHCI controller usb-%s state", controller->slot_name);
-
-	/* dumps some of the state we know about */
-	ohci_dump_status (controller);
-	if (verbose)
-		ep_print_int_eds (controller, "hcca");
-	dbg ("hcca frame #%04x", controller->hcca->frame_no);
-	ohci_dump_roothub (controller, 1);
-}
-
-
-#endif /* DEBUG */
-
-/*-------------------------------------------------------------------------*
- * Interface functions (URB)
- *-------------------------------------------------------------------------*/
-
-/* get a transfer request */
-
-int sohci_submit_job(struct usb_device *dev, unsigned long pipe, void *buffer,
-		int transfer_len, struct devrequest *setup, int interval)
-{
-	ohci_t *ohci;
-	ed_t * ed;
-	urb_priv_t *purb_priv;
-	int i, size = 0;
-
-	ohci = &gohci;
-
-	/* when controller's hung, permit only roothub cleanup attempts
-	 * such as powering down ports */
-	if (ohci->disabled) {
-		err("sohci_submit_job: EPIPE");
-		return -1;
-	}
-
-	/* every endpoint has a ed, locate and fill it */
-	if (!(ed = ep_add_ed (dev, pipe))) {
-		err("sohci_submit_job: ENOMEM");
-		return -1;
-	}
-
-	/* for the private part of the URB we need the number of TDs (size) */
-	switch (usb_pipetype (pipe)) {
-		case PIPE_BULK: /* one TD for every 4096 Byte */
-			size = (transfer_len - 1) / 4096 + 1;
-			break;
-		case PIPE_CONTROL: /* 1 TD for setup, 1 for ACK and 1 for every 4096 B */
-			size = (transfer_len == 0)? 2:
-						(transfer_len - 1) / 4096 + 3;
-			break;
-	}
-
-	if (size >= (N_URB_TD - 1)) {
-		err("need %d TDs, only have %d", size, N_URB_TD);
-		return -1;
-	}
-	purb_priv = &urb_priv;
-	purb_priv->pipe = pipe;
-
-	/* fill the private part of the URB */
-	purb_priv->length = size;
-	purb_priv->ed = ed;
-	purb_priv->actual_length = 0;
-
-	/* allocate the TDs */
-	/* note that td[0] was allocated in ep_add_ed */
-	for (i = 0; i < size; i++) {
-		purb_priv->td[i] = td_alloc (dev);
-		if (!purb_priv->td[i]) {
-			purb_priv->length = i;
-			urb_free_priv (purb_priv);
-			err("sohci_submit_job: ENOMEM");
-			return -1;
-		}
-	}
-
-	if (ed->state == ED_NEW || (ed->state & ED_DEL)) {
-		urb_free_priv (purb_priv);
-		err("sohci_submit_job: EINVAL");
-		return -1;
-	}
-
-	/* link the ed into a chain if is not already */
-	if (ed->state != ED_OPER)
-		ep_link (ohci, ed);
-
-	/* fill the TDs and link it to the ed */
-	td_submit_job(dev, pipe, buffer, transfer_len, setup, purb_priv, interval);
-
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-#ifdef DEBUG
-/* tell us the current USB frame number */
-
-static int sohci_get_current_frame_number (struct usb_device *usb_dev)
-{
-	ohci_t *ohci = &gohci;
-
-	return m16_swap (ohci->hcca->frame_no);
-}
-#endif
-
-/*-------------------------------------------------------------------------*
- * ED handling functions
- *-------------------------------------------------------------------------*/
-
-/* link an ed into one of the HC chains */
-
-static int ep_link (ohci_t *ohci, ed_t *edi)
-{
-	volatile ed_t *ed = edi;
-
-	ed->state = ED_OPER;
-
-	switch (ed->type) {
-	case PIPE_CONTROL:
-		ed->hwNextED = 0;
-		if (ohci->ed_controltail == NULL) {
-			writel (ed, &ohci->regs->ed_controlhead);
-		} else {
-			ohci->ed_controltail->hwNextED = m32_swap (ed);
-		}
-		ed->ed_prev = ohci->ed_controltail;
-		if (!ohci->ed_controltail && !ohci->ed_rm_list[0] &&
-			!ohci->ed_rm_list[1] && !ohci->sleeping) {
-			ohci->hc_control |= OHCI_CTRL_CLE;
-			writel (ohci->hc_control, &ohci->regs->control);
-		}
-		ohci->ed_controltail = edi;
-		break;
-
-	case PIPE_BULK:
-		ed->hwNextED = 0;
-		if (ohci->ed_bulktail == NULL) {
-			writel (ed, &ohci->regs->ed_bulkhead);
-		} else {
-			ohci->ed_bulktail->hwNextED = m32_swap (ed);
-		}
-		ed->ed_prev = ohci->ed_bulktail;
-		if (!ohci->ed_bulktail && !ohci->ed_rm_list[0] &&
-			!ohci->ed_rm_list[1] && !ohci->sleeping) {
-			ohci->hc_control |= OHCI_CTRL_BLE;
-			writel (ohci->hc_control, &ohci->regs->control);
-		}
-		ohci->ed_bulktail = edi;
-		break;
-	}
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* unlink an ed from one of the HC chains.
- * just the link to the ed is unlinked.
- * the link from the ed still points to another operational ed or 0
- * so the HC can eventually finish the processing of the unlinked ed */
-
-static int ep_unlink (ohci_t *ohci, ed_t *ed)
-{
-	ed->hwINFO |= m32_swap (OHCI_ED_SKIP);
-
-	switch (ed->type) {
-	case PIPE_CONTROL:
-		if (ed->ed_prev == NULL) {
-			if (!ed->hwNextED) {
-				ohci->hc_control &= ~OHCI_CTRL_CLE;
-				writel (ohci->hc_control, &ohci->regs->control);
-			}
-			writel (m32_swap (*((__u32 *)&ed->hwNextED)), &ohci->regs->ed_controlhead);
-		} else {
-			ed->ed_prev->hwNextED = ed->hwNextED;
-		}
-		if (ohci->ed_controltail == ed) {
-			ohci->ed_controltail = ed->ed_prev;
-		} else {
-			((ed_t *)m32_swap (*((__u32 *)&ed->hwNextED)))->ed_prev = ed->ed_prev;
-		}
-		break;
-
-	case PIPE_BULK:
-		if (ed->ed_prev == NULL) {
-			if (!ed->hwNextED) {
-				ohci->hc_control &= ~OHCI_CTRL_BLE;
-				writel (ohci->hc_control, &ohci->regs->control);
-			}
-			writel (m32_swap (*((__u32 *)&ed->hwNextED)), &ohci->regs->ed_bulkhead);
-		} else {
-			ed->ed_prev->hwNextED = ed->hwNextED;
-		}
-		if (ohci->ed_bulktail == ed) {
-			ohci->ed_bulktail = ed->ed_prev;
-		} else {
-			((ed_t *)m32_swap (*((__u32 *)&ed->hwNextED)))->ed_prev = ed->ed_prev;
-		}
-		break;
-	}
-	ed->state = ED_UNLINK;
-	return 0;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* add/reinit an endpoint; this should be done once at the usb_set_configuration command,
- * but the USB stack is a little bit stateless	so we do it at every transaction
- * if the state of the ed is ED_NEW then a dummy td is added and the state is changed to ED_UNLINK
- * in all other cases the state is left unchanged
- * the ed info fields are setted anyway even though most of them should not change */
-
-static ed_t * ep_add_ed (struct usb_device *usb_dev, unsigned long pipe)
-{
-	td_t *td;
-	ed_t *ed_ret;
-	volatile ed_t *ed;
-
-	ed = ed_ret = &ohci_dev.ed[(usb_pipeendpoint (pipe) << 1) |
-			(usb_pipecontrol (pipe)? 0: usb_pipeout (pipe))];
-
-	if ((ed->state & ED_DEL) || (ed->state & ED_URB_DEL)) {
-		err("ep_add_ed: pending delete");
-		/* pending delete request */
-		return NULL;
-	}
-
-	if (ed->state == ED_NEW) {
-		ed->hwINFO = m32_swap (OHCI_ED_SKIP); /* skip ed */
-		/* dummy td; end of td list for ed */
-		td = td_alloc (usb_dev);
-		ed->hwTailP = m32_swap (td);
-		ed->hwHeadP = ed->hwTailP;
-		ed->state = ED_UNLINK;
-		ed->type = usb_pipetype (pipe);
-		ohci_dev.ed_cnt++;
-	}
-
-	ed->hwINFO = m32_swap (usb_pipedevice (pipe)
-			| usb_pipeendpoint (pipe) << 7
-			| (usb_pipeisoc (pipe)? 0x8000: 0)
-			| (usb_pipecontrol (pipe)? 0: (usb_pipeout (pipe)? 0x800: 0x1000))
-			| usb_pipeslow (pipe) << 13
-			| usb_maxpacket (usb_dev, pipe) << 16);
-
-	return ed_ret;
-}
-
-/*-------------------------------------------------------------------------*
- * TD handling functions
- *-------------------------------------------------------------------------*/
-
-/* enqueue next TD for this URB (OHCI spec 5.2.8.2) */
-
-static void td_fill (ohci_t *ohci, unsigned int info,
-	void *data, int len,
-	struct usb_device *dev, int index, urb_priv_t *urb_priv)
-{
-	volatile td_t  *td, *td_pt;
-#ifdef OHCI_FILL_TRACE
-	int i;
-#endif
-
-	if (index > urb_priv->length) {
-		err("index > length");
-		return;
-	}
-	/* use this td as the next dummy */
-	td_pt = urb_priv->td [index];
-	td_pt->hwNextTD = 0;
-
-	/* fill the old dummy TD */
-	td = urb_priv->td [index] = (td_t *)(m32_swap (urb_priv->ed->hwTailP) & ~0xf);
-
-	td->ed = urb_priv->ed;
-	td->next_dl_td = NULL;
-	td->index = index;
-	td->data = (__u32)data;
-#ifdef OHCI_FILL_TRACE
-	if ((usb_pipetype(urb_priv->pipe) == PIPE_BULK) && usb_pipeout(urb_priv->pipe)) {
-		for (i = 0; i < len; i++)
-		printf("td->data[%d] %#2x ",i, ((unsigned char *)td->data)[i]);
-		printf("\n");
-	}
-#endif
-	if (!len)
-		data = 0;
-
-	td->hwINFO = m32_swap (info);
-	td->hwCBP = m32_swap (data);
-	if (data)
-		td->hwBE = m32_swap (data + len - 1);
-	else
-		td->hwBE = 0;
-	td->hwNextTD = m32_swap (td_pt);
-	td->hwPSW [0] = m16_swap (((__u32)data & 0x0FFF) | 0xE000);
-
-	/* append to queue */
-	td->ed->hwTailP = td->hwNextTD;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* prepare all TDs of a transfer */
-
-static void td_submit_job (struct usb_device *dev, unsigned long pipe, void *buffer,
-	int transfer_len, struct devrequest *setup, urb_priv_t *urb, int interval)
-{
-	ohci_t *ohci = &gohci;
-	int data_len = transfer_len;
-	void *data;
-	int cnt = 0;
-	__u32 info = 0;
-	unsigned int toggle = 0;
-
-	/* OHCI handles the DATA-toggles itself, we just use the USB-toggle bits for reseting */
-	if(usb_gettoggle(dev, usb_pipeendpoint(pipe), usb_pipeout(pipe))) {
-		toggle = TD_T_TOGGLE;
-	} else {
-		toggle = TD_T_DATA0;
-		usb_settoggle(dev, usb_pipeendpoint(pipe), usb_pipeout(pipe), 1);
-	}
-	urb->td_cnt = 0;
-	if (data_len)
-		data = buffer;
-	else
-		data = 0;
-
-	switch (usb_pipetype (pipe)) {
-	case PIPE_BULK:
-		info = usb_pipeout (pipe)?
-			TD_CC | TD_DP_OUT : TD_CC | TD_DP_IN ;
-		while(data_len > 4096) {
-			td_fill (ohci, info | (cnt? TD_T_TOGGLE:toggle), data, 4096, dev, cnt, urb);
-			data += 4096; data_len -= 4096; cnt++;
-		}
-		info = usb_pipeout (pipe)?
-			TD_CC | TD_DP_OUT : TD_CC | TD_R | TD_DP_IN ;
-		td_fill (ohci, info | (cnt? TD_T_TOGGLE:toggle), data, data_len, dev, cnt, urb);
-		cnt++;
-
-		if (!ohci->sleeping)
-			writel (OHCI_BLF, &ohci->regs->cmdstatus); /* start bulk list */
-		break;
-
-	case PIPE_CONTROL:
-		info = TD_CC | TD_DP_SETUP | TD_T_DATA0;
-		td_fill (ohci, info, setup, 8, dev, cnt++, urb);
-		if (data_len > 0) {
-			info = usb_pipeout (pipe)?
-				TD_CC | TD_R | TD_DP_OUT | TD_T_DATA1 : TD_CC | TD_R | TD_DP_IN | TD_T_DATA1;
-			/* NOTE:  mishandles transfers >8K, some >4K */
-			td_fill (ohci, info, data, data_len, dev, cnt++, urb);
-		}
-		info = usb_pipeout (pipe)?
-			TD_CC | TD_DP_IN | TD_T_DATA1: TD_CC | TD_DP_OUT | TD_T_DATA1;
-		td_fill (ohci, info, data, 0, dev, cnt++, urb);
-		if (!ohci->sleeping)
-			writel (OHCI_CLF, &ohci->regs->cmdstatus); /* start Control list */
-		break;
-	}
-	if (urb->length != cnt)
-		dbg("TD LENGTH %d != CNT %d", urb->length, cnt);
-}
-
-/*-------------------------------------------------------------------------*
- * Done List handling functions
- *-------------------------------------------------------------------------*/
-
-
-/* calculate the transfer length and update the urb */
-
-static void dl_transfer_length(td_t * td)
-{
-	__u32 tdINFO, tdBE, tdCBP;
-	urb_priv_t *lurb_priv = &urb_priv;
-
-	tdINFO = m32_swap (td->hwINFO);
-	tdBE   = m32_swap (td->hwBE);
-	tdCBP  = m32_swap (td->hwCBP);
-
-
-	if (!(usb_pipetype (lurb_priv->pipe) == PIPE_CONTROL &&
-	    ((td->index == 0) || (td->index == lurb_priv->length - 1)))) {
-		if (tdBE != 0) {
-			if (td->hwCBP == 0)
-				lurb_priv->actual_length += tdBE - td->data + 1;
-			else
-				lurb_priv->actual_length += tdCBP - td->data;
-		}
-	}
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* replies to the request have to be on a FIFO basis so
- * we reverse the reversed done-list */
-
-static td_t * dl_reverse_done_list (ohci_t *ohci)
-{
-	__u32 td_list_hc;
-	td_t *td_rev = NULL;
-	td_t *td_list = NULL;
-	urb_priv_t *lurb_priv = NULL;
-
-	td_list_hc = m32_swap (ohci->hcca->done_head) & 0xfffffff0;
-	ohci->hcca->done_head = 0;
-
-	while (td_list_hc) {
-		td_list = (td_t *)td_list_hc;
-
-		if (TD_CC_GET (m32_swap (td_list->hwINFO))) {
-			lurb_priv = &urb_priv;
-			dbg(" USB-error/status: %x : %p",
-					TD_CC_GET (m32_swap (td_list->hwINFO)), td_list);
-			if (td_list->ed->hwHeadP & m32_swap (0x1)) {
-				if (lurb_priv && ((td_list->index + 1) < lurb_priv->length)) {
-					td_list->ed->hwHeadP =
-						(lurb_priv->td[lurb_priv->length - 1]->hwNextTD & m32_swap (0xfffffff0)) |
-									(td_list->ed->hwHeadP & m32_swap (0x2));
-					lurb_priv->td_cnt += lurb_priv->length - td_list->index - 1;
-				} else
-					td_list->ed->hwHeadP &= m32_swap (0xfffffff2);
-			}
-		}
-
-		td_list->next_dl_td = td_rev;
-		td_rev = td_list;
-		td_list_hc = m32_swap (td_list->hwNextTD) & 0xfffffff0;
-	}
-	return td_list;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* td done list */
-static int dl_done_list (ohci_t *ohci, td_t *td_list)
-{
-	td_t *td_list_next = NULL;
-	ed_t *ed;
-	int cc = 0;
-	int stat = 0;
-	/* urb_t *urb; */
-	urb_priv_t *lurb_priv;
-	__u32 tdINFO, edHeadP, edTailP;
-
-	while (td_list) {
-		td_list_next = td_list->next_dl_td;
-
-		lurb_priv = &urb_priv;
-		tdINFO = m32_swap (td_list->hwINFO);
-
-		ed = td_list->ed;
-
-		dl_transfer_length(td_list);
-
-		/* error code of transfer */
-		cc = TD_CC_GET (tdINFO);
-		if (cc != 0) {
-			dbg("ConditionCode %#x", cc);
-			stat = cc_to_error[cc];
-		}
-
-		if (ed->state != ED_NEW) {
-			edHeadP = m32_swap (ed->hwHeadP) & 0xfffffff0;
-			edTailP = m32_swap (ed->hwTailP);
-
-			/* unlink eds if they are not busy */
-			if ((edHeadP == edTailP) && (ed->state == ED_OPER))
-				ep_unlink (ohci, ed);
-		}
-
-		td_list = td_list_next;
-	}
-	return stat;
-}
-
-/*-------------------------------------------------------------------------*
- * Virtual Root Hub
- *-------------------------------------------------------------------------*/
-
-/* Device descriptor */
-static __u8 root_hub_dev_des[] =
-{
-	0x12,	    /*	__u8  bLength; */
-	0x01,	    /*	__u8  bDescriptorType; Device */
-	0x10,	    /*	__u16 bcdUSB; v1.1 */
-	0x01,
-	0x09,	    /*	__u8  bDeviceClass; HUB_CLASSCODE */
-	0x00,	    /*	__u8  bDeviceSubClass; */
-	0x00,	    /*	__u8  bDeviceProtocol; */
-	0x08,	    /*	__u8  bMaxPacketSize0; 8 Bytes */
-	0x00,	    /*	__u16 idVendor; */
-	0x00,
-	0x00,	    /*	__u16 idProduct; */
-	0x00,
-	0x00,	    /*	__u16 bcdDevice; */
-	0x00,
-	0x00,	    /*	__u8  iManufacturer; */
-	0x01,	    /*	__u8  iProduct; */
-	0x00,	    /*	__u8  iSerialNumber; */
-	0x01	    /*	__u8  bNumConfigurations; */
-};
-
-
-/* Configuration descriptor */
-static __u8 root_hub_config_des[] =
-{
-	0x09,	    /*	__u8  bLength; */
-	0x02,	    /*	__u8  bDescriptorType; Configuration */
-	0x19,	    /*	__u16 wTotalLength; */
-	0x00,
-	0x01,	    /*	__u8  bNumInterfaces; */
-	0x01,	    /*	__u8  bConfigurationValue; */
-	0x00,	    /*	__u8  iConfiguration; */
-	0x40,	    /*	__u8  bmAttributes;
-		 Bit 7: Bus-powered, 6: Self-powered, 5 Remote-wakwup, 4..0: resvd */
-	0x00,	    /*	__u8  MaxPower; */
-
-	/* interface */
-	0x09,	    /*	__u8  if_bLength; */
-	0x04,	    /*	__u8  if_bDescriptorType; Interface */
-	0x00,	    /*	__u8  if_bInterfaceNumber; */
-	0x00,	    /*	__u8  if_bAlternateSetting; */
-	0x01,	    /*	__u8  if_bNumEndpoints; */
-	0x09,	    /*	__u8  if_bInterfaceClass; HUB_CLASSCODE */
-	0x00,	    /*	__u8  if_bInterfaceSubClass; */
-	0x00,	    /*	__u8  if_bInterfaceProtocol; */
-	0x00,	    /*	__u8  if_iInterface; */
-
-	/* endpoint */
-	0x07,	    /*	__u8  ep_bLength; */
-	0x05,	    /*	__u8  ep_bDescriptorType; Endpoint */
-	0x81,	    /*	__u8  ep_bEndpointAddress; IN Endpoint 1 */
-	0x03,	    /*	__u8  ep_bmAttributes; Interrupt */
-	0x02,	    /*	__u16 ep_wMaxPacketSize; ((MAX_ROOT_PORTS + 1) / 8 */
-	0x00,
-	0xff	    /*	__u8  ep_bInterval; 255 ms */
-};
-
-static unsigned char root_hub_str_index0[] =
-{
-	0x04,			/*  __u8  bLength; */
-	0x03,			/*  __u8  bDescriptorType; String-descriptor */
-	0x09,			/*  __u8  lang ID */
-	0x04,			/*  __u8  lang ID */
-};
-
-static unsigned char root_hub_str_index1[] =
-{
-	28,			/*  __u8  bLength; */
-	0x03,			/*  __u8  bDescriptorType; String-descriptor */
-	'O',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'H',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'C',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'I',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	' ',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'R',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'o',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'o',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	't',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	' ',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'H',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'u',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-	'b',			/*  __u8  Unicode */
-	0,				/*  __u8  Unicode */
-};
-
-/* Hub class-specific descriptor is constructed dynamically */
-
-
-/*-------------------------------------------------------------------------*/
-
-#define OK(x)			len = (x); break
-#ifdef DEBUG
-#define WR_RH_STAT(x)		{info("WR:status %#8x", (x));writel((x), &gohci.regs->roothub.status);}
-#define WR_RH_PORTSTAT(x)	{info("WR:portstatus[%d] %#8x", wIndex-1, (x));writel((x), &gohci.regs->roothub.portstatus[wIndex-1]);}
-#else
-#define WR_RH_STAT(x)		writel((x), &gohci.regs->roothub.status)
-#define WR_RH_PORTSTAT(x)	writel((x), &gohci.regs->roothub.portstatus[wIndex-1])
-#endif
-#define RD_RH_STAT		roothub_status(&gohci)
-#define RD_RH_PORTSTAT		roothub_portstatus(&gohci,wIndex-1)
-
-/* request to virtual root hub */
-
-int rh_check_port_status(ohci_t *controller)
-{
-	__u32 temp, ndp, i;
-	int res;
-
-	res = -1;
-	temp = roothub_a (controller);
-	ndp = (temp & RH_A_NDP);
-#ifdef CONFIG_AT91C_PQFP_UHPBUG
-	ndp = (ndp == 2) ? 1:0;
-#endif
-
-	for (i = 0; i < ndp; i++) {
-		temp = roothub_portstatus (controller, i);
-		/* check for a device disconnect */
-		if (((temp & (RH_PS_PESC | RH_PS_CSC)) ==
-			(RH_PS_PESC | RH_PS_CSC)) &&
-			((temp & RH_PS_CCS) == 0)) {
-			res = i;
-			break;
-		}
-	}
-	return res;
-}
-
-static int ohci_submit_rh_msg(struct usb_device *dev, unsigned long pipe,
-		void *buffer, int transfer_len, struct devrequest *cmd)
-{
-	void * data = buffer;
-	int leni = transfer_len;
-	int len = 0;
-	int stat = 0;
-	__u32 datab[4];
-	__u8 *data_buf = (__u8 *)datab;
-	__u16 bmRType_bReq;
-	__u16 wValue;
-	__u16 wIndex;
-	__u16 wLength;
-
-#ifdef DEBUG
-urb_priv.actual_length = 0;
-pkt_print(dev, pipe, buffer, transfer_len, cmd, "SUB(rh)", usb_pipein(pipe));
-#else
-	wait_ms(1);
-#endif
-	if ((pipe & PIPE_INTERRUPT) == PIPE_INTERRUPT) {
-		info("Root-Hub submit IRQ: NOT implemented");
-		return 0;
-	}
-
-	bmRType_bReq  = cmd->requesttype | (cmd->request << 8);
-	wValue	      = m16_swap (cmd->value);
-	wIndex	      = m16_swap (cmd->index);
-	wLength	      = m16_swap (cmd->length);
-
-	info("Root-Hub: adr: %2x cmd(%1x): %08x %04x %04x %04x",
-		dev->devnum, 8, bmRType_bReq, wValue, wIndex, wLength);
-
-	switch (bmRType_bReq) {
-	/* Request Destination:
-	   without flags: Device,
-	   RH_INTERFACE: interface,
-	   RH_ENDPOINT: endpoint,
-	   RH_CLASS means HUB here,
-	   RH_OTHER | RH_CLASS	almost ever means HUB_PORT here
-	*/
-
-	case RH_GET_STATUS:
-			*(__u16 *) data_buf = m16_swap (1); OK (2);
-	case RH_GET_STATUS | RH_INTERFACE:
-			*(__u16 *) data_buf = m16_swap (0); OK (2);
-	case RH_GET_STATUS | RH_ENDPOINT:
-			*(__u16 *) data_buf = m16_swap (0); OK (2);
-	case RH_GET_STATUS | RH_CLASS:
-			*(__u32 *) data_buf = m32_swap (
-				RD_RH_STAT & ~(RH_HS_CRWE | RH_HS_DRWE));
-			OK (4);
-	case RH_GET_STATUS | RH_OTHER | RH_CLASS:
-			*(__u32 *) data_buf = m32_swap (RD_RH_PORTSTAT); OK (4);
-
-	case RH_CLEAR_FEATURE | RH_ENDPOINT:
-		switch (wValue) {
-			case (RH_ENDPOINT_STALL): OK (0);
-		}
-		break;
-
-	case RH_CLEAR_FEATURE | RH_CLASS:
-		switch (wValue) {
-			case RH_C_HUB_LOCAL_POWER:
-				OK(0);
-			case (RH_C_HUB_OVER_CURRENT):
-					WR_RH_STAT(RH_HS_OCIC); OK (0);
-		}
-		break;
-
-	case RH_CLEAR_FEATURE | RH_OTHER | RH_CLASS:
-		switch (wValue) {
-			case (RH_PORT_ENABLE):
-					WR_RH_PORTSTAT (RH_PS_CCS ); OK (0);
-			case (RH_PORT_SUSPEND):
-					WR_RH_PORTSTAT (RH_PS_POCI); OK (0);
-			case (RH_PORT_POWER):
-					WR_RH_PORTSTAT (RH_PS_LSDA); OK (0);
-			case (RH_C_PORT_CONNECTION):
-					WR_RH_PORTSTAT (RH_PS_CSC ); OK (0);
-			case (RH_C_PORT_ENABLE):
-					WR_RH_PORTSTAT (RH_PS_PESC); OK (0);
-			case (RH_C_PORT_SUSPEND):
-					WR_RH_PORTSTAT (RH_PS_PSSC); OK (0);
-			case (RH_C_PORT_OVER_CURRENT):
-					WR_RH_PORTSTAT (RH_PS_OCIC); OK (0);
-			case (RH_C_PORT_RESET):
-					WR_RH_PORTSTAT (RH_PS_PRSC); OK (0);
-		}
-		break;
-
-	case RH_SET_FEATURE | RH_OTHER | RH_CLASS:
-		switch (wValue) {
-			case (RH_PORT_SUSPEND):
-					WR_RH_PORTSTAT (RH_PS_PSS ); OK (0);
-			case (RH_PORT_RESET): /* BUG IN HUP CODE *********/
-					if (RD_RH_PORTSTAT & RH_PS_CCS)
-					    WR_RH_PORTSTAT (RH_PS_PRS);
-					OK (0);
-			case (RH_PORT_POWER):
-					WR_RH_PORTSTAT (RH_PS_PPS ); OK (0);
-			case (RH_PORT_ENABLE): /* BUG IN HUP CODE *********/
-					if (RD_RH_PORTSTAT & RH_PS_CCS)
-					    WR_RH_PORTSTAT (RH_PS_PES );
-					OK (0);
-		}
-		break;
-
-	case RH_SET_ADDRESS: gohci.rh.devnum = wValue; OK(0);
-
-	case RH_GET_DESCRIPTOR:
-		switch ((wValue & 0xff00) >> 8) {
-			case (0x01): /* device descriptor */
-				len = min_t(unsigned int,
-					  leni,
-					  min_t(unsigned int,
-					      sizeof (root_hub_dev_des),
-					      wLength));
-				data_buf = root_hub_dev_des; OK(len);
-			case (0x02): /* configuration descriptor */
-				len = min_t(unsigned int,
-					  leni,
-					  min_t(unsigned int,
-					      sizeof (root_hub_config_des),
-					      wLength));
-				data_buf = root_hub_config_des; OK(len);
-			case (0x03): /* string descriptors */
-				if(wValue==0x0300) {
-					len = min_t(unsigned int,
-						  leni,
-						  min_t(unsigned int,
-						      sizeof (root_hub_str_index0),
-						      wLength));
-					data_buf = root_hub_str_index0;
-					OK(len);
-				}
-				if(wValue==0x0301) {
-					len = min_t(unsigned int,
-						  leni,
-						  min_t(unsigned int,
-						      sizeof (root_hub_str_index1),
-						      wLength));
-					data_buf = root_hub_str_index1;
-					OK(len);
-			}
-			default:
-				stat = USB_ST_STALLED;
-		}
-		break;
-
-	case RH_GET_DESCRIPTOR | RH_CLASS:
-	{
-		__u32 temp = roothub_a (&gohci);
-
-		data_buf [0] = 9;		/* min length; */
-		data_buf [1] = 0x29;
-		data_buf [2] = temp & RH_A_NDP;
-#ifdef CONFIG_AT91C_PQFP_UHPBUG
-		data_buf [2] = (data_buf [2] == 2) ? 1:0;
-#endif
-		data_buf [3] = 0;
-		if (temp & RH_A_PSM)	/* per-port power switching? */
-			data_buf [3] |= 0x1;
-		if (temp & RH_A_NOCP)	/* no overcurrent reporting? */
-			data_buf [3] |= 0x10;
-		else if (temp & RH_A_OCPM)	/* per-port overcurrent reporting? */
-			data_buf [3] |= 0x8;
-
-		/* corresponds to data_buf[4-7] */
-		datab [1] = 0;
-		data_buf [5] = (temp & RH_A_POTPGT) >> 24;
-		temp = roothub_b (&gohci);
-		data_buf [7] = temp & RH_B_DR;
-		if (data_buf [2] < 7) {
-			data_buf [8] = 0xff;
-		} else {
-			data_buf [0] += 2;
-			data_buf [8] = (temp & RH_B_DR) >> 8;
-			data_buf [10] = data_buf [9] = 0xff;
-		}
-
-		len = min_t(unsigned int, leni,
-		min_t(unsigned int, data_buf [0], wLength));
-		OK (len);
-	}
-
-	case RH_GET_CONFIGURATION:	*(__u8 *) data_buf = 0x01; OK (1);
-
-	case RH_SET_CONFIGURATION:	WR_RH_STAT (0x10000); OK (0);
-
-	default:
-		dbg ("unsupported root hub command");
-		stat = USB_ST_STALLED;
-	}
-
-#ifdef	DEBUG
-	ohci_dump_roothub (&gohci, 1);
-#else
-	wait_ms(1);
-#endif
-
-	len = min_t(int, len, leni);
-	if (data != data_buf)
-	    memcpy (data, data_buf, len);
-	dev->act_len = len;
-	dev->status = stat;
-
-#ifdef DEBUG
-	if (transfer_len)
-		urb_priv.actual_length = transfer_len;
-	pkt_print(dev, pipe, buffer, transfer_len, cmd, "RET(rh)", 0/*usb_pipein(pipe)*/);
-#else
-	wait_ms(1);
-#endif
-
-	return stat;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* common code for handling submit messages - used for all but root hub */
-/* accesses. */
-int submit_common_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
-		int transfer_len, struct devrequest *setup, int interval)
-{
-	int stat = 0;
-	int maxsize = usb_maxpacket(dev, pipe);
-	int timeout;
-
-	/* device pulled? Shortcut the action. */
-	if (devgone == dev) {
-		dev->status = USB_ST_CRC_ERR;
-		return 0;
-	}
-
-#ifdef DEBUG
-	urb_priv.actual_length = 0;
-	pkt_print(dev, pipe, buffer, transfer_len, setup, "SUB", usb_pipein(pipe));
-#else
-	wait_ms(1);
-#endif
-	if (!maxsize) {
-		err("submit_common_message: pipesize for pipe %lx is zero",
-			pipe);
-		return -1;
-	}
-
-	if (sohci_submit_job(dev, pipe, buffer, transfer_len, setup, interval) < 0) {
-		err("sohci_submit_job failed");
-		return -1;
-	}
-
-	wait_ms(10);
-	/* ohci_dump_status(&gohci); */
-
-	/* allow more time for a BULK device to react - some are slow */
-#define BULK_TO	 5000	/* timeout in milliseconds */
-	if (usb_pipetype (pipe) == PIPE_BULK)
-		timeout = BULK_TO;
-	else
-		timeout = 100;
-
-	/* wait for it to complete */
-	for (;;) {
-		/* check whether the controller is done */
-		stat = hc_interrupt();
-		if (stat < 0) {
-			stat = USB_ST_CRC_ERR;
-			break;
-		}
-		if (stat >= 0 && stat != 0xff) {
-			/* 0xff is returned for an SF-interrupt */
-			break;
-		}
-		if (--timeout) {
-			wait_ms(1);
-		} else {
-			err("CTL:TIMEOUT ");
-			stat = USB_ST_CRC_ERR;
-			break;
-		}
-	}
-	/* we got an Root Hub Status Change interrupt */
-	if (got_rhsc) {
-#ifdef DEBUG
-		ohci_dump_roothub (&gohci, 1);
-#endif
-		got_rhsc = 0;
-		/* abuse timeout */
-		timeout = rh_check_port_status(&gohci);
-		if (timeout >= 0) {
-#if 0 /* this does nothing useful, but leave it here in case that changes */
-			/* the called routine adds 1 to the passed value */
-			usb_hub_port_connect_change(gohci.rh.dev, timeout - 1);
-#endif
-			/*
-			 * XXX
-			 * This is potentially dangerous because it assumes
-			 * that only one device is ever plugged in!
-			 */
-			devgone = dev;
-		}
-	}
-
-	dev->status = stat;
-	dev->act_len = transfer_len;
-
-#ifdef DEBUG
-	pkt_print(dev, pipe, buffer, transfer_len, setup, "RET(ctlr)", usb_pipein(pipe));
-#else
-	wait_ms(1);
-#endif
-
-	/* free TDs in urb_priv */
-	urb_free_priv (&urb_priv);
-	return 0;
-}
-
-/* submit routines called from usb.c */
-int submit_bulk_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
-		int transfer_len)
-{
-	info("submit_bulk_msg");
-	return submit_common_msg(dev, pipe, buffer, transfer_len, NULL, 0);
-}
-
-int submit_control_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
-		int transfer_len, struct devrequest *setup)
-{
-	int maxsize = usb_maxpacket(dev, pipe);
-
-	info("submit_control_msg");
-#ifdef DEBUG
-	urb_priv.actual_length = 0;
-	pkt_print(dev, pipe, buffer, transfer_len, setup, "SUB", usb_pipein(pipe));
-#else
-	wait_ms(1);
-#endif
-	if (!maxsize) {
-		err("submit_control_message: pipesize for pipe %lx is zero",
-			pipe);
-		return -1;
-	}
-	if (((pipe >> 8) & 0x7f) == gohci.rh.devnum) {
-		gohci.rh.dev = dev;
-		/* root hub - redirect */
-		return ohci_submit_rh_msg(dev, pipe, buffer, transfer_len,
-			setup);
-	}
-
-	return submit_common_msg(dev, pipe, buffer, transfer_len, setup, 0);
-}
-
-int submit_int_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
-		int transfer_len, int interval)
-{
-	info("submit_int_msg");
-	return -1;
-}
-
-/*-------------------------------------------------------------------------*
- * HC functions
- *-------------------------------------------------------------------------*/
-
-/* reset the HC and BUS */
-
-static int hc_reset (ohci_t *ohci)
-{
-	int timeout = 30;
-	int smm_timeout = 50; /* 0,5 sec */
-
-	dbg("%s\n", __FUNCTION__);
-
-	if (readl (&ohci->regs->control) & OHCI_CTRL_IR) { /* SMM owns the HC */
-		writel (OHCI_OCR, &ohci->regs->cmdstatus); /* request ownership */
-		info("USB HC TakeOver from SMM");
-		while (readl (&ohci->regs->control) & OHCI_CTRL_IR) {
-			wait_ms (10);
-			if (--smm_timeout == 0) {
-				err("USB HC TakeOver failed!");
-				return -1;
-			}
-		}
-	}
-
-	/* Disable HC interrupts */
-	writel (OHCI_INTR_MIE, &ohci->regs->intrdisable);
-
-	dbg("USB HC reset_hc usb-%s: ctrl = 0x%X ;\n",
-		ohci->slot_name,
-		readl(&ohci->regs->control));
-
-	/* Reset USB (needed by some controllers) */
-	writel (0, &ohci->regs->control);
-
-	/* HC Reset requires max 10 us delay */
-	writel (OHCI_HCR,  &ohci->regs->cmdstatus);
-	while ((readl (&ohci->regs->cmdstatus) & OHCI_HCR) != 0) {
-		if (--timeout == 0) {
-			err("USB HC reset timed out!");
-			return -1;
-		}
-		udelay (1);
-	}
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* Start an OHCI controller, set the BUS operational
- * enable interrupts
- * connect the virtual root hub */
-
-static int hc_start (ohci_t * ohci)
-{
-	__u32 mask;
-	unsigned int fminterval;
-
-	ohci->disabled = 1;
-
-	/* Tell the controller where the control and bulk lists are
-	 * The lists are empty now. */
-
-	writel (0, &ohci->regs->ed_controlhead);
-	writel (0, &ohci->regs->ed_bulkhead);
-
-	writel ((__u32)ohci->hcca, &ohci->regs->hcca); /* a reset clears this */
-
-	fminterval = 0x2edf;
-	writel ((fminterval * 9) / 10, &ohci->regs->periodicstart);
-	fminterval |= ((((fminterval - 210) * 6) / 7) << 16);
-	writel (fminterval, &ohci->regs->fminterval);
-	writel (0x628, &ohci->regs->lsthresh);
-
-	/* start controller operations */
-	ohci->hc_control = OHCI_CONTROL_INIT | OHCI_USB_OPER;
-	ohci->disabled = 0;
-	writel (ohci->hc_control, &ohci->regs->control);
-
-	/* disable all interrupts */
-	mask = (OHCI_INTR_SO | OHCI_INTR_WDH | OHCI_INTR_SF | OHCI_INTR_RD |
-			OHCI_INTR_UE | OHCI_INTR_FNO | OHCI_INTR_RHSC |
-			OHCI_INTR_OC | OHCI_INTR_MIE);
-	writel (mask, &ohci->regs->intrdisable);
-	/* clear all interrupts */
-	mask &= ~OHCI_INTR_MIE;
-	writel (mask, &ohci->regs->intrstatus);
-	/* Choose the interrupts we care about now  - but w/o MIE */
-	mask = OHCI_INTR_RHSC | OHCI_INTR_UE | OHCI_INTR_WDH | OHCI_INTR_SO;
-	writel (mask, &ohci->regs->intrenable);
-
-#ifdef	OHCI_USE_NPS
-	/* required for AMD-756 and some Mac platforms */
-	writel ((roothub_a (ohci) | RH_A_NPS) & ~RH_A_PSM,
-		&ohci->regs->roothub.a);
-	writel (RH_HS_LPSC, &ohci->regs->roothub.status);
-#endif	/* OHCI_USE_NPS */
-
-#define mdelay(n) ({unsigned long msec=(n); while (msec--) udelay(1000);})
-	/* POTPGT delay is bits 24-31, in 2 ms units. */
-	mdelay ((roothub_a (ohci) >> 23) & 0x1fe);
-
-	/* connect the virtual root hub */
-	ohci->rh.devnum = 0;
-
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* an interrupt happens */
-
-static int
-hc_interrupt (void)
-{
-	ohci_t *ohci = &gohci;
-	struct ohci_regs *regs = ohci->regs;
-	int ints;
-	int stat = -1;
-
-	if ((ohci->hcca->done_head != 0) && !(m32_swap (ohci->hcca->done_head) & 0x01)) {
-		ints =	OHCI_INTR_WDH;
-	} else {
-		ints = readl (&regs->intrstatus);
-	}
-
-	/* dbg("Interrupt: %x frame: %x", ints, le16_to_cpu (ohci->hcca->frame_no)); */
-
-	if (ints & OHCI_INTR_RHSC) {
-		got_rhsc = 1;
-	}
-
-	if (ints & OHCI_INTR_UE) {
-		ohci->disabled++;
-		err ("OHCI Unrecoverable Error, controller usb-%s disabled",
-			ohci->slot_name);
-		/* e.g. due to PCI Master/Target Abort */
-
-#ifdef	DEBUG
-		ohci_dump (ohci, 1);
-#else
-	wait_ms(1);
-#endif
-		/* FIXME: be optimistic, hope that bug won't repeat often. */
-		/* Make some non-interrupt context restart the controller. */
-		/* Count and limit the retries though; either hardware or */
-		/* software errors can go forever... */
-		hc_reset (ohci);
-		return -1;
-	}
-
-	if (ints & OHCI_INTR_WDH) {
-		wait_ms(1);
-		writel (OHCI_INTR_WDH, &regs->intrdisable);
-		stat = dl_done_list (&gohci, dl_reverse_done_list (&gohci));
-		writel (OHCI_INTR_WDH, &regs->intrenable);
-	}
-
-	if (ints & OHCI_INTR_SO) {
-		dbg("USB Schedule overrun\n");
-		writel (OHCI_INTR_SO, &regs->intrenable);
-		stat = -1;
-	}
-
-	/* FIXME:  this assumes SOF (1/ms) interrupts don't get lost... */
-	if (ints & OHCI_INTR_SF) {
-		unsigned int frame = m16_swap (ohci->hcca->frame_no) & 1;
-		wait_ms(1);
-		writel (OHCI_INTR_SF, &regs->intrdisable);
-		if (ohci->ed_rm_list[frame] != NULL)
-			writel (OHCI_INTR_SF, &regs->intrenable);
-		stat = 0xff;
-	}
-
-	writel (ints, &regs->intrstatus);
-	return stat;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/*-------------------------------------------------------------------------*/
-
-/* De-allocate all resources.. */
-
-static void hc_release_ohci (ohci_t *ohci)
-{
-	dbg ("USB HC release ohci usb-%s", ohci->slot_name);
-
-	if (!ohci->disabled)
-		hc_reset (ohci);
-}
-
-/*-------------------------------------------------------------------------*/
-
-/*
- * low level initalisation routine, called from usb.c
- */
-static char ohci_inited = 0;
-
-int usb_lowlevel_init(void)
-{
-	/*
-	 * Enable USB host clock.
-	 */
-	*AT91C_PMC_SCER = AT91C_PMC_UHP;	/* 48MHz clock enabled for UHP */
-	*AT91C_PMC_PCER = 1 << AT91C_ID_UHP;	/* Peripheral Clock Enable Register */
-
-	memset (&gohci, 0, sizeof (ohci_t));
-	memset (&urb_priv, 0, sizeof (urb_priv_t));
-
-	/* align the storage */
-	if ((__u32)&ghcca[0] & 0xff) {
-		err("HCCA not aligned!!");
-		return -1;
-	}
-	phcca = &ghcca[0];
-	info("aligned ghcca %p", phcca);
-	memset(&ohci_dev, 0, sizeof(struct ohci_device));
-	if ((__u32)&ohci_dev.ed[0] & 0x7) {
-		err("EDs not aligned!!");
-		return -1;
-	}
-	memset(gtd, 0, sizeof(td_t) * (NUM_TD + 1));
-	if ((__u32)gtd & 0x7) {
-		err("TDs not aligned!!");
-		return -1;
-	}
-	ptd = gtd;
-	gohci.hcca = phcca;
-	memset (phcca, 0, sizeof (struct ohci_hcca));
-
-	gohci.disabled = 1;
-	gohci.sleeping = 0;
-	gohci.irq = -1;
-	gohci.regs = (struct ohci_regs *)AT91_USB_HOST_BASE;
-
-	gohci.flags = 0;
-	gohci.slot_name = "at91rm9200";
-
-	if (hc_reset (&gohci) < 0) {
-		hc_release_ohci (&gohci);
-		/* Initialization failed */
-		*AT91C_PMC_PCER = AT91C_ID_UHP;
-		*AT91C_PMC_SCDR = 1 << AT91C_PMC_UHP;	/* 48MHz clock disabled for UHP */
-		return -1;
-	}
-
-	/* FIXME this is a second HC reset; why?? */
-/*	writel (gohci.hc_control = OHCI_USB_RESET, &gohci.regs->control);
-	wait_ms (10);*/
-
-	if (hc_start (&gohci) < 0) {
-		err ("can't start usb-%s", gohci.slot_name);
-		hc_release_ohci (&gohci);
-		/* Initialization failed */
-		*AT91C_PMC_PCER = AT91C_ID_UHP;
-		*AT91C_PMC_SCDR = 1 << AT91C_PMC_UHP;	/* 48MHz clock disabled for UHP */
-		return -1;
-	}
-
-#ifdef	DEBUG
-	ohci_dump (&gohci, 1);
-#else
-	wait_ms(1);
-#endif
-	ohci_inited = 1;
-	return 0;
-}
-
-int usb_lowlevel_stop(void)
-{
-	/* this gets called really early - before the controller has */
-	/* even been initialized! */
-	if (!ohci_inited)
-		return 0;
-	/* TODO release any interrupts, etc. */
-	/* call hc_release_ohci() here ? */
-	hc_reset (&gohci);
-	/* may not want to do this */
-	*AT91C_PMC_PCER = 1 << AT91C_ID_UHP;
-	*AT91C_PMC_SCDR = 1 << AT91C_PMC_UHP;	/* 48MHz clock disabled for UHP */
-	return 0;
-}
-
-#endif /* CONFIG_USB_OHCI */
diff --git a/cpu/arm920t/at91rm9200/usb_ohci.h b/cpu/arm920t/at91rm9200/usb_ohci.h
deleted file mode 100644
index ecb4e937b1..0000000000
--- a/cpu/arm920t/at91rm9200/usb_ohci.h
+++ /dev/null
@@ -1,419 +0,0 @@
-/*
- * URB OHCI HCD (Host Controller Driver) for USB.
- *
- * (C) Copyright 1999 Roman Weissgaerber <weissg@vienna.at>
- * (C) Copyright 2000-2001 David Brownell <dbrownell@users.sourceforge.net>
- *
- * usb-ohci.h
- */
-
-
-static int cc_to_error[16] = {
-
-/* mapping of the OHCI CC status to error codes */
-	/* No  Error  */	       0,
-	/* CRC Error  */	       USB_ST_CRC_ERR,
-	/* Bit Stuff  */	       USB_ST_BIT_ERR,
-	/* Data Togg  */	       USB_ST_CRC_ERR,
-	/* Stall      */	       USB_ST_STALLED,
-	/* DevNotResp */	       -1,
-	/* PIDCheck   */	       USB_ST_BIT_ERR,
-	/* UnExpPID   */	       USB_ST_BIT_ERR,
-	/* DataOver   */	       USB_ST_BUF_ERR,
-	/* DataUnder  */	       USB_ST_BUF_ERR,
-	/* reservd    */	       -1,
-	/* reservd    */	       -1,
-	/* BufferOver */	       USB_ST_BUF_ERR,
-	/* BuffUnder  */	       USB_ST_BUF_ERR,
-	/* Not Access */	       -1,
-	/* Not Access */	       -1
-};
-
-/* ED States */
-
-#define ED_NEW		0x00
-#define ED_UNLINK	0x01
-#define ED_OPER		0x02
-#define ED_DEL		0x04
-#define ED_URB_DEL	0x08
-
-/* usb_ohci_ed */
-struct ed {
-	__u32 hwINFO;
-	__u32 hwTailP;
-	__u32 hwHeadP;
-	__u32 hwNextED;
-
-	struct ed *ed_prev;
-	__u8 int_period;
-	__u8 int_branch;
-	__u8 int_load;
-	__u8 int_interval;
-	__u8 state;
-	__u8 type;
-	__u16 last_iso;
-	struct ed *ed_rm_list;
-
-	struct usb_device *usb_dev;
-	__u32 unused[3];
-} __attribute((aligned(16)));
-typedef struct ed ed_t;
-
-
-/* TD info field */
-#define TD_CC	    0xf0000000
-#define TD_CC_GET(td_p) ((td_p >>28) & 0x0f)
-#define TD_CC_SET(td_p, cc) (td_p) = ((td_p) & 0x0fffffff) | (((cc) & 0x0f) << 28)
-#define TD_EC	    0x0C000000
-#define TD_T	    0x03000000
-#define TD_T_DATA0  0x02000000
-#define TD_T_DATA1  0x03000000
-#define TD_T_TOGGLE 0x00000000
-#define TD_R	    0x00040000
-#define TD_DI	    0x00E00000
-#define TD_DI_SET(X) (((X) & 0x07)<< 21)
-#define TD_DP	    0x00180000
-#define TD_DP_SETUP 0x00000000
-#define TD_DP_IN    0x00100000
-#define TD_DP_OUT   0x00080000
-
-#define TD_ISO	    0x00010000
-#define TD_DEL	    0x00020000
-
-/* CC Codes */
-#define TD_CC_NOERROR	   0x00
-#define TD_CC_CRC	   0x01
-#define TD_CC_BITSTUFFING  0x02
-#define TD_CC_DATATOGGLEM  0x03
-#define TD_CC_STALL	   0x04
-#define TD_DEVNOTRESP	   0x05
-#define TD_PIDCHECKFAIL	   0x06
-#define TD_UNEXPECTEDPID   0x07
-#define TD_DATAOVERRUN	   0x08
-#define TD_DATAUNDERRUN	   0x09
-#define TD_BUFFEROVERRUN   0x0C
-#define TD_BUFFERUNDERRUN  0x0D
-#define TD_NOTACCESSED	   0x0F
-
-
-#define MAXPSW 1
-
-struct td {
-	__u32 hwINFO;
-	__u32 hwCBP;		/* Current Buffer Pointer */
-	__u32 hwNextTD;		/* Next TD Pointer */
-	__u32 hwBE;		/* Memory Buffer End Pointer */
-
-	__u16 hwPSW[MAXPSW];
-	__u8 unused;
-	__u8 index;
-	struct ed *ed;
-	struct td *next_dl_td;
-	struct usb_device *usb_dev;
-	int transfer_len;
-	__u32 data;
-
-	__u32 unused2[2];
-} __attribute((aligned(32)));
-typedef struct td td_t;
-
-#define OHCI_ED_SKIP	(1 << 14)
-
-/*
- * The HCCA (Host Controller Communications Area) is a 256 byte
- * structure defined in the OHCI spec. that the host controller is
- * told the base address of.  It must be 256-byte aligned.
- */
-
-#define NUM_INTS 32	/* part of the OHCI standard */
-struct ohci_hcca {
-	__u32	int_table[NUM_INTS];	/* Interrupt ED table */
-	__u16	frame_no;		/* current frame number */
-	__u16	pad1;			/* set to 0 on each frame_no change */
-	__u32	done_head;		/* info returned for an interrupt */
-	u8		reserved_for_hc[116];
-} __attribute((aligned(256)));
-
-
-/*
- * Maximum number of root hub ports.
- */
-#define MAX_ROOT_PORTS	15	/* maximum OHCI root hub ports */
-
-/*
- * This is the structure of the OHCI controller's memory mapped I/O
- * region.  This is Memory Mapped I/O.	You must use the readl() and
- * writel() macros defined in asm/io.h to access these!!
- */
-struct ohci_regs {
-	/* control and status registers */
-	__u32	revision;
-	__u32	control;
-	__u32	cmdstatus;
-	__u32	intrstatus;
-	__u32	intrenable;
-	__u32	intrdisable;
-	/* memory pointers */
-	__u32	hcca;
-	__u32	ed_periodcurrent;
-	__u32	ed_controlhead;
-	__u32	ed_controlcurrent;
-	__u32	ed_bulkhead;
-	__u32	ed_bulkcurrent;
-	__u32	donehead;
-	/* frame counters */
-	__u32	fminterval;
-	__u32	fmremaining;
-	__u32	fmnumber;
-	__u32	periodicstart;
-	__u32	lsthresh;
-	/* Root hub ports */
-	struct	ohci_roothub_regs {
-		__u32	a;
-		__u32	b;
-		__u32	status;
-		__u32	portstatus[MAX_ROOT_PORTS];
-	} roothub;
-} __attribute((aligned(32)));
-
-
-/* OHCI CONTROL AND STATUS REGISTER MASKS */
-
-/*
- * HcControl (control) register masks
- */
-#define OHCI_CTRL_CBSR	(3 << 0)	/* control/bulk service ratio */
-#define OHCI_CTRL_PLE	(1 << 2)	/* periodic list enable */
-#define OHCI_CTRL_IE	(1 << 3)	/* isochronous enable */
-#define OHCI_CTRL_CLE	(1 << 4)	/* control list enable */
-#define OHCI_CTRL_BLE	(1 << 5)	/* bulk list enable */
-#define OHCI_CTRL_HCFS	(3 << 6)	/* host controller functional state */
-#define OHCI_CTRL_IR	(1 << 8)	/* interrupt routing */
-#define OHCI_CTRL_RWC	(1 << 9)	/* remote wakeup connected */
-#define OHCI_CTRL_RWE	(1 << 10)	/* remote wakeup enable */
-
-/* pre-shifted values for HCFS */
-#	define OHCI_USB_RESET	(0 << 6)
-#	define OHCI_USB_RESUME	(1 << 6)
-#	define OHCI_USB_OPER	(2 << 6)
-#	define OHCI_USB_SUSPEND (3 << 6)
-
-/*
- * HcCommandStatus (cmdstatus) register masks
- */
-#define OHCI_HCR	(1 << 0)	/* host controller reset */
-#define OHCI_CLF	(1 << 1)	/* control list filled */
-#define OHCI_BLF	(1 << 2)	/* bulk list filled */
-#define OHCI_OCR	(1 << 3)	/* ownership change request */
-#define OHCI_SOC	(3 << 16)	/* scheduling overrun count */
-
-/*
- * masks used with interrupt registers:
- * HcInterruptStatus (intrstatus)
- * HcInterruptEnable (intrenable)
- * HcInterruptDisable (intrdisable)
- */
-#define OHCI_INTR_SO	(1 << 0)	/* scheduling overrun */
-#define OHCI_INTR_WDH	(1 << 1)	/* writeback of done_head */
-#define OHCI_INTR_SF	(1 << 2)	/* start frame */
-#define OHCI_INTR_RD	(1 << 3)	/* resume detect */
-#define OHCI_INTR_UE	(1 << 4)	/* unrecoverable error */
-#define OHCI_INTR_FNO	(1 << 5)	/* frame number overflow */
-#define OHCI_INTR_RHSC	(1 << 6)	/* root hub status change */
-#define OHCI_INTR_OC	(1 << 30)	/* ownership change */
-#define OHCI_INTR_MIE	(1 << 31)	/* master interrupt enable */
-
-
-/* Virtual Root HUB */
-struct virt_root_hub {
-	int devnum; /* Address of Root Hub endpoint */
-	void *dev;  /* was urb */
-	void *int_addr;
-	int send;
-	int interval;
-};
-
-/* USB HUB CONSTANTS (not OHCI-specific; see hub.h) */
-
-/* destination of request */
-#define RH_INTERFACE		   0x01
-#define RH_ENDPOINT		   0x02
-#define RH_OTHER		   0x03
-
-#define RH_CLASS		   0x20
-#define RH_VENDOR		   0x40
-
-/* Requests: bRequest << 8 | bmRequestType */
-#define RH_GET_STATUS		0x0080
-#define RH_CLEAR_FEATURE	0x0100
-#define RH_SET_FEATURE		0x0300
-#define RH_SET_ADDRESS		0x0500
-#define RH_GET_DESCRIPTOR	0x0680
-#define RH_SET_DESCRIPTOR	0x0700
-#define RH_GET_CONFIGURATION	0x0880
-#define RH_SET_CONFIGURATION	0x0900
-#define RH_GET_STATE		0x0280
-#define RH_GET_INTERFACE	0x0A80
-#define RH_SET_INTERFACE	0x0B00
-#define RH_SYNC_FRAME		0x0C80
-/* Our Vendor Specific Request */
-#define RH_SET_EP		0x2000
-
-
-/* Hub port features */
-#define RH_PORT_CONNECTION	   0x00
-#define RH_PORT_ENABLE		   0x01
-#define RH_PORT_SUSPEND		   0x02
-#define RH_PORT_OVER_CURRENT	   0x03
-#define RH_PORT_RESET		   0x04
-#define RH_PORT_POWER		   0x08
-#define RH_PORT_LOW_SPEED	   0x09
-
-#define RH_C_PORT_CONNECTION	   0x10
-#define RH_C_PORT_ENABLE	   0x11
-#define RH_C_PORT_SUSPEND	   0x12
-#define RH_C_PORT_OVER_CURRENT	   0x13
-#define RH_C_PORT_RESET		   0x14
-
-/* Hub features */
-#define RH_C_HUB_LOCAL_POWER	   0x00
-#define RH_C_HUB_OVER_CURRENT	   0x01
-
-#define RH_DEVICE_REMOTE_WAKEUP	   0x00
-#define RH_ENDPOINT_STALL	   0x01
-
-#define RH_ACK			   0x01
-#define RH_REQ_ERR		   -1
-#define RH_NACK			   0x00
-
-
-/* OHCI ROOT HUB REGISTER MASKS */
-
-/* roothub.portstatus [i] bits */
-#define RH_PS_CCS	     0x00000001		/* current connect status */
-#define RH_PS_PES	     0x00000002		/* port enable status*/
-#define RH_PS_PSS	     0x00000004		/* port suspend status */
-#define RH_PS_POCI	     0x00000008		/* port over current indicator */
-#define RH_PS_PRS	     0x00000010		/* port reset status */
-#define RH_PS_PPS	     0x00000100		/* port power status */
-#define RH_PS_LSDA	     0x00000200		/* low speed device attached */
-#define RH_PS_CSC	     0x00010000		/* connect status change */
-#define RH_PS_PESC	     0x00020000		/* port enable status change */
-#define RH_PS_PSSC	     0x00040000		/* port suspend status change */
-#define RH_PS_OCIC	     0x00080000		/* over current indicator change */
-#define RH_PS_PRSC	     0x00100000		/* port reset status change */
-
-/* roothub.status bits */
-#define RH_HS_LPS	     0x00000001		/* local power status */
-#define RH_HS_OCI	     0x00000002		/* over current indicator */
-#define RH_HS_DRWE	     0x00008000		/* device remote wakeup enable */
-#define RH_HS_LPSC	     0x00010000		/* local power status change */
-#define RH_HS_OCIC	     0x00020000		/* over current indicator change */
-#define RH_HS_CRWE	     0x80000000		/* clear remote wakeup enable */
-
-/* roothub.b masks */
-#define RH_B_DR		0x0000ffff		/* device removable flags */
-#define RH_B_PPCM	0xffff0000		/* port power control mask */
-
-/* roothub.a masks */
-#define RH_A_NDP	(0xff << 0)		/* number of downstream ports */
-#define RH_A_PSM	(1 << 8)		/* power switching mode */
-#define RH_A_NPS	(1 << 9)		/* no power switching */
-#define RH_A_DT		(1 << 10)		/* device type (mbz) */
-#define RH_A_OCPM	(1 << 11)		/* over current protection mode */
-#define RH_A_NOCP	(1 << 12)		/* no over current protection */
-#define RH_A_POTPGT	(0xff << 24)		/* power on to power good time */
-
-/* urb */
-#define N_URB_TD 48
-typedef struct
-{
-	ed_t *ed;
-	__u16 length;	/* number of tds associated with this request */
-	__u16 td_cnt;	/* number of tds already serviced */
-	int   state;
-	unsigned long pipe;
-	int actual_length;
-	td_t *td[N_URB_TD];	/* list pointer to all corresponding TDs associated with this request */
-} urb_priv_t;
-#define URB_DEL 1
-
-/*
- * This is the full ohci controller description
- *
- * Note how the "proper" USB information is just
- * a subset of what the full implementation needs. (Linus)
- */
-
-
-typedef struct ohci {
-	struct ohci_hcca *hcca;		/* hcca */
-	/*dma_addr_t hcca_dma;*/
-
-	int irq;
-	int disabled;			/* e.g. got a UE, we're hung */
-	int sleeping;
-	unsigned long flags;		/* for HC bugs */
-
-	struct ohci_regs *regs; /* OHCI controller's memory */
-
-	ed_t *ed_rm_list[2];	 /* lists of all endpoints to be removed */
-	ed_t *ed_bulktail;	 /* last endpoint of bulk list */
-	ed_t *ed_controltail;	 /* last endpoint of control list */
-	int intrstatus;
-	__u32 hc_control;		/* copy of the hc control reg */
-	struct usb_device *dev[32];
-	struct virt_root_hub rh;
-
-	const char	*slot_name;
-} ohci_t;
-
-#define NUM_EDS 8		/* num of preallocated endpoint descriptors */
-
-struct ohci_device {
-	ed_t	ed[NUM_EDS];
-	int ed_cnt;
-};
-
-/* hcd */
-/* endpoint */
-static int ep_link(ohci_t * ohci, ed_t * ed);
-static int ep_unlink(ohci_t * ohci, ed_t * ed);
-static ed_t * ep_add_ed(struct usb_device * usb_dev, unsigned long pipe);
-
-/*-------------------------------------------------------------------------*/
-
-/* we need more TDs than EDs */
-#define NUM_TD 64
-
-/* +1 so we can align the storage */
-td_t gtd[NUM_TD+1];
-/* pointers to aligned storage */
-td_t *ptd;
-
-/* TDs ... */
-static inline struct td *
-td_alloc (struct usb_device *usb_dev)
-{
-	int i;
-	struct td	*td;
-
-	td = NULL;
-	for (i = 0; i < NUM_TD; i++)
-	{
-		if (ptd[i].usb_dev == NULL)
-		{
-			td = &ptd[i];
-			td->usb_dev = usb_dev;
-			break;
-		}
-	}
-
-	return td;
-}
-
-static inline void
-ed_free (struct ed *ed)
-{
-	ed->usb_dev = NULL;
-}
diff --git a/cpu/arm920t/s3c24x0/Makefile b/cpu/arm920t/s3c24x0/Makefile
index 3a7c4b35fd..0ff36c596a 100644
--- a/cpu/arm920t/s3c24x0/Makefile
+++ b/cpu/arm920t/s3c24x0/Makefile
@@ -26,7 +26,7 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(SOC).a
 
 COBJS	= i2c.o interrupts.o serial.o speed.o \
-	  usb_ohci.o
+	  usb.o
 
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/arm920t/s3c24x0/usb.c b/cpu/arm920t/s3c24x0/usb.c
new file mode 100644
index 0000000000..ef5d5bf71b
--- /dev/null
+++ b/cpu/arm920t/s3c24x0/usb.c
@@ -0,0 +1,72 @@
+/*
+ * (C) Copyright 2006
+ * DENX Software Engineering <mk@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#if defined(CONFIG_USB_OHCI_NEW) && defined(CFG_USB_OHCI_CPU_INIT)
+# if defined(CONFIG_S3C2400) || defined(CONFIG_S3C2410)
+
+#if defined(CONFIG_S3C2400)
+# include <s3c2400.h>
+#elif defined(CONFIG_S3C2410)
+# include <s3c2410.h>
+#endif
+
+int usb_cpu_init (void)
+{
+
+	S3C24X0_CLOCK_POWER * const clk_power = S3C24X0_GetBase_CLOCK_POWER();
+	S3C24X0_GPIO * const gpio = S3C24X0_GetBase_GPIO();
+
+	/*
+	 * Set the 48 MHz UPLL clocking. Values are taken from
+	 * "PLL value selection guide", 6-23, s3c2400_UM.pdf.
+	 */
+	clk_power->UPLLCON = ((40 << 12) + (1 << 4) + 2);
+	gpio->MISCCR |= 0x8; /* 1 = use pads related USB for USB host */
+
+	/*
+	 * Enable USB host clock.
+	 */
+	clk_power->CLKCON |= (1 << 4);
+
+	return 0;
+}
+
+int usb_cpu_stop (void)
+{
+	S3C24X0_CLOCK_POWER * const clk_power = S3C24X0_GetBase_CLOCK_POWER();
+	/* may not want to do this */
+	clk_power->CLKCON &= ~(1 << 4);
+	return 0;
+}
+
+int usb_cpu_init_fail (void)
+{
+	S3C24X0_CLOCK_POWER * const clk_power = S3C24X0_GetBase_CLOCK_POWER();
+	clk_power->CLKCON &= ~(1 << 4);
+	return 0;
+}
+
+# endif /* defined(CONFIG_S3C2400) || defined(CONFIG_S3C2410) */
+#endif /* defined(CONFIG_USB_OHCI) && defined(CFG_USB_OHCI_CPU_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/i386/serial.c b/cpu/i386/serial.c
index e7299a7ebb..baf35e53d5 100644
--- a/cpu/i386/serial.c
+++ b/cpu/i386/serial.c
@@ -394,7 +394,7 @@ int serial_buffered_tstc(void)
 #endif	/* CONFIG_SERIAL_SOFTWARE_FIFO */
 
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 /*
   AS HARNOIS : according to CONFIG_KGDB_SER_INDEX kgdb uses serial port
   number 0 or number 1
@@ -500,4 +500,4 @@ void kgdb_interruptible(int yes)
 	return;
 }
 #endif	/* (CONFIG_KGDB_SER_INDEX & 2) */
-#endif	/* CFG_CMD_KGDB */
+#endif
diff --git a/cpu/i386/start.S b/cpu/i386/start.S
index afcbb24520..1a54dd10e3 100644
--- a/cpu/i386/start.S
+++ b/cpu/i386/start.S
@@ -149,7 +149,7 @@ data_ok:
 .progress3:
 
 	/* clear bss section in ram, size must be 4-byte aligned  */
-	movl	$_i386boot_bss_start, %eax        /* BSS start */
+	movl	$_i386boot_bss_start, %edi        /* MK_CHG BSS start */
 	movl	$_i386boot_bss_size, %ecx         /* BSS size */
 	movl	%ecx, %eax
 	andl	$3, %eax
diff --git a/cpu/ixp/cpu.c b/cpu/ixp/cpu.c
index 7f9f3344b3..2c7d5a01be 100644
--- a/cpu/ixp/cpu.c
+++ b/cpu/ixp/cpu.c
@@ -85,7 +85,7 @@ int cpu_init (void)
 	FIQ_STACK_START = IRQ_STACK_START - CONFIG_STACKSIZE_IRQ;
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_PCI) || defined (CONFIG_PCI)
+#if defined(CONFIG_CMD_PCI) || defined (CONFIG_PCI)
 	pci_init();
 #endif
 	return 0;
diff --git a/cpu/ixp/npe/npe.c b/cpu/ixp/npe/npe.c
index ab7ca8bef0..7e4af44105 100644
--- a/cpu/ixp/npe/npe.c
+++ b/cpu/ixp/npe/npe.c
@@ -682,7 +682,7 @@ int npe_initialize(bd_t * bis)
 
 		eth_register(dev);
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 		miiphy_register(dev->name, npe_miiphy_read, npe_miiphy_write);
 #endif
 
diff --git a/cpu/mcf52x2/fec.c b/cpu/mcf52x2/fec.c
index b6540b55af..ef9c7984ca 100644
--- a/cpu/mcf52x2/fec.c
+++ b/cpu/mcf52x2/fec.c
@@ -53,7 +53,7 @@
 #undef	ET_DEBUG
 #undef	MII_DEBUG
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(FEC_ENET)
+#if defined(CONFIG_CMD_NET) && defined(FEC_ENET)
 
 #ifdef CFG_DISCOVER_PHY
 #include <miiphy.h>
@@ -363,7 +363,7 @@ void eth_halt (void)
 }
 
 
-#if defined(CFG_DISCOVER_PHY) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CFG_DISCOVER_PHY) || defined(CONFIG_CMD_MII)
 
 static int phyaddr = -1;	/* didn't find a PHY yet */
 static uint phytype;
@@ -419,7 +419,7 @@ static uint mii_send (uint mii_cmd)
 #endif
 	return (mii_reply & 0xffff);	/* data read from phy */
 }
-#endif /* CFG_DISCOVER_PHY || (CONFIG_COMMANDS & CFG_CMD_MII) */
+#endif
 
 #if defined(CFG_DISCOVER_PHY)
 static void mii_discover_phy (void)
@@ -485,7 +485,7 @@ static void mii_discover_phy (void)
 }
 #endif /* CFG_DISCOVER_PHY */
 
-#if (CONFIG_COMMANDS & CFG_CMD_MII) && !defined(CONFIG_BITBANGMII)
+#if defined(CONFIG_CMD_MII) && !defined(CONFIG_BITBANGMII)
 
 static int mii_init_done = 0;
 
@@ -591,13 +591,13 @@ int mcf52x2_miiphy_write (char *devname, unsigned char addr,
 
 	return 0;
 }
-#endif /* (CONFIG_COMMANDS & CFG_CMD_MII) && !defined(CONFIG_BITBANGMII) */
-#endif /* CFG_CMD_NET, FEC_ENET */
+#endif
+#endif
 
 int mcf52x2_miiphy_initialize(bd_t *bis)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(FEC_ENET)
-#if (CONFIG_COMMANDS & CFG_CMD_MII) && !defined(CONFIG_BITBANGMII)
+#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
diff --git a/cpu/microblaze/cache.c b/cpu/microblaze/cache.c
index 4f36a84ec4..6ce0b55b24 100755..100644
--- a/cpu/microblaze/cache.c
+++ b/cpu/microblaze/cache.c
@@ -25,7 +25,7 @@
 #include <common.h>
 #include <asm/asm.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_CACHE)
+#if defined(CONFIG_CMD_CACHE)
 
 int dcache_status (void)
 {
diff --git a/cpu/microblaze/interrupts.c b/cpu/microblaze/interrupts.c
index b61153f8e6..3f04b29983 100755..100644
--- a/cpu/microblaze/interrupts.c
+++ b/cpu/microblaze/interrupts.c
@@ -190,7 +190,7 @@ void interrupt_handler (void)
 }
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 #ifdef CFG_INTC_0
 int do_irqinfo (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 {
@@ -217,4 +217,4 @@ int do_irqinfo (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 	puts ("Undefined interrupt controller\n");
 }
 #endif
-#endif				/* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/microblaze/irq.S b/cpu/microblaze/irq.S
index e1fc19046c..e1fc19046c 100755..100644
--- a/cpu/microblaze/irq.S
+++ b/cpu/microblaze/irq.S
diff --git a/cpu/mips/au1x00_eth.c b/cpu/mips/au1x00_eth.c
index 078e8328b6..b69741ae68 100644
--- a/cpu/mips/au1x00_eth.c
+++ b/cpu/mips/au1x00_eth.c
@@ -63,7 +63,7 @@
 #include <asm/io.h>
 #include <asm/au1x00.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_CMD_MII)
 #include <miiphy.h>
 #endif
 
@@ -241,7 +241,7 @@ int au1x00_enet_initialize(bd_t *bis){
 
 	eth_register(dev);
 
-#if (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_CMD_MII)
 	miiphy_register(dev->name,
 		au1x00_miiphy_read, au1x00_miiphy_write);
 #endif
@@ -249,7 +249,7 @@ int au1x00_enet_initialize(bd_t *bis){
 	return 1;
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_CMD_MII)
 int  au1x00_miiphy_read(char *devname, unsigned char addr,
 		unsigned char reg, unsigned short * value)
 {
@@ -306,6 +306,6 @@ int  au1x00_miiphy_write(char *devname, unsigned char addr,
 	*mii_control_reg = mii_control;
 	return 0;
 }
-#endif	/* CONFIG_COMMANDS & CFG_CMD_MII */
+#endif
 
 #endif /* CONFIG_AU1X00 */
diff --git a/cpu/mpc512x/Makefile b/cpu/mpc512x/Makefile
new file mode 100644
index 0000000000..2be35b2bc6
--- /dev/null
+++ b/cpu/mpc512x/Makefile
@@ -0,0 +1,46 @@
+#
+# (C) Copyright 2007 DENX Software Engineering
+#
+# 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$(CPU).a
+
+START	= start.o
+COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o serial.o fec.o i2c.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)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/mpc512x/config.mk b/cpu/mpc512x/config.mk
new file mode 100644
index 0000000000..8a07c5a3b6
--- /dev/null
+++ b/cpu/mpc512x/config.mk
@@ -0,0 +1,25 @@
+#
+# (C) Copyright 2007 DENX Software Engineering
+#
+# 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 += -fPIC -ffixed-r14 -meabi
+
+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
new file mode 100644
index 0000000000..accae6e066
--- /dev/null
+++ b/cpu/mpc512x/cpu.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2004-2006 Freescale Semiconductor, Inc.
+ * (C) Copyright 2007 DENX Software Engineering
+ *
+ * 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 code for the MPC512x family.
+ *
+ * Derived from the MPC83xx code.
+ */
+
+#include <common.h>
+#include <command.h>
+#include <mpc512x.h>
+#include <asm/processor.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+int checkcpu (void)
+{
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+	ulong clock = gd->cpu_clk;
+	u32 pvr = get_pvr ();
+	u32 spridr = immr->sysconf.spridr;
+	char buf[32];
+
+	puts ("CPU:   ");
+
+	switch (spridr & 0xffff0000) {
+	case SPR_5121E:
+		puts ("MPC5121e ");
+		break;
+	default:
+		printf ("Unknown part ID %08x ", spridr & 0xffff0000);
+	}
+	printf ("rev. %d.%d, Core ", SVR_MJREV (spridr), SVR_MNREV (spridr));
+
+	switch (pvr & 0xffff0000) {
+	case PVR_E300C4:
+		puts ("e300c4 ");
+		break;
+	default:
+		puts ("unknown ");
+	}
+	printf ("at %s MHz, CSB at %3d MHz\n", strmhz(buf, clock),
+		gd->csb_clk / 1000000);
+	return 0;
+}
+
+
+int
+do_reset (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	ulong msr;
+	volatile immap_t *immap = (immap_t *) CFG_IMMR;
+
+	/* Interrupts and MMU off */
+	__asm__ __volatile__ ("mfmsr    %0":"=r" (msr):);
+
+	msr &= ~( MSR_EE | MSR_IR | MSR_DR);
+	__asm__ __volatile__ ("mtmsr    %0"::"r" (msr));
+
+	/*
+	 * Enable Reset Control Reg - "RSTE" is the magic word that let us go
+	 */
+	immap->reset.rpr = 0x52535445;
+
+	/* Verify Reset Control Reg is enabled */
+	while (!((immap->reset.rcer) & RCER_CRE))
+		;
+
+	printf ("Resetting the board.\n");
+	udelay(200);
+
+	/* Perform reset */
+	immap->reset.rcr = RCR_SWHR;
+
+	/* Unreached... */
+	return 1;
+}
+
+
+/*
+ * Get timebase clock frequency (like cpu_clk in Hz)
+ */
+unsigned long get_tbclk (void)
+{
+	ulong tbclk;
+
+	tbclk = (gd->bus_clk + 3L) / 4L;
+
+	return tbclk;
+}
+
+
+#if defined(CONFIG_WATCHDOG)
+void watchdog_reset (void)
+{
+	int re_enable = disable_interrupts ();
+
+	/* Reset watchdog */
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+	immr->wdt.swsrr = 0x556c;
+	immr->wdt.swsrr = 0xaa39;
+
+	if (re_enable)
+		enable_interrupts ();
+}
+#endif
diff --git a/cpu/mpc512x/cpu_init.c b/cpu/mpc512x/cpu_init.c
new file mode 100644
index 0000000000..d6949f6bba
--- /dev/null
+++ b/cpu/mpc512x/cpu_init.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2004-2006 Freescale Semiconductor, Inc.
+ * (C) Copyright 2007 DENX Software Engineering
+ *
+ * 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
+ *
+ * Derived from the MPC83xx code.
+ *
+ */
+
+#include <common.h>
+#include <mpc512x.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/*
+ * Set up the memory map, initialize registers,
+ */
+void cpu_init_f (volatile immap_t * im)
+{
+	u32 ips_div;
+
+	/* Pointer is writable since we allocated a register for it */
+	gd = (gd_t *) (CFG_INIT_RAM_ADDR + CFG_GBL_DATA_OFFSET);
+
+	/* Clear initial global data */
+	memset ((void *) gd, 0, sizeof (gd_t));
+
+	/* system performance tweaking */
+
+#ifdef CFG_ACR_PIPE_DEP
+	/* Arbiter pipeline depth */
+	im->arbiter.acr = (im->arbiter.acr & ~ACR_PIPE_DEP) |
+			  (CFG_ACR_PIPE_DEP << ACR_PIPE_DEP_SHIFT);
+#endif
+
+#ifdef CFG_ACR_RPTCNT
+	/* Arbiter repeat count */
+	im->arbiter.acr = ((im->arbiter.acr & ~(ACR_RPTCNT)) |
+			   (CFG_ACR_RPTCNT << ACR_RPTCNT_SHIFT));
+#endif
+
+	/* RSR - Reset Status Register - clear all status */
+	gd->reset_status = im->reset.rsr;
+	im->reset.rsr = ~(RSR_RES);
+
+	/*
+	 * RMR - Reset Mode Register - enable checkstop reset
+	 */
+	im->reset.rmr = (RMR_CSRE & (1 << RMR_CSRE_SHIFT));
+
+	/* Set IPS-CSB divider: IPS = 1/2 CSB */
+	ips_div = im->clk.scfr[0];
+	ips_div &= ~(SCFR1_IPS_DIV_MASK);
+	ips_div |= SCFR1_IPS_DIV << SCFR1_IPS_DIV_SHIFT;
+	im->clk.scfr[0] = ips_div;
+
+	/*
+	 * Enable Time Base/Decrementer
+	 *
+	 * NOTICE: TB needs to be enabled as early as possible in order to
+	 * have udelay() working; if not enabled, usually leads to a hang, like
+	 * during FLASH chip identification etc.
+	 */
+	im->sysconf.spcr |= SPCR_TBEN;
+}
+
+int cpu_init_r (void)
+{
+	return 0;
+}
diff --git a/cpu/mpc512x/fec.c b/cpu/mpc512x/fec.c
new file mode 100644
index 0000000000..3c142a9e58
--- /dev/null
+++ b/cpu/mpc512x/fec.c
@@ -0,0 +1,801 @@
+/*
+ * (C) Copyright 2003-2007
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Derived from the MPC8xx FEC driver.
+ * Adapted for MPC512x by Grzegorz Bernacki <gjb@semihalf.com>
+ */
+
+#include <common.h>
+#include <mpc512x.h>
+#include <malloc.h>
+#include <net.h>
+#include <miiphy.h>
+#include "fec.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define DEBUG 0
+
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
+	defined(CONFIG_MPC512x_FEC)
+
+#if !(defined(CONFIG_MII) || defined(CONFIG_CMD_MII))
+#error "CONFIG_MII has to be defined!"
+#endif
+
+#if (DEBUG & 0x40)
+static uint32 local_crc32(char *string, unsigned int crc_value, int len);
+#endif
+
+int fec512x_miiphy_read(char *devname, uint8 phyAddr, uint8 regAddr, uint16 * retVal);
+int fec512x_miiphy_write(char *devname, uint8 phyAddr, uint8 regAddr, uint16 data);
+int mpc512x_fec_init_phy(struct eth_device *dev, bd_t * bis);
+
+/********************************************************************/
+#if (DEBUG & 0x2)
+static void mpc512x_fec_phydump (char *devname)
+{
+	uint16 phyStatus, i;
+	uint8 phyAddr = CONFIG_PHY_ADDR;
+	uint8 reg_mask[] = {
+		/* regs to print: 0...8, 21,27,31 */
+		1, 1, 1, 1,  1, 1, 1, 1,     1, 0, 0, 0,  0, 0, 0, 0,
+		0, 0, 0, 0,  0, 1, 0, 0,     0, 0, 0, 1,  0, 0, 0, 1,
+	};
+
+	for (i = 0; i < 32; i++) {
+		if (reg_mask[i]) {
+			miiphy_read (devname, phyAddr, i, &phyStatus);
+			printf ("Mii reg %d: 0x%04x\n", i, phyStatus);
+		}
+	}
+}
+#endif
+
+/********************************************************************/
+static int mpc512x_fec_bd_init (mpc512x_fec_priv *fec)
+{
+	int ix;
+
+	/*
+	 * Receive BDs init
+	 */
+	for (ix = 0; ix < FEC_RBD_NUM; ix++) {
+		fec->bdBase->rbd[ix].dataPointer = (uint32)&fec->bdBase->recv_frames[ix];
+		fec->bdBase->rbd[ix].status = FEC_RBD_EMPTY;
+		fec->bdBase->rbd[ix].dataLength = 0;
+	}
+
+	/*
+	 * have the last RBD to close the ring
+	 */
+	fec->bdBase->rbd[ix - 1].status |= FEC_RBD_WRAP;
+	fec->rbdIndex = 0;
+
+	/*
+	 * Trasmit BDs init
+	 */
+	for (ix = 0; ix < FEC_TBD_NUM; ix++) {
+		fec->bdBase->tbd[ix].status = 0;
+	}
+
+	/*
+	 * Have the last TBD to close the ring
+	 */
+	fec->bdBase->tbd[ix - 1].status |= FEC_TBD_WRAP;
+
+	/*
+	 * Initialize some indices
+	 */
+	fec->tbdIndex = 0;
+	fec->usedTbdIndex = 0;
+	fec->cleanTbdNum = FEC_TBD_NUM;
+
+	return 0;
+}
+
+/********************************************************************/
+static void mpc512x_fec_rbd_clean (mpc512x_fec_priv *fec, volatile FEC_RBD * pRbd)
+{
+	/*
+	 * Reset buffer descriptor as empty
+	 */
+	if ((fec->rbdIndex) == (FEC_RBD_NUM - 1))
+		pRbd->status = (FEC_RBD_WRAP | FEC_RBD_EMPTY);
+	else
+		pRbd->status = FEC_RBD_EMPTY;
+
+	pRbd->dataLength = 0;
+
+	/*
+	 * Increment BD count
+	 */
+	fec->rbdIndex = (fec->rbdIndex + 1) % FEC_RBD_NUM;
+
+	/*
+	 * Now, we have an empty RxBD, notify FEC
+	 */
+	fec->eth->r_des_active = 0x01000000;	/* Descriptor polling active */
+}
+
+/********************************************************************/
+static void mpc512x_fec_tbd_scrub (mpc512x_fec_priv *fec)
+{
+	volatile FEC_TBD *pUsedTbd;
+
+#if (DEBUG & 0x1)
+	printf ("tbd_scrub: fec->cleanTbdNum = %d, fec->usedTbdIndex = %d\n",
+		fec->cleanTbdNum, fec->usedTbdIndex);
+#endif
+
+	/*
+	 * process all the consumed TBDs
+	 */
+	while (fec->cleanTbdNum < FEC_TBD_NUM) {
+		pUsedTbd = &fec->bdBase->tbd[fec->usedTbdIndex];
+		if (pUsedTbd->status & FEC_TBD_READY) {
+#if (DEBUG & 0x20)
+			printf ("Cannot clean TBD %d, in use\n", fec->usedTbdIndex);
+#endif
+			return;
+		}
+
+		/*
+		 * clean this buffer descriptor
+		 */
+		if (fec->usedTbdIndex == (FEC_TBD_NUM - 1))
+			pUsedTbd->status = FEC_TBD_WRAP;
+		else
+			pUsedTbd->status = 0;
+
+		/*
+		 * update some indeces for a correct handling of the TBD ring
+		 */
+		fec->cleanTbdNum++;
+		fec->usedTbdIndex = (fec->usedTbdIndex + 1) % FEC_TBD_NUM;
+	}
+}
+
+/********************************************************************/
+static void mpc512x_fec_set_hwaddr (mpc512x_fec_priv *fec, char *mac)
+{
+	uint8 currByte;			/* byte for which to compute the CRC */
+	int byte;			/* loop - counter */
+	int bit;			/* loop - counter */
+	uint32 crc = 0xffffffff;	/* initial value */
+
+	/*
+	 * The algorithm used is the following:
+	 * we loop on each of the six bytes of the provided address,
+	 * and we compute the CRC by left-shifting the previous
+	 * value by one position, so that each bit in the current
+	 * byte of the address may contribute the calculation. If
+	 * the latter and the MSB in the CRC are different, then
+	 * the CRC value so computed is also ex-ored with the
+	 * "polynomium generator". The current byte of the address
+	 * is also shifted right by one bit at each iteration.
+	 * This is because the CRC generatore in hardware is implemented
+	 * as a shift-register with as many ex-ores as the radixes
+	 * in the polynomium. This suggests that we represent the
+	 * polynomiumm itself as a 32-bit constant.
+	 */
+	for (byte = 0; byte < 6; byte++) {
+		currByte = mac[byte];
+		for (bit = 0; bit < 8; bit++) {
+			if ((currByte & 0x01) ^ (crc & 0x01)) {
+				crc >>= 1;
+				crc = crc ^ 0xedb88320;
+			} else {
+				crc >>= 1;
+			}
+			currByte >>= 1;
+		}
+	}
+
+	crc = crc >> 26;
+
+	/*
+	 * Set individual hash table register
+	 */
+	if (crc >= 32) {
+		fec->eth->iaddr1 = (1 << (crc - 32));
+		fec->eth->iaddr2 = 0;
+	} else {
+		fec->eth->iaddr1 = 0;
+		fec->eth->iaddr2 = (1 << crc);
+	}
+
+	/*
+	 * Set physical address
+	 */
+	fec->eth->paddr1 = (mac[0] << 24) + (mac[1] << 16) + (mac[2] << 8) + mac[3];
+	fec->eth->paddr2 = (mac[4] << 24) + (mac[5] << 16) + 0x8808;
+}
+
+/********************************************************************/
+static int mpc512x_fec_init (struct eth_device *dev, bd_t * bis)
+{
+	mpc512x_fec_priv *fec = (mpc512x_fec_priv *)dev->priv;
+
+#if (DEBUG & 0x1)
+	printf ("mpc512x_fec_init... Begin\n");
+#endif
+
+	/* Set interrupt mask register */
+	fec->eth->imask = 0x00000000;
+
+	/* Clear FEC-Lite interrupt event register(IEVENT) */
+	fec->eth->ievent = 0xffffffff;
+
+	/* Set transmit fifo watermark register(X_WMRK), default = 64 */
+	fec->eth->x_wmrk = 0x0;
+
+	/* Set Opcode/Pause Duration Register */
+	fec->eth->op_pause = 0x00010020;
+
+	/* Frame length=1518; MII mode */
+	fec->eth->r_cntrl = 0x05ee000c;
+
+	/* Half-duplex, heartbeat disabled */
+	fec->eth->x_cntrl = 0x00000000;
+
+	/* Enable MIB counters */
+	fec->eth->mib_control = 0x0;
+
+	/* Setup recv fifo start and buff size */
+	fec->eth->r_fstart = 0x500;
+	fec->eth->r_buff_size = 0x5e0;
+
+	/* Setup BD base addresses */
+	fec->eth->r_des_start = (uint32)fec->bdBase->rbd;
+	fec->eth->x_des_start = (uint32)fec->bdBase->tbd;
+
+	/* DMA Control */
+	fec->eth->dma_control = 0xc0000000;
+
+	/* Enable FEC */
+	fec->eth->ecntrl |= 0x00000006;
+
+	/* Initilize addresses and status words of BDs */
+	mpc512x_fec_bd_init (fec);
+
+	 /* Descriptor polling active */
+	fec->eth->r_des_active = 0x01000000;
+
+#if (DEBUG & 0x1)
+	printf("mpc512x_fec_init... Done \n");
+#endif
+	return 1;
+}
+
+/********************************************************************/
+int mpc512x_fec_init_phy (struct eth_device *dev, bd_t * bis)
+{
+	mpc512x_fec_priv *fec = (mpc512x_fec_priv *)dev->priv;
+	const uint8 phyAddr = CONFIG_PHY_ADDR;	/* Only one PHY */
+	int timeout = 1;
+	uint16 phyStatus;
+
+#if (DEBUG & 0x1)
+	printf ("mpc512x_fec_init_phy... Begin\n");
+#endif
+
+	/*
+	 * Clear FEC-Lite interrupt event register(IEVENT)
+	 */
+	fec->eth->ievent = 0xffffffff;
+
+	/*
+	 * Set interrupt mask register
+	 */
+	fec->eth->imask = 0x00000000;
+
+	if (fec->xcv_type != SEVENWIRE) {
+		/*
+		 * Set MII_SPEED = (1/(mii_speed * 2)) * System Clock
+		 * and do not drop the Preamble.
+		 */
+		fec->eth->mii_speed = (((gd->ipb_clk / 1000000) / 5) + 1) << 1;
+
+		/*
+		 * Reset PHY, then delay 300ns
+		 */
+		miiphy_write (dev->name, phyAddr, 0x0, 0x8000);
+		udelay (1000);
+
+		if (fec->xcv_type == MII10) {
+		/*
+		 * Force 10Base-T, FDX operation
+		 */
+#if (DEBUG & 0x2)
+			printf ("Forcing 10 Mbps ethernet link... ");
+#endif
+			miiphy_read (dev->name, phyAddr, 0x1, &phyStatus);
+
+			miiphy_write (dev->name, phyAddr, 0x0, 0x0180);
+
+			timeout = 20;
+			do {    /* wait for link status to go down */
+				udelay (10000);
+				if ((timeout--) == 0) {
+#if (DEBUG & 0x2)
+					printf ("hmmm, should not have waited...");
+#endif
+					break;
+				}
+				miiphy_read (dev->name, phyAddr, 0x1, &phyStatus);
+#if (DEBUG & 0x2)
+				printf ("=");
+#endif
+			} while ((phyStatus & 0x0004)); /* !link up */
+
+			timeout = 1000;
+			do {    /* wait for link status to come back up */
+				udelay (10000);
+				if ((timeout--) == 0) {
+					printf ("failed. Link is down.\n");
+					break;
+				}
+				miiphy_read (dev->name, phyAddr, 0x1, &phyStatus);
+#if (DEBUG & 0x2)
+				printf ("+");
+#endif
+			} while (!(phyStatus & 0x0004)); /* !link up */
+
+#if (DEBUG & 0x2)
+			printf ("done.\n");
+#endif
+		} else {	/* MII100 */
+			/*
+			 * Set the auto-negotiation advertisement register bits
+			 */
+			miiphy_write (dev->name, phyAddr, 0x4, 0x01e1);
+
+			/*
+			 * Set MDIO bit 0.12 = 1(&& bit 0.9=1?) to enable auto-negotiation
+			 */
+			miiphy_write (dev->name, phyAddr, 0x0, 0x1200);
+
+			/*
+			 * Wait for AN completion
+			 */
+			timeout = 50000;
+			do {
+				udelay (1000);
+
+				if ((timeout--) == 0) {
+#if (DEBUG & 0x2)
+					printf ("PHY auto neg 0 failed...\n");
+#endif
+					return -1;
+				}
+
+				if (miiphy_read (dev->name, phyAddr, 0x1, &phyStatus) != 0) {
+#if (DEBUG & 0x2)
+					printf ("PHY auto neg 1 failed 0x%04x...\n", phyStatus);
+#endif
+					return -1;
+				}
+			} while (!(phyStatus & 0x0004));
+
+#if (DEBUG & 0x2)
+			printf ("PHY auto neg complete! \n");
+#endif
+		}
+	}
+
+#if (DEBUG & 0x2)
+	if (fec->xcv_type != SEVENWIRE)
+		mpc512x_fec_phydump (dev->name);
+#endif
+
+#if (DEBUG & 0x1)
+	printf ("mpc512x_fec_init_phy... Done \n");
+#endif
+	return 1;
+}
+
+/********************************************************************/
+static void mpc512x_fec_halt (struct eth_device *dev)
+{
+	mpc512x_fec_priv *fec = (mpc512x_fec_priv *)dev->priv;
+	int counter = 0xffff;
+
+#if (DEBUG & 0x2)
+	if (fec->xcv_type != SEVENWIRE)
+		mpc512x_fec_phydump (dev->name);
+#endif
+
+	/*
+	 * mask FEC chip interrupts
+	 */
+	fec->eth->imask = 0;
+
+	/*
+	 * issue graceful stop command to the FEC transmitter if necessary
+	 */
+	fec->eth->x_cntrl |= 0x00000001;
+
+	/*
+	 * wait for graceful stop to register
+	 */
+	while ((counter--) && (!(fec->eth->ievent & 0x10000000))) ;
+
+	/*
+	 * Disable the Ethernet Controller
+	 */
+	fec->eth->ecntrl &= 0xfffffffd;
+
+	/*
+	 * Issue a reset command to the FEC chip
+	 */
+	fec->eth->ecntrl |= 0x1;
+
+	/*
+	 * wait at least 16 clock cycles
+	 */
+	udelay (10);
+#if (DEBUG & 0x3)
+	printf ("Ethernet task stopped\n");
+#endif
+}
+
+/********************************************************************/
+
+static int mpc512x_fec_send (struct eth_device *dev, volatile void *eth_data,
+		int data_length)
+{
+	/*
+	 * This routine transmits one frame.  This routine only accepts
+	 * 6-byte Ethernet addresses.
+	 */
+	mpc512x_fec_priv *fec = (mpc512x_fec_priv *)dev->priv;
+	volatile FEC_TBD *pTbd;
+
+#if (DEBUG & 0x20)
+	printf("tbd status: 0x%04x\n", fec->tbdBase[fec->tbdIndex].status);
+#endif
+
+	/*
+	 * Clear Tx BD ring at first
+	 */
+	mpc512x_fec_tbd_scrub (fec);
+
+	/*
+	 * Check for valid length of data.
+	 */
+	if ((data_length > 1500) || (data_length <= 0)) {
+		return -1;
+	}
+
+	/*
+	 * Check the number of vacant TxBDs.
+	 */
+	if (fec->cleanTbdNum < 1) {
+#if (DEBUG & 0x20)
+		printf ("No available TxBDs ...\n");
+#endif
+		return -1;
+	}
+
+	/*
+	 * Get the first TxBD to send the mac header
+	 */
+	pTbd = &fec->bdBase->tbd[fec->tbdIndex];
+	pTbd->dataLength = data_length;
+	pTbd->dataPointer = (uint32)eth_data;
+	pTbd->status |= FEC_TBD_LAST | FEC_TBD_TC | FEC_TBD_READY;
+	fec->tbdIndex = (fec->tbdIndex + 1) % FEC_TBD_NUM;
+
+	/* Activate transmit Buffer Descriptor polling */
+	fec->eth->x_des_active = 0x01000000;	/* Descriptor polling active	*/
+
+#if (DEBUG & 0x8)
+	printf ( "+" );
+#endif
+
+	fec->cleanTbdNum -= 1;
+
+	/*
+	 * wait until frame is sent .
+	 */
+	while (pTbd->status & FEC_TBD_READY) {
+		udelay (10);
+#if (DEBUG & 0x8)
+		printf ("TDB status = %04x\n", pTbd->status);
+#endif
+	}
+
+	return 0;
+}
+
+
+/********************************************************************/
+static int mpc512x_fec_recv (struct eth_device *dev)
+{
+	/*
+	 * This command pulls one frame from the card
+	 */
+	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];
+
+#if (DEBUG & 0x1)
+	printf ("mpc512x_fec_recv %d Start...\n", fec->rbdIndex);
+#endif
+#if (DEBUG & 0x8)
+	printf( "-" );
+#endif
+
+	/*
+	 * Check if any critical events have happened
+	 */
+	ievent = fec->eth->ievent;
+	fec->eth->ievent = ievent;
+	if (ievent & 0x20060000) {
+		/* BABT, Rx/Tx FIFO errors */
+		mpc512x_fec_halt (dev);
+		mpc512x_fec_init (dev, NULL);
+		return 0;
+	}
+	if (ievent & 0x80000000) {
+		/* Heartbeat error */
+		fec->eth->x_cntrl |= 0x00000001;
+	}
+	if (ievent & 0x10000000) {
+		/* Graceful stop complete */
+		if (fec->eth->x_cntrl & 0x00000001) {
+			mpc512x_fec_halt (dev);
+			fec->eth->x_cntrl &= ~0x00000001;
+			mpc512x_fec_init (dev, NULL);
+		}
+	}
+
+	if (!(pRbd->status & FEC_RBD_EMPTY)) {
+		if ((pRbd->status & FEC_RBD_LAST) &&
+			!(pRbd->status & FEC_RBD_ERR) &&
+			((pRbd->dataLength - 4) > 14)) {
+
+			/*
+			 * Get buffer size
+			 */
+			frame_length = pRbd->dataLength - 4;
+
+#if (DEBUG & 0x20)
+			{
+				int i;
+				printf ("recv data hdr:");
+				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;
+		}
+
+		/*
+		 * Reset buffer descriptor as empty
+		 */
+		mpc512x_fec_rbd_clean (fec, pRbd);
+	}
+
+	/* Try to fill Buffer Descriptors */
+	fec->eth->r_des_active = 0x01000000;	/* Descriptor polling active */
+	return len;
+}
+
+/********************************************************************/
+int mpc512x_fec_initialize (bd_t * bis)
+{
+
+	immap_t *im = (immap_t*) CFG_IMMR;
+	mpc512x_fec_priv *fec;
+	struct eth_device *dev;
+	int i;
+	char *tmp, *end, env_enetaddr[6];
+	uint32 *reg;
+	void * bd;
+
+	fec = (mpc512x_fec_priv *) malloc (sizeof(*fec));
+	dev = (struct eth_device *) malloc (sizeof(*dev));
+	memset (dev, 0, sizeof *dev);
+
+	fec->eth = (ethernet_regs *) MPC512X_FEC;
+
+# ifndef CONFIG_FEC_10MBIT
+	fec->xcv_type = MII100;
+# else
+	fec->xcv_type = MII10;
+# endif
+	dev->priv = (void *)fec;
+	dev->iobase = MPC512X_FEC;
+	dev->init = mpc512x_fec_init;
+	dev->halt = mpc512x_fec_halt;
+	dev->send = mpc512x_fec_send;
+	dev->recv = mpc512x_fec_recv;
+
+	sprintf (dev->name, "FEC ETHERNET");
+	eth_register (dev);
+
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
+	miiphy_register (dev->name,
+			fec512x_miiphy_read, fec512x_miiphy_write);
+#endif
+
+	/*
+	 * Initialize I\O pins
+	 */
+	reg = (uint32 *) &(im->io_ctrl.regs[PSC0_0_IDX]);
+
+	for (i = 0; i < 15; i++)
+		reg[i] = IOCTRL_MUX_FEC | 0x00000001;
+
+	im->io_ctrl.regs[SPDIF_TXCLOCK_IDX] = IOCTRL_MUX_FEC | 0x00000001;
+	im->io_ctrl.regs[SPDIF_TX_IDX] = IOCTRL_MUX_FEC | 0x00000001;
+	im->io_ctrl.regs[SPDIF_RX_IDX] = IOCTRL_MUX_FEC | 0x00000001;
+
+	/* Clean up space FEC's MIB and FIFO RAM ...*/
+	memset ((void *) MPC512X_FEC + 0x200, 0x00, 0x400);
+
+	/*
+	 * Malloc space for BDs  (must be quad word-aligned)
+	 * this pointer is lost, so cannot be freed
+	 */
+	bd = malloc (sizeof(mpc512x_buff_descs) + 0x1f);
+	fec->bdBase = (mpc512x_buff_descs*)((uint32)bd & 0xfffffff0);
+	memset ((void *) bd, 0x00, sizeof(mpc512x_buff_descs) + 0x1f);
+
+	/*
+	 * Set interrupt mask register
+	 */
+	fec->eth->imask = 0x00000000;
+
+	/*
+	 * Clear FEC-Lite interrupt event register(IEVENT)
+	 */
+	fec->eth->ievent = 0xffffffff;
+
+	/*
+	 * Try to set the mac address now. The fec mac address is
+	 * a garbage after reset. When not using fec for booting
+	 * the Linux fec driver will try to work with this garbage.
+	 */
+	tmp = getenv ("ethaddr");
+	if (tmp) {
+		for (i=0; i<6; i++) {
+			env_enetaddr[i] = tmp ? simple_strtoul (tmp, &end, 16) : 0;
+			if (tmp)
+				tmp = (*end) ? end+1 : end;
+		}
+		mpc512x_fec_set_hwaddr (fec, env_enetaddr);
+		fec->eth->gaddr1 = 0x00000000;
+		fec->eth->gaddr2 = 0x00000000;
+	}
+
+	mpc512x_fec_init_phy (dev, bis);
+
+	return 1;
+}
+
+/* MII-interface related functions */
+/********************************************************************/
+int fec512x_miiphy_read (char *devname, uint8 phyAddr, uint8 regAddr, uint16 * retVal)
+{
+	ethernet_regs *eth = (ethernet_regs *) MPC512X_FEC;
+	uint32 reg;		/* convenient holder for the PHY register */
+	uint32 phy;		/* convenient holder for the PHY */
+	int timeout = 0xffff;
+
+	/*
+	 * reading from any PHY's register is done by properly
+	 * programming the FEC's MII data register.
+	 */
+	reg = regAddr << FEC_MII_DATA_RA_SHIFT;
+	phy = phyAddr << FEC_MII_DATA_PA_SHIFT;
+
+	eth->mii_data = (FEC_MII_DATA_ST | FEC_MII_DATA_OP_RD | FEC_MII_DATA_TA | phy | reg);
+
+	/*
+	 * wait for the related interrupt
+	 */
+	while ((timeout--) && (!(eth->ievent & 0x00800000))) ;
+
+	if (timeout == 0) {
+#if (DEBUG & 0x2)
+		printf ("Read MDIO failed...\n");
+#endif
+		return -1;
+	}
+
+	/*
+	 * clear mii interrupt bit
+	 */
+	eth->ievent = 0x00800000;
+
+	/*
+	 * it's now safe to read the PHY's register
+	 */
+	*retVal = (uint16) eth->mii_data;
+
+	return 0;
+}
+
+/********************************************************************/
+int fec512x_miiphy_write (char *devname, uint8 phyAddr, uint8 regAddr, uint16 data)
+{
+	ethernet_regs *eth = (ethernet_regs *) MPC512X_FEC;
+	uint32 reg;		/* convenient holder for the PHY register */
+	uint32 phy;		/* convenient holder for the PHY */
+	int timeout = 0xffff;
+
+	reg = regAddr << FEC_MII_DATA_RA_SHIFT;
+	phy = phyAddr << FEC_MII_DATA_PA_SHIFT;
+
+	eth->mii_data = (FEC_MII_DATA_ST | FEC_MII_DATA_OP_WR |
+			FEC_MII_DATA_TA | phy | reg | data);
+
+	/*
+	 * wait for the MII interrupt
+	 */
+	while ((timeout--) && (!(eth->ievent & 0x00800000))) ;
+
+	if (timeout == 0) {
+#if (DEBUG & 0x2)
+		printf ("Write MDIO failed...\n");
+#endif
+		return -1;
+	}
+
+	/*
+	 * clear MII interrupt bit
+	 */
+	eth->ievent = 0x00800000;
+
+	return 0;
+}
+
+#if (DEBUG & 0x40)
+static uint32 local_crc32 (char *string, unsigned int crc_value, int len)
+{
+	int i;
+	char c;
+	unsigned int crc, count;
+
+	/*
+	 * crc32 algorithm
+	 */
+	/*
+	 * crc = 0xffffffff; * The initialized value should be 0xffffffff
+	 */
+	crc = crc_value;
+
+	for (i = len; --i >= 0;) {
+		c = *string++;
+		for (count = 0; count < 8; count++) {
+			if ((c & 0x01) ^ (crc & 0x01)) {
+				crc >>= 1;
+				crc = crc ^ 0xedb88320;
+			} else {
+				crc >>= 1;
+			}
+			c >>= 1;
+		}
+	}
+
+	/*
+	 * In big endian system, do byte swaping for crc value
+	 */
+	 /**/ return crc;
+}
+#endif	/* DEBUG */
+
+#endif /* CONFIG_MPC512x_FEC */
diff --git a/cpu/mpc512x/fec.h b/cpu/mpc512x/fec.h
new file mode 100644
index 0000000000..d2d877aa52
--- /dev/null
+++ b/cpu/mpc512x/fec.h
@@ -0,0 +1,224 @@
+/*
+ * (C) Copyright 2003 - 2007
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Derived from the MPC8xx driver's header file.
+ */
+
+#ifndef __MPC512X_FEC_H
+#define __MPC512X_FEC_H
+
+#include <common.h>
+#include <mpc512x.h>
+
+typedef unsigned long uint32;
+typedef unsigned short uint16;
+typedef unsigned char uint8;
+
+typedef struct ethernet_register_set {
+
+/* [10:2]addr = 00 */
+
+/*  Control and status Registers (offset 000-1FF) */
+
+	volatile uint32 fec_id;			/* MBAR_ETH + 0x000 */
+	volatile uint32 ievent;			/* MBAR_ETH + 0x004 */
+	volatile uint32 imask;			/* MBAR_ETH + 0x008 */
+
+	volatile uint32 RES0[1];		/* MBAR_ETH + 0x00C */
+	volatile uint32 r_des_active;		/* MBAR_ETH + 0x010 */
+	volatile uint32 x_des_active;		/* MBAR_ETH + 0x014 */
+
+	volatile uint32 RES1[3];		/* MBAR_ETH + 0x018-020 */
+	volatile uint32 ecntrl;			/* MBAR_ETH + 0x024 */
+
+	volatile uint32 RES2[6];		/* MBAR_ETH + 0x028-03C */
+	volatile uint32 mii_data;		/* MBAR_ETH + 0x040 */
+	volatile uint32 mii_speed;		/* MBAR_ETH + 0x044 */
+
+	volatile uint32 RES3[7];		/* MBAR_ETH + 0x048-060 */
+	volatile uint32 mib_control;		/* MBAR_ETH + 0x064 */
+
+	volatile uint32 RES4[7];		/* MBAR_ETH + 0x068-80 */
+	volatile uint32 r_cntrl;		/* MBAR_ETH + 0x084 */
+	volatile uint32 r_hash;			/* MBAR_ETH + 0x088 */
+
+	volatile uint32 RES5[14];		/* MBAR_ETH + 0x08c-0C0 */
+	volatile uint32 x_cntrl;		/* MBAR_ETH + 0x0C4 */
+
+	volatile uint32 RES6[7];		/* MBAR_ETH + 0x0C8-0E0 */
+	volatile uint32 paddr1;			/* MBAR_ETH + 0x0E4 */
+	volatile uint32 paddr2;			/* MBAR_ETH + 0x0E8 */
+	volatile uint32 op_pause;		/* MBAR_ETH + 0x0EC */
+
+	volatile uint32 RES7[10];		/* MBAR_ETH + 0x0F0-114 */
+	volatile uint32 iaddr1;			/* MBAR_ETH + 0x118 */
+	volatile uint32 iaddr2;			/* MBAR_ETH + 0x11C */
+	volatile uint32 gaddr1;			/* MBAR_ETH + 0x120 */
+	volatile uint32 gaddr2;			/* MBAR_ETH + 0x124 */
+
+	volatile uint32 RES8[6];		/* MBAR_ETH + 0x128-13C */
+	volatile uint32 fifo_id;		/* MBAR_ETH + 0x140 */
+	volatile uint32 x_wmrk;			/* MBAR_ETH + 0x144 */
+	volatile uint32 RES9[1];		/* MBAR_ETH + 0x148 */
+	volatile uint32 r_bound;		/* MBAR_ETH + 0x14C */
+	volatile uint32 r_fstart;		/* MBAR_ETH + 0x150 */
+
+	volatile uint32 RES10[11];		/* MBAR_ETH + 0x154-17C */
+	volatile uint32 r_des_start;		/* MBAR_ETH + 0x180 */
+	volatile uint32 x_des_start;		/* MBAR_ETH + 0x184 */
+	volatile uint32 r_buff_size;		/* MBAR_ETH + 0x188 */
+	volatile uint32 RES11[26];		/* MBAR_ETH + 0x18C-1F0 */
+	volatile uint32 dma_control;		/* MBAR_ETH + 0x1F4 */
+	volatile uint32 RES12[2];		/* MBAR_ETH + 0x1F8-1FC */
+
+/*  MIB COUNTERS (Offset 200-2FF) */
+
+	volatile uint32 rmon_t_drop;		/* MBAR_ETH + 0x200 */
+	volatile uint32 rmon_t_packets;		/* MBAR_ETH + 0x204 */
+	volatile uint32 rmon_t_bc_pkt;		/* MBAR_ETH + 0x208 */
+	volatile uint32 rmon_t_mc_pkt;		/* MBAR_ETH + 0x20C */
+	volatile uint32 rmon_t_crc_align;	/* MBAR_ETH + 0x210 */
+	volatile uint32 rmon_t_undersize;	/* MBAR_ETH + 0x214 */
+	volatile uint32 rmon_t_oversize;	/* MBAR_ETH + 0x218 */
+	volatile uint32 rmon_t_frag;		/* MBAR_ETH + 0x21C */
+	volatile uint32 rmon_t_jab;		/* MBAR_ETH + 0x220 */
+	volatile uint32 rmon_t_col;		/* MBAR_ETH + 0x224 */
+	volatile uint32 rmon_t_p64;		/* MBAR_ETH + 0x228 */
+	volatile uint32 rmon_t_p65to127;	/* MBAR_ETH + 0x22C */
+	volatile uint32 rmon_t_p128to255;	/* MBAR_ETH + 0x230 */
+	volatile uint32 rmon_t_p256to511;	/* MBAR_ETH + 0x234 */
+	volatile uint32 rmon_t_p512to1023;	/* MBAR_ETH + 0x238 */
+	volatile uint32 rmon_t_p1024to2047;	/* MBAR_ETH + 0x23C */
+	volatile uint32 rmon_t_p_gte2048;	/* MBAR_ETH + 0x240 */
+	volatile uint32 rmon_t_octets;		/* MBAR_ETH + 0x244 */
+	volatile uint32 ieee_t_drop;		/* MBAR_ETH + 0x248 */
+	volatile uint32 ieee_t_frame_ok;	/* MBAR_ETH + 0x24C */
+	volatile uint32 ieee_t_1col;		/* MBAR_ETH + 0x250 */
+	volatile uint32 ieee_t_mcol;		/* MBAR_ETH + 0x254 */
+	volatile uint32 ieee_t_def;		/* MBAR_ETH + 0x258 */
+	volatile uint32 ieee_t_lcol;		/* MBAR_ETH + 0x25C */
+	volatile uint32 ieee_t_excol;		/* MBAR_ETH + 0x260 */
+	volatile uint32 ieee_t_macerr;		/* MBAR_ETH + 0x264 */
+	volatile uint32 ieee_t_cserr;		/* MBAR_ETH + 0x268 */
+	volatile uint32 ieee_t_sqe;		/* MBAR_ETH + 0x26C */
+	volatile uint32 t_fdxfc;		/* MBAR_ETH + 0x270 */
+	volatile uint32 ieee_t_octets_ok;	/* MBAR_ETH + 0x274 */
+
+	volatile uint32 RES13[2];		/* MBAR_ETH + 0x278-27C */
+	volatile uint32 rmon_r_drop;		/* MBAR_ETH + 0x280 */
+	volatile uint32 rmon_r_packets;		/* MBAR_ETH + 0x284 */
+	volatile uint32 rmon_r_bc_pkt;		/* MBAR_ETH + 0x288 */
+	volatile uint32 rmon_r_mc_pkt;		/* MBAR_ETH + 0x28C */
+	volatile uint32 rmon_r_crc_align;	/* MBAR_ETH + 0x290 */
+	volatile uint32 rmon_r_undersize;	/* MBAR_ETH + 0x294 */
+	volatile uint32 rmon_r_oversize;	/* MBAR_ETH + 0x298 */
+	volatile uint32 rmon_r_frag;		/* MBAR_ETH + 0x29C */
+	volatile uint32 rmon_r_jab;		/* MBAR_ETH + 0x2A0 */
+
+	volatile uint32 rmon_r_resvd_0;		/* MBAR_ETH + 0x2A4 */
+
+	volatile uint32 rmon_r_p64;		/* MBAR_ETH + 0x2A8 */
+	volatile uint32 rmon_r_p65to127;	/* MBAR_ETH + 0x2AC */
+	volatile uint32 rmon_r_p128to255;	/* MBAR_ETH + 0x2B0 */
+	volatile uint32 rmon_r_p256to511;	/* MBAR_ETH + 0x2B4 */
+	volatile uint32 rmon_r_p512to1023;	/* MBAR_ETH + 0x2B8 */
+	volatile uint32 rmon_r_p1024to2047;	/* MBAR_ETH + 0x2BC */
+	volatile uint32 rmon_r_p_gte2048;	/* MBAR_ETH + 0x2C0 */
+	volatile uint32 rmon_r_octets;		/* MBAR_ETH + 0x2C4 */
+	volatile uint32 ieee_r_drop;		/* MBAR_ETH + 0x2C8 */
+	volatile uint32 ieee_r_frame_ok;	/* MBAR_ETH + 0x2CC */
+	volatile uint32 ieee_r_crc;		/* MBAR_ETH + 0x2D0 */
+	volatile uint32 ieee_r_align;		/* MBAR_ETH + 0x2D4 */
+	volatile uint32 r_macerr;		/* MBAR_ETH + 0x2D8 */
+	volatile uint32 r_fdxfc;		/* MBAR_ETH + 0x2DC */
+	volatile uint32 ieee_r_octets_ok;	/* MBAR_ETH + 0x2E0 */
+
+	volatile uint32 RES14[6];		/* MBAR_ETH + 0x2E4-2FC */
+
+	volatile uint32 RES15[64];		/* MBAR_ETH + 0x300-3FF */
+} ethernet_regs;
+
+/* Receive & Transmit Buffer Descriptor definitions */
+typedef struct BufferDescriptor {
+	uint16 status;
+	uint16 dataLength;
+	uint32 dataPointer;
+} FEC_RBD;
+
+typedef struct {
+	uint16 status;
+	uint16 dataLength;
+	uint32 dataPointer;
+} FEC_TBD;
+
+/* private structure */
+typedef enum {
+	SEVENWIRE,			/* 7-wire       */
+	MII10,				/* MII 10Mbps   */
+	MII100				/* MII 100Mbps  */
+} xceiver_type;
+
+/* BD Numer definitions */
+#define FEC_TBD_NUM		48	/* The user can adjust this value */
+#define FEC_RBD_NUM		32	/* The user can adjust this value */
+
+/* packet size limit */
+#define FEC_MAX_PKT_SIZE	1536
+
+typedef struct {
+	uint8 frame[FEC_MAX_PKT_SIZE];
+} mpc512x_frame;
+
+typedef struct {
+	FEC_RBD rbd[FEC_RBD_NUM];			/* RBD ring */
+	FEC_TBD tbd[FEC_TBD_NUM];			/* TBD ring */
+	mpc512x_frame recv_frames[FEC_RBD_NUM];		/* receive buff */
+} mpc512x_buff_descs;
+
+typedef struct {
+	ethernet_regs *eth;
+	xceiver_type xcv_type;		/* transceiver type */
+	mpc512x_buff_descs *bdBase;	/* BD rings and recv buffer */
+	uint16 rbdIndex;		/* next receive BD to read */
+	uint16 tbdIndex;		/* next transmit BD to send */
+	uint16 usedTbdIndex;		/* next transmit BD to clean */
+	uint16 cleanTbdNum;		/* the number of available transmit BDs */
+} mpc512x_fec_priv;
+
+/* RBD bits definitions */
+#define FEC_RBD_EMPTY		0x8000	/* Buffer is empty */
+#define FEC_RBD_WRAP		0x2000	/* Last BD in ring */
+#define FEC_RBD_LAST		0x0800	/* Buffer is last in frame(useless) */
+#define FEC_RBD_MISS		0x0100	/* Miss bit for prom mode */
+#define FEC_RBD_BC		0x0080	/* The received frame is broadcast frame */
+#define FEC_RBD_MC		0x0040	/* The received frame is multicast frame */
+#define FEC_RBD_LG		0x0020	/* Frame length violation */
+#define FEC_RBD_NO		0x0010	/* Nonoctet align frame */
+#define FEC_RBD_SH		0x0008	/* Short frame */
+#define FEC_RBD_CR		0x0004	/* CRC error */
+#define FEC_RBD_OV		0x0002	/* Receive FIFO overrun */
+#define FEC_RBD_TR		0x0001	/* Frame is truncated */
+#define FEC_RBD_ERR		(FEC_RBD_LG | FEC_RBD_NO | FEC_RBD_CR | \
+				FEC_RBD_OV | FEC_RBD_TR)
+
+/* TBD bits definitions */
+#define FEC_TBD_READY		0x8000	/* Buffer is ready */
+#define FEC_TBD_WRAP		0x2000	/* Last BD in ring */
+#define FEC_TBD_LAST		0x0800	/* Buffer is last in frame */
+#define FEC_TBD_TC		0x0400	/* Transmit the CRC */
+#define FEC_TBD_ABC		0x0200	/* Append bad CRC */
+
+/* MII-related definitios */
+#define FEC_MII_DATA_ST		0x40000000	/* Start of frame delimiter */
+#define FEC_MII_DATA_OP_RD	0x20000000	/* Perform a read operation */
+#define FEC_MII_DATA_OP_WR	0x10000000	/* Perform a write operation */
+#define FEC_MII_DATA_PA_MSK	0x0f800000	/* PHY Address field mask */
+#define FEC_MII_DATA_RA_MSK	0x007c0000	/* PHY Register field mask */
+#define FEC_MII_DATA_TA		0x00020000	/* Turnaround */
+#define FEC_MII_DATA_DATAMSK	0x0000ffff	/* PHY data field */
+
+#define FEC_MII_DATA_RA_SHIFT	18	/* MII Register address bits */
+#define FEC_MII_DATA_PA_SHIFT	23	/* MII PHY address bits */
+
+#endif	/* __MPC512X_FEC_H */
diff --git a/cpu/mpc512x/i2c.c b/cpu/mpc512x/i2c.c
new file mode 100644
index 0000000000..00e28d6404
--- /dev/null
+++ b/cpu/mpc512x/i2c.c
@@ -0,0 +1,431 @@
+/*
+ * (C) Copyright 2003 - 2007
+ * 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
+ *
+ * Based on the MPC5xxx code.
+ */
+
+#include <common.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#ifdef CONFIG_HARD_I2C
+
+#include <mpc512x.h>
+#include <i2c.h>
+
+#define immr ((immap_t *)CFG_IMMR)
+
+/* by default set I2C bus 0 active */
+static unsigned int bus_num = 0;
+
+#define I2C_TIMEOUT	100
+#define I2C_RETRIES	3
+
+struct mpc512x_i2c_tap {
+	int scl2tap;
+	int tap2tap;
+};
+
+static int  mpc_reg_in(volatile u32 *reg);
+static void mpc_reg_out(volatile u32 *reg, int val, int mask);
+static int  wait_for_bb(void);
+static int  wait_for_pin(int *status);
+static int  do_address(uchar chip, char rdwr_flag);
+static int  send_bytes(uchar chip, char *buf, int len);
+static int  receive_bytes(uchar chip, char *buf, int len);
+static int  mpc_get_fdr(int);
+
+static int mpc_reg_in (volatile u32 *reg)
+{
+	int ret = *reg >> 24;
+	__asm__ __volatile__ ("eieio");
+	return ret;
+}
+
+static void mpc_reg_out (volatile u32 *reg, int val, int mask)
+{
+	int tmp;
+
+	if (!mask) {
+		*reg = val << 24;
+	} else {
+		tmp = mpc_reg_in (reg);
+		*reg = ((tmp & ~mask) | (val & mask)) << 24;
+	}
+	__asm__ __volatile__ ("eieio");
+
+	return;
+}
+
+static int wait_for_bb (void)
+{
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int timeout = I2C_TIMEOUT;
+	int status;
+
+	status = mpc_reg_in (&regs->msr);
+
+	while (timeout-- && (status & I2C_BB)) {
+		volatile int temp;
+		mpc_reg_out (&regs->mcr, I2C_STA, I2C_STA);
+		temp = mpc_reg_in (&regs->mdr);
+		mpc_reg_out (&regs->mcr, 0, I2C_STA);
+		mpc_reg_out (&regs->mcr, 0, 0);
+		mpc_reg_out (&regs->mcr, I2C_EN, 0);
+
+		udelay (1000);
+		status = mpc_reg_in (&regs->msr);
+	}
+
+	return (status & I2C_BB);
+}
+
+static int wait_for_pin (int *status)
+{
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int timeout = I2C_TIMEOUT;
+
+	*status = mpc_reg_in (&regs->msr);
+
+	while (timeout-- && !(*status & I2C_IF)) {
+		udelay (1000);
+		*status = mpc_reg_in (&regs->msr);
+	}
+
+	if (!(*status & I2C_IF)) {
+		return -1;
+	}
+
+	mpc_reg_out (&regs->msr, 0, I2C_IF);
+
+	return 0;
+}
+
+static int do_address (uchar chip, char rdwr_flag)
+{
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int status;
+
+	chip <<= 1;
+
+	if (rdwr_flag) {
+		chip |= 1;
+	}
+
+	mpc_reg_out (&regs->mcr, I2C_TX, I2C_TX);
+	mpc_reg_out (&regs->mdr, chip, 0);
+
+	if (wait_for_pin (&status)) {
+		return -2;
+	}
+
+	if (status & I2C_RXAK) {
+		return -3;
+	}
+
+	return 0;
+}
+
+static int send_bytes (uchar chip, char *buf, int len)
+{
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int wrcount;
+	int status;
+
+	for (wrcount = 0; wrcount < len; ++wrcount) {
+
+		mpc_reg_out (&regs->mdr, buf[wrcount], 0);
+
+		if (wait_for_pin (&status)) {
+			break;
+		}
+
+		if (status & I2C_RXAK) {
+			break;
+		}
+
+	}
+
+	return !(wrcount == len);
+}
+
+static int receive_bytes (uchar chip, char *buf, int len)
+{
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int dummy   = 1;
+	int rdcount = 0;
+	int status;
+	int i;
+
+	mpc_reg_out (&regs->mcr, 0, I2C_TX);
+
+	for (i = 0; i < len; ++i) {
+		buf[rdcount] = mpc_reg_in (&regs->mdr);
+
+		if (dummy) {
+			dummy = 0;
+		} else {
+			rdcount++;
+		}
+
+		if (wait_for_pin (&status)) {
+			return -4;
+		}
+	}
+
+	mpc_reg_out (&regs->mcr, I2C_TXAK, I2C_TXAK);
+	buf[rdcount++] = mpc_reg_in (&regs->mdr);
+
+	if (wait_for_pin (&status)) {
+		return -5;
+	}
+
+	mpc_reg_out (&regs->mcr, 0, I2C_TXAK);
+
+	return 0;
+}
+
+/**************** I2C API ****************/
+
+void i2c_init (int speed, int saddr)
+{
+	int i;
+	for(i = 0; i < I2C_BUS_CNT; i++){
+		i2c512x_dev_t *regs = &immr->i2c.dev[i];
+		mpc_reg_out (&regs->mcr, 0, 0);
+
+		/* Set clock */
+		mpc_reg_out (&regs->mfdr, mpc_get_fdr (speed), 0);
+		mpc_reg_out (&regs->madr, saddr << 1, 0);
+
+		/* Enable module */
+		mpc_reg_out (&regs->mcr, I2C_EN, I2C_INIT_MASK);
+		mpc_reg_out (&regs->msr, 0, I2C_IF);
+	}
+
+	/* Disable interrupts */
+	immr->i2c.icr = 0;
+	/* Turn off filters */
+	immr->i2c.mifr = 0;
+	return;
+}
+
+static int mpc_get_fdr (int speed)
+{
+	static int fdr = -1;
+
+	if (fdr == -1) {
+		ulong best_speed = 0;
+		ulong divider;
+		ulong ipb, scl;
+		ulong bestmatch = 0xffffffffUL;
+		int best_i = 0, best_j = 0, i, j;
+		int SCL_Tap[] = { 9, 10, 12, 15, 5, 6, 7, 8};
+		struct mpc512x_i2c_tap scltap[] = {
+			{4, 1},
+			{4, 2},
+			{6, 4},
+			{6, 8},
+			{14, 16},
+			{30, 32},
+			{62, 64},
+			{126, 128}
+		};
+
+		ipb = gd->ipb_clk;
+		for (i = 7; i >= 0; i--) {
+			for (j = 7; j >= 0; j--) {
+				scl = 2 * (scltap[j].scl2tap +
+					   (SCL_Tap[i] - 1) * scltap[j].tap2tap
+					   + 2);
+				if (ipb <= speed*scl) {
+					if ((speed*scl - ipb) < bestmatch) {
+						bestmatch = speed*scl - ipb;
+						best_i = i;
+						best_j = j;
+						best_speed = ipb/scl;
+					}
+				}
+			}
+		}
+		divider = (best_i & 3) | ((best_i & 4) << 3) | (best_j << 2);
+		if (gd->flags & GD_FLG_RELOC) {
+			fdr = divider;
+		} else {
+			debug("%ld kHz, \n", best_speed / 1000);
+			return divider;
+		}
+	}
+
+	return fdr;
+}
+
+int i2c_probe (uchar chip)
+{
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int i;
+
+	for (i = 0; i < I2C_RETRIES; i++) {
+		mpc_reg_out (&regs->mcr, I2C_STA, I2C_STA);
+
+		if (! do_address (chip, 0)) {
+			mpc_reg_out (&regs->mcr, 0, I2C_STA);
+			udelay (500);
+			break;
+		}
+
+		mpc_reg_out (&regs->mcr, 0, I2C_STA);
+		udelay (500);
+	}
+
+	return (i == I2C_RETRIES);
+}
+
+int i2c_read (uchar chip, uint addr, int alen, uchar *buf, int len)
+{
+	char xaddr[4];
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int ret = -1;
+
+	xaddr[0] = (addr >> 24) & 0xFF;
+	xaddr[1] = (addr >> 16) & 0xFF;
+	xaddr[2] = (addr >>  8) & 0xFF;
+	xaddr[3] =  addr	& 0xFF;
+
+	if (wait_for_bb ()) {
+		printf ("i2c_read: bus is busy\n");
+		goto Done;
+	}
+
+	mpc_reg_out (&regs->mcr, I2C_STA, I2C_STA);
+	if (do_address (chip, 0)) {
+		printf ("i2c_read: failed to address chip\n");
+		goto Done;
+	}
+
+	if (send_bytes (chip, &xaddr[4-alen], alen)) {
+		printf ("i2c_read: send_bytes failed\n");
+		goto Done;
+	}
+
+	mpc_reg_out (&regs->mcr, I2C_RSTA, I2C_RSTA);
+	if (do_address (chip, 1)) {
+		printf ("i2c_read: failed to address chip\n");
+		goto Done;
+	}
+
+	if (receive_bytes (chip, (char *)buf, len)) {
+		printf ("i2c_read: receive_bytes failed\n");
+		goto Done;
+	}
+
+	ret = 0;
+Done:
+	mpc_reg_out (&regs->mcr, 0, I2C_STA);
+	return ret;
+}
+
+int i2c_write (uchar chip, uint addr, int alen, uchar *buf, int len)
+{
+	char xaddr[4];
+	i2c512x_dev_t *regs = &immr->i2c.dev[bus_num];
+	int ret = -1;
+
+	xaddr[0] = (addr >> 24) & 0xFF;
+	xaddr[1] = (addr >> 16) & 0xFF;
+	xaddr[2] = (addr >>  8) & 0xFF;
+	xaddr[3] =  addr	& 0xFF;
+
+	if (wait_for_bb ()) {
+		printf ("i2c_write: bus is busy\n");
+		goto Done;
+	}
+
+	mpc_reg_out (&regs->mcr, I2C_STA, I2C_STA);
+	if (do_address (chip, 0)) {
+		printf ("i2c_write: failed to address chip\n");
+		goto Done;
+	}
+
+	if (send_bytes (chip, &xaddr[4-alen], alen)) {
+		printf ("i2c_write: send_bytes failed\n");
+		goto Done;
+	}
+
+	if (send_bytes (chip, (char *)buf, len)) {
+		printf ("i2c_write: send_bytes failed\n");
+		goto Done;
+	}
+
+	ret = 0;
+Done:
+	mpc_reg_out (&regs->mcr, 0, I2C_STA);
+	return ret;
+}
+
+uchar i2c_reg_read (uchar chip, uchar reg)
+{
+	uchar buf;
+
+	i2c_read (chip, reg, 1, &buf, 1);
+
+	return buf;
+}
+
+void i2c_reg_write (uchar chip, uchar reg, uchar val)
+{
+	i2c_write (chip, reg, 1, &val, 1);
+
+	return;
+}
+
+
+int i2c_set_bus_num (unsigned int bus)
+{
+	if (bus >= I2C_BUS_CNT) {
+		return -1;
+	}
+	bus_num = bus;
+
+	return 0;
+}
+
+unsigned int i2c_get_bus_num (void)
+{
+	return bus_num;
+}
+
+/* TODO */
+unsigned int i2c_get_bus_speed (void)
+{
+	return -1;
+}
+
+int i2c_set_bus_speed (unsigned int speed)
+{
+	if (speed != CFG_I2C_SPEED)
+		return -1;
+
+	return 0;
+}
+
+#endif	/* CONFIG_HARD_I2C */
diff --git a/cpu/mpc512x/interrupts.c b/cpu/mpc512x/interrupts.c
new file mode 100644
index 0000000000..8cc241c899
--- /dev/null
+++ b/cpu/mpc512x/interrupts.c
@@ -0,0 +1,61 @@
+/*
+ * (C) Copyright 2000-2007
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright 2004 Freescale Semiconductor, Inc.
+ *
+ * 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
+ *
+ * Derived from the MPC83xx code.
+ */
+
+#include <common.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct irq_action {
+	interrupt_handler_t *handler;
+	void *arg;
+	ulong count;
+};
+
+int interrupt_init_cpu (unsigned *decrementer_count)
+{
+	*decrementer_count = get_tbclk () / CFG_HZ;
+
+	return 0;
+}
+
+/*
+ * Install and free an interrupt handler.
+ */
+void
+irq_install_handler (int irq, interrupt_handler_t * handler, void *arg)
+{
+}
+
+void irq_free_handler (int irq)
+{
+}
+
+void timer_interrupt_cpu (struct pt_regs *regs)
+{
+	/* nothing to do here */
+	return;
+}
diff --git a/cpu/mpc512x/serial.c b/cpu/mpc512x/serial.c
new file mode 100644
index 0000000000..200ff2c496
--- /dev/null
+++ b/cpu/mpc512x/serial.c
@@ -0,0 +1,197 @@
+/*
+ * (C) Copyright 2000 - 2007
+ * 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
+ *
+ * Based ont the MPC5200 PSC driver.
+ * Adapted for MPC512x by Jan Wrobel <wrr@semihalf.com>
+ */
+
+/*
+ * Minimal serial functions needed to use one of the PSC ports
+ * as serial console interface.
+ */
+
+#include <common.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#if defined(CONFIG_PSC_CONSOLE)
+
+static void fifo_init (volatile psc512x_t *psc)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+
+	/* reset Rx & Tx fifo slice */
+	psc->rfcmd = PSC_FIFO_RESET_SLICE;
+	psc->tfcmd = PSC_FIFO_RESET_SLICE;
+
+	/* disable Tx & Rx FIFO interrupts */
+	psc->rfintmask = 0;
+	psc->tfintmask = 0;
+
+	psc->tfsize = CONSOLE_FIFO_TX_SIZE | (CONSOLE_FIFO_TX_ADDR << 16);
+	psc->rfsize = CONSOLE_FIFO_RX_SIZE | (CONSOLE_FIFO_RX_ADDR << 16);
+
+	/* enable Tx & Rx FIFO slice */
+	psc->rfcmd = PSC_FIFO_ENABLE_SLICE;
+	psc->tfcmd = PSC_FIFO_ENABLE_SLICE;
+
+	im->fifoc.fifoc_cmd = FIFOC_DISABLE_CLOCK_GATE;
+	__asm__ volatile ("sync");
+}
+
+int serial_init(void)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+	unsigned long baseclk;
+	int div;
+
+	fifo_init (psc);
+
+	/* set MR register to point to MR1 */
+	psc->command = PSC_SEL_MODE_REG_1;
+
+	/* disable Tx/Rx */
+	psc->command = PSC_TX_DISABLE | PSC_RX_DISABLE;
+
+	/* choose the prescaler	by 16 for the Tx/Rx clock generation */
+	psc->psc_clock_select =  0xdd00;
+
+	/* switch to UART mode */
+	psc->sicr = 0;
+
+	/* mode register points to mr1 */
+	/* configure parity, bit length and so on in mode register 1*/
+	psc->mode = PSC_MODE_8_BITS | PSC_MODE_PARNONE;
+	/* now, mode register points to mr2 */
+	psc->mode = PSC_MODE_1_STOPBIT;
+
+	/* calculate dividor for setting PSC CTUR and CTLR registers */
+	baseclk = (gd->ipb_clk + 8) / 16;
+	div = (baseclk + (gd->baudrate / 2)) / gd->baudrate;
+
+	psc->ctur = (div >> 8) & 0xff;
+	/* set baudrate */
+	psc->ctlr = div & 0xff;
+
+	/* disable all interrupts */
+	psc->psc_imr = 0;
+
+	/* reset and enable Rx/Tx */
+	psc->command = PSC_RST_RX;
+	psc->command = PSC_RST_TX;
+	psc->command = PSC_RX_ENABLE | PSC_TX_ENABLE;
+
+	return 0;
+}
+
+void serial_putc (const char c)
+{
+	volatile immap_t *im = (immap_t *)CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+
+	if (c == '\n')
+		serial_putc ('\r');
+
+	/* Wait for last character to go. */
+	while (!(psc->psc_status & PSC_SR_TXEMP))
+		;
+
+	psc->tfdata_8 = c;
+}
+
+void serial_putc_raw (const char c)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+
+	/* Wait for last character to go. */
+	while (!(psc->psc_status & PSC_SR_TXEMP))
+		;
+
+	psc->tfdata_8 = c;
+}
+
+
+void serial_puts (const char *s)
+{
+	while (*s) {
+		serial_putc (*s++);
+	}
+}
+
+int serial_getc (void)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+
+	/* Wait for a character to arrive. */
+	while (psc->rfstat & PSC_FIFO_EMPTY)
+		;
+
+	return psc->rfdata_8;
+}
+
+int serial_tstc (void)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+
+	return !(psc->rfstat & PSC_FIFO_EMPTY);
+}
+
+void serial_setbrg (void)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+	unsigned long baseclk, div;
+
+	baseclk = (gd->csb_clk + 8) / 16;
+	div = (baseclk + (gd->baudrate / 2)) / gd->baudrate;
+
+	psc->ctur = (div >> 8) & 0xFF;
+	psc->ctlr =  div & 0xff; /* set baudrate */
+}
+
+void serial_setrts(int s)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+
+	if (s) {
+		/* Assert RTS (become LOW) */
+		psc->op1 = 0x1;
+	}
+	else {
+		/* Negate RTS (become HIGH) */
+		psc->op0 = 0x1;
+	}
+}
+
+int serial_getcts(void)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	volatile psc512x_t *psc = (psc512x_t *) &im->psc[CONFIG_PSC_CONSOLE];
+
+	return (psc->ip & 0x1) ? 0 : 1;
+}
+#endif /* CONFIG_PSC_CONSOLE */
diff --git a/cpu/mpc512x/speed.c b/cpu/mpc512x/speed.c
new file mode 100644
index 0000000000..a609827387
--- /dev/null
+++ b/cpu/mpc512x/speed.c
@@ -0,0 +1,135 @@
+/*
+ * (C) Copyright 2000-2007
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2006 Freescale Semiconductor, Inc.
+ *
+ * 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
+ *
+ * Based on the MPC83xx code.
+ */
+
+#include <common.h>
+#include <mpc512x.h>
+#include <command.h>
+#include <asm/processor.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int spmf_mult[] = {
+	68, 1, 12, 16,
+	20, 24, 28, 32,
+	36, 40, 44, 48,
+	52, 56, 60, 64
+};
+
+static int cpmf_mult[][2] = {
+	{0, 1}, {0, 1}, /* 0 and 1 are not valid */
+	{1, 1}, {3, 2},
+	{2, 1}, {5, 2},
+	{3, 1}, {7, 2},
+	{0, 1}, {0, 1}, /* and all above 7 are not valid too */
+	{0, 1}, {0, 1},
+	{0, 1}, {0, 1},
+	{0, 1}, {0, 1}
+};
+
+static int sys_dividors[][2] = {
+	{2, 1}, {5, 2}, {3, 1}, {7, 2}, {4, 1},
+	{9, 2}, {5, 1}, {7, 1}, {6, 1}, {8, 1},
+	{9, 1}, {11, 1}, {10, 1}, {12, 1}, {13, 1},
+	{15, 1}, {14, 1}, {16, 1}, {17, 1}, {19, 1},
+	{18, 1}, {20, 1}, {21, 1}, {23, 1}, {22, 1},
+	{24, 1}, {25, 1}, {27, 1}, {26, 1}, {28, 1},
+	{29, 1}, {31, 1}, {30, 1}, {32, 1}, {33, 1}
+};
+
+int get_clocks (void)
+{
+	volatile immap_t *im = (immap_t *) CFG_IMMR;
+	u8 spmf;
+	u8 cpmf;
+	u8 sys_div;
+	u8 ips_div;
+	u32 ref_clk = CFG_MPC512X_CLKIN;
+	u32 spll;
+	u32 sys_clk;
+	u32 core_clk;
+	u32 csb_clk;
+	u32 ips_clk;
+
+	if ((im->sysconf.immrbar & IMMRBAR_BASE_ADDR) != (u32) im)
+		return -1;
+
+	spmf = (im->clk.spmr & SPMR_SPMF) >> SPMR_SPMF_SHIFT;
+	spll = ref_clk * spmf_mult[spmf];
+
+	sys_div = (im->clk.scfr[1] & SCFR2_SYS_DIV) >> SCFR2_SYS_DIV_SHIFT;
+	sys_clk = (spll * sys_dividors[sys_div][1]) / sys_dividors[sys_div][0];
+
+	csb_clk = sys_clk / 2;
+
+	cpmf = (im->clk.spmr & SPMR_CPMF) >> SPMR_CPMF_SHIFT;
+	core_clk = (csb_clk * cpmf_mult[cpmf][0]) / cpmf_mult[cpmf][1];
+
+	ips_div = (im->clk.scfr[0] & SCFR1_IPS_DIV_MASK) >> SCFR1_IPS_DIV_SHIFT;
+	if (ips_div != 0) {
+		ips_clk = csb_clk / ips_div;
+	} else {
+		/* in case we cannot get a sane IPS divisor, fail gracefully */
+		ips_clk = 0;
+	}
+
+	gd->ipb_clk = ips_clk;
+	gd->csb_clk = csb_clk;
+	gd->cpu_clk = core_clk;
+	gd->bus_clk = csb_clk;
+	return 0;
+
+}
+
+/********************************************
+ * get_bus_freq
+ * return system bus freq in Hz
+ *********************************************/
+ulong get_bus_freq (ulong dummy)
+{
+	return gd->csb_clk;
+}
+
+int do_clocks (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	printf ("Clock configuration:\n");
+	printf ("  CPU:                 %4d MHz\n", gd->cpu_clk / 1000000);
+	printf ("  Coherent System Bus: %4d MHz\n", gd->csb_clk / 1000000);
+	printf ("  IPS Bus:             %4d MHz\n", gd->ipb_clk / 1000000);
+	printf ("  DDR:                 %4d MHz\n", 2 * gd->csb_clk / 1000000);
+	return 0;
+}
+
+U_BOOT_CMD(clocks, 1, 0, do_clocks,
+	"clocks  - print clock configuration\n",
+	"    clocks\n"
+);
+
+int prt_mpc512x_clks (void)
+{
+	do_clocks (NULL, 0, 0, NULL);
+	return (0);
+}
diff --git a/cpu/mpc512x/start.S b/cpu/mpc512x/start.S
new file mode 100644
index 0000000000..244c69b812
--- /dev/null
+++ b/cpu/mpc512x/start.S
@@ -0,0 +1,780 @@
+/*
+ * Copyright (C) 1998  Dan Malek <dmalek@jlc.net>
+ * Copyright (C) 1999  Magnus Damm <kieraypc01.p.y.kie.era.ericsson.se>
+ * Copyright (C) 2000, 2001, 2002, 2007 Wolfgang Denk <wd@denx.de>
+ * Copyright Freescale Semiconductor, Inc. 2004, 2006. All rights reserved.
+ *
+ * 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
+ *
+ * Based on the MPC83xx code.
+ */
+
+/*
+ *  U-Boot - Startup Code for MPC512x based Embedded Boards
+ */
+
+#include <config.h>
+#include <mpc512x.h>
+#include <version.h>
+
+#define CONFIG_521X	1		/* needed for Linux kernel header files*/
+
+#include <ppc_asm.tmpl>
+#include <ppc_defs.h>
+
+#include <asm/cache.h>
+#include <asm/mmu.h>
+
+#ifndef  CONFIG_IDENT_STRING
+#define  CONFIG_IDENT_STRING "MPC512X"
+#endif
+
+/*
+ * Floating Point enable, Machine Check and Recoverable Interr.
+ */
+#undef	MSR_KERNEL
+#ifdef DEBUG
+#define MSR_KERNEL (MSR_FP|MSR_RI)
+#else
+#define MSR_KERNEL (MSR_FP|MSR_ME|MSR_RI)
+#endif
+
+/* Macros for manipulating CSx_START/STOP */
+#define START_REG(start)	((start) >> 16)
+#define STOP_REG(start, size)	(((start) + (size) - 1) >> 16)
+
+/*
+ * Set up GOT: Global Offset Table
+ *
+ * Use r14 to access the GOT
+ */
+	START_GOT
+	GOT_ENTRY(_GOT2_TABLE_)
+	GOT_ENTRY(_FIXUP_TABLE_)
+
+	GOT_ENTRY(_start)
+	GOT_ENTRY(_start_of_vectors)
+	GOT_ENTRY(_end_of_vectors)
+	GOT_ENTRY(transfer_to_handler)
+
+	GOT_ENTRY(__init_end)
+	GOT_ENTRY(_end)
+	GOT_ENTRY(__bss_start)
+	END_GOT
+
+/*
+ * Magic number and version string
+ */
+	.long	0x27051956		/* U-Boot Magic Number */
+	.globl	version_string
+version_string:
+	.ascii U_BOOT_VERSION
+	.ascii " (", __DATE__, " - ", __TIME__, ")"
+	.ascii " ", CONFIG_IDENT_STRING, "\0"
+
+/*
+ * Vector Table
+ */
+	.text
+	. = EXC_OFF_SYS_RESET
+
+	.globl	_start
+	/* Start from here after reset/power on */
+_start:
+	li	r21, BOOTFLAG_COLD  /* Normal Power-On: Boot from FLASH */
+	b	boot_cold
+
+	.globl	_start_of_vectors
+_start_of_vectors:
+
+/* Machine check */
+	STD_EXCEPTION(0x200, MachineCheck, MachineCheckException)
+
+/* Data Storage exception. */
+	STD_EXCEPTION(0x300, DataStorage, UnknownException)
+
+/* Instruction Storage exception. */
+	STD_EXCEPTION(0x400, InstStorage, UnknownException)
+
+/* External Interrupt exception. */
+	STD_EXCEPTION(0x500, ExtInterrupt, UnknownException)
+
+/* Alignment exception. */
+	. = 0x600
+Alignment:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	mfspr	r4,DAR
+	stw	r4,_DAR(r21)
+	mfspr	r5,DSISR
+	stw	r5,_DSISR(r21)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_Alignment:
+	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
+	.long	int_return - _start + EXC_OFF_SYS_RESET
+
+/* Program check exception */
+	. = 0x700
+ProgramCheck:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_ProgramCheck:
+	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
+	.long	int_return - _start + EXC_OFF_SYS_RESET
+
+/* Floating Point Unit unavailable exception */
+	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
+
+/* Decrementer */
+	STD_EXCEPTION(0x900, Decrementer, timer_interrupt)
+
+/* Critical interrupt */
+	STD_EXCEPTION(0xa00, Critical, UnknownException)
+
+/* System Call */
+	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
+
+/* Trace interrupt */
+	STD_EXCEPTION(0xd00, Trace, UnknownException)
+
+/* Performance Monitor interrupt */
+	STD_EXCEPTION(0xf00, PerfMon, UnknownException)
+
+/* Intruction Translation Miss */
+	STD_EXCEPTION(0x1000, InstructionTLBMiss, UnknownException)
+
+/* Data Load Translation Miss */
+	STD_EXCEPTION(0x1100, DataLoadTLBMiss, UnknownException)
+
+/* Data Store Translation Miss */
+	STD_EXCEPTION(0x1200, DataStoreTLBMiss, UnknownException)
+
+/* Instruction Address Breakpoint */
+	STD_EXCEPTION(0x1300, InstructionAddrBreakpoint, DebugException)
+
+/* System Management interrupt */
+	STD_EXCEPTION(0x1400, SystemMgmtInterrupt, UnknownException)
+
+	.globl	_end_of_vectors
+_end_of_vectors:
+
+	. = 0x3000
+boot_cold:
+	/* Save msr contents */
+	mfmsr	r5
+
+	/* Set IMMR area to our preferred location */
+	lis	r4, CONFIG_DEFAULT_IMMR@h
+	lis	r3, CFG_IMMR@h
+	ori	r3, r3, CFG_IMMR@l
+	stw	r3, IMMRBAR(r4)
+	mtspr	MBAR, r3		/* IMMRBAR is mirrored into the MBAR SPR (311) */
+
+	/* Initialise the machine */
+	bl	cpu_early_init
+
+	/*
+	 * Set up Local Access Windows:
+	 *
+	 * 1) Boot/CS0 (boot FLASH)
+	 * 2) On-chip SRAM (initial stack purposes)
+	 */
+
+	/* Boot CS/CS0 window range */
+	lis     r3, CFG_IMMR@h
+	ori     r3, r3, CFG_IMMR@l
+
+	lis	r4, START_REG(CFG_FLASH_BASE)
+	ori	r4, r4, STOP_REG(CFG_FLASH_BASE, CFG_FLASH_SIZE)
+	stw	r4, LPCS0AW(r3)
+
+	/*
+	 * The SRAM window has a fixed size (256K), so only the start address
+	 * is necessary
+	 */
+	lis 	r4, START_REG(CFG_SRAM_BASE) & 0xff00
+	stw	r4, SRAMBAR(r3)
+
+	/*
+	 * According to MPC5121e RM, configuring local access windows should
+	 * be followed by a dummy read of the config register that was
+	 * modified last and an isync
+	 */
+	lwz	r4, SRAMBAR(r3)
+	isync
+
+	/*
+	 * Set configuration of the Boot/CS0, the SRAM window does not have a
+	 * config register so no params can be set for it
+	 */
+	lis     r3, (CFG_IMMR + LPC_OFFSET)@h
+	ori     r3, r3, (CFG_IMMR + LPC_OFFSET)@l
+
+	lis     r4, CFG_CS0_CFG@h
+	ori     r4, r4, CFG_CS0_CFG@l
+	stw     r4, CS0_CONFIG(r3)
+
+	/* Master enable all CS's */
+	lis	r4, CS_CTRL_ME@h
+	ori	r4, r4, CS_CTRL_ME@l
+	stw	r4, CS_CTRL(r3)
+
+	lis	r4, (CFG_MONITOR_BASE)@h
+	ori	r4, r4, (CFG_MONITOR_BASE)@l
+	addi	r5, r4, in_flash - _start + EXC_OFF_SYS_RESET
+	mtlr	r5
+	blr
+
+in_flash:
+	lis	r1, (CFG_INIT_RAM_ADDR + CFG_GBL_DATA_OFFSET)@h
+	ori	r1, r1, (CFG_INIT_RAM_ADDR + CFG_GBL_DATA_OFFSET)@l
+
+	li	r0, 0		/* Make room for stack frame header and	*/
+	stwu	r0, -4(r1)	/* clear final stack frame so that	*/
+	stwu	r0, -4(r1)	/* stack backtraces terminate cleanly	*/
+
+	/* let the C-code set up the rest			*/
+	/*							*/
+	/* Be careful to keep code relocatable & stack humble	*/
+	/*------------------------------------------------------*/
+
+	GET_GOT			/* initialize GOT access	*/
+
+	/* r3: IMMR */
+	lis	r3, CFG_IMMR@h
+	/* run low-level CPU init code (in Flash) */
+	bl	cpu_init_f
+
+	/* r3: BOOTFLAG */
+	mr	r3, r21
+	/* run 1st part of board init code (in Flash) */
+	bl	board_init_f
+
+	/* NOTREACHED - board_init_f() does not return */
+
+/*
+ * This code finishes saving the registers to the exception frame
+ * and jumps to the appropriate handler for the exception.
+ * Register r21 is pointer into trap frame, r1 has new stack pointer.
+ */
+	.globl	transfer_to_handler
+transfer_to_handler:
+	stw	r22,_NIP(r21)
+	lis	r22,MSR_POW@h
+	andc	r23,r23,r22
+	stw	r23,_MSR(r21)
+	SAVE_GPR(7, r21)
+	SAVE_4GPRS(8, r21)
+	SAVE_8GPRS(12, r21)
+	SAVE_8GPRS(24, r21)
+	mflr	r23
+	andi.	r24,r23,0x3f00		/* get vector offset */
+	stw	r24,TRAP(r21)
+	li	r22,0
+	stw	r22,RESULT(r21)
+	lwz	r24,0(r23)		/* virtual address of handler */
+	lwz	r23,4(r23)		/* where to go when done */
+	mtspr	SRR0,r24
+	mtspr	SRR1,r20
+	mtlr	r23
+	SYNC
+	rfi				/* jump to handler, enable MMU */
+
+int_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	SRR0,r2
+	mtspr	SRR1,r0
+	lwz	r0,GPR0(r1)
+	lwz	r2,GPR2(r1)
+	lwz	r1,GPR1(r1)
+	SYNC
+	rfi
+
+/*
+ * This code initialises the machine, it expects original MSR contents to be in r5.
+ */
+cpu_early_init:
+	/* Initialize machine status; enable machine check interrupt */
+	/*-----------------------------------------------------------*/
+
+	li	r3, MSR_KERNEL			/* Set ME and RI flags */
+	rlwimi	r3, r5, 0, 25, 25		/* preserve IP bit */
+#ifdef DEBUG
+	rlwimi	r3, r5, 0, 21, 22		/* debugger might set SE, BE bits */
+#endif
+	mtmsr	r3
+	SYNC
+	mtspr	SRR1, r3			/* Mirror current MSR state in SRR1 */
+
+	lis	r3, CFG_IMMR@h
+
+#if defined(CONFIG_WATCHDOG)
+	/* Initialise the watchdog and reset it */
+	/*--------------------------------------*/
+	lis r4, CFG_WATCHDOG_VALUE
+	ori r4, r4, (SWCRR_SWEN | SWCRR_SWRI | SWCRR_SWPR)
+	stw r4, SWCRR(r3)
+
+	/* reset */
+	li	r4, 0x556C
+	sth	r4, SWSRR@l(r3)
+	li	r4, 0x0
+	ori	r4, r4, 0xAA39
+	sth	r4, SWSRR@l(r3)
+#else
+	/* Disable the watchdog */
+	/*----------------------*/
+	lwz r4, SWCRR(r3)
+	/*
+	 * Check to see if it's enabled for disabling: once disabled by s/w
+	 * it's not possible to re-enable it
+	 */
+	andi. r4, r4, 0x4
+	beq 1f
+	xor r4, r4, r4
+	stw r4, SWCRR(r3)
+1:
+#endif /* CONFIG_WATCHDOG */
+
+	/* Initialize the Hardware Implementation-dependent Registers */
+	/* HID0 also contains cache control			*/
+	/*------------------------------------------------------*/
+	lis	r3, CFG_HID0_INIT@h
+	ori	r3, r3, CFG_HID0_INIT@l
+	SYNC
+	mtspr	HID0, r3
+
+	lis	r3, CFG_HID0_FINAL@h
+	ori	r3, r3, CFG_HID0_FINAL@l
+	SYNC
+	mtspr	HID0, r3
+
+	lis	r3, CFG_HID2@h
+	ori	r3, r3, CFG_HID2@l
+	SYNC
+	mtspr	HID2, r3
+	sync
+	blr
+
+
+/* Cache functions.
+ *
+ * Note: requires that all cache bits in
+ * HID0 are in the low half word.
+ */
+	.globl	icache_enable
+icache_enable:
+	mfspr	r3, HID0
+	ori	r3, r3, HID0_ICE
+	lis	r4, 0
+	ori	r4, r4, HID0_ILOCK
+	andc	r3, r3, r4
+	ori	r4, r3, HID0_ICFI
+	isync
+	mtspr	HID0, r4    /* sets enable and invalidate, clears lock */
+	isync
+	mtspr	HID0, r3	/* clears invalidate */
+	blr
+
+	.globl	icache_disable
+icache_disable:
+	mfspr	r3, HID0
+	lis	r4, 0
+	ori	r4, r4, HID0_ICE|HID0_ILOCK
+	andc	r3, r3, r4
+	ori	r4, r3, HID0_ICFI
+	isync
+	mtspr	HID0, r4     /* sets invalidate, clears enable and lock*/
+	isync
+	mtspr	HID0, r3	/* clears invalidate */
+	blr
+
+	.globl	icache_status
+icache_status:
+	mfspr	r3, HID0
+	rlwinm	r3, r3, (31 - HID0_ICE_SHIFT + 1), 31, 31
+	blr
+
+	.globl	dcache_enable
+dcache_enable:
+	mfspr	r3, HID0
+	li	r5, HID0_DCFI|HID0_DLOCK
+	andc	r3, r3, r5
+	mtspr	HID0, r3		/* no invalidate, unlock */
+	ori	r3, r3, HID0_DCE
+	ori	r5, r3, HID0_DCFI
+	mtspr	HID0, r5		/* enable + invalidate */
+	mtspr	HID0, r3		/* enable */
+	sync
+	blr
+
+	.globl	dcache_disable
+dcache_disable:
+	mfspr	r3, HID0
+	lis	r4, 0
+	ori	r4, r4, HID0_DCE|HID0_DLOCK
+	andc	r3, r3, r4
+	ori	r4, r3, HID0_DCI
+	sync
+	mtspr	HID0, r4	/* sets invalidate, clears enable and lock */
+	sync
+	mtspr	HID0, r3	/* clears invalidate */
+	blr
+
+	.globl	dcache_status
+dcache_status:
+	mfspr	r3, HID0
+	rlwinm	r3, r3, (31 - HID0_DCE_SHIFT + 1), 31, 31
+	blr
+
+	.globl get_pvr
+get_pvr:
+	mfspr	r3, PVR
+	blr
+
+/*------------------------------------------------------------------------------- */
+/* Function:	 ppcDcbf */
+/* Description:	 Data Cache block flush */
+/* Input:	 r3 = effective address */
+/* Output:	 none. */
+/*------------------------------------------------------------------------------- */
+	.globl	ppcDcbf
+ppcDcbf:
+	dcbf	r0,r3
+	blr
+
+/*------------------------------------------------------------------------------- */
+/* Function:	 ppcDcbi */
+/* Description:	 Data Cache block Invalidate */
+/* Input:	 r3 = effective address */
+/* Output:	 none. */
+/*------------------------------------------------------------------------------- */
+	.globl	ppcDcbi
+ppcDcbi:
+	dcbi	r0,r3
+	blr
+
+/*--------------------------------------------------------------------------
+ * Function:	 ppcDcbz
+ * Description:	 Data Cache block zero.
+ * Input:	 r3 = effective address
+ * Output:	 none.
+ *-------------------------------------------------------------------------- */
+
+	.globl	ppcDcbz
+ppcDcbz:
+	dcbz	r0,r3
+	blr
+
+	.globl	ppcDWstore
+ppcDWstore:
+	lfd	1, 0(r4)
+	stfd	1, 0(r3)
+	blr
+
+	.globl	ppcDWload
+ppcDWload:
+	lfd	1, 0(r3)
+	stfd	1, 0(r4)
+	blr
+
+/*-------------------------------------------------------------------*/
+
+/*
+ * 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:
+	mr	r1,  r3		/* Set new stack pointer	*/
+	mr	r9,  r4		/* Save copy of Global 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
+	lwz	r5, GOT(__init_end)
+	sub	r5, r5, r4
+	li	r6, CFG_CACHELINE_SIZE		/* Cache Line Size */
+
+	/*
+	 * Fix GOT pointer:
+	 *
+	 * New GOT-PTR = (old GOT-PTR - CFG_MONITOR_BASE)
+	 *		+ Destination Address
+	 *
+	 * Offset:
+	 */
+	sub	r15, r10, r4
+
+	/* First our own GOT */
+	add	r14, r14, r15
+	/* then the one used by the C code */
+	add	r30, r30, r15
+
+	/*
+	 * Now relocate code
+	 */
+	cmplw	cr1,r3,r4
+	addi	r0,r5,3
+	srwi.	r0,r0,2
+	beq	cr1,4f		/* In place copy is not necessary */
+	beq	7f		/* Protect against 0 count	  */
+	mtctr	r0
+	bge	cr1,2f
+	la	r8,-4(r4)
+	la	r7,-4(r3)
+
+	/* copy */
+1:	lwzu	r0,4(r8)
+	stwu	r0,4(r7)
+	bdnz	1b
+
+	addi	r0,r5,3
+	srwi.	r0,r0,2
+	mtctr	r0
+	la	r8,-4(r4)
+	la	r7,-4(r3)
+
+	/* and compare */
+20:	lwzu	r20,4(r8)
+	lwzu	r21,4(r7)
+	xor. r22, r20, r21
+	bne  30f
+	bdnz	20b
+	b 4f
+
+	/* compare failed */
+30:	li r3, 0
+	blr
+
+2:	slwi	r0,r0,2 /* re copy in reverse order ... y do we needed it? */
+	add	r8,r4,r0
+	add	r7,r3,r0
+3:	lwzu	r0,-4(r8)
+	stwu	r0,-4(r7)
+	bdnz	3b
+
+/*
+ * Now flush the cache: note that we must start from a cache aligned
+ * address. Otherwise we might miss one cache line.
+ */
+4:	cmpwi	r6,0
+	add	r5,r3,r5
+	beq	7f		/* Always flush prefetch queue in any case */
+	subi	r0,r6,1
+	andc	r3,r3,r0
+	mr	r4,r3
+5:	dcbst	0,r4
+	add	r4,r4,r6
+	cmplw	r4,r5
+	blt	5b
+	sync			/* Wait for all dcbst to complete on bus */
+	mr	r4,r3
+6:	icbi	0,r4
+	add	r4,r4,r6
+	cmplw	r4,r5
+	blt	6b
+7:	sync			/* Wait for all icbi to complete on bus	*/
+	isync
+
+/*
+ * We are done. Do not return, instead branch to second part of board
+ * initialization, now running from RAM.
+ */
+	addi	r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
+	mtlr	r0
+	blr
+
+in_ram:
+	/*
+	 * Relocation Function, r14 point to got2+0x8000
+	 *
+	 * Adjust got2 pointers, no need to check for 0, this code
+	 * already puts a few entries in the table.
+	 */
+	li	r0,__got2_entries@sectoff@l
+	la	r3,GOT(_GOT2_TABLE_)
+	lwz	r11,GOT(_GOT2_TABLE_)
+	mtctr	r0
+	sub	r11,r3,r11
+	addi	r3,r3,-4
+1:	lwzu	r0,4(r3)
+	add	r0,r0,r11
+	stw	r0,0(r3)
+	bdnz	1b
+
+	/*
+	 * Now adjust the fixups and the pointers to the fixups
+	 * in case we need to move ourselves again.
+	 */
+2:	li	r0,__fixup_entries@sectoff@l
+	lwz	r3,GOT(_FIXUP_TABLE_)
+	cmpwi	r0,0
+	mtctr	r0
+	addi	r3,r3,-4
+	beq	4f
+3:	lwzu	r4,4(r3)
+	lwzux	r0,r4,r11
+	add	r0,r0,r11
+	stw	r10,0(r3)
+	stw	r0,0(r4)
+	bdnz	3b
+4:
+clear_bss:
+	/*
+	 * Now clear BSS segment
+	 */
+	lwz	r3,GOT(__bss_start)
+	lwz	r4,GOT(_end)
+
+	cmplw	0, r3, r4
+	beq	6f
+
+	li	r0, 0
+5:
+	stw	r0, 0(r3)
+	addi	r3, r3, 4
+	cmplw	0, r3, r4
+	bne	5b
+6:
+	mr	r3, r9		/* Global Data pointer		*/
+	mr	r4, r10		/* Destination Address		*/
+	bl	board_init_r
+
+	/*
+	 * Copy exception vector code to low memory
+	 *
+	 * r3: dest_addr
+	 * r7: source address, r8: end address, r9: target address
+	 */
+	.globl	trap_init
+trap_init:
+	lwz	r7, GOT(_start)
+	lwz	r8, GOT(_end_of_vectors)
+
+	li	r9, 0x100	/* reset vector at 0x100 */
+
+	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
+	bne	1b
+
+	/*
+	 * relocate `hdlr' and `int_return' entries
+	 */
+	li	r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
+	li	r8, Alignment - _start + EXC_OFF_SYS_RESET
+2:
+	bl	trap_reloc
+	addi	r7, r7, 0x100		/* next exception vector */
+	cmplw	0, r7, r8
+	blt	2b
+
+	li	r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
+	bl	trap_reloc
+
+	li	r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
+	bl	trap_reloc
+
+	li	r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
+	li	r8, SystemCall - _start + EXC_OFF_SYS_RESET
+3:
+	bl	trap_reloc
+	addi	r7, r7, 0x100		/* next exception vector */
+	cmplw	0, r7, r8
+	blt	3b
+
+	li	r7, .L_Trace - _start + EXC_OFF_SYS_RESET
+	li	r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
+4:
+	bl	trap_reloc
+	addi	r7, r7, 0x100		/* next exception vector */
+	cmplw	0, r7, r8
+	blt	4b
+
+	mfmsr	r3			/* now that the vectors have */
+	lis	r7, MSR_IP@h		/* relocated into low memory */
+	ori	r7, r7, MSR_IP@l	/* MSR[IP] can be turned off */
+	andc	r3, r3, r7		/* (if it was on) */
+	SYNC				/* Some chip revs need this... */
+	mtmsr	r3
+	SYNC
+
+	mtlr	r4			/* restore link register    */
+	blr
+
+	/*
+	 * 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, 4(r7)		/* int_return ...	*/
+	add	r0, r0, r3		/*  ... += dest_addr	*/
+	stw	r0, 4(r7)
+
+	blr
diff --git a/cpu/mpc512x/traps.c b/cpu/mpc512x/traps.c
new file mode 100644
index 0000000000..8455c92761
--- /dev/null
+++ b/cpu/mpc512x/traps.c
@@ -0,0 +1,205 @@
+/*
+ * (C) Copyright 2000 - 2007
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 1995-1996  Gary Thomas (gdt@linuxppc.org)
+ *
+ * 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
+ *
+ * Derived from the MPC83xx code.
+ */
+
+/*
+ * This file handles the architecture-dependent parts of hardware
+ * exceptions
+ */
+
+#include <common.h>
+#include <asm/processor.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+extern unsigned long search_exception_table(unsigned long);
+
+#define END_OF_MEM	(gd->bd->bi_memstart + gd->bd->bi_memsize)
+
+/*
+ * Trap & Exception support
+ */
+
+void
+print_backtrace (unsigned long *sp)
+{
+	int cnt = 0;
+	unsigned long i;
+
+	puts ("Call backtrace: ");
+	while (sp) {
+		if ((uint)sp > END_OF_MEM)
+			break;
+
+		i = sp[1];
+		if (cnt++ % 7 == 0)
+			putc ('\n');
+		printf ("%08lX ", i);
+		if (cnt > 32) break;
+		sp = (unsigned long *) *sp;
+	}
+	putc ('\n');
+}
+
+void show_regs (struct pt_regs * regs)
+{
+	int i;
+
+	printf ("NIP: %08lX XER: %08lX LR: %08lX REGS: %p TRAP: %04lx DAR: %08lX\n",
+	       regs->nip, regs->xer, regs->link, regs, regs->trap, regs->dar);
+	printf ("MSR: %08lx EE: %01x PR: %01x FP: %01x ME: %01x IR/DR: %01x%01x\n",
+	       regs->msr, regs->msr & MSR_EE ? 1 : 0, regs->msr & MSR_PR ? 1 : 0,
+	       regs->msr & MSR_FP ? 1 : 0,regs->msr & MSR_ME ? 1 : 0,
+	       regs->msr & MSR_IR ? 1 : 0,
+	       regs->msr & MSR_DR ? 1 : 0);
+
+	putc ('\n');
+	for (i = 0;  i < 32;  i++) {
+		if ((i % 8) == 0) {
+			printf ("GPR%02d: ", i);
+		}
+
+		printf ("%08lX ", regs->gpr[i]);
+		if ((i % 8) == 7) {
+			putc ('\n');
+		}
+	}
+}
+
+
+void
+_exception (int signr, struct pt_regs *regs)
+{
+	show_regs (regs);
+	print_backtrace ((unsigned long *)regs->gpr[1]);
+	panic ("Exception at pc %lx signal %d", regs->nip,signr);
+}
+
+
+void
+MachineCheckException (struct pt_regs *regs)
+{
+	unsigned long fixup;
+
+	if ((fixup = search_exception_table (regs->nip)) != 0) {
+		regs->nip = fixup;
+		return;
+	}
+
+#ifdef CONFIG_CMD_KGDB
+	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+		return;
+#endif
+
+	puts ("Machine check.\nCaused by (from msr): ");
+	printf ("regs %p ",regs);
+	switch (regs->msr & 0x00FF0000) {
+	case (0x80000000 >> 10):
+		puts ("Instruction cache parity signal\n");
+		break;
+	case (0x80000000 >> 11):
+		puts ("Data cache parity signal\n");
+		break;
+	case (0x80000000 >> 12):
+		puts ("Machine check signal\n");
+		break;
+	case (0x80000000 >> 13):
+		puts ("Transfer error ack signal\n");
+		break;
+	case (0x80000000 >> 14):
+		puts ("Data parity signal\n");
+		break;
+	case (0x80000000 >> 15):
+		puts ("Address parity signal\n");
+		break;
+	default:
+		puts ("Unknown values in msr\n");
+	}
+	show_regs (regs);
+	print_backtrace ((unsigned long *)regs->gpr[1]);
+
+	panic ("machine check");
+}
+
+void
+AlignmentException (struct pt_regs *regs)
+{
+#ifdef CONFIG_CMD_KGDB
+	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+		return;
+#endif
+	show_regs (regs);
+	print_backtrace ((unsigned long *)regs->gpr[1]);
+	panic ("Alignment Exception");
+}
+
+void
+ProgramCheckException (struct pt_regs *regs)
+{
+#ifdef CONFIG_CMD_KGDB
+	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+		return;
+#endif
+	show_regs (regs);
+	print_backtrace ((unsigned long *)regs->gpr[1]);
+	panic ("Program Check Exception");
+}
+
+void
+SoftEmuException (struct pt_regs *regs)
+{
+#ifdef CONFIG_CMD_KGDB
+	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+		return;
+#endif
+	show_regs (regs);
+	print_backtrace ((unsigned long *)regs->gpr[1]);
+	panic ("Software Emulation Exception");
+}
+
+
+void
+UnknownException (struct pt_regs *regs)
+{
+#ifdef CONFIG_CMD_KGDB
+	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+		return;
+#endif
+	printf ("Bad trap at PC: %lx, SR: %lx, vector=%lx\n",
+	       regs->nip, regs->msr, regs->trap);
+	_exception (0, regs);
+}
+
+#ifdef CONFIG_CMD_BEDBUG
+extern void do_bedbug_breakpoint (struct pt_regs *);
+#endif
+
+void
+DebugException (struct pt_regs *regs)
+{
+	printf ("Debugger trap at @ %lx\n", regs->nip );
+	show_regs (regs);
+#ifdef CONFIG_CMD_BEDBUG
+	do_bedbug_breakpoint (regs);
+#endif
+}
diff --git a/cpu/mpc5xx/config.mk b/cpu/mpc5xx/config.mk
index 5b26a76b3e..e95b8a1a85 100644
--- a/cpu/mpc5xx/config.mk
+++ b/cpu/mpc5xx/config.mk
@@ -28,6 +28,9 @@
 #
 
 
-PLATFORM_RELFLAGS += 	-fPIC -ffixed-r14 -meabi
+PLATFORM_RELFLAGS += 	-fPIC -ffixed-r14 -meabi -mrelocatable
 
 PLATFORM_CPPFLAGS +=	-DCONFIG_5xx -ffixed-r2 -ffixed-r29 -mpowerpc -msoft-float
+
+# Use default linker script.  Board port can override in board/*/config.mk
+LDSCRIPT := $(SRCTREE)/cpu/mpc5xx/u-boot.lds
diff --git a/cpu/mpc5xx/interrupts.c b/cpu/mpc5xx/interrupts.c
index 7f6e1363e7..a4f47c74b5 100644
--- a/cpu/mpc5xx/interrupts.c
+++ b/cpu/mpc5xx/interrupts.c
@@ -178,7 +178,7 @@ void timer_interrupt_cpu (struct pt_regs *regs)
 	return;
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 /*******************************************************************************
  *
  * irqinfo - print information about IRQs
@@ -204,4 +204,4 @@ int do_irqinfo(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 }
 
 
-#endif  /* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/mpc5xx/start.S b/cpu/mpc5xx/start.S
index 087435e5be..0637003ce2 100644
--- a/cpu/mpc5xx/start.S
+++ b/cpu/mpc5xx/start.S
@@ -155,7 +155,7 @@ in_flash:
 	/* Initialize some SPRs that are hard to access from C			*/
 	/*----------------------------------------------------------------------*/
 
-	lis     r3, CFG_IMMR@h          	/* Pass IMMR as arg1 to C routine */
+	lis     r3, CFG_IMMR@h			/* Pass IMMR as arg1 to C routine */
 	lis	r2, CFG_INIT_SP_ADDR@h
 	ori	r1, r2, CFG_INIT_SP_ADDR@l	/* Set up the stack in internal SRAM */
 	/* Note: R0 is still 0 here */
@@ -210,7 +210,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -228,7 +228,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
diff --git a/cpu/mpc5xx/traps.c b/cpu/mpc5xx/traps.c
index 14fd59e4fa..d22b89a1f5 100644
--- a/cpu/mpc5xx/traps.c
+++ b/cpu/mpc5xx/traps.c
@@ -36,11 +36,11 @@
 #include <command.h>
 #include <asm/processor.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -131,7 +131,7 @@ void MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -165,7 +165,7 @@ void MachineCheckException(struct pt_regs *regs)
  */
 void AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -179,7 +179,7 @@ void AlignmentException(struct pt_regs *regs)
  */
 void ProgramCheckException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -193,7 +193,7 @@ void ProgramCheckException(struct pt_regs *regs)
  */
 void SoftEmuException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -208,7 +208,7 @@ void SoftEmuException(struct pt_regs *regs)
  */
 void UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -224,7 +224,7 @@ void DebugException(struct pt_regs *regs)
 {
 	printf("Debugger trap at @ %lx\n", regs->nip );
   	show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
   	do_bedbug_breakpoint( regs );
 #endif
 }
diff --git a/cpu/mpc5xx/u-boot.lds b/cpu/mpc5xx/u-boot.lds
new file mode 100644
index 0000000000..10001b1c1d
--- /dev/null
+++ b/cpu/mpc5xx/u-boot.lds
@@ -0,0 +1,139 @@
+/*
+ * (C) Copyright 2001	Wolfgang Denk, DENX Software Engineering, wd@denx.de
+ * (C) Copyright 2003	Martin Winistoerfer, martinwinistoerfer@gmx.ch
+ *
+ * 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
+ */
+
+OUTPUT_ARCH(powerpc)
+SEARCH_DIR(/lib); SEARCH_DIR(/usr/lib); SEARCH_DIR(/usr/local/lib); SEARCH_DIR(/usr/local/powerpc-any-elf/lib);
+/* Do we need any of these for elf?
+   __DYNAMIC = 0;    */
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  . = + SIZEOF_HEADERS;
+  .interp : { *(.interp) }
+  .hash          : { *(.hash)		}
+  .dynsym        : { *(.dynsym)		}
+  .dynstr        : { *(.dynstr)		}
+  .rel.text      : { *(.rel.text)		}
+  .rela.text     : { *(.rela.text) 	}
+  .rel.data      : { *(.rel.data)		}
+  .rela.data     : { *(.rela.data) 	}
+  .rel.rodata    : { *(.rel.rodata) 	}
+  .rela.rodata   : { *(.rela.rodata) 	}
+  .rel.got       : { *(.rel.got)		}
+  .rela.got      : { *(.rela.got)		}
+  .rel.ctors     : { *(.rel.ctors)	}
+  .rela.ctors    : { *(.rela.ctors)	}
+  .rel.dtors     : { *(.rel.dtors)	}
+  .rela.dtors    : { *(.rela.dtors)	}
+  .rel.bss       : { *(.rel.bss)		}
+  .rela.bss      : { *(.rela.bss)		}
+  .rel.plt       : { *(.rel.plt)		}
+  .rela.plt      : { *(.rela.plt)		}
+  .init          : { *(.init)	}
+  .plt : { *(.plt) }
+  .text      :
+  {
+    /* WARNING - the following is hand-optimized to fit within	*/
+    /* the sector layout of our flash chips!	XXX FIXME XXX	*/
+
+    cpu/mpc5xx/start.o	(.text)
+
+    *(.text)
+    *(.got1)
+  }
+  _etext = .;
+  PROVIDE (etext = .);
+  .rodata    :
+  {
+    *(.rodata)
+    *(.rodata1)
+    *(.rodata.str1.4)
+    *(.eh_frame)
+  }
+  .fini      : { *(.fini)    } =0
+  .ctors     : { *(.ctors)   }
+  .dtors     : { *(.dtors)   }
+
+  /* Read-write section, merged into data segment: */
+  . = (. + 0x00FF) & 0xFFFFFF00;
+  _erotext = .;
+  PROVIDE (erotext = .);
+  .reloc   :
+  {
+    *(.got)
+    _GOT2_TABLE_ = .;
+    *(.got2)
+    _FIXUP_TABLE_ = .;
+    *(.fixup)
+  }
+  __got2_entries = (_FIXUP_TABLE_ - _GOT2_TABLE_) >>2;
+  __fixup_entries = (. - _FIXUP_TABLE_)>>2;
+
+  .data    :
+  {
+    *(.data)
+    *(.data1)
+    *(.sdata)
+    *(.sdata2)
+    *(.dynamic)
+    CONSTRUCTORS
+  }
+  _edata  =  .;
+  PROVIDE (edata = .);
+
+  . = .;
+  __u_boot_cmd_start = .;
+  .u_boot_cmd : { *(.u_boot_cmd) }
+  __u_boot_cmd_end = .;
+
+
+  . = .;
+  __start___ex_table = .;
+  __ex_table : { *(__ex_table) }
+  __stop___ex_table = .;
+
+  . = ALIGN(256);
+  __init_begin = .;
+  .text.init : { *(.text.init) }
+  .data.init : { *(.data.init) }
+  . = ALIGN(256);
+  __init_end = .;
+
+  __bss_start = .;
+  .bss       :
+  {
+   *(.sbss) *(.scommon)
+   *(.dynbss)
+   *(.bss)
+   *(COMMON)
+  }
+
+  _end = . ;
+  PROVIDE (end = .);
+/*   . = env_start;
+	.ppcenv	:
+	{
+		common/environment.o (.ppcenv)
+	}
+*/
+}
diff --git a/cpu/mpc5xxx/Makefile b/cpu/mpc5xxx/Makefile
index 235adb7c04..312b0bfc6c 100644
--- a/cpu/mpc5xxx/Makefile
+++ b/cpu/mpc5xxx/Makefile
@@ -28,7 +28,7 @@ LIB	= $(obj)lib$(CPU).a
 START	= start.o
 SOBJS	= io.o firmware_sc_task_bestcomm.impl.o firmware_sc_task.impl.o
 COBJS	= i2c.o traps.o cpu.o cpu_init.o fec.o ide.o interrupts.o \
-	  loadtask.o pci_mpc5200.o serial.o speed.o usb_ohci.o
+	  loadtask.o pci_mpc5200.o serial.o speed.o usb_ohci.o usb.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/mpc5xxx/config.mk b/cpu/mpc5xxx/config.mk
index ecd94e9b34..0e861c4a0f 100644
--- a/cpu/mpc5xxx/config.mk
+++ b/cpu/mpc5xxx/config.mk
@@ -21,7 +21,10 @@
 # MA 02111-1307 USA
 #
 
-PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi
+PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -mrelocatable
 
 PLATFORM_CPPFLAGS += -DCONFIG_MPC5xxx -ffixed-r2 -ffixed-r29 \
 		     -mstring -mcpu=603e -mmultiple
+
+# Use default linker script.  Board port can override in board/*/config.mk
+LDSCRIPT := $(SRCTREE)/cpu/mpc5xxx/u-boot.lds
diff --git a/cpu/mpc5xxx/cpu_init.c b/cpu/mpc5xxx/cpu_init.c
index d7440308a4..bc6201ec0a 100644
--- a/cpu/mpc5xxx/cpu_init.c
+++ b/cpu/mpc5xxx/cpu_init.c
@@ -198,7 +198,7 @@ int cpu_init_r (void)
 	/* route critical ints to normal ints */
 	*(vu_long *)MPC5XXX_ICTL_EXT |= 0x00000001;
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_MPC5xxx_FEC)
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_MPC5xxx_FEC)
 	/* load FEC microcode */
 	loadtask(0, 2);
 #endif
diff --git a/cpu/mpc5xxx/fec.c b/cpu/mpc5xxx/fec.c
index 813636655b..1d3da779a7 100644
--- a/cpu/mpc5xxx/fec.c
+++ b/cpu/mpc5xxx/fec.c
@@ -18,10 +18,10 @@ DECLARE_GLOBAL_DATA_PTR;
 
 /* #define DEBUG	0x28 */
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
 	defined(CONFIG_MPC5xxx_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
 
@@ -889,13 +889,20 @@ int mpc5xxx_fec_initialize(bd_t * bis)
 	fec->eth = (ethernet_regs *)MPC5XXX_FEC;
 	fec->tbdBase = (FEC_TBD *)FEC_BD_BASE;
 	fec->rbdBase = (FEC_RBD *)(FEC_BD_BASE + FEC_TBD_NUM * sizeof(FEC_TBD));
-#if defined(CONFIG_CANMB)    || defined(CONFIG_HMI1001)	|| \
-    defined(CONFIG_ICECUBE)  || defined(CONFIG_INKA4X0)	|| \
-    defined(CONFIG_JUPITER)  || defined(CONFIG_MCC200)	|| \
-    defined(CONFIG_MOTIONPRO)|| defined(CONFIG_O2DNT)	|| \
-    defined(CONFIG_PM520)    || defined(CONFIG_TOP5200)	|| \
-    defined(CONFIG_TQM5200)  || defined(CONFIG_UC101)	|| \
-    defined(CONFIG_V38B)
+#if defined(CONFIG_CANMB)		|| \
+	defined(CONFIG_CM5200)		|| \
+	defined(CONFIG_HMI1001)		|| \
+	defined(CONFIG_ICECUBE)		|| \
+	defined(CONFIG_INKA4X0)		|| \
+	defined(CONFIG_JUPITER)		|| \
+	defined(CONFIG_MCC200)		|| \
+	defined(CONFIG_MOTIONPRO)	|| \
+	defined(CONFIG_O2DNT)		|| \
+	defined(CONFIG_PM520)		|| \
+	defined(CONFIG_TOP5200)		|| \
+	defined(CONFIG_TQM5200)		|| \
+	defined(CONFIG_UC101)		|| \
+	defined(CONFIG_V38B)
 # ifndef CONFIG_FEC_10MBIT
 	fec->xcv_type = MII100;
 # else
@@ -917,7 +924,7 @@ int mpc5xxx_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,
 			fec5xxx_miiphy_read, fec5xxx_miiphy_write);
 #endif
diff --git a/cpu/mpc5xxx/ide.c b/cpu/mpc5xxx/ide.c
index 29b99f6b15..087ddac109 100644
--- a/cpu/mpc5xxx/ide.c
+++ b/cpu/mpc5xxx/ide.c
@@ -24,7 +24,7 @@
  */
 #include <common.h>
 
-#ifdef CFG_CMD_IDE
+#if defined(CONFIG_CMD_IDE)
 #include <mpc5xxx.h>
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -85,4 +85,4 @@ int ide_preinit (void)
 
 	return (0);
 }
-#endif /* CFG_CMD_IDE */
+#endif
diff --git a/cpu/mpc5xxx/interrupts.c b/cpu/mpc5xxx/interrupts.c
index beeb222636..8816dd1e2d 100644
--- a/cpu/mpc5xxx/interrupts.c
+++ b/cpu/mpc5xxx/interrupts.c
@@ -310,7 +310,7 @@ void irq_free_handler(int irq)
 
 /****************************************************************************/
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 void do_irqinfo(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
 {
 	int irq, re_enable;
diff --git a/cpu/mpc5xxx/start.S b/cpu/mpc5xxx/start.S
index 3936b5551f..9b1bd48c73 100644
--- a/cpu/mpc5xxx/start.S
+++ b/cpu/mpc5xxx/start.S
@@ -208,7 +208,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -227,7 +227,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
diff --git a/cpu/mpc5xxx/traps.c b/cpu/mpc5xxx/traps.c
index 2ee782b9c8..daa1ec6b58 100644
--- a/cpu/mpc5xxx/traps.c
+++ b/cpu/mpc5xxx/traps.c
@@ -37,7 +37,7 @@
 #include <command.h>
 #include <asm/processor.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
@@ -123,7 +123,7 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -158,7 +158,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -170,7 +170,7 @@ AlignmentException(struct pt_regs *regs)
 void
 ProgramCheckException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -182,7 +182,7 @@ ProgramCheckException(struct pt_regs *regs)
 void
 SoftEmuException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -195,7 +195,7 @@ SoftEmuException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -204,7 +204,7 @@ UnknownException(struct pt_regs *regs)
 	_exception(0, regs);
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -214,7 +214,7 @@ DebugException(struct pt_regs *regs)
 
   printf("Debugger trap at @ %lx\n", regs->nip );
   show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
   do_bedbug_breakpoint( regs );
 #endif
 }
diff --git a/cpu/mpc5xxx/u-boot-customlayout.lds b/cpu/mpc5xxx/u-boot-customlayout.lds
new file mode 100644
index 0000000000..11079430d5
--- /dev/null
+++ b/cpu/mpc5xxx/u-boot-customlayout.lds
@@ -0,0 +1,135 @@
+/*
+ * (C) Copyright 2003-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
+ */
+
+OUTPUT_ARCH(powerpc)
+SEARCH_DIR(/lib); SEARCH_DIR(/usr/lib); SEARCH_DIR(/usr/local/lib); SEARCH_DIR(/usr/local/powerpc-any-elf/lib);
+/* Do we need any of these for elf?
+   __DYNAMIC = 0;    */
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  . = + SIZEOF_HEADERS;
+  .interp : { *(.interp) }
+  .hash          : { *(.hash)		}
+  .dynsym        : { *(.dynsym)		}
+  .dynstr        : { *(.dynstr)		}
+  .rel.text      : { *(.rel.text)		}
+  .rela.text     : { *(.rela.text) 	}
+  .rel.data      : { *(.rel.data)		}
+  .rela.data     : { *(.rela.data) 	}
+  .rel.rodata    : { *(.rel.rodata) 	}
+  .rela.rodata   : { *(.rela.rodata) 	}
+  .rel.got       : { *(.rel.got)		}
+  .rela.got      : { *(.rela.got)		}
+  .rel.ctors     : { *(.rel.ctors)	}
+  .rela.ctors    : { *(.rela.ctors)	}
+  .rel.dtors     : { *(.rel.dtors)	}
+  .rela.dtors    : { *(.rela.dtors)	}
+  .rel.bss       : { *(.rel.bss)		}
+  .rela.bss      : { *(.rela.bss)		}
+  .rel.plt       : { *(.rel.plt)		}
+  .rela.plt      : { *(.rela.plt)		}
+  .init          : { *(.init)	}
+  .plt : { *(.plt) }
+  .text      :
+  {
+    /* WARNING - the following is hand-optimized to fit within  */
+    /* the sector layout of our flash chips!    XXX FIXME XXX   */
+
+    cpu/mpc5xxx/start.o          (.text)
+    cpu/mpc5xxx/traps.o          (.text)
+    lib_generic/crc32.o         (.text)
+    lib_ppc/cache.o             (.text)
+    lib_ppc/time.o              (.text)
+
+    . = DEFINED(env_offset) ? env_offset : .;
+    common/environment.o        (.ppcenv)
+
+    *(.text)
+    *(.got1)
+    . = ALIGN(16);
+    *(.rodata)
+    *(.rodata1)
+    *(.rodata.str1.4)
+    *(.eh_frame)
+  }
+  .fini      : { *(.fini)    } =0
+  .ctors     : { *(.ctors)   }
+  .dtors     : { *(.dtors)   }
+
+  /* Read-write section, merged into data segment: */
+  . = (. + 0x0FFF) & 0xFFFFF000;
+  _erotext = .;
+  PROVIDE (erotext = .);
+  .reloc   :
+  {
+    *(.got)
+    _GOT2_TABLE_ = .;
+    *(.got2)
+    _FIXUP_TABLE_ = .;
+    *(.fixup)
+  }
+  __got2_entries = (_FIXUP_TABLE_ - _GOT2_TABLE_) >> 2;
+  __fixup_entries = (. - _FIXUP_TABLE_) >> 2;
+
+  .data    :
+  {
+    *(.data)
+    *(.data1)
+    *(.sdata)
+    *(.sdata2)
+    *(.dynamic)
+    CONSTRUCTORS
+  }
+  _edata  =  .;
+  PROVIDE (edata = .);
+
+  . = .;
+  __u_boot_cmd_start = .;
+  .u_boot_cmd : { *(.u_boot_cmd) }
+  __u_boot_cmd_end = .;
+
+
+  . = .;
+  __start___ex_table = .;
+  __ex_table : { *(__ex_table) }
+  __stop___ex_table = .;
+
+  . = ALIGN(4096);
+  __init_begin = .;
+  .text.init : { *(.text.init) }
+  .data.init : { *(.data.init) }
+  . = ALIGN(4096);
+  __init_end = .;
+
+  __bss_start = .;
+  .bss       :
+  {
+   *(.sbss) *(.scommon)
+   *(.dynbss)
+   *(.bss)
+   *(COMMON)
+  }
+  _end = . ;
+  PROVIDE (end = .);
+}
diff --git a/cpu/mpc5xxx/u-boot.lds b/cpu/mpc5xxx/u-boot.lds
new file mode 100644
index 0000000000..a28a3afc71
--- /dev/null
+++ b/cpu/mpc5xxx/u-boot.lds
@@ -0,0 +1,124 @@
+/*
+ * (C) Copyright 2003-2007
+ * 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
+ */
+
+OUTPUT_ARCH(powerpc)
+SEARCH_DIR(/lib); SEARCH_DIR(/usr/lib); SEARCH_DIR(/usr/local/lib); SEARCH_DIR(/usr/local/powerpc-any-elf/lib);
+/* Do we need any of these for elf?
+   __DYNAMIC = 0;    */
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  . = + SIZEOF_HEADERS;
+  .interp : { *(.interp) }
+  .hash          : { *(.hash)		}
+  .dynsym        : { *(.dynsym)		}
+  .dynstr        : { *(.dynstr)		}
+  .rel.text      : { *(.rel.text)		}
+  .rela.text     : { *(.rela.text) 	}
+  .rel.data      : { *(.rel.data)		}
+  .rela.data     : { *(.rela.data) 	}
+  .rel.rodata    : { *(.rel.rodata) 	}
+  .rela.rodata   : { *(.rela.rodata) 	}
+  .rel.got       : { *(.rel.got)		}
+  .rela.got      : { *(.rela.got)		}
+  .rel.ctors     : { *(.rel.ctors)	}
+  .rela.ctors    : { *(.rela.ctors)	}
+  .rel.dtors     : { *(.rel.dtors)	}
+  .rela.dtors    : { *(.rela.dtors)	}
+  .rel.bss       : { *(.rel.bss)		}
+  .rela.bss      : { *(.rela.bss)		}
+  .rel.plt       : { *(.rel.plt)		}
+  .rela.plt      : { *(.rela.plt)		}
+  .init          : { *(.init)	}
+  .plt : { *(.plt) }
+  .text      :
+  {
+    cpu/mpc5xxx/start.o	(.text)
+    *(.text)
+    *(.got1)
+    . = ALIGN(16);
+    *(.rodata)
+    *(.rodata1)
+    *(.rodata.str1.4)
+    *(.eh_frame)
+  }
+  .fini      : { *(.fini)    } =0
+  .ctors     : { *(.ctors)   }
+  .dtors     : { *(.dtors)   }
+
+  /* Read-write section, merged into data segment: */
+  . = (. + 0x0FFF) & 0xFFFFF000;
+  _erotext = .;
+  PROVIDE (erotext = .);
+  .reloc   :
+  {
+    *(.got)
+    _GOT2_TABLE_ = .;
+    *(.got2)
+    _FIXUP_TABLE_ = .;
+    *(.fixup)
+  }
+  __got2_entries = (_FIXUP_TABLE_ - _GOT2_TABLE_) >> 2;
+  __fixup_entries = (. - _FIXUP_TABLE_) >> 2;
+
+  .data    :
+  {
+    *(.data)
+    *(.data1)
+    *(.sdata)
+    *(.sdata2)
+    *(.dynamic)
+    CONSTRUCTORS
+  }
+  _edata  =  .;
+  PROVIDE (edata = .);
+
+  . = .;
+  __u_boot_cmd_start = .;
+  .u_boot_cmd : { *(.u_boot_cmd) }
+  __u_boot_cmd_end = .;
+
+
+  . = .;
+  __start___ex_table = .;
+  __ex_table : { *(__ex_table) }
+  __stop___ex_table = .;
+
+  . = ALIGN(4096);
+  __init_begin = .;
+  .text.init : { *(.text.init) }
+  .data.init : { *(.data.init) }
+  . = ALIGN(4096);
+  __init_end = .;
+
+  __bss_start = .;
+  .bss       :
+  {
+   *(.sbss) *(.scommon)
+   *(.dynbss)
+   *(.bss)
+   *(COMMON)
+  }
+  _end = . ;
+  PROVIDE (end = .);
+}
diff --git a/cpu/mpc5xxx/usb.c b/cpu/mpc5xxx/usb.c
new file mode 100644
index 0000000000..ce709fc652
--- /dev/null
+++ b/cpu/mpc5xxx/usb.c
@@ -0,0 +1,54 @@
+/*
+ * (C) Copyright 2007
+ * Markus Klotzbuecher, DENX Software Engineering <mk@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#if defined(CONFIG_USB_OHCI_NEW) && defined(CFG_USB_OHCI_CPU_INIT)
+
+#include <mpc5xxx.h>
+
+int usb_cpu_init()
+{
+	/* Set the USB Clock						     */
+	*(vu_long *)MPC5XXX_CDM_48_FDC = CONFIG_USB_CLOCK;
+
+	/* remove all USB bits first before ORing in ours */
+	*(vu_long *)MPC5XXX_GPS_PORT_CONFIG &= ~0x00807000;
+
+	/* Activate USB port						     */
+	*(vu_long *)MPC5XXX_GPS_PORT_CONFIG |= CONFIG_USB_CONFIG;
+
+	return 0;
+}
+
+int usb_cpu_stop()
+{
+	return 0;
+}
+
+int usb_cpu_init_fail()
+{
+	return 0;
+}
+
+#endif /* defined(CONFIG_USB_OHCI) && defined(CFG_USB_OHCI_CPU_INIT) */
diff --git a/cpu/mpc8220/config.mk b/cpu/mpc8220/config.mk
index 6fec5dfe60..c41cafe97f 100644
--- a/cpu/mpc8220/config.mk
+++ b/cpu/mpc8220/config.mk
@@ -21,7 +21,10 @@
 # MA 02111-1307 USA
 #
 
-PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi
+PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -mrelocatable
 
 PLATFORM_CPPFLAGS += -DCONFIG_MPC8220 -ffixed-r2 -ffixed-r29 \
 		     -mstring -mcpu=603e -mmultiple
+
+# Use default linker script.  Board port can override in board/*/config.mk
+LDSCRIPT := $(SRCTREE)/cpu/mpc8220/u-boot.lds
diff --git a/cpu/mpc8220/cpu_init.c b/cpu/mpc8220/cpu_init.c
index 3cf5f66a13..0daac5bbd4 100644
--- a/cpu/mpc8220/cpu_init.c
+++ b/cpu/mpc8220/cpu_init.c
@@ -128,7 +128,7 @@ int cpu_init_r (void)
 	/* route critical ints to normal ints */
 	*(vu_long *) 0xf0000710 |= 0x00000001;
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_MPC8220_FEC)
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_MPC8220_FEC)
 	/* load FEC microcode */
 	loadtask (0, 2);
 #endif
diff --git a/cpu/mpc8220/fec.c b/cpu/mpc8220/fec.c
index 1201e794df..992e0ffbc4 100644
--- a/cpu/mpc8220/fec.c
+++ b/cpu/mpc8220/fec.c
@@ -15,10 +15,10 @@
 #include "fec.h"
 
 #undef  DEBUG
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
     defined(CONFIG_MPC8220_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
 
@@ -847,7 +847,7 @@ int mpc8220_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,
 			fec8220_miiphy_read, fec8220_miiphy_write);
 #endif
diff --git a/cpu/mpc8220/start.S b/cpu/mpc8220/start.S
index 52332023ec..b5145ca035 100644
--- a/cpu/mpc8220/start.S
+++ b/cpu/mpc8220/start.S
@@ -169,7 +169,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -188,7 +188,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16	    /* copy EE bit from saved MSR */
diff --git a/cpu/mpc8220/traps.c b/cpu/mpc8220/traps.c
index cdee2be78b..89cca1d223 100644
--- a/cpu/mpc8220/traps.c
+++ b/cpu/mpc8220/traps.c
@@ -37,7 +37,7 @@
 #include <command.h>
 #include <asm/processor.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler) (struct pt_regs *) = 0;
 #endif
 
@@ -118,7 +118,7 @@ void MachineCheckException (struct pt_regs *regs)
 		regs->nip = fixup;
 		return;
 	}
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler
 	    && (*debugger_exception_handler) (regs))
 		return;
@@ -152,7 +152,7 @@ void MachineCheckException (struct pt_regs *regs)
 
 void AlignmentException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler
 	    && (*debugger_exception_handler) (regs))
 		return;
@@ -164,7 +164,7 @@ void AlignmentException (struct pt_regs *regs)
 
 void ProgramCheckException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler
 	    && (*debugger_exception_handler) (regs))
 		return;
@@ -176,7 +176,7 @@ void ProgramCheckException (struct pt_regs *regs)
 
 void SoftEmuException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler
 	    && (*debugger_exception_handler) (regs))
 		return;
@@ -189,7 +189,7 @@ void SoftEmuException (struct pt_regs *regs)
 
 void UnknownException (struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler
 	    && (*debugger_exception_handler) (regs))
 		return;
@@ -199,7 +199,7 @@ void UnknownException (struct pt_regs *regs)
 	_exception (0, regs);
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint (struct pt_regs *);
 #endif
 
@@ -208,7 +208,7 @@ void DebugException (struct pt_regs *regs)
 
 	printf ("Debugger trap at @ %lx\n", regs->nip);
 	show_regs (regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 	do_bedbug_breakpoint (regs);
 #endif
 }
diff --git a/cpu/mpc8220/u-boot.lds b/cpu/mpc8220/u-boot.lds
new file mode 100644
index 0000000000..a199a64f13
--- /dev/null
+++ b/cpu/mpc8220/u-boot.lds
@@ -0,0 +1,124 @@
+/*
+ * (C) Copyright 2003-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
+ */
+
+OUTPUT_ARCH(powerpc)
+SEARCH_DIR(/lib); SEARCH_DIR(/usr/lib); SEARCH_DIR(/usr/local/lib); SEARCH_DIR(/usr/local/powerpc-any-elf/lib);
+/* Do we need any of these for elf?
+   __DYNAMIC = 0;    */
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  . = + SIZEOF_HEADERS;
+  .interp : { *(.interp) }
+  .hash          : { *(.hash)		}
+  .dynsym        : { *(.dynsym)		}
+  .dynstr        : { *(.dynstr)		}
+  .rel.text      : { *(.rel.text)		}
+  .rela.text     : { *(.rela.text) 	}
+  .rel.data      : { *(.rel.data)		}
+  .rela.data     : { *(.rela.data) 	}
+  .rel.rodata    : { *(.rel.rodata) 	}
+  .rela.rodata   : { *(.rela.rodata) 	}
+  .rel.got       : { *(.rel.got)		}
+  .rela.got      : { *(.rela.got)		}
+  .rel.ctors     : { *(.rel.ctors)	}
+  .rela.ctors    : { *(.rela.ctors)	}
+  .rel.dtors     : { *(.rel.dtors)	}
+  .rela.dtors    : { *(.rela.dtors)	}
+  .rel.bss       : { *(.rel.bss)		}
+  .rela.bss      : { *(.rela.bss)		}
+  .rel.plt       : { *(.rel.plt)		}
+  .rela.plt      : { *(.rela.plt)		}
+  .init          : { *(.init)	}
+  .plt : { *(.plt) }
+  .text      :
+  {
+    cpu/mpc8220/start.o	(.text)
+    *(.text)
+    *(.got1)
+    . = ALIGN(16);
+    *(.rodata)
+    *(.rodata1)
+    *(.rodata.str1.4)
+    *(.eh_frame)
+  }
+  .fini      : { *(.fini)    } =0
+  .ctors     : { *(.ctors)   }
+  .dtors     : { *(.dtors)   }
+
+  /* Read-write section, merged into data segment: */
+  . = (. + 0x0FFF) & 0xFFFFF000;
+  _erotext = .;
+  PROVIDE (erotext = .);
+  .reloc   :
+  {
+    *(.got)
+    _GOT2_TABLE_ = .;
+    *(.got2)
+    _FIXUP_TABLE_ = .;
+    *(.fixup)
+  }
+  __got2_entries = (_FIXUP_TABLE_ - _GOT2_TABLE_) >> 2;
+  __fixup_entries = (. - _FIXUP_TABLE_) >> 2;
+
+  .data    :
+  {
+    *(.data)
+    *(.data1)
+    *(.sdata)
+    *(.sdata2)
+    *(.dynamic)
+    CONSTRUCTORS
+  }
+  _edata  =  .;
+  PROVIDE (edata = .);
+
+  . = .;
+  __u_boot_cmd_start = .;
+  .u_boot_cmd : { *(.u_boot_cmd) }
+  __u_boot_cmd_end = .;
+
+
+  . = .;
+  __start___ex_table = .;
+  __ex_table : { *(__ex_table) }
+  __stop___ex_table = .;
+
+  . = ALIGN(4096);
+  __init_begin = .;
+  .text.init : { *(.text.init) }
+  .data.init : { *(.data.init) }
+  . = ALIGN(4096);
+  __init_end = .;
+
+  __bss_start = .;
+  .bss       :
+  {
+   *(.sbss) *(.scommon)
+   *(.dynbss)
+   *(.bss)
+   *(COMMON)
+  }
+  _end = . ;
+  PROVIDE (end = .);
+}
diff --git a/cpu/mpc824x/config.mk b/cpu/mpc824x/config.mk
index dac61d8d3e..17fdb21d35 100644
--- a/cpu/mpc824x/config.mk
+++ b/cpu/mpc824x/config.mk
@@ -21,6 +21,9 @@
 # MA 02111-1307 USA
 #
 
-PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -fno-strict-aliasing
+PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -fno-strict-aliasing -mrelocatable
 
 PLATFORM_CPPFLAGS += -DCONFIG_MPC824X -ffixed-r2 -ffixed-r29 -mstring -mcpu=603e -msoft-float
+
+# Use default linker script.  Board port can override in board/*/config.mk
+LDSCRIPT := $(SRCTREE)/cpu/mpc824x/u-boot.lds
diff --git a/cpu/mpc824x/start.S b/cpu/mpc824x/start.S
index 9ff052c3b0..784edc36a0 100644
--- a/cpu/mpc824x/start.S
+++ b/cpu/mpc824x/start.S
@@ -220,7 +220,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = EXC_OFF_ALIGN
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -238,7 +238,7 @@ Alignment:
 /* Program check exception */
 	. = EXC_OFF_PROGRAM
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
diff --git a/cpu/mpc824x/traps.c b/cpu/mpc824x/traps.c
index 071d003f8c..0a7243020b 100644
--- a/cpu/mpc824x/traps.c
+++ b/cpu/mpc824x/traps.c
@@ -175,7 +175,7 @@ UnknownException(struct pt_regs *regs)
 	_exception(0, regs);
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -185,7 +185,7 @@ DebugException(struct pt_regs *regs)
 
   printf("Debugger trap at @ %lx\n", regs->nip );
   show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
   do_bedbug_breakpoint( regs );
 #endif
 }
diff --git a/cpu/mpc824x/u-boot.lds b/cpu/mpc824x/u-boot.lds
new file mode 100644
index 0000000000..8cbef4aed4
--- /dev/null
+++ b/cpu/mpc824x/u-boot.lds
@@ -0,0 +1,124 @@
+/*
+ * (C) Copyright 2001-2007
+ * 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
+ */
+
+OUTPUT_ARCH(powerpc)
+SEARCH_DIR(/lib); SEARCH_DIR(/usr/lib); SEARCH_DIR(/usr/local/lib); SEARCH_DIR(/usr/local/powerpc-any-elf/lib);
+/* Do we need any of these for elf?
+   __DYNAMIC = 0;    */
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  . = + SIZEOF_HEADERS;
+  .interp : { *(.interp) }
+  .hash          : { *(.hash)		}
+  .dynsym        : { *(.dynsym)		}
+  .dynstr        : { *(.dynstr)		}
+  .rel.text      : { *(.rel.text)		}
+  .rela.text     : { *(.rela.text) 	}
+  .rel.data      : { *(.rel.data)		}
+  .rela.data     : { *(.rela.data) 	}
+  .rel.rodata    : { *(.rel.rodata) 	}
+  .rela.rodata   : { *(.rela.rodata) 	}
+  .rel.got       : { *(.rel.got)		}
+  .rela.got      : { *(.rela.got)		}
+  .rel.ctors     : { *(.rel.ctors)	}
+  .rela.ctors    : { *(.rela.ctors)	}
+  .rel.dtors     : { *(.rel.dtors)	}
+  .rela.dtors    : { *(.rela.dtors)	}
+  .rel.bss       : { *(.rel.bss)		}
+  .rela.bss      : { *(.rela.bss)		}
+  .rel.plt       : { *(.rel.plt)		}
+  .rela.plt      : { *(.rela.plt)		}
+  .init          : { *(.init)	}
+  .plt : { *(.plt) }
+  .text      :
+  {
+    cpu/mpc824x/start.o		(.text)
+    *(.text)
+    *(.got1)
+    . = ALIGN(16);
+    *(.rodata)
+    *(.rodata1)
+    *(.rodata.str1.4)
+    *(.eh_frame)
+  }
+  .fini      : { *(.fini)    } =0
+  .ctors     : { *(.ctors)   }
+  .dtors     : { *(.dtors)   }
+
+  /* Read-write section, merged into data segment: */
+  . = (. + 0x0FFF) & 0xFFFFF000;
+  _erotext = .;
+  PROVIDE (erotext = .);
+  .reloc   :
+  {
+    *(.got)
+    _GOT2_TABLE_ = .;
+    *(.got2)
+    _FIXUP_TABLE_ = .;
+    *(.fixup)
+  }
+  __got2_entries = (_FIXUP_TABLE_ - _GOT2_TABLE_) >> 2;
+  __fixup_entries = (. - _FIXUP_TABLE_) >> 2;
+
+  .data    :
+  {
+    *(.data)
+    *(.data1)
+    *(.sdata)
+    *(.sdata2)
+    *(.dynamic)
+    CONSTRUCTORS
+  }
+  _edata  =  .;
+  PROVIDE (edata = .);
+
+  . = .;
+  __u_boot_cmd_start = .;
+  .u_boot_cmd : { *(.u_boot_cmd) }
+  __u_boot_cmd_end = .;
+
+
+  . = .;
+  __start___ex_table = .;
+  __ex_table : { *(__ex_table) }
+  __stop___ex_table = .;
+
+  . = ALIGN(4096);
+  __init_begin = .;
+  .text.init : { *(.text.init) }
+  .data.init : { *(.data.init) }
+  . = ALIGN(4096);
+  __init_end = .;
+
+  __bss_start = .;
+  .bss       :
+  {
+   *(.sbss) *(.scommon)
+   *(.dynbss)
+   *(.bss)
+   *(COMMON)
+  }
+  _end = . ;
+  PROVIDE (end = .);
+}
diff --git a/cpu/mpc8260/bedbug_603e.c b/cpu/mpc8260/bedbug_603e.c
index be09cfb5c6..f1be485e8e 100644
--- a/cpu/mpc8260/bedbug_603e.c
+++ b/cpu/mpc8260/bedbug_603e.c
@@ -10,7 +10,8 @@
 #include <bedbug/regs.h>
 #include <bedbug/ppc.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG) && (defined(CONFIG_MPC824X) || defined(CONFIG_MPC8260))
+#if defined(CONFIG_CMD_BEDBUG) \
+	&& (defined(CONFIG_MPC824X) || defined(CONFIG_MPC8260))
 
 #define MAX_BREAK_POINTS 1
 
diff --git a/cpu/mpc8260/config.mk b/cpu/mpc8260/config.mk
index dd7a71fdf3..d401e4ca04 100644
--- a/cpu/mpc8260/config.mk
+++ b/cpu/mpc8260/config.mk
@@ -21,7 +21,10 @@
 # MA 02111-1307 USA
 #
 
-PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi
+PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -mrelocatable
 
 PLATFORM_CPPFLAGS += -DCONFIG_8260 -DCONFIG_CPM2 -ffixed-r2 -ffixed-r29 \
 		     -mstring -mcpu=603e -mmultiple
+
+# Use default linker script.  Board port can override in board/*/config.mk
+LDSCRIPT := $(SRCTREE)/cpu/mpc8260/u-boot.lds
diff --git a/cpu/mpc8260/cpu_init.c b/cpu/mpc8260/cpu_init.c
index 380d7af13d..36fc1eba5f 100644
--- a/cpu/mpc8260/cpu_init.c
+++ b/cpu/mpc8260/cpu_init.c
@@ -182,7 +182,7 @@ void cpu_init_f (volatile immap_t * immr)
 #endif
 
 	/* now restrict to preliminary range */
-	/* the PS came from the HRCW, don´t change it */
+	/* the PS came from the HRCW, don´t change it */
 	memctl->memc_br0 = SET_VAL_MASK(memctl->memc_br0 , CFG_BR0_PRELIM, BRx_PS_MSK);
 	memctl->memc_or0 = CFG_OR0_PRELIM;
 
diff --git a/cpu/mpc8260/ether_fcc.c b/cpu/mpc8260/ether_fcc.c
index 584c40f17a..37bf4456ed 100644
--- a/cpu/mpc8260/ether_fcc.c
+++ b/cpu/mpc8260/ether_fcc.c
@@ -47,13 +47,13 @@
 #include <config.h>
 #include <net.h>
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 #include <miiphy.h>
 #endif
 
 DECLARE_GLOBAL_DATA_PTR;
 
-#if defined(CONFIG_ETHER_ON_FCC) && (CONFIG_COMMANDS & CFG_CMD_NET) && \
+#if defined(CONFIG_ETHER_ON_FCC) && defined(CONFIG_CMD_NET) && \
 	defined(CONFIG_NET_MULTI)
 
 static struct ether_fcc_info_s
@@ -393,7 +393,7 @@ int fec_initialize(bd_t *bis)
 
 		eth_register(dev);
 
-#if (defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)) \
+#if (defined(CONFIG_MII) || defined(CONFIG_CMD_MII)) \
 		&& defined(CONFIG_BITBANGMII)
 		miiphy_register(dev->name,
 				bb_miiphy_read,	bb_miiphy_write);
@@ -1187,4 +1187,4 @@ eth_loopback_test (void)
 
 #endif /* CONFIG_ETHER_LOOPBACK_TEST */
 
-#endif	/* CONFIG_ETHER_ON_FCC && CFG_CMD_NET && CONFIG_NET_MULTI */
+#endif
diff --git a/cpu/mpc8260/ether_scc.c b/cpu/mpc8260/ether_scc.c
index a733b45c01..e56839d3aa 100644
--- a/cpu/mpc8260/ether_scc.c
+++ b/cpu/mpc8260/ether_scc.c
@@ -36,7 +36,7 @@
 #include <command.h>
 #include <config.h>
 
-#if defined(CONFIG_ETHER_ON_SCC) && (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_ETHER_ON_SCC) && defined(CONFIG_CMD_NET)
 
 #if (CONFIG_ETHER_INDEX == 1)
 #  define PROFF_ENET            PROFF_SCC1
@@ -353,4 +353,4 @@ void restart(void)
 }
 #endif
 
-#endif  /* CONFIG_ETHER_ON_SCC && CFG_CMD_NET */
+#endif
diff --git a/cpu/mpc8260/interrupts.c b/cpu/mpc8260/interrupts.c
index 56e9a72137..bf0d4d0d59 100644
--- a/cpu/mpc8260/interrupts.c
+++ b/cpu/mpc8260/interrupts.c
@@ -246,7 +246,7 @@ void timer_interrupt_cpu (struct pt_regs *regs)
 
 /****************************************************************************/
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 
 /* ripped this out of ppc4xx/interrupts.c */
 
@@ -276,4 +276,4 @@ do_irqinfo (cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
 		enable_interrupts ();
 }
 
-#endif							/* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/mpc8260/kgdb.S b/cpu/mpc8260/kgdb.S
index 2a250249b8..dae87bb97c 100644
--- a/cpu/mpc8260/kgdb.S
+++ b/cpu/mpc8260/kgdb.S
@@ -34,7 +34,7 @@
 #include <asm/cache.h>
 #include <asm/mmu.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 
  /*
  * cache flushing routines for kgdb
@@ -69,4 +69,4 @@ kgdb_flush_cache_range:
 	SYNC
 	blr
 
-#endif /* CFG_CMD_KGDB */
+#endif
diff --git a/cpu/mpc8260/start.S b/cpu/mpc8260/start.S
index 2e93bbbb86..7f5dc819cd 100644
--- a/cpu/mpc8260/start.S
+++ b/cpu/mpc8260/start.S
@@ -279,7 +279,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -298,7 +298,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
@@ -676,13 +676,13 @@ init_debug:
 	bdnz	1b
 
 	/* Load the Instruction Address Breakpoint Register (IABR).	*/
-	/* 								*/
+	/*								*/
 	/* The address to load is stored in the first word of dual port	*/
 	/* ram and should be preserved while the power is on, so you	*/
 	/* can plug addresses into that location then reset the cpu and	*/
 	/* this code will load that address into the IABR after the	*/
 	/* reset.							*/
-	/* 								*/
+	/*								*/
 	/* When the program counter matches the contents of the IABR,	*/
 	/* an exception is generated (before the instruction at that	*/
 	/* location completes). The vector for this exception is 0x1300 */
diff --git a/cpu/mpc8260/traps.c b/cpu/mpc8260/traps.c
index 0c39e434ed..b5d416c974 100644
--- a/cpu/mpc8260/traps.c
+++ b/cpu/mpc8260/traps.c
@@ -37,7 +37,7 @@
 #include <asm/processor.h>
 #include <asm/m8260_pci.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
@@ -150,7 +150,7 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -186,7 +186,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -198,7 +198,7 @@ AlignmentException(struct pt_regs *regs)
 void
 ProgramCheckException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -210,7 +210,7 @@ ProgramCheckException(struct pt_regs *regs)
 void
 SoftEmuException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -223,7 +223,7 @@ SoftEmuException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -232,7 +232,7 @@ UnknownException(struct pt_regs *regs)
 	_exception(0, regs);
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -242,7 +242,7 @@ DebugException(struct pt_regs *regs)
 
   printf("Debugger trap at @ %lx\n", regs->nip );
   show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
   do_bedbug_breakpoint( regs );
 #endif
 }
diff --git a/cpu/mpc8260/u-boot.lds b/cpu/mpc8260/u-boot.lds
new file mode 100644
index 0000000000..b8abc17d41
--- /dev/null
+++ b/cpu/mpc8260/u-boot.lds
@@ -0,0 +1,124 @@
+/*
+ * (C) Copyright 2001-2007
+ * 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
+ */
+
+OUTPUT_ARCH(powerpc)
+SEARCH_DIR(/lib); SEARCH_DIR(/usr/lib); SEARCH_DIR(/usr/local/lib); SEARCH_DIR(/usr/local/powerpc-any-elf/lib);
+/* Do we need any of these for elf?
+   __DYNAMIC = 0;    */
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  . = + SIZEOF_HEADERS;
+  .interp : { *(.interp) }
+  .hash          : { *(.hash)		}
+  .dynsym        : { *(.dynsym)		}
+  .dynstr        : { *(.dynstr)		}
+  .rel.text      : { *(.rel.text)		}
+  .rela.text     : { *(.rela.text) 	}
+  .rel.data      : { *(.rel.data)		}
+  .rela.data     : { *(.rela.data) 	}
+  .rel.rodata    : { *(.rel.rodata) 	}
+  .rela.rodata   : { *(.rela.rodata) 	}
+  .rel.got       : { *(.rel.got)		}
+  .rela.got      : { *(.rela.got)		}
+  .rel.ctors     : { *(.rel.ctors)	}
+  .rela.ctors    : { *(.rela.ctors)	}
+  .rel.dtors     : { *(.rel.dtors)	}
+  .rela.dtors    : { *(.rela.dtors)	}
+  .rel.bss       : { *(.rel.bss)		}
+  .rela.bss      : { *(.rela.bss)		}
+  .rel.plt       : { *(.rel.plt)		}
+  .rela.plt      : { *(.rela.plt)		}
+  .init          : { *(.init)	}
+  .plt : { *(.plt) }
+  .text      :
+  {
+    cpu/mpc8260/start.o		(.text)
+    *(.text)
+    *(.got1)
+    . = ALIGN(16);
+    *(.rodata)
+    *(.rodata1)
+    *(.rodata.str1.4)
+    *(.eh_frame)
+  }
+  .fini      : { *(.fini)    } =0
+  .ctors     : { *(.ctors)   }
+  .dtors     : { *(.dtors)   }
+
+  /* Read-write section, merged into data segment: */
+  . = (. + 0x0FFF) & 0xFFFFF000;
+  _erotext = .;
+  PROVIDE (erotext = .);
+  .reloc   :
+  {
+    *(.got)
+    _GOT2_TABLE_ = .;
+    *(.got2)
+    _FIXUP_TABLE_ = .;
+    *(.fixup)
+  }
+  __got2_entries = (_FIXUP_TABLE_ - _GOT2_TABLE_) >> 2;
+  __fixup_entries = (. - _FIXUP_TABLE_) >> 2;
+
+  .data    :
+  {
+    *(.data)
+    *(.data1)
+    *(.sdata)
+    *(.sdata2)
+    *(.dynamic)
+    CONSTRUCTORS
+  }
+  _edata  =  .;
+  PROVIDE (edata = .);
+
+  . = .;
+  __u_boot_cmd_start = .;
+  .u_boot_cmd : { *(.u_boot_cmd) }
+  __u_boot_cmd_end = .;
+
+
+  . = .;
+  __start___ex_table = .;
+  __ex_table : { *(__ex_table) }
+  __stop___ex_table = .;
+
+  . = ALIGN(4096);
+  __init_begin = .;
+  .text.init : { *(.text.init) }
+  .data.init : { *(.data.init) }
+  . = ALIGN(4096);
+  __init_end = .;
+
+  __bss_start = .;
+  .bss       :
+  {
+   *(.sbss) *(.scommon)
+   *(.dynbss)
+   *(.bss)
+   *(COMMON)
+  }
+  _end = . ;
+  PROVIDE (end = .);
+}
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/config.mk b/cpu/mpc83xx/config.mk
index 8b4ff92b19..2ec395d4ca 100644
--- a/cpu/mpc83xx/config.mk
+++ b/cpu/mpc83xx/config.mk
@@ -20,7 +20,10 @@
 # MA 02111-1307 USA
 #
 
-PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi
+PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -mrelocatable
 
 PLATFORM_CPPFLAGS += -DCONFIG_MPC83XX -DCONFIG_E300 \
 			-ffixed-r2 -ffixed-r29 -msoft-float
+
+# Use default linker script.  Board port can override in board/*/config.mk
+LDSCRIPT := $(SRCTREE)/cpu/mpc83xx/u-boot.lds
diff --git a/cpu/mpc83xx/cpu.c b/cpu/mpc83xx/cpu.c
index e078f27a23..adf8083010 100644
--- a/cpu/mpc83xx/cpu.c
+++ b/cpu/mpc83xx/cpu.c
@@ -33,8 +33,7 @@
 #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
@@ -113,12 +112,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 +151,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,154 +331,167 @@ 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);
+	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);
+	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);
+	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);
+	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.
 	 */
 	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));
 }
 
+
 /*
- * 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",
-		fdt_set_busfreq
+		"timebase-frequency",
+		fdt_set_tbfreq
 	},
-	{	"/cpus/" OF_SOC,
+	{	"/cpus/" OF_CPU,
 		"bus-frequency",
 		fdt_set_busfreq
 	},
-	{	"/" OF_SOC "/serial@4500/",
+	{	"/cpus/" OF_CPU,
+		"clock-frequency",
+		fdt_set_busfreq
+	},
+	{	"/" OF_SOC "/serial@4500",
 		"clock-frequency",
 		fdt_set_busfreq
 	},
-	{	"/" OF_SOC "/serial@4600/",
+	{	"/" OF_SOC "/serial@4600",
 		"clock-frequency",
 		fdt_set_busfreq
 	},
-#ifdef CONFIG_MPC83XX_TSEC1
-	{	"/" OF_SOC "/ethernet@24000,
+#ifdef CONFIG_TSEC1
+	{	"/" 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_MPC83XX_TSEC2
-	{	"/" OF_SOC "/ethernet@25000,
+#ifdef CONFIG_TSEC2
+	{	"/" 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_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",
+	{	"/" 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 */
 };
 
 void
@@ -487,20 +502,23 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	int  j;
 
 	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",
+				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)
+#elif defined(CONFIG_OF_FLAT_TREE)
 void
 ft_cpu_setup(void *blob, bd_t *bd)
 {
@@ -525,7 +543,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	if (p != NULL)
 		*p = cpu_to_be32(clock);
 
-#ifdef CONFIG_MPC83XX_TSEC1
+#ifdef CONFIG_TSEC1
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@24000/mac-address", &len);
 	if (p != NULL)
 		memcpy(p, bd->bi_enetaddr, 6);
@@ -535,7 +553,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 		memcpy(p, bd->bi_enetaddr, 6);
 #endif
 
-#ifdef CONFIG_MPC83XX_TSEC2
+#ifdef CONFIG_TSEC2
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@25000/mac-address", &len);
 	if (p != NULL)
 		memcpy(p, bd->bi_enet1addr, 6);
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/interrupts.c b/cpu/mpc83xx/interrupts.c
index bb1fe1af3f..98ed21ccfa 100644
--- a/cpu/mpc83xx/interrupts.c
+++ b/cpu/mpc83xx/interrupts.c
@@ -81,7 +81,7 @@ void timer_interrupt_cpu (struct pt_regs *regs)
 }
 
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 
 /* ripped this out of ppc4xx/interrupts.c */
 
@@ -94,4 +94,4 @@ do_irqinfo(cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[])
 {
 }
 
-#endif		/* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/mpc83xx/pci.c b/cpu/mpc83xx/pci.c
index 785d6129da..2298218870 100644
--- a/cpu/mpc83xx/pci.c
+++ b/cpu/mpc83xx/pci.c
@@ -25,7 +25,14 @@
 
 #include <common.h>
 #include <pci.h>
+
+#if defined(CONFIG_OF_LIBFDT)
+#include <libfdt.h>
+#include <libfdt_env.h>
+#elif defined(CONFIG_OF_FLAT_TREE)
 #include <ft_build.h>
+#endif
+
 #include <asm/mpc8349_pci.h>
 
 #ifdef CONFIG_83XX_GENERIC_PCI
@@ -163,7 +170,34 @@ 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));
+	}
+
+	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));
+	}
+}
+#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..54f0c83d45 100644
--- a/cpu/mpc83xx/spd_sdram.c
+++ b/cpu/mpc83xx/spd_sdram.c
@@ -574,7 +574,10 @@ long int spd_sdram()
 
 	/* Check DIMM data bus width */
 	if (spd.dataw_lsb == 0x20) {
-		burstlen = 0x03; /* 32 bit data bus, burst len is 8 */
+		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 */
 		printf("\n   DDR DIMM: data bus width is 32 bit");
 	} else {
 		burstlen = 0x02; /* Others act as 64 bit bus, burst len is 4 */
@@ -730,8 +733,12 @@ long int spd_sdram()
 		sdram_cfg |= 0x10000000;
 
 	/* 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 |= 0x000C0000;
+		if (spd.mem_type == SPD_MEMTYPE_DDR2)
+			sdram_cfg |= 0x00080000;
+	}
 
 	ddrc_ecc_enable = 0;
 
diff --git a/cpu/mpc83xx/start.S b/cpu/mpc83xx/start.S
index 6ee9ec96c9..496c8a5861 100644
--- a/cpu/mpc83xx/start.S
+++ b/cpu/mpc83xx/start.S
@@ -263,7 +263,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -282,7 +282,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
diff --git a/cpu/mpc83xx/traps.c b/cpu/mpc83xx/traps.c
index 152fa73561..dfd6c03864 100644
--- a/cpu/mpc83xx/traps.c
+++ b/cpu/mpc83xx/traps.c
@@ -140,7 +140,7 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -176,7 +176,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -188,7 +188,7 @@ AlignmentException(struct pt_regs *regs)
 void
 ProgramCheckException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -200,7 +200,7 @@ ProgramCheckException(struct pt_regs *regs)
 void
 SoftEmuException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -213,7 +213,7 @@ SoftEmuException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -222,7 +222,7 @@ UnknownException(struct pt_regs *regs)
 	_exception(0, regs);
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -231,7 +231,7 @@ DebugException(struct pt_regs *regs)
 {
 	printf("Debugger trap at @ %lx\n", regs->nip );
 	show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 	do_bedbug_breakpoint( regs );
 #endif
 }
diff --git a/cpu/mpc83xx/u-boot.lds b/cpu/mpc83xx/u-boot.lds
new file mode 100644
index 0000000000..ca663bc87b
--- /dev/null
+++ b/cpu/mpc83xx/u-boot.lds
@@ -0,0 +1,122 @@
+/*
+ * (C) Copyright 2006
+ * 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
+ */
+
+OUTPUT_ARCH(powerpc)
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  . = + SIZEOF_HEADERS;
+  .interp : { *(.interp) }
+  .hash          : { *(.hash)		}
+  .dynsym        : { *(.dynsym)		}
+  .dynstr        : { *(.dynstr)		}
+  .rel.text      : { *(.rel.text)		}
+  .rela.text     : { *(.rela.text) 	}
+  .rel.data      : { *(.rel.data)		}
+  .rela.data     : { *(.rela.data) 	}
+  .rel.rodata    : { *(.rel.rodata) 	}
+  .rela.rodata   : { *(.rela.rodata) 	}
+  .rel.got       : { *(.rel.got)		}
+  .rela.got      : { *(.rela.got)		}
+  .rel.ctors     : { *(.rel.ctors)	}
+  .rela.ctors    : { *(.rela.ctors)	}
+  .rel.dtors     : { *(.rel.dtors)	}
+  .rela.dtors    : { *(.rela.dtors)	}
+  .rel.bss       : { *(.rel.bss)		}
+  .rela.bss      : { *(.rela.bss)		}
+  .rel.plt       : { *(.rel.plt)		}
+  .rela.plt      : { *(.rela.plt)		}
+  .init          : { *(.init)	}
+  .plt : { *(.plt) }
+  .text      :
+  {
+    cpu/mpc83xx/start.o	(.text)
+    *(.text)
+    *(.got1)
+    . = ALIGN(16);
+    *(.rodata)
+    *(.rodata1)
+    *(.rodata.str1.4)
+    *(.eh_frame)
+  }
+  .fini      : { *(.fini)    } =0
+  .ctors     : { *(.ctors)   }
+  .dtors     : { *(.dtors)   }
+
+  /* Read-write section, merged into data segment: */
+  . = (. + 0x0FFF) & 0xFFFFF000;
+  _erotext = .;
+  PROVIDE (erotext = .);
+  .reloc   :
+  {
+    *(.got)
+    _GOT2_TABLE_ = .;
+    *(.got2)
+    _FIXUP_TABLE_ = .;
+    *(.fixup)
+  }
+  __got2_entries = (_FIXUP_TABLE_ - _GOT2_TABLE_) >> 2;
+  __fixup_entries = (. - _FIXUP_TABLE_) >> 2;
+
+  .data    :
+  {
+    *(.data)
+    *(.data1)
+    *(.sdata)
+    *(.sdata2)
+    *(.dynamic)
+    CONSTRUCTORS
+  }
+  _edata  =  .;
+  PROVIDE (edata = .);
+
+  . = .;
+  __u_boot_cmd_start = .;
+  .u_boot_cmd : { *(.u_boot_cmd) }
+  __u_boot_cmd_end = .;
+
+
+  . = .;
+  __start___ex_table = .;
+  __ex_table : { *(__ex_table) }
+  __stop___ex_table = .;
+
+  . = ALIGN(4096);
+  __init_begin = .;
+  .text.init : { *(.text.init) }
+  .data.init : { *(.data.init) }
+  . = ALIGN(4096);
+  __init_end = .;
+
+  __bss_start = .;
+  .bss       :
+  {
+   *(.sbss) *(.scommon)
+   *(.dynbss)
+   *(.bss)
+   *(COMMON)
+  }
+  _end = . ;
+  PROVIDE (end = .);
+}
+ENTRY(_start)
diff --git a/cpu/mpc85xx/cpu.c b/cpu/mpc85xx/cpu.c
index 7735a52ccf..1d791c9b9b 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_MPC85XX_TSEC1)
+#if defined(CONFIG_TSEC1)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@24000/mac-address", &len);
 	if (p)
 		memcpy(p, bd->bi_enetaddr, 6);
diff --git a/cpu/mpc85xx/ether_fcc.c b/cpu/mpc85xx/ether_fcc.c
index d15d242492..5b23a80e1c 100644
--- a/cpu/mpc85xx/ether_fcc.c
+++ b/cpu/mpc85xx/ether_fcc.c
@@ -48,13 +48,13 @@
 #include <config.h>
 #include <net.h>
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 #include <miiphy.h>
 #endif
 
 #if defined(CONFIG_CPM2)
 
-#if defined(CONFIG_ETHER_ON_FCC) && (CONFIG_COMMANDS & CFG_CMD_NET) && \
+#if defined(CONFIG_ETHER_ON_FCC) && defined(CONFIG_CMD_NET) && \
 	defined(CONFIG_NET_MULTI)
 
 static struct ether_fcc_info_s
@@ -458,7 +458,7 @@ int fec_initialize(bd_t *bis)
 
 		eth_register(dev);
 
-#if (defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)) \
+#if (defined(CONFIG_MII) || defined(CONFIG_CMD_MII)) \
 		&& defined(CONFIG_BITBANGMII)
 		miiphy_register(dev->name,
 				bb_miiphy_read,	bb_miiphy_write);
@@ -468,6 +468,6 @@ int fec_initialize(bd_t *bis)
 	return 1;
 }
 
-#endif	/* CONFIG_ETHER_ON_FCC && CFG_CMD_NET && CONFIG_NET_MULTI */
+#endif
 
 #endif /* CONFIG_CPM2 */
diff --git a/cpu/mpc85xx/interrupts.c b/cpu/mpc85xx/interrupts.c
index 832781bab8..dc246dca02 100644
--- a/cpu/mpc85xx/interrupts.c
+++ b/cpu/mpc85xx/interrupts.c
@@ -144,7 +144,7 @@ void set_timer (ulong t)
 	timestamp = t;
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 
 /*******************************************************************************
  *
@@ -159,4 +159,4 @@ do_irqinfo(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 	return 0;
 }
 
-#endif  /* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/mpc85xx/start.S b/cpu/mpc85xx/start.S
index 20c7ebc723..77c155c5bd 100644
--- a/cpu/mpc85xx/start.S
+++ b/cpu/mpc85xx/start.S
@@ -457,7 +457,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x0600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -475,7 +475,7 @@ Alignment:
 /* Program check exception */
 	. = 0x0700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
diff --git a/cpu/mpc85xx/traps.c b/cpu/mpc85xx/traps.c
index 904f052339..9cd621c3ac 100644
--- a/cpu/mpc85xx/traps.c
+++ b/cpu/mpc85xx/traps.c
@@ -41,7 +41,7 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
@@ -74,7 +74,7 @@ static __inline__ unsigned long get_esr(void)
 #define ESR_DIZ 0x00400000
 #define ESR_U0F 0x00008000
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -159,7 +159,7 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -192,7 +192,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -207,7 +207,7 @@ ProgramCheckException(struct pt_regs *regs)
 {
 	long esr_val;
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -244,7 +244,7 @@ PITException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -259,7 +259,7 @@ DebugException(struct pt_regs *regs)
 {
 	printf("Debugger trap at @ %lx\n", regs->nip );
 	show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 	do_bedbug_breakpoint( regs );
 #endif
 }
diff --git a/cpu/mpc86xx/Makefile b/cpu/mpc86xx/Makefile
index fffcfd2402..6d9300e22e 100644
--- a/cpu/mpc86xx/Makefile
+++ b/cpu/mpc86xx/Makefile
@@ -1,4 +1,5 @@
 #
+# Copyright 2007 Freescale Semiconductor, Inc.
 # (C) Copyright 2002,2003 Motorola Inc.
 # Xianghua Xiao,X.Xiao@motorola.com
 #
@@ -30,7 +31,7 @@ LIB	= $(obj)lib$(CPU).a
 START	= start.o #resetvec.o
 SOBJS	= cache.o
 COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o \
-	  pci.o pcie_indirect.o spd_sdram.o
+	  spd_sdram.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/mpc86xx/cpu.c b/cpu/mpc86xx/cpu.c
index a33acfec4d..9456471e84 100644
--- a/cpu/mpc86xx/cpu.c
+++ b/cpu/mpc86xx/cpu.c
@@ -278,7 +278,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	if (p != NULL)
 		*p = cpu_to_be32(clock);
 
-#if defined(CONFIG_MPC86XX_TSEC1)
+#if defined(CONFIG_TSEC1)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@24000/mac-address", &len);
 	if (p != NULL)
 		memcpy(p, bd->bi_enetaddr, 6);
@@ -287,7 +287,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 		memcpy(p, bd->bi_enetaddr, 6);
 #endif
 
-#if defined(CONFIG_MPC86XX_TSEC2)
+#if defined(CONFIG_TSEC2)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@25000/mac-address", &len);
 	if (p != NULL)
 		memcpy(p, bd->bi_enet1addr, 6);
@@ -296,7 +296,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 		memcpy(p, bd->bi_enet1addr, 6);
 #endif
 
-#if defined(CONFIG_MPC86XX_TSEC3)
+#if defined(CONFIG_TSEC3)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@26000/mac-address", &len);
 	if (p != NULL)
 		memcpy(p, bd->bi_enet2addr, 6);
@@ -305,7 +305,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 		memcpy(p, bd->bi_enet2addr, 6);
 #endif
 
-#if defined(CONFIG_MPC86XX_TSEC4)
+#if defined(CONFIG_TSEC4)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@27000/mac-address", &len);
 	if (p != NULL)
 		memcpy(p, bd->bi_enet3addr, 6);
diff --git a/cpu/mpc86xx/cpu_init.c b/cpu/mpc86xx/cpu_init.c
index 4673d05e71..c8e4666694 100644
--- a/cpu/mpc86xx/cpu_init.c
+++ b/cpu/mpc86xx/cpu_init.c
@@ -104,8 +104,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 49820bbd81..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)
  *
@@ -71,7 +71,7 @@ static __inline__ void set_dec(unsigned long val)
 }
 
 /* interrupt is not supported yet */
-int interrupt_init_cpu(unsigned *decrementer_count)
+int interrupt_init_cpu(unsigned long *decrementer_count)
 {
 	return 0;
 }
@@ -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);
@@ -107,7 +92,7 @@ int interrupt_init(void)
 		return ret;
 
 	decrementer_count = get_tbclk() / CFG_HZ;
-	debug("interrupt init: tbclk() = %d MHz, decrementer_count = %d\n",
+	debug("interrupt init: tbclk() = %d MHz, decrementer_count = %ld\n",
 	      (get_tbclk() / 1000000),
 	      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(&timestamp);
-
 	/* Restore Decrementer Count */
 	set_dec(decrementer_count);
 
diff --git a/cpu/mpc86xx/pci.c b/cpu/mpc86xx/pci.c
deleted file mode 100644
index b86548db4f..0000000000
--- a/cpu/mpc86xx/pci.c
+++ /dev/null
@@ -1,146 +0,0 @@
-/*
- * Copyright (C) Freescale Semiconductor,Inc.
- * 2005, 2006. All rights reserved.
- *
- * Ed Swarthout (ed.swarthout@freescale.com)
- * Jason Jin (Jason.jin@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
- */
-
-/*
- * PCIE Configuration space access support for PCIE Bridge
- */
-#include <common.h>
-#include <pci.h>
-
-#if defined(CONFIG_PCI)
-void
-pci_mpc86xx_init(struct pci_controller *hose)
-{
-	volatile immap_t *immap = (immap_t *) CFG_CCSRBAR;
-	volatile ccsr_pex_t *pcie1 = &immap->im_pex1;
-	u16 temp16;
-	u32 temp32;
-
-	volatile ccsr_gur_t *gur = &immap->im_gur;
-	uint host1_agent = (gur->porbmsr & MPC86xx_PORBMSR_HA) >> 17;
-	uint pcie1_host = (host1_agent == 2) || (host1_agent == 3);
-	uint pcie1_agent = (host1_agent == 0) || (host1_agent == 1);
-	uint devdisr = gur->devdisr;
-	uint io_sel = (gur->pordevsr & MPC86xx_PORDEVSR_IO_SEL) >> 16;
-
-	if ((io_sel == 2 || io_sel == 3 || io_sel == 5 || io_sel == 6 ||
-	     io_sel == 7 || io_sel == 0xf)
-	    && !(devdisr & MPC86xx_DEVDISR_PCIEX1)) {
-		printf("PCI-EXPRESS 1: Configured as %s \n",
-		       pcie1_agent ? "Agent" : "Host");
-		if (pcie1_agent)
-			return;	/*Don't scan bus when configured as agent */
-		printf("               Scanning PCIE bus");
-		debug("0x%08x=0x%08x ",
-		      &pcie1->pme_msg_det,
-		      pcie1->pme_msg_det);
-		if (pcie1->pme_msg_det) {
-			pcie1->pme_msg_det = 0xffffffff;
-			debug(" with errors.  Clearing.  Now 0x%08x",
-			      pcie1->pme_msg_det);
-		}
-		debug("\n");
-	} else {
-		printf("PCI-EXPRESS 1 disabled!\n");
-		return;
-	}
-
-	/*
-	 * Set first_bus=0 only skipped B0:D0:F0 which is
-	 * a reserved device in M1575, but make it easy for
-	 * most of the scan process.
-	 */
-	hose->first_busno = 0x00;
-	hose->last_busno = 0xfe;
-
-	pcie_setup_indirect(hose, (CFG_IMMR + 0x8000), (CFG_IMMR + 0x8004));
-
-	pci_hose_read_config_word(hose,
-				  PCI_BDF(0, 0, 0), PCI_COMMAND, &temp16);
-	temp16 |= PCI_COMMAND_SERR | PCI_COMMAND_MASTER |
-	    PCI_COMMAND_MEMORY | PCI_COMMAND_IO;
-	pci_hose_write_config_word(hose,
-				   PCI_BDF(0, 0, 0), PCI_COMMAND, temp16);
-
-	pci_hose_write_config_word(hose, PCI_BDF(0, 0, 0), PCI_STATUS, 0xffff);
-	pci_hose_write_config_byte(hose,
-				   PCI_BDF(0, 0, 0), PCI_LATENCY_TIMER, 0x80);
-
-	pci_hose_read_config_dword(hose, PCI_BDF(0, 0, 0), PCI_PRIMARY_BUS,
-				   &temp32);
-	temp32 = (temp32 & 0xff000000) | (0xff) | (0x0 << 8) | (0xfe << 16);
-	pci_hose_write_config_dword(hose, PCI_BDF(0, 0, 0), PCI_PRIMARY_BUS,
-				    temp32);
-
-	pcie1->powar1 = 0;
-	pcie1->powar2 = 0;
-	pcie1->piwar1 = 0;
-	pcie1->piwar1 = 0;
-
-	pcie1->powbar1 = (CFG_PCI1_MEM_BASE >> 12) & 0x000fffff;
-	pcie1->powar1 = 0x8004401c;	/* 512M MEM space */
-	pcie1->potar1 = (CFG_PCI1_MEM_BASE >> 12) & 0x000fffff;
-	pcie1->potear1 = 0x00000000;
-
-	pcie1->powbar2 = (CFG_PCI1_IO_BASE >> 12) & 0x000fffff;
-	pcie1->powar2 = 0x80088017;	/* 16M IO space */
-	pcie1->potar2 = 0x00000000;
-	pcie1->potear2 = 0x00000000;
-
-	pcie1->pitar1 = 0x00000000;
-	pcie1->piwbar1 = 0x00000000;
-	/* Enable, Prefetch, Local Mem, * Snoop R/W, 2G */
-	pcie1->piwar1 = 0xa0f5501e;
-
-	pci_set_region(hose->regions + 0,
-		       CFG_PCI_MEMORY_BUS,
-		       CFG_PCI_MEMORY_PHYS,
-		       CFG_PCI_MEMORY_SIZE,
-		       PCI_REGION_MEM | PCI_REGION_MEMORY);
-
-	pci_set_region(hose->regions + 1,
-		       CFG_PCI1_MEM_BASE,
-		       CFG_PCI1_MEM_PHYS,
-		       CFG_PCI1_MEM_SIZE,
-		       PCI_REGION_MEM);
-
-	pci_set_region(hose->regions + 2,
-		       CFG_PCI1_IO_BASE,
-		       CFG_PCI1_IO_PHYS,
-		       CFG_PCI1_IO_SIZE,
-		       PCI_REGION_IO);
-
-	hose->region_count = 3;
-
-	pci_register_hose(hose);
-
-	hose->last_busno = pci_hose_scan(hose);
-	debug("pcie_mpc86xx_init: last_busno %x\n", hose->last_busno);
-	debug("pcie_mpc86xx init: current_busno %x\n ", hose->current_busno);
-
-	printf("....PCIE1 scan & enumeration done\n");
-}
-#endif				/* CONFIG_PCI */
diff --git a/cpu/mpc86xx/pcie_indirect.c b/cpu/mpc86xx/pcie_indirect.c
deleted file mode 100644
index b00ad76ab8..0000000000
--- a/cpu/mpc86xx/pcie_indirect.c
+++ /dev/null
@@ -1,199 +0,0 @@
-/*
- * Support for indirect PCI bridges.
- *
- * Copyright (c) Freescale Semiconductor, Inc.
- * 2006. All rights reserved.
- *
- * Jason Jin <Jason.jin@freescale.com>
- *
- * 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.
- *
- * partly derived from
- * arch/powerpc/platforms/86xx/mpc86xx_pcie.c
- */
-
-#include <common.h>
-
-#ifdef CONFIG_PCI
-
-#include <asm/processor.h>
-#include <asm/io.h>
-#include <pci.h>
-
-#define PCI_CFG_OUT 	out_be32
-#define PEX_FIX		out_be32(hose->cfg_addr+0x4, 0x0400ffff)
-
-static int
-indirect_read_config_pcie(struct pci_controller *hose,
-			  pci_dev_t dev,
-			  int offset,
-			  int len,
-			  u32 *val)
-{
-	int bus = PCI_BUS(dev);
-
-	volatile unsigned char *cfg_data;
-	u32 temp;
-
-	PEX_FIX;
-	if (bus == 0xff) {
-		PCI_CFG_OUT(hose->cfg_addr,
-			    dev | (offset & 0xfc) | 0x80000001);
-	} else {
-		PCI_CFG_OUT(hose->cfg_addr,
-			    dev | (offset & 0xfc) | 0x80000000);
-	}
-	/*
-	 * Note: the caller has already checked that offset is
-	 * suitably aligned and that len is 1, 2 or 4.
-	 */
-	/* ERRATA PCI-Ex 12 - Configuration Address/Data Alignment */
-	cfg_data = hose->cfg_data;
-	PEX_FIX;
-	temp = in_le32((u32 *) cfg_data);
-	switch (len) {
-	case 1:
-		*val = (temp >> (((offset & 3)) * 8)) & 0xff;
-		break;
-	case 2:
-		*val = (temp >> (((offset & 3)) * 8)) & 0xffff;
-		break;
-	default:
-		*val = temp;
-		break;
-	}
-
-	return 0;
-}
-
-static int
-indirect_write_config_pcie(struct pci_controller *hose,
-			   pci_dev_t dev,
-			   int offset,
-			   int len,
-			   u32 val)
-{
-	int bus = PCI_BUS(dev);
-	volatile unsigned char *cfg_data;
-	u32 temp;
-
-	PEX_FIX;
-	if (bus == 0xff) {
-		PCI_CFG_OUT(hose->cfg_addr,
-			    dev | (offset & 0xfc) | 0x80000001);
-	} else {
-		PCI_CFG_OUT(hose->cfg_addr,
-			    dev | (offset & 0xfc) | 0x80000000);
-	}
-
-	/*
-	 * Note: the caller has already checked that offset is
-	 * suitably aligned and that len is 1, 2 or 4.
-	 */
-	/* ERRATA PCI-Ex 12 - Configuration Address/Data Alignment */
-	cfg_data = hose->cfg_data;
-	switch (len) {
-	case 1:
-		PEX_FIX;
-		temp = in_le32((u32 *) cfg_data);
-		temp = (temp & ~(0xff << ((offset & 3) * 8))) |
-		    (val << ((offset & 3) * 8));
-		PEX_FIX;
-		out_le32((u32 *) cfg_data, temp);
-		break;
-	case 2:
-		PEX_FIX;
-		temp = in_le32((u32 *) cfg_data);
-		temp = (temp & ~(0xffff << ((offset & 3) * 8)));
-		temp |= (val << ((offset & 3) * 8));
-		PEX_FIX;
-		out_le32((u32 *) cfg_data, temp);
-		break;
-	default:
-		PEX_FIX;
-		out_le32((u32 *) cfg_data, val);
-		break;
-	}
-	PEX_FIX;
-	return 0;
-}
-
-static int
-indirect_read_config_byte_pcie(struct pci_controller *hose,
-			       pci_dev_t dev,
-			       int offset,
-			       u8 *val)
-{
-	u32 val32;
-	indirect_read_config_pcie(hose, dev, offset, 1, &val32);
-	*val = (u8) val32;
-	return 0;
-}
-
-static int
-indirect_read_config_word_pcie(struct pci_controller *hose,
-			       pci_dev_t dev,
-			       int offset,
-			       u16 *val)
-{
-	u32 val32;
-	indirect_read_config_pcie(hose, dev, offset, 2, &val32);
-	*val = (u16) val32;
-	return 0;
-}
-
-static int
-indirect_read_config_dword_pcie(struct pci_controller *hose,
-				pci_dev_t dev,
-				int offset,
-				u32 *val)
-{
-	return indirect_read_config_pcie(hose, dev, offset, 4, val);
-}
-
-static int
-indirect_write_config_byte_pcie(struct pci_controller *hose,
-				pci_dev_t dev,
-				int offset,
-				u8 val)
-{
-	return indirect_write_config_pcie(hose, dev, offset, 1, (u32) val);
-}
-
-static int
-indirect_write_config_word_pcie(struct pci_controller *hose,
-				pci_dev_t dev,
-				int offset,
-				unsigned short val)
-{
-	return indirect_write_config_pcie(hose, dev, offset, 2, (u32) val);
-}
-
-static int
-indirect_write_config_dword_pcie(struct pci_controller *hose,
-				 pci_dev_t dev,
-				 int offset,
-				 u32 val)
-{
-	return indirect_write_config_pcie(hose, dev, offset, 4, val);
-}
-
-void
-pcie_setup_indirect(struct pci_controller *hose, u32 cfg_addr, u32 cfg_data)
-{
-	pci_set_ops(hose,
-		    indirect_read_config_byte_pcie,
-		    indirect_read_config_word_pcie,
-		    indirect_read_config_dword_pcie,
-		    indirect_write_config_byte_pcie,
-		    indirect_write_config_word_pcie,
-		    indirect_write_config_dword_pcie);
-
-	hose->cfg_addr = (unsigned int *)cfg_addr;
-	hose->cfg_data = (unsigned char *)cfg_data;
-}
-
-#endif				/* CONFIG_PCI */
diff --git a/cpu/mpc86xx/resetvec.S b/cpu/mpc86xx/resetvec.S
deleted file mode 100644
index 9a552f6624..0000000000
--- a/cpu/mpc86xx/resetvec.S
+++ /dev/null
@@ -1,2 +0,0 @@
-	.section .resetvec,"ax"
-	b _start
diff --git a/cpu/mpc86xx/start.S b/cpu/mpc86xx/start.S
index 67c56db1a3..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
@@ -116,7 +114,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -134,7 +132,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
@@ -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 8ea14e575f..c84bfbf6aa 100644
--- a/cpu/mpc86xx/traps.c
+++ b/cpu/mpc86xx/traps.c
@@ -34,7 +34,7 @@
 #include <command.h>
 #include <asm/processor.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
@@ -122,7 +122,7 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler) (regs))
 		return;
 #endif
@@ -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;
@@ -155,7 +158,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler) (regs))
 		return;
 #endif
@@ -170,7 +173,7 @@ ProgramCheckException(struct pt_regs *regs)
 	unsigned char *p = regs ? (unsigned char *)(regs->nip) : NULL;
 	int i, j;
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler) (regs))
 		return;
 #endif
@@ -193,7 +196,7 @@ ProgramCheckException(struct pt_regs *regs)
 void
 SoftEmuException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler) (regs))
 		return;
 #endif
@@ -205,10 +208,11 @@ SoftEmuException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler) (regs))
 		return;
 #endif
+	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/mpc8xx/bedbug_860.c b/cpu/mpc8xx/bedbug_860.c
index e91a1006f7..5d52366392 100644
--- a/cpu/mpc8xx/bedbug_860.c
+++ b/cpu/mpc8xx/bedbug_860.c
@@ -10,7 +10,7 @@
 #include <bedbug/ppc.h>
 #include <bedbug/type.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG) && defined(CONFIG_8xx)
+#if defined(CONFIG_CMD_BEDBUG) && defined(CONFIG_8xx)
 
 #define MAX_BREAK_POINTS 2
 
diff --git a/cpu/mpc8xx/fec.c b/cpu/mpc8xx/fec.c
index 6d2755e830..08a3715812 100644
--- a/cpu/mpc8xx/fec.c
+++ b/cpu/mpc8xx/fec.c
@@ -31,7 +31,7 @@ DECLARE_GLOBAL_DATA_PTR;
 
 #undef	ET_DEBUG
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && \
+#if defined(CONFIG_CMD_NET) && \
 	(defined(FEC_ENET) || defined(CONFIG_ETHER_ON_FEC1) || defined(CONFIG_ETHER_ON_FEC2))
 
 /* compatibility test, if only FEC_ENET defined assume ETHER on FEC1 */
@@ -49,7 +49,7 @@ DECLARE_GLOBAL_DATA_PTR;
 #if defined(WANT_MII)
 #include <miiphy.h>
 
-#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
 
@@ -182,7 +182,7 @@ int fec_initialize(bd_t *bis)
 
 		eth_register(dev);
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 		miiphy_register(dev->name,
 			fec8xx_miiphy_read, fec8xx_miiphy_write);
 #endif
@@ -268,7 +268,7 @@ static int fec_recv (struct eth_device *dev)
 
 			length -= 4;
 
-#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+#if defined(CONFIG_CMD_CDP)
 			if ((rx[0] & 1) != 0
 			    && memcmp ((uchar *) rx, NetBcastAddr, 6) != 0
 			    && memcmp ((uchar *) rx, NetCDPAddr, 6) != 0)
@@ -608,7 +608,7 @@ static int fec_init (struct eth_device *dev, bd_t * bd)
 	fecp->fec_addr_high = (ea[4] << 8) | (ea[5]);
 #undef ea
 
-#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+#if defined(CONFIG_CMD_CDP)
 	/*
 	 * Turn on multicast address hash table
 	 */
@@ -787,7 +787,7 @@ static void fec_halt(struct eth_device* dev)
 	efis->initialized = 0;
 }
 
-#if defined(CFG_DISCOVER_PHY) || defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CFG_DISCOVER_PHY) || defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 
 /* Make MII read/write commands for the FEC.
 */
@@ -852,7 +852,7 @@ mii_send(uint mii_cmd)
 #endif
 	return (mii_reply & 0xffff);		/* data read from phy */
 }
-#endif /* CFG_DISCOVER_PHY || (CONFIG_COMMANDS & CFG_CMD_MII) */
+#endif
 
 #if defined(CFG_DISCOVER_PHY)
 static int mii_discover_phy(struct eth_device *dev)
@@ -926,7 +926,7 @@ static int mii_discover_phy(struct eth_device *dev)
 }
 #endif	/* CFG_DISCOVER_PHY */
 
-#if (defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)) && !defined(CONFIG_BITBANGMII)
+#if (defined(CONFIG_MII) || defined(CONFIG_CMD_MII)) && !defined(CONFIG_BITBANGMII)
 
 /****************************************************************************
  * mii_init -- Initialize the MII for MII command without ethernet
@@ -1020,6 +1020,6 @@ int fec8xx_miiphy_write(char *devname, unsigned char  addr,
 #endif
 	return 0;
 }
-#endif /* (CONFIG_COMMANDS & CFG_CMD_MII) && !defined(CONFIG_BITBANGMII)*/
+#endif
 
-#endif	/* CFG_CMD_NET, FEC_ENET */
+#endif
diff --git a/cpu/mpc8xx/kgdb.S b/cpu/mpc8xx/kgdb.S
index 11c3c69339..812baa3ecd 100644
--- a/cpu/mpc8xx/kgdb.S
+++ b/cpu/mpc8xx/kgdb.S
@@ -34,7 +34,7 @@
 #include <asm/cache.h>
 #include <asm/mmu.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 
  /*
  * cache flushing routines for kgdb
@@ -71,4 +71,4 @@ kgdb_flush_cache_range:
 	SYNC
 	blr
 
-#endif /* CFG_CMD_KGDB */
+#endif
diff --git a/cpu/mpc8xx/scc.c b/cpu/mpc8xx/scc.c
index 6b9110f130..744dcdde91 100644
--- a/cpu/mpc8xx/scc.c
+++ b/cpu/mpc8xx/scc.c
@@ -38,7 +38,7 @@
 #include <net.h>
 #include <command.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(SCC_ENET)
+#if defined(CONFIG_CMD_NET) && defined(SCC_ENET)
 
 /* Ethernet Transmit and Receive Buffers */
 #define DBUF_LENGTH  1520
@@ -567,4 +567,4 @@ void restart (void)
 		(SCC_GSMRL_ENR | SCC_GSMRL_ENT);
 }
 #endif
-#endif	/* CFG_CMD_NET, SCC_ENET */
+#endif
diff --git a/cpu/mpc8xx/serial.c b/cpu/mpc8xx/serial.c
index ffc898c587..68804cc439 100644
--- a/cpu/mpc8xx/serial.c
+++ b/cpu/mpc8xx/serial.c
@@ -666,7 +666,7 @@ void enable_putc(void)
 }
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 
 void
 kgdb_serial_init(void)
@@ -723,6 +723,6 @@ kgdb_interruptible (int yes)
 {
 	return;
 }
-#endif	/* CFG_CMD_KGDB	*/
+#endif
 
 #endif	/* CONFIG_8xx_CONS_NONE */
diff --git a/cpu/mpc8xx/start.S b/cpu/mpc8xx/start.S
index 33a3f6c88e..eca4b50626 100644
--- a/cpu/mpc8xx/start.S
+++ b/cpu/mpc8xx/start.S
@@ -224,7 +224,7 @@ _start_of_vectors:
 /* Alignment exception. */
 	. = 0x600
 Alignment:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	mfspr	r4,DAR
 	stw	r4,_DAR(r21)
 	mfspr	r5,DSISR
@@ -242,7 +242,7 @@ Alignment:
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
-	EXCEPTION_PROLOG
+	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
 	li	r20,MSR_KERNEL
 	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
diff --git a/cpu/mpc8xx/traps.c b/cpu/mpc8xx/traps.c
index 67b75cce2f..e1ec88961a 100644
--- a/cpu/mpc8xx/traps.c
+++ b/cpu/mpc8xx/traps.c
@@ -36,11 +36,11 @@
 #include <command.h>
 #include <asm/processor.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -126,7 +126,7 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -159,7 +159,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -171,7 +171,7 @@ AlignmentException(struct pt_regs *regs)
 void
 ProgramCheckException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -183,7 +183,7 @@ ProgramCheckException(struct pt_regs *regs)
 void
 SoftEmuException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -196,7 +196,7 @@ SoftEmuException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -210,7 +210,7 @@ DebugException(struct pt_regs *regs)
 {
   printf("Debugger trap at @ %lx\n", regs->nip );
   show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
   do_bedbug_breakpoint( regs );
 #endif
 }
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/nios/interrupts.c b/cpu/nios/interrupts.c
index 48fc81e584..75e491d843 100644
--- a/cpu/nios/interrupts.c
+++ b/cpu/nios/interrupts.c
@@ -173,7 +173,7 @@ void irq_install_handler (int vec, interrupt_handler_t *handler, void *arg)
 }
 
 /*************************************************************************/
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 int do_irqinfo (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 {
 	int vec;
@@ -193,4 +193,4 @@ int do_irqinfo (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 
 	return (0);
 }
-#endif  /* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/nios2/interrupts.c b/cpu/nios2/interrupts.c
index 4685161b88..aeb5b65b33 100644
--- a/cpu/nios2/interrupts.c
+++ b/cpu/nios2/interrupts.c
@@ -204,7 +204,7 @@ int interrupt_init (void)
 
 
 /*************************************************************************/
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 int do_irqinfo (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 {
 	int i;
@@ -228,4 +228,4 @@ int do_irqinfo (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 
 	return (0);
 }
-#endif  /* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/ppc4xx/405gp_pci.c b/cpu/ppc4xx/405gp_pci.c
index 7134355635..28379298b7 100644
--- a/cpu/ppc4xx/405gp_pci.c
+++ b/cpu/ppc4xx/405gp_pci.c
@@ -77,11 +77,21 @@
 #include <asm/processor.h>
 #include <pci.h>
 
+#ifdef CONFIG_PCI
+
 DECLARE_GLOBAL_DATA_PTR;
 
-#if defined(CONFIG_405GP) || defined(CONFIG_405EP)
+/*
+ * Board-specific pci initialization
+ * Platform code can reimplement pci_pre_init() if needed
+ */
+int __pci_pre_init(struct pci_controller *hose)
+{
+	return 1;
+}
+int pci_pre_init(struct pci_controller *hose) __attribute__((weak, alias("__pci_pre_init")));
 
-#ifdef CONFIG_PCI
+#if defined(CONFIG_405GP) || defined(CONFIG_405EP)
 
 #if defined(CONFIG_PMC405)
 ushort pmc405_pci_subsys_deviceid(void);
@@ -191,6 +201,13 @@ void pci_405gp_init(struct pci_controller *hose)
 	if (hose->pci_fb)
 		pciauto_region_init(hose->pci_fb);
 
+	/* Let board change/modify hose & do initial checks */
+	if (pci_pre_init (hose) == 0) {
+		printf("PCI: Board-specific initialization failed.\n");
+		printf("PCI: Configuration aborted.\n");
+		return;
+	}
+
 	pci_register_hose(hose);
 
 	/*--------------------------------------------------------------------------+
@@ -380,7 +397,7 @@ void pci_405gp_setup_vga(struct pci_controller *hose, pci_dev_t dev,
 	pci_hose_write_config_dword(hose, dev, PCI_COMMAND, cmdstat);
 }
 
-#if !(defined(CONFIG_PIP405) || defined (CONFIG_MIP405)) && !(defined (CONFIG_SOLIDCARD3))
+#if !(defined(CONFIG_PIP405) || defined (CONFIG_MIP405)) && !(defined (CONFIG_SC3))
 
 /*
  *As is these functs get called out of flash Not a horrible
@@ -416,14 +433,12 @@ void pci_init_board(void)
 
 #endif
 
-#endif /* CONFIG_PCI */
-
 #endif /* CONFIG_405GP */
 
 /*-----------------------------------------------------------------------------+
  * CONFIG_440
  *-----------------------------------------------------------------------------*/
-#if defined(CONFIG_440) && defined(CONFIG_PCI)
+#if defined(CONFIG_440)
 
 static struct pci_controller ppc440_hose = {0};
 
@@ -496,14 +511,12 @@ void pci_440_init (struct pci_controller *hose)
 
 	pci_setup_indirect(hose, PCIX0_CFGADR, PCIX0_CFGDATA);
 
-#if defined(CFG_PCI_PRE_INIT)
 	/* Let board change/modify hose & do initial checks */
 	if (pci_pre_init (hose) == 0) {
 		printf("PCI: Board-specific initialization failed.\n");
 		printf("PCI: Configuration aborted.\n");
 		return;
 	}
-#endif
 
 	pci_register_hose( hose );
 
@@ -575,4 +588,5 @@ void pci_init_board(void)
 #endif
 }
 
-#endif /* CONFIG_440 & CONFIG_PCI */
+#endif /* CONFIG_440 */
+#endif /* CONFIG_PCI */
diff --git a/cpu/ppc4xx/44x_spd_ddr.c b/cpu/ppc4xx/44x_spd_ddr.c
index c500d3f242..6d6fba1802 100644
--- a/cpu/ppc4xx/44x_spd_ddr.c
+++ b/cpu/ppc4xx/44x_spd_ddr.c
@@ -70,6 +70,15 @@
 
 #define ONE_BILLION	1000000000
 
+/*
+ * Board-specific Platform code can reimplement spd_ddr_init_hang () if needed
+ */
+void __spd_ddr_init_hang (void)
+{
+	hang ();
+}
+void spd_ddr_init_hang (void) __attribute__((weak, alias("__spd_ddr_init_hang")));
+
 /*-----------------------------------------------------------------------------
   |  Memory Controller Options 0
   +-----------------------------------------------------------------------------*/
@@ -262,7 +271,7 @@ typedef struct bank_param BANKPARMS;
 #ifdef CFG_SIMULATE_SPD_EEPROM
 extern unsigned char cfg_simulate_spd_eeprom[128];
 #endif
-void program_tlb(u32 start, u32 size, u32 tlb_word2_i_value);
+void program_tlb(u32 phys_addr, u32 virt_addr, u32 size, u32 tlb_word2_i_value);
 
 static unsigned char spd_read(uchar chip, uint addr);
 static void get_spd_info(unsigned long *dimm_populated,
@@ -373,7 +382,7 @@ long int spd_sdram(void) {
 
 #ifdef CONFIG_PROG_SDRAM_TLB /* this define should eventually be removed */
 	/* and program tlb entries for this size (dynamic) */
-	program_tlb(0, total_size, MY_TLB_WORD2_I_ENABLE);
+	program_tlb(0, 0, total_size, MY_TLB_WORD2_I_ENABLE);
 #endif
 
 	/*
@@ -467,7 +476,7 @@ static void get_spd_info(unsigned long *dimm_populated,
 
 	if (dimm_found == FALSE) {
 		printf("ERROR - No memory installed. Install a DDR-SDRAM DIMM.\n\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 }
 
@@ -490,7 +499,7 @@ static void check_mem_type(unsigned long *dimm_populated,
 				       dimm_num);
 				printf("Only DDR SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 		}
@@ -510,7 +519,7 @@ static void check_volt_type(unsigned long *dimm_populated,
 			if (voltage_type != 0x04) {
 				printf("ERROR: DIMM %lu with unsupported voltage level.\n",
 				       dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 			} else {
 				debug("DIMM %lu voltage level supported.\n", dimm_num);
 			}
@@ -581,7 +590,7 @@ static void program_cfg0(unsigned long *dimm_populated,
 				printf("WARNING: DIMM with datawidth of %lu bits.\n",
 				       data_width);
 				printf("Only DIMMs with 32 or 64 bit datawidths supported.\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 			break;
 		}
@@ -769,7 +778,7 @@ static void program_tr0(unsigned long *dimm_populated,
 				if ((tcyc_reg & 0x0F) >= 10) {
 					printf("ERROR: Tcyc incorrect for DIMM in slot %lu\n",
 					       dimm_num);
-					hang();
+					spd_ddr_init_hang ();
 				}
 
 				cycle_time_ns_x_10[cas_index] =
@@ -849,7 +858,7 @@ static void program_tr0(unsigned long *dimm_populated,
 		printf("ERROR: No supported CAS latency with the installed DIMMs.\n");
 		printf("Only CAS latencies of 2.0, 2.5, and 3.0 are supported.\n");
 		printf("Make sure the PLB speed is within the supported range.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	/*
@@ -1008,6 +1017,7 @@ static int short_mem_test(void)
 			 */
 			for (i = 0; i < NUMMEMTESTS; i++) {
 				for (j = 0; j < NUMMEMWORDS; j++) {
+					/* printf("bank enabled base:%x\n", &membase[j]); */
 					membase[j] = test[i][j];
 					ppcDcbf((unsigned long)&(membase[j]));
 				}
@@ -1160,7 +1170,7 @@ static void program_tr1(void)
 	 */
 	if (window_found == FALSE) {
 		printf("ERROR: Cannot determine a common read delay.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	/*
@@ -1310,7 +1320,7 @@ static unsigned long program_bxcr(unsigned long *dimm_populated,
 				printf("ERROR: Unsupported value for the banksize: %d.\n",
 				       bank_size_id);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 
 			switch (num_col_addr) {
@@ -1332,7 +1342,7 @@ static unsigned long program_bxcr(unsigned long *dimm_populated,
 				printf("ERROR: Unsupported value for number of "
 				       "column addresses: %d.\n", num_col_addr);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 
 			/*
@@ -1340,14 +1350,14 @@ static unsigned long program_bxcr(unsigned long *dimm_populated,
 			 */
 			cr |= SDRAM_BXCR_SDBE;
 
- 			for (i = 0; i < num_banks; i++) {
+			for (i = 0; i < num_banks; i++) {
 				bank_parms[ctrl_bank_num[dimm_num]+i].bank_size_bytes =
 					(4 << 20) * bank_size_id;
 				bank_parms[ctrl_bank_num[dimm_num]+i].cr = cr;
 				debug("DIMM%d-bank %d (SDRAM0_B%dCR): bank_size_bytes=%d\n",
 				      dimm_num, i, ctrl_bank_num[dimm_num]+i,
 				      bank_parms[ctrl_bank_num[dimm_num]+i].bank_size_bytes);
- 			}
+			}
 		}
 	}
 
diff --git a/cpu/ppc4xx/44x_spd_ddr2.c b/cpu/ppc4xx/44x_spd_ddr2.c
index 48b9ee2f7e..5fef27b984 100644
--- a/cpu/ppc4xx/44x_spd_ddr2.c
+++ b/cpu/ppc4xx/44x_spd_ddr2.c
@@ -58,8 +58,8 @@
 #define SDRAM_DDR2	2
 #define SDRAM_NONE	0
 
-#define MAXDIMMS 	2
-#define MAXRANKS 	4
+#define MAXDIMMS	2
+#define MAXRANKS	4
 #define MAXBXCF		4
 #define MAX_SPD_BYTES	256   /* Max number of bytes on the DIMM's SPD EEPROM */
 
@@ -129,6 +129,16 @@
 #define MY_TLB_WORD2_I_ENABLE	TLB_WORD2_I_ENABLE	/* disable caching on SDRAM */
 #endif
 
+/*
+ * Board-specific Platform code can reimplement spd_ddr_init_hang () if needed
+ */
+void __spd_ddr_init_hang (void)
+{
+	hang ();
+}
+void spd_ddr_init_hang (void) __attribute__((weak, alias("__spd_ddr_init_hang")));
+
+
 /* Private Structure Definitions */
 
 /* enum only to ease code for cas latency setting */
@@ -144,7 +154,7 @@ typedef enum ddr_cas_id {
  * Prototypes
  *-----------------------------------------------------------------------------*/
 static unsigned long sdram_memsize(void);
-void program_tlb(u32 start, u32 size, u32 tlb_word2_i_value);
+void program_tlb(u32 phys_addr, u32 virt_addr, u32 size, u32 tlb_word2_i_value);
 static void get_spd_info(unsigned long *dimm_populated,
 			 unsigned char *iic0_dimm_addr,
 			 unsigned long num_dimm_banks);
@@ -528,7 +538,7 @@ long int initdram(int board_type)
 	dram_size = sdram_memsize();
 
 	/* and program tlb entries for this size (dynamic) */
-	program_tlb(0, dram_size, MY_TLB_WORD2_I_ENABLE);
+	program_tlb(0, 0, dram_size, MY_TLB_WORD2_I_ENABLE);
 
 	/*------------------------------------------------------------------
 	 * DQS calibration.
@@ -582,7 +592,7 @@ static void get_spd_info(unsigned long *dimm_populated,
 
 	if (dimm_found == FALSE) {
 		printf("ERROR - No memory installed. Install a DDR-SDRAM DIMM.\n\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 }
 
@@ -629,42 +639,42 @@ static void check_mem_type(unsigned long *dimm_populated,
 				       "slot %d.\n", (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 2:
 				printf("ERROR: EDO DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 3:
 				printf("ERROR: Pipelined Nibble DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 4:
 				printf("ERROR: SDRAM DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 5:
 				printf("ERROR: Multiplexed ROM DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 6:
 				printf("ERROR: SGRAM DIMM detected in slot %d.\n",
 				       (unsigned int)dimm_num);
 				printf("Only DDR and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 7:
 				debug("DIMM slot %d: DDR1 SDRAM detected\n", dimm_num);
@@ -679,7 +689,7 @@ static void check_mem_type(unsigned long *dimm_populated,
 				       (unsigned int)dimm_num);
 				printf("Only DDR1 and DDR2 SDRAM DIMMs are supported.\n");
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 		}
@@ -689,7 +699,7 @@ static void check_mem_type(unsigned long *dimm_populated,
 		    && (dimm_populated[dimm_num]   != SDRAM_NONE)
 		    && (dimm_populated[dimm_num-1] != dimm_populated[dimm_num])) {
 			printf("ERROR: DIMM's DDR1 and DDR2 type can not be mixed.\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 }
@@ -764,7 +774,7 @@ static void check_frequency(unsigned long *dimm_populated,
 				       (unsigned int)(calc_cycle_time*10));
 				printf("Replace the DIMM, or change DDR frequency via "
 				       "strapping bits.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 		}
 	}
@@ -796,7 +806,7 @@ static void check_rank_number(unsigned long *dimm_populated,
 				       "slot %d is not supported.\n", dimm_rank, dimm_num);
 				printf("Only %d ranks are supported for all DIMM.\n", MAXRANKS);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			} else
 				total_rank += dimm_rank;
 		}
@@ -805,7 +815,7 @@ static void check_rank_number(unsigned long *dimm_populated,
 			       "for all slots.\n", (unsigned int)total_rank);
 			printf("Only %d ranks are supported for all DIMM.\n", MAXRANKS);
 			printf("Remove one of the DIMM modules.\n\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 }
@@ -830,28 +840,28 @@ static void check_voltage_type(unsigned long *dimm_populated,
 				printf("This DIMM is 5.0 Volt/TTL.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x01:
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("This DIMM is LVTTL.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x02:
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("This DIMM is 1.5 Volt.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x03:
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("This DIMM is 3.3 Volt/TTL.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			case 0x04:
 				/* 2.5 Voltage only for DDR1 */
@@ -863,7 +873,7 @@ static void check_voltage_type(unsigned long *dimm_populated,
 				printf("ERROR: Only DIMMs DDR 2.5V or DDR2 1.8V are supported.\n");
 				printf("Replace the DIMM module in slot %d with a supported DIMM.\n\n",
 				       (unsigned int)dimm_num);
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 		}
@@ -1006,13 +1016,13 @@ static void program_copt1(unsigned long *dimm_populated,
 	if ((dimm_populated[0] != SDRAM_NONE) && (dimm_populated[1] != SDRAM_NONE)) {
 		if (buf0 != buf1) {
 			printf("ERROR: DIMM's buffered/unbuffered, registered, clocking don't match.\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 
 	if ((dimm_64bit == TRUE) && (dimm_32bit == TRUE)) {
 		printf("ERROR: Cannot mix 32 bit and 64 bit DDR-SDRAM DIMMs together.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 	else if ((dimm_64bit == TRUE) && (dimm_32bit == FALSE)) {
 		mcopt1 |= SDRAM_MCOPT1_DMWD_64;
@@ -1020,7 +1030,7 @@ static void program_copt1(unsigned long *dimm_populated,
 		mcopt1 |= SDRAM_MCOPT1_DMWD_32;
 	} else {
 		printf("ERROR: Please install only 32 or 64 bit DDR-SDRAM DIMMs.\n\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	if (ecc_enabled == TRUE)
@@ -1209,7 +1219,7 @@ static void program_initplr(unsigned long *dimm_populated,
 			break;
 		default:
 			printf("ERROR: ucode error on selected_cas value %d", selected_cas);
-			hang();
+			spd_ddr_init_hang ();
 			break;
 		}
 
@@ -1241,7 +1251,7 @@ static void program_initplr(unsigned long *dimm_populated,
 			break;
 		default:
 			printf("ERROR: write recovery not support (%d)", write_recovery);
-			hang();
+			spd_ddr_init_hang ();
 			break;
 		}
 #else
@@ -1259,7 +1269,7 @@ static void program_initplr(unsigned long *dimm_populated,
 			ods = ODS_REDUCED;
 		} else {
 			printf("ERROR: Unsupported number of DIMM's (%d)", total_dimm);
-			hang();
+			spd_ddr_init_hang ();
 		}
 
 		mr = CMD_EMR | SELECT_MR | BURST_LEN_4 | wr | cas;
@@ -1284,7 +1294,7 @@ static void program_initplr(unsigned long *dimm_populated,
 		mtsdram(SDRAM_INITPLR13, 0x80800000 | emr);		/* EMR OCD Exit */
 	} else {
 		printf("ERROR: ucode error as unknown DDR type in program_initplr");
-		hang();
+		spd_ddr_init_hang ();
 	}
 }
 
@@ -1389,7 +1399,7 @@ static void program_mode(unsigned long *dimm_populated,
 					} else {
 						printf("ERROR: SPD reported Tcyc is incorrect for DIMM "
 						       "in slot %d\n", (unsigned int)dimm_num);
-						hang();
+						spd_ddr_init_hang ();
 					}
 				} else {
 					/* Convert from hex to decimal */
@@ -1526,7 +1536,7 @@ static void program_mode(unsigned long *dimm_populated,
 			printf("ERROR: Cannot find a supported CAS latency with the installed DIMMs.\n");
 			printf("Only DIMMs DDR1 with CAS latencies of 2.0, 2.5, and 3.0 are supported.\n");
 			printf("Make sure the PLB speed is within the supported range of the DIMMs.\n\n");
-			hang();
+			spd_ddr_init_hang ();
 		}
 	} else { /* DDR2 */
 		debug("cas_3_0_available=%d\n", cas_3_0_available);
@@ -1549,7 +1559,7 @@ static void program_mode(unsigned long *dimm_populated,
 			       cas_3_0_available, cas_4_0_available, cas_5_0_available);
 			printf("sdram_freq=%d cycle3=%d cycle4=%d cycle5=%d\n\n",
 			       sdram_freq, cycle_3_0_clk, cycle_4_0_clk, cycle_5_0_clk);
-			hang();
+			spd_ddr_init_hang ();
 		}
 	}
 
@@ -1658,7 +1668,7 @@ static void program_rtr(unsigned long *dimm_populated,
 				printf("ERROR: DIMM %d unsupported refresh rate/type.\n",
 				       (unsigned int)dimm_num);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 				break;
 			}
 
@@ -2066,7 +2076,7 @@ static void program_bxcf(unsigned long *dimm_populated,
 					printf("ERROR: Unsupported value for number of "
 					       "column addresses: %d.\n", (unsigned int)num_col_addr);
 					printf("Replace the DIMM module with a supported DIMM.\n\n");
-					hang();
+					spd_ddr_init_hang ();
 				}
 			}
 
@@ -2148,7 +2158,7 @@ static void program_memory_queue(unsigned long *dimm_populated,
 				printf("ERROR: Unsupported value for the banksize: %d.\n",
 				       (unsigned int)rank_size_id);
 				printf("Replace the DIMM module with a supported DIMM.\n\n");
-				hang();
+				spd_ddr_init_hang ();
 			}
 
 			if ((dimm_populated[dimm_num] != SDRAM_NONE) && (dimm_num == 1))
@@ -2693,7 +2703,7 @@ calibration_loop:
 		printf("\nERROR: Cannot determine a common read delay for the "
 		       "DIMM(s) installed.\n");
 		debug("%s[%d] ERROR : \n", __FUNCTION__,__LINE__);
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	blank_string(strlen(str));
@@ -2849,7 +2859,7 @@ static void test(void)
 	if (window_found == FALSE) {
 		printf("ERROR: Cannot determine a common read delay for the "
 		       "DIMM(s) installed.\n");
-		hang();
+		spd_ddr_init_hang ();
 	}
 
 	/*------------------------------------------------------------------
diff --git a/cpu/ppc4xx/4xx_enet.c b/cpu/ppc4xx/4xx_enet.c
index 1200d021af..d78279171e 100644
--- a/cpu/ppc4xx/4xx_enet.c
+++ b/cpu/ppc4xx/4xx_enet.c
@@ -94,9 +94,9 @@
  * network support enabled.
  * Remark: CONFIG_405 describes Xilinx PPC405 FPGA without EMAC controller!
  */
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && !defined(CONFIG_405) && !defined(CONFIG_IOP480)
+#if defined(CONFIG_CMD_NET) && !defined(CONFIG_405) && !defined(CONFIG_IOP480)
 
-#if !(defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII))
+#if !(defined(CONFIG_MII) || defined(CONFIG_CMD_MII))
 #error "CONFIG_MII has to be defined!"
 #endif
 
@@ -1683,7 +1683,7 @@ int ppc_4xx_eth_initialize (bd_t * bis)
 #endif
 
 #if defined(CONFIG_NET_MULTI)
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 		miiphy_register (dev->name,
 				 emac4xx_miiphy_read, emac4xx_miiphy_write);
 #endif
@@ -1724,7 +1724,7 @@ int eth_rx(void)
 
 int emac4xx_miiphy_initialize (bd_t * bis)
 {
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 	miiphy_register ("ppc_4xx_eth0",
 			 emac4xx_miiphy_read, emac4xx_miiphy_write);
 #endif
@@ -1733,4 +1733,4 @@ int emac4xx_miiphy_initialize (bd_t * bis)
 }
 #endif /* !defined(CONFIG_NET_MULTI) */
 
-#endif /* #if (CONFIG_COMMANDS & CFG_CMD_NET) */
+#endif
diff --git a/cpu/ppc4xx/Makefile b/cpu/ppc4xx/Makefile
index 4068b53208..af9da5b95f 100644
--- a/cpu/ppc4xx/Makefile
+++ b/cpu/ppc4xx/Makefile
@@ -27,12 +27,12 @@ LIB	= $(obj)lib$(CPU).a
 
 START	= start.o resetvec.o kgdb.o
 SOBJS	= dcr.o
-COBJS	= 405gp_pci.o 4xx_enet.o \
+COBJS	= 405gp_pci.o 440spe_pcie.o 4xx_enet.o \
 	  bedbug_405.o commproc.o \
 	  cpu.o cpu_init.o gpio.o i2c.o interrupts.o \
 	  miiphy.o ndfc.o sdram.o serial.o \
 	  40x_spd_sdram.o 44x_spd_ddr.o 44x_spd_ddr2.o speed.o \
-	  tlb.o traps.o usb_ohci.o usbdev.o \
+	  tlb.o traps.o usb_ohci.o usb.o usbdev.o \
 	  440spe_pcie.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
diff --git a/cpu/ppc4xx/bedbug_405.c b/cpu/ppc4xx/bedbug_405.c
index a3c2119764..5ef5607918 100644
--- a/cpu/ppc4xx/bedbug_405.c
+++ b/cpu/ppc4xx/bedbug_405.c
@@ -10,7 +10,7 @@
 #include <bedbug/regs.h>
 #include <bedbug/ppc.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG) && defined(CONFIG_4xx)
+#if defined(CONFIG_CMD_BEDBUG) && defined(CONFIG_4xx)
 
 #define MAX_BREAK_POINTS 4
 
diff --git a/cpu/ppc4xx/config.mk b/cpu/ppc4xx/config.mk
index 119e061b89..4fd510899c 100644
--- a/cpu/ppc4xx/config.mk
+++ b/cpu/ppc4xx/config.mk
@@ -22,5 +22,13 @@
 #
 
 PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -fno-strict-aliasing
+PLATFORM_CPPFLAGS += -DCONFIG_4xx -ffixed-r2 -ffixed-r29 -mstring -msoft-float
 
-PLATFORM_CPPFLAGS += -DCONFIG_4xx -ffixed-r2 -ffixed-r29 -mstring -Wa,-m405 -mcpu=405 -msoft-float
+cfg=$(shell grep configs $(OBJTREE)/include/config.h | sed 's/.*<\(configs.*\)>/\1/')
+is440=$(shell grep CONFIG_440 $(TOPDIR)/include/$(cfg))
+
+ifneq (,$(findstring CONFIG_440,$(is440)))
+PLATFORM_CPPFLAGS += -Wa,-m440 -mcpu=440
+else
+PLATFORM_CPPFLAGS += -Wa,-m405 -mcpu=405
+endif
diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index 66e8637260..351da36e85 100644
--- a/cpu/ppc4xx/cpu_init.c
+++ b/cpu/ppc4xx/cpu_init.c
@@ -153,7 +153,7 @@ cpu_init_f (void)
 	 */
 	asm volatile("	bl	0f"		::: "lr");
 	asm volatile("0:	mflr	3"		::: "r3");
-	asm volatile("	addi 	4, 0, 14"	::: "r4");
+	asm volatile("	addi	4, 0, 14"	::: "r4");
 	asm volatile("	mtctr	4"		::: "ctr");
 	asm volatile("1:	icbt	0, 3");
 	asm volatile("	addi	3, 3, 32"	::: "r3");
@@ -211,6 +211,8 @@ cpu_init_f (void)
 	val = mfspr(tcr);
 #if defined(CONFIG_440EP) || defined(CONFIG_440GR)
 	val |= 0xb8000000;      /* generate system reset after 1.34 seconds */
+#elif defined(CONFIG_440EPX)
+	val |= 0xb0000000;      /* generate system reset after 1.34 seconds */
 #else
 	val |= 0xf0000000;      /* generate system reset after 2.684 seconds */
 #endif
diff --git a/cpu/ppc4xx/dcr.S b/cpu/ppc4xx/dcr.S
index 7102364ebd..93465a3b51 100644
--- a/cpu/ppc4xx/dcr.S
+++ b/cpu/ppc4xx/dcr.S
@@ -22,7 +22,7 @@
  */
 #include <config.h>
 
-#if defined(CONFIG_4xx) && defined(CFG_CMD_SETGETDCR)
+#if defined(CONFIG_4xx) && defined(CONFIG_CMD_SETGETDCR)
 
 #include <ppc4xx.h>
 
@@ -195,4 +195,4 @@ set_dcr:
 		blr				/* Return to calling function */
 .Lfe4:		.size	set_dcr,.Lfe4-set_dcr
 /* end set_dcr() */
-#endif /* CONFIG_4xx & CFG_CMD_SETGETDCR */
+#endif
diff --git a/cpu/ppc4xx/gpio.c b/cpu/ppc4xx/gpio.c
index dd84e58a1f..5235203ea2 100644
--- a/cpu/ppc4xx/gpio.c
+++ b/cpu/ppc4xx/gpio.c
@@ -103,6 +103,18 @@ void gpio_write_bit(int pin, int val)
 		out32(GPIO0_OR + offs, in32(GPIO0_OR + offs) & ~GPIO_VAL(pin));
 }
 
+int gpio_read_out_bit(int pin)
+{
+	u32 offs = 0;
+
+	if (pin >= GPIO_MAX) {
+		offs = 0x100;
+		pin -= GPIO_MAX;
+	}
+
+	return (in32(GPIO0_OR + offs) & GPIO_VAL(pin) ? 1 : 0);
+}
+
 #if defined(CFG_440_GPIO_TABLE)
 void gpio_set_chip_configuration(void)
 {
@@ -157,12 +169,38 @@ void gpio_set_chip_configuration(void)
 				switch (gpio_tab[gpio_core][i].alt_nb) {
 				case GPIO_SEL:
 					if (gpio_core == GPIO0) {
-						reg = in32(GPIO0_TCR) | (0x80000000 >> (j));
+						/*
+						 * Setup output value
+						 * 1 -> high level
+						 * 0 -> low level
+						 * else -> don't touch
+						 */
+						reg = in32(GPIO0_OR);
+						if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_1)
+							reg |= (0x80000000 >> (i));
+						else if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_0)
+							reg &= ~(0x80000000 >> (i));
+						out32(GPIO0_OR, reg);
+
+						reg = in32(GPIO0_TCR) | (0x80000000 >> (i));
 						out32(GPIO0_TCR, reg);
 					}
 
 					if (gpio_core == GPIO1) {
-						reg = in32(GPIO1_TCR) | (0x80000000 >> (j));
+						/*
+						 * Setup output value
+						 * 1 -> high level
+						 * 0 -> low level
+						 * else -> don't touch
+						 */
+						reg = in32(GPIO0_OR);
+						if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_1)
+							reg |= (0x80000000 >> (i));
+						else if (gpio_tab[gpio_core][i].out_val == GPIO_OUT_0)
+							reg &= ~(0x80000000 >> (i));
+						out32(GPIO0_OR, reg);
+
+						reg = in32(GPIO1_TCR) | (0x80000000 >> (i));
 						out32(GPIO1_TCR, reg);
 					}
 
diff --git a/cpu/ppc4xx/interrupts.c b/cpu/ppc4xx/interrupts.c
index c5a9f02566..ca565cc3e0 100644
--- a/cpu/ppc4xx/interrupts.c
+++ b/cpu/ppc4xx/interrupts.c
@@ -628,7 +628,7 @@ void timer_interrupt_cpu (struct pt_regs *regs)
 
 /****************************************************************************/
 
-#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#if defined(CONFIG_CMD_IRQ)
 
 /*******************************************************************************
  *
@@ -698,4 +698,4 @@ do_irqinfo(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 
 	return 0;
 }
-#endif  /* CONFIG_COMMANDS & CFG_CMD_IRQ */
+#endif
diff --git a/cpu/ppc4xx/kgdb.S b/cpu/ppc4xx/kgdb.S
index be283403e9..8c4bbf2e4d 100644
--- a/cpu/ppc4xx/kgdb.S
+++ b/cpu/ppc4xx/kgdb.S
@@ -34,7 +34,7 @@
 #include <asm/cache.h>
 #include <asm/mmu.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
  /*
  * cache flushing routines for kgdb
  */
@@ -75,4 +75,4 @@ kgdb_flush_cache_range:
 	SYNC
 	blr
 
-#endif /* CFG_CMD_KGDB */
+#endif
diff --git a/cpu/ppc4xx/ndfc.c b/cpu/ppc4xx/ndfc.c
index f63fc79f6c..398457726f 100644
--- a/cpu/ppc4xx/ndfc.c
+++ b/cpu/ppc4xx/ndfc.c
@@ -31,7 +31,7 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY) && \
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY) && \
 	(defined(CONFIG_440EP) || defined(CONFIG_440GR) ||	     \
 	 defined(CONFIG_440EPX) || defined(CONFIG_440GRX) ||	     \
 	 defined(CONFIG_405EZ))
diff --git a/cpu/ppc4xx/serial.c b/cpu/ppc4xx/serial.c
index e62dd9dac5..3f67136be5 100644
--- a/cpu/ppc4xx/serial.c
+++ b/cpu/ppc4xx/serial.c
@@ -843,7 +843,7 @@ int serial_buffered_tstc (void)
 
 #endif	/* CONFIG_SERIAL_SOFTWARE_FIFO */
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 /*
   AS HARNOIS : according to CONFIG_KGDB_SER_INDEX kgdb uses serial port
   number 0 or number 1
@@ -949,7 +949,7 @@ void kgdb_interruptible (int yes)
 	return;
 }
 #endif	/* (CONFIG_KGDB_SER_INDEX & 2) */
-#endif	/* CFG_CMD_KGDB */
+#endif
 
 
 #if defined(CONFIG_SERIAL_MULTI)
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index 78de30031d..8ecaaea4d9 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -22,26 +22,27 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  * MA 02111-1307 USA
  */
-/*------------------------------------------------------------------------------+ */
-/* */
-/*	 This source code has been made available to you by IBM on an AS-IS */
-/*	 basis.	 Anyone receiving this source is licensed under IBM */
-/*	 copyrights to use it in any way he or she deems fit, including */
-/*	 copying it, modifying it, compiling it, and redistributing it either */
-/*	 with or without modifications.	 No license under IBM patents or */
-/*	 patent applications is to be implied by the copyright license. */
-/* */
-/*	 Any user of this software should understand that IBM cannot provide */
-/*	 technical support for this software and will not be responsible for */
-/*	 any consequences resulting from the use of this software. */
-/* */
-/*	 Any person who transfers this source code or any derivative work */
-/*	 must include the IBM copyright notice, this paragraph, and the */
-/*	 preceding two paragraphs in the transferred software. */
-/* */
-/*	 COPYRIGHT   I B M   CORPORATION 1995 */
-/*	 LICENSED MATERIAL  -  PROGRAM PROPERTY OF I B M */
-/*------------------------------------------------------------------------------- */
+/*------------------------------------------------------------------------------+
+ *
+ *	 This source code has been made available to you by IBM on an AS-IS
+ *	 basis.	 Anyone receiving this source is licensed under IBM
+ *	 copyrights to use it in any way he or she deems fit, including
+ *	 copying it, modifying it, compiling it, and redistributing it either
+ *	 with or without modifications.	 No license under IBM patents or
+ *	 patent applications is to be implied by the copyright license.
+ *
+ *	 Any user of this software should understand that IBM cannot provide
+ *	 technical support for this software and will not be responsible for
+ *	 any consequences resulting from the use of this software.
+ *
+ *	 Any person who transfers this source code or any derivative work
+ *	 must include the IBM copyright notice, this paragraph, and the
+ *	 preceding two paragraphs in the transferred software.
+ *
+ *	 COPYRIGHT   I B M   CORPORATION 1995
+ *	 LICENSED MATERIAL  -  PROGRAM PROPERTY OF I B M
+ *-------------------------------------------------------------------------------
+ */
 
 /*  U-Boot - Startup Code for AMCC 4xx PowerPC based Embedded Boards
  *
@@ -59,7 +60,6 @@
  *  address and (s)dram will be positioned at address 0
  */
 #include <config.h>
-#include <mpc8xx.h>
 #include <ppc4xx.h>
 #include <version.h>
 
@@ -110,11 +110,11 @@
 # endif
 #endif /* CFG_INIT_DCACHE_CS */
 
-#define function_prolog(func_name)      .text; \
+#define function_prolog(func_name)	.text; \
 					.align 2; \
 					.globl func_name; \
 					func_name:
-#define function_epilog(func_name)      .type func_name,@function; \
+#define function_epilog(func_name)	.type func_name,@function; \
 					.size func_name,.-func_name
 
 /* We don't want the  MMU yet.
@@ -155,7 +155,9 @@
 	 * NAND U-Boot image is started from offset 0
 	 */
 	.text
+#if defined(CONFIG_440)
 	bl	reconfig_tlb0
+#endif
 	GET_GOT
 	bl	cpu_init_f	/* run low-level CPU init code	   (from Flash) */
 	bl	board_init_f
@@ -292,11 +294,13 @@ skip_debug_init:
 	mtspr	ivor7,r1	/* Floating point unavailable */
 	li	r1,0x0c00
 	mtspr	ivor8,r1	/* System call */
-	li	r1,0x1000
-	mtspr	ivor10,r1	/* Decrementer (PIT for 440) */
-	li	r1,0x1400
-	mtspr	ivor13,r1	/* Data TLB error */
+	li	r1,0x0a00
+	mtspr	ivor9,r1	/* Auxiliary Processor unavailable */
+	li	r1,0x0900
+	mtspr	ivor10,r1	/* Decrementer */
 	li	r1,0x1300
+	mtspr	ivor13,r1	/* Data TLB error */
+	li	r1,0x1400
 	mtspr	ivor14,r1	/* Instr TLB error */
 	li	r1,0x2000
 	mtspr	ivor15,r1	/* Debug */
@@ -501,11 +505,81 @@ version_string:
 	.ascii " (", __DATE__, " - ", __TIME__, ")"
 	.ascii CONFIG_IDENT_STRING, "\0"
 
-/*
- * Maybe this should be moved somewhere else because the current
- * location (0x100) is where the CriticalInput Execption should be.
- */
 	. = EXC_OFF_SYS_RESET
+	.globl	_start_of_vectors
+_start_of_vectors:
+
+/* Critical input. */
+	CRIT_EXCEPTION(0x100, CritcalInput, UnknownException)
+
+#ifdef CONFIG_440
+/* Machine check */
+	MCK_EXCEPTION(0x200, MachineCheck, MachineCheckException)
+#else
+	CRIT_EXCEPTION(0x200, MachineCheck, MachineCheckException)
+#endif /* CONFIG_440 */
+
+/* Data Storage exception. */
+	STD_EXCEPTION(0x300, DataStorage, UnknownException)
+
+/* Instruction Storage exception. */
+	STD_EXCEPTION(0x400, InstStorage, UnknownException)
+
+/* External Interrupt exception. */
+	STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
+
+/* Alignment exception. */
+	. = 0x600
+Alignment:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	mfspr	r4,DAR
+	stw	r4,_DAR(r21)
+	mfspr	r5,DSISR
+	stw	r5,_DSISR(r21)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_Alignment:
+	.long	AlignmentException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
+
+/* Program check exception */
+	. = 0x700
+ProgramCheck:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_ProgramCheck:
+	.long	ProgramCheckException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
+
+#ifdef CONFIG_440
+	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
+	STD_EXCEPTION(0x900, Decrementer, DecrementerPITException)
+	STD_EXCEPTION(0xa00, APU, UnknownException)
+#endif
+	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
+
+#ifdef CONFIG_440
+	STD_EXCEPTION(0x1300, DataTLBError, UnknownException)
+	STD_EXCEPTION(0x1400, InstructionTLBError, UnknownException)
+#else
+	STD_EXCEPTION(0x1000, PIT, DecrementerPITException)
+	STD_EXCEPTION(0x1100, InstructionTLBMiss, UnknownException)
+	STD_EXCEPTION(0x1200, DataTLBMiss, UnknownException)
+#endif
+	CRIT_EXCEPTION(0x2000, DebugBreakpoint, DebugException )
+
+	.globl	_end_of_vectors
+_end_of_vectors:
+	. = _START_OFFSET
 #endif
 	.globl	_start
 _start:
@@ -815,21 +889,21 @@ _start:
 	 */
 	lis	r3,CFG_OCM_DATA_ADDR@h	/* OCM location */
 	ori	r3,r3,CFG_OCM_DATA_ADDR@l
-	ori	r3,r3,0x8270	/* 32K Offset, 16K for Bank 1, R/W/Enable */
+	ori	r3,r3,0x0270		/* 16K for Bank 1, R/W/Enable */
 	mtdcr	ocmplb3cr1,r3		/* Set PLB Access */
 	ori	r3,r3,0x4000		/* Add 0x4000 for bank 2 */
 	mtdcr	ocmplb3cr2,r3		/* Set PLB Access */
 	isync
 
-	lis	r3,CFG_OCM_DATA_ADDR@h  /* OCM location */
+	lis	r3,CFG_OCM_DATA_ADDR@h	/* OCM location */
 	ori	r3,r3,CFG_OCM_DATA_ADDR@l
-	ori	r3,r3,0x0270            /* 16K for Bank 1, R/W/Enable */
-	mtdcr	ocmdscr1, r3            /* Set Data Side */
-	mtdcr	ocmiscr1, r3            /* Set Instruction Side */
+	ori	r3,r3,0x0270		/* 16K for Bank 1, R/W/Enable */
+	mtdcr	ocmdscr1, r3		/* Set Data Side */
+	mtdcr	ocmiscr1, r3		/* Set Instruction Side */
 	ori	r3,r3,0x4000		/* Add 0x4000 for bank 2 */
-	mtdcr	ocmdscr2, r3            /* Set Data Side */
-	mtdcr	ocmiscr2, r3            /* Set Instruction Side */
-	addis	r3,0,0x0800             /* OCM Data Parity Disable - 1 Wait State */
+	mtdcr	ocmdscr2, r3		/* Set Data Side */
+	mtdcr	ocmiscr2, r3		/* Set Instruction Side */
+	addis	r3,0,0x0800		/* OCM Data Parity Disable - 1 Wait State */
 	mtdcr	ocmdsisdpc,r3
 
 	isync
@@ -848,7 +922,7 @@ _start:
 	mtdcr	ocmdscntl, r4		/* set data-side IRAM config */
 	isync
 
-	lis	r3,CFG_OCM_DATA_ADDR@h  /* OCM location */
+	lis	r3,CFG_OCM_DATA_ADDR@h	/* OCM location */
 	ori	r3,r3,CFG_OCM_DATA_ADDR@l
 	mtdcr	ocmdsarc, r3
 	addis	r4, 0, 0xC000		/* OCM data area enabled */
@@ -857,6 +931,38 @@ _start:
 #endif /* CONFIG_405EZ */
 #endif
 
+#ifdef CONFIG_NAND_SPL
+	/*
+	 * Copy SPL from cache into internal SRAM
+	 */
+	li	r4,(CFG_NAND_BOOT_SPL_SIZE >> 2) - 1
+	mtctr	r4
+	lis	r2,CFG_NAND_BOOT_SPL_SRC@h
+	ori	r2,r2,CFG_NAND_BOOT_SPL_SRC@l
+	lis	r3,CFG_NAND_BOOT_SPL_DST@h
+	ori	r3,r3,CFG_NAND_BOOT_SPL_DST@l
+spl_loop:
+	lwzu	r4,4(r2)
+	stwu	r4,4(r3)
+	bdnz	spl_loop
+
+	/*
+	 * Jump to code in RAM
+	 */
+	bl	00f
+00:	mflr	r10
+	lis	r3,(CFG_NAND_BOOT_SPL_SRC - CFG_NAND_BOOT_SPL_DST)@h
+	ori	r3,r3,(CFG_NAND_BOOT_SPL_SRC - CFG_NAND_BOOT_SPL_DST)@l
+	sub	r10,r10,r3
+	addi	r10,r10,28
+	mtlr	r10
+	blr
+
+start_ram:
+	sync
+	isync
+#endif /* CONFIG_NAND_SPL */
+
 	/*----------------------------------------------------------------------- */
 	/* Setup temporary stack in DCACHE or OCM if needed for SDRAM SPD. */
 	/*----------------------------------------------------------------------- */
@@ -967,119 +1073,22 @@ _start:
 	stw	r0, +12(r1)		/* Save return addr (underflow vect) */
 #endif /* !(CFG_INIT_DCACHE_CS	|| !CFG_TEM_STACK_OCM) */
 
+#ifdef CONFIG_NAND_SPL
+	bl	nand_boot		/* will not return */
+#else
 	GET_GOT			/* initialize GOT access			*/
 
 	bl	cpu_init_f	/* run low-level CPU init code	   (from Flash) */
 
 	/* NEVER RETURNS! */
 	bl	board_init_f	/* run first part of init code (from Flash)	*/
+#endif /* CONFIG_NAND_SPL */
 
 #endif	/* CONFIG_405GP || CONFIG_405CR || CONFIG_405 || CONFIG_405EP */
 	/*----------------------------------------------------------------------- */
 
 
 #ifndef CONFIG_NAND_SPL
-/*****************************************************************************/
-	.globl	_start_of_vectors
-_start_of_vectors:
-
-#if 0
-/*TODO Fixup _start above so we can do this*/
-/* Critical input. */
-	CRIT_EXCEPTION(0x100, CritcalInput, CritcalInputException)
-#endif
-
-/* Machine check */
-	CRIT_EXCEPTION(0x200, MachineCheck, MachineCheckException)
-
-/* Data Storage exception. */
-	STD_EXCEPTION(0x300, DataStorage, UnknownException)
-
-/* Instruction Storage exception. */
-	STD_EXCEPTION(0x400, InstStorage, UnknownException)
-
-/* External Interrupt exception. */
-	STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
-
-/* Alignment exception. */
-	. = 0x600
-Alignment:
-	EXCEPTION_PROLOG
-	mfspr	r4,DAR
-	stw	r4,_DAR(r21)
-	mfspr	r5,DSISR
-	stw	r5,_DSISR(r21)
-	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
-
-/* Program check exception */
-	. = 0x700
-ProgramCheck:
-	EXCEPTION_PROLOG
-	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
-
-	/* No FPU on MPC8xx.  This exception is not supposed to happen.
-	*/
-	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
-
-	/* I guess we could implement decrementer, and may have
-	 * to someday for timekeeping.
-	 */
-	STD_EXCEPTION(0x900, Decrementer, timer_interrupt)
-	STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
-	STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
-	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
-	STD_EXCEPTION(0xd00, SingleStep, UnknownException)
-
-	STD_EXCEPTION(0xe00, Trap_0e, UnknownException)
-	STD_EXCEPTION(0xf00, Trap_0f, UnknownException)
-
-	/* On the MPC8xx, this is a software emulation interrupt.  It occurs
-	 * for all unimplemented and illegal instructions.
-	 */
-	STD_EXCEPTION(0x1000, PIT, PITException)
-
-	STD_EXCEPTION(0x1100, InstructionTLBMiss, UnknownException)
-	STD_EXCEPTION(0x1200, DataTLBMiss, UnknownException)
-	STD_EXCEPTION(0x1300, InstructionTLBError, UnknownException)
-	STD_EXCEPTION(0x1400, DataTLBError, UnknownException)
-
-	STD_EXCEPTION(0x1500, Reserved5, UnknownException)
-	STD_EXCEPTION(0x1600, Reserved6, UnknownException)
-	STD_EXCEPTION(0x1700, Reserved7, UnknownException)
-	STD_EXCEPTION(0x1800, Reserved8, UnknownException)
-	STD_EXCEPTION(0x1900, Reserved9, UnknownException)
-	STD_EXCEPTION(0x1a00, ReservedA, UnknownException)
-	STD_EXCEPTION(0x1b00, ReservedB, UnknownException)
-
-	STD_EXCEPTION(0x1c00, DataBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1d00, InstructionBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1e00, PeripheralBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1f00, DevPortBreakpoint, UnknownException)
-
-	CRIT_EXCEPTION(0x2000, DebugBreakpoint, DebugException )
-
-	.globl	_end_of_vectors
-_end_of_vectors:
-
-
-	. = 0x2100
-
 /*
  * This code finishes saving the registers to the exception frame
  * and jumps to the appropriate handler for the exception.
@@ -1095,28 +1104,12 @@ transfer_to_handler:
 	SAVE_4GPRS(8, r21)
 	SAVE_8GPRS(12, r21)
 	SAVE_8GPRS(24, r21)
-#if 0
-	andi.	r23,r23,MSR_PR
-	mfspr	r23,SPRG3		/* if from user, fix up tss.regs */
-	beq	2f
-	addi	r24,r1,STACK_FRAME_OVERHEAD
-	stw	r24,PT_REGS(r23)
-2:	addi	r2,r23,-TSS		/* set r2 to current */
-	tovirt(r2,r2,r23)
-#endif
 	mflr	r23
 	andi.	r24,r23,0x3f00		/* get vector offset */
 	stw	r24,TRAP(r21)
 	li	r22,0
 	stw	r22,RESULT(r21)
 	mtspr	SPRG2,r22		/* r1 is now kernel sp */
-#if 0
-	addi	r24,r2,TASK_STRUCT_SIZE /* check for kernel stack overflow */
-	cmplw	0,r1,r2
-	cmplw	1,r1,r24
-	crand	1,1,4
-	bgt	stack_ovf		/* if r2 < r1 < r2+TASK_STRUCT_SIZE */
-#endif
 	lwz	r24,0(r23)		/* virtual address of handler */
 	lwz	r23,4(r23)		/* where to go when done */
 	mtspr	SRR0,r24
@@ -1177,33 +1170,72 @@ crit_return:
 	REST_GPR(31, r1)
 	lwz	r2,_NIP(r1)	/* Restore environment */
 	lwz	r0,_MSR(r1)
-	mtspr	990,r2		/* SRR2 */
-	mtspr	991,r0		/* SRR3 */
+	mtspr	csrr0,r2
+	mtspr	csrr1,r0
 	lwz	r0,GPR0(r1)
 	lwz	r2,GPR2(r1)
 	lwz	r1,GPR1(r1)
 	SYNC
 	rfci
 
-/* Cache functions.
-*/
-invalidate_icache:
-	iccci	r0,r0			/* for 405, iccci invalidates the */
-	blr				/*   entire I cache */
+#ifdef CONFIG_440
+mck_return:
+	mfmsr	r28		/* Disable interrupts */
+	li	r4,0
+	ori	r4,r4,MSR_EE
+	andc	r28,r28,r4
+	SYNC			/* Some chip revs need this... */
+	mtmsr	r28
+	SYNC
+	lwz	r2,_CTR(r1)
+	lwz	r0,_LINK(r1)
+	mtctr	r2
+	mtlr	r0
+	lwz	r2,_XER(r1)
+	lwz	r0,_CCR(r1)
+	mtspr	XER,r2
+	mtcrf	0xFF,r0
+	REST_10GPRS(3, r1)
+	REST_10GPRS(13, r1)
+	REST_8GPRS(23, r1)
+	REST_GPR(31, r1)
+	lwz	r2,_NIP(r1)	/* Restore environment */
+	lwz	r0,_MSR(r1)
+	mtspr	mcsrr0,r2
+	mtspr	mcsrr1,r0
+	lwz	r0,GPR0(r1)
+	lwz	r2,GPR2(r1)
+	lwz	r1,GPR1(r1)
+	SYNC
+	rfmci
+#endif /* CONFIG_440 */
 
-invalidate_dcache:
-	addi	r6,0,0x0000		/* clear GPR 6 */
-	/* Do loop for # of dcache congruence classes. */
-	lis	r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@ha	/* TBS for large sized cache */
-	ori	r7, r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@l
-					/* NOTE: dccci invalidates both */
-	mtctr	r7			/* ways in the D cache */
-..dcloop:
-	dccci	0,r6			/* invalidate line */
-	addi	r6,r6, CFG_CACHELINE_SIZE /* bump to next line */
-	bdnz	..dcloop
+
+/*
+ * Cache functions.
+ *
+ * NOTE: currently the 440s run with dcache _disabled_ once relocated to DRAM,
+ * although for some cache-ralated calls stubs have to be provided to satisfy
+ * symbols resolution.
+ * Icache-related functions are used in POST framework.
+ *
+ */
+#ifdef CONFIG_440
+       .globl  dcache_disable
+       .globl  icache_disable
+       .globl  icache_enable
+dcache_disable:
+icache_disable:
+icache_enable:
 	blr
 
+	.globl	dcache_status
+	.globl	icache_status
+dcache_status:
+icache_status:
+	mr	r3,  0
+	blr
+#else
 flush_dcache:
 	addis	r9,r0,0x0002		/* set mask for EE and CE msr bits */
 	ori	r9,r9,0x8000
@@ -1282,24 +1314,13 @@ dcache_status:
 	mfdccr	r3
 	srwi	r3, r3, 31	/* >>31 => select bit 0 */
 	blr
+#endif
 
 	.globl get_pvr
 get_pvr:
 	mfspr	r3, PVR
 	blr
 
-#if !defined(CONFIG_440)
-	.globl wr_pit
-wr_pit:
-	mtspr	pit, r3
-	blr
-#endif
-
-	.globl wr_tcr
-wr_tcr:
-	mtspr	tcr, r3
-	blr
-
 /*------------------------------------------------------------------------------- */
 /* Function:	 out16 */
 /* Description:	 Output 16 bits */
@@ -1497,7 +1518,7 @@ relocate_code:
  * initialization, now running from RAM.
  */
 
-	addi	r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
+	addi	r0, r10, in_ram - _start + _START_OFFSET
 	mtlr	r0
 	blr				/* NEVER RETURNS! */
 
@@ -1567,7 +1588,7 @@ clear_bss:
 	 */
 	.globl	trap_init
 trap_init:
-	lwz	r7, GOT(_start)
+	lwz	r7, GOT(_start_of_vectors)
 	lwz	r8, GOT(_end_of_vectors)
 
 	li	r9, 0x100		/* reset vector always at 0x100 */
@@ -1587,35 +1608,48 @@ trap_init:
 	/*
 	 * relocate `hdlr' and `int_return' entries
 	 */
-	li	r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
-	li	r8, Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_MachineCheck - _start + _START_OFFSET
+	li	r8, Alignment - _start + _START_OFFSET
 2:
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
+	addi	r7, r7, 0x100		/* next exception vector */
 	cmplw	0, r7, r8
 	blt	2b
 
-	li	r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_Alignment - _start + _START_OFFSET
 	bl	trap_reloc
 
-	li	r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_ProgramCheck - _start + _START_OFFSET
 	bl	trap_reloc
 
-	li	r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
-	li	r8, SystemCall - _start + EXC_OFF_SYS_RESET
-3:
+#ifdef CONFIG_440
+	li	r7, .L_FPUnavailable - _start + _START_OFFSET
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
-	blt	3b
 
-	li	r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
-	li	r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
-4:
+	li	r7, .L_Decrementer - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_APU - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_InstructionTLBError - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_DataTLBError - _start + _START_OFFSET
+	bl	trap_reloc
+#else /* CONFIG_440 */
+	li	r7, .L_PIT - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_InstructionTLBMiss - _start + _START_OFFSET
+	bl	trap_reloc
+
+	li	r7, .L_DataTLBMiss - _start + _START_OFFSET
+	bl	trap_reloc
+#endif /* CONFIG_440 */
+
+	li	r7, .L_DebugBreakpoint - _start + _START_OFFSET
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
-	blt	4b
 
 #if !defined(CONFIG_440)
 	addi	r7,r0,0x1000		/* set ME bit (Machine Exceptions) */
@@ -1658,13 +1692,13 @@ trap_reloc:
 +----------------------------------------------------------------------------*/
 	function_prolog(dcbz_area)
 	rlwinm. r5,r4,0,27,31
-	rlwinm  r5,r4,27,5,31
-	beq     ..d_ra2
-	addi    r5,r5,0x0001
-..d_ra2:mtctr   r5
-..d_ag2:dcbz    r0,r3
-	addi    r3,r3,32
-	bdnz    ..d_ag2
+	rlwinm	r5,r4,27,5,31
+	beq	..d_ra2
+	addi	r5,r5,0x0001
+..d_ra2:mtctr	r5
+..d_ag2:dcbz	r0,r3
+	addi	r3,r3,32
+	bdnz	..d_ag2
 	sync
 	blr
 	function_epilog(dcbz_area)
@@ -1673,26 +1707,26 @@ trap_reloc:
 | dflush.  Assume 32K at vector address is cachable.
 +----------------------------------------------------------------------------*/
 	function_prolog(dflush)
-	mfmsr   r9
-	rlwinm  r8,r9,0,15,13
-	rlwinm  r8,r8,0,17,15
-	mtmsr   r8
-	addi    r3,r0,0x0000
-	mtspr   dvlim,r3
-	mfspr   r3,ivpr
-	addi    r4,r0,1024
-	mtctr   r4
+	mfmsr	r9
+	rlwinm	r8,r9,0,15,13
+	rlwinm	r8,r8,0,17,15
+	mtmsr	r8
+	addi	r3,r0,0x0000
+	mtspr	dvlim,r3
+	mfspr	r3,ivpr
+	addi	r4,r0,1024
+	mtctr	r4
 ..dflush_loop:
-	lwz     r6,0x0(r3)
-	addi    r3,r3,32
-	bdnz    ..dflush_loop
-	addi    r3,r3,-32
-	mtctr   r4
-..ag:   dcbf    r0,r3
-	addi    r3,r3,-32
-	bdnz    ..ag
+	lwz	r6,0x0(r3)
+	addi	r3,r3,32
+	bdnz	..dflush_loop
+	addi	r3,r3,-32
+	mtctr	r4
+..ag:	dcbf	r0,r3
+	addi	r3,r3,-32
+	bdnz	..ag
 	sync
-	mtmsr   r9
+	mtmsr	r9
 	blr
 	function_epilog(dflush)
 #endif /* CONFIG_440 */
@@ -1734,6 +1768,23 @@ in32:
 	lwz	3,0x0000(3)
 	blr
 
+invalidate_icache:
+	iccci	r0,r0			/* for 405, iccci invalidates the */
+	blr				/*   entire I cache */
+
+invalidate_dcache:
+	addi	r6,0,0x0000		/* clear GPR 6 */
+	/* Do loop for # of dcache congruence classes. */
+	lis	r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@ha	/* TBS for large sized cache */
+	ori	r7, r7, (CFG_DCACHE_SIZE / CFG_CACHELINE_SIZE / 2)@l
+					/* NOTE: dccci invalidates both */
+	mtctr	r7			/* ways in the D cache */
+..dcloop:
+	dccci	0,r6			/* invalidate line */
+	addi	r6,r6, CFG_CACHELINE_SIZE /* bump to next line */
+	bdnz	..dcloop
+	blr
+
 /**************************************************************************/
 /* PPC405EP specific stuff						  */
 /**************************************************************************/
diff --git a/cpu/ppc4xx/tlb.c b/cpu/ppc4xx/tlb.c
index 50344a491c..049a785495 100644
--- a/cpu/ppc4xx/tlb.c
+++ b/cpu/ppc4xx/tlb.c
@@ -36,7 +36,8 @@ typedef struct region {
 	unsigned long tlb_word2_i_value;
 } region_t;
 
-static int add_tlb_entry(unsigned long base_addr,
+static int add_tlb_entry(unsigned long phys_addr,
+			 unsigned long virt_addr,
 			 unsigned long tlb_word0_size_value,
 			 unsigned long tlb_word2_i_value)
 {
@@ -55,9 +56,9 @@ static int add_tlb_entry(unsigned long base_addr,
 		return -1;
 
 	/* Second, create the TLB entry */
-	tlb_word0_value = TLB_WORD0_EPN_ENCODE(base_addr) | TLB_WORD0_V_ENABLE |
+	tlb_word0_value = TLB_WORD0_EPN_ENCODE(virt_addr) | TLB_WORD0_V_ENABLE |
 		TLB_WORD0_TS_0 | tlb_word0_size_value;
-	tlb_word1_value = TLB_WORD1_RPN_ENCODE(base_addr) | TLB_WORD1_ERPN_ENCODE(0);
+	tlb_word1_value = TLB_WORD1_RPN_ENCODE(phys_addr) | TLB_WORD1_ERPN_ENCODE(0);
 	tlb_word2_value = TLB_WORD2_U0_DISABLE | TLB_WORD2_U1_DISABLE |
 		TLB_WORD2_U2_DISABLE | TLB_WORD2_U3_DISABLE |
 		TLB_WORD2_W_DISABLE | tlb_word2_i_value |
@@ -81,7 +82,9 @@ static int add_tlb_entry(unsigned long base_addr,
 	return 0;
 }
 
-static void program_tlb_addr(unsigned long base_addr, unsigned long mem_size,
+static void program_tlb_addr(unsigned long phys_addr,
+			     unsigned long virt_addr,
+			     unsigned long mem_size,
 			     unsigned long tlb_word2_i_value)
 {
 	int rc;
@@ -91,70 +94,86 @@ static void program_tlb_addr(unsigned long base_addr, unsigned long mem_size,
 	while (mem_size != 0) {
 		rc = 0;
 		/* Add the TLB entries in to map the region. */
-		if (((base_addr & TLB_256MB_ALIGN_MASK) == base_addr) &&
+		if (((phys_addr & TLB_256MB_ALIGN_MASK) == phys_addr) &&
 		    (mem_size >= TLB_256MB_SIZE)) {
 			/* Add a 256MB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_256MB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_256MB, tlb_i)) == 0) {
 				mem_size -= TLB_256MB_SIZE;
-				base_addr += TLB_256MB_SIZE;
+				phys_addr += TLB_256MB_SIZE;
+				virt_addr += TLB_256MB_SIZE;
 			}
-		} else if (((base_addr & TLB_16MB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_16MB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_16MB_SIZE)) {
 			/* Add a 16MB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_16MB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_16MB, tlb_i)) == 0) {
 				mem_size -= TLB_16MB_SIZE;
-				base_addr += TLB_16MB_SIZE;
+				phys_addr += TLB_16MB_SIZE;
+				virt_addr += TLB_16MB_SIZE;
 			}
-		} else if (((base_addr & TLB_1MB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_1MB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_1MB_SIZE)) {
 			/* Add a 1MB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_1MB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_1MB, tlb_i)) == 0) {
 				mem_size -= TLB_1MB_SIZE;
-				base_addr += TLB_1MB_SIZE;
+				phys_addr += TLB_1MB_SIZE;
+				virt_addr += TLB_1MB_SIZE;
 			}
-		} else if (((base_addr & TLB_256KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_256KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_256KB_SIZE)) {
 			/* Add a 256KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_256KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_256KB, tlb_i)) == 0) {
 				mem_size -= TLB_256KB_SIZE;
-				base_addr += TLB_256KB_SIZE;
+				phys_addr += TLB_256KB_SIZE;
+				virt_addr += TLB_256KB_SIZE;
 			}
-		} else if (((base_addr & TLB_64KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_64KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_64KB_SIZE)) {
 			/* Add a 64KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_64KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_64KB, tlb_i)) == 0) {
 				mem_size -= TLB_64KB_SIZE;
-				base_addr += TLB_64KB_SIZE;
+				phys_addr += TLB_64KB_SIZE;
+				virt_addr += TLB_64KB_SIZE;
 			}
-		} else if (((base_addr & TLB_16KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_16KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_16KB_SIZE)) {
 			/* Add a 16KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_16KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_16KB, tlb_i)) == 0) {
 				mem_size -= TLB_16KB_SIZE;
-				base_addr += TLB_16KB_SIZE;
+				phys_addr += TLB_16KB_SIZE;
+				virt_addr += TLB_16KB_SIZE;
 			}
-		} else if (((base_addr & TLB_4KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_4KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_4KB_SIZE)) {
 			/* Add a 4KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_4KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_4KB, tlb_i)) == 0) {
 				mem_size -= TLB_4KB_SIZE;
-				base_addr += TLB_4KB_SIZE;
+				phys_addr += TLB_4KB_SIZE;
+				virt_addr += TLB_4KB_SIZE;
 			}
-		} else if (((base_addr & TLB_1KB_ALIGN_MASK) == base_addr) &&
+		} else if (((phys_addr & TLB_1KB_ALIGN_MASK) == phys_addr) &&
 			   (mem_size >= TLB_1KB_SIZE)) {
 			/* Add a 1KB TLB entry */
-			if ((rc = add_tlb_entry(base_addr, TLB_WORD0_SIZE_1KB, tlb_i)) == 0) {
+			if ((rc = add_tlb_entry(phys_addr, virt_addr,
+						TLB_WORD0_SIZE_1KB, tlb_i)) == 0) {
 				mem_size -= TLB_1KB_SIZE;
-				base_addr += TLB_1KB_SIZE;
+				phys_addr += TLB_1KB_SIZE;
+				virt_addr += TLB_1KB_SIZE;
 			}
 		} else {
 			printf("ERROR: no TLB size exists for the base address 0x%0X.\n",
-				base_addr);
+				phys_addr);
 		}
 
 		if (rc != 0)
 			printf("ERROR: no TLB entries available for the base addr 0x%0X.\n",
-				base_addr);
+				phys_addr);
 	}
 
 	return;
@@ -166,16 +185,16 @@ static void program_tlb_addr(unsigned long base_addr, unsigned long mem_size,
  * Common usage for boards with SDRAM DIMM modules to dynamically
  * configure the TLB's for the SDRAM
  */
-void program_tlb(u32 start, u32 size, u32 tlb_word2_i_value)
+void program_tlb(u32 phys_addr, u32 virt_addr, u32 size, u32 tlb_word2_i_value)
 {
 	region_t region_array;
 
-	region_array.base = start;
+	region_array.base = phys_addr;
 	region_array.size = size;
 	region_array.tlb_word2_i_value = tlb_word2_i_value;	/* en-/disable cache */
 
 	/* Call the routine to add in the tlb entries for the memory regions */
-	program_tlb_addr(region_array.base, region_array.size,
+	program_tlb_addr(region_array.base, virt_addr, region_array.size,
 			 region_array.tlb_word2_i_value);
 
 	return;
diff --git a/cpu/ppc4xx/traps.c b/cpu/ppc4xx/traps.c
index 6aecca2db9..899cdbd1f4 100644
--- a/cpu/ppc4xx/traps.c
+++ b/cpu/ppc4xx/traps.c
@@ -36,7 +36,9 @@
 #include <command.h>
 #include <asm/processor.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+DECLARE_GLOBAL_DATA_PTR;
+
+#if defined(CONFIG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
 
@@ -45,8 +47,7 @@ extern unsigned long search_exception_table(unsigned long);
 
 /* THIS NEEDS CHANGING to use the board info structure.
  */
-#define END_OF_MEM	0x00400000
-
+#define END_OF_MEM	(gd->bd->bi_memstart + gd->bd->bi_memsize)
 
 static __inline__ void set_tsr(unsigned long val)
 {
@@ -77,7 +78,7 @@ static __inline__ unsigned long get_esr(void)
 #define ESR_DIZ 0x00400000
 #define ESR_U0F 0x00008000
 
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 extern void do_bedbug_breakpoint(struct pt_regs *);
 #endif
 
@@ -110,7 +111,7 @@ void show_regs(struct pt_regs * regs)
 {
 	int i;
 
-	printf("NIP: %08lX XER: %08lX LR: %08lX REGS: %p TRAP: %04lx DAR: %08lX\n",
+	printf("NIP: %08lX XER: %08lX LR: %08lX REGS: %p TRAP: %04lx DEAR: %08lX\n",
 	       regs->nip, regs->xer, regs->link, regs, regs->trap, regs->dar);
 	printf("MSR: %08lx EE: %01x PR: %01x FP: %01x ME: %01x IR/DR: %01x%01x\n",
 	       regs->msr, regs->msr&MSR_EE ? 1 : 0, regs->msr&MSR_PR ? 1 : 0,
@@ -120,14 +121,12 @@ void show_regs(struct pt_regs * regs)
 
 	printf("\n");
 	for (i = 0;  i < 32;  i++) {
-		if ((i % 8) == 0)
-		{
+		if ((i % 8) == 0) {
 			printf("GPR%02d: ", i);
 		}
 
 		printf("%08lX ", regs->gpr[i]);
-		if ((i % 8) == 7)
-		{
+		if ((i % 8) == 7) {
 			printf("\n");
 		}
 	}
@@ -139,13 +138,16 @@ _exception(int signr, struct pt_regs *regs)
 {
 	show_regs(regs);
 	print_backtrace((unsigned long *)regs->gpr[1]);
-	panic("Exception in kernel pc %lx signal %d",regs->nip,signr);
+	panic("Exception");
 }
 
 void
 MachineCheckException(struct pt_regs *regs)
 {
-	unsigned long fixup;
+	unsigned long fixup, val;
+#if defined(CONFIG_440EPX) || defined(CONFIG_440GRX)
+	u32 value2;
+#endif
 
 	/* Probing PCI using config cycles cause this exception
 	 * when a device is not present.  Catch it and return to
@@ -156,31 +158,137 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
 
-	printf("Machine check in kernel mode.\n");
+	printf("Machine Check Exception.\n");
 	printf("Caused by (from msr): ");
-	printf("regs %p ",regs);
-	switch( regs->msr & 0x000F0000) {
-	case (0x80000000>>12):
-		printf("Machine check signal - probably due to mm fault\n"
-		       "with mmu off\n");
+	printf("regs %p ", regs);
+
+	val = get_esr();
+
+#if !defined(CONFIG_440)
+	if (val& ESR_IMCP) {
+		printf("Instruction");
+		mtspr(ESR, val & ~ESR_IMCP);
+	} else {
+		printf("Data");
+	}
+	printf(" machine check.\n");
+
+#elif defined(CONFIG_440)
+	if (val& ESR_IMCP){
+		printf("Instruction Synchronous Machine Check exception\n");
+		mtspr(SPRN_ESR, val & ~ESR_IMCP);
+	} else {
+		val = mfspr(MCSR);
+		if (val & MCSR_IB)
+			printf("Instruction Read PLB Error\n");
+		if (val & MCSR_DRB)
+			printf("Data Read PLB Error\n");
+		if (val & MCSR_DWB)
+			printf("Data Write PLB Error\n");
+		if (val & MCSR_TLBP)
+			printf("TLB Parity Error\n");
+		if (val & MCSR_ICP){
+			/*flush_instruction_cache(); */
+			printf("I-Cache Parity Error\n");
+		}
+		if (val & MCSR_DCSP)
+			printf("D-Cache Search Parity Error\n");
+		if (val & MCSR_DCFP)
+			printf("D-Cache Flush Parity Error\n");
+		if (val & MCSR_IMPE)
+			printf("Machine Check exception is imprecise\n");
+
+		/* Clear MCSR */
+		mtspr(SPRN_MCSR, val);
+	}
+#if defined(CONFIG_440EPX) || defined(CONFIG_440GRX)
+	mfsdram(DDR0_00, val) ;
+	printf("DDR0: DDR0_00 %p\n", val);
+	val = (val >> 16) & 0xff;
+	if (val & 0x80)
+		printf("DDR0: At least one interrupt active\n");
+	if (val & 0x40)
+		printf("DDR0: DRAM initialization complete.\n");
+	if (val & 0x20)
+		printf("DDR0: Multiple uncorrectable ECC events.\n");
+	if (val & 0x10)
+		printf("DDR0: Single uncorrectable ECC event.\n");
+	if (val & 0x08)
+		printf("DDR0: Multiple correctable ECC events.\n");
+	if (val & 0x04)
+		printf("DDR0: Single correctable ECC event.\n");
+	if (val & 0x02)
+		printf("Multiple accesses outside the defined"
+		       " physical memory space detected\n");
+	if (val & 0x01)
+		printf("DDR0: Single access outside the defined"
+		       " physical memory space detected.\n");
+
+	mfsdram(DDR0_01, val);
+	val = (val >> 8) & 0x7;
+	switch (val ) {
+	case 0:
+		printf("DDR0: Write Out-of-Range command\n");
+		break;
+	case 1:
+		printf("DDR0: Read Out-of-Range command\n");
 		break;
-	case (0x80000000>>13):
-		printf("Transfer error ack signal\n");
+	case 2:
+		printf("DDR0: Masked write Out-of-Range command\n");
 		break;
-	case (0x80000000>>14):
-		printf("Data parity signal\n");
+	case 4:
+		printf("DDR0: Wrap write Out-of-Range command\n");
 		break;
-	case (0x80000000>>15):
-		printf("Address parity signal\n");
+	case 5:
+		printf("DDR0: Wrap read Out-of-Range command\n");
 		break;
 	default:
-		printf("Unknown values in msr\n");
+		mfsdram(DDR0_01, value2);
+		printf("DDR0: No DDR0 error know 0x%x %p\n", val, value2);
 	}
+	mfsdram(DDR0_23, val);
+	if ( (val >> 16) & 0xff)
+		printf("DDR0: Syndrome for correctable ECC event 0x%x\n",
+		       (val >> 16) & 0xff);
+	mfsdram(DDR0_23, val);
+	if ( (val >> 8) & 0xff)
+		printf("DDR0: Syndrome for uncorrectable ECC event 0x%x\n",
+		       (val >> 8) & 0xff);
+	mfsdram(DDR0_33, val);
+	if (val)
+		printf("DDR0: Address of command that caused an "
+		       "Out-of-Range interrupt %p\n", val);
+	mfsdram(DDR0_34, val);
+	if (val)
+		printf("DDR0: Address of uncorrectable ECC event %p\n", val);
+	mfsdram(DDR0_35, val);
+	if (val)
+		printf("DDR0: Address of uncorrectable ECC event %p\n", val);
+	mfsdram(DDR0_36, val);
+	if (val)
+		printf("DDR0: Data of uncorrectable ECC event 0x%08x\n", val);
+	mfsdram(DDR0_37, val);
+	if (val)
+		printf("DDR0: Data of uncorrectable ECC event 0x%08x\n", val);
+	mfsdram(DDR0_38, val);
+	if (val)
+		printf("DDR0: Address of correctable ECC event %p\n", val);
+	mfsdram(DDR0_39, val);
+	if (val)
+		printf("DDR0: Address of correctable ECC event %p\n", val);
+	mfsdram(DDR0_40, val);
+	if (val)
+		printf("DDR0: Data of correctable ECC event 0x%08x\n", val);
+	mfsdram(DDR0_41, val);
+	if (val)
+		printf("DDR0: Data of correctable ECC event 0x%08x\n", val);
+#endif /* CONFIG_440EPX */
+#endif /* CONFIG_440 */
 	show_regs(regs);
 	print_backtrace((unsigned long *)regs->gpr[1]);
 	panic("machine check");
@@ -189,7 +297,7 @@ MachineCheckException(struct pt_regs *regs)
 void
 AlignmentException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -204,7 +312,7 @@ ProgramCheckException(struct pt_regs *regs)
 {
 	long esr_val;
 
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -224,7 +332,7 @@ ProgramCheckException(struct pt_regs *regs)
 }
 
 void
-PITException(struct pt_regs *regs)
+DecrementerPITException(struct pt_regs *regs)
 {
 	/*
 	 * Reset PIT interrupt
@@ -241,7 +349,7 @@ PITException(struct pt_regs *regs)
 void
 UnknownException(struct pt_regs *regs)
 {
-#if (CONFIG_COMMANDS & CFG_CMD_KGDB)
+#if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
@@ -256,7 +364,7 @@ DebugException(struct pt_regs *regs)
 {
 	printf("Debugger trap at @ %lx\n", regs->nip );
 	show_regs(regs);
-#if (CONFIG_COMMANDS & CFG_CMD_BEDBUG)
+#if defined(CONFIG_CMD_BEDBUG)
 	do_bedbug_breakpoint( regs );
 #endif
 }
@@ -272,17 +380,17 @@ addr_probe(uint *addr)
 
 	__asm__ __volatile__(			\
 		"1:	lwz %0,0(%1)\n"		\
-						"	eieio\n"		\
-						"	li %0,0\n"		\
-						"2:\n"				\
-						".section .fixup,\"ax\"\n"	\
-						"3:	li %0,-1\n"		\
-						"	b 2b\n"			\
-						".section __ex_table,\"a\"\n"	\
-						"	.align 2\n"		\
-						"	.long 1b,3b\n"		\
-						".text"				\
-						: "=r" (retval) : "r"(addr));
+		"	eieio\n"		\
+		"	li %0,0\n"		\
+		"2:\n"				\
+		".section .fixup,\"ax\"\n"	\
+		"3:	li %0,-1\n"		\
+		"	b 2b\n"			\
+		".section __ex_table,\"a\"\n"	\
+		"	.align 2\n"		\
+		"	.long 1b,3b\n"		\
+		".text"				\
+		: "=r" (retval) : "r"(addr));
 
 	return (retval);
 #endif
diff --git a/cpu/ppc4xx/usb.c b/cpu/ppc4xx/usb.c
new file mode 100644
index 0000000000..2837b37c58
--- /dev/null
+++ b/cpu/ppc4xx/usb.c
@@ -0,0 +1,50 @@
+/*
+ * (C) Copyright 2007
+ * Markus Klotzbuecher, DENX Software Engineering <mk@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#if defined(CONFIG_USB_OHCI_NEW) && defined(CFG_USB_OHCI_CPU_INIT)
+
+#include "usbdev.h"
+
+int usb_cpu_init()
+{
+
+#if defined(CONFIG_440EP) || defined(CONFIG_440EPX)
+	usb_dev_init();
+#endif
+
+	return 0;
+}
+
+int usb_cpu_stop()
+{
+	return 0;
+}
+
+int usb_cpu_init_fail()
+{
+	return 0;
+}
+
+#endif /* defined(CONFIG_USB_OHCI) && defined(CFG_USB_OHCI_CPU_INIT) */
diff --git a/cpu/ppc4xx/usbdev.c b/cpu/ppc4xx/usbdev.c
index 6140d2a908..5924a6cb84 100644
--- a/cpu/ppc4xx/usbdev.c
+++ b/cpu/ppc4xx/usbdev.c
@@ -3,7 +3,7 @@
 #include <common.h>
 #include <asm/processor.h>
 
-#if (defined(CONFIG_440EP) || defined(CONFIG_440EPX)) && (CONFIG_COMMANDS & CFG_CMD_USB)
+#if (defined(CONFIG_440EP) || defined(CONFIG_440EPX)) && defined(CONFIG_CMD_USB)
 
 #include <usb.h>
 #include "usbdev.h"
diff --git a/cpu/pxa/Makefile b/cpu/pxa/Makefile
index cded7ffd35..8b4367e205 100644
--- a/cpu/pxa/Makefile
+++ b/cpu/pxa/Makefile
@@ -26,7 +26,7 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(CPU).a
 
 START	= start.o
-COBJS	= serial.o interrupts.o cpu.o i2c.o pxafb.o mmc.o
+COBJS	= serial.o interrupts.o cpu.o i2c.o pxafb.o mmc.o usb.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/pxa/usb.c b/cpu/pxa/usb.c
new file mode 100644
index 0000000000..65f457fe59
--- /dev/null
+++ b/cpu/pxa/usb.c
@@ -0,0 +1,79 @@
+/*
+ * (C) Copyright 2006
+ * Markus Klotzbuecher, DENX Software Engineering <mk@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#if defined(CONFIG_USB_OHCI_NEW) && defined(CFG_USB_OHCI_CPU_INIT)
+# if defined(CONFIG_CPU_MONAHANS) || defined(CONFIG_PXA27X)
+
+#include <asm/arch/pxa-regs.h>
+
+int usb_cpu_init()
+{
+#if defined(CONFIG_CPU_MONAHANS)
+	/* Enable USB host clock. */
+	CKENA |= (CKENA_2_USBHOST |  CKENA_20_UDC);
+	udelay(100);
+#endif
+#if defined(CONFIG_PXA27X)
+	/* Enable USB host clock. */
+	CKEN |= CKEN10_USBHOST;
+#endif
+
+#if defined(CONFIG_CPU_MONAHANS)
+	/* Configure Port 2 for Host (USB Client Registers) */
+	UP2OCR = 0x3000c;
+#endif
+
+	UHCHR |= UHCHR_FHR;
+	wait_ms(11);
+	UHCHR &= ~UHCHR_FHR;
+
+	UHCHR |= UHCHR_FSBIR;
+	while (UHCHR & UHCHR_FSBIR)
+		udelay(1);
+
+#if defined(CONFIG_CPU_MONAHANS)
+	UHCHR &= ~UHCHR_SSEP0;
+#endif
+#if defined(CONFIG_PXA27X)
+	UHCHR &= ~UHCHR_SSEP2;
+#endif
+	UHCHR &= ~UHCHR_SSEP1;
+	UHCHR &= ~UHCHR_SSE;
+
+	return 0;
+}
+
+int usb_cpu_stop()
+{
+	return 0;
+}
+
+int usb_cpu_init_fail()
+{
+	return 0;
+}
+
+# endif /* defined(CONFIG_CPU_MONAHANS) || defined(CONFIG_PXA27X) */
+#endif /* defined(CONFIG_USB_OHCI) && defined(CFG_USB_OHCI_CPU_INIT) */