[edk2,v2,2/2] ArmPkg/OpteeLib: Add APIs to communicate with OP-TEE

diff --git a/ArmPkg/Include/Library/OpteeLib.h b/ArmPkg/Include/Library/OpteeLib.h
index f65d8674d9b8..89d6b5a7b34f 100644
--- a/ArmPkg/Include/Library/OpteeLib.h
+++ b/ArmPkg/Include/Library/OpteeLib.h
@@ -25,10 +25,97 @@ 
 #define OPTEE_OS_UID2          0xaf630002
 #define OPTEE_OS_UID3          0xa5d5c51b
 
+#define OPTEE_MSG_ATTR_TYPE_NONE                0x0
+#define OPTEE_MSG_ATTR_TYPE_VALUE_INPUT         0x1
+#define OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT        0x2
+#define OPTEE_MSG_ATTR_TYPE_VALUE_INOUT         0x3
+#define OPTEE_MSG_ATTR_TYPE_MEM_INPUT           0x9
+#define OPTEE_MSG_ATTR_TYPE_MEM_OUTPUT          0xa
+#define OPTEE_MSG_ATTR_TYPE_MEM_INOUT           0xb
+
+#define OPTEE_MSG_ATTR_TYPE_MASK                0xff
+
+typedef struct {
+  UINT64    BufPtr;
+  UINT64    Size;
+  UINT64    ShmRef;
+} OPTEE_MSG_PARAM_MEM;
+
+typedef struct {
+  UINT64    A;
+  UINT64    B;
+  UINT64    C;
+} OPTEE_MSG_PARAM_VALUE;
+
+typedef struct {
+  UINT64 Attr;
+  union {
+    OPTEE_MSG_PARAM_MEM      Mem;
+    OPTEE_MSG_PARAM_VALUE    Value;
+  } U;
+} OPTEE_MSG_PARAM;
+
+#define MAX_PARAMS           4
+
+typedef struct {
+        UINT32             Cmd;
+        UINT32             Func;
+        UINT32             Session;
+        UINT32             CancelId;
+        UINT32             Pad;
+        UINT32             Ret;
+        UINT32             RetOrigin;
+        UINT32             NumParams;
+
+        // NumParams tells the actual number of element in Params
+        OPTEE_MSG_PARAM    Params[MAX_PARAMS];
+} OPTEE_MSG_ARG;
+
+#define OPTEE_UUID_LEN       16
+
+typedef struct {
+        UINT8     Uuid[OPTEE_UUID_LEN]; // [in] UUID of the Trusted Application
+        UINT32    Session;              // [out] Session id
+        UINT32    Ret;                  // [out] Return value
+        UINT32    RetOrigin;            // [out] Origin of the return value
+} OPTEE_OPEN_SESSION_ARG;
+
+typedef struct {
+        UINT32             Func;    // [in] Trusted App func, specific to the TA
+        UINT32             Session; // [in] Session id
+        UINT32             Ret;     // [out] Return value
+        UINT32             RetOrigin; // [out] Origin of the return value
+        OPTEE_MSG_PARAM    Params[MAX_PARAMS]; // Params for func to be invoked
+} OPTEE_INVOKE_FUNC_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
+OpteeInvokeFunc (
+  IN OUT OPTEE_INVOKE_FUNC_ARG       *InvokeFuncArg
+  );
+
 #endif
diff --git a/ArmPkg/Library/OpteeLib/Optee.c b/ArmPkg/Library/OpteeLib/Optee.c
index 574527f8b5ea..2111022d3662 100644
--- a/ArmPkg/Library/OpteeLib/Optee.c
+++ b/ArmPkg/Library/OpteeLib/Optee.c
@@ -14,11 +14,19 @@ 
 
 **/
 
