[bootwrapper PATCH 06/13] aarch64: initialize SCTLR_ELx for the boot-wrapper

[Mark Rutland]

The SCTLR_ELx registers contain fields which are UNKNOWN or
IMPLEMENTATION DEFINED out of reset. This includes SCTLR_ELx.EE, which
defines the endianness of memory accesses (e.g. reads from literal
pools). Due to this, portions of boot-wrapper code are not guaranteed
top work correctly.

Rework the startup code to explicitly initialize SCTLR_ELx for the
exception level the boot-wrapper was entered at. When entered at EL2
it's necessary to first initialise HCR_EL2.E2H as this affects the RESx
behaviour of bits in SCTLR_EL2, and also aliases SCTLR_EL1 to SCTLR_EL2,
which would break the initialization performed in jump_kernel.

As we plan to eventually support the highest implemented EL being any of
EL3/EL2/EL1, code is added to handle all of these exception levels, even
though we do not currently support starting at EL1.

We'll initialize other registers in subsequent patches.

Signed-off-by: Mark Rutland <mark.rutland at arm.com>
---
 arch/aarch64/boot.S            | 74 +++++++++++++++++++++++++++-------
 arch/aarch64/include/asm/cpu.h | 27 ++++++++++++-
 2 files changed, 85 insertions(+), 16 deletions(-)

diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
index 900b9f8..45a0367 100644
--- a/arch/aarch64/boot.S
+++ b/arch/aarch64/boot.S
@@ -26,26 +26,26 @@
 	 *   PSCI is not supported when entered in this exception level.
 	 */
 ASM_FUNC(_start)
-	cpuid	x0, x1
-	bl	find_logical_id
-	cmp	x0, #MPIDR_INVALID
-	beq	err_invalid_id
-	bl	setup_stack
-
-	/*
-	 * EL3 initialisation
-	 */
 	mrs	x0, CurrentEL
 	cmp	x0, #CURRENTEL_EL3
-	b.eq	1f
+	b.eq	reset_at_el3
+	cmp	x0, #CURRENTEL_EL2
+	b.eq	reset_at_el2
+	cmp	x0, #CURRENTEL_EL1
+	b.eq	reset_at_el1
 
-	mov	w0, #1
-	ldr	x1, =flag_no_el3
-	str	w0, [x1]
+	/* Booting at EL0 is not supported */
+	b	.
 
-	b	start_no_el3
+	/*
+	 * EL3 initialisation
+	 */
+reset_at_el3:
+	mov_64	x0, SCTLR_EL3_RESET
+	msr	sctlr_el3, x0
+	isb
 
-1:	mov	x0, #0x30			// RES1
+	mov	x0, #0x30			// RES1
 	orr	x0, x0, #(1 << 0)		// Non-secure EL1
 	orr	x0, x0, #(1 << 8)		// HVC enable
 
@@ -135,10 +135,54 @@ ASM_FUNC(_start)
 	ldr	x0, =COUNTER_FREQ
 	msr	cntfrq_el0, x0
 
+	cpuid	x0, x1
+	bl	find_logical_id
+	cmp	x0, #MPIDR_INVALID
+	b.eq	err_invalid_id
+	bl	setup_stack
+
 	bl	gic_secure_init
 
 	b	start_el3
 
+	/*
+	 * EL2 initialization
+	 */
+reset_at_el2:
+	// Ensure E2H is not in use
+	mov_64	x0, HCR_EL2_RESET
+	msr	hcr_el2, x0
+	isb
+
+	mov_64	x0, SCTLR_EL2_RESET
+	msr	sctlr_el2, x0
+	isb
+
+	b	reset_no_el3
+
+	/*
+	 * EL1 initialization
+	 */
+reset_at_el1:
+	mov_64	x0, SCTLR_EL1_RESET
+	msr	sctlr_el1, x0
+	isb
+
+	b	reset_no_el3
+
+reset_no_el3:
+	cpuid	x0, x1
+	bl	find_logical_id
+	cmp	x0, #MPIDR_INVALID
+	b.eq	err_invalid_id
+	bl	setup_stack
+
+	mov	w0, #1
+	ldr	x1, =flag_no_el3
+	str	w0, [x1]
+
+	b	start_no_el3
+
 err_invalid_id:
 	b	.
 
diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index 1053414..f0edd2a 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -14,6 +14,32 @@
 #define MPIDR_ID_BITS		0xff00ffffff
 
 #define CURRENTEL_EL3		(3 << 2)
+#define CURRENTEL_EL2		(2 << 2)
+#define CURRENTEL_EL1		(1 << 2)
+
+/*
+ * RES1 bit definitions definitions as of ARM DDI 0487G.b
+ *
+ * These includes bits which are RES1 in some configurations.
+ */
+#define SCTLR_EL3_RES1		(1 << 29 | 1 << 28 | 1 << 23 | 1 << 22 | \
+				 1 << 18 | 1 << 16 | 1 << 11 | 1 << 5 | 1 << 4)
+
+#define SCTLR_EL2_RES1		(1 << 29 | 1 << 28 | 1 << 23 | 1 << 22 | \
+				 1 << 18 | 1 << 16 | 1 << 11 | 1 << 5 | 1 << 4)
+
+#define SCTLR_EL1_RES1		(1 << 29 | 1 << 28 | 1 << 23 | 1 << 22 | \
+				 1 << 11 | 1 << 8 | 1 << 7 | 1 << 4)
+
+#define HCR_EL2_RES1		(1 << 1)
+
+/*
+ * Initial register values required for the boot-wrapper to run out-of-reset.
+ */
+#define SCTLR_EL3_RESET		SCTLR_EL3_RES1
+#define SCTLR_EL2_RESET		SCTLR_EL2_RES1
+#define SCTLR_EL1_RESET		SCTLR_EL1_RES1
+#define HCR_EL2_RESET		HCR_EL2_RES1
 
 #define ID_AA64PFR0_EL1_GIC	BITS(27, 24)
 
@@ -43,7 +69,6 @@
 #define ZCR_EL3_LEN_MASK	0x1ff
 
 #define SCTLR_EL1_CP15BEN	(1 << 5)
-#define SCTLR_EL1_RES1		(3 << 28 | 3 << 22 | 1 << 11)
 
 #ifdef KERNEL_32
 /* 32-bit kernel decompressor uses CP15 barriers */
-- 
2.30.2