feat(tsp): add FF-A support to the TSP

This patch adds the FF-A programming model in the test
secure payload to ensure that it can be used to test
the following spec features.

1. SP initialisation on the primary and secondary cpus.
2. An event loop to receive direct requests and respond
   with direct responses.
3. Ability to receive messages that indicate power on
   and off of a cpu.
4. Ability to handle a secure interrupt.

Signed-off-by: Achin Gupta <achin.gupta@arm.com>
Signed-off-by: Marc Bonnici <marc.bonnici@arm.com>
Signed-off-by: Shruti <shruti.gupta@arm.com>
Change-Id: I81cf744904d5cdc0b27862b5e4bc6f2cfe58a13a
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 7d77f47..e5ea88c 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,12 +10,16 @@
 #include <asm_macros.S>
 #include <bl32/tsp/tsp.h>
 #include <lib/xlat_tables/xlat_tables_defs.h>
+#include <smccc_helpers.h>
 
 #include "../tsp_private.h"
 
 
 	.globl	tsp_entrypoint
 	.globl  tsp_vector_table
+#if SPMC_AT_EL3
+	.globl tsp_cpu_on_entry
+#endif
 
 
 
@@ -25,10 +29,10 @@
 	 * ---------------------------------------------
 	 */
 	.macro restore_args_call_smc
