8 files changed, 59 insertions, 1 deletions
diff --git a/include/common/aarch32/el3_common_macros.S b/include/common/aarch32/el3_common_macros.S
index 50ce952f..0018ea4b 100644
--- a/include/common/aarch32/el3_common_macros.S
+++ b/include/common/aarch32/el3_common_macros.S
@@ -67,6 +67,14 @@
 	orr	r0, r0, #SCR_SIF_BIT
 	stcopr	r0, SCR
 
+	/* -----------------------------------------------------------------
+	 * Reset those registers that may have architecturally unknown reset
+	 * values
+	 * -----------------------------------------------------------------
+	 */
+	mov	r0, #0
+	stcopr	r0, SDCR
+
 	/* -----------------------------------------------------
 	 * Enable the Asynchronous data abort now that the
 	 * exception vectors have been setup.
diff --git a/include/common/aarch64/el3_common_macros.S b/include/common/aarch64/el3_common_macros.S
index 9b22a734..a4189116 100644
--- a/include/common/aarch64/el3_common_macros.S
+++ b/include/common/aarch64/el3_common_macros.S
@@ -77,6 +77,13 @@
 	 */
 	mov	x0, #(SCR_RES1_BITS | SCR_EA_BIT | SCR_SIF_BIT)
 	msr	scr_el3, x0
+
+	/* ---------------------------------------------------------------------
+	 * Reset registers that may have architecturally unknown reset values
+	 * ---------------------------------------------------------------------
+	 */
+	msr	mdcr_el3, xzr
+
 	/* ---------------------------------------------------------------------
 	 * Enable External Aborts and SError Interrupts now that the exception
 	 * vectors have been setup.
diff --git a/include/lib/aarch32/arch.h b/include/lib/aarch32/arch.h
index 4968e245..3c5ab26e 100644
--- a/include/lib/aarch32/arch.h
+++ b/include/lib/aarch32/arch.h
@@ -318,6 +318,11 @@
 
 #define MAX_CACHE_LINE_SIZE	0x800 /* 2KB */
 
+/* PMCR definitions */
+#define PMCR_N_SHIFT		11
+#define PMCR_N_MASK		0x1f
+#define PMCR_N_BITS		(PMCR_N_MASK << PMCR_N_SHIFT)
+
 /*******************************************************************************
  * Definitions of register offsets and fields in the CNTCTLBase Frame of the
  * system level implementation of the Generic Timer.
@@ -375,6 +380,11 @@
 #define CSSELR		p15, 2, c0, c0, 0
 #define CCSIDR		p15, 1, c0, c0, 0
 
+/* Debug register defines. The format is: coproc, opt1, CRn, CRm, opt2 */
+#define HDCR		p15, 4, c1, c1, 1
+#define SDCR		p15, 0, c1, c3, 1
+#define PMCR		p15, 0, c9, c12, 0
+
 /* GICv3 CPU Interface system register defines. The format is: coproc, opt1, CRn, CRm, opt2 */
 #define ICC_IAR1	p15, 0, c12, c12, 0
 #define ICC_IAR0	p15, 0, c12, c8, 0
diff --git a/include/lib/aarch32/arch_helpers.h b/include/lib/aarch32/arch_helpers.h
index 3b4349c3..0633bca2 100644
--- a/include/lib/aarch32/arch_helpers.h
+++ b/include/lib/aarch32/arch_helpers.h
@@ -249,6 +249,9 @@ DEFINE_COPROCR_RW_FUNCS(icc_iar1_el1, ICC_IAR1)
 DEFINE_COPROCR_RW_FUNCS(icc_eoir0_el1, ICC_EOIR0)
 DEFINE_COPROCR_RW_FUNCS(icc_eoir1_el1, ICC_EOIR1)
 
+DEFINE_COPROCR_RW_FUNCS(hdcr, HDCR)
+DEFINE_COPROCR_READ_FUNC(pmcr, PMCR)
+
 /*
  * TLBI operation prototypes
  */
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index bef60323..a034ae20 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -411,4 +411,9 @@
 #define CNTACR_RWVT_SHIFT	0x4
 #define CNTACR_RWPT_SHIFT	0x5
 
+/* PMCR_EL0 definitions */
+#define PMCR_EL0_N_SHIFT	11
+#define PMCR_EL0_N_MASK		0x1f
+#define PMCR_EL0_N_BITS		(PMCR_EL0_N_MASK << PMCR_EL0_N_SHIFT)
+
 #endif /* __ARCH_H__ */
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 4d936ad5..37db0313 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -279,6 +279,9 @@ DEFINE_SYSREG_READ_FUNC(isr_el1)
 
 DEFINE_SYSREG_READ_FUNC(ctr_el0)
 
+DEFINE_SYSREG_RW_FUNCS(mdcr_el2)
+DEFINE_SYSREG_READ_FUNC(pmcr_el0)
+
 DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sre_el1, ICC_SRE_EL1)
 DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sre_el2, ICC_SRE_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sre_el3, ICC_SRE_EL3)
diff --git a/lib/el3_runtime/aarch32/context_mgmt.c b/lib/el3_runtime/aarch32/context_mgmt.c
index 02ae2a7e..29532e8c 100644
--- a/lib/el3_runtime/aarch32/context_mgmt.c
+++ b/lib/el3_runtime/aarch32/context_mgmt.c
@@ -200,7 +200,10 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			isb();
 		} else if (read_id_pfr1() &
 			(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) {
-			/* Set the NS bit to access HCR, HCPTR, CNTHCTL, VPIDR, VMPIDR */
+			/*
+			 * Set the NS bit to access NS copies of certain banked
+			 * registers
+			 */
 			write_scr(read_scr() | SCR_NS_BIT);
 			isb();
 
@@ -231,6 +234,15 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			 * translation are disabled.
 			 */
 			write64_vttbr(0);
+
+			/*
+			 * Avoid unexpected debug traps in case where HDCR
+			 * is not completely reset by the hardware - set
+			 * HDCR.HPMN to PMCR.N and zero the remaining bits.
+			 * The HDCR.HPMN and PMCR.N fields are the same size
+			 * (5 bits) and HPMN is at offset zero within HDCR.
+			 */
+			write_hdcr((read_pmcr() & PMCR_N_BITS) >> PMCR_N_SHIFT);
 			isb();
 
 			write_scr(read_scr() & ~SCR_NS_BIT);
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 4b5d0ee5..fadc1dbf 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -259,6 +259,16 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			 * translation are disabled.
 			 */
 			write_vttbr_el2(0);
+			/*
+			 * Avoid unexpected debug traps in case where MDCR_EL2
+			 * is not completely reset by the hardware - set
+			 * MDCR_EL2.HPMN to PMCR_EL0.N and zero the remaining
+			 * bits.
+			 * MDCR_EL2.HPMN and PMCR_EL0.N fields are the same size
+			 * (5 bits) and HPMN is at offset zero within MDCR_EL2.
+			 */
+			write_mdcr_el2((read_pmcr_el0() & PMCR_EL0_N_BITS)
+					>> PMCR_EL0_N_SHIFT);
 		}
 	}