diff --git a/libcpu/arm/am335x/start_gcc.S b/libcpu/arm/am335x/start_gcc.S
index 6d9ec8be2561be18955c95b77e306d1ef0049c40..f80aca622364337fab263c429edce19e4d12c3ec 100644
--- a/libcpu/arm/am335x/start_gcc.S
+++ b/libcpu/arm/am335x/start_gcc.S
@@ -33,7 +33,7 @@
 .equ I_Bit,           0x80            @ when I bit is set, IRQ is disabled
 .equ F_Bit,           0x40            @ when F bit is set, FIQ is disabled
 
-.equ UND_Stack_Size,  0x00000000
+.equ UND_Stack_Size,  0x00000200
 .equ SVC_Stack_Size,  0x00000100
 .equ ABT_Stack_Size,  0x00000000
 .equ FIQ_Stack_Size,  0x00000000
@@ -135,7 +135,15 @@ vector_undef:
     sub     sp, sp, #72
     stmia   sp, {r0 - r12}          @/* Calling r0-r12                  */
     add     r8, sp, #60
-    stmdb   r8, {sp, lr}            @/* Calling SP, LR                  */
+
+    mrs     r1, cpsr
+    mrs     r2, spsr
+    orr     r2,r2, #I_Bit|F_Bit
+    msr     cpsr_c, r2
+    mov     r0, r0
+    stmdb   r8, {sp, lr}           @/* Calling SP, LR                  */
+    msr     cpsr_c, r1             @/* return to Undefined Instruction mode  */
+
     str     lr, [r8, #0]            @/* Save calling PC                 */
     mrs     r6, spsr
     str     r6, [r8, #4]            @/* Save CPSR                       */
@@ -144,6 +152,12 @@ vector_undef:
 
     bl      rt_hw_trap_udef
 
+    ldmia    sp, {r0 - r12}         @/* Calling r0 - r2  */
+    mov      r0, r0
+    ldr      lr, [sp, #60]          @/* Get PC   */
+    add      sp, sp, #72
+    movs     pc, lr                 @/* return & move spsr_svc into cpsr */
+
     .align  5
 .globl vector_swi
 vector_swi:
@@ -169,6 +183,12 @@ vector_dabt:
 
     bl      rt_hw_trap_dabt
 
+    ldmia    sp, {r0 - r12}         @/* Calling r0 - r2  */
+    mov      r0, r0
+    ldr      lr, [sp, #60]          @/* Get PC   */
+    add      sp, sp, #72
+    movs     pc, lr                 @/* return & move spsr_svc into cpsr */
+
     .align 5
 .globl vector_resv
 vector_resv:
diff --git a/libcpu/arm/am335x/trap.c b/libcpu/arm/am335x/trap.c
index 925ec564341d48335ec13a2a31dc1dd200b567a0..89dc14255ae15128b7b3972794510bbcfdbd2092 100644
--- a/libcpu/arm/am335x/trap.c
+++ b/libcpu/arm/am335x/trap.c
@@ -18,6 +18,10 @@
 #include "am33xx.h"
 #include "interrupt.h"
 
+#ifdef RT_USING_GDB
+#include "gdb_stub.h"
+#endif
+
 /**
  * @addtogroup AM33XX
  */
@@ -55,6 +59,13 @@ void rt_hw_show_register (struct rt_hw_register *regs)
  */
 void rt_hw_trap_udef(struct rt_hw_register *regs)
 {
+
+#ifdef RT_USING_GDB
+    regs->pc -= 4; //lr in undef is pc + 4
+    if (gdb_undef_hook(regs))
+        return;
+#endif
+
 	rt_hw_show_register(regs);
 
 	rt_kprintf("undefined instruction\n");
@@ -114,6 +125,13 @@ void rt_hw_trap_pabt(struct rt_hw_register *regs)
  */
 void rt_hw_trap_dabt(struct rt_hw_register *regs)
 {
+
+#ifdef RT_USING_GDB
+    if (gdb_mem_fault_handler) {
+        regs->pc = (unsigned long)gdb_mem_fault_handler; 
+        return;
+    }
+#endif
 	rt_hw_show_register(regs);
 
 	rt_kprintf("data abort\n");