Cactus: Refactor handling of commands

Added helper macros to define a command handler, build a command table
in which each element is a pair of the handler and respective command
ID.
Message loop has been refactored to traverse the command table looking
for the ID, and to call the respective command handler.
Available tests have been moved to their own command handler.

Signed-off-by: J-Alves <joao.alves@arm.com>
Change-Id: I38a50a8d47b083c81c21950d98341de660891c61

3 files changed, 247 insertions, 0 deletions

diff --git a/spm/cactus/cactus_tests/cactus_test_cpu_features.c b/spm/cactus/cactus_tests/cactus_test_cpu_features.c
new file mode 100644
index 0000000..d39bdc4
--- /dev/null
+++ b/spm/cactus/cactus_tests/cactus_test_cpu_features.c
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "cactus_test_cmds.h"
+#include "spm_common.h"
+
+/*
+ * Fill SIMD vectors from secure world side with a unique value.
+ * 0x22 is just a dummy value to be distinguished from the value
+ * in the normal world.
+ */
+CACTUS_CMD_HANDLER(req_simd_fill, CACTUS_REQ_SIMD_FILL_CMD)
+{
+	simd_vector_t simd_vectors[SIMD_NUM_VECTORS];
+
+	for (unsigned int num = 0U; num < SIMD_NUM_VECTORS; num++) {
+		memset(simd_vectors[num], 0x22 * num, sizeof(simd_vector_t));
+	}
+
+	fill_simd_vector_regs(simd_vectors);
+
+	return cactus_response(ffa_dir_msg_dest(*args),
+			       ffa_dir_msg_source(*args),
+			       CACTUS_SUCCESS);
+}
diff --git a/spm/cactus/cactus_tests/cactus_test_direct_messaging.c b/spm/cactus/cactus_tests/cactus_test_direct_messaging.c
new file mode 100644
index 0000000..f4c7890
--- /dev/null
+++ b/spm/cactus/cactus_tests/cactus_test_direct_messaging.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "cactus_test_cmds.h"
+#include <debug.h>
+#include <ffa_helpers.h>
+
+CACTUS_CMD_HANDLER(echo_cmd, CACTUS_ECHO_CMD)
+{
+	uint64_t echo_val = cactus_echo_get_val(*args);
+
+	VERBOSE("Received echo at %x, value %llx.\n", ffa_dir_msg_dest(*args),
+						      echo_val);
+
+	return cactus_success_resp(ffa_dir_msg_dest(*args),
+				   ffa_dir_msg_source(*args),
+				   echo_val);
+}
+
+CACTUS_CMD_HANDLER(req_echo_cmd, CACTUS_REQ_ECHO_CMD)
+{
+	smc_ret_values ffa_ret;
+	ffa_vm_id_t vm_id = ffa_dir_msg_dest(*args);
+	ffa_vm_id_t echo_dest = cactus_req_echo_get_echo_dest(*args);
+	uint64_t echo_val = cactus_echo_get_val(*args);
+
+	VERBOSE("%x requested to send echo to %x, value %llx\n",
+		ffa_dir_msg_source(*args), echo_dest, echo_val);
+
+	ffa_ret = cactus_echo_send_cmd(vm_id, echo_dest, echo_val);
+
+	if (!is_ffa_direct_response(ffa_ret)) {
+		ERROR("Failed to send message. error: %x\n",
+			ffa_error_code(ffa_ret));
+		return cactus_error_resp(vm_id, ffa_dir_msg_source(*args),
+					 CACTUS_ERROR_FFA_CALL);
+	}
+
+	if (cactus_get_response(ffa_ret) != CACTUS_SUCCESS ||
+	    cactus_echo_get_val(ffa_ret) != echo_val) {
+		ERROR("Echo Failed!\n");
+		return cactus_error_resp(vm_id, ffa_dir_msg_source(*args),
+					 CACTUS_ERROR_TEST);
+	}
+
+	return cactus_success_resp(vm_id, ffa_dir_msg_source(*args), 0);
+}
+
+static smc_ret_values base_deadlock_handler(ffa_vm_id_t vm_id,
+					    ffa_vm_id_t source,
+					    ffa_vm_id_t deadlock_dest,
+					    ffa_vm_id_t deadlock_next_dest)
+{
+	smc_ret_values ffa_ret;
+
+	ffa_ret = cactus_deadlock_send_cmd(vm_id, deadlock_dest,
+					   deadlock_next_dest);
+
+	/*
+	 * Should be true for the last partition to attempt
+	 * an FF-A direct message, to the first partition.
+	 */
+	bool is_deadlock_detected = (ffa_func_id(ffa_ret) == FFA_ERROR) &&
+				    (ffa_error_code(ffa_ret) == FFA_ERROR_BUSY);
+
+	/*
+	 * Should be true after the deadlock has been detected and after the
+	 * first response has been sent down the request chain.
+	 */
+	bool is_returning_from_deadlock =
+		(is_ffa_direct_response(ffa_ret)) &&
+		(cactus_get_response(ffa_ret) == CACTUS_SUCCESS);
+
+	if (is_deadlock_detected) {
+		VERBOSE("Attempt to create deadlock failed\n");
+	}
+
+	if (is_deadlock_detected || is_returning_from_deadlock) {
+		/*
+		 * This is not the partition, that would have created the
+		 * deadlock. As such, reply back to the partitions.
+		 */
+		return cactus_success_resp(vm_id, source, 0);
+	}
+
+	/* Shouldn't get to this point */
+	ERROR("Deadlock test went wrong!\n");
+	return cactus_error_resp(vm_id, source, CACTUS_ERROR_TEST);
+}
+
+CACTUS_CMD_HANDLER(deadlock_cmd, CACTUS_DEADLOCK_CMD)
+{
+	ffa_vm_id_t source = ffa_dir_msg_source(*args);
+	ffa_vm_id_t deadlock_dest = cactus_deadlock_get_next_dest(*args);
+	ffa_vm_id_t deadlock_next_dest = source;
+
+	VERBOSE("%x is creating deadlock. next: %x\n", source, deadlock_dest);
+
+	return base_deadlock_handler(ffa_dir_msg_dest(*args), source,
+				      deadlock_dest, deadlock_next_dest);
+}
+
+CACTUS_CMD_HANDLER(req_deadlock_cmd, CACTUS_REQ_DEADLOCK_CMD)
+{
+	ffa_vm_id_t vm_id = ffa_dir_msg_dest(*args);
+	ffa_vm_id_t source = ffa_dir_msg_source(*args);
+	ffa_vm_id_t deadlock_dest = cactus_deadlock_get_next_dest(*args);
+	ffa_vm_id_t deadlock_next_dest = cactus_deadlock_get_next_dest2(*args);
+
+	VERBOSE("%x requested deadlock with %x and %x\n",
+		ffa_dir_msg_source(*args), deadlock_dest, deadlock_next_dest);
+
+	return base_deadlock_handler(vm_id, source, deadlock_dest, deadlock_next_dest);
+}
diff --git a/spm/cactus/cactus_tests/cactus_test_memory_sharing.c b/spm/cactus/cactus_tests/cactus_test_memory_sharing.c
new file mode 100644
index 0000000..63c3d13
--- /dev/null
+++ b/spm/cactus/cactus_tests/cactus_test_memory_sharing.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "cactus_def.h"
+#include "cactus_test_cmds.h"
+#include "cactus_tests.h"
+#include <ffa_helpers.h>
+#include <sp_helpers.h>
+#include <xlat_tables_defs.h>
+
+/* Memory section to be used for memory share operations */
+static __aligned(PAGE_SIZE) uint8_t share_page[PAGE_SIZE];
+
+CACTUS_CMD_HANDLER(mem_send_cmd, CACTUS_MEM_SEND_CMD)
+{
+	ffa_memory_management_test(
+		mb, ffa_dir_msg_dest(*args), ffa_dir_msg_source(*args),
+		cactus_req_mem_send_get_mem_func(*args),
+		cactus_mem_send_get_handle(*args));
+
+	return cactus_success_resp(ffa_dir_msg_dest(*args),
+				   ffa_dir_msg_source(*args), 0);
+}
+
+CACTUS_CMD_HANDLER(req_mem_send_cmd, CACTUS_REQ_MEM_SEND_CMD)
+{
+	smc_ret_values ffa_ret;
+	uint32_t mem_func = cactus_req_mem_send_get_mem_func(*args);
+	ffa_vm_id_t receiver = cactus_req_mem_send_get_receiver(*args);
+	ffa_memory_handle_t handle;
+	ffa_vm_id_t vm_id = ffa_dir_msg_dest(*args);
+	ffa_vm_id_t source = ffa_dir_msg_source(*args);
+
+	VERBOSE("%x requested to send memory to %x (func: %x)\n",
+		source, receiver, mem_func);
+
+	const struct ffa_memory_region_constituent constituents[] = {
+		{(void *)share_page, 1, 0}
+	};
+
+	const uint32_t constituents_count = (sizeof(constituents) /
+					     sizeof(constituents[0]));
+
+	handle = ffa_memory_init_and_send(
+		(struct ffa_memory_region *)mb->send, PAGE_SIZE,
+		vm_id, receiver, constituents,
+		constituents_count, mem_func);
+
+	/*
+	 * If returned an invalid handle, we should break the test.
+	 */
+	expect(handle != FFA_MEMORY_HANDLE_INVALID, true);
+
+	ffa_ret = cactus_mem_send_cmd(vm_id, receiver, mem_func, handle);
+
+	if (!is_ffa_direct_response(ffa_ret)) {
+		ERROR("Failed to send message. ret: %x error: %x\n",
+		      ffa_func_id(ffa_ret),
+		      ffa_error_code(ffa_ret));
+		return cactus_error_resp(vm_id, source, CACTUS_ERROR_FFA_CALL);
+	}
+
+	/* If anything went bad on the receiver's end. */
+	if (cactus_get_response(ffa_ret) == CACTUS_ERROR) {
+		ERROR("Received error from receiver!\n");
+		return cactus_error_resp(vm_id, source, CACTUS_ERROR_TEST);
+	}
+
+	if (mem_func != FFA_MEM_DONATE_SMC32) {
+		/*
+		 * Do a memory reclaim only if the mem_func regards to memory
+		 * share or lend operations, as with a donate the owner is
+		 * permanently given up access to the memory region.
+		 */
+		ffa_ret = ffa_mem_reclaim(handle, 0);
+		if (ffa_func_id(ffa_ret) == FFA_ERROR) {
+			ERROR("Failed to reclaim memory! error: %x\n",
+			      ffa_error_code(ffa_ret));
+			return cactus_error_resp(vm_id, source,
+						 CACTUS_ERROR_TEST);
+		}
+
+		/**
+		 * Read Content that has been written to memory to validate
+		 * access to memory segment has been reestablished, and receiver
+		 * made use of memory region.
+		 */
+		#if (LOG_LEVEL >= LOG_LEVEL_VERBOSE)
+			uint32_t *ptr = (uint32_t *)constituents->address;
+
+			VERBOSE("Memory contents after receiver SP's use:\n");
+			for (unsigned int i = 0U; i < 5U; i++) {
+				VERBOSE("      %u: %x\n", i, ptr[i]);
+			}
+		#endif
+	}
+
+	return cactus_success_resp(vm_id, source, 0);
+}