Merge tag 'arm-dt-6.0' of git://git.kernel.org/pub/scm/linux/kernel/git/soc/soc

[linux-2.6-microblaze.git] / arch / arm64 / kernel / hyp-stub.S

diff --git a/arch/arm64/kernel/hyp-stub.S b/arch/arm64/kernel/hyp-stub.S
index de1ab98..12c7fad 100644 (file)
--- a/arch/arm64/kernel/hyp-stub.S
+++ b/arch/arm64/kernel/hyp-stub.S
@@ -98,6 +98,59 @@ SYM_CODE_START_LOCAL(elx_sync)
 SYM_CODE_END(elx_sync)
 
 SYM_CODE_START_LOCAL(__finalise_el2)
+       check_override id_aa64pfr0 ID_AA64PFR0_SVE_SHIFT .Linit_sve .Lskip_sve
+
+.Linit_sve:    /* SVE register access */
+       mrs     x0, cptr_el2                    // Disable SVE traps
+       bic     x0, x0, #CPTR_EL2_TZ
+       msr     cptr_el2, x0
+       isb
+       mov     x1, #ZCR_ELx_LEN_MASK           // SVE: Enable full vector
+       msr_s   SYS_ZCR_EL2, x1                 // length for EL1.
+
+.Lskip_sve:
+       check_override id_aa64pfr1 ID_AA64PFR1_SME_SHIFT .Linit_sme .Lskip_sme
+
+.Linit_sme:    /* SME register access and priority mapping */
+       mrs     x0, cptr_el2                    // Disable SME traps
+       bic     x0, x0, #CPTR_EL2_TSM
+       msr     cptr_el2, x0
+       isb
+
+       mrs     x1, sctlr_el2
+       orr     x1, x1, #SCTLR_ELx_ENTP2        // Disable TPIDR2 traps
+       msr     sctlr_el2, x1
+       isb
+
+       mov     x0, #0                          // SMCR controls
+
+       // Full FP in SM?
+       mrs_s   x1, SYS_ID_AA64SMFR0_EL1
+       __check_override id_aa64smfr0 ID_AA64SMFR0_EL1_FA64_SHIFT 1 .Linit_sme_fa64 .Lskip_sme_fa64
+
+.Linit_sme_fa64:
+       orr     x0, x0, SMCR_ELx_FA64_MASK
+.Lskip_sme_fa64:
+
+       orr     x0, x0, #SMCR_ELx_LEN_MASK      // Enable full SME vector
+       msr_s   SYS_SMCR_EL2, x0                // length for EL1.
+
+       mrs_s   x1, SYS_SMIDR_EL1               // Priority mapping supported?
+       ubfx    x1, x1, #SMIDR_EL1_SMPS_SHIFT, #1
+       cbz     x1, .Lskip_sme
+
+       msr_s   SYS_SMPRIMAP_EL2, xzr           // Make all priorities equal
+
+       mrs     x1, id_aa64mmfr1_el1            // HCRX_EL2 present?
+       ubfx    x1, x1, #ID_AA64MMFR1_HCX_SHIFT, #4
+       cbz     x1, .Lskip_sme
+
+       mrs_s   x1, SYS_HCRX_EL2
+       orr     x1, x1, #HCRX_EL2_SMPME_MASK    // Enable priority mapping
+       msr_s   SYS_HCRX_EL2, x1
+
+.Lskip_sme:
+
        // nVHE? No way! Give me the real thing!
        // Sanity check: MMU *must* be off
        mrs     x1, sctlr_el2