add blackfin cache and traps handling

6 files changed, 675 insertions, 156 deletions

diff --git a/arch/blackfin/lib/Makefile b/arch/blackfin/lib/Makefile
index 10ea0beceb..3f625b77d2 100644
--- a/arch/blackfin/lib/Makefile
+++ b/arch/blackfin/lib/Makefile
@@ -12,3 +12,5 @@ obj-y += divsi3.o
 obj-y += modsi3.o
 obj-y += cpu.o
 obj-y += flush.o
+obj-y += interrupt.o
+obj-y += traps.o
diff --git a/arch/blackfin/lib/cache.c b/arch/blackfin/lib/cache.c
deleted file mode 100644
index 847278d226..0000000000
--- a/arch/blackfin/lib/cache.c
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * U-boot - cache.c
- *
- * Copyright (c) 2005 blackfin.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., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- */
-
-/* for now: just dummy functions to satisfy the linker */
-extern void blackfin_icache_range (unsigned long *, unsigned long *);
-extern void blackfin_dcache_range (unsigned long *, unsigned long *);
-void flush_cache (unsigned long dummy1, unsigned long dummy2)
-{
-	if (icache_status ()) {
-		blackfin_icache_flush_range (dummy1, dummy1 + dummy2);
-	}
-	if (dcache_status ()) {
-		blackfin_dcache_flush_range (dummy1, dummy1 + dummy2);
-	}
-	return;
-}
diff --git a/arch/blackfin/lib/cpu.c b/arch/blackfin/lib/cpu.c
index e75c0372a9..41a6ad4334 100644
--- a/arch/blackfin/lib/cpu.c
+++ b/arch/blackfin/lib/cpu.c
@@ -29,66 +29,13 @@
 #include <asm/blackfin.h>
 #include <command.h>
 #include <asm/entry.h>
-#include <asm/cpu/defBF561_extn.h> /* FIXME */
-
-#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[])
+#include <asm/cpu.h>
+#include <init.h>
+
+void reset_cpu(ulong ignored)
 {
+	icache_disable();
+
 	__asm__ __volatile__
 	("cli r3;"
 	"P0 = %0;"
@@ -96,43 +43,52 @@ int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 	:
 	: "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)
+void icache_disable(void)
 {
-	return 0;
+#ifdef __ADSPBF537__
+	if ((*pCHIPID >> 28) < 2)
+		return;
+#endif
+	__builtin_bfin_ssync();
+	asm(" .align 8; ");
+	*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+	__builtin_bfin_ssync();
 }
 
 void icache_enable(void)
 {
-	unsigned int *I0,*I1;
-	int i;
+	unsigned int *I0, *I1;
+	int 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];
-		}
-	SSYNC();
+	/* We only setup instruction caching for U-Boot itself.
+	 * This has the nice side effect that we trigger an
+	 * exception when U-Boot goes crazy.
+	 */
+	*I0++ = TEXT_BASE & ~((1 << 20) - 1);
+	*I1++ = PAGE_SIZE_1MB | CPLB_L1_CHBL | CPLB_USER_RD | CPLB_VALID | CPLB_LOCK;
+	j++;
+
+	/* Fill the rest with invalid entry */
+	for ( ; j < 16 ; j++) {
+		debug("filling %i with 0\n",j);
+		*I1++ = 0x0;
+	}
+
+	__builtin_bfin_ssync();
+	asm(" .align 8; ");
 	*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
-	SSYNC();
-}
-
-void icache_disable(void)
-{
-	SSYNC();
-	*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
-	SSYNC();
+	__builtin_bfin_ssync();
 }
 
 int icache_status(void)
@@ -140,44 +96,37 @@ int icache_status(void)
 	unsigned int value;
 	value = *(unsigned int *)IMEM_CONTROL;
 
-	if( value & (IMC|ENICPLB) )
-		return CACHE_ON;
+	if (value & (IMC | ENICPLB))
+		return 1;
 	else
-		return CACHE_OFF;
+		return 0;
 }
 
-void dcache_enable(void)
+static void blackfin_init_exceptions(void)
 {
-	unsigned int *I0,*I1;
-	unsigned int temp;
-	int i;
-	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];
-		}
-
-	temp = *(unsigned int *)DMEM_CONTROL;
-	SSYNC();
-	*(unsigned int *)DMEM_CONTROL = ACACHE_BCACHE |ENDCPLB |PORT_PREF0|temp;
-	SSYNC();
+	*(unsigned volatile long *) (SIC_IMASK) = SIC_UNMASK_ALL;
+#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;
+	*(volatile unsigned long *) ILAT = 0;
+	asm("csync;");
+	*(volatile unsigned long *) IMASK = 0x3f;
+	asm("csync;");
 }
 
-void dcache_disable(void)
+static int blackfin_init_core(void)
 {
-	SSYNC();
-	*(unsigned int *)DMEM_CONTROL &= ~(ACACHE_BCACHE |ENDCPLB |PORT_PREF0);
-	SSYNC();
-}
+	blackfin_init_exceptions();
+	icache_enable();
 