-	ldp	x6, x7, [x0, #TSP_ARG6]
-	ldp	x4, x5, [x0, #TSP_ARG4]
-	ldp	x2, x3, [x0, #TSP_ARG2]
-	ldp	x0, x1, [x0, #TSP_ARG0]
+	ldp	x6, x7, [x0, #SMC_ARG6]
+	ldp	x4, x5, [x0, #SMC_ARG4]
+	ldp	x2, x3, [x0, #SMC_ARG2]
+	ldp	x0, x1, [x0, #SMC_ARG0]
 	smc	#0
 	.endm
 
diff --git a/bl32/tsp/tsp.mk b/bl32/tsp/tsp.mk
index 3fd6d99..3dfa802 100644
--- a/bl32/tsp/tsp.mk
+++ b/bl32/tsp/tsp.mk
@@ -1,17 +1,23 @@
 #
-# Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
 INCLUDES		+=	-Iinclude/bl32/tsp
 
-BL32_SOURCES		+=	bl32/tsp/tsp_main.c			\
-				bl32/tsp/aarch64/tsp_entrypoint.S	\
+ifeq (${SPMC_AT_EL3},1)
+   BL32_SOURCES            +=      bl32/tsp/tsp_ffa_main.c
+else
+   BL32_SOURCES            +=      bl32/tsp/tsp_main.c
+endif
+
+BL32_SOURCES		+=	bl32/tsp/aarch64/tsp_entrypoint.S	\
 				bl32/tsp/aarch64/tsp_exceptions.S	\
 				bl32/tsp/aarch64/tsp_request.S		\
 				bl32/tsp/tsp_interrupt.c		\
 				bl32/tsp/tsp_timer.c			\
+				bl32/tsp/tsp_common.c			\
 				common/aarch64/early_exceptions.S	\
 				lib/locks/exclusive/aarch64/spinlock.S
 
diff --git a/bl32/tsp/tsp_common.c b/bl32/tsp/tsp_common.c
new file mode 100644
index 0000000..028421e
--- /dev/null
+++ b/bl32/tsp/tsp_common.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <bl32/tsp/tsp.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
+#include <platform_tsp.h>
+#include "tsp_private.h"
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * Lock to control access to the console
+ ******************************************************************************/
+spinlock_t console_lock;
+
+/*******************************************************************************
+ * Per cpu data structure to populate parameters for an SMC in C code and use
+ * a pointer to this structure in assembler code to populate x0-x7.
+ ******************************************************************************/
+static smc_args_t tsp_smc_args[PLATFORM_CORE_COUNT];
+
+/*******************************************************************************
+ * Per cpu data structure to keep track of TSP activity
+ ******************************************************************************/
+work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
+
+smc_args_t *set_smc_args(uint64_t arg0,
+			 uint64_t arg1,
+			 uint64_t arg2,
+			 uint64_t arg3,
+			 uint64_t arg4,
+			 uint64_t arg5,
+			 uint64_t arg6,
+			 uint64_t arg7)
+{
+	uint32_t linear_id;
+	smc_args_t *pcpu_smc_args;
+
+	/*
+	 * Return to Secure Monitor by raising an SMC. The results of the
+	 * service are passed as an arguments to the SMC.
+	 */
+	linear_id = plat_my_core_pos();
+	pcpu_smc_args = &tsp_smc_args[linear_id];
+	write_sp_arg(pcpu_smc_args, SMC_ARG0, arg0);
+	write_sp_arg(pcpu_smc_args, SMC_ARG1, arg1);
+	write_sp_arg(pcpu_smc_args, SMC_ARG2, arg2);
+	write_sp_arg(pcpu_smc_args, SMC_ARG3, arg3);
+	write_sp_arg(pcpu_smc_args, SMC_ARG4, arg4);
+	write_sp_arg(pcpu_smc_args, SMC_ARG5, arg5);
+	write_sp_arg(pcpu_smc_args, SMC_ARG6, arg6);
+	write_sp_arg(pcpu_smc_args, SMC_ARG7, arg7);
+
+	return pcpu_smc_args;
+}
+
+/*******************************************************************************
+ * Setup function for TSP.
+ ******************************************************************************/
+void tsp_setup(void)
+{
+	/* Perform early platform-specific setup. */
+	tsp_early_platform_setup();
+
+	/* Perform late platform-specific setup. */
+	tsp_plat_arch_setup();
+
+#if ENABLE_PAUTH
+	/*
+	 * Assert that the ARMv8.3-PAuth registers are present or an access
+	 * fault will be triggered when they are being saved or restored.
+	 */
+	assert(is_armv8_3_pauth_present());
+#endif /* ENABLE_PAUTH */
+}
+
+/*******************************************************************************
+ * This function performs any remaining bookkeeping in the test secure payload
+ * before the system is switched off (in response to a psci SYSTEM_OFF request).
+ ******************************************************************************/
+smc_args_t *tsp_system_off_main(uint64_t arg0,
+				uint64_t arg1,
+				uint64_t arg2,
+				uint64_t arg3,
+				uint64_t arg4,
+				uint64_t arg5,
+				uint64_t arg6,
+				uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%lx SYSTEM_OFF request\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
+	     tsp_stats[linear_id].smc_count,
+	     tsp_stats[linear_id].eret_count);
+	spin_unlock(&console_lock);
+#endif
+
+	/* Indicate to the SPD that we have completed this request. */
+	return set_smc_args(TSP_SYSTEM_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * This function performs any remaining bookkeeping in the test secure payload
+ * before the system is reset (in response to a psci SYSTEM_RESET request).
+ ******************************************************************************/
+smc_args_t *tsp_system_reset_main(uint64_t arg0,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%lx SYSTEM_RESET request\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
+	     tsp_stats[linear_id].smc_count,
+	     tsp_stats[linear_id].eret_count);
+	spin_unlock(&console_lock);
+#endif
+
+	/* Indicate to the SPD that we have completed this request. */
+	return set_smc_args(TSP_SYSTEM_RESET_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * TSP smc abort handler. This function is called when aborting a preempted
+ * yielding SMC request. It should cleanup all resources owned by the SMC
+ * handler such as locks or dynamically allocated memory so following SMC
+ * request are executed in a clean environment.
+ ******************************************************************************/
+smc_args_t *tsp_abort_smc_handler(uint64_t func,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7)
+{
+	return set_smc_args(TSP_ABORT_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
diff --git a/bl32/tsp/tsp_ffa_main.c b/bl32/tsp/tsp_ffa_main.c
new file mode 100644
index 0000000..edd6c24
--- /dev/null
+++ b/bl32/tsp/tsp_ffa_main.c
@@ -0,0 +1,369 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+#include <stdint.h>
+
+#include "../../services/std_svc/spm/el3_spmc/spmc.h"
+#include "../../services/std_svc/spm/el3_spmc/spmc_shared_mem.h"
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <bl32/tsp/tsp.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
+#include <platform_tsp.h>
+#include <services/ffa_svc.h>
+#include "tsp_private.h"
+
+#include <platform_def.h>
+
+extern void tsp_cpu_on_entry(void);
+
+static ffa_endpoint_id16_t tsp_id, spmc_id;
+
+static smc_args_t *send_ffa_pm_success(void)
+{
+	return set_smc_args(FFA_MSG_SEND_DIRECT_RESP_SMC32,
+			    tsp_id << FFA_DIRECT_MSG_SOURCE_SHIFT |
+			    spmc_id,
+			    FFA_FWK_MSG_BIT |
+			    (FFA_PM_MSG_PM_RESP & FFA_FWK_MSG_MASK),
+			    0, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * This function performs any remaining book keeping in the test secure payload
+ * before this cpu is turned off in response to a psci cpu_off request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_off_main(uint64_t arg0,
+			     uint64_t arg1,
+			     uint64_t arg2,
+			     uint64_t arg3,
+			     uint64_t arg4,
+			     uint64_t arg5,
+			     uint64_t arg6,
+			     uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/*
+	 * This cpu is being turned off, so disable the timer to prevent the
+	 * secure timer interrupt from interfering with power down. A pending
+	 * interrupt will be lost but we do not care as we are turning off.
+	 */
+	tsp_generic_timer_stop();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_off_count++;
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%lx off request\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu off requests\n",
+		read_mpidr(),
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_off_count);
+	spin_unlock(&console_lock);
+#endif
+
+	return send_ffa_pm_success();
+}
+
+/*******************************************************************************
+ * This function performs any book keeping in the test secure payload before
+ * this cpu's architectural state is saved in response to an earlier psci
+ * cpu_suspend request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_suspend_main(uint64_t arg0,
+				 uint64_t arg1,
+				 uint64_t arg2,
+				 uint64_t arg3,
+				 uint64_t arg4,
+				 uint64_t arg5,
+				 uint64_t arg6,
+				 uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/*
+	 * Save the time context and disable it to prevent the secure timer
+	 * interrupt from interfering with wakeup from the suspend state.
+	 */
+	tsp_generic_timer_save();
+	tsp_generic_timer_stop();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_suspend_count++;
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n",
+		read_mpidr(),
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_suspend_count);
+	spin_unlock(&console_lock);
+#endif
+
+	return send_ffa_pm_success();
+}
+
+/*******************************************************************************
+ * This function performs any bookkeeping in the test secure payload after this
+ * cpu's architectural state has been restored after wakeup from an earlier psci
+ * cpu_suspend request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
+				uint64_t arg1,
+				uint64_t arg2,
+				uint64_t arg3,
+				uint64_t arg4,
+				uint64_t arg5,
+				uint64_t arg6,
+				uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Restore the generic timer context. */
+	tsp_generic_timer_restore();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_resume_count++;
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%lx resumed. maximum off power level %" PRId64 "\n",
+	     read_mpidr(), max_off_pwrlvl);
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n",
+		read_mpidr(),
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_resume_count);
+	spin_unlock(&console_lock);
+#endif
+
+	return send_ffa_pm_success();
+}
+
+/*******************************************************************************
+ * This function handles framework messages. Currently only PM.
+ ******************************************************************************/
+static smc_args_t *handle_framework_message(uint64_t arg0,
+					    uint64_t arg1,
+					    uint64_t arg2,
+					    uint64_t arg3,
+					    uint64_t arg4,
+					    uint64_t arg5,
+					    uint64_t arg6,
+					    uint64_t arg7)
+{
+	/* Check if it is a power management message from the SPMC. */
+	if (ffa_endpoint_source(arg1) != spmc_id) {
+		goto err;
+	}
+
+	/* Check if it is a PM request message. */
+	if ((arg2 & FFA_FWK_MSG_MASK) == FFA_FWK_MSG_PSCI) {
+		/* Check if it is a PSCI CPU_OFF request. */
+		if (arg3 == PSCI_CPU_OFF) {
+			return tsp_cpu_off_main(arg0, arg1, arg2, arg3,
+						arg4, arg5, arg6, arg7);
+		} else if (arg3 == PSCI_CPU_SUSPEND_AARCH64) {
+			return tsp_cpu_suspend_main(arg0, arg1, arg2, arg3,
+						arg4, arg5, arg6, arg7);
+		}
+	} else if ((arg2 & FFA_FWK_MSG_MASK) == FFA_PM_MSG_WB_REQ) {
+		/* Check it is a PSCI Warm Boot request. */
+		if (arg3 == FFA_WB_TYPE_NOTS2RAM) {
+			return tsp_cpu_resume_main(arg0, arg1, arg2, arg3,
+						arg4, arg5, arg6, arg7);
+		}
+	}
+
+err:
+	ERROR("%s: Unknown framework message!\n", __func__);
+	panic();
+}
+
+/*******************************************************************************
+ * This function implements the event loop for handling FF-A ABI invocations.
+ ******************************************************************************/
+static smc_args_t *tsp_event_loop(uint64_t smc_fid,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7)
+{
+	/* Panic if the SPMC did not forward an FF-A call. */
+	if (!is_ffa_fid(smc_fid)) {
+		ERROR("%s: Unknown SMC FID (0x%lx)\n", __func__, smc_fid);
+		panic();
+	}
+
+	switch (smc_fid) {
+	case FFA_INTERRUPT:
+		/*
+		 * IRQs were enabled upon re-entry into the TSP. The interrupt
+		 * must have been handled by now. Return to the SPMC indicating
+		 * the same.
+		 */
+		return set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0);
+
+	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
+		/* Check if a framework message, handle accordingly. */
+		if ((arg2 & FFA_FWK_MSG_BIT)) {
+			return handle_framework_message(smc_fid, arg1, arg2, arg3,
+							arg4, arg5, arg6, arg7);
+		}
+	default:
+		break;
+	}
+
+	ERROR("%s: Unsupported FF-A FID (0x%lx)\n", __func__, smc_fid);
+	panic();
+}
+
+static smc_args_t *tsp_loop(smc_args_t *args)
+{
+	smc_args_t ret;
+
+	do {
+		/* --------------------------------------------
+		 * Mask FIQ interrupts to avoid preemption
+		 * in case EL3 SPMC delegates an IRQ next or a
+		 * managed exit. Lastly, unmask IRQs so that
+		 * they can be handled immediately upon re-entry
+		 *  ---------------------------------------------
+		 */
+		write_daifset(DAIF_FIQ_BIT);
+		write_daifclr(DAIF_IRQ_BIT);
+		ret = smc_helper(args->_regs[0], args->_regs[1], args->_regs[2],
+			       args->_regs[3], args->_regs[4], args->_regs[5],
+			       args->_regs[6], args->_regs[7]);
+		args = tsp_event_loop(ret._regs[0], ret._regs[1], ret._regs[2],
+				      ret._regs[3], ret._regs[4], ret._regs[5],
+				      ret._regs[6], ret._regs[7]);
+	} while (1);
+
+	/* Not Reached. */
+	return NULL;
+}
+
+/*******************************************************************************
+ * TSP main entry point where it gets the opportunity to initialize its secure
+ * state/applications. Once the state is initialized, it must return to the
+ * SPD with a pointer to the 'tsp_vector_table' jump table.
+ ******************************************************************************/
+uint64_t tsp_main(void)
+{
+	smc_args_t smc_args = {0};
+
+	NOTICE("TSP: %s\n", version_string);
+	NOTICE("TSP: %s\n", build_message);
+	INFO("TSP: Total memory base : 0x%lx\n", (unsigned long) BL32_BASE);
+	INFO("TSP: Total memory size : 0x%lx bytes\n", BL32_TOTAL_SIZE);
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Initialize the platform. */
+	tsp_platform_setup();
+
+	/* Initialize secure/applications state here. */
+	tsp_generic_timer_start();
+
+	/* Register secondary entrypoint with the SPMC. */
+	smc_args = smc_helper(FFA_SECONDARY_EP_REGISTER_SMC64,
+			(uint64_t) tsp_cpu_on_entry,
+			0, 0, 0, 0, 0, 0);
+	if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) {
+		ERROR("TSP could not register secondary ep (0x%lx)\n",
+				smc_args._regs[2]);
+		panic();
+	}
+	/* Get TSP's endpoint id */
+	smc_args = smc_helper(FFA_ID_GET, 0, 0, 0, 0, 0, 0, 0);
+	if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) {
+		ERROR("TSP could not get own ID (0x%lx) on core%d\n",
+				smc_args._regs[2], linear_id);
+		panic();
+	}
+
+	tsp_id = smc_args._regs[2];
+	INFO("TSP FF-A endpoint id = 0x%x\n", tsp_id);
+	/* Get the SPMC ID */
+	smc_args = smc_helper(FFA_SPM_ID_GET, 0, 0, 0, 0, 0, 0, 0);
+	if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) {
+		ERROR("TSP could not get SPMC ID (0x%lx) on core%d\n",
+				smc_args._regs[2], linear_id);
+		panic();
+	}
+
+	spmc_id = smc_args._regs[2];
+
+	/* Update this cpu's statistics */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_on_count++;
+
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
+			read_mpidr(),
+			tsp_stats[linear_id].smc_count,
+			tsp_stats[linear_id].eret_count,
+			tsp_stats[linear_id].cpu_on_count);
+
+	/* Tell SPMD that we are done initialising. */
+	tsp_loop(set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0));
+
+	/* Not reached. */
+	return 0;
+}
+
+/*******************************************************************************
+ * This function performs any remaining book keeping in the test secure payload
+ * after this cpu's architectural state has been setup in response to an earlier
+ * psci cpu_on request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_on_main(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Initialize secure/applications state here. */
+	tsp_generic_timer_start();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_on_count++;
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%lx turned on\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
+			read_mpidr(),
+			tsp_stats[linear_id].smc_count,
+			tsp_stats[linear_id].eret_count,
+			tsp_stats[linear_id].cpu_on_count);
+	spin_unlock(&console_lock);
+#endif
+	/* ---------------------------------------------
+	 * Jump to the main event loop to return to EL3
+	 * and be ready for the next request on this cpu.
+	 * ---------------------------------------------
+	 */
+	return tsp_loop(set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0));
+}
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
index 430b5dd..6644c50 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -91,8 +91,18 @@
 	id = plat_ic_get_pending_interrupt_id();
 
 	/* TSP can only handle the secure physical timer interrupt */
