[PATCH v4 1/1] ArmPkg/OpteeLib: Add APIs to communicate with OP-TEE

[Sumit Garg <sumit.garg@linaro.org>]

Add following APIs to communicate with OP-TEE pseudo/early TAs:
1. OpteeInit
2. OpteeOpenSession
3. OpteeCloseSession
4. OpteeInvokeFunc

Cc: Ard Biesheuvel <ard.biesheuvel@linaro.org>
Cc: Leif Lindholm <leif.lindholm@linaro.org>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Sumit Garg <sumit.garg@linaro.org>
---
 ArmPkg/Library/OpteeLib/OpteeLib.inf |   2 +
 ArmPkg/Include/Library/OpteeLib.h    |  88 +++++
 ArmPkg/Library/OpteeLib/OpteeSmc.h   |  43 +++
 ArmPkg/Library/OpteeLib/Optee.c      | 397 ++++++++++++++++++++
 4 files changed, 530 insertions(+)

diff --git a/ArmPkg/Library/OpteeLib/OpteeLib.inf b/ArmPkg/Library/OpteeLib/OpteeLib.inf
index 5abd427379cc..e03054a7167d 100644
--- a/ArmPkg/Library/OpteeLib/OpteeLib.inf
+++ b/ArmPkg/Library/OpteeLib/OpteeLib.inf
@@ -23,11 +23,13 @@ [Defines]
 
 [Sources]
   Optee.c
+  OpteeSmc.h
 
 [Packages]
   ArmPkg/ArmPkg.dec
   MdePkg/MdePkg.dec
 
 [LibraryClasses]
+  ArmMmuLib
   ArmSmcLib
   BaseLib
diff --git a/ArmPkg/Include/Library/OpteeLib.h b/ArmPkg/Include/Library/OpteeLib.h
index f65d8674d9b8..6884d5681831 100644
--- a/ArmPkg/Include/Library/OpteeLib.h
+++ b/ArmPkg/Include/Library/OpteeLib.h
@@ -25,10 +25,98 @@
 #define OPTEE_OS_UID2          0xaf630002
 #define OPTEE_OS_UID3          0xa5d5c51b
 
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_NONE                0x0
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INPUT         0x1
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_OUTPUT        0x2
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INOUT         0x3
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_INPUT        0x9
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_OUTPUT       0xa
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_INOUT        0xb
+
+#define OPTEE_MESSAGE_ATTRIBUTE_TYPE_MASK                0xff
+
+#define OPTEE_ORIGIN_COMMUNICATION              0x00000002
+#define OPTEE_ERROR_COMMUNICATION               0xFFFF000E
+
+typedef struct {
+  UINT64    BufferAddress;
+  UINT64    Size;
+  UINT64    SharedMemoryReference;
+} OPTEE_MESSAGE_PARAM_MEMORY;
+
+typedef struct {
+  UINT64    A;
+  UINT64    B;
+  UINT64    C;
+} OPTEE_MESSAGE_PARAM_VALUE;
+
+typedef struct {
+  UINT64 Attribute;
+  union {
+    OPTEE_MESSAGE_PARAM_MEMORY   Memory;
+    OPTEE_MESSAGE_PARAM_VALUE    Value;
+  } Union;
+} OPTEE_MESSAGE_PARAM;
+
+#define OPTEE_MAX_CALL_PARAMS       4
+
+typedef struct {
+  UINT32    Command;
+  UINT32    Function;
+  UINT32    Session;
+  UINT32    CancelId;
+  UINT32    Pad;
+  UINT32    Return;
+  UINT32    ReturnOrigin;
+  UINT32    NumParams;
+
+  // NumParams tells the actual number of element in Params
+  OPTEE_MESSAGE_PARAM  Params[OPTEE_MAX_CALL_PARAMS];
+} OPTEE_MESSAGE_ARG;
+
+typedef struct {
+  EFI_GUID  Uuid;           // [in] GUID/UUID of the Trusted Application
+  UINT32    Session;        // [out] Session id
+  UINT32    Return;         // [out] Return value
+  UINT32    ReturnOrigin;   // [out] Origin of the return value
+} OPTEE_OPEN_SESSION_ARG;
+
+typedef struct {
+  UINT32    Function;       // [in] Trusted Application function, specific to the TA
+  UINT32    Session;        // [in] Session id
+  UINT32    Return;         // [out] Return value
+  UINT32    ReturnOrigin;   // [out] Origin of the return value
+  OPTEE_MESSAGE_PARAM  Params[OPTEE_MAX_CALL_PARAMS]; // Params for function to be invoked
+} OPTEE_INVOKE_FUNCTION_ARG;
+
 BOOLEAN
 EFIAPI
 IsOpteePresent (
   VOID
   );
 
