refactor(cpufeat): enable FEAT_TWED for FEAT_STATE_CHECKED

At the moment we only support FEAT_TWED to be either unconditionally
compiled in, or to be not supported at all.

Add support for runtime detection (ENABLE_FEAT_TWED=2), by splitting
is_armv8_6_twed_present() into an ID register reading function and a
second function to report the support status. That function considers
both build time settings and runtime information (if needed), and is
used before we set the trap delay time.

Change the FVP platform default to the now supported dynamic option (=2),
so the right decision can be made by the code at runtime.

Change-Id: I58626230ef0af49886c0a197abace01e81f661d2
Signed-off-by: Andre Przywara <andre.przywara@arm.com>

4 files changed, 27 insertions, 23 deletions

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);
-
-	/* Enable WFE delay */
-	scr_el3 |= SCR_TWEDEn_BIT;
-#endif /* ENABLE_FEAT_TWED */
+	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;
+	}
 
 	/*
 	 * 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