Merge tag 'arm64-upstream' of git://git.kernel.org/pub/scm/linux/kernel/git/arm64...

[cascardo/linux.git] / arch / arm64 / kernel / head.S

diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index a2cf0e3..4203d5f 100644 (file)
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -31,6 +31,7 @@
 #include <asm/cputype.h>
 #include <asm/elf.h>
 #include <asm/kernel-pgtable.h>
+#include <asm/kvm_arm.h>
 #include <asm/memory.h>
 #include <asm/pgtable-hwdef.h>
 #include <asm/pgtable.h>
@@ -534,9 +535,27 @@ CPU_LE(    bic     x0, x0, #(3 << 24)      )       // Clear the EE and E0E bits for EL1
        isb
        ret
 
+2:
+#ifdef CONFIG_ARM64_VHE
+       /*
+        * Check for VHE being present. For the rest of the EL2 setup,
+        * x2 being non-zero indicates that we do have VHE, and that the
+        * kernel is intended to run at EL2.
+        */
+       mrs     x2, id_aa64mmfr1_el1
+       ubfx    x2, x2, #8, #4
+#else
+       mov     x2, xzr
+#endif
+
        /* Hyp configuration. */
-2:     mov     x0, #(1 << 31)                  // 64-bit EL1
+       mov     x0, #HCR_RW                     // 64-bit EL1
+       cbz     x2, set_hcr
+       orr     x0, x0, #HCR_TGE                // Enable Host Extensions
+       orr     x0, x0, #HCR_E2H
+set_hcr:
        msr     hcr_el2, x0
+       isb
 
        /* Generic timers. */
        mrs     x0, cnthctl_el2
@@ -596,6 +615,13 @@ CPU_LE(    movk    x0, #0x30d0, lsl #16    )       // Clear EE and E0E on LE systems
        /* Stage-2 translation */
        msr     vttbr_el2, xzr
 
+       cbz     x2, install_el2_stub
+
+       mov     w20, #BOOT_CPU_MODE_EL2         // This CPU booted in EL2
+       isb
+       ret
+
+install_el2_stub:
        /* Hypervisor stub */
        adrp    x0, __hyp_stub_vectors
        add     x0, x0, #:lo12:__hyp_stub_vectors