7 files changed, 113 insertions, 85 deletions

diff --git a/arch/sh/kernel/Makefile b/arch/sh/kernel/Makefile
index 2f6d2bcb1c93..ff30d7f58043 100644
--- a/arch/sh/kernel/Makefile
+++ b/arch/sh/kernel/Makefile
@@ -6,7 +6,8 @@ extra-y	:= head.o init_task.o vmlinux.lds
 
 obj-y   := process.o signal.o traps.o irq.o \
         ptrace.o setup.o time.o sys_sh.o semaphore.o \
-        io.o io_generic.o sh_ksyms.o syscalls.o
+        io.o io_generic.o sh_ksyms.o syscalls.o \
+        debugtraps.o
 
 obj-y                           += cpu/ timers/
 obj-$(CONFIG_VSYSCALL)          += vsyscall/
diff --git a/arch/sh/kernel/cpu/sh2/entry.S b/arch/sh/kernel/cpu/sh2/entry.S
index d51fa5e9904a..8de48102ac85 100644
--- a/arch/sh/kernel/cpu/sh2/entry.S
+++ b/arch/sh/kernel/cpu/sh2/entry.S
@@ -206,7 +206,7 @@ trap_entry:
 
 #if defined(CONFIG_SH_STANDARD_BIOS)
         /* Unwind the stack and jmp to the debug entry */
-debug_kernel_fw:
+ENTRY(sh_bios_handler)
         mov     r15,r0
         add     #(22-4)*4-4,r0
         ldc.l   @r0+,gbr
diff --git a/arch/sh/kernel/cpu/sh3/entry.S b/arch/sh/kernel/cpu/sh3/entry.S
index 8c0dc2700c69..014ac37ca16a 100644
--- a/arch/sh/kernel/cpu/sh3/entry.S
+++ b/arch/sh/kernel/cpu/sh3/entry.S
@@ -173,7 +173,7 @@ call_dae:
 
 #if defined(CONFIG_SH_STANDARD_BIOS)
         /* Unwind the stack and jmp to the debug entry */
-debug_kernel_fw:
+ENTRY(sh_bios_handler)
         mov.l   @r15+, r0
         mov.l   @r15+, r1
         mov.l   @r15+, r2
diff --git a/arch/sh/kernel/debugtraps.S b/arch/sh/kernel/debugtraps.S
new file mode 100644
index 000000000000..13b66746410a
--- /dev/null
+++ b/arch/sh/kernel/debugtraps.S
@@ -0,0 +1,41 @@
+/*
+ * arch/sh/kernel/debugtraps.S
+ *
+ * Debug trap jump tables for SuperH
+ *
+ *  Copyright (C) 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
+ * for more details.
+ */
+#include <linux/sys.h>
+#include <linux/linkage.h>
+
+#if !defined(CONFIG_SH_KGDB)
+#define kgdb_handle_exception   debug_trap_handler
+#endif
+
+#if !defined(CONFIG_SH_STANDARD_BIOS)
+#define sh_bios_handler         debug_trap_handler
+#endif
+
+        .data
+
+ENTRY(debug_trap_table)
+        .long debug_trap_handler        /* 0x30 */
+        .long debug_trap_handler        /* 0x31 */
+        .long debug_trap_handler        /* 0x32 */
+        .long debug_trap_handler        /* 0x33 */
+        .long debug_trap_handler        /* 0x34 */
+        .long debug_trap_handler        /* 0x35 */
+        .long debug_trap_handler        /* 0x36 */
+        .long debug_trap_handler        /* 0x37 */
+        .long debug_trap_handler        /* 0x38 */
+        .long debug_trap_handler        /* 0x39 */
+        .long debug_trap_handler        /* 0x3a */
+        .long debug_trap_handler        /* 0x3b */
+        .long kgdb_handle_exception     /* 0x3c */
+        .long debug_trap_handler        /* 0x3d */
+        .long bug_trap_handler          /* 0x3e */
+        .long sh_bios_handler           /* 0x3f */
diff --git a/arch/sh/kernel/entry-common.S b/arch/sh/kernel/entry-common.S
index fc279aeb73ab..ab4ebb856c2a 100644
--- a/arch/sh/kernel/entry-common.S
+++ b/arch/sh/kernel/entry-common.S
@@ -54,79 +54,24 @@
 #  define resume_kernel         __restore_all
 #endif
 
