[libcpu/aarch64] add smp support

21 files changed, 1054 insertions, 567 deletions

diff --git a/libcpu/aarch64/common/armv8.h b/libcpu/aarch64/common/armv8.h
index 77fb4de8a2..99baa81c6e 100644
--- a/libcpu/aarch64/common/armv8.h
+++ b/libcpu/aarch64/common/armv8.h
@@ -6,18 +6,23 @@
  * Change Logs:
  * Date           Author       Notes
  * 2011-09-15     Bernard      first version
+ * 2021-12-28     GuEe-GUI     add fpu support
  */
 
 #ifndef __ARMV8_H__
 #define __ARMV8_H__
 
+#include <rtdef.h>
+
 /* the exception stack without VFP registers */
 struct rt_hw_exp_stack
 {
     unsigned long long pc;
     unsigned long long spsr;
     unsigned long long x30;
-    unsigned long long xz;
+    unsigned long long xzr;
+    unsigned long long fpcr;
+    unsigned long long fpsr;
     unsigned long long x28;
     unsigned long long x29;
     unsigned long long x26;
@@ -48,6 +53,8 @@ struct rt_hw_exp_stack
     unsigned long long x3;
     unsigned long long x0;
     unsigned long long x1;
+
+    unsigned long long fpu[16];
 };
 
 #define SP_ELx                      ( ( unsigned long long ) 0x01 )
