[Blackfin]PATCH-1/2]: Remove obsolete blackfin port and add bf533 platform support

20 files changed, 1217 insertions, 1454 deletions

diff --git a/cpu/bf533/Makefile b/cpu/bf533/Makefile
index 9f4a0d8..6fd5e33 100644
--- a/cpu/bf533/Makefile
+++ b/cpu/bf533/Makefile
@@ -2,7 +2,7 @@
 #
 # Copyright (c) 2005 blackfin.uclinux.org
 #
-# (C) Copyright 2000-2006
+# (C) Copyright 2000-2004
 # Wolfgang Denk, DENX Software Engineering, wd@denx.de.
 #
 # See file CREDITS for list of people who contributed to this
@@ -26,25 +26,23 @@
 
 include $(TOPDIR)/config.mk
 
-LIB	= $(obj)lib$(CPU).a
+LIB	= 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
+OBJS	= cpu.o traps.o ints.o serial.o interrupts.o video.o
 
-SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
-OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
-START	:= $(addprefix $(obj),$(START))
+EXTRA = init_sdram_bootrom_initblock.o
 
-all:	$(obj).depend $(START) $(LIB)
+all:	.depend $(START) $(LIB) .depend $(EXTRA)
 
 $(LIB):	$(OBJS)
-	$(AR) $(ARFLAGS) $@ $(OBJS)
+	$(AR) cr $@ $(OBJS)
 
 #########################################################################
 
-# defines $(obj).depend target
-include $(SRCTREE)/rules.mk
+.depend:	Makefile $(START:.o=.S) $(OBJS:.o=.c)
+		$(CC) -M $(CFLAGS) $(START:.o=.S) $(OBJS:.o=.c) > $@
 
-sinclude $(obj).depend
+sinclude .depend
 
 #########################################################################
diff --git a/cpu/bf533/bf533_serial.h b/cpu/bf533/bf533_serial.h
index d430e6c..82fcd57 100644
--- a/cpu/bf533/bf533_serial.h
+++ b/cpu/bf533/bf533_serial.h
@@ -3,7 +3,7 @@
  *
  * Copyright (c) 2005 blackfin.uclinux.org
  *
- * This file is based on
+ * 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)
@@ -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..d2b34a9 100644
--- a/cpu/bf533/cache.S
+++ b/cpu/bf533/cache.S
@@ -1,18 +1,17 @@
-
-
 #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;
 	P1 = R1;
 	CSYNC;
-1:
+	1:
 	IFLUSH[P0++];
 	CC = P0 < P1(iu);
 	IF CC JUMP 1b(bp);
@@ -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
-	 * cache are set to the invalid state
-	 */
-	BITCLR(R7,IMC_P);
+/*
+ * Clear the IMC bit , All valid bits in the instruction
+ * cache are set to the invalid state
+ */
+	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
-	 * cache are set to the invalid state
-	 */
-	BITCLR(R7,DMC0_P);
-	BITCLR(R7,DMC1_P);
+/* 
+ * 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;		/* 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..10817d9 100644
--- a/cpu/bf533/config.mk
+++ b/cpu/bf533/config.mk
@@ -24,4 +24,4 @@
 # MA 02111-1307 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..bd393d5 100644
--- a/cpu/bf533/cpu.c
+++ b/cpu/bf533/cpu.c
@@ -29,72 +29,28 @@
 #include <asm/blackfin.h>
 #include <command.h>
 #include <asm/entry.h>
+#include <asm/cplb.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];
+
+#ifdef DEBUG
+#define pr_debug(fmt,arg...)  printf(fmt,##arg)
+#else
+static inline int
+    __attribute__ ((format(printf, 1, 2))) pr_debug(const char *fmt, ...)
 {
-	__asm__ __volatile__
-	("cli r3;"
-	"P0 = %0;"
-	"JUMP (P0);"
-	:
-	: "r" (L1_ISRAM)
-	);
+	return 0;
+}
+#endif
+
+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;
 }
@@ -112,29 +68,70 @@ int cleanup_before_linux(void)
 
 void icache_enable(void)
 {
-	unsigned int *I0,*I1;
-	int i;
-
+	unsigned int *I0, *I1;
+	int i, j = 0;
+#ifdef __ADSPBF537__
+	if ((*pCHIPID >> 28) < 2)
+		return;
+#endif
+
+	/* 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]) {
+			pr_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])) {
+			pr_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++) {
+			pr_debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+
+	}
+
 	cli();
-	SSYNC();
+	__builtin_bfin_ssync();
+	asm(" .align 8; ");
 	*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
-	SSYNC();
+	__builtin_bfin_ssync();
 	sti();
 }
 
 void icache_disable(void)
 {
+#ifdef __ADSPBF537__
+	if ((*pCHIPID >> 28) < 2)
+		return;
+#endif
 	cli();
-	SSYNC();
+	__builtin_bfin_ssync();
+	asm(" .align 8; ");
 	*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
-	SSYNC();
+	__builtin_bfin_ssync();
 	sti();
 }
 
@@ -143,7 +140,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 +148,91 @@ 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]) {
+			pr_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 {
+			pr_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])) {
+			pr_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++) {
+			pr_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();
+	__builtin_bfin_ssync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL =
+	    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | temp;
+	__builtin_bfin_ssync();
 	sti();
 }
 
 void dcache_disable(void)
 {
+
+	unsigned int *I0, *I1;
+	int i;
+
 	cli();
-	SSYNC();
-	*(unsigned int *)DMEM_CONTROL &= ~(ACACHE_BCACHE |ENDCPLB |PORT_PREF0);
-	SSYNC();
+	__builtin_bfin_ssync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL &=
+	    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	__builtin_bfin_ssync();
 	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..821363e 100644
--- a/cpu/bf533/cpu.h
+++ b/cpu/bf533/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..8010f72 100644
--- a/cpu/bf533/flush.S
+++ b/cpu/bf533/flush.S
@@ -3,13 +3,12 @@
  *
  * 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;
@@ -60,7 +59,7 @@ ENTRY(icplb_flush)
 	[--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
@@ -86,11 +85,12 @@ 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)
@@ -101,7 +101,7 @@ ENTRY(icplb_flush)
 	 * 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);
@@ -119,7 +119,7 @@ ENTRY(icplb_flush)
 	 * 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*/