-	if (id != TSP_IRQ_SEC_PHY_TIMER)
+	if (id != TSP_IRQ_SEC_PHY_TIMER) {
+#if SPMC_AT_EL3
+		/*
+		 * With the EL3 FF-A SPMC we expect only Timer secure interrupt to fire in
+		 * the TSP, so panic if any other interrupt does.
+		 */
+		ERROR("Unexpected interrupt id %u\n", id);
+		panic();
+#else
 		return tsp_handle_preemption();
+#endif
+	}
 
 	/*
 	 * Acknowledge and handle the secure timer interrupt. Also sanity check
@@ -105,13 +115,9 @@
 
 	/* Update the statistics and print some messages */
 	tsp_stats[linear_id].sel1_intr_count++;
-#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
-	spin_lock(&console_lock);
 	VERBOSE("TSP: cpu 0x%lx handled S-EL1 interrupt %d\n",
 	       read_mpidr(), id);
 	VERBOSE("TSP: cpu 0x%lx: %d S-EL1 requests\n",
 	     read_mpidr(), tsp_stats[linear_id].sel1_intr_count);
-	spin_unlock(&console_lock);
-#endif
 	return 0;
 }
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index 522c1b4..051080e 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -15,85 +15,10 @@
 #include <common/debug.h>
 #include <lib/spinlock.h>
 #include <plat/common/platform.h>
