diff mbox series

[v2,01/21] mips: add support to restore exception vector base before booting linux

Message ID 1579247145-32356-1-git-send-email-weijie.gao@mediatek.com
State Superseded
Headers show
Series [v2,01/21] mips: add support to restore exception vector base before booting linux | expand

Commit Message

Weijie Gao Jan. 17, 2020, 7:45 a.m. UTC
In U-Boot the exception vector base will be moved to top of memory, to be
used to display register dump when exception occurs.

But some old linux kernel does not honor the base set in CP0_EBASE. A
modified exception vector base will cause kernel crash.

This patch adds an option to enable reset exception vector base to its
previous value, or a user configured value before booting linux kernel.

Signed-off-by: Weijie Gao <weijie.gao at mediatek.com>
---
Changes since v1:
  Moved core operations to trap_restore() in arch/mips/lib/traps.c.
  Save previous ebase instead of using 0x80000000 directly.
  Add options to use customized ebase.
---
 arch/mips/Kconfig                   | 30 +++++++++++++++++++++++++++++
 arch/mips/include/asm/u-boot-mips.h |  2 ++
 arch/mips/lib/bootm.c               |  3 +++
 arch/mips/lib/traps.c               | 19 ++++++++++++++++++
 4 files changed, 54 insertions(+)

Comments

Daniel Schwierzeck Jan. 17, 2020, 12:15 p.m. UTC | #1
Am 17.01.20 um 08:45 schrieb Weijie Gao:
> In U-Boot the exception vector base will be moved to top of memory, to be
> used to display register dump when exception occurs.
> 
> But some old linux kernel does not honor the base set in CP0_EBASE. A
> modified exception vector base will cause kernel crash.
> 
> This patch adds an option to enable reset exception vector base to its
> previous value, or a user configured value before booting linux kernel.
> 
> Signed-off-by: Weijie Gao <weijie.gao at mediatek.com>
> ---
> Changes since v1:
>   Moved core operations to trap_restore() in arch/mips/lib/traps.c.
>   Save previous ebase instead of using 0x80000000 directly.
>   Add options to use customized ebase.
> ---
>  arch/mips/Kconfig                   | 30 +++++++++++++++++++++++++++++
>  arch/mips/include/asm/u-boot-mips.h |  2 ++
>  arch/mips/lib/bootm.c               |  3 +++
>  arch/mips/lib/traps.c               | 19 ++++++++++++++++++
>  4 files changed, 54 insertions(+)
> 
> diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
> index a3ae603044..5e20feeefb 100644
> --- a/arch/mips/Kconfig
> +++ b/arch/mips/Kconfig
> @@ -287,6 +287,36 @@ config MIPS_RELOCATION_TABLE_SIZE
>  
>  	  If unsure, leave at the default value.
>  
> +config RESTORE_EXCEPTION_VECTOR_BASE
> +	bool "Restore exception vector base before booting linux kernel"
> +	default n
> +	help
> +	  In U-Boot the exception vector base will be moved to top of memory,
> +	  to be used to display register dump when exception occurs.
> +	  But some old linux kernel does not honor the base set in CP0_EBASE.
> +	  A modified exception vector base will cause kernel crash.
> +
> +	  This option will restore the exception vector base to its previous
> +	  value.
> +
> +	  If unsure, say N.
> +
> +config OVERRIDE_EXCEPTION_VECTOR_BASE
> +	bool "Override the exception vector base to be restored"
> +	depends on RESTORE_EXCEPTION_VECTOR_BASE
> +	default n
> +	help
> +	  Enable this option if you want to use a different exception vector
> +	  base rather than the previously saved one.
> +
> +config NEW_EXCEPTION_VECTOR_BASE
> +	hex "New exception vector base"
> +	depends on OVERRIDE_EXCEPTION_VECTOR_BASE
> +	range 0x80000000 0xbffff000
> +	default 0x80000000
> +	help
> +	  The exception vector base to be restored before booting linux kernel
> +
>  endmenu
>  
>  menu "OS boot interface"
> diff --git a/arch/mips/include/asm/u-boot-mips.h b/arch/mips/include/asm/u-boot-mips.h
> index 88438b9576..8b37cc4029 100644
> --- a/arch/mips/include/asm/u-boot-mips.h
> +++ b/arch/mips/include/asm/u-boot-mips.h
> @@ -9,4 +9,6 @@ void except_vec_ejtag_debug(void);
>  
>  int arch_misc_init(void);
>  
> +void trap_restore(void);
> +
>  #endif /* _U_BOOT_MIPS_H_ */
> diff --git a/arch/mips/lib/bootm.c b/arch/mips/lib/bootm.c
> index 8c0d7672f2..f1db6d23b8 100644
> --- a/arch/mips/lib/bootm.c
> +++ b/arch/mips/lib/bootm.c
> @@ -294,6 +294,9 @@ static void boot_jump_linux(bootm_headers_t *images)
>  	bootstage_report();
>  #endif
>  
> +	if (CONFIG_IS_ENABLED(RESTORE_EXCEPTION_VECTOR_BASE))
> +		trap_restore();
> +
>  	if (images->ft_len)
>  		kernel(-2, (ulong)images->ft_addr, 0, 0);
>  	else
> diff --git a/arch/mips/lib/traps.c b/arch/mips/lib/traps.c
> index 6fe8ebd16b..20f45fc4ed 100644
> --- a/arch/mips/lib/traps.c
> +++ b/arch/mips/lib/traps.c
> @@ -19,6 +19,8 @@
>  
>  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);
> @@ -101,7 +103,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();
> +}
> 

