From 1f8be7fc66fb59b197dde3b4ea83314b1728c6b8 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Tue, 21 Mar 2023 14:44:59 +0000
Subject: [PATCH 01/12] fix(mpam): feat_detect: support major/minor

The MPAM CPU ID version number is split between two CPU ID register
fields, with the second being a fractional field, allowing for instance
for a "MPAM v1.1" number. The read_feat_mpam_version() function merges
those two fields to form a "4.4" fixed point fractional number, but the
limit check in the check_feature() function was not taking this into
account.
To support MPAM major version 1, extend the limit from "1" to "17", to
cover the current maximum version of "MPAM v1.1".
This fixes FVP runs with "has_mpam=1" and FEATURE_DETECTION enabled.

Change-Id: Icb557741d597e4e43eaf658b78f18af6e9fb439e
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 common/feat_detect.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common/feat_detect.c b/common/feat_detect.c
index abb6d718c..7a6c9193e 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -274,7 +274,7 @@ void detect_arch_features(void)
 	check_feature(ENABLE_FEAT_AMUv1, read_feat_amu_id_field(),
 		      "AMUv1", 1, 2);
 	check_feature(ENABLE_MPAM_FOR_LOWER_ELS, read_feat_mpam_version(),
-		      "MPAM", 1, 1);
+		      "MPAM", 1, 17);
 	read_feat_nv2();
 	read_feat_sel2();
 	check_feature(ENABLE_TRF_FOR_NS, read_feat_trf_id_field(),

From d7f3ed3655b85223583d8c2d9e719f364266ef26 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Wed, 22 Mar 2023 13:25:00 +0000
Subject: [PATCH 02/12] fix(cpufeat): make stub enable functions "static
 inline"

For builds where we don't have a certain feature enabled, we provide
empty stub xxx_enable() functions in a header file. This way we
avoid #ifdef's in the code, and can call the enable function
unconditionally. When compiling with -O1 or higher, the compiler will
even optimise out the whole call, so the symbol will never make it into
any object file.
When compiling with optimisations turned off, the function stub will
survive, and could make it into multiple object files, which would lead
to a multiple definitons error.

Avoid this by defining those stub functions as "static inline". The
"static" will avoid the multiple definitions problems, the "inline" will
avoid a potential compiler warning about unused functions.
This patterns is used extensively in the Linux kernel.

Change-Id: Iad07bb946aab372587c83f2423b4983bf3817990
Reported-by: Chris Kay <chris.kay@arm.com>
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 include/lib/extensions/brbe.h | 2 +-
 include/lib/extensions/mpam.h | 2 +-
 include/lib/extensions/spe.h  | 4 ++--
 include/lib/extensions/trbe.h | 2 +-
 include/lib/extensions/trf.h  | 2 +-
 5 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/include/lib/extensions/brbe.h b/include/lib/extensions/brbe.h
index aee208d04..9ee2444f6 100644
--- a/include/lib/extensions/brbe.h
+++ b/include/lib/extensions/brbe.h
@@ -10,7 +10,7 @@
 #if ENABLE_BRBE_FOR_NS
 void brbe_enable(void);
 #else
-void brbe_enable(void)
+static inline void brbe_enable(void)
 {
 }
 #endif /* ENABLE_BRBE_FOR_NS */
diff --git a/include/lib/extensions/mpam.h b/include/lib/extensions/mpam.h
index 120a921d1..4327278e0 100644
--- a/include/lib/extensions/mpam.h
+++ b/include/lib/extensions/mpam.h
@@ -12,7 +12,7 @@
 #if ENABLE_MPAM_FOR_LOWER_ELS
 void mpam_enable(bool el2_unused);
 #else
-void mpam_enable(bool el2_unused)
+static inline void mpam_enable(bool el2_unused)
 {
 }
 #endif
diff --git a/include/lib/extensions/spe.h b/include/lib/extensions/spe.h
index d443f18fb..02fccae92 100644
--- a/include/lib/extensions/spe.h
+++ b/include/lib/extensions/spe.h
@@ -13,10 +13,10 @@
 void spe_enable(bool el2_unused);
 void spe_disable(void);
 #else
-void spe_enable(bool el2_unused)
+static inline void spe_enable(bool el2_unused)
 {
 }
-void spe_disable(void)
+static inline void spe_disable(void)
 {
 }
 #endif
diff --git a/include/lib/extensions/trbe.h b/include/lib/extensions/trbe.h
index 0eff7c4ee..861a4ad65 100644
--- a/include/lib/extensions/trbe.h
+++ b/include/lib/extensions/trbe.h
@@ -10,7 +10,7 @@
 #if ENABLE_TRBE_FOR_NS
 void trbe_enable(void);
 #else
-void trbe_enable(void)
+static inline void trbe_enable(void)
 {
 }
 #endif /* ENABLE_TRBE_FOR_NS */
diff --git a/include/lib/extensions/trf.h b/include/lib/extensions/trf.h
index a440abd4b..91a96153c 100644
--- a/include/lib/extensions/trf.h
+++ b/include/lib/extensions/trf.h
@@ -10,7 +10,7 @@
 #if ENABLE_TRF_FOR_NS
 void trf_enable(void);
 #else
-void trf_enable(void)
+static inline void trf_enable(void)
 {
 }
 #endif /* ENABLE_TRF_FOR_NS */

From 603a0c6faef4194bd58fd2104971e4100648ed1c Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Thu, 17 Nov 2022 16:42:09 +0000
Subject: [PATCH 03/12] refactor(cpufeat): enable SYS_REG_TRACE for
 FEAT_STATE_CHECKED

At the moment we only support access to the trace unit by system
registers (SYS_REG_TRACE) to be either unconditionally compiled in, or
to be not supported at all.

Add support for runtime detection (ENABLE_SYS_REG_TRACE_FOR_NS=2), by
adding is_feat_sys_reg_trace_supported(). That function considers both
build time settings and runtime information (if needed), and is used
before we access SYS_REG_TRACE related registers.

The FVP platform decided to compile in support unconditionally (=1),
even though this is an optional feature, so it is not available with the
FVP model's default command line.
Change that to the now supported dynamic option (=2), so the right
decision can be made by the code at runtime.

Change-Id: I450a574a4f6bd9fc269887037049c94c906f54b2
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 Makefile                                      |  2 +-
 bl31/bl31.mk                                  |  2 +-
 bl32/sp_min/sp_min.mk                         |  2 +-
 docs/getting_started/build-options.rst        |  5 ++--
 include/arch/aarch32/arch_features.h          | 18 ++++++++++++
 include/arch/aarch64/arch_features.h          | 18 ++++++++++++
 include/lib/extensions/sys_reg_trace.h        | 14 +++++++++
 lib/el3_runtime/aarch32/context_mgmt.c        |  6 ++--
 lib/el3_runtime/aarch64/context_mgmt.c        |  6 ++--
 .../sys_reg_trace/aarch32/sys_reg_trace.c     | 27 +++++------------
 .../sys_reg_trace/aarch64/sys_reg_trace.c     | 29 ++++++-------------
 plat/arm/board/fvp/platform.mk                |  2 +-
 12 files changed, 80 insertions(+), 51 deletions(-)

diff --git a/Makefile b/Makefile
index 797b2cfeb..8e789def0 100644
--- a/Makefile
+++ b/Makefile
@@ -1143,7 +1143,6 @@ $(eval $(call assert_booleans,\
         COT_DESC_IN_DTB \
         USE_SP804_TIMER \
         PSA_FWU_SUPPORT \
-        ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
         SIMICS_BUILD \
@@ -1182,6 +1181,7 @@ $(eval $(call assert_numerics,\
         ENABLE_MPAM_FOR_LOWER_ELS \
         ENABLE_RME \
         ENABLE_SPE_FOR_NS \
+        ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_TRF_FOR_NS \
         FW_ENC_STATUS \
         NR_OF_FW_BANKS \
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 91406cfc5..006843efd 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -120,7 +120,7 @@ ifneq (${ENABLE_BRBE_FOR_NS},0)
 BL31_SOURCES		+=	lib/extensions/brbe/brbe.c
 endif
 
-ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
+ifneq (${ENABLE_SYS_REG_TRACE_FOR_NS},0)
 BL31_SOURCES		+=      lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
 endif
 
diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index 2a6612ad9..e85e2738c 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk
@@ -46,7 +46,7 @@ BL32_SOURCES		+=	services/std_svc/trng/trng_main.c	\
 				services/std_svc/trng/trng_entropy_pool.c
 endif
 
-ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
+ifneq (${ENABLE_SYS_REG_TRACE_FOR_NS},0)
 BL32_SOURCES		+=	lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
 endif
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 9415871df..94c12defe 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -1079,10 +1079,11 @@ Common build options
   ``FEATURE_DETECTION`` mechanism. The default is 0 and it is automatically
   disabled when the target architecture is AArch32.
 
-- ``ENABLE_SYS_REG_TRACE_FOR_NS``: Boolean option to enable trace system
+- ``ENABLE_SYS_REG_TRACE_FOR_NS``: Numeric value to enable trace system
   registers access from NS ELs, NS-EL2 or NS-EL1 (when NS-EL2 is implemented
   but unused). This feature is available if trace unit such as ETMv4.x, and
-  ETE(extending ETM feature) is implemented. This flag is disabled by default.
+  ETE(extending ETM feature) is implemented. This flag can take the values
+  0 to 2, to align with the ``FEATURE_DETECTION`` mechanism. The default is 0.
 
 - ``ENABLE_TRF_FOR_NS``: Numeric value to enable trace filter control registers
   access from NS ELs, NS-EL2 or NS-EL1 (when NS-EL2 is implemented but unused),
diff --git a/include/arch/aarch32/arch_features.h b/include/arch/aarch32/arch_features.h
index 12df6da4b..252b40734 100644
--- a/include/arch/aarch32/arch_features.h
+++ b/include/arch/aarch32/arch_features.h
@@ -43,6 +43,24 @@ static inline bool is_feat_trf_supported(void)
 	return read_feat_trf_id_field() != 0U;
 }
 
+static inline unsigned int read_feat_coptrc_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_dfr0(), ID_DFR0_COPTRC);
+}
+
+static inline bool is_feat_sys_reg_trace_supported(void)
+{
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_coptrc_id_field() != 0U;
+}
+
 static inline bool is_feat_spe_supported(void)
 {
 	/* FEAT_SPE is AArch64 only */
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index f1a13d290..ad938aea3 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -320,6 +320,24 @@ static inline bool is_armv8_4_feat_dit_present(void)
 		ID_AA64PFR0_DIT_MASK) == ID_AA64PFR0_DIT_SUPPORTED);
 }
 