-#include <platform_def.h>
 #include <platform_tsp.h>
-
 #include "tsp_private.h"
 
-
-/*******************************************************************************
- * Lock to control access to the console
- ******************************************************************************/
-spinlock_t console_lock;
-
-/*******************************************************************************
- * Per cpu data structure to populate parameters for an SMC in C code and use
- * a pointer to this structure in assembler code to populate x0-x7
- ******************************************************************************/
-static tsp_args_t tsp_smc_args[PLATFORM_CORE_COUNT];
-
-/*******************************************************************************
- * Per cpu data structure to keep track of TSP activity
- ******************************************************************************/
-work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
-
-/*******************************************************************************
- * The TSP memory footprint starts at address BL32_BASE and ends with the
- * linker symbol __BL32_END__. Use these addresses to compute the TSP image
- * size.
- ******************************************************************************/
-#define BL32_TOTAL_LIMIT BL32_END
-#define BL32_TOTAL_SIZE (BL32_TOTAL_LIMIT - (unsigned long) BL32_BASE)
-
-static tsp_args_t *set_smc_args(uint64_t arg0,
-			     uint64_t arg1,
-			     uint64_t arg2,
-			     uint64_t arg3,
-			     uint64_t arg4,
-			     uint64_t arg5,
-			     uint64_t arg6,
-			     uint64_t arg7)
-{
-	uint32_t linear_id;
-	tsp_args_t *pcpu_smc_args;
-
-	/*
-	 * Return to Secure Monitor by raising an SMC. The results of the
-	 * service are passed as an arguments to the SMC
-	 */
-	linear_id = plat_my_core_pos();
-	pcpu_smc_args = &tsp_smc_args[linear_id];
-	write_sp_arg(pcpu_smc_args, TSP_ARG0, arg0);
-	write_sp_arg(pcpu_smc_args, TSP_ARG1, arg1);
-	write_sp_arg(pcpu_smc_args, TSP_ARG2, arg2);
-	write_sp_arg(pcpu_smc_args, TSP_ARG3, arg3);
-	write_sp_arg(pcpu_smc_args, TSP_ARG4, arg4);
-	write_sp_arg(pcpu_smc_args, TSP_ARG5, arg5);
-	write_sp_arg(pcpu_smc_args, TSP_ARG6, arg6);
-	write_sp_arg(pcpu_smc_args, TSP_ARG7, arg7);
-
-	return pcpu_smc_args;
-}
-
-/*******************************************************************************
- * Setup function for TSP.
- ******************************************************************************/
-void tsp_setup(void)
-{
-	/* Perform early platform-specific setup */
-	tsp_early_platform_setup();
-
-	/* Perform late platform-specific setup */
-	tsp_plat_arch_setup();
-
-#if ENABLE_PAUTH
-	/*
-	 * Assert that the ARMv8.3-PAuth registers are present or an access
-	 * fault will be triggered when they are being saved or restored.
-	 */
-	assert(is_armv8_3_pauth_present());
-#endif /* ENABLE_PAUTH */
-}
+#include <platform_def.h>
 
 /*******************************************************************************
  * TSP main entry point where it gets the opportunity to initialize its secure
@@ -137,7 +62,7 @@
  * after this cpu's architectural state has been setup in response to an earlier
  * psci cpu_on request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_on_main(void)
+smc_args_t *tsp_cpu_on_main(void)
 {
 	uint32_t linear_id = plat_my_core_pos();
 
@@ -167,7 +92,7 @@
  * This function performs any remaining book keeping in the test secure payload
  * before this cpu is turned off in response to a psci cpu_off request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_off_main(uint64_t arg0,
+smc_args_t *tsp_cpu_off_main(uint64_t arg0,
 			   uint64_t arg1,
 			   uint64_t arg2,
 			   uint64_t arg3,
@@ -210,7 +135,7 @@
  * this cpu's architectural state is saved in response to an earlier psci
  * cpu_suspend request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_suspend_main(uint64_t arg0,
+smc_args_t *tsp_cpu_suspend_main(uint64_t arg0,
 			       uint64_t arg1,
 			       uint64_t arg2,
 			       uint64_t arg3,
@@ -252,7 +177,7 @@
  * cpu's architectural state has been restored after wakeup from an earlier psci
  * cpu_suspend request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
+smc_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
 			      uint64_t arg1,
 			      uint64_t arg2,
 			      uint64_t arg3,
@@ -287,76 +212,12 @@
 }
 
 /*******************************************************************************
- * This function performs any remaining bookkeeping in the test secure payload
- * before the system is switched off (in response to a psci SYSTEM_OFF request)
- ******************************************************************************/
-tsp_args_t *tsp_system_off_main(uint64_t arg0,
-				uint64_t arg1,
-				uint64_t arg2,
-				uint64_t arg3,
-				uint64_t arg4,
-				uint64_t arg5,
-				uint64_t arg6,
-				uint64_t arg7)
-{
-	uint32_t linear_id = plat_my_core_pos();
-
-	/* Update this cpu's statistics */
-	tsp_stats[linear_id].smc_count++;
-	tsp_stats[linear_id].eret_count++;
-
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
-	INFO("TSP: cpu 0x%lx SYSTEM_OFF request\n", read_mpidr());
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count);
-	spin_unlock(&console_lock);
-#endif
-
-	/* Indicate to the SPD that we have completed this request */
-	return set_smc_args(TSP_SYSTEM_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
-}
-
-/*******************************************************************************
- * This function performs any remaining bookkeeping in the test secure payload
- * before the system is reset (in response to a psci SYSTEM_RESET request)
- ******************************************************************************/
-tsp_args_t *tsp_system_reset_main(uint64_t arg0,
-				uint64_t arg1,
-				uint64_t arg2,
-				uint64_t arg3,
-				uint64_t arg4,
-				uint64_t arg5,
-				uint64_t arg6,
-				uint64_t arg7)
-{
-	uint32_t linear_id = plat_my_core_pos();
-
-	/* Update this cpu's statistics */
-	tsp_stats[linear_id].smc_count++;
-	tsp_stats[linear_id].eret_count++;
-
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
-	INFO("TSP: cpu 0x%lx SYSTEM_RESET request\n", read_mpidr());
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count);
-	spin_unlock(&console_lock);
-#endif
-
-	/* Indicate to the SPD that we have completed this request */
-	return set_smc_args(TSP_SYSTEM_RESET_DONE, 0, 0, 0, 0, 0, 0, 0);
-}
-
-/*******************************************************************************
  * TSP fast smc handler. The secure monitor jumps to this function by
  * doing the ERET after populating X0-X7 registers. The arguments are received
  * in the function arguments in order. Once the service is rendered, this
  * function returns to Secure Monitor by raising SMC.
  ******************************************************************************/
