Extend sp_msg register count by moving routing extension bits

Move routing extension bits to the top of W3 and make the rest of W3
available in the sp_msg structure. This way the caller of the
sp_messaging layer can use up to five argument registers. If the routing
extension is enabled the two MSBs of the first register are not
available for the caller.

Signed-off-by: Imre Kis <imre.kis@arm.com>
Change-Id: I06b155272631662adb8713aade1edbf48a3bc4a9
diff --git a/components/messaging/ffa/libsp/ffa_direct_msg_routing_extension.c b/components/messaging/ffa/libsp/ffa_direct_msg_routing_extension.c
index 03a372f..f62d3f2 100644
--- a/components/messaging/ffa/libsp/ffa_direct_msg_routing_extension.c
+++ b/components/messaging/ffa/libsp/ffa_direct_msg_routing_extension.c
@@ -8,8 +8,9 @@
 #include <stdbool.h>
 
 #define SP_ID_INVALID FFA_ID_GET_ID_MASK
-#define FFA_ROUTING_EXT_RC_BIT BIT(0)
-#define FFA_ROUTING_EXT_ERROR_BIT BIT(1)
+#define FFA_ROUTING_EXT_RC_BIT		BIT32(31)
+#define FFA_ROUTING_EXT_ERROR_BIT	BIT32(30)
+#define FFA_ROUTING_EXT_BITS_MASK	(FFA_ROUTING_EXT_RC_BIT | FFA_ROUTING_EXT_ERROR_BIT)
 
 enum sp_rc_state { idle = 0, root, leaf, rc_root, internal, forwarding };
 
@@ -99,6 +100,9 @@
 	caller_id = req->source_id;
 	callee_id = SP_ID_INVALID;
 
+	if (FFA_IS_32_BIT_FUNC(req->function_id))
+		req->args.args32[0] &= ~FFA_ROUTING_EXT_BITS_MASK;
+
 	return FFA_OK;
 }
 
@@ -107,10 +111,17 @@
 	return request_received_hook(req);
 }
 
-void ffa_direct_msg_routing_ext_req_pre_hook(struct ffa_direct_msg *req)
+ffa_result ffa_direct_msg_routing_ext_req_pre_hook(struct ffa_direct_msg *req)
 {
+	if (FFA_IS_32_BIT_FUNC(req->function_id)) {
+		if (req->args.args32[0] & FFA_ROUTING_EXT_BITS_MASK)
+			return FFA_INVALID_PARAMETERS;
+	}
+
 	state = internal;
 	callee_id = req->destination_id;
+
+	return FFA_OK;
 }
 
 ffa_result ffa_direct_msg_routing_ext_req_post_hook(struct ffa_direct_msg *resp)
@@ -179,11 +190,18 @@
 	callee_id = SP_ID_INVALID;
 }
 
-void ffa_direct_msg_routing_ext_resp_pre_hook(struct ffa_direct_msg *resp)
+ffa_result ffa_direct_msg_routing_ext_resp_pre_hook(struct ffa_direct_msg *resp)
 {
+	if (FFA_IS_32_BIT_FUNC(resp->function_id)) {
+		if (resp->args.args32[0] & FFA_ROUTING_EXT_BITS_MASK)
+			return FFA_INVALID_PARAMETERS;
+	}
+
 	state = idle;
 	caller_id = SP_ID_INVALID;
 	callee_id = SP_ID_INVALID;
+
+	return FFA_OK;
 }
 
 ffa_result ffa_direct_msg_routing_ext_resp_post_hook(struct ffa_direct_msg *req)
@@ -195,10 +213,17 @@
 {
 }
 
-void ffa_direct_msg_routing_ext_rc_req_pre_hook(struct ffa_direct_msg *req)
+ffa_result ffa_direct_msg_routing_ext_rc_req_pre_hook(struct ffa_direct_msg *req)
 {
-	req->args.args32[0] = FFA_ROUTING_EXT_RC_BIT;
+	if (FFA_IS_32_BIT_FUNC(req->function_id)) {
+		if (req->args.args32[0] & FFA_ROUTING_EXT_BITS_MASK)
+			return FFA_INVALID_PARAMETERS;
+	}
+
+	req->args.args32[0] |= FFA_ROUTING_EXT_RC_BIT;
 	state = rc_root;
+
+	return FFA_OK;
 }
 
 ffa_result
