[PATCH V2 2/3] aarch64: Enable access into 128 bit system registers from EL2 and below

[Anshuman Khandual <anshuman.khandual@arm.com>]

FEAT_D128 adds 128 bit system registers like PAR_EL1, TTBR0_EL1, TTBR1_EL1,
TTBR0_EL2, TTBR1_EL2, TTBR0_EL12, TTBR1_EL12 , VTTBR_EL2, RCWMASK_EL1, and
RCWSMASK_EL1. But access into these register from EL2 and below trap to EL3
unless SCR_EL3.D128En is set.

Enable access to 128 bit registers when they are implemented.

Signed-off-by: Anshuman Khandual <anshuman.khandual@arm.com>
---
 arch/aarch64/include/asm/cpu.h | 2 ++
 arch/aarch64/init.c            | 3 +++
 2 files changed, 5 insertions(+)

diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index 85e735b..57c9cf2 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -61,6 +61,7 @@
 #define SCR_EL3_TCR2EN			BIT(43)
 #define SCR_EL3_SCTLR2En		BIT(44)
 #define SCR_EL3_PIEN			BIT(45)
+#define SCR_EL3_D128En			BIT(47)
 
 #define HCR_EL2_RES1			BIT(1)
 
@@ -90,6 +91,7 @@
 #define ID_AA64MMFR3_EL1_S2PIE		BITS(15, 12)
 #define ID_AA64MMFR3_EL1_S1POE		BITS(19, 16)
 #define ID_AA64MMFR3_EL1_S2POE		BITS(23, 20)
+#define ID_AA64MMFR3_EL1_D128		BITS(35, 32)
 
 #define ID_AA64PFR1_EL1_MTE		BITS(11, 8)
 #define ID_AA64PFR1_EL1_SME		BITS(27, 24)
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index fc0d2e3..afe4398 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -95,6 +95,9 @@ void cpu_init_el3(void)
 		msr(SCTLR2_EL1, 0);
 	}
 
+	if (mrs_field(ID_AA64MMFR3_EL1, D128))
+		scr |= SCR_EL3_D128En;
+
 	msr(SCR_EL3, scr);
 
 	msr(CPTR_EL3, cptr);
-- 
2.25.1