Merge pull request #5319 from RT-Thread/master

【PSE分支】同步master到PSE分支

4 files changed, 29 insertions, 38 deletions

diff --git a/libcpu/aarch64/common/context_gcc.S b/libcpu/aarch64/common/context_gcc.S
index 934db7ed84..b1a8313916 100644
--- a/libcpu/aarch64/common/context_gcc.S
+++ b/libcpu/aarch64/common/context_gcc.S
@@ -5,10 +5,11 @@
  *
  * Change Logs:
  * Date           Author       Notes
- * 2018-10-06     ZhaoXiaowei    the first version
+ * 2018-10-06     ZhaoXiaowei  the first version
+ * 2021-11-04     GuEe-GUI     set sp with SP_ELx
  */
 
-  /*
+/*
  *enable gtimer
  */
 .globl rt_hw_gtimer_enable
@@ -18,6 +19,14 @@ rt_hw_gtimer_enable:
 	RET
 
 /*
+ *disable gtimer
+ */
+.globl rt_hw_gtimer_disable
+rt_hw_gtimer_disable:
+    MSR CNTP_CTL_EL0,XZR
+    RET
+
+/*
  *set gtimer CNTP_TVAL_EL0 value
  */
 .globl rt_hw_set_gtimer_val
@@ -48,10 +57,6 @@ rt_hw_get_gtimer_frq:
 	RET
 
 .macro SAVE_CONTEXT