-int dcache_status(void)
-{
-	unsigned int value;
-	value = *(unsigned int *)DMEM_CONTROL;
-	if( value & (ENDCPLB))
-		return CACHE_ON;
-	else
-		return CACHE_OFF;
+	return 0;
 }
+
+core_initcall(blackfin_init_core);
+
diff --git a/arch/blackfin/lib/flush.S b/arch/blackfin/lib/flush.S
new file mode 100644
index 0000000000..62aa496986
--- /dev/null
+++ b/arch/blackfin/lib/flush.S
@@ -0,0 +1,402 @@
+/* Copyright (C) 2003 Analog Devices, Inc. All Rights Reserved.
+ * Copyright (C) 2004 LG SOft India. 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
+ */
+#define ASSEMBLY
+
+#include <asm/linkage.h>
+#include <asm/cplb.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*/
+	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)
+	 * 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/arch/blackfin/lib/interrupt.S b/arch/blackfin/lib/interrupt.S
new file mode 100644
index 0000000000..820a33cc7d
--- /dev/null
+++ b/arch/blackfin/lib/interrupt.S
@@ -0,0 +1,96 @@
+/*
+ * U-boot - interrupt.S Processing of interrupts and exception handling
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * (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., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 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/irq.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_ivhw
+_evt_ivhw:
+	SAVE_CONTEXT
+	RAISE 14;
+
+_evt_ivhw_exit:
+	 rti;
+
diff --git a/arch/blackfin/lib/traps.c b/arch/blackfin/lib/traps.c
new file mode 100644
index 0000000000..fcda7861a3
--- /dev/null
+++ b/arch/blackfin/lib/traps.c
@@ -0,0 +1,110 @@
+/*
+ * U-boot - traps.c Routines related to interrupts and exceptions
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * 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., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <linux/types.h>
+#include <asm/system.h>
+#include <asm/traps.h>
+#include <asm/page.h>
+#include <asm/cplb.h>
+#include <asm/ptrace.h>
+#include <asm/cpu.h>
+
+void dump_regs(struct pt_regs *fp)
+{
+	printf("DCPLB_FAULT_ADDR=%p\n", *pDCPLB_FAULT_ADDR);
+	printf("ICPLB_FAULT_ADDR=%p\n", *pICPLB_FAULT_ADDR);
+
+	printf("stack frame=0x%x, ", (unsigned int) fp);
+	printf("bad PC=0x%04x\n", (unsigned int) fp->pc);
+        printf("RETE:  %08lx  RETN: %08lx  RETX: %08lx  RETS: %08lx\n", fp->rete, fp->retn, fp->retx, fp->rets);
+        printf("IPEND: %04lx  SYSCFG: %04lx\n", fp->ipend, fp->syscfg);
+        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("LB0: %08lx  LT0: %08lx  LC0: %08lx\n", fp->lb0, fp->lt0, fp->lc0);
+        printf("LB1: %08lx  LT1: %08lx  LC1: %08lx\n", fp->lb1, fp->lt1, fp->lc1);
+        printf("B0: %08lx  L0: %08lx  M0: %08lx  I0: %08lx\n", fp->b0, fp->l0, fp->m0, fp->i0);
+        printf("B1: %08lx  L1: %08lx  M1: %08lx  I1: %08lx\n", fp->b1, fp->l1, fp->m1, fp->i1);
+        printf("B2: %08lx  L2: %08lx  M2: %08lx  I2: %08lx\n", fp->b2, fp->l2, fp->m2, fp->i2);
+        printf("B3: %08lx  L3: %08lx  M3: %08lx  I3: %08lx\n", fp->b3, fp->l3, fp->m3, fp->i3);
+}
+
+static const char *trap_to_string(int trapnr)
+{
+	switch (trapnr) {
+	case VEC_MISALI_D:
+		return "Data access misaligned";
+	case VEC_MISALI_I:
+		return "Instruction fetch misaligned";
+	case VEC_CPLB_I_M:
+		return "Instruction fetch CPLB miss";
+	}
+	return NULL;
+}
+
+void trap_c (struct pt_regs *regs)
+{
+	unsigned long trapnr = (regs->seqstat) & SEQSTAT_EXCAUSE;
+	const char *str;
+
+	printf("Exception occured!\n\n");
+
+	str = trap_to_string(trapnr);
+	if (str)
+		printf("%s\n", str);
+	printf("code=[0x%x]\n", trapnr);
+
+	dump_regs(regs);
+
+	printf("\nPlease reset the board\n");
+
+	reset_cpu(0);
+}
+
+void blackfin_irq_panic(int reason, struct pt_regs *regs)
+{
+	printf("\n\nException: IRQ 0x%x entered\n", reason);
+	dump_regs(regs);
+	printf("Unhandled IRQ or exceptions!\n");
+	printf("Please reset the board \n");
+
+	reset_cpu(0);
+}
+