diff --git a/components/messaging/ffa/libsp/include/ffa_api_defines.h b/components/messaging/ffa/libsp/include/ffa_api_defines.h
index f6e0212..8c304e7 100644
--- a/components/messaging/ffa/libsp/include/ffa_api_defines.h
+++ b/components/messaging/ffa/libsp/include/ffa_api_defines.h
@@ -62,6 +62,7 @@
 
 /* Utility macros */
 #define FFA_TO_32_BIT_FUNC(x)		((x) & (~UINT32_C(0x40000000)))
+#define FFA_TO_64_BIT_FUNC(x)		((x) | UINT32_C(0x40000000))
 #define FFA_IS_32_BIT_FUNC(x)		(((x) & UINT32_C(0x40000000)) == 0)
 #define FFA_IS_64_BIT_FUNC(x)		(((x) & UINT32_C(0x40000000)) != 0)
 
diff --git a/components/messaging/ffa/libsp/include/ffa_direct_msg_routing_extension.h b/components/messaging/ffa/libsp/include/ffa_direct_msg_routing_extension.h
index 3715f50..8944200 100644
--- a/components/messaging/ffa/libsp/include/ffa_direct_msg_routing_extension.h
+++ b/components/messaging/ffa/libsp/include/ffa_direct_msg_routing_extension.h
@@ -14,22 +14,18 @@
 extern "C" {
 #endif
 
-ffa_result
-ffa_direct_msg_routing_ext_wait_post_hook(struct ffa_direct_msg *req);
+ffa_result ffa_direct_msg_routing_ext_wait_post_hook(struct ffa_direct_msg *req);
 
-void ffa_direct_msg_routing_ext_req_pre_hook(struct ffa_direct_msg *req);
-ffa_result
-ffa_direct_msg_routing_ext_req_post_hook(struct ffa_direct_msg *resp);
+ffa_result ffa_direct_msg_routing_ext_req_pre_hook(struct ffa_direct_msg *req);
+ffa_result ffa_direct_msg_routing_ext_req_post_hook(struct ffa_direct_msg *resp);
 void ffa_direct_msg_routing_ext_req_error_hook(void);
 
-void ffa_direct_msg_routing_ext_resp_pre_hook(struct ffa_direct_msg *resp);
-ffa_result
-ffa_direct_msg_routing_ext_resp_post_hook(struct ffa_direct_msg *req);
+ffa_result ffa_direct_msg_routing_ext_resp_pre_hook(struct ffa_direct_msg *resp);
+ffa_result ffa_direct_msg_routing_ext_resp_post_hook(struct ffa_direct_msg *req);
 void ffa_direct_msg_routing_ext_resp_error_hook(void);
 
-void ffa_direct_msg_routing_ext_rc_req_pre_hook(struct ffa_direct_msg *req);
-ffa_result
-ffa_direct_msg_routing_ext_rc_req_post_hook(struct ffa_direct_msg *resp);
+ffa_result ffa_direct_msg_routing_ext_rc_req_pre_hook(struct ffa_direct_msg *req);
+ffa_result ffa_direct_msg_routing_ext_rc_req_post_hook(struct ffa_direct_msg *resp);
 void ffa_direct_msg_routing_ext_rc_req_error_hook(void);
 
 #ifdef __cplusplus
diff --git a/components/messaging/ffa/libsp/include/sp_messaging.h b/components/messaging/ffa/libsp/include/sp_messaging.h
index 7173a92..c1c5a7a 100644
--- a/components/messaging/ffa/libsp/include/sp_messaging.h
+++ b/components/messaging/ffa/libsp/include/sp_messaging.h
@@ -16,7 +16,7 @@
 extern "C" {
 #endif
 
-#define SP_MSG_ARG_COUNT (4)
+#define SP_MSG_ARG_COUNT (5)
 
 /**
  * @brief      SP message type
diff --git a/components/messaging/ffa/libsp/sp_messaging.c b/components/messaging/ffa/libsp/sp_messaging.c
index 392006b..67fd7ee 100644
--- a/components/messaging/ffa/libsp/sp_messaging.c
+++ b/components/messaging/ffa/libsp/sp_messaging.c
@@ -12,21 +12,18 @@
 
 #include <string.h>
 
-#define SP_MSG_ARG_OFFSET (1)
-
 static void pack_ffa_direct_msg(const struct sp_msg *msg,
 				struct ffa_direct_msg *ffa_msg)
 {
 	ffa_msg->source_id = msg->source_id;
 	ffa_msg->destination_id = msg->destination_id;
 
-	ffa_msg->args.args64[0] = 0;
-	if (msg->is_64bit_message)
-		memcpy(&ffa_msg->args.args64[SP_MSG_ARG_OFFSET],
-		       msg->args.args64, sizeof(msg->args.args64));
-	else
-		memcpy(&ffa_msg->args.args32[SP_MSG_ARG_OFFSET],
-		       msg->args.args32, sizeof(msg->args.args32));
+	if (msg->is_64bit_message) {
+		ffa_msg->function_id = FFA_TO_64_BIT_FUNC(0);
+		memcpy(ffa_msg->args.args64, msg->args.args64, sizeof(msg->args.args64));
+	} else {
+		memcpy(ffa_msg->args.args32, msg->args.args32, sizeof(msg->args.args32));
+	}
 }
 
 static void unpack_ffa_direct_msg(const struct ffa_direct_msg *ffa_msg,
@@ -39,10 +36,9 @@
 		 */
 		msg->source_id = ffa_msg->source_id;
 		msg->destination_id = ffa_msg->destination_id;
-		msg->is_64bit_message = FFA_IS_64_BIT_FUNC(ffa_msg->function_id);
+		msg->is_64bit_message = false;
 
-		memcpy(msg->args.args32, &ffa_msg->args.args32[SP_MSG_ARG_OFFSET],
-		       sizeof(msg->args.args32));
+		memcpy(msg->args.args32, ffa_msg->args.args32, sizeof(msg->args.args32));
 	} else if (ffa_msg->function_id == FFA_MSG_SEND_DIRECT_REQ_64 ||
 		   ffa_msg->function_id == FFA_MSG_SEND_DIRECT_RESP_64) {
 		/*
@@ -50,10 +46,9 @@
 		 */
 		msg->source_id = ffa_msg->source_id;
 		msg->destination_id = ffa_msg->destination_id;
-		msg->is_64bit_message = FFA_IS_64_BIT_FUNC(ffa_msg->function_id);
+		msg->is_64bit_message = true;
 
-		memcpy(msg->args.args64, &ffa_msg->args.args64[SP_MSG_ARG_OFFSET],
-		       sizeof(msg->args.args64));
+		memcpy(msg->args.args64, ffa_msg->args.args64, sizeof(msg->args.args64));
 	} else {
 		/* Success has no message parameters */
 		*msg = (struct sp_msg){ 0 };
@@ -104,7 +99,11 @@
 	pack_ffa_direct_msg(req, &ffa_req);
 
 #if FFA_DIRECT_MSG_ROUTING_EXTENSION
-	ffa_direct_msg_routing_ext_req_pre_hook(&ffa_req);
+	ffa_res = ffa_direct_msg_routing_ext_req_pre_hook(&ffa_req);
+	if (ffa_res != FFA_OK) {
+		*resp = (struct sp_msg){ 0 };
+		return SP_RESULT_INVALID_PARAMETERS;
+	}
 #endif
 
 	if (req->is_64bit_message)
@@ -158,7 +157,11 @@
 	pack_ffa_direct_msg(resp, &ffa_resp);
 
 #if FFA_DIRECT_MSG_ROUTING_EXTENSION
-	ffa_direct_msg_routing_ext_resp_pre_hook(&ffa_resp);
+	ffa_res = ffa_direct_msg_routing_ext_resp_pre_hook(&ffa_resp);
+	if (ffa_res != FFA_OK) {
+		*req = (struct sp_msg){ 0 };
+		return SP_RESULT_INVALID_PARAMETERS;
+	}
 #endif
 
 	if (resp->is_64bit_message)
@@ -212,7 +215,11 @@
 
 	pack_ffa_direct_msg(req, &ffa_req);
 
-	ffa_direct_msg_routing_ext_rc_req_pre_hook(&ffa_req);
+	ffa_res = ffa_direct_msg_routing_ext_rc_req_pre_hook(&ffa_req);
+	if (ffa_res != FFA_OK) {
+		*resp = (struct sp_msg){ 0 };
+		return SP_RESULT_INVALID_PARAMETERS;
+	}
 
 	ffa_res = ffa_msg_send_direct_resp_32(ffa_req.source_id,
 					   ffa_req.destination_id,
diff --git a/components/messaging/ffa/libsp/test/test_sp_messaging.cpp b/components/messaging/ffa/libsp/test/test_sp_messaging.cpp
index e6582e5..15aba01 100644
--- a/components/messaging/ffa/libsp/test/test_sp_messaging.cpp
+++ b/components/messaging/ffa/libsp/test/test_sp_messaging.cpp
@@ -10,11 +10,9 @@
 #include "mock_ffa_api.h"
 #include "../include/sp_messaging.h"
 
-#define SP_MSG_ARG_OFFSET (1)
-
 #if FFA_DIRECT_MSG_ROUTING_EXTENSION
-#define ROUTING_EXT_RC_BIT BIT(0)
-#define ROUTING_EXT_ERR_BIT BIT(1)
+#define ROUTING_EXT_RC_BIT BIT(31)
+#define ROUTING_EXT_ERR_BIT BIT(30)
 #endif
 
 TEST_GROUP(sp_messaging)
@@ -37,7 +35,7 @@
 		int i = 0;
 
 		for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
-			ffa_args[i + SP_MSG_ARG_OFFSET] = sp_args[i];
+			ffa_args[i] = sp_args[i];
 		}
 	}
 
@@ -46,7 +44,7 @@
 		int i = 0;
 
 		for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
-			ffa_args[i + SP_MSG_ARG_OFFSET] = sp_args[i];
+			ffa_args[i] = sp_args[i];
 		}
 	}
 