+EFI_STATUS
+EFIAPI
+OpteeInit (
+  VOID
+  );
+
+EFI_STATUS
+EFIAPI
+OpteeOpenSession (
+  IN OUT OPTEE_OPEN_SESSION_ARG      *OpenSessionArg
+  );
+
+EFI_STATUS
+EFIAPI
+OpteeCloseSession (
+  IN UINT32                      Session
+  );
+
+EFI_STATUS
+EFIAPI
+OpteeInvokeFunction (
+  IN OUT OPTEE_INVOKE_FUNCTION_ARG       *InvokeFunctionArg
+  );
+
 #endif
diff --git a/ArmPkg/Library/OpteeLib/OpteeSmc.h b/ArmPkg/Library/OpteeLib/OpteeSmc.h
new file mode 100644
index 000000000000..21ff4b22ab92
--- /dev/null
+++ b/ArmPkg/Library/OpteeLib/OpteeSmc.h
@@ -0,0 +1,43 @@
+/** @file
+  OP-TEE SMC header file.
+
+  Copyright (c) 2018, Linaro Ltd. All rights reserved.<BR>
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _OPTEE_SMC_H_
+#define _OPTEE_SMC_H_
+
+/* Returned in Arg0 only from Trusted OS functions */
+#define OPTEE_SMC_RETURN_OK                     0x0
+
+#define OPTEE_SMC_RETURN_FROM_RPC               0x32000003
+#define OPTEE_SMC_CALL_WITH_ARG                 0x32000004
+#define OPTEE_SMC_GET_SHARED_MEMORY_CONFIG      0xb2000007
+
+#define OPTEE_SMC_SHARED_MEMORY_CACHED          1
+
+#define OPTEE_SMC_RETURN_RPC_FOREIGN_INTERRUPT  0xffff0004
+
+#define OPTEE_MESSAGE_COMMAND_OPEN_SESSION      0
+#define OPTEE_MESSAGE_COMMAND_INVOKE_FUNCTION   1
+#define OPTEE_MESSAGE_COMMAND_CLOSE_SESSION     2
+
+#define OPTEE_MESSAGE_ATTRIBUTE_META            0x100
+
+#define OPTEE_LOGIN_PUBLIC                      0x0
+
+typedef struct {
+  UINTN    Base;
+  UINTN    Size;
+} OPTEE_SHARED_MEMORY_INFORMATION;
+
+#endif
diff --git a/ArmPkg/Library/OpteeLib/Optee.c b/ArmPkg/Library/OpteeLib/Optee.c
index 574527f8b5ea..6617126e8bdb 100644
--- a/ArmPkg/Library/OpteeLib/Optee.c
+++ b/ArmPkg/Library/OpteeLib/Optee.c
@@ -14,11 +14,18 @@
 
 **/
 
+#include <Library/ArmMmuLib.h>
 #include <Library/ArmSmcLib.h>
+#include <Library/BaseMemoryLib.h>
 #include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
 #include <Library/OpteeLib.h>
 
 #include <IndustryStandard/ArmStdSmc.h>
