4 files changed, 44 insertions, 24 deletions

diff --git a/arch/sh/kernel/cpu/sh3/ex.S b/arch/sh/kernel/cpu/sh3/ex.S
index 6be46f0686b7..ba3082d640b5 100644
--- a/arch/sh/kernel/cpu/sh3/ex.S
+++ b/arch/sh/kernel/cpu/sh3/ex.S
@@ -4,7 +4,7 @@
  *  The SH-3 exception vector table.
 
  *  Copyright (C) 1999, 2000, 2002  Niibe Yutaka
- *  Copyright (C) 2003  Paul Mundt
+ *  Copyright (C) 2003 - 2006  Paul Mundt
  *
  * This file is subject to the terms and conditions of the GNU General Public
  * License.  See the file "COPYING" in the main directory of this archive
@@ -49,3 +49,10 @@ ENTRY(nmi_slot)
 #endif
 ENTRY(user_break_point_trap)
         .long   break_point_trap        /* 1E0 */
+
+        /*
+         * Pad the remainder of the table out, exceptions residing in far
+         * away offsets can be manually inserted in to their appropriate
+         * location via set_exception_table_{evt,vec}().
+         */
+        .balign 4096,0,4096
diff --git a/arch/sh/kernel/cpu/sh4/ex.S b/arch/sh/kernel/cpu/sh4/ex.S
index 3f4cd043e900..ac8ab57413cc 100644
--- a/arch/sh/kernel/cpu/sh4/ex.S
+++ b/arch/sh/kernel/cpu/sh4/ex.S
@@ -4,7 +4,7 @@
  *  The SH-4 exception vector table.
 
  *  Copyright (C) 1999, 2000, 2002  Niibe Yutaka
- *  Copyright (C) 2003  Paul Mundt
+ *  Copyright (C) 2003 - 2006  Paul Mundt
  *
  * This file is subject to the terms and conditions of the GNU General Public
  * License.  See the file "COPYING" in the main directory of this archive
@@ -53,3 +53,10 @@ ENTRY(nmi_slot)
 #endif
 ENTRY(user_break_point_trap)
         .long   break_point_trap        /* 1E0 */
+
+        /*
+         * Pad the remainder of the table out, exceptions residing in far
+         * away offsets can be manually inserted in to their appropriate
+         * location via set_exception_table_{evt,vec}().
+         */
+        .balign 4096,0,4096
diff --git a/arch/sh/kernel/traps.c b/arch/sh/kernel/traps.c
index ffe127f09f3e..53dfa55f3156 100644
--- a/arch/sh/kernel/traps.c
+++ b/arch/sh/kernel/traps.c
@@ -11,27 +11,15 @@
  * License.  See the file "COPYING" in the main directory of this archive
  * for more details.
  */
-#include <linux/sched.h>
 #include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/errno.h>
 #include <linux/ptrace.h>
-#include <linux/timer.h>
-#include <linux/mm.h>
-#include <linux/smp.h>
-#include <linux/smp_lock.h>
 #include <linux/init.h>
-#include <linux/delay.h>
 #include <linux/spinlock.h>
 #include <linux/module.h>
 #include <linux/kallsyms.h>
-
+#include <linux/io.h>
 #include <asm/system.h>
 #include <asm/uaccess.h>
-#include <asm/io.h>
-#include <asm/atomic.h>
-#include <asm/processor.h>
-#include <asm/sections.h>
 
 #ifdef CONFIG_SH_KGDB
 #include <asm/kgdb.h>
@@ -581,7 +569,10 @@ int is_dsp_inst(struct pt_regs *regs)
 #define is_dsp_inst(regs)       (0)
 #endif /* CONFIG_SH_DSP */
 
-extern int do_fpu_inst(unsigned short, struct pt_regs*);
+/* arch/sh/kernel/cpu/sh4/fpu.c */
+extern int do_fpu_inst(unsigned short, struct pt_regs *);
+extern asmlinkage void do_fpu_state_restore(unsigned long r4, unsigned long r5,
+                unsigned long r6, unsigned long r7, struct pt_regs regs);
 
 asmlinkage void do_reserved_inst(unsigned long r4, unsigned long r5,
                                 unsigned long r6, unsigned long r7,
@@ -740,14 +731,20 @@ void __init per_cpu_trap_init(void)
                      : "memory");
 }
 
-void __init trap_init(void)
+void *set_exception_table_vec(unsigned int vec, void *handler)
 {
         extern void *exception_handling_table[];
+        void *old_handler;
+        
+        old_handler = exception_handling_table[vec];
+        exception_handling_table[vec] = handler;
+        return old_handler;
+}
 
-        exception_handling_table[TRAP_RESERVED_INST]
-                = (void *)do_reserved_inst;
-        exception_handling_table[TRAP_ILLEGAL_SLOT_INST]
-                = (void *)do_illegal_slot_inst;
+void __init trap_init(void)
+{
+        set_exception_table_vec(TRAP_RESERVED_INST, do_reserved_inst);
+        set_exception_table_vec(TRAP_ILLEGAL_SLOT_INST, do_illegal_slot_inst);
 
 #if defined(CONFIG_CPU_SH4) && !defined(CONFIG_SH_FPU) || \
     defined(CONFIG_SH_FPU_EMU)
@@ -756,9 +753,11 @@ void __init trap_init(void)
          * reserved. They'll be handled in the math-emu case, or faulted on
          * otherwise.
          */
-        /* entry 64 corresponds to EXPEVT=0x800 */
-        exception_handling_table[64] = (void *)do_reserved_inst;
-        exception_handling_table[65] = (void *)do_illegal_slot_inst;
+        set_exception_table_evt(0x800, do_reserved_inst);
+        set_exception_table_evt(0x820, do_illegal_slot_inst);
+#elif defined(CONFIG_SH_FPU)
+        set_exception_table_evt(0x800, do_fpu_state_restore);
+        set_exception_table_evt(0x820, do_fpu_state_restore);
 #endif
                 
         /* Setup VBR for boot cpu */
diff --git a/include/asm-sh/system.h b/include/asm-sh/system.h
index 6c1f8fde5ac4..3340126f4e0f 100644
--- a/include/asm-sh/system.h
+++ b/include/asm-sh/system.h
@@ -353,6 +353,13 @@ static inline unsigned long __cmpxchg(volatile void * ptr, unsigned long old,
                                     (unsigned long)_n_, sizeof(*(ptr))); \
   })
 
+extern void *set_exception_table_vec(unsigned int vec, void *handler);
+
+static inline void *set_exception_table_evt(unsigned int evt, void *handler)
+{
+        return set_exception_table_vec(evt >> 5, handler);
+}
+
 /* XXX
  * disable hlt during certain critical i/o operations
  */