looks actually good now, thanks. But now I'm thinking that it should be
enough to simply set ST0_BEV to use the CPU's default EBase. Restoring
the original EBase and clearing ST0_BEV again seems redundant. How about
this?

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);
	clear_c0_status(ST0_BEV);
	execution_hazard_barrier();
#endif
}
Weijie Gao Jan. 19, 2020, 4:18 a.m. UTC | #2
On Fri, 2020-01-17 at 13:15 +0100, Daniel Schwierzeck wrote:
> 
> Am 17.01.20 um 08:45 schrieb Weijie Gao:
> > In U-Boot the exception vector base will be moved to top of memory, to be
> > used to display register dump when exception occurs.
> > 
> > But some old linux kernel does not honor the base set in CP0_EBASE. A
> > modified exception vector base will cause kernel crash.
> > 
> > This patch adds an option to enable reset exception vector base to its
> > previous value, or a user configured value before booting linux kernel.
> > 
> > Signed-off-by: Weijie Gao <weijie.gao at mediatek.com>
> > ---
> > Changes since v1:
> >   Moved core operations to trap_restore() in arch/mips/lib/traps.c.
> >   Save previous ebase instead of using 0x80000000 directly.
> >   Add options to use customized ebase.
> > ---
> >  arch/mips/Kconfig                   | 30 +++++++++++++++++++++++++++++
> >  arch/mips/include/asm/u-boot-mips.h |  2 ++
> >  arch/mips/lib/bootm.c               |  3 +++
> >  arch/mips/lib/traps.c               | 19 ++++++++++++++++++
> >  4 files changed, 54 insertions(+)
> > 
> > diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
> > index a3ae603044..5e20feeefb 100644
> > --- a/arch/mips/Kconfig
> > +++ b/arch/mips/Kconfig
> > @@ -287,6 +287,36 @@ config MIPS_RELOCATION_TABLE_SIZE
> >  
> >  	  If unsure, leave at the default value.
> >  
> > +config RESTORE_EXCEPTION_VECTOR_BASE
> > +	bool "Restore exception vector base before booting linux kernel"
> > +	default n
> > +	help
> > +	  In U-Boot the exception vector base will be moved to top of memory,
> > +	  to be used to display register dump when exception occurs.
> > +	  But some old linux kernel does not honor the base set in CP0_EBASE.
> > +	  A modified exception vector base will cause kernel crash.
> > +
> > +	  This option will restore the exception vector base to its previous
> > +	  value.
> > +
> > +	  If unsure, say N.
> > +
> > +config OVERRIDE_EXCEPTION_VECTOR_BASE
> > +	bool "Override the exception vector base to be restored"
> > +	depends on RESTORE_EXCEPTION_VECTOR_BASE
> > +	default n
> > +	help
> > +	  Enable this option if you want to use a different exception vector
> > +	  base rather than the previously saved one.
> > +
> > +config NEW_EXCEPTION_VECTOR_BASE
> > +	hex "New exception vector base"
> > +	depends on OVERRIDE_EXCEPTION_VECTOR_BASE
> > +	range 0x80000000 0xbffff000
> > +	default 0x80000000
> > +	help
> > +	  The exception vector base to be restored before booting linux kernel
> > +
> >  endmenu
> >  
> >  menu "OS boot interface"
> > diff --git a/arch/mips/include/asm/u-boot-mips.h b/arch/mips/include/asm/u-boot-mips.h
> > index 88438b9576..8b37cc4029 100644
> > --- a/arch/mips/include/asm/u-boot-mips.h
> > +++ b/arch/mips/include/asm/u-boot-mips.h
> > @@ -9,4 +9,6 @@ void except_vec_ejtag_debug(void);
> >  
> >  int arch_misc_init(void);
> >  
> > +void trap_restore(void);
> > +
> >  #endif /* _U_BOOT_MIPS_H_ */
> > diff --git a/arch/mips/lib/bootm.c b/arch/mips/lib/bootm.c
> > index 8c0d7672f2..f1db6d23b8 100644
> > --- a/arch/mips/lib/bootm.c
> > +++ b/arch/mips/lib/bootm.c
> > @@ -294,6 +294,9 @@ static void boot_jump_linux(bootm_headers_t *images)
> >  	bootstage_report();
> >  #endif
> >  
> > +	if (CONFIG_IS_ENABLED(RESTORE_EXCEPTION_VECTOR_BASE))
> > +		trap_restore();
> > +
> >  	if (images->ft_len)
> >  		kernel(-2, (ulong)images->ft_addr, 0, 0);
> >  	else
> > diff --git a/arch/mips/lib/traps.c b/arch/mips/lib/traps.c
> > index 6fe8ebd16b..20f45fc4ed 100644
> > --- a/arch/mips/lib/traps.c
> > +++ b/arch/mips/lib/traps.c
> > @@ -19,6 +19,8 @@
> >  
> >  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);
> > @@ -101,7 +103,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();
> > +}
> > 
> 
> looks actually good now, thanks. But now I'm thinking that it should be
> enough to simply set ST0_BEV to use the CPU's default EBase. Restoring
> the original EBase and clearing ST0_BEV again seems redundant. How about
> this?
> 
> 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);
> 	clear_c0_status(ST0_BEV);
> 	execution_hazard_barrier();
> #endif
> }
> 

