[boot-wrapper 1/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 124ef91..0b8b463 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -57,6 +57,7 @@
 #define SCR_EL3_EnTP2			BIT(41)
 #define SCR_EL3_TCR2EN			BIT(43)
 #define SCR_EL3_PIEN			BIT(45)
+#define SCR_EL3_D128En			BIT(47)
 
 #define HCR_EL2_RES1			BIT(1)
 
@@ -85,6 +86,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 37cb45f..7d9d0d9 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -89,6 +89,9 @@ void cpu_init_el3(void)
 	if (!kernel_is_32bit())
 		scr |= SCR_EL3_RW;
 
+	if (mrs_field(ID_AA64MMFR3_EL1, D128))
+		scr |= SCR_EL3_D128En;
+
 	msr(SCR_EL3, scr);
 
 	msr(CPTR_EL3, cptr);
-- 
2.25.1