-
-    /* Switch to use the EL0 stack pointer. */
-    MSR 	SPSEL, #0
-
     /* Save the entire context. */
     STP 	X0, X1, [SP, #-0x10]!
     STP 	X2, X3, [SP, #-0x10]!
@@ -98,16 +103,9 @@ rt_hw_get_gtimer_frq:
 
     MOV 	X0, SP   /* Move SP into X0 for saving. */
 
-    /* Switch to use the ELx stack pointer. */
-    MSR 	SPSEL, #1
-
     .endm
 
 .macro SAVE_CONTEXT_T
-
-    /* Switch to use the EL0 stack pointer. */
-    MSR 	SPSEL, #0
-
     /* Save the entire context. */
     STP 	X0, X1, [SP, #-0x10]!
     STP 	X2, X3, [SP, #-0x10]!
@@ -135,15 +133,15 @@ rt_hw_get_gtimer_frq:
     B.EQ	1f
     B 		.
 3:
-    MRS		X3, SPSR_EL3
+    MOV		X3, 0x0d
     MOV		X2, X30
     B		0f
 2:
-    MRS		X3, SPSR_EL2
+    MOV		X3, 0x09
     MOV		X2, X30
     B		0f
 1:
-    MRS		X3, SPSR_EL1
+    MOV		X3, 0x05
     MOV		X2, X30
     B		0f
 0:
@@ -152,16 +150,10 @@ rt_hw_get_gtimer_frq:
 
     MOV 	X0, SP   /* Move SP into X0 for saving. */
 
-    /* Switch to use the ELx stack pointer. */
-    MSR 	SPSEL, #1
-
     .endm
 
 .macro RESTORE_CONTEXT
 
-    /* Switch to use the EL0 stack pointer. */
-    MSR 	SPSEL, #0
-
     /* Set the SP to point to the stack of the task being restored. */
     MOV		SP, X0
 
@@ -206,9 +198,6 @@ rt_hw_get_gtimer_frq:
     LDP 	X2, X3, [SP], #0x10
     LDP 	X0, X1, [SP], #0x10
 
-    /* Switch to use the ELx stack pointer.  _RB_ Might not be required. */
-    MSR 	SPSEL, #1
-
     ERET
 
     .endm
diff --git a/libcpu/aarch64/common/stack.c b/libcpu/aarch64/common/stack.c
index 0fe00d8674..d93706fcb9 100644
--- a/libcpu/aarch64/common/stack.c
+++ b/libcpu/aarch64/common/stack.c
@@ -7,15 +7,16 @@
  * Date           Author       Notes
  * 2011-09-23     Bernard      the first version
  * 2011-10-05     Bernard      add thumb mode
+ * 2021-11-04     GuEe-GUI     set sp with SP_ELx
  */
 #include <rtthread.h>
 #include <board.h>
 
 #include <armv8.h>
 
-#define INITIAL_SPSR_EL3 (PSTATE_EL3 | SP_EL0)
-#define INITIAL_SPSR_EL2 (PSTATE_EL2 | SP_EL0)
-#define INITIAL_SPSR_EL1 (PSTATE_EL1 | SP_EL0)
+#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
diff --git a/libcpu/aarch64/common/vector_gcc.S b/libcpu/aarch64/common/vector_gcc.S
index fc88cce7f8..50ff41a8e3 100644
--- a/libcpu/aarch64/common/vector_gcc.S
+++ b/libcpu/aarch64/common/vector_gcc.S
@@ -18,7 +18,7 @@ system_vectors:
 .align 11
     .set    VBAR, system_vectors
     .org    VBAR
-    // Exception from CurrentEL (EL1) with SP_EL0 (SPSEL=1)
+    // Exception from CurrentEL (EL1) with SP_EL0 (SPSEL=0)
     .org (VBAR + 0x00 + 0)
     B vector_error      // 			Synchronous
     .org (VBAR + 0x80 + 0)
diff --git a/libcpu/aarch64/cortex-a/entry_point.S b/libcpu/aarch64/cortex-a/entry_point.S
index ac7b793811..0385dd4cf8 100644
--- a/libcpu/aarch64/cortex-a/entry_point.S
+++ b/libcpu/aarch64/cortex-a/entry_point.S
@@ -6,6 +6,7 @@
  * Date           Author       Notes
  * 2020-01-15     bigmagic     the first version
  * 2020-08-10     SummerGift   support clang compiler
+ * 2021-11-04     GuEe-GUI     set sp with SP_ELx
  */
 
 .section ".text.entrypoint","ax"
@@ -32,14 +33,15 @@ cpu_setup:
     bne     cpu_not_in_el3
 
     /* Should never be executed, just for completeness. (EL3) */
-    mov     x0, #(1 << 0)           /* EL0 and EL1 are in Non-Secure state */
-    orr     x0, x0, #(1 << 4)       /* RES1 */
-    orr     x0, x0, #(1 << 5)       /* RES1 */
-    orr     x0, x0, #(1 << 7)       /* SMC instructions are undefined at EL1 and above */
-    orr     x0, x0, #(1 << 8)       /* HVC instructions are enabled at EL1 and above */
-    orr     x0, x0, #(1 << 10)      /* The next lower level is AArch64 */
+    mov     x2, #(1 << 0)           /* EL0 and EL1 are in Non-Secure state */
+    orr     x2, x2, #(1 << 4)       /* RES1 */
+    orr     x2, x2, #(1 << 5)       /* RES1 */
+    orr     x2, x2, #(1 << 7)       /* SMC instructions are undefined at EL1 and above */
+    orr     x2, x2, #(1 << 8)       /* HVC instructions are enabled at EL1 and above */
+    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
@@ -51,8 +53,6 @@ cpu_not_in_el3:                     /* Running at EL2 or EL1 */
     beq     cpu_in_el1              /* Halt this core if running in El1 */
 
 cpu_in_el2:
-    msr     sp_el1, x1
-
     /* Enable CNTP for EL1 */
     mrs     x0, cnthctl_el2         /* Counter-timer Hypervisor Control register */
     orr     x0, x0, #3
@@ -71,6 +71,7 @@ cpu_in_el2:
     eret
 
 cpu_in_el1:
+    msr     spsel, #1
     mov     sp, x1                  /* Set sp in el1 */
 
     /* Avoid trap from SIMD or float point instruction */
@@ -89,7 +90,7 @@ cpu_in_el1:
 clean_bss_loop:
     cbz     w2, jump_to_entry
     str     xzr, [x1], #8
-    sub     w2, w2, #1
+    sub     w2, w2, #8
     cbnz    w2, clean_bss_loop
 
 jump_to_entry: