diff --git a/spm/cactus/cactus.ld.S b/spm/cactus/cactus.ld.S
index 11b28ba..50fc576 100644
--- a/spm/cactus/cactus.ld.S
+++ b/spm/cactus/cactus.ld.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2017-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -52,6 +52,10 @@
         . = ALIGN(PAGE_SIZE);
         __DATA_START__ = .;
         *(.data*)
+        . = ALIGN(PAGE_SIZE);
+        cactus_cmd_handler_begin = .;
+        KEEP(*(.cactus_handler))
+        cactus_cmd_handler_end = .;
         . = NEXT(PAGE_SIZE);
         __DATA_END__ = .;
     }
diff --git a/spm/cactus/cactus.mk b/spm/cactus/cactus.mk
index 4a9cfcc..971a9d6 100644
--- a/spm/cactus/cactus.mk
+++ b/spm/cactus/cactus.mk
@@ -35,11 +35,17 @@
 		cactus_debug.c				\
 		cactus_ffa_tests.c 			\
 		cactus_main.c				\
+		cactus_test_cmds.c			\
 	)						\
 	$(addprefix spm/common/,			\
 		aarch64/sp_arch_helpers.S		\
 		sp_helpers.c				\
 	)						\
+	$(addprefix spm/cactus/cactus_tests/,		\
+		cactus_test_cpu_features.c		\
+		cactus_test_direct_messaging.c		\
+		cactus_test_memory_sharing.c		\
+	)
 
 # TODO: Remove dependency on TFTF files.
 CACTUS_SOURCES	+=					\
diff --git a/spm/cactus/cactus_ffa_tests.c b/spm/cactus/cactus_ffa_tests.c
index 032d2e2..8d34e37 100644
--- a/spm/cactus/cactus_ffa_tests.c
+++ b/spm/cactus/cactus_ffa_tests.c
@@ -26,22 +26,6 @@
 static const uint32_t null_uuid[4] = {0};
 
 /*
- * 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.
- */
-void fill_simd_vectors(void)
-{
-	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);
-}
-
-/*
  * Test FFA_FEATURES interface.
  */
 static void ffa_features_test(void)
diff --git a/spm/cactus/cactus_main.c b/spm/cactus/cactus_main.c
index 9388ec5..37b8bd5 100644
--- a/spm/cactus/cactus_main.c
+++ b/spm/cactus/cactus_main.c
@@ -29,9 +29,6 @@
 extern const char build_message[];
 extern const char version_string[];
 
-/* Memory section to be used for memory share operations */
-static __aligned(PAGE_SIZE) uint8_t share_page[PAGE_SIZE];
-
 /*
  *
  * Message loop function
@@ -40,11 +37,11 @@
  * but rather through Hafnium print hypercall.
  *
  */