-#if defined(CONFIG_SH_STANDARD_BIOS) || defined(CONFIG_SH_KGDB)
-! Handle kernel debug if either kgdb (SW) or gdb-stub (FW) is present.
-! If both are configured, handle the debug traps (breakpoints) in SW,
-! but still allow BIOS traps to FW.
-
-        .align  2
-debug_kernel:
-#if defined(CONFIG_SH_STANDARD_BIOS) && defined(CONFIG_SH_KGDB)
-        /* Force BIOS call to FW (debug_trap put TRA in r8) */
-        mov     r8,r0
-        shlr2   r0
-        cmp/eq  #0x3f,r0
-        bt      debug_kernel_fw
-#endif /* CONFIG_SH_STANDARD_BIOS && CONFIG_SH_KGDB */
-
-debug_enter:            
-#if defined(CONFIG_SH_KGDB)
-        /* Jump to kgdb, pass stacked regs as arg */
-debug_kernel_sw:
-        mov.l   3f, r0
-        jmp     @r0
-         mov    r15, r4
-        .align  2
-3:      .long   kgdb_handle_exception
-#endif /* CONFIG_SH_KGDB */
-#ifdef CONFIG_SH_STANDARD_BIOS
-        bra     debug_kernel_fw
-         nop
-#endif
-#endif /* CONFIG_SH_STANDARD_BIOS || CONFIG_SH_KGDB */
-
-        .align  2
-debug_trap:     
-#if defined(CONFIG_SH_STANDARD_BIOS) || defined(CONFIG_SH_KGDB)
-        mov     r8, r0
-        shlr2   r0
-        cmp/eq  #0x3f, r0               ! sh_bios() trap
-        bf      1f
-#ifdef CONFIG_SH_KGDB
-        cmp/eq  #0xff, r0               ! XXX: KGDB trap, fix for SH-2.
-        bf      1f
-#endif
-        mov     #OFF_SR, r0
-        mov.l   @(r0,r15), r0           ! get status register
-        shll    r0
-        shll    r0                      ! kernel space?
-        bt/s    debug_kernel
-1:
-#endif
-         mov.l  @r15, r0                ! Restore R0 value
-        mov.l   1f, r8
-        jmp     @r8
-         nop
 
         .align  2
 ENTRY(exception_error)
         !
 #ifdef CONFIG_TRACE_IRQFLAGS
-        mov.l   3f, r0
+        mov.l   2f, r0
         jsr     @r0
          nop
 #endif
         sti
-        mov.l   2f, r0
+        mov.l   1f, r0
         jmp     @r0
          nop
 
-!
         .align  2
-1:      .long   break_point_trap_software
-2:      .long   do_exception_error
+1:      .long   do_exception_error
 #ifdef CONFIG_TRACE_IRQFLAGS
-3:      .long   trace_hardirqs_on
+2:      .long   trace_hardirqs_on
 #endif
 
         .align  2
@@ -331,16 +276,31 @@ __restore_all:
 1:      .long   restore_all
 
         .align  2
-not_syscall_tra:        
-        bra     debug_trap
-         nop
-
-        .align  2
 syscall_badsys:                 ! Bad syscall number
         mov     #-ENOSYS, r0
         bra     resume_userspace
          mov.l  r0, @(OFF_R0,r15)       ! Return value
