openrisc: Add cpu files

Signed-off-by: Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>

7 files changed, 923 insertions, 0 deletions

diff --git a/arch/openrisc/config.mk b/arch/openrisc/config.mk
new file mode 100644
index 000000000..521e73aae
--- /dev/null
+++ b/arch/openrisc/config.mk
@@ -0,0 +1,27 @@
+#
+# (C) Copyright 2011
+# Julius Baxter <julius@opencores.org>
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+# MA 02111-1307 USA
+#
+
+CROSS_COMPILE ?= or32-elf-
+
+# r10 used for global object pointer, already set in OR32 GCC but just to be
+# clear
+PLATFORM_CPPFLAGS += -DCONFIG_OPENRISC -D__OR1K__ -ffixed-r10
+
+CONFIG_STANDALONE_LOAD_ADDR ?= 0x40000
diff --git a/arch/openrisc/cpu/Makefile b/arch/openrisc/cpu/Makefile
new file mode 100644
index 000000000..b3b1a243c
--- /dev/null
+++ b/arch/openrisc/cpu/Makefile
@@ -0,0 +1,47 @@
+#
+# (C) Copyright 2011
+# Julius Baxter <julius@opencores.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
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(CPU).o
+
+START	= start.o
+COBJS-y	= cache.o cpu.o exceptions.o interrupts.o
+
+SRCS	:= $(START:.o=.S) $(COBJS-y:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS-y))
+START	:= $(addprefix $(obj),$(START))
+
+all:	$(obj).depend $(START) $(LIB)
+
+$(LIB):	$(OBJS)
+	$(call cmd_link_o_target, $(OBJS))
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/arch/openrisc/cpu/cache.c b/arch/openrisc/cpu/cache.c
new file mode 100644
index 000000000..2a73a4f2c
--- /dev/null
+++ b/arch/openrisc/cpu/cache.c
@@ -0,0 +1,151 @@
+/*
+ * (C) Copyright 2011, Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>
+ * (C) Copyright 2011, Julius Baxter <julius@opencores.org>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/system.h>
+
+void flush_dcache_range(unsigned long addr, unsigned long stop)
+{
+	ulong block_size = (mfspr(SPR_DCCFGR) & SPR_DCCFGR_CBS) ? 32 : 16;
+
+	while (addr < stop) {
+		mtspr(SPR_DCBFR, addr);
+		addr += block_size;
+	}
+}
+
+void invalidate_dcache_range(unsigned long addr, unsigned long stop)
+{
+	ulong block_size = (mfspr(SPR_DCCFGR) & SPR_DCCFGR_CBS) ? 32 : 16;
+
+	while (addr < stop) {
+		mtspr(SPR_DCBIR, addr);
+		addr += block_size;
+	}
+}
+
+static void invalidate_icache_range(unsigned long addr, unsigned long stop)
+{
+	ulong block_size = (mfspr(SPR_ICCFGR) & SPR_ICCFGR_CBS) ? 32 : 16;
+
+	while (addr < stop) {
+		mtspr(SPR_ICBIR, addr);
+		addr += block_size;
+	}
+}
+
+void flush_cache(unsigned long addr, unsigned long size)
+{
+	flush_dcache_range(addr, addr + size);
+	invalidate_icache_range(addr, addr + size);
+}
+
+int icache_status(void)
+{
+	return mfspr(SPR_SR) & SPR_SR_ICE;
+}
+
+int checkicache(void)
+{
+	unsigned long iccfgr;
+	unsigned long cache_set_size;
+	unsigned long cache_ways;
+	unsigned long cache_block_size;
+
+	iccfgr = mfspr(SPR_ICCFGR);
+	cache_ways = 1 << (iccfgr & SPR_ICCFGR_NCW);
+	cache_set_size = 1 << ((iccfgr & SPR_ICCFGR_NCS) >> 3);
+	cache_block_size = (iccfgr & SPR_ICCFGR_CBS) ? 32 : 16;
+
+	return cache_set_size * cache_ways * cache_block_size;
+}
+
+int dcache_status(void)
+{
+	return mfspr(SPR_SR) & SPR_SR_DCE;
+}
+
+int checkdcache(void)
+{
+	unsigned long dccfgr;
+	unsigned long cache_set_size;
+	unsigned long cache_ways;
+	unsigned long cache_block_size;
+
+	dccfgr = mfspr(SPR_DCCFGR);
+	cache_ways = 1 << (dccfgr & SPR_DCCFGR_NCW);
+	cache_set_size = 1 << ((dccfgr & SPR_DCCFGR_NCS) >> 3);
+	cache_block_size = (dccfgr & SPR_DCCFGR_CBS) ? 32 : 16;
+
+	return cache_set_size * cache_ways * cache_block_size;
+}
+
+void dcache_enable(void)
+{
+	mtspr(SPR_SR, mfspr(SPR_SR) | SPR_SR_DCE);
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+}
+
+void dcache_disable(void)
+{
+	mtspr(SPR_SR, mfspr(SPR_SR) & ~SPR_SR_DCE);
+}
+
+void icache_enable(void)
+{
+	mtspr(SPR_SR, mfspr(SPR_SR) | SPR_SR_ICE);
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+	asm volatile("l.nop");
+}
+
+void icache_disable(void)
+{
+	mtspr(SPR_SR, mfspr(SPR_SR) & ~SPR_SR_ICE);
+}
+
+int cache_init(void)
+{
+	if (mfspr(SPR_UPR) & SPR_UPR_ICP) {
+		icache_disable();
+		invalidate_icache_range(0, checkicache());
+		icache_enable();
+	}
+
+	if (mfspr(SPR_UPR) & SPR_UPR_DCP) {
+		dcache_disable();
+		invalidate_dcache_range(0, checkdcache());
+		dcache_enable();
+	}
+
+	return 0;
+}
diff --git a/arch/openrisc/cpu/cpu.c b/arch/openrisc/cpu/cpu.c
new file mode 100644
index 000000000..25cd6249d
--- /dev/null
+++ b/arch/openrisc/cpu/cpu.c
@@ -0,0 +1,157 @@
+/*
+ * (C) Copyright 2011, Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>
+ * (C) Copyright 2011, Julius Baxter <julius@opencores.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
+ */
+
+#include <common.h>
+#include <asm/system.h>
+#include <asm/openrisc_exc.h>
+
+static volatile int illegal_instruction;
+
+static void illegal_instruction_handler(void)
+{
+	ulong *epcr = (ulong *)mfspr(SPR_EPCR_BASE);
+
+	/* skip over the illegal instruction */
+	mtspr(SPR_EPCR_BASE, (ulong)(++epcr));
+	illegal_instruction = 1;
+}
+
+static void checkinstructions(void)
+{
+	ulong ra = 1, rb = 1, rc;
+
+	exception_install_handler(EXC_ILLEGAL_INSTR,
+				illegal_instruction_handler);
+
+	illegal_instruction = 0;
+	asm volatile("l.mul %0,%1,%2" : "=r" (rc) : "r" (ra), "r" (rb));
+	printf("           Hardware multiplier: %s\n",
+		illegal_instruction ? "no" : "yes");
+
+	illegal_instruction = 0;
+	asm volatile("l.div %0,%1,%2" : "=r" (rc) : "r" (ra), "r" (rb));
+	printf("           Hardware divider: %s\n",
+		illegal_instruction ? "no" : "yes");
+
+	exception_free_handler(EXC_ILLEGAL_INSTR);
+}
+
+int checkcpu(void)
+{
+	ulong upr = mfspr(SPR_UPR);
+	ulong vr = mfspr(SPR_VR);
+	ulong iccfgr = mfspr(SPR_ICCFGR);
+	ulong dccfgr = mfspr(SPR_DCCFGR);
+	ulong immucfgr = mfspr(SPR_IMMUCFGR);
+	ulong dmmucfgr = mfspr(SPR_DMMUCFGR);
+	ulong cpucfgr = mfspr(SPR_CPUCFGR);
+	uint ver = (vr & SPR_VR_VER) >> 24;
+	uint rev = vr & SPR_VR_REV;
+	uint block_size;
+	uint ways;
+	uint sets;
+
+	printf("CPU:   OpenRISC-%x00 (rev %d) @ %d MHz\n",
+		ver, rev, (CONFIG_SYS_CLK_FREQ / 1000000));
+
+	if (upr & SPR_UPR_DCP) {
+		block_size = (dccfgr & SPR_DCCFGR_CBS) ? 32 : 16;
+		ways = 1 << (dccfgr & SPR_DCCFGR_NCW);
+		printf("       D-Cache: %d bytes, %d bytes/line, %d way(s)\n",
+		       checkdcache(), block_size, ways);
+	} else {
+		printf("       D-Cache: no\n");
+	}
+
+	if (upr & SPR_UPR_ICP) {
+		block_size = (iccfgr & SPR_ICCFGR_CBS) ? 32 : 16;
+		ways = 1 << (iccfgr & SPR_ICCFGR_NCW);
+		printf("       I-Cache: %d bytes, %d bytes/line, %d way(s)\n",
+		       checkicache(), block_size, ways);
+	} else {
+		printf("       I-Cache: no\n");
+	}
+
+	if (upr & SPR_UPR_DMP) {
+		sets = 1 << ((dmmucfgr & SPR_DMMUCFGR_NTS) >> 2);
+		ways = (dmmucfgr & SPR_DMMUCFGR_NTW) + 1;
+		printf("       DMMU: %d sets, %d way(s)\n",
+		       sets, ways);
+	} else {
+		printf("       DMMU: no\n");
+	}
+
+	if (upr & SPR_UPR_IMP) {
+		sets = 1 << ((immucfgr & SPR_IMMUCFGR_NTS) >> 2);
+		ways = (immucfgr & SPR_IMMUCFGR_NTW) + 1;
+		printf("       IMMU: %d sets, %d way(s)\n",
+		       sets, ways);
+	} else {
+		printf("       IMMU: no\n");
+	}
+
+	printf("       MAC unit: %s\n",
+		(upr & SPR_UPR_MP) ? "yes" : "no");
+	printf("       Debug unit: %s\n",
+		(upr & SPR_UPR_DUP) ? "yes" : "no");
+	printf("       Performance counters: %s\n",
+		(upr & SPR_UPR_PCUP) ? "yes" : "no");
+	printf("       Power management: %s\n",
+		(upr & SPR_UPR_PMP) ? "yes" : "no");
+	printf("       Interrupt controller: %s\n",
+		(upr & SPR_UPR_PICP) ? "yes" : "no");
+	printf("       Timer: %s\n",
+		(upr & SPR_UPR_TTP) ? "yes" : "no");
+	printf("       Custom unit(s): %s\n",
+		(upr & SPR_UPR_CUP) ? "yes" : "no");
+
+	printf("       Supported instructions:\n");
+	printf("           ORBIS32: %s\n",
+		(cpucfgr & SPR_CPUCFGR_OB32S) ? "yes" : "no");
+	printf("           ORBIS64: %s\n",
+		(cpucfgr & SPR_CPUCFGR_OB64S) ? "yes" : "no");
+	printf("           ORFPX32: %s\n",
+		(cpucfgr & SPR_CPUCFGR_OF32S) ? "yes" : "no");
+	printf("           ORFPX64: %s\n",
+		(cpucfgr & SPR_CPUCFGR_OF64S) ? "yes" : "no");
+
+	checkinstructions();
+
+	return 0;
+}
+
+int cleanup_before_linux(void)
+{
+	disable_interrupts();
+	return 0;
+}
+
+extern void __reset(void);
+
+int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	disable_interrupts();
+	__reset();
+	/* not reached, __reset does not return */
+	return 0;
+}
diff --git a/arch/openrisc/cpu/exceptions.c b/arch/openrisc/cpu/exceptions.c
new file mode 100644
index 000000000..5d9f117df
--- /dev/null
+++ b/arch/openrisc/cpu/exceptions.c
@@ -0,0 +1,85 @@
+/*
+ * (C) Copyright 2011, Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>
+ * (C) Copyright 2011, Julius Baxter <julius@opencores.org>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <stdio_dev.h>
+#include <asm/system.h>
+
+static const char * const excp_table[] = {
+	"Unknown exception",
+	"Reset",
+	"Bus Error",
+	"Data Page Fault",
+	"Instruction Page Fault",
+	"Tick Timer",
+	"Alignment",
+	"Illegal Instruction",
+	"External Interrupt",
+	"D-TLB Miss",
+	"I-TLB Miss",
+	"Range",
+	"System Call",
+	"Floating Point",
+	"Trap",
+};
+
+static void (*handlers[32])(void);
+
+void exception_install_handler(int exception, void (*handler)(void))
+{
+	if (exception < 0 || exception > 31)
+		return;
+
+	handlers[exception] = handler;
+}
+
+void exception_free_handler(int exception)
+{
+	if (exception < 0 || exception > 31)
+		return;
+
+	handlers[exception] = 0;
+}
+
+static void exception_hang(int vect)
+{
+	printf("Unhandled exception at 0x%x ", vect & 0xff00);
+
+	vect = ((vect >> 8) & 0xff);
+	if (vect < ARRAY_SIZE(excp_table))
+		printf("(%s)\n", excp_table[vect]);
+	else
+		printf("(%s)\n", excp_table[0]);
+
+	printf("EPCR: 0x%08lx\n", mfspr(SPR_EPCR_BASE));
+	printf("EEAR: 0x%08lx\n", mfspr(SPR_EEAR_BASE));
+	printf("ESR:  0x%08lx\n", mfspr(SPR_ESR_BASE));
+	hang();
+}
+
+void exception_handler(int vect)
+{
+	int exception = vect >> 8;
+
+	if (handlers[exception])
+		handlers[exception]();
+	else
+		exception_hang(vect);
+}
diff --git a/arch/openrisc/cpu/interrupts.c b/arch/openrisc/cpu/interrupts.c
new file mode 100644
index 000000000..8f0672427
--- /dev/null
+++ b/arch/openrisc/cpu/interrupts.c
@@ -0,0 +1,121 @@
+/*
+ * (C) Copyright 2011, Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>
+ * (C) Copyright 2011, Julius Baxter <julius@opencores.org>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/types.h>
+#include <asm/ptrace.h>
+#include <asm/system.h>
+#include <asm/openrisc_exc.h>
+
+struct irq_action {
+	interrupt_handler_t *handler;
+	void *arg;
+	int count;
+};
+
+static struct irq_action handlers[32];
+
+void interrupt_handler(void)
+{
+	int irq;
+
+	while ((irq = ffs(mfspr(SPR_PICSR)))) {
+		if (handlers[--irq].handler) {
+			handlers[irq].handler(handlers[irq].arg);
+			handlers[irq].count++;
+		} else {
+			/* disable the interrupt */
+			mtspr(SPR_PICMR, mfspr(SPR_PICMR) & ~(1 << irq));
+			printf("Unhandled interrupt: %d\n", irq);
+		}
+		/* clear the interrupt */
+		mtspr(SPR_PICSR, mfspr(SPR_PICSR) & ~(1 << irq));
+	}
+}
+
+int interrupt_init(void)
+{
+	/* install handler for external interrupt exception */
+	exception_install_handler(EXC_EXT_IRQ, interrupt_handler);
+	/* Enable interrupts in supervisor register */
+	mtspr(SPR_SR, mfspr(SPR_SR) | SPR_SR_IEE);
+
+	return 0;
+}
+
+void enable_interrupts(void)
+{
+	/* Set interrupt enable bit in supervisor register */
+	mtspr(SPR_SR, mfspr(SPR_SR) | SPR_SR_IEE);
+	/* Enable timer exception */
+	mtspr(SPR_SR, mfspr(SPR_SR) | SPR_SR_TEE);
+}
+
+int disable_interrupts(void)
+{
+	/* Clear interrupt enable bit in supervisor register */
+	mtspr(SPR_SR, mfspr(SPR_SR) & ~SPR_SR_IEE);
+	/* Disable timer exception */
+	mtspr(SPR_SR, mfspr(SPR_SR) & ~SPR_SR_TEE);
+
+	return 0;
+}
+
+void irq_install_handler(int irq, interrupt_handler_t *handler, void *arg)
+{
+	if (irq < 0 || irq > 31)
+		return;
+
+	handlers[irq].handler = handler;
+	handlers[irq].arg = arg;
+}
+
+void irq_free_handler(int irq)
+{
+	if (irq < 0 || irq > 31)
+		return;
+
+	handlers[irq].handler = 0;
+	handlers[irq].arg = 0;
+}
+
+#if defined(CONFIG_CMD_IRQ)
+int do_irqinfo(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	int i;
+
+	printf("\nInterrupt-Information:\n\n");
+	printf("Nr  Routine   Arg       Count\n");
+	printf("-----------------------------\n");
+
+	for (i = 0; i < 32; i++) {
+		if (handlers[i].handler) {
+			printf("%02d  %08lx  %08lx  %d\n",
+				i,
+				(ulong)handlers[i].handler,
+				(ulong)handlers[i].arg,
+				handlers[i].count);
+		}
+	}
+	printf("\n");
+
+	return 0;
+}
+#endif
diff --git a/arch/openrisc/cpu/start.S b/arch/openrisc/cpu/start.S
new file mode 100644
index 000000000..3a4271750
--- /dev/null
+++ b/arch/openrisc/cpu/start.S
@@ -0,0 +1,335 @@
+/*
+ * (C) Copyright 2011, Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>
+ * (C) Copyright 2011, Julius Baxter <julius@opencores.org>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <config.h>
+#include <asm-offsets.h>
+#include <asm/spr-defs.h>
+
+#define EXCEPTION_STACK_SIZE (128+128)
+
+#define HANDLE_EXCEPTION			\
+	l.addi	r1, r1, -EXCEPTION_STACK_SIZE	;\
+	l.sw	0x1c(r1), r9			;\
+	l.jal	_exception_handler		;\
+	 l.nop					;\
+	l.lwz 	r9, 0x1c(r1)			;\
+	l.addi	r1, r1, EXCEPTION_STACK_SIZE	;\
+	l.rfe					;\
+	 l.nop
+
+	.section .vectors, "ax"
+	.global __reset
+
+	/* reset */
+	.org	0x100
+__reset:
+	/* there is no guarantee r0 is hardwired to zero, clear it here */
+	l.andi	r0, r0, 0
+	/* reset stack and frame pointers */
+	l.andi	r1, r0, 0
+	l.andi	r2, r0, 0
+
+	/* set supervisor mode */
+	l.ori	r3,r0,SPR_SR_SM
+	l.mtspr	r0,r3,SPR_SR
+
+	/* Relocate u-boot */
+	l.movhi	r3,hi(__start)		/* source start address */
+	l.ori	r3,r3,lo(__start)
+	l.movhi	r4,hi(_stext)		/* dest start address */
+	l.ori	r4,r4,lo(_stext)
+	l.movhi	r5,hi(__end)		/* dest end address */
+	l.ori	r5,r5,lo(__end)
+
+.L_reloc:
+	l.lwz	r6,0(r3)
+	l.sw	0(r4),r6
+	l.addi	r3,r3,4
+	l.sfltu	r4,r5
+	l.bf	.L_reloc
+	 l.addi	r4,r4,4			/* delay slot */
+
+#ifdef CONFIG_SYS_RELOCATE_VECTORS
+	/* Relocate vectors from 0xf0000000 to 0x00000000 */
+	l.movhi r4, 0xf000 /* source */
+	l.movhi r5, 0      /* destination */
+	l.addi	r6, r5, CONFIG_SYS_VECTORS_LEN /* length */
+.L_relocvectors:
+	l.lwz	r7, 0(r4)
+	l.sw	0(r5), r7
+	l.addi	r5, r5, 4
+	l.sfeq	r5,r6
+	l.bnf	.L_relocvectors
+	 l.addi	r4,r4, 4
+#endif
+
+	l.j	_start
+	 l.nop
+
+	/* bus error */
+	.org	0x200
+	HANDLE_EXCEPTION
+
+	/* data page fault */
+	.org	0x300
+	HANDLE_EXCEPTION
+
+	/* instruction page fault */
+	.org	0x400
+	HANDLE_EXCEPTION
+
+	/* tick timer */
+	.org	0x500
+	HANDLE_EXCEPTION
+
+	/* alignment */
+	.org	0x600
+	HANDLE_EXCEPTION
+
+	/* illegal instruction */
+	.org	0x700
+	HANDLE_EXCEPTION
+
+	/* external interrupt */
+	.org	0x800
+	HANDLE_EXCEPTION
+
+	/* D-TLB miss */
+	.org	0x900
+	HANDLE_EXCEPTION
+
+	/* I-TLB miss */
+	.org	0xa00
+	HANDLE_EXCEPTION
+
+	/* range */
+	.org	0xb00
+	HANDLE_EXCEPTION
+
+	/* system call */
+	.org	0xc00
+	HANDLE_EXCEPTION
+
+	/* floating point */
+	.org	0xd00
+	HANDLE_EXCEPTION
+
+	/* trap */
+	.org	0xe00
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0xf00
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1100
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1200
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1300
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1400
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1500
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1600
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1700
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1800
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1900
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1a00
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1b00
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1c00
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1d00
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1e00
+	HANDLE_EXCEPTION
+
+	/* reserved */
+	.org	0x1f00
+	HANDLE_EXCEPTION
+
+	/* Startup routine */
+	.text
+	.global _start
+_start:
+	/* Init stack and frame pointers */
+	l.movhi	r1, hi(CONFIG_SYS_INIT_SP_ADDR)
+	l.ori	r1, r1, lo(CONFIG_SYS_INIT_SP_ADDR)
+	l.or	r2, r0, r1
+
+	/* clear BSS segments */
+	l.movhi	r4, hi(_bss_start)
+	l.ori	r4, r4, lo(_bss_start)
+	l.movhi	r5, hi(_bss_end)
+	l.ori	r5, r5, lo(_bss_end)
+.L_clear_bss:
+	l.sw	0(r4), r0
+	l.sfltu	r4,r5
+	l.bf	.L_clear_bss
+	 l.addi	r4,r4,4
+
+	/* Reset registers before jumping to board_init */
+	l.andi	r3, r0, 0
+	l.andi	r4, r0, 0
+	l.andi	r5, r0, 0
+	l.andi	r6, r0, 0
+	l.andi	r7, r0, 0
+	l.andi	r8, r0, 0
+	l.andi	r9, r0, 0
+	l.andi	r10, r0, 0
+	l.andi	r11, r0, 0
+	l.andi	r12, r0, 0
+	l.andi	r13, r0, 0
+	l.andi	r14, r0, 0
+	l.andi	r15, r0, 0
+	l.andi	r17, r0, 0
+	l.andi	r18, r0, 0
+	l.andi	r19, r0, 0
+	l.andi	r20, r0, 0
+	l.andi	r21, r0, 0
+	l.andi	r22, r0, 0
+	l.andi	r23, r0, 0
+	l.andi	r24, r0, 0
+	l.andi	r25, r0, 0
+	l.andi	r26, r0, 0
+	l.andi	r27, r0, 0
+	l.andi	r28, r0, 0
+	l.andi	r29, r0, 0
+	l.andi	r30, r0, 0
+	l.andi	r31, r0, 0
+
+	l.j	board_init
+	 l.nop
+
+	.size	_start, .-_start
+
+/*
+ * Store state onto stack and call the real exception handler
+ */
+	.section .text
+	.extern	exception_handler
+	.type	_exception_handler,@function
+
+_exception_handler:
+	/* Store state (r9 already saved)*/
+	l.sw	0x00(r1), r2
+	l.sw	0x04(r1), r3
+	l.sw	0x08(r1), r4
+	l.sw	0x0c(r1), r5
+	l.sw	0x10(r1), r6
+	l.sw	0x14(r1), r7
+	l.sw	0x18(r1), r8
+	l.sw	0x20(r1), r10
+	l.sw	0x24(r1), r11
+	l.sw	0x28(r1), r12
+	l.sw	0x2c(r1), r13
+	l.sw	0x30(r1), r14
+	l.sw	0x34(r1), r15
+	l.sw	0x38(r1), r16
+	l.sw	0x3c(r1), r17
+	l.sw	0x40(r1), r18
+	l.sw	0x44(r1), r19
+	l.sw	0x48(r1), r20
+	l.sw	0x4c(r1), r21
+	l.sw	0x50(r1), r22
+	l.sw	0x54(r1), r23
+	l.sw	0x58(r1), r24
+	l.sw	0x5c(r1), r25
+	l.sw	0x60(r1), r26
+	l.sw	0x64(r1), r27
+	l.sw	0x68(r1), r28
+	l.sw	0x6c(r1), r29
+	l.sw	0x70(r1), r30
+	l.sw	0x74(r1), r31
+
+	/* Save return address */
+	l.or	r14, r0, r9
+	/* Call exception handler with the link address as argument */
+	l.jal	exception_handler
+	 l.or	r3, r0, r14
+	/* Load return address */
+	l.or	r9, r0, r14
+
+	/* Restore state */
+	l.lwz	r2, 0x00(r1)
+	l.lwz	r3, 0x04(r1)
+	l.lwz	r4, 0x08(r1)
+	l.lwz	r5, 0x0c(r1)
+	l.lwz	r6, 0x10(r1)
+	l.lwz	r7, 0x14(r1)
+	l.lwz	r8, 0x18(r1)
+	l.lwz	r10, 0x20(r1)
+	l.lwz	r11, 0x24(r1)
+	l.lwz	r12, 0x28(r1)
+	l.lwz	r13, 0x2c(r1)
+	l.lwz	r14, 0x30(r1)
+	l.lwz	r15, 0x34(r1)
+	l.lwz	r16, 0x38(r1)
+	l.lwz	r17, 0x3c(r1)
+	l.lwz	r18, 0x40(r1)
+	l.lwz	r19, 0x44(r1)
+	l.lwz	r20, 0x48(r1)
+	l.lwz	r21, 0x4c(r1)
+	l.lwz	r22, 0x50(r1)
+	l.lwz	r23, 0x54(r1)
+	l.lwz	r24, 0x58(r1)
+	l.lwz	r25, 0x5c(r1)
+	l.lwz	r26, 0x60(r1)
+	l.lwz	r27, 0x64(r1)
+	l.lwz	r28, 0x68(r1)
+	l.lwz	r29, 0x6c(r1)
+	l.lwz	r30, 0x70(r1)
+	l.lwz	r31, 0x74(r1)
+	l.jr	r9
+	 l.nop