diff --git a/arch/arm64/src/common/arm64_arch.h b/arch/arm64/src/common/arm64_arch.h
index 35c4cdc105c..06935472fb5 100644
--- a/arch/arm64/src/common/arm64_arch.h
+++ b/arch/arm64/src/common/arm64_arch.h
@@ -299,9 +299,6 @@ struct regs_context
   uint64_t  spsr;
   uint64_t  sp_el0;
   uint64_t  exe_depth;
-#ifdef CONFIG_ARCH_FPU
-  struct fpu_reg fpu_regs;
-#endif
 };
 
 /****************************************************************************
diff --git a/arch/arm64/src/common/arm64_exit.c b/arch/arm64/src/common/arm64_exit.c
index cb0d4bd24a2..0052adac94a 100644
--- a/arch/arm64/src/common/arm64_exit.c
+++ b/arch/arm64/src/common/arm64_exit.c
@@ -65,6 +65,12 @@ void up_exit(int status)
    */
 
   enter_critical_section();
+
+  /* Destroy the task at the head of the ready to run list. */
+#ifdef CONFIG_ARCH_FPU
+  arm64_destory_fpu(tcb);
+#endif
+
   nxtask_exit();
 
   /* Now, perform the context switch to the new ready-to-run task at the
diff --git a/arch/arm64/src/common/arm64_fpu.c b/arch/arm64/src/common/arm64_fpu.c
index e80e7240aee..c1accb48ebf 100644
--- a/arch/arm64/src/common/arm64_fpu.c
+++ b/arch/arm64/src/common/arm64_fpu.c
@@ -77,10 +77,11 @@ struct arm64_fpu_procfs_file_s
  * Private Data
  ***************************************************************************/
 
-#ifdef CONFIG_FS_PROCFS_REGISTER
-
+static struct fpu_reg g_idle_thread_fpu[CONFIG_SMP_NCPUS];
 static struct arm64_cpu_fpu_context g_cpu_fpu_ctx[CONFIG_SMP_NCPUS];
 
+#ifdef CONFIG_FS_PROCFS_REGISTER
+
 /* procfs methods */
 
 static int arm64_fpu_procfs_open(struct file *filep, const char *relpath,
@@ -261,6 +262,141 @@ static int arm64_fpu_procfs_stat(const char *relpath, struct stat *buf)
 }
 #endif
 