@@ -58,9 +56,8 @@
 		msg->source_id = source_id;
 		msg->destination_id = dest_id;
 
-		msg->args.args32[0] = 0;
 		for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
-			msg->args.args32[i + SP_MSG_ARG_OFFSET] = args32[i];
+			msg->args.args32[i] = args32[i];
 		}
 	}
 
@@ -72,9 +69,8 @@
 		msg->source_id = source_id;
 		msg->destination_id = dest_id;
 
-		msg->args.args64[0] = 0;
 		for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
-			msg->args.args64[i + SP_MSG_ARG_OFFSET] = args64[i];
+			msg->args.args64[i] = args64[i];
 		}
 	}
 
@@ -86,7 +82,7 @@
 		msg->destination_id = dest_id;
 		msg->is_64bit_message = false;
 		for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
-			msg->args.args32[i] = args32[i + SP_MSG_ARG_OFFSET];
+			msg->args.args32[i] = args32[i];
 		}
 	}
 
@@ -98,7 +94,7 @@
 		msg->destination_id = dest_id;
 		msg->is_64bit_message = true;
 		for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
-			msg->args.args64[i] = args64[i + SP_MSG_ARG_OFFSET];
+			msg->args.args64[i] = args64[i];
 		}
 	}
 
@@ -114,13 +110,13 @@
 		if (sp_msg->is_64bit_message) {
 			for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
 				UNSIGNED_LONGLONGS_EQUAL(
-					ffa_msg->args.args64[i + SP_MSG_ARG_OFFSET],
+					ffa_msg->args.args64[i],
 					sp_msg->args.args64[i]);
 			}
 		} else {
 			for (i = 0; i < SP_MSG_ARG_COUNT; i++) {
 				UNSIGNED_LONGS_EQUAL(
-					ffa_msg->args.args32[i + SP_MSG_ARG_OFFSET],
+					ffa_msg->args.args32[i],
 					sp_msg->args.args32[i]);
 			}
 		}
