Re: [PATCH V2 13/28] UefiCpuPkg: Enable Tdx support in MpInitLib

["Ni, Ray" <ray.ni@intel.com>]

Min,
The change is to provide a totally different MP service in TDX case.
It makes the MpInitLib more complicated.

How about?
1. Change CpuMpPei/CpuMpDxe to return directly in TDX case.
2. Add new TdxCpuMpPei/TdxCpuMpDxe to provide a new set simple MP service in TDX case.

This makes the whole code easier to understand.

Thanks,
Ray

-----Original Message-----
From: Xu, Min M <min.m.xu@intel.com> 
Sent: Tuesday, October 5, 2021 11:39 AM
To: devel@edk2.groups.io
Cc: Xu, Min M <min.m.xu@intel.com>; Brijesh Singh <brijesh.singh@amd.com>; Erdem Aktas <erdemaktas@google.com>; James Bottomley <jejb@linux.ibm.com>; Yao, Jiewen <jiewen.yao@intel.com>; Tom Lendacky <thomas.lendacky@amd.com>; Dong, Eric <eric.dong@intel.com>; Ni, Ray <ray.ni@intel.com>; Kumar, Rahul1 <rahul1.kumar@intel.com>
Subject: [PATCH V2 13/28] UefiCpuPkg: Enable Tdx support in MpInitLib

RFC： https://bugzilla.tianocore.org/show_bug.cgi?id=3429

In TDVF BSP and APs are simplified. BSP is the vCPU-0, while the others are treated as APs.

So MP intialization is rather simple. The processor info is retrieved by TDCALL, ApWorker is not supported, BSP is always the working processor, while the APs are just in a wait-for-precedure state.

Cc: Brijesh Singh <brijesh.singh@amd.com>
Cc: Erdem Aktas <erdemaktas@google.com>
Cc: James Bottomley <jejb@linux.ibm.com>
Cc: Jiewen Yao <jiewen.yao@intel.com>
Cc: Tom Lendacky <thomas.lendacky@amd.com>
Cc: Eric Dong <eric.dong@intel.com>
Cc: Ray Ni <ray.ni@intel.com>
Cc: Rahul Kumar <rahul1.kumar@intel.com>
Signed-off-by: Min Xu <min.m.xu@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf |   4 +
 UefiCpuPkg/Library/MpInitLib/DxeMpLib.c       |  14 +-
 UefiCpuPkg/Library/MpInitLib/MpIntelTdx.h     | 107 ++++++++++
 UefiCpuPkg/Library/MpInitLib/MpLib.c          |  26 +++
 UefiCpuPkg/Library/MpInitLib/MpLibTdx.c       | 186 ++++++++++++++++++
 UefiCpuPkg/Library/MpInitLib/MpLibTdxNull.c   | 117 +++++++++++
 UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf |   4 +
 .../Library/MpInitLib/X64/IntelTdcall.nasm    | 120 +++++++++++
 8 files changed, 577 insertions(+), 1 deletion(-)  create mode 100644 UefiCpuPkg/Library/MpInitLib/MpIntelTdx.h
 create mode 100644 UefiCpuPkg/Library/MpInitLib/MpLibTdx.c
 create mode 100644 UefiCpuPkg/Library/MpInitLib/MpLibTdxNull.c
 create mode 100644 UefiCpuPkg/Library/MpInitLib/X64/IntelTdcall.nasm

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf b/UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf
index d34419c2a524..084e025564ef 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf
@@ -22,10 +22,13 @@
 #
 
 [Sources.IA32]
+  MpLibTdxNull.c
   Ia32/MpFuncs.nasm
 
 [Sources.X64]
+  MpLibTdx.c
   X64/MpFuncs.nasm
+  X64/IntelTdcall.nasm
 
 [Sources.common]
   MpEqu.inc
@@ -33,6 +36,7 @@
   MpLib.c
   MpLib.h
   Microcode.c
+  MpIntelTdx.h
 
 [Packages]
   MdePkg/MdePkg.dec
diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index 93fc63bf93e3..b7275db3d564 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -7,6 +7,7 @@
 **/
 
 #include "MpLib.h"
+#include "MpIntelTdx.h"
 
 #include <Library/UefiLib.h>
 #include <Library/UefiBootServicesTableLib.h>
