lib: sm: Add support handling for FFA_MSG_SEND_DIRECT_REQ2 requests Bug: 380065728 Change-Id: Ib2ff3133e0ffce57acdb7deb1f8f1481f8b23d9b

diff --git a/lib/sm/sm.c b/lib/sm/sm.c
index 2756f1d..c51f203 100644
--- a/lib/sm/sm.c
+++ b/lib/sm/sm.c

@@ -320,6 +320,8 @@
 
 static void sm_ffa_loop(long ret, struct smc32_args* args) {
     struct smc_ret18 regs = {0};
+    uint64_t extended_args[ARM_FFA_MSG_EXTENDED_ARGS_COUNT];
+    STATIC_ASSERT(sizeof extended_args == sizeof regs.req2_params);
     enum arm_ffa_init_state ffa_init_state = arm_ffa_init_state();
 
     if (atomic_load(&platform_halted)) {
@@ -366,6 +368,35 @@
             regs = arm_ffa_msg_send_direct_resp(&regs, (ulong)ret, 0, 0, 0, 0);
             break;
 
+        case SMC_FC64_FFA_MSG_SEND_DIRECT_REQ2:
+            if (atomic_load(&platform_halted)) {
+                regs = arm_ffa_call_error(FFA_ERROR_ABORTED);
+                break;
+            }
+
+            ret = arm_ffa_handle_direct_req2(&regs);
+            /*
+             * Whereas sm_ffa_handle_direct_req returns secure monitor error
+             * codes, arm_ffa_handle_direct_req2 can fail with Trusty error
+             * codes not understood by the caller, e.g., if no handler is found.
+             */
+            if (ret) {
+                dprintf(CRITICAL,
+                        "Failed to handle FFA_MSG_SEND_DIRECT_REQ2: %lx\n",
+                        ret);
+                regs = arm_ffa_call_error(FFA_ERROR_ABORTED);
+                break;
+            }
+
+            LTRACEF_LEVEL(5, "Calling FFA_MSG_SEND_DIRECT_RESP2 (%ld)\n", ret);
+            /*
+             * copy req2_params into a fresh buffer `args` since the former can
+             * be overwritten at any time by the callee.
+             */
+            memcpy(extended_args, regs.req2_params, sizeof extended_args);
+            regs = arm_ffa_msg_send_direct_resp2(&regs, extended_args);
+            break;
+
         case SMC_FC_FFA_RUN:
             if (atomic_load(&platform_halted)) {
                 /* Return to NS since we have nothing to do */