@@ -149,9 +145,10 @@
 	const uint16_t source_id = 0x1234;
 	const uint16_t dest_id = 0x5678;
 	const uint32_t args32[SP_MSG_ARG_COUNT] = { 0x01234567, 0x12345678,
-						    0x23456789, 0x3456789a };
+						    0x23456789, 0x3456789a, 0xbcdef012 };
 	const uint64_t args64[SP_MSG_ARG_COUNT] = { 0x0123456776543210, 0x1234567887654321,
-						    0x2345678998765432, 0x3456789aa9876543 };
+						    0x2345678998765432, 0x3456789aa9876543,
+						    0x210fedcba9876543 };
 	const sp_result result = -1;
 	const sp_msg empty_sp_msg = (const sp_msg){ 0 };
 };
@@ -299,6 +296,17 @@
 }
 
 #if FFA_DIRECT_MSG_ROUTING_EXTENSION
+TEST(sp_messaging, sp_msg_send_direct_req_rc_bits_conflict)
+{
+	sp_msg sp_req = { 0 };
+	sp_msg sp_resp = { 0 };
+
+	fill_sp_msg_32(&sp_req);
+	sp_req.args.args32[0] |= ROUTING_EXT_RC_BIT;
+
+	LONGS_EQUAL(SP_RESULT_INVALID_PARAMETERS, sp_msg_send_direct_req(&sp_req, &sp_resp));
+}
+
 TEST(sp_messaging, sp_msg_send_direct_req_rc_forwarding_success)
 {
 	const uint16_t root_id = 1;
@@ -345,9 +353,9 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_req, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					  req.args.args32[1], req.args.args32[2],
+					  req.args.args32[3], req.args.args32[4], &rc_req, FFA_OK);
 
 	/* Forwarding RC request to root and receiving RC response */
 	expect_ffa_msg_send_direct_resp_32(own_id, root_id, rc_req.args.args32[0],
@@ -395,9 +403,10 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_err, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					  req.args.args32[1], req.args.args32[2],
+					  req.args.args32[3], req.args.args32[4],
+					  &rc_err, FFA_OK);
 
 	LONGS_EQUAL(SP_RESULT_FFA(result),
 		    sp_msg_send_direct_req(&sp_req, &sp_resp));