-        
+
+/*
+ * The main debug trap handler.
+ *
+ * r8=TRA (not the trap number!)
+ *
+ * Note: This assumes that the trapa value is left in its original
+ * form (without the shlr2 shift) so the calculation for the jump
+ * call table offset remains a simple in place mask.
+ */
+debug_trap:
+        mov     r8, r0
+        and     #(0xf << 2), r0
+        mov.l   1f, r8
+        add     r0, r8
+        mov.l   @r8, r8
+        jmp     @r8
+         nop
+
+        .align  2
+1:      .long   debug_trap_table
 
 /*
  * Syscall interface:
@@ -348,17 +308,19 @@ syscall_badsys:			! Bad syscall number
  *      Syscall #: R3
  *      Arguments #0 to #3: R4--R7
  *      Arguments #4 to #6: R0, R1, R2
- *      TRA: (number of arguments + 0x10) x 4
+ *      TRA: (number of arguments + ABI revision) x 4
  *
  * This code also handles delegating other traps to the BIOS/gdb stub
  * according to:
  *
  * Trap number
- * (TRA>>2)         Purpose
- * --------         -------
- * 0x0-0xf          old syscall ABI
- * 0x10-0x1f        new syscall ABI
- * 0x20-0xff        delegated through debug_trap to BIOS/gdb stub.
+ * (TRA>>2)     Purpose
+ * --------     -------
+ * 0x00-0x0f    original SH-3/4 syscall ABI (not in general use).
+ * 0x10-0x1f    general SH-3/4 syscall ABI.
+ * 0x20-0x2f    syscall ABI for SH-2 parts.
+ * 0x30-0x3f    debug traps used by the kernel.
+ * 0x40-0xff    Not supported by all parts, so left unhandled.
  *
  * Note: When we're first called, the TRA value must be shifted
  * right 2 bits in order to get the value that was used as the "trapa"
@@ -375,17 +337,22 @@ ret_from_fork:
          nop
         .align  2
 1:      .long   schedule_tail
-        !
+
+/*
+ * The poorly named main trapa decode and dispatch routine, for
+ * system calls and debug traps through their respective jump tables.
+ */
 ENTRY(system_call)
 #if !defined(CONFIG_CPU_SH2)
         mov.l   1f, r9
         mov.l   @r9, r8         ! Read from TRA (Trap Address) Register
 #endif
-        !
-        ! Is the trap argument >= 0x20? (TRA will be >= 0x80)
-        mov     #0x7f, r9
+        /*
+         * Check the trap type
+         */
+        mov     #((0x20 << 2) - 1), r9
         cmp/hi  r9, r8
-        bt/s    not_syscall_tra
+        bt/s    debug_trap              ! it's a debug trap..
          mov    #OFF_TRA, r9
         add     r15, r9
         mov.l   r8, @r9                 ! set TRA value to tra
diff --git a/arch/sh/kernel/kgdb_stub.c b/arch/sh/kernel/kgdb_stub.c
index 9c6315f0335d..d8927d85492e 100644
--- a/arch/sh/kernel/kgdb_stub.c
+++ b/arch/sh/kernel/kgdb_stub.c
@@ -1323,8 +1323,11 @@ static void kgdb_command_loop(const int excep_code, const int trapa_value)
 }
 
 /* There has been an exception, most likely a breakpoint. */
-void kgdb_handle_exception(struct pt_regs *regs)
+asmlinkage void kgdb_handle_exception(unsigned long r4, unsigned long r5,
+                                      unsigned long r6, unsigned long r7,
+                                      struct pt_regs __regs)
 {
+        struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
         int excep_code, vbr_val;
         int count;
         int trapa_value = ctrl_inl(TRA);
@@ -1368,8 +1371,6 @@ void kgdb_handle_exception(struct pt_regs *regs)
 
         vbr_val = trap_registers.vbr;
         asm("ldc %0, vbr": :"r"(vbr_val));
-
-        return;
 }
 
 /* Trigger a breakpoint by function */
diff --git a/arch/sh/kernel/process.c b/arch/sh/kernel/process.c
index 486c06e18033..cc8f306fd682 100644
--- a/arch/sh/kernel/process.c
+++ b/arch/sh/kernel/process.c
@@ -493,9 +493,27 @@ asmlinkage void break_point_trap(void)
         force_sig(SIGTRAP, current);
 }
 
-asmlinkage void break_point_trap_software(unsigned long r4, unsigned long r5,
-                                          unsigned long r6, unsigned long r7,
-                                          struct pt_regs __regs)
+/*
+ * Generic trap handler.
+ */
+asmlinkage void debug_trap_handler(unsigned long r4, unsigned long r5,
+                                   unsigned long r6, unsigned long r7,
+                                   struct pt_regs __regs)
+{
+        struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
+
+        /* Rewind */
+        regs->pc -= 2;
+
+        force_sig(SIGTRAP, current);
+}
+
+/*
+ * Special handler for BUG() traps.
+ */
+asmlinkage void bug_trap_handler(unsigned long r4, unsigned long r5,
+                                 unsigned long r6, unsigned long r7,
+                                 struct pt_regs __regs)
 {
         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);