+/***************************************************************************
+ * Public Functions
+ ***************************************************************************/
+
+void arm64_init_fpu(struct tcb_s *tcb)
+{
+  if (tcb->pid < CONFIG_SMP_NCPUS)
+    {
+#ifdef CONFIG_SMP
+      int cpu = tcb->cpu;
+#else
+      int cpu = 0;
+#endif
+      memset(&g_cpu_fpu_ctx[cpu], 0,
+             sizeof(struct arm64_cpu_fpu_context));
+      g_cpu_fpu_ctx[cpu].idle_thread = tcb;
+
+      tcb->xcp.fpu_regs = (uint64_t *)&g_idle_thread_fpu[cpu];
+    }
+
+  memset(tcb->xcp.fpu_regs, 0, sizeof(struct fpu_reg));
+}
+
+void arm64_destory_fpu(struct tcb_s *tcb)
+{
+  struct tcb_s *owner;
+
+  /* save current fpu owner's context */
+
+  owner = g_cpu_fpu_ctx[this_cpu()].fpu_owner;
+
+  if (owner == tcb)
+    {
+      g_cpu_fpu_ctx[this_cpu()].fpu_owner = NULL;
+    }
+}
+
+/***************************************************************************
+ * Name: arm64_fpu_enter_exception
+ *
+ * Description:
+ *   called at every time get into a exception
+ *
+ ***************************************************************************/
+
+void arm64_fpu_enter_exception(void)
+{
+}
+
+void arm64_fpu_exit_exception(void)
+{
+}
+
+void arm64_fpu_trap(struct regs_context *regs)
+{
+  struct tcb_s *owner;
+
+  UNUSED(regs);
+
+  /* disable fpu trap access */
+
+  arm64_fpu_access_trap_disable();
+
+  /* save current fpu owner's context */
+
+  owner = g_cpu_fpu_ctx[this_cpu()].fpu_owner;
+
+  if (owner != NULL)
+    {
+      arm64_fpu_save((struct fpu_reg *)owner->xcp.fpu_regs);
+      ARM64_DSB();
+      g_cpu_fpu_ctx[this_cpu()].save_count++;
+      g_cpu_fpu_ctx[this_cpu()].fpu_owner = NULL;
+    }
+
+  if (arch_get_exception_depth() > 1)
+    {
+      /* if get_exception_depth > 1
+       * it means FPU access exception occurred in exception context
+       * switch FPU owner to idle thread
+       */
+
+      owner = g_cpu_fpu_ctx[this_cpu()].idle_thread;
+    }
+  else
+    {
+      owner = running_task();
+    }
+
+  /* restore our context */
+
+  arm64_fpu_restore((struct fpu_reg *)owner->xcp.fpu_regs);
+  g_cpu_fpu_ctx[this_cpu()].restore_count++;
+
+  /* become new owner */
+
+  g_cpu_fpu_ctx[this_cpu()].fpu_owner = owner;
+}
+
+void arm64_fpu_context_restore(void)
+{
+  struct tcb_s *new_tcb = running_task();
+
+  arm64_fpu_access_trap_disable();
+
+  /* FPU trap has happened at this task */
+
+  if (new_tcb == g_cpu_fpu_ctx[this_cpu()].fpu_owner)
+    {
+      arm64_fpu_access_trap_disable();
+    }
+  else
+    {
+      arm64_fpu_access_trap_enable();
+    }
+
+  g_cpu_fpu_ctx[this_cpu()].switch_count++;
+}
+
+#ifdef CONFIG_SMP
+void arm64_fpu_context_save(void)
+{
+  struct tcb_s *tcb = running_task();
+
+  if (tcb == g_cpu_fpu_ctx[this_cpu()].fpu_owner)
+    {
+      arm64_fpu_access_trap_disable();
+      arm64_fpu_save((struct fpu_reg *)tcb->xcp.fpu_regs);
+      ARM64_DSB();
+      g_cpu_fpu_ctx[this_cpu()].save_count++;
+      g_cpu_fpu_ctx[this_cpu()].fpu_owner = NULL;
+    }
+}
+#endif
+
 void arm64_fpu_enable(void)
 {
   irqstate_t flags = up_irq_save();
diff --git a/arch/arm64/src/common/arm64_initialstate.c b/arch/arm64/src/common/arm64_initialstate.c
index 32058db4855..93aa822fff6 100644
--- a/arch/arm64/src/common/arm64_initialstate.c
+++ b/arch/arm64/src/common/arm64_initialstate.c
@@ -47,6 +47,10 @@
 #include "chip.h"
 #include "arm64_fatal.h"
 
+#ifdef CONFIG_ARCH_FPU
+#include "arm64_fpu.h"
+#endif
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -66,6 +70,17 @@ void arm64_new_task(struct tcb_s * tcb)
     }
 #endif
 
+#ifdef CONFIG_ARCH_FPU
+  struct fpu_reg *pfpuctx;
+  pfpuctx = STACK_PTR_TO_FRAME(struct fpu_reg, stack_ptr);
+  tcb->xcp.fpu_regs   = (uint64_t *)pfpuctx;
+
+  /* set fpu context */
+
+  arm64_init_fpu(tcb);
+  stack_ptr  = (uintptr_t)pfpuctx;
+#endif
+
   pinitctx = STACK_PTR_TO_FRAME(struct regs_context, stack_ptr);
   memset(pinitctx, 0, sizeof(struct regs_context));
   pinitctx->elr       = (uint64_t)tcb->start;
@@ -135,6 +150,11 @@ void up_initial_state(struct tcb_s *tcb)
       tcb->stack_base_ptr  = tcb->stack_alloc_ptr;
       tcb->adj_stack_size  = CONFIG_IDLETHREAD_STACKSIZE;
 
+#ifdef CONFIG_ARCH_FPU
+      /* set fpu context */
+
+      arm64_init_fpu(tcb);
+#endif
       /* set initialize idle thread tcb and exception depth
        * core 0, idle0
        */
