cactus: handle 'CACTUS_REQ_ECHO_CMD' and 'CACTUS_ECHO_CMD'
Changing message loop to handle newly added commands.
The intent is to use these commands to validate FF-A direct messaging
between SPs.
Signed-off-by: J-Alves <joao.alves@arm.com>
Change-Id: Id0ecbfafb13918fa2d349e70164b735f9ade6841
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);