MIPS: add initial exceptions handling

Checking exception handling:

    $ make qemu-malta_defconfig
    $ make

    ...

    $ qemu-system-mips -nodefaults -M malta -m 256 \
       -nographic -serial stdio -bios ./barebox.bin

    ...

    barebox:/ md -l 0x03

    Ooops, address error on load or ifetch!
    EPC = 0xa082783c
    CP0_STATUS = 0x00000006
    CP0_CAUSE = 0x00000410
    CP0_CONFIG = 0x80008482

    ### ERROR ### Please RESET the board ###

Signed-off-by: Antony Pavlov <antonynpavlov@gmail.com>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

1 files changed, 47 insertions, 0 deletions

diff --git a/arch/mips/boot/main_entry.c b/arch/mips/boot/main_entry.c
index 8f5f6fc00e..a38ad31d69 100644
--- a/arch/mips/boot/main_entry.c
+++ b/arch/mips/boot/main_entry.c
@@ -25,11 +25,56 @@
 #include <string.h>
 #include <asm/sections.h>
 #include <asm/cpu-features.h>
+#include <asm/mipsregs.h>
+#include <asm/addrspace.h>
 
 extern void start_barebox(void);
+extern void handle_reserved(void);
 
 void main_entry(void);
 
+unsigned long exception_handlers[32];
+
+static void set_except_vector(int n, void *addr)
+{
+	unsigned handler = (unsigned long) addr;
+
+	exception_handlers[n] = handler;
+}
+
+static void trap_init(void)
+{
+	extern char except_vec3_generic;
+	int i;
+
+	unsigned long ebase;
+
+	ebase = CKSEG1;
+
+	/*
+	 * Copy the generic exception handlers to their final destination.
+	 * This will be overriden later as suitable for a particular
+	 * configuration.
+	 */
+	memcpy((void *)(ebase + 0x180), &except_vec3_generic, 0x80);
+
+	/*
+	 * Setup default vectors
+	 */
+	for (i = 0; i <= 31; i++) {
+		set_except_vector(i, &handle_reserved);
+	}
+
+	if (!cpu_has_4kex)
+		memcpy((void *)(ebase + 0x080), &except_vec3_generic, 0x80);
+
+	/* FIXME: handle tlb */
+	memcpy((void *)(ebase), &except_vec3_generic, 0x80);
+
+	/* unset BOOT EXCEPTION VECTOR bit */
+	write_c0_status(read_c0_status() & ~ST0_BEV);
+}
+
 /**
  * Called plainly from assembler code
  *
@@ -48,5 +93,7 @@ void main_entry(void)
 		r4k_cache_init();
 	}
 
+	trap_init();
+
 	start_barebox();
 }