refactor(cm): set MDCR_EL3/CPTR_EL3 bits in respective feat_init_el3() only

These bits (MDCR_EL3.{NSTB, NSTBE, TTRF, TPM}, CPTR_EL3.TTA) only affect
EL2 (and lower) execution. Each feat_init_el3() is called long before
any lower EL has had a chance to execute, so setting the bits at reset
is redundant. Removing them from reset code also improves readability of
the immutable EL3 state.

Preserve the original intention for the TTA bit of "enabled for NS and
disabled everywhere else" (inferred from commit messages d4582d3088 and
2031d6166a and the comment). This is because CPTR_EL3 will be contexted
and so everyone will eventually get whatever NS has anyway.

Signed-off-by: Boyan Karatotev <boyan.karatotev@arm.com>
Change-Id: I3d24b45d3ea80882c8e450b2d9db9d5531facec1

8 files changed, 44 insertions, 51 deletions

diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 60a11bf54..b19e8af78 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -596,7 +596,7 @@
 #define MDCR_SBRBE_MASK		ULL(0x3)
 #define MDCR_NSTB(x)		((x) << 24)
 #define MDCR_NSTB_EL1		ULL(0x3)
-#define MDCR_NSTBE		(ULL(1) << 26)
+#define MDCR_NSTBE_BIT		(ULL(1) << 26)
 #define MDCR_MTPME_BIT		(ULL(1) << 28)
 #define MDCR_TDCC_BIT		(ULL(1) << 27)
 #define MDCR_SCCD_BIT		(ULL(1) << 23)
diff --git a/include/arch/aarch64/el3_common_macros.S b/include/arch/aarch64/el3_common_macros.S
index 6360461fd..fa9310ef1 100644
--- a/include/arch/aarch64/el3_common_macros.S
+++ b/include/arch/aarch64/el3_common_macros.S
@@ -115,33 +115,11 @@
 	 * MDCR_EL3.TDA: Set to zero to allow EL0, EL1 and EL2 access to the
 	 *  debug registers, other than those registers that are controlled by
 	 *  MDCR_EL3.TDOSA.
-	 *
-	 * MDCR_EL3.TPM: Set to zero so that EL0, EL1, and EL2 System register
-	 *  accesses to all Performance Monitors registers do not trap to EL3.
-	 *
-	 * MDCR_EL3.NSTB, MDCR_EL3.NSTBE: Set to zero so that Trace Buffer
-	 *  owning security state is Secure state. If FEAT_TRBE is implemented,
-	 *  accesses to Trace Buffer control registers at EL2 and EL1 in any
-	 *  security state generates trap exceptions to EL3.
-	 *  If FEAT_TRBE is not implemented, these bits are RES0.
-	 *
-	 * MDCR_EL3.TTRF: Set to one so that access to trace filter control
-	 *  registers in non-monitor mode generate EL3 trap exception,
-	 *  unless the access generates a higher priority exception when trace
-	 *  filter control(FEAT_TRF) is implemented.
-	 *  When FEAT_TRF is not implemented, this bit is RES0.
-	 * ---------------------------------------------------------------------
 	 */
 	mov_imm	x0, ((MDCR_EL3_RESET_VAL | MDCR_SDD_BIT | \
 		      MDCR_SPD32(MDCR_SPD32_DISABLE)) & \
-		    ~(MDCR_TDOSA_BIT | MDCR_TDA_BIT | MDCR_TPM_BIT | \
-		      MDCR_NSTB(MDCR_NSTB_EL1) | MDCR_NSTBE | MDCR_TTRF_BIT))
+		    ~(MDCR_TDOSA_BIT | MDCR_TDA_BIT))
 
-	mrs	x1, id_aa64dfr0_el1
-	ubfx	x1, x1, #ID_AA64DFR0_TRACEFILT_SHIFT, #ID_AA64DFR0_TRACEFILT_LENGTH
-	cbz	x1, 1f
-	orr	x0, x0, #MDCR_TTRF_BIT
-1:
 	msr	mdcr_el3, x0
 
 	/* ---------------------------------------------------------------------
@@ -158,15 +136,6 @@
 	 * CPTR_EL3.TCPAC: Set to zero so that any accesses to CPACR_EL1,
 	 *  CPTR_EL2, CPACR, or HCPTR do not trap to EL3.
 	 *
-	 * CPTR_EL3.TTA: Set to one so that accesses to the trace system
-	 *  registers trap to EL3 from all exception levels and security
-	 *  states when system register trace is implemented.
-	 *  When system register trace is not implemented, this bit is RES0 and
-	 *  hence set to zero.
-	 *
-	 * CPTR_EL3.TTA: Set to zero so that System register accesses to the
-	 *  trace registers do not trap to EL3.
-	 *
 	 * CPTR_EL3.TFP: Set to zero so that accesses to the V- or Z- registers
 	 *  by Advanced SIMD, floating-point or SVE instructions (if implemented)
 	 *  do not trap to EL3.
@@ -181,12 +150,7 @@
 	 *  to EL3 by default.
 	 */
 
