diff --git a/arch/arm64/src/common/arm64_arch.h b/arch/arm64/src/common/arm64_arch.h
index 06935472fb595..35c4cdc105c15 100644
--- a/arch/arm64/src/common/arm64_arch.h
+++ b/arch/arm64/src/common/arm64_arch.h
@@ -299,6 +299,9 @@ 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 0052adac94a74..cb0d4bd24a2ff 100644
--- a/arch/arm64/src/common/arm64_exit.c
+++ b/arch/arm64/src/common/arm64_exit.c
@@ -65,12 +65,6 @@ 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 c1accb48ebfa5..e80e7240aee29 100644
--- a/arch/arm64/src/common/arm64_fpu.c
+++ b/arch/arm64/src/common/arm64_fpu.c
@@ -77,11 +77,10 @@ struct arm64_fpu_procfs_file_s
  * Private Data
  ***************************************************************************/
 
-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
 
+static struct arm64_cpu_fpu_context g_cpu_fpu_ctx[CONFIG_SMP_NCPUS];
+
 /* procfs methods */
 
 static int arm64_fpu_procfs_open(struct file *filep, const char *relpath,
@@ -262,141 +261,6 @@ 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 93aa822fff6ec..32058db4855d4 100644
--- a/arch/arm64/src/common/arm64_initialstate.c
+++ b/arch/arm64/src/common/arm64_initialstate.c
@@ -47,10 +47,6 @@
 #include "chip.h"
 #include "arm64_fatal.h"
 
-#ifdef CONFIG_ARCH_FPU
-#include "arm64_fpu.h"
-#endif
-
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -70,17 +66,6 @@ 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;
@@ -150,11 +135,6 @@ 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 888c8106268dd..4f0534345a2ff 100644
--- a/arch/arm64/src/common/arm64_schedulesigaction.c
+++ b/arch/arm64/src/common/arm64_schedulesigaction.c
@@ -38,10 +38,6 @@
 #include "irq/irq.h"
 #include "arm64_fatal.h"
 
-#ifdef CONFIG_ARCH_FPU
-#include "arm64_fpu.h"
-#endif
-
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -58,16 +54,6 @@ 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;
@@ -177,9 +163,6 @@ 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());
 
@@ -201,9 +184,6 @@ 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;
@@ -279,9 +259,6 @@ 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;
@@ -300,9 +277,6 @@ 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());
 
@@ -339,9 +313,6 @@ 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 15e7946c7c007..0d46a98410db9 100644
--- a/arch/arm64/src/common/arm64_sigdeliver.c
+++ b/arch/arm64/src/common/arm64_sigdeliver.c
@@ -38,10 +38,6 @@
 #include "irq/irq.h"
 #include "arm64_fatal.h"
 
-#ifdef CONFIG_ARCH_FPU
-#include "arm64_fpu.h"
-#endif
-
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -157,11 +153,6 @@ void arm64_sigdeliver(void)
   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 318e7e0cf60f3..c59aef35cbd0c 100644
