diff --git a/common/feat_detect.c b/common/feat_detect.c index bdadde1f4..b335a610f 100644 --- a/common/feat_detect.c +++ b/common/feat_detect.c @@ -154,16 +154,6 @@ static void read_feat_amuv1p1(void) #endif } -/*********************************************************** - * Feature : FEAT_TWED (Delayed Trapping of WFE Instruction) - **********************************************************/ -static void read_feat_twed(void) -{ -#if (ENABLE_FEAT_TWED == FEAT_STATE_ALWAYS) - feat_detect_panic(is_armv8_6_twed_present(), "TWED"); -#endif -} - /************************************************** * Feature : FEAT_RME (Realm Management Extension) *************************************************/ @@ -248,7 +238,8 @@ void detect_arch_features(void) read_feat_amuv1p1(); check_feature(ENABLE_FEAT_FGT, read_feat_fgt_id_field(), "FGT", 1, 1); check_feature(ENABLE_FEAT_ECV, read_feat_ecv_id_field(), "ECV", 1, 2); - read_feat_twed(); + check_feature(ENABLE_FEAT_TWED, read_feat_twed_id_field(), + "TWED", 1, 1); /* v8.7 features */ check_feature(ENABLE_FEAT_HCX, read_feat_hcx_id_field(), "HCX", 1, 1); diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h index 0342d2f7c..fa8015476 100644 --- a/include/arch/aarch64/arch_features.h +++ b/include/arch/aarch64/arch_features.h @@ -119,10 +119,22 @@ static inline bool is_armv8_4_sel2_present(void) ID_AA64PFR0_SEL2_MASK) == 1ULL; } -static inline bool is_armv8_6_twed_present(void) +static inline unsigned int read_feat_twed_id_field(void) { - return (((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_TWED_SHIFT) & - ID_AA64MMFR1_EL1_TWED_MASK) == ID_AA64MMFR1_EL1_TWED_SUPPORTED); + return ISOLATE_FIELD(read_id_aa64mmfr1_el1(), ID_AA64MMFR1_EL1_TWED); +} + +static inline bool is_feat_twed_supported(void) +{ + if (ENABLE_FEAT_TWED == FEAT_STATE_DISABLED) { + return false; + } + + if (ENABLE_FEAT_TWED == FEAT_STATE_ALWAYS) { + return true; + } + + return read_feat_twed_id_field() != 0U; } static unsigned int read_feat_fgt_id_field(void) diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c index 8430c06e8..8f46b80dc 100644 --- a/lib/el3_runtime/aarch64/context_mgmt.c +++ b/lib/el3_runtime/aarch64/context_mgmt.c @@ -385,16 +385,16 @@ static void setup_context_common(cpu_context_t *ctx, const entry_point_info_t *e } } -#if ENABLE_FEAT_TWED /* Enable WFE trap delay in SCR_EL3 if supported and configured */ - /* Set delay in SCR_EL3 */ - scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT); - scr_el3 |= ((TWED_DELAY & SCR_TWEDEL_MASK) - << SCR_TWEDEL_SHIFT); + if (is_feat_twed_supported()) { + /* Set delay in SCR_EL3 */ + scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT); + scr_el3 |= ((TWED_DELAY & SCR_TWEDEL_MASK) + << SCR_TWEDEL_SHIFT); - /* Enable WFE delay */ - scr_el3 |= SCR_TWEDEn_BIT; -#endif /* ENABLE_FEAT_TWED */ + /* Enable WFE delay */ + scr_el3 |= SCR_TWEDEn_BIT; + } /* * Populate EL3 state so that we've the right context diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk index 03fe9cc0b..a6f8eaf64 100644 --- a/plat/arm/board/fvp/platform.mk +++ b/plat/arm/board/fvp/platform.mk @@ -473,6 +473,7 @@ ENABLE_FEAT_TCR2 := 2 ENABLE_FEAT_CSV2_2 := 2 ENABLE_FEAT_ECV := 2 ENABLE_FEAT_PAN := 2 +ENABLE_FEAT_TWED := 2 ENABLE_FEAT_VHE := 2 ENABLE_MPAM_FOR_LOWER_ELS := 2