4 files changed, 55 insertions, 33 deletions

diff --git a/arch/sh/include/asm/sh_bios.h b/arch/sh/include/asm/sh_bios.h
index d9c96d7cf6c7..d8a67a17777c 100644
--- a/arch/sh/include/asm/sh_bios.h
+++ b/arch/sh/include/asm/sh_bios.h
@@ -1,13 +1,13 @@
 #ifndef __ASM_SH_BIOS_H
 #define __ASM_SH_BIOS_H
 
+#ifdef CONFIG_SH_STANDARD_BIOS
+
 /*
  * Copyright (C) 2000 Greg Banks, Mitch Davis
  * C API to interface to the standard LinuxSH BIOS
  * usually from within the early stages of kernel boot.
  */
-
-
 extern void sh_bios_console_write(const char *buf, unsigned int len);
 extern void sh_bios_char_out(char ch);
 extern void sh_bios_gdb_detach(void);
@@ -15,4 +15,14 @@ extern void sh_bios_gdb_detach(void);
 extern void sh_bios_get_node_addr(unsigned char *node_addr);
 extern void sh_bios_shutdown(unsigned int how);
 
+extern void sh_bios_vbr_init(void);
+extern void sh_bios_vbr_reload(void);
+
+#else
+
+static inline void sh_bios_vbr_init(void) { }
+static inline void sh_bios_vbr_reload(void) { }
+
+#endif /* CONFIG_SH_STANDARD_BIOS */
+
 #endif /* __ASM_SH_BIOS_H */
diff --git a/arch/sh/kernel/machine_kexec.c b/arch/sh/kernel/machine_kexec.c
index 76f280223ebd..f52d8ed69e12 100644
--- a/arch/sh/kernel/machine_kexec.c
+++ b/arch/sh/kernel/machine_kexec.c
@@ -21,6 +21,7 @@
 #include <asm/mmu_context.h>
 #include <asm/io.h>
 #include <asm/cacheflush.h>
+#include <asm/sh_bios.h>
 
 typedef void (*relocate_new_kernel_t)(unsigned long indirection_page,
                                       unsigned long reboot_code_buffer,
@@ -28,7 +29,6 @@ typedef void (*relocate_new_kernel_t)(unsigned long indirection_page,
 
 extern const unsigned char relocate_new_kernel[];
 extern const unsigned int relocate_new_kernel_size;
-extern void *gdb_vbr_vector;
 extern void *vbr_base;
 
 void machine_shutdown(void)
@@ -117,11 +117,7 @@ void machine_kexec(struct kimage *image)
         kexec_info(image);
         flush_cache_all();
 
-#if defined(CONFIG_SH_STANDARD_BIOS)
-        asm volatile("ldc %0, vbr" :
-                     : "r" (((unsigned long) gdb_vbr_vector) - 0x100)
-                     : "memory");
-#endif
+        sh_bios_vbr_reload();
 
         /* now call it */
         rnk = (relocate_new_kernel_t) reboot_code_buffer;
diff --git a/arch/sh/kernel/sh_bios.c b/arch/sh/kernel/sh_bios.c
index c852f7805728..2a5f2e0d505d 100644
--- a/arch/sh/kernel/sh_bios.c
+++ b/arch/sh/kernel/sh_bios.c
@@ -55,3 +55,40 @@ void sh_bios_shutdown(unsigned int how)
 {
         sh_bios_call(BIOS_CALL_SHUTDOWN, how, 0, 0, 0);
 }
+
+void *gdb_vbr_vector = NULL;
+
+/*
+ * Read the old value of the VBR register to initialise the vector
+ * through which debug and BIOS traps are delegated by the Linux trap
+ * handler.
+ */
+void sh_bios_vbr_init(void)
+{
+        unsigned long vbr;
+
+        if (unlikely(gdb_vbr_vector))
+                return;
+
+        __asm__ __volatile__ ("stc vbr, %0" : "=r" (vbr));
+
+        gdb_vbr_vector = (void *)(vbr + 0x100);
+        printk(KERN_NOTICE "Setting GDB trap vector to %p\n", gdb_vbr_vector);
+}
+
+/**
+ * sh_bios_vbr_reload - Re-load the system VBR from the BIOS vector.
+ *
+ * This can be used by save/restore code to reinitialize the system VBR
+ * from the fixed BIOS VBR. A no-op if no BIOS VBR is known.
+ */
+void sh_bios_vbr_reload(void)
+{
+        if (gdb_vbr_vector)
+                __asm__ __volatile__ (
+                        "ldc %0, vbr"
+                        :
+                        : "r" (((unsigned long) gdb_vbr_vector) - 0x100)
+                        : "memory"
+                );
+}
diff --git a/arch/sh/kernel/traps_32.c b/arch/sh/kernel/traps_32.c
index 86639beac3a2..efcbdfe52f52 100644
--- a/arch/sh/kernel/traps_32.c
+++ b/arch/sh/kernel/traps_32.c
@@ -31,6 +31,7 @@
 #include <asm/uaccess.h>
 #include <asm/fpu.h>
 #include <asm/kprobes.h>
+#include <asm/sh_bios.h>
 
 #ifdef CONFIG_CPU_SH2
 # define TRAP_RESERVED_INST     4
@@ -876,35 +877,10 @@ asmlinkage void do_exception_error(unsigned long r4, unsigned long r5,
         die_if_kernel("exception", regs, ex);
 }
 
-#if defined(CONFIG_SH_STANDARD_BIOS)
-void *gdb_vbr_vector;
-
-static inline void __init gdb_vbr_init(void)
-{
-        register unsigned long vbr;
-
-        /*
-         * Read the old value of the VBR register to initialise
-         * the vector through which debug and BIOS traps are
-         * delegated by the Linux trap handler.
-         */
-        asm volatile("stc vbr, %0" : "=r" (vbr));
-
-        gdb_vbr_vector = (void *)(vbr + 0x100);
-        printk("Setting GDB trap vector to 0x%08lx\n",
-               (unsigned long)gdb_vbr_vector);
-}
-#endif
-
 void __cpuinit per_cpu_trap_init(void)
 {
         extern void *vbr_base;
 
-#ifdef CONFIG_SH_STANDARD_BIOS
-        if (raw_smp_processor_id() == 0)
-                gdb_vbr_init();
-#endif
-
         /* NOTE: The VBR value should be at P1
            (or P2, virtural "fixed" address space).
            It's definitely should not in physical address.  */
@@ -959,6 +935,9 @@ void __init trap_init(void)
         set_exception_table_vec(TRAP_UBC, break_point_trap);
 #endif
 
+        /* Save off the BIOS VBR, if there is one */
+        sh_bios_vbr_init();
+
         /* Setup VBR for boot cpu */
         per_cpu_trap_init();
 }