From fe38cc68975b23084b4ba512254926941c865a07 Mon Sep 17 00:00:00 2001
From: Manish Pandey <manish.pandey2@arm.com>
Date: Mon, 24 Apr 2023 10:46:21 +0100
Subject: [PATCH] feat(fvp): introduce PLATFORM_TEST_EA_FFH config

FVP currently does not have proper handler to do Firmware First Handling
(FFH) of lower EL External aborts and it ends up in EL3 panic.

To test the scenarios sensibly we need a proper handling when the FVP is
under test so that we do not change the default behavior.

Introduce PLATFORM_TEST_EA_FFH config which will be enabled in CI
scripts and implement a proper handling for Sync EA and SErrors from
lower EL.

Signed-off-by: Manish Pandey <manish.pandey2@arm.com>
Change-Id: Ib130154206b17f72c49c9f07de2d92f35a97ab0b
---
 plat/arm/board/fvp/aarch64/fvp_ea.c | 54 +++++++++++++++++++++++++++++
 plat/arm/board/fvp/platform.mk      |  8 +++++
 2 files changed, 62 insertions(+)
 create mode 100644 plat/arm/board/fvp/aarch64/fvp_ea.c

diff --git a/plat/arm/board/fvp/aarch64/fvp_ea.c b/plat/arm/board/fvp/aarch64/fvp_ea.c
new file mode 100644
index 000000000..07053a9cf
--- /dev/null
+++ b/plat/arm/board/fvp/aarch64/fvp_ea.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <bl31/ea_handle.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <context.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+
+/*
+ * This source file with custom plat_ea_handler function is compiled only when
+ * building TF-A with compile option PLATFORM_TEST_EA_FFH
+ */
+
+/* Test address(non-existent) used in tftf to cause External aborts */
+#define TEST_ADDRESS	UL(0x7FFFF000)
+
+void plat_ea_handler(unsigned int ea_reason, uint64_t syndrome, void *cookie,
+		void *handle, uint64_t flags)
+{
+#ifdef PLATFORM_TEST_EA_FFH
+	u_register_t elr_el3;
+	u_register_t fault_address;
+	cpu_context_t *ctx = cm_get_context(NON_SECURE);
+	el3_state_t *el3_ctx = get_el3state_ctx(ctx);
+	gp_regs_t *gpregs_ctx = get_gpregs_ctx(ctx);
+	unsigned int level = (unsigned int)GET_EL(read_spsr_el3());
+
+	fault_address = read_ctx_reg(gpregs_ctx, CTX_GPREG_X0);
+
+	if ((level < MODE_EL3) && (fault_address == TEST_ADDRESS)) {
+		if (ea_reason == ERROR_EA_SYNC) {
+			INFO("Handled sync EA from lower EL at address 0x%lx\n", fault_address);
+			/* To avoid continuous faults, forward return address */
+			elr_el3 = read_ctx_reg(el3_ctx, CTX_ELR_EL3);
+			elr_el3 += 4;
+			write_ctx_reg(el3_ctx, CTX_ELR_EL3, elr_el3);
+			return;
+		} else if (ea_reason == ERROR_EA_ASYNC) {
+			INFO("Handled Serror from lower EL at address 0x%lx\n", fault_address);
+			return;
+		}
+	}
+#endif
+	plat_default_ea_handler(ea_reason, syndrome, cookie, handle, flags);
+}
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 214064bf1..ba7e5707a 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -511,3 +511,11 @@ PLAT_BL_COMMON_SOURCES	+=	plat/arm/board/fvp/fvp_el3_spmc.c
 endif
 
 PSCI_OS_INIT_MODE	:=	1
+
+$(eval $(call add_define,PLATFORM_TEST_EA_FFH))
+ifeq (${PLATFORM_TEST_EA_FFH}, 1)
+    ifeq (${HANDLE_EA_EL3_FIRST_NS}, 0)
+         $(error "PLATFORM_TEST_EA_FFH expects HANDLE_EA_EL3_FIRST_NS to be 1")
+    endif
+BL31_SOURCES	+= plat/arm/board/fvp/aarch64/fvp_ea.c
+endif
-- 
2.39.5