-tsp_args_t *tsp_smc_handler(uint64_t func,
+smc_args_t *tsp_smc_handler(uint64_t func,
 			       uint64_t arg1,
 			       uint64_t arg2,
 			       uint64_t arg3,
@@ -451,21 +312,3 @@
 			    results[1],
 			    0, 0, 0, 0);
 }
-
-/*******************************************************************************
- * TSP smc abort handler. This function is called when aborting a preempted
- * yielding SMC request. It should cleanup all resources owned by the SMC
- * handler such as locks or dynamically allocated memory so following SMC
- * request are executed in a clean environment.
- ******************************************************************************/
-tsp_args_t *tsp_abort_smc_handler(uint64_t func,
-				  uint64_t arg1,
-				  uint64_t arg2,
-				  uint64_t arg3,
-				  uint64_t arg4,
-				  uint64_t arg5,
-				  uint64_t arg6,
-				  uint64_t arg7)
-{
-	return set_smc_args(TSP_ABORT_DONE, 0, 0, 0, 0, 0, 0, 0);
-}
diff --git a/bl32/tsp/tsp_private.h b/bl32/tsp/tsp_private.h
index 38d9732..30df1f0 100644
--- a/bl32/tsp/tsp_private.h
+++ b/bl32/tsp/tsp_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,28 +7,22 @@
 #ifndef TSP_PRIVATE_H
 #define TSP_PRIVATE_H
 
