[boot-wrapper] aarch64: Enable access into FEAT_FGT2 registers from EL2 and below

[Anshuman Khandual]

FEAT_FGT2 adds system registers HDFGRTR2_EL2, HDFGWTR2_EL2, HFGITR2_EL2,
HFGRTR2_EL2 and HFGWTR2_EL2. But access into these system registers from
EL2 and below exception levels, will trap into EL3 unless SCR_EL3.FGTEN2
is set.

Enable access to FEAT_FGT2 registers when they are implemented. Given that
these new FEAT_FGT2 trap registers have fields that reset to UNKNOWN values
when resets are taken to EL3, this initialises all registers as cleared.

Signed-off-by: Anshuman Khandual <anshuman.khandual at arm.com>
---
This picks up from an earlier discussion regarding [PATCH 2/2].

https://lore.kernel.org/all/20240404073726.947215-1-anshuman.khandual@arm.com/

 arch/aarch64/include/asm/cpu.h | 7 +++++++
 arch/aarch64/init.c            | 9 +++++++++
 2 files changed, 16 insertions(+)

diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index a5744e1..878d03d 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -23,6 +23,12 @@
 #define SCTLR2_EL1		s3_0_c1_c0_3
 #define SCTLR2_EL2		s3_4_c1_c0_3
 
+#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 HFGITR2_EL2		s3_4_c3_c1_7
+
 /*
  * RES1 bit definitions definitions as of ARM DDI 0487G.b
  *
@@ -63,6 +69,7 @@
 #define SCR_EL3_SCTLR2En		BIT(44)
 #define SCR_EL3_PIEN			BIT(45)
 #define SCR_EL3_D128En			BIT(47)
+#define SCR_EL3_FGTEN2			BIT(59)
 
 #define HCR_EL2_RES1			BIT(1)
 
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index c9fc7f1..b4d64a0 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -68,6 +68,15 @@ void cpu_init_el3(void)
 	if (mrs_field(ID_AA64MMFR0_EL1, FGT))
 		scr |= SCR_EL3_FGTEN;
 
+	if (mrs_field(ID_AA64MMFR0_EL1, FGT) >= 2) {
+		scr |= SCR_EL3_FGTEN2;
+		msr(HDFGRTR2_EL2, 0);
+		msr(HDFGWTR2_EL2, 0);
+		msr(HFGITR2_EL2, 0);
+		msr(HFGRTR2_EL2, 0);
+		msr(HFGWTR2_EL2, 0);
+	}
+
 	if (mrs_field(ID_AA64MMFR0_EL1, ECV) >= 2)
 		scr |= SCR_EL3_ECVEN;
 
-- 
2.25.1