@@ -15,7 +16,6 @@
 #include <Library/VmgExitLib.h>
 #include <Register/Amd/Fam17Msr.h>
 #include <Register/Amd/Ghcb.h>
-
 #include <Protocol/Timer.h>
 
 #define  AP_SAFE_STACK_SIZE    128
@@ -801,6 +801,10 @@ MpInitLibStartupThisAP (  {
   EFI_STATUS              Status;
 
+  if (MpTdxIsEnabled ()) {
+    return EFI_UNSUPPORTED;
+  }
+
   //
   // temporarily stop checkAllApsStatus for avoid resource dead-lock.
   //
@@ -857,6 +861,10 @@ MpInitLibSwitchBSP (
   EFI_TIMER_ARCH_PROTOCOL      *Timer;
   UINT64                       TimerPeriod;
 
+  if (MpTdxIsEnabled ()) {
+    return EFI_UNSUPPORTED;
+  }
+
   TimerPeriod = 0;
   //
   // Locate Timer Arch Protocol
@@ -930,6 +938,10 @@ MpInitLibEnableDisableAP (
   EFI_STATUS     Status;
   BOOLEAN        TempStopCheckState;
 
+  if (MpTdxIsEnabled ()) {
+    return EFI_UNSUPPORTED;
+  }
+
   TempStopCheckState = FALSE;
   //
   // temporarily stop checkAllAPsStatus for initialize parameters.
diff --git a/UefiCpuPkg/Library/MpInitLib/MpIntelTdx.h b/UefiCpuPkg/Library/MpInitLib/MpIntelTdx.h
new file mode 100644
index 000000000000..59bd739eed22
--- /dev/null
+++ b/UefiCpuPkg/Library/MpInitLib/MpIntelTdx.h
@@ -0,0 +1,107 @@
+/** @file
+  Intel Tdx header file.
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef MP_INTEL_TDX_H_
+#define MP_INTEL_TDX_H_
+
+#include <PiPei.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Uefi/UefiBaseType.h>
+#include <Protocol/MpService.h>
+
+/**
+  Gets detailed MP-related information on the requested processor at 
+the
+  instant this call is made. This service may only be called from the BSP.
+
+  @param[in]  ProcessorNumber       The handle number of processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where information for
+                                    the requested processor is deposited.
+  @param[out]  HealthData            Return processor health data.
+
+  @retval EFI_SUCCESS             Processor information was returned.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist in the platform.
+  @retval EFI_NOT_READY           MP Initialize Library is not initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+TdxMpInitLibGetProcessorInfo (
+  IN  UINTN                      ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer,
+  OUT EFI_HEALTH_FLAGS           *HealthData  OPTIONAL
+  );
+
+/**
+  Retrieves the number of logical processor in the platform and the 
+number of
+  those logical processors that are enabled on this boot. This service 
+may only
+  be called from the BSP.
+
+  @param[out] NumberOfProcessors          Pointer to the total number of logical
+                                          processors in the system, including the BSP
+                                          and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled logical
+                                          processors that exist in system, including
+                                          the BSP.
+
+  @retval EFI_SUCCESS             The number of logical processors and enabled
+                                  logical processors was retrieved.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL and NumberOfEnabledProcessors
+                                  is NULL.
+  @retval EFI_NOT_READY           MP Initialize Library is not initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+TdxMpInitLibGetNumberOfProcessors (
+  OUT UINTN                     *NumberOfProcessors,       OPTIONAL
+  OUT UINTN                     *NumberOfEnabledProcessors OPTIONAL
+  );
+
+/**
+  The TDCALL instruction causes a VM exit to the Intel TDX module.  It 
+is
+  used to call guest-side Intel TDX functions, either local or a TD 
+exit
+  to the host VMM, as selected by Leaf.
+
+  @param[in]      Leaf        Leaf number of TDCALL instruction
+  @param[in]      Arg1        Arg1
+  @param[in]      Arg2        Arg2
+  @param[in]      Arg3        Arg3
+  @param[in,out]  Results  Returned result of the Leaf function
+
+  @return EFI_SUCCESS
+  @return Other           See individual leaf functions
+**/
+EFI_STATUS
+EFIAPI
+MpTdCall (
+  IN UINT64           Leaf,
+  IN UINT64           Arg1,
+  IN UINT64           Arg2,
+  IN UINT64           Arg3,
+  IN OUT VOID         *Results
+  );
+
+/**
+  Whether Intel TDX is enabled.
+
+  @return TRUE    TDX enabled
+  @return FALSE   TDX not enabled
+**/
+BOOLEAN
+EFIAPI
+MpTdxIsEnabled (
+  VOID
+  );
+
+#endif
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index b9a06747edbf..d03ad8ff483e 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -9,6 +9,7 @@
 **/
 
 #include "MpLib.h"
+#include "MpIntelTdx.h"
 #include <Library/VmgExitLib.h>
 #include <Register/Amd/Fam17Msr.h>
 #include <Register/Amd/Ghcb.h>
@@ -1965,6 +1966,10 @@ MpInitLibInitialize (
   UINTN                    BackupBufferAddr;
   UINTN                    ApIdtBase;
 
+  if (MpTdxIsEnabled ()) {
+    return EFI_SUCCESS;
+  }
+
   OldCpuMpData = GetCpuMpDataFromGuidedHob ();
   if (OldCpuMpData == NULL) {
     MaxLogicalProcessorNumber = PcdGet32(PcdCpuMaxLogicalProcessorNumber);
@@ -2215,6 +2220,10 @@ MpInitLibGetProcessorInfo (
   CPU_INFO_IN_HOB        *CpuInfoInHob;
   UINTN                  OriginalProcessorNumber;
 
+  if (MpTdxIsEnabled ()) {
+    return TdxMpInitLibGetProcessorInfo (ProcessorNumber, 
+ ProcessorInfoBuffer, HealthData);  }
+
   CpuMpData = GetCpuMpData ();
   CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
 
@@ -2446,6 +2455,10 @@ EnableDisableApWorker (
   CPU_MP_DATA               *CpuMpData;
   UINTN                     CallerNumber;
 
+  if (MpTdxIsEnabled ()) {
+    return EFI_UNSUPPORTED;
+  }
+
   CpuMpData = GetCpuMpData ();
 
   //
@@ -2506,6 +2519,11 @@ MpInitLibWhoAmI (
     return EFI_INVALID_PARAMETER;
   }
 
+  if (MpTdxIsEnabled ()) {
+    *ProcessorNumber = 0;
+    return EFI_SUCCESS;
+  }
+
   CpuMpData = GetCpuMpData ();
 
   return GetProcessorNumber (CpuMpData, ProcessorNumber); @@ -2544,6 +2562,10 @@ MpInitLibGetNumberOfProcessors (
   UINTN                   EnabledProcessorNumber;
   UINTN                   Index;
 
+  if (MpTdxIsEnabled ()) {
+    return TdxMpInitLibGetNumberOfProcessors(NumberOfProcessors, 
+ NumberOfEnabledProcessors);  }
+
   CpuMpData = GetCpuMpData ();
 
   if ((NumberOfProcessors == NULL) && (NumberOfEnabledProcessors == NULL)) { @@ -2629,6 +2651,10 @@ StartupAllCPUsWorker (
   BOOLEAN                 HasEnabledAp;
   CPU_STATE               ApState;
 
+  if (MpTdxIsEnabled ()) {
+    return EFI_SUCCESS;
+  }
+
   CpuMpData = GetCpuMpData ();
 
   if (FailedCpuList != NULL) {
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLibTdx.c b/UefiCpuPkg/Library/MpInitLib/MpLibTdx.c
new file mode 100644
index 000000000000..d8bd4eb65d25
--- /dev/null
+++ b/UefiCpuPkg/Library/MpInitLib/MpLibTdx.c
@@ -0,0 +1,186 @@
+/** @file
+  CPU MP Initialize Library common functions.
+
+  Copyright (c) 2016 - 2020, Intel Corporation. All rights 
+ reserved.<BR>  Copyright (c) 2020, AMD Inc. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include "MpLib.h"
+#include "MpIntelTdx.h"
+#include <Library/BaseLib.h>
+#include <Library/PcdLib.h>
+#include <Library/DebugLib.h>
+#include <IndustryStandard/Tdx.h>
+
+BOOLEAN mMpTdxEnabled = FALSE;
+BOOLEAN mMpTdxProbed = FALSE;
+
+/**
+  Gets detailed MP-related information on the requested processor at 
+the
+  instant this call is made. This service may only be called from the BSP.
+
+  @param[in]  ProcessorNumber       The handle number of processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where information for
+                                    the requested processor is deposited.
+  @param[out]  HealthData            Return processor health data.
+
+  @retval EFI_SUCCESS             Processor information was returned.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist in the platform.
+  @retval EFI_NOT_READY           MP Initialize Library is not initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+TdxMpInitLibGetProcessorInfo (
+  IN  UINTN                      ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer,
+  OUT EFI_HEALTH_FLAGS           *HealthData  OPTIONAL
+  )
+{
+  EFI_STATUS              Status;
+  TD_RETURN_DATA          TdReturnData;
+
+  if (ProcessorInfoBuffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = MpTdCall(TDCALL_TDINFO, 0, 0, 0, &TdReturnData);  
+ ASSERT(Status == EFI_SUCCESS);
+
+  if (ProcessorNumber >= TdReturnData.TdInfo.NumVcpus) {
+    return EFI_NOT_FOUND;
+  }
+
+  ProcessorInfoBuffer->ProcessorId = ProcessorNumber;  
+ ProcessorInfoBuffer->StatusFlag  = 0;  if (ProcessorNumber == 0) {
+    ProcessorInfoBuffer->StatusFlag |= PROCESSOR_AS_BSP_BIT;  }  
+ ProcessorInfoBuffer->StatusFlag |= PROCESSOR_ENABLED_BIT;
+
+  //
+  // Get processor location information  //  
+ GetProcessorLocationByApicId (
+    (UINT32)ProcessorNumber,
+    &ProcessorInfoBuffer->Location.Package,
+    &ProcessorInfoBuffer->Location.Core,
+    &ProcessorInfoBuffer->Location.Thread
+    );
+
+  if (HealthData != NULL) {
+    HealthData->Uint32 = 0;
+  }
+
+  return Status;
+}
+
+/**
+  Retrieves the number of logical processor in the platform and the 
+number of
+  those logical processors that are enabled on this boot. This service 
+may only
+  be called from the BSP.
+
+  @param[out] NumberOfProcessors          Pointer to the total number of logical
+                                          processors in the system, including the BSP
+                                          and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled logical
+                                          processors that exist in system, including
+                                          the BSP.
+
+  @retval EFI_SUCCESS             The number of logical processors and enabled
+                                  logical processors was retrieved.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL and NumberOfEnabledProcessors
+                                  is NULL.
+  @retval EFI_NOT_READY           MP Initialize Library is not initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+TdxMpInitLibGetNumberOfProcessors (
+  OUT UINTN                     *NumberOfProcessors,       OPTIONAL
+  OUT UINTN                     *NumberOfEnabledProcessors OPTIONAL
+  )
+{
+  EFI_STATUS              Status;
+  TD_RETURN_DATA          TdReturnData;
+
+  if ((NumberOfProcessors == NULL) && (NumberOfEnabledProcessors == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = MpTdCall(TDCALL_TDINFO, 0, 0, 0, &TdReturnData);  
+ ASSERT(Status == EFI_SUCCESS);
+
+  if (NumberOfProcessors != NULL) {
+    *NumberOfProcessors = TdReturnData.TdInfo.NumVcpus;  }  if 
+ (NumberOfEnabledProcessors != NULL) {
+    *NumberOfEnabledProcessors = TdReturnData.TdInfo.MaxVcpus;  }
+
+  return Status;
+}
+
+/**
+  Whether Intel TDX is enabled.
+
+  @return TRUE    TDX enabled
+  @return FALSE   TDX not enabled
+**/
+BOOLEAN
+EFIAPI
+MpTdxIsEnabled (
+  VOID
+  )
+{
+  UINT32    Eax;
+  UINT32    Ebx;
+  UINT32    Ecx;
+  UINT32    Edx;
+  UINT32    LargestEax;
+
+  if (mMpTdxProbed) {
+    return mMpTdxEnabled;
+  }
+
+  mMpTdxEnabled = FALSE;
+
+  do {
+    AsmCpuid (0, &LargestEax, &Ebx, &Ecx, &Edx);
+
+    if (Ebx != SIGNATURE_32 ('G', 'e', 'n', 'u')
+      || Edx != SIGNATURE_32 ('i', 'n', 'e', 'I')
+      || Ecx != SIGNATURE_32 ('n', 't', 'e', 'l')) {
+      break;
+    }
+
+    AsmCpuid (1, NULL, NULL, &Ecx, NULL);
+    if ((Ecx & BIT31) == 0) {
+      break;
+    }
+
+    if (LargestEax < 0x21) {
+      break;
+    }
+
+    AsmCpuidEx (0x21, 0, &Eax, &Ebx, &Ecx, &Edx);
+    if (Ebx != SIGNATURE_32 ('I', 'n', 't', 'e')
+      || Edx != SIGNATURE_32 ('l', 'T', 'D', 'X')
+      || Ecx != SIGNATURE_32 (' ', ' ', ' ', ' ')) {
+      break;
+    }
+
+    mMpTdxEnabled = TRUE;
+  }while (FALSE);
+
+  mMpTdxProbed = TRUE;
+
+  return mMpTdxEnabled;
+}
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLibTdxNull.c b/UefiCpuPkg/Library/MpInitLib/MpLibTdxNull.c
new file mode 100644
index 000000000000..f9cfedf01240
--- /dev/null
+++ b/UefiCpuPkg/Library/MpInitLib/MpLibTdxNull.c
@@ -0,0 +1,117 @@
+/** @file
+  CPU MP Initialize Library common functions.
+
+  Copyright (c) 2016 - 2020, Intel Corporation. All rights 
+ reserved.<BR>  Copyright (c) 2020, AMD Inc. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include "MpLib.h"
+#include "MpIntelTdx.h"
+#include <Library/DebugLib.h>
+
+/**
+  Gets detailed MP-related information on the requested processor at 
+the
+  instant this call is made. This service may only be called from the BSP.
+
+  @param[in]  ProcessorNumber       The handle number of processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where information for
+                                    the requested processor is deposited.
+  @param[out]  HealthData            Return processor health data.
+
+  @retval EFI_SUCCESS             Processor information was returned.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist in the platform.
+  @retval EFI_NOT_READY           MP Initialize Library is not initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+TdxMpInitLibGetProcessorInfo (
+  IN  UINTN                      ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer,
+  OUT EFI_HEALTH_FLAGS           *HealthData  OPTIONAL
+  )
+{
+  ASSERT (FALSE);
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Retrieves the number of logical processor in the platform and the 
+number of
+  those logical processors that are enabled on this boot. This service 
+may only
+  be called from the BSP.
+
+  @param[out] NumberOfProcessors          Pointer to the total number of logical
+                                          processors in the system, including the BSP
+                                          and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled logical
+                                          processors that exist in system, including
+                                          the BSP.
+
+  @retval EFI_SUCCESS             The number of logical processors and enabled
+                                  logical processors was retrieved.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL and NumberOfEnabledProcessors
+                                  is NULL.
+  @retval EFI_NOT_READY           MP Initialize Library is not initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+TdxMpInitLibGetNumberOfProcessors (
+  OUT UINTN                     *NumberOfProcessors,       OPTIONAL
+  OUT UINTN                     *NumberOfEnabledProcessors OPTIONAL
+  )
+{
+  ASSERT (FALSE);
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Whether Intel TDX is enabled.
+
+  @return TRUE    TDX enabled
+  @return FALSE   TDX not enabled
+**/
+BOOLEAN
+EFIAPI
+MpTdxIsEnabled (
+  VOID
+  )
+{
+  return FALSE;
+}
+
+/**
+  The TDCALL instruction causes a VM exit to the Intel TDX module.  It 
+is
+  used to call guest-side Intel TDX functions, either local or a TD 
+exit
+  to the host VMM, as selected by Leaf.
+  Leaf functions are described at <https://software.intel.com/content/
+  www/us/en/develop/articles/intel-trust-domain-extensions.html>
+
+  @param[in]      Leaf        Leaf number of TDCALL instruction
+  @param[in]      Arg1        Arg1
+  @param[in]      Arg2        Arg2
+  @param[in]      Arg3        Arg3
+  @param[in,out]  Results  Returned result of the Leaf function
+
+  @return EFI_SUCCESS
+  @return Other           See individual leaf functions
+**/
+EFI_STATUS
+EFIAPI
+MpTdCall (
+  IN UINT64           Leaf,
+  IN UINT64           Arg1,
+  IN UINT64           Arg2,
+  IN UINT64           Arg3,
+  IN OUT VOID         *Results
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf b/UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf
index 36fcb96b5852..b48dab2b1537 100644
--- a/UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf
+++ b/UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf
@@ -22,10 +22,13 @@
 #
 
 [Sources.IA32]
+  MpLibTdxNull.c
   Ia32/MpFuncs.nasm
 
 [Sources.X64]
+  MpLibTdx.c
   X64/MpFuncs.nasm
+  X64/IntelTdcall.nasm
 
 [Sources.common]
   MpEqu.inc
@@ -33,6 +36,7 @@
   MpLib.c
   MpLib.h
   Microcode.c
+  MpIntelTdx.h
 
 [Packages]
   MdePkg/MdePkg.dec
diff --git a/UefiCpuPkg/Library/MpInitLib/X64/IntelTdcall.nasm b/UefiCpuPkg/Library/MpInitLib/X64/IntelTdcall.nasm
new file mode 100644
index 000000000000..5e98557d5590
--- /dev/null
+++ b/UefiCpuPkg/Library/MpInitLib/X64/IntelTdcall.nasm
@@ -0,0 +1,120 @@
+;----------------------------------------------------------------------
+--------
+;*
+;* Copyright (c) 2020 - 2021, Intel Corporation. All rights 
+reserved.<BR>
+;* SPDX-License-Identifier: BSD-2-Clause-Patent
+;*
+;*
+;----------------------------------------------------------------------
+--------
+
+DEFAULT REL
+SECTION .text
+
+%macro tdcall 0
+    db 0x66,0x0f,0x01,0xcc
+%endmacro
+
+%macro tdcall_push_regs 0
+    push rbp
+    mov  rbp, rsp
+    push r15
+    push r14
+    push r13
+    push r12
+    push rbx
+    push rsi
+    push rdi
+%endmacro
+
+%macro tdcall_pop_regs 0
+    pop rdi
+    pop rsi
+    pop rbx
+    pop r12
+    pop r13
+    pop r14
+    pop r15
+    pop rbp
+%endmacro
+
+%define number_of_regs_pushed 8
+%define number_of_parameters  4
+
+;
+; Keep these in sync for push_regs/pop_regs, code below ; uses them to 
+find 5th or greater parameters ; %define first_variable_on_stack_offset 
+\
+  ((number_of_regs_pushed * 8) + (number_of_parameters * 8) + 8) 
+%define second_variable_on_stack_offset \
+  ((first_variable_on_stack_offset) + 8)
+
+%macro tdcall_regs_preamble 2
+    mov rax, %1
+
+    mov ecx, %2
+
+    ; R10 = 0 (standard TDVMCALL)
+
+    xor r10d, r10d
+
+    ; Zero out unused (for standard TDVMCALL) registers to avoid leaking
+    ; secrets to the VMM.
+
+    xor ebx, ebx
+    xor esi, esi
+    xor edi, edi
+
+    xor edx, edx
+    xor ebp, ebp
+    xor r8d, r8d
+    xor r9d, r9d
+%endmacro
+
+%macro tdcall_regs_postamble 0
+    xor ebx, ebx
+    xor esi, esi
+    xor edi, edi
+
+    xor ecx, ecx
+    xor edx, edx
+    xor r8d,  r8d
+    xor r9d,  r9d
+    xor r10d, r10d
+    xor r11d, r11d
+%endmacro
+
+;  MpTdCall (
+;    UINT64  Leaf,    // Rcx
+;    UINT64  P1,      // Rdx
+;    UINT64  P2,      // R8
+;    UINT64  P3,      // R9
+;    UINT64  Results, // rsp + 0x28
+;    )
+global ASM_PFX(MpTdCall)
+ASM_PFX(MpTdCall):
+       tdcall_push_regs
+
+       mov rax, rcx
+       mov rcx, rdx
+       mov rdx, r8
+       mov r8, r9
+
+       tdcall
+
+       ; exit if tdcall reports failure.
+       test rax, rax
+       jnz .exit
+
+       ; test if caller wanted results
+       mov r12, [rsp + first_variable_on_stack_offset ]
+       test r12, r12
+       jz .exit
+       mov [r12 + 0 ], rcx
+       mov [r12 + 8 ], rdx
+       mov [r12 + 16], r8
+       mov [r12 + 24], r9
+       mov [r12 + 32], r10
+       mov [r12 + 40], r11
+.exit:
+       tdcall_pop_regs
+       ret
--
2.29.2.windows.2