diff --git a/libcpu/aarch64/common/asm_fpu.h b/libcpu/aarch64/common/asm_fpu.h
new file mode 100644
index 0000000000..f40ac7a9a2
--- /dev/null
+++ b/libcpu/aarch64/common/asm_fpu.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-05-18     Jesven       the first version
+ */
+
+.macro SAVE_FPU, reg
+    STR Q0, [\reg, #-0x10]!
+    STR Q1, [\reg, #-0x10]!
+    STR Q2, [\reg, #-0x10]!
+    STR Q3, [\reg, #-0x10]!
+    STR Q4, [\reg, #-0x10]!
+    STR Q5, [\reg, #-0x10]!
+    STR Q6, [\reg, #-0x10]!
+    STR Q7, [\reg, #-0x10]!
+    STR Q8, [\reg, #-0x10]!
+    STR Q9, [\reg, #-0x10]!
+    STR Q10, [\reg, #-0x10]!
+    STR Q11, [\reg, #-0x10]!
+    STR Q12, [\reg, #-0x10]!
+    STR Q13, [\reg, #-0x10]!
+    STR Q14, [\reg, #-0x10]!
+    STR Q15, [\reg, #-0x10]!
+.endm
+
+.macro RESTORE_FPU, reg
+    LDR Q15, [\reg], #0x10
+    LDR Q14, [\reg], #0x10
+    LDR Q13, [\reg], #0x10
+    LDR Q12, [\reg], #0x10
+    LDR Q11, [\reg], #0x10
+    LDR Q10, [\reg], #0x10
+    LDR Q9, [\reg], #0x10
+    LDR Q8, [\reg], #0x10
+    LDR Q7, [\reg], #0x10
+    LDR Q6, [\reg], #0x10
+    LDR Q5, [\reg], #0x10
+    LDR Q4, [\reg], #0x10
+    LDR Q3, [\reg], #0x10
+    LDR Q2, [\reg], #0x10
+    LDR Q1, [\reg], #0x10
+    LDR Q0, [\reg], #0x10
+.endm
diff --git a/libcpu/aarch64/common/cache.S b/libcpu/aarch64/common/cache.S
index 7f295a2b02..38c476ac47 100644
--- a/libcpu/aarch64/common/cache.S
+++ b/libcpu/aarch64/common/cache.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2006-2020, RT-Thread Development Team
+ * Copyright (c) 2006-2021, RT-Thread Development Team
  *
  * SPDX-License-Identifier: Apache-2.0
  *
@@ -134,6 +134,57 @@ __asm_flush_dcache_range:
     dsb    sy
     ret
 
+/* void __asm_invalidate_dcache_range(start, end)
+ *
+ * invalidate data cache in the range
+ *
+ * x0: start address
+ * x1: end address
+ */
+.globl __asm_invalidate_dcache_range
+__asm_invalidate_dcache_range:
+    mrs    x3, ctr_el0
+    lsr    x3, x3, #16
+    and    x3, x3, #0xf
+    mov    x2, #4
+    lsl    x2, x2, x3        /* cache line size */
+
+    /* x2 <- minimal cache line size in cache system */
+    sub    x3, x2, #1
+    bic    x0, x0, x3
+
+1:  dc   ivac, x0    /* invalidate data or unified cache */
+    add    x0, x0, x2
+    cmp    x0, x1
+    b.lo    1b
+    dsb    sy
+    ret
+
+/* void __asm_invalidate_icache_range(start, end)
+ *
+ * invalidate icache in the range
+ *
+ * x0: start address
+ * x1: end address
+ */
+.globl __asm_invalidate_icache_range
+__asm_invalidate_icache_range:
+    mrs    x3, ctr_el0
+    and    x3, x3, #0xf
+    mov    x2, #4
+    lsl    x2, x2, x3        /* cache line size */
+
+    /* x2 <- minimal cache line size in cache system */
+    sub    x3, x2, #1
+    bic    x0, x0, x3
+
+1:  ic   ivau, x0    /* invalidate instruction or unified cache */
+    add    x0, x0, x2
+    cmp    x0, x1
+    b.lo    1b
+    dsb    sy
+    ret
+
 /*
  * void __asm_invalidate_icache_all(void)
  *
diff --git a/libcpu/aarch64/common/cache_ops.c b/libcpu/aarch64/common/cache_ops.c
new file mode 100644
index 0000000000..517845e924
--- /dev/null
+++ b/libcpu/aarch64/common/cache_ops.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-12-28     GuEe-GUI     the first version
+ */
+
+#include <rthw.h>
+#include <rtdef.h>
+
+void __asm_flush_dcache_all(void);
+void __asm_invalidate_dcache_all(void);
+void __asm_flush_dcache_range(unsigned long start, unsigned long end);
+void __asm_invalidate_dcache_range(unsigned long start, unsigned long end);
+
+void __asm_invalidate_icache_all(void);
+void __asm_invalidate_icache_range(unsigned long start, unsigned long end);
+
+void rt_hw_dcache_flush_all(void)
+{
+    __asm_flush_dcache_all();
+}
+
+void rt_hw_dcache_invalidate_all(void)
+{
+    __asm_invalidate_dcache_all();
+}
+
+void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size)
+{
+    __asm_flush_dcache_range(start_addr, start_addr + size);
+}
+
+void rt_hw_dcache_invalidate_range(unsigned long start_addr,unsigned long size)
+{
+    __asm_invalidate_dcache_range(start_addr, start_addr + size);
+}
+
+void rt_hw_icache_invalidate_all()
+{
+    __asm_invalidate_icache_all();
+}
+
+void rt_hw_icache_invalidate_range(unsigned long start_addr, int size)
+{
+    __asm_invalidate_icache_range(start_addr, start_addr + size);
+}
+
+void rt_hw_cpu_icache_ops(int ops, void *addr, int size)
+{
+    if (ops == RT_HW_CACHE_INVALIDATE)
+    {
+        rt_hw_icache_invalidate_range((unsigned long)addr, size);
+    }
+}
+
+void rt_hw_cpu_dcache_ops(int ops, void *addr, int size)
+{
+    if (ops == RT_HW_CACHE_FLUSH)
+    {
+        rt_hw_dcache_flush_range((unsigned long)addr, size);
+    }
+    else if (ops == RT_HW_CACHE_INVALIDATE)
+    {
+        rt_hw_dcache_invalidate_range((unsigned long)addr, size);
+    }
+}
diff --git a/libcpu/aarch64/common/context_gcc.S b/libcpu/aarch64/common/context_gcc.S
index b1a8313916..9d7f52b585 100644
--- a/libcpu/aarch64/common/context_gcc.S
+++ b/libcpu/aarch64/common/context_gcc.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2006-2020, RT-Thread Development Team
+ * Copyright (c) 2006-2021, RT-Thread Development Team
  *
  * SPDX-License-Identifier: Apache-2.0
  *
@@ -7,8 +7,17 @@
  * Date           Author       Notes
  * 2018-10-06     ZhaoXiaowei  the first version
  * 2021-11-04     GuEe-GUI     set sp with SP_ELx
+ * 2021-12-28     GuEe-GUI     add fpu and smp support
  */
 
+#include "rtconfig.h"
+#include "asm_fpu.h"
+
+#ifdef RT_USING_SMP
+#define rt_hw_interrupt_disable rt_hw_local_irq_disable
+#define rt_hw_interrupt_enable rt_hw_local_irq_enable
+#endif
+
 /*
  *enable gtimer
  */
@@ -58,6 +67,7 @@ rt_hw_get_gtimer_frq:
 
 .macro SAVE_CONTEXT
     /* Save the entire context. */
+    SAVE_FPU SP
     STP 	X0, X1, [SP, #-0x10]!
     STP 	X2, X3, [SP, #-0x10]!
     STP 	X4, X5, [SP, #-0x10]!
@@ -73,6 +83,9 @@ rt_hw_get_gtimer_frq:
     STP 	X24, X25, [SP, #-0x10]!
     STP 	X26, X27, [SP, #-0x10]!
     STP 	X28, X29, [SP, #-0x10]!
+    MRS     X28, FPCR
+    MRS     X29, FPSR
+    STP     X28, X29, [SP, #-0x10]!
     STP 	X30, XZR, [SP, #-0x10]!
 
     MRS		X0, CurrentEL
@@ -107,6 +120,7 @@ rt_hw_get_gtimer_frq:
 
 .macro SAVE_CONTEXT_T
     /* Save the entire context. */
+    SAVE_FPU SP
     STP 	X0, X1, [SP, #-0x10]!
     STP 	X2, X3, [SP, #-0x10]!
     STP 	X4, X5, [SP, #-0x10]!
@@ -122,6 +136,9 @@ rt_hw_get_gtimer_frq:
     STP 	X24, X25, [SP, #-0x10]!
     STP 	X26, X27, [SP, #-0x10]!
     STP 	X28, X29, [SP, #-0x10]!
+    MRS     X28, FPCR
+    MRS     X29, FPSR
+    STP     X28, X29, [SP, #-0x10]!
     STP 	X30, XZR, [SP, #-0x10]!
 
     MRS		X0, CurrentEL
@@ -182,6 +199,9 @@ rt_hw_get_gtimer_frq:
 0:
 
     LDP 	X30, XZR, [SP], #0x10
+    LDP     X28, X29, [SP], #0x10
+    MSR     FPCR, X28
+    MSR     FPSR, X29
     LDP 	X28, X29, [SP], #0x10
     LDP 	X26, X27, [SP], #0x10
     LDP 	X24, X25, [SP], #0x10
@@ -197,6 +217,7 @@ rt_hw_get_gtimer_frq:
     LDP 	X4, X5, [SP], #0x10
     LDP 	X2, X3, [SP], #0x10
     LDP 	X0, X1, [SP], #0x10
+    RESTORE_FPU SP
 
     ERET
 
@@ -227,22 +248,46 @@ rt_hw_interrupt_enable_exit:
     RET
 
 /*
+ * #ifdef RT_USING_SMP
+ * void rt_hw_context_switch_to(rt_ubase_t to, stuct rt_thread *to_thread);
+ * #else
  * void rt_hw_context_switch_to(rt_ubase_t to);
- * r0 --> to
+ * #endif
+ * X0 --> to
+ * X1 --> to_thread
  */
 .globl rt_hw_context_switch_to
 rt_hw_context_switch_to:
+#ifdef RT_USING_SMP
+    STR     X0, [SP, #-0x8]!
+    MOV     X0, X1
+    BL      rt_cpus_lock_status_restore
+    LDR     X0, [SP], #0x8
+#endif /*RT_USING_SMP*/
     LDR		X0, [X0]
     RESTORE_CONTEXT
 
 .text
 /*
+ * #ifdef RT_USING_SMP
+ * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to, struct rt_thread *to_thread);
+ * #else
  * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to);
- * r0 --> from
- * r1 --> to
+ * #endif
+ * X0 --> from
+ * X1 --> to
+ * X2 --> to_thread
  */
 .globl rt_hw_context_switch
 rt_hw_context_switch:
+#ifdef RT_USING_SMP
+    STP     X0, X1, [SP, #-0x10]!
+    STR     X30, [SP, #-0x8]!
+    MOV     X0, X2
+    BL      rt_cpus_lock_status_restore
+    LDR     X30, [SP], #0x8
+    LDP     X0, X1, [SP], #0x10
+#endif /*RT_USING_SMP*/
 
     MOV		X8,X0
     MOV		X9,X1
@@ -262,19 +307,32 @@ rt_hw_context_switch:
 .globl rt_interrupt_to_thread
 .globl rt_hw_context_switch_interrupt
 rt_hw_context_switch_interrupt:
-    ADR 	X2, rt_thread_switch_interrupt_flag
+#ifdef RT_USING_SMP
+    /* x0 = context */
+    /* x1 = &current_thread->sp */
+    /* x2 = &to_thread->sp, */
+    /* x3 = to_thread TCB */
+    STR     X0, [X1]
+    LDR     X0, [x2]
+    MOV     SP, X0
+    MOV     X0, X3
+    BL      rt_cpus_lock_status_restore
+    MOV     X0, SP
+    RESTORE_CONTEXT
+#else
+    LDR 	X2, =rt_thread_switch_interrupt_flag
     LDR 	X3, [X2]
     CMP 	X3, #1
     B.EQ 	_reswitch
-    ADR 	X4, rt_interrupt_from_thread   // set rt_interrupt_from_thread
+    LDR 	X4, =rt_interrupt_from_thread  // set rt_interrupt_from_thread
     MOV 	X3, #1              // set rt_thread_switch_interrupt_flag to 1
     STR 	X0, [X4]
     STR 	X3, [X2]
 _reswitch:
-    ADR 	X2, rt_interrupt_to_thread     // set rt_interrupt_to_thread
+    LDR 	X2, =rt_interrupt_to_thread    // set rt_interrupt_to_thread
     STR 	X1, [X2]
     RET
-
+#endif
 .text
 
 // -- Exception handlers ----------------------------------
@@ -308,10 +366,15 @@ vector_irq:
     BL      rt_interrupt_leave
     
     LDP 	X0, X1, [SP], #0x10
+#ifdef RT_USING_SMP
+    /* Never reture If can switch */
+    BL      rt_scheduler_do_irq_switch
+    MOV     X0, SP
+#endif
 
     // if rt_thread_switch_interrupt_flag set, jump to
     // rt_hw_context_switch_interrupt_do and don't return
-    ADR 	X1, rt_thread_switch_interrupt_flag
+    LDR 	X1, =rt_thread_switch_interrupt_flag
     LDR     X2, [X1]
     CMP     X2, #1
     B.NE     vector_irq_exit
@@ -319,11 +382,11 @@ vector_irq:
     MOV     X2,  #0         // clear flag
     STR     X2,  [X1]
 
-    ADR     X3,  rt_interrupt_from_thread
+    LDR     X3, =rt_interrupt_from_thread
     LDR     X4,  [X3]
     STR     x0,  [X4]       // store sp in preempted tasks's TCB
 
-    ADR     x3,  rt_interrupt_to_thread
+    LDR     x3, =rt_interrupt_to_thread
     LDR     X4,  [X3]
     LDR     x0,  [X4]       // get new task's stack pointer
     
diff --git a/libcpu/aarch64/common/cp15.h b/libcpu/aarch64/common/cp15.h
deleted file mode 100644
index e6d8626ab9..0000000000
--- a/libcpu/aarch64/common/cp15.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * Copyright (c) 2006-2021, RT-Thread Development Team
- *
- * SPDX-License-Identifier: Apache-2.0
- *
- * Change Logs:
- * Date           Author       Notes
- * 2011-09-15     Bernard      first version
- */
-
-#ifndef __CP15_H__
-#define __CP15_H__
-
-#ifndef   __STATIC_FORCEINLINE
-#define __STATIC_FORCEINLINE     __attribute__((always_inline)) static inline
-#endif
-
-#define __WFI()    __asm__ volatile ("wfi":::"memory")
-
-#define __WFE()    __asm__ volatile ("wfe":::"memory")
-
-#define __SEV()    __asm__ volatile ("sev")
-
-__STATIC_FORCEINLINE  void __ISB(void)
-{
-    __asm__ volatile ("isb 0xF":::"memory");
-}
-
-/**
-  \brief   Data Synchronization Barrier
-  \details Acts as a special kind of Data Memory Barrier.
-           It completes when all explicit memory accesses before this instruction complete.
- */
-__STATIC_FORCEINLINE  void __DSB(void)
-{
-    __asm__ volatile ("dsb 0xF":::"memory");
-}
-
-/**
-  \brief   Data Memory Barrier
-  \details Ensures the apparent order of the explicit memory operations before
-           and after the instruction, without ensuring their completion.
- */
-
-__STATIC_FORCEINLINE  void __DMB(void)
-{
-    __asm__ volatile ("dmb 0xF":::"memory");
-}
-
-#ifdef RT_USING_SMP
-static inline void send_ipi_msg(int cpu, int ipi_vector)
-{
-    IPI_MAILBOX_SET(cpu) = 1 << ipi_vector;
-}
-
-static inline void setup_bootstrap_addr(int cpu, int addr)
-{
-    CORE_MAILBOX3_SET(cpu) = addr;
-}
-
-static inline void enable_cpu_ipi_intr(int cpu)
-{
-    COREMB_INTCTL(cpu) = IPI_MAILBOX_INT_MASK;
-}
-
-static inline void enable_cpu_timer_intr(int cpu)
-{
-    CORETIMER_INTCTL(cpu) = 0x8;
-}
-
-static inline void enable_cntv(void)
-{
-    rt_uint32_t cntv_ctl;
-    cntv_ctl = 1;
-    asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
-}
-
-static inline void disable_cntv(void)
-{
-    rt_uint32_t cntv_ctl;
-    cntv_ctl = 0;
-    asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
-}
-
-static inline  void mask_cntv(void)
-{
-    rt_uint32_t cntv_ctl;
-    cntv_ctl = 2;
-    asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
-}
-
-static inline void unmask_cntv(void)
-{
-    rt_uint32_t cntv_ctl;
-    cntv_ctl = 1;
-    asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
-}
-
-static inline rt_uint64_t read_cntvct(void)
-{
-    rt_uint32_t val,val1;
-    asm volatile("mrrc p15, 1, %0, %1, c14" : "=r" (val),"=r" (val1));
-    return (val);
-}
-
-static inline rt_uint64_t read_cntvoff(void)
-{
-
-    rt_uint64_t val;
-    asm volatile("mrrc p15, 4, %Q0, %R0, c14" : "=r" (val));
-    return (val);
-}
-
-static inline rt_uint32_t read_cntv_tval(void)
-{
-    rt_uint32_t val;
-    asm volatile ("mrc p15, 0, %0, c14, c3, 0" : "=r"(val) );
-    return val;
-}
-
-
-static inline  void write_cntv_tval(rt_uint32_t val)
-{
-    asm volatile ("mcr p15, 0, %0, c14, c3, 0" :: "r"(val) );
-    return;
-}
-
-static inline rt_uint32_t read_cntfrq(void)
-{
-    rt_uint32_t val;
-    asm volatile ("mrc p15, 0, %0, c14, c0, 0" : "=r"(val) );
-    return val;
-}
-
-
-static inline  rt_uint32_t read_cntctrl(void)
-{
-    rt_uint32_t val;
-    asm volatile ("mrc p15, 0, %0, c14, c1, 0" : "=r"(val) );
-    return val;
-}
-
-static inline uint32_t write_cntctrl(uint32_t val)
-{
-
-    asm volatile ("mcr p15, 0, %0, c14, c1, 0" : :"r"(val) );
-    return val;
-}
-#endif
-
-unsigned long rt_cpu_get_smp_id(void);
-
-void rt_cpu_mmu_disable(void);
-void rt_cpu_mmu_enable(void);
-void rt_cpu_tlb_set(volatile unsigned long*);
-
-void rt_cpu_dcache_clean_flush(void);
-void rt_cpu_icache_flush(void);
-
-void rt_cpu_vector_set_base(rt_ubase_t addr);
-void rt_hw_mmu_init(void);
-void rt_hw_vector_init(void);
-
-void set_timer_counter(unsigned int counter);
-void set_timer_control(unsigned int control);
-#endif
diff --git a/libcpu/aarch64/common/cpu.c b/libcpu/aarch64/common/cpu.c
index 66b540a5e5..3c1bb69b77 100644
--- a/libcpu/aarch64/common/cpu.c
+++ b/libcpu/aarch64/common/cpu.c
@@ -7,25 +7,38 @@
  * Date           Author       Notes
  * 2011-09-15     Bernard      first version
  * 2019-07-28     zdzn         add smp support
+ * 2021-12-21     GuEe-GUI     set tpidr_el2 as multiprocessor id instead of mpidr_el1
+ * 2021-12-28     GuEe-GUI     add spinlock for aarch64
  */
 
 #include <rthw.h>
 #include <rtthread.h>
-#include <board.h>
-#include "cp15.h"
+#include <cpuport.h>
+
+#ifdef RT_USING_SMP
+/* The more common mpidr_el1 table, redefine it in BSP if it is in other cases */
+RT_WEAK rt_uint64_t rt_cpu_mpidr_early[] =
+{
+    [0] = 0x80000000,
+    [1] = 0x80000001,
+    [2] = 0x80000002,
+    [3] = 0x80000003,
+    [4] = 0x80000004,
+    [5] = 0x80000005,
+    [6] = 0x80000006,
+    [7] = 0x80000007,
+    [RT_CPUS_NR] = 0
+};
+#endif
 
 int rt_hw_cpu_id(void)
 {
-    int cpu_id;
     rt_base_t value;
 
-    __asm__ volatile (
-            "mrs %0, mpidr_el1"
-            :"=r"(value)
-            );
-    cpu_id = value & 0xf;
-    return cpu_id;
-};
+    __asm__ volatile ("mrs %0, tpidr_el1":"=r"(value));
+
+    return value;
+}
 
 #ifdef RT_USING_SMP
 void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock)
@@ -35,38 +48,45 @@ void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock)
 
 void rt_hw_spin_lock(rt_hw_spinlock_t *lock)
 {
-    unsigned long tmp;
-    unsigned long newval;
-    rt_hw_spinlock_t lockval;
-    __asm__ __volatile__(
-            "pld [%0]"
-            ::"r"(&lock->slock)
-            );
-
-    __asm__ __volatile__(
-            "1: ldrex   %0, [%3]\n"
-            "   add %1, %0, %4\n"
-            "   strex   %2, %1, [%3]\n"
-            "   teq %2, #0\n"
-            "   bne 1b"
-            : "=&r" (lockval), "=&r" (newval), "=&r" (tmp)
-            : "r" (&lock->slock), "I" (1 << 16)
-            : "cc");
-
-    while (lockval.tickets.next != lockval.tickets.owner) {
-        __WFE();
-        lockval.tickets.owner = *(volatile unsigned short *)(&lock->tickets.owner);
-    }
+    rt_hw_spinlock_t lock_val, new_lockval;
+    unsigned int tmp;
 
+    __asm__ volatile (
+        /* Increment the next ticket. */
+        "   prfm    pstl1strm, %3\n"
+        "1: ldaxr   %w0, %3\n"
+        "   add %w1, %w0, %w5\n"
+        "   stxr    %w2, %w1, %3\n"
+        "   cbnz    %w2, 1b\n"
+        /* Check wether we get the lock */
+        "   eor     %w1, %w0, %w0, ror #16\n"
+        "   cbz     %w1, 3f\n"
+        /*
+         * Didn't get lock and spin on the owner.
+         * Should send a local event to avoid missing an
+         * unlock before the exclusive load.
+         */
+        "   sevl\n"
+        "2: wfe\n"
+        "   ldaxrh  %w2, %4\n"
+        "   eor     %w1, %w2, %w0, lsr #16\n"
+        "   cbnz    %w1, 2b\n"
+        /* got the lock. */
+        "3:"
+        : "=&r" (lock_val), "=&r" (new_lockval), "=&r" (tmp), "+Q" (*lock)
+        : "Q" (lock->tickets.owner), "I" (1 << 16)
+        : "memory");
     __DMB();
 }
 
 void rt_hw_spin_unlock(rt_hw_spinlock_t *lock)
 {
     __DMB();
-    lock->tickets.owner++;
-    __DSB();
-    __SEV();
+    __asm__ volatile (
+        "stlrh   %w1, %0\n"
+        : "=Q" (lock->tickets.owner)
+        : "r" (lock->tickets.owner + 1)
+        : "memory");
 }
 #endif /*RT_USING_SMP*/
 
diff --git a/libcpu/aarch64/common/cpuport.h b/libcpu/aarch64/common/cpuport.h
index 91c4a7f77b..ab398573ff 100644
--- a/libcpu/aarch64/common/cpuport.h
+++ b/libcpu/aarch64/common/cpuport.h
@@ -13,6 +13,13 @@
 
 #include <rtdef.h>
 
+#define __WFI() __asm__ volatile ("wfi":::"memory")
+#define __WFE() __asm__ volatile ("wfe":::"memory")
+#define __SEV() __asm__ volatile ("sev")
+#define __ISB() __asm__ volatile ("isb 0xf":::"memory")
+#define __DSB() __asm__ volatile ("dsb 0xf":::"memory")
+#define __DMB() __asm__ volatile ("dmb 0xf":::"memory")
+
 rt_inline void rt_hw_isb(void)
 {
     __asm__ volatile ("isb":::"memory");
diff --git a/libcpu/aarch64/common/gic.c b/libcpu/aarch64/common/gic.c
index d23ece42df..7a2c368d23 100644
--- a/libcpu/aarch64/common/gic.c
+++ b/libcpu/aarch64/common/gic.c
@@ -15,7 +15,7 @@
 #include <rtthread.h>
 
 #include <gic.h>
-#include <cp15.h>
+#include <cpuport.h>
 
 struct arm_gic
 {
diff --git a/libcpu/aarch64/common/gtimer.c b/libcpu/aarch64/common/gtimer.c
new file mode 100644
index 0000000000..74187a92cb
--- /dev/null
+++ b/libcpu/aarch64/common/gtimer.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2011-12-20     GuEe-GUI     first version
+ */
+
+#include <rtthread.h>
+#include <rthw.h>
+#include <gtimer.h>
+#include <cpuport.h>
+
+#define EL1_PHY_TIMER_IRQ_NUM 30
+
+static volatile rt_uint64_t timer_step;
+
+static void rt_hw_timer_isr(int vector, void *parameter)
+{
+    rt_hw_set_gtimer_val(timer_step);
+    rt_tick_increase();
+}
+
+void rt_hw_gtimer_init(void)
+{
+    rt_hw_interrupt_install(EL1_PHY_TIMER_IRQ_NUM, rt_hw_timer_isr, RT_NULL, "tick");
+    __ISB();
+    timer_step = rt_hw_get_gtimer_frq();
+    __DSB();
+    timer_step /= RT_TICK_PER_SECOND;
+    rt_hw_gtimer_local_enable();
+}
+
+void rt_hw_gtimer_local_enable(void)
+{
+    rt_hw_gtimer_disable();
+    rt_hw_set_gtimer_val(timer_step);
+    rt_hw_interrupt_umask(EL1_PHY_TIMER_IRQ_NUM);
+    rt_hw_gtimer_enable();
+}
+
+void rt_hw_gtimer_local_disable(void)
+{
+    rt_hw_gtimer_disable();
+    rt_hw_interrupt_mask(EL1_PHY_TIMER_IRQ_NUM);
+}
diff --git a/libcpu/aarch64/common/gtimer.h b/libcpu/aarch64/common/gtimer.h
new file mode 100644
index 0000000000..956e2ed5aa
--- /dev/null
+++ b/libcpu/aarch64/common/gtimer.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2011-12-20     GuEe-GUI     first version
+ */
+
+#ifndef __GTIMER_H__
+#define __GTIMER_H__
+
+#include <rtdef.h>
+
+void rt_hw_gtimer_init(void);
+void rt_hw_gtimer_local_enable(void);
+void rt_hw_gtimer_local_disable(void);
+
+void rt_hw_gtimer_enable();
+void rt_hw_gtimer_disable();
+void rt_hw_set_gtimer_val(rt_uint64_t value);
+rt_uint64_t rt_hw_get_gtimer_val();
+rt_uint64_t rt_hw_get_cntpct_val();
+rt_uint64_t rt_hw_get_gtimer_frq();
+
+#endif /* __GTIMER_H__ */
diff --git a/libcpu/aarch64/common/interrupt.c b/libcpu/aarch64/common/interrupt.c
index c4e3f20b46..895088c805 100644
--- a/libcpu/aarch64/common/interrupt.c
+++ b/libcpu/aarch64/common/interrupt.c
@@ -15,16 +15,15 @@
 #include "gic.h"
 #include "armv8.h"
 #include "mmu.h"
+#include "cpuport.h"
 
 /* exception and interrupt handler table */
 struct rt_irq_desc isr_table[MAX_HANDLERS];
 
-#ifndef RT_USING_SMP
 /* Those variables will be accessed in ISR, so we need to share them. */
 rt_ubase_t rt_interrupt_from_thread        = 0;
 rt_ubase_t rt_interrupt_to_thread          = 0;
 rt_ubase_t rt_thread_switch_interrupt_flag = 0;
-#endif
 
 const unsigned int VECTOR_BASE = 0x00;
 extern int system_vectors;
@@ -39,9 +38,9 @@ extern volatile rt_uint8_t rt_interrupt_nest;
 static void default_isr_handler(int vector, void *param)
 {
 #ifdef RT_USING_SMP
-    rt_kprintf("cpu %d unhandled irq: %d\n", rt_hw_cpu_id(),vector);
+    rt_kprintf("cpu %d unhandled irq: %d\n", rt_hw_cpu_id(), vector);
 #else
-    rt_kprintf("unhandled irq: %d\n",vector);
+    rt_kprintf("unhandled irq: %d\n", vector);
 #endif
 }
 #endif
@@ -138,7 +137,7 @@ void rt_hw_interrupt_mask(int vector)
 void rt_hw_interrupt_umask(int vector)
 {
 #ifndef BSP_USING_GIC
-if (vector < 32)
+    if (vector < 32)
     {
         IRQ_ENABLE1 = (1 << vector);
     }
@@ -397,7 +396,20 @@ void rt_hw_ipi_send(int ipi_vector, unsigned int cpu_mask)
 {
 #ifdef BSP_USING_GIC
     arm_gic_send_sgi(0, ipi_vector, cpu_mask, 0);
+#else
+    int i;
+
+    __DSB();
+
+    for (i = 0; i < RT_CPUS_NR; ++i)
+    {
+        if (cpu_mask & (1 << i))
+        {
+            IPI_MAILBOX_SET(i) = 1 << ipi_vector;
+        }
+    }
 #endif
+    __DSB();
 }
 
 void rt_hw_ipi_handler_install(int ipi_vector, rt_isr_handler_t ipi_isr_handler)
diff --git a/libcpu/aarch64/common/mmu.c b/libcpu/aarch64/common/mmu.c
index 2a41c4f813..59292c6db2 100644
--- a/libcpu/aarch64/common/mmu.c
+++ b/libcpu/aarch64/common/mmu.c
@@ -4,364 +4,248 @@
  * SPDX-License-Identifier: Apache-2.0
  *
  * Change Logs:
- * Date           Author             Notes
- * 2020-02-20     bigmagic           first version
+ * Date           Author       Notes
+ * 2021-11-28     GuEe-GUI     first version
  */
-#include <mmu.h>
-#include <stddef.h>
-#include <rthw.h>
-
-#define TTBR_CNP    1
-
-typedef unsigned long int    uint64_t;
-
-static unsigned long main_tbl[512 * 20] __attribute__((aligned (4096)));
-
-#define IS_ALIGNED(x, a)        (((x) & ((typeof(x))(a) - 1)) == 0)
-
-#define PMD_TYPE_SECT        (1 << 0)
 
-#define PMD_TYPE_TABLE        (3 << 0)
-
-#define PTE_TYPE_PAGE        (3 << 0)
-
-#define BITS_PER_VA          39
-
-/* Granule size of 4KB is being used */
-#define GRANULE_SIZE_SHIFT         12
-#define GRANULE_SIZE               (1 << GRANULE_SIZE_SHIFT)
-#define XLAT_ADDR_MASK             ((1UL << BITS_PER_VA) - GRANULE_SIZE)
-
-#define PMD_TYPE_MASK               (3 << 0)
-
-int free_idx = 1;
+#include <rtthread.h>
+#include <rthw.h>
 
-void __asm_invalidate_icache_all(void);
-void __asm_flush_dcache_all(void);
-int __asm_flush_l3_cache(void);
-void __asm_flush_dcache_range(unsigned long long start, unsigned long long end);
-void __asm_invalidate_dcache_all(void);
-void __asm_invalidate_icache_all(void);
+#include <cpuport.h>
+#include <mmu.h>
 
-void mmu_memset(char *dst, char v,  size_t len)
-{
-    while (len--)
-    {
-        *dst++ = v;
-    }
-}
+#define ARCH_SECTION_SHIFT  21
+#define ARCH_SECTION_SIZE   (1 << ARCH_SECTION_SHIFT)
+#define ARCH_SECTION_MASK   (ARCH_SECTION_SIZE - 1)
+#define ARCH_PAGE_SHIFT     12
+#define ARCH_PAGE_SIZE      (1 << ARCH_PAGE_SHIFT)
+#define ARCH_PAGE_MASK      (ARCH_PAGE_SIZE - 1)
 
-static unsigned long __page_off = 0;
-static unsigned long get_free_page(void)
-{
-    __page_off += 512;
-    return (unsigned long)(main_tbl + __page_off);
-}
+#define MMU_LEVEL_MASK      0x1ffUL
+#define MMU_LEVEL_SHIFT     9
+#define MMU_ADDRESS_BITS    39
+#define MMU_ADDRESS_MASK    0x0000fffffffff000UL
+#define MMU_ATTRIB_MASK     0xfff0000000000ffcUL
 
+#define MMU_TYPE_MASK       3UL
+#define MMU_TYPE_USED       1UL
+#define MMU_TYPE_BLOCK      1UL
+#define MMU_TYPE_TABLE      3UL
+#define MMU_TYPE_PAGE       3UL
 
-static inline unsigned int get_sctlr(void)
-{
-    unsigned int val;
-    asm volatile("mrs %0, sctlr_el1" : "=r" (val) : : "cc");
-    return val;
-}
+#define MMU_TBL_BLOCK_2M_LEVEL  2
+#define MMU_TBL_PAGE_NR_MAX     32
 
-static inline void set_sctlr(unsigned int val)
+/* only map 4G io/memory */
+static volatile unsigned long MMUTable[512] __attribute__((aligned(4096)));
+static volatile struct
 {
-    asm volatile("msr sctlr_el1, %0" : : "r" (val) : "cc");
-    asm volatile("isb");
-}
+    unsigned long entry[512];
+} MMUPage[MMU_TBL_PAGE_NR_MAX] __attribute__((aligned(4096)));
 
-void mmu_init(void)
+static unsigned long _kernel_free_page(void)
 {
-    unsigned long val64;
-    unsigned long val32;
-
-    val64 = 0x007f6eUL;
-    __asm__ volatile("msr MAIR_EL1, %0\n dsb sy\n"::"r"(val64));
-    __asm__ volatile("mrs %0, MAIR_EL1\n dsb sy\n":"=r"(val64));
-
-    //TCR_EL1
-    val32 = (16UL << 0)//48bit
-        | (0x0UL << 6)
-        | (0x0UL << 7)
-        | (0x3UL << 8)
-        | (0x3UL << 10)//Inner Shareable
-        | (0x2UL << 12)
-        | (0x0UL << 14)//4K
-        | (0x0UL << 16)
-        | (0x0UL << 22)
-        | (0x1UL << 23)
-        | (0x2UL << 30)
-        | (0x1UL << 32)
-        | (0x0UL << 35)
-        | (0x0UL << 36)
-        | (0x0UL << 37)
-        | (0x0UL << 38);
-    __asm__ volatile("msr TCR_EL1, %0\n"::"r"(val32));
-    __asm__ volatile("mrs %0, TCR_EL1\n":"=r"(val32));
-
-    __asm__ volatile("msr TTBR0_EL1, %0\n dsb sy\n"::"r"(main_tbl));
-    __asm__ volatile("mrs %0, TTBR0_EL1\n dsb sy\n":"=r"(val64));
+    static unsigned long i = 0;
 
-    mmu_memset((char *)main_tbl, 0, 4096);
-}
-
-void mmu_enable(void)
-{
-    unsigned long val64;
-    unsigned long val32;
-
-    __asm__ volatile("mrs %0, SCTLR_EL1\n":"=r"(val64));
-    val64 &= ~0x1000; //disable I
-    __asm__ volatile("dmb sy\n msr SCTLR_EL1, %0\n isb sy\n"::"r"(val64));
-
-    __asm__ volatile("IC IALLUIS\n dsb sy\n isb sy\n");
-    __asm__ volatile("tlbi vmalle1\n dsb sy\n isb sy\n");
+    if (i >= MMU_TBL_PAGE_NR_MAX)
+    {
+        return RT_NULL;
+    }
 
-    //SCTLR_EL1, turn on mmu
-    __asm__ volatile("mrs %0, SCTLR_EL1\n":"=r"(val32));
-    val32 |= 0x1005; //enable mmu, I C M
-    __asm__ volatile("dmb sy\n msr SCTLR_EL1, %0\nisb sy\n"::"r"(val32));
-    rt_hw_icache_enable();
-    rt_hw_dcache_enable();
+    ++i;
 
+    return (unsigned long)&MMUPage[i - 1].entry;
 }
 
-static int map_single_page_2M(unsigned long* lv0_tbl, unsigned long va, unsigned long pa, unsigned long attr)
+static int _kenrel_map_2M(unsigned long *tbl, unsigned long va, unsigned long pa, unsigned long attr)
 {
     int level;
-    unsigned long* cur_lv_tbl = lv0_tbl;
+    unsigned long *cur_lv_tbl = tbl;
     unsigned long page;
     unsigned long off;
-    int level_shift = 39;
+    int level_shift = MMU_ADDRESS_BITS;
 
-    if (va & (0x200000UL - 1))
+    if (va & ARCH_SECTION_MASK)
     {
         return MMU_MAP_ERROR_VANOTALIGN;
     }
-    if (pa & (0x200000UL - 1))
+    if (pa & ARCH_SECTION_MASK)
     {
         return MMU_MAP_ERROR_PANOTALIGN;
     }
-    for (level = 0; level < 2; level++)
+
+    for (level = 0; level < MMU_TBL_BLOCK_2M_LEVEL; ++level)
     {
         off = (va >> level_shift);
         off &= MMU_LEVEL_MASK;
-        if ((cur_lv_tbl[off] & 1) == 0)
+
+        if (!(cur_lv_tbl[off] & MMU_TYPE_USED))
         {
-            page = get_free_page();
+            page = _kernel_free_page();
+
             if (!page)
             {
                 return MMU_MAP_ERROR_NOPAGE;
             }
-            mmu_memset((char *)page, 0, 4096);
-            cur_lv_tbl[off] = page | 0x3UL;
+
+            rt_memset((char *)page, 0, ARCH_PAGE_SIZE);
+            rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, (void *)page, ARCH_PAGE_SIZE);
+            cur_lv_tbl[off] = page | MMU_TYPE_TABLE;
+            rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, cur_lv_tbl + off, sizeof(void *));
+        }
+        else
+        {
+            page = cur_lv_tbl[off];
+            page &= MMU_ADDRESS_MASK;
         }
+
         page = cur_lv_tbl[off];
-        if (!(page & 0x2))
+        if ((page & MMU_TYPE_MASK) == MMU_TYPE_BLOCK)
         {
-            //is block! error!
+            /* is block! error! */
             return MMU_MAP_ERROR_CONFLICT;
         }
-        cur_lv_tbl = (unsigned long*)(page & 0x0000fffffffff000UL);
-        level_shift -= 9;
+
+        /* next level */
+        cur_lv_tbl = (unsigned long *)(page & MMU_ADDRESS_MASK);
+        level_shift -= MMU_LEVEL_SHIFT;
     }
-    attr &= 0xfff0000000000ffcUL;
-    pa |= (attr | 0x1UL); //block
-    off = (va >> 21);
+
+    attr &= MMU_ATTRIB_MASK;
+    pa |= (attr | MMU_TYPE_BLOCK);
+    off = (va >> ARCH_SECTION_SHIFT);
     off &= MMU_LEVEL_MASK;
     cur_lv_tbl[off] = pa;
+    rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, cur_lv_tbl + off, sizeof(void *));
+
     return 0;
 }
 
-int armv8_map_2M(unsigned long va, unsigned long pa, int count, unsigned long attr)
+int rt_hw_mmu_setmtt(unsigned long vaddr_start, unsigned long vaddr_end,
+        unsigned long paddr_start, unsigned long attr)
 {
+    int ret = -1;
     int i;
-    int ret;
+    unsigned long count;
+    unsigned long map_attr = MMU_MAP_CUSTOM(MMU_AP_KAUN, attr);
 
-    if (va & (0x200000 - 1))
+    if (vaddr_start > vaddr_end)
+    {
+        goto end;
+    }
+    if (vaddr_start % ARCH_SECTION_SIZE)
+    {
+        vaddr_start = (vaddr_start / ARCH_SECTION_SIZE) * ARCH_SECTION_SIZE;
+    }
+    if (paddr_start % ARCH_SECTION_SIZE)
     {
-        return -1;
+        paddr_start = (paddr_start / ARCH_SECTION_SIZE) * ARCH_SECTION_SIZE;
     }
-    if (pa & (0x200000 - 1))
+    if (vaddr_end % ARCH_SECTION_SIZE)
     {
-        return -1;
+        vaddr_end = (vaddr_end / ARCH_SECTION_SIZE + 1) * ARCH_SECTION_SIZE;
     }
+
+    count = (vaddr_end - vaddr_start) >> ARCH_SECTION_SHIFT;
+
     for (i = 0; i < count; i++)
     {
-        ret = map_single_page_2M((unsigned long *)main_tbl, va, pa, attr);
-        va += 0x200000;
-        pa += 0x200000;
+        ret = _kenrel_map_2M((void *)MMUTable, vaddr_start, paddr_start, map_attr);
+        vaddr_start += ARCH_SECTION_SIZE;
+        paddr_start += ARCH_SECTION_SIZE;
+
         if (ret != 0)
         {
-            return ret;
+            goto end;
         }
     }
-    return 0;
-}
 
-static void set_table(uint64_t *pt, uint64_t *table_addr)
-{
-    uint64_t val;
-    val = (0x3UL | (uint64_t)table_addr);
-    *pt = val;
+end:
+    return ret;
 }
 
-void mmu_memset2(unsigned char *dst, char v,  int len)
+void rt_hw_init_mmu_table(struct mem_desc *mdesc, rt_size_t desc_nr)
 {
-    while (len--)
+    rt_memset((void *)MMUTable, 0, sizeof(MMUTable));
+    rt_memset((void *)MMUPage, 0, sizeof(MMUPage));
+
+    /* set page table */
+    for (; desc_nr > 0; --desc_nr)
     {
-        *dst++ = v;
+        rt_hw_mmu_setmtt(mdesc->vaddr_start, mdesc->vaddr_end, mdesc->paddr_start, mdesc->attr);
+        ++mdesc;
     }
-}
 
-static uint64_t *create_table(void)
-{
-    uint64_t *new_table = (uint64_t *)((unsigned char *)&main_tbl[0] + free_idx * 4096); //+ free_idx * GRANULE_SIZE;
-    /* Mark all entries as invalid */
-    mmu_memset2((unsigned char *)new_table, 0, 4096);
-    free_idx++;
-    return new_table;
+    rt_hw_dcache_flush_range((unsigned long)MMUTable, sizeof(MMUTable));
 }
 
-static int pte_type(uint64_t *pte)
+void rt_hw_mmu_tlb_invalidate(void)
 {
-    return *pte & PMD_TYPE_MASK;
+    __asm__ volatile (
+        "tlbi vmalle1\n\r"
+        "dsb sy\n\r"
+        "isb sy\n\r"
+        "ic ialluis\n\r"
+        "dsb sy\n\r"
+        "isb sy");
 }
 
-static int level2shift(int level)
+void rt_hw_mmu_init(void)
 {
-    /* Page is 12 bits wide, every level translates 9 bits */
-    return (12 + 9 * (3 - level));
-}
+    unsigned long reg_val;
 
-static uint64_t *get_level_table(uint64_t *pte)
-{
-    uint64_t *table = (uint64_t *)(*pte & XLAT_ADDR_MASK);
+    reg_val = 0x00447fUL;
+    __asm__ volatile("msr mair_el1, %0"::"r"(reg_val));
 
-    if (pte_type(pte) != PMD_TYPE_TABLE)
-    {
-        table = create_table();
-        set_table(pte, table);
-    }
-    return table;
-}
+    rt_hw_isb();
 
-static void map_region(uint64_t virt, uint64_t phys, uint64_t size, uint64_t attr)
-{
-    uint64_t block_size = 0;
-    uint64_t block_shift = 0;
-    uint64_t *pte;
-    uint64_t idx = 0;
-    uint64_t addr = 0;
-    uint64_t *table = 0;
-    int level = 0;
+    reg_val = (16UL << 0)   /* t0sz 48bit */
+            | (0UL  << 6)   /* reserved */
+            | (0UL  << 7)   /* epd0 */
+            | (3UL  << 8)   /* t0 wb cacheable */
+            | (3UL  << 10)  /* inner shareable */
+            | (2UL  << 12)  /* t0 outer shareable */
+            | (0UL  << 14)  /* t0 4K */
+            | (16UL << 16)  /* t1sz 48bit */
+            | (0UL  << 22)  /* define asid use ttbr0.asid */
+            | (0UL  << 23)  /* epd1 */
+            | (3UL  << 24)  /* t1 inner wb cacheable */
+            | (3UL  << 26)  /* t1 outer wb cacheable */
+            | (2UL  << 28)  /* t1 outer shareable */
+            | (2UL  << 30)  /* t1 4k */
+            | (1UL  << 32)  /* 001b 64GB PA */
+            | (0UL  << 35)  /* reserved */
+            | (1UL  << 36)  /* as: 0:8bit 1:16bit */
+            | (0UL  << 37)  /* tbi0 */
+            | (0UL  << 38); /* tbi1 */
+    __asm__ volatile("msr tcr_el1, %0"::"r"(reg_val));
 
-    addr = virt;
-    while (size)
-    {
-        table = &main_tbl[0];
-        for (level = 0; level < 4; level++)
-        {
-            block_shift = level2shift(level);
-            idx = addr >> block_shift;
-            idx = idx%512;
-            block_size = (uint64_t)(1L << block_shift);
-            pte = table + idx;
+    rt_hw_isb();
 
-            if (size >= block_size && IS_ALIGNED(addr, block_size))
-            {
-                attr &= 0xfff0000000000ffcUL;
-                if(level != 3)
-                {
-                    *pte = phys | (attr | 0x1UL);
-                }
-                else
-                {
-                    *pte = phys | (attr | 0x3UL);
-                }
-                addr += block_size;
-                phys += block_size;
-                size -= block_size;
-                break;
-            }
-            table = get_level_table(pte);
-        }
-    }
-}
+    __asm__ volatile ("mrs %0, sctlr_el1":"=r"(reg_val));
 
-void armv8_map(unsigned long va, unsigned long pa, unsigned long size, unsigned long attr)
-{
-    map_region(va, pa, size, attr);
-}
+    reg_val |= 1 << 2;  /* enable dcache */
+    reg_val |= 1 << 0;  /* enable mmu */
 
-void rt_hw_dcache_enable(void)
-{
-    if (!(get_sctlr() & CR_M))
-    {
-        rt_kprintf("please init mmu!\n");
-    }
-    else
-    {
-        set_sctlr(get_sctlr() | CR_C);
-    }
-}
-
-void rt_hw_dcache_flush_all(void)
-{
-    int ret;
+    __asm__ volatile (
+        "msr ttbr0_el1, %0\n\r"
+        "msr sctlr_el1, %1\n\r"
+        "dsb sy\n\r"
+        "isb sy\n\r"
+        ::"r"(MMUTable), "r"(reg_val) :"memory");
 
-    __asm_flush_dcache_all();
-    ret = __asm_flush_l3_cache();
-    if (ret)
-    {
-        rt_kprintf("flushing dcache returns 0x%x\n", ret);
-    }
-    else
-    {
-        rt_kprintf("flushing dcache successfully.\n");
-    }
-}
-
-void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size)
-{
-    __asm_flush_dcache_range(start_addr, start_addr + size);
-}
-void rt_hw_dcache_invalidate_range(unsigned long start_addr,unsigned long size)
-{
-    __asm_flush_dcache_range(start_addr, start_addr + size);
+    rt_hw_mmu_tlb_invalidate();
 }
 
-void rt_hw_dcache_invalidate_all(void)
+int rt_hw_mmu_map(unsigned long addr, unsigned long size, unsigned long attr)
 {
-    __asm_invalidate_dcache_all();
-}
-
-void rt_hw_dcache_disable(void)
-{
-    /* if cache isn't enabled no need to disable */
-    if(!(get_sctlr() & CR_C))
-    {
-        rt_kprintf("need enable cache!\n");
-        return;
-    }
-    set_sctlr(get_sctlr() & ~CR_C);
-}
+    int ret;
+    rt_ubase_t level;
 
-//icache
-void rt_hw_icache_enable(void)
-{
-    __asm_invalidate_icache_all();
-    set_sctlr(get_sctlr() | CR_I);
-}
+    level = rt_hw_interrupt_disable();
+    ret = rt_hw_mmu_setmtt(addr, addr + size, addr, attr);
 
-void rt_hw_icache_invalidate_all(void)
-{
-    __asm_invalidate_icache_all();
-}
+    rt_hw_interrupt_enable(level);
 
-void rt_hw_icache_disable(void)
-{
-    set_sctlr(get_sctlr() & ~CR_I);
+    return ret;
 }
diff --git a/libcpu/aarch64/common/mmu.h b/libcpu/aarch64/common/mmu.h
index f3261c3a1d..c2837a7dce 100644
--- a/libcpu/aarch64/common/mmu.h
+++ b/libcpu/aarch64/common/mmu.h
@@ -4,75 +4,71 @@
  * SPDX-License-Identifier: Apache-2.0
  *
  * Change Logs:
- * Date           Author             Notes
- * 2020-02-20     bigmagic           first version
+ * Date           Author       Notes
+ * 2021-11-28     GuEe-GUI     the first version
  */
 
-#ifndef  __MMU_H__
-#define  __MMU_H__
+#ifndef __MMU_H_
+#define __MMU_H_
 
-/*
- * CR1 bits (CP#15 CR1)
- */
-#define CR_M    (1 << 0)    /* MMU enable                */
-#define CR_A    (1 << 1)    /* Alignment abort enable        */
-#define CR_C    (1 << 2)    /* Dcache enable            */
-#define CR_W    (1 << 3)    /* Write buffer enable            */
-#define CR_P    (1 << 4)    /* 32-bit exception handler        */
-#define CR_D    (1 << 5)    /* 32-bit data address range        */
-#define CR_L    (1 << 6)    /* Implementation defined        */
-#define CR_B    (1 << 7)    /* Big endian                */
-#define CR_S    (1 << 8)    /* System MMU protection        */
-#define CR_R    (1 << 9)    /* ROM MMU protection            */
-#define CR_F    (1 << 10)    /* Implementation defined        */
-#define CR_Z    (1 << 11)    /* Implementation defined        */
-#define CR_I    (1 << 12)    /* Icache enable            */
-#define CR_V    (1 << 13)    /* Vectors relocated to 0xffff0000    */
-#define CR_RR   (1 << 14)    /* Round Robin cache replacement    */
-#define CR_L4   (1 << 15)    /* LDR pc can set T bit            */
-#define CR_DT   (1 << 16)
-#define CR_IT   (1 << 18)
-#define CR_ST   (1 << 19)
-#define CR_FI   (1 << 21)    /* Fast interrupt (lower latency mode)    */
-#define CR_U    (1 << 22)    /* Unaligned access operation        */
-#define CR_XP   (1 << 23)    /* Extended page tables            */
-#define CR_VE   (1 << 24)    /* Vectored interrupts            */
-#define CR_EE   (1 << 25)    /* Exception (Big) Endian        */
-#define CR_TRE  (1 << 28)    /* TEX remap enable            */
-#define CR_AFE  (1 << 29)    /* Access flag enable            */
-#define CR_TE   (1 << 30)    /* Thumb exception enable        */
+#include <rtthread.h>
 
-#define MMU_LEVEL_MASK 0x1ffUL
-#define MMU_MAP_ERROR_VANOTALIGN  -1
-#define MMU_MAP_ERROR_PANOTALIGN  -2
-#define MMU_MAP_ERROR_NOPAGE      -3
-#define MMU_MAP_ERROR_CONFLICT    -4
+/* normal memory wra mapping type */
+#define NORMAL_MEM          0
+/* normal nocache memory mapping type */
+#define NORMAL_NOCACHE_MEM  1
+/* device mapping type */
+#define DEVICE_MEM          2
 
-#define MEM_ATTR_MEMORY ((0x1UL << 10) | (0x2UL << 8) | (0x0UL << 6) | (0x1UL << 2))
-#define MEM_ATTR_IO     ((0x1UL << 10) | (0x2UL << 8) | (0x0UL << 6) | (0x2UL << 2))
+#define MMU_MAP_ERROR_VANOTALIGN    (-1)
+#define MMU_MAP_ERROR_PANOTALIGN    (-2)
+#define MMU_MAP_ERROR_NOPAGE        (-3)
+#define MMU_MAP_ERROR_CONFLICT      (-4)
 
-#define BUS_ADDRESS(phys)    (((phys) & ~0xC0000000)  |  0xC0000000)
+struct mem_desc
+{
+    unsigned long vaddr_start;
+    unsigned long vaddr_end;
+    unsigned long paddr_start;
+    unsigned long attr;
+};
 
-void mmu_init(void);
+#define MMU_AF_SHIFT        10
+#define MMU_SHARED_SHIFT    8
+#define MMU_AP_SHIFT        6
+#define MMU_MA_SHIFT        2
 
-void mmu_enable(void);
+#define MMU_AP_KAUN         0UL /* kernel r/w, user none */
+#define MMU_AP_KAUA         1UL /* kernel r/w, user r/w */
+#define MMU_AP_KRUN         2UL /* kernel r, user none */
+#define MMU_AP_KRUR         3UL /* kernel r, user r */
 
-int armv8_map_2M(unsigned long va, unsigned long pa, int count, unsigned long attr);
+#define MMU_MAP_CUSTOM(ap, mtype) \
+(\
+    (0x1UL << MMU_AF_SHIFT) |\
+    (0x2UL << MMU_SHARED_SHIFT) |\
+    ((ap) << MMU_AP_SHIFT) |\
+    ((mtype) << MMU_MA_SHIFT)\
+)
+#define MMU_MAP_K_RO        MMU_MAP_CUSTOM(MMU_AP_KRUN, NORMAL_MEM)
+#define MMU_MAP_K_RWCB      MMU_MAP_CUSTOM(MMU_AP_KAUN, NORMAL_MEM)
+#define MMU_MAP_K_RW        MMU_MAP_CUSTOM(MMU_AP_KAUN, NORMAL_NOCACHE_MEM)
+#define MMU_MAP_K_DEVICE    MMU_MAP_CUSTOM(MMU_AP_KAUN, DEVICE_MEM)
+#define MMU_MAP_U_RO        MMU_MAP_CUSTOM(MMU_AP_KRUR, NORMAL_NOCACHE_MEM)
+#define MMU_MAP_U_RWCB      MMU_MAP_CUSTOM(MMU_AP_KAUA, NORMAL_MEM)
+#define MMU_MAP_U_RW        MMU_MAP_CUSTOM(MMU_AP_KAUA, NORMAL_NOCACHE_MEM)
+#define MMU_MAP_U_DEVICE    MMU_MAP_CUSTOM(MMU_AP_KAUA, DEVICE_MEM)
 
-void armv8_map(unsigned long va, unsigned long pa, unsigned long size, unsigned long attr);
+void rt_hw_init_mmu_table(struct mem_desc *mdesc, rt_size_t desc_nr);
+void rt_hw_mmu_init(void);
+int rt_hw_mmu_map(unsigned long addr, unsigned long size, unsigned long attr);
 
-//dcache
-void rt_hw_dcache_enable(void);
 void rt_hw_dcache_flush_all(void);
+void rt_hw_dcache_invalidate_all(void);
 void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size);
 void rt_hw_dcache_invalidate_range(unsigned long start_addr,unsigned long size);
-void rt_hw_dcache_invalidate_all(void);
-void rt_hw_dcache_disable(void);
-
-//icache
-void rt_hw_icache_enable(void);
-void rt_hw_icache_invalidate_all(void);
-void rt_hw_icache_disable(void);
 
+void rt_hw_icache_invalidate_all();
+void rt_hw_icache_invalidate_range(unsigned long start_addr, int size);
 
-#endif  /*__MMU_H__*/
+#endif /* __MMU_H_ */
diff --git a/libcpu/aarch64/common/psci.c b/libcpu/aarch64/common/psci.c
new file mode 100644
index 0000000000..400db977ee
--- /dev/null
+++ b/libcpu/aarch64/common/psci.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-09-09     GuEe-GUI     The first version
+ */
+
+#include <psci.h>
+#include <smccc.h>
+#include <armv8.h>
+
+typedef uint64_t (*psci_call_handle)(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2);
+
+static uint64_t psci_smc_call(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2)
+{
+    return arm_smc_call(fn, arg0, arg1, arg2, 0, 0, 0, 0).x0;
+}
+
+static uint64_t psci_hvc_call(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2)
+{
+    return arm_hvc_call(fn, arg0, arg1, arg2, 0, 0, 0, 0).x0;
+}
+
+static psci_call_handle psci_call = psci_smc_call;
+
+static uint64_t shutdown_args[3] = {0, 0, 0};
+static uint64_t reboot_args[3] = {0, 0, 0};
+
+void arm_psci_init(uint64_t *platform_shutdown_args, uint64_t *platform_reboot_args)
+{
+    if (rt_hw_get_current_el() < 2)
+    {
+        psci_call = psci_hvc_call;
+    }
+
+    if (platform_shutdown_args != RT_NULL)
+    {
+        shutdown_args[0] = platform_shutdown_args[0];
+        shutdown_args[1] = platform_shutdown_args[1];
+        shutdown_args[2] = platform_shutdown_args[2];
+    }
+
+    if (platform_reboot_args != RT_NULL)
+    {
+        reboot_args[0] = platform_reboot_args[0];
+        reboot_args[1] = platform_reboot_args[1];
+        reboot_args[2] = platform_reboot_args[2];
+    }
+}
+
+uint32_t arm_psci_get_version()
+{
+    return (uint32_t)psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
+}
+
+uint32_t arm_psci_get_affinity_info(uint64_t target_affinity, uint64_t lowest_affinity_level)
+{
+    return (uint32_t)psci_call(PSCI_0_2_FN_AFFINITY_INFO, target_affinity, lowest_affinity_level, 0);
+}
+
+uint32_t arm_psci_get_feature(uint32_t psci_func_id)
+{
+    return (uint32_t)psci_call(PSCI_1_0_FN_PSCI_FEATURES, psci_func_id, 0, 0);
+}
+
+uint32_t arm_psci_cpu_off(uint64_t state)
+{
+    return (uint32_t)psci_call(PSCI_0_2_FN_CPU_OFF, state, 0, 0);
+}
+
+uint32_t arm_psci_cpu_on(uint64_t mpid, uint64_t entry)
+{
+    /* [40:63] and [24:31] must be zero, other is aff3, aff2, aff1, aff0 */
+    mpid = mpid & 0xff00ffffff;
+
+    return (uint32_t)psci_call(PSCI_0_2_FN_CPU_ON, mpid, entry, 0);
+}
+
+uint32_t arm_psci_cpu_suspend(uint32_t power_state, uint64_t entry)
+{
+    return (uint32_t)psci_call(PSCI_0_2_FN_CPU_SUSPEND, power_state, entry, 0);
+}
+
+void arm_psci_system_off()
+{
+    psci_call(PSCI_0_2_FN_SYSTEM_OFF, shutdown_args[0], shutdown_args[1], shutdown_args[2]);
+}
+
+void arm_psci_system_reboot()
+{
+    psci_call(PSCI_0_2_FN_SYSTEM_RESET, reboot_args[0], reboot_args[1], reboot_args[2]);
+}
diff --git a/libcpu/aarch64/common/psci.h b/libcpu/aarch64/common/psci.h
new file mode 100644
index 0000000000..e8b2e3129d
--- /dev/null
+++ b/libcpu/aarch64/common/psci.h
@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-09-09     GuEe-GUI     The first version
+ */
+
+#ifndef __PSCI_H__
+#define __PSCI_H__
+
+#include <stdint.h>
+
+/*
+ * Non-Confidential PSCI 1.0 release (30 January 2015), and errata fix for PSCI 0.2, unsupport PSCI 0.1
+ */
+#define PSCI_VER_0_2                        0x00000002
+
+/* PSCI 0.2 interface */
+#define PSCI_0_2_FN_BASE                    0x84000000
+#define PSCI_0_2_FN(n)                      (PSCI_0_2_FN_BASE + (n))
+#define PSCI_0_2_FN_END                     0x8400001F
+
+#define PSCI_0_2_FN64_BASE                  0xC4000000
+#define PSCI_0_2_FN64(n)                    (PSCI_0_2_FN64_BASE + (n))
+#define PSCI_0_2_FN64_END                   0xC400001F
+
+#define PSCI_0_2_FN_PSCI_VERSION            PSCI_0_2_FN(0)
+#define PSCI_0_2_FN_CPU_SUSPEND             PSCI_0_2_FN(1)
+#define PSCI_0_2_FN_CPU_OFF                 PSCI_0_2_FN(2)
+#define PSCI_0_2_FN_CPU_ON                  PSCI_0_2_FN(3)
+#define PSCI_0_2_FN_AFFINITY_INFO           PSCI_0_2_FN(4)
+#define PSCI_0_2_FN_MIGRATE                 PSCI_0_2_FN(5)
+#define PSCI_0_2_FN_MIGRATE_INFO_TYPE       PSCI_0_2_FN(6)
+#define PSCI_0_2_FN_MIGRATE_INFO_UP_CPU     PSCI_0_2_FN(7)
+#define PSCI_0_2_FN_SYSTEM_OFF              PSCI_0_2_FN(8)
+#define PSCI_0_2_FN_SYSTEM_RESET            PSCI_0_2_FN(9)
+
+#define PSCI_0_2_FN64_CPU_SUSPEND           PSCI_0_2_FN64(1)
+#define PSCI_0_2_FN64_CPU_ON                PSCI_0_2_FN64(3)
+#define PSCI_0_2_FN64_AFFINITY_INFO         PSCI_0_2_FN64(4)
+#define PSCI_0_2_FN64_MIGRATE               PSCI_0_2_FN64(5)
+#define PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU   PSCI_0_2_FN64(7)
+
+/* PSCI 1.0 interface */
+#define PSCI_1_0_FN_PSCI_FEATURES           PSCI_0_2_FN(10)
+#define PSCI_1_0_FN_CPU_FREEZE              PSCI_0_2_FN(11)
+#define PSCI_1_0_FN_CPU_DEFAULT_SUSPEND     PSCI_0_2_FN(12)
+#define PSCI_1_0_FN_NODE_HW_STATE           PSCI_0_2_FN(13)
+#define PSCI_1_0_FN_SYSTEM_SUSPEND          PSCI_0_2_FN(14)
+#define PSCI_1_0_FN_SET_SUSPEND_MODE        PSCI_0_2_FN(15)
+#define PSCI_1_0_FN_STAT_RESIDENCY          PSCI_0_2_FN(16)
+#define PSCI_1_0_FN_STAT_COUNT              PSCI_0_2_FN(17)
+
+#define PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND   PSCI_0_2_FN64(12)
+#define PSCI_1_0_FN64_NODE_HW_STATE         PSCI_0_2_FN64(13)
+#define PSCI_1_0_FN64_SYSTEM_SUSPEND        PSCI_0_2_FN64(14)
+#define PSCI_1_0_FN64_STAT_RESIDENCY        PSCI_0_2_FN64(16)
+#define PSCI_1_0_FN64_STAT_COUNT            PSCI_0_2_FN64(17)
+
+/* 1KB stack per core */
+#define PSCI_STACK_SHIFT                    10
+#define PSCI_STACK_SIZE                     (1 << PSCI_STACK_SHIFT)
+
+/* PSCI affinity level state returned by AFFINITY_INFO */
+#define PSCI_AFFINITY_LEVEL_ON              0
+#define PSCI_AFFINITY_LEVEL_OFF             1
+#define PSCI_AFFINITY_LEVEL_ON_PENDING      2
+
+/*
+ * PSCI power state
+ *  power_level:
+ *      Level 0: cores
+ *      Level 1: clusters
+ *      Level 2: system
+ *  state_type:
+ *      value 0: standby or retention state
+ *      value 1: powerdown state(entry and context_id is valid)
+ *  state_id:
+ *      StateID
+ */
+#define PSCI_POWER_STATE(power_level, state_type, state_id) \
+( \
+    ((power_level) << 24) | \
+    ((state_type) << 16)  | \
+    ((state_id) << 24) \
+)
+
+/*
+ * For system, cluster, core
+ *  0: run
+ *  1: standby(only core)
+ *  2: retention
+ *  3: powerdown
+ */
+#define PSCI_POWER_STATE_ID(state_id_power_level, system, cluster, core) \
+( \
+    ((state_id_power_level) << 12) | \
+    ((system) << 8)  | \
+    ((cluster) << 4) | \
+    (core) \
+)
+
+#define PSCI_RET_SUCCESS                    0
+#define PSCI_RET_NOT_SUPPORTED              (-1)
+#define PSCI_RET_INVALID_PARAMETERS         (-2)
+#define PSCI_RET_DENIED                     (-3)
+#define PSCI_RET_ALREADY_ON                 (-4)
+#define PSCI_RET_ON_PENDING                 (-5)
+#define PSCI_RET_INTERNAL_FAILURE           (-6)
+#define PSCI_RET_NOT_PRESENT                (-7)
+#define PSCI_RET_DISABLED                   (-8)
+#define PSCI_RET_INVALID_ADDRESS            (-9)
+
+void arm_psci_init(uint64_t *platform_shutdown_args, uint64_t *platform_reboot_args);
+
+uint32_t arm_psci_get_version();
+uint32_t arm_psci_get_affinity_info(uint64_t target_affinity, uint64_t lowest_affinity_level);
+uint32_t arm_psci_get_feature(uint32_t psci_func_id);
+
+uint32_t arm_psci_cpu_off(uint64_t state);
+uint32_t arm_psci_cpu_on(uint64_t mpid, uint64_t entry);
+uint32_t arm_psci_cpu_suspend(uint32_t power_state, uint64_t entry);
+
+void arm_psci_system_off();
+void arm_psci_system_reboot();
+
+#endif  /*__PSCI_H__*/
diff --git a/libcpu/aarch64/common/smccc.S b/libcpu/aarch64/common/smccc.S
new file mode 100644
index 0000000000..501d210c16
--- /dev/null
+++ b/libcpu/aarch64/common/smccc.S
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-09-09     GuEe-GUI     The first version
+ */
+
+/*
+ * smc calling convention call
+ */
+.macro SMCCC_CALL INS
+    stp     x8, x29, [sp,#-16]! /* push the frame pointer (x29) for the purposes of AAPCS64 compatibility */
+    \INS    #0
+    ldp     x8, x29, [sp], #16
+
+    stp     x0, x1, [x8]
+    stp     x2, x3, [x8, #16]
+    str     x6, [x8, #32]
+    ret
+.endm
+
+/*
+ * smc call
+ */
+.globl arm_smc_call
+arm_smc_call:
+    SMCCC_CALL smc
+
+/*
+ * hvc call
+ */
+.globl arm_hvc_call
+arm_hvc_call:
+    SMCCC_CALL hvc
diff --git a/libcpu/aarch64/common/smccc.h b/libcpu/aarch64/common/smccc.h
new file mode 100644
index 0000000000..9a84fd1452
--- /dev/null
+++ b/libcpu/aarch64/common/smccc.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-09-09     GuEe-GUI     The first version
+ */
+
+#include <stdint.h>
+
+/*
+ * The ARM SMCCC v1.0 calling convention provides the following guarantees about registers:
+ *  Register     Modified    Return State
+ *  X0...X3      Yes         Result values
+ *  X4...X17     Yes         Unpredictable
+ *  X18...X30    No          Preserved
+ *  SP_EL0       No          Preserved
+ *  SP_ELx       No          Preserved
+ */
+
+struct arm_smccc_ret
+{
+    uint64_t x0;    /* Parameter registers */
+    uint64_t x1;    /* Parameter registers */
+    uint64_t x2;    /* Parameter registers */
+    uint64_t x3;    /* Parameter registers */
+    uint64_t x6;    /* Parameter register: Optional Session ID register */
+};
+
+struct arm_smccc_ret arm_smc_call(uint32_t w0, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4, uint64_t x5, uint64_t x6, uint32_t w7);
+struct arm_smccc_ret arm_hvc_call(uint32_t w0, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4, uint64_t x5, uint64_t x6, uint32_t w7);
diff --git a/libcpu/aarch64/common/stack.c b/libcpu/aarch64/common/stack.c
index d93706fcb9..a0ec935e5d 100644
--- a/libcpu/aarch64/common/stack.c
+++ b/libcpu/aarch64/common/stack.c
@@ -8,10 +8,9 @@
  * 2011-09-23     Bernard      the first version
  * 2011-10-05     Bernard      add thumb mode
  * 2021-11-04     GuEe-GUI     set sp with SP_ELx
+ * 2021-12-28     GuEe-GUI     add fpu support
  */
 #include <rtthread.h>
-#include <board.h>
-
 #include <armv8.h>
 
 #define INITIAL_SPSR_EL3 (PSTATE_EL3 | SP_ELx)
@@ -35,6 +34,39 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_ad
 
     stk      = (rt_ubase_t*)stack_addr;
 
+    *(--stk) = (rt_ubase_t) 0;              /* Q0 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q0 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q1 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q1 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q2 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q2 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q3 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q3 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q4 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q4 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q5 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q5 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q6 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q6 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q7 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q7 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q8 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q8 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q9 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q9 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q10 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q10 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q11 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q11 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q12 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q12 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q13 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q13 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q14 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q14 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q15 */
+    *(--stk) = (rt_ubase_t) 0;              /* Q15 */
+
     *(--stk) = ( rt_ubase_t ) 11;           /* X1 */
     *(--stk) = ( rt_ubase_t ) parameter;    /* X0 */
     *(--stk) = ( rt_ubase_t ) 33;           /* X3 */
@@ -65,6 +97,8 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_ad
     *(--stk) = ( rt_ubase_t ) 26;           /* X26 */
     *(--stk) = ( rt_ubase_t ) 29;           /* X29 */
     *(--stk) = ( rt_ubase_t ) 28;           /* X28 */
+    *(--stk) = ( rt_ubase_t ) 0;            /* FPSR */
+    *(--stk) = ( rt_ubase_t ) 0;            /* FPCR */
     *(--stk) = ( rt_ubase_t ) 0;            /* XZR - has no effect, used so there are an even number of registers. */
     *(--stk) = ( rt_ubase_t ) texit;        /* X30 - procedure call link register. */
 
diff --git a/libcpu/aarch64/common/trap.c b/libcpu/aarch64/common/trap.c
index e93ad78fc5..a648c2dd88 100644
--- a/libcpu/aarch64/common/trap.c
+++ b/libcpu/aarch64/common/trap.c
@@ -64,15 +64,16 @@ void rt_hw_trap_irq(void)
     uint32_t irq;
     rt_isr_handler_t isr_func;
     extern struct rt_irq_desc isr_table[];
-    uint32_t value = 0;
-    value = IRQ_PEND_BASIC & 0x3ff;
+    uint32_t value = IRQ_PEND_BASIC & 0x3ff;
 
-#ifdef BSP_USING_CORETIMER
-    uint32_t cpu_id = 0;
 #ifdef RT_USING_SMP
-    cpu_id = rt_hw_cpu_id();
+    uint32_t cpu_id = rt_hw_cpu_id();
+    uint32_t mailbox_data = IPI_MAILBOX_CLEAR(cpu_id);
+#else
+    uint32_t cpu_id = 0;
 #endif
     uint32_t int_source = CORE_IRQSOURCE(cpu_id) & 0x3ff;
+
     if (int_source & 0x02)
     {
         isr_func = isr_table[IRQ_ARM_TIMER].handler;
@@ -84,8 +85,34 @@ void rt_hw_trap_irq(void)
             param = isr_table[IRQ_ARM_TIMER].param;
             isr_func(IRQ_ARM_TIMER, param);
         }
+        return;
     }
+
+#ifdef RT_USING_SMP
+    if (int_source & 0xf0)
+    {
+        /* it's a ipi interrupt */
+        if (mailbox_data & 0x1)
+        {
+            /* clear mailbox */
+            IPI_MAILBOX_CLEAR(cpu_id) = mailbox_data;
+            isr_func = isr_table[IRQ_ARM_MAILBOX].handler;
+#ifdef RT_USING_INTERRUPT_INFO
+            isr_table[IRQ_ARM_MAILBOX].counter++;
 #endif
+            if (isr_func)
+            {
+                param = isr_table[IRQ_ARM_MAILBOX].param;
+                isr_func(IRQ_ARM_MAILBOX, param);
+            }
+        }
+        else
+        {
+            CORE_MAILBOX3_CLEAR(cpu_id) = mailbox_data;
+        }
+        return;
+    }
+#endif /* RT_USING_SMP */
 
     /* local interrupt*/
     if (value)
diff --git a/libcpu/aarch64/cortex-a/entry_point.S b/libcpu/aarch64/cortex-a/entry_point.S
index 0385dd4cf8..2228677f1c 100644
--- a/libcpu/aarch64/cortex-a/entry_point.S
+++ b/libcpu/aarch64/cortex-a/entry_point.S
@@ -7,16 +7,54 @@
  * 2020-01-15     bigmagic     the first version
  * 2020-08-10     SummerGift   support clang compiler
  * 2021-11-04     GuEe-GUI     set sp with SP_ELx
+ * 2021-12-28     GuEe-GUI     add smp support
  */
 
+#include "rtconfig.h"
 .section ".text.entrypoint","ax"
 
+#define SECONDARY_STACK_SIZE 4096
+
 .globl _start
+.globl secondary_cpu_start
 _start:
-    /* Read cpu id */
     mrs     x1, mpidr_el1
-    and     x1, x1, #3
-    cbz     x1, cpu_setup           /* If cpu id > 0, stop slave cores */
+    and     x1, x1, #0xff
+    cbnz    x1, cpu_idle            /* If cpu id > 0, stop slave cores */
+
+secondary_cpu_start:
+#ifdef RT_USING_SMP
+    /* Read cpu mpidr_el1 */
+    mrs     x1, mpidr_el1
+
+    /* Read cpu id */
+    ldr     x0, =rt_cpu_mpidr_early /* BSP must be defined `rt_cpu_mpidr_early' table in smp */
+    mov     x2, #0
+
+cpu_id_confirm:
+    add     x2, x2, #1              /* Next cpu id inc */
+    ldr     x3, [x0], #8
+    cmp     x3, #0
+    beq     cpu_idle                /* Mean that `rt_cpu_mpidr_early' table is end */
+    cmp     x3, x1
+    bne     cpu_id_confirm
+
+    /* Get cpu id success */
+    sub     x0, x2, #1
+    msr     tpidr_el1, x0           /* Save cpu id global */
+    cbz     x0, cpu_setup           /* Only go to cpu_setup when cpu id = 0 */
+
+    /* Set current cpu's stack top */
+    sub     x0, x0, #1
+    mov     x1, #SECONDARY_STACK_SIZE
+    adr     x2, .secondary_cpu_stack_top
+    msub    x1, x0, x1, x2
+
+    b       cpu_check_el
+#else
+    msr     tpidr_el1, xzr
+    b       cpu_setup
+#endif /* RT_USING_SMP */
 
 cpu_idle:
     wfe
@@ -25,6 +63,7 @@ cpu_idle:
 cpu_setup:
     ldr     x1, =_start
 
+cpu_check_el:
     mrs     x0, CurrentEL           /* CurrentEL Register. bit 2, 3. Others reserved */
     and     x0, x0, #12             /* Clear reserved bits */
 
@@ -84,15 +123,41 @@ cpu_in_el1:
     bic     x1, x1, #(1 << 1)       /* Disable Alignment check */
     msr     sctlr_el1, x1
 
-    ldr     x1, =__bss_start
-    ldr     w2, =__bss_size
+#ifdef RT_USING_SMP
+    ldr     x1, =_start
+    cmp     sp, x1
+    bne     secondary_cpu_c_start
+#endif /* RT_USING_SMP */
+
+    ldr     x0, =__bss_start
+    ldr     x1, =__bss_end
+    sub     x2, x1, x0
+    mov     x3, x1
+    cmp     x2, #7
+    bls     clean_bss_check
 
-clean_bss_loop:
-    cbz     w2, jump_to_entry
-    str     xzr, [x1], #8
-    sub     w2, w2, #8
-    cbnz    w2, clean_bss_loop
+clean_bss_loop_quad:
+    str     xzr, [x0], #8
+    sub     x2, x3, x0
+    cmp     x2, #7
+    bhi     clean_bss_loop_quad
+    cmp     x1, x0
+    bls     jump_to_entry
+
+clean_bss_loop_byte:
+    str     xzr, [x0], #1
+
+clean_bss_check:
+    cmp     x1, x0
+    bhi     clean_bss_loop_byte
 
 jump_to_entry:
     b       rtthread_startup
     b       cpu_idle                /* For failsafe, halt this core too */
+
+#ifdef RT_USING_SMP
+.align 12
+.secondary_cpu_stack:
+.space (SECONDARY_STACK_SIZE * (RT_CPUS_NR - 1))
+.secondary_cpu_stack_top:
+#endif