diff --git a/arch/arm64/src/common/arm64_schedulesigaction.c b/arch/arm64/src/common/arm64_schedulesigaction.c
index 4f0534345a2..888c8106268 100644
--- a/arch/arm64/src/common/arm64_schedulesigaction.c
+++ b/arch/arm64/src/common/arm64_schedulesigaction.c
@@ -38,6 +38,10 @@
 #include "irq/irq.h"
 #include "arm64_fatal.h"
 
+#ifdef CONFIG_ARCH_FPU
+#include "arm64_fpu.h"
+#endif
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -54,6 +58,16 @@ void arm64_init_signal_process(struct tcb_s *tcb, struct regs_context *regs)
   struct regs_context  *psigctx;
   char *stack_ptr    = (char *)pctx->sp_elx - sizeof(struct regs_context);
 
+#ifdef CONFIG_ARCH_FPU
+  struct fpu_reg      *pfpuctx;
+  pfpuctx            = STACK_PTR_TO_FRAME(struct fpu_reg, stack_ptr);
+  tcb->xcp.fpu_regs  = (uint64_t *)pfpuctx;
+
+  /* set fpu context */
+
+  arm64_init_fpu(tcb);
+  stack_ptr          = (char *)pfpuctx;
+#endif
   psigctx            = STACK_PTR_TO_FRAME(struct regs_context, stack_ptr);
   memset(psigctx, 0, sizeof(struct regs_context));
   psigctx->elr       = (uint64_t)arm64_sigdeliver;
@@ -163,6 +177,9 @@ void up_schedule_sigaction(struct tcb_s *tcb, sig_deliver_t sigdeliver)
               /* create signal process context */
 
               tcb->xcp.saved_reg = up_current_regs();
+#ifdef CONFIG_ARCH_FPU
+              tcb->xcp.saved_fpu_regs = tcb->xcp.fpu_regs;
+#endif
               arm64_init_signal_process(tcb,
               (struct regs_context *)up_current_regs());
 
@@ -184,6 +201,9 @@ void up_schedule_sigaction(struct tcb_s *tcb, sig_deliver_t sigdeliver)
            * have been delivered.
            */
 
+#ifdef CONFIG_ARCH_FPU
+          tcb->xcp.saved_fpu_regs = tcb->xcp.fpu_regs;
+#endif
           /* create signal process context */
 
           tcb->xcp.saved_reg = tcb->xcp.regs;
@@ -259,6 +279,9 @@ void up_schedule_sigaction(struct tcb_s *tcb, sig_deliver_t sigdeliver)
                    * been delivered.
                    */
 
+#ifdef CONFIG_ARCH_FPU
+                  tcb->xcp.saved_fpu_regs = tcb->xcp.fpu_regs;
+#endif
                   /* create signal process context */
 
                   tcb->xcp.saved_reg = tcb->xcp.regs;
@@ -277,6 +300,9 @@ void up_schedule_sigaction(struct tcb_s *tcb, sig_deliver_t sigdeliver)
                   /* create signal process context */
 
                   tcb->xcp.saved_reg = up_current_regs();
+#ifdef CONFIG_ARCH_FPU
+                  tcb->xcp.saved_fpu_regs = tcb->xcp.fpu_regs;
+#endif
                   arm64_init_signal_process(tcb,
                   (struct regs_context *)up_current_regs());
 
@@ -313,6 +339,9 @@ void up_schedule_sigaction(struct tcb_s *tcb, sig_deliver_t sigdeliver)
            * have been delivered.
            */
 
+#ifdef CONFIG_ARCH_FPU
+          tcb->xcp.saved_fpu_regs = tcb->xcp.fpu_regs;
+#endif
           tcb->xcp.saved_reg = tcb->xcp.regs;
 
           /* create signal process context */
diff --git a/arch/arm64/src/common/arm64_sigdeliver.c b/arch/arm64/src/common/arm64_sigdeliver.c
index 0d46a98410d..15e7946c7c0 100644
--- a/arch/arm64/src/common/arm64_sigdeliver.c
+++ b/arch/arm64/src/common/arm64_sigdeliver.c
@@ -38,6 +38,10 @@
 #include "irq/irq.h"
 #include "arm64_fatal.h"
 
+#ifdef CONFIG_ARCH_FPU
+#include "arm64_fpu.h"
+#endif
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -153,6 +157,11 @@ retry:
   rtcb->xcp.sigdeliver = NULL;  /* Allows next handler to be scheduled */
   rtcb->xcp.regs = rtcb->xcp.saved_reg;
 
