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 18622c223d6aad65d9384cc452cd6fdc2fac0881..b1ae02fcab8abd064b6a0000c017c6cc0a58f438 100644 (file)
--- a/arch/mips/lib/traps.c
+++ b/arch/mips/lib/traps.c
@@ -1,3 +1,4 @@
+// SPDX-License-Identifier: GPL-2.0+
 /*
  * Copyright (C) 1994 - 1999, 2000, 01, 06 Ralf Baechle
  * Copyright (C) 1995, 1996 Paul M. Antoine
@@ -7,17 +8,22 @@
  * Copyright (C) 2002, 2003, 2004, 2005, 2007  Maciej W. Rozycki
  * Copyright (C) 2000, 2001, 2012 MIPS Technologies, Inc.  All rights reserved.
  * Copyright (C) 2014, Imagination Technologies Ltd.
- *
- * SPDX-License-Identifier:    GPL-2.0+
  */
 
 #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);
@@ -100,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();
+}