Merge pull request #167 from sifive/fix_mtvec_overwrite

Fix mtvec overwrite

Makefile.am

@@ -186,6 +186,7 @@ libriscv__mmachine__@MACHINE_NAME@_a_SOURCES = \
 	src/clock.c \
 	src/cpu.c \
 	src/entry.S \
+	src/trap.S \
 	src/gpio.c \
 	src/init.c \
 	src/interrupt.c \

src/drivers/riscv_cpu.c

@@ -320,10 +320,10 @@ int __metal_exception_register(struct metal_interrupt *controller, int ecode,
     return -1;
 }
 
+extern void early_trap_vector(void);
 void __metal_driver_riscv_cpu_controller_interrupt_init(
     struct metal_interrupt *controller) {
     struct __metal_driver_riscv_cpu_intc *intc = (void *)(controller);
-    uintptr_t val;
 
     if (!intc->init_done) {
         /* Default to use direct interrupt, setup sw cb table*/
@@ -337,9 +337,18 @@ void __metal_driver_riscv_cpu_controller_interrupt_init(
             intc->metal_exception_table[i] = __metal_default_exception_handler;
         }
 
-        __metal_controller_interrupt_vector(
-            METAL_DIRECT_MODE, (void *)(uintptr_t)&__metal_exception_handler);
-
+        /*
+         * Set the real trap handler if the value of mtvec is equal to
+         * early_trap_vector. If mtvec is not equal to early_trap_vector,
+         * that means user has own trap handler, then we don't overwrite it.
+         */
+        uintptr_t mtvec;
+        __asm__ volatile("csrr %0, mtvec" : "=r"(mtvec));
+        if (mtvec == (uintptr_t)&early_trap_vector) {
+            __metal_controller_interrupt_vector(
+                METAL_DIRECT_MODE,
+                (void *)(uintptr_t)&__metal_exception_handler);
+        }
         intc->init_done = 1;
     }
 }

src/entry.S

@@ -80,18 +80,6 @@ _enter:
 
     .cfi_endproc
 
-/* For sanity's sake we set up an early trap vector that just does nothing.  If
- * you end up here then there's a bug in the early boot code somewhere. */
-.section .text.metal.init.trapvec
-.align 2
-early_trap_vector:
-    .cfi_startproc
-    csrr t0, mcause
-    csrr t1, mepc
-    csrr t2, mtval
-    j early_trap_vector
-    .cfi_endproc
-
 /* The GCC port might not emit a __register_frame_info symbol, which eventually
  * results in a weak undefined reference that eventually causes crash when it
  * is dereference early in boot.  We really shouldn't need to put this here,

src/trap.S

@@ -51,3 +51,18 @@ _metal_trap:
     /* Jump to mtvec */
     jr t0
 
+
+/*
+ * For sanity's sake we set up an early trap vector that just does nothing.
+ * If you end up here then there's a bug in the early boot code somewhere.
+ */
+.section .text.metal.init.trapvec
+.global early_trap_vector
+.align 2
+early_trap_vector:
+    .cfi_startproc
+    csrr t0, mcause
+    csrr t1, mepc
+    csrr t2, mtval
+    j early_trap_vector
+    .cfi_endproc