feat(sys_reg_trace): enable trace system registers access from lower NS ELs

Introduced a build flag 'ENABLE_SYS_REG_TRACE_FOR_NS' to enable trace
system registers access in NS-EL2, or NS-EL1 (when NS-EL2 is
implemented but unused).

Change-Id: Idc1acede4186e101758cbf7bed5af7b634d7d18d
Signed-off-by: Manish V Badarkhe <Manish.Badarkhe@arm.com>

11 files changed, 124 insertions, 0 deletions

diff --git a/Makefile b/Makefile
index c8ffdab06..5de0e07fa 100644
--- a/Makefile
+++ b/Makefile
@@ -965,6 +965,7 @@ $(eval $(call assert_booleans,\
         ENABLE_FEAT_SB \
         PSA_FWU_SUPPORT \
         ENABLE_TRBE_FOR_NS \
+        ENABLE_SYS_REG_TRACE_FOR_NS \
 )))
 
 $(eval $(call assert_numerics,\
@@ -1066,6 +1067,7 @@ $(eval $(call add_defines,\
         NR_OF_IMAGES_IN_FW_BANK \
         PSA_FWU_SUPPORT \
         ENABLE_TRBE_FOR_NS \
+        ENABLE_SYS_REG_TRACE_FOR_NS \
 )))
 
 ifeq (${SANITIZE_UB},trap)
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index dc3d3c0be..ccd54bb5d 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -94,6 +94,10 @@ ifeq (${ENABLE_TRBE_FOR_NS},1)
 BL31_SOURCES		+=	lib/extensions/trbe/trbe.c
 endif
 
+ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
+BL31_SOURCES		+=      lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
+endif
+
 ifeq (${WORKAROUND_CVE_2017_5715},1)
 BL31_SOURCES		+=	lib/cpus/aarch64/wa_cve_2017_5715_bpiall.S	\
 				lib/cpus/aarch64/wa_cve_2017_5715_mmu.S
diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index 8b5eddd66..486d39d91 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk
@@ -42,6 +42,10 @@ BL32_SOURCES		+=	services/std_svc/trng/trng_main.c	\
 				services/std_svc/trng/trng_entropy_pool.c
 endif
 
+ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
+BL32_SOURCES		+=	lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
+endif
+
 BL32_LINKERFILE	:=	bl32/sp_min/sp_min.ld.S
 
 # Include the platform-specific SP_MIN Makefile
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 57b880a9f..15535d4a8 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -781,6 +781,11 @@ Common build options
   feature for AArch64. The default is 0 and it is automatically disabled when
   the target architecture is AArch32.
 
+- ``ENABLE_SYS_REG_TRACE_FOR_NS``: Boolean option to enable trace system
+  registers access from NS ELs, NS-EL2 or NS-EL1 (when NS-EL2 is implemented
+  but unused). This feature is available if trace unit such as ETMv4.x, and
+  ETE(extending ETM feature) is implemented. This flag is disabled by default.
+
 GICv3 driver options
 --------------------
 
diff --git a/include/arch/aarch32/arch_helpers.h b/include/arch/aarch32/arch_helpers.h
index 726baf596..8d628c1d5 100644
--- a/include/arch/aarch32/arch_helpers.h
+++ b/include/arch/aarch32/arch_helpers.h
@@ -217,6 +217,7 @@ DEFINE_SYSREG_RW_FUNCS(cpsr)
 DEFINE_COPROCR_READ_FUNC(mpidr, MPIDR)
 DEFINE_COPROCR_READ_FUNC(midr, MIDR)
 DEFINE_COPROCR_READ_FUNC(id_mmfr4, ID_MMFR4)
+DEFINE_COPROCR_READ_FUNC(id_dfr0, ID_DFR0)
 DEFINE_COPROCR_READ_FUNC(id_pfr0, ID_PFR0)
 DEFINE_COPROCR_READ_FUNC(id_pfr1, ID_PFR1)
 DEFINE_COPROCR_READ_FUNC(isr, ISR)
diff --git a/include/lib/extensions/sys_reg_trace.h b/include/lib/extensions/sys_reg_trace.h
new file mode 100644
index 000000000..74470fee2
--- /dev/null
+++ b/include/lib/extensions/sys_reg_trace.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SYS_REG_TRACE_H
+#define SYS_REG_TRACE_H
+
+#include <context.h>
+
+#if __aarch64__
+void sys_reg_trace_enable(cpu_context_t *context);
+#else
+void sys_reg_trace_enable(void);
+#endif /* __aarch64__ */
+
+#endif /* SYS_REG_TRACE_H */
diff --git a/lib/el3_runtime/aarch32/context_mgmt.c b/lib/el3_runtime/aarch32/context_mgmt.c
index 81d793b46..2e5b38ff3 100644
--- a/lib/el3_runtime/aarch32/context_mgmt.c
+++ b/lib/el3_runtime/aarch32/context_mgmt.c
@@ -16,6 +16,7 @@
 #include <context.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/extensions/amu.h>
+#include <lib/extensions/sys_reg_trace.h>
 #include <lib/utils.h>
 
 /*******************************************************************************
@@ -136,6 +137,10 @@ static void enable_extensions_nonsecure(bool el2_unused)
 #if ENABLE_AMU
 	amu_enable(el2_unused);
 #endif
+
+#if ENABLE_SYS_REG_TRACE_FOR_NS
+	sys_reg_trace_enable();
+#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
 #endif
 }
 
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 9eb66ed84..0eb800a8f 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -22,6 +22,7 @@
 #include <lib/extensions/mpam.h>
 #include <lib/extensions/spe.h>
 #include <lib/extensions/sve.h>
+#include <lib/extensions/sys_reg_trace.h>
 #include <lib/extensions/trbe.h>
 #include <lib/extensions/twed.h>
 #include <lib/utils.h>
@@ -354,6 +355,10 @@ static void enable_extensions_nonsecure(bool el2_unused, cpu_context_t *ctx)
 	trbe_enable();
 #endif /* ENABLE_TRBE_FOR_NS */
 
+#if ENABLE_SYS_REG_TRACE_FOR_NS
+	sys_reg_trace_enable(ctx);
+#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
+
 #endif
 }
 