+#include <OpteeSmc.h>
+#include <Uefi.h>
+
+STATIC OPTEE_SHARED_MEMORY_INFORMATION OpteeSharedMemoryInformation = { 0 };
 
 /**
   Check for OP-TEE presence.
@@ -31,6 +38,7 @@ IsOpteePresent (
 {
   ARM_SMC_ARGS ArmSmcArgs;
 
+  ZeroMem (&ArmSmcArgs, sizeof (ARM_SMC_ARGS));
   // Send a Trusted OS Calls UID command
   ArmSmcArgs.Arg0 = ARM_SMC_ID_TOS_UID;
   ArmCallSmc (&ArmSmcArgs);
@@ -44,3 +52,392 @@ IsOpteePresent (
     return FALSE;
   }
 }
+
+STATIC
+EFI_STATUS
+OpteeSharedMemoryRemap (
+  VOID
+  )
+{
+  ARM_SMC_ARGS                 ArmSmcArgs;
+  EFI_PHYSICAL_ADDRESS         PhysicalAddress;
+  EFI_PHYSICAL_ADDRESS         Start;
+  EFI_PHYSICAL_ADDRESS         End;
+  EFI_STATUS                   Status;
+  UINTN                        Size;
+
+  ZeroMem (&ArmSmcArgs, sizeof (ARM_SMC_ARGS));
+  ArmSmcArgs.Arg0 = OPTEE_SMC_GET_SHARED_MEMORY_CONFIG;
+
+  ArmCallSmc (&ArmSmcArgs);
+  if (ArmSmcArgs.Arg0 != OPTEE_SMC_RETURN_OK) {
+    DEBUG ((DEBUG_WARN, "OP-TEE shared memory not supported\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  if (ArmSmcArgs.Arg3 != OPTEE_SMC_SHARED_MEMORY_CACHED) {
+    DEBUG ((DEBUG_WARN, "OP-TEE: Only normal cached shared memory supported\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  Start = (ArmSmcArgs.Arg1 + SIZE_4KB - 1) & ~(SIZE_4KB - 1);
+  End = (ArmSmcArgs.Arg1 + ArmSmcArgs.Arg2) & ~(SIZE_4KB - 1);
+  PhysicalAddress = Start;
+  Size = End - Start;
+
+  if (Size < SIZE_4KB) {
+    DEBUG ((DEBUG_WARN, "OP-TEE shared memory too small\n"));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  Status = ArmSetMemoryAttributes (PhysicalAddress, Size, EFI_MEMORY_WB);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  OpteeSharedMemoryInformation.Base = (UINTN)PhysicalAddress;
+  OpteeSharedMemoryInformation.Size = Size;
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+OpteeInit (
+  VOID
+  )
+{
+  EFI_STATUS      Status;
+
+  if (!IsOpteePresent ()) {
+    DEBUG ((DEBUG_WARN, "OP-TEE not present\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = OpteeSharedMemoryRemap ();
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_WARN, "OP-TEE shared memory remap failed\n"));
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Does Standard SMC to OP-TEE in secure world.
+
+  @param[in]  PhysicalArg   Physical address of message to pass to secure world
+
+  @return                   0 on success, secure world return code otherwise
+
+**/
+STATIC
+UINT32
+OpteeCallWithArg (
+  IN EFI_PHYSICAL_ADDRESS PhysicalArg
+  )
+{
+  ARM_SMC_ARGS ArmSmcArgs;
+
+  ZeroMem (&ArmSmcArgs, sizeof (ARM_SMC_ARGS));
+  ArmSmcArgs.Arg0 = OPTEE_SMC_CALL_WITH_ARG;
+  ArmSmcArgs.Arg1 = (UINT32)(PhysicalArg >> 32);
+  ArmSmcArgs.Arg2 = (UINT32)PhysicalArg;
+
+  while (TRUE) {
+    ArmCallSmc (&ArmSmcArgs);
+
+    if (ArmSmcArgs.Arg0 == OPTEE_SMC_RETURN_RPC_FOREIGN_INTERRUPT) {
+      //
+      // A foreign interrupt was raised while secure world was
+      // executing, since they are handled in UEFI a dummy RPC is
+      // performed to let UEFI take the interrupt through the normal
+      // vector.
+      //
+      ArmSmcArgs.Arg0 = OPTEE_SMC_RETURN_FROM_RPC;
+    } else {
+      break;
+    }
+  }
+
+  return ArmSmcArgs.Arg0;
+}
+
+STATIC
+VOID
+UuidToOctets (
+  OUT UINT8              *UuidOctet,
+  IN EFI_GUID            *Uuid
+  )
+{
+  UuidOctet[0] = Uuid->Data1 >> 24;
+  UuidOctet[1] = Uuid->Data1 >> 16;
+  UuidOctet[2] = Uuid->Data1 >> 8;
+  UuidOctet[3] = Uuid->Data1;
+  UuidOctet[4] = Uuid->Data2 >> 8;
+  UuidOctet[5] = Uuid->Data2;
+  UuidOctet[6] = Uuid->Data3 >> 8;
+  UuidOctet[7] = Uuid->Data3;
+  CopyMem (UuidOctet + 8, Uuid->Data4, sizeof (Uuid->Data4));
+}
+
+EFI_STATUS
+EFIAPI
+OpteeOpenSession (
+  IN OUT OPTEE_OPEN_SESSION_ARG      *OpenSessionArg
+  )
+{
+  OPTEE_MESSAGE_ARG    *MessageArg;
+
+  MessageArg = NULL;
+
+  if (OpteeSharedMemoryInformation.Base == 0) {
+    DEBUG ((DEBUG_WARN, "OP-TEE not initialized\n"));
+    return EFI_NOT_STARTED;
+  }
+
+  MessageArg = (OPTEE_MESSAGE_ARG *)OpteeSharedMemoryInformation.Base;
+  ZeroMem (MessageArg, sizeof (OPTEE_MESSAGE_ARG));
+
+  MessageArg->Command = OPTEE_MESSAGE_COMMAND_OPEN_SESSION;
+
+  //
+  // Initialize and add the meta parameters needed when opening a
+  // session.
+  //
+  MessageArg->Params[0].Attribute = OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INPUT |
+                                    OPTEE_MESSAGE_ATTRIBUTE_META;
+  MessageArg->Params[1].Attribute = OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INPUT |
+                                    OPTEE_MESSAGE_ATTRIBUTE_META;
+  UuidToOctets (
+    (UINT8 *)&MessageArg->Params[0].Union.Value,
+    &OpenSessionArg->Uuid
+    );
+  ZeroMem (&MessageArg->Params[1].Union.Value, sizeof (EFI_GUID));
+  MessageArg->Params[1].Union.Value.C = OPTEE_LOGIN_PUBLIC;
+
+  MessageArg->NumParams = 2;
+
+  if (OpteeCallWithArg ((EFI_PHYSICAL_ADDRESS)MessageArg)) {
+    MessageArg->Return = OPTEE_ERROR_COMMUNICATION;
+    MessageArg->ReturnOrigin = OPTEE_ORIGIN_COMMUNICATION;
+  }
+
+  OpenSessionArg->Session = MessageArg->Session;
+  OpenSessionArg->Return = MessageArg->Return;
+  OpenSessionArg->ReturnOrigin = MessageArg->ReturnOrigin;
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+OpteeCloseSession (
+  IN UINT32                  Session
+  )
+{
+  OPTEE_MESSAGE_ARG    *MessageArg;
+
+  MessageArg = NULL;
+
+  if (OpteeSharedMemoryInformation.Base == 0) {
+    DEBUG ((DEBUG_WARN, "OP-TEE not initialized\n"));
+    return EFI_NOT_STARTED;
+  }
+
+  MessageArg = (OPTEE_MESSAGE_ARG *)OpteeSharedMemoryInformation.Base;
+  ZeroMem (MessageArg, sizeof (OPTEE_MESSAGE_ARG));
+
+  MessageArg->Command = OPTEE_MESSAGE_COMMAND_CLOSE_SESSION;
+  MessageArg->Session = Session;
+
+  OpteeCallWithArg ((EFI_PHYSICAL_ADDRESS)MessageArg);
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+OpteeToMessageParam (
+  OUT OPTEE_MESSAGE_PARAM    *MessageParams,
+  IN UINT32                  NumParams,
+  IN OPTEE_MESSAGE_PARAM     *InParams
+  )
+{
+  UINT32                  Idx;
+  UINTN                   ParamSharedMemoryAddress;
+  UINTN                   SharedMemorySize;
+  UINTN                   Size;
+
+  Size = (sizeof (OPTEE_MESSAGE_ARG) + sizeof (UINT64) - 1) &
+          ~(sizeof (UINT64) - 1);
+  ParamSharedMemoryAddress = OpteeSharedMemoryInformation.Base + Size;
+  SharedMemorySize = OpteeSharedMemoryInformation.Size - Size;
+
+  for (Idx = 0; Idx < NumParams; Idx++) {
+    CONST OPTEE_MESSAGE_PARAM    *InParam;
+    OPTEE_MESSAGE_PARAM          *MessageParam;
+    UINT32                       Attribute;
+
+    InParam = InParams + Idx;
+    MessageParam = MessageParams + Idx;
+    Attribute = InParam->Attribute & OPTEE_MESSAGE_ATTRIBUTE_TYPE_MASK;
+
+    switch (Attribute) {
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_NONE:
+      MessageParam->Attribute = OPTEE_MESSAGE_ATTRIBUTE_TYPE_NONE;
+      ZeroMem (&MessageParam->Union, sizeof (MessageParam->Union));
+      break;
+
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_OUTPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INOUT:
+      MessageParam->Attribute = Attribute;
+      MessageParam->Union.Value.A = InParam->Union.Value.A;
+      MessageParam->Union.Value.B = InParam->Union.Value.B;
+      MessageParam->Union.Value.C = InParam->Union.Value.C;
+      break;
+
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_INPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_OUTPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_INOUT:
+      MessageParam->Attribute = Attribute;
+
+      if (InParam->Union.Memory.Size > SharedMemorySize) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      CopyMem (
+        (VOID *)ParamSharedMemoryAddress,
+        (VOID *)InParam->Union.Memory.BufferAddress,
+        InParam->Union.Memory.Size
+        );
+      MessageParam->Union.Memory.BufferAddress = (UINT64)ParamSharedMemoryAddress;
+      MessageParam->Union.Memory.Size = InParam->Union.Memory.Size;
+
+      Size = (InParam->Union.Memory.Size + sizeof (UINT64) - 1) &
+              ~(sizeof (UINT64) - 1);
+      ParamSharedMemoryAddress += Size;
+      SharedMemorySize -= Size;
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+OpteeFromMessageParam (
+  OUT OPTEE_MESSAGE_PARAM    *OutParams,
+  IN UINT32                  NumParams,
+  IN OPTEE_MESSAGE_PARAM     *MessageParams
+  )
+{
+  UINT32                 Idx;
+
+  for (Idx = 0; Idx < NumParams; Idx++) {
+    OPTEE_MESSAGE_PARAM          *OutParam;
+    CONST OPTEE_MESSAGE_PARAM    *MessageParam;
+    UINT32                   Attribute;
+
+    OutParam = OutParams + Idx;
+    MessageParam = MessageParams + Idx;
+    Attribute = MessageParam->Attribute & OPTEE_MESSAGE_ATTRIBUTE_TYPE_MASK;
+
+    switch (Attribute) {
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_NONE:
+      OutParam->Attribute = OPTEE_MESSAGE_ATTRIBUTE_TYPE_NONE;
+      ZeroMem (&OutParam->Union, sizeof (OutParam->Union));
+      break;
+
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_OUTPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_VALUE_INOUT:
+      OutParam->Attribute = Attribute;
+      OutParam->Union.Value.A = MessageParam->Union.Value.A;
+      OutParam->Union.Value.B = MessageParam->Union.Value.B;
+      OutParam->Union.Value.C = MessageParam->Union.Value.C;
+      break;
+
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_INPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_OUTPUT:
+    case OPTEE_MESSAGE_ATTRIBUTE_TYPE_MEMORY_INOUT:
+      OutParam->Attribute = Attribute;
+
+      if (MessageParam->Union.Memory.Size > OutParam->Union.Memory.Size) {
+        return EFI_BAD_BUFFER_SIZE;
+      }
+
+      CopyMem (
+        (VOID *)OutParam->Union.Memory.BufferAddress,
+        (VOID *)MessageParam->Union.Memory.BufferAddress,
+        MessageParam->Union.Memory.Size
+        );
+      OutParam->Union.Memory.Size = MessageParam->Union.Memory.Size;
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+OpteeInvokeFunction (
+  IN OUT OPTEE_INVOKE_FUNCTION_ARG       *InvokeFunctionArg
+  )
+{
+  EFI_STATUS       Status;
+  OPTEE_MESSAGE_ARG    *MessageArg;
+
+  MessageArg = NULL;
+
+  if (OpteeSharedMemoryInformation.Base == 0) {
+    DEBUG ((DEBUG_WARN, "OP-TEE not initialized\n"));
+    return EFI_NOT_STARTED;
+  }
+
+  MessageArg = (OPTEE_MESSAGE_ARG *)OpteeSharedMemoryInformation.Base;
+  ZeroMem (MessageArg, sizeof (OPTEE_MESSAGE_ARG));
+
+  MessageArg->Command = OPTEE_MESSAGE_COMMAND_INVOKE_FUNCTION;
+  MessageArg->Function = InvokeFunctionArg->Function;
+  MessageArg->Session = InvokeFunctionArg->Session;
+
+  Status = OpteeToMessageParam (
+             MessageArg->Params,
+             OPTEE_MAX_CALL_PARAMS,
+             InvokeFunctionArg->Params
+             );
+  if (Status) {
+    return Status;
+  }
+
+  MessageArg->NumParams = OPTEE_MAX_CALL_PARAMS;
+
+  if (OpteeCallWithArg ((EFI_PHYSICAL_ADDRESS)MessageArg)) {
+    MessageArg->Return = OPTEE_ERROR_COMMUNICATION;
+    MessageArg->ReturnOrigin = OPTEE_ORIGIN_COMMUNICATION;
+  }
+
+  if (OpteeFromMessageParam (
+        InvokeFunctionArg->Params,
+        OPTEE_MAX_CALL_PARAMS,
+        MessageArg->Params
+        )) {
+    MessageArg->Return = OPTEE_ERROR_COMMUNICATION;
+    MessageArg->ReturnOrigin = OPTEE_ORIGIN_COMMUNICATION;
+  }
+
+  InvokeFunctionArg->Return = MessageArg->Return;
+  InvokeFunctionArg->ReturnOrigin = MessageArg->ReturnOrigin;
+
+  return EFI_SUCCESS;
+}
-- 
2.7.4