-/* Definitions to help the assembler access the SMC/ERET args structure */
-#define TSP_ARGS_SIZE		0x40
-#define TSP_ARG0		0x0
-#define TSP_ARG1		0x8
-#define TSP_ARG2		0x10
-#define TSP_ARG3		0x18
-#define TSP_ARG4		0x20
-#define TSP_ARG5		0x28
-#define TSP_ARG6		0x30
-#define TSP_ARG7		0x38
-#define TSP_ARGS_END		0x40
-
+/*******************************************************************************
+ * The TSP memory footprint starts at address BL32_BASE and ends with the
+ * linker symbol __BL32_END__. Use these addresses to compute the TSP image
+ * size.
+ ******************************************************************************/
+#define BL32_TOTAL_LIMIT BL32_END
+#define BL32_TOTAL_SIZE (BL32_TOTAL_LIMIT - (unsigned long) BL32_BASE)
 
 #ifndef __ASSEMBLER__
 
 #include <stdint.h>
 
-#include <platform_def.h> /* For CACHE_WRITEBACK_GRANULE */
-
 #include <bl32/tsp/tsp.h>
 #include <lib/cassert.h>
 #include <lib/spinlock.h>
+#include <smccc_helpers.h>
 
 typedef struct work_statistics {
 	/* Number of s-el1 interrupts on this cpu */
@@ -47,23 +41,22 @@
 	uint32_t cpu_resume_count;	/* Number of cpu resume requests */
 } __aligned(CACHE_WRITEBACK_GRANULE) work_statistics_t;
 