@@ -463,6 +468,8 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			 * CPTR_EL2.TTA: Set to zero so that Non-secure System
 			 *  register accesses to the trace registers from both
 			 *  Execution states do not trap to EL2.
+			 *  If PE trace unit System registers are not implemented
+			 *  then this bit is reserved, and must be set to zero.
 			 *
 			 * CPTR_EL2.TFP: Set to zero so that Non-secure accesses
 			 *  to SIMD and floating-point functionality from both
diff --git a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
new file mode 100644
index 000000000..89b8029ca
--- /dev/null
+++ b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/extensions/sys_reg_trace.h>
+
+static bool sys_reg_trace_supported(void)
+{
+	uint32_t features;
+
+	features = read_id_dfr0() >> ID_DFR0_COPTRC_SHIFT;
+	return ((features & ID_DFR0_COPTRC_MASK) ==
+		ID_DFR0_COPTRC_SUPPORTED);
+}
+
+void sys_reg_trace_enable(void)
+{
+	uint32_t val;
+
+	if (sys_reg_trace_supported()) {
+		/*
+		 * NSACR.NSTRCDIS = b0
+		 * enable NS system register access to implemented trace
+		 * registers.
+		 */
+		val = read_nsacr();
+		val &= ~NSTRCDIS_BIT;
+		write_nsacr(val);
+	}
+}
diff --git a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
new file mode 100644
index 000000000..960d69842
--- /dev/null
+++ b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/extensions/sys_reg_trace.h>
+
+static bool sys_reg_trace_supported(void)
+{
+	uint64_t features;
+
+	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_TRACEVER_SHIFT;
+	return ((features & ID_AA64DFR0_TRACEVER_MASK) ==
+		ID_AA64DFR0_TRACEVER_SUPPORTED);
+}
+
+void sys_reg_trace_enable(cpu_context_t *ctx)
+{
+	uint64_t val;
+
+	if (sys_reg_trace_supported()) {
+		/* 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.
+		 */
+		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/make_helpers/defaults.mk b/make_helpers/defaults.mk
index e95d2268f..060bc3864 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -366,3 +366,8 @@ ifneq (${ARCH},aarch32)
 else
     override ENABLE_TRBE_FOR_NS	:= 0
 endif
+
+# By default, disable access of trace system registers from NS lower
+# ELs  i.e. NS-EL2, or NS-EL1 if NS-EL2 implemented but unused if
+# system register trace is implemented.
+ENABLE_SYS_REG_TRACE_FOR_NS	:= 0