feat(fgt2): add support for FEAT_FGT2

This patch disables trapping to EL3 when the FEAT_FGT2
specific trap registers are accessed by setting the
SCR_EL3.FGTEn2 bit

Signed-off-by: Arvind Ram Prakash <arvind.ramprakash@arm.com>
Change-Id: I6d2b614affb9067b2bc3d7bf0ae7d169d031592a

Makefile

@@ -1231,6 +1231,7 @@ $(eval $(call assert_numerics,\
 	ENABLE_FEAT_DIT \
 	ENABLE_FEAT_ECV \
 	ENABLE_FEAT_FGT \
+	ENABLE_FEAT_FGT2 \
 	ENABLE_FEAT_HCX \
 	ENABLE_FEAT_MTE2 \
 	ENABLE_FEAT_PAN \
@@ -1383,6 +1384,7 @@ $(eval $(call add_defines,\
 	ENABLE_MPMM \
 	ENABLE_MPMM_FCONF \
 	ENABLE_FEAT_FGT \
+	ENABLE_FEAT_FGT2 \
 	ENABLE_FEAT_ECV \
 	ENABLE_FEAT_AMUv1p1 \
 	ENABLE_FEAT_SEL2 \

bl31/bl31.mk

@@ -105,6 +105,10 @@ ifneq (${ENABLE_FEAT_AMU},0)
 BL31_SOURCES		+=	${AMU_SOURCES}
 endif
 
+ifneq (${ENABLE_FEAT_FGT2},0)
+BL31_SOURCES		+=	lib/extensions/fgt/fgt2.c
+endif
+
 ifeq (${ENABLE_MPMM},1)
 BL31_SOURCES		+=	${MPMM_SOURCES}
 endif

changelog.yaml

@@ -104,6 +104,9 @@ subsections:
       - title: Extended Translation Control Register (FEAT_TCR2).
         scope: tcr2
 
+      - title: Fine-grained Traps 2 (FEAT_FGT2).
+        scope: fgt2
+
       - title: CPU feature / ID register handling in general
         scope: cpufeat
 

common/feat_detect.c

@@ -334,7 +334,8 @@ void detect_arch_features(void)
 	/* v8.6 features */
 	check_feature(ENABLE_FEAT_AMUv1p1, read_feat_amu_id_field(),
 		      "AMUv1p1", 2, 2);
-	check_feature(ENABLE_FEAT_FGT, read_feat_fgt_id_field(), "FGT", 1, 1);
+	check_feature(ENABLE_FEAT_FGT, read_feat_fgt_id_field(), "FGT", 1, 2);
+	check_feature(ENABLE_FEAT_FGT2, read_feat_fgt_id_field(), "FGT2", 2, 2);
 	check_feature(ENABLE_FEAT_ECV, read_feat_ecv_id_field(), "ECV", 1, 2);
 	check_feature(ENABLE_FEAT_TWED, read_feat_twed_id_field(),
 		      "TWED", 1, 1);

docs/getting_started/build-options.rst

@@ -346,6 +346,13 @@ Common build options
    This flag can take the values 0 to 2, to align  with the ``ENABLE_FEAT``
    mechanism. Default value is ``0``.
 
+-  ``ENABLE_FEAT_FGT2``: Numeric value to enable support for FGT2
+   (Fine Grain Traps 2) feature allowing for access to Fine-grained trap 2 registers
+   during  EL2 to EL3 context save/restore operations.
+   Its an optional architectural feature and is available from v8.8 and upwards.
+   This flag can take the values 0 to 2, to align  with the ``ENABLE_FEAT``
+   mechanism. Default value is ``0``.
+
 -  ``ENABLE_FEAT_HCX``: Numeric value to set the bit SCR_EL3.HXEn in EL3 to
    allow access to HCRX_EL2 (extended hypervisor control register) from EL2 as
    well as adding HCRX_EL2 to the EL2 context save/restore operations. Its a

include/arch/aarch64/arch.h

@@ -114,9 +114,14 @@
  * Definitions for EL2 system registers for save/restore routine
  ******************************************************************************/
 #define CNTPOFF_EL2		S3_4_C14_C0_6
-#define HAFGRTR_EL2		S3_4_C3_C1_6
+#define HDFGRTR2_EL2		S3_4_C3_C1_0
+#define HDFGWTR2_EL2		S3_4_C3_C1_1
+#define HFGRTR2_EL2		S3_4_C3_C1_2
+#define HFGWTR2_EL2		S3_4_C3_C1_3
 #define HDFGRTR_EL2		S3_4_C3_C1_4
 #define HDFGWTR_EL2		S3_4_C3_C1_5
+#define HAFGRTR_EL2		S3_4_C3_C1_6
+#define HFGITR2_EL2		S3_4_C3_C1_7
 #define HFGITR_EL2		S3_4_C1_C1_6
 #define HFGRTR_EL2		S3_4_C1_C1_4
 #define HFGWTR_EL2		S3_4_C1_C1_5
@@ -331,6 +336,7 @@
 
 #define ID_AA64MMFR0_EL1_FGT_SHIFT		U(56)
 #define ID_AA64MMFR0_EL1_FGT_MASK		ULL(0xf)
+#define FGT2_IMPLEMENTED			ULL(0x2)
 #define FGT_IMPLEMENTED				ULL(0x1)
 #define FGT_NOT_IMPLEMENTED			ULL(0x0)
 
@@ -574,6 +580,7 @@
 /* SCR definitions */
 #define SCR_RES1_BITS		((U(1) << 4) | (U(1) << 5))
 #define SCR_NSE_SHIFT		U(62)
+#define SCR_FGTEN2_BIT		(UL(1) << 59)
 #define SCR_NSE_BIT		(ULL(1) << SCR_NSE_SHIFT)
 #define SCR_GPF_BIT		(UL(1) << 48)
 #define SCR_TWEDEL_SHIFT	U(30)

include/arch/aarch64/arch_features.h

@@ -130,6 +130,8 @@ CREATE_FEATURE_SUPPORTED(name, is_ ## name ## _present, guard)
  * +----------------------------+
  * |	FEAT_MTPMU		|
  * +----------------------------+
+ * |	FEAT_FGT2		|
+ * +----------------------------+
  */
 
 static inline bool is_armv7_gentimer_present(void)
@@ -216,6 +218,10 @@ CREATE_FEATURE_FUNCS(feat_twed, id_aa64mmfr1_el1, ID_AA64MMFR1_EL1_TWED_SHIFT,
 CREATE_FEATURE_FUNCS(feat_fgt, id_aa64mmfr0_el1, ID_AA64MMFR0_EL1_FGT_SHIFT,
 		     ID_AA64MMFR0_EL1_FGT_MASK, 1U, ENABLE_FEAT_FGT)
 
+/* FEAT_FGT2: Fine-grained traps extended */
+CREATE_FEATURE_FUNCS(feat_fgt2, id_aa64mmfr0_el1, ID_AA64MMFR0_EL1_FGT_SHIFT,
+		     ID_AA64MMFR0_EL1_FGT_MASK, FGT2_IMPLEMENTED, ENABLE_FEAT_FGT2)
+
 /* FEAT_ECV: Enhanced Counter Virtualization */
 CREATE_FEATURE_FUNCS(feat_ecv, id_aa64mmfr0_el1, ID_AA64MMFR0_EL1_ECV_SHIFT,
 		     ID_AA64MMFR0_EL1_ECV_MASK, 1U, ENABLE_FEAT_ECV)

include/arch/aarch64/arch_helpers.h

@@ -639,6 +639,13 @@ DEFINE_RENAME_SYSREG_RW_FUNCS(hcrx_el2, HCRX_EL2)
 /* Armv8.9 system registers */
 DEFINE_RENAME_IDREG_READ_FUNC(id_aa64mmfr3_el1, ID_AA64MMFR3_EL1)
 
+/* Armv8.9 FEAT_FGT2 Registers */
+DEFINE_RENAME_SYSREG_RW_FUNCS(hdfgrtr2_el2, HDFGRTR2_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(hdfgwtr2_el2, HDFGWTR2_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(hfgitr2_el2, HFGITR2_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(hfgrtr2_el2, HFGRTR2_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(hfgwtr2_el2, HFGWTR2_EL2)
+
 /* FEAT_TCR2 Register */
 DEFINE_RENAME_SYSREG_RW_FUNCS(tcr2_el1, TCR2_EL1)
 DEFINE_RENAME_SYSREG_RW_FUNCS(tcr2_el2, TCR2_EL2)

include/lib/el3_runtime/context_el2.h

@@ -62,6 +62,14 @@ typedef struct el2_fgt_regs {
 	uint64_t hfgwtr_el2;
 } el2_fgt_regs_t;
 
+typedef struct el2_fgt2_regs {
+	uint64_t hdfgrtr2_el2;
+	uint64_t hdfgwtr2_el2;
+	uint64_t hfgitr2_el2;
+	uint64_t hfgrtr2_el2;
+	uint64_t hfgwtr2_el2;
+} el2_fgt2_regs_t;
+
 typedef struct el2_ecv_regs {
 	uint64_t cntpoff_el2;
 } el2_ecv_regs_t;
@@ -140,6 +148,10 @@ typedef struct el2_sysregs {
 	el2_fgt_regs_t fgt;
 #endif
 
+#if ENABLE_FEAT_FGT2
+	el2_fgt2_regs_t fgt2;
+#endif
+
 #if ENABLE_FEAT_ECV
 	el2_ecv_regs_t ecv;
 #endif
@@ -221,6 +233,15 @@ typedef struct el2_sysregs {
 #define write_el2_ctx_fgt(ctx, reg, val)
 #endif /* ENABLE_FEAT_FGT */
 
+#if ENABLE_FEAT_FGT2
+#define read_el2_ctx_fgt2(ctx, reg)		(((ctx)->fgt2).reg)
+#define write_el2_ctx_fgt2(ctx, reg, val)	((((ctx)->fgt2).reg)	\
+							= (uint64_t) (val))
+#else
+#define read_el2_ctx_fgt2(ctx, reg)		ULL(0)
+#define write_el2_ctx_fgt2(ctx, reg, val)
+#endif /* ENABLE_FEAT_FGT */
+
 #if ENABLE_FEAT_ECV
 #define read_el2_ctx_ecv(ctx, reg)		(((ctx)->ecv).reg)
 #define write_el2_ctx_ecv(ctx, reg, val)	((((ctx)->ecv).reg)	\

include/lib/extensions/fgt2.h

@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FGT2_H
+#define FGT2_H
+
+#include <context.h>
+
+#if ENABLE_FEAT_FGT2
+void fgt2_enable(cpu_context_t *ctx);
+#else
+static inline void fgt2_enable(cpu_context_t *ctx)
+{
+}
+#endif /* ENABLE_FEAT_FGT2 */
+
+#endif /* FGT2_H */

lib/el3_runtime/aarch64/context_mgmt.c

@@ -25,6 +25,7 @@
 #include <lib/extensions/amu.h>
 #include <lib/extensions/brbe.h>
 #include <lib/extensions/debug_v8p9.h>
+#include <lib/extensions/fgt2.h>
 #include <lib/extensions/mpam.h>
 #include <lib/extensions/pmuv3.h>
 #include <lib/extensions/sme.h>
@@ -773,6 +774,10 @@ static void manage_extensions_nonsecure(cpu_context_t *ctx)
 		sme_enable(ctx);
 	}
 
+	if (is_feat_fgt2_supported()) {
+		fgt2_enable(ctx);
+	}
+
 	if (is_feat_debugv8p9_supported()) {
 		debugv8p9_extended_bp_wp_enable(ctx);
 	}
@@ -1103,6 +1108,24 @@ static void el2_sysregs_context_restore_fgt(el2_sysregs_t *ctx)
 	write_hfgwtr_el2(read_el2_ctx_fgt(ctx, hfgwtr_el2));
 }
 
+static void el2_sysregs_context_save_fgt2(el2_sysregs_t *ctx)
+{
+	write_el2_ctx_fgt2(ctx, hdfgrtr2_el2, read_hdfgrtr2_el2());
+	write_el2_ctx_fgt2(ctx, hdfgwtr2_el2, read_hdfgwtr2_el2());
+	write_el2_ctx_fgt2(ctx, hfgitr2_el2, read_hfgitr2_el2());
+	write_el2_ctx_fgt2(ctx, hfgrtr2_el2, read_hfgrtr2_el2());
+	write_el2_ctx_fgt2(ctx, hfgwtr2_el2, read_hfgwtr2_el2());
+}
+
+static void el2_sysregs_context_restore_fgt2(el2_sysregs_t *ctx)
+{
+	write_hdfgrtr2_el2(read_el2_ctx_fgt2(ctx, hdfgrtr2_el2));
+	write_hdfgwtr2_el2(read_el2_ctx_fgt2(ctx, hdfgwtr2_el2));
+	write_hfgitr2_el2(read_el2_ctx_fgt2(ctx, hfgitr2_el2));
+	write_hfgrtr2_el2(read_el2_ctx_fgt2(ctx, hfgrtr2_el2));
+	write_hfgwtr2_el2(read_el2_ctx_fgt2(ctx, hfgwtr2_el2));
+}
+
 static void el2_sysregs_context_save_mpam(el2_sysregs_t *ctx)
 {
 	u_register_t mpam_idr = read_mpamidr_el1();
@@ -1342,6 +1365,10 @@ void cm_el2_sysregs_context_save(uint32_t security_state)
 		el2_sysregs_context_save_fgt(el2_sysregs_ctx);
 	}
 
+	if (is_feat_fgt2_supported()) {
+		el2_sysregs_context_save_fgt2(el2_sysregs_ctx);
+	}
+
 	if (is_feat_ecv_v2_supported()) {
 		write_el2_ctx_ecv(el2_sysregs_ctx, cntpoff_el2, read_cntpoff_el2());
 	}
@@ -1425,6 +1452,10 @@ void cm_el2_sysregs_context_restore(uint32_t security_state)
 		el2_sysregs_context_restore_fgt(el2_sysregs_ctx);
 	}
 
+	if (is_feat_fgt2_supported()) {
+		el2_sysregs_context_restore_fgt2(el2_sysregs_ctx);
+	}
+
 	if (is_feat_ecv_v2_supported()) {
 		write_cntpoff_el2(read_el2_ctx_ecv(el2_sysregs_ctx, cntpoff_el2));
 	}

lib/extensions/fgt/fgt2.c

@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <lib/extensions/fgt2.h>
+
+void fgt2_enable(cpu_context_t *context)
+{
+	u_register_t reg;
+	el3_state_t *state;
+
+	state = get_el3state_ctx(context);
+
+	/* Set the FGTEN2 bit in SCR_EL3 to enable access to HFGITR2_EL2,
+	 * HFGRTR2_EL2, HFGWTR_EL2, HDFGRTR2_EL2, and HDFGWTR2_EL2.
+	 */
+
+	reg = read_ctx_reg(state, CTX_SCR_EL3);
+	reg |= SCR_FGTEN2_BIT;
+	write_ctx_reg(state, CTX_SCR_EL3, reg);
+}
+

make_helpers/arch_features.mk

@@ -337,6 +337,9 @@ TWED_DELAY				?=	0
 # Disable MTPMU if FEAT_MTPMU is supported.
 DISABLE_MTPMU				?=	0
 
+# Flag to enable FEAT_FGT2 (Fine Granular Traps 2)
+ENABLE_FEAT_FGT2			?=	0
+
 #----
 # 8.9
 #----

plat/arm/board/fvp/platform.mk

@@ -66,6 +66,7 @@ ENABLE_FEAT_SEL2		:= 2
 ENABLE_TRF_FOR_NS		:= 2
 ENABLE_FEAT_ECV			:= 2
 ENABLE_FEAT_FGT			:= 2
+ENABLE_FEAT_FGT2		:= 2
 ENABLE_FEAT_TCR2		:= 2
 ENABLE_FEAT_S2PIE		:= 2
 ENABLE_FEAT_S1PIE		:= 2