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

116 files changed, 11113 insertions, 2810 deletions

diff --git a/cpu/74xx_7xx/cpu.c b/cpu/74xx_7xx/cpu.c
index f4e5fc5..9c8998b 100644
--- a/cpu/74xx_7xx/cpu.c
+++ b/cpu/74xx_7xx/cpu.c
@@ -44,6 +44,10 @@
 #include <74xx_7xx.h>
 #include <asm/cache.h>
 
+#if defined(CONFIG_OF_FLAT_TREE)
+#include <ft_build.h>
+#endif
+
 #ifdef CONFIG_AMIGAONEG3SE
 #include "../board/MAI/AmigaOneG3SE/via686.h"
 #include "../board/MAI/AmigaOneG3SE/memio.h"
@@ -101,6 +105,10 @@ get_cpu_type(void)
 		type = CPU_7457;
 		break;
 
+	case 0x8003:
+		type = CPU_7447A;
+		break;
+
 	case 0x8004:
 		type = CPU_7448;
 		break;
@@ -156,6 +164,10 @@ int checkcpu (void)
 		str = "MPC7410";
 		break;
 
+	case CPU_7447A:
+		str = "MPC7447A";
+		break;
+
 	case CPU_7448:
 		str = "MPC7448";
 		break;
@@ -264,20 +276,19 @@ do_reset (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 /*
  * For the 7400 the TB clock runs at 1/4 the cpu bus speed.
  */
-#ifdef CONFIG_AMIGAONEG3SE
+#if defined(CONFIG_AMIGAONEG3SE) || defined(CFG_CONFIG_BUS_CLK)
 unsigned long get_tbclk(void)
 {
 	return (gd->bus_clk / 4);
 }
-#else	/* ! CONFIG_AMIGAONEG3SE */
+#else	/* ! CONFIG_AMIGAONEG3SE and !CFG_CONFIG_BUS_CLK*/
 
 unsigned long get_tbclk (void)
 {
 	return CFG_BUS_HZ / 4;
 }
-#endif	/* CONFIG_AMIGAONEG3SE */
+#endif	/* CONFIG_AMIGAONEG3SE or CFG_CONFIG_BUS_CLK*/
 /* ------------------------------------------------------------------------- */
-
 #if defined(CONFIG_WATCHDOG)
 #if !defined(CONFIG_PCIPPC2) && !defined(CONFIG_BAB7xx)
 void
@@ -289,3 +300,30 @@ watchdog_reset(void)
 #endif	/* CONFIG_WATCHDOG */
 
 /* ------------------------------------------------------------------------- */
+
+#ifdef CONFIG_OF_FLAT_TREE
+void
+ft_cpu_setup (void *blob, bd_t *bd)
+{
+	u32 *p;
+	ulong clock;
+	int len;
+
+	clock = bd->bi_busfreq;
+
+	p = ft_get_prop (blob, "/cpus/" OF_CPU "/bus-frequency", &len);
+	if (p != NULL)
+		*p = cpu_to_be32 (clock);
+
+#if defined(CONFIG_TSI108_ETH)
+	p = ft_get_prop (blob, "/" OF_TSI "/ethernet@6200/address", &len);
+		memcpy (p, bd->bi_enetaddr, 6);
+#endif
+
+#if defined(CONFIG_HAS_ETH1)
+	p = ft_get_prop (blob, "/" OF_TSI "/ethernet@6600/address", &len);
+		memcpy (p, bd->bi_enet1addr, 6);
+#endif
+}
+#endif
+/* ------------------------------------------------------------------------- */
diff --git a/cpu/74xx_7xx/cpu_init.c b/cpu/74xx_7xx/cpu_init.c
index e02a4cc..1dd1b2c 100644
--- a/cpu/74xx_7xx/cpu_init.c
+++ b/cpu/74xx_7xx/cpu_init.c
@@ -43,6 +43,7 @@ cpu_init_f (void)
 	case CPU_7450:
 	case CPU_7455:
 	case CPU_7457:
+	case CPU_7447A:
 	case CPU_7448:
 		/* enable the timebase bit in HID0 */
 		set_hid0(get_hid0() | 0x4000000);
diff --git a/cpu/74xx_7xx/speed.c b/cpu/74xx_7xx/speed.c
index d1800ed..d8c40ce 100644
--- a/cpu/74xx_7xx/speed.c
+++ b/cpu/74xx_7xx/speed.c
@@ -31,6 +31,8 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+extern unsigned long get_board_bus_clk (void);
+
 static const int hid1_multipliers_x_10[] = {
 	25,	/* 0000 - 2.5x */
 	75,	/* 0001 - 7.5x */
@@ -50,6 +52,42 @@ static const int hid1_multipliers_x_10[] = {
 	0	/* 1111 - off */
 };
 
+/* PLL_CFG[0:4] table for cpu 7448/7447A/7455/7457 */
+static const int hid1_74xx_multipliers_x_10[] = {
+	115,	/* 00000 - 11.5x  */
+	170,	/* 00001 - 17x    */
+	75,	/* 00010 -  7.5x  */
+	150,	/* 00011 - 15x    */
+	70,	/* 00100 -  7x    */
+	180,	/* 00101 - 18x    */
+	10,	/* 00110 - bypass */
+	200,	/* 00111 - 20x    */
+	20,	/* 01000 -  2x    */
+	210,	/* 01001 - 21x    */
+	65,	/* 01010 -  6.5x  */
+	130,	/* 01011 - 13x    */
+	85,	/* 01100 -  8.5x  */
+	240,	/* 01101 - 24x    */
+	95,	/* 01110 -  9.5x  */
+	90,	/* 01111 -  9x    */
+	30,	/* 10000 -  3x    */
+	105,	/* 10001 - 10.5x  */
+	55,	/* 10010 -  5.5x  */
+	110,	/* 10011 - 11x    */
+	40,	/* 10100 -  4x    */
+	100,	/* 10101 - 10x    */
+	50,	/* 10110 -  5x    */
+	120,	/* 10111 - 12x    */
+	80,	/* 11000 -  8x    */
+	140,	/* 11001 - 14x    */
+	60,	/* 11010 -  6x    */
+	160,	/* 11011 - 16x    */
+	135,	/* 11100 - 13.5x  */
+	280,	/* 11101 - 28x    */
+	0,	/* 11110 - off    */
+	125	/* 11111 - 12.5x  */
+};
+
 static const int hid1_fx_multipliers_x_10[] = {
 	00,	/* 0000 - off */
 	00,	/* 0001 - off */
@@ -89,22 +127,30 @@ int get_clocks (void)
 {
 	ulong clock = 0;
 
+#ifdef CFG_BUS_CLK
+	gd->bus_clk = CFG_BUS_CLK;	/* bus clock is a fixed frequency */
+#else
+	gd->bus_clk = get_board_bus_clk ();	/* bus clock is configurable */
+#endif
+
 	/* calculate the clock frequency based upon the CPU type */
 	switch (get_cpu_type()) {
+	case CPU_7447A:
 	case CPU_7448:
 	case CPU_7455:
 	case CPU_7457:
 		/*
-		 * It is assumed that the PLL_EXT line is zero.
 		 * Make sure division is done before multiplication to prevent 32-bit
 		 * arithmetic overflows which will cause a negative number
 		 */
-		clock = (CFG_BUS_CLK / 10) * hid1_multipliers_x_10[(get_hid1 () >> 13) & 0xF];
+		clock = (gd->bus_clk / 10) *
+			hid1_74xx_multipliers_x_10[(get_hid1 () >> 12) & 0x1F];
 		break;
 
 	case CPU_750GX:
 	case CPU_750FX:
-		clock = CFG_BUS_CLK * hid1_fx_multipliers_x_10[get_hid1 () >> 27] / 10;
+		clock = gd->bus_clk *
+			hid1_fx_multipliers_x_10[get_hid1 () >> 27] / 10;
 		break;
 
 	case CPU_7450:
@@ -121,7 +167,8 @@ int get_clocks (void)
 		 * Make sure division is done before multiplication to prevent 32-bit
 		 * arithmetic overflows which will cause a negative number
 		 */
-		clock = (CFG_BUS_CLK / 10) * hid1_multipliers_x_10[get_hid1 () >> 28];
+		clock = (gd->bus_clk / 10) *
+			hid1_multipliers_x_10[get_hid1 () >> 28];
 		break;
 
 	case CPU_UNKNOWN:
@@ -131,7 +178,6 @@ int get_clocks (void)
 	}
 
 	gd->cpu_clk = clock;
-	gd->bus_clk = CFG_BUS_CLK;
 
 	return (0);
 }
diff --git a/cpu/at32ap/Makefile b/cpu/at32ap/Makefile
index f62ec8b..f69b1f3 100644
--- a/cpu/at32ap/Makefile
+++ b/cpu/at32ap/Makefile
@@ -30,7 +30,7 @@ LIB	:= $(obj)lib$(CPU).a
 START	:= start.o
 SOBJS	:= entry.o
 COBJS	:= cpu.o hsdramc.o exception.o cache.o
-COBJS	+= interrupts.o device.o pm.o pio.o
+COBJS	+= interrupts.o pio.o atmel_mci.o
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
 START	:= $(addprefix $(obj),$(START))
diff --git a/cpu/at32ap/at32ap7000/Makefile b/cpu/at32ap/at32ap7000/Makefile
index 2ed74d2..d276712 100644
--- a/cpu/at32ap/at32ap7000/Makefile
+++ b/cpu/at32ap/at32ap7000/Makefile
@@ -24,7 +24,7 @@ include $(TOPDIR)/config.mk
 
 LIB	:= $(obj)lib$(SOC).a
 
-COBJS	:= hebi.o devices.o
+COBJS	:= gpio.o
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
 
diff --git a/cpu/at32ap/at32ap7000/devices.c b/cpu/at32ap/at32ap7000/devices.c
deleted file mode 100644
index 8b216e9..0000000
--- a/cpu/at32ap/at32ap7000/devices.c
+++ /dev/null
@@ -1,448 +0,0 @@
-/*
- * Copyright (C) 2006 Atmel Corporation
- *
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- */
-#include <common.h>
-
-#include <asm/arch/memory-map.h>
-#include <asm/arch/platform.h>
-
-#include "../sm.h"
-
-#define ARRAY_SIZE(x)	(sizeof(x) / sizeof((x)[0]))
-
-const struct clock_domain chip_clock[] = {
-	[CLOCK_CPU] = {
-		.reg	= SM_PM_CPU_MASK,
-		.id	= CLOCK_CPU,
-		.bridge	= NO_DEVICE,
-	},
-	[CLOCK_HSB] = {
-		.reg	= SM_PM_HSB_MASK,
-		.id	= CLOCK_HSB,
-		.bridge	= NO_DEVICE,
-	},
-	[CLOCK_PBA] = {
-		.reg	= SM_PM_PBA_MASK,
-		.id	= CLOCK_PBA,
-		.bridge	= DEVICE_PBA_BRIDGE,
-	},
-	[CLOCK_PBB] = {
-		.reg	= SM_PM_PBB_MASK,
-		.id	= CLOCK_PBB,
-		.bridge	= DEVICE_PBB_BRIDGE,
-	},
-};
-
-static const struct resource hebi_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 0 },
-		},
-	}, {
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBB, 13 },
-		},
-	}, {
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBB, 14 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio	= { 27, DEVICE_PIOE, GPIO_FUNC_A, 0 },
-		},
-	},
-};
-static const struct resource pba_bridge_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 1 },
-		}
-	}, {
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			/* HSB-HSB Bridge */
-			.clock	= { CLOCK_HSB, 4 },
-		},
-	},
-};
-static const struct resource pbb_bridge_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 2 },
-		},
-	},
-};
-static const struct resource hramc_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 3 },
-		},
-	},
-};
-static const struct resource pioa_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 10 },
-		},
-	},
-};
-static const struct resource piob_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 11 },
-		},
-	},
-};
-static const struct resource pioc_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 12 },
-		},
-	},
-};
-static const struct resource piod_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 13 },
-		},
-	},
-};
-static const struct resource pioe_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 14 },
-		},
-	},
-};
-static const struct resource sm_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBB, 0 },
-		},
-	},
-};
-static const struct resource intc_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock = { CLOCK_PBB, 1 },
-		},
-	},
-};
-static const struct resource hmatrix_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock = { CLOCK_PBB, 2 },
-		},
-	},
-};
-#if defined(CFG_HPDC)
-static const struct resource hpdc_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 16 },
-		},
-	},
-};
-#endif
-#if defined(CFG_MACB0)
-static const struct resource macb0_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 8 },
-		},
-	}, {
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBB, 6 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio	= { 19, DEVICE_PIOC, GPIO_FUNC_A, 0 },
-		},
-	},
-};
-#endif
-#if defined(CFG_MACB1)
-static const struct resource macb1_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 9 },
-		},
-	}, {
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBB, 7 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio	= { 12, DEVICE_PIOC, GPIO_FUNC_B, 19 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio	= { 14, DEVICE_PIOD, GPIO_FUNC_B, 2 },
-		},
-	},
-};
-#endif
-#if defined(CFG_LCDC)
-static const struct resource lcdc_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 7 },
-		},
-	},
-};
-#endif
-#if defined(CFG_USART0)
-static const struct resource usart0_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 3 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio = { 2, DEVICE_PIOA, GPIO_FUNC_B, 8 },
-		},
-	},
-};
-#endif
-#if defined(CFG_USART1)
-static const struct resource usart1_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 4 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio = { 2, DEVICE_PIOA, GPIO_FUNC_A, 17 },
-		},
-	},
-};
-#endif
-#if defined(CFG_USART2)
-static const struct resource usart2_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 5 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio = { 2, DEVICE_PIOB, GPIO_FUNC_B, 26 },
-		},
-	},
-};
-#endif
-#if defined(CFG_USART3)
-static const struct resource usart3_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBA, 6 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio = { 2, DEVICE_PIOB, GPIO_FUNC_B, 17 },
-		},
-	},
-};
-#endif
-#if defined(CFG_MMCI)
-static const struct resource mmci_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_PBB, 9 },
-		},
-	}, {
-		.type	= RESOURCE_GPIO,
-		.u	= {
-			.gpio = { 6, DEVICE_PIOA, GPIO_FUNC_A, 10 },
-		},
-	},
-};
-#endif
-#if defined(CFG_DMAC)
-static const struct resource dmac_resource[] = {
-	{
-		.type	= RESOURCE_CLOCK,
-		.u	= {
-			.clock	= { CLOCK_HSB, 10 },
-		},
-	},
-};
-#endif
-
-const struct device chip_device[] = {
-	[DEVICE_HEBI] = {
-		.regs		= (void *)HSMC_BASE,
-		.nr_resources	= ARRAY_SIZE(hebi_resource),
-		.resource	= hebi_resource,
-	},
-	[DEVICE_PBA_BRIDGE] = {
-		.nr_resources	= ARRAY_SIZE(pba_bridge_resource),
-		.resource	= pba_bridge_resource,
-	},
-	[DEVICE_PBB_BRIDGE] = {
-		.nr_resources	= ARRAY_SIZE(pbb_bridge_resource),
-		.resource	= pbb_bridge_resource,
-	},
-	[DEVICE_HRAMC] = {
-		.nr_resources	= ARRAY_SIZE(hramc_resource),
-		.resource	= hramc_resource,
-	},
-	[DEVICE_PIOA] = {
-		.regs		= (void *)PIOA_BASE,
-		.nr_resources	= ARRAY_SIZE(pioa_resource),
-		.resource	= pioa_resource,
-	},
-	[DEVICE_PIOB] = {
-		.regs		= (void *)PIOB_BASE,
-		.nr_resources	= ARRAY_SIZE(piob_resource),
-		.resource	= piob_resource,
-	},
-	[DEVICE_PIOC] = {
-		.regs		= (void *)PIOC_BASE,
-		.nr_resources	= ARRAY_SIZE(pioc_resource),
-		.resource	= pioc_resource,
-	},
-	[DEVICE_PIOD] = {
-		.regs		= (void *)PIOD_BASE,
-		.nr_resources	= ARRAY_SIZE(piod_resource),
-		.resource	= piod_resource,
-	},
-	[DEVICE_PIOE] = {
-		.regs		= (void *)PIOE_BASE,
-		.nr_resources	= ARRAY_SIZE(pioe_resource),
-		.resource	= pioe_resource,
-	},
-	[DEVICE_SM] = {
-		.regs		= (void *)SM_BASE,
-		.nr_resources	= ARRAY_SIZE(sm_resource),
-		.resource	= sm_resource,
-	},
-	[DEVICE_INTC] = {
-		.regs		= (void *)INTC_BASE,
-		.nr_resources	= ARRAY_SIZE(intc_resource),
-		.resource	= intc_resource,
-	},
-	[DEVICE_HMATRIX] = {
-		.regs		= (void *)HMATRIX_BASE,
-		.nr_resources	= ARRAY_SIZE(hmatrix_resource),
-		.resource	= hmatrix_resource,
-	},
-#if defined(CFG_HPDC)
-	[DEVICE_HPDC] = {
-		.nr_resources	= ARRAY_SIZE(hpdc_resource),
-		.resource	= hpdc_resource,
-	},
-#endif
-#if defined(CFG_MACB0)
-	[DEVICE_MACB0] = {
-		.regs		= (void *)MACB0_BASE,
-		.nr_resources	= ARRAY_SIZE(macb0_resource),
-		.resource	= macb0_resource,
-	},
-#endif
-#if defined(CFG_MACB1)
-	[DEVICE_MACB1] = {
-		.regs		= (void *)MACB1_BASE,
-		.nr_resources	= ARRAY_SIZE(macb1_resource),
-		.resource	= macb1_resource,
-	},
-#endif
-#if defined(CFG_LCDC)
-	[DEVICE_LCDC] = {
-		.nr_resources	= ARRAY_SIZE(lcdc_resource),
-		.resource	= lcdc_resource,
-	},
-#endif
-#if defined(CFG_USART0)
-	[DEVICE_USART0] = {
-		.regs		= (void *)USART0_BASE,
-		.nr_resources	= ARRAY_SIZE(usart0_resource),
-		.resource	= usart0_resource,
-	},
-#endif
-#if defined(CFG_USART1)
-	[DEVICE_USART1] = {
-		.regs		= (void *)USART1_BASE,
-		.nr_resources	= ARRAY_SIZE(usart1_resource),
-		.resource	= usart1_resource,
-	},
-#endif
-#if defined(CFG_USART2)
-	[DEVICE_USART2] = {
-		.regs		= (void *)USART2_BASE,
-		.nr_resources	= ARRAY_SIZE(usart2_resource),
-		.resource	= usart2_resource,
-	},
-#endif
-#if defined(CFG_USART3)
-	[DEVICE_USART3] = {
-		.regs		= (void *)USART3_BASE,
-		.nr_resources	= ARRAY_SIZE(usart3_resource),
-		.resource	= usart3_resource,
-	},
-#endif
-#if defined(CFG_MMCI)
-	[DEVICE_MMCI] = {
-		.regs		= (void *)MMCI_BASE,
-		.nr_resources	= ARRAY_SIZE(mmci_resource),
-		.resource	= mmci_resource,
-	},
-#endif
-#if defined(CFG_DMAC)
-	[DEVICE_DMAC] = {
-		.regs		= (void *)DMAC_BASE,
-		.nr_resources	= ARRAY_SIZE(dmac_resource),
-		.resource	= dmac_resource,
-	},
-#endif
-};
diff --git a/cpu/at32ap/at32ap7000/gpio.c b/cpu/at32ap/at32ap7000/gpio.c
new file mode 100644
index 0000000..52f5372
--- /dev/null
+++ b/cpu/at32ap/at32ap7000/gpio.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2006 Atmel Corporation
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+#include <common.h>
+
+#include <asm/arch/gpio.h>
+
+/*
+ * Lots of small functions here. We depend on --gc-sections getting
+ * rid of the ones we don't need.
+ */
+void gpio_enable_ebi(void)
+{
+#ifdef CFG_HSDRAMC
+#ifndef CFG_SDRAM_16BIT
+	gpio_select_periph_A(GPIO_PIN_PE0, 0);
+	gpio_select_periph_A(GPIO_PIN_PE1, 0);
+	gpio_select_periph_A(GPIO_PIN_PE2, 0);
+	gpio_select_periph_A(GPIO_PIN_PE3, 0);
+	gpio_select_periph_A(GPIO_PIN_PE4, 0);
+	gpio_select_periph_A(GPIO_PIN_PE5, 0);
+	gpio_select_periph_A(GPIO_PIN_PE6, 0);
+	gpio_select_periph_A(GPIO_PIN_PE7, 0);
+	gpio_select_periph_A(GPIO_PIN_PE8, 0);
+	gpio_select_periph_A(GPIO_PIN_PE9, 0);
+	gpio_select_periph_A(GPIO_PIN_PE10, 0);
+	gpio_select_periph_A(GPIO_PIN_PE11, 0);
+	gpio_select_periph_A(GPIO_PIN_PE12, 0);
+	gpio_select_periph_A(GPIO_PIN_PE13, 0);
+	gpio_select_periph_A(GPIO_PIN_PE14, 0);
+	gpio_select_periph_A(GPIO_PIN_PE15, 0);
+#endif
+	gpio_select_periph_A(GPIO_PIN_PE26, 0);
+#endif
+}
+
+void gpio_enable_usart0(void)
+{
+	gpio_select_periph_B(GPIO_PIN_PA8, 0);
+	gpio_select_periph_B(GPIO_PIN_PA9, 0);
+}
+
+void gpio_enable_usart1(void)
+{
+	gpio_select_periph_A(GPIO_PIN_PA17, 0);
+	gpio_select_periph_A(GPIO_PIN_PA18, 0);
+}
+
+void gpio_enable_usart2(void)
+{
+	gpio_select_periph_B(GPIO_PIN_PB26, 0);
+	gpio_select_periph_B(GPIO_PIN_PB27, 0);
+}
+
+void gpio_enable_usart3(void)
+{
+	gpio_select_periph_B(GPIO_PIN_PB18, 0);
+	gpio_select_periph_B(GPIO_PIN_PB19, 0);
+}
+
+void gpio_enable_macb0(void)
+{
+	gpio_select_periph_A(GPIO_PIN_PC3,  0);	/* TXD0	*/
+	gpio_select_periph_A(GPIO_PIN_PC4,  0);	/* TXD1	*/
+	gpio_select_periph_A(GPIO_PIN_PC7,  0);	/* TXEN	*/
+	gpio_select_periph_A(GPIO_PIN_PC8,  0);	/* TXCK */
+	gpio_select_periph_A(GPIO_PIN_PC9,  0);	/* RXD0	*/
+	gpio_select_periph_A(GPIO_PIN_PC10, 0);	/* RXD1	*/
+	gpio_select_periph_A(GPIO_PIN_PC13, 0);	/* RXER	*/
+	gpio_select_periph_A(GPIO_PIN_PC15, 0);	/* RXDV	*/
+	gpio_select_periph_A(GPIO_PIN_PC16, 0);	/* MDC	*/
+	gpio_select_periph_A(GPIO_PIN_PC17, 0);	/* MDIO	*/
+#if !defined(CONFIG_RMII)
+	gpio_select_periph_A(GPIO_PIN_PC0,  0);	/* COL	*/
+	gpio_select_periph_A(GPIO_PIN_PC1,  0);	/* CRS	*/
+	gpio_select_periph_A(GPIO_PIN_PC2,  0);	/* TXER	*/
+	gpio_select_periph_A(GPIO_PIN_PC5,  0);	/* TXD2	*/
+	gpio_select_periph_A(GPIO_PIN_PC6,  0);	/* TXD3 */
+	gpio_select_periph_A(GPIO_PIN_PC11, 0);	/* RXD2	*/
+	gpio_select_periph_A(GPIO_PIN_PC12, 0);	/* RXD3	*/
+	gpio_select_periph_A(GPIO_PIN_PC14, 0);	/* RXCK	*/
+	gpio_select_periph_A(GPIO_PIN_PC18, 0);	/* SPD	*/
+#endif
+}
+
+void gpio_enable_macb1(void)
+{
+	gpio_select_periph_B(GPIO_PIN_PD13, 0);	/* TXD0	*/
+	gpio_select_periph_B(GPIO_PIN_PD14, 0);	/* TXD1	*/
+	gpio_select_periph_B(GPIO_PIN_PD11, 0);	/* TXEN	*/
+	gpio_select_periph_B(GPIO_PIN_PD12, 0);	/* TXCK */
+	gpio_select_periph_B(GPIO_PIN_PD10, 0);	/* RXD0	*/
+	gpio_select_periph_B(GPIO_PIN_PD6,  0);	/* RXD1	*/
+	gpio_select_periph_B(GPIO_PIN_PD5,  0);	/* RXER	*/
+	gpio_select_periph_B(GPIO_PIN_PD4,  0);	/* RXDV	*/
+	gpio_select_periph_B(GPIO_PIN_PD3,  0);	/* MDC	*/
+	gpio_select_periph_B(GPIO_PIN_PD2,  0);	/* MDIO	*/
+#if !defined(CONFIG_RMII)
+	gpio_select_periph_B(GPIO_PIN_PC19, 0);	/* COL	*/
+	gpio_select_periph_B(GPIO_PIN_PC23, 0);	/* CRS	*/
+	gpio_select_periph_B(GPIO_PIN_PC26, 0);	/* TXER	*/
+	gpio_select_periph_B(GPIO_PIN_PC27, 0);	/* TXD2	*/
+	gpio_select_periph_B(GPIO_PIN_PC28, 0);	/* TXD3 */
+	gpio_select_periph_B(GPIO_PIN_PC29, 0);	/* RXD2	*/
+	gpio_select_periph_B(GPIO_PIN_PC30, 0);	/* RXD3	*/
+	gpio_select_periph_B(GPIO_PIN_PC24, 0);	/* RXCK	*/
+	gpio_select_periph_B(GPIO_PIN_PD15, 0);	/* SPD	*/
+#endif
+}
+
+void gpio_enable_mmci(void)
+{
+	gpio_select_periph_A(GPIO_PIN_PA10, 0);	/* CLK	 */
+	gpio_select_periph_A(GPIO_PIN_PA11, 0);	/* CMD	 */
+	gpio_select_periph_A(GPIO_PIN_PA12, 0);	/* DATA0 */
+	gpio_select_periph_A(GPIO_PIN_PA13, 0);	/* DATA1 */
+	gpio_select_periph_A(GPIO_PIN_PA14, 0);	/* DATA2 */
+	gpio_select_periph_A(GPIO_PIN_PA15, 0);	/* DATA3 */
+}
diff --git a/cpu/at32ap/atmel_mci.c b/cpu/at32ap/atmel_mci.c
new file mode 100644
index 0000000..9f62c0f
--- /dev/null
+++ b/cpu/at32ap/atmel_mci.c
@@ -0,0 +1,477 @@
+/*
+ * Copyright (C) 2004-2006 Atmel Corporation
+ *
+ * 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_MMC
+
+#include <part.h>
+#include <mmc.h>
+
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/byteorder.h>
+#include <asm/arch/clk.h>
+#include <asm/arch/memory-map.h>
+
+#include "atmel_mci.h"
+
+#ifdef DEBUG
+#define pr_debug(fmt, args...) printf(fmt, ##args)
+#else
+#define pr_debug(...) do { } while(0)
+#endif
+
+#ifndef CFG_MMC_CLK_OD
+#define CFG_MMC_CLK_OD		150000
+#endif
+
+#ifndef CFG_MMC_CLK_PP
+#define CFG_MMC_CLK_PP		5000000
+#endif
+
+#ifndef CFG_MMC_OP_COND
+#define CFG_MMC_OP_COND		0x00100000
+#endif
+
+#define MMC_DEFAULT_BLKLEN	512
+#define MMC_DEFAULT_RCA		1
+
+static unsigned int mmc_rca;
+static block_dev_desc_t mmc_blkdev;
+
+block_dev_desc_t *mmc_get_dev(int dev)
+{
+	return &mmc_blkdev;
+}
+
+static void mci_set_mode(unsigned long hz, unsigned long blklen)
+{
+	unsigned long bus_hz;
+	unsigned long clkdiv;
+
+	bus_hz = get_mci_clk_rate();
+	clkdiv = (bus_hz / hz) / 2 - 1;
+
+	pr_debug("mmc: setting clock %lu Hz, block size %lu\n",
+		 hz, blklen);
+
+	if (clkdiv & ~255UL) {
+		clkdiv = 255;
+		printf("mmc: clock %lu too low; setting CLKDIV to 255\n",
+			hz);
+	}
+
+	blklen &= 0xfffc;
+	mmci_writel(MR, (MMCI_BF(CLKDIV, clkdiv)
+			 | MMCI_BF(BLKLEN, blklen)));
+}
+
+#define RESP_NO_CRC	1
+#define R1		MMCI_BF(RSPTYP, 1)
+#define R2		MMCI_BF(RSPTYP, 2)
+#define R3		(R1 | RESP_NO_CRC)
+#define R6		R1
+#define NID		MMCI_BF(MAXLAT, 0)
+#define NCR		MMCI_BF(MAXLAT, 1)
+#define TRCMD_START	MMCI_BF(TRCMD, 1)
+#define TRDIR_READ	MMCI_BF(TRDIR, 1)
+#define TRTYP_BLOCK	MMCI_BF(TRTYP, 0)
+#define INIT_CMD	MMCI_BF(SPCMD, 1)
+#define OPEN_DRAIN	MMCI_BF(OPDCMD, 1)
+
+#define ERROR_FLAGS	(MMCI_BIT(DTOE)			\
+			 | MMCI_BIT(RDIRE)		\
+			 | MMCI_BIT(RENDE)		\
+			 | MMCI_BIT(RINDE)		\
+			 | MMCI_BIT(RTOE))
+
+static int
+mmc_cmd(unsigned long cmd, unsigned long arg,
+	void *resp, unsigned long flags)
+{
+	unsigned long *response = resp;
+	int i, response_words = 0;
+	unsigned long error_flags;
+	u32 status;
+
+	pr_debug("mmc: CMD%lu 0x%lx (flags 0x%lx)\n",
+		 cmd, arg, flags);
+
+	error_flags = ERROR_FLAGS;
+	if (!(flags & RESP_NO_CRC))
+		error_flags |= MMCI_BIT(RCRCE);
+
+	flags &= ~MMCI_BF(CMDNB, ~0UL);
+
+	if (MMCI_BFEXT(RSPTYP, flags) == MMCI_RSPTYP_48_BIT_RESP)
+		response_words = 1;
+	else if (MMCI_BFEXT(RSPTYP, flags) == MMCI_RSPTYP_136_BIT_RESP)
+		response_words = 4;
+
+	mmci_writel(ARGR, arg);
+	mmci_writel(CMDR, cmd | flags);
+	do {
+		udelay(40);
+		status = mmci_readl(SR);
+	} while (!(status & MMCI_BIT(CMDRDY)));
+
+	pr_debug("mmc: status 0x%08lx\n", status);
+
+	if (status & ERROR_FLAGS) {
+		printf("mmc: command %lu failed (status: 0x%08lx)\n",
+		       cmd, status);
+		return -EIO;
+	}
+
+	if (response_words)
+		pr_debug("mmc: response:");
+
+	for (i = 0; i < response_words; i++) {
+		response[i] = mmci_readl(RSPR);
+		pr_debug(" %08lx", response[i]);
+	}
+	pr_debug("\n");
+
+	return 0;
+}
+
+static int mmc_acmd(unsigned long cmd, unsigned long arg,
+		    void *resp, unsigned long flags)
+{
+	unsigned long aresp[4];
+	int ret;
+
+	/*
+	 * Seems like the APP_CMD part of an ACMD has 64 cycles max
+	 * latency even though the ACMD part doesn't. This isn't
+	 * entirely clear in the SD Card spec, but some cards refuse
+	 * to work if we attempt to use 5 cycles max latency here...
+	 */
+	ret = mmc_cmd(MMC_CMD_APP_CMD, 0, aresp,
+		      R1 | NCR | (flags & OPEN_DRAIN));
+	if (ret)
+		return ret;
+	if ((aresp[0] & (R1_ILLEGAL_COMMAND | R1_APP_CMD)) != R1_APP_CMD)
+		return -ENODEV;
+
+	ret = mmc_cmd(cmd, arg, resp, flags);
+	return ret;
+}
+
+static unsigned long
+mmc_bread(int dev, unsigned long start, lbaint_t blkcnt,
+	  unsigned long *buffer)
+{
+	int ret, i = 0;
+	unsigned long resp[4];
+	unsigned long card_status, data;
+	unsigned long wordcount;
+	u32 status;
+
+	if (blkcnt == 0)
+		return 0;
+
+	pr_debug("mmc_bread: dev %d, start %lx, blkcnt %lx\n",
+		 dev, start, blkcnt);
+
+	/* Put the device into Transfer state */
+	ret = mmc_cmd(MMC_CMD_SELECT_CARD, mmc_rca << 16, resp, R1 | NCR);
+	if (ret) goto fail;
+
+	/* Set block length */
+	ret = mmc_cmd(MMC_CMD_SET_BLOCKLEN, mmc_blkdev.blksz, resp, R1 | NCR);
+	if (ret) goto fail;
+
+	pr_debug("MCI_DTOR = %08lx\n", mmci_readl(DTOR));
+
+	for (i = 0; i < blkcnt; i++, start++) {
+		ret = mmc_cmd(MMC_CMD_READ_SINGLE_BLOCK,
+			      start * mmc_blkdev.blksz, resp,
+			      (R1 | NCR | TRCMD_START | TRDIR_READ
+			       | TRTYP_BLOCK));
+		if (ret) goto fail;
+
+		ret = -EIO;
+		wordcount = 0;
+		do {
+			do {
+				status = mmci_readl(SR);
+				if (status & (ERROR_FLAGS | MMCI_BIT(OVRE)))
+					goto fail;
+			} while (!(status & MMCI_BIT(RXRDY)));
+
+			if (status & MMCI_BIT(RXRDY)) {
+				data = mmci_readl(RDR);
+				/* pr_debug("%x\n", data); */
+				*buffer++ = data;
+				wordcount++;
+			}
+		} while(wordcount < (512 / 4));
+
+		pr_debug("mmc: read %u words, waiting for BLKE\n", wordcount);
+
+		do {
+			status = mmci_readl(SR);
+		} while (!(status & MMCI_BIT(BLKE)));
+
+		putc('.');
+	}
+
+out:
+	/* Put the device back into Standby state */
+	mmc_cmd(MMC_CMD_SELECT_CARD, 0, resp, NCR);
+	return i;
+
+fail:
+	mmc_cmd(MMC_CMD_SEND_STATUS, mmc_rca << 16, &card_status, R1 | NCR);
+	printf("mmc: bread failed, card status = ", card_status);
+	goto out;
+}
+
+static void mmc_parse_cid(struct mmc_cid *cid, unsigned long *resp)
+{
+	cid->mid = resp[0] >> 24;
+	cid->oid = (resp[0] >> 8) & 0xffff;
+	cid->pnm[0] = resp[0];
+	cid->pnm[1] = resp[1] >> 24;
+	cid->pnm[2] = resp[1] >> 16;
+	cid->pnm[3] = resp[1] >> 8;
+	cid->pnm[4] = resp[1];
+	cid->pnm[5] = resp[2] >> 24;
+	cid->pnm[6] = 0;
+	cid->prv = resp[2] >> 16;
+	cid->psn = (resp[2] << 16) | (resp[3] >> 16);
+	cid->mdt = resp[3] >> 8;
+}
+
+static void sd_parse_cid(struct mmc_cid *cid, unsigned long *resp)
+{
+	cid->mid = resp[0] >> 24;
+	cid->oid = (resp[0] >> 8) & 0xffff;
+	cid->pnm[0] = resp[0];
+	cid->pnm[1] = resp[1] >> 24;
+	cid->pnm[2] = resp[1] >> 16;
+	cid->pnm[3] = resp[1] >> 8;
+	cid->pnm[4] = resp[1];
+	cid->pnm[5] = 0;
+	cid->pnm[6] = 0;
+	cid->prv = resp[2] >> 24;
+	cid->psn = (resp[2] << 8) | (resp[3] >> 24);
+	cid->mdt = (resp[3] >> 8) & 0x0fff;
+}
+
+static void mmc_dump_cid(const struct mmc_cid *cid)
+{
+	printf("Manufacturer ID:       %02lX\n", cid->mid);
+	printf("OEM/Application ID:    %04lX\n", cid->oid);
+	printf("Product name:          %s\n", cid->pnm);
+	printf("Product Revision:      %lu.%lu\n",
+	       cid->prv >> 4, cid->prv & 0x0f);
+	printf("Product Serial Number: %lu\n", cid->psn);
+	printf("Manufacturing Date:    %02lu/%02lu\n",
+	       cid->mdt >> 4, cid->mdt & 0x0f);
+}
+
+static void mmc_dump_csd(const struct mmc_csd *csd)
+{
+	unsigned long *csd_raw = (unsigned long *)csd;
+	printf("CSD data: %08lx %08lx %08lx %08lx\n",
+	       csd_raw[0], csd_raw[1], csd_raw[2], csd_raw[3]);
+	printf("CSD structure version:   1.%u\n", csd->csd_structure);
+	printf("MMC System Spec version: %u\n", csd->spec_vers);
+	printf("Card command classes:    %03x\n", csd->ccc);
+	printf("Read block length:       %u\n", 1 << csd->read_bl_len);
+	if (csd->read_bl_partial)
+		puts("Supports partial reads\n");
+	else
+		puts("Does not support partial reads\n");
+	printf("Write block length:      %u\n", 1 << csd->write_bl_len);
+	if (csd->write_bl_partial)
+		puts("Supports partial writes\n");
+	else
+		puts("Does not support partial writes\n");
+	if (csd->wp_grp_enable)
+		printf("Supports group WP:      %u\n", csd->wp_grp_size + 1);
+	else
+		puts("Does not support group WP\n");
+	printf("Card capacity:		%u bytes\n",
+	       (csd->c_size + 1) * (1 << (csd->c_size_mult + 2)) *
+	       (1 << csd->read_bl_len));
+	printf("File format:            %u/%u\n",
+	       csd->file_format_grp, csd->file_format);
+	puts("Write protection:        ");
+	if (csd->perm_write_protect)
+		puts(" permanent");
+	if (csd->tmp_write_protect)
+		puts(" temporary");
+	putc('\n');
+}
+
+static int mmc_idle_cards(void)
+{
+	int ret;
+
+	/* Reset and initialize all cards */
+	ret = mmc_cmd(MMC_CMD_GO_IDLE_STATE, 0, NULL, 0);
+	if (ret)
+		return ret;
+
+	/* Keep the bus idle for 74 clock cycles */
+	return mmc_cmd(0, 0, NULL, INIT_CMD);
+}
+
+static int sd_init_card(struct mmc_cid *cid, int verbose)
+{
+	unsigned long resp[4];
+	int i, ret = 0;
+
+	mmc_idle_cards();
+	for (i = 0; i < 1000; i++) {
+		ret = mmc_acmd(MMC_ACMD_SD_SEND_OP_COND, CFG_MMC_OP_COND,
+			       resp, R3 | NID);
+		if (ret || (resp[0] & 0x80000000))
+			break;
+		ret = -ETIMEDOUT;
+	}
+
+	if (ret)
+		return ret;
+
+	ret = mmc_cmd(MMC_CMD_ALL_SEND_CID, 0, resp, R2 | NID);
+	if (ret)
+		return ret;
+	sd_parse_cid(cid, resp);
+	if (verbose)
+		mmc_dump_cid(cid);
+
+	/* Get RCA of the card that responded */
+	ret = mmc_cmd(MMC_CMD_SD_SEND_RELATIVE_ADDR, 0, resp, R6 | NCR);
+	if (ret)
+		return ret;
+
+	mmc_rca = resp[0] >> 16;
+	if (verbose)
+		printf("SD Card detected (RCA %u)\n", mmc_rca);
+	return 0;
+}
+
+static int mmc_init_card(struct mmc_cid *cid, int verbose)
+{
+	unsigned long resp[4];
+	int i, ret = 0;
+
+	mmc_idle_cards();
+	for (i = 0; i < 1000; i++) {
+		ret = mmc_cmd(MMC_CMD_SEND_OP_COND, CFG_MMC_OP_COND, resp,
+			      R3 | NID | OPEN_DRAIN);
+		if (ret || (resp[0] & 0x80000000))
+			break;
+		ret = -ETIMEDOUT;
+	}
+
+	if (ret)
+		return ret;
+
+	/* Get CID of all cards. FIXME: Support more than one card */
+	ret = mmc_cmd(MMC_CMD_ALL_SEND_CID, 0, resp, R2 | NID | OPEN_DRAIN);
+	if (ret)
+		return ret;
+	mmc_parse_cid(cid, resp);
+	if (verbose)
+		mmc_dump_cid(cid);
+
+	/* Set Relative Address of the card that responded */
+	ret = mmc_cmd(MMC_CMD_SET_RELATIVE_ADDR, mmc_rca << 16, resp,
+		      R1 | NCR | OPEN_DRAIN);
+	return ret;
+}
+
+int mmc_init(int verbose)
+{
+	struct mmc_cid cid;
+	struct mmc_csd csd;
+	int ret;
+
+	/* Initialize controller */
+	mmci_writel(CR, MMCI_BIT(SWRST));
+	mmci_writel(CR, MMCI_BIT(MCIEN));
+	mmci_writel(DTOR, 0x5f);
+	mmci_writel(IDR, ~0UL);
+	mci_set_mode(CFG_MMC_CLK_OD, MMC_DEFAULT_BLKLEN);
+
+	ret = sd_init_card(&cid, verbose);
+	if (ret) {
+		mmc_rca = MMC_DEFAULT_RCA;
+		ret = mmc_init_card(&cid, verbose);
+	}
+	if (ret)
+		return ret;
+
+	/* Get CSD from the card */
+	ret = mmc_cmd(MMC_CMD_SEND_CSD, mmc_rca << 16, &csd, R2 | NCR);
+	if (ret)
+		return ret;
+	if (verbose)
+		mmc_dump_csd(&csd);
+
+	/* Initialize the blockdev structure */
+	mmc_blkdev.if_type = IF_TYPE_MMC;
+	mmc_blkdev.part_type = PART_TYPE_DOS;
+	mmc_blkdev.block_read = mmc_bread;
+	sprintf((char *)mmc_blkdev.vendor,
+		"Man %02x%04x Snr %08x",
+		cid.mid, cid.oid, cid.psn);
+	strncpy((char *)mmc_blkdev.product, cid.pnm,
+		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;
+	mmc_blkdev.lba = (csd.c_size + 1) * (1 << (csd.c_size_mult + 2));
+
+	mci_set_mode(CFG_MMC_CLK_PP, mmc_blkdev.blksz);
+
+#if 0
+	if (fat_register_device(&mmc_blkdev, 1))
+		printf("Could not register MMC fat device\n");
+#else
+	init_part(&mmc_blkdev);
+#endif
+
+	return 0;
+}
+
+int mmc_read(ulong src, uchar *dst, int size)
+{
+	return -ENOSYS;
+}
+
+int mmc_write(uchar *src, ulong dst, int size)
+{
+	return -ENOSYS;
+}
+
+int mmc2info(ulong addr)
+{
+	return 0;
+}
+
+#endif /* CONFIG_MMC */
diff --git a/cpu/at32ap/atmel_mci.h b/cpu/at32ap/atmel_mci.h
new file mode 100644
index 0000000..0ffbc4f
--- /dev/null
+++ b/cpu/at32ap/atmel_mci.h
@@ -0,0 +1,197 @@
+/*
+ * Copyright (C) 2005-2006 Atmel Corporation
+ *
+ * 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
+ */
+#ifndef __CPU_AT32AP_ATMEL_MCI_H__
+#define __CPU_AT32AP_ATMEL_MCI_H__
+
+/* Atmel MultiMedia Card Interface (MCI) registers */
+#define MMCI_CR					0x0000
+#define MMCI_MR					0x0004
+#define MMCI_DTOR				0x0008
+#define MMCI_SDCR				0x000c
+#define MMCI_ARGR				0x0010
+#define MMCI_CMDR				0x0014
+#define MMCI_RSPR				0x0020
+#define MMCI_RSPR1				0x0024
+#define MMCI_RSPR2				0x0028
+#define MMCI_RSPR3				0x002c
+#define MMCI_RDR				0x0030
+#define MMCI_TDR				0x0034
+#define MMCI_SR					0x0040
+#define MMCI_IER				0x0044
+#define MMCI_IDR				0x0048
+#define MMCI_IMR				0x004c
+
+/* Bitfields in CR */
+#define MMCI_MCIEN_OFFSET			0
+#define MMCI_MCIEN_SIZE				1
+#define MMCI_MCIDIS_OFFSET			1
+#define MMCI_MCIDIS_SIZE			1
+#define MMCI_PWSEN_OFFSET			2
+#define MMCI_PWSEN_SIZE				1
+#define MMCI_PWSDIS_OFFSET			3
+#define MMCI_PWSDIS_SIZE			1
+#define MMCI_SWRST_OFFSET			7
+#define MMCI_SWRST_SIZE				1
+
+/* Bitfields in MR */
+#define MMCI_CLKDIV_OFFSET			0
+#define MMCI_CLKDIV_SIZE			8
+#define MMCI_PWSDIV_OFFSET			8
+#define MMCI_PWSDIV_SIZE			3
+#define MMCI_PDCPADV_OFFSET			14
+#define MMCI_PDCPADV_SIZE			1
+#define MMCI_PDCMODE_OFFSET			15
+#define MMCI_PDCMODE_SIZE			1
+#define MMCI_BLKLEN_OFFSET			16
+#define MMCI_BLKLEN_SIZE			16
+
+/* Bitfields in DTOR */
+#define MMCI_DTOCYC_OFFSET			0
+#define MMCI_DTOCYC_SIZE			4
+#define MMCI_DTOMUL_OFFSET			4
+#define MMCI_DTOMUL_SIZE			3
+
+/* Bitfields in SDCR */
+#define MMCI_SCDSEL_OFFSET			0
+#define MMCI_SCDSEL_SIZE			4
+#define MMCI_SCDBUS_OFFSET			7
+#define MMCI_SCDBUS_SIZE			1
+
+/* Bitfields in ARGR */
+#define MMCI_ARG_OFFSET				0
+#define MMCI_ARG_SIZE				32
+
+/* Bitfields in CMDR */
+#define MMCI_CMDNB_OFFSET			0
+#define MMCI_CMDNB_SIZE				6
+#define MMCI_RSPTYP_OFFSET			6
+#define MMCI_RSPTYP_SIZE			2
+#define MMCI_SPCMD_OFFSET			8
+#define MMCI_SPCMD_SIZE				3
+#define MMCI_OPDCMD_OFFSET			11
+#define MMCI_OPDCMD_SIZE			1
+#define MMCI_MAXLAT_OFFSET			12
+#define MMCI_MAXLAT_SIZE			1
+#define MMCI_TRCMD_OFFSET			16
+#define MMCI_TRCMD_SIZE				2
+#define MMCI_TRDIR_OFFSET			18
+#define MMCI_TRDIR_SIZE				1
+#define MMCI_TRTYP_OFFSET			19
+#define MMCI_TRTYP_SIZE				2
+
+/* Bitfields in RSPRx */
+#define MMCI_RSP_OFFSET				0
+#define MMCI_RSP_SIZE				32
+
+/* Bitfields in SR/IER/IDR/IMR */
+#define MMCI_CMDRDY_OFFSET			0
+#define MMCI_CMDRDY_SIZE			1
+#define MMCI_RXRDY_OFFSET			1
+#define MMCI_RXRDY_SIZE				1
+#define MMCI_TXRDY_OFFSET			2
+#define MMCI_TXRDY_SIZE				1
+#define MMCI_BLKE_OFFSET			3
+#define MMCI_BLKE_SIZE				1
+#define MMCI_DTIP_OFFSET			4
+#define MMCI_DTIP_SIZE				1
+#define MMCI_NOTBUSY_OFFSET			5
+#define MMCI_NOTBUSY_SIZE			1
+#define MMCI_ENDRX_OFFSET			6
+#define MMCI_ENDRX_SIZE				1
+#define MMCI_ENDTX_OFFSET			7
+#define MMCI_ENDTX_SIZE				1
+#define MMCI_RXBUFF_OFFSET			14
+#define MMCI_RXBUFF_SIZE			1
+#define MMCI_TXBUFE_OFFSET			15
+#define MMCI_TXBUFE_SIZE			1
+#define MMCI_RINDE_OFFSET			16
+#define MMCI_RINDE_SIZE				1
+#define MMCI_RDIRE_OFFSET			17
+#define MMCI_RDIRE_SIZE				1
+#define MMCI_RCRCE_OFFSET			18
+#define MMCI_RCRCE_SIZE				1
+#define MMCI_RENDE_OFFSET			19
+#define MMCI_RENDE_SIZE				1
+#define MMCI_RTOE_OFFSET			20
+#define MMCI_RTOE_SIZE				1
+#define MMCI_DCRCE_OFFSET			21
+#define MMCI_DCRCE_SIZE				1
+#define MMCI_DTOE_OFFSET			22
+#define MMCI_DTOE_SIZE				1
+#define MMCI_OVRE_OFFSET			30
+#define MMCI_OVRE_SIZE				1
+#define MMCI_UNRE_OFFSET			31
+#define MMCI_UNRE_SIZE				1
+
+/* Constants for DTOMUL */
+#define MMCI_DTOMUL_1_CYCLE			0
+#define MMCI_DTOMUL_16_CYCLES			1
+#define MMCI_DTOMUL_128_CYCLES			2
+#define MMCI_DTOMUL_256_CYCLES			3
+#define MMCI_DTOMUL_1024_CYCLES			4
+#define MMCI_DTOMUL_4096_CYCLES			5
+#define MMCI_DTOMUL_65536_CYCLES		6
+#define MMCI_DTOMUL_1048576_CYCLES		7
+
+/* Constants for RSPTYP */
+#define MMCI_RSPTYP_NO_RESP			0
+#define MMCI_RSPTYP_48_BIT_RESP			1
+#define MMCI_RSPTYP_136_BIT_RESP		2
+
+/* Constants for SPCMD */
+#define MMCI_SPCMD_NO_SPEC_CMD			0
+#define MMCI_SPCMD_INIT_CMD			1
+#define MMCI_SPCMD_SYNC_CMD			2
+#define MMCI_SPCMD_INT_CMD			4
+#define MMCI_SPCMD_INT_RESP			5
+
+/* Constants for TRCMD */
+#define MMCI_TRCMD_NO_TRANS			0
+#define MMCI_TRCMD_START_TRANS			1
+#define MMCI_TRCMD_STOP_TRANS			2
+
+/* Constants for TRTYP */
+#define MMCI_TRTYP_BLOCK			0
+#define MMCI_TRTYP_MULTI_BLOCK			1
+#define MMCI_TRTYP_STREAM			2
+
+/* Bit manipulation macros */
+#define MMCI_BIT(name)					\
+	(1 << MMCI_##name##_OFFSET)
+#define MMCI_BF(name,value)				\
+	(((value) & ((1 << MMCI_##name##_SIZE) - 1))	\
+	 << MMCI_##name##_OFFSET)
+#define MMCI_BFEXT(name,value)				\
+	(((value) >> MMCI_##name##_OFFSET)\
+	 & ((1 << MMCI_##name##_SIZE) - 1))
+#define MMCI_BFINS(name,value,old)			\
+	(((old) & ~(((1 << MMCI_##name##_SIZE) - 1)	\
+		    << MMCI_##name##_OFFSET))		\
+	 | MMCI_BF(name,value))
+
+/* Register access macros */
+#define mmci_readl(reg)					\
+	readl((void *)MMCI_BASE + MMCI_##reg)
+#define mmci_writel(reg,value)				\
+	writel((value), (void *)MMCI_BASE + MMCI_##reg)
+
+#endif /* __CPU_AT32AP_ATMEL_MCI_H__ */
diff --git a/cpu/at32ap/cpu.c b/cpu/at32ap/cpu.c
index 37e3ea0..311466b 100644
--- a/cpu/at32ap/cpu.c
+++ b/cpu/at32ap/cpu.c
@@ -26,33 +26,79 @@
 #include <asm/sections.h>
 #include <asm/sysreg.h>
 
+#include <asm/arch/clk.h>
 #include <asm/arch/memory-map.h>
-#include <asm/arch/platform.h>
 
 #include "hsmc3.h"
+#include "sm.h"
+
+/* Sanity checks */
+#if (CFG_CLKDIV_CPU > CFG_CLKDIV_HSB)		\
+	|| (CFG_CLKDIV_HSB > CFG_CLKDIV_PBA)	\
+	|| (CFG_CLKDIV_HSB > CFG_CLKDIV_PBB)
+# error Constraint fCPU >= fHSB >= fPB{A,B} violated
+#endif
+#if defined(CONFIG_PLL) && ((CFG_PLL0_MUL < 1) || (CFG_PLL0_DIV < 1))
+# error Invalid PLL multiplier and/or divider
+#endif
 
 DECLARE_GLOBAL_DATA_PTR;
 
+static void pm_init(void)
+{
+	uint32_t cksel;
+
+#ifdef CONFIG_PLL
+	/* Initialize the PLL */
+	sm_writel(PM_PLL0, (SM_BF(PLLCOUNT, CFG_PLL0_SUPPRESS_CYCLES)
+			    | SM_BF(PLLMUL, CFG_PLL0_MUL - 1)
+			    | SM_BF(PLLDIV, CFG_PLL0_DIV - 1)
+			    | SM_BF(PLLOPT, CFG_PLL0_OPT)
+			    | SM_BF(PLLOSC, 0)
+			    | SM_BIT(PLLEN)));
+
+	/* Wait for lock */
+	while (!(sm_readl(PM_ISR) & SM_BIT(LOCK0))) ;
+#endif
+
+	/* Set up clocks for the CPU and all peripheral buses */
+	cksel = 0;
+	if (CFG_CLKDIV_CPU)
+		cksel |= SM_BIT(CPUDIV) | SM_BF(CPUSEL, CFG_CLKDIV_CPU - 1);
+	if (CFG_CLKDIV_HSB)
+		cksel |= SM_BIT(HSBDIV) | SM_BF(HSBSEL, CFG_CLKDIV_HSB - 1);
+	if (CFG_CLKDIV_PBA)
+		cksel |= SM_BIT(PBADIV) | SM_BF(PBASEL, CFG_CLKDIV_PBA - 1);
+	if (CFG_CLKDIV_PBB)
+		cksel |= SM_BIT(PBBDIV) | SM_BF(PBBSEL, CFG_CLKDIV_PBB - 1);
+	sm_writel(PM_CKSEL, cksel);
+
+	gd->cpu_hz = get_cpu_clk_rate();
+
+#ifdef CONFIG_PLL
+	/* Use PLL0 as main clock */
+	sm_writel(PM_MCCTRL, SM_BIT(PLLSEL));
+#endif
+}
+
 int cpu_init(void)
 {
-	const struct device *hebi;
 	extern void _evba(void);
 	char *p;
 
 	gd->cpu_hz = CFG_OSC0_HZ;
 
-	/* fff03400: 00010001 04030402 00050005 10011103 */
-	hebi = get_device(DEVICE_HEBI);
-	hsmc3_writel(hebi, MODE0, 0x00031103);
-	hsmc3_writel(hebi, CYCLE0, 0x000c000d);
-	hsmc3_writel(hebi, PULSE0, 0x0b0a0906);
-	hsmc3_writel(hebi, SETUP0, 0x00010002);
+	/* TODO: Move somewhere else, but needs to be run before we
+	 * increase the clock frequency. */
+	hsmc3_writel(MODE0, 0x00031103);
+	hsmc3_writel(CYCLE0, 0x000c000d);
+	hsmc3_writel(PULSE0, 0x0b0a0906);
+	hsmc3_writel(SETUP0, 0x00010002);
 
 	pm_init();
 
 	sysreg_write(EVBA, (unsigned long)&_evba);
 	asm volatile("csrf	%0" : : "i"(SYSREG_EM_OFFSET));
-	gd->console_uart = get_device(CFG_CONSOLE_UART_DEV);
 
 	/* Lock everything that mess with the flash in the icache */
 	for (p = __flashprog_start; p <= (__flashprog_end + CFG_ICACHE_LINESZ);
diff --git a/cpu/at32ap/device.c b/cpu/at32ap/device.c
deleted file mode 100644
index 89914b6..0000000
--- a/cpu/at32ap/device.c
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- * Copyright (C) 2006 Atmel Corporation
- *
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- */
-#include <common.h>
-
-#include <asm/arch/platform.h>
-
-#include "sm.h"
-
-struct device_state {
-	int refcount;
-};
-
-static struct device_state device_state[NR_DEVICES];
-
-static int claim_resource(const struct resource *res)
-{
-	int ret = 0;
-
-	switch (res->type) {
-	case RESOURCE_GPIO:
-		ret = gpio_set_func(res->u.gpio.gpio_dev,
-				    res->u.gpio.start,
-				    res->u.gpio.nr_pins,
-				    res->u.gpio.func);
-		break;
-	case RESOURCE_CLOCK:
-		ret = pm_enable_clock(res->u.clock.id, res->u.clock.index);
-		break;
-	}
-
-	return ret;
-}
-
-static void free_resource(const struct resource *res)
-{
-	switch (res->type) {
-	case RESOURCE_GPIO:
-		gpio_free(res->u.gpio.gpio_dev, res->u.gpio.start,
-			  res->u.gpio.nr_pins);
-		break;
-	case RESOURCE_CLOCK:
-		pm_disable_clock(res->u.clock.id, res->u.clock.index);
-		break;
-	}
-}
-
-static int init_dev(const struct device *dev)
-{
-	unsigned int i;
-	int ret = 0;
-
-	for (i = 0; i < dev->nr_resources; i++) {
-		ret = claim_resource(&dev->resource[i]);
-		if (ret)
-			goto cleanup;
-	}
-
-	return 0;
-
-cleanup:
-	while (i--)
-		free_resource(&dev->resource[i]);
-
-	return ret;
-}
-
-const struct device *get_device(enum device_id devid)
-{
-	struct device_state *devstate;
-	const struct device *dev;
-	unsigned long flags;
-	int initialized = 0;
-	int ret = 0;
-
-	devstate = &device_state[devid];
-	dev = &chip_device[devid];
-
-	flags = disable_interrupts();
-	if (devstate->refcount++)
-		initialized = 1;
-	if (flags)
-		enable_interrupts();
-
-	if (!initialized)
-		ret = init_dev(dev);
-
-	return ret ? NULL : dev;
-}
-
-void put_device(const struct device *dev)
-{
-	struct device_state *devstate;
-	unsigned long devid, flags;
-
-	devid = (unsigned long)(dev - chip_device) / sizeof(struct device);
-	devstate = &device_state[devid];
-
-	flags = disable_interrupts();
-	devstate--;
-	if (!devstate) {
-		unsigned int i;
-		for (i = 0; i < dev->nr_resources; i++)
-			free_resource(&dev->resource[i]);
-	}
-	if (flags)
-		enable_interrupts();
-}
diff --git a/cpu/at32ap/entry.S b/cpu/at32ap/entry.S
index b52d798..a6fc688 100644
--- a/cpu/at32ap/entry.S
+++ b/cpu/at32ap/entry.S
@@ -42,8 +42,7 @@ timer_interrupt_handler:
 	 * We're running at interrupt level 3, so we don't need to save
 	 * r8-r12 or lr to the stack.
 	 */
-	mov	r8, lo(timer_overflow)
-	orh	r8, hi(timer_overflow)
+	lda.w	r8, timer_overflow
 	ld.w	r9, r8[0]
 	mov	r10, -1
 	mtsr	SYSREG_COMPARE, r10
diff --git a/cpu/at32ap/exception.c b/cpu/at32ap/exception.c
index 4123c44..0672685 100644
--- a/cpu/at32ap/exception.c
+++ b/cpu/at32ap/exception.c
@@ -24,6 +24,8 @@
 #include <asm/sysreg.h>
 #include <asm/ptrace.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 static const char * const cpu_modes[8] = {
 	"Application", "Supervisor", "Interrupt level 0", "Interrupt level 1",
 	"Interrupt level 2", "Interrupt level 3", "Exception", "NMI"
@@ -109,11 +111,10 @@ void do_unknown_exception(unsigned int ecr, struct pt_regs *regs)
 	printf("CPU Mode: %s\n", cpu_modes[mode]);
 
 	/* Avoid exception loops */
-	if (regs->sp >= CFG_INIT_SP_ADDR
-	    || regs->sp < (CFG_INIT_SP_ADDR - CONFIG_STACKSIZE))
+	if (regs->sp < CFG_SDRAM_BASE || regs->sp >= gd->stack_end)
 		printf("\nStack pointer seems bogus, won't do stack dump\n");
 	else
-		dump_mem("\nStack: ", regs->sp, CFG_INIT_SP_ADDR);
+		dump_mem("\nStack: ", regs->sp, gd->stack_end);
 
 	panic("Unhandled exception\n");
 }
diff --git a/cpu/at32ap/hsdramc.c b/cpu/at32ap/hsdramc.c
index f36da35..a936e03 100644
--- a/cpu/at32ap/hsdramc.c
+++ b/cpu/at32ap/hsdramc.c
@@ -25,17 +25,11 @@
 #include <asm/io.h>
 #include <asm/sdram.h>
 
-#include <asm/arch/platform.h>
+#include <asm/arch/clk.h>
+#include <asm/arch/memory-map.h>
 
 #include "hsdramc1.h"
 
-struct hsdramc {
-	const struct device *hebi;
-	void *regs;
-};
-
-static struct hsdramc hsdramc;
-
 unsigned long sdram_init(const struct sdram_info *info)
 {
 	unsigned long *sdram = (unsigned long *)uncached(info->phys_addr);
@@ -44,16 +38,6 @@ unsigned long sdram_init(const struct sdram_info *info)
 	unsigned long bus_hz;
 	unsigned int i;
 
-	hsdramc.hebi = get_device(DEVICE_HEBI);
-	if (!hsdramc.hebi)
-		return 0;
-
-	/* FIXME: Both of these lines are complete hacks */
-	hsdramc.regs = hsdramc.hebi->regs + 0x400;
-	bus_hz = pm_get_clock_freq(hsdramc.hebi->resource[0].u.clock.id);
-
-	cpu_enable_sdram();
-
 	tmp = (HSDRAMC1_BF(NC, info->col_bits - 8)
 	       | HSDRAMC1_BF(NR, info->row_bits - 11)
 	       | HSDRAMC1_BF(NB, info->bank_bits - 1)
@@ -74,7 +58,7 @@ unsigned long sdram_init(const struct sdram_info *info)
 			   + info->bank_bits + 2);
 #endif
 
-	hsdramc1_writel(&hsdramc, CR, tmp);
+	hsdramc1_writel(CR, tmp);
 
 	/*
 	 * Initialization sequence for SDRAM, from the data sheet:
@@ -87,15 +71,15 @@ unsigned long sdram_init(const struct sdram_info *info)
 	/*
 	 * 2. A Precharge All command is issued to the SDRAM
 	 */
-	hsdramc1_writel(&hsdramc, MR, HSDRAMC1_MODE_BANKS_PRECHARGE);
-	hsdramc1_readl(&hsdramc, MR);
+	hsdramc1_writel(MR, HSDRAMC1_MODE_BANKS_PRECHARGE);
+	hsdramc1_readl(MR);
 	writel(0, sdram);
 
 	/*
 	 * 3. Eight auto-refresh (CBR) cycles are provided
 	 */
-	hsdramc1_writel(&hsdramc, MR, HSDRAMC1_MODE_AUTO_REFRESH);
-	hsdramc1_readl(&hsdramc, MR);
+	hsdramc1_writel(MR, HSDRAMC1_MODE_AUTO_REFRESH);
+	hsdramc1_readl(MR);
 	for (i = 0; i < 8; i++)
 		writel(0, sdram);
 
@@ -106,8 +90,8 @@ unsigned long sdram_init(const struct sdram_info *info)
 	 *
 	 * CAS from info struct, burst length 1, serial burst type
 	 */
-	hsdramc1_writel(&hsdramc, MR, HSDRAMC1_MODE_LOAD_MODE);
-	hsdramc1_readl(&hsdramc, MR);
+	hsdramc1_writel(MR, HSDRAMC1_MODE_LOAD_MODE);
+	hsdramc1_readl(MR);
 	writel(0, sdram + (info->cas << 4));
 
 	/*
@@ -117,9 +101,9 @@ unsigned long sdram_init(const struct sdram_info *info)
 	 * From the timing diagram, it looks like tMRD is 3
 	 * cycles...try a dummy read from the peripheral bus.
 	 */
-	hsdramc1_readl(&hsdramc, MR);
-	hsdramc1_writel(&hsdramc, MR, HSDRAMC1_MODE_NORMAL);
-	hsdramc1_readl(&hsdramc, MR);
+	hsdramc1_readl(MR);
+	hsdramc1_writel(MR, HSDRAMC1_MODE_NORMAL);
+	hsdramc1_readl(MR);
 	writel(0, sdram);
 
 	/*
@@ -128,7 +112,8 @@ unsigned long sdram_init(const struct sdram_info *info)
 	 *
 	 * 15.6 us is a typical value for a burst of length one
 	 */
-	hsdramc1_writel(&hsdramc, TR, (156 * (bus_hz / 1000)) / 10000);
+	bus_hz = get_sdram_clk_rate();
+	hsdramc1_writel(TR, (156 * (bus_hz / 1000)) / 10000);
 
 	printf("SDRAM: %u MB at address 0x%08lx\n",
 	       sdram_size >> 20, info->phys_addr);
diff --git a/cpu/at32ap/hsdramc1.h b/cpu/at32ap/hsdramc1.h
index ce229bc..305d2cb 100644
--- a/cpu/at32ap/hsdramc1.h
+++ b/cpu/at32ap/hsdramc1.h
@@ -135,9 +135,9 @@
 	 | HSDRAMC1_BF(name,value))
 
 /* Register access macros */
-#define hsdramc1_readl(port,reg)				\
-	readl((port)->regs + HSDRAMC1_##reg)
-#define hsdramc1_writel(port,reg,value)				\
-	writel((value), (port)->regs + HSDRAMC1_##reg)
+#define hsdramc1_readl(reg)					\
+	readl((void *)HSDRAMC_BASE + HSDRAMC1_##reg)
+#define hsdramc1_writel(reg,value)				\
+	writel((value), (void *)HSDRAMC_BASE + HSDRAMC1_##reg)
 
 #endif /* __ASM_AVR32_HSDRAMC1_H__ */
diff --git a/cpu/at32ap/hsmc3.h b/cpu/at32ap/hsmc3.h
index ec78cee..ca533b9 100644
--- a/cpu/at32ap/hsmc3.h
+++ b/cpu/at32ap/hsmc3.h
@@ -118,9 +118,9 @@
 	 | HSMC3_BF(name,value))
 
 /* Register access macros */
-#define hsmc3_readl(port,reg)					\
-	readl((port)->regs + HSMC3_##reg)
-#define hsmc3_writel(port,reg,value)				\
-	writel((value), (port)->regs + HSMC3_##reg)
+#define hsmc3_readl(reg)					\
+	readl((void *)HSMC_BASE + HSMC3_##reg)
+#define hsmc3_writel(reg,value)					\
+	writel((value), (void *)HSMC_BASE + HSMC3_##reg)
 
 #endif /* __CPU_AT32AP_HSMC3_H__ */
diff --git a/cpu/at32ap/interrupts.c b/cpu/at32ap/interrupts.c
index d720cfa..c9e0499 100644
--- a/cpu/at32ap/interrupts.c
+++ b/cpu/at32ap/interrupts.c
@@ -27,7 +27,7 @@
 #include <asm/processor.h>
 #include <asm/sysreg.h>
 
-#include <asm/arch/platform.h>
+#include <asm/arch/memory-map.h>
 
 #define HANDLER_MASK	0x00ffffff
 #define INTLEV_SHIFT	30
@@ -44,8 +44,6 @@ volatile unsigned long timer_overflow;
  */
 static unsigned long tb_factor;
 
-static const struct device *intc_dev;
-
 unsigned long get_tbclk(void)
 {
 	return gd->cpu_hz;
@@ -117,16 +115,19 @@ void udelay(unsigned long usec)
 static int set_interrupt_handler(unsigned int nr, void (*handler)(void),
 				 unsigned int priority)
 {
+	extern void _evba(void);
 	unsigned long intpr;
 	unsigned long handler_addr = (unsigned long)handler;
 
+	handler_addr -= (unsigned long)&_evba;
+
 	if ((handler_addr & HANDLER_MASK) != handler_addr
 	    || (priority & INTLEV_MASK) != priority)
 		return -EINVAL;
 
 	intpr = (handler_addr & HANDLER_MASK);
 	intpr |= (priority & INTLEV_MASK) << INTLEV_SHIFT;
-	writel(intpr, intc_dev->regs + 4 * nr);
+	writel(intpr, (void *)INTC_BASE + 4 * nr);
 
 	return 0;
 }
@@ -143,10 +144,7 @@ void timer_init(void)
 	do_div(tmp, gd->cpu_hz);
 	tb_factor = (u32)tmp;
 
-	intc_dev = get_device(DEVICE_INTC);
-
-	if (!intc_dev
-	    || set_interrupt_handler(0, &timer_interrupt_handler, 3))
+	if (set_interrupt_handler(0, &timer_interrupt_handler, 3))
 		return;
 
 	/* For all practical purposes, this gives us an overflow interrupt */
diff --git a/cpu/at32ap/pio.c b/cpu/at32ap/pio.c
index 8b6c3a3..9ba0b8e 100644
--- a/cpu/at32ap/pio.c
+++ b/cpu/at32ap/pio.c
@@ -21,74 +21,40 @@
  */
 #include <common.h>
 
-#include <asm/errno.h>
 #include <asm/io.h>
-#include <asm/arch/platform.h>
+#include <asm/arch/gpio.h>
+#include <asm/arch/memory-map.h>
 
 #include "pio2.h"
 
-struct pio_state {
-	const struct device *dev;
-	u32 alloc_mask;
-};
-
-static struct pio_state pio_state[CFG_NR_PIOS];
-
-int gpio_set_func(enum device_id gpio_devid, unsigned int start,
-		  unsigned int nr_pins, enum gpio_func func)
+void gpio_select_periph_A(unsigned int pin, int use_pullup)
 {
-	const struct device *gpio;
-	struct pio_state *state;
-	u32 mask;
-
-	state = &pio_state[gpio_devid - DEVICE_PIOA];
-
-	gpio = get_device(gpio_devid);
-	if (!gpio)
-		return -EBUSY;
-
-	state->dev = gpio;
-	mask = ((1 << nr_pins) - 1) << start;
-
-	if (mask & state->alloc_mask) {
-		put_device(gpio);
-		return -EBUSY;
-	}
-	state->alloc_mask |= mask;
-
-	switch (func) {
-	case GPIO_FUNC_GPIO:
-		/* TODO */
-		return -EINVAL;
-	case GPIO_FUNC_A:
-		pio2_writel(gpio, ASR, mask);
-		pio2_writel(gpio, PDR, mask);
-		pio2_writel(gpio, PUDR, mask);
-		break;
-	case GPIO_FUNC_B:
-		pio2_writel(gpio, BSR, mask);
-		pio2_writel(gpio, PDR, mask);
-		pio2_writel(gpio, PUDR, mask);
-		break;
-	}
-
-	return 0;
+	void *base = gpio_pin_to_addr(pin);
+	uint32_t mask = 1 << (pin & 0x1f);
+
+	if (!base)
+		panic("Invalid GPIO pin %u\n", pin);
+
+	pio2_writel(base, ASR, mask);
+	pio2_writel(base, PDR, mask);
+	if (use_pullup)
+		pio2_writel(base, PUER, mask);
+	else
+		pio2_writel(base, PUDR, mask);
 }
 
-void gpio_free(enum device_id gpio_devid, unsigned int start,
-	       unsigned int nr_pins)
+void gpio_select_periph_B(unsigned int pin, int use_pullup)
 {
-	const struct device *gpio;
-	struct pio_state *state;
-	u32 mask;
-
-	state = &pio_state[gpio_devid - DEVICE_PIOA];
-	gpio = state->dev;
-	mask = ((1 << nr_pins) - 1) << start;
-
-	pio2_writel(gpio, ODR, mask);
-	pio2_writel(gpio, PER, mask);
-
-	state->alloc_mask &= ~mask;
-	put_device(gpio);
+	void *base = gpio_pin_to_addr(pin);
+	uint32_t mask = 1 << (pin & 0x1f);
+
+	if (!base)
+		panic("Invalid GPIO pin %u\n", pin);
+
+	pio2_writel(base, BSR, mask);
+	pio2_writel(base, PDR, mask);
+	if (use_pullup)
+		pio2_writel(base, PUER, mask);
+	else
+		pio2_writel(base, PUDR, mask);
 }
diff --git a/cpu/at32ap/pio2.h b/cpu/at32ap/pio2.h
index 6b79de3..9719ea8 100644
--- a/cpu/at32ap/pio2.h
+++ b/cpu/at32ap/pio2.h
@@ -36,9 +36,9 @@
 #define PIO2_OWSR				0x00a8
 
 /* Register access macros */
-#define pio2_readl(port,reg)				\
-	readl((port)->regs + PIO2_##reg)
-#define pio2_writel(port,reg,value)			\
-	writel((value), (port)->regs + PIO2_##reg)
+#define pio2_readl(base,reg)				\
+	readl((void *)base + PIO2_##reg)
+#define pio2_writel(base,reg,value)			\
+	writel((value), (void *)base + PIO2_##reg)
 
 #endif /* __CPU_AT32AP_PIO2_H__ */
diff --git a/cpu/at32ap/pm.c b/cpu/at32ap/pm.c
index 01ac325..c78d547 100644
--- a/cpu/at32ap/pm.c
+++ b/cpu/at32ap/pm.c
@@ -26,138 +26,17 @@
 #include <asm/io.h>
 
 #include <asm/arch/memory-map.h>
-#include <asm/arch/platform.h>
 
 #include "sm.h"
 
-/* Sanity checks */
-#if (CFG_CLKDIV_CPU > CFG_CLKDIV_HSB)		\
-	|| (CFG_CLKDIV_HSB > CFG_CLKDIV_PBA)	\
-	|| (CFG_CLKDIV_HSB > CFG_CLKDIV_PBB)
-# error Constraint fCPU >= fHSB >= fPB{A,B} violated
-#endif
-#if defined(CONFIG_PLL) && ((CFG_PLL0_MUL < 1) || (CFG_PLL0_DIV < 1))
-# error Invalid PLL multiplier and/or divider
-#endif
-
-DECLARE_GLOBAL_DATA_PTR;
-
-struct clock_domain_state {
-	const struct device *bridge;
-	unsigned long freq;
-	u32 mask;
-};
-static struct clock_domain_state ckd_state[NR_CLOCK_DOMAINS];
-
-int pm_enable_clock(enum clock_domain_id id, unsigned int index)
-{
-	const struct clock_domain *ckd = &chip_clock[id];
-	struct clock_domain_state *state = &ckd_state[id];
-
-	if (ckd->bridge != NO_DEVICE) {
-		state->bridge = get_device(ckd->bridge);
-		if (!state->bridge)
-			return -EBUSY;
-	}
-
-	state->mask |= 1 << index;
-	if (gd->sm)
-		writel(state->mask, gd->sm->regs + ckd->reg);
-
-	return 0;
-}
-
-void pm_disable_clock(enum clock_domain_id id, unsigned int index)
-{
-	const struct clock_domain *ckd = &chip_clock[id];
-	struct clock_domain_state *state = &ckd_state[id];
-
-	state->mask &= ~(1 << index);
-	if (gd->sm)
-		writel(state->mask, gd->sm->regs + ckd->reg);
-
-	if (ckd->bridge)
-		put_device(state->bridge);
-}
-
-unsigned long pm_get_clock_freq(enum clock_domain_id domain)
-{
-	return ckd_state[domain].freq;
-}
-
-void pm_init(void)
-{
-	uint32_t cksel = 0;
-	unsigned long main_clock;
-
-	/* Make sure we don't disable any device we're already using */
-	get_device(DEVICE_HRAMC);
-	get_device(DEVICE_HEBI);
-
-	/* Enable the PICO as well */
-	ckd_state[CLOCK_CPU].mask |= 1;
-
-	gd->sm = get_device(DEVICE_SM);
-	if (!gd->sm)
-		panic("Unable to claim system manager device!\n");
-
-	/* Disable any devices that haven't been explicitly claimed */
-	sm_writel(gd->sm, PM_PBB_MASK, ckd_state[CLOCK_PBB].mask);
-	sm_writel(gd->sm, PM_PBA_MASK, ckd_state[CLOCK_PBA].mask);
-	sm_writel(gd->sm, PM_HSB_MASK, ckd_state[CLOCK_HSB].mask);
-	sm_writel(gd->sm, PM_CPU_MASK, ckd_state[CLOCK_CPU].mask);
 
 #ifdef CONFIG_PLL
-	/* Initialize the PLL */
-	main_clock = (CFG_OSC0_HZ / CFG_PLL0_DIV) * CFG_PLL0_MUL;
-
-	sm_writel(gd->sm, PM_PLL0, (SM_BF(PLLCOUNT, CFG_PLL0_SUPPRESS_CYCLES)
-				    | SM_BF(PLLMUL, CFG_PLL0_MUL - 1)
-				    | SM_BF(PLLDIV, CFG_PLL0_DIV - 1)
-				    | SM_BF(PLLOPT, CFG_PLL0_OPT)
-				    | SM_BF(PLLOSC, 0)
-				    | SM_BIT(PLLEN)));
-
-	/* Wait for lock */
-	while (!(sm_readl(gd->sm, PM_ISR) & SM_BIT(LOCK0))) ;
+#define MAIN_CLK_RATE ((CFG_OSC0_HZ / CFG_PLL0_DIV) * CFG_PLL0_MUL)
 #else
-	main_clock = CFG_OSC0_HZ;
+#define MAIN_CLK_RATE (CFG_OSC0_HZ)
 #endif
 
-	/* Set up clocks for the CPU and all peripheral buses */
-	if (CFG_CLKDIV_CPU) {
-		cksel |= SM_BIT(CPUDIV) | SM_BF(CPUSEL, CFG_CLKDIV_CPU - 1);
-		ckd_state[CLOCK_CPU].freq = main_clock / (1 << CFG_CLKDIV_CPU);
-	} else {
-		ckd_state[CLOCK_CPU].freq = main_clock;
-	}
-	if (CFG_CLKDIV_HSB) {
-		cksel |= SM_BIT(HSBDIV) | SM_BF(HSBSEL, CFG_CLKDIV_HSB - 1);
-		ckd_state[CLOCK_HSB].freq = main_clock / (1 << CFG_CLKDIV_HSB);
-	} else {
-		ckd_state[CLOCK_HSB].freq = main_clock;
-	}
-	if (CFG_CLKDIV_PBA) {
-		cksel |= SM_BIT(PBADIV) | SM_BF(PBASEL, CFG_CLKDIV_PBA - 1);
-		ckd_state[CLOCK_PBA].freq = main_clock / (1 << CFG_CLKDIV_PBA);
-	} else {
-		ckd_state[CLOCK_PBA].freq = main_clock;
-	}
-	if (CFG_CLKDIV_PBB) {
-		cksel |= SM_BIT(PBBDIV) | SM_BF(PBBSEL, CFG_CLKDIV_PBB - 1);
-		ckd_state[CLOCK_PBB].freq = main_clock / (1 << CFG_CLKDIV_PBB);
-	} else {
-		ckd_state[CLOCK_PBB].freq = main_clock;
-	}
-	sm_writel(gd->sm, PM_CKSEL, cksel);
-
-	/* CFG_HZ currently depends on cpu_hz */
-	gd->cpu_hz = ckd_state[CLOCK_CPU].freq;
+DECLARE_GLOBAL_DATA_PTR;
 
-#ifdef CONFIG_PLL
-	/* Use PLL0 as main clock */
-	sm_writel(gd->sm, PM_MCCTRL, SM_BIT(PLLSEL));
-#endif
-}
 
 #endif /* CFG_POWER_MANAGER */
diff --git a/cpu/at32ap/sm.h b/cpu/at32ap/sm.h
index ce81ef0..6492c8e 100644
--- a/cpu/at32ap/sm.h
+++ b/cpu/at32ap/sm.h
@@ -196,9 +196,9 @@
 	 | SM_BF(name,value))
 
 /* Register access macros */
-#define sm_readl(port,reg)				\
-	readl((port)->regs + SM_##reg)
-#define sm_writel(port,reg,value)			\
-	writel((value), (port)->regs + SM_##reg)
+#define sm_readl(reg)					\
+	readl((void *)SM_BASE + SM_##reg)
+#define sm_writel(reg,value)				\
+	writel((value), (void *)SM_BASE + SM_##reg)
 
 #endif /* __CPU_AT32AP_SM_H__ */
diff --git a/cpu/at32ap/start.S b/cpu/at32ap/start.S
index 79ee33b..ab8c2b7 100644
--- a/cpu/at32ap/start.S
+++ b/cpu/at32ap/start.S
@@ -70,32 +70,12 @@ _start:
 
 2:	lddpc	sp, sp_init
 
-	/*
-	 * Relocate the data section and initialize .bss.  Everything
-	 * is guaranteed to be at least doubleword aligned by the
-	 * linker script.
-	 */
-	lddpc	r12, .Ldata_vma
-	lddpc	r11, .Ldata_lma
-	lddpc	r10, .Ldata_end
-	sub	r10, r12
-4:	ld.d	r8, r11++
-	sub	r10, 8
-	st.d	r12++, r8
-	brne	4b
-
-	mov	r8, 0
-	mov	r9, 0
-	lddpc	r10, .Lbss_end
-	sub	r10, r12
-4:	sub	r10, 8
-	st.d	r12++, r8
-	brne	4b
-
 	/* Initialize the GOT pointer */
 	lddpc	r6, got_init
 3:	rsub	r6, pc
-	ld.w	pc, r6[start_u_boot@got]
+
+	/* Let's go */
+	rjmp	board_init_f
 
 	.align	2
 	.type	sp_init,@object
@@ -103,11 +83,82 @@ sp_init:
 	.long	CFG_INIT_SP_ADDR
 got_init:
 	.long	3b - _GLOBAL_OFFSET_TABLE_
-.Ldata_lma:
-	.long	__data_lma
-.Ldata_vma:
-	.long	_data
-.Ldata_end:
-	.long	_edata
-.Lbss_end:
-	.long	_end
+
+	/*
+	 * void	relocate_code(new_sp, new_gd, monitor_addr)
+	 *
+	 * Relocate the u-boot image into RAM and continue from there.
+	 * Does not return.
+	 */
+	.global	relocate_code
+	.type	relocate_code,@function
+relocate_code:
+	mov	sp, r12		/* use new stack */
+	mov	r12, r11	/* save new_gd */
+	mov	r11, r10	/* save destination address */
+
+	/* copy .text section and flush the cache along the way */
+	lda.w	r8, _text
+	lda.w	r9, _etext
+	sub	lr, r10, r8	/* relocation offset */
+
+1:	ldm	r8++, r0-r3
+	stm	r10, r0-r3
+	sub	r10, -16
+	ldm	r8++, r0-r3
+	stm	r10, r0-r3
+	sub	r10, -16
+	cp.w	r8, r9
+	cache	r10[-4], 0x0d	/* dcache clean/invalidate */
+	cache	r10[-4], 0x01	/* icache invalidate */
+	brlt	1b
+
+	/* flush write buffer */
+	sync	0
+
+	/* copy data sections */
+	lda.w	r9, _edata
+1:	ld.d	r0, r8++
+	st.d	r10++, r0
+	cp.w	r8, r9
+	brlt	1b
+
+	/* zero out .bss */
+	mov	r0, 0
+	mov	r1, 0
+	lda.w	r9, _end
+	sub	r9, r8
+1:	st.d	r10++, r0
+	sub	r9, 8
+	brgt	1b
+
+	/* jump to RAM */
+	sub	r0, pc, . - in_ram
+	add	pc, r0, lr
+
+	.align	2
+in_ram:
+	/* find the new GOT and relocate it */
+	lddpc	r6, got_init_reloc
+3:	rsub	r6, pc
+	mov	r8, r6
+	lda.w	r9, _egot
+	lda.w	r10, _got
+	sub	r9, r10
+1:	ld.w	r0, r8[0]
+	add	r0, lr
+	st.w	r8++, r0
+	sub	r9, 4
+	brgt	1b
+
+	/* Move the exception handlers */
+	mfsr	r2, SYSREG_EVBA
+	add	r2, lr
+	mtsr	SYSREG_EVBA, r2
+
+	/* Do the rest of the initialization sequence */
+	call	board_init_r
+
+	.align	2
+got_init_reloc:
+	.long	3b - _GLOBAL_OFFSET_TABLE_
diff --git a/cpu/bf533/Makefile b/cpu/bf533/Makefile
index 9f4a0d8..dd4f299 100644
--- a/cpu/bf533/Makefile
+++ b/cpu/bf533/Makefile
@@ -1,6 +1,6 @@
 # U-boot - Makefile
 #
-# Copyright (c) 2005 blackfin.uclinux.org
+# Copyright (c) 2005-2007 Analog Devices Inc.
 #
 # (C) Copyright 2000-2006
 # Wolfgang Denk, DENX Software Engineering, wd@denx.de.
@@ -20,22 +20,24 @@
 #
 # 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
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+# MA 02110-1301 USA
 #
 
 include $(TOPDIR)/config.mk
 
 LIB	= $(obj)lib$(CPU).a
 
-START	= start.o start1.o interrupt.o cache.o cplbhdlr.o cplbmgr.o flush.o
-COBJS	= cpu.o traps.o ints.o serial.o interrupts.o
+START	= start.o start1.o interrupt.o cache.o flush.o init_sdram.o
+COBJS	= cpu.o traps.o ints.o serial.o interrupts.o video.o
+
+EXTRA = init_sdram_bootrom_initblock.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
-OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
+OBJS	:= $(addprefix $(obj),$(COBJS) $(SOBJS))
 START	:= $(addprefix $(obj),$(START))
 
-all:	$(obj).depend $(START) $(LIB)
+all:	$(obj).depend $(START) $(LIB) $(obj).depend $(EXTRA)
 
 $(LIB):	$(OBJS)
 	$(AR) $(ARFLAGS) $@ $(OBJS)
diff --git a/cpu/bf533/bf533_serial.h b/cpu/bf533/bf533_serial.h
index d430e6c..25b96a9 100644
--- a/cpu/bf533/bf533_serial.h
+++ b/cpu/bf533/bf533_serial.h
@@ -1,7 +1,7 @@
 /*
  * U-boot - bf533_serial.h Serial Driver defines
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * This file is based on
  * bf533_serial.h: Definitions for the BlackFin BF533 DSP serial driver.
@@ -38,8 +38,8 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #ifndef _Bf533_SERIAL_H
@@ -63,8 +63,7 @@ int serial_getc(void);
 void serial_puts(const char *s);
 static void local_put_char(char ch);
 
-extern int get_clock(void);
-int baud_table[5] = {9600, 19200, 38400, 57600, 115200};
+int baud_table[5] = { 9600, 19200, 38400, 57600, 115200 };
 
 struct {
 	unsigned char dl_high;
diff --git a/cpu/bf533/cache.S b/cpu/bf533/cache.S
index 8fac402..03aebe4 100644
--- a/cpu/bf533/cache.S
+++ b/cpu/bf533/cache.S
@@ -1,12 +1,11 @@
-
-
 #define ASSEMBLY
 #include <asm/linkage.h>
-#include <asm/cpu/def_LPBlackfin.h>
+#include <config.h>
+#include <asm/blackfin.h>
 
 .text
 .align 2
-ENTRY(blackfin_icache_flush_range)
+ENTRY(_blackfin_icache_flush_range)
 	R2 = -32;
 	R2 = R0 & R2;
 	P0 = R2;
@@ -20,7 +19,7 @@ ENTRY(blackfin_icache_flush_range)
 	SSYNC;
 	RTS;
 
-ENTRY(blackfin_dcache_flush_range)
+ENTRY(_blackfin_dcache_flush_range)
 	R2 = -32;
 	R2 = R0 & R2;
 	P0 = R2;
@@ -35,19 +34,21 @@ ENTRY(blackfin_dcache_flush_range)
 	RTS;
 
 ENTRY(_icache_invalidate)
-ENTRY(invalidate_entire_icache)
-	[--SP] = ( R7:5);
+ENTRY(_invalidate_entire_icache)
+	[--SP] = (R7:5);
 
 	P0.L = (IMEM_CONTROL & 0xFFFF);
 	P0.H = (IMEM_CONTROL >> 16);
-	R7 = [P0];
+	R7 =[P0];
 
-	/* Clear the IMC bit , All valid bits in the instruction
+	/*
+	 * Clear the IMC bit , All valid bits in the instruction
 	 * cache are set to the invalid state
 	 */
-	BITCLR(R7,IMC_P);
+	BITCLR(R7, IMC_P);
 	CLI R6;
-	SSYNC;		/* SSYNC required before invalidating cache. */
+	/* SSYNC required before invalidating cache. */
+	SSYNC;
 	.align 8;
 	[P0] = R7;
 	SSYNC;
@@ -58,54 +59,55 @@ ENTRY(invalidate_entire_icache)
 	R7 = R7 | R6;
 
 	CLI R6;
-	SSYNC;		/* SSYNC required before writing to IMEM_CONTROL. */
+	SSYNC;
 	.align 8;
 	[P0] = R7;
 	SSYNC;
 	STI R6;
 
-	( R7:5) = [SP++];
+	(R7:5) =[SP++];
 	RTS;
 
-/* Invalidate the Entire Data cache by
+/*
+ * Invalidate the Entire Data cache by
  * clearing DMC[1:0] bits
  */
-ENTRY(invalidate_entire_dcache)
+ENTRY(_invalidate_entire_dcache)
 ENTRY(_dcache_invalidate)
-	[--SP] = ( R7:6);
+	[--SP] = (R7:6);
 
 	P0.L = (DMEM_CONTROL & 0xFFFF);
 	P0.H = (DMEM_CONTROL >> 16);
-	R7 = [P0];
+	R7 =[P0];
 
-	/* Clear the DMC[1:0] bits, All valid bits in the data
+	/*
+	 * Clear the DMC[1:0] bits, All valid bits in the data
 	 * cache are set to the invalid state
 	 */
-	BITCLR(R7,DMC0_P);
-	BITCLR(R7,DMC1_P);
+	BITCLR(R7, DMC0_P);
+	BITCLR(R7, DMC1_P);
 	CLI R6;
-	SSYNC;		/* SSYNC required before writing to DMEM_CONTROL. */
+	SSYNC;
 	.align 8;
 	[P0] = R7;
 	SSYNC;
 	STI R6;
-
 	/* Configures the data cache again */
 
 	R6 = (ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
 	R7 = R7 | R6;
 
 	CLI R6;
-	SSYNC;		/* SSYNC required before writing to DMEM_CONTROL. */
+	SSYNC;
 	.align 8;
 	[P0] = R7;
 	SSYNC;
 	STI R6;
 
-	( R7:6) = [SP++];
+	(R7:6) =[SP++];
 	RTS;
 
-ENTRY(blackfin_dcache_invalidate_range)
+ENTRY(_blackfin_dcache_invalidate_range)
 	R2 = -32;
 	R2 = R0 & R2;
 	P0 = R2;
@@ -113,13 +115,14 @@ ENTRY(blackfin_dcache_invalidate_range)
 	CSYNC;
 1:
 	FLUSHINV[P0++];
-	CC = P0 < P1 (iu);
-	IF CC JUMP 1b (bp);
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
 
-	/* If the data crosses a cache line, then we'll be pointing to
-	** the last cache line, but won't have flushed/invalidated it yet, so do
-	** one more.
-	*/
+	/*
+	 * If the data crosses a cache line, then we'll be pointing to
+	 * the last cache line, but won't have flushed/invalidated it yet, so do
+	 * one more.
+	 */
 	FLUSHINV[P0];
 	SSYNC;
 	RTS;
diff --git a/cpu/bf533/config.mk b/cpu/bf533/config.mk
index a9d529e..6a713c3 100644
--- a/cpu/bf533/config.mk
+++ b/cpu/bf533/config.mk
@@ -1,6 +1,6 @@
 # U-boot - config.mk
 #
-# Copyright (c) 2005 blackfin.uclinux.org
+# Copyright (c) 2005-2007 Analog Devices Inc.
 #
 # (C) Copyright 2000-2004
 # Wolfgang Denk, DENX Software Engineering, wd@denx.de.
@@ -20,8 +20,8 @@
 #
 # 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
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+# MA 02110-1301 USA
 #
 
-PLATFORM_RELFLAGS += -ffixed-P5
+PLATFORM_RELFLAGS += -mcpu=bf533 -ffixed-P5
diff --git a/cpu/bf533/cplbhdlr.S b/cpu/bf533/cplbhdlr.S
deleted file mode 100644
index 61be5bb..0000000
--- a/cpu/bf533/cplbhdlr.S
+++ /dev/null
@@ -1,193 +0,0 @@
-/* Copyright (C) 2003 Analog Devices, Inc. All Rights Reserved.
- *
- * This file is subject to the terms and conditions of the GNU General Public
- * License.
- *
- * Blackfin BF533/2.6 support : LG Soft India
- */
-
-
-/* Include an exception handler to invoke the CPLB manager
- */
-
-#include <asm-blackfin/linkage.h>
-#include <asm/cplb.h>
-#include <asm/entry.h>
-
-
-.text
-
-.globl _cplb_hdr;
-.type _cplb_hdr, STT_FUNC;
-.extern _cplb_mgr;
-.type _cplb_mgr, STT_FUNC;
-.extern __unknown_exception_occurred;
-.type __unknown_exception_occurred, STT_FUNC;
-.extern __cplb_miss_all_locked;
-.type __cplb_miss_all_locked, STT_FUNC;
-.extern __cplb_miss_without_replacement;
-.type __cplb_miss_without_replacement, STT_FUNC;
-.extern __cplb_protection_violation;
-.type __cplb_protection_violation, STT_FUNC;
-.extern panic_pv;
-
-.align 2;
-
-ENTRY(_cplb_hdr)
-	SSYNC;
-	[--SP] = ( R7:0, P5:0 );
-	[--SP] = ASTAT;
-	[--SP] = SEQSTAT;
-	[--SP] = I0;
-	[--SP] = I1;
-	[--SP] = I2;
-	[--SP] = I3;
-	[--SP] = LT0;
-	[--SP] = LB0;
-	[--SP] = LC0;
-	[--SP] = LT1;
-	[--SP] = LB1;
-	[--SP] = LC1;
-	R2 = SEQSTAT;
-
-	/*Mask the contents of SEQSTAT and leave only EXCAUSE in R2*/
-	R2 <<= 26;
-	R2 >>= 26;
-
-	R1 = 0x23; /* Data access CPLB protection violation */
-	CC = R2 == R1;
-	IF !CC JUMP not_data_write;
-	R0 = 2;		/* is a write to data space*/
-	JUMP is_icplb_miss;
-
-not_data_write:
-	R1 = 0x2C; /* CPLB miss on an instruction fetch */
-	CC = R2 == R1;
-	R0 = 0;		/* is_data_miss == False*/
-	IF CC JUMP is_icplb_miss;
-
-	R1 = 0x26;
-	CC = R2 == R1;
-	IF !CC JUMP unknown;
-
-	R0 = 1;		/* is_data_miss == True*/
-
-is_icplb_miss:
-
-#if ( defined (CONFIG_BLKFIN_CACHE) || defined (CONFIG_BLKFIN_DCACHE))
-#if ( defined (CONFIG_BLKFIN_CACHE) && !defined (CONFIG_BLKFIN_DCACHE))
-	R1 = CPLB_ENABLE_ICACHE;
-#endif
-#if ( !defined (CONFIG_BLKFIN_CACHE) && defined (CONFIG_BLKFIN_DCACHE))
-	R1 = CPLB_ENABLE_DCACHE;
-#endif
-#if ( defined (CONFIG_BLKFIN_CACHE) && defined (CONFIG_BLKFIN_DCACHE))
-	R1 = CPLB_ENABLE_DCACHE | CPLB_ENABLE_ICACHE;
-#endif
-#else
-	R1 = 0;
-#endif
-
-	[--SP] = RETS;
-	CALL _cplb_mgr;
-	RETS = [SP++];
-	CC = R0 == 0;
-	IF !CC JUMP not_replaced;
-	LC1 = [SP++];
-	LB1 = [SP++];
-	LT1 = [SP++];
-	LC0 = [SP++];
-	LB0 = [SP++];
-	LT0 = [SP++];
-	I3 = [SP++];
-	I2 = [SP++];
-	I1 = [SP++];
-	I0 = [SP++];
-	SEQSTAT = [SP++];
-	ASTAT = [SP++];
-	( R7:0, P5:0 ) = [SP++];
-	RTS;
-
-unknown:
-	[--SP] = RETS;
-	CALL __unknown_exception_occurred;
-	RETS = [SP++];
-	JUMP unknown;
-not_replaced:
-	CC = R0 == CPLB_NO_UNLOCKED;
-	IF !CC JUMP next_check;
-	[--SP] = RETS;
-	CALL __cplb_miss_all_locked;
-	RETS = [SP++];
-next_check:
-	CC = R0 == CPLB_NO_ADDR_MATCH;
-	IF !CC JUMP next_check2;
-	[--SP] = RETS;
-	CALL __cplb_miss_without_replacement;
-	RETS = [SP++];
-	JUMP not_replaced;
-next_check2:
-	CC = R0 == CPLB_PROT_VIOL;
-	IF !CC JUMP strange_return_from_cplb_mgr;
-	[--SP] = RETS;
-	CALL __cplb_protection_violation;
-	RETS = [SP++];
-	JUMP not_replaced;
-strange_return_from_cplb_mgr:
-	IDLE;
-	CSYNC;
-	JUMP strange_return_from_cplb_mgr;
-
-/************************************
- * Diagnostic exception handlers
- */
-
-__cplb_miss_all_locked:
-	sp += -12;
-	R0 = CPLB_NO_UNLOCKED;
-	call panic_bfin;
-	SP += 12;
-	RTS;
-
- __cplb_miss_without_replacement:
-	sp += -12;
-	R0 = CPLB_NO_ADDR_MATCH;
-	call panic_bfin;
-	SP += 12;
-	RTS;
-
-__cplb_protection_violation:
-	sp += -12;
-	R0 = CPLB_PROT_VIOL;
-	call panic_bfin;
-	SP += 12;
-	RTS;
-
-__unknown_exception_occurred:
-
-	/* This function is invoked by the default exception
-	 * handler, if it does not recognise the kind of
-	 * exception that has occurred. In other words, the
-	 * default handler only handles some of the system's
-	 * exception types, and it does not expect any others
-	 * to occur. If your application is going to be using
-	 * other kinds of exceptions, you must replace the
-	 * default handler with your own, that handles all the
-	 * exceptions you will use.
-	 *
-	 * Since there's nothing we can do, we just loop here
-	 * at what we hope is a suitably informative label.
-	 */
-
-	IDLE;
-do_not_know_what_to_do:
-	CSYNC;
-	JUMP __unknown_exception_occurred;
-
-	RTS;
-.__unknown_exception_occurred.end:
-.global __unknown_exception_occurred;
-.type __unknown_exception_occurred, STT_FUNC;
-
-panic_bfin:
-	RTS;
diff --git a/cpu/bf533/cplbmgr.S b/cpu/bf533/cplbmgr.S
deleted file mode 100644
index 7a0b048..0000000
--- a/cpu/bf533/cplbmgr.S
+++ /dev/null
@@ -1,601 +0,0 @@
-/*This file is subject to the terms and conditions of the GNU General Public
- * License.
- *
- * Blackfin BF533/2.6 support : LG Soft India
- * Modification: Dec 07 2004
- *	1. Correction in icheck_lock.  Valid lock entries were
- *	   geting victimized, for instruction cplb replacement.
- *	2. Setup loop's are modified as now toolchain support's P Indexed
- *	   addressing
- *	   :LG Soft India
- *
- */
-
-/* Usage: int _cplb_mgr(is_data_miss,int enable_cache)
- * is_data_miss==2 => Mark as Dirty, write to the clean data page
- * is_data_miss==1 => Replace a data CPLB.
- * is_data_miss==0 => Replace an instruction CPLB.
- *
- * Returns:
- * CPLB_RELOADED	=> Successfully updated CPLB table.
- * CPLB_NO_UNLOCKED	=> All CPLBs are locked, so cannot be evicted.This indicates
- *				that the CPLBs in the configuration tablei are badly
- *				configured, as this should never occur.
- * CPLB_NO_ADDR_MATCH	=> The address being accessed, that triggered the exception,
- *				is not covered by any of the CPLBs in the configuration
- *				table. The application isi presumably misbehaving.
- * CPLB_PROT_VIOL	=> The address being accessed, that triggered thei exception,
- *				was not a first-write to a clean Write Back Data page,
- *				and so presumably is a genuine violation of the page's
- *				protection attributes. The application is misbehaving.
- */
-#define ASSEMBLY
-
-#include <asm-blackfin/linkage.h>
-#include <asm-blackfin/blackfin.h>
-#include <asm-blackfin/cplbtab.h>
-#include <asm-blackfin/cplb.h>
-
-.text
-
-.align 2;
-ENTRY(_cplb_mgr)
-
-	[--SP]=( R7:0,P5:0 );
-
-	CC = R0 == 2;
-	IF CC JUMP dcplb_write;
-
-	CC = R0 == 0;
-	IF !CC JUMP dcplb_miss_compare;
-
-	/* ICPLB Miss Exception. We need to choose one of the
-	* currently-installed CPLBs, and replace it with one
-	* from the configuration table.
-	*/
-
-	P4.L = (ICPLB_FAULT_ADDR & 0xFFFF);
-	P4.H = (ICPLB_FAULT_ADDR >> 16);
-
-	P1 = 16;
-	P5.L = page_size_table;
-	P5.H = page_size_table;
-
-	P0.L = (ICPLB_DATA0 & 0xFFFF);
-	P0.H = (ICPLB_DATA0 >> 16);
-	R4 = [P4];		/* Get faulting address*/
-	R6 = 64;		/* Advance past the fault address, which*/
-	R6 = R6 + R4;		/* we'll use if we find a match*/
-	R3 = ((16 << 8) | 2);	/* Extract mask, bits 16 and 17.*/
-
-	R5 = 0;
-isearch:
-
-	R1 = [P0-0x100];	/* Address for this CPLB */
-
-	R0 = [P0++];		/* Info for this CPLB*/
-	CC = BITTST(R0,0);	/* Is the CPLB valid?*/
-	IF !CC JUMP nomatch;	/* Skip it, if not.*/
-	CC = R4 < R1(IU);	/* If fault address less than page start*/
-	IF CC JUMP nomatch;	/* then skip this one.*/
-	R2 = EXTRACT(R0,R3.L) (Z);	/* Get page size*/
-	P1 = R2;
-	P1 = P5 + (P1<<2);	/* index into page-size table*/
-	R2 = [P1];		/* Get the page size*/
-	R1 = R1 + R2;		/* and add to page start, to get page end*/
-	CC = R4 < R1(IU);	/* and see whether fault addr is in page.*/
-	IF !CC R4 = R6;		/* If so, advance the address and finish loop.*/
-	IF !CC JUMP isearch_done;
-nomatch:
-	/* Go around again*/
-	R5 += 1;
-	CC = BITTST(R5, 4);	/* i.e CC = R5 >= 16*/
-	IF !CC JUMP isearch;
-
-isearch_done:
-	I0 = R4;		/* Fault address we'll search for*/
-
-	/* set up pointers */
-	P0.L = (ICPLB_DATA0 & 0xFFFF);
-	P0.H = (ICPLB_DATA0 >> 16);
-
-	/* The replacement procedure for ICPLBs */
-
-	P4.L = (IMEM_CONTROL & 0xFFFF);
-	P4.H = (IMEM_CONTROL >> 16);
-
-	/* disable cplbs */
-	R5 = [P4];		/* Control Register*/
-	BITCLR(R5,ENICPLB_P);
-	CLI R1;
-	SSYNC;		/* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R1;
-
-	R1 = -1;		/* end point comparison */
-	R3 = 16;		/* counter */
-
-	/* Search through CPLBs for first non-locked entry */
-	/* Overwrite it by moving everyone else up by 1 */
-icheck_lock:
-	R0 = [P0++];
-	R3 = R3 + R1;
-	CC = R3 == R1;
-	IF CC JUMP all_locked;
-	CC = BITTST(R0, 0);		/* an invalid entry is good */
-	IF !CC JUMP ifound_victim;
-	CC = BITTST(R0,1);		/* but a locked entry isn't */
-	IF CC JUMP icheck_lock;
-
-ifound_victim:
-#ifdef CONFIG_CPLB_INFO
-	R7 = [P0 - 0x104];
-	P2.L = ipdt_table;
-	P2.H = ipdt_table;
-	P3.L = ipdt_swapcount_table;
-	P3.H = ipdt_swapcount_table;
-	P3 += -4;
-icount:
-	R2 = [P2];	/* address from config table */
-	P2 += 8;
-	P3 += 8;
-	CC = R2==-1;
-	IF CC JUMP icount_done;
-	CC = R7==R2;
-	IF !CC JUMP icount;
-	R7 = [P3];
-	R7 += 1;
-	[P3] = R7;
-	CSYNC;
-icount_done:
-#endif
-	LC0=R3;
-	LSETUP(is_move,ie_move) LC0;
-is_move:
-	R0 = [P0];
-	[P0 - 4] = R0;
-	R0 = [P0 - 0x100];
-	[P0-0x104] = R0;
-ie_move:P0+=4;
-
-	/* We've made space in the ICPLB table, so that ICPLB15
-	 * is now free to be overwritten. Next, we have to determine
-	 * which CPLB we need to install, from the configuration
-	 * table. This is a matter of getting the start-of-page
-	 * addresses and page-lengths from the config table, and
-	 * determining whether the fault address falls within that
-	 * range.
-	 */
-
-	P2.L = ipdt_table;
-	P2.H = ipdt_table;
-#ifdef	CONFIG_CPLB_INFO
-	P3.L = ipdt_swapcount_table;
-	P3.H = ipdt_swapcount_table;
-	P3 += -8;
-#endif
-	P0.L = page_size_table;
-	P0.H = page_size_table;
-
-	/* Retrieve our fault address (which may have been advanced
-	 * because the faulting instruction crossed a page boundary).
-	 */
-
-	R0 = I0;
-
-	/* An extraction pattern, to get the page-size bits from
-	 * the CPLB data entry. Bits 16-17, so two bits at posn 16.
-	 */
-
-	R1 = ((16<<8)|2);
-inext:	R4 = [P2++];	/* address from config table */
-	R2 = [P2++];	/* data from config table */
-#ifdef	CONFIG_CPLB_INFO
-	P3 += 8;
-#endif
-
-	CC = R4 == -1;	/* End of config table*/
-	IF CC JUMP no_page_in_table;
-
-	/* See if failed address > start address */
-	CC = R4 <= R0(IU);
-	IF !CC JUMP inext;
-
-	/* extract page size (17:16)*/
-	R3 = EXTRACT(R2, R1.L) (Z);
-
-	/* add page size to addr to get range */
-
-	P5 = R3;
-	P5 = P0 + (P5 << 2);	/* scaled, for int access*/
-	R3 = [P5];
-	R3 = R3 + R4;
-
-	/* See if failed address < (start address + page size) */
-	CC = R0 < R3(IU);
-	IF !CC JUMP inext;
-
-	/* We've found a CPLB in the config table that covers
-	 * the faulting address, so install this CPLB into the
-	 * last entry of the table.
-	 */
-
-	P1.L = (ICPLB_DATA15 & 0xFFFF);		/*ICPLB_DATA15*/
-	P1.H = (ICPLB_DATA15 >> 16);
-	[P1] = R2;
-	[P1-0x100] = R4;
-#ifdef	CONFIG_CPLB_INFO
-	R3 = [P3];
-	R3 += 1;
-	[P3] = R3;
-#endif
-
-	/* P4 points to IMEM_CONTROL, and R5 contains its old
-	 * value, after we disabled ICPLBS. Re-enable them.
-	 */
-
-	BITSET(R5,ENICPLB_P);
-	CLI R2;
-	SSYNC;		/* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:0,P5:0 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-
-/* FAILED CASES*/
-no_page_in_table:
-	( R7:0,P5:0 ) = [SP++];
-	R0 = CPLB_NO_ADDR_MATCH;
-	RTS;
-all_locked:
-	( R7:0,P5:0 ) = [SP++];
-	R0 = CPLB_NO_UNLOCKED;
-	RTS;
-prot_violation:
-	( R7:0,P5:0 ) = [SP++];
-	R0 = CPLB_PROT_VIOL;
-	RTS;
-
-dcplb_write:
-
-	/* if a DCPLB is marked as write-back (CPLB_WT==0), and
-	 * it is clean (CPLB_DIRTY==0), then a write to the
-	 * CPLB's page triggers a protection violation. We have to
-	 * mark the CPLB as dirty, to indicate that there are
-	 * pending writes associated with the CPLB.
-	 */
-
-	P4.L = (DCPLB_STATUS & 0xFFFF);
-	P4.H = (DCPLB_STATUS >> 16);
-	P3.L = (DCPLB_DATA0 & 0xFFFF);
-	P3.H = (DCPLB_DATA0 >> 16);
-	R5 = [P4];
-
-	/* A protection violation can be caused by more than just writes
-	 * to a clean WB page, so we have to ensure that:
-	 * - It's a write
-	 * - to a clean WB page
-	 * - and is allowed in the mode the access occurred.
-	 */
-
-	CC = BITTST(R5, 16);	/* ensure it was a write*/
-	IF !CC JUMP prot_violation;
-
-	/* to check the rest, we have to retrieve the DCPLB.*/
-
-	/* The low half of DCPLB_STATUS is a bit mask*/
-
-	R2 = R5.L (Z);	/* indicating which CPLB triggered the event.*/
-	R3 = 30;	/* so we can use this to determine the offset*/
-	R2.L = SIGNBITS R2;
-	R2 = R2.L (Z);	/* into the DCPLB table.*/
-	R3 = R3 - R2;
-	P4 = R3;
-	P3 = P3 + (P4<<2);
-	R3 = [P3];	/* Retrieve the CPLB*/
-
-	/* Now we can check whether it's a clean WB page*/
-
-	CC = BITTST(R3, 14);	/* 0==WB, 1==WT*/
-	IF CC JUMP prot_violation;
-	CC = BITTST(R3, 7);	/* 0 == clean, 1 == dirty*/
-	IF CC JUMP prot_violation;
-
-	/* Check whether the write is allowed in the mode that was active.*/
-
-	R2 = 1<<3;		/* checking write in user mode*/
-	CC = BITTST(R5, 17);	/* 0==was user, 1==was super*/
-	R5 = CC;
-	R2 <<= R5;		/* if was super, check write in super mode*/
-	R2 = R3 & R2;
-	CC = R2 == 0;
-	IF CC JUMP prot_violation;
-
-	/* It's a genuine write-to-clean-page.*/
-
-	BITSET(R3, 7);		/* mark as dirty*/
-	[P3] = R3;		/* and write back.*/
-	CSYNC;
-	( R7:0,P5:0 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-
-dcplb_miss_compare:
-
-	/* Data CPLB Miss event. We need to choose a CPLB to
-	 * evict, and then locate a new CPLB to install from the
-	 * config table, that covers the faulting address.
-	 */
-
-	P1.L = (DCPLB_DATA15 & 0xFFFF);
-	P1.H = (DCPLB_DATA15 >> 16);
-
-	P4.L = (DCPLB_FAULT_ADDR & 0xFFFF);
-	P4.H = (DCPLB_FAULT_ADDR >> 16);
-	R4 = [P4];
-	I0 = R4;
-
-	/* The replacement procedure for DCPLBs*/
-
-	R6 = R1;	/* Save for later*/
-
-	/* Turn off CPLBs while we work.*/
-	P4.L = (DMEM_CONTROL & 0xFFFF);
-	P4.H = (DMEM_CONTROL >> 16);
-	R5 = [P4];
-	BITCLR(R5,ENDCPLB_P);
-	CLI R0;
-	SSYNC;		/* SSYNC required before writing to DMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R0;
-
-	/* Start looking for a CPLB to evict. Our order of preference
-	 * is: invalid CPLBs, clean CPLBs, dirty CPLBs. Locked CPLBs
-	 * are no good.
-	 */
-
-	I1.L = (DCPLB_DATA0 & 0xFFFF);
-	I1.H = (DCPLB_DATA0 >> 16);
-	P1 = 3;
-	P2 = 16;
-	I2.L = dcplb_preference;
-	I2.H = dcplb_preference;
-	LSETUP(sdsearch1, edsearch1) LC0 = P1;
-sdsearch1:
-	R0 = [I2++];		/* Get the bits we're interested in*/
-	P0 = I1;		/* Go back to start of table*/
-	LSETUP (sdsearch2, edsearch2) LC1 = P2;
-sdsearch2:
-	R1 = [P0++];		/* Fetch each installed CPLB in turn*/
-	R2 = R1 & R0;		/* and test for interesting bits.*/
-	CC = R2 == 0;		/* If none are set, it'll do.*/
-	IF !CC JUMP skip_stack_check;
-
-	R2 = [P0 - 0x104];	/* R2 - PageStart */
-	P3.L = page_size_table; /* retrive end address */
-	P3.H = page_size_table; /* retrive end address */
-	R3 = 0x2;		/* 0th - position, 2 bits -length */
-	nop;			/*Anamoly 05000209*/
-	R7 = EXTRACT(R1,R3.l);
-	R7 = R7 << 2;		/* Page size index offset */
-	P5 = R7;
-	P3 = P3 + P5;
-	R7 = [P3];		/* page size in 1K bytes */
-
-	R7 = R7 << 0xA;		/* in bytes * 1024*/
-	R7 = R2 + R7;		/* R7 - PageEnd */
-	R4 = SP;		/* Test SP is in range */
-
-	CC = R7 < R4;		/* if PageEnd < SP */
-	IF CC JUMP dfound_victim;
-	R3 = 0x284;		/* stack length from start of trap till the point */
-				/* 20 stack locations for future modifications */
-	R4 = R4 + R3;
-	CC = R4 < R2;		/* if SP + stacklen < PageStart */
-	IF CC JUMP dfound_victim;
-skip_stack_check:
-
-edsearch2: NOP;
-edsearch1: NOP;
-
-	/* If we got here, we didn't find a DCPLB we considered
-	 * replacable, which means all of them were locked.
-	 */
-
-	JUMP all_locked;
-dfound_victim:
-
-#ifdef CONFIG_CPLB_INFO
-	R1 = [P0 - 0x104];
-	P2.L = dpdt_table;
-	P2.H = dpdt_table;
-	P3.L = dpdt_swapcount_table;
-	P3.H = dpdt_swapcount_table;
-	P3 += -4;
-dicount:
-	R2 = [P2];
-	P2 += 8;
-	P3 += 8;
-	CC = R2==-1;
-	IF CC JUMP dicount_done;
-	CC = R1==R2;
-	IF !CC JUMP dicount;
-	R1 = [P3];
-	R1 += 1;
-	[P3] = R1;
-	CSYNC;
-dicount_done:
-#endif
-
-	/* Clean down the hardware loops*/
-	R2 = 0;
-	LC1 = R2;
-	LC0 = R2;
-
-	/* There's a suitable victim in [P0-4] (because we've
-	 * advanced already). If it's a valid dirty write-back
-	 * CPLB, we need to flush the pending writes first.
-	 */
-
-	CC = BITTST(R1, 0);	/* Is it valid?*/
-	IF !CC JUMP Ddoverwrite;/* nope.*/
-	CC = BITTST(R1, 7);	/* Is it dirty?*/
-	IF !CC JUMP Ddoverwrite (BP);	/* Nope.*/
-	CC = BITTST(R1, 14);	/* Is it Write-Through?*/
-	IF CC JUMP Ddoverwrite; /* Yep*/
-
-	/* This is a dirty page, so we need to flush all writes
-	 * that are pending on the page.
-	 */
-
-	/* Retrieve the page start address*/
-	R0 = [P0 - 0x104];
-	[--sp] = rets;
-	CALL dcplb_flush;	/* R0==CPLB addr, R1==CPLB data*/
-	rets = [sp++];
-Ddoverwrite:
-
-	/* [P0-4] is a suitable victim CPLB, so we want to
-	 * overwrite it by moving all the following CPLBs
-	 * one space closer to the start.
-	 */
-
-	R1.L = ((DCPLB_DATA15+4) & 0xFFFF);		/*DCPLB_DATA15+4*/
-	R1.H = ((DCPLB_DATA15+4) >> 16);
-	R0 = P0;
-
-	/* If the victim happens to be in DCPLB15,
-	 * we don't need to move anything.
-	 */
-
-	CC = R1 == R0;
-	IF CC JUMP de_moved;
-	R1 = R1 - R0;
-	R1 >>= 2;
-	P1 = R1;
-	LSETUP(ds_move, de_move) LC0=P1;
-ds_move:
-	 R0 = [P0++];	/* move data */
-	[P0 - 8] = R0;
-	R0 = [P0-0x104] /* move address */
-de_move: [P0-0x108] = R0;
-
-	/* We've now made space in DCPLB15 for the new CPLB to be
-	 * installed. The next stage is to locate a CPLB in the
-	 * config table that covers the faulting address.
-	 */
-
-de_moved:NOP;
-	R0 = I0;		/* Our faulting address */
-
-	P2.L = dpdt_table;
-	P2.H = dpdt_table;
-#ifdef	CONFIG_CPLB_INFO
-	P3.L = dpdt_swapcount_table;
-	P3.H = dpdt_swapcount_table;
-	P3 += -8;
-#endif
-
-	P1.L = page_size_table;
-	P1.H = page_size_table;
-
-	/* An extraction pattern, to retrieve bits 17:16.*/
-
-	R1 = (16<<8)|2;
-dnext:	R4 = [P2++];	/* address */
-	R2 = [P2++];	/* data */
-#ifdef	CONFIG_CPLB_INFO
-	P3 += 8;
-#endif
-
-	CC = R4 == -1;
-	IF CC JUMP no_page_in_table;
-
-	/* See if failed address > start address */
-	CC = R4 <= R0(IU);
-	IF !CC JUMP dnext;
-
-	/* extract page size (17:16)*/
-	R3 = EXTRACT(R2, R1.L) (Z);
-
-	/* add page size to addr to get range */
-
-	P5 = R3;
-	P5 = P1 + (P5 << 2);
-	R3 = [P5];
-	R3 = R3 + R4;
-
-	/* See if failed address < (start address + page size) */
-	CC = R0 < R3(IU);
-	IF !CC JUMP dnext;
-
-	/* We've found the CPLB that should be installed, so
-	 * write it into CPLB15, masking off any caching bits
-	 * if necessary.
-	 */
-
-	P1.L = (DCPLB_DATA15 & 0xFFFF);
-	P1.H = (DCPLB_DATA15 >> 16);
-
-	/* If the DCPLB has cache bits set, but caching hasn't
-	 * been enabled, then we want to mask off the cache-in-L1
-	 * bit before installing. Moreover, if caching is off, we
-	 * also want to ensure that the DCPLB has WT mode set, rather
-	 * than WB, since WB pages still trigger first-write exceptions
-	 * even when not caching is off, and the page isn't marked as
-	 * cachable. Finally, we could mark the page as clean, not dirty,
-	 * but we choose to leave that decision to the user; if the user
-	 * chooses to have a CPLB pre-defined as dirty, then they always
-	 * pay the cost of flushing during eviction, but don't pay the
-	 * cost of first-write exceptions to mark the page as dirty.
-	 */
-
-#ifdef CONFIG_BLKFIN_WT
-	BITSET(R6, 14);		/* Set WT*/
-#endif
-
-	[P1] = R2;
-	[P1-0x100] = R4;
-#ifdef	CONFIG_CPLB_INFO
-	R3 = [P3];
-	R3 += 1;
-	[P3] = R3;
-#endif
-
-	/* We've installed the CPLB, so re-enable CPLBs. P4
-	 * points to DMEM_CONTROL, and R5 is the value we
-	 * last wrote to it, when we were disabling CPLBs.
-	 */
-
-	BITSET(R5,ENDCPLB_P);
-	CLI R2;
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:0,P5:0 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-
-.data
-.align 4;
-page_size_table:
-.byte4	0x00000400;	/* 1K */
-.byte4	0x00001000;	/* 4K */
-.byte4	0x00100000;	/* 1M */
-.byte4	0x00400000;	/* 4M */
-
-.align 4;
-dcplb_preference:
-.byte4	0x00000001;	/* valid bit */
-.byte4	0x00000082;	/* dirty+lock bits */
-.byte4	0x00000002;	/* lock bit */
diff --git a/cpu/bf533/cpu.c b/cpu/bf533/cpu.c
index 78e2b96..8118861 100644
--- a/cpu/bf533/cpu.c
+++ b/cpu/bf533/cpu.c
@@ -1,7 +1,7 @@
 /*
  * U-boot - cpu.c CPU specific functions
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * (C) Copyright 2000-2004
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
@@ -21,80 +21,27 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #include <common.h>
 #include <asm/blackfin.h>
 #include <command.h>
 #include <asm/entry.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
 
-#define SSYNC() asm("ssync;")
 #define CACHE_ON 1
 #define CACHE_OFF 0
 
-/* Data Attibutes*/
-
-#define SDRAM_IGENERIC		(PAGE_SIZE_4MB | CPLB_L1_CHBL | CPLB_USER_RD | CPLB_VALID)
-#define SDRAM_IKERNEL		(PAGE_SIZE_4MB | CPLB_L1_CHBL | CPLB_USER_RD | CPLB_VALID | CPLB_LOCK)
-#define L1_IMEMORY            	(PAGE_SIZE_1MB | CPLB_L1_CHBL | CPLB_USER_RD | CPLB_VALID | CPLB_LOCK)
-#define SDRAM_INON_CHBL		(PAGE_SIZE_4MB | CPLB_USER_RD | CPLB_VALID)
-
-#define ANOMALY_05000158		0x200
-#define SDRAM_DGENERIC          (PAGE_SIZE_4MB | CPLB_L1_CHBL | CPLB_WT | CPLB_L1_AOW | CPLB_SUPV_WR | CPLB_USER_RD | CPLB_USER_WR | CPLB_VALID | ANOMALY_05000158)
-#define SDRAM_DNON_CHBL         (PAGE_SIZE_4MB | CPLB_WT | CPLB_L1_AOW | CPLB_SUPV_WR | CPLB_USER_WR | CPLB_USER_RD | CPLB_VALID | ANOMALY_05000158)
-#define SDRAM_DKERNEL           (PAGE_SIZE_4MB | CPLB_L1_CHBL | CPLB_WT | CPLB_L1_AOW | CPLB_USER_RD | CPLB_SUPV_WR | CPLB_USER_WR | CPLB_VALID | CPLB_LOCK | ANOMALY_05000158)
-#define L1_DMEMORY              (PAGE_SIZE_4KB | CPLB_L1_CHBL | CPLB_L1_AOW | CPLB_WT | CPLB_SUPV_WR | CPLB_USER_WR | CPLB_VALID | ANOMALY_05000158)
-#define SDRAM_EBIU              (PAGE_SIZE_4MB | CPLB_WT | CPLB_L1_AOW | CPLB_USER_RD | CPLB_USER_WR | CPLB_SUPV_WR | CPLB_VALID | ANOMALY_05000158)
-
-static unsigned int icplb_table[16][2]={
-			{0xFFA00000, L1_IMEMORY},
-			{0x00000000, SDRAM_IKERNEL},	/*SDRAM_Page1*/
-			{0x00400000, SDRAM_IKERNEL},	/*SDRAM_Page1*/
-			{0x07C00000, SDRAM_IKERNEL},    /*SDRAM_Page14*/
-			{0x00800000, SDRAM_IGENERIC},	/*SDRAM_Page2*/
-			{0x00C00000, SDRAM_IGENERIC},	/*SDRAM_Page2*/
-			{0x01000000, SDRAM_IGENERIC},	/*SDRAM_Page4*/
-			{0x01400000, SDRAM_IGENERIC},	/*SDRAM_Page5*/
-			{0x01800000, SDRAM_IGENERIC},	/*SDRAM_Page6*/
-			{0x01C00000, SDRAM_IGENERIC},	/*SDRAM_Page7*/
-			{0x02000000, SDRAM_IGENERIC},	/*SDRAM_Page8*/
-			{0x02400000, SDRAM_IGENERIC},	/*SDRAM_Page9*/
-			{0x02800000, SDRAM_IGENERIC},	/*SDRAM_Page10*/
-			{0x02C00000, SDRAM_IGENERIC},	/*SDRAM_Page11*/
-			{0x03000000, SDRAM_IGENERIC},	/*SDRAM_Page12*/
-			{0x03400000, SDRAM_IGENERIC},	/*SDRAM_Page13*/
-};
-
-static unsigned int dcplb_table[16][2]={
-			{0xFFA00000,L1_DMEMORY},
-			{0x00000000,SDRAM_DKERNEL},	/*SDRAM_Page1*/
-			{0x00400000,SDRAM_DKERNEL},	/*SDRAM_Page1*/
-			{0x07C00000,SDRAM_DKERNEL},	/*SDRAM_Page15*/
-			{0x00800000,SDRAM_DGENERIC},	/*SDRAM_Page2*/
-			{0x00C00000,SDRAM_DGENERIC},	/*SDRAM_Page3*/
-			{0x01000000,SDRAM_DGENERIC},	/*SDRAM_Page4*/
-			{0x01400000,SDRAM_DGENERIC},	/*SDRAM_Page5*/
-			{0x01800000,SDRAM_DGENERIC},	/*SDRAM_Page6*/
-			{0x01C00000,SDRAM_DGENERIC},	/*SDRAM_Page7*/
-			{0x02000000,SDRAM_DGENERIC},	/*SDRAM_Page8*/
-			{0x02400000,SDRAM_DGENERIC},	/*SDRAM_Page9*/
-			{0x02800000,SDRAM_DGENERIC},	/*SDRAM_Page10*/
-			{0x02C00000,SDRAM_DGENERIC},	/*SDRAM_Page11*/
-			{0x03000000,SDRAM_DGENERIC},	/*SDRAM_Page12*/
-			{0x20000000,SDRAM_EBIU},	/*For Network */
-};
-
-int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 {
-	__asm__ __volatile__
-	("cli r3;"
-	"P0 = %0;"
-	"JUMP (P0);"
-	:
-	: "r" (L1_ISRAM)
-	);
+	__asm__ __volatile__("cli r3;" "P0 = %0;" "JUMP (P0);"::"r"(L1_ISRAM)
+	    );
 
 	return 0;
 }
@@ -112,29 +59,62 @@ int cleanup_before_linux(void)
 
 void icache_enable(void)
 {
-	unsigned int *I0,*I1;
-	int i;
+	unsigned int *I0, *I1;
+	int i, j = 0;
 
+	/* Before enable icache, disable it first */
+	icache_disable();
 	I0 = (unsigned int *)ICPLB_ADDR0;
 	I1 = (unsigned int *)ICPLB_DATA0;
 
-	for(i=0;i<16;i++){
-		*I0++ = icplb_table[i][0];
-		*I1++ = icplb_table[i][1];
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & icplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & icplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
+		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
 		}
+
+	}
+
 	cli();
-	SSYNC();
+	sync();
+	asm(" .align 8; ");
 	*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
-	SSYNC();
+	sync();
 	sti();
 }
 
 void icache_disable(void)
 {
 	cli();
-	SSYNC();
+	sync();
+	asm(" .align 8; ");
 	*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
-	SSYNC();
+	sync();
 	sti();
 }
 
@@ -143,7 +123,7 @@ int icache_status(void)
 	unsigned int value;
 	value = *(unsigned int *)IMEM_CONTROL;
 
-	if( value & (IMC|ENICPLB) )
+	if (value & (IMC | ENICPLB))
 		return CACHE_ON;
 	else
 		return CACHE_OFF;
@@ -151,38 +131,90 @@ int icache_status(void)
 
 void dcache_enable(void)
 {
-	unsigned int *I0,*I1;
+	unsigned int *I0, *I1;
 	unsigned int temp;
-	int i;
+	int i, j = 0;
+
+	/* Before enable dcache, disable it first */
+	dcache_disable();
 	I0 = (unsigned int *)DCPLB_ADDR0;
 	I1 = (unsigned int *)DCPLB_DATA0;
 
-	for(i=0;i<16;i++){
-		*I0++ = dcplb_table[i][0];
-		*I1++ = dcplb_table[i][1];
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & dcplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+		} else {
+			debug("skip   %02i %02i 0x%08x 0x%08x\n", i, j,
+				 dcplb_table[i][0], dcplb_table[i][1]);
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & dcplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
 		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+	}
+
 	cli();
 	temp = *(unsigned int *)DMEM_CONTROL;
-	SSYNC();
-	*(unsigned int *)DMEM_CONTROL = ACACHE_BCACHE |ENDCPLB |PORT_PREF0|temp;
-	SSYNC();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL =
+	    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | temp;
+	sync();
 	sti();
 }
 
 void dcache_disable(void)
 {
+	unsigned int *I0, *I1;
+	int i;
+
 	cli();
-	SSYNC();
-	*(unsigned int *)DMEM_CONTROL &= ~(ACACHE_BCACHE |ENDCPLB |PORT_PREF0);
-	SSYNC();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL &=
+	    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	sync();
 	sti();
+
+	/* after disable dcache,
+	 * clear it so we don't confuse the next application
+	 */
+	I0 = (unsigned int *)DCPLB_ADDR0;
+	I1 = (unsigned int *)DCPLB_DATA0;
+
+	for (i = 0; i < 16; i++) {
+		*I0++ = 0x0;
+		*I1++ = 0x0;
+	}
 }
 
 int dcache_status(void)
 {
 	unsigned int value;
 	value = *(unsigned int *)DMEM_CONTROL;
-	if( value & (ENDCPLB))
+	if (value & (ENDCPLB))
 		return CACHE_ON;
 	else
 		return CACHE_OFF;
diff --git a/cpu/bf533/cpu.h b/cpu/bf533/cpu.h
index 7ec3387..b6b73b1 100644
--- a/cpu/bf533/cpu.h
+++ b/cpu/bf533/cpu.h
@@ -1,7 +1,7 @@
 /*
  *  U-boot - cpu.h
  *
- *  Copyright (c) 2005 blackfin.uclinux.org
+ *  Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -18,8 +18,8 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #ifndef _CPU_H_
@@ -32,8 +32,8 @@
 #define DEF_INTERRUPT_FLAGS 1
 #define MAX_TIM_LOAD	0xFFFFFFFF
 
-void blackfin_irq_panic(int reason, struct pt_regs * reg);
-extern void dump(struct pt_regs * regs);
+void blackfin_irq_panic(int reason, struct pt_regs *reg);
+extern void dump(struct pt_regs *regs);
 void display_excp(void);
 asmlinkage void evt_nmi(void);
 asmlinkage void evt_exception(void);
@@ -50,16 +50,17 @@ asmlinkage void evt_evt12(void);
 asmlinkage void evt_evt13(void);
 asmlinkage void evt_soft_int1(void);
 asmlinkage void evt_system_call(void);
-void blackfin_irq_panic(int reason, struct pt_regs * regs);
+void blackfin_irq_panic(int reason, struct pt_regs *regs);
 void blackfin_free_irq(unsigned int irq, void *dev_id);
-void call_isr(int irq, struct pt_regs * fp);
+void call_isr(int irq, struct pt_regs *fp);
 void blackfin_do_irq(int vec, struct pt_regs *fp);
 void blackfin_init_IRQ(void);
 void blackfin_enable_irq(unsigned int irq);
 void blackfin_disable_irq(unsigned int irq);
-extern int do_reset (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[]);
+extern int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[]);
 int blackfin_request_irq(unsigned int irq,
-		     void (*handler)(int, void *, struct pt_regs *),
-		     unsigned long flags,const char *devname,void *dev_id);
+			 void (*handler) (int, void *, struct pt_regs *),
+			 unsigned long flags, const char *devname,
+			 void *dev_id);
 void timer_init(void);
 #endif
diff --git a/cpu/bf533/flush.S b/cpu/bf533/flush.S
index 9fbdefc..62e3d65 100644
--- a/cpu/bf533/flush.S
+++ b/cpu/bf533/flush.S
@@ -1,15 +1,14 @@
-/* Copyright (C) 2003 Analog Devices, Inc. All Rights Reserved.
- * Copyright (C) 2004 LG SOft India. All Rights Reserved.
+/* Copyright (C) 2003-2007 Analog Devices Inc.
  *
  * This file is subject to the terms and conditions of the GNU General Public
  * License.
- *
- * Blackfin BF533/2.6 support : LG Soft India
  */
+
 #define ASSEMBLY
 
 #include <asm/linkage.h>
 #include <asm/cplb.h>
+#include <config.h>
 #include <asm/blackfin.h>
 
 .text
@@ -20,7 +19,7 @@
  * in the instruction cache.
  */
 
-ENTRY(flush_instruction_cache)
+ENTRY(_flush_instruction_cache)
 	[--SP] = ( R7:6, P5:4 );
 	LINK 12;
 	SP += -12;
@@ -33,7 +32,7 @@ ENTRY(flush_instruction_cache)
 inext:	R0 = [P5++];
 	R1 = [P4++];
 	[--SP] =  RETS;
-	CALL icplb_flush;	/* R0 = page, R1 = data*/
+	CALL _icplb_flush;	/* R0 = page, R1 = data*/
 	RETS = [SP++];
 iskip:	R6 += -1;
 	CC = R6;
@@ -52,7 +51,7 @@ iskip:	R6 += -1;
  */
 
 .align 2
-ENTRY(icplb_flush)
+ENTRY(_icplb_flush)
 	[--SP] = ( R7:0, P5:0 );
 	[--SP] = LC0;
 	[--SP] = LT0;
@@ -86,16 +85,17 @@ ENTRY(icplb_flush)
 	 */
 
 	R3 = ((12<<8)|2);		/* Extraction pattern */
-	nop;				/*Anamoly 05000209*/
-	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits*/
-	R3.H = R4.L << 0 ;		/* Save in extraction pattern for later deposit.*/
+	nop;				/* Anamoly 05000209 */
+	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits */
 
+	/* Save in extraction pattern for later deposit. */
+	R3.H = R4.L << 0;
 
 	/* So:
 	 * R0 = Page start
 	 * R1 = Page length (actually, offset into size/prefix tables)
 	 * R3 = sub-bank deposit values
-  	 *
+	 *
 	 * The cache has 2 Ways, and 64 sets, so we iterate through
 	 * the sets, accessing the tag for each Way, for our Bank and
 	 * sub-bank, looking for dirty, valid tags that match our
@@ -180,8 +180,10 @@ iflush_whole_page:
 	SSYNC;
 	IFLUSH [P0++];		/* because CSYNC can't end loops.*/
 	LSETUP (isall, ieall) LC0 = P1;
-isall:IFLUSH [P0++];
-ieall: NOP;
+isall:
+	IFLUSH [P0++];
+ieall:
+	NOP;
 	SSYNC;
 	JUMP ifinished;
 
@@ -191,7 +193,7 @@ ieall: NOP;
  * in the data cache.
  */
 
-ENTRY(flush_data_cache)
+ENTRY(_flush_data_cache)
 	[--SP] = ( R7:6, P5:4 );
 	LINK 12;
 	SP += -12;
@@ -209,7 +211,7 @@ next:	R0 = [P5++];
 	CC = R2;
 	IF !CC JUMP skip;	/* If not, ignore it.*/
 	[--SP] = RETS;
-	CALL dcplb_flush;	/* R0 = page, R1 = data*/
+	CALL _dcplb_flush;	/* R0 = page, R1 = data*/
 	RETS = [SP++];
 skip:	R6 += -1;
 	CC = R6;
@@ -228,7 +230,7 @@ skip:	R6 += -1;
  */
 
 .align 2
-ENTRY(dcplb_flush)
+ENTRY(_dcplb_flush)
 	[--SP] = ( R7:0, P5:0 );
 	[--SP] = LC0;
 	[--SP] = LT0;
@@ -290,14 +292,15 @@ bank_chosen:
 	R3 = ((12<<8)|2);		/* Extraction pattern */
 	nop;				/*Anamoly 05000209*/
 	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits*/
-	R3.H = R4.L << 0 ;		/* Save in extraction pattern for later deposit.*/
+	/* Save in extraction pattern for later deposit.*/
+	R3.H = R4.L << 0;
 
 	/* So:
 	 * R0 = Page start
 	 * R1 = Page length (actually, offset into size/prefix tables)
 	 * R2 = Bank select mask
 	 * R3 = sub-bank deposit values
-  	 *
+	 *
 	 * The cache has 2 Ways, and 64 sets, so we iterate through
 	 * the sets, accessing the tag for each Way, for our Bank and
 	 * sub-bank, looking for dirty, valid tags that match our
@@ -386,7 +389,7 @@ dflush_whole_page:
 	CC = BITTST(R1, 16);	/* Whether 1K or 4K*/
 	IF CC P1 = P2;
 	P1 += -1;		/* Unroll one iteration*/
-    SSYNC;
+	SSYNC;
 	FLUSHINV [P0++];	/* because CSYNC can't end loops.*/
 	LSETUP (eall, eall) LC0 = P1;
 eall:	FLUSHINV [P0++];
diff --git a/cpu/bf533/init_sdram.S b/cpu/bf533/init_sdram.S
new file mode 100644
index 0000000..e1a8e2f
--- /dev/null
+++ b/cpu/bf533/init_sdram.S
@@ -0,0 +1,179 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+#if (BFIN_BOOT_MODE == BF533_SPI_BOOT)
+	p0.h = hi(SPI_BAUD);
+	p0.l = lo(SPI_BAUD);
+	r0.l = CONFIG_SPI_BAUD;
+	w[p0] = r0.l;
+	SSYNC;
+#endif
+
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN      : 1=All Clocks off
+	 *   - [3]     = STOPCK    : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier         */
+	r0 = r0 << 9;			/* Shift it over,                  */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2?*/
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL?                 */
+	r1 = r1 << 8;			/* Shift it over                   */
+	r0 = r1 | r0;			/* add them all together           */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address                */
+	cli r2;				/* Disable interrupts              */
+	ssync;
+	w[p0] = r0.l;			/* Set the value                   */
+	idle;				/* Wait for the PLL to stablize    */
+	sti r2;				/* Enable interrupts               */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 */
+
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf533/init_sdram_bootrom_initblock.S b/cpu/bf533/init_sdram_bootrom_initblock.S
new file mode 100644
index 0000000..99ed920
--- /dev/null
+++ b/cpu/bf533/init_sdram_bootrom_initblock.S
@@ -0,0 +1,179 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+#if (BFIN_BOOT_MODE == BF533_SPI_BOOT)
+	p0.h = hi(SPI_BAUD);
+	p0.l = lo(SPI_BAUD);
+	r0.l = CONFIG_SPI_BAUD_INITBLOCK;
+	w[p0] = r0.l;
+	SSYNC;
+#endif
+
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN      : 1=All Clocks off
+	 *   - [3]     = STOPCK    : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier         */
+	r0 = r0 << 9;			/* Shift it over,                  */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2?*/
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL?                 */
+	r1 = r1 << 8;			/* Shift it over                   */
+	r0 = r1 | r0;			/* add them all together           */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address                */
+	cli r2;				/* Disable interrupts              */
+	ssync;
+	w[p0] = r0.l;			/* Set the value                   */
+	idle;				/* Wait for the PLL to stablize    */
+	sti r2;				/* Enable interrupts               */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 */
+
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf533/interrupt.S b/cpu/bf533/interrupt.S
index e780dc6..c356d53 100644
--- a/cpu/bf533/interrupt.S
+++ b/cpu/bf533/interrupt.S
@@ -1,7 +1,7 @@
 /*
  * U-boot - interrupt.S Processing of interrupts and exception handling
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * (C) Copyright 2000-2004
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
@@ -35,208 +35,63 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #define ASSEMBLY
-
+#include <config.h>
+#include <asm/blackfin.h>
 #include <asm/hw_irq.h>
 #include <asm/entry.h>
 #include <asm/blackfin_defs.h>
-#include <asm/cpu/bf533_irq.h>
 
-.global blackfin_irq_panic;
+.global _blackfin_irq_panic;
 
 .text
 .align 2
 
 #ifndef CONFIG_KGDB
-.global evt_emulation
-evt_emulation:
+.global _evt_emulation
+_evt_emulation:
 	SAVE_CONTEXT
 	r0 = IRQ_EMU;
 	r1 = seqstat;
 	sp += -12;
-	call blackfin_irq_panic;
+	call _blackfin_irq_panic;
 	sp += 12;
 	rte;
 #endif
 
-.global evt_nmi
-evt_nmi:
+.global _evt_nmi
+_evt_nmi:
 	SAVE_CONTEXT
 	r0 = IRQ_NMI;
 	r1 = RETN;
 	sp += -12;
-	call blackfin_irq_panic;
+	call _blackfin_irq_panic;
 	sp += 12;
 
 _evt_nmi_exit:
 	rtn;
 
-.global trap
-trap:
-	[--sp] = r0;
-	[--sp] = r1;
-	[--sp] = p0;
-	[--sp] = p1;
-	[--sp] = astat;
-	r0 = seqstat;
-	R0 <<= 26;
-	R0 >>= 26;
-	p0 = r0;
-	p1.l = EVTABLE;
-	p1.h = EVTABLE;
-	p0 = p1 + (p0 << 1);
-	r1 = W[p0] (Z);
-	p1 = r1;
-	jump (pc + p1);
-
-.global _EVENT1
-_EVENT1:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT2
-_EVENT2:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT3
-_EVENT3:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT4
-_EVENT4:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT5
-_EVENT5:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT6
-_EVENT6:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT7
-_EVENT7:
-	RAISE 15;
-	JUMP.S _EXIT;
-
-.global _EVENT8
-_EVENT8:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT9
-_EVENT9:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT10
-_EVENT10:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT11
-_EVENT11:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT12
-_EVENT12:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT13
-_EVENT13:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT14
-_EVENT14:
-/*	RAISE 14;	*/
-	CALL	_cplb_hdr;
-	JUMP.S _EXIT;
-
-.global _EVENT19
-_EVENT19:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT20
-_EVENT20:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EVENT21
-_EVENT21:
-	RAISE 14;
-	JUMP.S _EXIT;
-
-.global _EXIT
-_EXIT:
-	ASTAT = [sp++];
-	p1 = [sp++];
-	p0 = [sp++];
-	r1 = [sp++];
-	r0 = [sp++];
-	RTX;
-
-EVTABLE:
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x0000;
-	.byte2 0x003E;
-	.byte2 0x0042;
-	.byte4 0x0000;
-	.byte4 0x0000;
-	.byte4 0x0000;
-	.byte4 0x0000;
-	.byte4 0x0000;
-	.byte4 0x0000;
-	.byte4 0x0000;
-	.byte2 0x0000;
-	.byte2 0x001E;
-	.byte2 0x0022;
-	.byte2 0x0032;
-	.byte2 0x002e;
-	.byte2 0x0002;
-	.byte2 0x0036;
-	.byte2 0x002A;
-	.byte2 0x001A;
-	.byte2 0x0016;
-	.byte2 0x000A;
-	.byte2 0x000E;
-	.byte2 0x0012;
-	.byte2 0x0006;
-	.byte2 0x0026;
+.global _trap
+_trap:
+	SAVE_ALL_SYS
+	r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
+	sp += -12;
+	call _trap_c
+	sp += 12;
+	RESTORE_ALL_SYS
+	rtx;
 
-.global evt_rst
-evt_rst:
+.global _evt_rst
+_evt_rst:
 	SAVE_CONTEXT
 	r0 = IRQ_RST;
 	r1 = RETN;
 	sp += -12;
-	call do_reset;
+	call _do_reset;
 	sp += 12;
 
 _evt_rst_exit:
@@ -246,19 +101,19 @@ irq_panic:
 	r0 = IRQ_EVX;
 	r1 =  sp;
 	sp += -12;
-	call blackfin_irq_panic;
+	call _blackfin_irq_panic;
 	sp += 12;
 
-.global evt_ivhw
-evt_ivhw:
+.global _evt_ivhw
+_evt_ivhw:
 	SAVE_CONTEXT
 	RAISE 14;
 
 _evt_ivhw_exit:
 	 rti;
 
-.global evt_timer
-evt_timer:
+.global _evt_timer
+_evt_timer:
 	SAVE_CONTEXT
 	r0 = IRQ_CORETMR;
 	sp += -12;
@@ -269,91 +124,91 @@ evt_timer:
 	rti;
 	nop;
 
-.global evt_evt7
-evt_evt7:
+.global _evt_evt7
+_evt_evt7:
 	SAVE_CONTEXT
 	r0 = 7;
 	sp += -12;
-	call process_int;
+	call _process_int;
 	sp += 12;
 
 evt_evt7_exit:
 	RESTORE_CONTEXT
 	rti;
 
-.global evt_evt8
-evt_evt8:
+.global _evt_evt8
+_evt_evt8:
 	SAVE_CONTEXT
 	r0 = 8;
 	sp += -12;
-	call process_int;
+	call _process_int;
 	sp += 12;
 
 evt_evt8_exit:
 	RESTORE_CONTEXT
 	rti;
 
-.global evt_evt9
-evt_evt9:
+.global _evt_evt9
+_evt_evt9:
 	SAVE_CONTEXT
 	r0 = 9;
 	sp += -12;
-	call process_int;
+	call _process_int;
 	sp += 12;
 
 evt_evt9_exit:
 	RESTORE_CONTEXT
 	rti;
 
-.global evt_evt10
-evt_evt10:
+.global _evt_evt10
+_evt_evt10:
 	SAVE_CONTEXT
 	r0 = 10;
 	sp += -12;
-	call process_int;
+	call _process_int;
 	sp += 12;
 
 evt_evt10_exit:
 	RESTORE_CONTEXT
 	rti;
 
-.global evt_evt11
-evt_evt11:
+.global _evt_evt11
+_evt_evt11:
 	SAVE_CONTEXT
 	r0 = 11;
 	sp += -12;
-	call process_int;
+	call _process_int;
 	sp += 12;
 
 evt_evt11_exit:
 	RESTORE_CONTEXT
 	rti;
 
-.global evt_evt12
-evt_evt12:
+.global _evt_evt12
+_evt_evt12:
 	SAVE_CONTEXT
 	r0 = 12;
 	sp += -12;
-	call process_int;
+	call _process_int;
 	sp += 12;
 evt_evt12_exit:
 	 RESTORE_CONTEXT
 	 rti;
 
-.global evt_evt13
-evt_evt13:
+.global _evt_evt13
+_evt_evt13:
 	SAVE_CONTEXT
 	r0 = 13;
 	sp += -12;
-	call process_int;
+	call _process_int;
 	sp += 12;
 
 evt_evt13_exit:
 	 RESTORE_CONTEXT
 	 rti;
 
-.global evt_system_call
-evt_system_call:
+.global _evt_system_call
+_evt_system_call:
 	[--sp] = r0;
 	[--SP] = RETI;
 	r0 = [sp++];
@@ -363,7 +218,7 @@ evt_system_call:
 	r0 = [SP++];
 	SAVE_CONTEXT
 	sp += -12;
-	call display_excp;
+	call _exception_handle;
 	sp += 12;
 	RESTORE_CONTEXT
 	RTI;
@@ -371,8 +226,8 @@ evt_system_call:
 evt_system_call_exit:
 	rti;
 
-.global evt_soft_int1
-evt_soft_int1:
+.global _evt_soft_int1
+_evt_soft_int1:
 	[--sp] = r0;
 	[--SP] = RETI;
 	r0 = [sp++];
@@ -382,7 +237,7 @@ evt_soft_int1:
 	r0 = [SP++];
 	SAVE_CONTEXT
 	sp += -12;
-	call display_excp;
+	call _exception_handle;
 	sp += 12;
 	RESTORE_CONTEXT
 	RTI;
diff --git a/cpu/bf533/interrupts.c b/cpu/bf533/interrupts.c
index df1a25e..14d06cf 100644
--- a/cpu/bf533/interrupts.c
+++ b/cpu/bf533/interrupts.c
@@ -1,7 +1,7 @@
 /*
  * U-boot - interrupts.c Interrupt related routines
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * This file is based on interrupts.c
  * Copyright 1996 Roman Zippel
@@ -10,7 +10,7 @@
  * Copyright 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
  * Copyright 2003 Metrowerks/Motorola
  * Copyright 2003 Bas Vermeulen <bas@buyways.nl>,
- * 			BuyWays B.V. (www.buyways.nl)
+ *			BuyWays B.V. (www.buyways.nl)
  *
  * (C) Copyright 2000-2004
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
@@ -30,21 +30,22 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #include <common.h>
 #include <asm/machdep.h>
 #include <asm/irq.h>
-#include <asm/cpu/defBF533.h>
+#include <config.h>
+#include <asm/blackfin.h>
 #include "cpu.h"
 
 static ulong timestamp;
 static ulong last_time;
 static int int_flag;
 
-int irq_flags; /* needed by asm-blackfin/system.h */
+int irq_flags;			/* needed by asm-blackfin/system.h */
 
 /* Functions just to satisfy the linker */
 
@@ -61,7 +62,7 @@ unsigned long long get_ticks(void)
  * This function is derived from PowerPC code (timebase clock frequency).
  * On BF533 it returns the number of timer ticks per second.
  */
-ulong get_tbclk (void)
+ulong get_tbclk(void)
 {
 	ulong tbclk;
 
@@ -91,22 +92,22 @@ void udelay(unsigned long usec)
 	unsigned long cclk;
 	cclk = (CONFIG_CCLK_HZ);
 
-	while ( usec > 1 ) {
-	       /*
-		* how many clock ticks to delay?
-		*  - request(in useconds) * clock_ticks(Hz) / useconds/second
-		*/
+	while (usec > 1) {
+		/*
+		 * how many clock ticks to delay?
+		 *  - request(in useconds) * clock_ticks(Hz) / useconds/second
+		 */
 		if (usec < 1000) {
-			delay = (usec * (cclk/244)) >> 12 ;
+			delay = (usec * (cclk / 244)) >> 12;
 			usec = 0;
 		} else {
-			delay = (1000 * (cclk/244)) >> 12 ;
+			delay = (1000 * (cclk / 244)) >> 12;
 			usec -= 1000;
 		}
 
-		asm volatile (" %0 = CYCLES;": "=g"(start));
+		asm volatile (" %0 = CYCLES;":"=r" (start));
 		do {
-			asm volatile (" %0 = CYCLES; ": "=g"(stop));
+			asm volatile (" %0 = CYCLES; ":"=r" (stop));
 		} while (stop - start < delay);
 	}
 
@@ -117,7 +118,7 @@ void timer_init(void)
 {
 	*pTCNTL = 0x1;
 	*pTSCALE = 0x0;
-	*pTCOUNT  = MAX_TIM_LOAD;
+	*pTCOUNT = MAX_TIM_LOAD;
 	*pTPERIOD = MAX_TIM_LOAD;
 	*pTCNTL = 0x7;
 	asm("CSYNC;");
@@ -146,20 +147,23 @@ ulong get_timer(ulong base)
 	/* Number of clocks elapsed */
 	ulong clocks = (MAX_TIM_LOAD - (*pTCOUNT));
 
-	/* Find if the TCOUNT is reset
-	timestamp gives the number of times
-	TCOUNT got reset */
-	if(clocks < last_time)
+	/**
+	 * Find if the TCOUNT is reset
+	 * timestamp gives the number of times
+	 * TCOUNT got reset
+	 */
+	if (clocks < last_time)
 		timestamp++;
 	last_time = clocks;
 
 	/* Get the number of milliseconds */
-	milisec = clocks/(CONFIG_CCLK_HZ / 1000);
+	milisec = clocks / (CONFIG_CCLK_HZ / 1000);
 
-	/* Find the number of millisonds
-	that got elapsed before this TCOUNT
-	cycle */
-	milisec += timestamp * (MAX_TIM_LOAD/(CONFIG_CCLK_HZ / 1000));
+	/**
+	 * Find the number of millisonds
+	 * that got elapsed before this TCOUNT cycle
+	 */
+	milisec += timestamp * (MAX_TIM_LOAD / (CONFIG_CCLK_HZ / 1000));
 
 	return (milisec - base);
 }
diff --git a/cpu/bf533/ints.c b/cpu/bf533/ints.c
index 859f4b2..5586689 100644
--- a/cpu/bf533/ints.c
+++ b/cpu/bf533/ints.c
@@ -1,7 +1,7 @@
 /*
  * U-boot - ints.c Interrupt related routines
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * This file is based on ints.c
  *
@@ -32,8 +32,8 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #include <common.h>
@@ -51,9 +51,9 @@
 void blackfin_irq_panic(int reason, struct pt_regs *regs)
 {
 	printf("\n\nException: IRQ 0x%x entered\n", reason);
-	printf("code=[0x%x], ", (unsigned int) (regs->seqstat & 0x3f));
-	printf("stack frame=0x%x, ", (unsigned int) regs);
-	printf("bad PC=0x%04x\n", (unsigned int) regs->pc);
+	printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+	printf("stack frame=0x%x, ", (unsigned int)regs);
+	printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
 	dump(regs);
 	printf("Unhandled IRQ or exceptions!\n");
 	printf("Please reset the board \n");
@@ -61,46 +61,56 @@ void blackfin_irq_panic(int reason, struct pt_regs *regs)
 
 void blackfin_init_IRQ(void)
 {
-	*(unsigned volatile long *) (SIC_IMASK) = SIC_UNMASK_ALL;
+	*(unsigned volatile long *)(SIC_IMASK) = SIC_UNMASK_ALL;
 	cli();
 #ifndef CONFIG_KGDB
-	*(unsigned volatile long *) (EVT_EMULATION_ADDR) = 0x0;
+	*(unsigned volatile long *)(EVT_EMULATION_ADDR) = 0x0;
 #endif
-	*(unsigned volatile long *) (EVT_NMI_ADDR) =
-		(unsigned volatile long) evt_nmi;
-	*(unsigned volatile long *) (EVT_EXCEPTION_ADDR) =
-		(unsigned volatile long) trap;
-	*(unsigned volatile long *) (EVT_HARDWARE_ERROR_ADDR) =
-		(unsigned volatile long) evt_ivhw;
-	*(unsigned volatile long *) (EVT_RESET_ADDR) =
-		(unsigned volatile long) evt_rst;
-	*(unsigned volatile long *) (EVT_TIMER_ADDR) =
-		(unsigned volatile long) evt_timer;
-	*(unsigned volatile long *) (EVT_IVG7_ADDR) =
-		(unsigned volatile long) evt_evt7;
-	*(unsigned volatile long *) (EVT_IVG8_ADDR) =
-		(unsigned volatile long) evt_evt8;
-	*(unsigned volatile long *) (EVT_IVG9_ADDR) =
-		(unsigned volatile long) evt_evt9;
-	*(unsigned volatile long *) (EVT_IVG10_ADDR) =
-		(unsigned volatile long) evt_evt10;
-	*(unsigned volatile long *) (EVT_IVG11_ADDR) =
-		(unsigned volatile long) evt_evt11;
-	*(unsigned volatile long *) (EVT_IVG12_ADDR) =
-		(unsigned volatile long) evt_evt12;
-	*(unsigned volatile long *) (EVT_IVG13_ADDR) =
-		(unsigned volatile long) evt_evt13;
-	*(unsigned volatile long *) (EVT_IVG14_ADDR) =
-		(unsigned volatile long) evt_system_call;
-	*(unsigned volatile long *) (EVT_IVG15_ADDR) =
-		(unsigned volatile long) evt_soft_int1;
-	*(volatile unsigned long *) ILAT = 0;
+	*(unsigned volatile long *)(EVT_NMI_ADDR) =
+	    (unsigned volatile long)evt_nmi;
+	*(unsigned volatile long *)(EVT_EXCEPTION_ADDR) =
+	    (unsigned volatile long)trap;
+	*(unsigned volatile long *)(EVT_HARDWARE_ERROR_ADDR) =
+	    (unsigned volatile long)evt_ivhw;
+	*(unsigned volatile long *)(EVT_RESET_ADDR) =
+	    (unsigned volatile long)evt_rst;
+	*(unsigned volatile long *)(EVT_TIMER_ADDR) =
+	    (unsigned volatile long)evt_timer;
+	*(unsigned volatile long *)(EVT_IVG7_ADDR) =
+	    (unsigned volatile long)evt_evt7;
+	*(unsigned volatile long *)(EVT_IVG8_ADDR) =
+	    (unsigned volatile long)evt_evt8;
+	*(unsigned volatile long *)(EVT_IVG9_ADDR) =
+	    (unsigned volatile long)evt_evt9;
+	*(unsigned volatile long *)(EVT_IVG10_ADDR) =
+	    (unsigned volatile long)evt_evt10;
+	*(unsigned volatile long *)(EVT_IVG11_ADDR) =
+	    (unsigned volatile long)evt_evt11;
+	*(unsigned volatile long *)(EVT_IVG12_ADDR) =
+	    (unsigned volatile long)evt_evt12;
+	*(unsigned volatile long *)(EVT_IVG13_ADDR) =
+	    (unsigned volatile long)evt_evt13;
+	*(unsigned volatile long *)(EVT_IVG14_ADDR) =
+	    (unsigned volatile long)evt_system_call;
+	*(unsigned volatile long *)(EVT_IVG15_ADDR) =
+	    (unsigned volatile long)evt_soft_int1;
+	*(volatile unsigned long *)ILAT = 0;
 	asm("csync;");
 	sti();
-	*(volatile unsigned long *) IMASK = 0xffbf;
+	*(volatile unsigned long *)IMASK = 0xffbf;
 	asm("csync;");
 }
 
+void exception_handle(void)
+{
+#if defined (CONFIG_PANIC_HANG)
+	display_excp();
+#else
+	udelay(100000);		/* allow messages to go out */
+	do_reset(NULL, 0, 0, NULL);
+#endif
+}
+
 void display_excp(void)
 {
 	printf("Exception!\n");
diff --git a/cpu/bf533/serial.c b/cpu/bf533/serial.c
index 7b43ffd..6cab5da 100644
--- a/cpu/bf533/serial.c
+++ b/cpu/bf533/serial.c
@@ -1,7 +1,7 @@
 /*
  * U-boot - serial.c Serial driver for BF533
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * This file is based on
  * bf533_serial.c: Serial driver for BlackFin BF533 DSP internal UART.
@@ -38,8 +38,8 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #include <common.h>
@@ -49,6 +49,7 @@
 #include <asm/bitops.h>
 #include <asm/delay.h>
 #include <asm/uaccess.h>
+#include <asm/io.h>
 #include "bf533_serial.h"
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -58,15 +59,16 @@ unsigned long pll_div_fact;
 void calc_baud(void)
 {
 	unsigned char i;
-	int	temp;
+	int temp;
+	u_long sclk = get_sclk();
 
-	for(i = 0; i < sizeof(baud_table)/sizeof(int); i++) {
-		temp =  CONFIG_SCLK_HZ/(baud_table[i]*8);
-		if ( temp && 0x1 == 1 ) {
+	for (i = 0; i < sizeof(baud_table) / sizeof(int); i++) {
+		temp = sclk / (baud_table[i] * 8);
+		if ((temp & 0x1) == 1) {
 			temp++;
 		}
-		temp = temp/2;
-		hw_baud_table[i].dl_high = (temp >> 8)& 0xFF;
+		temp = temp / 2;
+		hw_baud_table[i].dl_high = (temp >> 8) & 0xFF;
 		hw_baud_table[i].dl_low = (temp) & 0xFF;
 	}
 }
@@ -74,6 +76,7 @@ void calc_baud(void)
 void serial_setbrg(void)
 {
 	int i;
+	DECLARE_GLOBAL_DATA_PTR;
 
 	calc_baud();
 
@@ -84,29 +87,29 @@ void serial_setbrg(void)
 
 	/* Enable UART */
 	*pUART_GCTL |= UART_GCTL_UCEN;
-	asm("ssync;");
+	sync();
 
 	/* Set DLAB in LCR to Access DLL and DLH */
 	ACCESS_LATCH;
-	asm("ssync;");
+	sync();
 
 	*pUART_DLL = hw_baud_table[i].dl_low;
-	asm("ssync;");
+	sync();
 	*pUART_DLH = hw_baud_table[i].dl_high;
-	asm("ssync;");
+	sync();
 
 	/* Clear DLAB in LCR to Access THR RBR IER */
 	ACCESS_PORT_IER;
-	asm("ssync;");
+	sync();
 
 	/* Enable  ERBFI and ELSI interrupts
-	* to poll SIC_ISR register*/
+	 * to poll SIC_ISR register*/
 	*pUART_IER = UART_IER_ELSI | UART_IER_ERBFI | UART_IER_ETBEI;
-	asm("ssync;");
+	sync();
 
 	/* Set LCR to Word Lengh 8-bit word select */
 	*pUART_LCR = UART_LCR_WLS8;
-	asm("ssync;");
+	sync();
 
 	return;
 }
@@ -119,8 +122,7 @@ int serial_init(void)
 
 void serial_putc(const char c)
 {
-	if ((*pUART_LSR) & UART_LSR_TEMT)
-	{
+	if ((*pUART_LSR) & UART_LSR_TEMT) {
 		if (c == '\n')
 			serial_putc('\r');
 
@@ -148,17 +150,16 @@ int serial_getc(void)
 	int ret;
 
 	/* Poll for RX Interrupt */
-	while (!((isr_val = *(volatile unsigned long *)SIC_ISR) & IRQ_UART_RX_BIT));
+	while (!((isr_val =
+		  *(volatile unsigned long *)SIC_ISR) & IRQ_UART_RX_BIT)) ;
 	asm("csync;");
 
 	uart_lsr_val = *pUART_LSR;	/* Clear status bit */
 	uart_rbr_val = *pUART_RBR;	/* getc() */
 
 	if (isr_val & IRQ_UART_ERROR_BIT) {
-		ret =  -1;
-	}
-	else
-	{
+		ret = -1;
+	} else {
 		ret = uart_rbr_val & 0xff;
 	}
 
@@ -180,10 +181,10 @@ static void local_put_char(char ch)
 	save_and_cli(flags);
 
 	/* Poll for TX Interruput */
-	while (!((isr_val = *pSIC_ISR) & IRQ_UART_TX_BIT));
+	while (!((isr_val = *pSIC_ISR) & IRQ_UART_TX_BIT)) ;
 	asm("csync;");
 
-	*pUART_THR = ch;			/* putc() */
+	*pUART_THR = ch;	/* putc() */
 
 	if (isr_val & IRQ_UART_ERROR_BIT) {
 		printf("?");
@@ -191,5 +192,5 @@ static void local_put_char(char ch)
 
 	restore_flags(flags);
 
-	return ;
+	return;
 }
diff --git a/cpu/bf533/start.S b/cpu/bf533/start.S
index 6d58575..67a60cf 100644
--- a/cpu/bf533/start.S
+++ b/cpu/bf533/start.S
@@ -1,7 +1,7 @@
 /*
- * U-boot - start.S Startup file of u-boot for BF533
+ * U-boot - start.S Startup file of u-boot for BF533/BF561
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * This file is based on head.S
  * Copyright (c) 2003  Metrowerks/Motorola
@@ -26,8 +26,8 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 /*
@@ -38,9 +38,23 @@
 #define ASSEMBLY
 
 #include <linux/config.h>
-#include <asm/blackfin.h>
 #include <config.h>
-#include <asm/mem_init.h>
+#include <asm/blackfin.h>
+
+.global _stext;
+.global __bss_start;
+.global start;
+.global _start;
+.global _rambase;
+.global _ramstart;
+.global _ramend;
+.global _bf533_data_dest;
+.global _bf533_data_size;
+.global edata;
+.global _initialize;
+.global _exit;
+.global flashdataend;
+.global init_sdram;
 
 #if (CONFIG_CCLK_DIV == 1)
 #define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
@@ -58,26 +72,12 @@
 #define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
 #endif
 
-.global _stext;
-.global __bss_start;
-.global start;
-.global _start;
-.global _rambase;
-.global _ramstart;
-.global _ramend;
-.global _bf533_data_dest;
-.global _bf533_data_size;
-.global edata;
-.global _initialize;
-.global _exit;
-.global flashdataend;
-
 .text
 _start:
 start:
 _stext:
 
-	R0 = 0x30;
+	R0 = 0x32;
 	SYSCFG = R0;
 	SSYNC;
 
@@ -120,8 +120,9 @@ _stext:
 	/* Set loop counters to zero, to make sure that
 	 * hw loops are disabled.
 	 */
-	lc0 = 0;
-	lc1 = 0;
+	r0  = 0;
+	lc0 = r0;
+	lc1 = r0;
 
 	SSYNC;
 
@@ -150,105 +151,40 @@ no_soft_reset:
 	LSETUP(4,4) lc0 = p1;
 	[ p0 ++ ] = r1;
 
-	/*
-	 *  Set PLL_CTL
-	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
-	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
-	 *   - [7]     = output delay (add 200ps of delay to mem signals)
-	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
-	 *   - [5]     = PDWN      : 1=All Clocks off
-	 *   - [3]     = STOPCK    : 1=Core Clock off
-	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
-	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
-	 *   all other bits set to zero
-	 */
-
-	r0 = CONFIG_VCO_MULT;	/* Load the VCO multiplier */
-	r0 = r0 << 9;		/* Shift it over */
-	r1 =  CONFIG_CLKIN_HALF;	/* Do we need to divide CLKIN by 2? */
-	r0 = r1 | r0;
-	r1 = CONFIG_PLL_BYPASS;	/* Bypass the PLL?                 */
-	r1 = r1 << 8;	/* Shift it over */
-	r0 = r1 | r0;	/* add them all together */
-
-	p0.h = (PLL_CTL >> 16);
-	p0.l = (PLL_CTL & 0xFFFF);	/* Load the address */
-	cli r2;				/* Disable interrupts */
-	w[p0] = r0;			/* Set the value */
-	idle;				/* Wait for the PLL to stablize */
-	sti r2;				/* Enable interrupts */
-	ssync;
-
-	/*
-	 * Turn on the CYCLES COUNTER
-	 */
-	r2 = SYSCFG;
-	BITSET (r2,1);
-	SYSCFG = r2;
-
-	/* Configure SCLK & CCLK Dividers */
-	r0 = CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV;
-	p0.h = (PLL_DIV >> 16);
-	p0.l = (PLL_DIV & 0xFFFF);
-	w[p0] = r0;
-	ssync;
-
-wait_for_pll_stab:
-	p0.h = (PLL_STAT >> 16);
-	p0.l = (PLL_STAT & 0xFFFF);
-	r0.l = w[p0];
-	cc = bittst(r0,5);
-	if !cc jump wait_for_pll_stab;
-
-	/* Configure SDRAM if SDRAM is already not enabled */
-	p0.l = (EBIU_SDSTAT & 0xFFFF);
-	p0.h = (EBIU_SDSTAT >> 16);
-	r0.l = w[p0];
-	cc = bittst(r0, 3);
-	if !cc jump skip_sdram_enable;
-
-	/* SDRAM initialization */
-	p0.l = (EBIU_SDGCTL & 0xFFFF);
-	p0.h = (EBIU_SDGCTL >> 16);	/* SDRAM Memory Global Control Register */
-	r0.h = (mem_SDGCTL >> 16);
-	r0.l = (mem_SDGCTL & 0xFFFF);
-	[p0] = r0;
-	ssync;
-
-	p0.l = (EBIU_SDBCTL & 0xFFFF);
-	p0.h = (EBIU_SDBCTL >> 16);	/* SDRAM Memory Bank Control Register */
-	r0 = mem_SDBCTL;
+	p0.h = hi(SIC_IWR);
+	p0.l = lo(SIC_IWR);
+	r0.l = 0x1;
 	w[p0] = r0.l;
-	ssync;
+	SSYNC;
 
-	p0.l = (EBIU_SDRRC & 0xFFFF);
-	p0.h = (EBIU_SDRRC >> 16);	/* SDRAM Refresh Rate Control Register */
-	r0 = mem_SDRRC;
-	w[p0] = r0.l;
-	ssync;
+	sp.l = (0xffb01000 & 0xFFFF);
+	sp.h = (0xffb01000 >> 16);
 
-skip_sdram_enable:
-	nop;
+	call init_sdram;
 
-#ifndef	CFG_NO_FLASH
 	/* relocate into to RAM */
-	p1.l = (CFG_FLASH_BASE & 0xffff);
-	p1.h = (CFG_FLASH_BASE >> 16);
+	call get_pc;
+offset:
+	r2.l = offset;
+	r2.h = offset;
+	r3.l = start;
+	r3.h = start;
+	r1 = r2 - r3;
+
+	r0 = r0 - r1;
+	p1 = r0;
+
 	p2.l = (CFG_MONITOR_BASE & 0xffff);
 	p2.h = (CFG_MONITOR_BASE >> 16);
-	r0.l = (CFG_MONITOR_LEN & 0xffff);
-	r0.h = (CFG_MONITOR_LEN >> 16);
+
+	p3 = 0x04;
+	p4.l = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) & 0xffff);
+	p4.h = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) >> 16);
 loop1:
-	r1 = [p1];
-	[p2] = r1;
-	p3=0x4;
-	p1=p1+p3;
-	p2=p2+p3;
-	r2=0x4;
-	r0=r0-r2;
-	cc=r0==0x0;
+	r1 = [p1 ++ p3];
+	[p2 ++ p3] = r1;
+	cc=p2==p4;
 	if !cc jump loop1;
-#endif
 	/*
 	 * configure STACK
 	 */
@@ -273,7 +209,8 @@ loop1:
 
 	p0.l = (IMASK & 0xFFFF);
 	p0.h = (IMASK >> 16);
-	r0 = IVG15_POS;
+	r0.l = LO(IVG15_POS);
+	r0.h = HI(IVG15_POS);
 	[p0] = r0;
 	raise 15;
 	p0.l = WAIT_HERE;
@@ -288,37 +225,10 @@ WAIT_HERE:
 _real_start:
 	[ -- sp ] = reti;
 
-#ifdef CONFIG_EZKIT533
-	p0.l = (WDOG_CTL & 0xFFFF);
-	p0.h = (WDOG_CTL >> 16);
-	r0 = WATCHDOG_DISABLE(z);
-	w[p0] = r0;
-#endif
-
-	/* Code for initializing Async mem banks */
-	p2.h = (EBIU_AMBCTL1 >> 16);
-	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
-	r0.h = (AMBCTL1VAL >> 16);
-	r0.l = (AMBCTL1VAL & 0xFFFF);
-	[p2] = r0;
-	ssync;
-
-	p2.h = (EBIU_AMBCTL0 >> 16);
-	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
-	r0.h = (AMBCTL0VAL >> 16);
-	r0.l = (AMBCTL0VAL & 0xFFFF);
-	[p2] = r0;
-	ssync;
-
-	p2.h = (EBIU_AMGCTL >> 16);
-	p2.l = (EBIU_AMGCTL & 0xffff);
-	r0 = AMGCTLVAL;
-	w[p2] = r0;
-	ssync;
-
 	/* DMA reset code to Hi of L1 SRAM */
 copy:
-	P1.H = hi(SYSMMR_BASE);	/* P1 Points to the beginning of SYSTEM MMR Space */
+	/* P1 Points to the beginning of SYSTEM MMR Space */
+	P1.H = hi(SYSMMR_BASE);
 	P1.L = lo(SYSMMR_BASE);
 
 	R0.H = reset_start;	/* Source Address (high) */
@@ -329,7 +239,8 @@ copy:
 	R1.H = hi(L1_ISRAM);	/* Destination Address (high) */
 	R1.L = lo(L1_ISRAM);	/* Destination Address (low) */
 	R3.L = DMAEN;		/* Source DMAConfig Value (8-bit words) */
-	R4.L = (DI_EN | WNR | DMAEN);	/* Destination DMAConfig Value (8-bit words) */
+	/* Destination DMAConfig Value (8-bit words) */
+	R4.L = (DI_EN | WNR | DMAEN);
 
 DMA:
 	R6 = 0x1 (Z);
@@ -342,57 +253,24 @@ DMA:
 	Memory Read,  8-Bit Transfers, 1-D DMA, Flow - Stop */
 	W[P1+OFFSET_(MDMA_S0_CONFIG)] = R3;
 
-	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;	/* Set Destination Base Address */
-	W[P1+OFFSET_(MDMA_D0_X_COUNT)] = R2;	/* Set Destination Count */
-	/* Set Destination DMAConfig = DMA Enable,
-	Memory Write, 8-Bit Transfers, 1-D DMA, Flow - Stop, IOC */
-	W[P1+OFFSET_(MDMA_D0_CONFIG)] = R4;
-
-	IDLE;	/* Wait for DMA to Complete */
-
-	R0 = 0x1;
-	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0;	/* Write 1 to clear DMA interrupt */
-
-	/* DMA reset code to DATA BANK A which uses this port
-	 * to avoid following problem
-	 * " Data from a Data Cache fill can be corrupoted after or during
-	 *   instruction DMA if certain core stalls exist"
-	 */
-
-copy_as_data:
-	R0.H = reset_start;	/* Source Address (high) */
-	R0.L = reset_start;	/* Source Address (low) */
-	R1.H = reset_end;
-	R1.L = reset_end;
-	R2 = R1 - R0;	/* Count */
-	R1.H = hi(DATA_BANKA_SRAM);	/* Destination Address (high) */
-	R1.L = lo(DATA_BANKA_SRAM);	/* Destination Address (low) */
-	R3.L = DMAEN;	/* Source DMAConfig Value (8-bit words) */
-	R4.L = (DI_EN | WNR | DMAEN);	/* Destination DMAConfig Value (8-bit words) */
-
-DMA_DATA:
-	R6 = 0x1 (Z);
-	W[P1+OFFSET_(MDMA_S0_X_MODIFY)] = R6;	/* Source Modify = 1 */
-	W[P1+OFFSET_(MDMA_D0_X_MODIFY)] = R6;	/* Destination Modify = 1 */
-
- 	[P1+OFFSET_(MDMA_S0_START_ADDR)] = R0;	/* Set Source Base Address */
-	W[P1+OFFSET_(MDMA_S0_X_COUNT)] = R2;	/* Set Source Count */
-	/* Set Source      DMAConfig = DMA Enable,
-	Memory Read,  8-Bit Transfers, 1-D DMA, Flow - Stop */
-	W[P1+OFFSET_(MDMA_S0_CONFIG)] = R3;
-
-	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;	/* Set Destination Base Address */
+	/* Set Destination Base Address */
+	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;
 	W[P1+OFFSET_(MDMA_D0_X_COUNT)] = R2;	/* Set Destination Count */
 	/* Set Destination DMAConfig = DMA Enable,
 	Memory Write, 8-Bit Transfers, 1-D DMA, Flow - Stop, IOC */
 	W[P1+OFFSET_(MDMA_D0_CONFIG)] = R4;
 
-	IDLE;	/* Wait for DMA to Complete */
+WAIT_DMA_DONE:
+	p0.h = hi(MDMA_D0_IRQ_STATUS);
+	p0.l = lo(MDMA_D0_IRQ_STATUS);
+	R0 = W[P0](Z);
+	CC = BITTST(R0, 0);
+	if ! CC jump WAIT_DMA_DONE
 
 	R0 = 0x1;
-	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0;	/* Write 1 to clear DMA interrupt */
 
-copy_end: nop;
+	/* Write 1 to clear DMA interrupt */
+	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0;
 
 	/* Initialize BSS Section with 0 s */
 	p1.l = __bss_start;
@@ -433,3 +311,6 @@ reset_end: nop;
 
 _exit:
 	jump.s	_exit;
+get_pc:
+	r0 = rets;
+	rts;
diff --git a/cpu/bf533/start1.S b/cpu/bf533/start1.S
index 6f48124..6d4731b 100644
--- a/cpu/bf533/start1.S
+++ b/cpu/bf533/start1.S
@@ -1,7 +1,7 @@
 /*
  * U-boot - start1.S Code running out of RAM after relocation
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -18,14 +18,14 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #define ASSEMBLY
 #include <linux/config.h>
-#include <asm/blackfin.h>
 #include <config.h>
+#include <asm/blackfin.h>
 
 .global	start1;
 .global	_start1;
@@ -34,5 +34,5 @@
 _start1:
 start1:
 	sp += -12;
-	call	board_init_f;
+	call	_board_init_f;
 	sp += 12;
diff --git a/cpu/bf533/traps.c b/cpu/bf533/traps.c
index 37470d5..19b1fde 100644
--- a/cpu/bf533/traps.c
+++ b/cpu/bf533/traps.c
@@ -1,7 +1,7 @@
 /*
  * U-boot - traps.c Routines related to interrupts and exceptions
  *
- * Copyright (c) 2005 blackfin.uclinux.org
+ * Copyright (c) 2005-2007 Analog Devices Inc.
  *
  * This file is based on
  * No original Copyright holder listed,
@@ -29,8 +29,8 @@
  *
  * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
  */
 
 #include <common.h>
@@ -39,9 +39,11 @@
 #include <asm/irq.h>
 #include <asm/system.h>
 #include <asm/traps.h>
-#include <asm/page.h>
 #include <asm/machdep.h>
 #include "cpu.h"
+#include <asm/arch/anomaly.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
 
 void init_IRQ(void)
 {
@@ -51,23 +53,187 @@ void init_IRQ(void)
 
 void process_int(unsigned long vec, struct pt_regs *fp)
 {
+	printf("interrupt\n");
 	return;
 }
 
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+unsigned long last_cplb_fault_retx;
+
+static unsigned int cplb_sizes[4] =
+    { 1024, 4 * 1024, 1024 * 1024, 4 * 1024 * 1024 };
+
+void trap_c(struct pt_regs *regs)
+{
+	unsigned int addr;
+	unsigned long trapnr = (regs->seqstat) & SEQSTAT_EXCAUSE;
+	unsigned int i, j, size, *I0, *I1;
+	unsigned short data = 0;
+
+	switch (trapnr) {
+	/* 0x26 - Data CPLB Miss */
+	case VEC_CPLB_M:
+
+#ifdef ANOMALY_05000261
+		/*
+		 * Work around an anomaly: if we see a new DCPLB fault,
+		 * return without doing anything. Then,
+		 * if we get the same fault again, handle it.
+		 */
+		addr = last_cplb_fault_retx;
+		last_cplb_fault_retx = regs->retx;
+		printf("this time, curr = 0x%08x last = 0x%08x\n",
+		       addr, last_cplb_fault_retx);
+		if (addr != last_cplb_fault_retx)
+			goto trap_c_return;
+#endif
+		data = 1;
+
+	case VEC_CPLB_I_M:
+
+		if (data) {
+			addr = *(unsigned int *)pDCPLB_FAULT_ADDR;
+		} else {
+			addr = *(unsigned int *)pICPLB_FAULT_ADDR;
+		}
+		for (i = 0; i < page_descriptor_table_size; i++) {
+			if (data) {
+				size = cplb_sizes[dcplb_table[i][1] >> 16];
+				j = dcplb_table[i][0];
+			} else {
+				size = cplb_sizes[icplb_table[i][1] >> 16];
+				j = icplb_table[i][0];
+			}
+			if ((j <= addr) && ((j + size) > addr)) {
+				debug("found %i 0x%08x\n", i, j);
+				break;
+			}
+		}
+		if (i == page_descriptor_table_size) {
+			printf("something is really wrong\n");
+			do_reset(NULL, 0, 0, NULL);
+		}
+
+		/* Turn the cache off */
+		if (data) {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL &=
+			    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+			sync();
+		}
+
+		if (data) {
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		j = 0;
+		while (*I1 & CPLB_LOCK) {
+			debug("skipping %i %08p - %08x\n", j, I1, *I1);
+			*I0++;
+			*I1++;
+			j++;
+		}
+
+		debug("remove %i 0x%08x  0x%08x\n", j, *I0, *I1);
+
+		for (; j < 15; j++) {
+			debug("replace %i 0x%08x  0x%08x\n", j, I0, I0 + 1);
+			*I0++ = *(I0 + 1);
+			*I1++ = *(I1 + 1);
+		}
+
+		if (data) {
+			*I0 = dcplb_table[i][0];
+			*I1 = dcplb_table[i][1];
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			*I0 = icplb_table[i][0];
+			*I1 = icplb_table[i][1];
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		for (j = 0; j < 16; j++) {
+			debug("%i 0x%08x  0x%08x\n", j, *I0++, *I1++);
+		}
+
+		/* Turn the cache back on */
+		if (data) {
+			j = *(unsigned int *)DMEM_CONTROL;
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL =
+			    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | j;
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+			sync();
+		}
+
+		break;
+	default:
+		/* All traps come here */
+		printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+		printf("stack frame=0x%x, ", (unsigned int)regs);
+		printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
+		dump(regs);
+		printf("\n\n");
+
+		printf("Unhandled IRQ or exceptions!\n");
+		printf("Please reset the board \n");
+		do_reset(NULL, 0, 0, NULL);
+	}
+
+	return;
+
+}
+
 void dump(struct pt_regs *fp)
 {
-	printf("PC: %08lx\n", fp->pc);
-	printf("SEQSTAT: %08lx    SP: %08lx\n", (long) fp->seqstat,
-		(long) fp);
-	printf("R0: %08lx    R1: %08lx    R2: %08lx    R3: %08lx\n",
-		fp->r0, fp->r1, fp->r2, fp->r3);
-	printf("R4: %08lx    R5: %08lx    R6: %08lx    R7: %08lx\n",
-		fp->r4, fp->r5, fp->r6, fp->r7);
-	printf("P0: %08lx    P1: %08lx    P2: %08lx    P3: %08lx\n",
-		fp->p0, fp->p1, fp->p2, fp->p3);
-	printf("P4: %08lx    P5: %08lx    FP: %08lx\n", fp->p4, fp->p5,
-		fp->fp);
-	printf("A0.w: %08lx    A0.x: %08lx    A1.w: %08lx    A1.x: %08lx\n",
-		fp->a0w, fp->a0x, fp->a1w, fp->a1x);
-	printf("\n");
+	debug("RETE:  %08lx  RETN: %08lx  RETX: %08lx  RETS: %08lx\n",
+		 fp->rete, fp->retn, fp->retx, fp->rets);
+	debug("IPEND: %04lx  SYSCFG: %04lx\n", fp->ipend, fp->syscfg);
+	debug("SEQSTAT: %08lx    SP: %08lx\n", (long)fp->seqstat, (long)fp);
+	debug("R0: %08lx    R1: %08lx    R2: %08lx    R3: %08lx\n",
+		 fp->r0, fp->r1, fp->r2, fp->r3);
+	debug("R4: %08lx    R5: %08lx    R6: %08lx    R7: %08lx\n",
+		 fp->r4, fp->r5, fp->r6, fp->r7);
+	debug("P0: %08lx    P1: %08lx    P2: %08lx    P3: %08lx\n",
+		 fp->p0, fp->p1, fp->p2, fp->p3);
+	debug("P4: %08lx    P5: %08lx    FP: %08lx\n",
+		 fp->p4, fp->p5, fp->fp);
+	debug("A0.w: %08lx    A0.x: %08lx    A1.w: %08lx    A1.x: %08lx\n",
+		 fp->a0w, fp->a0x, fp->a1w, fp->a1x);
+
+	debug("LB0: %08lx  LT0: %08lx  LC0: %08lx\n",
+		 fp->lb0, fp->lt0, fp->lc0);
+	debug("LB1: %08lx  LT1: %08lx  LC1: %08lx\n",
+		 fp->lb1, fp->lt1, fp->lc1);
+	debug("B0: %08lx  L0: %08lx  M0: %08lx  I0: %08lx\n",
+		 fp->b0, fp->l0, fp->m0, fp->i0);
+	debug("B1: %08lx  L1: %08lx  M1: %08lx  I1: %08lx\n",
+		 fp->b1, fp->l1, fp->m1, fp->i1);
+	debug("B2: %08lx  L2: %08lx  M2: %08lx  I2: %08lx\n",
+		 fp->b2, fp->l2, fp->m2, fp->i2);
+	debug("B3: %08lx  L3: %08lx  M3: %08lx  I3: %08lx\n",
+		 fp->b3, fp->l3, fp->m3, fp->i3);
+
+	debug("DCPLB_FAULT_ADDR=%p\n", *pDCPLB_FAULT_ADDR);
+	debug("ICPLB_FAULT_ADDR=%p\n", *pICPLB_FAULT_ADDR);
+
 }
diff --git a/cpu/bf533/video.c b/cpu/bf533/video.c
new file mode 100644
index 0000000..3ff0151
--- /dev/null
+++ b/cpu/bf533/video.c
@@ -0,0 +1,194 @@
+/*
+ * (C) Copyright 2000
+ * Paolo Scaffardi, AIRVENT SAM s.p.a - RIMINI(ITALY), arsenio@tin.it
+ * (C) Copyright 2002
+ * Wolfgang Denk, wd@denx.de
+ * (C) Copyright 2006
+ * Aubrey Li, aubrey.li@analog.com
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <stdarg.h>
+#include <common.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <i2c.h>
+#include <linux/types.h>
+#include <devices.h>
+
+#ifdef CONFIG_VIDEO
+#define NTSC_FRAME_ADDR 0x06000000
+#include "video.h"
+
+/* NTSC OUTPUT SIZE  720 * 240 */
+#define VERTICAL	2
+#define HORIZONTAL	4
+
+int is_vblank_line(const int line)
+{
+	/*
+	 *  This array contains a single bit for each line in
+	 *  an NTSC frame.
+	 */
+	if ((line <= 18) || (line >= 264 && line <= 281) || (line == 528))
+		return true;
+
+	return false;
+}
+
+int NTSC_framebuffer_init(char *base_address)
+{
+	const int NTSC_frames = 1;
+	const int NTSC_lines = 525;
+	char *dest = base_address;
+	int frame_num, line_num;
+
+	for (frame_num = 0; frame_num < NTSC_frames; ++frame_num) {
+		for (line_num = 1; line_num <= NTSC_lines; ++line_num) {
+			unsigned int code;
+			int offset = 0;
+			int i;
+
+			if (is_vblank_line(line_num))
+				offset++;
+
+			if (line_num > 266 || line_num < 3)
+				offset += 2;
+
+			/* Output EAV code */
+			code = SystemCodeMap[offset].EAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output horizontal blanking */
+			for (i = 0; i < 67 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+
+			/* Output SAV */
+			code = SystemCodeMap[offset].SAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output empty horizontal data */
+			for (i = 0; i < 360 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+		}
+	}
+
+	return dest - base_address;
+}
+
+void fill_frame(char *Frame, int Value)
+{
+	int *OddPtr32;
+	int OddLine;
+	int *EvenPtr32;
+	int EvenLine;
+	int i;
+	int *data;
+	int m, n;
+
+	/* fill odd and even frames */
+	for (OddLine = 22, EvenLine = 285; OddLine < 263; OddLine++, EvenLine++) {
+		OddPtr32 = (int *)((Frame + (OddLine * 1716)) + 276);
+		EvenPtr32 = (int *)((Frame + (EvenLine * 1716)) + 276);
+		for (i = 0; i < 360; i++, OddPtr32++, EvenPtr32++) {
+			*OddPtr32 = Value;
+			*EvenPtr32 = Value;
+		}
+	}
+
+	for (m = 0; m < VERTICAL; m++) {
+		data = (int *)u_boot_logo.data;
+		for (OddLine = (22 + m), EvenLine = (285 + m);
+		     OddLine < (u_boot_logo.height * VERTICAL) + (22 + m);
+		     OddLine += VERTICAL, EvenLine += VERTICAL) {
+			OddPtr32 = (int *)((Frame + ((OddLine) * 1716)) + 276);
+			EvenPtr32 =
+			    (int *)((Frame + ((EvenLine) * 1716)) + 276);
+			for (i = 0; i < u_boot_logo.width / 2; i++) {
+				/* enlarge one pixel to m x n */
+				for (n = 0; n < HORIZONTAL; n++) {
+					*OddPtr32++ = *data;
+					*EvenPtr32++ = *data;
+				}
+				data++;
+			}
+		}
+	}
+}
+
+void video_putc(const char c)
+{
+}
+
+void video_puts(const char *s)
+{
+}
+
+static int video_init(void)
+{
+	char *NTSCFrame;
+	NTSCFrame = (char *)NTSC_FRAME_ADDR;
+	NTSC_framebuffer_init(NTSCFrame);
+	fill_frame(NTSCFrame, BLUE);
+
+	*pPPI_CONTROL = 0x0082;
+	*pPPI_FRAME = 0x020D;
+
+	*pDMA0_START_ADDR = NTSCFrame;
+	*pDMA0_X_COUNT = 0x035A;
+	*pDMA0_X_MODIFY = 0x0002;
+	*pDMA0_Y_COUNT = 0x020D;
+	*pDMA0_Y_MODIFY = 0x0002;
+	*pDMA0_CONFIG = 0x1015;
+	*pPPI_CONTROL = 0x0083;
+	return 0;
+}
+
+int drv_video_init(void)
+{
+	int error, devices = 1;
+
+	device_t videodev;
+
+	video_init();		/* Video initialization */
+
+	memset(&videodev, 0, sizeof(videodev));
+
+	strcpy(videodev.name, "video");
+	videodev.ext = DEV_EXT_VIDEO;	/* Video extensions */
+	videodev.flags = DEV_FLAGS_OUTPUT;	/* Output only */
+	videodev.putc = video_putc;	/* 'putc' function */
+	videodev.puts = video_puts;	/* 'puts' function */
+
+	error = device_register(&videodev);
+
+	return (error == 0) ? devices : error;
+}
+#endif
diff --git a/cpu/bf533/video.h b/cpu/bf533/video.h
new file mode 100644
index 0000000..d237f6a
--- /dev/null
+++ b/cpu/bf533/video.h
@@ -0,0 +1,25 @@
+#include <video_logo.h>
+#define write_dest_byte(val) {*dest++=val;}
+#define BLACK   (0x01800180)	/* black pixel pattern  */
+#define BLUE    (0x296E29F0)	/* blue pixel pattern   */
+#define RED     (0x51F0515A)	/* red pixel pattern    */
+#define MAGENTA (0x6ADE6ACA)	/* magenta pixel pattern */
+#define GREEN   (0x91229136)	/* green pixel pattern  */
+#define CYAN    (0xAA10AAA6)	/* cyan pixel pattern   */
+#define YELLOW  (0xD292D210)	/* yellow pixel pattern */
+#define WHITE   (0xFE80FE80)	/* white pixel pattern  */
+
+#define true 	1
+#define false	0
+
+typedef struct {
+	unsigned int SAV;
+	unsigned int EAV;
+} SystemCodeType;
+
+const SystemCodeType SystemCodeMap[4] = {
+	{0xFF000080, 0xFF00009D},
+	{0xFF0000AB, 0xFF0000B6},
+	{0xFF0000C7, 0xFF0000DA},
+	{0xFF0000EC, 0xFF0000F1}
+};
diff --git a/cpu/bf537/Makefile b/cpu/bf537/Makefile
new file mode 100644
index 0000000..8b0f9c0
--- /dev/null
+++ b/cpu/bf537/Makefile
@@ -0,0 +1,52 @@
+# U-boot - Makefile
+#
+# Copyright (c) 2005-2007 Analog Devices Inc.
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+# MA 02110-1301 USA
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(CPU).a
+
+START	= start.o start1.o interrupt.o cache.o flush.o init_sdram.o
+COBJS	= cpu.o traps.o ints.o serial.o interrupts.o video.o i2c.o
+
+EXTRA = init_sdram_bootrom_initblock.o
+
+SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS) $(SOBJS))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(START) $(LIB) $(obj).depend $(EXTRA)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/bf537/cache.S b/cpu/bf537/cache.S
new file mode 100644
index 0000000..5bda5bf
--- /dev/null
+++ b/cpu/bf537/cache.S
@@ -0,0 +1,128 @@
+#define ASSEMBLY
+#include <asm/linkage.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.text
+.align 2
+ENTRY(_blackfin_icache_flush_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+	1:
+	IFLUSH[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+	IFLUSH[P0];
+	SSYNC;
+	RTS;
+
+ENTRY(_blackfin_dcache_flush_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+1:
+	FLUSH[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+	FLUSH[P0];
+	SSYNC;
+	RTS;
+
+ENTRY(_icache_invalidate)
+ENTRY(_invalidate_entire_icache)
+	[--SP] = (R7:5);
+
+	P0.L = (IMEM_CONTROL & 0xFFFF);
+	P0.H = (IMEM_CONTROL >> 16);
+	R7 =[P0];
+
+	/*
+	 * Clear the IMC bit , All valid bits in the instruction
+	 * cache are set to the invalid state
+	 */
+	BITCLR(R7, IMC_P);
+	CLI R6;
+	/* SSYNC required before invalidating cache. */
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	/* Configures the instruction cache agian */
+	R6 = (IMC | ENICPLB);
+	R7 = R7 | R6;
+
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	(R7:5) =[SP++];
+	RTS;
+
+/*
+ * Invalidate the Entire Data cache by
+ * clearing DMC[1:0] bits
+ */
+ENTRY(_invalidate_entire_dcache)
+ENTRY(_dcache_invalidate)
+	[--SP] = (R7:6);
+
+	P0.L = (DMEM_CONTROL & 0xFFFF);
+	P0.H = (DMEM_CONTROL >> 16);
+	R7 =[P0];
+
+	/*
+	 * Clear the DMC[1:0] bits, All valid bits in the data
+	 * cache are set to the invalid state
+	 */
+	BITCLR(R7, DMC0_P);
+	BITCLR(R7, DMC1_P);
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+	/* Configures the data cache again */
+
+	R6 = (ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	R7 = R7 | R6;
+
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	(R7:6) =[SP++];
+	RTS;
+
+ENTRY(_blackfin_dcache_invalidate_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+1:
+	FLUSHINV[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+
+	/*
+	 * If the data crosses a cache line, then we'll be pointing to
+	 * the last cache line, but won't have flushed/invalidated it yet, so do
+	 * one more.
+	 */
+	FLUSHINV[P0];
+	SSYNC;
+	RTS;
diff --git a/cpu/bf537/config.mk b/cpu/bf537/config.mk
new file mode 100644
index 0000000..8a35789
--- /dev/null
+++ b/cpu/bf537/config.mk
@@ -0,0 +1,27 @@
+# U-boot - config.mk
+#
+# Copyright (c) 2005-2007 Analog Devices Inc.
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+# MA 02110-1301 USA
+#
+
+PLATFORM_RELFLAGS += -mcpu=bf537 -ffixed-P5
diff --git a/cpu/bf537/cpu.c b/cpu/bf537/cpu.c
new file mode 100644
index 0000000..62f603b
--- /dev/null
+++ b/cpu/bf537/cpu.c
@@ -0,0 +1,227 @@
+/*
+ * U-boot - cpu.c CPU specific functions
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <asm/blackfin.h>
+#include <command.h>
+#include <asm/entry.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
+
+#define CACHE_ON 1
+#define CACHE_OFF 0
+
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	__asm__ __volatile__("cli r3;" "P0 = %0;" "JUMP (P0);"::"r"(L1_ISRAM)
+	    );
+
+	return 0;
+}
+
+/* These functions are just used to satisfy the linker */
+int cpu_init(void)
+{
+	return 0;
+}
+
+int cleanup_before_linux(void)
+{
+	return 0;
+}
+
+void icache_enable(void)
+{
+	unsigned int *I0, *I1;
+	int i, j = 0;
+
+	if ((*pCHIPID >> 28) < 2)
+		return;
+
+	/* Before enable icache, disable it first */
+	icache_disable();
+	I0 = (unsigned int *)ICPLB_ADDR0;
+	I1 = (unsigned int *)ICPLB_DATA0;
+
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & icplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & icplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
+		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+
+	}
+
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+	sync();
+	sti();
+}
+
+void icache_disable(void)
+{
+	if ((*pCHIPID >> 28) < 2)
+		return;
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+	sync();
+	sti();
+}
+
+int icache_status(void)
+{
+	unsigned int value;
+	value = *(unsigned int *)IMEM_CONTROL;
+
+	if (value & (IMC | ENICPLB))
+		return CACHE_ON;
+	else
+		return CACHE_OFF;
+}
+
+void dcache_enable(void)
+{
+	unsigned int *I0, *I1;
+	unsigned int temp;
+	int i, j = 0;
+
+	/* Before enable dcache, disable it first */
+	dcache_disable();
+	I0 = (unsigned int *)DCPLB_ADDR0;
+	I1 = (unsigned int *)DCPLB_DATA0;
+
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & dcplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+		} else {
+			debug("skip   %02i %02i 0x%08x 0x%08x\n", i, j,
+				 dcplb_table[i][0], dcplb_table[i][1]);
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & dcplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+				 dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
+		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+	}
+
+	cli();
+	temp = *(unsigned int *)DMEM_CONTROL;
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL =
+	    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | temp;
+	sync();
+	sti();
+}
+
+void dcache_disable(void)
+{
+	unsigned int *I0, *I1;
+	int i;
+
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL &=
+	    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	sync();
+	sti();
+
+	/* after disable dcache,
+	 * clear it so we don't confuse the next application
+	 */
+	I0 = (unsigned int *)DCPLB_ADDR0;
+	I1 = (unsigned int *)DCPLB_DATA0;
+
+	for (i = 0; i < 16; i++) {
+		*I0++ = 0x0;
+		*I1++ = 0x0;
+	}
+}
+
+int dcache_status(void)
+{
+	unsigned int value;
+	value = *(unsigned int *)DMEM_CONTROL;
+
+	if (value & (ENDCPLB))
+		return CACHE_ON;
+	else
+		return CACHE_OFF;
+}
diff --git a/cpu/bf537/cpu.h b/cpu/bf537/cpu.h
new file mode 100644
index 0000000..b6b73b1
--- /dev/null
+++ b/cpu/bf537/cpu.h
@@ -0,0 +1,66 @@
+/*
+ *  U-boot - cpu.h
+ *
+ *  Copyright (c) 2005-2007 Analog Devices 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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#ifndef _CPU_H_
+#define _CPU_H_
+
+#include <command.h>
+
+#define INTERNAL_IRQS (32)
+#define NUM_IRQ_NODES 16
+#define DEF_INTERRUPT_FLAGS 1
+#define MAX_TIM_LOAD	0xFFFFFFFF
+
+void blackfin_irq_panic(int reason, struct pt_regs *reg);
+extern void dump(struct pt_regs *regs);
+void display_excp(void);
+asmlinkage void evt_nmi(void);
+asmlinkage void evt_exception(void);
+asmlinkage void trap(void);
+asmlinkage void evt_ivhw(void);
+asmlinkage void evt_rst(void);
+asmlinkage void evt_timer(void);
+asmlinkage void evt_evt7(void);
+asmlinkage void evt_evt8(void);
+asmlinkage void evt_evt9(void);
+asmlinkage void evt_evt10(void);
+asmlinkage void evt_evt11(void);
+asmlinkage void evt_evt12(void);
+asmlinkage void evt_evt13(void);
+asmlinkage void evt_soft_int1(void);
+asmlinkage void evt_system_call(void);
+void blackfin_irq_panic(int reason, struct pt_regs *regs);
+void blackfin_free_irq(unsigned int irq, void *dev_id);
+void call_isr(int irq, struct pt_regs *fp);
+void blackfin_do_irq(int vec, struct pt_regs *fp);
+void blackfin_init_IRQ(void);
+void blackfin_enable_irq(unsigned int irq);
+void blackfin_disable_irq(unsigned int irq);
+extern int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[]);
+int blackfin_request_irq(unsigned int irq,
+			 void (*handler) (int, void *, struct pt_regs *),
+			 unsigned long flags, const char *devname,
+			 void *dev_id);
+void timer_init(void);
+#endif
diff --git a/cpu/bf537/flush.S b/cpu/bf537/flush.S
new file mode 100644
index 0000000..fbd26cc
--- /dev/null
+++ b/cpu/bf537/flush.S
@@ -0,0 +1,403 @@
+/* Copyright (C) 2003-2007 Analog Devices Inc.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.
+ */
+
+#define ASSEMBLY
+
+#include <asm/linkage.h>
+#include <asm/cplb.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.text
+
+/* This is an external function being called by the user
+ * application through __flush_cache_all. Currently this function
+ * serves the purpose of flushing all the pending writes in
+ * in the instruction cache.
+ */
+
+ENTRY(_flush_instruction_cache)
+	[--SP] = ( R7:6, P5:4 );
+	LINK 12;
+	SP += -12;
+	P5.H = (ICPLB_ADDR0 >> 16);
+	P5.L = (ICPLB_ADDR0 & 0xFFFF);
+	P4.H = (ICPLB_DATA0 >> 16);
+	P4.L = (ICPLB_DATA0 & 0xFFFF);
+	R7 = CPLB_VALID | CPLB_L1_CHBL;
+	R6 = 16;
+inext:	R0 = [P5++];
+	R1 = [P4++];
+	[--SP] =  RETS;
+	CALL _icplb_flush;	/* R0 = page, R1 = data*/
+	RETS = [SP++];
+iskip:	R6 += -1;
+	CC = R6;
+	IF CC JUMP inext;
+	SSYNC;
+	SP += 12;
+	UNLINK;
+	( R7:6, P5:4 ) = [SP++];
+	RTS;
+
+/* This is an internal function to flush all pending
+ * writes in the cache associated with a particular ICPLB.
+ *
+ * R0 -  page's start address
+ * R1 -  CPLB's data field.
+ */
+
+.align 2
+ENTRY(_icplb_flush)
+	[--SP] = ( R7:0, P5:0 );
+	[--SP] = LC0;
+	[--SP] = LT0;
+	[--SP] = LB0;
+	[--SP] = LC1;
+	[--SP] = LT1;
+	[--SP] = LB1;
+
+	/* If it's a 1K or 4K page, then it's quickest to
+	 * just systematically flush all the addresses in
+	 * the page, regardless of whether they're in the
+	 * cache, or dirty. If it's a 1M or 4M page, there
+	 * are too many addresses, and we have to search the
+	 * cache for lines corresponding to the page.
+	 */
+
+	CC = BITTST(R1, 17);	/* 1MB or 4MB */
+	IF !CC JUMP iflush_whole_page;
+
+	/* We're only interested in the page's size, so extract
+	 * this from the CPLB (bits 17:16), and scale to give an
+	 * offset into the page_size and page_prefix tables.
+	 */
+
+	R1 <<= 14;
+	R1 >>= 30;
+	R1 <<= 2;
+
+	/* We can also determine the sub-bank used, because this is
+	 * taken from bits 13:12 of the address.
+	 */
+
+	R3 = ((12<<8)|2);		/* Extraction pattern */
+	nop;				/* Anamoly 05000209 */
+	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits */
+
+	/* Save in extraction pattern for later deposit. */
+	R3.H = R4.L << 0;
+
+	/* So:
+	 * R0 = Page start
+	 * R1 = Page length (actually, offset into size/prefix tables)
+	 * R3 = sub-bank deposit values
+	 *
+	 * The cache has 2 Ways, and 64 sets, so we iterate through
+	 * the sets, accessing the tag for each Way, for our Bank and
+	 * sub-bank, looking for dirty, valid tags that match our
+	 * address prefix.
+	 */
+
+	P5.L = (ITEST_COMMAND & 0xFFFF);
+	P5.H = (ITEST_COMMAND >> 16);
+	P4.L = (ITEST_DATA0 & 0xFFFF);
+	P4.H = (ITEST_DATA0 >> 16);
+
+	P0.L = page_prefix_table;
+	P0.H = page_prefix_table;
+	P1 = R1;
+	R5 = 0;			/* Set counter*/
+	P0 = P1 + P0;
+	R4 = [P0];		/* This is the address prefix*/
+
+	/* We're reading (bit 1==0) the tag (bit 2==0), and we
+	 * don't care about which double-word, since we're only
+	 * fetching tags, so we only have to set Set, Bank,
+	 * Sub-bank and Way.
+	 */
+
+	P2 = 4;
+	LSETUP (ifs1, ife1) LC1 = P2;
+ifs1:	P0 = 32;		/* iterate over all sets*/
+	LSETUP (ifs0, ife0) LC0 = P0;
+ifs0:	R6 = R5 << 5;		/* Combine set*/
+	R6.H = R3.H << 0 ;	/* and sub-bank*/
+	[P5] = R6;		/* Issue Command*/
+	SSYNC;			/* CSYNC will not work here :(*/
+	R7 = [P4];		/* and read Tag.*/
+	CC = BITTST(R7, 0);	/* Check if valid*/
+	IF !CC JUMP ifskip;	/* and skip if not.*/
+
+	/* Compare against the page address. First, plant bits 13:12
+	 * into the tag, since those aren't part of the returned data.
+	 */
+
+	R7 = DEPOSIT(R7, R3);	/* set 13:12*/
+	R1 = R7 & R4;		/* Mask off lower bits*/
+	CC = R1 == R0;		/* Compare against page start.*/
+	IF !CC JUMP ifskip;	/* Skip it if it doesn't match.*/
+
+	/* Tag address matches against page, so this is an entry
+	 * we must flush.
+	 */
+
+	R7 >>= 10;		/* Mask off the non-address bits*/
+	R7 <<= 10;
+	P3 = R7;
+	IFLUSH [P3];		/* And flush the entry*/
+ifskip:
+ife0:	R5 += 1;		/* Advance to next Set*/
+ife1:	NOP;
+
+ifinished:
+	SSYNC;			/* Ensure the data gets out to mem.*/
+
+	/*Finished. Restore context.*/
+	LB1 = [SP++];
+	LT1 = [SP++];
+	LC1 = [SP++];
+	LB0 = [SP++];
+	LT0 = [SP++];
+	LC0 = [SP++];
+	( R7:0, P5:0 ) = [SP++];
+	RTS;
+
+iflush_whole_page:
+	/* It's a 1K or 4K page, so quicker to just flush the
+	 * entire page.
+	 */
+
+	P1 = 32;		/* For 1K pages*/
+	P2 = P1 << 2;		/* For 4K pages*/
+	P0 = R0;		/* Start of page*/
+	CC = BITTST(R1, 16);	/* Whether 1K or 4K*/
+	IF CC P1 = P2;
+	P1 += -1;		/* Unroll one iteration*/
+	SSYNC;
+	IFLUSH [P0++];		/* because CSYNC can't end loops.*/
+	LSETUP (isall, ieall) LC0 = P1;
+isall:IFLUSH [P0++];
+ieall: NOP;
+	SSYNC;
+	JUMP ifinished;
+
+/* This is an external function being called by the user
+ * application through __flush_cache_all. Currently this function
+ * serves the purpose of flushing all the pending writes in
+ * in the data cache.
+ */
+
+ENTRY(_flush_data_cache)
+	[--SP] = ( R7:6, P5:4 );
+	LINK 12;
+	SP += -12;
+	P5.H = (DCPLB_ADDR0 >> 16);
+	P5.L = (DCPLB_ADDR0 & 0xFFFF);
+	P4.H = (DCPLB_DATA0 >> 16);
+	P4.L = (DCPLB_DATA0 & 0xFFFF);
+	R7 = CPLB_VALID | CPLB_L1_CHBL | CPLB_DIRTY (Z);
+	R6 = 16;
+next:	R0 = [P5++];
+	R1 = [P4++];
+	CC = BITTST(R1, 14);	/* Is it write-through?*/
+	IF CC JUMP skip;	/* If so, ignore it.*/
+	R2 = R1 & R7;		/* Is it a dirty, cached page?*/
+	CC = R2;
+	IF !CC JUMP skip;	/* If not, ignore it.*/
+	[--SP] = RETS;
+	CALL _dcplb_flush;	/* R0 = page, R1 = data*/
+	RETS = [SP++];
+skip:	R6 += -1;
+	CC = R6;
+	IF CC JUMP next;
+	SSYNC;
+	SP += 12;
+	UNLINK;
+	( R7:6, P5:4 ) = [SP++];
+	RTS;
+
+/* This is an internal function to flush all pending
+ * writes in the cache associated with a particular DCPLB.
+ *
+ * R0 -  page's start address
+ * R1 -  CPLB's data field.
+ */
+
+.align 2
+ENTRY(_dcplb_flush)
+	[--SP] = ( R7:0, P5:0 );
+	[--SP] = LC0;
+	[--SP] = LT0;
+	[--SP] = LB0;
+	[--SP] = LC1;
+	[--SP] = LT1;
+	[--SP] = LB1;
+
+	/* If it's a 1K or 4K page, then it's quickest to
+	 * just systematically flush all the addresses in
+	 * the page, regardless of whether they're in the
+	 * cache, or dirty. If it's a 1M or 4M page, there
+	 * are too many addresses, and we have to search the
+	 * cache for lines corresponding to the page.
+	 */
+
+	CC = BITTST(R1, 17);	/* 1MB or 4MB */
+	IF !CC JUMP dflush_whole_page;
+
+	/* We're only interested in the page's size, so extract
+	 * this from the CPLB (bits 17:16), and scale to give an
+	 * offset into the page_size and page_prefix tables.
+	 */
+
+	R1 <<= 14;
+	R1 >>= 30;
+	R1 <<= 2;
+
+	/* The page could be mapped into Bank A or Bank B, depending
+	 * on (a) whether both banks are configured as cache, and
+	 * (b) on whether address bit A[x] is set. x is determined
+	 * by DCBS in DMEM_CONTROL
+	 */
+
+	R2 = 0;			/* Default to Bank A (Bank B would be 1)*/
+
+	P0.L = (DMEM_CONTROL & 0xFFFF);
+	P0.H = (DMEM_CONTROL >> 16);
+
+	R3 = [P0];		/* If Bank B is not enabled as cache*/
+	CC = BITTST(R3, 2);	/* then Bank A is our only option.*/
+	IF CC JUMP bank_chosen;
+
+	R4 = 1<<14;		/* If DCBS==0, use A[14].*/
+	R5 = R4 << 7;		/* If DCBS==1, use A[23];*/
+	CC = BITTST(R3, 4);
+	IF CC R4 = R5;		/* R4 now has either bit 14 or bit 23 set.*/
+	R5 = R0 & R4;		/* Use it to test the Page address*/
+	CC = R5;		/* and if that bit is set, we use Bank B,*/
+	R2 = CC;		/* else we use Bank A.*/
+	R2 <<= 23;		/* The Bank selection's at posn 23.*/
+
+bank_chosen:
+
+	/* We can also determine the sub-bank used, because this is
+	 * taken from bits 13:12 of the address.
+	 */
+
+	R3 = ((12<<8)|2);		/* Extraction pattern */
+	nop;				/*Anamoly 05000209*/
+	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits*/
+	/* Save in extraction pattern for later deposit.*/
+	R3.H = R4.L << 0;
+
+	/* So:
+	 * R0 = Page start
+	 * R1 = Page length (actually, offset into size/prefix tables)
+	 * R2 = Bank select mask
+	 * R3 = sub-bank deposit values
+	 *
+	 * The cache has 2 Ways, and 64 sets, so we iterate through
+	 * the sets, accessing the tag for each Way, for our Bank and
+	 * sub-bank, looking for dirty, valid tags that match our
+	 * address prefix.
+	 */
+
+	P5.L = (DTEST_COMMAND & 0xFFFF);
+	P5.H = (DTEST_COMMAND >> 16);
+	P4.L = (DTEST_DATA0 & 0xFFFF);
+	P4.H = (DTEST_DATA0 >> 16);
+
+	P0.L = page_prefix_table;
+	P0.H = page_prefix_table;
+	P1 = R1;
+	R5 = 0;			/* Set counter*/
+	P0 = P1 + P0;
+	R4 = [P0];		/* This is the address prefix*/
+
+
+	/* We're reading (bit 1==0) the tag (bit 2==0), and we
+	 * don't care about which double-word, since we're only
+	 * fetching tags, so we only have to set Set, Bank,
+	 * Sub-bank and Way.
+	 */
+
+	P2 = 2;
+	LSETUP (fs1, fe1) LC1 = P2;
+fs1:	P0 = 64;		/* iterate over all sets*/
+	LSETUP (fs0, fe0) LC0 = P0;
+fs0:	R6 = R5 << 5;		/* Combine set*/
+	R6.H = R3.H << 0 ;	/* and sub-bank*/
+	R6 = R6 | R2;		/* and Bank. Leave Way==0 at first.*/
+	BITSET(R6,14);
+	[P5] = R6;		/* Issue Command*/
+	SSYNC;
+	R7 = [P4];		/* and read Tag.*/
+	CC = BITTST(R7, 0);	/* Check if valid*/
+	IF !CC JUMP fskip;	/* and skip if not.*/
+	CC = BITTST(R7, 1);	/* Check if dirty*/
+	IF !CC JUMP fskip;	/* and skip if not.*/
+
+	/* Compare against the page address. First, plant bits 13:12
+	 * into the tag, since those aren't part of the returned data.
+	 */
+
+	R7 = DEPOSIT(R7, R3);	/* set 13:12*/
+	R1 = R7 & R4;		/* Mask off lower bits*/
+	CC = R1 == R0;		/* Compare against page start.*/
+	IF !CC JUMP fskip;	/* Skip it if it doesn't match.*/
+
+	/* Tag address matches against page, so this is an entry
+	 * we must flush.
+	 */
+
+	R7 >>= 10;		/* Mask off the non-address bits*/
+	R7 <<= 10;
+	P3 = R7;
+	SSYNC;
+	FLUSHINV [P3];		/* And flush the entry*/
+fskip:
+fe0:	R5 += 1;		/* Advance to next Set*/
+fe1:	BITSET(R2, 26);		/* Go to next Way.*/
+
+dfinished:
+	SSYNC;			/* Ensure the data gets out to mem.*/
+
+	/*Finished. Restore context.*/
+	LB1 = [SP++];
+	LT1 = [SP++];
+	LC1 = [SP++];
+	LB0 = [SP++];
+	LT0 = [SP++];
+	LC0 = [SP++];
+	( R7:0, P5:0 ) = [SP++];
+	RTS;
+
+dflush_whole_page:
+
+	/* It's a 1K or 4K page, so quicker to just flush the
+	 * entire page.
+	 */
+
+	P1 = 32;		/* For 1K pages*/
+	P2 = P1 << 2;		/* For 4K pages*/
+	P0 = R0;		/* Start of page*/
+	CC = BITTST(R1, 16);	/* Whether 1K or 4K*/
+	IF CC P1 = P2;
+	P1 += -1;		/* Unroll one iteration*/
+	SSYNC;
+	FLUSHINV [P0++];	/* because CSYNC can't end loops.*/
+	LSETUP (eall, eall) LC0 = P1;
+eall:	FLUSHINV [P0++];
+	SSYNC;
+	JUMP dfinished;
+
+.align 4;
+page_prefix_table:
+.byte4 	0xFFFFFC00;	/* 1K */
+.byte4	0xFFFFF000;	/* 4K */
+.byte4	0xFFF00000;	/* 1M */
+.byte4	0xFFC00000;	/* 4M */
+.page_prefix_table.end:
diff --git a/cpu/bf537/i2c.c b/cpu/bf537/i2c.c
new file mode 100644
index 0000000..3b0d026
--- /dev/null
+++ b/cpu/bf537/i2c.c
@@ -0,0 +1,460 @@
+/****************************************************************
+ * $ID: i2c.c	24 Oct 2006 12:00:00 +0800 $ 			*
+ *								*
+ * Description:							*
+ *								*
+ * Maintainer:  sonicz  <sonic.zhang@analog.com>		*
+ *								*
+ * CopyRight (c)  2006  Analog Device				*
+ * All rights reserved.						*
+ *								*
+ * This file is free software;					*
+ *	you are free to modify and/or redistribute it		*
+ *	under the terms of the GNU General Public Licence (GPL).*
+ *								*
+ ****************************************************************/
+
+#include <common.h>
+
+#ifdef CONFIG_HARD_I2C
+
+#include <asm/blackfin.h>
+#include <i2c.h>
+#include <asm/io.h>
+
+#define bfin_read16(addr) ({ unsigned __v; \
+			__asm__ __volatile__ (\
+			"%0 = w[%1] (z);\n\t"\
+			: "=d"(__v) : "a"(addr)); (unsigned short)__v; })
+
+#define bfin_write16(addr,val) ({\
+			__asm__ __volatile__ (\
+			"w[%0] = %1;\n\t"\
+			: : "a"(addr) , "d"(val) : "memory");})
+
+/* Two-Wire Interface		(0xFFC01400 - 0xFFC014FF) */
+#define bfin_read_TWI_CLKDIV()		bfin_read16(TWI_CLKDIV)
+#define bfin_write_TWI_CLKDIV(val)	bfin_write16(TWI_CLKDIV,val)
+#define bfin_read_TWI_CONTROL()		bfin_read16(TWI_CONTROL)
+#define bfin_write_TWI_CONTROL(val)	bfin_write16(TWI_CONTROL,val)
+#define bfin_read_TWI_SLAVE_CTL()	bfin_read16(TWI_SLAVE_CTL)
+#define bfin_write_TWI_SLAVE_CTL(val)	bfin_write16(TWI_SLAVE_CTL,val)
+#define bfin_read_TWI_SLAVE_STAT()	bfin_read16(TWI_SLAVE_STAT)
+#define bfin_write_TWI_SLAVE_STAT(val)	bfin_write16(TWI_SLAVE_STAT,val)
+#define bfin_read_TWI_SLAVE_ADDR()	bfin_read16(TWI_SLAVE_ADDR)
+#define bfin_write_TWI_SLAVE_ADDR(val)	bfin_write16(TWI_SLAVE_ADDR,val)
+#define bfin_read_TWI_MASTER_CTL()	bfin_read16(TWI_MASTER_CTL)
+#define bfin_write_TWI_MASTER_CTL(val)	bfin_write16(TWI_MASTER_CTL,val)
+#define bfin_read_TWI_MASTER_STAT()	bfin_read16(TWI_MASTER_STAT)
+#define bfin_write_TWI_MASTER_STAT(val)	bfin_write16(TWI_MASTER_STAT,val)
+#define bfin_read_TWI_MASTER_ADDR()	bfin_read16(TWI_MASTER_ADDR)
+#define bfin_write_TWI_MASTER_ADDR(val)	bfin_write16(TWI_MASTER_ADDR,val)
+#define bfin_read_TWI_INT_STAT()	bfin_read16(TWI_INT_STAT)
+#define bfin_write_TWI_INT_STAT(val)	bfin_write16(TWI_INT_STAT,val)
+#define bfin_read_TWI_INT_MASK()	bfin_read16(TWI_INT_MASK)
+#define bfin_write_TWI_INT_MASK(val)	bfin_write16(TWI_INT_MASK,val)
+#define bfin_read_TWI_FIFO_CTL()	bfin_read16(TWI_FIFO_CTL)
+#define bfin_write_TWI_FIFO_CTL(val)	bfin_write16(TWI_FIFO_CTL,val)
+#define bfin_read_TWI_FIFO_STAT()	bfin_read16(TWI_FIFO_STAT)
+#define bfin_write_TWI_FIFO_STAT(val)	bfin_write16(TWI_FIFO_STAT,val)
+#define bfin_read_TWI_XMT_DATA8()	bfin_read16(TWI_XMT_DATA8)
+#define bfin_write_TWI_XMT_DATA8(val)	bfin_write16(TWI_XMT_DATA8,val)
+#define bfin_read_TWI_XMT_DATA16()	bfin_read16(TWI_XMT_DATA16)
+#define bfin_write_TWI_XMT_DATA16(val)	bfin_write16(TWI_XMT_DATA16,val)
+#define bfin_read_TWI_RCV_DATA8()	bfin_read16(TWI_RCV_DATA8)
+#define bfin_write_TWI_RCV_DATA8(val)	bfin_write16(TWI_RCV_DATA8,val)
+#define bfin_read_TWI_RCV_DATA16()	bfin_read16(TWI_RCV_DATA16)
+#define bfin_write_TWI_RCV_DATA16(val)	bfin_write16(TWI_RCV_DATA16,val)
+
+#ifdef DEBUG_I2C
+#define PRINTD(fmt,args...)	do {	\
+	DECLARE_GLOBAL_DATA_PTR;	\
+	if (gd->have_console)		\
+		printf(fmt ,##args);	\
+	} while (0)
+#else
+#define PRINTD(fmt,args...)
+#endif
+
+#ifndef CONFIG_TWICLK_KHZ
+#define CONFIG_TWICLK_KHZ	50
+#endif
+
+/* All transfers are described by this data structure */
+struct i2c_msg {
+	u16 addr;		/* slave address */
+	u16 flags;
+#define I2C_M_STOP		0x2
+#define I2C_M_RD		0x1
+	u16 len;		/* msg length */
+	u8 *buf;		/* pointer to msg data */
+};
+
+/**
+ * i2c_reset: - reset the host controller
+ *
+ */
+
+static void i2c_reset(void)
+{
+	/* Disable TWI */
+	bfin_write_TWI_CONTROL(0);
+	sync();
+
+	/* Set TWI internal clock as 10MHz */
+	bfin_write_TWI_CONTROL(((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F);
+
+	/* Set Twi interface clock as specified */
+	if (CONFIG_TWICLK_KHZ > 400)
+		bfin_write_TWI_CLKDIV(((5 * 1024 / 400) << 8) | ((5 * 1024 /
+						400) & 0xFF));
+	else
+		bfin_write_TWI_CLKDIV(((5 * 1024 /
+					CONFIG_TWICLK_KHZ) << 8) | ((5 * 1024 /
+						CONFIG_TWICLK_KHZ)
+						& 0xFF));
+
+	/* Enable TWI */
+	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() | TWI_ENA);
+	sync();
+}
+
+int wait_for_completion(struct i2c_msg *msg, int timeout_count)
+{
+	unsigned short twi_int_stat;
+	unsigned short mast_stat;
+	int i;
+
+	for (i = 0; i < timeout_count; i++) {
+		twi_int_stat = bfin_read_TWI_INT_STAT();
+		mast_stat = bfin_read_TWI_MASTER_STAT();
+
+		if (XMTSERV & twi_int_stat) {
+			/* Transmit next data */
+			if (msg->len > 0) {
+				bfin_write_TWI_XMT_DATA8(*(msg->buf++));
+				msg->len--;
+			} else if (msg->flags & I2C_M_STOP)
+				bfin_write_TWI_MASTER_CTL
+				    (bfin_read_TWI_MASTER_CTL() | STOP);
+			sync();
+			/* Clear status */
+			bfin_write_TWI_INT_STAT(XMTSERV);
+			sync();
+			i = 0;
+		}
+		if (RCVSERV & twi_int_stat) {
+			if (msg->len > 0) {
+				/* Receive next data */
+				*(msg->buf++) = bfin_read_TWI_RCV_DATA8();
+				msg->len--;
+			} else if (msg->flags & I2C_M_STOP) {
+				bfin_write_TWI_MASTER_CTL
+				    (bfin_read_TWI_MASTER_CTL() | STOP);
+				sync();
+			}
+			/* Clear interrupt source */
+			bfin_write_TWI_INT_STAT(RCVSERV);
+			sync();
+			i = 0;
+		}
+		if (MERR & twi_int_stat) {
+			bfin_write_TWI_INT_STAT(MERR);
+			bfin_write_TWI_INT_MASK(0);
+			bfin_write_TWI_MASTER_STAT(0x3e);
+			bfin_write_TWI_MASTER_CTL(0);
+			sync();
+			/*
+			 * if both err and complete int stats are set,
+			 * return proper results.
+			 */
+			if (MCOMP & twi_int_stat) {
+				bfin_write_TWI_INT_STAT(MCOMP);
+				bfin_write_TWI_INT_MASK(0);
+				bfin_write_TWI_MASTER_CTL(0);
+				sync();
+				/*
+				 * If it is a quick transfer,
+				 * only address bug no data, not an err.
+				 */
+				if (msg->len == 0 && mast_stat & BUFRDERR)
+					return 0;
+				/*
+				 * If address not acknowledged return -3,
+				 * else return 0.
+				 */
+				else if (!(mast_stat & ANAK))
+					return 0;
+				else
+					return -3;
+			}
+			return -1;
+		}
+		if (MCOMP & twi_int_stat) {
+			bfin_write_TWI_INT_STAT(MCOMP);
+			sync();
+			bfin_write_TWI_INT_MASK(0);
+			bfin_write_TWI_MASTER_CTL(0);
+			sync();
+			return 0;
+		}
+	}
+	if (msg->flags & I2C_M_RD)
+		return -4;
+	else
+		return -2;
+}
+
+/**
+ * i2c_transfer: - Transfer one byte over the i2c bus
+ *
+ * This function can tranfer a byte over the i2c bus in both directions.
+ * It is used by the public API functions.
+ *
+ * @return:	 0: transfer successful
+ *		-1: transfer fail
+ *		-2: transmit timeout
+ *		-3: ACK missing
+ *		-4: receive timeout
+ *		-5: controller not ready
+ */
+int i2c_transfer(struct i2c_msg *msg)
+{
+	int ret = 0;
+	int timeout_count = 10000;
+	int len = msg->len;
+
+	if (!(bfin_read_TWI_CONTROL() & TWI_ENA)) {
+		ret = -5;
+		goto transfer_error;
+	}
+
+	while (bfin_read_TWI_MASTER_STAT() & BUSBUSY) ;
+
+	/* Set Transmit device address */
+	bfin_write_TWI_MASTER_ADDR(msg->addr);
+
+	/*
+	 * FIFO Initiation.
+	 * Data in FIFO should be discarded before start a new operation.
+	 */
+	bfin_write_TWI_FIFO_CTL(0x3);
+	sync();
+	bfin_write_TWI_FIFO_CTL(0);
+	sync();
+
+	if (!(msg->flags & I2C_M_RD)) {
+		/* Transmit first data */
+		if (msg->len > 0) {
+			PRINTD("1 in i2c_transfer: buf=%d, len=%d\n", *msg->buf,
+			       len);
+			bfin_write_TWI_XMT_DATA8(*(msg->buf++));
+			msg->len--;
+			sync();
+		}
+	}
+
+	/* clear int stat */
+	bfin_write_TWI_INT_STAT(MERR | MCOMP | XMTSERV | RCVSERV);
+
+	/* Interrupt mask . Enable XMT, RCV interrupt */
+	bfin_write_TWI_INT_MASK(MCOMP | MERR |
+			((msg->flags & I2C_M_RD) ? RCVSERV : XMTSERV));
+	sync();
+
+	if (len > 0 && len <= 255)
+		bfin_write_TWI_MASTER_CTL((len << 6));
+	else if (msg->len > 255) {
+		bfin_write_TWI_MASTER_CTL((0xff << 6));
+		msg->flags &= I2C_M_STOP;
+	} else
+		bfin_write_TWI_MASTER_CTL(0);
+
+	/* Master enable */
+	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
+			((msg->flags & I2C_M_RD)
+			 ? MDIR : 0) | ((CONFIG_TWICLK_KHZ >
+					 100) ? FAST : 0));
+	sync();
+
+	ret = wait_for_completion(msg, timeout_count);
+	PRINTD("3 in i2c_transfer: ret=%d\n", ret);
+
+transfer_error:
+	switch (ret) {
+	case 1:
+		PRINTD(("i2c_transfer: error: transfer fail\n"));
+		break;
+	case 2:
+		PRINTD(("i2c_transfer: error: transmit timeout\n"));
+		break;
+	case 3:
+		PRINTD(("i2c_transfer: error: ACK missing\n"));
+		break;
+	case 4:
+		PRINTD(("i2c_transfer: error: receive timeout\n"));
+		break;
+	case 5:
+		PRINTD(("i2c_transfer: error: controller not ready\n"));
+		i2c_reset();
+		break;
+	default:
+		break;
+	}
+	return ret;
+
+}
+
+/* ---------------------------------------------------------------------*/
+/* API Functions							*/
+/* ---------------------------------------------------------------------*/
+
+void i2c_init(int speed, int slaveaddr)
+{
+	i2c_reset();
+}
+
+/**
+ * i2c_probe: - Test if a chip answers for a given i2c address
+ *
+ * @chip:	address of the chip which is searched for
+ * @return: 	0 if a chip was found, -1 otherwhise
+ */
+
+int i2c_probe(uchar chip)
+{
+	struct i2c_msg msg;
+	u8 probebuf;
+
+	i2c_reset();
+
+	probebuf = 0;
+	msg.addr = chip;
+	msg.flags = 0;
+	msg.len = 1;
+	msg.buf = &probebuf;
+	if (i2c_transfer(&msg))
+		return -1;
+
+	msg.addr = chip;
+	msg.flags = I2C_M_RD;
+	msg.len = 1;
+	msg.buf = &probebuf;
+	if (i2c_transfer(&msg))
+		return -1;
+
+	return 0;
+}
+
+/**
+ *   i2c_read: - Read multiple bytes from an i2c device
+ *
+ *   chip:    I2C chip address, range 0..127
+ *   addr:    Memory (register) address within the chip
+ *   alen:    Number of bytes to use for addr (typically 1, 2 for larger
+ *		memories, 0 for register type devices with only one
+ *		register)
+ *   buffer:  Where to read/write the data
+ *   len:     How many bytes to read/write
+ *
+ *   Returns: 0 on success, not 0 on failure
+ */
+
+int i2c_read(uchar chip, uint addr, int alen, uchar * buffer, int len)
+{
+	struct i2c_msg msg;
+	u8 addr_bytes[3];	/* lowest...highest byte of data address */
+
+	PRINTD("i2c_read: chip=0x%x, addr=0x%x, alen=0x%x, len=0x%x\n", chip,
+			addr, alen, len);
+
+	if (alen > 0) {
+		addr_bytes[0] = (u8) ((addr >> 0) & 0x000000FF);
+		addr_bytes[1] = (u8) ((addr >> 8) & 0x000000FF);
+		addr_bytes[2] = (u8) ((addr >> 16) & 0x000000FF);
+		msg.addr = chip;
+		msg.flags = 0;
+		msg.len = alen;
+		msg.buf = addr_bytes;
+		if (i2c_transfer(&msg))
+			return -1;
+	}
+
+	/* start read sequence */
+	PRINTD(("i2c_read: start read sequence\n"));
+	msg.addr = chip;
+	msg.flags = I2C_M_RD;
+	msg.len = len;
+	msg.buf = buffer;
+	if (i2c_transfer(&msg))
+		return -1;
+
+	return 0;
+}
+
+/**
+ *   i2c_write: -  Write multiple bytes to an i2c device
+ *
+ *   chip:    I2C chip address, range 0..127
+ *   addr:    Memory (register) address within the chip
+ *   alen:    Number of bytes to use for addr (typically 1, 2 for larger
+ *		memories, 0 for register type devices with only one
+ *		register)
+ *   buffer:  Where to read/write the data
+ *   len:     How many bytes to read/write
+ *
+ *   Returns: 0 on success, not 0 on failure
+ */
+
+int i2c_write(uchar chip, uint addr, int alen, uchar * buffer, int len)
+{
+	struct i2c_msg msg;
+	u8 addr_bytes[3];	/* lowest...highest byte of data address */
+
+	PRINTD
+		("i2c_write: chip=0x%x, addr=0x%x, alen=0x%x, len=0x%x, buf0=0x%x\n",
+		 chip, addr, alen, len, buffer[0]);
+
+	/* chip address write */
+	if (alen > 0) {
+		addr_bytes[0] = (u8) ((addr >> 0) & 0x000000FF);
+		addr_bytes[1] = (u8) ((addr >> 8) & 0x000000FF);
+		addr_bytes[2] = (u8) ((addr >> 16) & 0x000000FF);
+		msg.addr = chip;
+		msg.flags = 0;
+		msg.len = alen;
+		msg.buf = addr_bytes;
+		if (i2c_transfer(&msg))
+			return -1;
+	}
+
+	/* start read sequence */
+	PRINTD(("i2c_write: start write sequence\n"));
+	msg.addr = chip;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = buffer;
+	if (i2c_transfer(&msg))
+		return -1;
+
+	return 0;
+
+}
+
+uchar i2c_reg_read(uchar chip, uchar reg)
+{
+	uchar buf;
+
+	PRINTD("i2c_reg_read: chip=0x%02x, reg=0x%02x\n", chip, reg);
+	i2c_read(chip, reg, 0, &buf, 1);
+	return (buf);
+}
+
+void i2c_reg_write(uchar chip, uchar reg, uchar val)
+{
+	PRINTD("i2c_reg_write: chip=0x%02x, reg=0x%02x, val=0x%02x\n", chip,
+			reg, val);
+	i2c_write(chip, reg, 0, &val, 1);
+}
+
+#endif				/* CONFIG_HARD_I2C */
diff --git a/cpu/bf537/init_sdram.S b/cpu/bf537/init_sdram.S
new file mode 100644
index 0000000..897a589
--- /dev/null
+++ b/cpu/bf537/init_sdram.S
@@ -0,0 +1,174 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (BFIN_BOOT_MODE != BF537_UART_BOOT)
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
+#endif
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+#if (BFIN_BOOT_MODE == BF537_SPI_MASTER_BOOT)
+	p0.h = hi(SIC_IWR);
+	p0.l = lo(SIC_IWR);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+
+	p0.h = hi(SPI_BAUD);
+	p0.l = lo(SPI_BAUD);
+	r0.l = CONFIG_SPI_BAUD;
+	w[p0] = r0.l;
+	SSYNC;
+#endif
+
+#if (BFIN_BOOT_MODE != BF537_UART_BOOT)
+
+#ifdef CONFIG_BF537
+	/* Enable PHY CLK buffer output */
+	p0.h = hi(VR_CTL);
+	p0.l = lo(VR_CTL);
+	r0.l = w[p0];
+	bitset(r0, 14);
+	w[p0] = r0.l;
+	ssync;
+#endif
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN      : 1=All Clocks off
+	 *   - [3]     = STOPCK    : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF	: 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier */
+	r0 = r0 << 9;			/* Shift it over */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2?*/
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL? */
+	r1 = r1 << 8;			/* Shift it over */
+	r0 = r1 | r0;			/* add them all together */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address */
+	cli r2;				/* Disable interrupts */
+	ssync;
+	w[p0] = r0.l;			/* Set the value */
+	idle;				/* Wait for the PLL to stablize */
+	sti r2;				/* Enable interrupts */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+#endif
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf537/init_sdram_bootrom_initblock.S b/cpu/bf537/init_sdram_bootrom_initblock.S
new file mode 100644
index 0000000..f9adbb9
--- /dev/null
+++ b/cpu/bf537/init_sdram_bootrom_initblock.S
@@ -0,0 +1,199 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (BFIN_BOOT_MODE != BF537_UART_BOOT)
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
+#endif
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+#if (BFIN_BOOT_MODE == BF537_SPI_MASTER_BOOT)
+	p0.h = hi(SIC_IWR);
+	p0.l = lo(SIC_IWR);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+
+	p0.h = hi(SPI_BAUD);
+	p0.l = lo(SPI_BAUD);
+	r0.l = CONFIG_SPI_BAUD_INITBLOCK;
+	w[p0] = r0.l;
+	SSYNC;
+#endif
+
+#if (BFIN_BOOT_MODE != BF537_UART_BOOT)
+
+#ifdef CONFIG_BF537
+	/* Enable PHY CLK buffer output */
+	p0.h = hi(VR_CTL);
+	p0.l = lo(VR_CTL);
+	r0.l = w[p0];
+	bitset(r0, 14);
+	w[p0] = r0.l;
+	ssync;
+#endif
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN      : 1=All Clocks off
+	 *   - [3]     = STOPCK    : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF	: 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier */
+	r0 = r0 << 9;			/* Shift it over */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2?*/
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL? */
+	r1 = r1 << 8;			/* Shift it over */
+	r0 = r1 | r0;			/* add them all together */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address */
+	cli r2;				/* Disable interrupts */
+	ssync;
+	w[p0] = r0.l;			/* Set the value */
+	idle;				/* Wait for the PLL to stablize */
+	sti r2;				/* Enable interrupts */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+#endif
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 */
+
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf537/interrupt.S b/cpu/bf537/interrupt.S
new file mode 100644
index 0000000..a71df55
--- /dev/null
+++ b/cpu/bf537/interrupt.S
@@ -0,0 +1,246 @@
+/*
+ * U-boot - interrupt.S Processing of interrupts and exception handling
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * This file is based on interrupt.S
+ *
+ * Copyright (C) 2003  Metrowerks, Inc. <mwaddel@metrowerks.com>
+ * Copyright (C) 2002  Arcturus Networks Ltd. Ted Ma <mated@sympatico.ca>
+ * Copyright (C) 1998  D. Jeff Dionne <jeff@ryeham.ee.ryerson.ca>,
+ *                     Kenneth Albanowski <kjahds@kjahds.com>,
+ *                     The Silver Hammer Group, Ltd.
+ *
+ * (c) 1995, Dionne & Associates
+ * (c) 1995, DKG Display Tech.
+ *
+ * This file is also based on exception.asm
+ * (C) Copyright 2001-2005 - Analog Devices, Inc.  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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#define ASSEMBLY
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/hw_irq.h>
+#include <asm/entry.h>
+#include <asm/blackfin_defs.h>
+
+.global _blackfin_irq_panic;
+
+.text
+.align 2
+
+#ifndef CONFIG_KGDB
+.global _evt_emulation
+_evt_emulation:
+	SAVE_CONTEXT
+	r0 = IRQ_EMU;
+	r1 = seqstat;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+	rte;
+#endif
+
+.global _evt_nmi
+_evt_nmi:
+	SAVE_CONTEXT
+	r0 = IRQ_NMI;
+	r1 = RETN;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+
+_evt_nmi_exit:
+	rtn;
+
+.global _trap
+_trap:
+	SAVE_ALL_SYS
+	r0 = sp;	/* stack frame pt_regs pointer argument ==> r0 */
+	sp += -12;
+	call _trap_c
+	sp += 12;
+	RESTORE_ALL_SYS
+	rtx;
+
+.global _evt_rst
+_evt_rst:
+	SAVE_CONTEXT
+	r0 = IRQ_RST;
+	r1 = RETN;
+	sp += -12;
+	call _do_reset;
+	sp += 12;
+
+_evt_rst_exit:
+	rtn;
+
+irq_panic:
+	r0 = IRQ_EVX;
+	r1 =  sp;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+
+.global _evt_ivhw
+_evt_ivhw:
+	SAVE_CONTEXT
+	RAISE 14;
+
+_evt_ivhw_exit:
+	 rti;
+
+.global _evt_timer
+_evt_timer:
+	SAVE_CONTEXT
+	r0 = IRQ_CORETMR;
+	sp += -12;
+	/* Polling method used now. */
+	/* call timer_int; */
+	sp += 12;
+	RESTORE_CONTEXT
+	rti;
+	nop;
+
+.global _evt_evt7
+_evt_evt7:
+	SAVE_CONTEXT
+	r0 = 7;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt7_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt8
+_evt_evt8:
+	SAVE_CONTEXT
+	r0 = 8;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt8_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt9
+_evt_evt9:
+	SAVE_CONTEXT
+	r0 = 9;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt9_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt10
+_evt_evt10:
+	SAVE_CONTEXT
+	r0 = 10;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt10_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt11
+_evt_evt11:
+	SAVE_CONTEXT
+	r0 = 11;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt11_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt12
+_evt_evt12:
+	SAVE_CONTEXT
+	r0 = 12;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+evt_evt12_exit:
+	 RESTORE_CONTEXT
+	 rti;
+
+.global _evt_evt13
+_evt_evt13:
+	SAVE_CONTEXT
+	r0 = 13;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt13_exit:
+	 RESTORE_CONTEXT
+	 rti;
+
+.global _evt_system_call
+_evt_system_call:
+	[--sp] = r0;
+	[--SP] = RETI;
+	r0 = [sp++];
+	r0 += 2;
+	[--sp] = r0;
+	RETI = [SP++];
+	r0 = [SP++];
+	SAVE_CONTEXT
+	sp += -12;
+	call _exception_handle;
+	sp += 12;
+	RESTORE_CONTEXT
+	RTI;
+
+evt_system_call_exit:
+	rti;
+
+.global _evt_soft_int1
+_evt_soft_int1:
+	[--sp] = r0;
+	[--SP] = RETI;
+	r0 = [sp++];
+	r0 += 2;
+	[--sp] = r0;
+	RETI = [SP++];
+	r0 = [SP++];
+	SAVE_CONTEXT
+	sp += -12;
+	call _exception_handle;
+	sp += 12;
+	RESTORE_CONTEXT
+	RTI;
+
+evt_soft_int1_exit:
+	rti;
diff --git a/cpu/bf537/interrupts.c b/cpu/bf537/interrupts.c
new file mode 100644
index 0000000..d2213b1
--- /dev/null
+++ b/cpu/bf537/interrupts.c
@@ -0,0 +1,174 @@
+/*
+ * U-boot - interrupts.c Interrupt related routines
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on interrupts.c
+ * Copyright 1996 Roman Zippel
+ * Copyright 1999 D. Jeff Dionne <jeff@uclinux.org>
+ * Copyright 2000-2001 Lineo, Inc. D. Jefff Dionne <jeff@lineo.ca>
+ * Copyright 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
+ * Copyright 2003 Metrowerks/Motorola
+ * Copyright 2003 Bas Vermeulen <bas@buyways.nl>,
+ *			BuyWays B.V. (www.buyways.nl)
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <asm/machdep.h>
+#include <asm/irq.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include "cpu.h"
+
+static ulong timestamp;
+static ulong last_time;
+static int int_flag;
+
+int irq_flags;			/* needed by asm-blackfin/system.h */
+
+/* Functions just to satisfy the linker */
+
+/*
+ * This function is derived from PowerPC code (read timebase as long long).
+ * On BF533 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 BF533 it returns the number of timer ticks per second.
+ */
+ulong get_tbclk (void)
+{
+	ulong tbclk;
+
+	tbclk = CFG_HZ;
+	return tbclk;
+}
+
+void enable_interrupts(void)
+{
+	restore_flags(int_flag);
+}
+
+int disable_interrupts(void)
+{
+	save_and_cli(int_flag);
+	return 1;
+}
+
+int interrupt_init(void)
+{
+	return (0);
+}
+
+void udelay(unsigned long usec)
+{
+	unsigned long delay, start, stop;
+	unsigned long cclk;
+	cclk = (CONFIG_CCLK_HZ);
+
+	while (usec > 1) {
+		/*
+		 * how many clock ticks to delay?
+		 *  - request(in useconds) * clock_ticks(Hz) / useconds/second
+		 */
+		if (usec < 1000) {
+			delay = (usec * (cclk / 244)) >> 12;
+			usec = 0;
+		} else {
+			delay = (1000 * (cclk / 244)) >> 12;
+			usec -= 1000;
+		}
+
+		asm volatile (" %0 = CYCLES;":"=r" (start));
+		do {
+			asm volatile (" %0 = CYCLES; ":"=r" (stop));
+		} while (stop - start < delay);
+	}
+
+	return;
+}
+
+void timer_init(void)
+{
+	*pTCNTL = 0x1;
+	*pTSCALE = 0x0;
+	*pTCOUNT = MAX_TIM_LOAD;
+	*pTPERIOD = MAX_TIM_LOAD;
+	*pTCNTL = 0x7;
+	asm("CSYNC;");
+
+	timestamp = 0;
+	last_time = 0;
+}
+
+/* Any network command or flash
+ * command is started get_timer shall
+ * be called before TCOUNT gets reset,
+ * to implement the accurate timeouts.
+ *
+ * How ever milliconds doesn't return
+ * the number that has been elapsed from
+ * the last reset.
+ *
+ *  As get_timer is used in the u-boot
+ *  only for timeouts this should be
+ *  sufficient
+ */
+ulong get_timer(ulong base)
+{
+	ulong milisec;
+
+	/* Number of clocks elapsed */
+	ulong clocks = (MAX_TIM_LOAD - (*pTCOUNT));
+
+	/**
+	 * Find if the TCOUNT is reset
+	 * timestamp gives the number of times
+	 * TCOUNT got reset
+	 */
+	if (clocks < last_time)
+		timestamp++;
+	last_time = clocks;
+
+	/* Get the number of milliseconds */
+	milisec = clocks / (CONFIG_CCLK_HZ / 1000);
+
+	/**
+	 * Find the number of millisonds
+	 * that got elapsed before this TCOUNT cycle
+	 */
+	milisec += timestamp * (MAX_TIM_LOAD / (CONFIG_CCLK_HZ / 1000));
+
+	return (milisec - base);
+}
+
+void reset_timer (void)
+{
+	timestamp = 0;
+}
diff --git a/cpu/bf537/ints.c b/cpu/bf537/ints.c
new file mode 100644
index 0000000..5586689
--- /dev/null
+++ b/cpu/bf537/ints.c
@@ -0,0 +1,117 @@
+/*
+ * U-boot - ints.c Interrupt related routines
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on ints.c
+ *
+ * Apr18 2003, Changed by HuTao to support interrupt cascading for Blackfin
+ *             drivers
+ *
+ * Copyright 1996 Roman Zippel
+ * Copyright 1999 D. Jeff Dionne <jeff@uclinux.org>
+ * Copyright 2000-2001 Lineo, Inc. D. Jefff Dionne <jeff@lineo.ca>
+ * Copyright 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
+ * Copyright 2003 Metrowerks/Motorola
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <linux/stddef.h>
+#include <asm/system.h>
+#include <asm/irq.h>
+#include <asm/traps.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/machdep.h>
+#include <asm/setup.h>
+#include <asm/blackfin.h>
+#include "cpu.h"
+
+void blackfin_irq_panic(int reason, struct pt_regs *regs)
+{
+	printf("\n\nException: IRQ 0x%x entered\n", reason);
+	printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+	printf("stack frame=0x%x, ", (unsigned int)regs);
+	printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
+	dump(regs);
+	printf("Unhandled IRQ or exceptions!\n");
+	printf("Please reset the board \n");
+}
+
+void blackfin_init_IRQ(void)
+{
+	*(unsigned volatile long *)(SIC_IMASK) = SIC_UNMASK_ALL;
+	cli();
+#ifndef CONFIG_KGDB
+	*(unsigned volatile long *)(EVT_EMULATION_ADDR) = 0x0;
+#endif
+	*(unsigned volatile long *)(EVT_NMI_ADDR) =
+	    (unsigned volatile long)evt_nmi;
+	*(unsigned volatile long *)(EVT_EXCEPTION_ADDR) =
+	    (unsigned volatile long)trap;
+	*(unsigned volatile long *)(EVT_HARDWARE_ERROR_ADDR) =
+	    (unsigned volatile long)evt_ivhw;
+	*(unsigned volatile long *)(EVT_RESET_ADDR) =
+	    (unsigned volatile long)evt_rst;
+	*(unsigned volatile long *)(EVT_TIMER_ADDR) =
+	    (unsigned volatile long)evt_timer;
+	*(unsigned volatile long *)(EVT_IVG7_ADDR) =
+	    (unsigned volatile long)evt_evt7;
+	*(unsigned volatile long *)(EVT_IVG8_ADDR) =
+	    (unsigned volatile long)evt_evt8;
+	*(unsigned volatile long *)(EVT_IVG9_ADDR) =
+	    (unsigned volatile long)evt_evt9;
+	*(unsigned volatile long *)(EVT_IVG10_ADDR) =
+	    (unsigned volatile long)evt_evt10;
+	*(unsigned volatile long *)(EVT_IVG11_ADDR) =
+	    (unsigned volatile long)evt_evt11;
+	*(unsigned volatile long *)(EVT_IVG12_ADDR) =
+	    (unsigned volatile long)evt_evt12;
+	*(unsigned volatile long *)(EVT_IVG13_ADDR) =
+	    (unsigned volatile long)evt_evt13;
+	*(unsigned volatile long *)(EVT_IVG14_ADDR) =
+	    (unsigned volatile long)evt_system_call;
+	*(unsigned volatile long *)(EVT_IVG15_ADDR) =
+	    (unsigned volatile long)evt_soft_int1;
+	*(volatile unsigned long *)ILAT = 0;
+	asm("csync;");
+	sti();
+	*(volatile unsigned long *)IMASK = 0xffbf;
+	asm("csync;");
+}
+
+void exception_handle(void)
+{
+#if defined (CONFIG_PANIC_HANG)
+	display_excp();
+#else
+	udelay(100000);		/* allow messages to go out */
+	do_reset(NULL, 0, 0, NULL);
+#endif
+}
+
+void display_excp(void)
+{
+	printf("Exception!\n");
+}
diff --git a/cpu/bf537/serial.c b/cpu/bf537/serial.c
new file mode 100644
index 0000000..e04d08a
--- /dev/null
+++ b/cpu/bf537/serial.c
@@ -0,0 +1,194 @@
+/*
+ * U-boot - serial.c Serial driver for BF537
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on
+ * bf537_serial.c: Serial driver for BlackFin BF537 internal UART.
+ * Copyright (c) 2003	Bas Vermeulen <bas@buyways.nl>,
+ * 			BuyWays B.V. (www.buyways.nl)
+ *
+ * Based heavily on blkfinserial.c
+ * blkfinserial.c: Serial driver for BlackFin DSP internal USRTs.
+ * Copyright(c) 2003	Metrowerks	<mwaddel@metrowerks.com>
+ * Copyright(c)	2001	Tony Z. Kou	<tonyko@arcturusnetworks.com>
+ * Copyright(c)	2001-2002 Arcturus Networks Inc. <www.arcturusnetworks.com>
+ *
+ * Based on code from 68328 version serial driver imlpementation which was:
+ * Copyright (C) 1995       David S. Miller    <davem@caip.rutgers.edu>
+ * Copyright (C) 1998       Kenneth Albanowski <kjahds@kjahds.com>
+ * Copyright (C) 1998, 1999 D. Jeff Dionne     <jeff@uclinux.org>
+ * Copyright (C) 1999       Vladimir Gurevich  <vgurevic@cisco.com>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/segment.h>
+#include <asm/bitops.h>
+#include <asm/delay.h>
+#include <asm/uaccess.h>
+#include <asm/io.h>
+#include "serial.h"
+
+unsigned long pll_div_fact;
+
+void calc_baud(void)
+{
+	unsigned char i;
+	int temp;
+	u_long sclk = get_sclk();
+
+	for (i = 0; i < sizeof(baud_table) / sizeof(int); i++) {
+		temp = sclk / (baud_table[i] * 8);
+		if ((temp & 0x1) == 1) {
+			temp++;
+		}
+		temp = temp / 2;
+		hw_baud_table[i].dl_high = (temp >> 8) & 0xFF;
+		hw_baud_table[i].dl_low = (temp) & 0xFF;
+	}
+}
+
+void serial_setbrg(void)
+{
+	int i;
+	DECLARE_GLOBAL_DATA_PTR;
+
+	calc_baud();
+
+	for (i = 0; i < sizeof(baud_table) / sizeof(int); i++) {
+		if (gd->baudrate == baud_table[i])
+			break;
+	}
+
+	/* Enable UART */
+	*pUART_GCTL |= UART_GCTL_UCEN;
+	sync();
+
+	/* Set DLAB in LCR to Access DLL and DLH */
+	ACCESS_LATCH;
+	sync();
+
+	*pUART_DLL = hw_baud_table[i].dl_low;
+	sync();
+	*pUART_DLH = hw_baud_table[i].dl_high;
+	sync();
+
+	/* Clear DLAB in LCR to Access THR RBR IER */
+	ACCESS_PORT_IER;
+	sync();
+
+	/* Enable  ERBFI and ELSI interrupts
+	 * to poll SIC_ISR register*/
+	*pUART_IER = UART_IER_ELSI | UART_IER_ERBFI | UART_IER_ETBEI;
+	sync();
+
+	/* Set LCR to Word Lengh 8-bit word select */
+	*pUART_LCR = UART_LCR_WLS8;
+	sync();
+
+	return;
+}
+
+int serial_init(void)
+{
+	serial_setbrg();
+	return (0);
+}
+
+void serial_putc(const char c)
+{
+	if ((*pUART_LSR) & UART_LSR_TEMT) {
+		if (c == '\n')
+			serial_putc('\r');
+
+		local_put_char(c);
+	}
+
+	while (!((*pUART_LSR) & UART_LSR_TEMT))
+		SYNC_ALL;
+
+	return;
+}
+
+int serial_tstc(void)
+{
+	if (*pUART_LSR & UART_LSR_DR)
+		return 1;
+	else
+		return 0;
+}
+
+int serial_getc(void)
+{
+	unsigned short uart_lsr_val, uart_rbr_val;
+	unsigned long isr_val;
+	int ret;
+
+	/* Poll for RX Interrupt */
+	while (!((isr_val =
+		  *(volatile unsigned long *)SIC_ISR) & IRQ_UART_RX_BIT)) ;
+	asm("csync;");
+
+	uart_lsr_val = *pUART_LSR;	/* Clear status bit */
+	uart_rbr_val = *pUART_RBR;	/* getc() */
+
+	if (isr_val & IRQ_UART_ERROR_BIT) {
+		ret = -1;
+	} else {
+		ret = uart_rbr_val & 0xff;
+	}
+
+	return ret;
+}
+
+void serial_puts(const char *s)
+{
+	while (*s) {
+		serial_putc(*s++);
+	}
+}
+
+static void local_put_char(char ch)
+{
+	int flags = 0;
+	unsigned long isr_val;
+
+	save_and_cli(flags);
+
+	/* Poll for TX Interruput */
+	while (!((isr_val = *pSIC_ISR) & IRQ_UART_TX_BIT)) ;
+	asm("csync;");
+
+	*pUART_THR = ch;	/* putc() */
+
+	if (isr_val & IRQ_UART_ERROR_BIT) {
+		printf("?");
+	}
+
+	restore_flags(flags);
+
+	return;
+}
diff --git a/cpu/bf537/serial.h b/cpu/bf537/serial.h
new file mode 100644
index 0000000..76555c2
--- /dev/null
+++ b/cpu/bf537/serial.h
@@ -0,0 +1,77 @@
+/*
+ * U-boot - bf537_serial.h Serial Driver defines
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on
+ * bf533_serial.h: Definitions for the BlackFin BF533 DSP serial driver.
+ * Copyright (C) 2003	Bas Vermeulen <bas@buyways.nl>
+ * 			BuyWays B.V. (www.buyways.nl)
+ *
+ * Based heavily on:
+ * blkfinserial.h: Definitions for the BlackFin DSP serial driver.
+ *
+ * Copyright (C) 2001	Tony Z. Kou	tonyko@arcturusnetworks.com
+ * Copyright (C) 2001   Arcturus Networks Inc. <www.arcturusnetworks.com>
+ *
+ * Based on code from 68328serial.c which was:
+ * Copyright (C) 1995       David S. Miller    <davem@caip.rutgers.edu>
+ * Copyright (C) 1998       Kenneth Albanowski <kjahds@kjahds.com>
+ * Copyright (C) 1998, 1999 D. Jeff Dionne     <jeff@uclinux.org>
+ * Copyright (C) 1999       Vladimir Gurevich  <vgurevic@cisco.com>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#ifndef _Bf537_SERIAL_H
+#define _Bf537_SERIAL_H
+
+#include <linux/config.h>
+#include <asm/blackfin.h>
+
+#define SYNC_ALL	__asm__ __volatile__ ("ssync;\n")
+#define ACCESS_LATCH	*pUART_LCR |= UART_LCR_DLAB;
+#define ACCESS_PORT_IER	*pUART_LCR &= (~UART_LCR_DLAB);
+
+void serial_setbrg(void);
+static void local_put_char(char ch);
+void calc_baud(void);
+void serial_setbrg(void);
+int serial_init(void);
+void serial_putc(const char c);
+int serial_tstc(void);
+int serial_getc(void);
+void serial_puts(const char *s);
+static void local_put_char(char ch);
+
+int baud_table[5] = { 9600, 19200, 38400, 57600, 115200 };
+
+struct {
+	unsigned char dl_high;
+	unsigned char dl_low;
+} hw_baud_table[5];
+
+#ifdef CONFIG_STAMP
+extern unsigned long pll_div_fact;
+#endif
+
+#endif
diff --git a/cpu/bf537/start.S b/cpu/bf537/start.S
new file mode 100644
index 0000000..4e02bcb
--- /dev/null
+++ b/cpu/bf537/start.S
@@ -0,0 +1,579 @@
+/*
+ * U-boot - start.S Startup file of u-boot for BF537
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on head.S
+ * Copyright (c) 2003  Metrowerks/Motorola
+ * Copyright (C) 1998  D. Jeff Dionne <jeff@ryeham.ee.ryerson.ca>,
+ *                     Kenneth Albanowski <kjahds@kjahds.com>,
+ *                     The Silver Hammer Group, Ltd.
+ * (c) 1995, Dionne & Associates
+ * (c) 1995, DKG Display Tech.
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+/*
+ * Note: A change in this file subsequently requires a change in
+ *       board/$(board_name)/config.mk for a valid u-boot.bin
+ */
+
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.global _stext;
+.global __bss_start;
+.global start;
+.global _start;
+.global _rambase;
+.global _ramstart;
+.global _ramend;
+.global _bf533_data_dest;
+.global _bf533_data_size;
+.global edata;
+.global _initialize;
+.global _exit;
+.global flashdataend;
+.global init_sdram;
+.global _icache_enable;
+.global _dcache_enable;
+#if defined(CONFIG_BF537)&&defined(CONFIG_POST)
+.global _memory_post_test;
+.global _post_flag;
+#endif
+
+#if (BFIN_BOOT_MODE == BF537_UART_BOOT)
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
+#endif
+#endif
+
+.text
+_start:
+start:
+_stext:
+
+	R0 = 0x32;
+	SYSCFG = R0;
+	SSYNC;
+
+	/* As per HW reference manual DAG registers,
+	 * DATA and Address resgister shall be zero'd
+	 * in initialization, after a reset state
+	 */
+	r1 = 0;	/* Data registers zero'd */
+	r2 = 0;
+	r3 = 0;
+	r4 = 0;
+	r5 = 0;
+	r6 = 0;
+	r7 = 0;
+
+	p0 = 0; /* Address registers zero'd */
+	p1 = 0;
+	p2 = 0;
+	p3 = 0;
+	p4 = 0;
+	p5 = 0;
+
+	i0 = 0; /* DAG Registers zero'd */
+	i1 = 0;
+	i2 = 0;
+	i3 = 0;
+	m0 = 0;
+	m1 = 0;
+	m3 = 0;
+	m3 = 0;
+	l0 = 0;
+	l1 = 0;
+	l2 = 0;
+	l3 = 0;
+	b0 = 0;
+	b1 = 0;
+	b2 = 0;
+	b3 = 0;
+
+	/* Set loop counters to zero, to make sure that
+	 * hw loops are disabled.
+	 */
+	r0  = 0;
+	lc0 = r0;
+	lc1 = r0;
+
+	SSYNC;
+
+	/* Check soft reset status */
+	p0.h = SWRST >> 16;
+	p0.l = SWRST & 0xFFFF;
+	r0.l = w[p0];
+
+	cc = bittst(r0, 15);
+	if !cc jump no_soft_reset;
+
+	/* Clear Soft reset */
+	r0 = 0x0000;
+	w[p0] = r0;
+	ssync;
+
+no_soft_reset:
+	nop;
+
+	/* Clear EVT registers */
+	p0.h = (EVT_EMULATION_ADDR >> 16);
+	p0.l = (EVT_EMULATION_ADDR & 0xFFFF);
+	p0 += 8;
+	p1 = 14;
+	r1 = 0;
+	LSETUP(4,4) lc0 = p1;
+	[ p0 ++ ] = r1;
+
+#if (BFIN_BOOT_MODE != BF537_SPI_MASTER_BOOT)
+	p0.h = hi(SIC_IWR);
+	p0.l = lo(SIC_IWR);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+#endif
+
+#if (BFIN_BOOT_MODE == BF537_UART_BOOT)
+
+	p0.h = hi(SIC_IWR);
+	p0.l = lo(SIC_IWR);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+
+	/*
+	* PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	*/
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	* Put SDRAM in self-refresh, incase anything is running
+	*/
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	*  Set PLL_CTL with the value that we calculate in R0
+	*   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	*   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	*   - [7]     = output delay (add 200ps of delay to mem signals)
+	*   - [6]     = input delay (add 200ps of input delay to mem signals)
+	*   - [5]     = PDWN      : 1=All Clocks off
+	*   - [3]     = STOPCK    : 1=Core Clock off
+	*   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	*   - [0]     = DF	  : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	*   all other bits set to zero
+	*/
+
+	r0 = CONFIG_VCO_MULT & 63;      /* Load the VCO multiplier         */
+	r0 = r0 << 9;                   /* Shift it over,                  */
+	r1 = CONFIG_CLKIN_HALF;        /* Do we need to divide CLKIN by 2?*/
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;         /* Bypass the PLL?                 */
+	r1 = r1 << 8;                   /* Shift it over                   */
+	r0 = r1 | r0;                   /* add them all together           */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);             /* Load the address                */
+	cli r2;                         /* Disable interrupts              */
+		ssync;
+	w[p0] = r0.l;                   /* Set the value                   */
+	idle;                           /* Wait for the PLL to stablize    */
+	sti r2;                         /* Enable interrupts               */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+#endif
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 * we need to read MAC address from FLASH
+	 */
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+#if ((BFIN_BOOT_MODE != BF537_SPI_MASTER_BOOT) && (BFIN_BOOT_MODE != BF537_UART_BOOT))
+	sp.l = (0xffb01000 & 0xFFFF);
+	sp.h = (0xffb01000 >> 16);
+
+	call init_sdram;
+#endif
+
+
+#if defined(CONFIG_BF537)&&defined(CONFIG_POST)
+	/* DMA POST code to Hi of L1 SRAM */
+postcopy:
+	/* P1 Points to the beginning of SYSTEM MMR Space */
+	P1.H = hi(SYSMMR_BASE);
+	P1.L = lo(SYSMMR_BASE);
+
+	R0.H = _text_l1;
+	R0.L = _text_l1;
+	R1.H = _etext_l1;
+	R1.L = _etext_l1;
+	R2 = R1 - R0;           /* Count */
+	R0.H = _etext;
+	R0.L = _etext;
+	R1.H = (CFG_MONITOR_BASE >> 16);
+	R1.L = (CFG_MONITOR_BASE & 0xFFFF);
+	R0 = R0 - R1;
+	R1.H = (CFG_FLASH_BASE >> 16);
+	R1.L = (CFG_FLASH_BASE & 0xFFFF);
+	R0 = R0 + R1;		/* Source Address */
+	R1.H = hi(L1_ISRAM);    /* Destination Address (high) */
+	R1.L = lo(L1_ISRAM);    /* Destination Address (low) */
+	R3.L = DMAEN;           /* Source DMAConfig Value (8-bit words) */
+	/* Destination DMAConfig Value (8-bit words) */
+	R4.L = (DI_EN | WNR | DMAEN);
+
+	R6 = 0x1 (Z);
+	W[P1+OFFSET_(MDMA_S0_X_MODIFY)] = R6;   /* Source Modify = 1 */
+	W[P1+OFFSET_(MDMA_D0_X_MODIFY)] = R6;   /* Destination Modify = 1 */
+
+	[P1+OFFSET_(MDMA_S0_START_ADDR)] = R0;  /* Set Source Base Address */
+	W[P1+OFFSET_(MDMA_S0_X_COUNT)] = R2;    /* Set Source Count */
+	/* Set Source  DMAConfig = DMA Enable,
+	Memory Read,  8-Bit Transfers, 1-D DMA, Flow - Stop */
+	W[P1+OFFSET_(MDMA_S0_CONFIG)] = R3;
+
+	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;  /* Set Destination Base Address */
+	W[P1+OFFSET_(MDMA_D0_X_COUNT)] = R2;    /* Set Destination Count */
+	/* Set Destination DMAConfig = DMA Enable,
+	Memory Write, 8-Bit Transfers, 1-D DMA, Flow - Stop, IOC */
+	W[P1+OFFSET_(MDMA_D0_CONFIG)] = R4;
+
+POST_DMA_DONE:
+	p0.h = hi(MDMA_D0_IRQ_STATUS);
+	p0.l = lo(MDMA_D0_IRQ_STATUS);
+	R0 = W[P0](Z);
+	CC = BITTST(R0, 0);
+	if ! CC jump POST_DMA_DONE
+
+	R0 = 0x1;
+	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0; /* Write 1 to clear DMA interrupt */
+
+	/* DMA POST data to Hi of L1 SRAM */
+	R0.H = _rodata_l1;
+	R0.L = _rodata_l1;
+	R1.H = _erodata_l1;
+	R1.L = _erodata_l1;
+	R2 = R1 - R0;           /* Count */
+	R0.H = _erodata;
+	R0.L = _erodata;
+	R1.H = (CFG_MONITOR_BASE >> 16);
+	R1.L = (CFG_MONITOR_BASE & 0xFFFF);
+	R0 = R0 - R1;
+	R1.H = (CFG_FLASH_BASE >> 16);
+	R1.L = (CFG_FLASH_BASE & 0xFFFF);
+	R0 = R0 + R1;           /* Source Address */
+	R1.H = hi(DATA_BANKB_SRAM);    /* Destination Address (high) */
+	R1.L = lo(DATA_BANKB_SRAM);    /* Destination Address (low) */
+	R3.L = DMAEN;           /* Source DMAConfig Value (8-bit words) */
+	R4.L = (DI_EN | WNR | DMAEN);   /* Destination DMAConfig Value (8-bit words) */
+
+	R6 = 0x1 (Z);
+	W[P1+OFFSET_(MDMA_S0_X_MODIFY)] = R6;   /* Source Modify = 1 */
+	W[P1+OFFSET_(MDMA_D0_X_MODIFY)] = R6;   /* Destination Modify = 1 */
+
+	[P1+OFFSET_(MDMA_S0_START_ADDR)] = R0;  /* Set Source Base Address */
+	W[P1+OFFSET_(MDMA_S0_X_COUNT)] = R2;    /* Set Source Count */
+	/* Set Source  DMAConfig = DMA Enable,
+	Memory Read,  8-Bit Transfers, 1-D DMA, Flow - Stop */
+	W[P1+OFFSET_(MDMA_S0_CONFIG)] = R3;
+
+	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;  /* Set Destination Base Address */
+	W[P1+OFFSET_(MDMA_D0_X_COUNT)] = R2;    /* Set Destination Count */
+	/* Set Destination DMAConfig = DMA Enable,
+	Memory Write, 8-Bit Transfers, 1-D DMA, Flow - Stop, IOC */
+	W[P1+OFFSET_(MDMA_D0_CONFIG)] = R4;
+
+POST_DATA_DMA_DONE:
+	p0.h = hi(MDMA_D0_IRQ_STATUS);
+	p0.l = lo(MDMA_D0_IRQ_STATUS);
+	R0 = W[P0](Z);
+	CC = BITTST(R0, 0);
+	if ! CC jump POST_DATA_DMA_DONE
+
+	R0 = 0x1;
+	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0; /* Write 1 to clear DMA interrupt */
+
+	p0.l = _memory_post_test;
+	p0.h = _memory_post_test;
+	r0 = 0x0;
+	call (p0);
+	r7 = r0;				/* save return value */
+
+	call init_sdram;
+#endif
+
+	/* relocate into to RAM */
+	call get_pc;
+offset:
+	r2.l = offset;
+	r2.h = offset;
+	r3.l = start;
+	r3.h = start;
+	r1 = r2 - r3;
+
+	r0 = r0 - r1;
+	p1 = r0;
+
+	p2.l = (CFG_MONITOR_BASE & 0xffff);
+	p2.h = (CFG_MONITOR_BASE >> 16);
+
+	p3 = 0x04;
+	p4.l = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) & 0xffff);
+	p4.h = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) >> 16);
+loop1:
+	r1 = [p1 ++ p3];
+	[p2 ++ p3] = r1;
+	cc=p2==p4;
+	if !cc jump loop1;
+	/*
+	 * configure STACK
+	 */
+	r0.h = (CONFIG_STACKBASE >> 16);
+	r0.l = (CONFIG_STACKBASE & 0xFFFF);
+	sp = r0;
+	fp = sp;
+
+	/*
+	 * This next section keeps the processor in supervisor mode
+	 * during kernel boot.  Switches to user mode at end of boot.
+	 * See page 3-9 of Hardware Reference manual for documentation.
+	 */
+
+	/* To keep ourselves in the supervisor mode */
+	p0.l = (EVT_IVG15_ADDR & 0xFFFF);
+	p0.h = (EVT_IVG15_ADDR >> 16);
+
+	p1.l = _real_start;
+	p1.h = _real_start;
+	[p0] = p1;
+
+	p0.l = (IMASK & 0xFFFF);
+	p0.h = (IMASK >> 16);
+	r0.l = LO(IVG15_POS);
+	r0.h = HI(IVG15_POS);
+	[p0] = r0;
+	raise 15;
+	p0.l = WAIT_HERE;
+	p0.h = WAIT_HERE;
+	reti = p0;
+	rti;
+
+WAIT_HERE:
+	jump WAIT_HERE;
+
+.global _real_start;
+_real_start:
+	[ -- sp ] = reti;
+
+#ifdef CONFIG_BF537
+/* Initialise General-Purpose I/O Modules on BF537
+ * Rev 0.0 Anomaly 05000212 - PORTx_FER,
+ * PORT_MUX Registers Do Not accept "writes" correctly
+ */
+	p0.h = hi(PORTF_FER);
+	p0.l = lo(PORTF_FER);
+	R0.L = W[P0]; /* Read */
+	nop;
+	nop;
+	nop;
+	ssync;
+	R0 = 0x000F(Z);
+	W[P0] = R0.L; /* Write */
+	nop;
+	nop;
+	nop;
+	ssync;
+	W[P0] = R0.L; /* Enable peripheral function of PORTF for UART0 and UART1 */
+	nop;
+	nop;
+	nop;
+	ssync;
+
+	p0.h = hi(PORTH_FER);
+	p0.l = lo(PORTH_FER);
+	R0.L = W[P0]; /* Read */
+	nop;
+	nop;
+	nop;
+	ssync;
+	R0 = 0xFFFF(Z);
+	W[P0] = R0.L; /* Write */
+	nop;
+	nop;
+	nop;
+	ssync;
+	W[P0] = R0.L; /* Enable peripheral function of PORTH for MAC */
+	nop;
+	nop;
+	nop;
+	ssync;
+
+#endif
+
+	/* DMA reset code to Hi of L1 SRAM */
+copy:
+	P1.H = hi(SYSMMR_BASE);	/* P1 Points to the beginning of SYSTEM MMR Space */
+	P1.L = lo(SYSMMR_BASE);
+
+	R0.H = reset_start;	/* Source Address (high) */
+	R0.L = reset_start;	/* Source Address (low) */
+	R1.H = reset_end;
+	R1.L = reset_end;
+	R2 = R1 - R0;		/* Count */
+	R1.H = hi(L1_ISRAM);	/* Destination Address (high) */
+	R1.L = lo(L1_ISRAM);	/* Destination Address (low) */
+	R3.L = DMAEN;		/* Source DMAConfig Value (8-bit words) */
+	R4.L = (DI_EN | WNR | DMAEN);	/* Destination DMAConfig Value (8-bit words) */
+
+DMA:
+	R6 = 0x1 (Z);
+	W[P1+OFFSET_(MDMA_S0_X_MODIFY)] = R6;	/* Source Modify = 1 */
+	W[P1+OFFSET_(MDMA_D0_X_MODIFY)] = R6;	/* Destination Modify = 1 */
+
+	[P1+OFFSET_(MDMA_S0_START_ADDR)] = R0;	/* Set Source Base Address */
+	W[P1+OFFSET_(MDMA_S0_X_COUNT)] = R2;	/* Set Source Count */
+	/* Set Source  DMAConfig = DMA Enable,
+	Memory Read,  8-Bit Transfers, 1-D DMA, Flow - Stop */
+	W[P1+OFFSET_(MDMA_S0_CONFIG)] = R3;
+
+	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;	/* Set Destination Base Address */
+	W[P1+OFFSET_(MDMA_D0_X_COUNT)] = R2;	/* Set Destination Count */
+	/* Set Destination DMAConfig = DMA Enable,
+	Memory Write, 8-Bit Transfers, 1-D DMA, Flow - Stop, IOC */
+	W[P1+OFFSET_(MDMA_D0_CONFIG)] = R4;
+
+WAIT_DMA_DONE:
+	p0.h = hi(MDMA_D0_IRQ_STATUS);
+	p0.l = lo(MDMA_D0_IRQ_STATUS);
+	R0 = W[P0](Z);
+	CC = BITTST(R0, 0);
+	if ! CC jump WAIT_DMA_DONE
+
+	R0 = 0x1;
+	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0;	/* Write 1 to clear DMA interrupt */
+
+	/* Initialize BSS Section with 0 s */
+	p1.l = __bss_start;
+	p1.h = __bss_start;
+	p2.l = _end;
+	p2.h = _end;
+	r1 = p1;
+	r2 = p2;
+	r3 = r2 - r1;
+	r3 = r3 >> 2;
+	p3 = r3;
+	lsetup (_clear_bss, _clear_bss_end ) lc1 = p3;
+	CC = p2<=p1;
+	if CC jump _clear_bss_skip;
+	r0 = 0;
+_clear_bss:
+_clear_bss_end:
+	[p1++] = r0;
+_clear_bss_skip:
+
+#if defined(CONFIG_BF537)&&defined(CONFIG_POST)
+	p0.l = _post_flag;
+	p0.h = _post_flag;
+	r0   = r7;
+	[p0] = r0;
+#endif
+
+	p0.l = _start1;
+	p0.h = _start1;
+	jump (p0);
+
+reset_start:
+	p0.h = WDOG_CNT >> 16;
+	p0.l = WDOG_CNT & 0xffff;
+	r0 = 0x0010;
+	w[p0] = r0;
+	p0.h = WDOG_CTL >> 16;
+	p0.l = WDOG_CTL & 0xffff;
+	r0 = 0x0000;
+	w[p0] = r0;
+reset_wait:
+	jump reset_wait;
+
+reset_end:
+	nop;
+
+_exit:
+	jump.s	_exit;
+get_pc:
+	r0 = rets;
+	rts;
diff --git a/cpu/bf537/start1.S b/cpu/bf537/start1.S
new file mode 100644
index 0000000..6d4731b
--- /dev/null
+++ b/cpu/bf537/start1.S
@@ -0,0 +1,38 @@
+/*
+ * U-boot - start1.S Code running out of RAM after relocation
+ *
+ * Copyright (c) 2005-2007 Analog Devices 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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#define ASSEMBLY
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.global	start1;
+.global	_start1;
+
+.text
+_start1:
+start1:
+	sp += -12;
+	call	_board_init_f;
+	sp += 12;
diff --git a/cpu/bf537/traps.c b/cpu/bf537/traps.c
new file mode 100644
index 0000000..4e18e27
--- /dev/null
+++ b/cpu/bf537/traps.c
@@ -0,0 +1,240 @@
+/*
+ * U-boot - traps.c Routines related to interrupts and exceptions
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on
+ * No original Copyright holder listed,
+ * Probabily original (C) Roman Zippel (assigned DJD, 1999)
+ *
+ * Copyright 2003 Metrowerks - for Blackfin
+ * Copyright 2000-2001 Lineo, Inc. D. Jeff Dionne <jeff@lineo.ca>
+ * Copyright 1999-2000 D. Jeff Dionne, <jeff@uclinux.org>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <linux/types.h>
+#include <asm/errno.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/traps.h>
+#include <asm/machdep.h>
+#include "cpu.h"
+#include <asm/arch/anomaly.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
+
+void init_IRQ(void)
+{
+	blackfin_init_IRQ();
+	return;
+}
+
+void process_int(unsigned long vec, struct pt_regs *fp)
+{
+	printf("interrupt\n");
+	return;
+}
+
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+unsigned long last_cplb_fault_retx;
+
+static unsigned int cplb_sizes[4] =
+    { 1024, 4 * 1024, 1024 * 1024, 4 * 1024 * 1024 };
+
+void trap_c(struct pt_regs *regs)
+{
+	unsigned int addr;
+	unsigned long trapnr = (regs->seqstat) & SEQSTAT_EXCAUSE;
+	unsigned int i, j, size, *I0, *I1;
+	unsigned short data = 0;
+
+	switch (trapnr) {
+		/* 0x26 - Data CPLB Miss */
+	case VEC_CPLB_M:
+
+#ifdef ANOMALY_05000261
+		/*
+		 * Work around an anomaly: if we see a new DCPLB fault,
+		 * return without doing anything. Then,
+		 * if we get the same fault again, handle it.
+		 */
+		addr = last_cplb_fault_retx;
+		last_cplb_fault_retx = regs->retx;
+		printf("this time, curr = 0x%08x last = 0x%08x\n",
+		       addr, last_cplb_fault_retx);
+		if (addr != last_cplb_fault_retx)
+			goto trap_c_return;
+#endif
+		data = 1;
+
+	case VEC_CPLB_I_M:
+
+		if (data) {
+			addr = *pDCPLB_FAULT_ADDR;
+		} else {
+			addr = *pICPLB_FAULT_ADDR;
+		}
+		for (i = 0; i < page_descriptor_table_size; i++) {
+			if (data) {
+				size = cplb_sizes[dcplb_table[i][1] >> 16];
+				j = dcplb_table[i][0];
+			} else {
+				size = cplb_sizes[icplb_table[i][1] >> 16];
+				j = icplb_table[i][0];
+			}
+			if ((j <= addr) && ((j + size) > addr)) {
+				debug("found %i 0x%08x\n", i, j);
+				break;
+			}
+		}
+		if (i == page_descriptor_table_size) {
+			printf("something is really wrong\n");
+			do_reset(NULL, 0, 0, NULL);
+		}
+
+		/* Turn the cache off */
+		if (data) {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL &=
+			    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+			sync();
+		}
+
+		if (data) {
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		j = 0;
+		while (*I1 & CPLB_LOCK) {
+			debug("skipping %i %08p - %08x\n", j, I1, *I1);
+			*I0++;
+			*I1++;
+			j++;
+		}
+
+		debug("remove %i 0x%08x  0x%08x\n", j, *I0, *I1);
+
+		for (; j < 15; j++) {
+			debug("replace %i 0x%08x  0x%08x\n", j, I0, I0 + 1);
+			*I0++ = *(I0 + 1);
+			*I1++ = *(I1 + 1);
+		}
+
+		if (data) {
+			*I0 = dcplb_table[i][0];
+			*I1 = dcplb_table[i][1];
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			*I0 = icplb_table[i][0];
+			*I1 = icplb_table[i][1];
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		for (j = 0; j < 16; j++) {
+			debug("%i 0x%08x  0x%08x\n", j, *I0++, *I1++);
+		}
+
+		/* Turn the cache back on */
+		if (data) {
+			j = *(unsigned int *)DMEM_CONTROL;
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL =
+			    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | j;
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+			sync();
+		}
+
+		break;
+	default:
+		/* All traps come here */
+		printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+		printf("stack frame=0x%x, ", (unsigned int)regs);
+		printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
+		dump(regs);
+		printf("\n\n");
+
+		printf("Unhandled IRQ or exceptions!\n");
+		printf("Please reset the board \n");
+		do_reset(NULL, 0, 0, NULL);
+	}
+
+trap_c_return:
+	return;
+
+}
+
+void dump(struct pt_regs *fp)
+{
+	debug("RETE:  %08lx  RETN: %08lx  RETX: %08lx  RETS: %08lx\n",
+		 fp->rete, fp->retn, fp->retx, fp->rets);
+	debug("IPEND: %04lx  SYSCFG: %04lx\n", fp->ipend, fp->syscfg);
+	debug("SEQSTAT: %08lx    SP: %08lx\n", (long)fp->seqstat, (long)fp);
+	debug("R0: %08lx    R1: %08lx    R2: %08lx    R3: %08lx\n",
+		 fp->r0, fp->r1, fp->r2, fp->r3);
+	debug("R4: %08lx    R5: %08lx    R6: %08lx    R7: %08lx\n",
+		 fp->r4, fp->r5, fp->r6, fp->r7);
+	debug("P0: %08lx    P1: %08lx    P2: %08lx    P3: %08lx\n",
+		 fp->p0, fp->p1, fp->p2, fp->p3);
+	debug("P4: %08lx    P5: %08lx    FP: %08lx\n",
+		 fp->p4, fp->p5, fp->fp);
+	debug("A0.w: %08lx    A0.x: %08lx    A1.w: %08lx    A1.x: %08lx\n",
+		 fp->a0w, fp->a0x, fp->a1w, fp->a1x);
+
+	debug("LB0: %08lx  LT0: %08lx  LC0: %08lx\n",
+		 fp->lb0, fp->lt0, fp->lc0);
+	debug("LB1: %08lx  LT1: %08lx  LC1: %08lx\n",
+		 fp->lb1, fp->lt1, fp->lc1);
+	debug("B0: %08lx  L0: %08lx  M0: %08lx  I0: %08lx\n",
+		 fp->b0, fp->l0, fp->m0, fp->i0);
+	debug("B1: %08lx  L1: %08lx  M1: %08lx  I1: %08lx\n",
+		 fp->b1, fp->l1, fp->m1, fp->i1);
+	debug("B2: %08lx  L2: %08lx  M2: %08lx  I2: %08lx\n",
+		 fp->b2, fp->l2, fp->m2, fp->i2);
+	debug("B3: %08lx  L3: %08lx  M3: %08lx  I3: %08lx\n",
+		 fp->b3, fp->l3, fp->m3, fp->i3);
+
+	debug("DCPLB_FAULT_ADDR=%p\n", *pDCPLB_FAULT_ADDR);
+	debug("ICPLB_FAULT_ADDR=%p\n", *pICPLB_FAULT_ADDR);
+
+}
diff --git a/cpu/bf537/video.c b/cpu/bf537/video.c
new file mode 100644
index 0000000..3ff0151
--- /dev/null
+++ b/cpu/bf537/video.c
@@ -0,0 +1,194 @@
+/*
+ * (C) Copyright 2000
+ * Paolo Scaffardi, AIRVENT SAM s.p.a - RIMINI(ITALY), arsenio@tin.it
+ * (C) Copyright 2002
+ * Wolfgang Denk, wd@denx.de
+ * (C) Copyright 2006
+ * Aubrey Li, aubrey.li@analog.com
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <stdarg.h>
+#include <common.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <i2c.h>
+#include <linux/types.h>
+#include <devices.h>
+
+#ifdef CONFIG_VIDEO
+#define NTSC_FRAME_ADDR 0x06000000
+#include "video.h"
+
+/* NTSC OUTPUT SIZE  720 * 240 */
+#define VERTICAL	2
+#define HORIZONTAL	4
+
+int is_vblank_line(const int line)
+{
+	/*
+	 *  This array contains a single bit for each line in
+	 *  an NTSC frame.
+	 */
+	if ((line <= 18) || (line >= 264 && line <= 281) || (line == 528))
+		return true;
+
+	return false;
+}
+
+int NTSC_framebuffer_init(char *base_address)
+{
+	const int NTSC_frames = 1;
+	const int NTSC_lines = 525;
+	char *dest = base_address;
+	int frame_num, line_num;
+
+	for (frame_num = 0; frame_num < NTSC_frames; ++frame_num) {
+		for (line_num = 1; line_num <= NTSC_lines; ++line_num) {
+			unsigned int code;
+			int offset = 0;
+			int i;
+
+			if (is_vblank_line(line_num))
+				offset++;
+
+			if (line_num > 266 || line_num < 3)
+				offset += 2;
+
+			/* Output EAV code */
+			code = SystemCodeMap[offset].EAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output horizontal blanking */
+			for (i = 0; i < 67 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+
+			/* Output SAV */
+			code = SystemCodeMap[offset].SAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output empty horizontal data */
+			for (i = 0; i < 360 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+		}
+	}
+
+	return dest - base_address;
+}
+
+void fill_frame(char *Frame, int Value)
+{
+	int *OddPtr32;
+	int OddLine;
+	int *EvenPtr32;
+	int EvenLine;
+	int i;
+	int *data;
+	int m, n;
+
+	/* fill odd and even frames */
+	for (OddLine = 22, EvenLine = 285; OddLine < 263; OddLine++, EvenLine++) {
+		OddPtr32 = (int *)((Frame + (OddLine * 1716)) + 276);
+		EvenPtr32 = (int *)((Frame + (EvenLine * 1716)) + 276);
+		for (i = 0; i < 360; i++, OddPtr32++, EvenPtr32++) {
+			*OddPtr32 = Value;
+			*EvenPtr32 = Value;
+		}
+	}
+
+	for (m = 0; m < VERTICAL; m++) {
+		data = (int *)u_boot_logo.data;
+		for (OddLine = (22 + m), EvenLine = (285 + m);
+		     OddLine < (u_boot_logo.height * VERTICAL) + (22 + m);
+		     OddLine += VERTICAL, EvenLine += VERTICAL) {
+			OddPtr32 = (int *)((Frame + ((OddLine) * 1716)) + 276);
+			EvenPtr32 =
+			    (int *)((Frame + ((EvenLine) * 1716)) + 276);
+			for (i = 0; i < u_boot_logo.width / 2; i++) {
+				/* enlarge one pixel to m x n */
+				for (n = 0; n < HORIZONTAL; n++) {
+					*OddPtr32++ = *data;
+					*EvenPtr32++ = *data;
+				}
+				data++;
+			}
+		}
+	}
+}
+
+void video_putc(const char c)
+{
+}
+
+void video_puts(const char *s)
+{
+}
+
+static int video_init(void)
+{
+	char *NTSCFrame;
+	NTSCFrame = (char *)NTSC_FRAME_ADDR;
+	NTSC_framebuffer_init(NTSCFrame);
+	fill_frame(NTSCFrame, BLUE);
+
+	*pPPI_CONTROL = 0x0082;
+	*pPPI_FRAME = 0x020D;
+
+	*pDMA0_START_ADDR = NTSCFrame;
+	*pDMA0_X_COUNT = 0x035A;
+	*pDMA0_X_MODIFY = 0x0002;
+	*pDMA0_Y_COUNT = 0x020D;
+	*pDMA0_Y_MODIFY = 0x0002;
+	*pDMA0_CONFIG = 0x1015;
+	*pPPI_CONTROL = 0x0083;
+	return 0;
+}
+
+int drv_video_init(void)
+{
+	int error, devices = 1;
+
+	device_t videodev;
+
+	video_init();		/* Video initialization */
+
+	memset(&videodev, 0, sizeof(videodev));
+
+	strcpy(videodev.name, "video");
+	videodev.ext = DEV_EXT_VIDEO;	/* Video extensions */
+	videodev.flags = DEV_FLAGS_OUTPUT;	/* Output only */
+	videodev.putc = video_putc;	/* 'putc' function */
+	videodev.puts = video_puts;	/* 'puts' function */
+
+	error = device_register(&videodev);
+
+	return (error == 0) ? devices : error;
+}
+#endif
diff --git a/cpu/bf537/video.h b/cpu/bf537/video.h
new file mode 100644
index 0000000..a43553f
--- /dev/null
+++ b/cpu/bf537/video.h
@@ -0,0 +1,25 @@
+#include <video_logo.h>
+#define write_dest_byte(val) {*dest++=val;}
+#define BLACK   (0x01800180)	/* black pixel pattern	*/
+#define BLUE    (0x296E29F0)	/* blue pixel pattern	*/
+#define RED     (0x51F0515A)	/* red pixel pattern	*/
+#define MAGENTA (0x6ADE6ACA)	/* magenta pixel pattern*/
+#define GREEN   (0x91229136)	/* green pixel pattern	*/
+#define CYAN    (0xAA10AAA6)	/* cyan pixel pattern	*/
+#define YELLOW  (0xD292D210)	/* yellow pixel pattern	*/
+#define WHITE   (0xFE80FE80)	/* white pixel pattern	*/
+
+#define true 	1
+#define false	0
+
+typedef struct {
+	unsigned int SAV;
+	unsigned int EAV;
+} SystemCodeType;
+
+const SystemCodeType SystemCodeMap[4] = {
+	{0xFF000080, 0xFF00009D},
+	{0xFF0000AB, 0xFF0000B6},
+	{0xFF0000C7, 0xFF0000DA},
+	{0xFF0000EC, 0xFF0000F1}
+};
diff --git a/cpu/bf561/Makefile b/cpu/bf561/Makefile
new file mode 100644
index 0000000..2947169
--- /dev/null
+++ b/cpu/bf561/Makefile
@@ -0,0 +1,52 @@
+# U-boot - Makefile
+#
+# Copyright (c) 2005-2007 Analog Devices Inc.
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+# MA 02110-1301 USA
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(CPU).a
+
+START	= start.o start1.o interrupt.o cache.o flush.o init_sdram.o
+COBJS	= cpu.o traps.o ints.o serial.o interrupts.o video.o
+
+EXTRA = init_sdram_bootrom_initblock.o
+
+SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS) $(SOBJS))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(START) $(LIB) $(obj).depend $(EXTRA)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/bf561/cache.S b/cpu/bf561/cache.S
new file mode 100644
index 0000000..5bda5bf
--- /dev/null
+++ b/cpu/bf561/cache.S
@@ -0,0 +1,128 @@
+#define ASSEMBLY
+#include <asm/linkage.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.text
+.align 2
+ENTRY(_blackfin_icache_flush_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+	1:
+	IFLUSH[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+	IFLUSH[P0];
+	SSYNC;
+	RTS;
+
+ENTRY(_blackfin_dcache_flush_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+1:
+	FLUSH[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+	FLUSH[P0];
+	SSYNC;
+	RTS;
+
+ENTRY(_icache_invalidate)
+ENTRY(_invalidate_entire_icache)
+	[--SP] = (R7:5);
+
+	P0.L = (IMEM_CONTROL & 0xFFFF);
+	P0.H = (IMEM_CONTROL >> 16);
+	R7 =[P0];
+
+	/*
+	 * Clear the IMC bit , All valid bits in the instruction
+	 * cache are set to the invalid state
+	 */
+	BITCLR(R7, IMC_P);
+	CLI R6;
+	/* SSYNC required before invalidating cache. */
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	/* Configures the instruction cache agian */
+	R6 = (IMC | ENICPLB);
+	R7 = R7 | R6;
+
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	(R7:5) =[SP++];
+	RTS;
+
+/*
+ * Invalidate the Entire Data cache by
+ * clearing DMC[1:0] bits
+ */
+ENTRY(_invalidate_entire_dcache)
+ENTRY(_dcache_invalidate)
+	[--SP] = (R7:6);
+
+	P0.L = (DMEM_CONTROL & 0xFFFF);
+	P0.H = (DMEM_CONTROL >> 16);
+	R7 =[P0];
+
+	/*
+	 * Clear the DMC[1:0] bits, All valid bits in the data
+	 * cache are set to the invalid state
+	 */
+	BITCLR(R7, DMC0_P);
+	BITCLR(R7, DMC1_P);
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+	/* Configures the data cache again */
+
+	R6 = (ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	R7 = R7 | R6;
+
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	(R7:6) =[SP++];
+	RTS;
+
+ENTRY(_blackfin_dcache_invalidate_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+1:
+	FLUSHINV[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+
+	/*
+	 * If the data crosses a cache line, then we'll be pointing to
+	 * the last cache line, but won't have flushed/invalidated it yet, so do
+	 * one more.
+	 */
+	FLUSHINV[P0];
+	SSYNC;
+	RTS;
diff --git a/cpu/bf561/config.mk b/cpu/bf561/config.mk
new file mode 100644
index 0000000..f4dc04b
--- /dev/null
+++ b/cpu/bf561/config.mk
@@ -0,0 +1,27 @@
+# U-boot - config.mk
+#
+# Copyright (c) 2005-2007 Analog Devices Inc.
+#
+# (C) Copyright 2000-2004
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+# MA 02110-1301 USA
+#
+
+PLATFORM_RELFLAGS += -mcpu=bf561 -ffixed-P5
diff --git a/cpu/bf561/cpu.c b/cpu/bf561/cpu.c
new file mode 100644
index 0000000..5b907cd
--- /dev/null
+++ b/cpu/bf561/cpu.c
@@ -0,0 +1,220 @@
+/*
+ * U-boot - cpu.c CPU specific functions
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <asm/blackfin.h>
+#include <command.h>
+#include <asm/entry.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
+
+#define CACHE_ON 1
+#define CACHE_OFF 0
+
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	__asm__ __volatile__("cli r3;" "P0 = %0;" "JUMP (P0);"::"r"(L1_ISRAM)
+	    );
+
+	return 0;
+}
+
+/* These functions are just used to satisfy the linker */
+int cpu_init(void)
+{
+	return 0;
+}
+
+int cleanup_before_linux(void)
+{
+	return 0;
+}
+
+void icache_enable(void)
+{
+	unsigned int *I0, *I1;
+	int i, j = 0;
+
+	/* Before enable icache, disable it first */
+	icache_disable();
+	I0 = (unsigned int *)ICPLB_ADDR0;
+	I1 = (unsigned int *)ICPLB_DATA0;
+
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & icplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & icplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
+		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+
+	}
+
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+	sync();
+	sti();
+}
+
+void icache_disable(void)
+{
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+	sync();
+	sti();
+}
+
+int icache_status(void)
+{
+	unsigned int value;
+	value = *(unsigned int *)IMEM_CONTROL;
+
+	if (value & (IMC | ENICPLB))
+		return CACHE_ON;
+	else
+		return CACHE_OFF;
+}
+
+void dcache_enable(void)
+{
+	unsigned int *I0, *I1;
+	unsigned int temp;
+	int i, j = 0;
+
+	/* Before enable dcache, disable it first */
+	dcache_disable();
+	I0 = (unsigned int *)DCPLB_ADDR0;
+	I1 = (unsigned int *)DCPLB_DATA0;
+
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & dcplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+		} else {
+			debug("skip   %02i %02i 0x%08x 0x%08x\n", i, j,
+			      dcplb_table[i][0], dcplb_table[i][1]);
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & dcplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
+		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+	}
+
+	cli();
+	temp = *(unsigned int *)DMEM_CONTROL;
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL =
+	    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | temp;
+	sync();
+	sti();
+}
+
+void dcache_disable(void)
+{
+
+	unsigned int *I0, *I1;
+	int i;
+
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL &=
+	    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	sync();
+	sti();
+
+	/* after disable dcache, clear it so we don't confuse the next application */
+	I0 = (unsigned int *)DCPLB_ADDR0;
+	I1 = (unsigned int *)DCPLB_DATA0;
+
+	for (i = 0; i < 16; i++) {
+		*I0++ = 0x0;
+		*I1++ = 0x0;
+	}
+}
+
+int dcache_status(void)
+{
+	unsigned int value;
+	value = *(unsigned int *)DMEM_CONTROL;
+	if (value & (ENDCPLB))
+		return CACHE_ON;
+	else
+		return CACHE_OFF;
+}
diff --git a/cpu/bf561/cpu.h b/cpu/bf561/cpu.h
new file mode 100644
index 0000000..b6b73b1
--- /dev/null
+++ b/cpu/bf561/cpu.h
@@ -0,0 +1,66 @@
+/*
+ *  U-boot - cpu.h
+ *
+ *  Copyright (c) 2005-2007 Analog Devices 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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#ifndef _CPU_H_
+#define _CPU_H_
+
+#include <command.h>
+
+#define INTERNAL_IRQS (32)
+#define NUM_IRQ_NODES 16
+#define DEF_INTERRUPT_FLAGS 1
+#define MAX_TIM_LOAD	0xFFFFFFFF
+
+void blackfin_irq_panic(int reason, struct pt_regs *reg);
+extern void dump(struct pt_regs *regs);
+void display_excp(void);
+asmlinkage void evt_nmi(void);
+asmlinkage void evt_exception(void);
+asmlinkage void trap(void);
+asmlinkage void evt_ivhw(void);
+asmlinkage void evt_rst(void);
+asmlinkage void evt_timer(void);
+asmlinkage void evt_evt7(void);
+asmlinkage void evt_evt8(void);
+asmlinkage void evt_evt9(void);
+asmlinkage void evt_evt10(void);
+asmlinkage void evt_evt11(void);
+asmlinkage void evt_evt12(void);
+asmlinkage void evt_evt13(void);
+asmlinkage void evt_soft_int1(void);
+asmlinkage void evt_system_call(void);
+void blackfin_irq_panic(int reason, struct pt_regs *regs);
+void blackfin_free_irq(unsigned int irq, void *dev_id);
+void call_isr(int irq, struct pt_regs *fp);
+void blackfin_do_irq(int vec, struct pt_regs *fp);
+void blackfin_init_IRQ(void);
+void blackfin_enable_irq(unsigned int irq);
+void blackfin_disable_irq(unsigned int irq);
+extern int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[]);
+int blackfin_request_irq(unsigned int irq,
+			 void (*handler) (int, void *, struct pt_regs *),
+			 unsigned long flags, const char *devname,
+			 void *dev_id);
+void timer_init(void);
+#endif
diff --git a/cpu/bf561/flush.S b/cpu/bf561/flush.S
new file mode 100644
index 0000000..0140a60
--- /dev/null
+++ b/cpu/bf561/flush.S
@@ -0,0 +1,402 @@
+/* Copyright (C) 2003-2007 Analog Devices Inc.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.
+ */
+
+#define ASSEMBLY
+
+#include <asm/linkage.h>
+#include <asm/cplb.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.text
+
+/* This is an external function being called by the user
+ * application through __flush_cache_all. Currently this function
+ * serves the purpose of flushing all the pending writes in
+ * in the instruction cache.
+ */
+
+ENTRY(_flush_instruction_cache)
+	[--SP] = ( R7:6, P5:4 );
+	LINK 12;
+	SP += -12;
+	P5.H = (ICPLB_ADDR0 >> 16);
+	P5.L = (ICPLB_ADDR0 & 0xFFFF);
+	P4.H = (ICPLB_DATA0 >> 16);
+	P4.L = (ICPLB_DATA0 & 0xFFFF);
+	R7 = CPLB_VALID | CPLB_L1_CHBL;
+	R6 = 16;
+inext:	R0 = [P5++];
+	R1 = [P4++];
+	[--SP] =  RETS;
+	CALL _icplb_flush;	/* R0 = page, R1 = data*/
+	RETS = [SP++];
+iskip:	R6 += -1;
+	CC = R6;
+	IF CC JUMP inext;
+	SSYNC;
+	SP += 12;
+	UNLINK;
+	( R7:6, P5:4 ) = [SP++];
+	RTS;
+
+/* This is an internal function to flush all pending
+ * writes in the cache associated with a particular ICPLB.
+ *
+ * R0 -  page's start address
+ * R1 -  CPLB's data field.
+ */
+
+.align 2
+ENTRY(_icplb_flush)
+	[--SP] = ( R7:0, P5:0 );
+	[--SP] = LC0;
+	[--SP] = LT0;
+	[--SP] = LB0;
+	[--SP] = LC1;
+	[--SP] = LT1;
+	[--SP] = LB1;
+
+	/* If it's a 1K or 4K page, then it's quickest to
+	 * just systematically flush all the addresses in
+	 * the page, regardless of whether they're in the
+	 * cache, or dirty. If it's a 1M or 4M page, there
+	 * are too many addresses, and we have to search the
+	 * cache for lines corresponding to the page.
+	 */
+
+	CC = BITTST(R1, 17);	/* 1MB or 4MB */
+	IF !CC JUMP iflush_whole_page;
+
+	/* We're only interested in the page's size, so extract
+	 * this from the CPLB (bits 17:16), and scale to give an
+	 * offset into the page_size and page_prefix tables.
+	 */
+
+	R1 <<= 14;
+	R1 >>= 30;
+	R1 <<= 2;
+
+	/* We can also determine the sub-bank used, because this is
+	 * taken from bits 13:12 of the address.
+	 */
+
+	R3 = ((12<<8)|2);		/* Extraction pattern */
+	nop;				/*Anamoly 05000209*/
+	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits*/
+	R3.H = R4.L << 0 ;		/* Save in extraction pattern for later deposit.*/
+
+
+	/* So:
+	 * R0 = Page start
+	 * R1 = Page length (actually, offset into size/prefix tables)
+	 * R3 = sub-bank deposit values
+	 *
+	 * The cache has 2 Ways, and 64 sets, so we iterate through
+	 * the sets, accessing the tag for each Way, for our Bank and
+	 * sub-bank, looking for dirty, valid tags that match our
+	 * address prefix.
+	 */
+
+	P5.L = (ITEST_COMMAND & 0xFFFF);
+	P5.H = (ITEST_COMMAND >> 16);
+	P4.L = (ITEST_DATA0 & 0xFFFF);
+	P4.H = (ITEST_DATA0 >> 16);
+
+	P0.L = page_prefix_table;
+	P0.H = page_prefix_table;
+	P1 = R1;
+	R5 = 0;			/* Set counter*/
+	P0 = P1 + P0;
+	R4 = [P0];		/* This is the address prefix*/
+
+	/* We're reading (bit 1==0) the tag (bit 2==0), and we
+	 * don't care about which double-word, since we're only
+	 * fetching tags, so we only have to set Set, Bank,
+	 * Sub-bank and Way.
+	 */
+
+	P2 = 4;
+	LSETUP (ifs1, ife1) LC1 = P2;
+ifs1:	P0 = 32;		/* iterate over all sets*/
+	LSETUP (ifs0, ife0) LC0 = P0;
+ifs0:	R6 = R5 << 5;		/* Combine set*/
+	R6.H = R3.H << 0 ;	/* and sub-bank*/
+	[P5] = R6;		/* Issue Command*/
+	SSYNC;			/* CSYNC will not work here :(*/
+	R7 = [P4];		/* and read Tag.*/
+	CC = BITTST(R7, 0);	/* Check if valid*/
+	IF !CC JUMP ifskip;	/* and skip if not.*/
+
+	/* Compare against the page address. First, plant bits 13:12
+	 * into the tag, since those aren't part of the returned data.
+	 */
+
+	R7 = DEPOSIT(R7, R3);	/* set 13:12*/
+	R1 = R7 & R4;		/* Mask off lower bits*/
+	CC = R1 == R0;		/* Compare against page start.*/
+	IF !CC JUMP ifskip;	/* Skip it if it doesn't match.*/
+
+	/* Tag address matches against page, so this is an entry
+	 * we must flush.
+	 */
+
+	R7 >>= 10;		/* Mask off the non-address bits*/
+	R7 <<= 10;
+	P3 = R7;
+	IFLUSH [P3];		/* And flush the entry*/
+ifskip:
+ife0:	R5 += 1;		/* Advance to next Set*/
+ife1:	NOP;
+
+ifinished:
+	SSYNC;			/* Ensure the data gets out to mem.*/
+
+	/*Finished. Restore context.*/
+	LB1 = [SP++];
+	LT1 = [SP++];
+	LC1 = [SP++];
+	LB0 = [SP++];
+	LT0 = [SP++];
+	LC0 = [SP++];
+	( R7:0, P5:0 ) = [SP++];
+	RTS;
+
+iflush_whole_page:
+	/* It's a 1K or 4K page, so quicker to just flush the
+	 * entire page.
+	 */
+
+	P1 = 32;		/* For 1K pages*/
+	P2 = P1 << 2;		/* For 4K pages*/
+	P0 = R0;		/* Start of page*/
+	CC = BITTST(R1, 16);	/* Whether 1K or 4K*/
+	IF CC P1 = P2;
+	P1 += -1;		/* Unroll one iteration*/
+	SSYNC;
+	IFLUSH [P0++];		/* because CSYNC can't end loops.*/
+	LSETUP (isall, ieall) LC0 = P1;
+isall:IFLUSH [P0++];
+ieall: NOP;
+	SSYNC;
+	JUMP ifinished;
+
+/* This is an external function being called by the user
+ * application through __flush_cache_all. Currently this function
+ * serves the purpose of flushing all the pending writes in
+ * in the data cache.
+ */
+
+ENTRY(_flush_data_cache)
+	[--SP] = ( R7:6, P5:4 );
+	LINK 12;
+	SP += -12;
+	P5.H = (DCPLB_ADDR0 >> 16);
+	P5.L = (DCPLB_ADDR0 & 0xFFFF);
+	P4.H = (DCPLB_DATA0 >> 16);
+	P4.L = (DCPLB_DATA0 & 0xFFFF);
+	R7 = CPLB_VALID | CPLB_L1_CHBL | CPLB_DIRTY (Z);
+	R6 = 16;
+next:	R0 = [P5++];
+	R1 = [P4++];
+	CC = BITTST(R1, 14);	/* Is it write-through?*/
+	IF CC JUMP skip;	/* If so, ignore it.*/
+	R2 = R1 & R7;		/* Is it a dirty, cached page?*/
+	CC = R2;
+	IF !CC JUMP skip;	/* If not, ignore it.*/
+	[--SP] = RETS;
+	CALL _dcplb_flush;	/* R0 = page, R1 = data*/
+	RETS = [SP++];
+skip:	R6 += -1;
+	CC = R6;
+	IF CC JUMP next;
+	SSYNC;
+	SP += 12;
+	UNLINK;
+	( R7:6, P5:4 ) = [SP++];
+	RTS;
+
+/* This is an internal function to flush all pending
+ * writes in the cache associated with a particular DCPLB.
+ *
+ * R0 -  page's start address
+ * R1 -  CPLB's data field.
+ */
+
+.align 2
+ENTRY(_dcplb_flush)
+	[--SP] = ( R7:0, P5:0 );
+	[--SP] = LC0;
+	[--SP] = LT0;
+	[--SP] = LB0;
+	[--SP] = LC1;
+	[--SP] = LT1;
+	[--SP] = LB1;
+
+	/* If it's a 1K or 4K page, then it's quickest to
+	 * just systematically flush all the addresses in
+	 * the page, regardless of whether they're in the
+	 * cache, or dirty. If it's a 1M or 4M page, there
+	 * are too many addresses, and we have to search the
+	 * cache for lines corresponding to the page.
+	 */
+
+	CC = BITTST(R1, 17);	/* 1MB or 4MB */
+	IF !CC JUMP dflush_whole_page;
+
+	/* We're only interested in the page's size, so extract
+	 * this from the CPLB (bits 17:16), and scale to give an
+	 * offset into the page_size and page_prefix tables.
+	 */
+
+	R1 <<= 14;
+	R1 >>= 30;
+	R1 <<= 2;
+
+	/* The page could be mapped into Bank A or Bank B, depending
+	 * on (a) whether both banks are configured as cache, and
+	 * (b) on whether address bit A[x] is set. x is determined
+	 * by DCBS in DMEM_CONTROL
+	 */
+
+	R2 = 0;			/* Default to Bank A (Bank B would be 1)*/
+
+	P0.L = (DMEM_CONTROL & 0xFFFF);
+	P0.H = (DMEM_CONTROL >> 16);
+
+	R3 = [P0];		/* If Bank B is not enabled as cache*/
+	CC = BITTST(R3, 2);	/* then Bank A is our only option.*/
+	IF CC JUMP bank_chosen;
+
+	R4 = 1<<14;		/* If DCBS==0, use A[14].*/
+	R5 = R4 << 7;		/* If DCBS==1, use A[23];*/
+	CC = BITTST(R3, 4);
+	IF CC R4 = R5;		/* R4 now has either bit 14 or bit 23 set.*/
+	R5 = R0 & R4;		/* Use it to test the Page address*/
+	CC = R5;		/* and if that bit is set, we use Bank B,*/
+	R2 = CC;		/* else we use Bank A.*/
+	R2 <<= 23;		/* The Bank selection's at posn 23.*/
+
+bank_chosen:
+
+	/* We can also determine the sub-bank used, because this is
+	 * taken from bits 13:12 of the address.
+	 */
+
+	R3 = ((12<<8)|2);		/* Extraction pattern */
+	nop;				/*Anamoly 05000209*/
+	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits*/
+	/* Save in extraction pattern for later deposit.*/
+	R3.H = R4.L << 0;
+
+	/* So:
+	 * R0 = Page start
+	 * R1 = Page length (actually, offset into size/prefix tables)
+	 * R2 = Bank select mask
+	 * R3 = sub-bank deposit values
+	 *
+	 * The cache has 2 Ways, and 64 sets, so we iterate through
+	 * the sets, accessing the tag for each Way, for our Bank and
+	 * sub-bank, looking for dirty, valid tags that match our
+	 * address prefix.
+	 */
+
+	P5.L = (DTEST_COMMAND & 0xFFFF);
+	P5.H = (DTEST_COMMAND >> 16);
+	P4.L = (DTEST_DATA0 & 0xFFFF);
+	P4.H = (DTEST_DATA0 >> 16);
+
+	P0.L = page_prefix_table;
+	P0.H = page_prefix_table;
+	P1 = R1;
+	R5 = 0;			/* Set counter*/
+	P0 = P1 + P0;
+	R4 = [P0];		/* This is the address prefix*/
+
+
+	/* We're reading (bit 1==0) the tag (bit 2==0), and we
+	 * don't care about which double-word, since we're only
+	 * fetching tags, so we only have to set Set, Bank,
+	 * Sub-bank and Way.
+	 */
+
+	P2 = 2;
+	LSETUP (fs1, fe1) LC1 = P2;
+fs1:	P0 = 64;		/* iterate over all sets*/
+	LSETUP (fs0, fe0) LC0 = P0;
+fs0:	R6 = R5 << 5;		/* Combine set*/
+	R6.H = R3.H << 0 ;	/* and sub-bank*/
+	R6 = R6 | R2;		/* and Bank. Leave Way==0 at first.*/
+	BITSET(R6,14);
+	[P5] = R6;		/* Issue Command*/
+	SSYNC;
+	R7 = [P4];		/* and read Tag.*/
+	CC = BITTST(R7, 0);	/* Check if valid*/
+	IF !CC JUMP fskip;	/* and skip if not.*/
+	CC = BITTST(R7, 1);	/* Check if dirty*/
+	IF !CC JUMP fskip;	/* and skip if not.*/
+
+	/* Compare against the page address. First, plant bits 13:12
+	 * into the tag, since those aren't part of the returned data.
+	 */
+
+	R7 = DEPOSIT(R7, R3);	/* set 13:12*/
+	R1 = R7 & R4;		/* Mask off lower bits*/
+	CC = R1 == R0;		/* Compare against page start.*/
+	IF !CC JUMP fskip;	/* Skip it if it doesn't match.*/
+
+	/* Tag address matches against page, so this is an entry
+	 * we must flush.
+	 */
+
+	R7 >>= 10;		/* Mask off the non-address bits*/
+	R7 <<= 10;
+	P3 = R7;
+	SSYNC;
+	FLUSHINV [P3];		/* And flush the entry*/
+fskip:
+fe0:	R5 += 1;		/* Advance to next Set*/
+fe1:	BITSET(R2, 26);		/* Go to next Way.*/
+
+dfinished:
+	SSYNC;			/* Ensure the data gets out to mem.*/
+
+	/*Finished. Restore context.*/
+	LB1 = [SP++];
+	LT1 = [SP++];
+	LC1 = [SP++];
+	LB0 = [SP++];
+	LT0 = [SP++];
+	LC0 = [SP++];
+	( R7:0, P5:0 ) = [SP++];
+	RTS;
+
+dflush_whole_page:
+
+	/* It's a 1K or 4K page, so quicker to just flush the
+	 * entire page.
+	 */
+
+	P1 = 32;		/* For 1K pages*/
+	P2 = P1 << 2;		/* For 4K pages*/
+	P0 = R0;		/* Start of page*/
+	CC = BITTST(R1, 16);	/* Whether 1K or 4K*/
+	IF CC P1 = P2;
+	P1 += -1;		/* Unroll one iteration*/
+	SSYNC;
+	FLUSHINV [P0++];	/* because CSYNC can't end loops.*/
+	LSETUP (eall, eall) LC0 = P1;
+eall:	FLUSHINV [P0++];
+	SSYNC;
+	JUMP dfinished;
+
+.align 4;
+page_prefix_table:
+.byte4 	0xFFFFFC00;	/* 1K */
+.byte4	0xFFFFF000;	/* 4K */
+.byte4	0xFFF00000;	/* 1M */
+.byte4	0xFFC00000;	/* 4M */
+.page_prefix_table.end:
diff --git a/cpu/bf561/init_sdram.S b/cpu/bf561/init_sdram.S
new file mode 100644
index 0000000..d763f27
--- /dev/null
+++ b/cpu/bf561/init_sdram.S
@@ -0,0 +1,171 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS	   : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN	   : 1=All Clocks off
+	 *   - [3]     = STOPCK	   : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF	   : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier */
+	r0 = r0 << 9;			/* Shift it over, */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2? */
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL? */
+	r1 = r1 << 8;			/* Shift it over */
+	r0 = r1 | r0;			/* add them all together */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address */
+	cli r2;				/* Disable interrupts */
+	ssync;
+	w[p0] = r0.l;			/* Set the value */
+	idle;				/* Wait for the PLL to stablize */
+	sti r2;				/* Enable interrupts */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 */
+
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf561/init_sdram_bootrom_initblock.S b/cpu/bf561/init_sdram_bootrom_initblock.S
new file mode 100644
index 0000000..5e3c88a
--- /dev/null
+++ b/cpu/bf561/init_sdram_bootrom_initblock.S
@@ -0,0 +1,185 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV	CONFIG_CCLK_DIV_not_defined_properly
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+
+	p0.h = hi(SICA_IWR0);
+	p0.l = lo(SICA_IWR0);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+
+	p0.h = hi(SPI_BAUD);
+	p0.l = lo(SPI_BAUD);
+	r0.l = CONFIG_SPI_BAUD_INITBLOCK;
+	w[p0] = r0.l;
+	SSYNC;
+
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN	   : 1=All Clocks off
+	 *   - [3]     = STOPCK	   : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF	   : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier */
+	r0 = r0 << 9;			/* Shift it over, */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2? */
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL? */
+	r1 = r1 << 8;			/* Shift it over */
+	r0 = r1 | r0;			/* add them all together */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address */
+	cli r2;				/* Disable interrupts */
+	ssync;
+	w[p0] = r0.l;			/* Set the value */
+	idle;				/* Wait for the PLL to stablize */
+	sti r2;				/* Enable interrupts */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 */
+
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf561/interrupt.S b/cpu/bf561/interrupt.S
new file mode 100644
index 0000000..21839ce
--- /dev/null
+++ b/cpu/bf561/interrupt.S
@@ -0,0 +1,246 @@
+/*
+ * U-boot - interrupt.S Processing of interrupts and exception handling
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * This file is based on interrupt.S
+ *
+ * Copyright (C) 2003	Metrowerks, Inc. <mwaddel@metrowerks.com>
+ * Copyright (C) 2002	Arcturus Networks Ltd. Ted Ma <mated@sympatico.ca>
+ * Copyright (C) 1998	D. Jeff Dionne <jeff@ryeham.ee.ryerson.ca>,
+ *			Kenneth Albanowski <kjahds@kjahds.com>,
+ *			The Silver Hammer Group, Ltd.
+ *
+ * (c) 1995, Dionne & Associates
+ * (c) 1995, DKG Display Tech.
+ *
+ * This file is also based on exception.asm
+ * (C) Copyright 2001-2005 - Analog Devices, Inc.  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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#define ASSEMBLY
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/hw_irq.h>
+#include <asm/entry.h>
+#include <asm/blackfin_defs.h>
+
+.global _blackfin_irq_panic;
+
+.text
+.align 2
+
+#ifndef CONFIG_KGDB
+.global _evt_emulation
+_evt_emulation:
+	SAVE_CONTEXT
+	r0 = IRQ_EMU;
+	r1 = seqstat;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+	rte;
+#endif
+
+.global _evt_nmi
+_evt_nmi:
+	SAVE_CONTEXT
+	r0 = IRQ_NMI;
+	r1 = RETN;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+
+_evt_nmi_exit:
+	rtn;
+
+.global _trap
+_trap:
+	SAVE_ALL_SYS
+	r0 = sp;	/* stack frame pt_regs pointer argument ==> r0 */
+	sp += -12;
+	call _trap_c
+	sp += 12;
+	RESTORE_ALL_SYS
+	rtx;
+
+.global _evt_rst
+_evt_rst:
+	SAVE_CONTEXT
+	r0 = IRQ_RST;
+	r1 = RETN;
+	sp += -12;
+	call _do_reset;
+	sp += 12;
+
+_evt_rst_exit:
+	rtn;
+
+irq_panic:
+	r0 = IRQ_EVX;
+	r1 =  sp;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+
+.global _evt_ivhw
+_evt_ivhw:
+	SAVE_CONTEXT
+	RAISE 14;
+
+_evt_ivhw_exit:
+	 rti;
+
+.global _evt_timer
+_evt_timer:
+	SAVE_CONTEXT
+	r0 = IRQ_CORETMR;
+	sp += -12;
+	/* Polling method used now. */
+	/* call timer_int; */
+	sp += 12;
+	RESTORE_CONTEXT
+	rti;
+	nop;
+
+.global _evt_evt7
+_evt_evt7:
+	SAVE_CONTEXT
+	r0 = 7;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt7_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt8
+_evt_evt8:
+	SAVE_CONTEXT
+	r0 = 8;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt8_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt9
+_evt_evt9:
+	SAVE_CONTEXT
+	r0 = 9;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt9_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt10
+_evt_evt10:
+	SAVE_CONTEXT
+	r0 = 10;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt10_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt11
+_evt_evt11:
+	SAVE_CONTEXT
+	r0 = 11;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt11_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt12
+_evt_evt12:
+	SAVE_CONTEXT
+	r0 = 12;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+evt_evt12_exit:
+	 RESTORE_CONTEXT
+	 rti;
+
+.global _evt_evt13
+_evt_evt13:
+	SAVE_CONTEXT
+	r0 = 13;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt13_exit:
+	 RESTORE_CONTEXT
+	 rti;
+
+.global _evt_system_call
+_evt_system_call:
+	[--sp] = r0;
+	[--SP] = RETI;
+	r0 = [sp++];
+	r0 += 2;
+	[--sp] = r0;
+	RETI = [SP++];
+	r0 = [SP++];
+	SAVE_CONTEXT
+	sp += -12;
+	call _exception_handle;
+	sp += 12;
+	RESTORE_CONTEXT
+	RTI;
+
+evt_system_call_exit:
+	rti;
+
+.global _evt_soft_int1
+_evt_soft_int1:
+	[--sp] = r0;
+	[--SP] = RETI;
+	r0 = [sp++];
+	r0 += 2;
+	[--sp] = r0;
+	RETI = [SP++];
+	r0 = [SP++];
+	SAVE_CONTEXT
+	sp += -12;
+	call _exception_handle;
+	sp += 12;
+	RESTORE_CONTEXT
+	RTI;
+
+evt_soft_int1_exit:
+	rti;
diff --git a/cpu/bf561/interrupts.c b/cpu/bf561/interrupts.c
new file mode 100644
index 0000000..ecbc6ad
--- /dev/null
+++ b/cpu/bf561/interrupts.c
@@ -0,0 +1,171 @@
+/*
+ * U-boot - interrupts.c Interrupt related routines
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on interrupts.c
+ * Copyright 1996 Roman Zippel
+ * Copyright 1999 D. Jeff Dionne <jeff@uclinux.org>
+ * Copyright 2000-2001 Lineo, Inc. D. Jefff Dionne <jeff@lineo.ca>
+ * Copyright 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
+ * Copyright 2003 Metrowerks/Motorola
+ * Copyright 2003 Bas Vermeulen <bas@buyways.nl>,
+ *			BuyWays B.V. (www.buyways.nl)
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <asm/machdep.h>
+#include <asm/irq.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include "cpu.h"
+
+static ulong timestamp;
+static ulong last_time;
+static int int_flag;
+
+int irq_flags;			/* needed by asm-blackfin/system.h */
+
+/* Functions just to satisfy the linker */
+
+/*
+ * This function is derived from PowerPC code (read timebase as long long).
+ * On BF561 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 BF561 it returns the number of timer ticks per second.
+ */
+ulong get_tbclk(void)
+{
+	ulong tbclk;
+
+	tbclk = CFG_HZ;
+	return tbclk;
+}
+
+void enable_interrupts(void)
+{
+	restore_flags(int_flag);
+}
+
+int disable_interrupts(void)
+{
+	save_and_cli(int_flag);
+	return 1;
+}
+
+int interrupt_init(void)
+{
+	return (0);
+}
+
+void udelay(unsigned long usec)
+{
+	unsigned long delay, start, stop;
+	unsigned long cclk;
+	cclk = (CONFIG_CCLK_HZ);
+
+	while (usec > 1) {
+		/*
+		 * how many clock ticks to delay?
+		 *  - request(in useconds) * clock_ticks(Hz) / useconds/second
+		 */
+		if (usec < 1000) {
+			delay = (usec * (cclk / 244)) >> 12;
+			usec = 0;
+		} else {
+			delay = (1000 * (cclk / 244)) >> 12;
+			usec -= 1000;
+		}
+
+		asm volatile (" %0 = CYCLES;":"=r" (start));
+		do {
+			asm volatile (" %0 = CYCLES; ":"=r" (stop));
+		} while (stop - start < delay);
+	}
+
+	return;
+}
+
+void timer_init(void)
+{
+	*pTCNTL = 0x1;
+	*pTSCALE = 0x0;
+	*pTCOUNT = MAX_TIM_LOAD;
+	*pTPERIOD = MAX_TIM_LOAD;
+	*pTCNTL = 0x7;
+	asm("CSYNC;");
+
+	timestamp = 0;
+	last_time = 0;
+}
+
+/*
+ * Any network command or flash
+ * command is started get_timer shall
+ * be called before TCOUNT gets reset,
+ * to implement the accurate timeouts.
+ *
+ * How ever milliconds doesn't return
+ * the number that has been elapsed from
+ * the last reset.
+ *
+ * As get_timer is used in the u-boot
+ * only for timeouts this should be
+ * sufficient
+ */
+ulong get_timer(ulong base)
+{
+	ulong milisec;
+
+	/* Number of clocks elapsed */
+	ulong clocks = (MAX_TIM_LOAD - (*pTCOUNT));
+
+	/*
+	 * Find if the TCOUNT is reset
+	 * timestamp gives the number of times
+	 * TCOUNT got reset
+	 */
+	if (clocks < last_time)
+		timestamp++;
+	last_time = clocks;
+
+	/* Get the number of milliseconds */
+	milisec = clocks / (CONFIG_CCLK_HZ / 1000);
+
+	/*
+	 * Find the number of millisonds
+	 * that got elapsed before this TCOUNT
+	 * cycle
+	 */
+	milisec += timestamp * (MAX_TIM_LOAD / (CONFIG_CCLK_HZ / 1000));
+
+	return (milisec - base);
+}
diff --git a/cpu/bf561/ints.c b/cpu/bf561/ints.c
new file mode 100644
index 0000000..27a38a3
--- /dev/null
+++ b/cpu/bf561/ints.c
@@ -0,0 +1,117 @@
+/*
+ * U-boot - ints.c Interrupt related routines
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on ints.c
+ *
+ * Apr18 2003, Changed by HuTao to support interrupt cascading for Blackfin
+ *	drivers
+ *
+ * Copyright 1996 Roman Zippel
+ * Copyright 1999 D. Jeff Dionne <jeff@uclinux.org>
+ * Copyright 2000-2001 Lineo, Inc. D. Jefff Dionne <jeff@lineo.ca>
+ * Copyright 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
+ * Copyright 2003 Metrowerks/Motorola
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <linux/stddef.h>
+#include <asm/system.h>
+#include <asm/irq.h>
+#include <asm/traps.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/machdep.h>
+#include <asm/setup.h>
+#include <asm/blackfin.h>
+#include "cpu.h"
+
+void blackfin_irq_panic(int reason, struct pt_regs *regs)
+{
+	printf("\n\nException: IRQ 0x%x entered\n", reason);
+	printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+	printf("stack frame=0x%x, ", (unsigned int)regs);
+	printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
+	dump(regs);
+	printf("Unhandled IRQ or exceptions!\n");
+	printf("Please reset the board \n");
+}
+
+void blackfin_init_IRQ(void)
+{
+	*(unsigned volatile long *)(SIC_IMASK) = SIC_UNMASK_ALL;
+	cli();
+#ifndef CONFIG_KGDB
+	*(unsigned volatile long *)(EVT_EMULATION_ADDR) = 0x0;
+#endif
+	*(unsigned volatile long *)(EVT_NMI_ADDR) =
+	    (unsigned volatile long)evt_nmi;
+	*(unsigned volatile long *)(EVT_EXCEPTION_ADDR) =
+	    (unsigned volatile long)trap;
+	*(unsigned volatile long *)(EVT_HARDWARE_ERROR_ADDR) =
+	    (unsigned volatile long)evt_ivhw;
+	*(unsigned volatile long *)(EVT_RESET_ADDR) =
+	    (unsigned volatile long)evt_rst;
+	*(unsigned volatile long *)(EVT_TIMER_ADDR) =
+	    (unsigned volatile long)evt_timer;
+	*(unsigned volatile long *)(EVT_IVG7_ADDR) =
+	    (unsigned volatile long)evt_evt7;
+	*(unsigned volatile long *)(EVT_IVG8_ADDR) =
+	    (unsigned volatile long)evt_evt8;
+	*(unsigned volatile long *)(EVT_IVG9_ADDR) =
+	    (unsigned volatile long)evt_evt9;
+	*(unsigned volatile long *)(EVT_IVG10_ADDR) =
+	    (unsigned volatile long)evt_evt10;
+	*(unsigned volatile long *)(EVT_IVG11_ADDR) =
+	    (unsigned volatile long)evt_evt11;
+	*(unsigned volatile long *)(EVT_IVG12_ADDR) =
+	    (unsigned volatile long)evt_evt12;
+	*(unsigned volatile long *)(EVT_IVG13_ADDR) =
+	    (unsigned volatile long)evt_evt13;
+	*(unsigned volatile long *)(EVT_IVG14_ADDR) =
+	    (unsigned volatile long)evt_system_call;
+	*(unsigned volatile long *)(EVT_IVG15_ADDR) =
+	    (unsigned volatile long)evt_soft_int1;
+	*(volatile unsigned long *)ILAT = 0;
+	asm("csync;");
+	sti();
+	*(volatile unsigned long *)IMASK = 0xffbf;
+	asm("csync;");
+}
+
+void exception_handle(void)
+{
+#if defined (CONFIG_PANIC_HANG)
+	display_excp();
+#else
+	udelay(100000);		/* allow messages to go out */
+	do_reset(NULL, 0, 0, NULL);
+#endif
+}
+
+void display_excp(void)
+{
+	printf("Exception!\n");
+}
diff --git a/cpu/bf561/serial.c b/cpu/bf561/serial.c
new file mode 100644
index 0000000..7f5c695
--- /dev/null
+++ b/cpu/bf561/serial.c
@@ -0,0 +1,196 @@
+/*
+ * U-boot - serial.c Serial driver for BF561
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on
+ * bf533_serial.c: Serial driver for BlackFin BF533 DSP internal UART.
+ * Copyright (c) 2003	Bas Vermeulen <bas@buyways.nl>,
+ * 			BuyWays B.V. (www.buyways.nl)
+ *
+ * Based heavily on blkfinserial.c
+ * blkfinserial.c: Serial driver for BlackFin DSP internal USRTs.
+ * Copyright(c) 2003	Metrowerks	<mwaddel@metrowerks.com>
+ * Copyright(c)	2001	Tony Z. Kou	<tonyko@arcturusnetworks.com>
+ * Copyright(c)	2001-2002 Arcturus Networks Inc. <www.arcturusnetworks.com>
+ *
+ * Based on code from 68328 version serial driver imlpementation which was:
+ * Copyright (C) 1995       David S. Miller    <davem@caip.rutgers.edu>
+ * Copyright (C) 1998       Kenneth Albanowski <kjahds@kjahds.com>
+ * Copyright (C) 1998, 1999 D. Jeff Dionne     <jeff@uclinux.org>
+ * Copyright (C) 1999       Vladimir Gurevich  <vgurevic@cisco.com>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/segment.h>
+#include <asm/bitops.h>
+#include <asm/delay.h>
+#include <asm/uaccess.h>
+#include "serial.h"
+#include <asm/io.h>
+
+unsigned long pll_div_fact;
+
+void calc_baud(void)
+{
+	unsigned char i;
+	int temp;
+	u_long sclk = get_sclk();
+
+	for (i = 0; i < sizeof(baud_table) / sizeof(int); i++) {
+		temp = sclk / (baud_table[i] * 8);
+		if ((temp & 0x1) == 1) {
+			temp++;
+		}
+		temp = temp / 2;
+		hw_baud_table[i].dl_high = (temp >> 8) & 0xFF;
+		hw_baud_table[i].dl_low = (temp) & 0xFF;
+	}
+}
+
+void serial_setbrg(void)
+{
+	int i;
+	DECLARE_GLOBAL_DATA_PTR;
+
+	calc_baud();
+
+	for (i = 0; i < sizeof(baud_table) / sizeof(int); i++) {
+		if (gd->baudrate == baud_table[i])
+			break;
+	}
+
+	/* Enable UART */
+	*pUART_GCTL |= UART_GCTL_UCEN;
+	sync();
+
+	/* Set DLAB in LCR to Access DLL and DLH */
+	ACCESS_LATCH;
+	sync();
+
+	*pUART_DLL = hw_baud_table[i].dl_low;
+	sync();
+	*pUART_DLH = hw_baud_table[i].dl_high;
+	sync();
+
+	/* Clear DLAB in LCR to Access THR RBR IER */
+	ACCESS_PORT_IER;
+	sync();
+
+	/*
+	 * Enable  ERBFI and ELSI interrupts
+	 * to poll SIC_ISR register
+	 */
+	*pUART_IER = UART_IER_ELSI | UART_IER_ERBFI | UART_IER_ETBEI;
+	sync();
+
+	/* Set LCR to Word Lengh 8-bit word select */
+	*pUART_LCR = UART_LCR_WLS8;
+	sync();
+
+	return;
+}
+
+int serial_init(void)
+{
+	serial_setbrg();
+	return (0);
+}
+
+void serial_putc(const char c)
+{
+	if ((*pUART_LSR) & UART_LSR_TEMT) {
+		if (c == '\n')
+			serial_putc('\r');
+
+		local_put_char(c);
+	}
+
+	while (!((*pUART_LSR) & UART_LSR_TEMT))
+		SYNC_ALL;
+
+	return;
+}
+
+int serial_tstc(void)
+{
+	if (*pUART_LSR & UART_LSR_DR)
+		return 1;
+	else
+		return 0;
+}
+
+int serial_getc(void)
+{
+	unsigned short uart_lsr_val, uart_rbr_val;
+	unsigned long isr_val;
+	int ret;
+
+	/* Poll for RX Interrupt */
+	while (!((isr_val =
+		  *(volatile unsigned long *)SIC_ISR) & IRQ_UART_RX_BIT)) ;
+	asm("csync;");
+
+	uart_lsr_val = *pUART_LSR;	/* Clear status bit */
+	uart_rbr_val = *pUART_RBR;	/* getc() */
+
+	if (isr_val & IRQ_UART_ERROR_BIT) {
+		ret = -1;
+	} else {
+		ret = uart_rbr_val & 0xff;
+	}
+
+	return ret;
+}
+
+void serial_puts(const char *s)
+{
+	while (*s) {
+		serial_putc(*s++);
+	}
+}
+
+static void local_put_char(char ch)
+{
+	int flags = 0;
+	unsigned long isr_val;
+
+	save_and_cli(flags);
+
+	/* Poll for TX Interruput */
+	while (!((isr_val = *pSIC_ISR) & IRQ_UART_TX_BIT)) ;
+	asm("csync;");
+
+	*pUART_THR = ch;	/* putc() */
+
+	if (isr_val & IRQ_UART_ERROR_BIT) {
+		printf("?");
+	}
+
+	restore_flags(flags);
+
+	return;
+}
diff --git a/cpu/bf561/serial.h b/cpu/bf561/serial.h
new file mode 100644
index 0000000..c1cbf36
--- /dev/null
+++ b/cpu/bf561/serial.h
@@ -0,0 +1,77 @@
+/*
+ * U-boot - bf561_serial.h Serial Driver defines
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on
+ * bf533_serial.h: Definitions for the BlackFin BF533 DSP serial driver.
+ * Copyright (C) 2003	Bas Vermeulen <bas@buyways.nl>
+ * 			BuyWays B.V. (www.buyways.nl)
+ *
+ * Based heavily on:
+ * blkfinserial.h: Definitions for the BlackFin DSP serial driver.
+ *
+ * Copyright (C) 2001	Tony Z. Kou	tonyko@arcturusnetworks.com
+ * Copyright (C) 2001   Arcturus Networks Inc. <www.arcturusnetworks.com>
+ *
+ * Based on code from 68328serial.c which was:
+ * Copyright (C) 1995       David S. Miller    <davem@caip.rutgers.edu>
+ * Copyright (C) 1998       Kenneth Albanowski <kjahds@kjahds.com>
+ * Copyright (C) 1998, 1999 D. Jeff Dionne     <jeff@uclinux.org>
+ * Copyright (C) 1999       Vladimir Gurevich  <vgurevic@cisco.com>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#ifndef _Bf561_SERIAL_H
+#define _Bf561_SERIAL_H
+
+#include <linux/config.h>
+#include <asm/blackfin.h>
+
+#define SYNC_ALL	__asm__ __volatile__ ("ssync;\n")
+#define ACCESS_LATCH	*pUART_LCR |= UART_LCR_DLAB;
+#define ACCESS_PORT_IER	*pUART_LCR &= (~UART_LCR_DLAB);
+
+void serial_setbrg(void);
+static void local_put_char(char ch);
+void calc_baud(void);
+void serial_setbrg(void);
+int serial_init(void);
+void serial_putc(const char c);
+int serial_tstc(void);
+int serial_getc(void);
+void serial_puts(const char *s);
+static void local_put_char(char ch);
+
+int baud_table[5] = { 9600, 19200, 38400, 57600, 115200 };
+
+struct {
+	unsigned char dl_high;
+	unsigned char dl_low;
+} hw_baud_table[5];
+
+#ifdef CONFIG_STAMP
+extern unsigned long pll_div_fact;
+#endif
+
+#endif
diff --git a/cpu/bf561/start.S b/cpu/bf561/start.S
new file mode 100644
index 0000000..bd26cf3
--- /dev/null
+++ b/cpu/bf561/start.S
@@ -0,0 +1,311 @@
+/*
+ * U-boot - start.S Startup file of u-boot for BF533/BF561
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on head.S
+ * Copyright (c) 2003  Metrowerks/Motorola
+ * Copyright (C) 1998  D. Jeff Dionne <jeff@ryeham.ee.ryerson.ca>,
+ *		       Kenneth Albanowski <kjahds@kjahds.com>,
+ *		       The Silver Hammer Group, Ltd.
+ * (c) 1995, Dionne & Associates
+ * (c) 1995, DKG Display Tech.
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+/*
+ * Note: A change in this file subsequently requires a change in
+ *       board/$(board_name)/config.mk for a valid u-boot.bin
+ */
+
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.global _stext;
+.global __bss_start;
+.global start;
+.global _start;
+.global _rambase;
+.global _ramstart;
+.global _ramend;
+.global edata;
+.global _initialize;
+.global _exit;
+.global flashdataend;
+.global init_sdram;
+
+.text
+_start:
+start:
+_stext:
+
+	R0 = 0x32;
+	SYSCFG = R0;
+	SSYNC;
+
+	/*
+	 * As per HW reference manual DAG registers,
+	 * DATA and Address resgister shall be zero'd
+	 * in initialization, after a reset state
+	 */
+	r1 = 0;	/* Data registers zero'd */
+	r2 = 0;
+	r3 = 0;
+	r4 = 0;
+	r5 = 0;
+	r6 = 0;
+	r7 = 0;
+
+	p0 = 0; /* Address registers zero'd */
+	p1 = 0;
+	p2 = 0;
+	p3 = 0;
+	p4 = 0;
+	p5 = 0;
+
+	i0 = 0; /* DAG Registers zero'd */
+	i1 = 0;
+	i2 = 0;
+	i3 = 0;
+	m0 = 0;
+	m1 = 0;
+	m3 = 0;
+	m3 = 0;
+	l0 = 0;
+	l1 = 0;
+	l2 = 0;
+	l3 = 0;
+	b0 = 0;
+	b1 = 0;
+	b2 = 0;
+	b3 = 0;
+
+	/*
+	 * Set loop counters to zero, to make sure that
+	 * hw loops are disabled.
+	 */
+	r0  = 0;
+	lc0 = r0;
+	lc1 = r0;
+
+	SSYNC;
+
+	/* Check soft reset status */
+	p0.h = SWRST >> 16;
+	p0.l = SWRST & 0xFFFF;
+	r0.l = w[p0];
+
+	cc = bittst(r0, 15);
+	if !cc jump no_soft_reset;
+
+	/* Clear Soft reset */
+	r0 = 0x0000;
+	w[p0] = r0;
+	ssync;
+
+no_soft_reset:
+	nop;
+
+	/* Clear EVT registers */
+	p0.h = (EVT_EMULATION_ADDR >> 16);
+	p0.l = (EVT_EMULATION_ADDR & 0xFFFF);
+	p0 += 8;
+	p1 = 14;
+	r1 = 0;
+	LSETUP(4,4) lc0 = p1;
+	[ p0 ++ ] = r1;
+
+	p0.h = hi(SIC_IWR);
+	p0.l = lo(SIC_IWR);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+
+	sp.l = (0xffb01000 & 0xFFFF);
+	sp.h = (0xffb01000 >> 16);
+
+	/*
+	 * Check if the code is in SDRAM
+	 * If the code is in SDRAM, skip SDRAM initializaiton
+	 */
+	call get_pc;
+	r3.l = 0x0;
+	r3.h = 0x2000;
+	cc = r0 < r3 (iu);
+	if cc jump sdram_initialized;
+	call init_sdram;
+	/* relocate into to RAM */
+sdram_initialized:
+	call get_pc;
+offset:
+	r2.l = offset;
+	r2.h = offset;
+	r3.l = start;
+	r3.h = start;
+	r1 = r2 - r3;
+
+	r0 = r0 - r1;
+	p1 = r0;
+
+	p2.l = (CFG_MONITOR_BASE & 0xffff);
+	p2.h = (CFG_MONITOR_BASE >> 16);
+
+	p3 = 0x04;
+	p4.l = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) & 0xffff);
+	p4.h = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) >> 16);
+loop1:
+	r1 = [p1 ++ p3];
+	[p2 ++ p3] = r1;
+	cc=p2==p4;
+	if !cc jump loop1;
+	/*
+	 * configure STACK
+	 */
+	r0.h = (CONFIG_STACKBASE >> 16);
+	r0.l = (CONFIG_STACKBASE & 0xFFFF);
+	sp = r0;
+	fp = sp;
+
+	/*
+	 * This next section keeps the processor in supervisor mode
+	 * during kernel boot.  Switches to user mode at end of boot.
+	 * See page 3-9 of Hardware Reference manual for documentation.
+	 */
+
+	/* To keep ourselves in the supervisor mode */
+	p0.l = (EVT_IVG15_ADDR & 0xFFFF);
+	p0.h = (EVT_IVG15_ADDR >> 16);
+
+	p1.l = _real_start;
+	p1.h = _real_start;
+	[p0] = p1;
+
+	p0.l = (IMASK & 0xFFFF);
+	p0.h = (IMASK >> 16);
+	r0.l = LO(IVG15_POS);
+	r0.h = HI(IVG15_POS);
+	[p0] = r0;
+	raise 15;
+	p0.l = WAIT_HERE;
+	p0.h = WAIT_HERE;
+	reti = p0;
+	rti;
+
+WAIT_HERE:
+	jump WAIT_HERE;
+
+.global _real_start;
+_real_start:
+	[ -- sp ] = reti;
+
+#ifdef CONFIG_EZKIT561
+	p0.l = (WDOG_CTL & 0xFFFF);
+	p0.h = (WDOG_CTL >> 16);
+	r0 = WATCHDOG_DISABLE(z);
+	w[p0] = r0;
+#endif
+
+	/* DMA reset code to Hi of L1 SRAM */
+copy:
+	P1.H = hi(SYSMMR_BASE);	/* P1 Points to the beginning of SYSTEM MMR Space */
+	P1.L = lo(SYSMMR_BASE);
+
+	R0.H = reset_start;	/* Source Address (high) */
+	R0.L = reset_start;	/* Source Address (low) */
+	R1.H = reset_end;
+	R1.L = reset_end;
+	R2 = R1 - R0;		/* Count */
+	R1.H = hi(L1_ISRAM);	/* Destination Address (high) */
+	R1.L = lo(L1_ISRAM);	/* Destination Address (low) */
+	R3.L = DMAEN;		/* Source DMAConfig Value (8-bit words) */
+	R4.L = (DI_EN | WNR | DMAEN);	/* Destination DMAConfig Value (8-bit words) */
+
+DMA:
+	R6 = 0x1 (Z);
+	W[P1+OFFSET_(MDMA_S0_X_MODIFY)] = R6;	/* Source Modify = 1 */
+	W[P1+OFFSET_(MDMA_D0_X_MODIFY)] = R6;	/* Destination Modify = 1 */
+
+	[P1+OFFSET_(MDMA_S0_START_ADDR)] = R0;	/* Set Source Base Address */
+	W[P1+OFFSET_(MDMA_S0_X_COUNT)] = R2;	/* Set Source Count */
+	/* Set Source  DMAConfig = DMA Enable,
+	Memory Read,  8-Bit Transfers, 1-D DMA, Flow - Stop */
+	W[P1+OFFSET_(MDMA_S0_CONFIG)] = R3;
+
+	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;	/* Set Destination Base Address */
+	W[P1+OFFSET_(MDMA_D0_X_COUNT)] = R2;	/* Set Destination Count */
+	/* Set Destination DMAConfig = DMA Enable,
+	Memory Write, 8-Bit Transfers, 1-D DMA, Flow - Stop, IOC */
+	W[P1+OFFSET_(MDMA_D0_CONFIG)] = R4;
+
+WAIT_DMA_DONE:
+	p0.h = hi(MDMA_D0_IRQ_STATUS);
+	p0.l = lo(MDMA_D0_IRQ_STATUS);
+	R0 = W[P0](Z);
+	CC = BITTST(R0, 0);
+	if ! CC jump WAIT_DMA_DONE
+
+	R0 = 0x1;
+	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0;	/* Write 1 to clear DMA interrupt */
+
+	/* Initialize BSS Section with 0 s */
+	p1.l = __bss_start;
+	p1.h = __bss_start;
+	p2.l = _end;
+	p2.h = _end;
+	r1 = p1;
+	r2 = p2;
+	r3 = r2 - r1;
+	r3 = r3 >> 2;
+	p3 = r3;
+	lsetup (_clear_bss, _clear_bss_end ) lc1 = p3;
+	CC = p2<=p1;
+	if CC jump _clear_bss_skip;
+	r0 = 0;
+_clear_bss:
+_clear_bss_end:
+	[p1++] = r0;
+_clear_bss_skip:
+
+	p0.l = _start1;
+	p0.h = _start1;
+	jump (p0);
+
+reset_start:
+	p0.h = WDOG_CNT >> 16;
+	p0.l = WDOG_CNT & 0xffff;
+	r0 = 0x0010;
+	w[p0] = r0;
+	p0.h = WDOG_CTL >> 16;
+	p0.l = WDOG_CTL & 0xffff;
+	r0 = 0x0000;
+	w[p0] = r0;
+reset_wait:
+	jump reset_wait;
+
+reset_end: nop;
+
+_exit:
+	jump.s	_exit;
+get_pc:
+	r0 = rets;
+	rts;
diff --git a/cpu/bf561/start1.S b/cpu/bf561/start1.S
new file mode 100644
index 0000000..6d4731b
--- /dev/null
+++ b/cpu/bf561/start1.S
@@ -0,0 +1,38 @@
+/*
+ * U-boot - start1.S Code running out of RAM after relocation
+ *
+ * Copyright (c) 2005-2007 Analog Devices 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., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#define ASSEMBLY
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.global	start1;
+.global	_start1;
+
+.text
+_start1:
+start1:
+	sp += -12;
+	call	_board_init_f;
+	sp += 12;
diff --git a/cpu/bf561/traps.c b/cpu/bf561/traps.c
new file mode 100644
index 0000000..7e2dcd1
--- /dev/null
+++ b/cpu/bf561/traps.c
@@ -0,0 +1,239 @@
+/*
+ * U-boot - traps.c Routines related to interrupts and exceptions
+ *
+ * Copyright (c) 2005-2007 Analog Devices Inc.
+ *
+ * This file is based on
+ * No original Copyright holder listed,
+ * Probabily original (C) Roman Zippel (assigned DJD, 1999)
+ *
+ * Copyright 2003 Metrowerks - for Blackfin
+ * Copyright 2000-2001 Lineo, Inc. D. Jeff Dionne <jeff@lineo.ca>
+ * Copyright 1999-2000 D. Jeff Dionne, <jeff@uclinux.org>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#include <common.h>
+#include <linux/types.h>
+#include <asm/errno.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/traps.h>
+#include <asm/machdep.h>
+#include "cpu.h"
+#include <asm/arch/anomaly.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
+
+void init_IRQ(void)
+{
+	blackfin_init_IRQ();
+	return;
+}
+
+void process_int(unsigned long vec, struct pt_regs *fp)
+{
+	printf("interrupt\n");
+	return;
+}
+
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+unsigned long last_cplb_fault_retx;
+
+static unsigned int cplb_sizes[4] =
+    { 1024, 4 * 1024, 1024 * 1024, 4 * 1024 * 1024 };
+
+void trap_c(struct pt_regs *regs)
+{
+	unsigned int addr;
+	unsigned long trapnr = (regs->seqstat) & SEQSTAT_EXCAUSE;
+	unsigned int i, j, size, *I0, *I1;
+	unsigned short data = 0;
+
+	switch (trapnr) {
+		/* 0x26 - Data CPLB Miss */
+	case VEC_CPLB_M:
+
+#ifdef ANOMALY_05000261
+		/*
+		 * Work around an anomaly: if we see a new DCPLB fault, return
+		 * without doing anything.  Then, if we get the same fault again,
+		 * handle it.
+		 */
+		addr = last_cplb_fault_retx;
+		last_cplb_fault_retx = regs->retx;
+		printf("this time, curr = 0x%08x last = 0x%08x\n", addr,
+		       last_cplb_fault_retx);
+		if (addr != last_cplb_fault_retx)
+			goto trap_c_return;
+#endif
+		data = 1;
+
+	case VEC_CPLB_I_M:
+
+		if (data)
+			addr = *pDCPLB_FAULT_ADDR;
+		else
+			addr = *pICPLB_FAULT_ADDR;
+
+		for (i = 0; i < page_descriptor_table_size; i++) {
+			if (data) {
+				size = cplb_sizes[dcplb_table[i][1] >> 16];
+				j = dcplb_table[i][0];
+			} else {
+				size = cplb_sizes[icplb_table[i][1] >> 16];
+				j = icplb_table[i][0];
+			}
+			if ((j <= addr) && ((j + size) > addr)) {
+				debug("found %i 0x%08x\n", i, j);
+				break;
+			}
+		}
+		if (i == page_descriptor_table_size) {
+			printf("something is really wrong\n");
+			do_reset(NULL, 0, 0, NULL);
+		}
+
+		/* Turn the cache off */
+		if (data) {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL &=
+			    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+			sync();
+		}
+
+		if (data) {
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		j = 0;
+		while (*I1 & CPLB_LOCK) {
+			debug("skipping %i %08p - %08x\n", j, I1, *I1);
+			*I0++;
+			*I1++;
+			j++;
+		}
+
+		debug("remove %i 0x%08x  0x%08x\n", j, *I0, *I1);
+
+		for (; j < 15; j++) {
+			debug("replace %i 0x%08x  0x%08x\n", j, I0, I0 + 1);
+			*I0++ = *(I0 + 1);
+			*I1++ = *(I1 + 1);
+		}
+
+		if (data) {
+			*I0 = dcplb_table[i][0];
+			*I1 = dcplb_table[i][1];
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			*I0 = icplb_table[i][0];
+			*I1 = icplb_table[i][1];
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		for (j = 0; j < 16; j++) {
+			debug("%i 0x%08x  0x%08x\n", j, *I0++, *I1++);
+		}
+
+		/* Turn the cache back on */
+		if (data) {
+			j = *(unsigned int *)DMEM_CONTROL;
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL =
+			    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | j;
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+			sync();
+		}
+
+		break;
+	default:
+		/* All traps come here */
+		printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+		printf("stack frame=0x%x, ", (unsigned int)regs);
+		printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
+		dump(regs);
+		printf("\n\n");
+
+		printf("Unhandled IRQ or exceptions!\n");
+		printf("Please reset the board \n");
+		do_reset(NULL, 0, 0, NULL);
+	}
+
+trap_c_return:
+	return;
+
+}
+
+void dump(struct pt_regs *fp)
+{
+	debug("RETE:  %08lx  RETN: %08lx  RETX: %08lx  RETS: %08lx\n", fp->rete,
+	      fp->retn, fp->retx, fp->rets);
+	debug("IPEND: %04lx  SYSCFG: %04lx\n", fp->ipend, fp->syscfg);
+	debug("SEQSTAT: %08lx    SP: %08lx\n", (long)fp->seqstat, (long)fp);
+	debug("R0: %08lx    R1: %08lx    R2: %08lx    R3: %08lx\n", fp->r0,
+	      fp->r1, fp->r2, fp->r3);
+	debug("R4: %08lx    R5: %08lx    R6: %08lx    R7: %08lx\n", fp->r4,
+	      fp->r5, fp->r6, fp->r7);
+	debug("P0: %08lx    P1: %08lx    P2: %08lx    P3: %08lx\n", fp->p0,
+	      fp->p1, fp->p2, fp->p3);
+	debug("P4: %08lx    P5: %08lx    FP: %08lx\n", fp->p4, fp->p5, fp->fp);
+	debug("A0.w: %08lx    A0.x: %08lx    A1.w: %08lx    A1.x: %08lx\n",
+	      fp->a0w, fp->a0x, fp->a1w, fp->a1x);
+
+	debug("LB0: %08lx  LT0: %08lx  LC0: %08lx\n", fp->lb0, fp->lt0,
+	      fp->lc0);
+	debug("LB1: %08lx  LT1: %08lx  LC1: %08lx\n", fp->lb1, fp->lt1,
+	      fp->lc1);
+	debug("B0: %08lx  L0: %08lx  M0: %08lx  I0: %08lx\n", fp->b0, fp->l0,
+	      fp->m0, fp->i0);
+	debug("B1: %08lx  L1: %08lx  M1: %08lx  I1: %08lx\n", fp->b1, fp->l1,
+	      fp->m1, fp->i1);
+	debug("B2: %08lx  L2: %08lx  M2: %08lx  I2: %08lx\n", fp->b2, fp->l2,
+	      fp->m2, fp->i2);
+	debug("B3: %08lx  L3: %08lx  M3: %08lx  I3: %08lx\n", fp->b3, fp->l3,
+	      fp->m3, fp->i3);
+
+	debug("DCPLB_FAULT_ADDR=%p\n", *pDCPLB_FAULT_ADDR);
+	debug("ICPLB_FAULT_ADDR=%p\n", *pICPLB_FAULT_ADDR);
+
+}
diff --git a/cpu/bf561/video.c b/cpu/bf561/video.c
new file mode 100644
index 0000000..3ff0151
--- /dev/null
+++ b/cpu/bf561/video.c
@@ -0,0 +1,194 @@
+/*
+ * (C) Copyright 2000
+ * Paolo Scaffardi, AIRVENT SAM s.p.a - RIMINI(ITALY), arsenio@tin.it
+ * (C) Copyright 2002
+ * Wolfgang Denk, wd@denx.de
+ * (C) Copyright 2006
+ * Aubrey Li, aubrey.li@analog.com
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <stdarg.h>
+#include <common.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <i2c.h>
+#include <linux/types.h>
+#include <devices.h>
+
+#ifdef CONFIG_VIDEO
+#define NTSC_FRAME_ADDR 0x06000000
+#include "video.h"
+
+/* NTSC OUTPUT SIZE  720 * 240 */
+#define VERTICAL	2
+#define HORIZONTAL	4
+
+int is_vblank_line(const int line)
+{
+	/*
+	 *  This array contains a single bit for each line in
+	 *  an NTSC frame.
+	 */
+	if ((line <= 18) || (line >= 264 && line <= 281) || (line == 528))
+		return true;
+
+	return false;
+}
+
+int NTSC_framebuffer_init(char *base_address)
+{
+	const int NTSC_frames = 1;
+	const int NTSC_lines = 525;
+	char *dest = base_address;
+	int frame_num, line_num;
+
+	for (frame_num = 0; frame_num < NTSC_frames; ++frame_num) {
+		for (line_num = 1; line_num <= NTSC_lines; ++line_num) {
+			unsigned int code;
+			int offset = 0;
+			int i;
+
+			if (is_vblank_line(line_num))
+				offset++;
+
+			if (line_num > 266 || line_num < 3)
+				offset += 2;
+
+			/* Output EAV code */
+			code = SystemCodeMap[offset].EAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output horizontal blanking */
+			for (i = 0; i < 67 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+
+			/* Output SAV */
+			code = SystemCodeMap[offset].SAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output empty horizontal data */
+			for (i = 0; i < 360 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+		}
+	}
+
+	return dest - base_address;
+}
+
+void fill_frame(char *Frame, int Value)
+{
+	int *OddPtr32;
+	int OddLine;
+	int *EvenPtr32;
+	int EvenLine;
+	int i;
+	int *data;
+	int m, n;
+
+	/* fill odd and even frames */
+	for (OddLine = 22, EvenLine = 285; OddLine < 263; OddLine++, EvenLine++) {
+		OddPtr32 = (int *)((Frame + (OddLine * 1716)) + 276);
+		EvenPtr32 = (int *)((Frame + (EvenLine * 1716)) + 276);
+		for (i = 0; i < 360; i++, OddPtr32++, EvenPtr32++) {
+			*OddPtr32 = Value;
+			*EvenPtr32 = Value;
+		}
+	}
+
+	for (m = 0; m < VERTICAL; m++) {
+		data = (int *)u_boot_logo.data;
+		for (OddLine = (22 + m), EvenLine = (285 + m);
+		     OddLine < (u_boot_logo.height * VERTICAL) + (22 + m);
+		     OddLine += VERTICAL, EvenLine += VERTICAL) {
+			OddPtr32 = (int *)((Frame + ((OddLine) * 1716)) + 276);
+			EvenPtr32 =
+			    (int *)((Frame + ((EvenLine) * 1716)) + 276);
+			for (i = 0; i < u_boot_logo.width / 2; i++) {
+				/* enlarge one pixel to m x n */
+				for (n = 0; n < HORIZONTAL; n++) {
+					*OddPtr32++ = *data;
+					*EvenPtr32++ = *data;
+				}
+				data++;
+			}
+		}
+	}
+}
+
+void video_putc(const char c)
+{
+}
+
+void video_puts(const char *s)
+{
+}
+
+static int video_init(void)
+{
+	char *NTSCFrame;
+	NTSCFrame = (char *)NTSC_FRAME_ADDR;
+	NTSC_framebuffer_init(NTSCFrame);
+	fill_frame(NTSCFrame, BLUE);
+
+	*pPPI_CONTROL = 0x0082;
+	*pPPI_FRAME = 0x020D;
+
+	*pDMA0_START_ADDR = NTSCFrame;
+	*pDMA0_X_COUNT = 0x035A;
+	*pDMA0_X_MODIFY = 0x0002;
+	*pDMA0_Y_COUNT = 0x020D;
+	*pDMA0_Y_MODIFY = 0x0002;
+	*pDMA0_CONFIG = 0x1015;
+	*pPPI_CONTROL = 0x0083;
+	return 0;
+}
+
+int drv_video_init(void)
+{
+	int error, devices = 1;
+
+	device_t videodev;
+
+	video_init();		/* Video initialization */
+
+	memset(&videodev, 0, sizeof(videodev));
+
+	strcpy(videodev.name, "video");
+	videodev.ext = DEV_EXT_VIDEO;	/* Video extensions */
+	videodev.flags = DEV_FLAGS_OUTPUT;	/* Output only */
+	videodev.putc = video_putc;	/* 'putc' function */
+	videodev.puts = video_puts;	/* 'puts' function */
+
+	error = device_register(&videodev);
+
+	return (error == 0) ? devices : error;
+}
+#endif
diff --git a/cpu/bf561/video.h b/cpu/bf561/video.h
new file mode 100644
index 0000000..d237f6a
--- /dev/null
+++ b/cpu/bf561/video.h
@@ -0,0 +1,25 @@
+#include <video_logo.h>
+#define write_dest_byte(val) {*dest++=val;}
+#define BLACK   (0x01800180)	/* black pixel pattern  */
+#define BLUE    (0x296E29F0)	/* blue pixel pattern   */
+#define RED     (0x51F0515A)	/* red pixel pattern    */
+#define MAGENTA (0x6ADE6ACA)	/* magenta pixel pattern */
+#define GREEN   (0x91229136)	/* green pixel pattern  */
+#define CYAN    (0xAA10AAA6)	/* cyan pixel pattern   */
+#define YELLOW  (0xD292D210)	/* yellow pixel pattern */
+#define WHITE   (0xFE80FE80)	/* white pixel pattern  */
+
+#define true 	1
+#define false	0
+
+typedef struct {
+	unsigned int SAV;
+	unsigned int EAV;
+} SystemCodeType;
+
+const SystemCodeType SystemCodeMap[4] = {
+	{0xFF000080, 0xFF00009D},
+	{0xFF0000AB, 0xFF0000B6},
+	{0xFF0000C7, 0xFF0000DA},
+	{0xFF0000EC, 0xFF0000F1}
+};
diff --git a/cpu/microblaze/Makefile b/cpu/microblaze/Makefile
index fd54425..db1afa5 100644
--- a/cpu/microblaze/Makefile
+++ b/cpu/microblaze/Makefile
@@ -26,7 +26,8 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(CPU).a
 
 START	= start.o
-COBJS	= cpu.o interrupts.o
+SOBJS	= dcache.o icache.o irq.o disable_int.o enable_int.o
+COBJS	= cpu.o interrupts.o cache.o exception.o timer.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/microblaze/cache.c b/cpu/microblaze/cache.c
new file mode 100644
index 0000000..fc388eb
--- /dev/null
+++ b/cpu/microblaze/cache.c
@@ -0,0 +1,48 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal SIMEK <moonstr@monstr.eu>
+ *
+ * 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 (CONFIG_COMMANDS & CFG_CMD_CACHE)
+
+int dcache_status (void)
+{
+	int i = 0;
+	int mask = 0x80;
+	__asm__ __volatile__ ("mfs %0,rmsr"::"r" (i):"memory");
+	/* i&=0x80 */
+	__asm__ __volatile__ ("and %0,%0,%1"::"r" (i), "r" (mask):"memory");
+	return i;
+}
+
+int icache_status (void)
+{
+	int i = 0;
+	int mask = 0x20;
+	__asm__ __volatile__ ("mfs %0,rmsr"::"r" (i):"memory");
+	/* i&=0x20 */
+	__asm__ __volatile__ ("and %0,%0,%1"::"r" (i), "r" (mask):"memory");
+	return i;
+}
+#endif
diff --git a/cpu/microblaze/dcache.S b/cpu/microblaze/dcache.S
new file mode 100644
index 0000000..eaf9671
--- /dev/null
+++ b/cpu/microblaze/dcache.S
@@ -0,0 +1,68 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * 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
+ */
+	.text
+	.globl	dcache_enable
+	.ent	dcache_enable
+	.align	2
+dcache_enable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Set the instruction enable bit */
+	ori	r12, r12, 0x80
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	dcache_enable
+
+	.text
+	.globl	dcache_disable
+	.ent	dcache_disable
+	.align	2
+dcache_disable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Clear the data cache enable bit */
+	andi	r12, r12, ~0x80
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	dcache_disable
diff --git a/cpu/microblaze/disable_int.S b/cpu/microblaze/disable_int.S
new file mode 100644
index 0000000..aecd795
--- /dev/null
+++ b/cpu/microblaze/disable_int.S
@@ -0,0 +1,46 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * 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
+ */
+
+	.text
+	.globl	microblaze_disable_interrupts
+	.ent	microblaze_disable_interrupts
+	.align	2
+microblaze_disable_interrupts:
+	#Make space on stack for a temporary
+	addi	r1, r1, -4
+	#Save register r12
+	swi	r12, r1, 0
+	#Read the MSR register
+	mfs	r12, rmsr
+	#Clear the interrupt enable bit
+	andi	r12, r12, ~2
+	#Save the MSR register
+	mts	rmsr, r12
+	#Load register r12
+	lwi	r12, r1, 0
+	#Return
+	rtsd	r15, 8
+	#Update stack in the delay slot
+	addi	r1, r1, 4
+	.end	microblaze_disable_interrupts
diff --git a/cpu/at32ap/at32ap7000/hebi.c b/cpu/microblaze/enable_int.S
index 3b32adf..c096c6c 100644
--- a/cpu/at32ap/at32ap7000/hebi.c
+++ b/cpu/microblaze/enable_int.S
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2006 Atmel Corporation
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstrmonstr.eu>
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -11,7 +13,7 @@
  *
  * 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
+ * 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
@@ -19,20 +21,18 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  * MA 02111-1307 USA
  */
-#include <common.h>
-
-#include <asm/io.h>
-
-#include <asm/arch/hmatrix2.h>
-#include <asm/arch/memory-map.h>
-#include <asm/arch/platform.h>
-
-void cpu_enable_sdram(void)
-{
-	const struct device *hmatrix;
-
-	hmatrix = get_device(DEVICE_HMATRIX);
 
-	/* Set the SDRAM_ENABLE bit in the HEBI SFR */
-	hmatrix2_writel(hmatrix, SFR4, 1 << 1);
-}
+	.text
+	.globl	microblaze_enable_interrupts
+	.ent	microblaze_enable_interrupts
+	.align	2
+microblaze_enable_interrupts:
+	addi	r1, r1, -4
+	swi	r12, r1, 0
+	mfs	r12, rmsr
+	ori	r12, r12, 2
+	mts	rmsr, r12
+	lwi	r12, r1, 0
+	rtsd	r15, 8
+	addi	r1, r1, 4
+	.end	microblaze_enable_interrupts
diff --git a/cpu/microblaze/exception.c b/cpu/microblaze/exception.c
new file mode 100644
index 0000000..b135acb
--- /dev/null
+++ b/cpu/microblaze/exception.c
@@ -0,0 +1,68 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * 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>
+
+void _hw_exception_handler (void)
+{
+	int address = 0;
+	int state = 0;
+	/* loading address of exception EAR */
+	__asm__ __volatile ("mfs %0,rear"::"r" (address):"memory");
+	/* loading excetpion state register ESR */
+	__asm__ __volatile ("mfs %0,resr"::"r" (state):"memory");
+	printf ("Hardware exception at 0x%x address\n", address);
+	switch (state & 0x1f) {	/* mask on exception cause */
+	case 0x1:
+		puts ("Unaligned data access exception\n");
+		break;
+	case 0x2:
+		puts ("Illegal op-code exception\n");
+		break;
+	case 0x3:
+		puts ("Instruction bus error exception\n");
+		break;
+	case 0x4:
+		puts ("Data bus error exception\n");
+		break;
+	case 0x5:
+		puts ("Divide by zero exception\n");
+		break;
+	default:
+		puts ("Undefined cause\n");
+		break;
+	}
+	printf ("Unaligned %sword access\n", ((state & 0x800) ? "" : "half"));
+	printf ("Unaligned %s access\n", ((state & 0x400) ? "store" : "load"));
+	printf ("Register R%x\n", (state & 0x3E) >> 5);
+	hang ();
+}
+
+#ifdef CFG_USR_EXCEP
+void _exception_handler (void)
+{
+	puts ("User vector_exception\n");
+	hang ();
+}
+#endif
diff --git a/cpu/microblaze/icache.S b/cpu/microblaze/icache.S
new file mode 100644
index 0000000..25940d1
--- /dev/null
+++ b/cpu/microblaze/icache.S
@@ -0,0 +1,69 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * 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
+ */
+
+	.text
+	.globl	icache_enable
+	.ent	icache_enable
+	.align	2
+icache_enable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Set the instruction enable bit */
+	ori	r12, r12, 0x20
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	icache_enable
+
+	.text
+	.globl	icache_disable
+	.ent	icache_disable
+	.align	2
+icache_disable:
+	/* Make space on stack for a temporary */
+	addi	r1, r1, -4
+	/* Save register r12 */
+	swi	r12, r1, 0
+	/* Read the MSR register */
+	mfs	r12, rmsr
+	/* Clear the instruction enable bit */
+	andi	r12, r12, ~0x20
+	/* Save the MSR register */
+	mts	rmsr, r12
+	/* Load register r12 */
+	lwi	r12, r1, 0
+	/* Return */
+	rtsd	r15, 8
+	/* Update stack in the delay slot */
+	addi	r1, r1, 4
+	.end	icache_disable
diff --git a/cpu/microblaze/interrupts.c b/cpu/microblaze/interrupts.c
index ccf67e1..2db847c 100644
--- a/cpu/microblaze/interrupts.c
+++ b/cpu/microblaze/interrupts.c
@@ -1,6 +1,8 @@
 /*
+ * (C) Copyright 2007 Michal Simek
  * (C) Copyright 2004 Atmark Techno, Inc.
  *
+ * Michal  SIMEK <monstr@monstr.eu>
  * Yasushi SHOJI <yashi@atmark-techno.com>
  *
  * See file CREDITS for list of people who contributed to this
@@ -13,7 +15,7 @@
  *
  * 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
+ * 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
@@ -22,11 +24,185 @@
  * MA 02111-1307 USA
  */
 
-void enable_interrupts(void)
+#include <common.h>
+#include <command.h>
+#include <asm/microblaze_intc.h>
+
+#undef DEBUG_INT
+
+extern void microblaze_disable_interrupts (void);
+extern void microblaze_enable_interrupts (void);
+
+void enable_interrupts (void)
 {
+	microblaze_enable_interrupts ();
 }
 
-int disable_interrupts(void)
+int disable_interrupts (void)
 {
+	microblaze_disable_interrupts ();
 	return 0;
 }
+
+#ifdef CFG_INTC_0
+#ifdef CFG_TIMER_0
+extern void timer_init (void);
+#endif
+
+static struct irq_action vecs[CFG_INTC_0_NUM];
+
+/* mapping structure to interrupt controller */
+microblaze_intc_t *intc = (microblaze_intc_t *) (CFG_INTC_0_ADDR);
+
+/* default handler */
+void def_hdlr (void)
+{
+	puts ("def_hdlr\n");
+}
+
+void enable_one_interrupt (int irq)
+{
+	int mask;
+	int offset = 1;
+	offset <<= irq;
+	mask = intc->ier;
+	intc->ier = (mask | offset);
+#ifdef DEBUG_INT
+	printf ("Enable one interrupt irq %x - mask %x,ier %x\n", offset, mask,
+		intc->ier);
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+#endif
+}
+
+void disable_one_interrupt (int irq)
+{
+	int mask;
+	int offset = 1;
+	offset <<= irq;
+	mask = intc->ier;
+	intc->ier = (mask & ~offset);
+#ifdef DEBUG_INT
+	printf ("Disable one interrupt irq %x - mask %x,ier %x\n", irq, mask,
+		intc->ier);
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+#endif
+}
+
+/* adding new handler for interrupt */
+void install_interrupt_handler (int irq, interrupt_handler_t * hdlr, void *arg)
+{
+	struct irq_action *act;
+	/* irq out of range */
+	if ((irq < 0) || (irq > CFG_INTC_0_NUM)) {
+		puts ("IRQ out of range\n");
+		return;
+	}
+	act = &vecs[irq];
+	if (hdlr) {		/* enable */
+		act->handler = hdlr;
+		act->arg = arg;
+		act->count = 0;
+		enable_one_interrupt (irq);
+	} else {		/* disable */
+
+		act->handler = (interrupt_handler_t *) def_hdlr;
+		act->arg = (void *)irq;
+		disable_one_interrupt (irq);
+	}
+}
+
+/* initialization interrupt controller - hardware */
+void intc_init (void)
+{
+	intc->mer = 0;
+	intc->ier = 0;
+	intc->iar = 0xFFFFFFFF;
+	/* XIntc_Start - hw_interrupt enable and all interrupt enable */
+	intc->mer = 0x3;
+#ifdef DEBUG_INT
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+#endif
+}
+
+int interrupts_init (void)
+{
+	int i;
+	/* initialize irq list */
+	for (i = 0; i < CFG_INTC_0_NUM; i++) {
+		vecs[i].handler = (interrupt_handler_t *) def_hdlr;
+		vecs[i].arg = (void *)i;
+		vecs[i].count = 0;
+	}
+	/* initialize intc controller */
+	intc_init ();
+#ifdef CFG_TIMER_0
+	timer_init ();
+#endif
+	enable_interrupts ();
+	return 0;
+}
+
+void interrupt_handler (void)
+{
+	int irqs;
+	irqs = (intc->isr & intc->ier);	/* find active interrupt */
+
+#ifdef DEBUG_INT
+	printf ("INTC isr %x, ier %x, iar %x, mer %x\n", intc->isr, intc->ier,
+		intc->iar, intc->mer);
+	printf ("Interrupt handler on %x line, r14 %x\n", irqs, value);
+#endif
+	struct irq_action *act = vecs;
+	while (irqs) {
+		if (irqs & 1) {
+#ifdef DEBUG_INT
+			printf
+			    ("Jumping to interrupt handler rutine addr %x,count %x,arg %x\n",
+			     act->handler, act->count, act->arg);
+#endif
+			act->handler (act->arg);
+			act->count++;
+		}
+		irqs >>= 1;
+		act++;
+	}
+	intc->iar = 0xFFFFFFFF;	/* erase all events */
+#ifdef DEBUG
+	printf ("Dump INTC reg, isr %x, ier %x, iar %x, mer %x\n", intc->isr,
+		intc->ier, intc->iar, intc->mer);
+	printf ("Interrupt handler on %x line, r14\n", irqs);
+#endif
+}
+#endif
+
+#if (CONFIG_COMMANDS & CFG_CMD_IRQ)
+#ifdef CFG_INTC_0
+int do_irqinfo (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	int i;
+	struct irq_action *act = vecs;
+
+	puts ("\nInterrupt-Information:\n\n"
+	      "Nr  Routine   Arg       Count\n"
+	      "-----------------------------\n");
+
+	for (i = 0; i < CFG_INTC_0_NUM; i++) {
+		if (act->handler != (interrupt_handler_t*) def_hdlr) {
+			printf ("%02d  %08lx  %08lx  %d\n", i,
+				(int)act->handler, (int)act->arg, act->count);
+		}
+		act++;
+	}
+	puts ("\n");
+	return (0);
+}
+#else
+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 */
diff --git a/cpu/microblaze/irq.S b/cpu/microblaze/irq.S
new file mode 100644
index 0000000..a4e3fbf
--- /dev/null
+++ b/cpu/microblaze/irq.S
@@ -0,0 +1,165 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * 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>
+	.text
+	.global _interrupt_handler
+_interrupt_handler:
+	addi	r1, r1, -4
+	swi	r2, r1, 0
+	addi	r1, r1, -4
+	swi	r3, r1, 0
+	addi	r1, r1, -4
+	swi	r4, r1, 0
+	addi	r1, r1, -4
+	swi	r5, r1, 0
+	addi	r1, r1, -4
+	swi	r6, r1, 0
+	addi	r1, r1, -4
+	swi	r7, r1, 0
+	addi	r1, r1, -4
+	swi	r8, r1, 0
+	addi	r1, r1, -4
+	swi	r9, r1, 0
+	addi	r1, r1, -4
+	swi	r10, r1, 0
+	addi	r1, r1, -4
+	swi	r11, r1, 0
+	addi	r1, r1, -4
+	swi	r12, r1, 0
+	addi	r1, r1, -4
+	swi	r13, r1, 0
+	addi	r1, r1, -4
+	swi	r14, r1, 0
+	addi	r1, r1, -4
+	swi	r15, r1, 0
+	addi	r1, r1, -4
+	swi	r16, r1, 0
+	addi	r1, r1, -4
+	swi	r17, r1, 0
+	addi	r1, r1, -4
+	swi	r18, r1, 0
+	addi	r1, r1, -4
+	swi	r19, r1, 0
+	addi	r1, r1, -4
+	swi	r20, r1, 0
+	addi	r1, r1, -4
+	swi	r21, r1, 0
+	addi	r1, r1, -4
+	swi	r22, r1, 0
+	addi	r1, r1, -4
+	swi	r23, r1, 0
+	addi	r1, r1, -4
+	swi	r24, r1, 0
+	addi	r1, r1, -4
+	swi	r25, r1, 0
+	addi	r1, r1, -4
+	swi	r26, r1, 0
+	addi	r1, r1, -4
+	swi	r27, r1, 0
+	addi	r1, r1, -4
+	swi	r28, r1, 0
+	addi	r1, r1, -4
+	swi	r29, r1, 0
+	addi	r1, r1, -4
+	swi	r30, r1, 0
+	addi	r1, r1, -4
+	swi	r31, r1, 0
+	brlid	r15, interrupt_handler
+	nop
+	nop
+	lwi	r31, r1, 0
+	addi	r1, r1, 4
+	lwi	r30, r1, 0
+	addi	r1, r1, 4
+	lwi	r29, r1, 0
+	addi	r1, r1, 4
+	lwi	r28, r1, 0
+	addi	r1, r1, 4
+	lwi	r27, r1, 0
+	addi	r1, r1, 4
+	lwi	r26, r1, 0
+	addi	r1, r1, 4
+	lwi	r25, r1, 0
+	addi	r1, r1, 4
+	lwi	r24, r1, 0
+	addi	r1, r1, 4
+	lwi	r23, r1, 0
+	addi	r1, r1, 4
+	lwi	r22, r1, 0
+	addi	r1, r1, 4
+	lwi	r21, r1, 0
+	addi	r1, r1, 4
+	lwi	r20, r1, 0
+	addi	r1, r1, 4
+	lwi	r19, r1, 0
+	addi	r1, r1, 4
+	lwi	r18, r1, 0
+	addi	r1, r1, 4
+	lwi	r17, r1, 0
+	addi	r1, r1, 4
+	lwi	r16, r1, 0
+	addi	r1, r1, 4
+	lwi	r15, r1, 0
+	addi	r1, r1, 4
+	lwi	r14, r1, 0
+	addi	r1, r1, 4
+	lwi	r13, r1, 0
+	addi	r1, r1, 4
+	lwi	r12, r1, 0
+	addi	r1, r1, 4
+	lwi	r11, r1, 0
+	addi	r1, r1, 4
+	lwi	r10, r1, 0
+	addi	r1, r1, 4
+	lwi	r9, r1, 0
+	addi	r1, r1, 4
+	lwi	r8, r1, 0
+	addi	r1, r1, 4
+	lwi	r7, r1, 0
+	addi	r1, r1, 4
+	lwi	r6, r1, 0
+	addi	r1, r1, 4
+	lwi	r5, r1, 0
+	addi	r1, r1, 4
+	lwi	r4, r1, 0
+	addi	r1, r1, 4
+	lwi	r3, r1, 0
+	addi	r1, r1, 4
+	lwi	r2, r1, 0
+	addi	r1, r1, 4
+
+	/* enable_interrupt */
+	addi	r1, r1, -4
+	swi	r12, r1, 0
+	mfs	r12, rmsr
+	ori	r12, r12, 2
+	mts	rmsr, r12
+	lwi	r12, r1, 0
+	addi	r1, r1, 4
+	nop
+	bra	r14
+	nop
+	nop
+	.size _interrupt_handler,.-_interrupt_handler
diff --git a/cpu/microblaze/start.S b/cpu/microblaze/start.S
index 7efdbb0..ca3befc 100644
--- a/cpu/microblaze/start.S
+++ b/cpu/microblaze/start.S
@@ -1,6 +1,8 @@
 /*
+ * (C) Copyright 2007 Michal Simek
  * (C) Copyright 2004 Atmark Techno, Inc.
  *
+ * Michal  SIMEK <monstr@monstr.eu>
  * Yasushi SHOJI <yashi@atmark-techno.com>
  *
  * See file CREDITS for list of people who contributed to this
@@ -13,7 +15,7 @@
  *
  * 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
+ * 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
@@ -27,10 +29,91 @@
 	.text
 	.global _start
 _start:
+	mts	rmsr, r0	/* disable cache */
+	addi	r1, r0, CFG_INIT_SP_OFFSET
+	addi	r1, r1, -4	/* Decrement SP to top of memory */
+	/* add opcode instruction for 32bit jump - 2 instruction imm & brai*/
+	addi	r6, r0, 0xb000	/* hex b000 opcode imm */
+	bslli	r6, r6, 16	/* shift */
+	swi	r6, r0, 0x0	/* reset address */
+	swi	r6, r0, 0x8	/* user vector exception */
+	swi	r6, r0, 0x10	/* interrupt */
+	swi	r6, r0, 0x20	/* hardware exception */
 
-	addi	r1, r0, CFG_SDRAM_BASE	/* init stack pointer */
-	addi	r1, r1, CFG_SDRAM_SIZE	/* set sp to high up */
+	addi	r6, r0, 0xb808	/* hew b808 opcode brai*/
+	bslli	r6, r6, 16
+	swi	r6, r0, 0x4	/* reset address */
+	swi	r6, r0, 0xC	/* user vector exception */
+	swi	r6, r0, 0x14	/* interrupt */
+	swi	r6, r0, 0x24	/* hardware exception */
 
-	brai	board_init
+#ifdef CFG_RESET_ADDRESS
+	/* reset address */
+	addik	r6, r0, CFG_RESET_ADDRESS
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0x2
+	shi	r6, r0, 0x6
+/*
+ * Copy U-Boot code to TEXT_BASE
+ * solve problem with sbrk_base
+ */
+#if (CFG_RESET_ADDRESS != TEXT_BASE)
+	addi	r4, r0, __end
+	addi	r5, r0, __text_start
+	rsub	r4, r5, r4	/* size = __end - __text_start */
+	addi	r6, r0, CFG_RESET_ADDRESS	/* source address */
+	addi	r7, r0, 0	/* counter */
+4:
+	lw	r8, r6, r7
+	sw	r8, r5, r7
+	addi	r7, r7, 0x4
+	cmp	r8, r4, r7
+	blti	r8, 4b
+#endif
+#endif
+
+#ifdef CFG_USR_EXCEP
+	/* user_vector_exception */
+	addik	r6, r0, _exception_handler
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0xa
+	shi	r6, r0, 0xe
+#endif
 
+#ifdef CFG_INTC_0
+	/* interrupt_handler */
+	addik	r6, r0, _interrupt_handler
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0x12
+	shi	r6, r0, 0x16
+#endif
+
+	/* hardware exception */
+	addik	r6, r0, _hw_exception_handler
+	sw	r6, r1, r0
+	lhu	r7, r1, r0
+	shi	r7, r0, 0x22
+	shi	r6, r0, 0x26
+
+	/* enable instruction and data cache */
+	mfs	r12, rmsr
+	ori	r12, r12, 0xa0
+	mts	rmsr, r12
+
+clear_bss:
+	/* clear BSS segments */
+	addi	r5, r0, __bss_start
+	addi	r4, r0, __bss_end
+	cmp	r6, r5, r4
+	beqi	r6, 3f
+2:
+	swi     r0, r5, 0 /* write zero to loc */
+	addi    r5, r5, 4 /* increment to next loc */
+	cmp     r6, r5, r4 /* check if we have reach the end */
+	bnei    r6, 2b
+3:	/* jumping to board_init */
+	brai	board_init
 1:	bri	1b
diff --git a/cpu/microblaze/timer.c b/cpu/microblaze/timer.c
new file mode 100644
index 0000000..be4fd57
--- /dev/null
+++ b/cpu/microblaze/timer.c
@@ -0,0 +1,68 @@
+/*
+ * (C) Copyright 2007 Michal Simek
+ *
+ * Michal  SIMEK <monstr@monstr.eu>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/microblaze_timer.h>
+
+volatile int timestamp = 0;
+
+void reset_timer (void)
+{
+	timestamp = 0;
+}
+
+ulong get_timer (ulong base)
+{
+	return (timestamp - base);
+}
+
+void set_timer (ulong t)
+{
+	timestamp = t;
+}
+
+#ifdef CFG_INTC_0
+#ifdef CFG_TIMER_0
+extern void install_interrupt_handler (int irq, interrupt_handler_t * hdlr,
+				       void *arg);
+
+microblaze_timer_t *tmr = (microblaze_timer_t *) (CFG_TIMER_0_ADDR);
+
+void timer_isr (void *arg)
+{
+	timestamp++;
+	tmr->control = tmr->control | TIMER_INTERRUPT;
+}
+
+void timer_init (void)
+{
+	tmr->loadreg = CFG_TIMER_0_PRELOAD;
+	tmr->control = TIMER_INTERRUPT | TIMER_RESET;
+	tmr->control =
+	    TIMER_ENABLE | TIMER_ENABLE_INTR | TIMER_RELOAD | TIMER_DOWN_COUNT;
+	reset_timer ();
+	install_interrupt_handler (CFG_TIMER_0_IRQ, timer_isr, (void *)tmr);
+}
+#endif
+#endif
diff --git a/cpu/mpc5xxx/fec.c b/cpu/mpc5xxx/fec.c
index e639234..e59bd85 100644
--- a/cpu/mpc5xxx/fec.c
+++ b/cpu/mpc5xxx/fec.c
@@ -467,6 +467,10 @@ static int mpc5xxx_fec_init_phy(struct eth_device *dev, bd_t * bis)
 		miiphy_write(dev->name, phyAddr, 0x0, 0x8000);
 		udelay(1000);
 
+#if defined(CONFIG_UC101)
+		/* Set the LED configuration Register for the UC101 Board */
+		miiphy_write(dev->name, phyAddr, 0x14, 0x4122);
+#endif
 		if (fec->xcv_type == MII10) {
 			/*
 			 * Force 10Base-T, FDX operation
@@ -878,12 +882,13 @@ 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_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_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
diff --git a/cpu/mpc8260/cpu_init.c b/cpu/mpc8260/cpu_init.c
index 7dcc949..380d7af 100644
--- a/cpu/mpc8260/cpu_init.c
+++ b/cpu/mpc8260/cpu_init.c
@@ -129,9 +129,9 @@ void cpu_init_f (volatile immap_t * immr)
 	/* BCR - Bus Configuration Register (4-25) */
 #if defined(CFG_BCR_60x) && (CFG_BCR_SINGLE)
 	if (immr->im_siu_conf.sc_bcr & BCR_EBM) {
-		immr->im_siu_conf.sc_bcr = CFG_BCR_60x;
+		immr->im_siu_conf.sc_bcr = SET_VAL_MASK(immr->im_siu_conf.sc_bcr, CFG_BCR_60x, 0x80000010);
 	} else {
-		immr->im_siu_conf.sc_bcr = CFG_BCR_SINGLE;
+		immr->im_siu_conf.sc_bcr = SET_VAL_MASK(immr->im_siu_conf.sc_bcr, CFG_BCR_SINGLE, 0x80000010);
 	}
 #else
 	immr->im_siu_conf.sc_bcr = CFG_BCR;
@@ -141,9 +141,9 @@ void cpu_init_f (volatile immap_t * immr)
 #if defined(CFG_SIUMCR_LOW) && (CFG_SIUMCR_HIGH)
 	cpu_clk = board_get_cpu_clk_f ();
 	if (cpu_clk >= 100000000) {
-		immr->im_siu_conf.sc_siumcr = CFG_SIUMCR_HIGH;
+		immr->im_siu_conf.sc_siumcr = SET_VAL_MASK(immr->im_siu_conf.sc_siumcr, CFG_SIUMCR_HIGH, 0x9f3cc000);
 	} else {
-		immr->im_siu_conf.sc_siumcr = CFG_SIUMCR_LOW;
+		immr->im_siu_conf.sc_siumcr = SET_VAL_MASK(immr->im_siu_conf.sc_siumcr, CFG_SIUMCR_LOW, 0x9f3cc000);
 	}
 #else
 	immr->im_siu_conf.sc_siumcr = CFG_SIUMCR;
diff --git a/cpu/mpc8260/pci.c b/cpu/mpc8260/pci.c
index 1edd6fb..75c6ab2 100644
--- a/cpu/mpc8260/pci.c
+++ b/cpu/mpc8260/pci.c
@@ -275,22 +275,7 @@ void pci_mpc8250_init (struct pci_controller *hose)
 				  | SIUMCR_BCTLC00
 				  | SIUMCR_MMR11;
 #elif defined(CONFIG_TQM8272)
-#if 0
-	immap->im_siu_conf.sc_siumcr = (immap->im_siu_conf.sc_siumcr &
-						~SIUMCR_LBPC11 &
-						~SIUMCR_CS10PC11 &
-						~SIUMCR_LBPC11) |
-					SIUMCR_LBPC01 |
-					SIUMCR_CS10PC01 |
-					SIUMCR_APPC10;
-#else
-#if 0
-	immap->im_siu_conf.sc_siumcr = (immap->im_siu_conf.sc_siumcr |
-					SIUMCR_APPC10);
-#else
-	immap->im_siu_conf.sc_siumcr = 0x88000000;
-#endif
-#endif
+/* nothing to do for this Board here */
 #else
 	/*
 	 * Setting required to enable IRQ1-IRQ7 (SIUMCR [DPPC]),
@@ -304,7 +289,6 @@ void pci_mpc8250_init (struct pci_controller *hose)
 					SIUMCR_CS10PC01 |
 					SIUMCR_APPC10;
 #endif
-printf("%s siumcr: %x\n", __FUNCTION__, immap->im_siu_conf.sc_siumcr);
 
 	/* Make PCI lowest priority */
 	/* Each 4 bits is a device bus request	and the MS 4bits
diff --git a/cpu/mpc83xx/cpu.c b/cpu/mpc83xx/cpu.c
index 1b51078..e934ba6 100644
--- a/cpu/mpc83xx/cpu.c
+++ b/cpu/mpc83xx/cpu.c
@@ -30,8 +30,14 @@
 #include <watchdog.h>
 #include <command.h>
 #include <mpc83xx.h>
-#include <ft_build.h>
 #include <asm/processor.h>
+#if defined(CONFIG_OF_FLAT_TREE)
+#include <ft_build.h>
+#endif
+#if defined(CONFIG_OF_LIBFDT)
+#include <libfdt.h>
+#include <libfdt_env.h>
+#endif
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -56,49 +62,78 @@ int checkcpu(void)
 	switch(spridr) {
 	case SPR_8349E_REV10:
 	case SPR_8349E_REV11:
+	case SPR_8349E_REV31:
 		puts("MPC8349E, ");
 		break;
 	case SPR_8349_REV10:
 	case SPR_8349_REV11:
+	case SPR_8349_REV31:
 		puts("MPC8349, ");
 		break;
 	case SPR_8347E_REV10_TBGA:
 	case SPR_8347E_REV11_TBGA:
+	case SPR_8347E_REV31_TBGA:
 	case SPR_8347E_REV10_PBGA:
 	case SPR_8347E_REV11_PBGA:
+	case SPR_8347E_REV31_PBGA:
 		puts("MPC8347E, ");
 		break;
 	case SPR_8347_REV10_TBGA:
 	case SPR_8347_REV11_TBGA:
+	case SPR_8347_REV31_TBGA:
 	case SPR_8347_REV10_PBGA:
 	case SPR_8347_REV11_PBGA:
+	case SPR_8347_REV31_PBGA:
 		puts("MPC8347, ");
 		break;
 	case SPR_8343E_REV10:
 	case SPR_8343E_REV11:
+	case SPR_8343E_REV31:
 		puts("MPC8343E, ");
 		break;
 	case SPR_8343_REV10:
 	case SPR_8343_REV11:
+	case SPR_8343_REV31:
 		puts("MPC8343, ");
 		break;
 	case SPR_8360E_REV10:
 	case SPR_8360E_REV11:
 	case SPR_8360E_REV12:
+	case SPR_8360E_REV20:
 		puts("MPC8360E, ");
 		break;
 	case SPR_8360_REV10:
 	case SPR_8360_REV11:
 	case SPR_8360_REV12:
+	case SPR_8360_REV20:
 		puts("MPC8360, ");
 		break;
+	case SPR_8323E_REV10:
+	case SPR_8323E_REV11:
+		puts("MPC8323E, ");
+		break;
+	case SPR_8323_REV10:
+	case SPR_8323_REV11:
+		puts("MPC8323, ");
+		break;
+	case SPR_8321E_REV10:
+	case SPR_8321E_REV11:
+		puts("MPC8321E, ");
+		break;
+	case SPR_8321_REV10:
+	case SPR_8321_REV11:
+		puts("MPC8321, ");
+		break;
 	default:
-		puts("Rev: Unknown\n");
-		return -1;	/* Not sure what this is */
+		puts("Rev: Unknown revision number.\nWarning: Unsupported cpu revision!\n");
+		return 0;
 	}
 
-#if defined(CONFIG_MPC8349)
-	printf("Rev: %02x at %s MHz\n", (spridr & 0x0000FFFF)>>4 |(spridr & 0x0000000F), strmhz(buf, clock));
+#if defined(CONFIG_MPC834X)
+	/* Multiple revisons of 834x processors may have the same SPRIDR value.
+	 * So use PVR to identify the revision number.
+	 */
+	printf("Rev: %02x at %s MHz\n", PVR_MAJ(pvr)<<4 | PVR_MIN(pvr), strmhz(buf, clock));
 #else
 	printf("Rev: %02x at %s MHz\n", spridr & 0x0000FFFF, strmhz(buf, clock));
 #endif
@@ -250,7 +285,6 @@ unsigned long get_tbclk(void)
 #if defined(CONFIG_WATCHDOG)
 void watchdog_reset (void)
 {
-#ifdef CONFIG_MPC834X
 	int re_enable = disable_interrupts();
 
 	/* Reset the 83xx watchdog */
@@ -260,9 +294,182 @@ void watchdog_reset (void)
 
 	if (re_enable)
 		enable_interrupts ();
-#else
-	hang();
+}
 #endif
+
+#if defined(CONFIG_OF_LIBFDT)
+
+/*
+ * "Setter" functions used to add/modify FDT entries.
+ */
+static int fdt_set_eth0(void *fdt, 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);
+	}
+	return -FDT_ERR_NOTFOUND;
+}
+#ifdef CONFIG_HAS_ETH1
+/* second onboard ethernet port */
+static int fdt_set_eth1(void *fdt, 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);
+	}
+	return -FDT_ERR_NOTFOUND;
+}
+#endif
+#ifdef CONFIG_HAS_ETH2
+/* third onboard ethernet port */
+static int fdt_set_eth2(void *fdt, 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);
+	}
+	return -FDT_ERR_NOTFOUND;
+}
+#endif
+#ifdef CONFIG_HAS_ETH3
+/* fourth onboard ethernet port */
+static int fdt_set_eth3(void *fdt, 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);
+	}
+	return -FDT_ERR_NOTFOUND;
+}
+#endif
+
+static int fdt_set_busfreq(void *fdt, 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));
+}
+
+/*
+ * 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.
+ */
+static const struct {
+	char *node;
+	char *prop;
+	int (*set_fn)(void *fdt, int nodeoffset, const char *name, bd_t *bd);
+} fixup_props[] = {
+	{	"/cpus/" OF_CPU,
+		 "bus-frequency",
+		fdt_set_busfreq
+	},
+	{	"/cpus/" OF_SOC,
+		"bus-frequency",
+		fdt_set_busfreq
+	},
+	{	"/" OF_SOC "/serial@4500/",
+		"clock-frequency",
+		fdt_set_busfreq
+	},
+	{	"/" OF_SOC "/serial@4600/",
+		"clock-frequency",
+		fdt_set_busfreq
+	},
+#ifdef CONFIG_MPC83XX_TSEC1
+	{	"/" OF_SOC "/ethernet@24000,
+		"mac-address",
+		fdt_set_eth0
+	},
+	{	"/" OF_SOC "/ethernet@24000,
+		"local-mac-address",
+		fdt_set_eth0
+	},
+#endif
+#ifdef CONFIG_MPC83XX_TSEC2
+	{	"/" OF_SOC "/ethernet@25000,
+		"mac-address",
+		fdt_set_eth1
+	},
+	{	"/" 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",
+		"mac-address",
+		fdt_set_eth0
+	},
+	{	"/" OF_QE "/ucc@2000/mac-address",
+		"local-mac-address",
+		fdt_set_eth0
+	},
+#elif CFG_UEC1_UCC_NUM == 2  /* UCC3 */
+	{	"/" OF_QE "/ucc@2200/mac-address",
+		"mac-address",
+		fdt_set_eth0
+	},
+	{	"/" OF_QE "/ucc@2200/mac-address",
+		"local-mac-address",
+		fdt_set_eth0
+	},
+#endif
+#endif
+#ifdef CONFIG_UEC_ETH2
+#if CFG_UEC2_UCC_NUM == 1  /* UCC2 */
+	{	"/" OF_QE "/ucc@3000/mac-address",
+		"mac-address",
+		fdt_set_eth1
+	},
+	{	"/" OF_QE "/ucc@3000/mac-address",
+		"local-mac-address",
+		fdt_set_eth1
+	},
+#elif CFG_UEC1_UCC_NUM == 3  /* UCC4 */
+	{	"/" OF_QE "/ucc@3200/mac-address",
+		"mac-address",
+		fdt_set_eth1
+	},
+	{	"/" OF_QE "/ucc@3200/mac-address",
+		"local-mac-address",
+		fdt_set_eth1
+	},
+#endif
+#endif
+};
+
+void
+ft_cpu_setup(void *blob, bd_t *bd)
+{
+	int  nodeoffset;
+	int  err;
+	int  j;
+
+	for (j = 0; j < (sizeof(fixup_props) / sizeof(fixup_props[0])); j++) {
+		nodeoffset = fdt_path_offset(fdt, fixup_props[j].node);
+		if (nodeoffset >= 0) {
+			err = (*fixup_props[j].set_fn)(blob, nodeoffset, fixup_props[j].prop, bd);
+			if (err < 0)
+				printf("set_fn/libfdt: %s %s returned %s\n",
+					fixup_props[j].node,
+					fixup_props[j].prop,
+					fdt_strerror(err));
+		}
+	}
 }
 #endif
 
@@ -292,13 +499,63 @@ ft_cpu_setup(void *blob, bd_t *bd)
 		*p = cpu_to_be32(clock);
 
 #ifdef CONFIG_MPC83XX_TSEC1
+	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@24000/mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enetaddr, 6);
+
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@24000/local-mac-address", &len);
+	if (p != NULL)
 		memcpy(p, bd->bi_enetaddr, 6);
 #endif
 
 #ifdef CONFIG_MPC83XX_TSEC2
+	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@25000/mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enet1addr, 6);
+
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@25000/local-mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enet1addr, 6);
+#endif
+
+#ifdef CONFIG_UEC_ETH1
+#if CFG_UEC1_UCC_NUM == 0  /* UCC1 */
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2000/mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enetaddr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2000/local-mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enetaddr, 6);
+#elif CFG_UEC1_UCC_NUM == 2  /* UCC3 */
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2200/mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enetaddr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2200/local-mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enetaddr, 6);
+#endif
+#endif
+
+#ifdef CONFIG_UEC_ETH2
+#if CFG_UEC2_UCC_NUM == 1  /* UCC2 */
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3000/mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enet1addr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3000/local-mac-address", &len);
+	if (p != NULL)
 		memcpy(p, bd->bi_enet1addr, 6);
+#elif CFG_UEC2_UCC_NUM == 3  /* UCC4 */
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3200/mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enet1addr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3200/local-mac-address", &len);
+	if (p != NULL)
+		memcpy(p, bd->bi_enet1addr, 6);
+#endif
 #endif
 }
 #endif
diff --git a/cpu/mpc83xx/cpu_init.c b/cpu/mpc83xx/cpu_init.c
index e5725fb..3ac9161 100644
--- a/cpu/mpc83xx/cpu_init.c
+++ b/cpu/mpc83xx/cpu_init.c
@@ -69,31 +69,53 @@ void cpu_init_f (volatile immap_t * im)
 
 #ifdef CFG_ACR_PIPE_DEP
 	/* Arbiter pipeline depth */
-	im->arbiter.acr = (im->arbiter.acr & ~ACR_PIPE_DEP) | (3 << ACR_PIPE_DEP_SHIFT);
+	im->arbiter.acr = (im->arbiter.acr & ~ACR_PIPE_DEP) |
+			  (CFG_ACR_PIPE_DEP << ACR_PIPE_DEP_SHIFT);
 #endif
 
 #ifdef CFG_SPCR_TSEC1EP
 	/* TSEC1 Emergency priority */
-	im->sysconf.spcr = (im->sysconf.spcr & ~SPCR_TSEC1EP) | (3 << SPCR_TSEC1EP_SHIFT);
+	im->sysconf.spcr = (im->sysconf.spcr & ~SPCR_TSEC1EP) | (CFG_SPCR_TSEC1EP << SPCR_TSEC1EP_SHIFT);
 #endif
 
 #ifdef CFG_SPCR_TSEC2EP
 	/* TSEC2 Emergency priority */
-	im->sysconf.spcr = (im->sysconf.spcr & ~SPCR_TSEC2EP) | (3 << SPCR_TSEC2EP_SHIFT);
+	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) | (1 << SCCR_TSEC1CM_SHIFT);
+	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) | (1 << SCCR_TSEC2CM_SHIFT);
+	im->clk.sccr = (im->clk.sccr & ~SCCR_TSEC2CM) | (CFG_SCCR_TSEC2CM << SCCR_TSEC2CM_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 */
+	im->clk.sccr = (im->clk.sccr & ~SCCR_PCICM) | (CFG_SCCR_PCICM << SCCR_PCICM_SHIFT);
+#endif
+
+#ifdef CFG_SCCR_USBDRCM
+	/* USB DR clock mode */
+	im->clk.sccr = (im->clk.sccr & ~SCCR_USBDRCM) | (CFG_SCCR_USBDRCM << SCCR_USBDRCM_SHIFT);
+#endif
+
+#ifdef CFG_SCCR_ENCCM
+	/* Encryption clock mode */
+	im->clk.sccr = (im->clk.sccr & ~SCCR_ENCCM) | (CFG_SCCR_ENCCM << SCCR_PCICM_SHIFT);
 #endif
 
 #ifdef CFG_ACR_RPTCNT
 	/* Arbiter repeat count */
-	im->arbiter.acr = ((im->arbiter.acr & ~(ACR_RPTCNT)) | (3 << ACR_RPTCNT_SHIFT));
+	im->arbiter.acr = ((im->arbiter.acr & ~(ACR_RPTCNT)) | (CFG_ACR_RPTCNT << ACR_RPTCNT_SHIFT));
 #endif
 
 	/* RSR - Reset Status Register - clear all status (4.6.1.3) */
@@ -119,6 +141,11 @@ void cpu_init_f (volatile immap_t * im)
 #ifdef CFG_SICRL
 	im->sysconf.sicrl = CFG_SICRL;
 #endif
+	/* DDR control driver register */
+#ifdef CFG_DDRCDR
+	im->sysconf.ddrcdr = CFG_DDRCDR;
+#endif
+
 #ifdef CONFIG_QE
 	/* Config QE ioports */
 	config_qe_ioports();
@@ -202,12 +229,12 @@ void cpu_init_f (volatile immap_t * im)
 	im->sysconf.lblaw[7].ar = CFG_LBLAWAR7_PRELIM;
 #endif
 #ifdef CFG_GPIO1_PRELIM
-	im->pgio[0].dir = CFG_GPIO1_DIR;
-	im->pgio[0].dat = CFG_GPIO1_DAT;
+	im->gpio[0].dir = CFG_GPIO1_DIR;
+	im->gpio[0].dat = CFG_GPIO1_DAT;
 #endif
 #ifdef CFG_GPIO2_PRELIM
-	im->pgio[1].dir = CFG_GPIO2_DIR;
-	im->pgio[1].dat = CFG_GPIO2_DAT;
+	im->gpio[1].dir = CFG_GPIO2_DIR;
+	im->gpio[1].dat = CFG_GPIO2_DAT;
 #endif
 }
 
diff --git a/cpu/mpc83xx/qe_io.c b/cpu/mpc83xx/qe_io.c
index ebe3487..8b3937a 100644
--- a/cpu/mpc83xx/qe_io.c
+++ b/cpu/mpc83xx/qe_io.c
@@ -35,7 +35,7 @@ void qe_config_iopin(u8 port, u8 pin, int dir, int open_drain, int assign)
 	u32			pin_1bit_mask;
 	u32			tmp_val;
 	volatile immap_t	*im = (volatile immap_t *)CFG_IMMR;
-	volatile gpio83xx_t	*par_io =(volatile gpio83xx_t *)&im->gpio;
+	volatile qepio83xx_t	*par_io = (volatile qepio83xx_t *)&im->qepio;
 
 	/* Caculate pin location and 2bit mask and dir */
 	pin_2bit_mask = (u32)(0x3 << (NUM_OF_PINS-(pin%(NUM_OF_PINS/2)+1)*2));
diff --git a/cpu/mpc83xx/spd_sdram.c b/cpu/mpc83xx/spd_sdram.c
index 0d93f2e..d9b8753 100644
--- a/cpu/mpc83xx/spd_sdram.c
+++ b/cpu/mpc83xx/spd_sdram.c
@@ -106,16 +106,29 @@ long int spd_sdram()
 	volatile ddr83xx_t *ddr = &immap->ddr;
 	volatile law83xx_t *ecm = &immap->sysconf.ddrlaw[0];
 	spd_eeprom_t spd;
+	unsigned int n_ranks;
+	unsigned int odt_rd_cfg, odt_wr_cfg;
+	unsigned char twr_clk, twtr_clk;
+	unsigned char sdram_type;
 	unsigned int memsize;
 	unsigned int law_size;
 	unsigned char caslat, caslat_ctrl;
+	unsigned int trfc, trfc_clk, trfc_low, trfc_high;
+	unsigned int trcd_clk, trtp_clk;
+	unsigned char cke_min_clk;
+	unsigned char add_lat, wr_lat;
+	unsigned char wr_data_delay;
+	unsigned char four_act;
+	unsigned char cpo;
 	unsigned char burstlen;
+	unsigned char odt_cfg, mode_odt_enable;
 	unsigned int max_bus_clk;
 	unsigned int max_data_rate, effective_data_rate;
 	unsigned int ddrc_clk;
 	unsigned int refresh_clk;
-	unsigned sdram_cfg;
+	unsigned int sdram_cfg;
 	unsigned int ddrc_ecc_enable;
+	unsigned int pvr = get_pvr();
 
 	/* Read SPD parameters with I2C */
 	CFG_READ_SPD(SPD_EEPROM_ADDRESS, 0, 1, (uchar *) & spd, sizeof (spd));
@@ -123,19 +136,25 @@ long int spd_sdram()
 	spd_debug(&spd);
 #endif
 	/* Check the memory type */
-	if (spd.mem_type != SPD_MEMTYPE_DDR) {
+	if (spd.mem_type != SPD_MEMTYPE_DDR && spd.mem_type != SPD_MEMTYPE_DDR2) {
 		printf("DDR: Module mem type is %02X\n", spd.mem_type);
 		return 0;
 	}
 
 	/* Check the number of physical bank */
-	if (spd.nrows > 2) {
-		printf("DDR: The number of physical bank is %02X\n", spd.nrows);
+	if (spd.mem_type == SPD_MEMTYPE_DDR) {
+		n_ranks = spd.nrows;
+	} else {
+		n_ranks = (spd.nrows & 0x7) + 1;
+	}
+
+	if (n_ranks > 2) {
+		printf("DDR: The number of physical bank is %02X\n", n_ranks);
 		return 0;
 	}
 
 	/* Check if the number of row of the module is in the range of DDRC */
-	if (spd.nrow_addr < 12 || spd.nrow_addr > 14) {
+	if (spd.nrow_addr < 12 || spd.nrow_addr > 15) {
 		printf("DDR: Row number is out of range of DDRC, row=%02X\n",
 							 spd.nrow_addr);
 		return 0;
@@ -147,20 +166,43 @@ long int spd_sdram()
 							 spd.ncol_addr);
 		return 0;
 	}
+
+#ifdef CFG_DDRCDR_VALUE
+	/*
+	 * Adjust DDR II IO voltage biasing.  It just makes it work.
+	 */
+	if(spd.mem_type == SPD_MEMTYPE_DDR2) {
+		immap->sysconf.ddrcdr = CFG_DDRCDR_VALUE;
+	}
+#endif
+
+	/*
+	 * ODT configuration recommendation from DDR Controller Chapter.
+	 */
+	odt_rd_cfg = 0;			/* Never assert ODT */
+	odt_wr_cfg = 0;			/* Never assert ODT */
+	if (spd.mem_type == SPD_MEMTYPE_DDR2) {
+		odt_wr_cfg = 1;		/* Assert ODT on writes to CSn */
+	}
+
 	/* Setup DDR chip select register */
 #ifdef CFG_83XX_DDR_USES_CS0
 	ddr->csbnds[0].csbnds = (banksize(spd.row_dens) >> 24) - 1;
 	ddr->cs_config[0] = ( 1 << 31
+			    | (odt_rd_cfg << 20)
+			    | (odt_wr_cfg << 16)
 			    | (spd.nrow_addr - 12) << 8
 			    | (spd.ncol_addr - 8) );
 	debug("\n");
 	debug("cs0_bnds = 0x%08x\n",ddr->csbnds[0].csbnds);
 	debug("cs0_config = 0x%08x\n",ddr->cs_config[0]);
 
-	if (spd.nrows == 2) {
+	if (n_ranks == 2) {
 		ddr->csbnds[1].csbnds = ( (banksize(spd.row_dens) >> 8)
 				  | ((banksize(spd.row_dens) >> 23) - 1) );
 		ddr->cs_config[1] = ( 1<<31
+				    | (odt_rd_cfg << 20)
+				    | (odt_wr_cfg << 16)
 				    | (spd.nrow_addr-12) << 8
 				    | (spd.ncol_addr-8) );
 		debug("cs1_bnds = 0x%08x\n",ddr->csbnds[1].csbnds);
@@ -170,16 +212,20 @@ long int spd_sdram()
 #else
 	ddr->csbnds[2].csbnds = (banksize(spd.row_dens) >> 24) - 1;
 	ddr->cs_config[2] = ( 1 << 31
+			    | (odt_rd_cfg << 20)
+			    | (odt_wr_cfg << 16)
 			    | (spd.nrow_addr - 12) << 8
 			    | (spd.ncol_addr - 8) );
 	debug("\n");
 	debug("cs2_bnds = 0x%08x\n",ddr->csbnds[2].csbnds);
 	debug("cs2_config = 0x%08x\n",ddr->cs_config[2]);
 
-	if (spd.nrows == 2) {
+	if (n_ranks == 2) {
 		ddr->csbnds[3].csbnds = ( (banksize(spd.row_dens) >> 8)
 				  | ((banksize(spd.row_dens) >> 23) - 1) );
 		ddr->cs_config[3] = ( 1<<31
+				    | (odt_rd_cfg << 20)
+				    | (odt_wr_cfg << 16)
 				    | (spd.nrow_addr-12) << 8
 				    | (spd.ncol_addr-8) );
 		debug("cs3_bnds = 0x%08x\n",ddr->csbnds[3].csbnds);
@@ -187,15 +233,10 @@ long int spd_sdram()
 	}
 #endif
 
-	if (spd.mem_type != 0x07) {
-		puts("No DDR module found!\n");
-		return 0;
-	}
-
 	/*
 	 * Figure out memory size in Megabytes.
 	 */
-	memsize = spd.nrows * banksize(spd.row_dens) / 0x100000;
+	memsize = n_ranks * banksize(spd.row_dens) / 0x100000;
 
 	/*
 	 * First supported LAW size is 16M, at LAWAR_SIZE_16M == 23.
@@ -215,24 +256,32 @@ long int spd_sdram()
 	 * in the spd.cas_lat field.  Translate it to a DDR
 	 * controller field value:
 	 *
-	 *	CAS Lat	 DDR I	   Ctrl
-	 *	Clocks	 SPD Bit   Value
-	 *	-------+--------+---------
-	 *	1.0	   0	    001
-	 *	1.5	   1	    010
-	 *	2.0	   2	    011
-	 *	2.5	   3	    100
-	 *	3.0	   4	    101
-	 *	3.5	   5	    110
-	 *	4.0	   6	    111
+	 *	CAS Lat	DDR I	DDR II	Ctrl
+	 *	Clocks	SPD Bit	SPD Bit	Value
+	 *	-------	-------	-------	-----
+	 *	1.0	0		0001
+	 *	1.5	1		0010
+	 *	2.0	2	2	0011
+	 *	2.5	3		0100
+	 *	3.0	4	3	0101
+	 *	3.5	5		0110
+	 *	4.0	6	4	0111
+	 *	4.5			1000
+	 *	5.0		5	1001
 	 */
 	caslat = __ilog2(spd.cas_lat);
-
-	if (caslat > 6 ) {
-		printf("DDR: Invalid SPD CAS Latency, caslat=%02X\n",
-			spd.cas_lat);
+	if ((spd.mem_type == SPD_MEMTYPE_DDR)
+	    && (caslat > 6)) {
+		printf("DDR I: Invalid SPD CAS Latency: 0x%x.\n", spd.cas_lat);
+		return 0;
+	} else if (spd.mem_type == SPD_MEMTYPE_DDR2
+		   && (caslat < 2 || caslat > 5)) {
+		printf("DDR II: Invalid SPD CAS Latency: 0x%x.\n",
+		       spd.cas_lat);
 		return 0;
 	}
+	debug("DDR: caslat SPD bit is %d\n", caslat);
+
 	max_bus_clk = 1000 *10 / (((spd.clk_cycle & 0xF0) >> 4) * 10
 			+ (spd.clk_cycle & 0x0f));
 	max_data_rate = max_bus_clk * 2;
@@ -240,10 +289,11 @@ long int spd_sdram()
 	debug("DDR:Module maximum data rate is: %dMhz\n", max_data_rate);
 
 	ddrc_clk = gd->ddr_clk / 1000000;
+	effective_data_rate = 0;
 
-	if (max_data_rate >= 390) { /* it is DDR 400 */
-		if (ddrc_clk <= 410 && ddrc_clk > 350) {
-			/* DDR controller clk at 350~410 */
+	if (max_data_rate >= 390 && max_data_rate < 460) { /* it is DDR 400 */
+		if (ddrc_clk <= 460 && ddrc_clk > 350) {
+			/* DDR controller clk at 350~460 */
 			effective_data_rate = 400; /* 5ns */
 			caslat = caslat;
 		} else if (ddrc_clk <= 350 && ddrc_clk > 280) {
@@ -258,16 +308,16 @@ long int spd_sdram()
 			effective_data_rate = 266; /* 7.5ns */
 			if (spd.clk_cycle3 == 0x75)
 				caslat = caslat - 2;
-			else if (spd.clk_cycle2 == 0x60)
+			else if (spd.clk_cycle2 == 0x75)
 				caslat = caslat - 1;
 			else
 				caslat = caslat;
 		} else if (ddrc_clk <= 230 && ddrc_clk > 90) {
 			/* DDR controller clk at 90~230 */
 			effective_data_rate = 200; /* 10ns */
-			if (spd.clk_cycle3 == 0x75)
+			if (spd.clk_cycle3 == 0xa0)
 				caslat = caslat - 2;
-			else if (spd.clk_cycle2 == 0x60)
+			else if (spd.clk_cycle2 == 0xa0)
 				caslat = caslat - 1;
 			else
 				caslat = caslat;
@@ -289,7 +339,7 @@ long int spd_sdram()
 			effective_data_rate = 200; /* 10ns */
 			if (spd.clk_cycle3 == 0xa0)
 				caslat = caslat - 2;
-			else if (spd.clk_cycle2 == 0x75)
+			else if (spd.clk_cycle2 == 0xa0)
 				caslat = caslat - 1;
 			else
 				caslat = caslat;
@@ -330,41 +380,197 @@ long int spd_sdram()
 	 * Errata DDR6 work around: input enable 2 cycles earlier.
 	 * including MPC834x Rev1.0/1.1 and MPC8360 Rev1.1/1.2.
 	 */
-	if (caslat == 2)
-		ddr->debug_reg = 0x201c0000; /* CL=2 */
-	else if (caslat == 3)
-		ddr->debug_reg = 0x202c0000; /* CL=2.5 */
-	else if (caslat == 4)
-		ddr->debug_reg = 0x202c0000; /* CL=3.0 */
+	if(PVR_MAJ(pvr) <= 1 && spd.mem_type == SPD_MEMTYPE_DDR){
+		if (caslat == 2)
+			ddr->debug_reg = 0x201c0000; /* CL=2 */
+		else if (caslat == 3)
+			ddr->debug_reg = 0x202c0000; /* CL=2.5 */
+		else if (caslat == 4)
+			ddr->debug_reg = 0x202c0000; /* CL=3.0 */
 
-	__asm__ __volatile__ ("sync");
+		__asm__ __volatile__ ("sync");
 
-	debug("Errata DDR6 (debug_reg=0x%08x)\n", ddr->debug_reg);
+		debug("Errata DDR6 (debug_reg=0x%08x)\n", ddr->debug_reg);
+	}
 
 	/*
-	 * note: caslat must also be programmed into ddr->sdram_mode
-	 * register.
-	 *
-	 * note: WRREC(Twr) and WRTORD(Twtr) are not in SPD,
-	 * use conservative value here.
+	 * Convert caslat clocks to DDR controller value.
+	 * Force caslat_ctrl to be DDR Controller field-sized.
+	 */
+	if (spd.mem_type == SPD_MEMTYPE_DDR) {
+		caslat_ctrl = (caslat + 1) & 0x07;
+	} else {
+		caslat_ctrl =  (2 * caslat - 1) & 0x0f;
+	}
+
+	debug("DDR: effective data rate is %d MHz\n", effective_data_rate);
+	debug("DDR: caslat SPD bit is %d, controller field is 0x%x\n",
+	      caslat, caslat_ctrl);
+
+	/*
+	 * Timing Config 0.
+	 * Avoid writing for DDR I.
+	 */
+	if (spd.mem_type == SPD_MEMTYPE_DDR2) {
+		unsigned char taxpd_clk = 8;		/* By the book. */
+		unsigned char tmrd_clk = 2;		/* By the book. */
+		unsigned char act_pd_exit = 2;		/* Empirical? */
+		unsigned char pre_pd_exit = 6;		/* Empirical? */
+
+		ddr->timing_cfg_0 = (0
+			| ((act_pd_exit & 0x7) << 20)	/* ACT_PD_EXIT */
+			| ((pre_pd_exit & 0x7) << 16)	/* PRE_PD_EXIT */
+			| ((taxpd_clk & 0xf) << 8)	/* ODT_PD_EXIT */
+			| ((tmrd_clk & 0xf) << 0)	/* MRS_CYC */
+			);
+		debug("DDR: timing_cfg_0 = 0x%08x\n", ddr->timing_cfg_0);
+	}
+
+	/*
+	 * For DDR I, WRREC(Twr) and WRTORD(Twtr) are not in SPD,
+	 * use conservative value.
+	 * For DDR II, they are bytes 36 and 37, in quarter nanos.
 	 */
-	caslat_ctrl = (caslat + 1) & 0x07; /* see as above */
+
+	if (spd.mem_type == SPD_MEMTYPE_DDR) {
+		twr_clk = 3;	/* Clocks */
+		twtr_clk = 1;	/* Clocks */
+	} else {
+		twr_clk = picos_to_clk(spd.twr * 250);
+		twtr_clk = picos_to_clk(spd.twtr * 250);
+	}
+
+	/*
+	 * Calculate Trfc, in picos.
+	 * DDR I:  Byte 42 straight up in ns.
+	 * DDR II: Byte 40 and 42 swizzled some, in ns.
+	 */
+	if (spd.mem_type == SPD_MEMTYPE_DDR) {
+		trfc = spd.trfc * 1000;		/* up to ps */
+	} else {
+		unsigned int byte40_table_ps[8] = {
+			0,
+			250,
+			330,
+			500,
+			660,
+			750,
+			0,
+			0
+		};
+
+		trfc = (((spd.trctrfc_ext & 0x1) * 256) + spd.trfc) * 1000
+			+ byte40_table_ps[(spd.trctrfc_ext >> 1) & 0x7];
+	}
+	trfc_clk = picos_to_clk(trfc);
+
+	/*
+	 * Trcd, Byte 29, from quarter nanos to ps and clocks.
+	 */
+	trcd_clk = picos_to_clk(spd.trcd * 250) & 0x7;
+
+	/*
+	 * Convert trfc_clk to DDR controller fields.  DDR I should
+	 * fit in the REFREC field (16-19) of TIMING_CFG_1, but the
+	 * 83xx controller has an extended REFREC field of three bits.
+	 * The controller automatically adds 8 clocks to this value,
+	 * so preadjust it down 8 first before splitting it up.
+	 */
+	trfc_low = (trfc_clk - 8) & 0xf;
+	trfc_high = ((trfc_clk - 8) >> 4) & 0x3;
 
 	ddr->timing_cfg_1 =
-	    (((picos_to_clk(spd.trp * 250) & 0x07) << 28 ) |
-	     ((picos_to_clk(spd.tras * 1000) & 0x0f ) << 24 ) |
-	     ((picos_to_clk(spd.trcd * 250) & 0x07) << 20 ) |
-	     ((caslat_ctrl & 0x07) << 16 ) |
-	     (((picos_to_clk(spd.trfc * 1000) - 8) & 0x0f) << 12 ) |
-	     ( 0x300 ) |
-	     ((picos_to_clk(spd.trrd * 250) & 0x07) << 4) | 1);
+	    (((picos_to_clk(spd.trp * 250) & 0x07) << 28 ) |	/* PRETOACT */
+	     ((picos_to_clk(spd.tras * 1000) & 0x0f ) << 24 ) | /* ACTTOPRE */
+	     (trcd_clk << 20 ) |  				/* ACTTORW */
+	     (caslat_ctrl << 16 ) |				/* CASLAT */
+	     (trfc_low << 12 ) |				/* REFEC */
+	     ((twr_clk & 0x07) << 8) |				/* WRRREC */
+	     ((picos_to_clk(spd.trrd * 250) & 0x07) << 4) |	/* ACTTOACT */
+	     ((twtr_clk & 0x07) << 0)				/* WRTORD */
+	    );
+
+	/*
+	 * Additive Latency
+	 * For DDR I, 0.
+	 * For DDR II, with ODT enabled, use "a value" less than ACTTORW,
+	 * which comes from Trcd, and also note that:
+	 *	add_lat + caslat must be >= 4
+	 */
+	add_lat = 0;
+	if (spd.mem_type == SPD_MEMTYPE_DDR2
+	    && (odt_wr_cfg || odt_rd_cfg)
+	    && (caslat < 4)) {
+		add_lat = trcd_clk - 1;
+		if ((add_lat + caslat) < 4) {
+			add_lat = 0;
+		}
+	}
 
-	ddr->timing_cfg_2 = 0x00000800;
+	/*
+	 * Write Data Delay
+	 * Historically 0x2 == 4/8 clock delay.
+	 * Empirically, 0x3 == 6/8 clock delay is suggested for DDR I 266.
+	 */
+	wr_data_delay = 2;
+
+	/*
+	 * Write Latency
+	 * Read to Precharge
+	 * Minimum CKE Pulse Width.
+	 * Four Activate Window
+	 */
+	if (spd.mem_type == SPD_MEMTYPE_DDR) {
+		/*
+		 * This is a lie.  It should really be 1, but if it is
+		 * set to 1, bits overlap into the old controller's
+		 * otherwise unused ACSM field.  If we leave it 0, then
+		 * the HW will magically treat it as 1 for DDR 1.  Oh Yea.
+		 */
+		wr_lat = 0;
+
+		trtp_clk = 2;		/* By the book. */
+		cke_min_clk = 1;	/* By the book. */
+		four_act = 1;		/* By the book. */
+
+	} else {
+		wr_lat = caslat - 1;
+
+		/* Convert SPD value from quarter nanos to picos. */
+		trtp_clk = picos_to_clk(spd.trtp * 250);
+
+		cke_min_clk = 3;	/* By the book. */
+		four_act = picos_to_clk(37500);	/* By the book. 1k pages? */
+	}
+
+	/*
+	 * Empirically set ~MCAS-to-preamble override for DDR 2.
+	 * Your milage will vary.
+	 */
+	cpo = 0;
+	if (spd.mem_type == SPD_MEMTYPE_DDR2) {
+		if (effective_data_rate == 266 || effective_data_rate == 333) {
+			cpo = 0x7;		/* READ_LAT + 5/4 */
+		} else if (effective_data_rate == 400) {
+			cpo = 0x9;		/* READ_LAT + 7/4 */
+		} else {
+			/* Automatic calibration */
+			cpo = 0x1f;
+		}
+	}
+
+	ddr->timing_cfg_2 = (0
+		| ((add_lat & 0x7) << 28)		/* ADD_LAT */
+		| ((cpo & 0x1f) << 23)			/* CPO */
+		| ((wr_lat & 0x7) << 19)		/* WR_LAT */
+		| ((trtp_clk & 0x7) << 13)		/* RD_TO_PRE */
+		| ((wr_data_delay & 0x7) << 10)		/* WR_DATA_DELAY */
+		| ((cke_min_clk & 0x7) << 6)		/* CKE_PLS */
+		| ((four_act & 0x1f) << 0)		/* FOUR_ACT */
+		);
 
 	debug("DDR:timing_cfg_1=0x%08x\n", ddr->timing_cfg_1);
 	debug("DDR:timing_cfg_2=0x%08x\n", ddr->timing_cfg_2);
-	/* Setup init value, but not enable */
-	ddr->sdram_cfg = 0x42000000;
 
 	/* Check DIMM data bus width */
 	if (spd.dataw_lsb == 0x20) {
@@ -384,7 +590,8 @@ long int spd_sdram()
 	/* Burst length is always 4 for 64 bit data bus, 8 for 32 bit data bus,
 	   Burst type is sequential
 	 */
-	switch (caslat) {
+	if (spd.mem_type == SPD_MEMTYPE_DDR) {
+		switch (caslat) {
 		case 1:
 			ddr->sdram_mode = 0x50 | burstlen; /* CL=1.5 */
 			break;
@@ -400,9 +607,36 @@ long int spd_sdram()
 		default:
 			printf("DDR:only CL 1.5, 2.0, 2.5, 3.0 is supported\n");
 			return 0;
+		}
+	} else {
+		mode_odt_enable = 0x0;                  /* Default disabled */
+		if (odt_wr_cfg || odt_rd_cfg) {
+			/*
+			 * Bits 6 and 2 in Extended MRS(1)
+			 * Bit 2 == 0x04 == 75 Ohm, with 2 DIMM modules.
+			 * Bit 6 == 0x40 == 150 Ohm, with 1 DIMM module.
+			 */
+			mode_odt_enable = 0x40;         /* 150 Ohm */
+		}
+
+		ddr->sdram_mode =
+			(0
+			 | (1 << (16 + 10))             /* DQS Differential disable */
+			 | (add_lat << (16 + 3))        /* Additive Latency in EMRS1 */
+			 | (mode_odt_enable << 16)      /* ODT Enable in EMRS1 */
+			 | ((twr_clk >> 1) << 9)        /* Write Recovery Autopre */
+			 | (caslat << 4)                /* caslat */
+			 | (burstlen << 0)              /* Burst length */
+			);
 	}
 	debug("DDR:sdram_mode=0x%08x\n", ddr->sdram_mode);
 
+	/*
+	 * Clear EMRS2 and EMRS3.
+	 */
+	ddr->sdram_mode2 = 0;
+	debug("DDR: sdram_mode2 = 0x%08x\n", ddr->sdram_mode2);
+
 	switch (spd.refresh) {
 		case 0x00:
 		case 0x80:
@@ -440,10 +674,31 @@ long int spd_sdram()
 	ddr->sdram_interval = ((refresh_clk & 0x3fff) << 16) | 0x100;
 	debug("DDR:sdram_interval=0x%08x\n", ddr->sdram_interval);
 
+	/*
+	 * SDRAM Cfg 2
+	 */
+	odt_cfg = 0;
+	if (odt_rd_cfg | odt_wr_cfg) {
+		odt_cfg = 0x2;		/* ODT to IOs during reads */
+	}
+	if (spd.mem_type == SPD_MEMTYPE_DDR2) {
+		ddr->sdram_cfg2 = (0
+			    | (0 << 26)	/* True DQS */
+			    | (odt_cfg << 21)	/* ODT only read */
+			    | (1 << 12)	/* 1 refresh at a time */
+			    );
+
+		debug("DDR: sdram_cfg2  = 0x%08x\n", ddr->sdram_cfg2);
+	}
+
+#ifdef CFG_DDR_SDRAM_CLK_CNTL	/* Optional platform specific value */
+	ddr->sdram_clk_cntl = CFG_DDR_SDRAM_CLK_CNTL;
+#else
 	/* SS_EN = 0, source synchronous disable
 	 * CLK_ADJST = 0, MCK/MCK# is launched aligned with addr/cmd
 	 */
 	ddr->sdram_clk_cntl = 0x00000000;
+#endif
 	debug("DDR:sdram_clk_cntl=0x%08x\n", ddr->sdram_clk_cntl);
 
 	asm("sync;isync");
@@ -458,11 +713,22 @@ long int spd_sdram()
 	 *
 	 * sdram_cfg[0]   = 1 (ddr sdram logic enable)
 	 * sdram_cfg[1]   = 1 (self-refresh-enable)
-	 * sdram_cfg[6:7] = 2 (SDRAM type = DDR SDRAM)
+	 * sdram_cfg[5:7] = (SDRAM type = DDR SDRAM)
+	 *			010 DDR 1 SDRAM
+	 *			011 DDR 2 SDRAM
 	 * sdram_cfg[12] = 0 (32_BE =0 , 64 bit bus mode)
 	 * sdram_cfg[13] = 0 (8_BE =0, 4-beat bursts)
 	 */
-	sdram_cfg = 0xC2000000;
+	if (spd.mem_type == SPD_MEMTYPE_DDR)
+		sdram_type = 2;
+	else
+		sdram_type = 3;
+
+	sdram_cfg = (0
+		     | (1 << 31)			/* DDR enable */
+		     | (1 << 30)			/* Self refresh */
+		     | (sdram_type << 24)		/* SDRAM type */
+		     );
 
 	/* sdram_cfg[3] = RD_EN - registered DIMM enable */
 	if (spd.mod_attr & 0x02)
diff --git a/cpu/mpc83xx/speed.c b/cpu/mpc83xx/speed.c
index 7e53b1e..c759930 100644
--- a/cpu/mpc83xx/speed.c
+++ b/cpu/mpc83xx/speed.c
@@ -99,7 +99,7 @@ int get_clocks(void)
 	u32 lcrr;
 
 	u32 csb_clk;
-#if defined(CONFIG_MPC8349)
+#if defined(CONFIG_MPC834X)
 	u32 tsec1_clk;
 	u32 tsec2_clk;
 	u32 usbmph_clk;
@@ -107,15 +107,19 @@ int get_clocks(void)
 #endif
 	u32 core_clk;
 	u32 i2c1_clk;
+#if !defined(CONFIG_MPC832X)
 	u32 i2c2_clk;
+#endif
 	u32 enc_clk;
 	u32 lbiu_clk;
 	u32 lclk_clk;
 	u32 ddr_clk;
-#if defined (CONFIG_MPC8360)
+#if defined(CONFIG_MPC8360)
+	u32 ddr_sec_clk;
+#endif
+#if defined(CONFIG_MPC8360) || defined(CONFIG_MPC832X)
 	u32 qepmf;
 	u32 qepdf;
-	u32 ddr_sec_clk;
 	u32 qe_clk;
 	u32 brg_clk;
 #endif
@@ -139,12 +143,12 @@ int get_clocks(void)
 #endif
 	}
 
-	spmf = ((im->reset.rcwl & RCWL_SPMF) >> RCWL_SPMF_SHIFT);
+	spmf = ((im->reset.rcwl & HRCWL_SPMF) >> HRCWL_SPMF_SHIFT);
 	csb_clk = pci_sync_in * (1 + clkin_div) * spmf;
 
 	sccr = im->clk.sccr;
 
-#if defined(CONFIG_MPC8349)
+#if defined(CONFIG_MPC834X)
 	switch ((sccr & SCCR_TSEC1CM) >> SCCR_TSEC1CM_SHIFT) {
 	case 0:
 		tsec1_clk = 0;
@@ -227,10 +231,12 @@ int get_clocks(void)
 		return -9;
 	}
 #endif
-#if defined (CONFIG_MPC8360)
+#if defined(CONFIG_MPC8360) || defined(CONFIG_MPC832X)
 	i2c1_clk = csb_clk;
 #endif
+#if !defined(CONFIG_MPC832X)
 	i2c2_clk = csb_clk;	/* i2c-2 clk is equal to csb clk */
+#endif
 
 	switch ((sccr & SCCR_ENCCM) >> SCCR_ENCCM_SHIFT) {
 	case 0:
@@ -249,12 +255,9 @@ int get_clocks(void)
 		/* unkown SCCR_ENCCM value */
 		return -6;
 	}
-#if defined(CONFIG_MPC8349) || defined(CONFIG_MPC8360)
+
 	lbiu_clk = csb_clk *
-	           (1 + ((im->reset.rcwl & RCWL_LBIUCM) >> RCWL_LBIUCM_SHIFT));
-#else
-#error Unknown MPC83xx chip
-#endif
+	           (1 + ((im->reset.rcwl & HRCWL_LBIUCM) >> HRCWL_LBIUCM_SHIFT));
 	lcrr = (im->lbus.lcrr & LCRR_CLKDIV) >> LCRR_CLKDIV_SHIFT;
 	switch (lcrr) {
 	case 2:
@@ -266,16 +269,13 @@ int get_clocks(void)
 		/* unknown lcrr */
 		return -10;
 	}
-#if defined(CONFIG_MPC8349) || defined(CONFIG_MPC8360)
+
 	ddr_clk = csb_clk *
-		  (1 + ((im->reset.rcwl & RCWL_DDRCM) >> RCWL_DDRCM_SHIFT));
-	corepll = (im->reset.rcwl & RCWL_COREPLL) >> RCWL_COREPLL_SHIFT;
-#if defined (CONFIG_MPC8360)
+		  (1 + ((im->reset.rcwl & HRCWL_DDRCM) >> HRCWL_DDRCM_SHIFT));
+	corepll = (im->reset.rcwl & HRCWL_COREPLL) >> HRCWL_COREPLL_SHIFT;
+#if defined(CONFIG_MPC8360)
 	ddr_sec_clk = csb_clk * (1 +
-		       ((im->reset.rcwl & RCWL_LBIUCM) >> RCWL_LBIUCM_SHIFT));
-#endif
-#else
-#error Unknown MPC83xx chip
+		       ((im->reset.rcwl & HRCWL_LBIUCM) >> HRCWL_LBIUCM_SHIFT));
 #endif
 
 	corecnf_tab_index = ((corepll & 0x1F) << 2) | ((corepll & 0x60) >> 5);
@@ -306,15 +306,15 @@ int get_clocks(void)
 		return -12;
 	}
 
-#if defined (CONFIG_MPC8360)
-	qepmf = (im->reset.rcwl & RCWL_CEPMF) >> RCWL_CEPMF_SHIFT;
-	qepdf = (im->reset.rcwl & RCWL_CEPDF) >> RCWL_CEPDF_SHIFT;
+#if defined(CONFIG_MPC8360) || defined(CONFIG_MPC832X)
+	qepmf = (im->reset.rcwl & HRCWL_CEPMF) >> HRCWL_CEPMF_SHIFT;
+	qepdf = (im->reset.rcwl & HRCWL_CEPDF) >> HRCWL_CEPDF_SHIFT;
 	qe_clk = (pci_sync_in * qepmf) / (1 + qepdf);
 	brg_clk = qe_clk / 2;
 #endif
 
 	gd->csb_clk = csb_clk;
-#if defined(CONFIG_MPC8349)
+#if defined(CONFIG_MPC834X)
 	gd->tsec1_clk = tsec1_clk;
 	gd->tsec2_clk = tsec2_clk;
 	gd->usbmph_clk = usbmph_clk;
@@ -322,13 +322,17 @@ int get_clocks(void)
 #endif
 	gd->core_clk = core_clk;
 	gd->i2c1_clk = i2c1_clk;
+#if !defined(CONFIG_MPC832X)
 	gd->i2c2_clk = i2c2_clk;
+#endif
 	gd->enc_clk = enc_clk;
 	gd->lbiu_clk = lbiu_clk;
 	gd->lclk_clk = lclk_clk;
 	gd->ddr_clk = ddr_clk;
-#if defined (CONFIG_MPC8360)
+#if defined(CONFIG_MPC8360)
 	gd->ddr_sec_clk = ddr_sec_clk;
+#endif
+#if defined(CONFIG_MPC8360) || defined(CONFIG_MPC832X)
 	gd->qe_clk = qe_clk;
 	gd->brg_clk = brg_clk;
 #endif
@@ -352,19 +356,22 @@ int print_clock_conf(void)
 	printf("Clock configuration:\n");
 	printf("  Coherent System Bus: %4d MHz\n", gd->csb_clk / 1000000);
 	printf("  Core:                %4d MHz\n", gd->core_clk / 1000000);
-#if defined (CONFIG_MPC8360)
+#if defined(CONFIG_MPC8360) || defined(CONFIG_MPC832X)
 	printf("  QE:                  %4d MHz\n", gd->qe_clk / 1000000);
+	printf("  BRG:                 %4d MHz\n", gd->brg_clk / 1000000);
 #endif
 	printf("  Local Bus Controller:%4d MHz\n", gd->lbiu_clk / 1000000);
 	printf("  Local Bus:           %4d MHz\n", gd->lclk_clk / 1000000);
 	printf("  DDR:                 %4d MHz\n", gd->ddr_clk / 1000000);
-#if defined (CONFIG_MPC8360)
+#if defined(CONFIG_MPC8360)
 	printf("  DDR Secondary:       %4d MHz\n", gd->ddr_sec_clk / 1000000);
 #endif
 	printf("  SEC:                 %4d MHz\n", gd->enc_clk / 1000000);
 	printf("  I2C1:                %4d MHz\n", gd->i2c1_clk / 1000000);
+#if !defined(CONFIG_MPC832X)
 	printf("  I2C2:                %4d MHz\n", gd->i2c2_clk / 1000000);
-#if defined(CONFIG_MPC8349)
+#endif
+#if defined(CONFIG_MPC834X)
 	printf("  TSEC1:               %4d MHz\n", gd->tsec1_clk / 1000000);
 	printf("  TSEC2:               %4d MHz\n", gd->tsec2_clk / 1000000);
 	printf("  USB MPH:             %4d MHz\n", gd->usbmph_clk / 1000000);
diff --git a/cpu/mpc83xx/start.S b/cpu/mpc83xx/start.S
index 0f27bb6..6ee9ec9 100644
--- a/cpu/mpc83xx/start.S
+++ b/cpu/mpc83xx/start.S
@@ -77,19 +77,11 @@
 	END_GOT
 
 /*
- * Version string - must be in data segment because MPC83xx uses the
- * first 256 bytes for the Hard Reset Configuration Word table (see
- * below).  Similarly, can't have the U-Boot Magic Number as the first
- * thing in the image - don't know how this will affect the image tools,
- * but I guess I'll find out soon.
+ * The Hard Reset Configuration Word (HRCW) table is in the first 64
+ * (0x40) bytes of flash.  It has 8 bytes, but each byte is repeated 8
+ * times so the processor can fetch it out of flash whether the flash
+ * is 8, 16, 32, or 64 bits wide (hardware trickery).
  */
-	.data
-	.globl	version_string
-version_string:
-	.ascii U_BOOT_VERSION
-	.ascii " (", __DATE__, " - ", __TIME__, ")"
-	.ascii " ", CONFIG_IDENT_STRING, "\0"
-
 	.text
 #define _HRCW_TABLE_ENTRY(w)		\
 	.fill	8,1,(((w)>>24)&0xff);	\
@@ -100,6 +92,18 @@ version_string:
 	_HRCW_TABLE_ENTRY(CFG_HRCW_LOW)
 	_HRCW_TABLE_ENTRY(CFG_HRCW_HIGH)
 
+/*
+ * Magic number and version string - put it after the HRCW since it
+ * cannot be first in flash like it is in many other processors.
+ */
+	.long	0x27051956		/* U-Boot Magic Number */
+
+	.globl	version_string
+version_string:
+	.ascii U_BOOT_VERSION
+	.ascii " (", __DATE__, " - ", __TIME__, ")"
+	.ascii " ", CONFIG_IDENT_STRING, "\0"
+
 
 #ifndef CONFIG_DEFAULT_IMMR
 #error CONFIG_DEFAULT_IMMR must be defined
diff --git a/cpu/mpc86xx/cpu.c b/cpu/mpc86xx/cpu.c
index 551b243..84f5bef 100644
--- a/cpu/mpc86xx/cpu.c
+++ b/cpu/mpc86xx/cpu.c
@@ -32,12 +32,6 @@
 #include <ft_build.h>
 #endif
 
-#ifdef CONFIG_MPC8641HPCN
-extern void mpc8641_reset_board(cmd_tbl_t *cmdtp, int flag,
-				int argc, char *argv[]);
-#endif
-
-
 int
 checkcpu(void)
 {
@@ -185,7 +179,7 @@ do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 
 #else /* CONFIG_MPC8641HPCN */
 
-	mpc8641_reset_board(cmdtp, flag, argc, argv);
+	out8(PIXIS_BASE + PIXIS_RST, 0);
 
 #endif /* !CONFIG_MPC8641HPCN */
 
diff --git a/cpu/mpc86xx/interrupts.c b/cpu/mpc86xx/interrupts.c
index 1df6cdc..49820bb 100644
--- a/cpu/mpc86xx/interrupts.c
+++ b/cpu/mpc86xx/interrupts.c
@@ -80,6 +80,26 @@ 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;
+	}
+
 	/* call cpu specific function from $(CPU)/interrupts.c */
 	ret = interrupt_init_cpu(&decrementer_count);
 
diff --git a/cpu/mpc86xx/spd_sdram.c b/cpu/mpc86xx/spd_sdram.c
index b18e822..ac9ff81 100644
--- a/cpu/mpc86xx/spd_sdram.c
+++ b/cpu/mpc86xx/spd_sdram.c
@@ -284,9 +284,9 @@ spd_init(unsigned char i2c_address, unsigned int ddr_num,
 	}
 
 	/*
-	 * Adjust DDR II IO voltage biasing.  It just makes it work.
+	 * Adjust DDR II IO voltage biasing.  Rev1 only
 	 */
-	if (spd.mem_type == SPD_MEMTYPE_DDR2) {
+	if (((get_svr() & 0xf0) == 0x10) && (spd.mem_type == SPD_MEMTYPE_DDR2)) {
 		gur->ddrioovcr = (0
 				  | 0x80000000		/* Enable */
 				  | 0x10000000		/* VSEL to 1.8V */
diff --git a/cpu/mpc8xx/serial.c b/cpu/mpc8xx/serial.c
index 9d0fc6b..ffc898c 100644
--- a/cpu/mpc8xx/serial.c
+++ b/cpu/mpc8xx/serial.c
@@ -236,8 +236,7 @@ static int smc_init (void)
 	im->im_ioport.iop_pdpar |= 0x800;
 	im->im_ioport.iop_pddir &= ~0x800;
 
-	cp->cp_simode = 0x0000;
-	cp->cp_simode |= 0x7000;
+	cp->cp_simode = ((cp->cp_simode & ~0xf000) | 0x7000);
 #else
 	/* Set up the baud rate generator */
 	smc_setbrg ();
diff --git a/cpu/ppc4xx/44x_spd_ddr.c b/cpu/ppc4xx/44x_spd_ddr.c
index 32d44db..10b4c18 100644
--- a/cpu/ppc4xx/44x_spd_ddr.c
+++ b/cpu/ppc4xx/44x_spd_ddr.c
@@ -46,6 +46,7 @@
 #include <asm/processor.h>
 #include <i2c.h>
 #include <ppc4xx.h>
+#include <asm/mmu.h>
 
 #if defined(CONFIG_SPD_EEPROM) &&					\
 	(defined(CONFIG_440GP) || defined(CONFIG_440GX) ||		\
@@ -229,6 +230,22 @@
 #define TRUE			1
 #define FALSE			0
 
+/*
+ * This DDR2 setup code can dynamically setup the TLB entries for the DDR2 memory
+ * region. Right now the cache should still be disabled in U-Boot because of the
+ * EMAC driver, that need it's buffer descriptor to be located in non cached
+ * memory.
+ *
+ * If at some time this restriction doesn't apply anymore, just define
+ * CFG_ENABLE_SDRAM_CACHE in the board config file and this code should setup
+ * everything correctly.
+ */
+#ifdef CFG_ENABLE_SDRAM_CACHE
+#define MY_TLB_WORD2_I_ENABLE	0			/* enable caching on SDRAM */
+#else
+#define MY_TLB_WORD2_I_ENABLE	TLB_WORD2_I_ENABLE	/* disable caching on SDRAM */
+#endif
+
 const unsigned long test[NUMMEMTESTS][NUMMEMWORDS] = {
 	{0x00000000, 0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000,
 	 0xFFFFFFFF, 0xFFFFFFFF},
@@ -259,6 +276,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);
 
 unsigned char spd_read(uchar chip, uint addr);
 
@@ -377,6 +395,11 @@ long int spd_sdram(void) {
 	total_size = program_bxcr(dimm_populated, iic0_dimm_addr,
 				  num_dimm_banks);
 
+#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);
+#endif
+
 	/*
 	 * program SDRAM Clock Timing Register (SDRAM0_CLKTR)
 	 */
@@ -1330,11 +1353,11 @@ unsigned long program_bxcr(unsigned long* dimm_populated,
 			 */
 			cr |= SDRAM_BXCR_SDBE;
 
-			for (i = 0; i < num_banks; i++) {
-				bank_parms[ctrl_bank_num[dimm_num]+i].bank_size_bytes =
+ 			for (i = 0; i < num_banks; i++) {
+				bank_parms[ctrl_bank_num[dimm_num]+i+dimm_num].bank_size_bytes =
 					(4 * 1024 * 1024) * bank_size_id;
-				bank_parms[ctrl_bank_num[dimm_num]+i].cr = cr;
-			}
+				bank_parms[ctrl_bank_num[dimm_num]+i+dimm_num].cr = cr;
+ 			}
 		}
 	}
 
diff --git a/cpu/ppc4xx/44x_spd_ddr2.c b/cpu/ppc4xx/44x_spd_ddr2.c
index 35b2315..2ecd3e4 100644
--- a/cpu/ppc4xx/44x_spd_ddr2.c
+++ b/cpu/ppc4xx/44x_spd_ddr2.c
@@ -107,10 +107,11 @@
 #define CALC_ODT_RW(n)	(CALC_ODT_R(n) | CALC_ODT_W(n))
 
 /* Defines for the Read Cycle Delay test */
-#define NUMMEMTESTS 8
-#define NUMMEMWORDS 8
+#define NUMMEMTESTS	8
+#define NUMMEMWORDS	8
+#define NUMLOOPS	256		/* memory test loops */
 
-#define CONFIG_ECC_ERROR_RESET		/* test-only: see description below, at check_ecc() */
+#undef CONFIG_ECC_ERROR_RESET		/* test-only: see description below, at check_ecc() */
 
 /*
  * This DDR2 setup code can dynamically setup the TLB entries for the DDR2 memory
@@ -168,8 +169,8 @@ static void program_codt(unsigned long *dimm_populated,
 static void program_mode(unsigned long *dimm_populated,
 			 unsigned char *iic0_dimm_addr,
 			 unsigned long num_dimm_banks,
-                         ddr_cas_id_t *selected_cas,
-                         int *write_recovery);
+			 ddr_cas_id_t *selected_cas,
+			 int *write_recovery);
 static void program_tr(unsigned long *dimm_populated,
 		       unsigned char *iic0_dimm_addr,
 		       unsigned long num_dimm_banks);
@@ -185,9 +186,10 @@ static void program_copt1(unsigned long *dimm_populated,
 static void program_initplr(unsigned long *dimm_populated,
 			    unsigned char *iic0_dimm_addr,
 			    unsigned long num_dimm_banks,
-                            ddr_cas_id_t selected_cas,
+			    ddr_cas_id_t selected_cas,
 			    int write_recovery);
 static unsigned long is_ecc_enabled(void);
+#ifdef CONFIG_DDR_ECC
 static void program_ecc(unsigned long *dimm_populated,
 			unsigned char *iic0_dimm_addr,
 			unsigned long num_dimm_banks,
@@ -195,6 +197,7 @@ static void program_ecc(unsigned long *dimm_populated,
 static void program_ecc_addr(unsigned long start_address,
 			     unsigned long num_bytes,
 			     unsigned long tlb_word2_i_value);
+#endif
 static void program_DQS_calibration(unsigned long *dimm_populated,
 				    unsigned char *iic0_dimm_addr,
 				    unsigned long num_dimm_banks);
@@ -255,15 +258,6 @@ static void mtdcr_any(u32 dcr, u32 val)
 	}
 }
 
-static void wait_ddr_idle(void)
-{
-	u32 val;
-
-	do {
-		mfsdram(SDRAM_MCSTAT, val);
-	} while ((val & SDRAM_MCSTAT_IDLE_MASK) == SDRAM_MCSTAT_IDLE_NOT);
-}
-
 static unsigned char spd_read(uchar chip, uint addr)
 {
 	unsigned char data[2];
@@ -491,7 +485,7 @@ long int initdram(int board_type)
 		(val & ~(SDRAM_MEMODE_DIC_MASK  | SDRAM_MEMODE_DLL_MASK |
 			 SDRAM_MEMODE_RTT_MASK | SDRAM_MEMODE_DQS_MASK)) |
 		(SDRAM_MEMODE_DIC_NORMAL | SDRAM_MEMODE_DLL_ENABLE
-		 | SDRAM_MEMODE_RTT_75OHM | SDRAM_MEMODE_DQS_ENABLE));
+		 | SDRAM_MEMODE_RTT_150OHM | SDRAM_MEMODE_DQS_ENABLE));
 
 	/*------------------------------------------------------------------
 	 * Program Initialization preload registers.
@@ -537,10 +531,12 @@ long int initdram(int board_type)
 	 *-----------------------------------------------------------------*/
 	program_DQS_calibration(dimm_populated, iic0_dimm_addr, num_dimm_banks);
 
+#ifdef CONFIG_DDR_ECC
 	/*------------------------------------------------------------------
 	 * If ecc is enabled, initialize the parity bits.
 	 *-----------------------------------------------------------------*/
 	program_ecc(dimm_populated, iic0_dimm_addr, num_dimm_banks, MY_TLB_WORD2_I_ENABLE);
+#endif
 
 #ifdef DEBUG
 	ppc440sp_sdram_register_dump();
@@ -589,10 +585,23 @@ static void get_spd_info(unsigned long *dimm_populated,
 #ifdef CONFIG_ADD_RAM_INFO
 void board_add_ram_info(int use_default)
 {
+	PPC440_SYS_INFO board_cfg;
+	u32 val;
+
 	if (is_ecc_enabled())
-		puts(" (ECC enabled)");
+		puts(" (ECC");
 	else
-		puts(" (ECC not enabled)");
+		puts(" (ECC not");
+
+	get_sys_info(&board_cfg);
+
+	mfsdr(SDR0_DDR0, val);
+	val = MULDIV64((board_cfg.freqPLB), SDR0_DDR0_DDRM_DECODE(val), 1);
+	printf(" enabled, %d MHz", (val * 2) / 1000000);
+
+	mfsdram(SDRAM_MMODE, val);
+	val = (val & SDRAM_MMODE_DCL_MASK) >> 4;
+	printf(", CL%d)", val);
 }
 #endif
 
@@ -702,7 +711,7 @@ static void check_frequency(unsigned long *dimm_populated,
 	 *-----------------------------------------------------------------*/
 	get_sys_info(&board_cfg);
 
-	mfsdr(sdr_ddr0, sdr_ddrpll);
+	mfsdr(SDR0_DDR0, sdr_ddrpll);
 	sdram_freq = ((board_cfg.freqPLB) * SDR0_DDR0_DDRM_DECODE(sdr_ddrpll));
 
 	/*
@@ -736,6 +745,7 @@ static void check_frequency(unsigned long *dimm_populated,
 			else
 				cycle_time = (((tcyc_reg & 0xF0) >> 4) * 100) +
 					((tcyc_reg & 0x0F)*10);
+			debug("cycle_time=%d [10 picoseconds]\n", cycle_time);
 
 			if  (cycle_time > (calc_cycle_time + 10)) {
 				/*
@@ -877,7 +887,11 @@ static void program_copt1(unsigned long *dimm_populated,
 	unsigned long ddrtype;
 	unsigned long val;
 
+#ifdef CONFIG_DDR_ECC
 	ecc_enabled = TRUE;
+#else
+	ecc_enabled = FALSE;
+#endif
 	dimm_32bit = FALSE;
 	dimm_64bit = FALSE;
 	buf0 = FALSE;
@@ -1110,7 +1124,7 @@ static void program_codt(unsigned long *dimm_populated,
 				modt3 = 0x00000000;
 			}
 		}
-  	} else {
+	} else {
 		codt |= SDRAM_CODT_DQS_2_5_V_DDR1;
 		modt0 = 0x00000000;
 		modt1 = 0x00000000;
@@ -1149,7 +1163,7 @@ static void program_codt(unsigned long *dimm_populated,
 static void program_initplr(unsigned long *dimm_populated,
 			    unsigned char *iic0_dimm_addr,
 			    unsigned long num_dimm_banks,
-                            ddr_cas_id_t selected_cas,
+			    ddr_cas_id_t selected_cas,
 			    int write_recovery)
 {
 	u32 cas = 0;
@@ -1314,8 +1328,9 @@ static void program_mode(unsigned long *dimm_populated,
 	 *-----------------------------------------------------------------*/
 	get_sys_info(&board_cfg);
 
-	mfsdr(sdr_ddr0, sdr_ddrpll);
+	mfsdr(SDR0_DDR0, sdr_ddrpll);
 	sdram_freq = MULDIV64((board_cfg.freqPLB), SDR0_DDR0_DDRM_DECODE(sdr_ddrpll), 1);
+	debug("sdram_freq=%d\n", sdram_freq);
 
 	/*------------------------------------------------------------------
 	 * Handle the timing.  We need to find the worst case timing of all
@@ -1345,6 +1360,7 @@ static void program_mode(unsigned long *dimm_populated,
 
 			/* t_wr_ns = max(t_wr_ns, (unsigned long)dimm_spd[dimm_num][36] >> 2); */ /*  not used in this loop. */
 			cas_bit = spd_read(iic0_dimm_addr[dimm_num], 18);
+			debug("cas_bit[SPD byte 18]=%02x\n", cas_bit);
 
 			/* For a particular DIMM, grab the three CAS values it supports */
 			for (cas_index = 0; cas_index < 3; cas_index++) {
@@ -1363,7 +1379,8 @@ static void program_mode(unsigned long *dimm_populated,
 				if ((tcyc_reg & 0x0F) >= 10) {
 					if ((tcyc_reg & 0x0F) == 0x0D) {
 						/* Convert from hex to decimal */
-						cycle_time_ns_x_100[cas_index] = (((tcyc_reg & 0xF0) >> 4) * 100) + 75;
+						cycle_time_ns_x_100[cas_index] =
+							(((tcyc_reg & 0xF0) >> 4) * 100) + 75;
 					} else {
 						printf("ERROR: SPD reported Tcyc is incorrect for DIMM "
 						       "in slot %d\n", (unsigned int)dimm_num);
@@ -1371,9 +1388,12 @@ static void program_mode(unsigned long *dimm_populated,
 					}
 				} else {
 					/* Convert from hex to decimal */
-					cycle_time_ns_x_100[cas_index] = (((tcyc_reg & 0xF0) >> 4) * 100) +
+					cycle_time_ns_x_100[cas_index] =
+						(((tcyc_reg & 0xF0) >> 4) * 100) +
 						((tcyc_reg & 0x0F)*10);
 				}
+				debug("cas_index=%d: cycle_time_ns_x_100=%d\n", cas_index,
+				      cycle_time_ns_x_100[cas_index]);
 			}
 
 			/* The rest of this routine determines if CAS 2.0, 2.5, 3.0, 4.0 and 5.0 are */
@@ -1386,8 +1406,10 @@ static void program_mode(unsigned long *dimm_populated,
 				 *  Bit   7    6    5    4    3    2    1    0
 				 *       TBD  4.0  3.5  3.0  2.5  2.0  1.5  1.0
 				 */
-				if (((cas_bit & 0x40) == 0x40) && (cas_index < 3) && (cycle_time_ns_x_100[cas_index] != 0)) {
-					max_4_0_tcyc_ns_x_100 = max(max_4_0_tcyc_ns_x_100, cycle_time_ns_x_100[cas_index]);
+				if (((cas_bit & 0x40) == 0x40) && (cas_index < 3) &&
+				    (cycle_time_ns_x_100[cas_index] != 0)) {
+					max_4_0_tcyc_ns_x_100 = max(max_4_0_tcyc_ns_x_100,
+								    cycle_time_ns_x_100[cas_index]);
 					cas_index++;
 				} else {
 					if (cas_index != 0)
@@ -1395,8 +1417,10 @@ static void program_mode(unsigned long *dimm_populated,
 					cas_4_0_available = FALSE;
 				}
 
-				if (((cas_bit & 0x10) == 0x10) && (cas_index < 3) && (cycle_time_ns_x_100[cas_index] != 0)) {
-					max_3_0_tcyc_ns_x_100 = max(max_3_0_tcyc_ns_x_100, cycle_time_ns_x_100[cas_index]);
+				if (((cas_bit & 0x10) == 0x10) && (cas_index < 3) &&
+				    (cycle_time_ns_x_100[cas_index] != 0)) {
+					max_3_0_tcyc_ns_x_100 = max(max_3_0_tcyc_ns_x_100,
+								    cycle_time_ns_x_100[cas_index]);
 					cas_index++;
 				} else {
 					if (cas_index != 0)
@@ -1404,8 +1428,10 @@ static void program_mode(unsigned long *dimm_populated,
 					cas_3_0_available = FALSE;
 				}
 
-				if (((cas_bit & 0x08) == 0x08) && (cas_index < 3) && (cycle_time_ns_x_100[cas_index] != 0)) {
-					max_2_5_tcyc_ns_x_100 = max(max_2_5_tcyc_ns_x_100, cycle_time_ns_x_100[cas_index]);
+				if (((cas_bit & 0x08) == 0x08) && (cas_index < 3) &&
+				    (cycle_time_ns_x_100[cas_index] != 0)) {
+					max_2_5_tcyc_ns_x_100 = max(max_2_5_tcyc_ns_x_100,
+								    cycle_time_ns_x_100[cas_index]);
 					cas_index++;
 				} else {
 					if (cas_index != 0)
@@ -1413,8 +1439,10 @@ static void program_mode(unsigned long *dimm_populated,
 					cas_2_5_available = FALSE;
 				}
 
-				if (((cas_bit & 0x04) == 0x04) && (cas_index < 3) && (cycle_time_ns_x_100[cas_index] != 0)) {
-					max_2_0_tcyc_ns_x_100 = max(max_2_0_tcyc_ns_x_100, cycle_time_ns_x_100[cas_index]);
+				if (((cas_bit & 0x04) == 0x04) && (cas_index < 3) &&
+				    (cycle_time_ns_x_100[cas_index] != 0)) {
+					max_2_0_tcyc_ns_x_100 = max(max_2_0_tcyc_ns_x_100,
+								    cycle_time_ns_x_100[cas_index]);
 					cas_index++;
 				} else {
 					if (cas_index != 0)
@@ -1427,8 +1455,10 @@ static void program_mode(unsigned long *dimm_populated,
 				 *  Bit   7    6    5    4    3    2    1    0
 				 *       TBD  6.0  5.0  4.0  3.0  2.0  TBD  TBD
 				 */
-				if (((cas_bit & 0x20) == 0x20) && (cas_index < 3) && (cycle_time_ns_x_100[cas_index] != 0)) {
-					max_5_0_tcyc_ns_x_100 = max(max_5_0_tcyc_ns_x_100, cycle_time_ns_x_100[cas_index]);
+				if (((cas_bit & 0x20) == 0x20) && (cas_index < 3) &&
+				    (cycle_time_ns_x_100[cas_index] != 0)) {
+					max_5_0_tcyc_ns_x_100 = max(max_5_0_tcyc_ns_x_100,
+								    cycle_time_ns_x_100[cas_index]);
 					cas_index++;
 				} else {
 					if (cas_index != 0)
@@ -1436,8 +1466,10 @@ static void program_mode(unsigned long *dimm_populated,
 					cas_5_0_available = FALSE;
 				}
 
-				if (((cas_bit & 0x10) == 0x10) && (cas_index < 3) && (cycle_time_ns_x_100[cas_index] != 0)) {
-					max_4_0_tcyc_ns_x_100 = max(max_4_0_tcyc_ns_x_100, cycle_time_ns_x_100[cas_index]);
+				if (((cas_bit & 0x10) == 0x10) && (cas_index < 3) &&
+				    (cycle_time_ns_x_100[cas_index] != 0)) {
+					max_4_0_tcyc_ns_x_100 = max(max_4_0_tcyc_ns_x_100,
+								    cycle_time_ns_x_100[cas_index]);
 					cas_index++;
 				} else {
 					if (cas_index != 0)
@@ -1445,8 +1477,10 @@ static void program_mode(unsigned long *dimm_populated,
 					cas_4_0_available = FALSE;
 				}
 
-				if (((cas_bit & 0x08) == 0x08) && (cas_index < 3) && (cycle_time_ns_x_100[cas_index] != 0)) {
-					max_3_0_tcyc_ns_x_100 = max(max_3_0_tcyc_ns_x_100, cycle_time_ns_x_100[cas_index]);
+				if (((cas_bit & 0x08) == 0x08) && (cas_index < 3) &&
+				    (cycle_time_ns_x_100[cas_index] != 0)) {
+					max_3_0_tcyc_ns_x_100 = max(max_3_0_tcyc_ns_x_100,
+								    cycle_time_ns_x_100[cas_index]);
 					cas_index++;
 				} else {
 					if (cas_index != 0)
@@ -1463,11 +1497,15 @@ static void program_mode(unsigned long *dimm_populated,
 	mfsdram(SDRAM_MMODE, mmode);
 	mmode = mmode & ~(SDRAM_MMODE_WR_MASK | SDRAM_MMODE_DCL_MASK);
 
-	cycle_2_0_clk = MULDIV64(ONE_BILLION, 100, max_2_0_tcyc_ns_x_100);
-	cycle_2_5_clk = MULDIV64(ONE_BILLION, 100, max_2_5_tcyc_ns_x_100);
-	cycle_3_0_clk = MULDIV64(ONE_BILLION, 100, max_3_0_tcyc_ns_x_100);
-	cycle_4_0_clk = MULDIV64(ONE_BILLION, 100, max_4_0_tcyc_ns_x_100);
-	cycle_5_0_clk = MULDIV64(ONE_BILLION, 100, max_5_0_tcyc_ns_x_100);
+	/* add 10 here because of rounding problems */
+	cycle_2_0_clk = MULDIV64(ONE_BILLION, 100, max_2_0_tcyc_ns_x_100) + 10;
+	cycle_2_5_clk = MULDIV64(ONE_BILLION, 100, max_2_5_tcyc_ns_x_100) + 10;
+	cycle_3_0_clk = MULDIV64(ONE_BILLION, 100, max_3_0_tcyc_ns_x_100) + 10;
+	cycle_4_0_clk = MULDIV64(ONE_BILLION, 100, max_4_0_tcyc_ns_x_100) + 10;
+	cycle_5_0_clk = MULDIV64(ONE_BILLION, 100, max_5_0_tcyc_ns_x_100) + 10;
+	debug("cycle_3_0_clk=%d\n", cycle_3_0_clk);
+	debug("cycle_4_0_clk=%d\n", cycle_4_0_clk);
+	debug("cycle_5_0_clk=%d\n", cycle_5_0_clk);
 
 	if (sdram_ddr1 == TRUE) { /* DDR1 */
 		if ((cas_2_0_available == TRUE) && (sdram_freq <= cycle_2_0_clk)) {
@@ -1486,6 +1524,9 @@ static void program_mode(unsigned long *dimm_populated,
 			hang();
 		}
 	} else { /* DDR2 */
+		debug("cas_3_0_available=%d\n", cas_3_0_available);
+		debug("cas_4_0_available=%d\n", cas_4_0_available);
+		debug("cas_5_0_available=%d\n", cas_5_0_available);
 		if ((cas_3_0_available == TRUE) && (sdram_freq <= cycle_3_0_clk)) {
 			mmode |= SDRAM_MMODE_DCL_DDR2_3_0_CLK;
 			*selected_cas = DDR_CAS_3;
@@ -1498,7 +1539,11 @@ static void program_mode(unsigned long *dimm_populated,
 		} else {
 			printf("ERROR: Cannot find a supported CAS latency with the installed DIMMs.\n");
 			printf("Only DIMMs DDR2 with CAS latencies of 3.0, 4.0, and 5.0 are supported.\n");
-			printf("Make sure the PLB speed is within the supported range of the DIMMs.\n\n");
+			printf("Make sure the PLB speed is within the supported range of the DIMMs.\n");
+			printf("cas3=%d cas4=%d cas5=%d\n",
+			       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();
 		}
 	}
@@ -1575,7 +1620,7 @@ static void program_rtr(unsigned long *dimm_populated,
 	/*------------------------------------------------------------------
 	 * Set the SDRAM Refresh Timing Register, SDRAM_RTR
 	 *-----------------------------------------------------------------*/
-	mfsdr(sdr_ddr0, sdr_ddrpll);
+	mfsdr(SDR0_DDR0, sdr_ddrpll);
 	sdram_freq = ((board_cfg.freqPLB) * SDR0_DDR0_DDRM_DECODE(sdr_ddrpll));
 
 	max_refresh_rate = 0;
@@ -1661,7 +1706,7 @@ static void program_tr(unsigned long *dimm_populated,
 	 *-----------------------------------------------------------------*/
 	get_sys_info(&board_cfg);
 
-	mfsdr(sdr_ddr0, sdr_ddrpll);
+	mfsdr(SDR0_DDR0, sdr_ddrpll);
 	sdram_freq = ((board_cfg.freqPLB) * SDR0_DDR0_DDRM_DECODE(sdr_ddrpll));
 
 	/*------------------------------------------------------------------
@@ -2069,7 +2114,7 @@ static void program_memory_queue(unsigned long *dimm_populated,
 			 * Set the sizes
 			 *-----------------------------------------------------------------*/
 			baseadd_size = 0;
-			rank_size_bytes = 1024 * 1024 * rank_size_id;
+			rank_size_bytes = 4 * 1024 * 1024 * rank_size_id;
 			switch (rank_size_id) {
 			case 0x02:
 				baseadd_size |= SDRAM_RXBAS_SDSZ_8;
@@ -2106,8 +2151,8 @@ static void program_memory_queue(unsigned long *dimm_populated,
 
 			for (i = 0; i < num_ranks; i++)	{
 				mtdcr_any(rank_reg+i+dimm_num+bank_0_populated,
-					  (rank_base_addr & SDRAM_RXBAS_SDBA_MASK) |
-					  baseadd_size);
+					  (SDRAM_RXBAS_SDBA_ENCODE(rank_base_addr) |
+					   baseadd_size));
 				rank_base_addr += rank_size_bytes;
 			}
 		}
@@ -2130,9 +2175,22 @@ static unsigned long is_ecc_enabled(void)
 		ecc = max(ecc, SDRAM_MCOPT1_MCHK_CHK_DECODE(val));
 	}
 
-	return(ecc);
+	return ecc;
 }
 
+static void blank_string(int size)
+{
+	int i;
+
+	for (i=0; i<size; i++)
+		putc('\b');
+	for (i=0; i<size; i++)
+		putc(' ');
+	for (i=0; i<size; i++)
+		putc('\b');
+}
+
+#ifdef CONFIG_DDR_ECC
 /*-----------------------------------------------------------------------------+
  * program_ecc.
  *-----------------------------------------------------------------------------*/
@@ -2208,6 +2266,15 @@ static void check_ecc(void)
 }
 #endif
 
+static void wait_ddr_idle(void)
+{
+	u32 val;
+
+	do {
+		mfsdram(SDRAM_MCSTAT, val);
+	} while ((val & SDRAM_MCSTAT_IDLE_MASK) == SDRAM_MCSTAT_IDLE_NOT);
+}
+
 /*-----------------------------------------------------------------------------+
  * program_ecc_addr.
  *-----------------------------------------------------------------------------*/
@@ -2219,8 +2286,10 @@ static void program_ecc_addr(unsigned long start_address,
 	unsigned long end_address;
 	unsigned long address_increment;
 	unsigned long mcopt1;
-	char str[] = "ECC generation...";
-	int i;
+	char str[] = "ECC generation -";
+	char slash[] = "\\|/-\\|/-";
+	int loop = 0;
+	int loopi = 0;
 
 	current_address = start_address;
 	mfsdram(SDRAM_MCOPT1, mcopt1);
@@ -2243,14 +2312,20 @@ static void program_ecc_addr(unsigned long start_address,
 			while (current_address < end_address) {
 				*((unsigned long *)current_address) = 0x00000000;
 				current_address += address_increment;
+
+				if ((loop++ % (2 << 20)) == 0) {
+					putc('\b');
+					putc(slash[loopi++ % 8]);
+				}
 			}
+
 		} else {
 			/* ECC bit set method for cached memory */
 			dcbz_area(start_address, num_bytes);
 			dflush();
 		}
-		for (i=0; i<strlen(str); i++)
-			putc('\b');
+
+		blank_string(strlen(str));
 
 		sync();
 		eieio();
@@ -2276,6 +2351,7 @@ static void program_ecc_addr(unsigned long start_address,
 #endif
 	}
 }
+#endif
 
 /*-----------------------------------------------------------------------------+
  * program_DQS_calibration.
@@ -2332,7 +2408,7 @@ static void program_DQS_calibration(unsigned long *dimm_populated,
 #endif
 }
 
-static u32 short_mem_test(void)
+static int short_mem_test(void)
 {
 	u32 *membase;
 	u32 bxcr_num;
@@ -2356,42 +2432,41 @@ static u32 short_mem_test(void)
 		 0xAA55AA55, 0xAA55AA55, 0x55AA55AA, 0x55AA55AA},
 		{0x55AA55AA, 0x55AA55AA, 0xAA55AA55, 0xAA55AA55,
 		 0x55AA55AA, 0x55AA55AA, 0xAA55AA55, 0xAA55AA55} };
+	int l;
 
 	for (bxcr_num = 0; bxcr_num < MAXBXCF; bxcr_num++) {
 		mfsdram(SDRAM_MB0CF + (bxcr_num << 2), bxcf);
 
 		/* Banks enabled */
 		if ((bxcf & SDRAM_BXCF_M_BE_MASK) == SDRAM_BXCF_M_BE_ENABLE) {
-
 			/* Bank is enabled */
-			membase = (u32 *)(SDRAM_RXBAS_SDBA_DECODE(mfdcr_any(SDRAM_R0BAS+bxcr_num)));
 
 			/*------------------------------------------------------------------
 			 * Run the short memory test.
 			 *-----------------------------------------------------------------*/
+			membase = (u32 *)(SDRAM_RXBAS_SDBA_DECODE(mfdcr_any(SDRAM_R0BAS+bxcr_num)));
+
 			for (i = 0; i < NUMMEMTESTS; i++) {
 				for (j = 0; j < NUMMEMWORDS; j++) {
 					membase[j] = test[i][j];
 					ppcDcbf((u32)&(membase[j]));
 				}
 				sync();
-				for (j = 0; j < NUMMEMWORDS; j++) {
-					if (membase[j] != test[i][j]) {
+				for (l=0; l<NUMLOOPS; l++) {
+					for (j = 0; j < NUMMEMWORDS; j++) {
+						if (membase[j] != test[i][j]) {
+							ppcDcbf((u32)&(membase[j]));
+							return 0;
+						}
 						ppcDcbf((u32)&(membase[j]));
-						break;
 					}
-					ppcDcbf((u32)&(membase[j]));
+					sync();
 				}
-				sync();
-				if (j < NUMMEMWORDS)
-					break;
 			}
-			if (i < NUMMEMTESTS)
-				break;
 		}	/* if bank enabled */
 	}		/* for bxcf_num */
 
-	return bxcr_num;
+	return 1;
 }
 
 #ifndef HARD_CODED_DQS
@@ -2400,12 +2475,10 @@ static u32 short_mem_test(void)
  *-----------------------------------------------------------------------------*/
 static void DQS_calibration_process(void)
 {
-	unsigned long ecc_temp;
 	unsigned long rfdc_reg;
 	unsigned long rffd;
 	unsigned long rqdc_reg;
 	unsigned long rqfd;
-	unsigned long bxcr_num;
 	unsigned long val;
 	long rqfd_average;
 	long rffd_average;
@@ -2425,6 +2498,10 @@ static void DQS_calibration_process(void)
 	long max_end;
 	unsigned char fail_found;
 	unsigned char pass_found;
+	u32 rqfd_start;
+	char str[] = "Auto calibration -";
+	char slash[] = "\\|/-\\|/-";
+	int loopi = 0;
 
 	/*------------------------------------------------------------------
 	 * Test to determine the best read clock delay tuning bits.
@@ -2449,11 +2526,16 @@ static void DQS_calibration_process(void)
 	 * we can clock the DDR interface at is 200 MHz (2x 100 MHz PLB speed),
 	 * from experimentation it is safe to say you will always have a failure.
 	 *-----------------------------------------------------------------*/
-	mfsdram(SDRAM_MCOPT1, ecc_temp);
-	ecc_temp &= SDRAM_MCOPT1_MCHK_MASK;
-	mfsdram(SDRAM_MCOPT1, val);
-	mtsdram(SDRAM_MCOPT1, (val & ~SDRAM_MCOPT1_MCHK_MASK) |
-		SDRAM_MCOPT1_MCHK_NON);
+
+	/* first fix RQDC[RQFD] to an average of 80 degre phase shift to find RFDC[RFFD] */
+	rqfd_start = 64; /* test-only: don't know if this is the _best_ start value */
+
+	puts(str);
+
+calibration_loop:
+	mfsdram(SDRAM_RQDC, rqdc_reg);
+	mtsdram(SDRAM_RQDC, (rqdc_reg & ~SDRAM_RQDC_RQFD_MASK) |
+		SDRAM_RQDC_RQFD_ENCODE(rqfd_start));
 
 	max_start = 0;
 	min_end = 0;
@@ -2477,9 +2559,6 @@ static void DQS_calibration_process(void)
 	fail_found = FALSE;
 	pass_found = FALSE;
 
-	/* first fix RQDC[RQFD] to an average of 80 degre phase shift to find RFDC[RFFD] */
-	/* rqdc_reg = mfsdram(SDRAM_RQDC) & ~(SDRAM_RQDC_RQFD_MASK); */
-
 	/*
 	 * get the delay line calibration register value
 	 */
@@ -2495,13 +2574,10 @@ static void DQS_calibration_process(void)
 		 *-----------------------------------------------------------------*/
 		mtsdram(SDRAM_RFDC, rfdc_reg | SDRAM_RFDC_RFFD_ENCODE(rffd));
 
-		/* do the small memory test */
-		bxcr_num = short_mem_test();
-
 		/*------------------------------------------------------------------
 		 * See if the rffd value passed.
 		 *-----------------------------------------------------------------*/
-		if (bxcr_num == MAXBXCF) {
+		if (short_mem_test()) {
 			if (fail_found == TRUE) {
 				pass_found = TRUE;
 				if (current_pass_length == 0)
@@ -2531,7 +2607,6 @@ static void DQS_calibration_process(void)
 		}
 	}		/* for rffd */
 
-
 	/*------------------------------------------------------------------
 	 * Set the average RFFD value
 	 *-----------------------------------------------------------------*/
@@ -2564,13 +2639,10 @@ static void DQS_calibration_process(void)
 		 *-----------------------------------------------------------------*/
 		mtsdram(SDRAM_RQDC, rqdc_reg | SDRAM_RQDC_RQFD_ENCODE(rqfd));
 
-		/* do the small memory test */
-		bxcr_num = short_mem_test();
-
 		/*------------------------------------------------------------------
 		 * See if the rffd value passed.
 		 *-----------------------------------------------------------------*/
-		if (bxcr_num == MAXBXCF) {
+		if (short_mem_test()) {
 			if (fail_found == TRUE) {
 				pass_found = TRUE;
 				if (current_pass_length == 0)
@@ -2598,17 +2670,28 @@ static void DQS_calibration_process(void)
 		}
 	}
 
+	rqfd_average = ((max_start + max_end) >> 1);
+
 	/*------------------------------------------------------------------
 	 * Make sure we found the valid read passing window.  Halt if not
 	 *-----------------------------------------------------------------*/
 	if (window_found == FALSE) {
-		printf("ERROR: Cannot determine a common read delay for the "
+		if (rqfd_start < SDRAM_RQDC_RQFD_MAX) {
+			putc('\b');
+			putc(slash[loopi++ % 8]);
+
+			/* try again from with a different RQFD start value */
+			rqfd_start++;
+			goto calibration_loop;
+		}
+
+		printf("\nERROR: Cannot determine a common read delay for the "
 		       "DIMM(s) installed.\n");
 		debug("%s[%d] ERROR : \n", __FUNCTION__,__LINE__);
 		hang();
 	}
 
-	rqfd_average = ((max_start + max_end) >> 1);
+	blank_string(strlen(str));
 
 	if (rqfd_average < 0)
 		rqfd_average = 0;
@@ -2616,12 +2699,6 @@ static void DQS_calibration_process(void)
 	if (rqfd_average > SDRAM_RQDC_RQFD_MAX)
 		rqfd_average = SDRAM_RQDC_RQFD_MAX;
 
-	/*------------------------------------------------------------------
-	 * Restore the ECC variable to what it originally was
-	 *-----------------------------------------------------------------*/
-	mfsdram(SDRAM_MCOPT1, val);
-	mtsdram(SDRAM_MCOPT1, (val & ~SDRAM_MCOPT1_MCHK_MASK) | ecc_temp);
-
 	mtsdram(SDRAM_RQDC,
 		(rqdc_reg & ~SDRAM_RQDC_RQFD_MASK) |
 		SDRAM_RQDC_RQFD_ENCODE(rqfd_average));
diff --git a/cpu/ppc4xx/4xx_enet.c b/cpu/ppc4xx/4xx_enet.c
index 4f55583..cf56581 100644
--- a/cpu/ppc4xx/4xx_enet.c
+++ b/cpu/ppc4xx/4xx_enet.c
@@ -1333,6 +1333,9 @@ int enetInt (struct eth_device *dev)
 			}
 		}
 		mtdcr (uicsr, MAL_UIC_DEF|EMAC_UIC_DEF|EMAC_UIC_DEF1);	/* Clear */
+#if defined(CONFIG_405EZ)
+		mtsdr (sdricintstat, SDR_ICRX_STAT | SDR_ICTX0_STAT | SDR_ICTX1_STAT);
+#endif	/* defined(CONFIG_405EZ) */
 	}
 	while (serviced);
 
diff --git a/cpu/ppc4xx/Makefile b/cpu/ppc4xx/Makefile
index 96f0f62..4068b53 100644
--- a/cpu/ppc4xx/Makefile
+++ b/cpu/ppc4xx/Makefile
@@ -29,7 +29,7 @@ START	= start.o resetvec.o kgdb.o
 SOBJS	= dcr.o
 COBJS	= 405gp_pci.o 4xx_enet.o \
 	  bedbug_405.o commproc.o \
-	  cpu.o cpu_init.o i2c.o interrupts.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 \
diff --git a/cpu/ppc4xx/cpu.c b/cpu/ppc4xx/cpu.c
index b02f6f4..8e6bc84 100644
--- a/cpu/ppc4xx/cpu.c
+++ b/cpu/ppc4xx/cpu.c
@@ -47,6 +47,9 @@ void board_reset(void);
 
 #if defined(CONFIG_440)
 #define FREQ_EBC		(sys_info.freqEPB)
+#elif defined(CONFIG_405EZ)
+#define FREQ_EBC		((CONFIG_SYS_CLK_FREQ * sys_info.pllFbkDiv) / \
+				 sys_info.pllExtBusDiv)
 #else
 #define FREQ_EBC		(sys_info.freqPLB / sys_info.pllExtBusDiv)
 #endif
@@ -122,6 +125,7 @@ int i2c_bootrom_enabled(void)
 	return (val & SDR0_SDCS_SDD);
 #endif
 }
+#endif
 
 #if defined(CONFIG_440GX)
 #define SDR0_PINSTP_SHIFT	29
@@ -175,16 +179,37 @@ static char *bootstrap_str[] = {
 };
 #endif
 
+#if defined(CONFIG_405EZ)
+#define SDR0_PINSTP_SHIFT	28
+static char *bootstrap_str[] = {
+	"EBC (8 bits)",
+	"SPI (fast)",
+	"NAND (512 page, 4 addr cycle)",
+	"I2C (Addr 0x50)",
+	"EBC (32 bits)",
+	"I2C (Addr 0x50)",
+	"NAND (2K page, 5 addr cycle)",
+	"I2C (Addr 0x50)",
+	"EBC (16 bits)",
+	"Reserved",
+	"NAND (2K page, 4 addr cycle)",
+	"I2C (Addr 0x50)",
+	"NAND (512 page, 3 addr cycle)",
+	"I2C (Addr 0x50)",
+	"SPI (slow)",
+	"I2C (Addr 0x50)",
+};
+#endif
+
 #if defined(SDR0_PINSTP_SHIFT)
 static int bootstrap_option(void)
 {
 	unsigned long val;
 
-	mfsdr(sdr_pinstp, val);
-	return ((val & 0xe0000000) >> SDR0_PINSTP_SHIFT);
+	mfsdr(SDR_PINSTP, val);
+	return ((val & 0xf0000000) >> SDR0_PINSTP_SHIFT);
 }
 #endif /* SDR0_PINSTP_SHIFT */
-#endif
 
 
 #if defined(CONFIG_440)
@@ -209,7 +234,8 @@ int checkcpu (void)
 
 	puts("AMCC PowerPC 4");
 
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405EP)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || \
+    defined(CONFIG_405EP) || defined(CONFIG_405EZ)
 	puts("05");
 #endif
 #if defined(CONFIG_440)
@@ -257,6 +283,10 @@ int checkcpu (void)
 		puts("EP Rev. B");
 		break;
 
+	case PVR_405EZ_RA:
+		puts("EZ Rev. A");
+		break;
+
 #if defined(CONFIG_440)
 	case PVR_440GP_RB:
 		puts("GP Rev. B");
@@ -386,20 +416,20 @@ int checkcpu (void)
 	}
 
 	printf (" at %s MHz (PLB=%lu, OPB=%lu, EBC=%lu MHz)\n", strmhz(buf, clock),
-	       sys_info.freqPLB / 1000000,
-	       sys_info.freqPLB / sys_info.pllOpbDiv / 1000000,
-	       FREQ_EBC / 1000000);
+		sys_info.freqPLB / 1000000,
+		get_OPB_freq() / 1000000,
+		FREQ_EBC / 1000000);
 
 	if (addstr[0] != 0)
 		printf("       %s\n", addstr);
 
 #if defined(I2C_BOOTROM)
 	printf ("       I2C boot EEPROM %sabled\n", i2c_bootrom_enabled() ? "en" : "dis");
+#endif	/* I2C_BOOTROM */
 #if defined(SDR0_PINSTP_SHIFT)
 	printf ("       Bootstrap Option %c - ", (char)bootstrap_option() + 'A');
 	printf ("Boot ROM Location %s\n", bootstrap_str[bootstrap_option()]);
 #endif	/* SDR0_PINSTP_SHIFT */
-#endif	/* I2C_BOOTROM */
 
 #if defined(CONFIG_PCI)
 	printf ("       Internal PCI arbiter %sabled", pci_arbiter_enabled() ? "en" : "dis");
@@ -418,7 +448,7 @@ int checkcpu (void)
 	putc('\n');
 #endif
 
-#if defined(CONFIG_405EP)
+#if defined(CONFIG_405EP) || defined(CONFIG_405EZ)
 	printf ("       16 kB I-Cache 16 kB D-Cache");
 #elif defined(CONFIG_440)
 	printf ("       32 kB I-Cache 32 kB D-Cache");
diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index 82ae443..66e8637 100644
--- a/cpu/ppc4xx/cpu_init.c
+++ b/cpu/ppc4xx/cpu_init.c
@@ -25,6 +25,7 @@
 #include <watchdog.h>
 #include <ppc4xx_enet.h>
 #include <asm/processor.h>
+#include <asm/gpio.h>
 #include <ppc4xx.h>
 
 #if defined(CONFIG_405GP)  || defined(CONFIG_405EP)
@@ -98,118 +99,6 @@ DECLARE_GLOBAL_DATA_PTR;
 # endif
 #endif /* CFG_INIT_DCACHE_CS */
 
-#if defined(CFG_440_GPIO_TABLE)
-gpio_param_s gpio_tab[GPIO_GROUP_MAX][GPIO_MAX] = CFG_440_GPIO_TABLE;
-
-void set_chip_gpio_configuration(gpio_param_s (*gpio_tab)[GPIO_GROUP_MAX][GPIO_MAX])
-{
-	unsigned char i=0, j=0, reg_offset = 0, gpio_core;
-	unsigned long gpio_reg, gpio_core_add;
-
-	for (gpio_core=0; gpio_core<GPIO_GROUP_MAX; gpio_core++) {
-		j = 0;
-		reg_offset = 0;
-		/* GPIO config of the GPIOs 0 to 31 */
-		for (i=0; i<GPIO_MAX; i++, j++) {
-			if (i == GPIO_MAX/2) {
-				reg_offset = 4;
-				j = i-16;
-			}
-
-			gpio_core_add = (*gpio_tab)[gpio_core][i].add;
-
-			if (((*gpio_tab)[gpio_core][i].in_out == GPIO_IN) ||
-			     ((*gpio_tab)[gpio_core][i].in_out == GPIO_BI)) {
-
-				switch ((*gpio_tab)[gpio_core][i].alt_nb) {
-				case GPIO_SEL:
-					break;
-
-				case GPIO_ALT1:
-					gpio_reg = in32(GPIO_IS1(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
-					out32(GPIO_IS1(gpio_core_add+reg_offset), gpio_reg);
-					break;
-
-				case GPIO_ALT2:
-					gpio_reg = in32(GPIO_IS2(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
-					out32(GPIO_IS2(gpio_core_add+reg_offset), gpio_reg);
-					break;
-
-				case GPIO_ALT3:
-					gpio_reg = in32(GPIO_IS3(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
-					out32(GPIO_IS3(gpio_core_add+reg_offset), gpio_reg);
-					break;
-				}
-			}
-
-			if (((*gpio_tab)[gpio_core][i].in_out == GPIO_OUT) ||
-			     ((*gpio_tab)[gpio_core][i].in_out == GPIO_BI)) {
-
-				switch ((*gpio_tab)[gpio_core][i].alt_nb) {
-				case GPIO_SEL:
-					if (gpio_core == GPIO0) {
-						gpio_reg = in32(GPIO0_TCR) | (0x80000000 >> (j));
-						out32(GPIO0_TCR, gpio_reg);
-					}
-
-					if (gpio_core == GPIO1) {
-						gpio_reg = in32(GPIO1_TCR) | (0x80000000 >> (j));
-						out32(GPIO1_TCR, gpio_reg);
-					}
-
-					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
-					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
-					break;
-
-				case GPIO_ALT1:
-					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_ALT1_SEL >> (j*2));
-					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
-					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_ALT1_SEL >> (j*2));
-					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
-					break;
-
-				case GPIO_ALT2:
-					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_ALT2_SEL >> (j*2));
-					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
-					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_ALT2_SEL >> (j*2));
-					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
-					break;
-
-				case GPIO_ALT3:
-					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_ALT3_SEL >> (j*2));
-					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
-					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
-						& ~(GPIO_MASK >> (j*2));
-					gpio_reg = gpio_reg | (GPIO_ALT3_SEL >> (j*2));
-					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
-					break;
-				}
-			}
-		}
-	}
-}
-#endif /* CFG_440_GPIO_TABLE */
-
 /*
  * Breath some life into the CPU...
  *
@@ -248,7 +137,7 @@ cpu_init_f (void)
 #endif /* CONFIG_405EP */
 
 #if defined(CFG_440_GPIO_TABLE)
-	set_chip_gpio_configuration(&gpio_tab);
+	gpio_set_chip_configuration();
 #endif /* CFG_440_GPIO_TABLE */
 
 	/*
@@ -256,7 +145,8 @@ cpu_init_f (void)
 	 */
 #if (defined(CFG_EBC_PB0AP) && defined(CFG_EBC_PB0CR))
 #if (defined(CONFIG_405GP) || defined(CONFIG_405CR) || \
-     defined(CONFIG_405EP) || defined(CONFIG_405))
+     defined(CONFIG_405EP) || defined(CONFIG_405EZ) || \
+     defined(CONFIG_405))
 	/*
 	 * Move the next instructions into icache, since these modify the flash
 	 * we are running from!
diff --git a/cpu/ppc4xx/gpio.c b/cpu/ppc4xx/gpio.c
new file mode 100644
index 0000000..dd84e58
--- /dev/null
+++ b/cpu/ppc4xx/gpio.c
@@ -0,0 +1,214 @@
+/*
+ * (C) Copyright 2007
+ * Stefan Roese, DENX Software Engineering, sr@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/processor.h>
+#include <asm/io.h>
+#include <asm/gpio.h>
+
+#if defined(CFG_440_GPIO_TABLE)
+gpio_param_s gpio_tab[GPIO_GROUP_MAX][GPIO_MAX] = CFG_440_GPIO_TABLE;
+#endif
+
+#if defined(GPIO0_OSRL)
+/* Only some 4xx variants support alternate funtions on the GPIO's */
+void gpio_config(int pin, int in_out, int gpio_alt, int out_val)
+{
+	u32 mask;
+	u32 mask2;
+	u32 val;
+	u32 offs = 0;
+	u32 offs2 = 0;
+	int pin2 = pin << 1;
+
+	if (pin >= GPIO_MAX) {
+		offs = 0x100;
+		pin -= GPIO_MAX;
+	}
+
+	if (pin >= GPIO_MAX/2) {
+		offs2 = 0x100;
+		pin2 = (pin - GPIO_MAX/2) << 1;
+	}
+
+	mask = 0x80000000 >> pin;
+	mask2 = 0xc0000000 >> (pin2 << 1);
+
+	/* first set TCR to 0 */
+	out32(GPIO0_TCR + offs, in32(GPIO0_TCR + offs) & ~mask);
+
+	if (in_out == GPIO_OUT) {
+		val = in32(GPIO0_OSRL + offs + offs2) & ~mask2;
+		switch (gpio_alt) {
+		case GPIO_ALT1:
+			val |= GPIO_ALT1_SEL >> pin2;
+			break;
+		case GPIO_ALT2:
+			val |= GPIO_ALT2_SEL >> pin2;
+			break;
+		case GPIO_ALT3:
+			val |= GPIO_ALT3_SEL >> pin2;
+			break;
+		}
+		out32(GPIO0_OSRL + offs + offs2, val);
+
+		/* setup requested output value */
+		if (out_val == GPIO_OUT_0)
+			out32(GPIO0_OR + offs, in32(GPIO0_OR + offs) & ~mask);
+		else if (out_val == GPIO_OUT_1)
+			out32(GPIO0_OR + offs, in32(GPIO0_OR + offs) | mask);
+
+		/* now configure TCR to drive output if selected */
+		out32(GPIO0_TCR + offs, in32(GPIO0_TCR + offs) | mask);
+	} else {
+		val = in32(GPIO0_ISR1L + offs + offs2) & ~mask2;
+		val |= GPIO_IN_SEL >> pin2;
+		out32(GPIO0_ISR1L + offs + offs2, val);
+	}
+}
+#endif /* GPIO_OSRL */
+
+void gpio_write_bit(int pin, int val)
+{
+	u32 offs = 0;
+
+	if (pin >= GPIO_MAX) {
+		offs = 0x100;
+		pin -= GPIO_MAX;
+	}
+
+	if (val)
+		out32(GPIO0_OR + offs, in32(GPIO0_OR + offs) | GPIO_VAL(pin));
+	else
+		out32(GPIO0_OR + offs, in32(GPIO0_OR + offs) & ~GPIO_VAL(pin));
+}
+
+#if defined(CFG_440_GPIO_TABLE)
+void gpio_set_chip_configuration(void)
+{
+	unsigned char i=0, j=0, offs=0, gpio_core;
+	unsigned long reg, core_add;
+
+	for (gpio_core=0; gpio_core<GPIO_GROUP_MAX; gpio_core++) {
+		j = 0;
+		offs = 0;
+		/* GPIO config of the GPIOs 0 to 31 */
+		for (i=0; i<GPIO_MAX; i++, j++) {
+			if (i == GPIO_MAX/2) {
+				offs = 4;
+				j = i-16;
+			}
+
+			core_add = gpio_tab[gpio_core][i].add;
+
+			if ((gpio_tab[gpio_core][i].in_out == GPIO_IN) ||
+			    (gpio_tab[gpio_core][i].in_out == GPIO_BI)) {
+
+				switch (gpio_tab[gpio_core][i].alt_nb) {
+				case GPIO_SEL:
+					break;
+
+				case GPIO_ALT1:
+					reg = in32(GPIO_IS1(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_IN_SEL >> (j*2));
+					out32(GPIO_IS1(core_add+offs), reg);
+					break;
+
+				case GPIO_ALT2:
+					reg = in32(GPIO_IS2(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_IN_SEL >> (j*2));
+					out32(GPIO_IS2(core_add+offs), reg);
+					break;
+
+				case GPIO_ALT3:
+					reg = in32(GPIO_IS3(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_IN_SEL >> (j*2));
+					out32(GPIO_IS3(core_add+offs), reg);
+					break;
+				}
+			}
+
+			if ((gpio_tab[gpio_core][i].in_out == GPIO_OUT) ||
+			    (gpio_tab[gpio_core][i].in_out == GPIO_BI)) {
+
+				switch (gpio_tab[gpio_core][i].alt_nb) {
+				case GPIO_SEL:
+					if (gpio_core == GPIO0) {
+						reg = in32(GPIO0_TCR) | (0x80000000 >> (j));
+						out32(GPIO0_TCR, reg);
+					}
+
+					if (gpio_core == GPIO1) {
+						reg = in32(GPIO1_TCR) | (0x80000000 >> (j));
+						out32(GPIO1_TCR, reg);
+					}
+
+					reg = in32(GPIO_OS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					out32(GPIO_OS(core_add+offs), reg);
+					reg = in32(GPIO_TS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					out32(GPIO_TS(core_add+offs), reg);
+					break;
+
+				case GPIO_ALT1:
+					reg = in32(GPIO_OS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_ALT1_SEL >> (j*2));
+					out32(GPIO_OS(core_add+offs), reg);
+					reg = in32(GPIO_TS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_ALT1_SEL >> (j*2));
+					out32(GPIO_TS(core_add+offs), reg);
+					break;
+
+				case GPIO_ALT2:
+					reg = in32(GPIO_OS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_ALT2_SEL >> (j*2));
+					out32(GPIO_OS(core_add+offs), reg);
+					reg = in32(GPIO_TS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_ALT2_SEL >> (j*2));
+					out32(GPIO_TS(core_add+offs), reg);
+					break;
+
+				case GPIO_ALT3:
+					reg = in32(GPIO_OS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_ALT3_SEL >> (j*2));
+					out32(GPIO_OS(core_add+offs), reg);
+					reg = in32(GPIO_TS(core_add+offs))
+						& ~(GPIO_MASK >> (j*2));
+					reg = reg | (GPIO_ALT3_SEL >> (j*2));
+					out32(GPIO_TS(core_add+offs), reg);
+					break;
+				}
+			}
+		}
+	}
+}
+#endif /* CFG_440_GPIO_TABLE */
diff --git a/cpu/ppc4xx/i2c.c b/cpu/ppc4xx/i2c.c
index 0b056a1..47c264e 100644
--- a/cpu/ppc4xx/i2c.c
+++ b/cpu/ppc4xx/i2c.c
@@ -91,7 +91,6 @@ static void _i2c_bus_reset(void)
 
 void i2c_init(int speed, int slaveadd)
 {
-	sys_info_t sysInfo;
 	unsigned long freqOPB;
 	int val, divisor;
 	int bus;
@@ -124,8 +123,7 @@ void i2c_init(int speed, int slaveadd)
 
 		/* Clock divide Register */
 		/* get OPB frequency */
-		get_sys_info(&sysInfo);
-		freqOPB = sysInfo.freqPLB / sysInfo.pllOpbDiv;
+		freqOPB = get_OPB_freq();
 		/* set divisor according to freqOPB */
 		divisor = (freqOPB - 1) / 10000000;
 		if (divisor == 0)
@@ -460,6 +458,7 @@ int i2c_set_bus_num(unsigned int bus)
 
 	return 0;
 }
+#endif	/* CONFIG_I2C_MULTI_BUS */
 
 /* TODO: add 100/400k switching */
 unsigned int i2c_get_bus_speed(void)
@@ -474,5 +473,4 @@ int i2c_set_bus_speed(unsigned int speed)
 
 	return 0;
 }
-#endif	/* CONFIG_I2C_MULTI_BUS */
 #endif	/* CONFIG_HARD_I2C */
diff --git a/cpu/ppc4xx/serial.c b/cpu/ppc4xx/serial.c
index fab0d95..e62dd9d 100644
--- a/cpu/ppc4xx/serial.c
+++ b/cpu/ppc4xx/serial.c
@@ -264,7 +264,8 @@ int serial_tstc ()
 #endif	/* CONFIG_IOP480 */
 
 /*****************************************************************************/
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405EP) || \
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || \
+    defined(CONFIG_405EP) || defined(CONFIG_405EZ) || \
     defined(CONFIG_440)
 
 #if defined(CONFIG_440)
@@ -309,7 +310,7 @@ int serial_tstc ()
 #define MFREG(a, d)	mfsdr(a, d)
 #define MTREG(a, d)	mtsdr(a, d)
 #endif /* #if defined(CONFIG_440GP) */
-#elif defined(CONFIG_405EP)
+#elif defined(CONFIG_405EP) || defined(CONFIG_405EZ)
 #define UART0_BASE      0xef600300
 #define UART1_BASE      0xef600400
 #define UCR0_MASK       0x0000007f
@@ -392,47 +393,95 @@ volatile static serial_buffer_t buf_info;
 
 #if defined(CONFIG_440) && !defined(CFG_EXT_SERIAL_CLOCK)
 static void serial_divs (int baudrate, unsigned long *pudiv,
-			 unsigned short *pbdiv )
+			 unsigned short *pbdiv)
 {
-	sys_info_t	sysinfo;
+	sys_info_t sysinfo;
 	unsigned long div;		/* total divisor udiv * bdiv */
 	unsigned long umin;		/* minimum udiv	*/
-	unsigned short diff;    /* smallest diff */
-	unsigned long udiv;     /* best udiv */
-
-	unsigned short idiff;   /* current diff */
-	unsigned short ibdiv;   /* current bdiv */
+	unsigned short diff;		/* smallest diff */
+	unsigned long udiv;		/* best udiv */
+	unsigned short idiff;		/* current diff */
+	unsigned short ibdiv;		/* current bdiv */
 	unsigned long i;
-	unsigned long est;      /* current estimate */
+	unsigned long est;		/* current estimate */
 
-	get_sys_info( &sysinfo );
+	get_sys_info(&sysinfo);
 
-	udiv = 32;     /* Assume lowest possible serial clk */
-	div = sysinfo.freqPLB/(16*baudrate); /* total divisor */
-	umin = sysinfo.pllOpbDiv<<1; /* 2 x OPB divisor */
-	diff = 32;      /* highest possible */
+	udiv = 32;			/* Assume lowest possible serial clk */
+	div = sysinfo.freqPLB / (16 * baudrate); /* total divisor */
+	umin = sysinfo.pllOpbDiv << 1;	/* 2 x OPB divisor */
+	diff = 32;			/* highest possible */
 
 	/* i is the test udiv value -- start with the largest
 	 * possible (32) to minimize serial clock and constrain
 	 * search to umin.
 	 */
-	for( i = 32; i > umin; i-- ){
-		ibdiv = div/i;
+	for (i = 32; i > umin; i--) {
+		ibdiv = div / i;
 		est = i * ibdiv;
 		idiff = (est > div) ? (est-div) : (div-est);
-		if( idiff == 0 ){
+		if (idiff == 0) {
 			udiv = i;
 			break;      /* can't do better */
-		}
-		else if( idiff < diff ){
+		} else if (idiff < diff) {
 			udiv = i;       /* best so far */
 			diff = idiff;   /* update lowest diff*/
 		}
 	}
 
 	*pudiv = udiv;
-	*pbdiv = div/udiv;
+	*pbdiv = div / udiv;
+}
+
+#elif defined(CONFIG_405EZ)
+
+static void serial_divs (int baudrate, unsigned long *pudiv,
+			 unsigned short *pbdiv)
+{
+	sys_info_t sysinfo;
+	unsigned long div;		/* total divisor udiv * bdiv */
+	unsigned long umin;		/* minimum udiv	*/
+	unsigned short diff;		/* smallest diff */
+	unsigned long udiv;		/* best udiv */
+	unsigned short idiff;		/* current diff */
+	unsigned short ibdiv;		/* current bdiv */
+	unsigned long i;
+	unsigned long est;		/* current estimate */
+	unsigned long plloutb;
+	u32 reg;
+
+	get_sys_info(&sysinfo);
 
+	plloutb = ((CONFIG_SYS_CLK_FREQ * sysinfo.pllFwdDiv * sysinfo.pllFbkDiv)
+		   / sysinfo.pllFwdDivB);
+	udiv = 256;			/* Assume lowest possible serial clk */
+	div = plloutb / (16 * baudrate); /* total divisor */
+	umin = (plloutb / get_OPB_freq()) << 1;	/* 2 x OPB divisor */
+	diff = 256;			/* highest possible */
+
+	/* i is the test udiv value -- start with the largest
+	 * possible (256) to minimize serial clock and constrain
+	 * search to umin.
+	 */
+	for (i = 256; i > umin; i--) {
+		ibdiv = div / i;
+		est = i * ibdiv;
+		idiff = (est > div) ? (est-div) : (div-est);
+		if (idiff == 0) {
+			udiv = i;
+			break;      /* can't do better */
+		} else if (idiff < diff) {
+			udiv = i;       /* best so far */
+			diff = idiff;   /* update lowest diff*/
+		}
+	}
+
+	*pudiv = udiv;
+	mfcpr(cprperd0, reg);
+	reg &= ~0x0000ffff;
+	reg |= ((udiv - 0) << 8) | (udiv - 0);
+	mtcpr(cprperd0, reg);
+	*pbdiv = div / udiv;
 }
 #endif /* defined(CONFIG_440) && !defined(CFG_EXT_SERIAL_CLK) */
 
@@ -518,6 +567,10 @@ int serial_init (void)
 	unsigned short bdiv;
 	volatile char val;
 
+#if defined(CONFIG_405EZ)
+	serial_divs(gd->baudrate, &udiv, &bdiv);
+	clk = tmp = reg = 0;
+#else
 #ifdef CONFIG_405EP
 	reg = mfdcr(cpc0_ucr) & ~(UCR0_MASK | UCR1_MASK);
 	clk = gd->cpu_clk;
@@ -548,9 +601,9 @@ int serial_init (void)
 	reg |= (udiv - 1) << CR0_UDIV_POS;	/* set the UART divisor */
 	mtdcr (cntrl0, reg);
 #endif /* CONFIG_405EP */
-
 	tmp = gd->baudrate * udiv * 16;
 	bdiv = (clk + tmp / 2) / tmp;
+#endif /* CONFIG_405EZ */
 
 	out8(UART_BASE + UART_LCR, 0x80);	/* set DLAB bit */
 	out8(UART_BASE + UART_DLL, bdiv);	/* set baudrate divisor */
diff --git a/cpu/ppc4xx/speed.c b/cpu/ppc4xx/speed.c
index 06220c3..028b11a 100644
--- a/cpu/ppc4xx/speed.c
+++ b/cpu/ppc4xx/speed.c
@@ -767,11 +767,119 @@ ulong get_PCI_freq (void)
 	return val;
 }
 
+#elif defined(CONFIG_405EZ)
+void get_sys_info (PPC405_SYS_INFO * sysInfo)
+{
+	unsigned long cpr_plld;
+	unsigned long cpr_primad;
+	unsigned long sysClkPeriodPs = ONE_BILLION / (CONFIG_SYS_CLK_FREQ/1000);
+	unsigned long primad_cpudv;
+	unsigned long m;
+
+	/*
+	 * Read PLL Mode registers
+	 */
+	mfcpr(cprplld, cpr_plld);
+
+	/*
+	 * Determine forward divider A
+	 */
+	sysInfo->pllFwdDiv = ((cpr_plld & PLLD_FWDVA_MASK) >> 16);
+
+	/*
+	 * Determine forward divider B (should be equal to A)
+	 */
+	sysInfo->pllFwdDivB = ((cpr_plld & PLLD_FWDVB_MASK) >> 8);
+	if (sysInfo->pllFwdDivB == 0) {
+		sysInfo->pllFwdDivB = 8;
+	}
+
+	/*
+	 * Determine FBK_DIV.
+	 */
+	sysInfo->pllFbkDiv = ((cpr_plld & PLLD_FBDV_MASK) >> 24);
+	if (sysInfo->pllFbkDiv == 0) {
+		sysInfo->pllFbkDiv = 256;
+	}
+
+	/*
+	 * Read CPR_PRIMAD register
+	 */
+	mfcpr(cprprimad, cpr_primad);
+	/*
+	 * Determine PLB_DIV.
+	 */
+	sysInfo->pllPlbDiv = ((cpr_primad & PRIMAD_PLBDV_MASK) >> 16);
+	if (sysInfo->pllPlbDiv == 0) {
+		sysInfo->pllPlbDiv = 16;
+	}
+
+	/*
+	 * Determine EXTBUS_DIV.
+	 */
+	sysInfo->pllExtBusDiv = (cpr_primad & PRIMAD_EBCDV_MASK);
+	if (sysInfo->pllExtBusDiv == 0) {
+		sysInfo->pllExtBusDiv = 16;
+	}
+
+	/*
+	 * Determine OPB_DIV.
+	 */
+	sysInfo->pllOpbDiv = ((cpr_primad & PRIMAD_OPBDV_MASK) >> 8);
+	if (sysInfo->pllOpbDiv == 0) {
+		sysInfo->pllOpbDiv = 16;
+	}
+
+	/*
+	 * Determine the M factor
+	 */
+	m = sysInfo->pllFbkDiv * sysInfo->pllFwdDivB;
+
+	/*
+	 * Determine VCO clock frequency
+	 */
+	sysInfo->freqVCOHz = (1000000000000LL * (unsigned long long)m) /
+		(unsigned long long)sysClkPeriodPs;
+
+	/*
+	 * Determine CPU clock frequency
+	 */
+	primad_cpudv = ((cpr_primad & PRIMAD_CPUDV_MASK) >> 24);
+	if (primad_cpudv == 0) {
+		primad_cpudv = 16;
+	}
+
+	sysInfo->freqProcessor = (CONFIG_SYS_CLK_FREQ * sysInfo->pllFbkDiv) / primad_cpudv;
+
+	/*
+	 * Determine PLB clock frequency
+	 */
+	sysInfo->freqPLB = (CONFIG_SYS_CLK_FREQ * sysInfo->pllFbkDiv) / sysInfo->pllPlbDiv;
+}
+
+/********************************************
+ * get_OPB_freq
+ * return OPB bus freq in Hz
+ *********************************************/
+ulong get_OPB_freq (void)
+{
+	ulong val = 0;
+
+	PPC405_SYS_INFO sys_info;
+
+	get_sys_info (&sys_info);
+	val = (CONFIG_SYS_CLK_FREQ * sys_info.pllFbkDiv) / sys_info.pllOpbDiv;
+
+	return val;
+}
+
 #endif
 
 int get_clocks (void)
 {
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_440) || defined(CONFIG_405) || defined(CONFIG_405EP)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || \
+    defined(CONFIG_405EP) || defined(CONFIG_405EZ) || \
+    defined(CONFIG_440) || defined(CONFIG_405)
 	sys_info_t sys_info;
 
 	get_sys_info (&sys_info);
@@ -796,7 +904,9 @@ ulong get_bus_freq (ulong dummy)
 {
 	ulong val;
 
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405) || defined(CONFIG_440) || defined(CONFIG_405EP)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || \
+    defined(CONFIG_405EP) || defined(CONFIG_405EZ) || \
+    defined(CONFIG_440) || defined(CONFIG_405)
 	sys_info_t sys_info;
 
 	get_sys_info (&sys_info);
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index 24b30df..a96083c 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -2,6 +2,7 @@
  *  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 Wolfgang Denk <wd@denx.de>
+ *  Copyright (C) 2007 Stefan Roese <sr@denx.de>, DENX Software Engineering
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -699,7 +700,9 @@ _start:
 #endif	/* CONFIG_IOP480 */
 
 /*****************************************************************************/
-#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || defined(CONFIG_405) || defined(CONFIG_405EP)
+#if defined(CONFIG_405GP) || defined(CONFIG_405CR) || \
+    defined(CONFIG_405EP) || defined(CONFIG_405EZ) || \
+    defined(CONFIG_405)
 	/*----------------------------------------------------------------------- */
 	/* Clear and set up some registers. */
 	/*----------------------------------------------------------------------- */
@@ -727,13 +730,13 @@ _start:
 	/*----------------------------------------------------------------------- */
 	/* Enable two 128MB cachable regions. */
 	/*----------------------------------------------------------------------- */
-	addis	r4,r0,0x8000
-	addi	r4,r4,0x0001
+	lis	r4,0x8000
+	ori	r4,r4,0x0001
 	mticcr	r4			/* instruction cache */
 	isync
 
-	addis	r4,r0,0x0000
-	addi	r4,r4,0x0000
+	lis	r4,0x0000
+	ori	r4,r4,0x0000
 	mtdccr	r4			/* data cache */
 
 #if !(defined(CFG_EBC_PB0AP) && defined(CFG_EBC_PB0CR))
@@ -755,6 +758,35 @@ _start:
 #endif /* CONFIG_405EP */
 
 #if defined(CFG_OCM_DATA_ADDR) && defined(CFG_OCM_DATA_SIZE)
+#if defined(CONFIG_405EZ)
+	/********************************************************************
+	 * Setup OCM - On Chip Memory - PPC405EZ uses OCM Controller V2
+	 *******************************************************************/
+	/*
+	 * We can map the OCM on the PLB3, so map it at
+	 * CFG_OCM_DATA_ADDR + 0x8000
+	 */
+	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 */
+	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 */
+	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,0x4000		/* Add 0x4000 for bank 2 */
+	mtdcr	ocmdscr2, r3            /* Set Data Side */
+	mtdcr	ocmiscr2, r3            /* Set Instruction Side */
+	addis	r3,0,0x0800             /* OCM Data Parity Disable - 1 Wait State */
+	mtdcr	ocmdsisdpc,r4
+
+	isync
+#else /* CONFIG_405EZ */
 	/********************************************************************
 	 * Setup OCM - On Chip Memory
 	 *******************************************************************/
@@ -762,18 +794,20 @@ _start:
 	lis	r0, 0x7FFF
 	ori	r0, r0, 0xFFFF
 	mfdcr	r3, ocmiscntl		/* get instr-side IRAM config */
-	mfdcr	r4, ocmdscntl	/* get data-side IRAM config */
-	and	r3, r3, r0	/* disable data-side IRAM */
-	and	r4, r4, r0	/* disable data-side IRAM */
-	mtdcr	ocmiscntl, r3	/* set instr-side IRAM config */
-	mtdcr	ocmdscntl, r4	/* set data-side IRAM config */
+	mfdcr	r4, ocmdscntl		/* get data-side IRAM config */
+	and	r3, r3, r0		/* disable data-side IRAM */
+	and	r4, r4, r0		/* disable data-side IRAM */
+	mtdcr	ocmiscntl, r3		/* set instr-side IRAM config */
+	mtdcr	ocmdscntl, r4		/* set data-side IRAM config */
 	isync
 
-	addis	r3, 0, 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 */
 	mtdcr	ocmdscntl, r4
 	isync
+#endif /* CONFIG_405EZ */
 #endif
 
 	/*----------------------------------------------------------------------- */
@@ -1361,7 +1395,7 @@ ppcSync:
 relocate_code:
 #if defined(CONFIG_440EP) || defined(CONFIG_440GR) || \
     defined(CONFIG_440EPX) || defined(CONFIG_440GRX) || \
-    defined(CONFIG_440SPE)
+    defined(CONFIG_440SP) || defined(CONFIG_440SPE)
 	/*
 	 * On some 440er platforms the cache is enabled in the first TLB (Boot-CS)
 	 * to speed up the boot process. Now this cache needs to be disabled.
@@ -1858,11 +1892,11 @@ pll_wait:
 #endif /* CONFIG_405EP */
 
 #if defined(CONFIG_440)
-#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
 
 /*----------------------------------------------------------------------------+
@@ -1916,43 +1950,43 @@ pll_wait:
 /*----------------------------------------------------------------------------+
 | dcbz_area.
 +----------------------------------------------------------------------------*/
-        function_prolog(dcbz_area)
-        rlwinm. r5,r4,0,27,31
-        rlwinm  r5,r4,27,5,31
-        beq     ..d_ra2
-        addi    r5,r5,0x0001
-..d_ra2:mtctr   r5
-..d_ag2:dcbz    r0,r3
-        addi    r3,r3,32
-        bdnz    ..d_ag2
-        sync
-        blr
-        function_epilog(dcbz_area)
+	function_prolog(dcbz_area)
+	rlwinm. r5,r4,0,27,31
+	rlwinm	r5,r4,27,5,31
+	beq	..d_ra2
+	addi	r5,r5,0x0001
+..d_ra2:mtctr	r5
+..d_ag2:dcbz	r0,r3
+	addi	r3,r3,32
+	bdnz	..d_ag2
+	sync
+	blr
+	function_epilog(dcbz_area)
 
 /*----------------------------------------------------------------------------+
 | dflush.  Assume 32K at vector address is cachable.
 +----------------------------------------------------------------------------*/
-        function_prolog(dflush)
-        mfmsr   r9
-        rlwinm  r8,r9,0,15,13
-        rlwinm  r8,r8,0,17,15
-        mtmsr   r8
-        addi    r3,r0,0x0000
-        mtspr   dvlim,r3
-        mfspr   r3,ivpr
-        addi    r4,r0,1024
-        mtctr   r4
+	function_prolog(dflush)
+	mfmsr	r9
+	rlwinm	r8,r9,0,15,13
+	rlwinm	r8,r8,0,17,15
+	mtmsr	r8
+	addi	r3,r0,0x0000
+	mtspr	dvlim,r3
+	mfspr	r3,ivpr
+	addi	r4,r0,1024
+	mtctr	r4
 ..dflush_loop:
-        lwz     r6,0x0(r3)
-        addi    r3,r3,32
-        bdnz    ..dflush_loop
-        addi    r3,r3,-32
-        mtctr   r4
-..ag:   dcbf    r0,r3
-        addi    r3,r3,-32
-        bdnz    ..ag
-        sync
-        mtmsr   r9
-        blr
-        function_epilog(dflush)
+	lwz	r6,0x0(r3)
+	addi	r3,r3,32
+	bdnz	..dflush_loop
+	addi	r3,r3,-32
+	mtctr	r4
+..ag:	dcbf	r0,r3
+	addi	r3,r3,-32
+	bdnz	..ag
+	sync
+	mtmsr	r9
+	blr
+	function_epilog(dflush)
 #endif /* CONFIG_440 */
diff --git a/cpu/ppc4xx/usb_ohci.c b/cpu/ppc4xx/usb_ohci.c
index ab852c5..c71a6a9 100644
--- a/cpu/ppc4xx/usb_ohci.c
+++ b/cpu/ppc4xx/usb_ohci.c
@@ -76,7 +76,7 @@
 #define m16_swap(x) swap_16(x)
 #define m32_swap(x) swap_32(x)
 
-#if defined(CONFIG_440EP) || defined(CONFIG_440EPX)
+#if defined(CONFIG_405EZ) || defined(CONFIG_440EP) || defined(CONFIG_440EPX)
 #define ohci_cpu_to_le16(x) (x)
 #define ohci_cpu_to_le32(x) (x)
 #else
@@ -1601,7 +1601,7 @@ int usb_lowlevel_init(void)
 	gohci.irq = -1;
 #if defined(CONFIG_440EP)
  	gohci.regs = (struct ohci_regs *)(CFG_PERIPHERAL_BASE | 0x1000);
-#elif defined(CONFIG_440EPX)
+#elif defined(CONFIG_440EPX) || defined(CFG_USB_HOST)
 	gohci.regs = (struct ohci_regs *)(CFG_USB_HOST);
 #endif
 
@@ -1625,8 +1625,10 @@ int usb_lowlevel_init(void)
 	ohci_inited = 1;
 	urb_finished = 1;
 
+#if defined(CONFIG_440EP) || defined(CONFIG_440EPX)
 	/* init the device driver */
 	usb_dev_init();
+#endif
 
 	return 0;
 }
diff --git a/cpu/ppc4xx/vecnum.h b/cpu/ppc4xx/vecnum.h
index 685d48b..bddf9e5 100644
--- a/cpu/ppc4xx/vecnum.h
+++ b/cpu/ppc4xx/vecnum.h
@@ -231,6 +231,47 @@
 
 #else /* !defined(CONFIG_440) */
 
+#if defined(CONFIG_405EZ)
+#define VECNUM_D0		0	/* DMA channel 0		*/
+#define VECNUM_D1		1	/* DMA channel 1		*/
+#define VECNUM_D2		2	/* DMA channel 2		*/
+#define VECNUM_D3		3	/* DMA channel 3		*/
+#define VECNUM_1588		4	/* IEEE 1588 network synchronization */
+#define VECNUM_U0		5	/* UART0			*/
+#define VECNUM_U1		6	/* UART1			*/
+#define VECNUM_CAN0		7	/* CAN 0			*/
+#define VECNUM_CAN1		8	/* CAN 1			*/
+#define VECNUM_SPI		9	/* SPI				*/
+#define VECNUM_IIC0		10	/* I2C				*/
+#define VECNUM_CHT0		11	/* Chameleon timer high pri interrupt */
+#define VECNUM_CHT1		12	/* Chameleon timer high pri interrupt */
+#define VECNUM_USBH1		13	/* USB Host 1			*/
+#define VECNUM_USBH2		14	/* USB Host 2			*/
+#define VECNUM_USBDEV		15	/* USB Device			*/
+#define VECNUM_ETH0		16	/* 10/100 Ethernet interrupt status */
+#define VECNUM_EWU0		17	/* Ethernet wakeup sequence detected */
+
+#define VECNUM_MADMAL		18	/* Logical OR of following MadMAL int */
+#define VECNUM_MS		18	/*	MAL_SERR_INT 		*/
+#define VECNUM_TXDE		18	/* 	MAL_TXDE_INT 		*/
+#define VECNUM_RXDE		18	/*	MAL_RXDE_INT 		*/
+
+#define VECNUM_MTE		19	/* MAL TXEOB			*/
+#define VECNUM_MTE1		20	/* MAL TXEOB1			*/
+#define VECNUM_MRE		21	/* MAL RXEOB			*/
+#define VECNUM_NAND		22	/* NAND Flash controller	*/
+#define VECNUM_ADC		23	/* ADC				*/
+#define VECNUM_DAC		24	/* DAC				*/
+#define VECNUM_OPB2PLB		25	/* OPB to PLB bridge interrupt	*/
+#define VECNUM_RESERVED0	26	/* Reserved			*/
+#define VECNUM_EIR0		27	/* External interrupt 0		*/
+#define VECNUM_EIR1		28	/* External interrupt 1		*/
+#define VECNUM_EIR2		29	/* External interrupt 2		*/
+#define VECNUM_EIR3		30	/* External interrupt 3		*/
+#define VECNUM_EIR4		31	/* External interrupt 4		*/
+
+#else	/* !CONFIG_405EZ */
+
 #define VECNUM_U0           0           /* UART0                        */
 #define VECNUM_U1           1           /* UART1                        */
 #define VECNUM_D0           5           /* DMA channel 0                */
@@ -251,6 +292,7 @@
 #define VECNUM_EIR4         29          /* External interrupt 4         */
 #define VECNUM_EIR5         30          /* External interrupt 5         */
 #define VECNUM_EIR6         31          /* External interrupt 6         */
+#endif	/* defined(CONFIG_405EZ) */
 
 #endif /* defined(CONFIG_440) */