+static inline unsigned int read_feat_tracever_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64dfr0_el1(), ID_AA64DFR0_TRACEVER);
+}
+
+static inline bool is_feat_sys_reg_trace_supported(void)
+{
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_tracever_id_field() != 0U;
+}
+
 /*************************************************************************
  * Function to identify the presence of FEAT_TRF (TraceLift)
  ************************************************************************/
diff --git a/include/lib/extensions/sys_reg_trace.h b/include/lib/extensions/sys_reg_trace.h
index 74470fee2..5915c5547 100644
--- a/include/lib/extensions/sys_reg_trace.h
+++ b/include/lib/extensions/sys_reg_trace.h
@@ -9,10 +9,24 @@
 
 #include <context.h>
 
+#if ENABLE_SYS_REG_TRACE_FOR_NS
 #if __aarch64__
 void sys_reg_trace_enable(cpu_context_t *context);
 #else
 void sys_reg_trace_enable(void);
 #endif /* __aarch64__ */
 
+#else /* !ENABLE_SYS_REG_TRACE_FOR_NS */
+
+#if __aarch64__
+static inline void sys_reg_trace_enable(cpu_context_t *context)
+{
+}
+#else
+static inline void sys_reg_trace_enable(void)
+{
+}
+#endif /* __aarch64__ */
+#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
+
 #endif /* SYS_REG_TRACE_H */
diff --git a/lib/el3_runtime/aarch32/context_mgmt.c b/lib/el3_runtime/aarch32/context_mgmt.c
index e494a86cf..e7a0e5871 100644
--- a/lib/el3_runtime/aarch32/context_mgmt.c
+++ b/lib/el3_runtime/aarch32/context_mgmt.c
@@ -140,9 +140,9 @@ static void enable_extensions_nonsecure(bool el2_unused)
 	amu_enable(el2_unused);
 #endif
 
-#if ENABLE_SYS_REG_TRACE_FOR_NS
-	sys_reg_trace_enable();
-#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_enable();
+	}
 
 	if (is_feat_trf_supported()) {
 		trf_enable();
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 20eb5f67d..c91070ece 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -510,9 +510,9 @@ static void manage_extensions_nonsecure(bool el2_unused, cpu_context_t *ctx)
 		brbe_enable();
 	}
 
