aarch64: initialize SCTLR_ELx for the boot-wrapper

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
to 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@arm.com>
Reviewed-by: Andre Przywara <andre.przywara@arm.com>
diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
index d682ba5..204c199 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
+	mrs	x0, CurrentEL
+	cmp	x0, #CURRENTEL_EL3
+	b.eq	reset_at_el3
+	cmp	x0, #CURRENTEL_EL2
+	b.eq	reset_at_el2
+	cmp	x0, #CURRENTEL_EL1
+	b.eq	reset_at_el1
+
+	/* Booting at EL0 is not supported */
+	b	.
 
 	/*
 	 * EL3 initialisation
 	 */
-	mrs	x0, CurrentEL
-	cmp	x0, #CURRENTEL_EL3
-	b.eq	1f
+reset_at_el3:
+	mov_64	x0, SCTLR_EL3_RESET
+	msr	sctlr_el3, x0
+	isb
 
-	mov	w0, #1
-	ldr	x1, =flag_no_el3
-	str	w0, [x1]
-
-	b	start_no_el3
-
-1:	mov	x0, #0x30			// RES1
+	mov	x0, #0x30			// RES1
 	orr	x0, x0, #(1 << 0)		// Non-secure EL1
 	orr	x0, x0, #(1 << 8)		// HVC enable
 
@@ -143,10 +143,54 @@
 	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 341a545..0a4085b 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -14,6 +14,35 @@
 #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							\
+	(BIT(29) | BIT(28) | BIT(23) | BIT(22) | BIT(18) | BIT(16) |	\
+	 BIT(11) | BIT(5) | BIT(4))
+
+#define SCTLR_EL2_RES1							\
+	(BIT(29) | BIT(28) | BIT(23) | BIT(22) | BIT(20) | BIT(18) |	\
+	 BIT(16) | BIT(11) | BIT(5) | BIT(4))
+
+#define SCTLR_EL1_RES1							\
+	(BIT(29) | BIT(28) | BIT(23) | BIT(22) | BIT(20) | BIT(11) |	\
+	 BIT(8) | BIT(7) | BIT(4))
+
+#define HCR_EL2_RES1		(BIT(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 +72,6 @@
 #define ZCR_EL3_LEN_MAX		0xf
 
 #define SCTLR_EL1_CP15BEN	(1 << 5)
-#define SCTLR_EL1_RES1		(3 << 28 | 3 << 22 | 1 << 11)
 
 #ifdef KERNEL_32
 /*