[boot-wrapper PATCH 2/5] aarch64: Rename labels and prepare for lower EL booting

@@ -12,7 +12,7 @@
 	.section .init
 
 	.globl	_start
-	.globl jump_kernel
+	.globl	jump_kernel
 
 _start:
 	cpuid	x0, x1

@@ -22,20 +22,30 @@ _start:
 	bl	setup_stack
 
 	/*
-	 * EL3 initialisation
+	 * Boot sequence
+	 * If CurrentEL == EL3, then goto EL3 initialisation and drop to
+	 *   lower EL before entering the kernel.
+	 * Else, no initialisation and keep the current EL before
+	 *   entering the kernel.
 	 */
 	mrs	x0, CurrentEL
 	cmp	x0, #CURRENTEL_EL3
-	b.eq	1f
+	beq	el3_init
 
+	/*
+	 * We stay in the current EL for entering the kernel
+	 */
 	mov	w0, #1
-	ldr	x1, =flag_no_el3
+	ldr	x1, =flag_keep_el
 	str	w0, [x1]
 
-	bl	setup_stack
-	b	start_no_el3
+	b	start_keep_el
 
-1:	mov	x0, #0x30			// RES1
+	/*
+	 * EL3 initialisation
+	 */
+el3_init:
+	mov	x0, #0x30			// RES1
 	orr	x0, x0, #(1 << 0)		// Non-secure EL1
 	orr	x0, x0, #(1 << 8)		// HVC enable
 

@@ -93,13 +103,23 @@ _start:
 	mov	x0, #ZCR_EL3_LEN_MASK		// SVE: Enable full vector len
 	msr	ZCR_EL3, x0			// for EL2.
 
-1:
+	/*
+	 * Save SPSR_KERNEL into spsr_to_elx.
+	 * The jump_kernel will load spsr_to_elx into spsr_el3
+	 */
+1:	mov	w0, #SPSR_KERNEL
+	ldr	x1, =spsr_to_elx
+	str	w0, [x1]
+	b	el_max_init
+
+el_max_init:
 	ldr	x0, =CNTFRQ
 	msr	cntfrq_el0, x0
+	isb
 
 	bl	gic_secure_init
 
-	b	start_el3
+	b	start_el_max
 
 err_invalid_id:
 	b	.

@@ -119,14 +139,18 @@ jump_kernel:
 	ldr	x0, =SCTLR_EL1_RESET
 	msr	sctlr_el1, x0
 
+	mrs	x0, CurrentEL
+	cmp	x0, #CURRENTEL_EL2
+	b.lt	1f
+
 	ldr	x0, =SCTLR_EL2_RESET
 	msr	sctlr_el2, x0
 
-	cpuid	x0, x1
+1:	cpuid	x0, x1
 	bl	find_logical_id
 	bl	setup_stack		// Reset stack pointer
 
-	ldr	w0, flag_no_el3
+	ldr	w0, flag_keep_el
 	cmp	w0, #0			// Prepare Z flag
 
 	mov	x0, x20

@@ -135,9 +159,9 @@ jump_kernel:
 	mov	x3, x23
 
 	b.eq	1f
-	br	x19			// No EL3
+	br	x19			// Keep current EL
 
-1:	mov	x4, #SPSR_KERNEL
+1:	ldr	w4, spsr_to_elx
 
 	/*
 	 * If bit 0 of the kernel address is set, we're entering in AArch32

@@ -153,5 +177,7 @@ jump_kernel:
 
 	.data
 	.align 3
-flag_no_el3:
+flag_keep_el:
+	.long 0
+spsr_to_elx:
 	.long 0

@@ -11,6 +11,7 @@
 
 #define MPIDR_ID_BITS		0xff00ffffff
 
+#define CURRENTEL_EL2		(2 << 2)
 #define CURRENTEL_EL3		(3 << 2)
 
 /*

@@ -24,6 +25,7 @@
 #define SPSR_I			(1 << 7)	/* IRQ masked */
 #define SPSR_F			(1 << 6)	/* FIQ masked */
 #define SPSR_T			(1 << 5)	/* Thumb */
+#define SPSR_EL1H		(5 << 0)	/* EL1 Handler mode */
 #define SPSR_EL2H		(9 << 0)	/* EL2 Handler mode */
 #define SPSR_HYP		(0x1a << 0)	/* M[3:0] = hyp, M[4] = AArch32 */
 

@@ -42,6 +44,7 @@
 #else
 #define SCTLR_EL1_RESET		SCTLR_EL1_RES1
 #define SPSR_KERNEL		(SPSR_A | SPSR_D | SPSR_I | SPSR_F | SPSR_EL2H)
+#define SPSR_KERNEL_EL1		(SPSR_A | SPSR_D | SPSR_I | SPSR_F | SPSR_EL1H)
 #endif
 
 #ifndef __ASSEMBLY__

@@ -45,8 +45,8 @@ vector:
 
 	.text
 
-	.globl start_no_el3
-	.globl start_el3
+	.globl start_keep_el
+	.globl start_el_max
 
 err_exception:
 	b err_exception

@@ -101,7 +101,7 @@ smc_exit:
 	eret
 
 
-start_el3:
+start_el_max:
 	ldr	x0, =vector
 	bl	setup_vector
 

@@ -111,10 +111,11 @@ start_el3:
 	b	psci_first_spin
 
 /*
- * This PSCI implementation requires EL3. Without EL3 we'll only boot the
- * primary cpu, all others will be trapped in an infinite loop.
+ * This PSCI implementation requires the highest EL(EL3 or Armv8-R EL2).
+ * Without the highest EL, we'll only boot the primary cpu, all others
+ * will be trapped in an infinite loop.
  */
-start_no_el3:
+start_keep_el:
 	cpuid	x0, x1
 	bl	find_logical_id
 	cbz	x0, psci_first_spin

@@ -11,11 +11,11 @@
 
 	.text
 
-	.globl start_no_el3
-	.globl start_el3
+	.globl start_keep_el
+	.globl start_el_max
 
-start_el3:
-start_no_el3:
+start_el_max:
+start_keep_el:
 	cpuid	x0, x1
 	bl	find_logical_id