-#if ENABLE_SYS_REG_TRACE_FOR_NS
-	sys_reg_trace_enable(ctx);
-#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_enable(ctx);
+	}
 
 	if (is_feat_trf_supported()) {
 		trf_enable();
diff --git a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
index 89b8029ca..b3f44b7c1 100644
--- a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
+++ b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
@@ -10,27 +10,16 @@
 #include <arch_helpers.h>
 #include <lib/extensions/sys_reg_trace.h>
 
-static bool sys_reg_trace_supported(void)
-{
-	uint32_t features;
-
-	features = read_id_dfr0() >> ID_DFR0_COPTRC_SHIFT;
-	return ((features & ID_DFR0_COPTRC_MASK) ==
-		ID_DFR0_COPTRC_SUPPORTED);
-}
-
 void sys_reg_trace_enable(void)
 {
 	uint32_t val;
 
-	if (sys_reg_trace_supported()) {
-		/*
-		 * NSACR.NSTRCDIS = b0
-		 * enable NS system register access to implemented trace
-		 * registers.
-		 */
-		val = read_nsacr();
-		val &= ~NSTRCDIS_BIT;
-		write_nsacr(val);
-	}
+	/*
+	 * NSACR.NSTRCDIS = b0
+	 * enable NS system register access to implemented trace
+	 * registers.
+	 */
+	val = read_nsacr();
+	val &= ~NSTRCDIS_BIT;
+	write_nsacr(val);
 }
diff --git a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
index 960d69842..e61cb9037 100644
--- a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
+++ b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
@@ -10,28 +10,17 @@
 #include <arch_helpers.h>
 #include <lib/extensions/sys_reg_trace.h>
 
-static bool sys_reg_trace_supported(void)
-{
-	uint64_t features;
-
-	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_TRACEVER_SHIFT;
-	return ((features & ID_AA64DFR0_TRACEVER_MASK) ==
-		ID_AA64DFR0_TRACEVER_SUPPORTED);
-}
-
 void sys_reg_trace_enable(cpu_context_t *ctx)
 {
 	uint64_t val;
 
-	if (sys_reg_trace_supported()) {
-		/* Retrieve CPTR_EL3 value from the given context 'ctx',
-		 * and update CPTR_EL3.TTA bit to 0.
-		 * This function is called while switching context to NS to
-		 * allow system trace register access to NS-EL2 and NS-EL1
-		 * when NS-EL2 is implemented but not used.
-		 */
-		val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
-		val &= ~TTA_BIT;
-		write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
-	}
+	/* Retrieve CPTR_EL3 value from the given context 'ctx',
+	 * and update CPTR_EL3.TTA bit to 0.
+	 * This function is called while switching context to NS to
+	 * allow system trace register access to NS-EL2 and NS-EL1
+	 * when NS-EL2 is implemented but not used.
+	 */
+	val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+	val &= ~TTA_BIT;
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
 }
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 6acc879e1..d5e59cd25 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -460,7 +460,7 @@ endif
 endif
 
 # enable trace system registers access to NS by default
-ENABLE_SYS_REG_TRACE_FOR_NS	:= 1
+ENABLE_SYS_REG_TRACE_FOR_NS	:= 2
 
 # enable trace filter control registers access to NS by default
 ENABLE_TRF_FOR_NS		:= 2

From 387b8801f956204b2970a5b65de0f9ad6821a4b7 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Fri, 25 Nov 2022 14:10:13 +0000
Subject: [PATCH 04/12] refactor(cpufeat): use alternative encoding for "SB"
 barrier

The "sb" barrier instruction is a rather new addition to the AArch64
instruction set, so it is not recognised by all toolchains. On top of
that, the GNU assembler denies this instruction, unless a compatible
processor is selected:
asm_macros.S:223: Error: selected processor does not support `sb'

Provide an alternative encoding of the "sb" instruction, by using a
system register write, as this is the group where the barrier
instructions borrow their encoding space from.
This results in the exact same opcode to be generated, and any
disassembler will decode this instruction as "sb".

Change-Id: I5f44c8321e0cc04c784e02bd838e964602a96a8e
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 include/arch/aarch64/arch.h       |  3 +++
 include/arch/aarch64/asm_macros.S | 16 ++++++++++++++--
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 85546ec47..58abd449e 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -1335,4 +1335,7 @@
 #define CPUMPMMCR_EL3_MPMM_EN_SHIFT	UINT64_C(0)
 #define CPUMPMMCR_EL3_MPMM_EN_MASK	UINT64_C(0x1)
 
+/* alternative system register encoding for the "sb" speculation barrier */
+#define SYSREG_SB			S0_3_C3_C0_7
+
 #endif /* ARCH_H */
diff --git a/include/arch/aarch64/asm_macros.S b/include/arch/aarch64/asm_macros.S
index 66c39e5f1..b4dab085e 100644
--- a/include/arch/aarch64/asm_macros.S
+++ b/include/arch/aarch64/asm_macros.S
@@ -214,13 +214,25 @@
 	ret
 	.endm
 
+	/*
+	 * The "sb" instruction was introduced later into the architecture,
+	 * so not all toolchains understand it. Some deny its usage unless
+	 * a supported processor is specified on the build command line.
+	 * Use sb's system register encoding to work around this, we already
+	 * guard the sb execution with a feature flag.
+	 */
+
+	.macro sb_barrier_insn
+	msr	SYSREG_SB, xzr
+	.endm
+
 	/*
 	 * Macro for using speculation barrier instruction introduced by
 	 * FEAT_SB, if it's enabled.
 	 */
 	.macro speculation_barrier
 #if ENABLE_FEAT_SB
-	sb
+	sb_barrier_insn
 #else
 	dsb	sy
 	isb
@@ -234,7 +246,7 @@
 	.macro exception_return
 	eret
 #if ENABLE_FEAT_SB
-	sb
+	sb_barrier_insn
 #else
 	dsb	nsh
 	isb

From 240770989f618da1b015f9a580fe0c83e10688a2 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Thu, 17 Nov 2022 16:42:09 +0000
Subject: [PATCH 05/12] refactor(cpufeat): align FEAT_SB to new feature
 handling

FEAT_SB introduces a new speculation barrier instruction, that is more
lightweight than a "dsb; isb" combination. We use that in a hot path,
so cannot afford and don't want a runtime detection mechanism.
Nevertheless align the implementation of the feature detection part
with the other features, but renaming the detection function, and
updating the FEAT_DETECTION code. Also update the documentation.

Change-Id: I2b86dfd1ad259c3bb99ab5186e2911ace454b54c
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 Makefile                               |  2 +-
 common/feat_detect.c                   | 12 +-----------
 docs/getting_started/build-options.rst | 11 +++++------
 include/arch/aarch64/arch_features.h   |  5 ++---
 4 files changed, 9 insertions(+), 21 deletions(-)

diff --git a/Makefile b/Makefile
index 8e789def0..90d8bbf18 100644
--- a/Makefile
+++ b/Makefile
@@ -1090,6 +1090,7 @@ $(eval $(call assert_booleans,\
         ENABLE_AMU_FCONF \
         AMU_RESTRICT_COUNTERS \
         ENABLE_ASSERTIONS \
+        ENABLE_FEAT_SB \
         ENABLE_PIE \
         ENABLE_PMF \
         ENABLE_PSCI_STAT \
@@ -1174,7 +1175,6 @@ $(eval $(call assert_numerics,\
         ENABLE_FEAT_PAN \
         ENABLE_FEAT_RNG \
         ENABLE_FEAT_RNG_TRAP \
-        ENABLE_FEAT_SB \
         ENABLE_FEAT_SEL2 \
         ENABLE_FEAT_TCR2 \
         ENABLE_FEAT_VHE \
diff --git a/common/feat_detect.c b/common/feat_detect.c
index 7a6c9193e..4b5dcd462 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -60,16 +60,6 @@ check_feature(int state, unsigned long field, const char *feat_name,
 	}
 }
 
-/******************************************
- * Feature : FEAT_SB (Speculation Barrier)
- *****************************************/
-static void read_feat_sb(void)
-{
-#if (ENABLE_FEAT_SB == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_0_feat_sb_present(), "SB");
-#endif
-}
-
 /******************************************************
  * Feature : FEAT_CSV2_2 (Cache Speculation Variant 2)
  *****************************************************/
@@ -256,7 +246,7 @@ void detect_arch_features(void)
 	tainted = false;
 
 	/* v8.0 features */
-	read_feat_sb();
+	check_feature(ENABLE_FEAT_SB, read_feat_sb_id_field(), "SB", 1, 1);
 	read_feat_csv2_2();
 
 	/* v8.1 features */
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 94c12defe..0540b6d2f 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -327,12 +327,11 @@ Common build options
    Default value is ``0``. ``FEAT_RNG_TRAP`` is an optional feature from
    Armv8.5 onwards.
 
--  ``ENABLE_FEAT_SB``: Numeric value to enable the ``FEAT_SB`` (Speculation
-   Barrier) extension allowing access to ``sb`` instruction. ``FEAT_SB`` is an
-   optional feature and defaults to ``0`` for pre-Armv8.5 CPUs but are mandatory
-   for Armv8.5 or later CPUs. This flag can take values 0 to 2, to align with
-   ``FEATURE_DETECTION`` mechanism. It is enabled from v8.5 and upwards and if
-   needed could be overidden from platforms explicitly. Default value is ``0``.
+-  ``ENABLE_FEAT_SB``: Boolean option to let the TF-A code use the ``FEAT_SB``
+   (Speculation Barrier) instruction ``FEAT_SB`` is an optional feature and
+   defaults to ``0`` for pre-Armv8.5 CPUs, but is mandatory for Armv8.5 or
+   later CPUs. It is enabled from v8.5 and upwards and if needed can be
+   overidden from platforms explicitly.
 
 -  ``ENABLE_FEAT_SEL2``: Numeric value to enable the ``FEAT_SEL2`` (Secure EL2)
    extension. ``FEAT_SEL2`` is a mandatory feature available on Arm v8.4.
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index ad938aea3..1af775876 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -256,10 +256,9 @@ static inline unsigned int get_armv9_2_feat_rme_support(void)
 /*********************************************************************************
  * Function to identify the presence of FEAT_SB (Speculation Barrier Instruction)
  ********************************************************************************/
-static inline bool is_armv8_0_feat_sb_present(void)
+static inline unsigned int read_feat_sb_id_field(void)
 {
-	return (((read_id_aa64isar1_el1() >> ID_AA64ISAR1_SB_SHIFT) &
-		ID_AA64ISAR1_SB_MASK) == ID_AA64ISAR1_SB_SUPPORTED);
+	return ISOLATE_FIELD(read_id_aa64isar1_el1(), ID_AA64ISAR1_SB);
 }
 
 /*********************************************************************************

From 4f5ef849c184313a2ba124ff0dd0b1545ddee217 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Thu, 26 Jan 2023 15:27:38 +0000
Subject: [PATCH 06/12] refactor(cpufeat): enable FEAT_PAN for
 FEAT_STATE_CHECKED

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

Add support for runtime detection (ENABLE_FEAT_PAN=2), by splitting
is_armv8_1_pan_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 PAN specific setup.

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: I58e5fe8d3c9332820391c7d93a8fb9dba4cf754a
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 common/feat_detect.c                   | 12 +-----------
 include/arch/aarch64/arch_features.h   | 18 +++++++++++++++---
 plat/arm/board/fvp/platform.mk         |  1 +
 services/std_svc/sdei/sdei_intr_mgmt.c |  2 +-
 4 files changed, 18 insertions(+), 15 deletions(-)

diff --git a/common/feat_detect.c b/common/feat_detect.c
index 4b5dcd462..c51da966c 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -70,16 +70,6 @@ static void read_feat_csv2_2(void)
 #endif
 }
 
-/***********************************************
- * Feature : FEAT_PAN (Privileged Access Never)
- **********************************************/
-static void read_feat_pan(void)
-{
-#if (ENABLE_FEAT_PAN == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_1_pan_present(), "PAN");
-#endif
-}
-
 /*******************************************************************************
  * Feature : FEAT_RAS (Reliability, Availability, and Serviceability Extension)
  ******************************************************************************/
@@ -250,7 +240,7 @@ void detect_arch_features(void)
 	read_feat_csv2_2();
 
 	/* v8.1 features */
-	read_feat_pan();
+	check_feature(ENABLE_FEAT_PAN, read_feat_pan_id_field(), "PAN", 1, 3);
 	check_feature(ENABLE_FEAT_VHE, read_feat_vhe_id_field(), "VHE", 1, 1);
 
 	/* v8.2 features */
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 1af775876..bdfdc4cce 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -21,10 +21,22 @@ static inline bool is_armv7_gentimer_present(void)
 	return true;
 }
 
-static inline bool is_armv8_1_pan_present(void)
+static inline unsigned int read_feat_pan_id_field(void)
 {
-	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_PAN_SHIFT) &
-		ID_AA64MMFR1_EL1_PAN_MASK) != 0U;
+	return ISOLATE_FIELD(read_id_aa64mmfr1_el1(), ID_AA64MMFR1_EL1_PAN);
+}
+
+static inline bool is_feat_pan_supported(void)
+{
+	if (ENABLE_FEAT_PAN == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_PAN == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_pan_id_field() != 0U;
 }
 
 static inline unsigned int read_feat_vhe_id_field(void)
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index d5e59cd25..3f1811253 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -470,6 +470,7 @@ ENABLE_FEAT_FGT			:= 2
 ENABLE_FEAT_HCX			:= 2
 ENABLE_FEAT_TCR2		:= 2
 
+ENABLE_FEAT_PAN			:= 2
 ENABLE_FEAT_VHE			:= 2
 ENABLE_MPAM_FOR_LOWER_ELS	:= 2
 
diff --git a/services/std_svc/sdei/sdei_intr_mgmt.c b/services/std_svc/sdei/sdei_intr_mgmt.c
index 9862e4f8d..3bdf4a2b6 100644
--- a/services/std_svc/sdei/sdei_intr_mgmt.c
+++ b/services/std_svc/sdei/sdei_intr_mgmt.c
@@ -274,7 +274,7 @@ static void sdei_set_elr_spsr(sdei_entry_t *se, sdei_dispatch_context_t *disp_ct
 			     (hcr_el2 & HCR_TGE_BIT) &&
 			     (hcr_el2 & HCR_E2H_BIT);
 
-	if (is_armv8_1_pan_present() &&
+	if (is_feat_pan_supported() &&
 	    ((client_el == MODE_EL1) ||
 		(client_el == MODE_EL2 && el_is_in_host)) &&
 	    ((client_el_sctlr & SCTLR_SPAN_BIT) == 0U)) {

From b8f03d29e172af7bd576eafbce9d485a9f626e2e Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Thu, 17 Nov 2022 17:30:43 +0000
Subject: [PATCH 07/12] refactor(cpufeat): enable FEAT_ECV for
 FEAT_STATE_CHECKED

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

Add support for runtime detection (ENABLE_FEAT_ECV=2), by splitting
is_feat_ecv_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 access the CNTPOFF_EL2 system register.
Also move the context saving code from assembly to C, and use the new
is_feat_ecv_supported() function to guard its execution.

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: I4acd5384929f1902b62a87ae073aafa1472cd66b
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 common/feat_detect.c                      | 15 +----------
 include/arch/aarch64/arch_features.h      | 31 ++++++++++++++++++++---
 include/arch/aarch64/arch_helpers.h       |  3 +++
 include/lib/el3_runtime/aarch64/context.h |  4 ---
 lib/el3_runtime/aarch64/context.S         | 18 -------------
 lib/el3_runtime/aarch64/context_mgmt.c    | 19 ++++++++------
 plat/arm/board/fvp/platform.mk            |  1 +
 7 files changed, 44 insertions(+), 47 deletions(-)

diff --git a/common/feat_detect.c b/common/feat_detect.c
index c51da966c..2f64deb51 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -164,19 +164,6 @@ static void read_feat_amuv1p1(void)
 #endif
 }
 
-/*******************************************************
- * Feature : FEAT_ECV (Enhanced Counter Virtualization)
- ******************************************************/
-static void read_feat_ecv(void)
-{
-#if (ENABLE_FEAT_ECV == FEAT_STATE_ALWAYS)
-	unsigned int ecv = get_armv8_6_ecv_support();
-
-	feat_detect_panic(((ecv == ID_AA64MMFR0_EL1_ECV_SUPPORTED) ||
-			(ecv == ID_AA64MMFR0_EL1_ECV_SELF_SYNCH)), "ECV");
-#endif
-}
-
 /***********************************************************
  * Feature : FEAT_TWED (Delayed Trapping of WFE Instruction)
  **********************************************************/
@@ -269,7 +256,7 @@ void detect_arch_features(void)
 	/* v8.6 features */
 	read_feat_amuv1p1();
 	check_feature(ENABLE_FEAT_FGT, read_feat_fgt_id_field(), "FGT", 1, 1);
-	read_feat_ecv();
+	check_feature(ENABLE_FEAT_ECV, read_feat_ecv_id_field(), "ECV", 1, 2);
 	read_feat_twed();
 
 	/* v8.7 features */
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index bdfdc4cce..029cdd112 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -143,10 +143,35 @@ static inline bool is_feat_fgt_supported(void)
 	return read_feat_fgt_id_field() != 0U;
 }
 
-static inline unsigned long int get_armv8_6_ecv_support(void)
+static unsigned int read_feat_ecv_id_field(void)
 {
-	return ((read_id_aa64mmfr0_el1() >> ID_AA64MMFR0_EL1_ECV_SHIFT) &
-		ID_AA64MMFR0_EL1_ECV_MASK);
+	return ISOLATE_FIELD(read_id_aa64mmfr0_el1(), ID_AA64MMFR0_EL1_ECV);
+}
+
+static inline bool is_feat_ecv_supported(void)
+{
+	if (ENABLE_FEAT_ECV == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_ECV == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_ecv_id_field() != 0U;
+}
+
+static inline bool is_feat_ecv_v2_supported(void)
+{
+	if (ENABLE_FEAT_ECV == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_ECV == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_ecv_id_field() >= ID_AA64MMFR0_EL1_ECV_SELF_SYNCH;
 }
 
 static inline bool is_armv8_5_rng_present(void)
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 6d115c7e5..720f6f3bd 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -590,6 +590,9 @@ DEFINE_RENAME_SYSREG_RW_FUNCS(hfgitr_el2, HFGITR_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(hfgrtr_el2, HFGRTR_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(hfgwtr_el2, HFGWTR_EL2)
 
+/* ARMv8.6 FEAT_ECV Register */
+DEFINE_RENAME_SYSREG_RW_FUNCS(cntpoff_el2, CNTPOFF_EL2)
+
 /* FEAT_HCX Register */
 DEFINE_RENAME_SYSREG_RW_FUNCS(hcrx_el2, HCRX_EL2)
 
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 9b50f3319..7d7fb5272 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -517,10 +517,6 @@ void el2_sysregs_context_restore_common(el2_sysregs_t *regs);
 void el2_sysregs_context_save_mte(el2_sysregs_t *regs);
 void el2_sysregs_context_restore_mte(el2_sysregs_t *regs);
 #endif /* CTX_INCLUDE_MTE_REGS */
-#if ENABLE_FEAT_ECV
-void el2_sysregs_context_save_ecv(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_ecv(el2_sysregs_t *regs);
-#endif /* ENABLE_FEAT_ECV */
 #if RAS_EXTENSION
 void el2_sysregs_context_save_ras(el2_sysregs_t *regs);
 void el2_sysregs_context_restore_ras(el2_sysregs_t *regs);
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index a5b64a5f2..baabd3ba5 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -17,10 +17,6 @@
 	.global	el2_sysregs_context_save_mte
 	.global	el2_sysregs_context_restore_mte
 #endif /* CTX_INCLUDE_MTE_REGS */
-#if ENABLE_FEAT_ECV
-	.global	el2_sysregs_context_save_ecv
-	.global	el2_sysregs_context_restore_ecv
-#endif /* ENABLE_FEAT_ECV */
 #if RAS_EXTENSION
 	.global	el2_sysregs_context_save_ras
 	.global	el2_sysregs_context_restore_ras
@@ -222,20 +218,6 @@ func el2_sysregs_context_restore_mte
 endfunc el2_sysregs_context_restore_mte
 #endif /* CTX_INCLUDE_MTE_REGS */
 
-#if ENABLE_FEAT_ECV
-func el2_sysregs_context_save_ecv
-	mrs	x11, CNTPOFF_EL2
-	str	x11, [x0, #CTX_CNTPOFF_EL2]
-	ret
-endfunc el2_sysregs_context_save_ecv
-
-func el2_sysregs_context_restore_ecv
-	ldr	x11, [x0, #CTX_CNTPOFF_EL2]
-	msr	CNTPOFF_EL2, x11
-	ret
-endfunc el2_sysregs_context_restore_ecv
-#endif /* ENABLE_FEAT_ECV */
-
 #if RAS_EXTENSION
 func el2_sysregs_context_save_ras
 	/*
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index c91070ece..cf4bb3043 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -380,8 +380,7 @@ static void setup_context_common(cpu_context_t *ctx, const entry_point_info_t *e
 			scr_el3 |= SCR_FGTEN_BIT;
 		}
 
-		if (get_armv8_6_ecv_support()
-		    == ID_AA64MMFR0_EL1_ECV_SELF_SYNCH) {
+		if (is_feat_ecv_supported()) {
 			scr_el3 |= SCR_ECVEN_BIT;
 		}
 	}
@@ -957,9 +956,11 @@ void cm_el2_sysregs_context_save(uint32_t security_state)
 			el2_sysregs_context_save_fgt(el2_sysregs_ctx);
 		}
 
-#if ENABLE_FEAT_ECV
-		el2_sysregs_context_save_ecv(el2_sysregs_ctx);
-#endif
+		if (is_feat_ecv_v2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_CNTPOFF_EL2,
+				      read_cntpoff_el2());
+		}
+
 		if (is_feat_vhe_supported()) {
 			write_ctx_reg(el2_sysregs_ctx, CTX_CONTEXTIDR_EL2,
 				      read_contextidr_el2());
@@ -1020,9 +1021,11 @@ void cm_el2_sysregs_context_restore(uint32_t security_state)
 			el2_sysregs_context_restore_fgt(el2_sysregs_ctx);
 		}
 
-#if ENABLE_FEAT_ECV
-		el2_sysregs_context_restore_ecv(el2_sysregs_ctx);
-#endif
+		if (is_feat_ecv_v2_supported()) {
+			write_cntpoff_el2(read_ctx_reg(el2_sysregs_ctx,
+						       CTX_CNTPOFF_EL2));
+		}
+
 		if (is_feat_vhe_supported()) {
 			write_contextidr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_CONTEXTIDR_EL2));
 			write_ttbr1_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TTBR1_EL2));
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 3f1811253..683234f16 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -470,6 +470,7 @@ ENABLE_FEAT_FGT			:= 2
 ENABLE_FEAT_HCX			:= 2
 ENABLE_FEAT_TCR2		:= 2
 
+ENABLE_FEAT_ECV			:= 2
 ENABLE_FEAT_PAN			:= 2
 ENABLE_FEAT_VHE			:= 2
 ENABLE_MPAM_FOR_LOWER_ELS	:= 2

From 7db710f0cb54c5943c9e60cb9c29eadd8817e2c5 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Thu, 17 Nov 2022 17:30:43 +0000
Subject: [PATCH 08/12] refactor(cpufeat): enable FEAT_CSV2_2 for
 FEAT_STATE_CHECKED

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

Add support for runtime detection (ENABLE_FEAT_CSV2_2=2), by splitting
is_armv8_0_feat_csv2_2_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 access the SCXTNUM_EL2 system register.
Also move the context saving code from assembly to C, and use the new
is_feat_csv2_2_supported() function to guard its execution.

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: I89c7bc883e6a65727fdbdd36eb3bfbffb2196da7
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 common/feat_detect.c                      | 13 ++-------
 include/arch/aarch64/arch_features.h      | 18 ++++++++++--
 include/arch/aarch64/arch_helpers.h       |  2 ++
 include/lib/el3_runtime/aarch64/context.h |  4 ---
 lib/el3_runtime/aarch64/context.S         | 24 ----------------
 lib/el3_runtime/aarch64/context_mgmt.c    | 34 +++++++++++++----------
 plat/arm/board/fvp/platform.mk            |  1 +
 7 files changed, 40 insertions(+), 56 deletions(-)

diff --git a/common/feat_detect.c b/common/feat_detect.c
index 2f64deb51..bdadde1f4 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -60,16 +60,6 @@ check_feature(int state, unsigned long field, const char *feat_name,
 	}
 }
 
-/******************************************************
- * Feature : FEAT_CSV2_2 (Cache Speculation Variant 2)
- *****************************************************/
-static void read_feat_csv2_2(void)
-{
-#if (ENABLE_FEAT_CSV2_2 == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_0_feat_csv2_2_present(), "CSV2_2");
-#endif
-}
-
 /*******************************************************************************
  * Feature : FEAT_RAS (Reliability, Availability, and Serviceability Extension)
  ******************************************************************************/
@@ -224,7 +214,8 @@ void detect_arch_features(void)
 
 	/* v8.0 features */
 	check_feature(ENABLE_FEAT_SB, read_feat_sb_id_field(), "SB", 1, 1);
-	read_feat_csv2_2();
+	check_feature(ENABLE_FEAT_CSV2_2, read_feat_csv2_id_field(),
+		      "CSV2_2", 2, 3);
 
 	/* v8.1 features */
 	check_feature(ENABLE_FEAT_PAN, read_feat_pan_id_field(), "PAN", 1, 3);
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 029cdd112..0342d2f7c 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -301,10 +301,22 @@ static inline unsigned int read_feat_sb_id_field(void)
 /*********************************************************************************
  * Function to identify the presence of FEAT_CSV2_2 (Cache Speculation Variant 2)
  ********************************************************************************/
-static inline bool is_armv8_0_feat_csv2_2_present(void)
+static inline unsigned int read_feat_csv2_id_field(void)
 {
-	return (((read_id_aa64pfr0_el1() >> ID_AA64PFR0_CSV2_SHIFT) &
-		ID_AA64PFR0_CSV2_MASK) == ID_AA64PFR0_CSV2_2_SUPPORTED);
+	return ISOLATE_FIELD(read_id_aa64pfr0_el1(), ID_AA64PFR0_CSV2);
+}
+
+static inline bool is_feat_csv2_2_supported(void)
+{
+	if (ENABLE_FEAT_CSV2_2 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_CSV2_2 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_csv2_id_field() >= ID_AA64PFR0_CSV2_2_SUPPORTED;
 }
 
 /**********************************************************************************
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 720f6f3bd..35027ca67 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -540,6 +540,8 @@ DEFINE_RENAME_SYSREG_READ_FUNC(erxaddr_el1, ERXADDR_EL1)
 DEFINE_RENAME_SYSREG_READ_FUNC(erxmisc0_el1, ERXMISC0_EL1)
 DEFINE_RENAME_SYSREG_READ_FUNC(erxmisc1_el1, ERXMISC1_EL1)
 
+DEFINE_RENAME_SYSREG_RW_FUNCS(scxtnum_el2, SCXTNUM_EL2)
+
 /* Armv8.1 VHE Registers */
 DEFINE_RENAME_SYSREG_RW_FUNCS(contextidr_el2, CONTEXTIDR_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(ttbr1_el2, TTBR1_EL2)
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 7d7fb5272..e70ff99fa 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -525,10 +525,6 @@ void el2_sysregs_context_restore_ras(el2_sysregs_t *regs);
 void el2_sysregs_context_save_nv2(el2_sysregs_t *regs);
 void el2_sysregs_context_restore_nv2(el2_sysregs_t *regs);
 #endif /* CTX_INCLUDE_NEVE_REGS */
-#if ENABLE_FEAT_CSV2_2
-void el2_sysregs_context_save_csv2(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_csv2(el2_sysregs_t *regs);
-#endif /* ENABLE_FEAT_CSV2_2 */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 #if CTX_INCLUDE_FPREGS
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index baabd3ba5..715406487 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -25,10 +25,6 @@
 	.global	el2_sysregs_context_save_nv2
 	.global	el2_sysregs_context_restore_nv2
 #endif /* CTX_INCLUDE_NEVE_REGS */
-#if ENABLE_FEAT_CSV2_2
-	.global	el2_sysregs_context_save_csv2
-	.global	el2_sysregs_context_restore_csv2
-#endif /* ENABLE_FEAT_CSV2_2 */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 	.global	el1_sysregs_context_save
@@ -262,26 +258,6 @@ func el2_sysregs_context_restore_nv2
 endfunc el2_sysregs_context_restore_nv2
 #endif /* CTX_INCLUDE_NEVE_REGS */
 
-#if ENABLE_FEAT_CSV2_2
-func el2_sysregs_context_save_csv2
-	/*
-	 * SCXTNUM_EL2 register is saved only when FEAT_CSV2_2 is supported.
-	 */
-	mrs	x13, scxtnum_el2
-	str	x13, [x0, #CTX_SCXTNUM_EL2]
-	ret
-endfunc el2_sysregs_context_save_csv2
-
-func el2_sysregs_context_restore_csv2
-	/*
-	 * SCXTNUM_EL2 register is restored only when FEAT_CSV2_2 is supported.
-	 */
-	ldr	x13, [x0, #CTX_SCXTNUM_EL2]
-	msr	scxtnum_el2, x13
-	ret
-endfunc el2_sysregs_context_restore_csv2
-#endif /* ENABLE_FEAT_CSV2_2 */
-
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 /* ------------------------------------------------------------------
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index cf4bb3043..8430c06e8 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -171,10 +171,10 @@ static void setup_realm_context(cpu_context_t *ctx, const struct entry_point_inf
 
 	scr_el3 |= SCR_NS_BIT | SCR_NSE_BIT;
 
-#if ENABLE_FEAT_CSV2_2
-	/* Enable access to the SCXTNUM_ELx registers. */
-	scr_el3 |= SCR_EnSCXT_BIT;
-#endif
+	if (is_feat_csv2_2_supported()) {
+		/* Enable access to the SCXTNUM_ELx registers. */
+		scr_el3 |= SCR_EnSCXT_BIT;
+	}
 
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 }
@@ -227,10 +227,10 @@ static void setup_ns_context(cpu_context_t *ctx, const struct entry_point_info *
 	scr_el3 |= SCR_TERR_BIT;
 #endif
 
-#if ENABLE_FEAT_CSV2_2
-	/* Enable access to the SCXTNUM_ELx registers. */
-	scr_el3 |= SCR_EnSCXT_BIT;
-#endif
+	if (is_feat_csv2_2_supported()) {
+		/* Enable access to the SCXTNUM_ELx registers. */
+		scr_el3 |= SCR_EnSCXT_BIT;
+	}
 
 #ifdef IMAGE_BL31
 	/*
@@ -976,9 +976,12 @@ void cm_el2_sysregs_context_save(uint32_t security_state)
 		if (is_feat_trf_supported()) {
 			write_ctx_reg(el2_sysregs_ctx, CTX_TRFCR_EL2, read_trfcr_el2());
 		}
-#if ENABLE_FEAT_CSV2_2
-		el2_sysregs_context_save_csv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_csv2_2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_SCXTNUM_EL2,
+				      read_scxtnum_el2());
+		}
+
 		if (is_feat_hcx_supported()) {
 			write_ctx_reg(el2_sysregs_ctx, CTX_HCRX_EL2, read_hcrx_el2());
 		}
@@ -1039,9 +1042,12 @@ void cm_el2_sysregs_context_restore(uint32_t security_state)
 		if (is_feat_trf_supported()) {
 			write_trfcr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TRFCR_EL2));
 		}
-#if ENABLE_FEAT_CSV2_2
-		el2_sysregs_context_restore_csv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_csv2_2_supported()) {
+			write_scxtnum_el2(read_ctx_reg(el2_sysregs_ctx,
+						       CTX_SCXTNUM_EL2));
+		}
+
 		if (is_feat_hcx_supported()) {
 			write_hcrx_el2(read_ctx_reg(el2_sysregs_ctx, CTX_HCRX_EL2));
 		}
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 683234f16..03fe9cc0b 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -470,6 +470,7 @@ ENABLE_FEAT_FGT			:= 2
 ENABLE_FEAT_HCX			:= 2
 ENABLE_FEAT_TCR2		:= 2
 
+ENABLE_FEAT_CSV2_2		:= 2
 ENABLE_FEAT_ECV			:= 2
 ENABLE_FEAT_PAN			:= 2
 ENABLE_FEAT_VHE			:= 2

From 1223d2a020c12f80d764323b8a5bf3cd317d8d12 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Fri, 27 Jan 2023 12:25:49 +0000
Subject: [PATCH 09/12] 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>
---
 common/feat_detect.c                   | 13 ++-----------
 include/arch/aarch64/arch_features.h   | 18 +++++++++++++++---
 lib/el3_runtime/aarch64/context_mgmt.c | 16 ++++++++--------
 plat/arm/board/fvp/platform.mk         |  1 +
 4 files changed, 26 insertions(+), 22 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);
+	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
 

From d5384b69d1180a596a48014d99e46eb4341f3455 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Fri, 27 Jan 2023 14:09:20 +0000
Subject: [PATCH 10/12] refactor(cpufeat): enable FEAT_NV2 for
 FEAT_STATE_CHECKED

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

Add support for runtime detection (CTX_INCLUDE_NEVE_REGS=2), by
splitting get_armv8_4_feat_nv_support() 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 access the VNCR_EL2 system register.
Also move the context saving code from assembly to C, and use the new
is_feat_nv2_supported() function to guard its execution.

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: I85b080641995fb72cfd4ac933f7a3f75770c2cb9
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 common/feat_detect.c                      | 15 ++------------
 include/arch/aarch64/arch.h               |  1 +
 include/arch/aarch64/arch_features.h      | 18 ++++++++++++++---
 include/arch/aarch64/arch_helpers.h       |  1 +
 include/lib/el3_runtime/aarch64/context.h |  4 ----
 lib/el3_runtime/aarch64/context.S         | 24 -----------------------
 lib/el3_runtime/aarch64/context_mgmt.c    | 16 +++++++++------
 plat/arm/board/fvp/platform.mk            |  1 +
 8 files changed, 30 insertions(+), 50 deletions(-)

diff --git a/common/feat_detect.c b/common/feat_detect.c
index b335a610f..1c505e211 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -90,18 +90,6 @@ static void read_feat_dit(void)
 #endif
 }
 
-/**************************************************************
- * Feature : FEAT_NV2 (Enhanced Nested Virtualization Support)
- *************************************************************/
-static void read_feat_nv2(void)
-{
-#if (CTX_INCLUDE_NEVE_REGS == FEAT_STATE_ALWAYS)
-	unsigned int nv = get_armv8_4_feat_nv_support();
-
-	feat_detect_panic((nv == ID_AA64MMFR2_EL1_NV2_SUPPORTED), "NV2");
-#endif
-}
-
 /***********************************
  * Feature : FEAT_SEL2 (Secure EL2)
  **********************************/
@@ -223,7 +211,8 @@ void detect_arch_features(void)
 		      "AMUv1", 1, 2);
 	check_feature(ENABLE_MPAM_FOR_LOWER_ELS, read_feat_mpam_version(),
 		      "MPAM", 1, 17);
-	read_feat_nv2();
+	check_feature(CTX_INCLUDE_NEVE_REGS, read_feat_nv_id_field(),
+		      "NV2", 2, 2);
 	read_feat_sel2();
 	check_feature(ENABLE_TRF_FOR_NS, read_feat_trf_id_field(),
 		      "TRF", 1, 1);
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 58abd449e..8663ab8e7 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -119,6 +119,7 @@
 #define MPAMVPM7_EL2		S3_4_C10_C6_7
 #define MPAMVPMV_EL2		S3_4_C10_C4_1
 #define TRFCR_EL2		S3_4_C1_C2_1
+#define VNCR_EL2		S3_4_C2_C2_0
 #define PMSCR_EL2		S3_4_C9_C9_0
 #define TFSR_EL2		S3_4_C5_C6_0
 #define CONTEXTIDR_EL2		S3_4_C13_C0_1
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index fa8015476..72927fbf9 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -423,10 +423,22 @@ static inline bool is_feat_trf_supported(void)
  * Function to identify the presence of FEAT_NV2 (Enhanced Nested Virtualization
  * Support)
  *******************************************************************************/
-static inline unsigned int get_armv8_4_feat_nv_support(void)
+static inline unsigned int read_feat_nv_id_field(void)
 {
-	return (((read_id_aa64mmfr2_el1() >> ID_AA64MMFR2_EL1_NV_SHIFT) &
-		ID_AA64MMFR2_EL1_NV_MASK));
+	return ISOLATE_FIELD(read_id_aa64mmfr2_el1(), ID_AA64MMFR2_EL1_NV);
+}
+
+static inline bool is_feat_nv2_supported(void)
+{
+	if (CTX_INCLUDE_NEVE_REGS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (CTX_INCLUDE_NEVE_REGS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_nv_id_field() >= ID_AA64MMFR2_EL1_NV2_SUPPORTED;
 }
 
 /*******************************************************************************
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 35027ca67..04b64be5c 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -573,6 +573,7 @@ DEFINE_RENAME_SYSREG_RW_FUNCS(dit, DIT)
 
 /* Armv8.4 FEAT_TRF Register */
 DEFINE_RENAME_SYSREG_RW_FUNCS(trfcr_el2, TRFCR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(vncr_el2, VNCR_EL2)
 
 /* Armv8.5 MTE Registers */
 DEFINE_RENAME_SYSREG_RW_FUNCS(tfsre0_el1, TFSRE0_EL1)
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index e70ff99fa..e5e7e7422 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -521,10 +521,6 @@ void el2_sysregs_context_restore_mte(el2_sysregs_t *regs);
 void el2_sysregs_context_save_ras(el2_sysregs_t *regs);
 void el2_sysregs_context_restore_ras(el2_sysregs_t *regs);
 #endif /* RAS_EXTENSION */
-#if CTX_INCLUDE_NEVE_REGS
-void el2_sysregs_context_save_nv2(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_nv2(el2_sysregs_t *regs);
-#endif /* CTX_INCLUDE_NEVE_REGS */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 #if CTX_INCLUDE_FPREGS
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 715406487..013a505d4 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -21,10 +21,6 @@
 	.global	el2_sysregs_context_save_ras
 	.global	el2_sysregs_context_restore_ras
 #endif /* RAS_EXTENSION */
-#if CTX_INCLUDE_NEVE_REGS
-	.global	el2_sysregs_context_save_nv2
-	.global	el2_sysregs_context_restore_nv2
-#endif /* CTX_INCLUDE_NEVE_REGS */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 	.global	el1_sysregs_context_save
@@ -238,26 +234,6 @@ func el2_sysregs_context_restore_ras
 endfunc el2_sysregs_context_restore_ras
 #endif /* RAS_EXTENSION */
 
-#if CTX_INCLUDE_NEVE_REGS
-func el2_sysregs_context_save_nv2
-	/*
-	 * VNCR_EL2 register is saved only when FEAT_NV2 is supported.
-	 */
-	mrs	x16, vncr_el2
-	str	x16, [x0, #CTX_VNCR_EL2]
-	ret
-endfunc el2_sysregs_context_save_nv2
-
-func el2_sysregs_context_restore_nv2
-	/*
-	 * VNCR_EL2 register is restored only when FEAT_NV2 is supported.
-	 */
-	ldr	x16, [x0, #CTX_VNCR_EL2]
-	msr	vncr_el2, x16
-	ret
-endfunc el2_sysregs_context_restore_nv2
-#endif /* CTX_INCLUDE_NEVE_REGS */
-
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 /* ------------------------------------------------------------------
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 8f46b80dc..2ba2f9ce2 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -970,9 +970,12 @@ void cm_el2_sysregs_context_save(uint32_t security_state)
 #if RAS_EXTENSION
 		el2_sysregs_context_save_ras(el2_sysregs_ctx);
 #endif
-#if CTX_INCLUDE_NEVE_REGS
-		el2_sysregs_context_save_nv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_nv2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_VNCR_EL2,
+				      read_vncr_el2());
+		}
+
 		if (is_feat_trf_supported()) {
 			write_ctx_reg(el2_sysregs_ctx, CTX_TRFCR_EL2, read_trfcr_el2());
 		}
@@ -1036,9 +1039,10 @@ void cm_el2_sysregs_context_restore(uint32_t security_state)
 #if RAS_EXTENSION
 		el2_sysregs_context_restore_ras(el2_sysregs_ctx);
 #endif
-#if CTX_INCLUDE_NEVE_REGS
-		el2_sysregs_context_restore_nv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_nv2_supported()) {
+			write_vncr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_VNCR_EL2));
+		}
 		if (is_feat_trf_supported()) {
 			write_trfcr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TRFCR_EL2));
 		}
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index a6f8eaf64..ef62e731b 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -470,6 +470,7 @@ ENABLE_FEAT_FGT			:= 2
 ENABLE_FEAT_HCX			:= 2
 ENABLE_FEAT_TCR2		:= 2
 
+CTX_INCLUDE_NEVE_REGS		:= 2
 ENABLE_FEAT_CSV2_2		:= 2
 ENABLE_FEAT_ECV			:= 2
 ENABLE_FEAT_PAN			:= 2

From 623f6140fc8eaa547d477179b67ca2fde2720fe4 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Wed, 22 Feb 2023 16:53:50 +0000
Subject: [PATCH 11/12] refactor(cpufeat): align FEAT_SEL2 to new feature
 handling

In ARMv8.4, the EL2 exception level got added to the secure world.
Adapt and rename the existing is_armv8_4_sel2_present() function, to
align its handling with the other CPU features.

Change-Id: If11e1942fdeb63c63f36ab9e89be810347d1a952
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 common/feat_detect.c                   | 13 ++-----------
 include/arch/aarch64/arch_features.h   | 18 +++++++++++++++---
 lib/el3_runtime/aarch64/context_mgmt.c |  2 +-
 plat/arm/board/fvp/platform.mk         |  1 +
 services/std_svc/spmd/spmd_main.c      |  2 +-
 5 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/common/feat_detect.c b/common/feat_detect.c
index 1c505e211..0f66157b3 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -90,16 +90,6 @@ static void read_feat_dit(void)
 #endif
 }
 
-/***********************************
- * Feature : FEAT_SEL2 (Secure EL2)
- **********************************/
-static void read_feat_sel2(void)
-{
-#if (ENABLE_FEAT_SEL2 == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_4_sel2_present(), "SEL2");
-#endif
-}
-
 /************************************************
  * Feature : FEAT_MTE (Memory Tagging Extension)
  ***********************************************/
@@ -213,7 +203,8 @@ void detect_arch_features(void)
 		      "MPAM", 1, 17);
 	check_feature(CTX_INCLUDE_NEVE_REGS, read_feat_nv_id_field(),
 		      "NV2", 2, 2);
-	read_feat_sel2();
+	check_feature(ENABLE_FEAT_SEL2, read_feat_sel2_id_field(),
+		      "SEL2", 1, 1);
 	check_feature(ENABLE_TRF_FOR_NS, read_feat_trf_id_field(),
 		      "TRF", 1, 1);
 
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 72927fbf9..03b005d1e 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -113,10 +113,22 @@ static inline unsigned int get_armv8_5_mte_support(void)
 		ID_AA64PFR1_EL1_MTE_MASK);
 }
 
-static inline bool is_armv8_4_sel2_present(void)
+static inline unsigned int read_feat_sel2_id_field(void)
 {
-	return ((read_id_aa64pfr0_el1() >> ID_AA64PFR0_SEL2_SHIFT) &
-		ID_AA64PFR0_SEL2_MASK) == 1ULL;
+	return ISOLATE_FIELD(read_id_aa64pfr0_el1(), ID_AA64PFR0_SEL2);
+}
+
+static inline bool is_feat_sel2_supported(void)
+{
+	if (ENABLE_FEAT_SEL2 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_SEL2 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_sel2_id_field() != 0U;
 }
 
 static inline unsigned int read_feat_twed_id_field(void)
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 2ba2f9ce2..04b685f2d 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -134,7 +134,7 @@ static void setup_secure_context(cpu_context_t *ctx, const struct entry_point_in
 #endif /* CTX_INCLUDE_MTE_REGS */
 
 	/* Enable S-EL2 if the next EL is EL2 and S-EL2 is present */
-	if ((GET_EL(ep->spsr) == MODE_EL2) && is_armv8_4_sel2_present()) {
+	if ((GET_EL(ep->spsr) == MODE_EL2) && is_feat_sel2_supported()) {
 		if (GET_RW(ep->spsr) != MODE_RW_64) {
 			ERROR("S-EL2 can not be used in AArch32\n.");
 			panic();
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index ef62e731b..d9cca5ec1 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -474,6 +474,7 @@ CTX_INCLUDE_NEVE_REGS		:= 2
 ENABLE_FEAT_CSV2_2		:= 2
 ENABLE_FEAT_ECV			:= 2
 ENABLE_FEAT_PAN			:= 2
+ENABLE_FEAT_SEL2		:= 2
 ENABLE_FEAT_TWED		:= 2
 ENABLE_FEAT_VHE			:= 2
 ENABLE_MPAM_FOR_LOWER_ELS	:= 2
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index e85109dfa..0e1899ede 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -399,7 +399,7 @@ static int spmd_spmc_init(void *pm_addr)
 	 * Check if S-EL2 is supported on this system if S-EL2
 	 * is required for SPM
 	 */
-	if (!is_armv8_4_sel2_present()) {
+	if (!is_feat_sel2_supported()) {
 		WARN("SPM Core run time S-EL2 is not supported.\n");
 		return -EINVAL;
 	}

From ac17e52c728e7786e80f41e41673420433637fa6 Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@arm.com>
Date: Wed, 22 Feb 2023 17:55:59 +0000
Subject: [PATCH 12/12] refactor(cpufeat): enable FEAT_RNG for
 FEAT_STATE_CHECKED

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

Add support for runtime detection (FEAT_RNG=2), by splitting
is_armv8_5_rng_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
access the RNDRRS system register.

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

Change-Id: I1a4a538d5ad395fead7324f297d0056bda4f84cb
Signed-off-by: Andre Przywara <andre.przywara@arm.com>
---
 common/feat_detect.c                    | 12 +-----------
 include/arch/aarch64/arch_features.h    | 18 +++++++++++++++---
 plat/qemu/common/qemu_stack_protector.c |  4 +---
 plat/qemu/qemu/platform.mk              |  3 +++
 4 files changed, 20 insertions(+), 17 deletions(-)

diff --git a/common/feat_detect.c b/common/feat_detect.c
index 0f66157b3..9218c07e1 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -102,16 +102,6 @@ static void read_feat_mte(void)
 #endif
 }
 
-/***********************************************
- * Feature : FEAT_RNG (Random Number Generator)
- **********************************************/
-static void read_feat_rng(void)
-{
-#if (ENABLE_FEAT_RNG == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_5_rng_present(), "RNG");
-#endif
-}
-
 /****************************************************
  * Feature : FEAT_BTI (Branch Target Identification)
  ***************************************************/
@@ -210,7 +200,7 @@ void detect_arch_features(void)
 
 	/* v8.5 features */
 	read_feat_mte();
-	read_feat_rng();
+	check_feature(ENABLE_FEAT_RNG, read_feat_rng_id_field(), "RNG", 1, 1);
 	read_feat_bti();
 	read_feat_rng_trap();
 
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 03b005d1e..d3c626367 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -198,10 +198,22 @@ static inline bool is_feat_ecv_v2_supported(void)
 	return read_feat_ecv_id_field() >= ID_AA64MMFR0_EL1_ECV_SELF_SYNCH;
 }
 
-static inline bool is_armv8_5_rng_present(void)
+static unsigned int read_feat_rng_id_field(void)
 {
-	return ((read_id_aa64isar0_el1() >> ID_AA64ISAR0_RNDR_SHIFT) &
-		ID_AA64ISAR0_RNDR_MASK);
+	return ISOLATE_FIELD(read_id_aa64isar0_el1(), ID_AA64ISAR0_RNDR);
+}
+
+static inline bool is_feat_rng_supported(void)
+{
+	if (ENABLE_FEAT_RNG == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_RNG == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_rng_id_field() != 0U;
 }
 
 static unsigned int read_feat_tcrx_id_field(void)
diff --git a/plat/qemu/common/qemu_stack_protector.c b/plat/qemu/common/qemu_stack_protector.c
index 15ce3d6d2..d0b4a0f1d 100644
--- a/plat/qemu/common/qemu_stack_protector.c
+++ b/plat/qemu/common/qemu_stack_protector.c
@@ -14,12 +14,10 @@
 
 u_register_t plat_get_stack_protector_canary(void)
 {
-#if ENABLE_FEAT_RNG
 	/* Use the RNDR instruction if the CPU supports it */
-	if (is_armv8_5_rng_present()) {
+	if (is_feat_rng_supported()) {
 		return read_rndr();
 	}
-#endif
 
 	/*
 	 * Ideally, a random number should be returned above. If a random
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index 32d2b8859..f82232871 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk
@@ -290,6 +290,9 @@ $(eval $(call add_define,ARM_LINUX_KERNEL_AS_BL33))
 ARM_PRELOADED_DTB_BASE := PLAT_QEMU_DT_BASE
 $(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
 
+# QEMU will use the RNDR instruction for the stack protector canary.
+ENABLE_FEAT_RNG			:= 2
+
 # Later QEMU versions support SME and SVE.
 ifneq (${ARCH},aarch32)
 	ENABLE_SVE_FOR_NS	:= 1