[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

bsp/qemu-virt64-aarch64/.config

@@ -85,6 +85,7 @@ CONFIG_RT_CONSOLE_DEVICE_NAME="uart0"
 CONFIG_RT_VER_NUM=0x40100
 CONFIG_ARCH_CPU_64BIT=y
 # CONFIG_RT_USING_CPU_FFS is not set
+CONFIG_ARCH_ARMV8=y
 # CONFIG_ARCH_CPU_STACK_GROWS_UPWARD is not set
 
 #
@@ -686,7 +687,6 @@ CONFIG_RT_LIBC_DEFAULT_TIMEZONE=8
 # CONFIG_PKG_USING_CONTROLLER is not set
 # CONFIG_PKG_USING_PHASE_LOCKED_LOOP is not set
 CONFIG_SOC_VIRT64_AARCH64=y
-CONFIG_BSP_SUPPORT_FPU=y
 
 #
 # AARCH64 qemu virt64 configs

bsp/qemu-virt64-aarch64/Kconfig

@@ -20,14 +20,10 @@ source "$PKGS_DIR/Kconfig"
 
 config SOC_VIRT64_AARCH64
     bool
-    select ARCH_ARM_CORTEX_A53
+    select ARCH_ARMV8
     select ARCH_CPU_64BIT
     select RT_USING_COMPONENTS_INIT
     select RT_USING_USER_MAIN
-    select BSP_USING_GIC
-    select RT_USING_GIC
-    select BSP_USING_GIC390
-    select RT_USING_RTC
     default y
 
 source "$BSP_DIR/driver/Kconfig"

bsp/qemu-virt64-aarch64/driver/Kconfig

-menuconfig BSP_SUPPORT_FPU
-    bool "Using Float"
-    default y
-
 menu "AARCH64 qemu virt64 configs"
 
     menuconfig BSP_USING_UART

bsp/qemu-virt64-aarch64/rtconfig.h

@@ -53,6 +53,7 @@
 #define RT_CONSOLE_DEVICE_NAME "uart0"
 #define RT_VER_NUM 0x40100
 #define ARCH_CPU_64BIT
+#define ARCH_ARMV8
 
 /* RT-Thread Components */
 
@@ -220,7 +221,6 @@
 /* entertainment: terminal games and other interesting software packages */
 
 #define SOC_VIRT64_AARCH64
-#define BSP_SUPPORT_FPU
 
 /* AARCH64 qemu virt64 configs */
 

bsp/raspberry-pi/raspi3-64/.config

@@ -711,7 +711,6 @@ CONFIG_RT_LIBC_DEFAULT_TIMEZONE=8
 # CONFIG_PKG_USING_CONTROLLER is not set
 # CONFIG_PKG_USING_PHASE_LOCKED_LOOP is not set
 CONFIG_BCM2836_SOC=y
-CONFIG_BSP_SUPPORT_FPU=y
 
 #
 # Hardware Drivers Config

bsp/raspberry-pi/raspi3-64/driver/Kconfig

-
-config BSP_SUPPORT_FPU
-    bool "Using Float"
-    default n
-
 menu "Hardware Drivers Config"
     menu "BCM Peripheral Drivers"
         menuconfig BSP_USING_UART

bsp/raspberry-pi/raspi3-64/rtconfig.h

@@ -232,7 +232,6 @@
 /* entertainment: terminal games and other interesting software packages */
 
 #define BCM2836_SOC
-#define BSP_SUPPORT_FPU
 
 /* Hardware Drivers Config */
 

bsp/raspberry-pi/raspi4-64/.config

@@ -778,7 +778,6 @@ CONFIG_YMODEM_USING_FILE_TRANSFER=y
 # CONFIG_PKG_USING_CONTROLLER is not set
 # CONFIG_PKG_USING_PHASE_LOCKED_LOOP is not set
 CONFIG_BCM2711_SOC=y
-CONFIG_BSP_SUPPORT_FPU=y
 
 #
 # Hardware Drivers Config

bsp/raspberry-pi/raspi4-64/driver/Kconfig

-
-config BSP_SUPPORT_FPU
-    bool "Using Float"
-    default n
-
 menu "Hardware Drivers Config"
     menu "BCM Peripheral Drivers"
         menuconfig BSP_USING_UART

bsp/raspberry-pi/raspi4-64/rtconfig.h

@@ -284,7 +284,6 @@
 /* entertainment: terminal games and other interesting software packages */
 
 #define BCM2711_SOC
-#define BSP_SUPPORT_FPU
 
 /* Hardware Drivers Config */
 

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

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__ */

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 */

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