@@ -142,7 +142,7 @@ ifs0:	R6 = R5 << 5;		/* Combine set*/
 	IF !CC JUMP ifskip;	/* Skip it if it doesn't match.*/
 
 	/* Tag address matches against page, so this is an entry
-	 * we must flush.
+	 * we must flush. 
 	 */
 
 	R7 >>= 10;		/* Mask off the non-address bits*/
@@ -181,17 +181,17 @@ iflush_whole_page:
 	IFLUSH [P0++];		/* because CSYNC can't end loops.*/
 	LSETUP (isall, ieall) LC0 = P1;
 isall:IFLUSH [P0++];
-ieall: NOP;
+ieall: NOP;	
 	SSYNC;
 	JUMP ifinished;
 
-/* This is an external function being called by the user
+/* 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)
+ENTRY(_flush_data_cache)
 	[--SP] = ( R7:6, P5:4 );
 	LINK 12;
 	SP += -12;
@@ -209,7 +209,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;
@@ -222,13 +222,13 @@ skip:	R6 += -1;
 
 /* 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)
+ENTRY(_dcplb_flush)
 	[--SP] = ( R7:0, P5:0 );
 	[--SP] = LC0;
 	[--SP] = LT0;
@@ -236,7 +236,7 @@ ENTRY(dcplb_flush)
 	[--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
@@ -250,9 +250,9 @@ ENTRY(dcplb_flush)
 
 	/* 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.
+	 * offset into the page_size and page_prefix tables.	
 	 */
-
+		
 	R1 <<= 14;
 	R1 >>= 30;
 	R1 <<= 2;
@@ -260,9 +260,9 @@ ENTRY(dcplb_flush)
 	/* 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
+	 * by DCBS in DMEM_CONTROL		
 	 */
-
+	
 	R2 = 0;			/* Default to Bank A (Bank B would be 1)*/
 
 	P0.L = (DMEM_CONTROL & 0xFFFF);
@@ -290,7 +290,8 @@ 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
@@ -303,7 +304,7 @@ bank_chosen:
 	 * 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);
@@ -322,7 +323,7 @@ bank_chosen:
 	 * 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*/
@@ -386,7 +387,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..d92c877
--- /dev/null
+++ b/cpu/bf533/init_sdram.S
@@ -0,0 +1,180 @@
+#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..67074f9
--- /dev/null
+++ b/cpu/bf533/init_sdram_bootrom_initblock.S
@@ -0,0 +1,181 @@
+#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..a5de96b 100644
--- a/cpu/bf533/interrupt.S
+++ b/cpu/bf533/interrupt.S
@@ -40,225 +40,80 @@
  */
 
 #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:
-	rtn;
+	rtn; 
 
 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;
+	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..9317f26 100644
--- a/cpu/bf533/interrupts.c
+++ b/cpu/bf533/interrupts.c
@@ -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.
@@ -37,14 +37,15 @@
 #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..f476f14 100644
--- a/cpu/bf533/ints.c
+++ b/cpu/bf533/ints.c
@@ -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..eb55205 100644
--- a/cpu/bf533/serial.c
+++ b/cpu/bf533/serial.c
@@ -51,22 +51,21 @@
 #include <asm/uaccess.h>
 #include "bf533_serial.h"
 
-DECLARE_GLOBAL_DATA_PTR;
-
 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 +73,7 @@ void calc_baud(void)
 void serial_setbrg(void)
 {
 	int i;
+	DECLARE_GLOBAL_DATA_PTR;
 
 	calc_baud();
 
@@ -84,29 +84,29 @@ void serial_setbrg(void)
 
 	/* Enable UART */
 	*pUART_GCTL |= UART_GCTL_UCEN;
-	asm("ssync;");
+	__builtin_bfin_ssync();
 
 	/* Set DLAB in LCR to Access DLL and DLH */
 	ACCESS_LATCH;
-	asm("ssync;");
+	__builtin_bfin_ssync();
 
 	*pUART_DLL = hw_baud_table[i].dl_low;
-	asm("ssync;");
+	__builtin_bfin_ssync();
 	*pUART_DLH = hw_baud_table[i].dl_high;
-	asm("ssync;");
+	__builtin_bfin_ssync();
 
 	/* Clear DLAB in LCR to Access THR RBR IER */
 	ACCESS_PORT_IER;
-	asm("ssync;");
+	__builtin_bfin_ssync();
 
 	/* 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;");
+	__builtin_bfin_ssync();
 
 	/* Set LCR to Word Lengh 8-bit word select */
 	*pUART_LCR = UART_LCR_WLS8;
-	asm("ssync;");
+	__builtin_bfin_ssync();
 
 	return;
 }
@@ -119,8 +119,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 +147,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 +178,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 +189,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..8e2d725 100644
--- a/cpu/bf533/start.S
+++ b/cpu/bf533/start.S
@@ -1,5 +1,5 @@
 /*
- * 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
  *
@@ -32,15 +32,29 @@
 
 /*
  * Note: A change in this file subsequently requires a change in
- *       board/$(board_name)/config.mk for a valid u-boot.bin
+ *       board/$(board_name)/config.mk for a valid u-boot.bin 
  */
 
 #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,31 +72,17 @@
 #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;
 
 	/* As per HW reference manual DAG registers,
-	 * DATA and Address resgister shall be zero'd
+	 * DATA and Address resgister shall be zero'd 
 	 * in initialization, after a reset state
 	 */
 	r1 = 0;	/* Data registers zero'd */
@@ -99,7 +99,7 @@ _stext:
 	p3 = 0;
 	p4 = 0;
 	p5 = 0;
-
+	
 	i0 = 0; /* DAG Registers zero'd */
 	i1 = 0;
 	i2 = 0;
@@ -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;
 
@@ -149,106 +150,41 @@ no_soft_reset:
 	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;
 
-	/*
-	 *  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;
+	sp.l = (0xffb01000 & 0xFFFF);
+	sp.h = (0xffb01000 >> 16);
 
-	p0.l = (EBIU_SDBCTL & 0xFFFF);
-	p0.h = (EBIU_SDBCTL >> 16);	/* SDRAM Memory Bank Control Register */
-	r0 = mem_SDBCTL;
-	w[p0] = r0.l;
-	ssync;
+	call init_sdram;
 
-	p0.l = (EBIU_SDRRC & 0xFFFF);
-	p0.h = (EBIU_SDRRC >> 16);	/* SDRAM Refresh Rate Control Register */
-	r0 = mem_SDRRC;
-	w[p0] = r0.l;
-	ssync;
+	/* relocate into to RAM */
+	call get_pc;
+offset:
+	r2.l = offset;
+	r2.h = offset;
+	r3.l = start;
+	r3.h = start;
+	r1 = r2 - r3;
 
-skip_sdram_enable:
-	nop;
+	r0 = r0 - r1;
+	p1 = r0;
 
-#ifndef	CFG_NO_FLASH
-	/* relocate into to RAM */
-	p1.l = (CFG_FLASH_BASE & 0xffff);
-	p1.h = (CFG_FLASH_BASE >> 16);
 	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 */
