From 721249b0c0cce9fbe60175af6ee895e2bb7a6d10 Mon Sep 17 00:00:00 2001
From: Arvind Ram Prakash <arvind.ramprakash@arm.com>
Date: Mon, 5 Aug 2024 16:11:42 -0500
Subject: [PATCH] feat(cm): asymmetric feature support for trbe

This patch checks if the Errata 2938996(Cortex-A520) , 2726228(Cortex-X4)
applies to cores and if affected applies the errata workaround which
disables TRBE.

Signed-off-by: Arvind Ram Prakash <arvind.ramprakash@arm.com>
Change-Id: I53b037839820c8b3a869f393588302a365d5b97c
---
 bl31/bl31.mk                           |  1 +
 lib/cpus/errata_common.c               | 30 ++++++++++++++++++++++++++
 lib/el3_runtime/aarch64/context_mgmt.c | 13 +++++++++++
 3 files changed, 44 insertions(+)
 create mode 100644 lib/cpus/errata_common.c

diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 7e9fde352..ec68a8d59 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -42,6 +42,7 @@ BL31_SOURCES		+=	bl31/bl31_main.c				\
 				bl31/bl31_context_mgmt.c			\
 				bl31/bl31_traps.c				\
 				common/runtime_svc.c				\
+				lib/cpus/errata_common.c			\
 				lib/cpus/aarch64/dsu_helpers.S			\
 				plat/common/aarch64/platform_mp_stack.S		\
 				services/arm_arch_svc/arm_arch_svc_setup.c	\
diff --git a/lib/cpus/errata_common.c b/lib/cpus/errata_common.c
new file mode 100644
index 000000000..9801245bc
--- /dev/null
+++ b/lib/cpus/errata_common.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Runtime C routines for errata workarounds and common routines */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <cortex_a520.h>
+#include <cortex_x4.h>
+#include <lib/cpus/cpu_ops.h>
+#include <lib/cpus/errata.h>
+
+#if ERRATA_A520_2938996 || ERRATA_X4_2726228
+unsigned int check_if_affected_core(void)
+{
+	uint32_t midr_val = read_midr();
+	long rev_var  = cpu_get_rev_var();
+
+	if (EXTRACT_PARTNUM(midr_val) == EXTRACT_PARTNUM(CORTEX_A520_MIDR)) {
+		return check_erratum_cortex_a520_2938996(rev_var);
+	} else if (EXTRACT_PARTNUM(midr_val) == EXTRACT_PARTNUM(CORTEX_X4_MIDR)) {
+		return check_erratum_cortex_x4_2726228(rev_var);
+	}
+
+	return ERRATA_NOT_APPLIES;
+}
+#endif
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 370a34dc9..ce3a4da09 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -19,6 +19,8 @@
 #include <common/debug.h>
 #include <context.h>
 #include <drivers/arm/gicv3.h>
+#include <lib/cpus/cpu_ops.h>
+#include <lib/cpus/errata.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/el3_runtime/cpu_data.h>
 #include <lib/el3_runtime/pubsub_events.h>
@@ -1548,6 +1550,17 @@ void cm_handle_asymmetric_features(void)
 		spe_disable(spe_ctx);
 	}
 #endif
+#if ERRATA_A520_2938996 || ERRATA_X4_2726228
+	cpu_context_t *trbe_ctx = cm_get_context(NON_SECURE);
+
+	assert(trbe_ctx != NULL);
+
+	if (check_if_affected_core() == ERRATA_APPLIES) {
+		if (is_feat_trbe_supported()) {
+			trbe_disable(trbe_ctx);
+		}
+	}
+#endif
 }
 #endif