diff --git a/include/rtdef.h b/include/rtdef.h
index d125c970c9..e1a64a433a 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
  *
@@ -659,6 +663,10 @@ struct rt_thread
     rt_ubase_t  init_tick;                              /**< thread's initialized tick */
     rt_ubase_t  remaining_tick;                         /**< remaining tick */
 
+#ifdef RT_USING_CPU_USAGE
+    rt_uint64_t  duration_tick;                          /**< cpu usage tick */
+#endif
+
     struct rt_timer thread_timer;                       /**< built-in thread timer */
 
     void (*cleanup)(struct rt_thread *tid);             /**< cleanup function when thread exit */
diff --git a/include/rtthread.h b/include/rtthread.h
index f2f4091b8a..cf47a48efe 100644
--- a/include/rtthread.h
+++ b/include/rtthread.h
@@ -186,6 +186,7 @@ rt_uint16_t rt_critical_level(void);
 
 #ifdef RT_USING_HOOK
 void rt_scheduler_sethook(void (*hook)(rt_thread_t from, rt_thread_t to));
+void rt_scheduler_switch_sethook(void (*hook)(struct rt_thread *tid));
 #endif
 
 #ifdef RT_USING_SMP
diff --git a/libcpu/arm/cortex-a/interrupt.c b/libcpu/arm/cortex-a/interrupt.c
index 474a23fee1..f90f1c0094 100644
--- a/libcpu/arm/cortex-a/interrupt.c
+++ b/libcpu/arm/cortex-a/interrupt.c
@@ -27,6 +27,20 @@ struct rt_irq_desc isr_table[MAX_HANDLERS];
 rt_uint32_t rt_interrupt_from_thread        = 0;
 rt_uint32_t rt_interrupt_to_thread          = 0;
 rt_uint32_t rt_thread_switch_interrupt_flag = 0;
+
+#ifdef RT_USING_HOOK
+static void (*rt_interrupt_switch_hook)(void);
+
+void rt_interrupt_switch_sethook(void (*hook)(void))
+{
+    rt_interrupt_switch_hook = hook;
+}
+#endif
+
+void rt_interrupt_hook(void)
+{
+    RT_OBJECT_HOOK_CALL(rt_interrupt_switch_hook, ());
+}
 #endif
 
 const unsigned int VECTOR_BASE = 0x00;
diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S
index 8fcf6abd2a..908468476e 100644
--- a/libcpu/arm/cortex-a/start_gcc.S
+++ b/libcpu/arm/cortex-a/start_gcc.S
@@ -341,6 +341,8 @@ rt_hw_context_switch_interrupt_do:
     ldr     r6,  [r6]
     ldr     sp,  [r6]       @ get new task's stack pointer
 
+    bl rt_interrupt_hook
+
 #ifdef RT_USING_FPU
 /* fpu context */
     ldmfd sp!, {r6}
@@ -373,9 +375,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 +493,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 3faa136677..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,23 +185,32 @@ 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)
 {
     void *param;
+    int int_ack;
     int ir;
     rt_isr_handler_t isr_func;
     extern struct rt_irq_desc isr_table[];
 
-    ir = rt_hw_interrupt_get_irq();
+    int_ack = rt_hw_interrupt_get_irq();
 
+    ir = int_ack & GIC_ACK_INTID_MASK;
     if (ir == 1023)
     {
         /* Spurious interrupt */
@@ -181,7 +231,7 @@ void rt_hw_trap_irq(void)
     }
 
     /* end of interrupt */
-    rt_hw_interrupt_ack(ir);
+    rt_hw_interrupt_ack(int_ack);
 }
 
 void rt_hw_trap_fiq(void)
diff --git a/src/irq.c b/src/irq.c
index e89b8504dc..6ddcb3dd4a 100644
--- a/src/irq.c
+++ b/src/irq.c
@@ -89,8 +89,8 @@ void rt_interrupt_leave(void)
                                 rt_interrupt_nest));
 
     level = rt_hw_interrupt_disable();
-    rt_interrupt_nest --;
     RT_OBJECT_HOOK_CALL(rt_interrupt_leave_hook,());
+    rt_interrupt_nest --;
     rt_hw_interrupt_enable(level);
 }
 RTM_EXPORT(rt_interrupt_leave);
diff --git a/src/scheduler.c b/src/scheduler.c
index 956d95d7cc..353fdad5cb 100644
--- a/src/scheduler.c
+++ b/src/scheduler.c
@@ -49,6 +49,7 @@ rt_uint8_t rt_current_priority;
 
 #ifdef RT_USING_HOOK
 static void (*rt_scheduler_hook)(struct rt_thread *from, struct rt_thread *to);
+static void (*rt_scheduler_switch_hook)(struct rt_thread *tid);
 
 /**
  * @addtogroup Hook
@@ -68,6 +69,12 @@ rt_scheduler_sethook(void (*hook)(struct rt_thread *from, struct rt_thread *to))
     rt_scheduler_hook = hook;
 }
 
+void
+rt_scheduler_switch_sethook(void (*hook)(struct rt_thread *tid))
+{
+    rt_scheduler_switch_hook = hook;
+}
+
 /**@}*/
 #endif /* RT_USING_HOOK */
 
@@ -364,6 +371,8 @@ void rt_schedule(void)
                 _rt_scheduler_stack_check(to_thread);
 #endif /* RT_USING_OVERFLOW_CHECK */
 
+                RT_OBJECT_HOOK_CALL(rt_scheduler_switch_hook, (current_thread));
+
                 rt_hw_context_switch((rt_ubase_t)&current_thread->sp,
                         (rt_ubase_t)&to_thread->sp, to_thread);
             }
@@ -473,6 +482,8 @@ void rt_schedule(void)
                 {
                     extern void rt_thread_handle_sig(rt_bool_t clean_state);
 
+                    RT_OBJECT_HOOK_CALL(rt_scheduler_switch_hook, (from_thread));
+
                     rt_hw_context_switch((rt_ubase_t)&from_thread->sp,
                             (rt_ubase_t)&to_thread->sp);
 
@@ -609,6 +620,8 @@ void rt_scheduler_do_irq_switch(void *context)
                 current_thread->cpus_lock_nest--;
                 current_thread->scheduler_lock_nest--;
 
+                RT_OBJECT_HOOK_CALL(rt_scheduler_switch_hook, (current_thread));
+
                 rt_hw_context_switch_interrupt(context, (rt_ubase_t)&current_thread->sp,
                         (rt_ubase_t)&to_thread->sp, to_thread);
             }
diff --git a/src/thread.c b/src/thread.c
index 88cc7b61ba..60e6ab91d1 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -228,6 +228,10 @@ static rt_err_t _rt_thread_init(struct rt_thread *thread,
     thread->lwp = RT_NULL;
 #endif /* RT_USING_LWP */
 
+#ifdef RT_USING_CPU_USAGE
+    thread->duration_tick = 0;
+#endif
+
     RT_OBJECT_HOOK_CALL(rt_thread_inited_hook, (thread));
 
     return RT_EOK;