[Blackfin][PATCH] Add BF561 EZKIT board support

18 files changed, 2865 insertions, 0 deletions

diff --git a/cpu/bf561/Makefile b/cpu/bf561/Makefile
new file mode 100644
index 0000000..ee7842a
--- /dev/null
+++ b/cpu/bf561/Makefile
@@ -0,0 +1,52 @@
+# U-boot - Makefile
+#
+# 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
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(CPU).a
+
+START	= start.o start1.o interrupt.o cache.o flush.o init_sdram.o
+COBJS	= cpu.o traps.o ints.o serial.o interrupts.o video.o
+
+EXTRA = init_sdram_bootrom_initblock.o
+
+SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS) $(SOBJS))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(START) $(LIB) $(obj).depend $(EXTRA)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/bf561/cache.S b/cpu/bf561/cache.S
new file mode 100644
index 0000000..5bda5bf
--- /dev/null
+++ b/cpu/bf561/cache.S
@@ -0,0 +1,128 @@
+#define ASSEMBLY
+#include <asm/linkage.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.text
+.align 2
+ENTRY(_blackfin_icache_flush_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+	1:
+	IFLUSH[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+	IFLUSH[P0];
+	SSYNC;
+	RTS;
+
+ENTRY(_blackfin_dcache_flush_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+1:
+	FLUSH[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+	FLUSH[P0];
+	SSYNC;
+	RTS;
+
+ENTRY(_icache_invalidate)
+ENTRY(_invalidate_entire_icache)
+	[--SP] = (R7:5);
+
+	P0.L = (IMEM_CONTROL & 0xFFFF);
+	P0.H = (IMEM_CONTROL >> 16);
+	R7 =[P0];
+
+	/*
+	 * Clear the IMC bit , All valid bits in the instruction
+	 * cache are set to the invalid state
+	 */
+	BITCLR(R7, IMC_P);
+	CLI R6;
+	/* SSYNC required before invalidating cache. */
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	/* Configures the instruction cache agian */
+	R6 = (IMC | ENICPLB);
+	R7 = R7 | R6;
+
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	(R7:5) =[SP++];
+	RTS;
+
+/*
+ * Invalidate the Entire Data cache by
+ * clearing DMC[1:0] bits
+ */
+ENTRY(_invalidate_entire_dcache)
+ENTRY(_dcache_invalidate)
+	[--SP] = (R7:6);
+
+	P0.L = (DMEM_CONTROL & 0xFFFF);
+	P0.H = (DMEM_CONTROL >> 16);
+	R7 =[P0];
+
+	/*
+	 * Clear the DMC[1:0] bits, All valid bits in the data
+	 * cache are set to the invalid state
+	 */
+	BITCLR(R7, DMC0_P);
+	BITCLR(R7, DMC1_P);
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+	/* Configures the data cache again */
+
+	R6 = (ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	R7 = R7 | R6;
+
+	CLI R6;
+	SSYNC;
+	.align 8;
+	[P0] = R7;
+	SSYNC;
+	STI R6;
+
+	(R7:6) =[SP++];
+	RTS;
+
+ENTRY(_blackfin_dcache_invalidate_range)
+	R2 = -32;
+	R2 = R0 & R2;
+	P0 = R2;
+	P1 = R1;
+	CSYNC;
+1:
+	FLUSHINV[P0++];
+	CC = P0 < P1(iu);
+	IF CC JUMP 1b(bp);
+
+	/*
+	 * If the data crosses a cache line, then we'll be pointing to
+	 * the last cache line, but won't have flushed/invalidated it yet, so do
+	 * one more.
+	 */
+	FLUSHINV[P0];
+	SSYNC;
+	RTS;
diff --git a/cpu/bf561/config.mk b/cpu/bf561/config.mk
new file mode 100644
index 0000000..c49a0ba
--- /dev/null
+++ b/cpu/bf561/config.mk
@@ -0,0 +1,27 @@
+# U-boot - config.mk
+#
+# 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
+#
+
+PLATFORM_RELFLAGS += -mcpu=bf561 -ffixed-P5
diff --git a/cpu/bf561/cpu.c b/cpu/bf561/cpu.c
new file mode 100644
index 0000000..a7b53d8
--- /dev/null
+++ b/cpu/bf561/cpu.c
@@ -0,0 +1,220 @@
+/*
+ * U-boot - cpu.c CPU specific functions
+ *
+ * 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
+ */
+
+#include <common.h>
+#include <asm/blackfin.h>
+#include <command.h>
+#include <asm/entry.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
+
+#define CACHE_ON 1
+#define CACHE_OFF 0
+
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
+{
+	__asm__ __volatile__("cli r3;" "P0 = %0;" "JUMP (P0);"::"r"(L1_ISRAM)
+	    );
+
+	return 0;
+}
+
+/* These functions are just used to satisfy the linker */
+int cpu_init(void)
+{
+	return 0;
+}
+
+int cleanup_before_linux(void)
+{
+	return 0;
+}
+
+void icache_enable(void)
+{
+	unsigned int *I0, *I1;
+	int i, j = 0;
+
+	/* Before enable icache, disable it first */
+	icache_disable();
+	I0 = (unsigned int *)ICPLB_ADDR0;
+	I1 = (unsigned int *)ICPLB_DATA0;
+
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & icplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & icplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      icplb_table[i][0], icplb_table[i][1]);
+			*I0++ = icplb_table[i][0];
+			*I1++ = icplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
+		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+
+	}
+
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+	sync();
+	sti();
+}
+
+void icache_disable(void)
+{
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+	sync();
+	sti();
+}
+
+int icache_status(void)
+{
+	unsigned int value;
+	value = *(unsigned int *)IMEM_CONTROL;
+
+	if (value & (IMC | ENICPLB))
+		return CACHE_ON;
+	else
+		return CACHE_OFF;
+}
+
+void dcache_enable(void)
+{
+	unsigned int *I0, *I1;
+	unsigned int temp;
+	int i, j = 0;
+
+	/* Before enable dcache, disable it first */
+	dcache_disable();
+	I0 = (unsigned int *)DCPLB_ADDR0;
+	I1 = (unsigned int *)DCPLB_DATA0;
+
+	/* make sure the locked ones go in first */
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (CPLB_LOCK & dcplb_table[i][1]) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+		} else {
+			debug("skip   %02i %02i 0x%08x 0x%08x\n", i, j,
+			      dcplb_table[i][0], dcplb_table[i][1]);
+		}
+	}
+
+	for (i = 0; i < page_descriptor_table_size; i++) {
+		if (!(CPLB_LOCK & dcplb_table[i][1])) {
+			debug("adding %02i %02i 0x%08x 0x%08x\n", i, j,
+			      dcplb_table[i][0], dcplb_table[i][1]);
+			*I0++ = dcplb_table[i][0];
+			*I1++ = dcplb_table[i][1];
+			j++;
+			if (j == 16) {
+				break;
+			}
+		}
+	}
+
+	/* Fill the rest with invalid entry */
+	if (j <= 15) {
+		for (; j < 16; j++) {
+			debug("filling %i with 0", j);
+			*I1++ = 0x0;
+		}
+	}
+
+	cli();
+	temp = *(unsigned int *)DMEM_CONTROL;
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL =
+	    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | temp;
+	sync();
+	sti();
+}
+
+void dcache_disable(void)
+{
+
+	unsigned int *I0, *I1;
+	int i;
+
+	cli();
+	sync();
+	asm(" .align 8; ");
+	*(unsigned int *)DMEM_CONTROL &=
+	    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+	sync();
+	sti();
+
+	/* after disable dcache, clear it so we don't confuse the next application */
+	I0 = (unsigned int *)DCPLB_ADDR0;
+	I1 = (unsigned int *)DCPLB_DATA0;
+
+	for (i = 0; i < 16; i++) {
+		*I0++ = 0x0;
+		*I1++ = 0x0;
+	}
+}
+
+int dcache_status(void)
+{
+	unsigned int value;
+	value = *(unsigned int *)DMEM_CONTROL;
+	if (value & (ENDCPLB))
+		return CACHE_ON;
+	else
+		return CACHE_OFF;
+}
diff --git a/cpu/bf561/cpu.h b/cpu/bf561/cpu.h
new file mode 100644
index 0000000..821363e
--- /dev/null
+++ b/cpu/bf561/cpu.h
@@ -0,0 +1,66 @@
+/*
+ *  U-boot - cpu.h
+ *
+ *  Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#ifndef _CPU_H_
+#define _CPU_H_
+
+#include <command.h>
+
+#define INTERNAL_IRQS (32)
+#define NUM_IRQ_NODES 16
+#define DEF_INTERRUPT_FLAGS 1
+#define MAX_TIM_LOAD	0xFFFFFFFF
+
+void blackfin_irq_panic(int reason, struct pt_regs *reg);
+extern void dump(struct pt_regs *regs);
+void display_excp(void);
+asmlinkage void evt_nmi(void);
+asmlinkage void evt_exception(void);
+asmlinkage void trap(void);
+asmlinkage void evt_ivhw(void);
+asmlinkage void evt_rst(void);
+asmlinkage void evt_timer(void);
+asmlinkage void evt_evt7(void);
+asmlinkage void evt_evt8(void);
+asmlinkage void evt_evt9(void);
+asmlinkage void evt_evt10(void);
+asmlinkage void evt_evt11(void);
+asmlinkage void evt_evt12(void);
+asmlinkage void evt_evt13(void);
+asmlinkage void evt_soft_int1(void);
+asmlinkage void evt_system_call(void);
+void blackfin_irq_panic(int reason, struct pt_regs *regs);
+void blackfin_free_irq(unsigned int irq, void *dev_id);
+void call_isr(int irq, struct pt_regs *fp);
+void blackfin_do_irq(int vec, struct pt_regs *fp);
+void blackfin_init_IRQ(void);
+void blackfin_enable_irq(unsigned int irq);
+void blackfin_disable_irq(unsigned int irq);
+extern int do_reset(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[]);
+int blackfin_request_irq(unsigned int irq,
+			 void (*handler) (int, void *, struct pt_regs *),
+			 unsigned long flags, const char *devname,
+			 void *dev_id);
+void timer_init(void);
+#endif
diff --git a/cpu/bf561/flush.S b/cpu/bf561/flush.S
new file mode 100644
index 0000000..7e12c83
--- /dev/null
+++ b/cpu/bf561/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.
+ */
+#define ASSEMBLY
+
+#include <asm/linkage.h>
+#include <asm/cplb.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.text
+
+/* This is an external function being called by the user
+ * application through __flush_cache_all. Currently this function
+ * serves the purpose of flushing all the pending writes in
+ * in the instruction cache.
+ */
+
+ENTRY(_flush_instruction_cache)
+	[--SP] = ( R7:6, P5:4 );
+	LINK 12;
+	SP += -12;
+	P5.H = (ICPLB_ADDR0 >> 16);
+	P5.L = (ICPLB_ADDR0 & 0xFFFF);
+	P4.H = (ICPLB_DATA0 >> 16);
+	P4.L = (ICPLB_DATA0 & 0xFFFF);
+	R7 = CPLB_VALID | CPLB_L1_CHBL;
+	R6 = 16;
+inext:	R0 = [P5++];
+	R1 = [P4++];
+	[--SP] =  RETS;
+	CALL _icplb_flush;	/* R0 = page, R1 = data*/
+	RETS = [SP++];
+iskip:	R6 += -1;
+	CC = R6;
+	IF CC JUMP inext;
+	SSYNC;
+	SP += 12;
+	UNLINK;
+	( R7:6, P5:4 ) = [SP++];
+	RTS;
+
+/* This is an internal function to flush all pending
+ * writes in the cache associated with a particular ICPLB.
+ *
+ * R0 -  page's start address
+ * R1 -  CPLB's data field.
+ */
+
+.align 2
+ENTRY(_icplb_flush)
+	[--SP] = ( R7:0, P5:0 );
+	[--SP] = LC0;
+	[--SP] = LT0;
+	[--SP] = LB0;
+	[--SP] = LC1;
+	[--SP] = LT1;
+	[--SP] = LB1;
+
+	/* If it's a 1K or 4K page, then it's quickest to
+	 * just systematically flush all the addresses in
+	 * the page, regardless of whether they're in the
+	 * cache, or dirty. If it's a 1M or 4M page, there
+	 * are too many addresses, and we have to search the
+	 * cache for lines corresponding to the page.
+	 */
+
+	CC = BITTST(R1, 17);	/* 1MB or 4MB */
+	IF !CC JUMP iflush_whole_page;
+
+	/* We're only interested in the page's size, so extract
+	 * this from the CPLB (bits 17:16), and scale to give an
+	 * offset into the page_size and page_prefix tables.
+	 */
+
+	R1 <<= 14;
+	R1 >>= 30;
+	R1 <<= 2;
+
+	/* We can also determine the sub-bank used, because this is
+	 * taken from bits 13:12 of the address.
+	 */
+
+	R3 = ((12<<8)|2);		/* Extraction pattern */
+	nop;				/*Anamoly 05000209*/
+	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits*/
+	R3.H = R4.L << 0 ;		/* Save in extraction pattern for later deposit.*/
+
+
+	/* So:
+	 * R0 = Page start
+	 * R1 = Page length (actually, offset into size/prefix tables)
+	 * R3 = sub-bank deposit values
+	 *
+	 * The cache has 2 Ways, and 64 sets, so we iterate through
+	 * the sets, accessing the tag for each Way, for our Bank and
+	 * sub-bank, looking for dirty, valid tags that match our
+	 * address prefix.
+	 */
+
+	P5.L = (ITEST_COMMAND & 0xFFFF);
+	P5.H = (ITEST_COMMAND >> 16);
+	P4.L = (ITEST_DATA0 & 0xFFFF);
+	P4.H = (ITEST_DATA0 >> 16);
+
+	P0.L = page_prefix_table;
+	P0.H = page_prefix_table;
+	P1 = R1;
+	R5 = 0;			/* Set counter*/
+	P0 = P1 + P0;
+	R4 = [P0];		/* This is the address prefix*/
+
+	/* We're reading (bit 1==0) the tag (bit 2==0), and we
+	 * don't care about which double-word, since we're only
+	 * fetching tags, so we only have to set Set, Bank,
+	 * Sub-bank and Way.
+	 */
+
+	P2 = 4;
+	LSETUP (ifs1, ife1) LC1 = P2;
+ifs1:	P0 = 32;		/* iterate over all sets*/
+	LSETUP (ifs0, ife0) LC0 = P0;
+ifs0:	R6 = R5 << 5;		/* Combine set*/
+	R6.H = R3.H << 0 ;	/* and sub-bank*/
+	[P5] = R6;		/* Issue Command*/
+	SSYNC;			/* CSYNC will not work here :(*/
+	R7 = [P4];		/* and read Tag.*/
+	CC = BITTST(R7, 0);	/* Check if valid*/
+	IF !CC JUMP ifskip;	/* and skip if not.*/
+
+	/* Compare against the page address. First, plant bits 13:12
+	 * into the tag, since those aren't part of the returned data.
+	 */
+
+	R7 = DEPOSIT(R7, R3);	/* set 13:12*/
+	R1 = R7 & R4;		/* Mask off lower bits*/
+	CC = R1 == R0;		/* Compare against page start.*/
+	IF !CC JUMP ifskip;	/* Skip it if it doesn't match.*/
+
+	/* Tag address matches against page, so this is an entry
+	 * we must flush.
+	 */
+
+	R7 >>= 10;		/* Mask off the non-address bits*/
+	R7 <<= 10;
+	P3 = R7;
+	IFLUSH [P3];		/* And flush the entry*/
+ifskip:
+ife0:	R5 += 1;		/* Advance to next Set*/
+ife1:	NOP;
+
+ifinished:
+	SSYNC;			/* Ensure the data gets out to mem.*/
+
+	/*Finished. Restore context.*/
+	LB1 = [SP++];
+	LT1 = [SP++];
+	LC1 = [SP++];
+	LB0 = [SP++];
+	LT0 = [SP++];
+	LC0 = [SP++];
+	( R7:0, P5:0 ) = [SP++];
+	RTS;
+
+iflush_whole_page:
+	/* It's a 1K or 4K page, so quicker to just flush the
+	 * entire page.
+	 */
+
+	P1 = 32;		/* For 1K pages*/
+	P2 = P1 << 2;		/* For 4K pages*/
+	P0 = R0;		/* Start of page*/
+	CC = BITTST(R1, 16);	/* Whether 1K or 4K*/
+	IF CC P1 = P2;
+	P1 += -1;		/* Unroll one iteration*/
+	SSYNC;
+	IFLUSH [P0++];		/* because CSYNC can't end loops.*/
+	LSETUP (isall, ieall) LC0 = P1;
+isall:IFLUSH [P0++];
+ieall: NOP;
+	SSYNC;
+	JUMP ifinished;
+
+/* This is an external function being called by the user
+ * application through __flush_cache_all. Currently this function
+ * serves the purpose of flushing all the pending writes in
+ * in the data cache.
+ */
+
+ENTRY(_flush_data_cache)
+	[--SP] = ( R7:6, P5:4 );
+	LINK 12;
+	SP += -12;
+	P5.H = (DCPLB_ADDR0 >> 16);
+	P5.L = (DCPLB_ADDR0 & 0xFFFF);
+	P4.H = (DCPLB_DATA0 >> 16);
+	P4.L = (DCPLB_DATA0 & 0xFFFF);
+	R7 = CPLB_VALID | CPLB_L1_CHBL | CPLB_DIRTY (Z);
+	R6 = 16;
+next:	R0 = [P5++];
+	R1 = [P4++];
+	CC = BITTST(R1, 14);	/* Is it write-through?*/
+	IF CC JUMP skip;	/* If so, ignore it.*/
+	R2 = R1 & R7;		/* Is it a dirty, cached page?*/
+	CC = R2;
+	IF !CC JUMP skip;	/* If not, ignore it.*/
+	[--SP] = RETS;
+	CALL _dcplb_flush;	/* R0 = page, R1 = data*/
+	RETS = [SP++];
+skip:	R6 += -1;
+	CC = R6;
+	IF CC JUMP next;
+	SSYNC;
+	SP += 12;
+	UNLINK;
+	( R7:6, P5:4 ) = [SP++];
+	RTS;
+
+/* This is an internal function to flush all pending
+ * writes in the cache associated with a particular DCPLB.
+ *
+ * R0 -  page's start address
+ * R1 -  CPLB's data field.
+ */
+
+.align 2
+ENTRY(_dcplb_flush)
+	[--SP] = ( R7:0, P5:0 );
+	[--SP] = LC0;
+	[--SP] = LT0;
+	[--SP] = LB0;
+	[--SP] = LC1;
+	[--SP] = LT1;
+	[--SP] = LB1;
+
+	/* If it's a 1K or 4K page, then it's quickest to
+	 * just systematically flush all the addresses in
+	 * the page, regardless of whether they're in the
+	 * cache, or dirty. If it's a 1M or 4M page, there
+	 * are too many addresses, and we have to search the
+	 * cache for lines corresponding to the page.
+	 */
+
+	CC = BITTST(R1, 17);	/* 1MB or 4MB */
+	IF !CC JUMP dflush_whole_page;
+
+	/* We're only interested in the page's size, so extract
+	 * this from the CPLB (bits 17:16), and scale to give an
+	 * offset into the page_size and page_prefix tables.
+	 */
+
+	R1 <<= 14;
+	R1 >>= 30;
+	R1 <<= 2;
+
+	/* The page could be mapped into Bank A or Bank B, depending
+	 * on (a) whether both banks are configured as cache, and
+	 * (b) on whether address bit A[x] is set. x is determined
+	 * by DCBS in DMEM_CONTROL
+	 */
+
+	R2 = 0;			/* Default to Bank A (Bank B would be 1)*/
+
+	P0.L = (DMEM_CONTROL & 0xFFFF);
+	P0.H = (DMEM_CONTROL >> 16);
+
+	R3 = [P0];		/* If Bank B is not enabled as cache*/
+	CC = BITTST(R3, 2);	/* then Bank A is our only option.*/
+	IF CC JUMP bank_chosen;
+
+	R4 = 1<<14;		/* If DCBS==0, use A[14].*/
+	R5 = R4 << 7;		/* If DCBS==1, use A[23];*/
+	CC = BITTST(R3, 4);
+	IF CC R4 = R5;		/* R4 now has either bit 14 or bit 23 set.*/
+	R5 = R0 & R4;		/* Use it to test the Page address*/
+	CC = R5;		/* and if that bit is set, we use Bank B,*/
+	R2 = CC;		/* else we use Bank A.*/
+	R2 <<= 23;		/* The Bank selection's at posn 23.*/
+
+bank_chosen:
+
+	/* We can also determine the sub-bank used, because this is
+	 * taken from bits 13:12 of the address.
+	 */
+
+	R3 = ((12<<8)|2);		/* Extraction pattern */
+	nop;				/*Anamoly 05000209*/
+	R4 = EXTRACT(R0, R3.L) (Z);	/* Extract bits*/
+	/* Save in extraction pattern for later deposit.*/
+	R3.H = R4.L << 0;
+
+	/* So:
+	 * R0 = Page start
+	 * R1 = Page length (actually, offset into size/prefix tables)
+	 * R2 = Bank select mask
+	 * R3 = sub-bank deposit values
+	 *
+	 * The cache has 2 Ways, and 64 sets, so we iterate through
+	 * the sets, accessing the tag for each Way, for our Bank and
+	 * sub-bank, looking for dirty, valid tags that match our
+	 * address prefix.
+	 */
+
+	P5.L = (DTEST_COMMAND & 0xFFFF);
+	P5.H = (DTEST_COMMAND >> 16);
+	P4.L = (DTEST_DATA0 & 0xFFFF);
+	P4.H = (DTEST_DATA0 >> 16);
+
+	P0.L = page_prefix_table;
+	P0.H = page_prefix_table;
+	P1 = R1;
+	R5 = 0;			/* Set counter*/
+	P0 = P1 + P0;
+	R4 = [P0];		/* This is the address prefix*/
+
+
+	/* We're reading (bit 1==0) the tag (bit 2==0), and we
+	 * don't care about which double-word, since we're only
+	 * fetching tags, so we only have to set Set, Bank,
+	 * Sub-bank and Way.
+	 */
+
+	P2 = 2;
+	LSETUP (fs1, fe1) LC1 = P2;
+fs1:	P0 = 64;		/* iterate over all sets*/
+	LSETUP (fs0, fe0) LC0 = P0;
+fs0:	R6 = R5 << 5;		/* Combine set*/
+	R6.H = R3.H << 0 ;	/* and sub-bank*/
+	R6 = R6 | R2;		/* and Bank. Leave Way==0 at first.*/
+	BITSET(R6,14);
+	[P5] = R6;		/* Issue Command*/
+	SSYNC;
+	R7 = [P4];		/* and read Tag.*/
+	CC = BITTST(R7, 0);	/* Check if valid*/
+	IF !CC JUMP fskip;	/* and skip if not.*/
+	CC = BITTST(R7, 1);	/* Check if dirty*/
+	IF !CC JUMP fskip;	/* and skip if not.*/
+
+	/* Compare against the page address. First, plant bits 13:12
+	 * into the tag, since those aren't part of the returned data.
+	 */
+
+	R7 = DEPOSIT(R7, R3);	/* set 13:12*/
+	R1 = R7 & R4;		/* Mask off lower bits*/
+	CC = R1 == R0;		/* Compare against page start.*/
+	IF !CC JUMP fskip;	/* Skip it if it doesn't match.*/
+
+	/* Tag address matches against page, so this is an entry
+	 * we must flush.
+	 */
+
+	R7 >>= 10;		/* Mask off the non-address bits*/
+	R7 <<= 10;
+	P3 = R7;
+	SSYNC;
+	FLUSHINV [P3];		/* And flush the entry*/
+fskip:
+fe0:	R5 += 1;		/* Advance to next Set*/
+fe1:	BITSET(R2, 26);		/* Go to next Way.*/
+
+dfinished:
+	SSYNC;			/* Ensure the data gets out to mem.*/
+
+	/*Finished. Restore context.*/
+	LB1 = [SP++];
+	LT1 = [SP++];
+	LC1 = [SP++];
+	LB0 = [SP++];
+	LT0 = [SP++];
+	LC0 = [SP++];
+	( R7:0, P5:0 ) = [SP++];
+	RTS;
+
+dflush_whole_page:
+
+	/* It's a 1K or 4K page, so quicker to just flush the
+	 * entire page.
+	 */
+
+	P1 = 32;		/* For 1K pages*/
+	P2 = P1 << 2;		/* For 4K pages*/
+	P0 = R0;		/* Start of page*/
+	CC = BITTST(R1, 16);	/* Whether 1K or 4K*/
+	IF CC P1 = P2;
+	P1 += -1;		/* Unroll one iteration*/
+	SSYNC;
+	FLUSHINV [P0++];	/* because CSYNC can't end loops.*/
+	LSETUP (eall, eall) LC0 = P1;
+eall:	FLUSHINV [P0++];
+	SSYNC;
+	JUMP dfinished;
+
+.align 4;
+page_prefix_table:
+.byte4 	0xFFFFFC00;	/* 1K */
+.byte4	0xFFFFF000;	/* 4K */
+.byte4	0xFFF00000;	/* 1M */
+.byte4	0xFFC00000;	/* 4M */
+.page_prefix_table.end:
diff --git a/cpu/bf561/init_sdram.S b/cpu/bf561/init_sdram.S
new file mode 100644
index 0000000..d763f27
--- /dev/null
+++ b/cpu/bf561/init_sdram.S
@@ -0,0 +1,171 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV   CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV   CONFIG_CCLK_DIV_not_defined_properly
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS	   : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN	   : 1=All Clocks off
+	 *   - [3]     = STOPCK	   : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF	   : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier */
+	r0 = r0 << 9;			/* Shift it over, */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2? */
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL? */
+	r1 = r1 << 8;			/* Shift it over */
+	r0 = r1 | r0;			/* add them all together */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address */
+	cli r2;				/* Disable interrupts */
+	ssync;
+	w[p0] = r0.l;			/* Set the value */
+	idle;				/* Wait for the PLL to stablize */
+	sti r2;				/* Enable interrupts */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 */
+
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf561/init_sdram_bootrom_initblock.S b/cpu/bf561/init_sdram_bootrom_initblock.S
new file mode 100644
index 0000000..5e3c88a
--- /dev/null
+++ b/cpu/bf561/init_sdram_bootrom_initblock.S
@@ -0,0 +1,185 @@
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <asm/mem_init.h>
+.global init_sdram;
+
+#if (CONFIG_CCLK_DIV == 1)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV1
+#endif
+#if (CONFIG_CCLK_DIV == 2)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV2
+#endif
+#if (CONFIG_CCLK_DIV == 4)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV4
+#endif
+#if (CONFIG_CCLK_DIV == 8)
+#define CONFIG_CCLK_ACT_DIV	CCLK_DIV8
+#endif
+#ifndef CONFIG_CCLK_ACT_DIV
+#define CONFIG_CCLK_ACT_DIV	CONFIG_CCLK_DIV_not_defined_properly
+#endif
+
+init_sdram:
+	[--SP] = ASTAT;
+	[--SP] = RETS;
+	[--SP] = (R7:0);
+	[--SP] = (P5:0);
+
+
+	p0.h = hi(SICA_IWR0);
+	p0.l = lo(SICA_IWR0);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+
+	p0.h = hi(SPI_BAUD);
+	p0.l = lo(SPI_BAUD);
+	r0.l = CONFIG_SPI_BAUD_INITBLOCK;
+	w[p0] = r0.l;
+	SSYNC;
+
+	/*
+	 * PLL_LOCKCNT - how many SCLK Cycles to delay while PLL becomes stable
+	 */
+	p0.h = hi(PLL_LOCKCNT);
+	p0.l = lo(PLL_LOCKCNT);
+	r0 = 0x300(Z);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * Put SDRAM in self-refresh, incase anything is running
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITSET (R0, 24);
+	[P2] = R0;
+	SSYNC;
+
+	/*
+	 *  Set PLL_CTL with the value that we calculate in R0
+	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
+	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
+	 *   - [7]     = output delay (add 200ps of delay to mem signals)
+	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
+	 *   - [5]     = PDWN	   : 1=All Clocks off
+	 *   - [3]     = STOPCK	   : 1=Core Clock off
+	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
+	 *   - [0]     = DF	   : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
+	 *   all other bits set to zero
+	 */
+
+	r0 = CONFIG_VCO_MULT & 63;	/* Load the VCO multiplier */
+	r0 = r0 << 9;			/* Shift it over, */
+	r1 = CONFIG_CLKIN_HALF;		/* Do we need to divide CLKIN by 2? */
+	r0 = r1 | r0;
+	r1 = CONFIG_PLL_BYPASS;		/* Bypass the PLL? */
+	r1 = r1 << 8;			/* Shift it over */
+	r0 = r1 | r0;			/* add them all together */
+
+	p0.h = hi(PLL_CTL);
+	p0.l = lo(PLL_CTL);		/* Load the address */
+	cli r2;				/* Disable interrupts */
+	ssync;
+	w[p0] = r0.l;			/* Set the value */
+	idle;				/* Wait for the PLL to stablize */
+	sti r2;				/* Enable interrupts */
+
+check_again:
+	p0.h = hi(PLL_STAT);
+	p0.l = lo(PLL_STAT);
+	R0 = W[P0](Z);
+	CC = BITTST(R0,5);
+	if ! CC jump check_again;
+
+	/* Configure SCLK & CCLK Dividers */
+	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+	p0.h = hi(PLL_DIV);
+	p0.l = lo(PLL_DIV);
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * We now are running at speed, time to set the Async mem bank wait states
+	 * This will speed up execution, since we are normally running from FLASH.
+	 */
+
+	p2.h = (EBIU_AMBCTL1 >> 16);
+	p2.l = (EBIU_AMBCTL1 & 0xFFFF);
+	r0.h = (AMBCTL1VAL >> 16);
+	r0.l = (AMBCTL1VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMBCTL0 >> 16);
+	p2.l = (EBIU_AMBCTL0 & 0xFFFF);
+	r0.h = (AMBCTL0VAL >> 16);
+	r0.l = (AMBCTL0VAL & 0xFFFF);
+	[p2] = r0;
+	ssync;
+
+	p2.h = (EBIU_AMGCTL >> 16);
+	p2.l = (EBIU_AMGCTL & 0xffff);
+	r0 = AMGCTLVAL;
+	w[p2] = r0;
+	ssync;
+
+	/*
+	 * Now, Initialize the SDRAM,
+	 * start with the SDRAM Refresh Rate Control Register
+	 */
+	p0.l = lo(EBIU_SDRRC);
+	p0.h = hi(EBIU_SDRRC);
+	r0 = mem_SDRRC;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Memory Bank Control Register - bank specific parameters
+	 */
+	p0.l = (EBIU_SDBCTL & 0xFFFF);
+	p0.h = (EBIU_SDBCTL >> 16);
+	r0 = mem_SDBCTL;
+	w[p0] = r0.l;
+	ssync;
+
+	/*
+	 * SDRAM Global Control Register - global programmable parameters
+	 * Disable self-refresh
+	 */
+	P2.H = hi(EBIU_SDGCTL);
+	P2.L = lo(EBIU_SDGCTL);
+	R0 = [P2];
+	BITCLR (R0, 24);
+
+	/*
+	 * Check if SDRAM is already powered up, if it is, enable self-refresh
+	 */
+	p0.h = hi(EBIU_SDSTAT);
+	p0.l = lo(EBIU_SDSTAT);
+	r2.l = w[p0];
+	cc = bittst(r2,3);
+	if !cc jump skip;
+	NOP;
+	BITSET (R0, 23);
+skip:
+	[P2] = R0;
+	SSYNC;
+
+	/* Write in the new value in the register */
+	R0.L = lo(mem_SDGCTL);
+	R0.H = hi(mem_SDGCTL);
+	[P2] = R0;
+	SSYNC;
+	nop;
+
+
+	(P5:0) = [SP++];
+	(R7:0) = [SP++];
+	RETS   = [SP++];
+	ASTAT  = [SP++];
+	RTS;
diff --git a/cpu/bf561/interrupt.S b/cpu/bf561/interrupt.S
new file mode 100644
index 0000000..f82fd9b
--- /dev/null
+++ b/cpu/bf561/interrupt.S
@@ -0,0 +1,246 @@
+/*
+ * 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>
+
+.global _blackfin_irq_panic;
+
+.text
+.align 2
+
+#ifndef CONFIG_KGDB
+.global _evt_emulation
+_evt_emulation:
+	SAVE_CONTEXT
+	r0 = IRQ_EMU;
+	r1 = seqstat;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+	rte;
+#endif
+
+.global _evt_nmi
+_evt_nmi:
+	SAVE_CONTEXT
+	r0 = IRQ_NMI;
+	r1 = RETN;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+
+_evt_nmi_exit:
+	rtn;
+
+.global _trap
+_trap:
+	SAVE_ALL_SYS
+	r0 = sp;	/* stack frame pt_regs pointer argument ==> r0 */
+	sp += -12;
+	call _trap_c
+	sp += 12;
+	RESTORE_ALL_SYS
+	rtx;
+
+.global _evt_rst
+_evt_rst:
+	SAVE_CONTEXT
+	r0 = IRQ_RST;
+	r1 = RETN;
+	sp += -12;
+	call _do_reset;
+	sp += 12;
+
+_evt_rst_exit:
+	rtn;
+
+irq_panic:
+	r0 = IRQ_EVX;
+	r1 =  sp;
+	sp += -12;
+	call _blackfin_irq_panic;
+	sp += 12;
+
+.global _evt_ivhw
+_evt_ivhw:
+	SAVE_CONTEXT
+	RAISE 14;
+
+_evt_ivhw_exit:
+	 rti;
+
+.global _evt_timer
+_evt_timer:
+	SAVE_CONTEXT
+	r0 = IRQ_CORETMR;
+	sp += -12;
+	/* Polling method used now. */
+	/* call timer_int; */
+	sp += 12;
+	RESTORE_CONTEXT
+	rti;
+	nop;
+
+.global _evt_evt7
+_evt_evt7:
+	SAVE_CONTEXT
+	r0 = 7;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt7_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt8
+_evt_evt8:
+	SAVE_CONTEXT
+	r0 = 8;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt8_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt9
+_evt_evt9:
+	SAVE_CONTEXT
+	r0 = 9;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt9_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt10
+_evt_evt10:
+	SAVE_CONTEXT
+	r0 = 10;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt10_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt11
+_evt_evt11:
+	SAVE_CONTEXT
+	r0 = 11;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt11_exit:
+	RESTORE_CONTEXT
+	rti;
+
+.global _evt_evt12
+_evt_evt12:
+	SAVE_CONTEXT
+	r0 = 12;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+evt_evt12_exit:
+	 RESTORE_CONTEXT
+	 rti;
+
+.global _evt_evt13
+_evt_evt13:
+	SAVE_CONTEXT
+	r0 = 13;
+	sp += -12;
+	call _process_int;
+	sp += 12;
+
+evt_evt13_exit:
+	 RESTORE_CONTEXT
+	 rti;
+
+.global _evt_system_call
+_evt_system_call:
+	[--sp] = r0;
+	[--SP] = RETI;
+	r0 = [sp++];
+	r0 += 2;
+	[--sp] = r0;
+	RETI = [SP++];
+	r0 = [SP++];
+	SAVE_CONTEXT
+	sp += -12;
+	call _exception_handle;
+	sp += 12;
+	RESTORE_CONTEXT
+	RTI;
+
+evt_system_call_exit:
+	rti;
+
+.global _evt_soft_int1
+_evt_soft_int1:
+	[--sp] = r0;
+	[--SP] = RETI;
+	r0 = [sp++];
+	r0 += 2;
+	[--sp] = r0;
+	RETI = [SP++];
+	r0 = [SP++];
+	SAVE_CONTEXT
+	sp += -12;
+	call _exception_handle;
+	sp += 12;
+	RESTORE_CONTEXT
+	RTI;
+
+evt_soft_int1_exit:
+	rti;
diff --git a/cpu/bf561/interrupts.c b/cpu/bf561/interrupts.c
new file mode 100644
index 0000000..e314f60
--- /dev/null
+++ b/cpu/bf561/interrupts.c
@@ -0,0 +1,171 @@
+/*
+ * U-boot - interrupts.c Interrupt related routines
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * This file is based on interrupts.c
+ * Copyright 1996 Roman Zippel
+ * Copyright 1999 D. Jeff Dionne <jeff@uclinux.org>
+ * Copyright 2000-2001 Lineo, Inc. D. Jefff Dionne <jeff@lineo.ca>
+ * Copyright 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
+ * Copyright 2003 Metrowerks/Motorola
+ * Copyright 2003 Bas Vermeulen <bas@buyways.nl>,
+ *			BuyWays B.V. (www.buyways.nl)
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/machdep.h>
+#include <asm/irq.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include "cpu.h"
+
+static ulong timestamp;
+static ulong last_time;
+static int int_flag;
+
+int irq_flags;			/* needed by asm-blackfin/system.h */
+
+/* Functions just to satisfy the linker */
+
+/*
+ * This function is derived from PowerPC code (read timebase as long long).
+ * On BF561 it just returns the timer value.
+ */
+unsigned long long get_ticks(void)
+{
+	return get_timer(0);
+}
+
+/*
+ * This function is derived from PowerPC code (timebase clock frequency).
+ * On BF561 it returns the number of timer ticks per second.
+ */
+ulong get_tbclk(void)
+{
+	ulong tbclk;
+
+	tbclk = CFG_HZ;
+	return tbclk;
+}
+
+void enable_interrupts(void)
+{
+	restore_flags(int_flag);
+}
+
+int disable_interrupts(void)
+{
+	save_and_cli(int_flag);
+	return 1;
+}
+
+int interrupt_init(void)
+{
+	return (0);
+}
+
+void udelay(unsigned long usec)
+{
+	unsigned long delay, start, stop;
+	unsigned long cclk;
+	cclk = (CONFIG_CCLK_HZ);
+
+	while (usec > 1) {
+		/*
+		 * how many clock ticks to delay?
+		 *  - request(in useconds) * clock_ticks(Hz) / useconds/second
+		 */
+		if (usec < 1000) {
+			delay = (usec * (cclk / 244)) >> 12;
+			usec = 0;
+		} else {
+			delay = (1000 * (cclk / 244)) >> 12;
+			usec -= 1000;
+		}
+
+		asm volatile (" %0 = CYCLES;":"=r" (start));
+		do {
+			asm volatile (" %0 = CYCLES; ":"=r" (stop));
+		} while (stop - start < delay);
+	}
+
+	return;
+}
+
+void timer_init(void)
+{
+	*pTCNTL = 0x1;
+	*pTSCALE = 0x0;
+	*pTCOUNT = MAX_TIM_LOAD;
+	*pTPERIOD = MAX_TIM_LOAD;
+	*pTCNTL = 0x7;
+	asm("CSYNC;");
+
+	timestamp = 0;
+	last_time = 0;
+}
+
+/*
+ * Any network command or flash
+ * command is started get_timer shall
+ * be called before TCOUNT gets reset,
+ * to implement the accurate timeouts.
+ *
+ * How ever milliconds doesn't return
+ * the number that has been elapsed from
+ * the last reset.
+ *
+ * As get_timer is used in the u-boot
+ * only for timeouts this should be
+ * sufficient
+ */
+ulong get_timer(ulong base)
+{
+	ulong milisec;
+
+	/* Number of clocks elapsed */
+	ulong clocks = (MAX_TIM_LOAD - (*pTCOUNT));
+
+	/*
+	 * Find if the TCOUNT is reset
+	 * timestamp gives the number of times
+	 * TCOUNT got reset
+	 */
+	if (clocks < last_time)
+		timestamp++;
+	last_time = clocks;
+
+	/* Get the number of milliseconds */
+	milisec = clocks / (CONFIG_CCLK_HZ / 1000);
+
+	/*
+	 * Find the number of millisonds
+	 * that got elapsed before this TCOUNT
+	 * cycle
+	 */
+	milisec += timestamp * (MAX_TIM_LOAD / (CONFIG_CCLK_HZ / 1000));
+
+	return (milisec - base);
+}
diff --git a/cpu/bf561/ints.c b/cpu/bf561/ints.c
new file mode 100644
index 0000000..328e5d8
--- /dev/null
+++ b/cpu/bf561/ints.c
@@ -0,0 +1,117 @@
+/*
+ * U-boot - ints.c Interrupt related routines
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * This file is based on ints.c
+ *
+ * Apr18 2003, Changed by HuTao to support interrupt cascading for Blackfin
+ *	drivers
+ *
+ * Copyright 1996 Roman Zippel
+ * Copyright 1999 D. Jeff Dionne <jeff@uclinux.org>
+ * Copyright 2000-2001 Lineo, Inc. D. Jefff Dionne <jeff@lineo.ca>
+ * Copyright 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
+ * Copyright 2003 Metrowerks/Motorola
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <linux/stddef.h>
+#include <asm/system.h>
+#include <asm/irq.h>
+#include <asm/traps.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/machdep.h>
+#include <asm/setup.h>
+#include <asm/blackfin.h>
+#include "cpu.h"
+
+void blackfin_irq_panic(int reason, struct pt_regs *regs)
+{
+	printf("\n\nException: IRQ 0x%x entered\n", reason);
+	printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+	printf("stack frame=0x%x, ", (unsigned int)regs);
+	printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
+	dump(regs);
+	printf("Unhandled IRQ or exceptions!\n");
+	printf("Please reset the board \n");
+}
+
+void blackfin_init_IRQ(void)
+{
+	*(unsigned volatile long *)(SIC_IMASK) = SIC_UNMASK_ALL;
+	cli();
+#ifndef CONFIG_KGDB
+	*(unsigned volatile long *)(EVT_EMULATION_ADDR) = 0x0;
+#endif
+	*(unsigned volatile long *)(EVT_NMI_ADDR) =
+	    (unsigned volatile long)evt_nmi;
+	*(unsigned volatile long *)(EVT_EXCEPTION_ADDR) =
+	    (unsigned volatile long)trap;
+	*(unsigned volatile long *)(EVT_HARDWARE_ERROR_ADDR) =
+	    (unsigned volatile long)evt_ivhw;
+	*(unsigned volatile long *)(EVT_RESET_ADDR) =
+	    (unsigned volatile long)evt_rst;
+	*(unsigned volatile long *)(EVT_TIMER_ADDR) =
+	    (unsigned volatile long)evt_timer;
+	*(unsigned volatile long *)(EVT_IVG7_ADDR) =
+	    (unsigned volatile long)evt_evt7;
+	*(unsigned volatile long *)(EVT_IVG8_ADDR) =
+	    (unsigned volatile long)evt_evt8;
+	*(unsigned volatile long *)(EVT_IVG9_ADDR) =
+	    (unsigned volatile long)evt_evt9;
+	*(unsigned volatile long *)(EVT_IVG10_ADDR) =
+	    (unsigned volatile long)evt_evt10;
+	*(unsigned volatile long *)(EVT_IVG11_ADDR) =
+	    (unsigned volatile long)evt_evt11;
+	*(unsigned volatile long *)(EVT_IVG12_ADDR) =
+	    (unsigned volatile long)evt_evt12;
+	*(unsigned volatile long *)(EVT_IVG13_ADDR) =
+	    (unsigned volatile long)evt_evt13;
+	*(unsigned volatile long *)(EVT_IVG14_ADDR) =
+	    (unsigned volatile long)evt_system_call;
+	*(unsigned volatile long *)(EVT_IVG15_ADDR) =
+	    (unsigned volatile long)evt_soft_int1;
+	*(volatile unsigned long *)ILAT = 0;
+	asm("csync;");
+	sti();
+	*(volatile unsigned long *)IMASK = 0xffbf;
+	asm("csync;");
+}
+
+void exception_handle(void)
+{
+#if defined (CONFIG_PANIC_HANG)
+	display_excp();
+#else
+	udelay(100000);		/* allow messages to go out */
+	do_reset(NULL, 0, 0, NULL);
+#endif
+}
+
+void display_excp(void)
+{
+	printf("Exception!\n");
+}
diff --git a/cpu/bf561/serial.c b/cpu/bf561/serial.c
new file mode 100644
index 0000000..baec1d3
--- /dev/null
+++ b/cpu/bf561/serial.c
@@ -0,0 +1,196 @@
+/*
+ * U-boot - serial.c Serial driver for BF561
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * This file is based on
+ * bf533_serial.c: Serial driver for BlackFin BF533 DSP internal UART.
+ * Copyright (c) 2003	Bas Vermeulen <bas@buyways.nl>,
+ * 			BuyWays B.V. (www.buyways.nl)
+ *
+ * Based heavily on blkfinserial.c
+ * blkfinserial.c: Serial driver for BlackFin DSP internal USRTs.
+ * Copyright(c) 2003	Metrowerks	<mwaddel@metrowerks.com>
+ * Copyright(c)	2001	Tony Z. Kou	<tonyko@arcturusnetworks.com>
+ * Copyright(c)	2001-2002 Arcturus Networks Inc. <www.arcturusnetworks.com>
+ *
+ * Based on code from 68328 version serial driver imlpementation which was:
+ * Copyright (C) 1995       David S. Miller    <davem@caip.rutgers.edu>
+ * Copyright (C) 1998       Kenneth Albanowski <kjahds@kjahds.com>
+ * Copyright (C) 1998, 1999 D. Jeff Dionne     <jeff@uclinux.org>
+ * Copyright (C) 1999       Vladimir Gurevich  <vgurevic@cisco.com>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/segment.h>
+#include <asm/bitops.h>
+#include <asm/delay.h>
+#include <asm/uaccess.h>
+#include "serial.h"
+#include <asm/io.h>
+
+unsigned long pll_div_fact;
+
+void calc_baud(void)
+{
+	unsigned char i;
+	int temp;
+	u_long sclk = get_sclk();
+
+	for (i = 0; i < sizeof(baud_table) / sizeof(int); i++) {
+		temp = sclk / (baud_table[i] * 8);
+		if ((temp & 0x1) == 1) {
+			temp++;
+		}
+		temp = temp / 2;
+		hw_baud_table[i].dl_high = (temp >> 8) & 0xFF;
+		hw_baud_table[i].dl_low = (temp) & 0xFF;
+	}
+}
+
+void serial_setbrg(void)
+{
+	int i;
+	DECLARE_GLOBAL_DATA_PTR;
+
+	calc_baud();
+
+	for (i = 0; i < sizeof(baud_table) / sizeof(int); i++) {
+		if (gd->baudrate == baud_table[i])
+			break;
+	}
+
+	/* Enable UART */
+	*pUART_GCTL |= UART_GCTL_UCEN;
+	sync();
+
+	/* Set DLAB in LCR to Access DLL and DLH */
+	ACCESS_LATCH;
+	sync();
+
+	*pUART_DLL = hw_baud_table[i].dl_low;
+	sync();
+	*pUART_DLH = hw_baud_table[i].dl_high;
+	sync();
+
+	/* Clear DLAB in LCR to Access THR RBR IER */
+	ACCESS_PORT_IER;
+	sync();
+
+	/*
+	 * Enable  ERBFI and ELSI interrupts
+	 * to poll SIC_ISR register
+	 */
+	*pUART_IER = UART_IER_ELSI | UART_IER_ERBFI | UART_IER_ETBEI;
+	sync();
+
+	/* Set LCR to Word Lengh 8-bit word select */
+	*pUART_LCR = UART_LCR_WLS8;
+	sync();
+
+	return;
+}
+
+int serial_init(void)
+{
+	serial_setbrg();
+	return (0);
+}
+
+void serial_putc(const char c)
+{
+	if ((*pUART_LSR) & UART_LSR_TEMT) {
+		if (c == '\n')
+			serial_putc('\r');
+
+		local_put_char(c);
+	}
+
+	while (!((*pUART_LSR) & UART_LSR_TEMT))
+		SYNC_ALL;
+
+	return;
+}
+
+int serial_tstc(void)
+{
+	if (*pUART_LSR & UART_LSR_DR)
+		return 1;
+	else
+		return 0;
+}
+
+int serial_getc(void)
+{
+	unsigned short uart_lsr_val, uart_rbr_val;
+	unsigned long isr_val;
+	int ret;
+
+	/* Poll for RX Interrupt */
+	while (!((isr_val =
+		  *(volatile unsigned long *)SIC_ISR) & IRQ_UART_RX_BIT)) ;
+	asm("csync;");
+
+	uart_lsr_val = *pUART_LSR;	/* Clear status bit */
+	uart_rbr_val = *pUART_RBR;	/* getc() */
+
+	if (isr_val & IRQ_UART_ERROR_BIT) {
+		ret = -1;
+	} else {
+		ret = uart_rbr_val & 0xff;
+	}
+
+	return ret;
+}
+
+void serial_puts(const char *s)
+{
+	while (*s) {
+		serial_putc(*s++);
+	}
+}
+
+static void local_put_char(char ch)
+{
+	int flags = 0;
+	unsigned long isr_val;
+
+	save_and_cli(flags);
+
+	/* Poll for TX Interruput */
+	while (!((isr_val = *pSIC_ISR) & IRQ_UART_TX_BIT)) ;
+	asm("csync;");
+
+	*pUART_THR = ch;	/* putc() */
+
+	if (isr_val & IRQ_UART_ERROR_BIT) {
+		printf("?");
+	}
+
+	restore_flags(flags);
+
+	return;
+}
diff --git a/cpu/bf561/serial.h b/cpu/bf561/serial.h
new file mode 100644
index 0000000..98c1242
--- /dev/null
+++ b/cpu/bf561/serial.h
@@ -0,0 +1,77 @@
+/*
+ * U-boot - bf561_serial.h Serial Driver defines
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * This file is based on
+ * bf533_serial.h: Definitions for the BlackFin BF533 DSP serial driver.
+ * Copyright (C) 2003	Bas Vermeulen <bas@buyways.nl>
+ * 			BuyWays B.V. (www.buyways.nl)
+ *
+ * Based heavily on:
+ * blkfinserial.h: Definitions for the BlackFin DSP serial driver.
+ *
+ * Copyright (C) 2001	Tony Z. Kou	tonyko@arcturusnetworks.com
+ * Copyright (C) 2001   Arcturus Networks Inc. <www.arcturusnetworks.com>
+ *
+ * Based on code from 68328serial.c which was:
+ * Copyright (C) 1995       David S. Miller    <davem@caip.rutgers.edu>
+ * Copyright (C) 1998       Kenneth Albanowski <kjahds@kjahds.com>
+ * Copyright (C) 1998, 1999 D. Jeff Dionne     <jeff@uclinux.org>
+ * Copyright (C) 1999       Vladimir Gurevich  <vgurevic@cisco.com>
+ *
+ * (C) Copyright 2000-2004
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#ifndef _Bf561_SERIAL_H
+#define _Bf561_SERIAL_H
+
+#include <linux/config.h>
+#include <asm/blackfin.h>
+
+#define SYNC_ALL	__asm__ __volatile__ ("ssync;\n")
+#define ACCESS_LATCH	*pUART_LCR |= UART_LCR_DLAB;
+#define ACCESS_PORT_IER	*pUART_LCR &= (~UART_LCR_DLAB);
+
+void serial_setbrg(void);
+static void local_put_char(char ch);
+void calc_baud(void);
+void serial_setbrg(void);
+int serial_init(void);
+void serial_putc(const char c);
+int serial_tstc(void);
+int serial_getc(void);
+void serial_puts(const char *s);
+static void local_put_char(char ch);
+
+int baud_table[5] = { 9600, 19200, 38400, 57600, 115200 };
+
+struct {
+	unsigned char dl_high;
+	unsigned char dl_low;
+} hw_baud_table[5];
+
+#ifdef CONFIG_STAMP
+extern unsigned long pll_div_fact;
+#endif
+
+#endif
diff --git a/cpu/bf561/start.S b/cpu/bf561/start.S
new file mode 100644
index 0000000..9333648
--- /dev/null
+++ b/cpu/bf561/start.S
@@ -0,0 +1,311 @@
+/*
+ * U-boot - start.S Startup file of u-boot for BF533/BF561
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * This file is based on head.S
+ * Copyright (c) 2003  Metrowerks/Motorola
+ * Copyright (C) 1998  D. Jeff Dionne <jeff@ryeham.ee.ryerson.ca>,
+ *		       Kenneth Albanowski <kjahds@kjahds.com>,
+ *		       The Silver Hammer Group, Ltd.
+ * (c) 1995, Dionne & Associates
+ * (c) 1995, DKG Display Tech.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+/*
+ * Note: A change in this file subsequently requires a change in
+ *       board/$(board_name)/config.mk for a valid u-boot.bin
+ */
+
+#define ASSEMBLY
+
+#include <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.global _stext;
+.global __bss_start;
+.global start;
+.global _start;
+.global _rambase;
+.global _ramstart;
+.global _ramend;
+.global edata;
+.global _initialize;
+.global _exit;
+.global flashdataend;
+.global init_sdram;
+
+.text
+_start:
+start:
+_stext:
+
+	R0 = 0x32;
+	SYSCFG = R0;
+	SSYNC;
+
+	/*
+	 * As per HW reference manual DAG registers,
+	 * DATA and Address resgister shall be zero'd
+	 * in initialization, after a reset state
+	 */
+	r1 = 0;	/* Data registers zero'd */
+	r2 = 0;
+	r3 = 0;
+	r4 = 0;
+	r5 = 0;
+	r6 = 0;
+	r7 = 0;
+
+	p0 = 0; /* Address registers zero'd */
+	p1 = 0;
+	p2 = 0;
+	p3 = 0;
+	p4 = 0;
+	p5 = 0;
+
+	i0 = 0; /* DAG Registers zero'd */
+	i1 = 0;
+	i2 = 0;
+	i3 = 0;
+	m0 = 0;
+	m1 = 0;
+	m3 = 0;
+	m3 = 0;
+	l0 = 0;
+	l1 = 0;
+	l2 = 0;
+	l3 = 0;
+	b0 = 0;
+	b1 = 0;
+	b2 = 0;
+	b3 = 0;
+
+	/*
+	 * Set loop counters to zero, to make sure that
+	 * hw loops are disabled.
+	 */
+	r0  = 0;
+	lc0 = r0;
+	lc1 = r0;
+
+	SSYNC;
+
+	/* Check soft reset status */
+	p0.h = SWRST >> 16;
+	p0.l = SWRST & 0xFFFF;
+	r0.l = w[p0];
+
+	cc = bittst(r0, 15);
+	if !cc jump no_soft_reset;
+
+	/* Clear Soft reset */
+	r0 = 0x0000;
+	w[p0] = r0;
+	ssync;
+
+no_soft_reset:
+	nop;
+
+	/* Clear EVT registers */
+	p0.h = (EVT_EMULATION_ADDR >> 16);
+	p0.l = (EVT_EMULATION_ADDR & 0xFFFF);
+	p0 += 8;
+	p1 = 14;
+	r1 = 0;
+	LSETUP(4,4) lc0 = p1;
+	[ p0 ++ ] = r1;
+
+	p0.h = hi(SIC_IWR);
+	p0.l = lo(SIC_IWR);
+	r0.l = 0x1;
+	w[p0] = r0.l;
+	SSYNC;
+
+	sp.l = (0xffb01000 & 0xFFFF);
+	sp.h = (0xffb01000 >> 16);
+
+	/*
+	 * Check if the code is in SDRAM
+	 * If the code is in SDRAM, skip SDRAM initializaiton
+	 */
+	call get_pc;
+	r3.l = 0x0;
+	r3.h = 0x2000;
+	cc = r0 < r3 (iu);
+	if cc jump sdram_initialized;
+	call init_sdram;
+	/* relocate into to RAM */
+sdram_initialized:
+	call get_pc;
+offset:
+	r2.l = offset;
+	r2.h = offset;
+	r3.l = start;
+	r3.h = start;
+	r1 = r2 - r3;
+
+	r0 = r0 - r1;
+	p1 = r0;
+
+	p2.l = (CFG_MONITOR_BASE & 0xffff);
+	p2.h = (CFG_MONITOR_BASE >> 16);
+
+	p3 = 0x04;
+	p4.l = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) & 0xffff);
+	p4.h = ((CFG_MONITOR_BASE + CFG_MONITOR_LEN) >> 16);
+loop1:
+	r1 = [p1 ++ p3];
+	[p2 ++ p3] = r1;
+	cc=p2==p4;
+	if !cc jump loop1;
+	/*
+	 * configure STACK
+	 */
+	r0.h = (CONFIG_STACKBASE >> 16);
+	r0.l = (CONFIG_STACKBASE & 0xFFFF);
+	sp = r0;
+	fp = sp;
+
+	/*
+	 * This next section keeps the processor in supervisor mode
+	 * during kernel boot.  Switches to user mode at end of boot.
+	 * See page 3-9 of Hardware Reference manual for documentation.
+	 */
+
+	/* To keep ourselves in the supervisor mode */
+	p0.l = (EVT_IVG15_ADDR & 0xFFFF);
+	p0.h = (EVT_IVG15_ADDR >> 16);
+
+	p1.l = _real_start;
+	p1.h = _real_start;
+	[p0] = p1;
+
+	p0.l = (IMASK & 0xFFFF);
+	p0.h = (IMASK >> 16);
+	r0.l = LO(IVG15_POS);
+	r0.h = HI(IVG15_POS);
+	[p0] = r0;
+	raise 15;
+	p0.l = WAIT_HERE;
+	p0.h = WAIT_HERE;
+	reti = p0;
+	rti;
+
+WAIT_HERE:
+	jump WAIT_HERE;
+
+.global _real_start;
+_real_start:
+	[ -- sp ] = reti;
+
+#ifdef CONFIG_EZKIT561
+	p0.l = (WDOG_CTL & 0xFFFF);
+	p0.h = (WDOG_CTL >> 16);
+	r0 = WATCHDOG_DISABLE(z);
+	w[p0] = r0;
+#endif
+
+	/* DMA reset code to Hi of L1 SRAM */
+copy:
+	P1.H = hi(SYSMMR_BASE);	/* P1 Points to the beginning of SYSTEM MMR Space */
+	P1.L = lo(SYSMMR_BASE);
+
+	R0.H = reset_start;	/* Source Address (high) */
+	R0.L = reset_start;	/* Source Address (low) */
+	R1.H = reset_end;
+	R1.L = reset_end;
+	R2 = R1 - R0;		/* Count */
+	R1.H = hi(L1_ISRAM);	/* Destination Address (high) */
+	R1.L = lo(L1_ISRAM);	/* Destination Address (low) */
+	R3.L = DMAEN;		/* Source DMAConfig Value (8-bit words) */
+	R4.L = (DI_EN | WNR | DMAEN);	/* Destination DMAConfig Value (8-bit words) */
+
+DMA:
+	R6 = 0x1 (Z);
+	W[P1+OFFSET_(MDMA_S0_X_MODIFY)] = R6;	/* Source Modify = 1 */
+	W[P1+OFFSET_(MDMA_D0_X_MODIFY)] = R6;	/* Destination Modify = 1 */
+
+	[P1+OFFSET_(MDMA_S0_START_ADDR)] = R0;	/* Set Source Base Address */
+	W[P1+OFFSET_(MDMA_S0_X_COUNT)] = R2;	/* Set Source Count */
+	/* Set Source  DMAConfig = DMA Enable,
+	Memory Read,  8-Bit Transfers, 1-D DMA, Flow - Stop */
+	W[P1+OFFSET_(MDMA_S0_CONFIG)] = R3;
+
+	[P1+OFFSET_(MDMA_D0_START_ADDR)] = R1;	/* Set Destination Base Address */
+	W[P1+OFFSET_(MDMA_D0_X_COUNT)] = R2;	/* Set Destination Count */
+	/* Set Destination DMAConfig = DMA Enable,
+	Memory Write, 8-Bit Transfers, 1-D DMA, Flow - Stop, IOC */
+	W[P1+OFFSET_(MDMA_D0_CONFIG)] = R4;
+
+WAIT_DMA_DONE:
+	p0.h = hi(MDMA_D0_IRQ_STATUS);
+	p0.l = lo(MDMA_D0_IRQ_STATUS);
+	R0 = W[P0](Z);
+	CC = BITTST(R0, 0);
+	if ! CC jump WAIT_DMA_DONE
+
+	R0 = 0x1;
+	W[P1+OFFSET_(MDMA_D0_IRQ_STATUS)] = R0;	/* Write 1 to clear DMA interrupt */
+
+	/* Initialize BSS Section with 0 s */
+	p1.l = __bss_start;
+	p1.h = __bss_start;
+	p2.l = _end;
+	p2.h = _end;
+	r1 = p1;
+	r2 = p2;
+	r3 = r2 - r1;
+	r3 = r3 >> 2;
+	p3 = r3;
+	lsetup (_clear_bss, _clear_bss_end ) lc1 = p3;
+	CC = p2<=p1;
+	if CC jump _clear_bss_skip;
+	r0 = 0;
+_clear_bss:
+_clear_bss_end:
+	[p1++] = r0;
+_clear_bss_skip:
+
+	p0.l = _start1;
+	p0.h = _start1;
+	jump (p0);
+
+reset_start:
+	p0.h = WDOG_CNT >> 16;
+	p0.l = WDOG_CNT & 0xffff;
+	r0 = 0x0010;
+	w[p0] = r0;
+	p0.h = WDOG_CTL >> 16;
+	p0.l = WDOG_CTL & 0xffff;
+	r0 = 0x0000;
+	w[p0] = r0;
+reset_wait:
+	jump reset_wait;
+
+reset_end: nop;
+
+_exit:
+	jump.s	_exit;
+get_pc:
+	r0 = rets;
+	rts;
diff --git a/cpu/bf561/start1.S b/cpu/bf561/start1.S
new file mode 100644
index 0000000..72cfafb
--- /dev/null
+++ b/cpu/bf561/start1.S
@@ -0,0 +1,38 @@
+/*
+ * U-boot - start1.S Code running out of RAM after relocation
+ *
+ * Copyright (c) 2005 blackfin.uclinux.org
+ *
+ * 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 <linux/config.h>
+#include <config.h>
+#include <asm/blackfin.h>
+
+.global	start1;
+.global	_start1;
+
+.text
+_start1:
+start1:
+	sp += -12;
+	call	_board_init_f;
+	sp += 12;
diff --git a/cpu/bf561/traps.c b/cpu/bf561/traps.c
new file mode 100644
index 0000000..f5ff3a8
--- /dev/null
+++ b/cpu/bf561/traps.c
@@ -0,0 +1,239 @@
+/*
+ * 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/errno.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/traps.h>
+#include <asm/machdep.h>
+#include "cpu.h"
+#include <asm/arch/anomaly.h>
+#include <asm/cplb.h>
+#include <asm/io.h>
+
+void init_IRQ(void)
+{
+	blackfin_init_IRQ();
+	return;
+}
+
+void process_int(unsigned long vec, struct pt_regs *fp)
+{
+	printf("interrupt\n");
+	return;
+}
+
+extern unsigned int icplb_table[page_descriptor_table_size][2];
+extern unsigned int dcplb_table[page_descriptor_table_size][2];
+
+unsigned long last_cplb_fault_retx;
+
+static unsigned int cplb_sizes[4] =
+    { 1024, 4 * 1024, 1024 * 1024, 4 * 1024 * 1024 };
+
+void trap_c(struct pt_regs *regs)
+{
+	unsigned int addr;
+	unsigned long trapnr = (regs->seqstat) & SEQSTAT_EXCAUSE;
+	unsigned int i, j, size, *I0, *I1;
+	unsigned short data = 0;
+
+	switch (trapnr) {
+		/* 0x26 - Data CPLB Miss */
+	case VEC_CPLB_M:
+
+#ifdef ANOMALY_05000261
+		/*
+		 * Work around an anomaly: if we see a new DCPLB fault, return
+		 * without doing anything.  Then, if we get the same fault again,
+		 * handle it.
+		 */
+		addr = last_cplb_fault_retx;
+		last_cplb_fault_retx = regs->retx;
+		printf("this time, curr = 0x%08x last = 0x%08x\n", addr,
+		       last_cplb_fault_retx);
+		if (addr != last_cplb_fault_retx)
+			goto trap_c_return;
+#endif
+		data = 1;
+
+	case VEC_CPLB_I_M:
+
+		if (data)
+			addr = *pDCPLB_FAULT_ADDR;
+		else
+			addr = *pICPLB_FAULT_ADDR;
+
+		for (i = 0; i < page_descriptor_table_size; i++) {
+			if (data) {
+				size = cplb_sizes[dcplb_table[i][1] >> 16];
+				j = dcplb_table[i][0];
+			} else {
+				size = cplb_sizes[icplb_table[i][1] >> 16];
+				j = icplb_table[i][0];
+			}
+			if ((j <= addr) && ((j + size) > addr)) {
+				debug("found %i 0x%08x\n", i, j);
+				break;
+			}
+		}
+		if (i == page_descriptor_table_size) {
+			printf("something is really wrong\n");
+			do_reset(NULL, 0, 0, NULL);
+		}
+
+		/* Turn the cache off */
+		if (data) {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL &=
+			    ~(ACACHE_BCACHE | ENDCPLB | PORT_PREF0);
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL &= ~(IMC | ENICPLB);
+			sync();
+		}
+
+		if (data) {
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		j = 0;
+		while (*I1 & CPLB_LOCK) {
+			debug("skipping %i %08p - %08x\n", j, I1, *I1);
+			*I0++;
+			*I1++;
+			j++;
+		}
+
+		debug("remove %i 0x%08x  0x%08x\n", j, *I0, *I1);
+
+		for (; j < 15; j++) {
+			debug("replace %i 0x%08x  0x%08x\n", j, I0, I0 + 1);
+			*I0++ = *(I0 + 1);
+			*I1++ = *(I1 + 1);
+		}
+
+		if (data) {
+			*I0 = dcplb_table[i][0];
+			*I1 = dcplb_table[i][1];
+			I0 = (unsigned int *)DCPLB_ADDR0;
+			I1 = (unsigned int *)DCPLB_DATA0;
+		} else {
+			*I0 = icplb_table[i][0];
+			*I1 = icplb_table[i][1];
+			I0 = (unsigned int *)ICPLB_ADDR0;
+			I1 = (unsigned int *)ICPLB_DATA0;
+		}
+
+		for (j = 0; j < 16; j++) {
+			debug("%i 0x%08x  0x%08x\n", j, *I0++, *I1++);
+		}
+
+		/* Turn the cache back on */
+		if (data) {
+			j = *(unsigned int *)DMEM_CONTROL;
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)DMEM_CONTROL =
+			    ACACHE_BCACHE | ENDCPLB | PORT_PREF0 | j;
+			sync();
+		} else {
+			sync();
+			asm(" .align 8; ");
+			*(unsigned int *)IMEM_CONTROL = IMC | ENICPLB;
+			sync();
+		}
+
+		break;
+	default:
+		/* All traps come here */
+		printf("code=[0x%x], ", (unsigned int)(regs->seqstat & 0x3f));
+		printf("stack frame=0x%x, ", (unsigned int)regs);
+		printf("bad PC=0x%04x\n", (unsigned int)regs->pc);
+		dump(regs);
+		printf("\n\n");
+
+		printf("Unhandled IRQ or exceptions!\n");
+		printf("Please reset the board \n");
+		do_reset(NULL, 0, 0, NULL);
+	}
+
+trap_c_return:
+	return;
+
+}
+
+void dump(struct pt_regs *fp)
+{
+	debug("RETE:  %08lx  RETN: %08lx  RETX: %08lx  RETS: %08lx\n", fp->rete,
+	      fp->retn, fp->retx, fp->rets);
+	debug("IPEND: %04lx  SYSCFG: %04lx\n", fp->ipend, fp->syscfg);
+	debug("SEQSTAT: %08lx    SP: %08lx\n", (long)fp->seqstat, (long)fp);
+	debug("R0: %08lx    R1: %08lx    R2: %08lx    R3: %08lx\n", fp->r0,
+	      fp->r1, fp->r2, fp->r3);
+	debug("R4: %08lx    R5: %08lx    R6: %08lx    R7: %08lx\n", fp->r4,
+	      fp->r5, fp->r6, fp->r7);
+	debug("P0: %08lx    P1: %08lx    P2: %08lx    P3: %08lx\n", fp->p0,
+	      fp->p1, fp->p2, fp->p3);
+	debug("P4: %08lx    P5: %08lx    FP: %08lx\n", fp->p4, fp->p5, fp->fp);
+	debug("A0.w: %08lx    A0.x: %08lx    A1.w: %08lx    A1.x: %08lx\n",
+	      fp->a0w, fp->a0x, fp->a1w, fp->a1x);
+
+	debug("LB0: %08lx  LT0: %08lx  LC0: %08lx\n", fp->lb0, fp->lt0,
+	      fp->lc0);
+	debug("LB1: %08lx  LT1: %08lx  LC1: %08lx\n", fp->lb1, fp->lt1,
+	      fp->lc1);
+	debug("B0: %08lx  L0: %08lx  M0: %08lx  I0: %08lx\n", fp->b0, fp->l0,
+	      fp->m0, fp->i0);
+	debug("B1: %08lx  L1: %08lx  M1: %08lx  I1: %08lx\n", fp->b1, fp->l1,
+	      fp->m1, fp->i1);
+	debug("B2: %08lx  L2: %08lx  M2: %08lx  I2: %08lx\n", fp->b2, fp->l2,
+	      fp->m2, fp->i2);
+	debug("B3: %08lx  L3: %08lx  M3: %08lx  I3: %08lx\n", fp->b3, fp->l3,
+	      fp->m3, fp->i3);
+
+	debug("DCPLB_FAULT_ADDR=%p\n", *pDCPLB_FAULT_ADDR);
+	debug("ICPLB_FAULT_ADDR=%p\n", *pICPLB_FAULT_ADDR);
+
+}
diff --git a/cpu/bf561/video.c b/cpu/bf561/video.c
new file mode 100644
index 0000000..3ff0151
--- /dev/null
+++ b/cpu/bf561/video.c
@@ -0,0 +1,194 @@
+/*
+ * (C) Copyright 2000
+ * Paolo Scaffardi, AIRVENT SAM s.p.a - RIMINI(ITALY), arsenio@tin.it
+ * (C) Copyright 2002
+ * Wolfgang Denk, wd@denx.de
+ * (C) Copyright 2006
+ * Aubrey Li, aubrey.li@analog.com
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <stdarg.h>
+#include <common.h>
+#include <config.h>
+#include <asm/blackfin.h>
+#include <i2c.h>
+#include <linux/types.h>
+#include <devices.h>
+
+#ifdef CONFIG_VIDEO
+#define NTSC_FRAME_ADDR 0x06000000
+#include "video.h"
+
+/* NTSC OUTPUT SIZE  720 * 240 */
+#define VERTICAL	2
+#define HORIZONTAL	4
+
+int is_vblank_line(const int line)
+{
+	/*
+	 *  This array contains a single bit for each line in
+	 *  an NTSC frame.
+	 */
+	if ((line <= 18) || (line >= 264 && line <= 281) || (line == 528))
+		return true;
+
+	return false;
+}
+
+int NTSC_framebuffer_init(char *base_address)
+{
+	const int NTSC_frames = 1;
+	const int NTSC_lines = 525;
+	char *dest = base_address;
+	int frame_num, line_num;
+
+	for (frame_num = 0; frame_num < NTSC_frames; ++frame_num) {
+		for (line_num = 1; line_num <= NTSC_lines; ++line_num) {
+			unsigned int code;
+			int offset = 0;
+			int i;
+
+			if (is_vblank_line(line_num))
+				offset++;
+
+			if (line_num > 266 || line_num < 3)
+				offset += 2;
+
+			/* Output EAV code */
+			code = SystemCodeMap[offset].EAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output horizontal blanking */
+			for (i = 0; i < 67 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+
+			/* Output SAV */
+			code = SystemCodeMap[offset].SAV;
+			write_dest_byte((char)(code >> 24) & 0xff);
+			write_dest_byte((char)(code >> 16) & 0xff);
+			write_dest_byte((char)(code >> 8) & 0xff);
+			write_dest_byte((char)(code) & 0xff);
+
+			/* Output empty horizontal data */
+			for (i = 0; i < 360 * 2; ++i) {
+				write_dest_byte(0x80);
+				write_dest_byte(0x10);
+			}
+		}
+	}
+
+	return dest - base_address;
+}
+
+void fill_frame(char *Frame, int Value)
+{
+	int *OddPtr32;
+	int OddLine;
+	int *EvenPtr32;
+	int EvenLine;
+	int i;
+	int *data;
+	int m, n;
+
+	/* fill odd and even frames */
+	for (OddLine = 22, EvenLine = 285; OddLine < 263; OddLine++, EvenLine++) {
+		OddPtr32 = (int *)((Frame + (OddLine * 1716)) + 276);
+		EvenPtr32 = (int *)((Frame + (EvenLine * 1716)) + 276);
+		for (i = 0; i < 360; i++, OddPtr32++, EvenPtr32++) {
+			*OddPtr32 = Value;
+			*EvenPtr32 = Value;
+		}
+	}
+
+	for (m = 0; m < VERTICAL; m++) {
+		data = (int *)u_boot_logo.data;
+		for (OddLine = (22 + m), EvenLine = (285 + m);
+		     OddLine < (u_boot_logo.height * VERTICAL) + (22 + m);
+		     OddLine += VERTICAL, EvenLine += VERTICAL) {
+			OddPtr32 = (int *)((Frame + ((OddLine) * 1716)) + 276);
+			EvenPtr32 =
+			    (int *)((Frame + ((EvenLine) * 1716)) + 276);
+			for (i = 0; i < u_boot_logo.width / 2; i++) {
+				/* enlarge one pixel to m x n */
+				for (n = 0; n < HORIZONTAL; n++) {
+					*OddPtr32++ = *data;
+					*EvenPtr32++ = *data;
+				}
+				data++;
+			}
+		}
+	}
+}
+
+void video_putc(const char c)
+{
+}
+
+void video_puts(const char *s)
+{
+}
+
+static int video_init(void)
+{
+	char *NTSCFrame;
+	NTSCFrame = (char *)NTSC_FRAME_ADDR;
+	NTSC_framebuffer_init(NTSCFrame);
+	fill_frame(NTSCFrame, BLUE);
+
+	*pPPI_CONTROL = 0x0082;
+	*pPPI_FRAME = 0x020D;
+
+	*pDMA0_START_ADDR = NTSCFrame;
+	*pDMA0_X_COUNT = 0x035A;
+	*pDMA0_X_MODIFY = 0x0002;
+	*pDMA0_Y_COUNT = 0x020D;
+	*pDMA0_Y_MODIFY = 0x0002;
+	*pDMA0_CONFIG = 0x1015;
+	*pPPI_CONTROL = 0x0083;
+	return 0;
+}
+
+int drv_video_init(void)
+{
+	int error, devices = 1;
+
+	device_t videodev;
+
+	video_init();		/* Video initialization */
+
+	memset(&videodev, 0, sizeof(videodev));
+
+	strcpy(videodev.name, "video");
+	videodev.ext = DEV_EXT_VIDEO;	/* Video extensions */
+	videodev.flags = DEV_FLAGS_OUTPUT;	/* Output only */
+	videodev.putc = video_putc;	/* 'putc' function */
+	videodev.puts = video_puts;	/* 'puts' function */
+
+	error = device_register(&videodev);
+
+	return (error == 0) ? devices : error;
+}
+#endif
diff --git a/cpu/bf561/video.h b/cpu/bf561/video.h
new file mode 100644
index 0000000..d237f6a
--- /dev/null
+++ b/cpu/bf561/video.h
@@ -0,0 +1,25 @@
+#include <video_logo.h>
+#define write_dest_byte(val) {*dest++=val;}
+#define BLACK   (0x01800180)	/* black pixel pattern  */
+#define BLUE    (0x296E29F0)	/* blue pixel pattern   */
+#define RED     (0x51F0515A)	/* red pixel pattern    */
+#define MAGENTA (0x6ADE6ACA)	/* magenta pixel pattern */
+#define GREEN   (0x91229136)	/* green pixel pattern  */
+#define CYAN    (0xAA10AAA6)	/* cyan pixel pattern   */
+#define YELLOW  (0xD292D210)	/* yellow pixel pattern */
+#define WHITE   (0xFE80FE80)	/* white pixel pattern  */
+
+#define true 	1
+#define false	0
+
+typedef struct {
+	unsigned int SAV;
+	unsigned int EAV;
+} SystemCodeType;
+
+const SystemCodeType SystemCodeMap[4] = {
+	{0xFF000080, 0xFF00009D},
+	{0xFF0000AB, 0xFF0000B6},
+	{0xFF0000C7, 0xFF0000DA},
+	{0xFF0000EC, 0xFF0000F1}
+};