+#ifdef CONFIG_ARCH_FPU
+  arm64_destory_fpu(rtcb);
+  rtcb->xcp.fpu_regs = rtcb->xcp.saved_fpu_regs;
+#endif
+
   /* Then restore the correct state for this thread of execution. */
 
 #ifdef CONFIG_SMP
diff --git a/arch/arm64/src/common/arm64_syscall.c b/arch/arm64/src/common/arm64_syscall.c
index c59aef35cbd..318e7e0cf60 100644
--- a/arch/arm64/src/common/arm64_syscall.c
+++ b/arch/arm64/src/common/arm64_syscall.c
@@ -193,6 +193,7 @@ uint64_t *arm64_syscall_switch(uint64_t * regs)
            */
 
           ret_regs = (uint64_t *)f_regs->regs[REG_X1];
+          f_regs->regs[REG_X1] = 0; /* set the saveregs = 0 */
 
           DEBUGASSERT(ret_regs);
         }
diff --git a/arch/arm64/src/common/arm64_vector_table.S b/arch/arm64/src/common/arm64_vector_table.S
index 21b70d25752..1c05814c876 100644
--- a/arch/arm64/src/common/arm64_vector_table.S
+++ b/arch/arm64/src/common/arm64_vector_table.S
@@ -45,8 +45,8 @@
  *     but only save ARM64_ESF_REGS
  */
 
