diff --git a/arch/sh/kernel/Makefile b/arch/sh/kernel/Makefile
index 2f6d2bc..ff30d7f 100644
--- a/arch/sh/kernel/Makefile
+++ b/arch/sh/kernel/Makefile
@@ -6,7 +6,8 @@
 
 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 d51fa5e9..8de4810 100644
--- a/arch/sh/kernel/cpu/sh2/entry.S
+++ b/arch/sh/kernel/cpu/sh2/entry.S
@@ -206,7 +206,7 @@
 
 #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 8c0dc27..014ac37 100644
--- a/arch/sh/kernel/cpu/sh3/entry.S
+++ b/arch/sh/kernel/cpu/sh3/entry.S
@@ -173,7 +173,7 @@
 
 #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 0000000..13b6674
--- /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 fc279ae..ab4ebb8 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 @@
 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 #: 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 @@
 	 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 9c6315f..d8927d8 100644
--- a/arch/sh/kernel/kgdb_stub.c
+++ b/arch/sh/kernel/kgdb_stub.c
@@ -1323,8 +1323,11 @@
 }
 
 /* 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 @@
 
 	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 486c06e..cc8f306 100644
--- a/arch/sh/kernel/process.c
+++ b/arch/sh/kernel/process.c
@@ -493,9 +493,27 @@
 	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);
 
diff --git a/include/asm-sh/kgdb.h b/include/asm-sh/kgdb.h
index 7b26f53..0095c66 100644
--- a/include/asm-sh/kgdb.h
+++ b/include/asm-sh/kgdb.h
@@ -85,10 +85,10 @@
 #define KGDB_PRINTK(...) printk("KGDB: " __VA_ARGS__)
 
 /* Forced breakpoint */
-#define BREAKPOINT() do {                                     \
-  if (kgdb_enabled) {                                         \
-    asm volatile("trapa   #0xff");                            \
-  }                                                           \
+#define BREAKPOINT()					\
+do {							\
+	if (kgdb_enabled)				\
+		__asm__ __volatile__("trapa   #0x3c");	\
 } while (0)
 
 /* KGDB should be able to flush all kernel text space */