feat(tcr2): support FEAT_TCR2

Arm v8.9 introduces FEAT_TCR2, adding extended translation control
registers. Support this, context switching TCR2_EL2 and disabling
traps so lower ELs can access the new registers.

Change the FVP platform to default to handling this as a dynamic option so
the right decision can be made by the code at runtime.

Signed-off-by: Mark Brown <broonie@kernel.org>
Change-Id: I297452acd8646d58bac64fc15e05b06a543e5148

10 files changed, 69 insertions, 0 deletions

diff --git a/Makefile b/Makefile
index 04b75fe59..cf2a40221 100644
--- a/Makefile
+++ b/Makefile
@@ -1162,6 +1162,7 @@ $(eval $(call assert_numerics,\
         ENABLE_FEAT_RNG_TRAP \
         ENABLE_FEAT_SB \
         ENABLE_FEAT_SEL2 \
+        ENABLE_FEAT_TCR2 \
         ENABLE_FEAT_VHE \
         ENABLE_MPAM_FOR_LOWER_ELS \
         ENABLE_RME \
@@ -1293,6 +1294,7 @@ $(eval $(call add_defines,\
         ENABLE_FEAT_VHE \
         ENABLE_FEAT_CSV2_2 \
         ENABLE_FEAT_PAN \
+        ENABLE_FEAT_TCR2 \
         FEATURE_DETECTION \
         TWED_DELAY \
         ENABLE_FEAT_TWED \
diff --git a/common/feat_detect.c b/common/feat_detect.c
index 5fb56b992..40997322a 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -314,6 +314,10 @@ void detect_arch_features(void)
 	/* v8.7 features */
 	check_feature(ENABLE_FEAT_HCX, read_feat_hcx_id_field(), "HCX", 1, 1);
 
+	/* v8.9 features */
+	check_feature(ENABLE_FEAT_TCR2, read_feat_tcrx_id_field(),
+		      "TCR2", 1, 1);
+
 	/* v9.0 features */
 	check_feature(ENABLE_BRBE_FOR_NS, read_feat_brbe_id_field(),
 		      "BRBE", 1, 2);
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index a285d31c3..38efe9ea8 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -348,6 +348,13 @@ Common build options
    values 0 to 2, to align  with the ``FEATURE_DETECTION`` mechanism.
    Default value is ``0``.
 
+-  ``ENABLE_FEAT_TCR2``: Numeric value to set the bit SCR_EL3.ENTCR2 in EL3 to
+   allow access to TCR2_EL2 (extended translation control) from EL2 as
+   well as adding TCR2_EL2 to the EL2 context save/restore operations. Its a
+   mandatory architectural feature and is enabled from v8.9 and upwards. This
+   flag can take the values 0 to 2, to align  with the ``FEATURE_DETECTION``
+   mechanism. Default value is ``0``.
+
 -  ``ENABLE_LTO``: Boolean option to enable Link Time Optimization (LTO)
    support in GCC for TF-A. This option is currently only supported for
    AArch64. Default is 0.
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 9e4a3b7bb..b78652103 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -349,6 +349,12 @@
 #define ID_AA64MMFR2_EL1_NV_SUPPORTED		ULL(0x1)
 #define ID_AA64MMFR2_EL1_NV2_SUPPORTED		ULL(0x2)
 
+/* ID_AA64MMFR3_EL1 definitions */
+#define ID_AA64MMFR3_EL1			S3_0_C0_C7_3
+
+#define ID_AA64MMFR3_EL1_TCRX_SHIFT		U(0)
+#define ID_AA64MMFR3_EL1_TCRX_MASK		ULL(0xf)
+
 /* ID_AA64PFR1_EL1 definitions */
 #define ID_AA64PFR1_EL1_SSBS_SHIFT	U(4)
 #define ID_AA64PFR1_EL1_SSBS_MASK	ULL(0xf)
@@ -501,6 +507,7 @@
 #define SCR_GPF_BIT		(UL(1) << 48)
 #define SCR_TWEDEL_SHIFT	U(30)
 #define SCR_TWEDEL_MASK		ULL(0xf)
+#define SCR_TCR2EN_BIT		(UL(1) << 43)
 #define SCR_TRNDR_BIT		(UL(1) << 40)
 #define SCR_HXEn_BIT		(UL(1) << 38)
 #define SCR_ENTP2_SHIFT		U(41)
@@ -1302,6 +1309,11 @@
 #define HCRX_EL2_EnAS0_BIT	(UL(1) << 0)
 
 /*******************************************************************************
+ * FEAT_TCR2 - Extended Translation Control Register
+ ******************************************************************************/
+#define TCR2_EL2		S3_4_C2_C0_3
+
+/*******************************************************************************
  * Definitions for DynamicIQ Shared Unit registers
  ******************************************************************************/
 #define CLUSTERPWRDN_EL1	S3_0_c15_c3_6
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 9ff81aa10..582aed12f 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -131,6 +131,24 @@ static inline bool is_armv8_5_rng_present(void)
 		ID_AA64ISAR0_RNDR_MASK);
 }
 
+static unsigned int read_feat_tcrx_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64mmfr3_el1(), ID_AA64MMFR3_EL1_TCRX);
+}
+
+static inline bool is_feat_tcr2_supported(void)
+{
+	if (ENABLE_FEAT_TCR2 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_TCR2 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_tcrx_id_field() != 0U;
+}
+
 /*******************************************************************************
  * Functions to identify the presence of the Activity Monitors Extension
  ******************************************************************************/
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 3350c8f90..81e0e0643 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -579,6 +579,12 @@ DEFINE_RENAME_SYSREG_RW_FUNCS(hfgwtr_el2, HFGWTR_EL2)
 /* FEAT_HCX Register */
 DEFINE_RENAME_SYSREG_RW_FUNCS(hcrx_el2, HCRX_EL2)
 
+/* Armv8.9 system registers */
+DEFINE_RENAME_IDREG_READ_FUNC(id_aa64mmfr3_el1, ID_AA64MMFR3_EL1)
+
+/* FEAT_TCR2 Register */
+DEFINE_RENAME_SYSREG_RW_FUNCS(tcr2_el2, TCR2_EL2)
+
 /* DynamIQ Shared Unit power management */
 DEFINE_RENAME_SYSREG_RW_FUNCS(clusterpwrdn_el1, CLUSTERPWRDN_EL1)
 
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 40bfa39c6..31d8f070c 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -229,6 +229,9 @@
 // Register for FEAT_HCX
 #define CTX_HCRX_EL2            U(0x1d0)
 
+// Starting with Armv8.9
+#define CTX_TCR2_EL2            U(0x1d8)
+
 /* Align to the next 16 byte boundary */
 #define CTX_EL2_SYSREGS_END	U(0x1e0)
 
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 4d2079d23..507a8ce8a 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -348,6 +348,13 @@ static void setup_context_common(cpu_context_t *ctx, const entry_point_info_t *e
 #endif
 
 	/*
+	 * SCR_EL3.TCR2EN: Enable access to TCR2_ELx for AArch64 if present.
+	 */
+	if (is_feat_tcr2_supported() && (GET_RW(ep->spsr) == MODE_RW_64)) {
+		scr_el3 |= SCR_TCR2EN_BIT;
+	}
+
+	/*
 	 * CPTR_EL3 was initialized out of reset, copy that value to the
 	 * context register.
 	 */
@@ -884,6 +891,9 @@ void cm_el2_sysregs_context_save(uint32_t security_state)
 		if (is_feat_hcx_supported()) {
 			write_ctx_reg(el2_sysregs_ctx, CTX_HCRX_EL2, read_hcrx_el2());
 		}
+		if (is_feat_tcr2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_TCR2_EL2, read_tcr2_el2());
+		}
 	}
 }
 
