[boot-wrapper PATCH v2 8/8] aarch64: Introduce EL2 boot code for Armv8-R AArch64

@@ -25,16 +25,24 @@ _start:
 	 * Boot sequence
 	 * If CurrentEL == EL3, then goto EL3 initialisation and drop to
 	 *   lower EL before entering the kernel.
+	 * If CurrentEL == EL2 && id_aa64mmfr0_el1.MSA == 0xf, then
+	 *   If id_aa64mmfr0_el1.MSA_frac == 0x2, then goto
+	 *     Armv8-R AArch64 initialisation and drop to EL1 before
+	 *     entering the kernel.
+	 *   Else, which means VMSA unsupported and cannot boot Linux,
+	 *     goto err_invalid_arch (dead loop).
 	 * Else, no initialisation and keep the current EL before
 	 *   entering the kernel.
 	 */
 	mrs	x0, CurrentEL
-	cmp	x0, #CURRENTEL_EL3
-	beq	el3_init
+	cmp	x0, #CURRENTEL_EL2
+	bgt	el3_init
+	beq	el2_init
 
 	/*
 	 * We stay in the current EL for entering the kernel
 	 */
+keep_el:
 	mov	w0, #1
 	ldr	x1, =flag_keep_el
 	str	w0, [x1]

@@ -127,6 +135,80 @@ el3_init:
 	str	w0, [x1]
 	b	el_max_init
 
+	/*
+	 * EL2 Armv8-R AArch64 initialisation
+	 */
+el2_init:
+	/* Detect Armv8-R AArch64 */
+	mrs	x1, id_aa64mmfr0_el1
+	/*
+	 * Check MSA, bits [51:48]:
+	 * 0xf means Armv8-R AArch64.
+	 * If not 0xf, goto keep_el.
+	 */
+	ubfx	x0, x1, #48, #4			// MSA
+	cmp	x0, 0xf
+	bne	keep_el
+	/*
+	 * Check MSA_frac, bits [55:52]:
+	 * 0x2 means EL1&0 translation regime also supports VMSAv8-64.
+	 */
+	ubfx	x0, x1, #52, #4			// MSA_frac
+	cmp	x0, 0x2
+	/* If not 0x2, no VMSA, so cannot boot Linux and dead loop. */
+	bne	err_invalid_arch
+
+	mrs	x0, midr_el1
+	msr	vpidr_el2, x0
+
+	mrs	x0, mpidr_el1
+	msr	vmpidr_el2, x0
+
+	mov	x0, #(1 << 31)			// VTCR_MSA: VMSAv8-64 support
+	msr	vtcr_el2, x0
+
+	/* Init HCR_EL2 */
+	mov	x0, #(1 << 31)			// RES1
+
+	mrs	x1, id_aa64pfr0_el1
+	ubfx	x2, x1, #56, 4
+	cmp	x2, 0x2
+	bne	1f
+	/*
+	 * Disable trap when accessing SCTXNUM_EL0 or SCTXNUM_EL1
+	 * if FEAT_CSV2.
+	 */
+	orr	x0, x0, #(1 << 53)		// EnSCXT
+
+1:	ubfx	x2, x1, #28, 4
+	cmp	x2, 0x2
+	bne	1f
+	/* Disable trap when accessing ERXPFGCDN_EL1 if FEAT_RASv1p1. */
+	orr	x0, x0, #(1 << 47)		// FIEN
+
+	/* Enable pointer authentication if present */
+1:	mrs	x1, id_aa64isar1_el1
+	/*
+	 * If ID_AA64ISAR1_EL1.{GPI, GPA, API, APA} == {0000, 0000, 0000, 0000}
+	 *   then HCR_EL2.APK and HCR_EL2.API are RES 0.
+	 * Else
+	 *   set HCR_EL2.APK and HCR_EL2.API.
+	 */
+	ldr	x2, =(((0xff) << 24) | (0xff << 4))
+	and	x1, x1, x2
+	cbz	x1, 1f
+
+	orr	x0, x0, #(1 << 40)		// APK
+	orr	x0, x0, #(1 << 41)		// API
+
+1:	msr	hcr_el2, x0
+	isb
+
+	mov	w0, #SPSR_KERNEL_EL1
+	ldr	x1, =spsr_to_elx
+	str	w0, [x1]
+	// fall through
+
 el_max_init:
 	ldr	x0, =CNTFRQ
 	msr	cntfrq_el0, x0

@@ -136,6 +218,7 @@ el_max_init:
 	b	start_el_max
 
 err_invalid_id:
+err_invalid_arch:
 	b	.
 
 	/*