diff --git a/include/runtime_services/ffa_helpers.h b/include/runtime_services/ffa_helpers.h
index e96787e..ed3302f 100644
--- a/include/runtime_services/ffa_helpers.h
+++ b/include/runtime_services/ffa_helpers.h
@@ -17,8 +17,17 @@
 /* Hypervisor ID at physical FFA instance */
 #define HYP_ID          (0)
 
-/* By convention, SP IDs (as opposed to VM IDs) have bit 15 set */
-#define SP_ID(x)        ((x) | (1 << 15))
+/*
+ * The bit 15 of the FF-A ID indicates whether the partition is executing
+ * in the normal world, in case it is a Virtual Machine (VM); or in the
+ * secure world, in case it is a Secure Partition (SP).
+ *
+ * If bit 15 is set partition is an SP; if bit 15 is clear partition is
+ * a VM.
+ */
+#define SP_ID_MASK	U(1 << 15)
+#define SP_ID(x)	((x) | SP_ID_MASK)
+#define IS_SP_ID(x)	((x & SP_ID_MASK) != 0U)
 
 typedef unsigned short ffa_vm_id_t;
 typedef unsigned short ffa_vm_count_t;
diff --git a/spm/cactus/cactus_main.c b/spm/cactus/cactus_main.c
index 531ce4b..7900e7f 100644
--- a/spm/cactus/cactus_main.c
+++ b/spm/cactus/cactus_main.c
@@ -43,6 +43,7 @@
 	smc_ret_values ffa_ret;
 	uint32_t sp_response;
 	ffa_vm_id_t source;
+	ffa_vm_id_t destination;
 
 	/*
 	* This initial wait call is necessary to inform SPMD that
@@ -55,6 +56,11 @@
 	for (;;) {
 		VERBOSE("Woke up with func id: %lx\n", ffa_ret.ret0);
 
+		if (ffa_ret.ret0 == FFA_ERROR) {
+			ERROR("Error: %lx\n", ffa_ret.ret2);
+			break;
+		}
+
 		if (ffa_ret.ret0 != FFA_MSG_SEND_DIRECT_REQ_SMC32 &&
 		    ffa_ret.ret0 != FFA_MSG_SEND_DIRECT_REQ_SMC64) {
 			ERROR("%s(%u) unknown func id 0x%lx\n",
@@ -62,18 +68,15 @@
 			break;
 		}
 
-		if (ffa_ret.ret1 != vm_id) {
+		destination = ffa_ret.ret1 & U(0xFFFF);
+
+		source = ffa_ret.ret1 >> 16;
+
+		if (destination != vm_id) {
 			ERROR("%s(%u) invalid vm id 0x%lx\n",
 				__func__, vm_id, ffa_ret.ret1);
 			break;
 		}
-		source = ffa_ret.ret2;
-
-		if (source != HYP_ID) {
-			ERROR("%s(%u) invalid hyp id 0x%lx\n",
-				__func__, vm_id, ffa_ret.ret2);
-			break;
-		}
 
 		PRINT_CMD(ffa_ret);
 
@@ -92,6 +95,44 @@
 			 */
 			ffa_ret = CACTUS_SUCCESS_RESP(vm_id, source);
 			break;
+
+		case CACTUS_ECHO_CMD:
+		{
+			uint64_t echo_val = CACTUS_ECHO_GET_VAL(ffa_ret);
+
+			VERBOSE("Received echo at %x, value %llx.\n",
+				destination, echo_val);
+			ffa_ret = CACTUS_RESPONSE(vm_id, 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 (ffa_ret.ret0 != FFA_MSG_SEND_DIRECT_RESP_SMC32) {
+				ERROR("Failed to send message. error: %lx\n",
+					ffa_ret.ret2);
+				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) :
+					    CACTUS_ERROR_RESP(vm_id, source);
+			break;
+		}
 		default:
 			/*
 			 * Currently direct message test is handled here.
@@ -189,6 +230,7 @@
 	assert(IS_IN_EL1() != 0);
 
 	struct mailbox_buffers mb;
+
 	/* Clear BSS */
 	memset((void *)CACTUS_BSS_START,
 	       0, CACTUS_BSS_END - CACTUS_BSS_START);