+	/* 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 */
-
-	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 */
-	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..72cfafb 100644
--- a/cpu/bf533/start1.S
+++ b/cpu/bf533/start1.S
@@ -24,8 +24,8 @@
 
 #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..5e2ce9b 100644
--- a/cpu/bf533/traps.c
+++ b/cpu/bf533/traps.c
@@ -42,6 +42,18 @@
 #include <asm/page.h>
 #include <asm/machdep.h>
 #include "cpu.h"
+#include <asm/arch/anomaly.h>
+#include <asm/cplb.h>
+
+#ifdef DEBUG
+#define pr_debug(fmt,arg...)  printf(fmt,##arg)
+#else
+static inline int
+    __attribute__ ((format(printf, 1, 2))) pr_debug(const char *fmt, ...)
+{
+	return 0;
+}
+#endif
 
 void init_IRQ(void)
 {
@@ -51,23 +63,188 @@ 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 = *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)) {
+				pr_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) {
+			__builtin_bfin_ssync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL &=
+			    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+			__builtin_bfin_ssync();
+		} else {
+			__builtin_bfin_ssync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+			__builtin_bfin_ssync();
+		}
+
+		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) {
+			pr_debug("skipping %i %08p - %08x\n", j, I1, *I1);
+			*I0++;
+			*I1++;
+			j++;
+		}
+
+		pr_debug("remove %i 0x%08x  0x%08x\n", j, *I0, *I1);
+
+		for (; j < 15; j++) {
+			pr_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++) {
+			pr_debug("%i 0x%08x  0x%08x\n", j, *I0++, *I1++);
+		}
+
+		/* Turn the cache back on */
+		if (data) {
+			j = *(unsigned int *)DMEM_CONTROL;
+			__builtin_bfin_ssync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL =
+			    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | j;
+			__builtin_bfin_ssync();
+		} else {
+			__builtin_bfin_ssync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+			__builtin_bfin_ssync();
+		}
+
+		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)
 {
-	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");
+	pr_debug("RETE:  %08lx  RETN: %08lx  RETX: %08lx  RETS: %08lx\n",
+		 fp->rete, fp->retn, fp->retx, fp->rets);
+	pr_debug("IPEND: %04lx  SYSCFG: %04lx\n", fp->ipend, fp->syscfg);
+	pr_debug("SEQSTAT: %08lx    SP: %08lx\n", (long)fp->seqstat, (long)fp);
+	pr_debug("R0: %08lx    R1: %08lx    R2: %08lx    R3: %08lx\n",
+		 fp->r0, fp->r1, fp->r2, fp->r3);
+	pr_debug("R4: %08lx    R5: %08lx    R6: %08lx    R7: %08lx\n",
+		 fp->r4, fp->r5, fp->r6, fp->r7);
+	pr_debug("P0: %08lx    P1: %08lx    P2: %08lx    P3: %08lx\n",
+		 fp->p0, fp->p1, fp->p2, fp->p3);
+	pr_debug("P4: %08lx    P5: %08lx    FP: %08lx\n",
+		 fp->p4, fp->p5, fp->fp);
+	pr_debug("A0.w: %08lx    A0.x: %08lx    A1.w: %08lx    A1.x: %08lx\n",
+		 fp->a0w, fp->a0x, fp->a1w, fp->a1x);
+
+	pr_debug("LB0: %08lx  LT0: %08lx  LC0: %08lx\n",
+		 fp->lb0, fp->lt0, fp->lc0);
+	pr_debug("LB1: %08lx  LT1: %08lx  LC1: %08lx\n",
+		 fp->lb1, fp->lt1, fp->lc1);
+	pr_debug("B0: %08lx  L0: %08lx  M0: %08lx  I0: %08lx\n",
+		 fp->b0, fp->l0, fp->m0, fp->i0);
+	pr_debug("B1: %08lx  L1: %08lx  M1: %08lx  I1: %08lx\n",
+		 fp->b1, fp->l1, fp->m1, fp->i1);
+	pr_debug("B2: %08lx  L2: %08lx  M2: %08lx  I2: %08lx\n",
+		 fp->b2, fp->l2, fp->m2, fp->i2);
+	pr_debug("B3: %08lx  L3: %08lx  M3: %08lx  I3: %08lx\n",
+		 fp->b3, fp->l3, fp->m3, fp->i3);
+
+	pr_debug("DCPLB_FAULT_ADDR=%p\n", *pDCPLB_FAULT_ADDR);
+	pr_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..056564a
--- /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}
+};