@@ -944,6 +954,9 @@ void cm_el2_sysregs_context_restore(uint32_t security_state)
 		if (is_feat_hcx_supported()) {
 			write_hcrx_el2(read_ctx_reg(el2_sysregs_ctx, CTX_HCRX_EL2));
 		}
+		if (is_feat_tcr2_supported()) {
+			write_tcr2_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TCR2_EL2));
+		}
 	}
 }
 #endif /* CTX_INCLUDE_EL2_REGS */
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index a66123a7d..cf1c2156e 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -176,6 +176,9 @@ ENABLE_FEAT_VHE 		:= 0
 # Flag to enable delayed trapping of WFE instruction (FEAT_TWED)
 ENABLE_FEAT_TWED		:= 0
 
+# Flag to enable access to TCR2 (FEAT_TCR2)
+ENABLE_FEAT_TCR2		:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 9b09a6bb9..c02eefe84 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -466,6 +466,7 @@ ENABLE_TRF_FOR_NS		:= 2
 # Linux relies on EL3 enablement if those features are present
 ENABLE_FEAT_FGT			:= 2
 ENABLE_FEAT_HCX			:= 2
+ENABLE_FEAT_TCR2		:= 2
 
 ifeq (${SPMC_AT_EL3}, 1)
 PLAT_BL_COMMON_SOURCES	+=	plat/arm/board/fvp/fvp_el3_spmc.c