If VHE is supported, continue booting at EL2, otherwise continue booting at EL1. Signed-off-by: Joey Gouly Acked-by: Marc Zyngier Reviewed-by: Eric Auger --- arm/cstart64.S | 21 +++++++++++++++++---- lib/arm64/asm/sysreg.h | 5 +++++ 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/arm/cstart64.S b/arm/cstart64.S index 2b93f234..49cf8ed6 100644 --- a/arm/cstart64.S +++ b/arm/cstart64.S @@ -15,10 +15,14 @@ #include #include +/* + * Initialise the EL used for running the tests. + * If started at EL2 and VHE is supported, EL2 is used, otherwise EL1 is used. + */ .macro init_el, tmp mrs \tmp, CurrentEL cmp \tmp, CurrentEL_EL2 - b.ne 1f + b.ne 2f /* EL2 setup */ mrs \tmp, mpidr_el1 msr vmpidr_el2, \tmp @@ -41,17 +45,26 @@ msr_s SYS_HFGWTR2_EL2, \tmp msr_s SYS_HFGITR2_EL2, \tmp .Lskip_fgt_\@: + /* check VHE is supported */ + mrs \tmp, ID_AA64MMFR1_EL1 + ubfx \tmp, \tmp, ID_AA64MMFR1_EL1_VH_SHIFT, #4 + cbz \tmp, 1f + ldr \tmp, =(INIT_HCR_EL2) + msr hcr_el2, \tmp + isb + b 2f +1: mov \tmp, #0 msr cptr_el2, \tmp ldr \tmp, =(INIT_HCR_EL2_EL1_ONLY) msr hcr_el2, \tmp mov \tmp, PSR_MODE_EL1t msr spsr_el2, \tmp - adrp \tmp, 1f - add \tmp, \tmp, :lo12:1f + adrp \tmp, 2f + add \tmp, \tmp, :lo12:2f msr elr_el2, \tmp eret -1: +2: .endm diff --git a/lib/arm64/asm/sysreg.h b/lib/arm64/asm/sysreg.h index ed776716..f2d05018 100644 --- a/lib/arm64/asm/sysreg.h +++ b/lib/arm64/asm/sysreg.h @@ -80,6 +80,8 @@ asm( #define ID_AA64MMFR0_EL1_FGT_SHIFT 56 #define ID_AA64MMFR0_EL1_FGT_FGT2 0x2 +#define ID_AA64MMFR1_EL1_VH_SHIFT 8 + #define ICC_PMR_EL1 sys_reg(3, 0, 4, 6, 0) #define ICC_SGI1R_EL1 sys_reg(3, 0, 12, 11, 5) #define ICC_IAR1_EL1 sys_reg(3, 0, 12, 12, 0) @@ -116,9 +118,12 @@ asm( #define SCTLR_EL1_TCF0_SHIFT 38 #define SCTLR_EL1_TCF0_MASK GENMASK_ULL(39, 38) +#define HCR_EL2_TGE _BITULL(27) #define HCR_EL2_RW _BITULL(31) +#define HCR_EL2_E2H _BITULL(34) #define INIT_HCR_EL2_EL1_ONLY (HCR_EL2_RW) +#define INIT_HCR_EL2 (HCR_EL2_TGE | HCR_EL2_E2H | HCR_EL2_RW) #define SYS_HFGRTR_EL2 sys_reg(3, 4, 1, 1, 4) #define SYS_HFGWTR_EL2 sys_reg(3, 4, 1, 1, 5) -- 2.25.1