+#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 <IndustryStandard/GlobalPlatform.h>
+#include <OpteeSmc.h>
+#include <Uefi.h>
+
+STATIC OPTEE_SHARED_MEMORY_INFO OpteeShmInfo = { 0 };
 
 /**
   Check for OP-TEE presence.
@@ -31,6 +39,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 +53,352 @@  IsOpteePresent (
     return FALSE;
   }
 }
+
+STATIC
+EFI_STATUS
+OpteeShmMemRemap (
+  VOID
+  )
+{
+  ARM_SMC_ARGS                 ArmSmcArgs;
+  EFI_PHYSICAL_ADDRESS         Paddr;
+  EFI_PHYSICAL_ADDRESS         Start;
+  EFI_PHYSICAL_ADDRESS         End;
+  EFI_STATUS                   Status;
+  UINTN                        Size;
+
+  ZeroMem (&ArmSmcArgs, sizeof (ARM_SMC_ARGS));
+  ArmSmcArgs.Arg0 = OPTEE_SMC_GET_SHM_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_SHM_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);
+  Paddr = 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 (Paddr, Size, EFI_MEMORY_WB);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  OpteeShmInfo.Base = (UINTN)Paddr;
+  OpteeShmInfo.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 = OpteeShmMemRemap ();
+  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]  Parg   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 Parg
+  )
+{
+  ARM_SMC_ARGS ArmSmcArgs;
+
+  ZeroMem (&ArmSmcArgs, sizeof (ARM_SMC_ARGS));
+  ArmSmcArgs.Arg0 = OPTEE_SMC_CALL_WITH_ARG;
+  ArmSmcArgs.Arg1 = (UINT32)(Parg >> 32);
+  ArmSmcArgs.Arg2 = (UINT32)Parg;
+
+  while (TRUE) {
+    ArmCallSmc (&ArmSmcArgs);
+
+    if (ArmSmcArgs.Arg0 == OPTEE_SMC_RETURN_RPC_FOREIGN_INTR) {
+      //
+      // 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;
+}
+
+EFI_STATUS
+EFIAPI
+OpteeOpenSession (
+  IN OUT OPTEE_OPEN_SESSION_ARG      *OpenSessionArg
+  )
+{
+  OPTEE_MSG_ARG    *MsgArg;
+
+  MsgArg = NULL;
+
+  if (OpteeShmInfo.Base == 0) {
+    DEBUG ((DEBUG_WARN, "OP-TEE not initialized\n"));
+    return EFI_NOT_STARTED;
+  }
+
+  MsgArg = (OPTEE_MSG_ARG *)OpteeShmInfo.Base;
+  ZeroMem (MsgArg, sizeof (OPTEE_MSG_ARG));
+
+  MsgArg->Cmd = OPTEE_MSG_CMD_OPEN_SESSION;
+
+  //
+  // Initialize and add the meta parameters needed when opening a
+  // session.
+  //
+  MsgArg->Params[0].Attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT |
+                           OPTEE_MSG_ATTR_META;
+  MsgArg->Params[1].Attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT |
+                           OPTEE_MSG_ATTR_META;
+  CopyMem (&MsgArg->Params[0].U.Value, OpenSessionArg->Uuid, OPTEE_UUID_LEN);
+  ZeroMem (&MsgArg->Params[1].U.Value, OPTEE_UUID_LEN);
+  MsgArg->Params[1].U.Value.C = TEE_LOGIN_PUBLIC;
+
+  MsgArg->NumParams = 2;
+
+  if (OpteeCallWithArg ((EFI_PHYSICAL_ADDRESS)MsgArg)) {
+    MsgArg->Ret = TEEC_ERROR_COMMUNICATION;
+    MsgArg->RetOrigin = TEEC_ORIGIN_COMMS;
+  }
+
+  OpenSessionArg->Session = MsgArg->Session;
+  OpenSessionArg->Ret = MsgArg->Ret;
+  OpenSessionArg->RetOrigin = MsgArg->RetOrigin;
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+OpteeCloseSession (
+  IN UINT32                      Session
+  )
+{
+  OPTEE_MSG_ARG    *MsgArg;
+
+  MsgArg = NULL;
+
+  if (OpteeShmInfo.Base == 0) {
+    DEBUG ((DEBUG_WARN, "OP-TEE not initialized\n"));
+    return EFI_NOT_STARTED;
+  }
+
+  MsgArg = (OPTEE_MSG_ARG *)OpteeShmInfo.Base;
+  ZeroMem (MsgArg, sizeof (OPTEE_MSG_ARG));
+
+  MsgArg->Cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
+  MsgArg->Session = Session;
+
+  OpteeCallWithArg ((EFI_PHYSICAL_ADDRESS)MsgArg);
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+OpteeToMsgParam (
+  OUT OPTEE_MSG_PARAM    *MsgParams,
+  IN UINT32              NumParams,
+  IN OPTEE_MSG_PARAM     *InParams
+  )
+{
+  UINT32                  Idx;
+  UINTN                   ParamShmAddr;
+  UINTN                   ShmSize;
+  UINTN                   Size;
+
+  Size = (sizeof (OPTEE_MSG_ARG) + sizeof (UINT64) - 1) & ~(sizeof (UINT64) - 1);
+  ParamShmAddr = OpteeShmInfo.Base + Size;
+  ShmSize = OpteeShmInfo.Size - Size;
+
+  for (Idx = 0; Idx < NumParams; Idx++) {
+    CONST OPTEE_MSG_PARAM    *Ip;
+    OPTEE_MSG_PARAM          *Mp;
+    UINT32                   Attr;
+
+    Ip = InParams + Idx;
+    Mp = MsgParams + Idx;
+    Attr = Ip->Attr & OPTEE_MSG_ATTR_TYPE_MASK;
+
+    switch (Attr) {
+    case OPTEE_MSG_ATTR_TYPE_NONE:
+      Mp->Attr = OPTEE_MSG_ATTR_TYPE_NONE;
+      ZeroMem (&Mp->U, sizeof (Mp->U));
+      break;
+
+    case OPTEE_MSG_ATTR_TYPE_VALUE_INPUT:
+    case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
+    case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
+      Mp->Attr = Attr;
+      Mp->U.Value.A = Ip->U.Value.A;
+      Mp->U.Value.B = Ip->U.Value.B;
+      Mp->U.Value.C = Ip->U.Value.C;
+      break;
+
+    case OPTEE_MSG_ATTR_TYPE_MEM_INPUT:
+    case OPTEE_MSG_ATTR_TYPE_MEM_OUTPUT:
+    case OPTEE_MSG_ATTR_TYPE_MEM_INOUT:
+      Mp->Attr = Attr;
+
+      if (Ip->U.Mem.Size > ShmSize) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      CopyMem ((VOID *)ParamShmAddr, (VOID *)Ip->U.Mem.BufPtr, Ip->U.Mem.Size);
+      Mp->U.Mem.BufPtr = (UINT64)ParamShmAddr;
+      Mp->U.Mem.Size = Ip->U.Mem.Size;
+
+      Size = (Ip->U.Mem.Size + sizeof (UINT64) - 1) & ~(sizeof (UINT64) - 1);
+      ParamShmAddr += Size;
+      ShmSize -= Size;
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+OpteeFromMsgParam (
+  OUT OPTEE_MSG_PARAM    *OutParams,
+  IN UINT32              NumParams,
+  IN OPTEE_MSG_PARAM     *MsgParams
+  )
+{
+  UINT32                 Idx;
+
+  for (Idx = 0; Idx < NumParams; Idx++) {
+    OPTEE_MSG_PARAM          *Op;
+    CONST OPTEE_MSG_PARAM    *Mp;
+    UINT32                   Attr;
+
+    Op = OutParams + Idx;
+    Mp = MsgParams + Idx;
+    Attr = Mp->Attr & OPTEE_MSG_ATTR_TYPE_MASK;
+
+    switch (Attr) {
+    case OPTEE_MSG_ATTR_TYPE_NONE:
+      Op->Attr = OPTEE_MSG_ATTR_TYPE_NONE;
+      ZeroMem (&Op->U, sizeof (Op->U));
+      break;
+
+    case OPTEE_MSG_ATTR_TYPE_VALUE_INPUT:
+    case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
+    case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
+      Op->Attr = Attr;
+      Op->U.Value.A = Mp->U.Value.A;
+      Op->U.Value.B = Mp->U.Value.B;
+      Op->U.Value.C = Mp->U.Value.C;
+      break;
+
+    case OPTEE_MSG_ATTR_TYPE_MEM_INPUT:
+    case OPTEE_MSG_ATTR_TYPE_MEM_OUTPUT:
+    case OPTEE_MSG_ATTR_TYPE_MEM_INOUT:
+      Op->Attr = Attr;
+
+      if (Mp->U.Mem.Size > Op->U.Mem.Size) {
+        return EFI_BAD_BUFFER_SIZE;
+      }
+
+      CopyMem ((VOID *)Op->U.Mem.BufPtr, (VOID *)Mp->U.Mem.BufPtr, Mp->U.Mem.Size);
+      Op->U.Mem.Size = Mp->U.Mem.Size;
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+OpteeInvokeFunc (
+  IN OUT OPTEE_INVOKE_FUNC_ARG       *InvokeFuncArg
+  )
+{
+  EFI_STATUS       Status;
+  OPTEE_MSG_ARG    *MsgArg;
+
+  MsgArg = NULL;
+
+  if (OpteeShmInfo.Base == 0) {
+    DEBUG ((DEBUG_WARN, "OP-TEE not initialized\n"));
+    return EFI_NOT_STARTED;
+  }
+
+  MsgArg = (OPTEE_MSG_ARG *)OpteeShmInfo.Base;
+  ZeroMem (MsgArg, sizeof (OPTEE_MSG_ARG));
+
+  MsgArg->Cmd = OPTEE_MSG_CMD_INVOKE_COMMAND;
+  MsgArg->Func = InvokeFuncArg->Func;
+  MsgArg->Session = InvokeFuncArg->Session;
+
+  Status = OpteeToMsgParam (MsgArg->Params, MAX_PARAMS, InvokeFuncArg->Params);
+  if (Status)
+    return Status;
+
+  MsgArg->NumParams = MAX_PARAMS;
+
+  if (OpteeCallWithArg ((EFI_PHYSICAL_ADDRESS)MsgArg)) {
+    MsgArg->Ret = TEEC_ERROR_COMMUNICATION;
+    MsgArg->RetOrigin = TEEC_ORIGIN_COMMS;
+  }
+
+  if (OpteeFromMsgParam (InvokeFuncArg->Params, MAX_PARAMS, MsgArg->Params)) {
+    MsgArg->Ret = TEEC_ERROR_COMMUNICATION;
+    MsgArg->RetOrigin = TEEC_ORIGIN_COMMS;
+  }
+
+  InvokeFuncArg->Ret = MsgArg->Ret;
+  InvokeFuncArg->RetOrigin = MsgArg->RetOrigin;
+
+  return EFI_SUCCESS;
+}
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/Library/OpteeLib/OpteeSmc.h b/ArmPkg/Library/OpteeLib/OpteeSmc.h
new file mode 100644
index 000000000000..e2ea35784a0a
--- /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_SHM_CONFIG                0xb2000007
+
+#define OPTEE_SMC_SHM_CACHED                    1
+
+#define OPTEE_SMC_RETURN_RPC_FOREIGN_INTR       0xffff0004
+
+#define OPTEE_MSG_CMD_OPEN_SESSION              0
+#define OPTEE_MSG_CMD_INVOKE_COMMAND            1
+#define OPTEE_MSG_CMD_CLOSE_SESSION             2
+
+#define OPTEE_MSG_ATTR_META                     0x100
+
+#define TEE_LOGIN_PUBLIC                        0x0
+
+typedef struct {
+  UINTN    Base;
+  UINTN    Size;
+} OPTEE_SHARED_MEMORY_INFO;
+
+#endif