-	mov_imm x0, (CPTR_EL3_RESET_VAL & ~(TCPAC_BIT | TTA_BIT | TFP_BIT))
-	mrs	x1, id_aa64dfr0_el1
-	ubfx	x1, x1, #ID_AA64DFR0_TRACEVER_SHIFT, #ID_AA64DFR0_TRACEVER_LENGTH
-	cbz	x1, 1f
-	orr	x0, x0, #TTA_BIT
-1:
+	mov_imm x0, (CPTR_EL3_RESET_VAL & ~(TCPAC_BIT | TFP_BIT))
 	msr	cptr_el3, x0
 
 	/*
diff --git a/include/lib/extensions/sys_reg_trace.h b/include/lib/extensions/sys_reg_trace.h
index d9f7f1b92..beda88a46 100644
--- a/include/lib/extensions/sys_reg_trace.h
+++ b/include/lib/extensions/sys_reg_trace.h
@@ -13,6 +13,7 @@
 
 #if __aarch64__
 void sys_reg_trace_enable(cpu_context_t *context);
+void sys_reg_trace_disable(cpu_context_t *context);
 void sys_reg_trace_init_el2_unused(void);
 #else
 void sys_reg_trace_init_el3(void);
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 8fd9f751e..9d717bb4e 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -662,6 +662,11 @@ static void manage_extensions_secure(cpu_context_t *ctx)
 			sme_disable(ctx);
 		}
 	}
+
+	/* NS can access this but Secure shouldn't */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_disable(ctx);
+	}
 #endif /* IMAGE_BL31 */
 }
 
diff --git a/lib/extensions/pmuv3/aarch64/pmuv3.c b/lib/extensions/pmuv3/aarch64/pmuv3.c
index fda71aa33..61fc47dc7 100644
--- a/lib/extensions/pmuv3/aarch64/pmuv3.c
+++ b/lib/extensions/pmuv3/aarch64/pmuv3.c
@@ -82,9 +82,12 @@ void pmuv3_init_el3(void)
 	 *   0  |  1   |    enabled   | disabled
 	 *   1  |  1   |    enabled   | disabled only for counters 0 to
 	 *                              MDCR_EL2.HPMN - 1. Enabled for the rest
+	 *
+	 * MDCR_EL3.TPM: Set to zero so that EL0, EL1, and EL2 System register
+	 *  accesses to all Performance Monitors registers do not trap to EL3.
 	 */
 	mdcr_el3 = (mdcr_el3 | MDCR_SCCD_BIT | MDCR_MCCD_BIT) &
-		  ~(MDCR_MPMX_BIT | MDCR_SPME_BIT);
+		  ~(MDCR_MPMX_BIT | MDCR_SPME_BIT | MDCR_TPM_BIT);
 	mdcr_el3 = mtpmu_disable_el3(mdcr_el3);
 	write_mdcr_el3(mdcr_el3);
 
diff --git a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
index 4b57f67da..134956689 100644
--- a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
+++ b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
@@ -12,16 +12,26 @@
 
 void sys_reg_trace_enable(cpu_context_t *ctx)
 {
-	uint64_t val;
+	/*
+	 * CPTR_EL3.TTA: Set to zero so that System register accesses to the
+	 *  trace registers do not trap to EL3.
+	 */
+	uint64_t val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+
+	val &= ~(TTA_BIT);
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
+}
 
-	/* Retrieve CPTR_EL3 value from the given context 'ctx',
-	 * and update CPTR_EL3.TTA bit to 0.
-	 * This function is called while switching context to NS to
-	 * allow system trace register access to NS-EL2 and NS-EL1
-	 * when NS-EL2 is implemented but not used.
+void sys_reg_trace_disable(cpu_context_t *ctx)
+{
+	/*
+	 * CPTR_EL3.TTA: Set to one so that System register accesses to the
+	 *  trace registers trap to EL3, unless it is trapped by CPACR.TRCDIS,
+	 *  CPACR_EL1.TTA, or CPTR_EL2.TTA
 	 */
-	val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
-	val &= ~TTA_BIT;
+	uint64_t val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+
+	val |= TTA_BIT;
 	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
 }
 
diff --git a/lib/extensions/trbe/trbe.c b/lib/extensions/trbe/trbe.c
index 461ea73a4..d4fbdfbc1 100644
--- a/lib/extensions/trbe/trbe.c
+++ b/lib/extensions/trbe/trbe.c
@@ -24,13 +24,18 @@ void trbe_init_el3(void)
 	u_register_t val;
 
 	/*
+	 * MDCR_EL3.NSTBE = 0b0
+	 *  Trace Buffer owning Security state is Non-secure state. If FEAT_RME
+	 *  is not implemented, this field is RES0.
+	 *
 	 * MDCR_EL3.NSTB = 0b11
-	 * Allow access of trace buffer control registers from NS-EL1
-	 * and NS-EL2, tracing is prohibited in Secure and Realm state
-	 * (if implemented).
+	 *  Allow access of trace buffer control registers from NS-EL1 and
+	 *  NS-EL2, tracing is prohibited in Secure and Realm state (if
+	 *  implemented).
 	 */
 	val = read_mdcr_el3();
 	val |= MDCR_NSTB(MDCR_NSTB_EL1);
+	val &= ~(MDCR_NSTBE_BIT);
 	write_mdcr_el3(val);
 }
 
diff --git a/services/std_svc/rmmd/rmmd_main.c b/services/std_svc/rmmd/rmmd_main.c
index c80b524e6..a929ea282 100644
--- a/services/std_svc/rmmd/rmmd_main.c
+++ b/services/std_svc/rmmd/rmmd_main.c
@@ -128,6 +128,11 @@ static void manage_extensions_realm(cpu_context_t *ctx)
 		sve_enable(ctx);
 	}
 
+	/* NS can access this but Realm shouldn't */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_disable(ctx);
+	}
+
 	pmuv3_enable(ctx);
 }