-.macro arm64_enter_exception
-    sub    sp, sp, #8 * XCPTCONTEXT_REGS
+.macro arm64_enter_exception xreg0, xreg1
+    sub    sp, sp, #8 * XCPTCONTEXT_GP_REGS
 
     stp    x0,  x1,  [sp, #8 * REG_X0]
     stp    x2,  x3,  [sp, #8 * REG_X2]
@@ -65,32 +65,30 @@
     stp    x28, x29, [sp, #8 * REG_X28]
 
     /* Save the current task's SP_ELx and x30 */
-    add    x0, sp, #8 * XCPTCONTEXT_REGS
-    stp    x30, x0,  [sp, #8 * REG_X30]
+    add    \xreg0, sp, #8 * XCPTCONTEXT_GP_REGS
+    stp    x30, \xreg0,  [sp, #8 * REG_X30]
 
     /* ELR and SPSR */
 #if CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3
-    mrs    x0, elr_el3
-    mrs    x1, spsr_el3
+    mrs    \xreg0, elr_el3
+    mrs    \xreg1, spsr_el3
 #else
-    mrs    x0, elr_el1
-    mrs    x1, spsr_el1
+    mrs    \xreg0, elr_el1
+    mrs    \xreg1, spsr_el1
 #endif
-    stp    x0, x1,  [sp, #8 * REG_ELR]
+    stp    \xreg0, \xreg1,  [sp, #8 * REG_ELR]
 
-    /* Increment exception depth */
+    /* increment exception depth */
 
-    mrs    x0, tpidrro_el0
-    mov    x1, #1
-    add    x0, x0, x1
-    msr    tpidrro_el0, x0
+    mrs    \xreg0, tpidrro_el0
+    mov    \xreg1, #1
+    add    \xreg0, \xreg0, \xreg1
+    msr    tpidrro_el0, \xreg0
 
-    /* Save the FPU registers */
-
-    add    x0, sp, #8 * XCPTCONTEXT_GP_REGS
 #ifdef CONFIG_ARCH_FPU
-    bl     arm64_fpu_save
+    bl    arm64_fpu_enter_exception
 #endif
+
 .endm
 
 /****************************************************************************
@@ -145,97 +143,97 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table)
     /* Current EL with SP0 / Synchronous */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x9, x10
     b    arm64_sync_exc
 
     /* Current EL with SP0 / IRQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_irq_handler
 
     /* Current EL with SP0 / FIQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b     arm64_fiq_handler
 
     /* Current EL with SP0 / SError */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_serror_handler
 
     /* Current EL with SPx / Synchronous */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x9, x10
     b    arm64_sync_exc
 
     /* Current EL with SPx / IRQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_irq_handler
 
     /* Current EL with SPx / FIQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b     arm64_fiq_handler
 
     /* Current EL with SPx / SError */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_serror_handler
 
     /* Lower EL using AArch64 / Synchronous */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x9, x10
     b    arm64_sync_exc
 
     /* Lower EL using AArch64 / IRQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_irq_handler
 
     /* Lower EL using AArch64 / FIQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b     arm64_fiq_handler
 
     /* Lower EL using AArch64 / SError */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_serror_handler
 
     /* Lower EL using AArch32 / Synchronous */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_mode32_handler
 
     /* Lower EL using AArch32 / IRQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_mode32_handler
 
     /* Lower EL using AArch32 / FIQ */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_mode32_handler
 
     /* Lower EL using AArch32 / SError */
 
     .align 7
-    arm64_enter_exception
+    arm64_enter_exception x0, x1
     b    arm64_mode32_handler
 
 /* Restore Corruptible Registers and exception context
@@ -244,11 +242,10 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table)
 
 GTEXT(arm64_exit_exception)
 SECTION_FUNC(text, arm64_exit_exception)
-
-    mov    sp, x0
-    add    x0, x0, #8 * XCPTCONTEXT_GP_REGS
 #ifdef CONFIG_ARCH_FPU
-    bl     arm64_fpu_restore
+    bl    arm64_fpu_exit_exception
+GTEXT(arm64_exit_exc_fpu_done)
+arm64_exit_exc_fpu_done:
 #endif
 
     /* restore spsr and elr at el1*/
@@ -286,6 +283,6 @@ SECTION_FUNC(text, arm64_exit_exception)
     ldp    x28, x29, [sp, #8 * REG_X28]
     ldp    x30, xzr, [sp, #8 * REG_X30]
 
-    add    sp, sp, #8 * XCPTCONTEXT_REGS
+    add    sp, sp, #8 * XCPTCONTEXT_GP_REGS
 
     eret
diff --git a/arch/arm64/src/common/arm64_vectors.S b/arch/arm64/src/common/arm64_vectors.S
index 05e874262e4..26aa2888b97 100644
--- a/arch/arm64/src/common/arm64_vectors.S
+++ b/arch/arm64/src/common/arm64_vectors.S
@@ -99,6 +99,63 @@ SECTION_FUNC(text, up_saveusercontext)
 
     ret
 
+/****************************************************************************
+ * Function: arm64_context_switch
+ *
+ * Description:
+ *  Routine to handle context switch
+ *
+ * arm64_context_switch( x0, x1)
+ *     x0: restore thread stack context
+ *     x1: save thread stack context
+ * note:
+ *     x1 = 0, only restore x0
+ *
+ ****************************************************************************/
+
+GTEXT(arm64_context_switch)
+SECTION_FUNC(text, arm64_context_switch)
+    cmp    x1, #0x0
+    beq    restore_new
+
+#if defined(CONFIG_ARCH_FPU) && defined(CONFIG_SMP)
+    stp    x0, x1, [sp, #-16]!
+    stp    xzr, x30, [sp, #-16]!
+    bl     arm64_fpu_context_save
+    ldp    xzr, x30, [sp], #16
+    ldp    x0, x1, [sp], #16
+#endif
+
+    /* Save the current SP_ELx */
+    add    x4, sp, #8 * XCPTCONTEXT_GP_REGS
+    str    x4, [x1, #8 * REG_SP_ELX]
+
+    /* Save the current task's SP_EL0 and exception depth */
+    mrs    x4, sp_el0
+    mrs    x5, tpidrro_el0
+    stp    x4, x5, [x1, #8 * REG_SP_EL0]
+
+restore_new:
+
+    /* Restore SP_EL0 and thread's exception dept */
+    ldp    x4,  x5,  [x0, #8 * REG_SP_EL0]
+    msr    tpidrro_el0, x5
+    msr    sp_el0, x4
+
+    /* retrieve new thread's SP_ELx */
+    ldr    x4, [x0, #8 * REG_SP_ELX]
+    sub    sp, x4, #8 * XCPTCONTEXT_GP_REGS
+
+#ifdef CONFIG_ARCH_FPU
+    stp    xzr, x30, [sp, #-16]!
+    bl     arm64_fpu_context_restore
+    ldp    xzr, x30, [sp], #16
+#endif
+
+    /* Return to arm64_sync_exc() or arm64_irq_handler() */
+
+    ret
+
 /****************************************************************************
  * Function: arm64_jump_to_user
  *
@@ -142,27 +199,28 @@ SECTION_FUNC(text, arm64_sync_exc)
     /* checking the EC value to see which exception need to be handle */
 
 #if CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3
-    mrs    x0, esr_el3
+    mrs    x9, esr_el3
 #else
-    mrs    x0, esr_el1
+    mrs    x9, esr_el1
 #endif
-    lsr    x1, x0, #26
+    lsr    x10, x9, #26
 
 #ifdef CONFIG_ARCH_FPU
     /* fpu trap */
 
-    cmp    x1, #0x07 /*Access to SIMD or floating-point */
+    cmp    x10, #0x07 /* Access to SIMD or floating-point */
     bne    1f
-    mov    x0, sp
+    mov    x0, sp     /* x0 = context */
+    bl     arm64_fpu_trap
 
     /* when the fpu trap is handled */
 
-    b      arm64_exit_exception
+    b      arm64_exit_exc_fpu_done
 1:
 #endif
     /* 0x15 = SVC system call */
 
-    cmp    x1, #0x15
+    cmp    x10, #0x15
 
     /* if this is a svc call ?*/
 
@@ -250,13 +308,17 @@ context_switch:
      */
 
     mov    x0, sp
-    str    x0, [sp, #-16]!
     bl    arm64_syscall_switch
 
     /* get save task reg context pointer */
 
-    ldr    x1, [sp], #16
+    ldr    x1, [sp, #8 * REG_X1]
+    cmp    x1, #0x0
 
+    beq do_switch
+    ldr x1, [x1]
+
+do_switch:
 #ifdef CONFIG_SMP
     /* Notes:
      * Complete any pending TLB or cache maintenance on this CPU in case
@@ -268,7 +330,15 @@ context_switch:
     dsb   ish
 #endif
 
+    bl    arm64_context_switch
+
+#ifdef CONFIG_ARCH_FPU
+    /* when the fpu trap is handled */
+
+    b    arm64_exit_exc_fpu_done
+#else
     b    arm64_exit_exception
+#endif
 
 save_context:
     arm64_exception_context_save x0 x1 sp
@@ -338,6 +408,14 @@ SECTION_FUNC(text, arm64_irq_handler)
 
     ldr    x1, [sp], #16
 
+    /* retrieve the task's stack. */
+
+    mov    sp, x1
+
+    cmp    x0, x1
+    beq    irq_exit
+
+irq_context_switch:
 #ifdef CONFIG_SMP
     /* Notes:
      * Complete any pending TLB or cache maintenance on this CPU in case
@@ -349,6 +427,19 @@ SECTION_FUNC(text, arm64_irq_handler)
 
 #endif
 
+    /*  Switch thread
+     *  - x0: restore task reg context, return by arm64_decodeirq,
+     *  - x1: save task reg context, save before call arm64_decodeirq
+     *    call arm64_context_switch(x0) to switch
+     */
+    bl    arm64_context_switch
+#ifdef CONFIG_ARCH_FPU
+    /* when the fpu trap is handled */
+
+    b     arm64_exit_exc_fpu_done
+#endif
+
+irq_exit:
     b     arm64_exit_exception
 
 /* TODO: if the arm64_fatal_handler return success, maybe need context switch */
@@ -411,6 +502,14 @@ SECTION_FUNC(text, arm64_fiq_handler)
 
     ldr    x1, [sp], #16
 
+    /* retrieve the task's stack. */
+
+    mov    sp, x1
+
+    cmp    x0, x1
+    beq    fiq_exit
+
+
 #ifdef CONFIG_SMP
     /* Notes:
      * Complete any pending TLB or cache maintenance on this CPU in case
@@ -422,5 +521,17 @@ SECTION_FUNC(text, arm64_fiq_handler)
 
 #endif
 
+    /*  Switch thread
+     *  - x0: restore task reg context, return by arm64_decodefiq,
+     *  - x1: save task reg context, save before call arm64_decodefiq
+     *    call arm64_context_switch(x0) to switch
+     */
+    bl    arm64_context_switch
+#ifdef CONFIG_ARCH_FPU
+    /* when the fpu trap is handled */
+
+    b     arm64_exit_exc_fpu_done
+#endif
+fiq_exit:
     b     arm64_exit_exception
 #endif