diff --git a/include/rtdef.h b/include/rtdef.h
index d125c970c9..2eb2cc1963 100644
--- a/include/rtdef.h
+++ b/include/rtdef.h
@@ -567,6 +567,10 @@ typedef siginfo_t rt_siginfo_t;
 #define RT_SCHEDULE_IPI                 0
 #endif
 
+#ifndef RT_STOP_IPI
+#define RT_STOP_IPI                     1
+#endif
+
 /**
  * CPUs definitions
  *
diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S
index 8fcf6abd2a..6c5dd81785 100644
--- a/libcpu/arm/cortex-a/start_gcc.S
+++ b/libcpu/arm/cortex-a/start_gcc.S
@@ -373,9 +373,18 @@ rt_hw_context_switch_interrupt_do:
     mrs     r6, spsr                @/* Save CPSR                       */
     str     lr, [r0, #15*4]         @/* Push PC                         */
     str     r6, [r0, #16*4]         @/* Push CPSR                       */
-    cps     #Mode_SVC
+    mrs     r5, cpsr                @/* Save CPSR                       */
+
+    and     r4, r6, #0x1F
+    cmp     r4, #Mode_USR
+    moveq   r6, #Mode_SYS
+
+    orr     r6, r6, #0x80           @/* Switch to previous mode, then save SP & PC */
+    msr     cpsr_c, r6
     str     sp, [r0, #13*4]         @/* Save calling SP                 */
     str     lr, [r0, #14*4]         @/* Save calling PC                 */
+
+    msr     cpsr_c, r5              @/* Switch back to current mode */
 .endm
 
     .align  5
@@ -482,6 +491,8 @@ secondary_cpu_start:
 .bss
 .align 2   //align to  2~2=4
 
+.global sub_stack_top /* used for backtrace to calculate stack top of irq mode */
+
 sub_stack_start:
     .space (SUB_ISR_Stack_Size * (RT_CPUS_NR-1))
 sub_stack_top:
diff --git a/libcpu/arm/cortex-a/trap.c b/libcpu/arm/cortex-a/trap.c
index f61b89871d..b3efe31a64 100644
--- a/libcpu/arm/cortex-a/trap.c
+++ b/libcpu/arm/cortex-a/trap.c
@@ -35,6 +35,18 @@ void rt_hw_show_register(struct rt_hw_exp_stack *regs)
     rt_kprintf("cpsr:0x%08x\n", regs->cpsr);
 }
 
+void (*rt_trap_hook)(struct rt_hw_exp_stack *regs, const char *ex, unsigned int exception_type);
+
+/**
+ * This function will set a hook function to trap handler.
+ *
+ * @param hook the hook function
+ */
+void rt_hw_trap_set_hook(void (*hook)(struct rt_hw_exp_stack *regs, const char *ex, unsigned int exception_type))
+{
+    rt_trap_hook = hook;
+}
+
 /**
  * When comes across an instruction which it cannot handle,
  * it takes the undefined instruction trap.
@@ -72,12 +84,20 @@ void rt_hw_trap_undef(struct rt_hw_exp_stack *regs)
         }
     }
 #endif
-    rt_kprintf("undefined instruction:\n");
-    rt_hw_show_register(regs);
+
+    if (rt_trap_hook == RT_NULL)
+    {
+        rt_kprintf("undefined instruction:\n");
+        rt_hw_show_register(regs);
 #ifdef RT_USING_FINSH
-    list_thread();
+        list_thread();
 #endif
-    rt_hw_cpu_shutdown();
+        rt_hw_cpu_shutdown();
+    }
+    else
+    {
+        rt_trap_hook(regs, "undefined instruction", UND_EXCEPTION);
+    }
 }
 
 /**
@@ -91,12 +111,19 @@ void rt_hw_trap_undef(struct rt_hw_exp_stack *regs)
  */
 void rt_hw_trap_swi(struct rt_hw_exp_stack *regs)
 {
-    rt_kprintf("software interrupt:\n");
-    rt_hw_show_register(regs);
+    if (rt_trap_hook == RT_NULL)
+    {
+        rt_kprintf("software interrupt:\n");
+        rt_hw_show_register(regs);
 #ifdef RT_USING_FINSH
-    list_thread();
+        list_thread();
 #endif
-    rt_hw_cpu_shutdown();
+        rt_hw_cpu_shutdown();
+    }
+    else
+    {
+        rt_trap_hook(regs, "software instruction", SWI_EXCEPTION);
+    }
 }
 
 /**
@@ -109,12 +136,19 @@ void rt_hw_trap_swi(struct rt_hw_exp_stack *regs)
  */
 void rt_hw_trap_pabt(struct rt_hw_exp_stack *regs)
 {
-    rt_kprintf("prefetch abort:\n");
-    rt_hw_show_register(regs);
+    if (rt_trap_hook == RT_NULL)
+    {
+        rt_kprintf("prefetch abort:\n");
+        rt_hw_show_register(regs);
 #ifdef RT_USING_FINSH
-    list_thread();
+        list_thread();
 #endif
-    rt_hw_cpu_shutdown();
+        rt_hw_cpu_shutdown();
+    }
+    else
+    {
+        rt_trap_hook(regs, "prefetch abort", PABT_EXCEPTION);
+    }
 }
 
 /**
@@ -127,12 +161,19 @@ void rt_hw_trap_pabt(struct rt_hw_exp_stack *regs)
  */
 void rt_hw_trap_dabt(struct rt_hw_exp_stack *regs)
 {
-    rt_kprintf("data abort:");
-    rt_hw_show_register(regs);
+    if (rt_trap_hook == RT_NULL)
+    {
+        rt_kprintf("data abort:");
+        rt_hw_show_register(regs);
 #ifdef RT_USING_FINSH
-    list_thread();
+        list_thread();
 #endif
-    rt_hw_cpu_shutdown();
+        rt_hw_cpu_shutdown();
+    }
+    else
+    {
+        rt_trap_hook(regs, "data abort", DABT_EXCEPTION);
+    }
 }
 
 /**
@@ -144,12 +185,19 @@ void rt_hw_trap_dabt(struct rt_hw_exp_stack *regs)
  */
 void rt_hw_trap_resv(struct rt_hw_exp_stack *regs)
 {
-    rt_kprintf("reserved trap:\n");
-    rt_hw_show_register(regs);
+    if (rt_trap_hook == RT_NULL)
+    {
+        rt_kprintf("reserved trap:\n");
+        rt_hw_show_register(regs);
 #ifdef RT_USING_FINSH
-    list_thread();
+        list_thread();
 #endif
-    rt_hw_cpu_shutdown();
+        rt_hw_cpu_shutdown();
+    }
+    else
+    {
+        rt_trap_hook(regs, "reserved trap", RESV_EXCEPTION);
+    }
 }
 
 void rt_hw_trap_irq(void)