[Patch V2 37/40] TigerlakeSiliconPkg/SystemAgent: Add SystemAgent modules

["Heng Luo" <heng.luo@intel.com>]

REF: https://bugzilla.tianocore.org/show_bug.cgi?id=3171

Adds the following files:
  * SystemAgent/SaInit/Dxe
  * SystemAgent/SaInit/Smm

Cc: Sai Chaganty <rangasai.v.chaganty@intel.com>
Cc: Nate DeSimone <nathaniel.l.desimone@intel.com>
Signed-off-by: Heng Luo <heng.luo@intel.com>
---
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaAcpi.c          | 431 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.c          | 120 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.h          |  58 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.c       | 181 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.h       | 136 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.inf     | 117 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/CpuPcieSmm.c      | 454 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.c   | 112 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.h   | 122 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.inf |  72 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 10 files changed, 1803 insertions(+)

diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaAcpi.c b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaAcpi.c
new file mode 100644
index 0000000000..d84a0c1fa4
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaAcpi.c
@@ -0,0 +1,431 @@
+/** @file
+  This is the driver that initializes the Intel System Agent.
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#include "SaInitDxe.h"
+#include "SaInit.h"
+#include <SaConfigHob.h>
+#include <Protocol/SaNvsArea.h>
+#include <Library/PchInfoLib.h>
+#include <CpuPcieInfo.h>
+#include <Library/DxeCpuPcieRpLib.h>
+#include <SaConfigHob.h>
+#include <CpuPcieHob.h>
+#include <HostBridgeDataHob.h>
+#include <CpuDataStruct.h>
+
+///
+/// Global Variables
+///
+GLOBAL_REMOVE_IF_UNREFERENCED SYSTEM_AGENT_NVS_AREA_PROTOCOL  mSaNvsAreaProtocol;
+GLOBAL_REMOVE_IF_UNREFERENCED SA_POLICY_PROTOCOL              *mSaPolicy;
+extern SA_CONFIG_HOB                                          *mSaConfigHob;
+
+/**
+  A protocol callback which updates 64bits MMIO Base and Length in SA GNVS area
+**/
+VOID
+UpdateSaGnvsForMmioResourceBaseLength (
+  VOID
+  )
+{
+  EFI_PHYSICAL_ADDRESS      PciBaseAddress;
+  UINT32                    Tolud;
+  UINT64                    Length;
+  UINT64                    McD0BaseAddress;
+  UINTN                     ResMemLimit1;
+  UINT8                     EnableAbove4GBMmioBiosAssignemnt;
+  HOST_BRIDGE_DATA_HOB      *HostBridgeDataHob;
+
+  PciBaseAddress = 0;
+  Tolud = 0;
+  Length = 0;
+  ResMemLimit1 = 0;
+  EnableAbove4GBMmioBiosAssignemnt = 0;
+  HostBridgeDataHob = NULL;
+  //
+  // Read memory map registers
+  //
+  McD0BaseAddress        = PCI_SEGMENT_LIB_ADDRESS (SA_SEG_NUM, SA_MC_BUS, 0, 0, 0);
+  Tolud                  = PciSegmentRead32 (McD0BaseAddress + R_SA_TOLUD) & B_SA_TOLUD_TOLUD_MASK;
+  PciBaseAddress         = Tolud;
+
+  ResMemLimit1 = (UINTN) PcdGet64 (PcdSiPciExpressBaseAddress);
+
+  Length = ResMemLimit1 - PciBaseAddress;
+
+  //
+  // Get HostBridgeData HOB and see if above 4GB MMIO BIOS assignment enabled
+  //
+  HostBridgeDataHob = (HOST_BRIDGE_DATA_HOB *) GetFirstGuidHob (&gHostBridgeDataHobGuid);
+  if ((HostBridgeDataHob != NULL) && (HostBridgeDataHob->EnableAbove4GBMmio == 1)) {
+    EnableAbove4GBMmioBiosAssignemnt = 1;
+  }
+
+  //
+  // Enable Above 4GB MMIO when Aperture Size is 2GB or higher
+  //
+  if ((mSaConfigHob != NULL) && (mSaConfigHob->ApertureSize >= 15)) {
+    EnableAbove4GBMmioBiosAssignemnt = 1;
+  }
+
+  //
+  // Check Enable Above 4GB MMIO or not
+  //
+
+  DEBUG ((DEBUG_INFO, "Update SA GNVS Area.\n"));
+  mSaNvsAreaProtocol.Area->Mmio32Base   = (UINT32) PciBaseAddress;
+  mSaNvsAreaProtocol.Area->Mmio32Length = (UINT32) Length;
+  if (EnableAbove4GBMmioBiosAssignemnt == 1) {
+    mSaNvsAreaProtocol.Area->Mmio64Base   = BASE_256GB;
+    mSaNvsAreaProtocol.Area->Mmio64Length = SIZE_256GB;
+  }
+  DEBUG ((DEBUG_INFO, "SaNvsAreaProtocol.Area->Mmio64Base = %lx\n", mSaNvsAreaProtocol.Area->Mmio64Base));
+  DEBUG ((DEBUG_INFO, "SaNvsAreaProtocol.Area->Mmio64Length = %lx\n", mSaNvsAreaProtocol.Area->Mmio64Length));
+  DEBUG ((DEBUG_INFO, "SaNvsAreaProtocol.Area->Mmio32Base = %lx\n", mSaNvsAreaProtocol.Area->Mmio32Base));
+  DEBUG ((DEBUG_INFO, "SaNvsAreaProtocol.Area->Mmio32Length = %lx\n", mSaNvsAreaProtocol.Area->Mmio32Length));
+}
+
+/**
+  Install SSDT Table
+
+  @retval EFI_SUCCESS - SSDT Table load successful.
+**/
+EFI_STATUS
+InstallSsdtAcpiTable (
+  IN GUID   SsdtTableGuid,
+  IN UINT64 Signature
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_HANDLE                    *HandleBuffer;
+  BOOLEAN                       LoadTable;
+  UINTN                         NumberOfHandles;
+  UINTN                         Index;
+  INTN                          Instance;
+  UINTN                         Size;
+  UINT32                        FvStatus;
+  UINTN                         TableHandle;
+  EFI_FV_FILETYPE               FileType;
+  EFI_FV_FILE_ATTRIBUTES        Attributes;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL *FwVol;
+  EFI_ACPI_TABLE_PROTOCOL       *AcpiTable;
+  EFI_ACPI_DESCRIPTION_HEADER   *TableHeader;
+  EFI_ACPI_COMMON_HEADER        *Table;
+
+  FwVol         = NULL;
+  Table         = NULL;
+
+  DEBUG ((DEBUG_INFO, "Loading SSDT Table GUID: %g\n", SsdtTableGuid));
+
+  ///
+  /// Locate FV protocol.
+  ///
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolume2ProtocolGuid,
+                  NULL,
+                  &NumberOfHandles,
+                  &HandleBuffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Look for FV with ACPI storage file
+  ///
+  for (Index = 0; Index < NumberOfHandles; Index++) {
+    ///
+    /// Get the protocol on this handle
+    /// This should not fail because of LocateHandleBuffer
+    ///
+    Status = gBS->HandleProtocol (
+                    HandleBuffer[Index],
+                    &gEfiFirmwareVolume2ProtocolGuid,
+                    (VOID **) &FwVol
+                    );
+    ASSERT_EFI_ERROR (Status);
+    if (FwVol == NULL) {
+      return EFI_NOT_FOUND;
+    }
+    ///
+    /// See if it has the ACPI storage file
+    ///
+    Size      = 0;
+    FvStatus  = 0;
+    Status = FwVol->ReadFile (
+                      FwVol,
+                      &SsdtTableGuid,
+                      NULL,
+                      &Size,
+                      &FileType,
+                      &Attributes,
+                      &FvStatus
+                      );
+
+    ///
+    /// If we found it, then we are done
+    ///
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+  }
+  ///
+  /// Our exit status is determined by the success of the previous operations
+  /// If the protocol was found, Instance already points to it.
+  ///
+  ///
+  /// Free any allocated buffers
+  ///
+  FreePool (HandleBuffer);
+
+  ///
+  /// Sanity check that we found our data file
+  ///
+  ASSERT (FwVol);
+
+  ///
+  /// Locate ACPI tables
+  ///
+  Status = gBS->LocateProtocol (&gEfiAcpiTableProtocolGuid, NULL, (VOID **) &AcpiTable);
+
+  ///
+  /// Read tables from the storage file.
+  ///
+  if (FwVol == NULL) {
+    ASSERT_EFI_ERROR (EFI_NOT_FOUND);
+    return EFI_NOT_FOUND;
+  }
+  Instance = 0;
+
+  while (Status == EFI_SUCCESS) {
+    ///
+    /// Read the ACPI tables
+    ///
+    Status = FwVol->ReadSection (
+                      FwVol,
+                      &SsdtTableGuid,
+                      EFI_SECTION_RAW,
+                      Instance,
+                      (VOID **) &Table,
+                      &Size,
+                      &FvStatus
+                      );
+    if (!EFI_ERROR (Status)) {
+      ///
+      /// check and load HybridGraphics SSDT table
+      ///
+      LoadTable   = FALSE;
+      TableHeader = (EFI_ACPI_DESCRIPTION_HEADER *) Table;
+
+      if (((EFI_ACPI_DESCRIPTION_HEADER *) TableHeader)->OemTableId == Signature) {
+        ///
+        /// This is the SSDT table that match the Signature
+        ///
+        DEBUG ((DEBUG_INFO, "Found out SSDT Table GUID: %g\n", SsdtTableGuid));
+        LoadTable = TRUE;
+      }
+
+      ///
+      /// Add the table
+      ///
+      if (LoadTable) {
+        TableHandle = 0;
+        Status = AcpiTable->InstallAcpiTable (
+                              AcpiTable,
+                              TableHeader,
+                              TableHeader->Length,
+                              &TableHandle
+                              );
+      }
+      ///
+      /// Increment the instance
+      ///
+      Instance++;
+      Table = NULL;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function gets registered as a callback to perform Dmar Igd
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+SaAcpiEndOfDxeCallback (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  EFI_STATUS          Status;
+
+  if (PciSegmentRead16 (PCI_SEGMENT_LIB_ADDRESS (SA_SEG_NUM, IGD_BUS_NUM, IGD_DEV_NUM, IGD_FUN_NUM, PCI_VENDOR_ID_OFFSET)) != 0xFFFF) {
+    Status = PostPmInitEndOfDxe ();
+    if (EFI_SUCCESS != Status) {
+      DEBUG ((DEBUG_WARN, "[SA] EndOfDxe GraphicsInit Error, Status = %r \n", Status));
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  if (PciSegmentRead16 (PCI_SEGMENT_LIB_ADDRESS (SA_SEG_NUM, IGD_BUS_NUM, IGD_DEV_NUM, IGD_FUN_NUM, PCI_VENDOR_ID_OFFSET)) != 0xFFFF) {
+    Status = GetVBiosVbtEndOfDxe ();
+    if (EFI_SUCCESS != Status) {
+      DEBUG ((DEBUG_WARN, "[SA] EndOfDxe Op Region Error, Status = %r \n", Status));
+    }
+
+    Status = UpdateIgdOpRegionEndOfDxe ();
+    if (EFI_SUCCESS != Status) {
+      DEBUG ((DEBUG_WARN, "[SA] EndOfDxe Update Op Region Error, Status = %r \n", Status));
+    }
+  }
+
+  return;
+}
+
+/**
+  SystemAgent Acpi Initialization.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+
+  @retval EFI_SUCCESS             The function completed successfully
+  @retval EFI_OUT_OF_RESOURCES    No enough buffer to allocate
+**/
+EFI_STATUS
+EFIAPI
+SaAcpiInit (
+  IN EFI_HANDLE         ImageHandle
+  )
+{
+  EFI_STATUS                Status;
+  EFI_CPUID_REGISTER        CpuidRegs;
+  EFI_EVENT                 EndOfDxeEvent;
+
+  CPU_PCIE_HOB              *CpuPcieHob;
+
+  AsmCpuid (1, &CpuidRegs.RegEax, 0, 0, 0);
+  ///
+  /// Get the platform setup policy.
+  ///
+  Status = gBS->LocateProtocol (&gSaPolicyProtocolGuid, NULL, (VOID **) &mSaPolicy);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Install System Agent Global NVS protocol
+  ///
+  DEBUG ((DEBUG_INFO, "Install SA GNVS protocol\n"));
+  Status = (gBS->AllocatePool) (EfiACPIMemoryNVS, sizeof (SYSTEM_AGENT_NVS_AREA), (VOID **) &mSaNvsAreaProtocol.Area);
+  ASSERT_EFI_ERROR (Status);
+  ZeroMem ((VOID *) mSaNvsAreaProtocol.Area, sizeof (SYSTEM_AGENT_NVS_AREA));
+  mSaNvsAreaProtocol.Area->XPcieCfgBaseAddress  = (UINT32) (PcdGet64 (PcdSiPciExpressBaseAddress));
+  mSaNvsAreaProtocol.Area->CpuIdInfo            = CpuidRegs.RegEax;
+
+  ///
+  /// Get CpuPcieHob HOB
+  ///
+  CpuPcieHob = NULL;
+  CpuPcieHob = (CPU_PCIE_HOB *) GetFirstGuidHob (&gCpuPcieHobGuid);
+  if (CpuPcieHob == NULL) {
+    DEBUG((DEBUG_ERROR, "CpuPcieHob not found\n"));
+    // @todo: Will add it back once it will get add into NVS library since currently it is failing for JSL
+    //ASSERT(CpuPcieHob != NULL);
+    //return EFI_NOT_FOUND;
+  } else {
+    mSaNvsAreaProtocol.Area->SlotSelection = CpuPcieHob->SlotSelection;
+    DEBUG((DEBUG_INFO, "RpEnabledMask == %x\n", CpuPcieHob->RpEnabledMask));
+    if (CpuPcieHob->RpEnabledMask == 0) {
+      DEBUG ((DEBUG_ERROR, "All CPU PCIe root ports are disabled!!\n"));
+    } else {
+      if (CpuPcieHob->RpEnabledMask & BIT0) {
+        mSaNvsAreaProtocol.Area->CpuPcieRp0Enable = 1;
+      }
+      if (CpuPcieHob->RpEnabledMask & BIT1) {
+        mSaNvsAreaProtocol.Area->CpuPcieRp1Enable = 1;
+      }
+      if (CpuPcieHob->RpEnabledMask & BIT2) {
+        mSaNvsAreaProtocol.Area->CpuPcieRp2Enable = 1;
+      }
+      if (CpuPcieHob->RpEnabledMask & BIT3) {
+        mSaNvsAreaProtocol.Area->CpuPcieRp3Enable = 1;
+      }
+    }
+    mSaNvsAreaProtocol.Area->MaxPegPortNumber = GetMaxCpuPciePortNum ();
+  }
+
+  mSaNvsAreaProtocol.Area->SimicsEnvironment = 0;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ImageHandle,
+                  &gSaNvsAreaProtocolGuid,
+                  &mSaNvsAreaProtocol,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// GtPostInit Initialization
+  ///
+  DEBUG ((DEBUG_INFO, "Initializing GT ACPI tables\n"));
+  GraphicsInit (ImageHandle, mSaPolicy);
+
+  ///
+  /// Audio (dHDA) Initialization
+  ///
+  ///
+  /// Vtd Initialization
+  ///
+  DEBUG ((DEBUG_INFO, "Initializing VT-d ACPI tables\n"));
+  VtdInit (mSaPolicy);
+
+  ///
+  /// IgdOpRegion Install Initialization
+  ///
+  DEBUG ((DEBUG_INFO, "Initializing IGD OpRegion\n"));
+  IgdOpRegionInit ();
+
+  ///
+  /// Register an end of DXE event for SA ACPI to do tasks before invoking any UEFI drivers,
+  /// applications, or connecting consoles,...
+  ///
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  SaAcpiEndOfDxeCallback,
+                  NULL,
+                  &gEfiEndOfDxeEventGroupGuid,
+                  &EndOfDxeEvent
+                  );
+
+  ///
+  /// Install System Agent Global NVS ACPI table
+  ///
+  Status = InstallSsdtAcpiTable (gSaSsdtAcpiTableStorageGuid, SIGNATURE_64 ('S', 'a', 'S', 's', 'd', 't', ' ', 0));
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Update CPU PCIE RP NVS AREA
+  ///
+  UpdateCpuPcieNVS();
+  ///
+  /// Install Intel Graphics SSDT
+  ///
+  Status = InstallSsdtAcpiTable (gGraphicsAcpiTableStorageGuid, SIGNATURE_64 ('I','g','f','x','S','s','d','t'));
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Install IPU SSDT if IPU is present.
+  ///
+  if (PciSegmentRead16 (PCI_SEGMENT_LIB_ADDRESS (SA_SEG_NUM, IPU_BUS_NUM, IPU_DEV_NUM, IPU_FUN_NUM, 0)) != V_SA_DEVICE_ID_INVALID) {
+      Status = InstallSsdtAcpiTable (gIpuAcpiTableStorageGuid, SIGNATURE_64 ('I','p','u','S','s','d','t',0));
+      ASSERT_EFI_ERROR (Status);
+
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.c b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.c
new file mode 100644
index 0000000000..5c0fea422b
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.c
@@ -0,0 +1,120 @@
+/** @file
+  This is the Common driver that initializes the Intel System Agent.
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#include "SaInit.h"
+#include <Library/PciSegmentLib.h>
+#include <HostBridgeDataHob.h>
+#include <SaConfigHob.h>
+#include <Protocol/PciEnumerationComplete.h>
+
+///
+/// Global Variables
+///
+GLOBAL_REMOVE_IF_UNREFERENCED SA_CONFIG_HOB                          *mSaConfigHob;
+BOOLEAN                                                              mSkipPamLock = FALSE;
+
+/*
+  Intel(R) Core Processor Skylake BWG version 0.4.0
+
+  18.6 System Agent Configuration Locking
+   For reliable operation and security, System BIOS must set the following bits:
+   1. For all modern Intel processors, Intel strongly recommends that BIOS should set
+       the D_LCK bit. Set B0:D0:F0.R088h [4] = 1b to lock down SMRAM space.
+  BaseAddr values for mSaSecurityRegisters that uses PciExpressBaseAddress will be initialized at
+  Runtime inside function CpuPcieInitPolicy().
+*/
+GLOBAL_REMOVE_IF_UNREFERENCED BOOT_SCRIPT_REGISTER_SETTING mSaSecurityRegisters[] = {
+  {0,  R_SA_SMRAMC,  0xFFFFFFFF,  BIT4}
+};
+
+/**
+  SystemAgent Initialization Common Function.
+
+  @retval EFI_SUCCESS   - Always.
+**/
+
+VOID
+SaInitEntryPoint (
+  VOID
+  )
+{
+  HOST_BRIDGE_DATA_HOB        *HostBridgeDataHob;
+
+  ///
+  /// Get Host Bridge Data HOB
+  ///
+  HostBridgeDataHob = NULL;
+  HostBridgeDataHob = (HOST_BRIDGE_DATA_HOB *) GetFirstGuidHob (&gHostBridgeDataHobGuid);
+  if (HostBridgeDataHob != NULL) {
+    mSkipPamLock = HostBridgeDataHob->SkipPamLock;
+  }
+  return;
+}
+
+/**
+  This function does SA security lock
+**/
+VOID
+SaSecurityLock (
+  VOID
+  )
+{
+  UINT8           Index;
+  UINT64          BaseAddress;
+  UINT32          RegOffset;
+  UINT32          Data32And;
+  UINT32          Data32Or;
+
+  ///
+  /// 17.2 System Agent Security Lock configuration
+  ///
+  DEBUG ((DEBUG_INFO, "DXE SaSecurityLock\n"));
+  for (Index = 0; Index < (sizeof (mSaSecurityRegisters) / sizeof (BOOT_SCRIPT_REGISTER_SETTING)); Index++) {
+    BaseAddress = mSaSecurityRegisters[Index].BaseAddr;
+    RegOffset   = mSaSecurityRegisters[Index].Offset;
+    Data32And   = mSaSecurityRegisters[Index].AndMask;
+    Data32Or    = mSaSecurityRegisters[Index].OrMask;
+
+    if (RegOffset == R_SA_SMRAMC) {
+      ///
+      /// SMRAMC LOCK must use CF8/CFC access
+      ///
+      PciCf8Or8 (PCI_CF8_LIB_ADDRESS (SA_MC_BUS, SA_MC_DEV, SA_MC_FUN, R_SA_SMRAMC), (UINT8) Data32Or);
+      BaseAddress = S3_BOOT_SCRIPT_LIB_PCI_ADDRESS (SA_MC_BUS, SA_MC_DEV, SA_MC_FUN, R_SA_SMRAMC);
+      S3BootScriptSavePciCfgReadWrite (
+        S3BootScriptWidthUint8,
+        (UINTN) BaseAddress,
+        &Data32Or,
+        &Data32And
+        );
+    }
+  }
+
+}
+
+/**
+  This function performs SA Security locking in EndOfDxe callback
+
+  @retval EFI_SUCCESS     - Security lock has done
+  @retval EFI_UNSUPPORTED - Security lock not done successfully
+**/
+EFI_STATUS
+SaSecurityInit (
+  VOID
+  )
+{
+
+  UINT8                     Index;
+
+  for (Index = 0; Index < (sizeof (mSaSecurityRegisters) / sizeof (BOOT_SCRIPT_REGISTER_SETTING)); Index++) {
+    if (mSaSecurityRegisters[Index].BaseAddr != PcdGet64 (PcdMchBaseAddress)) {
+      mSaSecurityRegisters[Index].BaseAddr = PcdGet64 (PcdSiPciExpressBaseAddress);
+    }
+  }
+  SaSecurityLock ();
+
+  return EFI_SUCCESS;
+}
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.h b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.h
new file mode 100644
index 0000000000..5c8f7dfd5f
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInit.h
@@ -0,0 +1,58 @@
+/** @file
+  Header file for SA Common Initialization Driver.
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#ifndef _SA_INITIALIZATION_DRIVER_H_
+#define _SA_INITIALIZATION_DRIVER_H_
+
+#include <Uefi.h>
+#include <IndustryStandard/Acpi.h>
+#include <IndustryStandard/Pci.h>
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/UefiLib.h>
+#include <Library/IoLib.h>
+#include <Library/PciCf8Lib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/S3BootScriptLib.h>
+#include <Library/SaPlatformLib.h>
+#include <Guid/EventGroup.h>
+#include <Protocol/SaPolicy.h>
+#include <Register/SaRegsHostBridge.h>
+#include <SaConfigHob.h>
+
+extern SA_POLICY_PROTOCOL              *mSaPolicy;
+extern SA_CONFIG_HOB                   *SaConfigHob;
+
+typedef struct {
+  UINT64  BaseAddr;
+  UINT32  Offset;
+  UINT32  AndMask;
+  UINT32  OrMask;
+} BOOT_SCRIPT_REGISTER_SETTING;
+
+/**
+  SystemAgent Initialization Common Function.
+
+  @retval EFI_SUCCESS   - Always.
+**/
+VOID
+SaInitEntryPoint (
+  VOID
+  );
+
+/**
+  This function performs SA Security locking in EndOfDxe callback
+
+  @retval EFI_SUCCESS     - Security lock has done
+  @retval EFI_UNSUPPORTED - Security lock not done successfully
+**/
+EFI_STATUS
+SaSecurityInit (
+  VOID
+  );
+
+#endif
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.c b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.c
new file mode 100644
index 0000000000..2a0e0accf5
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.c
@@ -0,0 +1,181 @@
+/** @file
+  This is the driver that initializes the Intel System Agent.
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#include "SaInitDxe.h"
+#include "SaInit.h"
+#include <SaConfigHob.h>
+#include <Protocol/PciEnumerationComplete.h>
+#include <MemInfoHob.h>
+#include <Protocol/SaIotrapSmi.h>
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINT16                   mPcieIoTrapAddress;
+
+///
+/// Global Variables
+///
+extern SA_CONFIG_HOB         *mSaConfigHob;
+
+
+/**
+  SystemAgent Dxe Initialization.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+  @param[in] SystemTable             Pointer to the EFI System Table
+
+  @retval EFI_SUCCESS             The function completed successfully
+  @retval EFI_OUT_OF_RESOURCES    No enough buffer to allocate
+**/
+EFI_STATUS
+EFIAPI
+SaInitEntryPointDxe (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+  VOID                      *Registration;
+  EFI_EVENT                 ReadyToBoot;
+
+
+  DEBUG ((DEBUG_INFO, "SaInitDxe Start\n"));
+
+  SaInitEntryPoint ();
+
+  Status = SaAcpiInit (ImageHandle);
+  ///
+  /// Create PCI Enumeration Completed callback for CPU PCIe
+  ///
+  EfiCreateProtocolNotifyEvent (
+    &gEfiPciEnumerationCompleteProtocolGuid,
+    TPL_CALLBACK,
+    CpuPciEnumCompleteCallback,
+    NULL,
+    &Registration
+    );
+
+  //
+  // Register a Ready to boot event to config PCIE power management setting after OPROM executed
+  //
+  Status = EfiCreateEventReadyToBootEx (
+             TPL_CALLBACK,
+             SaOnReadyToBoot,
+             NULL,
+             &ReadyToBoot
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  DEBUG ((DEBUG_INFO, "SaInitDxe End\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Do PCIE power management while resume from S3
+**/
+VOID
+ReconfigureCpuPciePowerManagementForS3 (
+  VOID
+  )
+{
+  EFI_STATUS                            Status;
+  UINT32                                Data32;
+  SA_IOTRAP_SMI_PROTOCOL                *CpuPcieIoTrapProtocol;
+
+  Status = gBS->LocateProtocol (&gCpuPcieIoTrapProtocolGuid, NULL, (VOID **) &CpuPcieIoTrapProtocol);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  mPcieIoTrapAddress = CpuPcieIoTrapProtocol->SaIotrapSmiAddress;
+  DEBUG ((DEBUG_INFO, "PcieIoTrapAddress: %0x\n", mPcieIoTrapAddress));
+
+  if (mPcieIoTrapAddress != 0) {
+    //
+    // Save PCH PCIE IoTrap address to re-config PCIE power management setting after resume from S3
+    //
+    Data32 = CpuPciePmTrap;
+    S3BootScriptSaveIoWrite (
+      S3BootScriptWidthUint32,
+      (UINTN) (mPcieIoTrapAddress),
+      1,
+      &Data32
+      );
+  } else {
+    ASSERT (FALSE);
+  }
+}
+
+
+/**
+  SA initialization before boot to OS
+
+  @param[in] Event                A pointer to the Event that triggered the callback.
+  @param[in] Context              A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+SaOnReadyToBoot (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  DEBUG ((DEBUG_INFO, "Uefi SaOnReadyToBoot() Start\n"));
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  //
+  // Trigger an Iotrap SMI to config PCIE power management setting after PCI enumrate is done
+  //
+#if FixedPcdGetBool(PcdCpuPcieEnable) == 1
+  if (mPcieIoTrapAddress != 0) {
+    IoWrite32 ((UINTN) mPcieIoTrapAddress, CpuPciePmTrap);
+  } else {
+    ASSERT (FALSE);
+  }
+#endif
+  DEBUG ((DEBUG_INFO, "Uefi SaOnReadyToBoot() End\n"));
+}
+
+
+/**
+  This function gets registered as a callback to perform CPU PCIe initialization before EndOfDxe
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+CpuPciEnumCompleteCallback (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  EFI_STATUS          Status;
+  VOID                *ProtocolPointer;
+
+  DEBUG ((DEBUG_INFO, "CpuPciEnumCompleteCallback Start\n"));
+  ///
+  /// Check if this is first time called by EfiCreateProtocolNotifyEvent() or not,
+  /// if it is, we will skip it until real event is triggered
+  ///
+  Status = gBS->LocateProtocol (&gEfiPciEnumerationCompleteProtocolGuid, NULL, (VOID **) &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+    return;
+  }
+
+  gBS->CloseEvent (Event);
+
+  ReconfigureCpuPciePowerManagementForS3();
+  //
+  // Routine for update DMAR
+  //
+  UpdateDmarEndOfPcieEnum ();
+
+  UpdateSaGnvsForMmioResourceBaseLength ();
+  DEBUG ((DEBUG_INFO, "CpuPciEnumCompleteCallback End\n"));
+  return;
+}
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.h b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.h
new file mode 100644
index 0000000000..7110d049a8
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.h
@@ -0,0 +1,136 @@
+/** @file
+  Header file for SA Initialization Driver.
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#ifndef _SA_INIT_DXE_DRIVER_H_
+#define _SA_INIT_DXE_DRIVER_H_
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Protocol/AcpiTable.h>
+#include <Library/DxeVtdInitLib.h>
+#include <Library/DxeIgdOpRegionInitLib.h>
+#include <Library/DxeGraphicsInitLib.h>
+#include <Register/SaRegsHostBridge.h>
+
+///
+/// Driver Consumed Protocol Prototypes
+///
+#include <Protocol/SaPolicy.h>
+
+extern EFI_GUID gSaAcpiTableStorageGuid;
+extern EFI_GUID gSaSsdtAcpiTableStorageGuid;
+
+typedef struct {
+  UINT64                Address;
+  EFI_BOOT_SCRIPT_WIDTH Width;
+  UINT32                Value;
+} BOOT_SCRIPT_PCI_REGISTER_SAVE;
+
+///
+/// Function Prototype
+///
+/**
+  This function gets registered as a callback to perform CPU PCIe initialization before ExitPmAuth
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+
+**/
+VOID
+EFIAPI
+CpuPciEnumCompleteCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  <b>System Agent Initialization DXE Driver Entry Point</b>
+  - <b>Introduction</b> \n
+    Based on the information/data in SA_POLICY_PROTOCOL, this module performs further SA initialization in DXE phase,
+    e.g. internal devices enable/disable, SSVID/SID programming, graphic power-management, VT-d, IGD OpRegion initialization.
+    From the perspective of a PCI Express hierarchy, the Broadwell System Agent and PCH together appear as a Root Complex with root ports the number of which depends on how the 8 PCH ports and 4 System Agent PCIe ports are configured [4x1, 2x8, 1x16].
+    There is an internal link (DMI or OPI) that connects the System Agent to the PCH component. This driver includes initialization of SA DMI, PCI Express, SA & PCH Root Complex Topology.
+    For iGFX, this module implements the initialization of the Graphics Technology Power Management from the Broadwell System Agent BIOS Specification and the initialization of the IGD OpRegion/Software SCI - BIOS Specification.
+    The ASL files that go along with the driver define the IGD OpRegion mailboxes in ACPI space and implement the software SCI interrupt mechanism.
+    The IGD OpRegion/Software SCI code serves as a communication interface between system BIOS, ASL, and Intel graphics driver including making a block of customizable data (VBT) from the Intel video BIOS available.
+    Reference Code for the SCI service functions "Get BIOS Data" and "System BIOS Callback" can be found in the ASL files, those functions can be platform specific, the sample provided in the reference code are implemented for Intel CRB.
+    This module implements the VT-d functionality described in the Broadwell System Agent BIOS Specification.
+    This module publishes the LegacyRegion protocol to control the read and write accesses to the Legacy BIOS ranges.
+    E000 and F000 segments are the legacy BIOS ranges and contain pointers to the ACPI regions, SMBIOS tables and so on. This is a private protocol used by Intel Framework.
+    This module registers CallBack function that performs SA security registers lockdown at end of post as required from Broadwell Bios Spec.
+    In addition, this module publishes the SaInfo Protocol with information such as current System Agent reference code version#.
+
+  - @pre
+    - EFI_FIRMWARE_VOLUME_PROTOCOL: Documented in Firmware Volume Specification, available at the URL: http://www.intel.com/technology/framework/spec.htm
+    - SA_POLICY_PROTOCOL: A protocol published by a platform DXE module executed earlier; this is documented in this document as well.
+    - EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL: Documented in the Unified Extensible Firmware Interface Specification, version 2.0, available at the URL: http://www.uefi.org/specs/
+    - EFI_BOOT_SCRIPT_SAVE_PROTOCOL: A protocol published by a platform DXE module executed earlier; refer to the Sample Code section of the Framework PCH Reference Code.
+    - EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL: Documented in the Unified Extensible Firmware Interface Specification, version 2.0, available at the URL: http://www.uefi.org/specs/
+    - EFI_ACPI_TABLE_PROTOCOL : Documented in PI Specification 1.2
+    - EFI_CPU_IO_PROTOCOL: Documented in CPU I/O Protocol Specification, available at the URL: http://www.intel.com/technology/framework/spec.htm
+    - EFI_DATA_HUB_PROTOCOL: Documented in EFI Data Hub Infrastructure Specification, available at the URL: http://www.intel.com/technology/framework/spec.htm
+    - EFI_HII_PROTOCOL (or EFI_HII_DATABASE_PROTOCOL for UEFI 2.1): Documented in Human Interface Infrastructure Specification, available at the URL: http://www.intel.com/technology/framework/spec.htm
+    (For EFI_HII_DATABASE_PROTOCOL, refer to UEFI Specification Version 2.1 available at the URL: http://www.uefi.org/)
+
+  - @result
+    IGD power-management functionality is initialized;  VT-d is initialized (meanwhile, the DMAR table is updated); IGD OpRegion is initialized - IGD_OPREGION_PROTOCOL installed, IGD OpRegion allocated and mailboxes initialized, chipset initialized and ready to generate Software SCI for Internal graphics events. Publishes the SA_INFO_PROTOCOL with current SA reference code version #. Publishes the EFI_LEGACY_REGION_PROTOCOL documented in the Compatibility Support Module Specification, version 0.9, available at the URL: http://www.intel.com/technology/framework/spec.htm
+
+  - <b>References</b> \n
+    IGD OpRegion/Software SCI for Broadwell
+    Advanced Configuration and Power Interface Specification Revision 4.0a.
+
+  - <b>Porting Recommendations</b> \n
+    No modification of the DXE driver should be typically necessary.
+    This driver should be executed after all related devices (audio, video, ME, etc.) are initialized to ensure correct data in DMAR table and DMA-remapping registers.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+  @param[in] SystemTable             Pointer to the EFI System Table
+
+  @retval EFI_SUCCESS             The function completed successfully
+  @retval EFI_OUT_OF_RESOURCES    No enough buffer to allocate
+**/
+EFI_STATUS
+EFIAPI
+SaInitEntryPointDxe (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  );
+
+/**
+  SystemAgent Acpi Initialization.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+
+  @retval EFI_SUCCESS             The function completed successfully
+  @retval EFI_OUT_OF_RESOURCES    No enough buffer to allocate
+**/
+EFI_STATUS
+EFIAPI
+SaAcpiInit (
+  IN EFI_HANDLE         ImageHandle
+  );
+
+/**
+  A protocol callback which updates 64bits MMIO Base and Length in SA GNVS area
+**/
+VOID
+UpdateSaGnvsForMmioResourceBaseLength (
+  VOID
+  );
+
+/**
+  SA initialization before boot to OS
+
+  @param[in] Event                A pointer to the Event that triggered the callback.
+  @param[in] Context              A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+SaOnReadyToBoot (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  );
+
+#endif
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.inf b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.inf
new file mode 100644
index 0000000000..d23ba0fa3b
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Dxe/SaInitDxe.inf
@@ -0,0 +1,117 @@
+## @file
+# Component description file for SystemAgent Initialization driver
+#
+#  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+
+[Defines]
+INF_VERSION = 0x00010017
+BASE_NAME = SaInitDxe
+FILE_GUID = DE23ACEE-CF55-4fb6-AA77-984AB53DE811
+VERSION_STRING = 1.0
+MODULE_TYPE = DXE_DRIVER
+ENTRY_POINT = SaInitEntryPointDxe
+#
+# The following information is for reference only and not required by the build tools.
+#
+# VALID_ARCHITECTURES = IA32 X64 EBC
+#
+
+
+
+[LibraryClasses]
+UefiDriverEntryPoint
+UefiLib
+UefiBootServicesTableLib
+DxeServicesTableLib
+DebugLib
+PciCf8Lib
+PciSegmentLib
+BaseMemoryLib
+MemoryAllocationLib
+IoLib
+S3BootScriptLib
+PmcLib
+PchInfoLib
+GpioLib
+ConfigBlockLib
+SaPlatformLib
+PchPcieRpLib
+DxeGraphicsInitLib
+DxeIgdOpRegionInitLib
+DxeVtdInitLib
+PciExpressHelpersLib
+DxeCpuPcieRpLib
+SataLib
+
+[Packages]
+TigerlakeSiliconPkg/SiPkg.dec
+MdePkg/MdePkg.dec
+UefiCpuPkg/UefiCpuPkg.dec
+IntelSiliconPkg/IntelSiliconPkg.dec
+
+[Pcd]
+gSiPkgTokenSpaceGuid.PcdSiPciExpressBaseAddress
+gSiPkgTokenSpaceGuid.PcdMchBaseAddress
+gSiPkgTokenSpaceGuid.PcdSiIoApicBaseAddress
+gSiPkgTokenSpaceGuid.PcdCpuPcieEnable                  ## CONSUMES
+
+[FixedPcd]
+
+[Sources]
+SaInitDxe.h
+SaInitDxe.c
+SaInit.h
+SaInit.c
+SaAcpi.c
+
+
+[Protocols]
+gEfiAcpiTableProtocolGuid              ## CONSUMES
+gSaNvsAreaProtocolGuid                 ## PRODUCES
+gSaPolicyProtocolGuid                  ## CONSUMES
+gEfiCpuArchProtocolGuid                ## CONSUMES
+gEfiPciEnumerationCompleteProtocolGuid ## CONSUMES
+gEfiPciRootBridgeIoProtocolGuid        ## CONSUMES
+gIgdOpRegionProtocolGuid               ## PRODUCES
+gEfiFirmwareVolume2ProtocolGuid        ## CONSUMES
+gGopComponentName2ProtocolGuid         ## CONSUMES
+gSaIotrapSmiProtocolGuid               ## CONSUMES
+gCpuPcieIoTrapProtocolGuid             ## CONSUMES
+
+[Guids]
+gSaConfigHobGuid
+gHgAcpiTablePchStorageGuid
+gSaAcpiTableStorageGuid
+gHgAcpiTableStorageGuid
+gSaSsdtAcpiTableStorageGuid
+gSegSsdtAcpiTableStorageGuid
+gTcssSsdtAcpiTableStorageGuid
+gGraphicsAcpiTableStorageGuid
+gIpuAcpiTableStorageGuid
+gEfiEndOfDxeEventGroupGuid
+gSiConfigHobGuid        ## CONSUMES
+gGraphicsDxeConfigGuid
+gMemoryDxeConfigGuid
+gPcieDxeConfigGuid
+gPchInfoHobGuid
+gTcssHobGuid
+gSaConfigHobGuid
+gSaDataHobGuid
+gCpuPcieHobGuid
+gHostBridgeDataHobGuid
+gVmdInfoHobGuid                   ## CONSUMES
+
+[FixedPcd]
+
+[Depex]
+gEfiAcpiTableProtocolGuid AND
+gEfiFirmwareVolume2ProtocolGuid AND
+gSaPolicyProtocolGuid AND
+gEfiPciRootBridgeIoProtocolGuid AND
+gEfiPciHostBridgeResourceAllocationProtocolGuid AND # This is to ensure that PCI MMIO resource has been prepared and available for this driver to allocate.
+gEfiHiiDatabaseProtocolGuid
+
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/CpuPcieSmm.c b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/CpuPcieSmm.c
new file mode 100644
index 0000000000..70d47e787f
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/CpuPcieSmm.c
@@ -0,0 +1,454 @@
+/** @file
+  CPU PCIe SMM Driver Entry
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#include "SaLateInitSmm.h"
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/SmmServicesTableLib.h>
+#include <PcieRegs.h>
+#include <Register/PchRegs.h>
+#include <Register/CpuPcieRegs.h>
+#include <Library/PcieHelperLib.h>
+#include <CpuPcieInfo.h>
+#include <Library/TimerLib.h>
+#include <Library/PciExpressHelpersLib.h>
+#include <Library/CpuPcieInfoFruLib.h>
+#include <CpuPcieConfig.h>
+#include <Library/CpuPcieRpLib.h>
+#include <Protocol/PchPcieSmiDispatch.h>
+#include <Protocol/PchSmiDispatch.h>
+#include <Protocol/SaIotrapSmi.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/S3BootScriptLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <CpuPcieHob.h>
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8                       mSaBusNumber;
+//
+// @note:
+// These temp bus numbers cannot be used in runtime (hot-plug).
+// These can be used only during boot.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8                       mTempRootPortBusNumMin;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8                       mTempRootPortBusNumMax;
+GLOBAL_REMOVE_IF_UNREFERENCED CPU_PCIE_ROOT_PORT_CONFIG   mCpuPcieRootPortConfig[CPU_PCIE_MAX_ROOT_PORTS];
+GLOBAL_REMOVE_IF_UNREFERENCED BOOLEAN                     mCpuPciePmTrapExecuted = FALSE;
+GLOBAL_REMOVE_IF_UNREFERENCED PCH_PCIE_DEVICE_OVERRIDE    *mDevAspmOverride;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32                      mNumOfDevAspmOverride;
+
+/**
+  An IoTrap callback to config PCIE power management settings
+**/
+VOID
+CpuPciePmIoTrapSmiCallback (
+  VOID
+  )
+{
+  UINT32                                    PortIndex;
+  UINT64                                    RpBase;
+  UINT8                                     MaxPciePortNum;
+  UINTN                                     RpDevice;
+  UINTN                                     RpFunction;
+
+  MaxPciePortNum = GetMaxCpuPciePortNum ();
+
+  for (PortIndex = 0; PortIndex < MaxPciePortNum; PortIndex++) {
+    GetCpuPcieRpDevFun (PortIndex, &RpDevice, &RpFunction);
+    RpBase = PCI_SEGMENT_LIB_ADDRESS (SA_SEG_NUM, SA_MC_BUS, (UINT32) RpDevice, (UINT32) RpFunction, 0);
+
+    if (PciSegmentRead16 (RpBase) != 0xFFFF) {
+      mDevAspmOverride                  = NULL;
+      mNumOfDevAspmOverride             = 0;
+      RootportDownstreamPmConfiguration (
+        SA_SEG_NUM,
+        SA_MC_BUS,
+        (UINT8)RpDevice,
+        (UINT8)RpFunction,
+        mTempRootPortBusNumMin,
+        mTempRootPortBusNumMax,
+        &mCpuPcieRootPortConfig[PortIndex].PcieRpCommonConfig,
+        mNumOfDevAspmOverride,
+        mDevAspmOverride
+      );
+
+    }
+  }
+}
+
+/**
+  Program Common Clock and ASPM of Downstream Devices
+
+  @param[in] PortIndex                  Pcie Root Port Number
+  @param[in] RpDevice                   Pcie Root Pci Device Number
+  @param[in] RpFunction                 Pcie Root Pci Function Number
+
+  @retval EFI_SUCCESS                   Root port complete successfully
+  @retval EFI_UNSUPPORTED               PMC has invalid vendor ID
+**/
+EFI_STATUS
+CpuPcieSmi (
+  IN  UINT8                             PortIndex,
+  IN  UINT8                             RpDevice,
+  IN  UINT8                             RpFunction
+  )
+{
+  UINT8                 SecBus;
+  UINT8                 SubBus;
+  UINT64                RpBase;
+  UINT64                EpBase;
+  UINT8                 EpPcieCapPtr;
+  UINT8                 EpMaxSpeed;
+  BOOLEAN               DownstreamDevicePresent;
+  UINT32                Timeout;
+  UINT32                MaxLinkSpeed;
+
+  RpBase   = PCI_SEGMENT_LIB_ADDRESS (
+               SA_SEG_NUM,
+               SA_MC_BUS,
+               (UINT32) RpDevice,
+               (UINT32) RpFunction,
+               0
+               );
+
+  if (PciSegmentRead16 (RpBase + PCI_VENDOR_ID_OFFSET) == 0xFFFF) {
+    DEBUG((DEBUG_INFO, "PCIe controller is disabled, return!!\n"));
+    return EFI_SUCCESS;
+  }
+  //
+  // Check presence detect state. Here the endpoint must be detected using PDS rather than
+  // the usual LinkActive check, because PDS changes immediately and LA takes a few milliseconds to stabilize
+  //
+  DownstreamDevicePresent = !!(PciSegmentRead16 (RpBase + R_PCIE_SLSTS) & B_PCIE_SLSTS_PDS);
+
+  if (DownstreamDevicePresent) {
+    ///
+    /// Make sure the link is active before trying to talk to device behind it
+    /// Wait up to 100ms, according to PCIE spec chapter 6.7.3.3
+    ///
+    Timeout = 100 * 1000;
+    while (CpuPcieIsLinkActive(RpBase) == 0) {
+      MicroSecondDelay (10);
+      Timeout-=10;
+      if (Timeout == 0) {
+        DEBUG((DEBUG_INFO, "PDS set but timeout while waiting for LA bit to get set!!!\n"));
+        return EFI_NOT_FOUND;
+      }
+    }
+    SecBus  = PciSegmentRead8 (RpBase + PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET);
+    SubBus  = PciSegmentRead8 (RpBase + PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET);
+    ASSERT (SecBus != 0 && SubBus != 0);
+    if (SecBus == 0) {
+      DEBUG((DEBUG_INFO, "Secondary Bus is 0, return!!!\n"));
+      return EFI_NOT_FOUND;
+    }
+    RootportDownstreamConfiguration (
+      SA_SEG_NUM,
+      SA_MC_BUS,
+      RpDevice,
+      RpFunction,
+      mTempRootPortBusNumMin,
+      mTempRootPortBusNumMax,
+      EnumCpuPcie
+      );
+    RootportDownstreamPmConfiguration (
+      SA_SEG_NUM,
+      SA_MC_BUS,
+      RpDevice,
+      RpFunction,
+      mTempRootPortBusNumMin,
+      mTempRootPortBusNumMax,
+      &mCpuPcieRootPortConfig[PortIndex].PcieRpCommonConfig,
+      mNumOfDevAspmOverride,
+      mDevAspmOverride
+    );
+    //
+    // Perform Equalization
+    //
+    EpBase = PCI_SEGMENT_LIB_ADDRESS (SA_SEG_NUM, SecBus, 0, 0, 0);
+    EpPcieCapPtr = PcieFindCapId (SA_SEG_NUM, SecBus, 0, 0, EFI_PCI_CAPABILITY_ID_PCIEXP);
+    EpMaxSpeed = PciSegmentRead8 (EpBase + EpPcieCapPtr + R_PCIE_LCAP_OFFSET) & B_PCIE_LCAP_MLS;
+    MaxLinkSpeed = CpuPcieGetMaxLinkSpeed (RpBase);
+    if (EpMaxSpeed < MaxLinkSpeed) {
+        MaxLinkSpeed = EpMaxSpeed;
+    }
+    if (EpMaxSpeed >= V_PCIE_LCAP_MLS_GEN3 && EpMaxSpeed <= V_PCIE_LCAP_MLS_GEN4) {
+      PciSegmentAndThenOr16 (RpBase + R_PCIE_LCTL2, (UINT16)~B_PCIE_LCTL2_TLS, (UINT16)MaxLinkSpeed);
+      PciSegmentOr32 (RpBase + R_PCIE_LCTL3, B_PCIE_LCTL3_PE);
+      PciSegmentOr32 (RpBase + R_PCIE_LCTL, B_PCIE_LCTL_RL);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  PCIE Hotplug SMI call back function for each Root port
+
+  @param[in] DispatchHandle             Handle of this dispatch function
+  @param[in] RpContext                  Rootport context, which contains RootPort Index,
+                                        and RootPort PCI BDF.
+**/
+VOID
+EFIAPI
+CpuPcieSmiRpHandlerFunction (
+  IN  EFI_HANDLE                        DispatchHandle,
+  IN  PCH_PCIE_SMI_RP_CONTEXT           *RpContext
+  )
+{
+  CpuPcieSmi (RpContext->RpIndex, RpContext->DevNum, RpContext->FuncNum);
+}
+
+/**
+  PCIE Link Active State Change Hotplug SMI call back function for all Root ports
+
+  @param[in] DispatchHandle             Handle of this dispatch function
+  @param[in] RpContext                  Rootport context, which contains RootPort Index,
+                                        and RootPort PCI BDF.
+**/
+VOID
+EFIAPI
+CpuPcieLinkActiveStateChange (
+  IN  EFI_HANDLE                        DispatchHandle,
+  IN  PCH_PCIE_SMI_RP_CONTEXT           *RpContext
+  )
+{
+  return;
+}
+
+/**
+  PCIE Link Equalization Request SMI call back function for all Root ports
+
+  @param[in] DispatchHandle             Handle of this dispatch function
+  @param[in] RpContext                  Rootport context, which contains RootPort Index,
+                                        and RootPort PCI BDF.
+**/
+VOID
+EFIAPI
+CpuPcieLinkEqHandlerFunction (
+  IN  EFI_HANDLE                        DispatchHandle,
+  IN  PCH_PCIE_SMI_RP_CONTEXT           *RpContext
+  )
+{
+  ///
+  /// From PCI Express specification, the PCIe device can request for Link Equalization. When the
+  /// Link Equalization is requested by the device, an SMI will be generated  by PCIe RP when
+  /// enabled and the SMI subroutine would invoke the Software Preset/Coefficient Search
+  /// software to re-equalize the link.
+  ///
+
+  return;
+
+}
+/**
+  An IoTrap callback to config PCIE power management settings
+
+  @param[in] DispatchHandle  - The handle of this callback, obtained when registering
+  @param[in] DispatchContext - Pointer to the EFI_SMM_IO_TRAP_DISPATCH_CALLBACK_CONTEXT
+
+**/
+VOID
+EFIAPI
+CpuPcieIoTrapSmiCallback (
+  IN  EFI_HANDLE                            DispatchHandle,
+  IN  EFI_SMM_IO_TRAP_CONTEXT               *CallbackContext,
+  IN OUT VOID                               *CommBuffer,
+  IN OUT UINTN                              *CommBufferSize
+  )
+{
+  if (CallbackContext->WriteData == CpuPciePmTrap) {
+    if (mCpuPciePmTrapExecuted == FALSE) {
+      CpuPciePmIoTrapSmiCallback ();
+      mCpuPciePmTrapExecuted = TRUE;
+    }
+  } else {
+    ASSERT_EFI_ERROR (EFI_INVALID_PARAMETER);
+  }
+}
+
+/**
+  This function clear the Io trap executed flag before enter S3
+
+  @param[in] Handle    Handle of the callback
+  @param[in] Context   The dispatch context
+
+  @retval EFI_SUCCESS  SA register saved
+**/
+EFI_STATUS
+EFIAPI
+CpuPcieS3EntryCallBack (
+  IN  EFI_HANDLE                   Handle,
+  IN CONST VOID                    *Context OPTIONAL,
+  IN OUT VOID                      *CommBuffer OPTIONAL,
+  IN OUT UINTN                     *CommBufferSize OPTIONAL
+  )
+{
+  mCpuPciePmTrapExecuted = FALSE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Register PCIE Hotplug SMI dispatch function to handle Hotplug enabling
+
+  @param[in] ImageHandle          The image handle of this module
+  @param[in] SystemTable          The EFI System Table
+
+  @retval EFI_SUCCESS             The function completes successfully
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpuPcieSmm (
+  IN      EFI_HANDLE            ImageHandle,
+  IN      EFI_SYSTEM_TABLE      *SystemTable
+  )
+{
+  EFI_STATUS                            Status;
+  UINT8                                 PortIndex;
+  UINT8                                 Data8;
+  UINT32                                Data32Or;
+  UINT32                                Data32And;
+  UINT64                                RpBase;
+  UINTN                                 RpDevice;
+  UINTN                                 RpFunction;
+  EFI_HANDLE                            PcieHandle;
+  EFI_HANDLE                            PchIoTrapHandle;
+  PCH_PCIE_SMI_DISPATCH_PROTOCOL        *PchPcieSmiDispatchProtocol;
+  EFI_SMM_IO_TRAP_REGISTER_CONTEXT      PchIoTrapContext;
+  EFI_SMM_SX_REGISTER_CONTEXT           SxDispatchContext;
+  SA_IOTRAP_SMI_PROTOCOL                *CpuPcieIoTrapProtocol;
+  EFI_HANDLE                            SxDispatchHandle;
+  UINT8                                 MaxPciePortNum;
+  CPU_PCIE_HOB                          *CpuPcieHob;
+
+  DEBUG ((DEBUG_INFO, "InitializeCpuPcieSmm () Start\n"));
+
+  MaxPciePortNum    = GetMaxCpuPciePortNum ();
+
+  //
+  // Locate Pch Pcie Smi Dispatch Protocol
+  //
+  Status = gSmst->SmmLocateProtocol (&gPchPcieSmiDispatchProtocolGuid, NULL, (VOID**) &PchPcieSmiDispatchProtocol);
+  ASSERT_EFI_ERROR (Status);
+
+  mTempRootPortBusNumMin = PcdGet8 (PcdSiliconInitTempPciBusMin);
+  mTempRootPortBusNumMax = PcdGet8 (PcdSiliconInitTempPciBusMax);
+
+  ///
+  /// Locate HOB for CPU PCIe
+  ///
+  CpuPcieHob = GetFirstGuidHob(&gCpuPcieHobGuid);
+  if (CpuPcieHob != NULL) {
+    ASSERT (sizeof mCpuPcieRootPortConfig == sizeof CpuPcieHob->RootPort);
+    CopyMem (
+      mCpuPcieRootPortConfig,
+      &(CpuPcieHob->RootPort),
+      sizeof (mCpuPcieRootPortConfig)
+      );
+  }
+
+  //
+  // Throught all PCIE root port function and register the SMI Handler for enabled ports.
+  //
+  for (PortIndex = 0; PortIndex < MaxPciePortNum; PortIndex++) {
+    GetCpuPcieRpDevFun (PortIndex, &RpDevice, &RpFunction);
+    RpBase = PCI_SEGMENT_LIB_ADDRESS (SA_SEG_NUM, SA_MC_BUS, (UINT32) RpDevice, (UINT32) RpFunction, 0);
+    //
+    // Skip the root port function which is not enabled
+    //
+    if (PciSegmentRead32 (RpBase) == 0xFFFFFFFF) {
+      continue;
+    }
+
+    //
+    // Register SMI Handlers for Hot Plug and Link Active State Change
+    //
+    Data8 = PciSegmentRead8 (RpBase + R_PCIE_SLCAP);
+    if (Data8 & B_PCIE_SLCAP_HPC) {
+      PcieHandle = NULL;
+      Status = PchPcieSmiDispatchProtocol->HotPlugRegister (
+                                             PchPcieSmiDispatchProtocol,
+                                             CpuPcieSmiRpHandlerFunction,
+                                             (PortIndex + CpuRpIndex0),
+                                             &PcieHandle
+                                             );
+      ASSERT_EFI_ERROR (Status);
+
+      Status = PchPcieSmiDispatchProtocol->LinkActiveRegister (
+                                             PchPcieSmiDispatchProtocol,
+                                             CpuPcieLinkActiveStateChange,
+                                             (PortIndex + CpuRpIndex0),
+                                             &PcieHandle
+                                             );
+      ASSERT_EFI_ERROR (Status);
+
+      Data32Or  = B_PCIE_MPC_HPME;
+      Data32And = (UINT32) ~B_PCIE_MPC_HPME;
+      S3BootScriptSaveMemReadWrite (
+        S3BootScriptWidthUint32,
+        PcdGet64 (PcdSiPciExpressBaseAddress) + RpBase + R_PCIE_MPC,
+        &Data32Or,  /// Data to be ORed
+        &Data32And  /// Data to be ANDed
+        );
+    }
+
+    //
+    // Register SMI Handler for Link Equalization Request from Gen 3 Devices.
+    //
+    Data8 = PciSegmentRead8 (RpBase + R_PCIE_LCAP);
+    if ((Data8 & B_PCIE_LCAP_MLS) == V_PCIE_LCAP_MLS_GEN3) {
+      Status = PchPcieSmiDispatchProtocol->LinkEqRegister (
+                                             PchPcieSmiDispatchProtocol,
+                                             CpuPcieLinkEqHandlerFunction,
+                                             (PortIndex + CpuRpIndex0),
+                                             &PcieHandle
+                                             );
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  ASSERT_EFI_ERROR (Status);
+  PchIoTrapContext.Type     = WriteTrap;
+  PchIoTrapContext.Length   = 4;
+  PchIoTrapContext.Address  = 0;
+  Status = mPchIoTrap->Register (
+                         mPchIoTrap,
+                         (EFI_SMM_HANDLER_ENTRY_POINT2) CpuPcieIoTrapSmiCallback,
+                         &PchIoTrapContext,
+                         &PchIoTrapHandle
+                         );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install the SA Pcie IoTrap protocol
+  //
+  (gBS->AllocatePool) (EfiBootServicesData, sizeof (SA_IOTRAP_SMI_PROTOCOL), (VOID **)&CpuPcieIoTrapProtocol);
+  CpuPcieIoTrapProtocol->SaIotrapSmiAddress = PchIoTrapContext.Address;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ImageHandle,
+                  &gCpuPcieIoTrapProtocolGuid,
+                  CpuPcieIoTrapProtocol,
+                  NULL
+                  );
+
+  //
+  // Register the callback for S3 entry
+  //
+  SxDispatchContext.Type  = SxS3;
+  SxDispatchContext.Phase = SxEntry;
+  Status = mSxDispatch->Register (
+                          mSxDispatch,
+                          CpuPcieS3EntryCallBack,
+                          &SxDispatchContext,
+                          &SxDispatchHandle
+                          );
+  ASSERT_EFI_ERROR (Status);
+
+  DEBUG ((DEBUG_INFO, "InitializeCpuPcieSmm, IoTrap @ %x () End\n", PchIoTrapContext.Address));
+
+  return EFI_SUCCESS;
+}
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.c b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.c
new file mode 100644
index 0000000000..0e9ba41f1b
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.c
@@ -0,0 +1,112 @@
+/** @file
+  This SMM driver will handle SA relevant late initialization
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#include <Library/BaseLib.h>
+#include <Base.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/IoLib.h>
+#include <Library/HobLib.h>
+#include <Library/SaPlatformLib.h>
+#include <Protocol/SmmIoTrapDispatch2.h>
+#include "SaLateInitSmm.h"
+#include <Library/PciSegmentLib.h>
+#include <CpuDataStruct.h>
+#include <CpuPcieHob.h>
+#include <Protocol/SaIotrapSmi.h>
+#include <PchPcieRpConfig.h>
+#include <SaConfigHob.h>
+#include "CpuPcieInfo.h"
+#include <Register/CpuPcieRegs.h>
+#include <IndustryStandard/Pci30.h>
+#include <Register/IgdRegs.h>
+#include <Register/CommonMsr.h>
+
+typedef enum {
+  EnumSaSmiCallbackForMaxPayLoad,
+  EnumSaSmiCallbackForSaSaveRestore,
+  EnumSaSmiCallbackForLateInit,
+  EnumSaSmiCallbackForS3resume,
+  EnumSaSmiCallbackMax
+} SMI_OPERATION;
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8                           mSaSmiCallbackPhase      = EnumSaSmiCallbackForMaxPayLoad;
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_SMM_IO_TRAP_DISPATCH2_PROTOCOL        *mPchIoTrap;
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_SMM_SX_DISPATCH2_PROTOCOL             *mSxDispatch;
+
+
+typedef struct {
+  UINT64  BaseAddr;
+  UINT32  Offset;
+  UINT32  AndMask;
+  UINT32  OrMask;
+} BOOT_SCRIPT_REGISTER_SETTING;
+
+/**
+  Initializes the SA SMM handler
+
+  @param[in] ImageHandle - The image handle of Wake On Lan driver
+  @param[in] SystemTable - The standard EFI system table
+
+  @retval EFI_SUCCESS    - SA SMM handler was installed or not necessary
+  @retval EFI_NOT_FOUND  - Fail to register SMI callback or required protocol/hob missing.
+**/
+EFI_STATUS
+EFIAPI
+SaLateInitSmmEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+#if FixedPcdGetBool(PcdCpuPcieEnable) == 1
+  CPU_PCIE_HOB                              *CpuPcieHob = NULL;
+  EFI_STATUS                                Status;
+#endif
+
+  DEBUG ((DEBUG_INFO, "SaLateInitSmmEntryPoint()\n"));
+
+#if FixedPcdGetBool(PcdCpuPcieEnable) == 1
+  CpuPcieHob = (CPU_PCIE_HOB *) GetFirstGuidHob (&gCpuPcieHobGuid);
+  Status = EFI_NOT_FOUND;
+  if (CpuPcieHob == NULL) {
+    DEBUG ((DEBUG_INFO, "CPU PCIE HOB Not found\n"));
+    ASSERT (CpuPcieHob != NULL);
+    return Status;
+  }
+
+  ///
+  /// Locate the PCH Trap dispatch protocol
+  ///
+  Status = gSmst->SmmLocateProtocol (&gEfiSmmIoTrapDispatch2ProtocolGuid, NULL, (VOID **) &mPchIoTrap);
+  ASSERT_EFI_ERROR (Status);
+  Status = gSmst->SmmLocateProtocol (&gEfiSmmSxDispatch2ProtocolGuid, NULL, (VOID**) &mSxDispatch);
+  ASSERT_EFI_ERROR (Status);
+  if (Status == EFI_SUCCESS) {
+    ///
+    /// If ASPM policy is set to "Before OPROM", this SMI callback is not necessary
+    /// Ensure the SMI callback handler will directly return and continue the POST.
+    ///
+    mSaSmiCallbackPhase = EnumSaSmiCallbackMax;
+    Status = EFI_SUCCESS;
+  }
+
+  Status = InitializeCpuPcieSmm (ImageHandle, SystemTable);
+  ASSERT_EFI_ERROR (Status);
+
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((DEBUG_ERROR, "Failed to register SaIotrapSmiCallback!\n"));
+    ///
+    /// System will halt when failing to register required SMI handler
+    ///
+    CpuDeadLoop ();
+  }
+#endif
+  return EFI_SUCCESS;
+}
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.h b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.h
new file mode 100644
index 0000000000..c93f92305c
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.h
@@ -0,0 +1,122 @@
+/** @file
+  Header file for SA SMM Handler
+
+  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#ifndef _SaLateInitSmm_H_
+#define _SaLateInitSmm_H_
+
+///
+/// Driver Consumed Protocol Prototypes
+///
+
+#include <Protocol/SmmBase2.h>
+#include <Protocol/SmmSxDispatch2.h>
+#include <Protocol/SmmIoTrapDispatch2.h>
+#include <Protocol/SaPolicy.h>
+#include <Library/S3BootScriptLib.h>
+#include <CpuPcieConfig.h>
+#include <Library/CpuPcieInfoFruLib.h>
+
+extern EFI_SMM_IO_TRAP_DISPATCH2_PROTOCOL        *mPchIoTrap;
+extern EFI_SMM_SX_DISPATCH2_PROTOCOL             *mSxDispatch;
+
+///
+/// The value before AutoConfig match the setting of PCI Express Base Specification 1.1, please be careful for adding new feature
+///
+typedef enum {
+  PcieAspmDisabled,
+  PcieAspmL0s,
+  PcieAspmL1,
+  PcieAspmL0sL1,
+  PcieAspmAutoConfig,
+  PcieAspmMax
+} CPU_PCIE_ASPM_CONFIG;
+
+typedef struct {
+  UINT64                    Address;
+  S3_BOOT_SCRIPT_LIB_WIDTH  Width;
+  UINT32                    Value;
+} BOOT_SCRIPT_PCI_REGISTER_SAVE;
+
+
+/**
+  <b>System Agent Initialization SMM Driver Entry Point</b>
+  - <b>Introduction</b> \n
+    This is an optional driver to support PCIe ASPM initialization later than Option ROM initialization.\n
+    In this scenario S3 Save Boot Script table has been closed per security consideration so the ASPM settings will be stored in SMM memory and restored during S3 resume.
+    If platform does not support this scenario this driver can be excluded and SI_SA_POLICY_PPI -> PCIE_CONFIG -> InitPcieAspmAfterOprom must be set to FALSE. \n
+    Note: When InitPcieAspmAfterOprom enabled, the SMI callback handler must be registered successfully, otherwise it will halt the system.
+
+  - @pre
+    - _EFI_SMM_BASE_PROTOCOL (or _EFI_SMM_BASE2_PROTOCOL for EDK2): Provides SMM infrastructure services.
+    - _EFI_SMM_IO_TRAP_DISPATCH_PROTOCOL (or _EFI_SMM_IO_TRAP_DISPATCH2_PROTOCOL for EDK2): Interface structure for the SMM IO trap specific SMI Dispatch Protocol
+    - SA_POLICY_PROTOCOL: A protocol published by a platform DXE module executed earlier; this is documented in this document as well.
+
+  - @result
+    PCIe ASPM has been initialized on all end point devices discovered and same settings will be restored during S3 resume.
+
+  @param[in] ImageHandle - The image handle of Wake On Lan driver
+  @param[in] SystemTable - The standard EFI system table
+
+  @retval EFI_SUCCESS    - SA SMM handler was installed or not necessary
+  @retval EFI_NOT_FOUND  - Fail to register SMI callback or required protocol/hob missing.
+**/
+EFI_STATUS
+EFIAPI
+SaLateInitSmmEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  );
+
+/**
+An IoTrap callback to config PCIE power management settings
+
+@param[in] DispatchHandle  - The handle of this callback, obtained when registering
+@param[in] DispatchContext - Pointer to the EFI_SMM_IO_TRAP_DISPATCH_CALLBACK_CONTEXT
+
+**/
+VOID
+EFIAPI
+CpuPcieIoTrapSmiCallback(
+IN     EFI_HANDLE                     DispatchHandle,
+IN     EFI_SMM_IO_TRAP_CONTEXT        *CallbackContext,
+IN OUT VOID                           *CommBuffer,
+IN OUT UINTN                          *CommBufferSize
+);
+
+/**
+  This function is used to set or clear flags at S3 entry
+  Clear the Io trap executed flag before enter S3
+
+  @param[in] Handle              Handle of the callback
+  @param[in] Context             The dispatch context
+  @param[in,out] CommBuffer      A pointer to a collection of data in memory that will be conveyed from a non-SMM environment into an SMM environment.
+  @param[in,out] CommBufferSize  The size of the CommBuffer.
+  @retval EFI_SUCCESS            SA register saved
+**/
+EFI_STATUS
+EFIAPI
+CpuPcieS3EntryCallBack (
+  IN  EFI_HANDLE                   Handle,
+  IN CONST VOID                    *Context OPTIONAL,
+  IN OUT VOID                      *CommBuffer OPTIONAL,
+  IN OUT UINTN                     *CommBufferSize OPTIONAL
+  );
+
+/**
+  Register PCIE Hotplug SMI dispatch function to handle Hotplug enabling
+
+  @param[in] ImageHandle          The image handle of this module
+  @param[in] SystemTable          The EFI System Table
+
+  @retval EFI_SUCCESS             The function completes successfully
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpuPcieSmm (
+  IN      EFI_HANDLE            ImageHandle,
+  IN      EFI_SYSTEM_TABLE      *SystemTable
+  );
+#endif
diff --git a/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.inf b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.inf
new file mode 100644
index 0000000000..ef3486d2a6
--- /dev/null
+++ b/Silicon/Intel/TigerlakeSiliconPkg/SystemAgent/SaInit/Smm/SaLateInitSmm.inf
@@ -0,0 +1,72 @@
+## @file
+# Component description file for the SA late initialization SMM module.
+#
+#  Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+INF_VERSION = 0x00010017
+BASE_NAME = SaLateInitSmm
+FILE_GUID = 2D1E361C-7B3F-4d15-8B1F-66E551FABDC7
+VERSION_STRING = 1.0
+MODULE_TYPE = DXE_SMM_DRIVER
+PI_SPECIFICATION_VERSION = 1.10
+ENTRY_POINT = SaLateInitSmmEntryPoint
+
+[LibraryClasses]
+UefiDriverEntryPoint
+UefiBootServicesTableLib
+DxeServicesTableLib
+DebugLib
+HobLib
+BaseLib
+S3BootScriptLib
+PciSegmentLib
+SaPlatformLib
+TimerLib
+PciExpressHelpersLib
+PcdLib
+S3BootScriptLib
+CpuPcieInfoFruLib
+ConfigBlockLib
+CpuPcieRpLib
+
+[Packages]
+MdePkg/MdePkg.dec
+TigerlakeSiliconPkg/SiPkg.dec
+
+[Pcd]
+gSiPkgTokenSpaceGuid.PcdSiPciExpressBaseAddress
+gSiPkgTokenSpaceGuid.PcdSiliconInitTempPciBusMax
+gSiPkgTokenSpaceGuid.PcdSiliconInitTempPciBusMin
+gSiPkgTokenSpaceGuid.PcdCpuPcieEnable              ## CONSUMES
+
+
+[Sources]
+SaLateInitSmm.c
+CpuPcieSmm.c
+SaLateInitSmm.h
+
+[Protocols]
+gSaPolicyProtocolGuid              ## CONSUMES
+gEfiSmmIoTrapDispatch2ProtocolGuid ## CONSUMES
+gSaIotrapSmiProtocolGuid           ## PRODUCES
+gCpuPcieIoTrapProtocolGuid         ## PRODUCES
+gEfiSmmSxDispatch2ProtocolGuid     ## CONSUMES
+gPchSmiDispatchProtocolGuid        ## CONSUMES
+gPchPcieSmiDispatchProtocolGuid    ## CONSUMES
+
+[Guids]
+gSaConfigHobGuid
+gCpuPcieHobGuid
+gPcieDxeConfigGuid
+gSaPegHobGuid
+
+[Depex]
+gEfiSmmBase2ProtocolGuid AND
+gEfiSmmSxDispatch2ProtocolGuid AND
+gEfiSmmIoTrapDispatch2ProtocolGuid AND
+gSaPolicyProtocolGuid
+
-- 
2.24.0.windows.2