[bootwrapper PATCH 1/2] aarch64: Disable trapping into EL3 while accessing MDSELR_EL1

[Anshuman Khandual]

This disables trapping into EL3 while accessing MDSELR_EL1 either in EL1 or
EL2 via setting MDCR_EL3.EBWE. But first ensure that FEAT_Debugv8p9 feature
is implemented and enabled looking into ID_AA64DFR0_EL1.

Signed-off-by: Anshuman Khandual <anshuman.khandual at 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 d1f8fd9..124ef91 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -41,6 +41,7 @@
 #define MDCR_EL3_NSTB_NS_NOTRAP			(UL(3) << 24)
 #define MDCR_EL3_SBRBE_NOTRAP_NOPROHIBIT	(UL(3) << 32)
 #define MDCR_EL3_ENPMSN				BIT(36)
+#define MDCR_EL3_EBWE				BIT(43)
 
 #define SCR_EL3_RES1			BITS(5, 4)
 #define SCR_EL3_NS			BIT(0)
@@ -62,6 +63,7 @@
 #define ID_AA64DFR0_EL1_PMSVER		BITS(35, 32)
 #define ID_AA64DFR0_EL1_TRACEBUFFER	BITS(47, 44)
 #define ID_AA64DFR0_EL1_BRBE		BITS(55, 52)
+#define ID_AA64DFR0_EL1_DEBUGVER	BITS(3, 0)
 
 #define ID_AA64ISAR0_EL1_TME		BITS(27, 24)
 
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index c4e91e4..37cb45f 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -105,6 +105,9 @@ void cpu_init_el3(void)
 	if (mrs_field(ID_AA64DFR0_EL1, BRBE))
 		mdcr |= MDCR_EL3_SBRBE_NOTRAP_NOPROHIBIT;
 
+	if (mrs_field(ID_AA64DFR0_EL1, DEBUGVER) >= 11)
+		mdcr |= MDCR_EL3_EBWE;
+
 	msr(MDCR_EL3, mdcr);
 
 	if (mrs_field(ID_AA64PFR0_EL1, SVE)) {
-- 
2.25.1