Actually it's a little bit redundant. saved_ebase is reserved for the
case that u-boot acts as a secondary bootloader, and the first stage
bootloader changes the ebase.

Currently it seems there is no such case. I'll simplify this patch.
Weijie Gao Jan. 19, 2020, 9:08 a.m. UTC | #3
On Sun, 2020-01-19 at 12:18 +0800, Weijie Gao wrote:
> On Fri, 2020-01-17 at 13:15 +0100, Daniel Schwierzeck wrote:
> > 
> > Am 17.01.20 um 08:45 schrieb Weijie Gao:
> > > In U-Boot the exception vector base will be moved to top of memory, to be
> > > used to display register dump when exception occurs.
> > > 
> > > But some old linux kernel does not honor the base set in CP0_EBASE. A
> > > modified exception vector base will cause kernel crash.
> > > 
> > > This patch adds an option to enable reset exception vector base to its
> > > previous value, or a user configured value before booting linux kernel.
> > > 
> > > Signed-off-by: Weijie Gao <weijie.gao at mediatek.com>
> > > ---
> > > Changes since v1:
> > >   Moved core operations to trap_restore() in arch/mips/lib/traps.c.
> > >   Save previous ebase instead of using 0x80000000 directly.
> > >   Add options to use customized ebase.
> > > ---
> > >  arch/mips/Kconfig                   | 30 +++++++++++++++++++++++++++++
> > >  arch/mips/include/asm/u-boot-mips.h |  2 ++
> > >  arch/mips/lib/bootm.c               |  3 +++
> > >  arch/mips/lib/traps.c               | 19 ++++++++++++++++++
> > >  4 files changed, 54 insertions(+)
> > > 
> > > diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
> > > index a3ae603044..5e20feeefb 100644
> > > --- a/arch/mips/Kconfig
> > > +++ b/arch/mips/Kconfig
> > > @@ -287,6 +287,36 @@ config MIPS_RELOCATION_TABLE_SIZE
> > >  
> > >  	  If unsure, leave at the default value.
> > >  
> > > +config RESTORE_EXCEPTION_VECTOR_BASE
> > > +	bool "Restore exception vector base before booting linux kernel"
> > > +	default n
> > > +	help
> > > +	  In U-Boot the exception vector base will be moved to top of memory,
> > > +	  to be used to display register dump when exception occurs.
> > > +	  But some old linux kernel does not honor the base set in CP0_EBASE.
> > > +	  A modified exception vector base will cause kernel crash.
> > > +
> > > +	  This option will restore the exception vector base to its previous
> > > +	  value.
> > > +
> > > +	  If unsure, say N.
> > > +
> > > +config OVERRIDE_EXCEPTION_VECTOR_BASE
> > > +	bool "Override the exception vector base to be restored"
> > > +	depends on RESTORE_EXCEPTION_VECTOR_BASE
> > > +	default n
> > > +	help
> > > +	  Enable this option if you want to use a different exception vector
> > > +	  base rather than the previously saved one.
> > > +
> > > +config NEW_EXCEPTION_VECTOR_BASE
> > > +	hex "New exception vector base"
> > > +	depends on OVERRIDE_EXCEPTION_VECTOR_BASE
> > > +	range 0x80000000 0xbffff000
> > > +	default 0x80000000
> > > +	help
> > > +	  The exception vector base to be restored before booting linux kernel
> > > +
> > >  endmenu
> > >  
> > >  menu "OS boot interface"
> > > diff --git a/arch/mips/include/asm/u-boot-mips.h b/arch/mips/include/asm/u-boot-mips.h
> > > index 88438b9576..8b37cc4029 100644
> > > --- a/arch/mips/include/asm/u-boot-mips.h
> > > +++ b/arch/mips/include/asm/u-boot-mips.h
> > > @@ -9,4 +9,6 @@ void except_vec_ejtag_debug(void);
> > >  
> > >  int arch_misc_init(void);
> > >  
> > > +void trap_restore(void);
> > > +
> > >  #endif /* _U_BOOT_MIPS_H_ */
> > > diff --git a/arch/mips/lib/bootm.c b/arch/mips/lib/bootm.c
> > > index 8c0d7672f2..f1db6d23b8 100644
> > > --- a/arch/mips/lib/bootm.c
> > > +++ b/arch/mips/lib/bootm.c
> > > @@ -294,6 +294,9 @@ static void boot_jump_linux(bootm_headers_t *images)
> > >  	bootstage_report();
> > >  #endif
> > >  
> > > +	if (CONFIG_IS_ENABLED(RESTORE_EXCEPTION_VECTOR_BASE))
> > > +		trap_restore();
> > > +
> > >  	if (images->ft_len)
> > >  		kernel(-2, (ulong)images->ft_addr, 0, 0);
> > >  	else
> > > diff --git a/arch/mips/lib/traps.c b/arch/mips/lib/traps.c
> > > index 6fe8ebd16b..20f45fc4ed 100644
> > > --- a/arch/mips/lib/traps.c
> > > +++ b/arch/mips/lib/traps.c
> > > @@ -19,6 +19,8 @@
> > >  
> > >  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);
> > > @@ -101,7 +103,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();
> > > +}
> > > 
> > 
> > looks actually good now, thanks. But now I'm thinking that it should be
> > enough to simply set ST0_BEV to use the CPU's default EBase. Restoring
> > the original EBase and clearing ST0_BEV again seems redundant. How about
> > this?
> > 
> > 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);
> > 	clear_c0_status(ST0_BEV);
> > 	execution_hazard_barrier();
> > #endif
> > }
> > 
> 
> Actually it's a little bit redundant. saved_ebase is reserved for the
> case that u-boot acts as a secondary bootloader, and the first stage
> bootloader changes the ebase.
> 
> Currently it seems there is no such case. I'll simplify this patch.
> 
> 