-typedef struct tsp_args {
-	uint64_t _regs[TSP_ARGS_END >> 3];
-} __aligned(CACHE_WRITEBACK_GRANULE) tsp_args_t;
-
 /* Macros to access members of the above structure using their offsets */
 #define read_sp_arg(args, offset)	((args)->_regs[offset >> 3])
 #define write_sp_arg(args, offset, val) (((args)->_regs[offset >> 3])	\
 					 = val)
-/*
- * Ensure that the assembler's view of the size of the tsp_args is the
- * same as the compilers
- */
-CASSERT(TSP_ARGS_SIZE == sizeof(tsp_args_t), assert_sp_args_size_mismatch);
 
 uint128_t tsp_get_magic(void);
 
-tsp_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
+smc_args_t *set_smc_args(uint64_t arg0,
+			 uint64_t arg1,
+			 uint64_t arg2,
+			 uint64_t arg3,
+			 uint64_t arg4,
+			 uint64_t arg5,
+			 uint64_t arg6,
+			 uint64_t arg7);
+smc_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
 				uint64_t arg1,
 				uint64_t arg2,
 				uint64_t arg3,
@@ -71,7 +64,7 @@
 				uint64_t arg5,
 				uint64_t arg6,
 				uint64_t arg7);
-tsp_args_t *tsp_cpu_suspend_main(uint64_t arg0,
+smc_args_t *tsp_cpu_suspend_main(uint64_t arg0,
 				 uint64_t arg1,
 				 uint64_t arg2,
 				 uint64_t arg3,
@@ -79,8 +72,8 @@
 				 uint64_t arg5,
 				 uint64_t arg6,
 				 uint64_t arg7);
-tsp_args_t *tsp_cpu_on_main(void);
-tsp_args_t *tsp_cpu_off_main(uint64_t arg0,
+smc_args_t *tsp_cpu_on_main(void);
+smc_args_t *tsp_cpu_off_main(uint64_t arg0,
 			     uint64_t arg1,
 			     uint64_t arg2,
 			     uint64_t arg3,
@@ -111,7 +104,7 @@
 int32_t tsp_common_int_handler(void);
 int32_t tsp_handle_preemption(void);
 
-tsp_args_t *tsp_abort_smc_handler(uint64_t func,
+smc_args_t *tsp_abort_smc_handler(uint64_t func,
 				  uint64_t arg1,
 				  uint64_t arg2,
 				  uint64_t arg3,
@@ -120,25 +113,25 @@
 				  uint64_t arg6,
 				  uint64_t arg7);
 
-tsp_args_t *tsp_smc_handler(uint64_t func,
-			       uint64_t arg1,
-			       uint64_t arg2,
-			       uint64_t arg3,
-			       uint64_t arg4,
-			       uint64_t arg5,
-			       uint64_t arg6,
-			       uint64_t arg7);
+smc_args_t *tsp_smc_handler(uint64_t func,
+			    uint64_t arg1,
+			    uint64_t arg2,
+			    uint64_t arg3,
+			    uint64_t arg4,
+			    uint64_t arg5,
+			    uint64_t arg6,
+			    uint64_t arg7);
 
-tsp_args_t *tsp_system_reset_main(uint64_t arg0,
-				uint64_t arg1,
-				uint64_t arg2,
-				uint64_t arg3,
-				uint64_t arg4,
-				uint64_t arg5,
-				uint64_t arg6,
-				uint64_t arg7);
+smc_args_t *tsp_system_reset_main(uint64_t arg0,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7);
 
-tsp_args_t *tsp_system_off_main(uint64_t arg0,
+smc_args_t *tsp_system_off_main(uint64_t arg0,
 				uint64_t arg1,
 				uint64_t arg2,
 				uint64_t arg3,
diff --git a/include/arch/aarch64/smccc_helpers.h b/include/arch/aarch64/smccc_helpers.h
index fac6fd9..920f294 100644
--- a/include/arch/aarch64/smccc_helpers.h
+++ b/include/arch/aarch64/smccc_helpers.h
@@ -9,12 +9,26 @@
 
 #include <lib/smccc.h>
 
+/* Definitions to help the assembler access the SMC/ERET args structure */
+#define SMC_ARGS_SIZE		0x40
+#define SMC_ARG0		0x0
+#define SMC_ARG1		0x8
+#define SMC_ARG2		0x10
+#define SMC_ARG3		0x18
+#define SMC_ARG4		0x20
+#define SMC_ARG5		0x28
+#define SMC_ARG6		0x30
+#define SMC_ARG7		0x38
+#define SMC_ARGS_END		0x40
+
 #ifndef __ASSEMBLER__
 
 #include <stdbool.h>
 
 #include <context.h>
 
+#include <platform_def.h> /* For CACHE_WRITEBACK_GRANULE */
+
 /* Convenience macros to return from SMC handler */
 #define SMC_RET0(_h)	{					\
 	return (uint64_t) (_h);					\
@@ -82,6 +96,49 @@
 		_x4 = read_ctx_reg(regs, CTX_GPREG_X4);		\
 	} while (false)
 
+typedef struct {
+	uint64_t _regs[SMC_ARGS_END >> 3];
+} __aligned(CACHE_WRITEBACK_GRANULE) smc_args_t;
+
+/*
+ * Ensure that the assembler's view of the size of the tsp_args is the
+ * same as the compilers.
+ */
+CASSERT(sizeof(smc_args_t) == SMC_ARGS_SIZE, assert_sp_args_size_mismatch);
+
+static inline smc_args_t smc_helper(uint32_t func, uint64_t arg0,
+	       uint64_t arg1, uint64_t arg2,
+	       uint64_t arg3, uint64_t arg4,
+	       uint64_t arg5, uint64_t arg6)
+{
+	smc_args_t ret_args = {0};
+
+	register uint64_t r0 __asm__("x0") = func;
+	register uint64_t r1 __asm__("x1") = arg0;
+	register uint64_t r2 __asm__("x2") = arg1;
+	register uint64_t r3 __asm__("x3") = arg2;
+	register uint64_t r4 __asm__("x4") = arg3;
+	register uint64_t r5 __asm__("x5") = arg4;
+	register uint64_t r6 __asm__("x6") = arg5;
+	register uint64_t r7 __asm__("x7") = arg6;
+
+	/* Output registers, also used as inputs ('+' constraint). */
+	__asm__ volatile("smc #0"
+			: "+r"(r0), "+r"(r1), "+r"(r2), "+r"(r3), "+r"(r4),
+			  "+r"(r5), "+r"(r6), "+r"(r7));
+
+	ret_args._regs[0] = r0;
+	ret_args._regs[1] = r1;
+	ret_args._regs[2] = r2;
+	ret_args._regs[3] = r3;
+	ret_args._regs[4] = r4;
+	ret_args._regs[5] = r5;
+	ret_args._regs[6] = r6;
+	ret_args._regs[7] = r7;
+
+	return ret_args;
+}
+
 #endif /*__ASSEMBLER__*/
 
 #endif /* SMCCC_HELPERS_H */
diff --git a/services/std_svc/spm/el3_spmc/spmc.mk b/services/std_svc/spm/el3_spmc/spmc.mk
index aa591d9..c674e71 100644
--- a/services/std_svc/spm/el3_spmc/spmc.mk
+++ b/services/std_svc/spm/el3_spmc/spmc.mk
@@ -24,3 +24,21 @@
 
 # Let the top-level Makefile know that we intend to include a BL32 image
 NEED_BL32		:=	yes
+
+ifndef BL32
+# The SPMC is paired with a Test Secure Payload source and we intend to
+# build the Test Secure Payload if no other image has been provided
+# for BL32.
+#
+# In cases where an associated Secure Payload lies outside this build
+# system/source tree, the dispatcher Makefile can either invoke an external
+# build command or assume it is pre-built.
+
+BL32_ROOT		:=	bl32/tsp
+
+# Conditionally include SP's Makefile. The assumption is that the TSP's build
+# system is compatible with that of Trusted Firmware, and it'll add and populate
+# necessary build targets and variables.
+
+include ${BL32_ROOT}/tsp.mk
+endif