@@ -456,9 +465,10 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_req, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					 req.args.args32[1], req.args.args32[2],
+					 req.args.args32[3], req.args.args32[4],
+					 &rc_req, FFA_OK);
 
 	/* Forwarding RC request to root and receiving a request to deny */
 	expect_ffa_msg_send_direct_resp_32(own_id, root_id, rc_req.args.args32[0],
@@ -534,9 +544,10 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_req, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					  req.args.args32[1], req.args.args32[2],
+					  req.args.args32[3], req.args.args32[4],
+					  &rc_req, FFA_OK);
 
 	/* Forwarding RC request to root and receiving RC response */
 	expect_ffa_msg_send_direct_resp_32(own_id, root_id, rc_req.args.args32[0],
@@ -612,9 +623,10 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_req, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					  req.args.args32[1], req.args.args32[2],
+					  req.args.args32[3], req.args.args32[4],
+					  &rc_req, FFA_OK);
 
 	/* Forwarding RC request to root and receiving RC response */
 	expect_ffa_msg_send_direct_resp_32(own_id, root_id, rc_req.args.args32[0],
@@ -693,9 +705,10 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_req, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					  req.args.args32[1], req.args.args32[2],
+					  req.args.args32[3], req.args.args32[4],
+					  &rc_req, FFA_OK);
 
 	/* Forwarding RC request to root and receiving RC response */
 	expect_ffa_msg_send_direct_resp_32(own_id, root_id, rc_req.args.args32[0],
@@ -767,9 +780,10 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_req, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					  req.args.args32[1], req.args.args32[2],
+					  req.args.args32[3], req.args.args32[4],
+					  &rc_req, FFA_OK);
 
 	/* Forwarding RC request to root and receiving RC response */
 	expect_ffa_msg_send_direct_resp_32(own_id, root_id, rc_req.args.args32[0],
@@ -834,9 +848,10 @@
 	wait_and_receive_request(root_id, own_id);
 
 	/* Sending request and receiving RC request from RC root */
-	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, 0, req.args.args32[1],
-				       req.args.args32[2], req.args.args32[3], req.args.args32[4],
-				       &rc_req, FFA_OK);
+	expect_ffa_msg_send_direct_req_32(own_id, rc_root_id, req.args.args32[0],
+					  req.args.args32[1], req.args.args32[2],
+					  req.args.args32[3], req.args.args32[4],
+					  &rc_req, FFA_OK);
 
 	/* Forwarding RC request to root and receiving RC response */
 	expect_ffa_msg_send_direct_resp_32(own_id, root_id, rc_req.args.args32[0],
@@ -940,6 +955,17 @@
 }
 
 #if FFA_DIRECT_MSG_ROUTING_EXTENSION
