[boot-wrapper-aarch64 PATCH] Enable GCS if it is present in the HW

[Tamas Kaman]

Signed-off-by: Tamas Kaman <tamas.kaman at arm.com>
---
 arch/aarch64/include/asm/cpu.h | 5 +++++
 arch/aarch64/init.c            | 7 +++++++
 2 files changed, 12 insertions(+)

diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index ac50474..9d6f0fc 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -70,6 +70,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)
@@ -125,6 +126,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
@@ -133,6 +135,9 @@
 #define ID_AA64SMFR0_EL1		s3_0_c0_c4_5
 #define ID_AA64SMFR0_EL1_FA64		BIT(63)
 
+#define HCRX_EL2			s3_4_c1_c2_2
+#define HCRX_EL2_GCSEn			BIT(22)
+
 /*
  * Initial register values required for the boot-wrapper to run out-of-reset.
  */
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index cb24f4e..f815e6a 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -118,6 +118,13 @@ static void cpu_init_el3(void)
 	if (mrs_field(ID_AA64PFR1_EL1, MTE) >= 2)
 		scr |= SCR_EL3_ATA;
 
+	if (mrs_field(ID_AA64PFR1_EL1, GCS)) {
+		scr |= SCR_EL3_GCSEn;
+		unsigned long hcrx_el2 = mrs(HCRX_EL2);
+		hcrx_el2 |= HCRX_EL2_GCSEn;
+		msr(HCRX_EL2, hcrx_el2);
+	}
+
 	if (!kernel_is_32bit())
 		scr |= SCR_EL3_RW;
 
-- 
2.34.1