[libcpu/aarch64] add gtimer frq set and stack align (#5642)

* [libcpu/aarch64] add smp support

* [libcpu/aarch64] rt_hw_trap_irq get irq instead of iar when using gicv2

* [libcpu/aarch64] disable irq/fiq when switch thread

* [libcpu/aarch64] add gtimer frq set and stack align

4 files changed, 38 insertions, 32 deletions

diff --git a/libcpu/aarch64/common/context_gcc.S b/libcpu/aarch64/common/context_gcc.S
index cad3b0db3f..20f65ffd35 100644
--- a/libcpu/aarch64/common/context_gcc.S
+++ b/libcpu/aarch64/common/context_gcc.S
@@ -65,6 +65,19 @@ rt_hw_get_gtimer_frq:
     MRS X0,CNTFRQ_EL0
     RET
 
+/*
+ *set gtimer frq value (only in EL3)
+ */
+.globl rt_hw_set_gtimer_frq
+rt_hw_set_gtimer_frq:
+    MRS X1, CurrentEL
+    CMP X1, 0xc
+    BNE rt_hw_set_gtimer_frq_exit
+    MSR CNTFRQ_EL0, X0
+    MOV X0, XZR
+rt_hw_set_gtimer_frq_exit:
+    RET
+
 .macro SAVE_CONTEXT
     /* Save the entire context. */
     SAVE_FPU SP
diff --git a/libcpu/aarch64/common/gtimer.h b/libcpu/aarch64/common/gtimer.h
index 956e2ed5aa..c420c1f886 100644
--- a/libcpu/aarch64/common/gtimer.h
+++ b/libcpu/aarch64/common/gtimer.h
@@ -23,5 +23,6 @@ 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();
+rt_uint64_t rt_hw_set_gtimer_frq(rt_uint64_t value);
 
 #endif /* __GTIMER_H__ */
diff --git a/libcpu/aarch64/common/stack.c b/libcpu/aarch64/common/stack.c
index a0ec935e5d..2743fd3eee 100644
--- a/libcpu/aarch64/common/stack.c
+++ b/libcpu/aarch64/common/stack.c
@@ -13,10 +13,6 @@
 #include <rtthread.h>
 #include <armv8.h>
 
-#define INITIAL_SPSR_EL3 (PSTATE_EL3 | SP_ELx)
-#define INITIAL_SPSR_EL2 (PSTATE_EL2 | SP_ELx)
-#define INITIAL_SPSR_EL1 (PSTATE_EL1 | SP_ELx)
-
 /**
  * This function will initialize thread stack
  *
@@ -29,10 +25,14 @@
  */
 rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_addr, void *texit)
 {
-    rt_ubase_t *stk;
-    rt_ubase_t current_el;
-
-    stk      = (rt_ubase_t*)stack_addr;
+    static const rt_ubase_t initial_spsr[] =
+    {
+        [1] = PSTATE_EL1 | SP_ELx,
+        [2] = PSTATE_EL2 | SP_ELx,
+        [3] = PSTATE_EL3 | SP_ELx
+    };
+    /* The AAPCS64 requires 128-bit (16 byte) stack alignment */
+    rt_ubase_t *stk = (rt_ubase_t*)RT_ALIGN_DOWN((rt_ubase_t)stack_addr, 16);
 
     *(--stk) = (rt_ubase_t) 0;              /* Q0 */
     *(--stk) = (rt_ubase_t) 0;              /* Q0 */
@@ -102,21 +102,7 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_ad
     *(--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. */
 
-    current_el = rt_hw_get_current_el();
-
-    if(current_el == 3)
-    {
-        *(--stk) = INITIAL_SPSR_EL3;
-    }
-    else if(current_el == 2)
-    {
-        *(--stk) = INITIAL_SPSR_EL2;
-    }
-    else
-    {
-        *(--stk) = INITIAL_SPSR_EL1;
-    }
-
+    *(--stk) = initial_spsr[rt_hw_get_current_el()];
     *(--stk) = ( rt_ubase_t ) tentry;       /* Exception return address. */
 
     /* return task's current stack address */
diff --git a/libcpu/aarch64/cortex-a/entry_point.S b/libcpu/aarch64/cortex-a/entry_point.S
index 2228677f1c..5bf2b8a3a6 100644
--- a/libcpu/aarch64/cortex-a/entry_point.S
+++ b/libcpu/aarch64/cortex-a/entry_point.S
@@ -80,16 +80,19 @@ cpu_check_el:
     orr     x2, x2, #(1 << 10)      /* The next lower level is AArch64 */
     msr     scr_el3, x2
 
-    /* Change execution level to EL2 */
-    mov     x2, #0x3c9
-    msr     spsr_el3, x2            /* 0b1111001001 */
-    adr     x2, cpu_not_in_el3
+    mov     x2, #9                  /* Next level is 0b1001->EL2h */
+    orr     x2, x2, #(1 << 6)       /* Mask FIQ */
+    orr     x2, x2, #(1 << 7)       /* Mask IRQ */
+    orr     x2, x2, #(1 << 8)       /* Mask SError */
+    orr     x2, x2, #(1 << 9)       /* Mask Debug Exception */
+    msr     spsr_el3, x2
+    adr     x2, cpu_in_el2
     msr     elr_el3, x2
-    eret                            /* Exception Return: from EL3, continue from cpu_not_in_el3 */
+    eret
 
 cpu_not_in_el3:                     /* Running at EL2 or EL1 */
     cmp     x0, #4                  /* EL1 = 0100  */
-    beq     cpu_in_el1              /* Halt this core if running in El1 */
+    beq     cpu_in_el1
 
 cpu_in_el2:
     /* Enable CNTP for EL1 */
@@ -102,9 +105,12 @@ cpu_in_el2:
     orr     x0, x0, #(1 << 1)       /* SWIO hardwired on Pi3 */
     msr     hcr_el2, x0
 
-    /* Change execution level to EL1 */
-    mov     x2, #0x3c4
-    msr     spsr_el2, x2            /* 0b1111000100 */
+    mov     x2, #5                  /* Next level is 0b0101->EL1h */
+    orr     x2, x2, #(1 << 6)       /* Mask FIQ */
+    orr     x2, x2, #(1 << 7)       /* Mask IRQ */
+    orr     x2, x2, #(1 << 8)       /* Mask SError */
+    orr     x2, x2, #(1 << 9)       /* Mask Debug Exception */
+    msr     spsr_el2, x2
     adr     x2, cpu_in_el1
     msr     elr_el2, x2
     eret