[BOOT-WRAPPER PATCH 3/3] aarch64: Enable use of GCS for EL2 and below

[Mark Rutland <mark.rutland@arm.com>]

FEAT_GCS adds a number of new system registers and instructions. Usage
of some of these can trap to EL3 unless SCR_EL3.GCSEn is set, and so
boot-wrapper support is necessary.

Support for FEAT_GCS was added to Linux in the v6.13-rc1 merge window
without any boot-wrapper support. Consequently when GCS is enabled in a
model, the kernel will hang when attempting to write to GCS control
registers, which happens early in boot when the kernel configures EL2,
before any console output is produced.

FEAT_GCS is described in the latest ARM ARM (ARM DDI 0487K.a), which can
be found at:

  https://developer.arm.com/documentation/ddi0487/ka/?lang=en

Add boot-wrapper support for FEAT_GCS. In addition to setting
SCR_EL3.GCSEn, it's necessary to initialize GCSCR_EL2, GCSCR_EL1, and
GCSCRE0_EL1 such that older kernel which are not aware of GCS don't find
GCS enabled unexpectedly.

Signed-off-by: Mark Rutland <mark.rutland@arm.com>
Cc: Mark Brown <broonie@kernel.org>
---
 arch/aarch64/include/asm/cpu.h | 6 ++++++
 arch/aarch64/init.c            | 7 +++++++
 2 files changed, 13 insertions(+)

diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index 3ef58f3..4cf0ff7 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -62,6 +62,7 @@
 #define SCR_EL3_ECVEN			BIT(28)
 #define SCR_EL3_TME			BIT(34)
 #define SCR_EL3_HXEn			BIT(38)
+#define SCR_EL3_GCSEn			BIT(39)
 #define SCR_EL3_EnTP2			BIT(41)
 #define SCR_EL3_RCWMASKEn		BIT(42)
 #define SCR_EL3_TCR2EN			BIT(43)
@@ -115,6 +116,7 @@
 #define ID_AA64PFR1_EL1_MTE		BITS(11, 8)
 #define ID_AA64PFR1_EL1_SME		BITS(27, 24)
 #define ID_AA64PFR1_EL1_CSV2_frac	BITS(35, 32)
+#define ID_AA64PFR1_EL1_GCS		BITS(47, 44)
 #define ID_AA64PFR1_EL1_THE		BITS(51, 48)
 
 #define ID_AA64PFR2_EL1			s3_0_c0_c4_2
@@ -169,6 +171,10 @@
 #define SMCR_EL3_FA64		BIT(31)
 #define SMCR_EL3_LEN_MAX	0xf
 
+#define GCSCRE0_EL1		s3_0_c2_c5_2
+#define GCSCR_EL1		s3_0_c2_c5_0
+#define GCSCR_EL2		s3_4_c2_c5_0
+
 #define ID_AA64ISAR2_EL1	s3_0_c0_c6_2
 
 #define ID_AA64MMFR3_EL1	s3_0_c0_c7_3
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index 1f38516..61b55f9 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -124,6 +124,13 @@ static void cpu_init_el3(void)
 	if (mrs_field(ID_AA64PFR1_EL1, THE))
 		scr |= SCR_EL3_RCWMASKEn;
 
+	if (mrs_field(ID_AA64PFR1_EL1, GCS)) {
+		scr |= SCR_EL3_GCSEn;
+		msr(GCSCR_EL2, 0);
+		msr(GCSCR_EL1, 0);
+		msr(GCSCRE0_EL1, 0);
+	}
+
 	if (mrs_field(ID_AA64PFR2_EL1, FPMR))
 		scr |= SCR_EL3_EnFPM;
 
-- 
2.30.2