Correction:

ST0_BEV only determines which exception base vector to be used. And in
order to use a writable exception vector, ST0_BEV will always be cleared
by the kernel.

If the MIPS core implementation supports VEIC or VInt, the kernel will
change the EBase and the value of EBase set by U-Boot will automatically
get overwritten.

But if both VEIC and VInt are not supported, the kernel will just use
the current value of EBase, and leave it untouched. (This has been fixed
half a year ago, and changed to check whether implementation is MIPS
Release 2 or higher. But most kernel images do not have this change)

The most important thing I found just now, is that the value of EBase
keeps unchanged whether ST0_BEV is set or not. So just setting the
ST0_BEV bit is not enough. The value of EBase must be changed or the
kernel will still use the value set by U-Boot.

My suggestion is leave this patch unchanged, or at most remove
saved_ebase.
diff mbox series

Patch

diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
index a3ae603044..5e20feeefb 100644
--- a/arch/mips/Kconfig
+++ b/arch/mips/Kconfig
@@ -287,6 +287,36 @@  config MIPS_RELOCATION_TABLE_SIZE
 
 	  If unsure, leave at the default value.
 
+config RESTORE_EXCEPTION_VECTOR_BASE
+	bool "Restore exception vector base before booting linux kernel"
+	default n
+	help
+	  In U-Boot the exception vector base will be moved to top of memory,
+	  to be used to display register dump when exception occurs.
+	  But some old linux kernel does not honor the base set in CP0_EBASE.
+	  A modified exception vector base will cause kernel crash.
+
+	  This option will restore the exception vector base to its previous
+	  value.
+
+	  If unsure, say N.
+
+config OVERRIDE_EXCEPTION_VECTOR_BASE
+	bool "Override the exception vector base to be restored"
+	depends on RESTORE_EXCEPTION_VECTOR_BASE
+	default n
+	help
+	  Enable this option if you want to use a different exception vector
+	  base rather than the previously saved one.
+
+config NEW_EXCEPTION_VECTOR_BASE
+	hex "New exception vector base"
+	depends on OVERRIDE_EXCEPTION_VECTOR_BASE
+	range 0x80000000 0xbffff000
+	default 0x80000000
+	help
+	  The exception vector base to be restored before booting linux kernel
+
 endmenu
 
 menu "OS boot interface"