--- a/arch/arm64/src/common/arm64_syscall.c
+++ b/arch/arm64/src/common/arm64_syscall.c
@@ -193,7 +193,6 @@ 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 1c05814c8763b..21b70d2575207 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 xreg0, xreg1
-    sub    sp, sp, #8 * XCPTCONTEXT_GP_REGS
+.macro arm64_enter_exception
+    sub    sp, sp, #8 * XCPTCONTEXT_REGS
 
     stp    x0,  x1,  [sp, #8 * REG_X0]
     stp    x2,  x3,  [sp, #8 * REG_X2]
@@ -65,30 +65,32 @@
     stp    x28, x29, [sp, #8 * REG_X28]
 
     /* Save the current task's SP_ELx and x30 */
-    add    \xreg0, sp, #8 * XCPTCONTEXT_GP_REGS
-    stp    x30, \xreg0,  [sp, #8 * REG_X30]
+    add    x0, sp, #8 * XCPTCONTEXT_REGS
+    stp    x30, x0,  [sp, #8 * REG_X30]
 
     /* ELR and SPSR */
 #if CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3
-    mrs    \xreg0, elr_el3
-    mrs    \xreg1, spsr_el3
+    mrs    x0, elr_el3
+    mrs    x1, spsr_el3
 #else
-    mrs    \xreg0, elr_el1
-    mrs    \xreg1, spsr_el1
+    mrs    x0, elr_el1
+    mrs    x1, spsr_el1
 #endif
-    stp    \xreg0, \xreg1,  [sp, #8 * REG_ELR]
+    stp    x0, x1,  [sp, #8 * REG_ELR]
 
-    /* increment exception depth */
+    /* Increment exception depth */
 
-    mrs    \xreg0, tpidrro_el0
-    mov    \xreg1, #1
-    add    \xreg0, \xreg0, \xreg1
-    msr    tpidrro_el0, \xreg0
+    mrs    x0, tpidrro_el0
+    mov    x1, #1
+    add    x0, x0, x1
+    msr    tpidrro_el0, x0
 
+    /* Save the FPU registers */
+
+    add    x0, sp, #8 * XCPTCONTEXT_GP_REGS
 #ifdef CONFIG_ARCH_FPU
-    bl    arm64_fpu_enter_exception
+    bl     arm64_fpu_save
 #endif
-
 .endm
 
 /****************************************************************************
@@ -143,97 +145,97 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table)
     /* Current EL with SP0 / Synchronous */
 
     .align 7
-    arm64_enter_exception x9, x10
+    arm64_enter_exception
     b    arm64_sync_exc
 
     /* Current EL with SP0 / IRQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_irq_handler
 
     /* Current EL with SP0 / FIQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b     arm64_fiq_handler
 
     /* Current EL with SP0 / SError */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_serror_handler
 
     /* Current EL with SPx / Synchronous */
 
     .align 7
-    arm64_enter_exception x9, x10
+    arm64_enter_exception
     b    arm64_sync_exc
 
     /* Current EL with SPx / IRQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_irq_handler
 
     /* Current EL with SPx / FIQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b     arm64_fiq_handler
 
     /* Current EL with SPx / SError */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_serror_handler
 
     /* Lower EL using AArch64 / Synchronous */
 
     .align 7
-    arm64_enter_exception x9, x10
+    arm64_enter_exception
     b    arm64_sync_exc
 
     /* Lower EL using AArch64 / IRQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_irq_handler
 
     /* Lower EL using AArch64 / FIQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b     arm64_fiq_handler
 
     /* Lower EL using AArch64 / SError */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_serror_handler
 
     /* Lower EL using AArch32 / Synchronous */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_mode32_handler
 
     /* Lower EL using AArch32 / IRQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_mode32_handler
 
     /* Lower EL using AArch32 / FIQ */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_mode32_handler
 
     /* Lower EL using AArch32 / SError */
 
     .align 7
-    arm64_enter_exception x0, x1
+    arm64_enter_exception
     b    arm64_mode32_handler
 
 /* Restore Corruptible Registers and exception context
@@ -242,10 +244,11 @@ 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_exit_exception
-GTEXT(arm64_exit_exc_fpu_done)
-arm64_exit_exc_fpu_done:
+    bl     arm64_fpu_restore
 #endif
 
     /* restore spsr and elr at el1*/
@@ -283,6 +286,6 @@ arm64_exit_exc_fpu_done:
     ldp    x28, x29, [sp, #8 * REG_X28]
     ldp    x30, xzr, [sp, #8 * REG_X30]
 
-    add    sp, sp, #8 * XCPTCONTEXT_GP_REGS
+    add    sp, sp, #8 * XCPTCONTEXT_REGS
 
     eret
diff --git a/arch/arm64/src/common/arm64_vectors.S b/arch/arm64/src/common/arm64_vectors.S
index e63e27ae36c0a..66b7eaefb73fd 100644
--- a/arch/arm64/src/common/arm64_vectors.S
+++ b/arch/arm64/src/common/arm64_vectors.S
@@ -99,63 +99,6 @@ 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_sync_exc
  *
@@ -169,28 +112,27 @@ 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    x9, esr_el3
+    mrs    x0, esr_el3
 #else
-    mrs    x9, esr_el1
+    mrs    x0, esr_el1
 #endif
-    lsr    x10, x9, #26
+    lsr    x1, x0, #26
 
 #ifdef CONFIG_ARCH_FPU
     /* fpu trap */
 
-    cmp    x10, #0x07 /* Access to SIMD or floating-point */
+    cmp    x1, #0x07 /*Access to SIMD or floating-point */
     bne    1f
-    mov    x0, sp     /* x0 = context */
-    bl     arm64_fpu_trap
+    mov    x0, sp
 
     /* when the fpu trap is handled */
 
-    b      arm64_exit_exc_fpu_done
+    b      arm64_exit_exception
 1:
 #endif
     /* 0x15 = SVC system call */
 
-    cmp    x10, #0x15
+    cmp    x1, #0x15
 
     /* if this is a svc call ?*/
 
@@ -278,17 +220,13 @@ context_switch:
      */
 
     mov    x0, sp
+    str    x0, [sp, #-16]!
     bl    arm64_syscall_switch
 
     /* get save task reg context pointer */
 
-    ldr    x1, [sp, #8 * REG_X1]
-    cmp    x1, #0x0
-
-    beq do_switch
-    ldr x1, [x1]
+    ldr    x1, [sp], #16
 
-do_switch:
 #ifdef CONFIG_SMP
     /* Notes:
      * Complete any pending TLB or cache maintenance on this CPU in case
@@ -300,15 +238,7 @@ do_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
@@ -378,14 +308,6 @@ 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
@@ -397,19 +319,6 @@ irq_context_switch:
 
 #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 */
@@ -472,14 +381,6 @@ 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
@@ -491,17 +392,5 @@ 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