Merge tag 'u-boot-atmel-fixes-2020.07-a' of https://gitlab.denx.de/u-boot/custodians...

[oweals/u-boot.git] / arch / mips / lib / traps.c

diff --git a/arch/mips/lib/traps.c b/arch/mips/lib/traps.c
index 976978c3e96edc746f8fc48a47dd70cf2460de86..b1ae02fcab8abd064b6a0000c017c6cc0a58f438 100644 (file)
--- a/arch/mips/lib/traps.c
+++ b/arch/mips/lib/traps.c
@@ -11,12 +11,19 @@
  */
 
 #include <common.h>
+#include <asm/ptrace.h>
+#include <cpu_func.h>
+#include <hang.h>
+#include <init.h>
+#include <log.h>
 #include <asm/mipsregs.h>
 #include <asm/addrspace.h>
 #include <asm/system.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
+static unsigned long saved_ebase;
+
 static void show_regs(const struct pt_regs *regs)
 {
        const int field = 2 * sizeof(unsigned long);
@@ -99,7 +106,24 @@ void trap_init(ulong reloc_addr)
        set_handler(0x180, &except_vec3_generic, 0x80);
        set_handler(0x280, &except_vec_ejtag_debug, 0x80);
 
+       saved_ebase = read_c0_ebase() & 0xfffff000;
+
        write_c0_ebase(ebase);
        clear_c0_status(ST0_BEV);
        execution_hazard_barrier();
 }
+
+void trap_restore(void)
+{
+       set_c0_status(ST0_BEV);
+       execution_hazard_barrier();
+
+#ifdef CONFIG_OVERRIDE_EXCEPTION_VECTOR_BASE
+       write_c0_ebase(CONFIG_NEW_EXCEPTION_VECTOR_BASE & 0xfffff000);
+#else
+       write_c0_ebase(saved_ebase);
+#endif
+
+       clear_c0_status(ST0_BEV);
+       execution_hazard_barrier();
+}