+TEST(sp_messaging, sp_msg_send_direct_resp_rc_bits_conflict)
+{
+	sp_msg sp_req = { 0 };
+	sp_msg sp_resp = { 0 };
+
+	fill_sp_msg_32(&sp_resp);
+	sp_resp.args.args32[0] |= ROUTING_EXT_RC_BIT;
+
+	LONGS_EQUAL(SP_RESULT_INVALID_PARAMETERS, sp_msg_send_direct_resp(&sp_resp, &sp_req));
+}
+
 TEST(sp_messaging, sp_msg_send_direct_resp_deny_rc_failure)
 {
 	uint32_t expected_ffa_args[5] = { 0 };
@@ -981,11 +1007,10 @@
 	fill_ffa_msg_32(&ffa_msg);
 	copy_sp_to_ffa_args_32(resp.args.args32, expected_ffa_args);
 
-	expect_ffa_msg_send_direct_resp_32(resp.source_id, resp.destination_id, 0,
-					expected_ffa_args[1],
-					expected_ffa_args[2],
-					expected_ffa_args[3],
-					expected_ffa_args[4], &rc_msg, FFA_OK);
+	expect_ffa_msg_send_direct_resp_32(resp.source_id, resp.destination_id,
+					   expected_ffa_args[0], expected_ffa_args[1],
+					   expected_ffa_args[2], expected_ffa_args[3],
+					   expected_ffa_args[4], &rc_msg, FFA_OK);
 
 	expect_ffa_msg_send_direct_resp_32(
 		rc_msg.destination_id, rc_msg.source_id,
@@ -1010,18 +1035,30 @@
 	MEMCMP_EQUAL(&empty_sp_msg, &resp, sizeof(empty_sp_msg));
 }
 
+TEST(sp_messaging, sp_msg_send_direct_rc_resp_rc_bits_conflict)
+{
+	sp_msg sp_req = { 0 };
+	sp_msg sp_resp = { 0 };
+
+	fill_sp_msg_32(&sp_req);
+	sp_req.args.args32[0] |= ROUTING_EXT_RC_BIT;
+
+	LONGS_EQUAL(SP_RESULT_INVALID_PARAMETERS, sp_msg_send_rc_req(&sp_req, &sp_resp));
+}
+
 TEST(sp_messaging, sp_msg_send_rc_req_ffa_error)
 {
 	ffa_result result = FFA_ABORTED;
 
 	fill_sp_msg_32(&resp);
 	memset(&req, 0x5a, sizeof(req));
+	req.args.args32[0] &= ~0xc0000000;
 	req.is_64bit_message = false;
 	fill_ffa_msg_32(&ffa_msg);
 
 	expect_ffa_msg_send_direct_resp_32(req.source_id, req.destination_id,
-					ROUTING_EXT_RC_BIT, req.args.args32[0],
-					req.args.args32[1], req.args.args32[2], req.args.args32[3],
+					ROUTING_EXT_RC_BIT | req.args.args32[0], req.args.args32[1],
+					req.args.args32[2], req.args.args32[3], req.args.args32[4],
 					&ffa_msg, result);
 
 	LONGS_EQUAL(SP_RESULT_FFA(result), sp_msg_send_rc_req(&req, &resp));
@@ -1046,8 +1083,8 @@
 	ffa_msg.args.args32[0] = 0;
 
 	expect_ffa_msg_send_direct_resp_32(req.source_id, req.destination_id,
-					ROUTING_EXT_RC_BIT, req.args.args32[0],
-					req.args.args32[1], req.args.args32[2], req.args.args32[3],
+					ROUTING_EXT_RC_BIT | req.args.args32[0], req.args.args32[1],
+					req.args.args32[2], req.args.args32[3], req.args.args32[4],
 					&ffa_msg, FFA_OK);
 
 	expect_ffa_msg_send_direct_resp_32(
@@ -1080,8 +1117,8 @@
 	ffa_msg.args.args32[1] = sp_err;
 
 	expect_ffa_msg_send_direct_resp_32(req.source_id, req.destination_id,
-					ROUTING_EXT_RC_BIT, req.args.args32[0],
-					req.args.args32[1], req.args.args32[2], req.args.args32[3],
+					ROUTING_EXT_RC_BIT | req.args.args32[0], req.args.args32[1],
+					req.args.args32[2], req.args.args32[3], req.args.args32[4],
 					&ffa_msg, FFA_OK);
 
 	LONGS_EQUAL(sp_err, sp_msg_send_rc_req(&req, &resp));
@@ -1102,11 +1139,11 @@
 	fill_ffa_msg_32(&ffa_msg);
 	ffa_msg.source_id = root_id;
 	ffa_msg.destination_id = own_id;
-	ffa_msg.args.args32[0] = ROUTING_EXT_RC_BIT;
+	ffa_msg.args.args32[0] = ROUTING_EXT_RC_BIT | req.args.args32[0];
 
 	expect_ffa_msg_send_direct_resp_32(req.source_id, req.destination_id,
-					ROUTING_EXT_RC_BIT, req.args.args32[0],
-					req.args.args32[1], req.args.args32[2], req.args.args32[3],
+					ROUTING_EXT_RC_BIT | req.args.args32[0], req.args.args32[1],
+					req.args.args32[2], req.args.args32[3], req.args.args32[4],
 					&ffa_msg, FFA_OK);
 
 	LONGS_EQUAL(SP_RESULT_OK, sp_msg_send_rc_req(&req, &resp));