git: 0054693392f0 - main - arm64: Boot into VHE mode when able

The branch main has been updated by andrew:

URL: https://cgit.FreeBSD.org/src/commit/?id=0054693392f094c035fd396db339b8ceb16f17dd

commit 0054693392f094c035fd396db339b8ceb16f17dd
Author:     Andrew Turner <andrew@FreeBSD.org>
AuthorDate: 2024-08-19 12:46:10 +0000
Commit:     Andrew Turner <andrew@FreeBSD.org>
CommitDate: 2024-08-21 08:46:59 +0000

    arm64: Boot into VHE mode when able
    
    When FEAT_VHE is present the HCR_EL2 E2H field will be implemented. Try
    setting this and checking if it's still set to decide if we can boot
    into VHE mode or not.
    
    When it is implemented the kernel will boot into EL2 rather than EL1
    it currently boots to. The parts of the kernel that need to know if
    they are in EL1 or EL2 have been updated other than the CoreSight
    driver as there doesn't appear to be any way to currently use it and
    it will be updated soon with the new HWT framework.
    
    Sponsored by:   Arm Ltd
    Differential Revision:  https://reviews.freebsd.org/D46087
---
 sys/arm64/arm64/locore.S | 65 +++++++++++++++++++++++++++++-------------------
 1 file changed, 40 insertions(+), 25 deletions(-)

diff --git a/sys/arm64/arm64/locore.S b/sys/arm64/arm64/locore.S
index ab1fea0c4716..b71e02538716 100644
--- a/sys/arm64/arm64/locore.S
+++ b/sys/arm64/arm64/locore.S
@@ -296,8 +296,12 @@ LEND(mpentry_common)
 #endif
 
 /*
- * If we are started in EL2, configure the required hypervisor
- * registers and drop to EL1.
+ * Enter the exception level the kernel will use:
+ *
+ *  - If in EL1 continue in EL1
+ *  - If the CPU supports FEAT_VHE then set HCR_E2H and HCR_TGE and continue
+ *    in EL2
+ *  - Configure EL2 to support running the kernel at EL1 and exit to that
  */
 LENTRY(enter_kernel_el)
 #define	INIT_SCTLR_EL1	(SCTLR_LSMAOE | SCTLR_nTLSMD | SCTLR_EIS | \
@@ -335,13 +339,14 @@ LENTRY(enter_kernel_el)
 	isb
 
 	/* Configure the Hypervisor */
-	ldr	x2, =(HCR_RW | HCR_APK | HCR_API)
+	ldr	x2, =(HCR_RW | HCR_APK | HCR_API | HCR_E2H)
 	msr	hcr_el2, x2
 
 	/* Stash value of HCR_EL2 for later */
 	isb
 	mrs	x4, hcr_el2
 
+
 	/* Load the Virtualization Process ID Register */
 	mrs	x2, midr_el1
 	msr	vpidr_el2, x2
@@ -354,41 +359,51 @@ LENTRY(enter_kernel_el)
 	ldr	x2, =INIT_SCTLR_EL1
 	msr	sctlr_el1, x2
 
+	/* Check if the E2H flag is set */
+	tst	x4, #HCR_E2H
+	b.eq	.Lno_vhe
+
 	/*
-	 * On some hardware, e.g., Apple M1, we can't clear E2H, so make sure we
-	 * don't trap to EL2 for SIMD register usage to have at least a
-	 * minimally usable system.
+	 * The kernel will be running in EL2, route exceptions here rather
+	 * than EL1.
 	 */
-	tst	x4, #HCR_E2H
-	mov	x3, #CPTR_RES1	/* HCR_E2H == 0 */
-	mov	x5, #CPTR_FPEN	/* HCR_E2H == 1 */
-	csel	x2, x3, x5, eq
+	orr	x4, x4, #(HCR_TGE)
+	msr	hcr_el2, x4
+	isb
+
+	msr	SCTLR_EL12_REG, x2
+	ldr	x2, =(CPTR_FPEN)
+	ldr	x3, =(CNTHCTL_E2H_EL1PCTEN | CNTHCTL_E2H_EL1PTEN)
+	ldr	x5, =(PSR_DAIF | PSR_M_EL2h)
+	b	.Ldone_vhe
+
+.Lno_vhe:
+	/* Hypervisor trap functions */
+	adrp	x2, hyp_stub_vectors
+	add	x2, x2, :lo12:hyp_stub_vectors
+	msr	vbar_el2, x2
+
+	ldr	x2, =(CPTR_RES1)
+	ldr	x3, =(CNTHCTL_EL1PCTEN | CNTHCTL_EL1PCEN)
+	ldr	x5, =(PSR_DAIF | PSR_M_EL1h)
+
+.Ldone_vhe:
+
 	msr	cptr_el2, x2
+	/* Enable access to the physical timers at EL1 */
+	msr	cnthctl_el2, x3
+	/* Set the return PSTATE */
+	msr	spsr_el2, x5
 
 	/* Don't trap to EL2 for CP15 traps */
 	msr	hstr_el2, xzr
 
-	/* Enable access to the physical timers at EL1 */
-	tst	x4, #HCR_E2H
-	ldr	x3, =(CNTHCTL_EL1PCTEN | CNTHCTL_EL1PCEN)
-	ldr	x5, =(CNTHCTL_E2H_EL1PCTEN | CNTHCTL_E2H_EL1PTEN)
-	csel	x2, x3, x5, eq
-	msr	cnthctl_el2, x2
-
 	/* Set the counter offset to a known value */
 	msr	cntvoff_el2, xzr
 
-	/* Hypervisor trap functions */
-	adrp	x2, hyp_stub_vectors
-	add	x2, x2, :lo12:hyp_stub_vectors
-	msr	vbar_el2, x2
-
 	/* Zero vttbr_el2 so a hypervisor can tell the host and guest apart */
 	msr	vttbr_el2, xzr
 
-	mov	x2, #(PSR_DAIF | PSR_M_EL1h)
-	msr	spsr_el2, x2
-
 	/* Configure GICv3 CPU interface */
 	mrs	x2, id_aa64pfr0_el1
 	/* Extract GIC bits from the register */