diff --git a/arch/microblaze/kernel/entry.S b/arch/microblaze/kernel/entry.S
index 397754c..5a5cb58 100644
--- a/arch/microblaze/kernel/entry.S
+++ b/arch/microblaze/kernel/entry.S
@@ -778,19 +778,22 @@
 
 	addik	r1, r1, -STATE_SAVE_SIZE; /* Make room on the stack.  */
 	SAVE_REGS;
+	swi	r17, r1, PTO+PT_R17;
+	swi	r16, r1, PTO+PT_R16;
+	swi	r16, r1, PTO+PT_PC;	/* Save LP */
 
 	swi	r0, r1, PTO + PT_MODE; /* Was in user-mode.  */
 	lwi	r11, r0, TOPHYS(PER_CPU(ENTRY_SP));
 	swi	r11, r1, PTO+PT_R1; /* Store user SP.  */
-2:
+2:	lwi	CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE));
 	tovirt(r1,r1)
 
 	set_vms;
-	addi	r5, r0, SIGTRAP		     /* send the trap signal */
-	add	r6, r0, CURRENT_TASK; /* Get current task ptr into r11 */
-	addk	r7, r0, r0		     /* 3rd param zero */
-dbtrap_call:	rtbd	r0, send_sig;
+	addik	r5, r1, PTO;
 	addik	r15, r0, dbtrap_call;
+dbtrap_call: /* return point for kernel/user entry */
+	rtbd	r0, sw_exception
+	nop
 
 	set_bip;			/*  Ints masked for state restore*/
 	lwi	r11, r1, PTO + PT_MODE;
@@ -838,6 +841,8 @@
 	tophys(r1,r1);
 
 	RESTORE_REGS
+	lwi	r17, r1, PTO+PT_R17;
+	lwi	r16, r1, PTO+PT_R16;
 	addik	r1, r1, STATE_SAVE_SIZE		/* Clean up stack space.  */
 
 
@@ -854,11 +859,10 @@
 	tovirt(r1,r1);
 6:
 DBTRAP_return:		/* Make global symbol for debugging */
-	rtbd	r14, 0;	/* Instructions to return from an IRQ */
+	rtbd	r16, 0;	/* Instructions to return from an IRQ */
 	nop;
 
 
-
 ENTRY(_switch_to)
 	/* prepare return value */
 	addk	r3, r0, CURRENT_TASK
@@ -946,13 +950,6 @@
 ENTRY(_reset)
 	brai	0x70; /* Jump back to FS-boot */
 
-ENTRY(_break)
-	mfs	r5, rmsr
-	swi	r5, r0, 0x250 + TOPHYS(r0_ram)
-	mfs	r5, resr
-	swi	r5, r0, 0x254 + TOPHYS(r0_ram)
-	bri	0
-
 	/* These are compiled and loaded into high memory, then
 	 * copied into place in mach_early_setup */
 	.section	.init.ivt, "ax"
@@ -964,12 +961,9 @@
 	nop
 	brai	TOPHYS(_user_exception); /* syscall handler */
 	brai	TOPHYS(_interrupt);	/* Interrupt handler */
-	brai	TOPHYS(_break);		/* nmi trap handler */
+	brai	TOPHYS(_debug_exception);	/* debug trap handler */
 	brai	TOPHYS(_hw_exception_handler);	/* HW exception handler */
 
-	.org	0x60
-	brai	TOPHYS(_debug_exception);	/* debug trap handler*/
-
 .section .rodata,"a"
 #include "syscall_table.S"
 
diff --git a/arch/microblaze/kernel/exceptions.c b/arch/microblaze/kernel/exceptions.c
index 02cbdfe..e0c6f8c 100644
--- a/arch/microblaze/kernel/exceptions.c
+++ b/arch/microblaze/kernel/exceptions.c
@@ -48,6 +48,12 @@
 	do_exit(err);
 }
 
+/* for user application debugging */
+void sw_exception(struct pt_regs *regs)
+{
+	_exception(SIGTRAP, regs, TRAP_BRKPT, regs->r16);
+}
+
 void _exception(int signr, struct pt_regs *regs, int code, unsigned long addr)
 {
 	siginfo_t info;
@@ -143,7 +149,7 @@
 #ifdef CONFIG_MMU
 	case MICROBLAZE_PRIVILEGED_EXCEPTION:
 		pr_debug(KERN_WARNING "Privileged exception\n");
-		/* "brk r0,r0" - used as debug breakpoint */
+		/* "brk r0,r0" - used as debug breakpoint - old toolchain */
 		if (get_user(code, (unsigned long *)regs->pc) == 0
 			&& code == 0x980c0000) {
 			_exception(SIGTRAP, regs, TRAP_BRKPT, addr);