+
 static void __dead2 message_loop(ffa_vm_id_t vm_id, struct mailbox_buffers *mb)
 {
 	smc_ret_values ffa_ret;
 	uint32_t sp_response;
-	ffa_vm_id_t source;
 	ffa_vm_id_t destination;
 	uint64_t cactus_cmd;
 
@@ -57,6 +54,8 @@
 	ffa_ret = ffa_msg_wait();
 
 	for (;;) {
+		/* temporary 'skip_switch' label. Deleted in following commit */
+		skip_switch:
 		VERBOSE("Woke up with func id: %x\n", ffa_func_id(ffa_ret));
 
 		if (ffa_func_id(ffa_ret) == FFA_ERROR) {
@@ -73,8 +72,6 @@
 
 		destination = ffa_dir_msg_dest(ffa_ret);
 
-		source = ffa_dir_msg_source(ffa_ret);
-
 		if (destination != vm_id) {
 			ERROR("%s(%u) invalid vm id 0x%x\n",
 				__func__, vm_id, destination);
@@ -85,208 +82,10 @@
 
 		cactus_cmd = cactus_get_cmd(ffa_ret);
 
+		if (cactus_handle_cmd(&ffa_ret, &ffa_ret, mb)) {
+			goto skip_switch;
+		}
 		switch (cactus_cmd) {
-		case CACTUS_MEM_SEND_CMD:
-			ffa_memory_management_test(
-					mb, vm_id, source,
-					cactus_req_mem_send_get_mem_func(
-						ffa_ret),
-					cactus_mem_send_get_handle(ffa_ret));
-
-			/*
-			 * If execution gets to this point means all operations
-			 * with memory retrieval went well, as such replying
-			 */
-			ffa_ret = cactus_success_resp(vm_id, source, 0);
-			break;
-		case CACTUS_REQ_MEM_SEND_CMD:
-		{
-			uint32_t mem_func =
-				cactus_req_mem_send_get_mem_func(ffa_ret);
-			ffa_vm_id_t receiver =
-				cactus_req_mem_send_get_receiver(ffa_ret);
-			ffa_memory_handle_t handle;
-
-			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. error: %x\n",
-					ffa_error_code(ffa_ret));
-				ffa_ret = cactus_error_resp(vm_id, source, 0);
-				break;
-			}
-
-			/* If anything went bad on the receiver's end. */
-			if (cactus_get_response(ffa_ret) == CACTUS_ERROR) {
-				ERROR("Received error from receiver!\n");
-				ffa_ret = cactus_error_resp(vm_id, source, 0);
-				break;
-			}
-
-			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.
-				 */
-				if (ffa_mem_reclaim(handle, 0)
-							.ret0 == FFA_ERROR) {
-					ERROR("Failed to reclaim memory!\n");
-					ffa_ret = cactus_error_resp(vm_id,
-								    source, 0);
-					break;
-				}
-
-				/**
-				 * 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
-			}
-
-			ffa_ret = cactus_success_resp(vm_id, source, 0);
-			break;
-		}
-		case CACTUS_ECHO_CMD:
-		{
-			uint64_t echo_val = cactus_echo_get_val(ffa_ret);
-
-			VERBOSE("Received echo at %x, value %llx from %x.\n",
-				destination, echo_val, source);
-			ffa_ret = cactus_response(destination, source, echo_val);
-			break;
-		}
-		case CACTUS_REQ_ECHO_CMD:
-		{
-			ffa_vm_id_t echo_dest =
-					cactus_req_echo_get_echo_dest(ffa_ret);
-			uint64_t echo_val = cactus_echo_get_val(ffa_ret);
-			bool success = true;
-
-			VERBOSE("%x requested to send echo to %x, value %llx\n",
-				source, 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));
-				success = false;
-			}
-
-			if (cactus_get_response(ffa_ret) != echo_val) {
-				ERROR("Echo Failed!\n");
-				success = false;
-			}
-
-			ffa_ret = success ?
-				  cactus_success_resp(vm_id, source, 0) :
-				  cactus_error_resp(vm_id, source, 0);
-			break;
-		}
-		case CACTUS_DEADLOCK_CMD:
-		case CACTUS_REQ_DEADLOCK_CMD:
-		{
-			ffa_vm_id_t deadlock_dest =
-				cactus_deadlock_get_next_dest(ffa_ret);
-			ffa_vm_id_t deadlock_next_dest = source;
-
-			if (cactus_cmd == CACTUS_DEADLOCK_CMD) {
-				VERBOSE("%x is creating deadlock. next: %x\n",
-					source, deadlock_dest);
-			} else if (cactus_cmd == CACTUS_REQ_DEADLOCK_CMD) {
-				VERBOSE(
-				"%x requested deadlock with %x and %x\n",
-				source, deadlock_dest, deadlock_next_dest);
-
-				deadlock_next_dest =
-					cactus_deadlock_get_next_dest2(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) {
-				NOTICE("Attempting dealock but got error %x\n",
-					ffa_error_code(ffa_ret));
-			}
-
-			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.
-				 */
-				ffa_ret = cactus_success_resp(vm_id, source, 0);
-				break;
-			}
-
-			/* Shouldn't get to this point */
-			ERROR("Deadlock test went wrong!\n");
-			ffa_ret = cactus_error_resp(vm_id, source, 0);
-			break;
-		}
-		case CACTUS_REQ_SIMD_FILL_CMD:
-			fill_simd_vectors();
-			ffa_ret = cactus_success_resp(vm_id, source, 0);
-			break;
 		default:
 			/*
 			 * Currently direct message test is handled here.
diff --git a/spm/cactus/cactus_test_cmds.c b/spm/cactus/cactus_test_cmds.c
new file mode 100644
index 0000000..128f4c0
--- /dev/null
+++ b/spm/cactus/cactus_test_cmds.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "cactus_test_cmds.h"
+#include <ffa_helpers.h>
+
+/**
+ * Begin and end of command handler table, respectively. Both symbols defined by
+ * the linker.
+ */
+extern struct cactus_cmd_handler cactus_cmd_handler_begin[];
+extern struct cactus_cmd_handler cactus_cmd_handler_end[];
+
+/**
+ * Traverses command table from section ".cactus_handler", searches for a
+ * registered command and invokes the respective handler.
+ */
+bool cactus_handle_cmd(smc_ret_values *cmd_args, smc_ret_values *ret,
+		       struct mailbox_buffers *mb)
+{
+	if (cmd_args == NULL || ret == NULL) {
+		ERROR("Invalid argumentos passed to %s!\n", __func__);
+		return false;
+	}
+
+	uint64_t in_cmd = cactus_get_cmd(*cmd_args);
+
+	for (struct cactus_cmd_handler *it_cmd = cactus_cmd_handler_begin;
+	     it_cmd < cactus_cmd_handler_end;
+	     it_cmd++) {
+		if (it_cmd->id == in_cmd) {
+			*ret = it_cmd->fn(cmd_args, mb);
+			return true;
+		}
+	}
+
+	return false;
+}
diff --git a/spm/cactus/cactus_test_cmds.h b/spm/cactus/cactus_test_cmds.h
index f814ba5..d8bba95 100644
--- a/spm/cactus/cactus_test_cmds.h
+++ b/spm/cactus/cactus_test_cmds.h
@@ -9,12 +9,20 @@
 
 #include <debug.h>
 #include <ffa_helpers.h>
+#include <spm_common.h>
 
 /**
  * Success and error return to be sent over a msg response.
  */
-#define CACTUS_SUCCESS	 U(0)
-#define CACTUS_ERROR	 U(-1)
+#define CACTUS_SUCCESS		U(0)
+#define CACTUS_ERROR		U(-1)
+
+/**
+ * Error codes.
+ */
+#define CACTUS_ERROR_INVALID		U(1)
+#define CACTUS_ERROR_TEST		U(2)
+#define CACTUS_ERROR_FFA_CALL		U(3)
 
 /**
  * Get command from struct smc_ret_values.
@@ -243,7 +251,44 @@
 static inline smc_ret_values cactus_req_simd_fill_send_cmd(
 	ffa_vm_id_t source, ffa_vm_id_t dest)
 {
-	return cactus_send_cmd(source, dest, CACTUS_REQ_SIMD_FILL_CMD, 0, 0, 0, 0);
+	return cactus_send_cmd(source, dest, CACTUS_REQ_SIMD_FILL_CMD, 0, 0, 0,
+			       0);
 }
 
+/**
+ * Pairs a command id with a function call, to handle the command ID.
+ */
+struct cactus_cmd_handler {
+	const uint64_t id;
+	smc_ret_values (*fn)(const smc_ret_values *args,
+			     struct mailbox_buffers *mb);
+};
+
+/**
+ * Helper to create the name of a handler function.
+ */
+#define CACTUS_HANDLER_FN_NAME(name) cactus_##name##_handler
+
+/**
+ * Define handler's function signature.
+ */
+#define CACTUS_HANDLER_FN(name)						\
+	static smc_ret_values CACTUS_HANDLER_FN_NAME(name)(		\
+		const smc_ret_values *args, struct mailbox_buffers *mb)
+
+/**
+ * Helper to define Cactus command handler, and pair it with a command ID.
+ * It also creates a table with this information, to be traversed by
+ * 'cactus_handle_cmd' function.
+ */
+#define CACTUS_CMD_HANDLER(name, ID)					\
+	CACTUS_HANDLER_FN(name);					\
+	struct cactus_cmd_handler name __section(".cactus_handler") = {	\
+		.id = ID, .fn = CACTUS_HANDLER_FN_NAME(name),		\
+	};								\
+	CACTUS_HANDLER_FN(name)
+
+bool cactus_handle_cmd(smc_ret_values *cmd_args, smc_ret_values *ret,
+		       struct mailbox_buffers *mb);
+
 #endif
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);
+}