arm64: Use CPACR_EL1 format to set CPTR_EL2 when E2H is set

When HCR_EL2.E2H is set, the CPTR_EL2 register takes the CPACR_EL1
format. Yes, this is good fun.

Hack the bits of startup code that assume E2H=0 while setting up
CPTR_EL2 to make them grok the CPTR_EL1 format.

Signed-off-by: Marc Zyngier <maz@kernel.org>
Reviewed-by: Catalin Marinas <catalin.marinas@arm.com>
Link: https://lore.kernel.org/r/20230609162200.2024064-8-maz@kernel.org
Signed-off-by: Oliver Upton <oliver.upton@linux.dev>

diff --git a/arch/arm64/include/asm/el2_setup.h b/arch/arm64/include/asm/el2_setup.h
index 225bf1f..bba508f 100644 (file)
--- a/arch/arm64/include/asm/el2_setup.h
+++ b/arch/arm64/include/asm/el2_setup.h
@@ -129,8 +129,15 @@
 .endm
 
 /* Coprocessor traps */
-.macro __init_el2_nvhe_cptr
+.macro __init_el2_cptr
+       mrs     x1, hcr_el2
+       and     x1, x1, #HCR_E2H
+       cbz     x1, .LnVHE_\@
+       mov     x0, #(CPACR_EL1_FPEN_EL1EN | CPACR_EL1_FPEN_EL0EN)
+       b       .Lset_cptr_\@
+.LnVHE_\@:
        mov     x0, #0x33ff
+.Lset_cptr_\@:
        msr     cptr_el2, x0                    // Disable copro. traps to EL2
 .endm
 
@@ -196,7 +203,7 @@
        __init_el2_gicv3
        __init_el2_hstr
        __init_el2_nvhe_idregs
-       __init_el2_nvhe_cptr
+       __init_el2_cptr
        __init_el2_fgt
        __init_el2_nvhe_prepare_eret
 .endm
@@ -244,7 +251,17 @@
 
 .Linit_sve_\@: /* SVE register access */
        mrs     x0, cptr_el2                    // Disable SVE traps
+       mrs     x1, hcr_el2
+       and     x1, x1, #HCR_E2H
+       cbz     x1, .Lcptr_nvhe_\@
+
+       // VHE case
+       orr     x0, x0, #(CPACR_EL1_ZEN_EL1EN | CPACR_EL1_ZEN_EL0EN)
+       b       .Lset_cptr_\@
+
+.Lcptr_nvhe_\@: // nVHE case
        bic     x0, x0, #CPTR_EL2_TZ
+.Lset_cptr_\@:
        msr     cptr_el2, x0
        isb
        mov     x1, #ZCR_ELx_LEN_MASK           // SVE: Enable full vector