sys/smp: Fix aarch64 AP trampoline EL2 stay path for non-VHE APs

common/sys/smp_trampoline.asm_aarch64

@@ -38,7 +38,11 @@ smp_trampoline_start:
     mrs x8, hcr_el2
     tbnz x8, #34, 6f
 
-    // Non-VHE: configure real EL1 page tables directly
+    // Non-VHE: check if we should stay at EL2 anyway
+    ldr x8, [x1, tpl_enter_in_el2]
+    cbnz x8, 10f
+
+    // Non-VHE drop to EL1: configure real EL1 page tables directly
     msr mair_el1, x3
     msr tcr_el1, x4
     msr ttbr0_el1, x5
@@ -49,6 +53,14 @@ smp_trampoline_start:
     isb
     b 7f
 
+10:
+    // Enable E2H on this AP and use VHE stay path
+    mrs x8, hcr_el2
+    orr x8, x8, #(1 << 34)
+    msr hcr_el2, x8
+    isb
+    b 8f
+
 6:
     // VHE (E2H=1): check if we should stay at EL2
     ldr x8, [x1, tpl_enter_in_el2]
@@ -66,17 +78,9 @@ smp_trampoline_start:
     b 7f
 
 8:
-    // VHE stay at EL2: use plain EL1 names (VHE redirects to EL2 registers)
-    msr mair_el1, x3
-    msr tcr_el1, x4
-    msr ttbr0_el1, x5
-    msr ttbr1_el1, x6
-    msr sctlr_el1, x2
-    isb
-    dsb sy
-    isb
+    // VHE stay at EL2: configure EL2 state before enabling MMU
+    msr spsel, #0
 
-    // Configure EL2 state for VHE
     mrs x8, cnthctl_el2
     orr x8, x8, #3
     msr cnthctl_el2, x8
@@ -98,10 +102,26 @@ smp_trampoline_start:
     // No stage 2 translation
     msr vttbr_el2, xzr
 
-    // Jump directly to higher-half continuation (MMU just enabled from off)
-    adrp x8, 3f
-    add x8, x8, :lo12:3f
+    // Set up VBAR for page table switch (VHE redirects VBAR_EL1 to VBAR_EL2)
+    adrp x8, 2f
+    add x8, x8, :lo12:2f
     add x8, x8, x7
+    msr vbar_el1, x8
+    isb
+    dsb sy
+    isb
+
+    // Switch page tables using VHE-redirected register names
+    msr mair_el1, x3
+    msr tcr_el1, x4
+    msr ttbr0_el1, x5
+    msr ttbr1_el1, x6
+    msr sctlr_el1, x2
+    isb
+    dsb sy
+    isb
+
+    // Jump to the higher half mapping in case we didn't immediately crash
     br x8
 
 7: