From feb6a27869aeadf9aff865a2f41438497cd65714 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Markus=20Fr=C3=B6schle?= <mfro@mubf.de>
Date: Fri, 2 Aug 2013 09:35:57 +0000
Subject: [PATCH] provide an early exception vector table to catch exceptions
 during startup, before the final table has been set up (in exceptions.S)

---
 Makefile                |  1 +
 bas.lk.in               |  4 +++-
 include/mmu.h           |  8 +++++++
 sources/fault_vectors.c | 49 +++++++++++++++++++++++++++++++++++++++++
 sources/mmu.c           |  4 ++--
 5 files changed, 63 insertions(+), 3 deletions(-)
 create mode 100644 sources/fault_vectors.c

diff --git a/Makefile b/Makefile
index f631135..793e3d4 100644
--- a/Makefile
+++ b/Makefile
@@ -61,6 +61,7 @@ BASFLASH_EXEC=basflash.$(EXE)
 CSRCS= \
 	$(SRCDIR)/sysinit.c \
 	$(SRCDIR)/init_fpga.c \
+	$(SRCDIR)/fault_vectors.c \
 	$(SRCDIR)/bas_printf.c \
 	$(SRCDIR)/bas_string.c \
 	$(SRCDIR)/BaS.c \
diff --git a/bas.lk.in b/bas.lk.in
index 96ee374..7b987da 100644
--- a/bas.lk.in
+++ b/bas.lk.in
@@ -11,6 +11,7 @@ SECTIONS
 	{
 		objs/startcf.o(.text) /* this one is the entry point so it must be the first */
 		objs/sysinit.o(.text)
+		objs/fault_vectors.o(.text)
 		objs/init_fpga.o(.text)
 		objs/wait.o(.text)
 		
@@ -70,7 +71,8 @@ SECTIONS
 	/* SDRAM Initialization @ 0000_0000 - 1FFF_FFFF 512Mbytes */
  	___SDRAM                = 0x00000000;
  	___SDRAM_SIZE           = 0x20000000;
-
+	_SDRAM_VECTOR_TABLE		= ___SDRAM;
+	
 	/* ST-RAM */
 	__STRAM                 = ___SDRAM;
 	__STRAM_END             = __TOS;
diff --git a/include/mmu.h b/include/mmu.h
index 92aef08..8a17dcf 100644
--- a/include/mmu.h
+++ b/include/mmu.h
@@ -16,6 +16,14 @@
 #define COPYBACK_MMUDR			(MCF_MMU_MMUDR_SZ(00) | MCF_MMU_MMUDR_CM(01) | MCF_MMU_MMUDR_R | MCF_MMU_MMUDR_W | MCF_MMU_MMUDR_X)
 #define NOCACHE_PRECISE_MMUDR	(MCF_MMU_MMUDR_SZ(00) | MCF_MMU_MMUDR_CM(10) | MCF_MMU_MMUDR_R | MCF_MMU_MMUDR_W | MCF_MMU_MMUDR_X)
 
+#define SCA_PAGE_ID		6
+
+/*
+ * global variables from linker script
+ */
+extern long video_tlb;
+extern long video_sbt;
+
 extern void mmu_init(void);
 extern void mmutr_miss(void) __attribute__((interrupt));
 
diff --git a/sources/fault_vectors.c b/sources/fault_vectors.c
new file mode 100644
index 0000000..53c1019
--- /dev/null
+++ b/sources/fault_vectors.c
@@ -0,0 +1,49 @@
+#include "MCF5475.h"
+#include "bas_types.h"
+#include "bas_printf.h"
+
+/*
+ * provide an early exception vector branch table to catch exceptions _before_ VBR has been setup eventually
+ * (to RAMBAR0, in exceptions.S)
+ */
+
+typedef void (*exception_handler)(void);
+extern exception_handler SDRAM_VECTOR_TABLE[];
+
+void fault_handler(uint32_t format_status, uint32_t pc)
+{
+	xprintf("\007\007exception!\r\n");
+	xprintf("format_status: %lx\r\n", format_status);
+	xprintf("pc: %lx\r\n", pc);
+	xprintf("processor halted.\r\n");
+}
+
+void handler(void)
+{
+	/*
+	 * for standard routines, we'd have to save registers here.
+	 * Since we do not intend to return anyway, we just ignore that
+	 */
+	__asm__ __volatile__("move.l	(sp),-(sp)\n\t"		/* format, fault status and status register values */
+						 "move.l	8(sp),-(sp)\n\t"	/* the program counter where the fault originated */
+						 "bra		_fault_handler\n\t"
+						 "halt\n\t"
+							: : :);
+}
+
+void setup_vectors(void)
+{
+	int i;
+
+	for (i = 0; i < 256; i++)
+	{
+		SDRAM_VECTOR_TABLE[i] = handler;
+	}
+
+	/*
+	 * make sure VBR points to our table
+	 */
+	__asm__ __volatile__("clr.l		d0\n\t"
+						 "movec		d0,VBR\n\t"
+						 "move.l	d0,_rt_vbr");
+}
diff --git a/sources/mmu.c b/sources/mmu.c
index d1186b1..35c3577 100644
--- a/sources/mmu.c
+++ b/sources/mmu.c
@@ -68,10 +68,10 @@ void mmu_init(void)
 	 * 0x00d00000 locked ID=6
 	 * video RAM: read write execute normal write true
 	 */
-	MCF_MMU_MMUTR = 0x00d00000 | MCF_MMU_MMUTR_ID(sca_page_ID) | STD_MMUTR;
+	MCF_MMU_MMUTR = 0x00d00000 | MCF_MMU_MMUTR_ID(SCA_PAGE_ID) | STD_MMUTR;
 	MCF_MMU_MMUDR = 0x60d00000 | WRITETHROUGH_MMUDR | MCF_MMU_MMUDR_LK;
 	MCF_MMU_MMUOR = MMUORD_D;
-	MCF_MMU_MMUOR = 0x00d00000 | std_mmutr;
+	MCF_MMU_MMUOR = 0x00d00000 | STD_MMUTR;
 
 	video_tlb = 0x2000;
 	video_sbt = 0;