If VHE is supported, continue booting at EL2, otherwise continue booting at EL1. Signed-off-by: Joey Gouly <joey.gouly@xxxxxxx> --- arm/cstart64.S | 17 +++++++++++++---- lib/arm64/asm/sysreg.h | 5 +++++ 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/arm/cstart64.S b/arm/cstart64.S index 79b93dd4..af7c81c1 100644 --- a/arm/cstart64.S +++ b/arm/cstart64.S @@ -18,7 +18,7 @@ .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 +41,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