diff --git a/arch/mips/include/asm/u-boot-mips.h b/arch/mips/include/asm/u-boot-mips.h
index 88438b9576..8b37cc4029 100644
--- a/arch/mips/include/asm/u-boot-mips.h
+++ b/arch/mips/include/asm/u-boot-mips.h
@@ -9,4 +9,6 @@  void except_vec_ejtag_debug(void);
 
 int arch_misc_init(void);
 
+void trap_restore(void);
+
 #endif /* _U_BOOT_MIPS_H_ */
diff --git a/arch/mips/lib/bootm.c b/arch/mips/lib/bootm.c
index 8c0d7672f2..f1db6d23b8 100644
--- a/arch/mips/lib/bootm.c
+++ b/arch/mips/lib/bootm.c
@@ -294,6 +294,9 @@  static void boot_jump_linux(bootm_headers_t *images)
 	bootstage_report();
 #endif
 
+	if (CONFIG_IS_ENABLED(RESTORE_EXCEPTION_VECTOR_BASE))
+		trap_restore();
+
 	if (images->ft_len)
 		kernel(-2, (ulong)images->ft_addr, 0, 0);
 	else
diff --git a/arch/mips/lib/traps.c b/arch/mips/lib/traps.c
index 6fe8ebd16b..20f45fc4ed 100644
--- a/arch/mips/lib/traps.c
+++ b/arch/mips/lib/traps.c
@@ -19,6 +19,8 @@ 
 
 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);
@@ -101,7 +103,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();
+}