init. 1AQQW051HEADmaster

55 files changed, 20217 insertions, 0 deletions

diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/AudioInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/AudioInit.c
new file mode 100644
index 0000000..a55603a
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/AudioInit.c
@@ -0,0 +1,849 @@
+/** @file
+  This is the driver that initializes the CPU Audio device.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+
+#include "AudioInit.h"
+
+extern SYSTEM_AGENT_GLOBAL_NVS_AREA_PROTOCOL  mSaGlobalNvsAreaProtocol;
+
+///
+/// Global variables
+///
+UINT32            *mVerbCodecSaveAddress = NULL;
+UINT8             mVerbCount = 0;
+
+UINT32            mSaHdaVerbTableDataDefault[] = {
+  ///
+  /// Audio Verb Table - 0x80862807
+  ///
+  ///
+  /// Pin Widget 5 - PORT B
+  ///
+  0x00571C10,
+  0x00571D00,
+  0x00571E56,
+  0x00571F18,
+  ///
+  /// Pin Widget 6 - PORT C
+  ///
+  0x00671C20,
+  0x00671D00,
+  0x00671E56,
+  0x00671F18,
+  ///
+  /// Pin Widget 7 - PORT D
+  ///
+  0x00771C30,
+  0x00771D00,
+  0x00771E56,
+  0x00771F18
+};
+
+SA_HDA_VERB_TABLE mSaHdaVerbTableDefault[] = {
+  {
+    ///
+    ///  VerbTable:
+    ///  Revision ID = 0xFF
+    ///  Codec Vendor: 0x80862807
+    ///
+    {
+      0x80862807, ///< Vendor ID/Device ID
+      0x0000,     ///< SubSystem ID
+      0xFF,       ///< Revision ID
+      0x02,       ///< Front panel support (1=yes, 2=no)
+      0x0003,     ///< Number of Rear Jacks
+      0x0000      ///< Number of Front Jacks
+    },
+    0             ///< Pointer to verb table data, need to be inited in the code.
+  }
+};
+
+/**
+  Polling the Status bit
+
+  @param[in] StatusReg               The regsiter address to read the status
+  @param[in] PollingBitMap           The bit mapping for polling
+  @param[in] PollingData             The Data for polling
+
+  @retval EFI_SUCCESS                The function completed successfully
+  @retval EFI_TIMEOUT                Polling the bit map time out
+**/
+EFI_STATUS
+StatusPolling (
+  IN      UINT32          StatusReg,
+  IN      UINT16          PollingBitMap,
+  IN      UINT16          PollingData
+  )
+{
+  UINT32  LoopTime;
+
+  for (LoopTime = 0; LoopTime < SA_HDA_MAX_LOOP_TIME; LoopTime++) {
+    if ((MmioRead16 (StatusReg) & PollingBitMap) == PollingData) {
+      break;
+    } else {
+      PchPmTimerStall (SA_HDA_WAIT_PERIOD);
+    }
+  }
+
+  if (LoopTime >= SA_HDA_MAX_LOOP_TIME) {
+    DEBUG ((EFI_D_ERROR, "Polling StatusReg 0x%X BitMap 0x%x TimeOut\n", StatusReg, PollingBitMap));
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send the command to the codec via the Immediate Command mechanism is written
+  to the IC register
+
+  @param[in] HdaBar                  Base address of Intel HD Audio memory mapped configuration registers
+  @param[in, out] CodecCommandData   The Codec Command to be sent to the codec
+  @param[in] ReadBack                Whether to get the response received from the codec
+
+  @exception EFI_DEVICE_ERROR        Device status error, operation failed
+  @retval EFI_SUCCESS                The function completed successfully
+**/
+EFI_STATUS
+SendCodecCommand (
+  IN      UINT32          HdaBar,
+  IN OUT  UINT32          *CodecCommandData,
+  IN      BOOLEAN         ReadBack
+  )
+{
+  EFI_STATUS  Status;
+
+  ///
+  /// Wait for Command Busy (ICB) bit to be cleared
+  ///
+  Status = StatusPolling (HdaBar + R_HDA_IRS, (UINT16) B_HDA_IRS_ICB, (UINT16) 0);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "ICB bit is not zero before SendCodecCommand! \n"));
+    return EFI_DEVICE_ERROR;
+  }
+  ///
+  /// Clear Result Valid (IRV) bit (by writing a one to it) before issuing a new command
+  ///
+  MmioOr16 ((UINTN) (HdaBar + R_HDA_IRS), (UINT16) (B_HDA_IRS_IRV));
+  ///
+  /// Send command to codec
+  ///
+  MmioWrite32 (HdaBar + R_HDA_IC, *CodecCommandData);
+  ///
+  /// Set ICB bit to issue the command currently stored in IC to the codec. 
+  ///
+  MmioOr16 ((UINTN) (HdaBar + R_HDA_IRS), (UINT16) (B_HDA_IRS_ICB));
+  ///
+  /// Wait for Command Busy (ICB) bit to be cleared
+  ///
+  Status = StatusPolling (HdaBar + R_HDA_IRS, (UINT16) B_HDA_IRS_ICB, (UINT16) 0);
+  if (EFI_ERROR (Status)) {
+    MmioAnd16 ((UINTN) (HdaBar + R_HDA_IRS), (UINT16)~(B_HDA_IRS_ICB));
+    return Status;
+  }
+
+  ///
+  /// Save Codec command for runtime code
+  ///
+  if (mVerbCodecSaveAddress != NULL) {
+    if ((mVerbCount * 4) < AUDIO_CODEC_MAX_SIZE) {
+      *(mVerbCodecSaveAddress + mVerbCount) = *CodecCommandData;
+      mVerbCount++;
+      DEBUG ((EFI_D_ERROR, "SendCodecCommand(0x%x): 0x%x \n",(mVerbCodecSaveAddress + (mVerbCount)), *CodecCommandData));
+    } else {
+      DEBUG ((EFI_D_ERROR, "Codec Command Save Area Overflow max value of %d\n", AUDIO_CODEC_MAX_SIZE));
+    }
+  }
+
+  if (ReadBack == TRUE) {
+    if ((MmioRead16 (HdaBar + R_HDA_IRS) & B_HDA_IRS_IRV) != 0) {
+      *CodecCommandData = MmioRead32 (HdaBar + R_HDA_IR);
+    } else {
+      DEBUG ((EFI_D_ERROR, "SendCodecCommand: ReadBack fail! \n"));
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set a "Send Codec Command" S3 dispatch item
+
+  @param[in] HdaBar                  Base address of Intel HD Audio memory mapped configuration registers
+
+  @retval EFI_SUCCESS                The function completed successfully
+**/
+EFI_STATUS
+SendCodecCommandS3Item (
+  IN      UINT32          HdaBar
+  )
+{
+  UINT16  BitMask;
+  UINT16  BitValue;
+  UINT16  Data16And;
+  UINT16  Data16Or;
+
+  ///
+  /// Wait for Command Busy (ICB) bit to be cleared
+  ///
+  BitMask   = (UINT16) B_HDA_IRS_ICB;
+  BitValue  = (UINT16) 0;
+  SCRIPT_MEM_POLL (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    HdaBar + R_HDA_IRS,
+    &BitMask,
+    &BitValue,
+    SA_HDA_WAIT_PERIOD,
+    SA_HDA_MAX_LOOP_TIME
+    );
+  ///
+  /// Clear Result Valid (IRV) bit (by writing a one to it) before issuing a new command
+  ///
+    Data16And = 0xFFFF;
+    Data16Or  = (UINT16) (B_HDA_IRS_IRV);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint16,
+      (UINTN) (HdaBar + R_HDA_IRS),
+      &Data16Or,  /// Data to be ORed
+      &Data16And  /// Data to be ANDed
+      );
+
+  ///
+  /// Send command to codec
+  ///
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (HdaBar + R_HDA_IC),
+    1,
+    (VOID *) (UINTN) (HdaBar + R_HDA_IC)
+    );
+
+  ///
+  /// Set ICB bit to issue the command currently stored in IC to the codec. 
+  ///
+  Data16And = 0xFFFF;
+  Data16Or  = (UINT16) (B_HDA_IRS_ICB );
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    (UINTN) (HdaBar + R_HDA_IRS),
+    &Data16Or,  /// Data to be ORed
+    &Data16And  /// Data to be ANDed
+    );
+
+  ///
+  /// Wait for Command Busy (ICB) bit to be cleared
+  ///
+  SCRIPT_MEM_POLL (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    HdaBar + R_HDA_IRS,
+    &BitMask,
+    &BitValue,
+    SA_HDA_WAIT_PERIOD,
+    SA_HDA_MAX_LOOP_TIME
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Detect And Initialize SA Audio Codec
+
+  @param[in] ImageHandle             Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy     SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS - Legacy Region protocol installed
+  @retval Other       - No protocol installed, unload driver.
+**/
+EFI_STATUS
+DetectAndInitializeCodec (
+  IN EFI_HANDLE                      mImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+{
+  EFI_STATUS                Status;
+  UINT32                    Index;
+  UINT32                    VendorDeviceId;
+  UINT32                    RevisionId;
+  UINT8                     ByteReg;
+  UINTN                     AudioBase;
+  UINT8                     AudioSDINo;
+  UINT32                    HdaBar;
+  UINT32                    *VerbTable;
+  UINT32                    LoopTime;
+  SA_HDA_VERB_TABLE_HEADER  *VerbHeaderTable;
+  EFI_PHYSICAL_ADDRESS      BaseAddressBarMem;
+  UINT8                     VerbTableNum;
+  UINT32                    Data32And;
+  UINT32                    Data32Or;
+  UINT32                    Data32;
+  UINT32                    CodecCmdData;
+  UINT16                    AudioDeviceId;
+  UINT16                    Data16;
+  UINT16                    BitMask;
+  UINT16                    BitValue;
+  CPU_STEPPING              CpuSteppingId;
+  CPU_FAMILY                CpuFamilyId;
+  UINT16                    IsUlx;
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  AudioBase     = MmPciAddress (0, SA_HDA_BUS_NUM, SA_HDA_DEV_NUM, SA_HDA_FUN_NUM, 0);
+  AudioDeviceId = MmioRead16 (AudioBase + PCI_DEVICE_ID_OFFSET);
+
+  ///
+  /// Allocate resource for HDBAR
+  ///
+  BaseAddressBarMem = 0x0FFFFFFFF;
+  Status = gDS->AllocateMemorySpace (
+                  EfiGcdAllocateMaxAddressSearchBottomUp,
+                  EfiGcdMemoryTypeMemoryMappedIo,
+                  14,
+                  SA_HDA_HDBAR_SIZE,
+                  &BaseAddressBarMem,
+                  mImageHandle,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ///
+  /// System BIOS should ensure that the High Definition Audio HDBAR D27:F0:Reg 10-17h contains a valid address value
+  /// and is enabled by setting D3:F0:Reg 04h[1].
+  ///
+  HdaBar = (UINT32) BaseAddressBarMem;
+  MmioWrite32 (AudioBase + SA_HDA_HDBARL, HdaBar);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (AudioBase + SA_HDA_HDBARL),
+    1,
+    (VOID *) (UINTN) (AudioBase + SA_HDA_HDBARL)
+    );
+
+  MmioWrite32 (AudioBase + SA_HDA_HDBARU, 0);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (AudioBase + SA_HDA_HDBARU),
+    1,
+    (VOID *) (UINTN) (AudioBase + SA_HDA_HDBARU)
+    );
+
+  MmioOr16 ((UINTN) (AudioBase + PCI_COMMAND_OFFSET), (UINT16) BIT1);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    (UINTN) (AudioBase + PCI_COMMAND_OFFSET),
+    1,
+    (VOID *) (UINTN) (AudioBase + PCI_COMMAND_OFFSET)
+    );
+
+  ///
+  /// AudioWA: Apply until C0, program Chicken bit: set Dev3 mmio 101C bit 29
+  ///
+  if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId < EnumHswC0)) ||
+      ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId < EnumCrwC0))) {
+    Data32And = 0xFFFFFFFF;
+    Data32Or  = (UINT32) (BIT29);
+    MmioOr32 ((UINTN) (HdaBar + 0x101C), Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (HdaBar + 0x101C),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+  }
+  if (DxePlatformSaPolicy->Revision >=4) {
+    IsUlx = 0;
+    Data16 = McD2PciCfg16 (0x2);
+    if ((Data16 == 0xA0E) || (Data16 == 0xA1E)) {
+      IsUlx = 1;
+    }
+    Data32Or = 4;
+    Data32 = 75;
+    if (DxePlatformSaPolicy->IgdConfig->CdClkVar != 0) {
+      if (DxePlatformSaPolicy->IgdConfig->CdClk == 0) {
+        Data32Or = 16;
+        Data32 = 225;
+      }
+      if (DxePlatformSaPolicy->IgdConfig->CdClk == 2) {
+        Data32Or = 4;
+        Data32 = 90;
+      }
+    }
+    if(IsUlx == 1) {
+      Data32Or = 16;
+      Data32 = 225;
+    }
+    ///
+    /// Program Dev3 EM4 and EM5 MMIO registers [17:00] based on Cd Clk frequency
+    ///
+    Data32And = 0xFFFC0000;
+    Mmio32AndThenOr ((UINTN) HdaBar, 0x100c, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (HdaBar + 0x100c),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+
+    Data32And = 0xFFFC0000;
+    Data32Or  = Data32;
+    Mmio32AndThenOr ((UINTN) HdaBar, 0x1010, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (HdaBar + 0x1010),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+  }
+
+  mSaGlobalNvsAreaProtocol.Area->AudioWaA = MmioRead32(HdaBar + 0x1010);
+  mSaGlobalNvsAreaProtocol.Area->AudioWaB = MmioRead32(HdaBar + 0x101C);
+  mSaGlobalNvsAreaProtocol.Area->AudioWaC = MmioRead32(HdaBar + 0x100C);
+
+  ///
+  /// Allocate an ACPI NVS memory buffer for Saving Codec value for Adapter Power on restore
+  /// , zero initialize, and set the pointer in the SA NVS area structure.
+  ///
+  Status = (gBS->AllocatePool) (EfiACPIMemoryNVS, AUDIO_CODEC_MAX_SIZE, (VOID **) &mVerbCodecSaveAddress);
+  if (EFI_ERROR(Status)) {
+	mSaGlobalNvsAreaProtocol.Area->AudioCodecSaveAddress = 0;
+    ASSERT_EFI_ERROR (Status);  
+  } else {
+	ZeroMem ((VOID *) mVerbCodecSaveAddress, AUDIO_CODEC_MAX_SIZE);
+    mSaGlobalNvsAreaProtocol.Area->AudioCodecSaveAddress = (UINT32) (UINTN) (mVerbCodecSaveAddress);
+  }
+  
+  ///
+  /// Codec Initialization Programming Sequence
+  /// System BIOS should also ensure that the Controller Reset# bit of Global Control register
+  /// in memory-mapped space (HDBAR+08h[0]) is set to 1 and read back as 1.
+  /// Deassert the HDA controller RESET# to start up the link
+  ///
+  DEBUG ((EFI_D_INFO, "Codec Initialization...\n"));
+  Data32And = 0xFFFFFFFF;
+  Data32Or  = (UINT32) (B_HDA_GCTL_CRST);
+  MmioOr32 ((UINTN) (HdaBar + R_HDA_GCTL), Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (HdaBar + R_HDA_GCTL),
+    &Data32Or,  /// Data to be ORed
+    &Data32And  /// Data to be ANDed
+    );
+
+  BitMask   = (UINT16) B_HDA_GCTL_CRST;
+  BitValue  = (UINT16) B_HDA_GCTL_CRST;
+  Status    = StatusPolling (HdaBar + R_HDA_GCTL, BitMask, BitValue);
+  SCRIPT_MEM_POLL (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    HdaBar + R_HDA_GCTL,
+    &BitMask,
+    &BitValue,
+    SA_HDA_WAIT_PERIOD,
+    SA_HDA_MAX_LOOP_TIME
+    );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Reset SA dHDA Codec - Set Controller Reset# bit and Poll: Time Out - 0! \n"));
+    goto ExitInitAudio;
+  }
+  ///
+  /// Read GCAP and write the same value back to the register once after Controller Reset# bit is set
+  ///
+  Data16 = MmioRead16 (HdaBar + R_HDA_GCAP);
+  MmioWrite16 (HdaBar + R_HDA_GCAP, Data16);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    (UINTN) (HdaBar + R_HDA_GCAP),
+    1,
+    (VOID *) (UINTN) (HdaBar + R_HDA_GCAP)
+    );
+  ///
+  /// Clear the "State Change Status Register" STATESTS bits for
+  /// each of the "SDIN Stat Change Status Flag"
+  ///
+  MmioOr8 ((UINTN) (HdaBar + R_HDA_STATESTS), (UINT8) (SA_HDA_MAX_SID_MASK));
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint8,
+    (UINTN) (HdaBar + R_HDA_STATESTS),
+    1,
+    (VOID *) (UINTN) (HdaBar + R_HDA_STATESTS)
+    );
+  ///
+  /// Turn off the link and poll RESET# bit until it reads back as 0 to get hardware reset report
+  ///
+  Data32And = (UINT32) (~B_HDA_GCTL_CRST);
+  Data32Or  = (UINT32) 0;
+  MmioAnd32 ((UINTN) (HdaBar + R_HDA_GCTL), Data32And);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (HdaBar + R_HDA_GCTL),
+    &Data32Or,  /// Data to be ORed
+    &Data32And  /// Data to be ANDed
+    );
+
+  BitMask   = (UINT16) B_HDA_GCTL_CRST;
+  BitValue  = 0;
+  Status    = StatusPolling (HdaBar + R_HDA_GCTL, BitMask, BitValue);
+  SCRIPT_MEM_POLL (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    HdaBar + R_HDA_GCTL,
+    &BitMask,
+    &BitValue,
+    SA_HDA_WAIT_PERIOD,
+    SA_HDA_MAX_LOOP_TIME
+    );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Reset SA dHDA Codec - Turn off the link and Poll: Time Out - 1! \n"));
+    goto ExitInitAudio;
+  }
+  ///
+  /// Turn on the link and poll RESET# bit until it reads back as 1
+  ///
+  Data32And = 0xFFFFFFFF;
+  Data32Or  = (UINT32) (B_HDA_GCTL_CRST);
+  MmioOr32 ((UINTN) (HdaBar + R_HDA_GCTL), Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (HdaBar + R_HDA_GCTL),
+    &Data32Or,  /// Data to be ORed
+    &Data32And  /// Data to be ANDed
+    );
+  ///
+  /// For some combo card that will need this delay because each codec has different latency to come out from RESET.
+  /// This delay can make sure all codecs be recognized by BIOS after RESET sequence.
+  /// Additional delay might be required to allow codec coming out of reset prior to subsequent operations,
+  /// please contact your codec vendor for detail. When clearing this bit and setting it afterward,
+  /// BIOS must ensure that minimum link timing requirements (minimum RESET# assertion time, etc.) are met..
+  ///
+  PchPmTimerStall (SA_HDA_WAIT_PERIOD);
+  SCRIPT_STALL (EFI_ACPI_S3_RESUME_SCRIPT_TABLE, 300 * STALL_ONE_MICRO_SECOND);
+
+  BitMask   = (UINT16) B_HDA_GCTL_CRST;
+  BitValue  = (UINT16) B_HDA_GCTL_CRST;
+  Status    = StatusPolling (HdaBar + R_HDA_GCTL, BitMask, BitValue);
+  SCRIPT_MEM_POLL (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    HdaBar + R_HDA_GCTL,
+    &BitMask,
+    &BitValue,
+    SA_HDA_WAIT_PERIOD,
+    SA_HDA_MAX_LOOP_TIME
+    );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Reset SA dHDA Codec - Turn on the link and Poll: Time Out - 2! \n"));
+    goto ExitInitAudio;
+  }
+  ///
+  /// Read the "State Change Status Register" STATESTS bits twice to find out if any SDIN is connected
+  /// to a codec.
+  ///
+  for (LoopTime = 0, ByteReg = 0, AudioSDINo = 0; LoopTime < SA_HDA_MAX_LOOP_TIME; LoopTime++) {
+    ByteReg = MmioRead8 (HdaBar + R_HDA_STATESTS) & SA_HDA_MAX_SID_MASK;
+    if (ByteReg != 0 && (ByteReg == AudioSDINo)) {
+      break;
+    } else {
+      AudioSDINo = ByteReg;
+    }
+
+    PchPmTimerStall (SA_HDA_WAIT_PERIOD);
+  }
+  ///
+  /// BIT3(1000) -- SDI3
+  /// BIT2(0100) -- SDI2
+  /// BIT1(0010) -- SDI1
+  /// BIT0(0001) -- SDI0
+  ///
+  if (ByteReg == 0) {
+    ///
+    /// No Codec Detected, Turn off the link
+    ///
+    DEBUG ((EFI_D_INFO, "No Codec device is detected.\n"));
+    Data32And = (UINT32) (~B_HDA_GCTL_CRST);
+    Data32Or  = (UINT32) 0;
+    MmioAnd32 ((UINTN) (HdaBar + R_HDA_GCTL), Data32And);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (HdaBar + R_HDA_GCTL),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+    Status = EFI_DEVICE_ERROR;
+    goto ExitInitAudio;
+  }
+
+  for (AudioSDINo = 0; AudioSDINo < SA_HDA_MAX_SID_NUMBER; AudioSDINo++, ByteReg >>= 1) {
+    if ((ByteReg & 0x1) == 0) {
+      ///
+      /// SDIx has no Codec Device
+      ///
+      DEBUG ((EFI_D_INFO, "SDI%d has no Codec device.\n", AudioSDINo));
+      continue;
+    }
+    ///
+    /// Verb:  31~28   27  26~20                   19~0
+    ///         CAd    1    NID   Verb Command and data
+    ///       0/1/2
+    ///
+    /// Read the Vendor ID/Device ID pair from the attached codec
+    ///
+    VendorDeviceId  = 0x000F0000 | (AudioSDINo << 28);
+    Status          = SendCodecCommand (HdaBar, &VendorDeviceId, TRUE);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Read the Codec Vendor ID/Device ID fail! \n"));
+      goto ExitInitAudio;
+    }
+    ///
+    /// Read the Revision ID from the attached codec
+    ///
+    RevisionId  = 0x000F0002 | (AudioSDINo << 28);
+    Status      = SendCodecCommand (HdaBar, &RevisionId, TRUE);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Read the Codec Revision ID fail! \n"));
+      goto ExitInitAudio;
+    }
+
+    RevisionId = (RevisionId >> 8) & 0xFF;
+
+    ///
+    /// Get the match codec verb table, RevID of 0xFF applies to all steppings.
+    ///
+    if (DxePlatformSaPolicy->MiscConfig->SaHdaVerbTableNum == 0) {
+      ///
+      /// Init Verb Table Data
+      ///
+      DxePlatformSaPolicy->MiscConfig->SaHdaVerbTable[0].VerbTableData = mSaHdaVerbTableDataDefault;
+      DxePlatformSaPolicy->MiscConfig->SaHdaVerbTableNum   = sizeof (mSaHdaVerbTableDefault) / sizeof (SA_HDA_VERB_TABLE);
+    }
+    for (VerbTableNum = 0, VerbHeaderTable = NULL, VerbTable = NULL;
+         VerbTableNum < DxePlatformSaPolicy->MiscConfig->SaHdaVerbTableNum;
+         VerbTableNum++
+        ) {
+      if ((VendorDeviceId == DxePlatformSaPolicy->MiscConfig->SaHdaVerbTable[VerbTableNum].VerbTableHeader.VendorDeviceId) &&
+          (
+            (DxePlatformSaPolicy->MiscConfig->SaHdaVerbTable[VerbTableNum].VerbTableHeader.RevisionId == 0xFF) ||
+          (RevisionId == DxePlatformSaPolicy->MiscConfig->SaHdaVerbTable[VerbTableNum].VerbTableHeader.RevisionId)
+        )
+          ) {
+        VerbHeaderTable = &(DxePlatformSaPolicy->MiscConfig->SaHdaVerbTable[VerbTableNum].VerbTableHeader);
+        VerbTable       = DxePlatformSaPolicy->MiscConfig->SaHdaVerbTable[VerbTableNum].VerbTableData;
+        if (VerbTable == 0) {
+          DEBUG ((EFI_D_ERROR, "VerbTableData of VendorID:0x%X is null.\n", VendorDeviceId));
+          Status = EFI_INVALID_PARAMETER;
+          goto ExitInitAudio;
+        }
+
+        DEBUG ((EFI_D_INFO, "Detected SA HDA Codec with verb table, VendorID = 0x%X", VendorDeviceId));
+        DEBUG ((EFI_D_INFO, " on SDI%d, revision = 0x%0x.\n", AudioSDINo, RevisionId));
+        ///
+        /// Enable 3rd Pin and Converter Widget
+        /// Clear CAd Field
+        ///
+        CodecCmdData = SA_HDA_ENABLE_3RD_PIN_WIDGET;
+        ///
+        /// Program CAd Field per the SDI number got during codec detection
+        ///
+        CodecCmdData |= (UINT32) (AudioSDINo << 28);
+        Status = SendCodecCommand (HdaBar, &CodecCmdData, FALSE);
+        if (EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_ERROR, "Error occurs at loading Verb 0x00878101"));
+          ///
+          /// Skip the verb table loading when find the verb table content is not
+          /// properly matched with the HDA hardware, though IDs match.
+          ///
+          DEBUG (
+            (EFI_D_ERROR,
+            "Detected Codec of VendorID:0x%X, error occurs during loading verb table.\n",
+            VendorDeviceId)
+            );
+          goto ExitInitAudio;
+        }
+
+        SendCodecCommandS3Item (HdaBar);
+        ///
+        /// Send the entire list of verbs in the matching verb table one by one to the codec
+        ///
+        for (Index = 0; Index < (UINT32) ((VerbHeaderTable->NumberOfFrontJacks + VerbHeaderTable->NumberOfRearJacks) * 4); Index++) {
+          ///
+          /// Clear CAd Field
+          ///
+          CodecCmdData = VerbTable[Index] & (UINT32)~(BIT31 | BIT30 | BIT29 | BIT28);
+          ///
+          /// Program CAd Field per the SDI number got during codec detection
+          ///
+          CodecCmdData |= (UINT32) (AudioSDINo << 28);
+          Status = SendCodecCommand (HdaBar, &CodecCmdData, FALSE);
+          if (EFI_ERROR (Status)) {
+            DEBUG ((EFI_D_ERROR, "Error occurs at loading Command Index:%x\n", Index));
+            ///
+            /// Skip the verb table loading when find the verb table content is not
+            /// properly matched with the HDA hardware, though IDs match.
+            ///
+            DEBUG (
+              (EFI_D_ERROR,
+              "Detected Codec of VendorID:0x%X, error occurs during loading verb table.\n",
+              VendorDeviceId)
+              );
+            goto ExitInitAudio;
+          }
+
+          SendCodecCommandS3Item (HdaBar);
+        }
+        ///
+        /// Disable 3rd Pin and Converter Widget
+        /// Clear CAd Field
+        ///
+        CodecCmdData = SA_HDA_DISABLE_3RD_PIN_WIDGET;
+        ///
+        /// Program CAd Field per the SDI number got during codec detection
+        ///
+        CodecCmdData |= (UINT32) (AudioSDINo << 28);
+        Status = SendCodecCommand (HdaBar, &CodecCmdData, FALSE);
+        if (EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_ERROR, "Error occurs at loading verb 0x00878100"));
+          ///
+          /// Skip the verb table loading when find the verb table content is not
+          /// properly matched with the HDA hardware, though IDs match.
+          ///
+          DEBUG (
+            (EFI_D_ERROR,
+            "Detected Codec of VendorID:0x%X, error occurs during loading verb table.\n",
+            VendorDeviceId)
+            );
+          goto ExitInitAudio;
+        }
+
+        SendCodecCommandS3Item (HdaBar);
+
+        DEBUG ((EFI_D_INFO, "Verb Table loading complete to Codec on SDI%d\n", AudioSDINo));
+        break;
+      }
+    }
+
+    if (VerbTableNum >= DxePlatformSaPolicy->MiscConfig->SaHdaVerbTableNum) {
+      DEBUG (
+        (EFI_D_INFO,
+        "Detected SA High Definition Audio Codec, VendorID = 0x%08x on SDI%d,",
+        VendorDeviceId,
+        AudioSDINo)
+        );
+      DEBUG ((EFI_D_INFO, " but no matching verb table found.\n"));
+    }
+  } // end of for
+  Status = EFI_SUCCESS;
+
+ExitInitAudio:
+  ///
+  /// Clear HdaBar and disable memory map access
+  ///
+  MmioAnd16 ((UINTN) (AudioBase + PCI_COMMAND_OFFSET), (UINT16) (~BIT1));
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    (UINTN) (AudioBase + PCI_COMMAND_OFFSET),
+    1,
+    (VOID *) (UINTN) (AudioBase + PCI_COMMAND_OFFSET)
+    );
+
+  MmioWrite32 (AudioBase + SA_HDA_HDBARL, 0);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (AudioBase + SA_HDA_HDBARL),
+    1,
+    (VOID *) (UINTN) (AudioBase + SA_HDA_HDBARL)
+    );
+
+  MmioWrite32 (AudioBase + SA_HDA_HDBARU, 0);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (AudioBase + SA_HDA_HDBARU),
+    1,
+    (VOID *) (UINTN) (AudioBase + SA_HDA_HDBARU)
+    );
+
+  gDS->FreeMemorySpace (
+        BaseAddressBarMem,
+        SA_HDA_HDBAR_SIZE
+        );
+  ///
+  /// Save the final count of Audio Codec Save data in SA NVS
+  ///
+  mSaGlobalNvsAreaProtocol.Area->AudioCodecSaveCount = mVerbCount;
+
+  return Status;
+}
+
+/**
+  Initialize SystemAgent Audio Device/Codec.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy     SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS - Audio/Codec initialization done
+**/
+EFI_STATUS
+AudioInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+{
+  EFI_STATUS  Status;
+
+  if ((DxePlatformSaPolicy->MiscConfig->AudioEnable == 0) || (McD2PciCfg16 (R_SA_IGD_VID) == 0xFFFF)) {
+    DEBUG ((
+      EFI_D_INFO, "Skip Audio Initialization when BIOS option set to disable Or iGfx is not enabled.\n"
+      )
+      );
+    return EFI_SUCCESS;
+  }
+
+  Status = DetectAndInitializeCodec (ImageHandle, DxePlatformSaPolicy);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "SA Audio Codec initialization failure!\n"));
+  }
+
+  DEBUG ((EFI_D_INFO, "AudioInit() End\n"));
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/AudioInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/AudioInit.h
new file mode 100644
index 0000000..cd7bc33
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/AudioInit.h
@@ -0,0 +1,66 @@
+/** @file
+  Header file for initialization of CPU Audio device.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _AUDIO_INIT_H_
+#define _AUDIO_INIT_H_
+
+#include "EdkIIGlueDxe.h"
+#include "Pci22.h"
+#include "SaAccess.h"
+#include "PchAccess.h"
+#include "PchPlatformLib.h"
+#include "EfiScriptLib.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+
+#include EFI_PROTOCOL_DEPENDENCY (BootScriptSave)
+#include EFI_PROTOCOL_DEFINITION (SaPlatformPolicy)
+#include EFI_PROTOCOL_DEFINITION (PciHostBridgeResourceAllocation)
+#include EFI_PROTOCOL_DEPENDENCY (SaGlobalNvsArea)
+
+#define SA_HDA_MAX_LOOP_TIME  10
+#define SA_HDA_WAIT_PERIOD    100
+#define SA_HDA_MAX_SID_NUMBER 4
+#define SA_HDA_MAX_SID_MASK   ((1 << SA_HDA_MAX_SID_NUMBER) - 1)
+
+#define AUDIO_CODEC_MAX_SIZE  0x80
+
+#define SA_HDA_ENABLE_3RD_PIN_WIDGET  0x00878101
+#define SA_HDA_DISABLE_3RD_PIN_WIDGET 0x00878100
+
+///
+/// Functions
+///
+/**
+  Initialize SystemAgent Audio Device/Codec.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy     SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS - Audio/Codec initialization Done.
+**/
+EFI_STATUS
+AudioInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/LegacyRegion.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/LegacyRegion.c
new file mode 100644
index 0000000..5c91336
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/LegacyRegion.c
@@ -0,0 +1,368 @@
+/** @file
+  This code provides a private implementation of the Legacy Region protocol.
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "LegacyRegion.h"
+
+///
+/// Module Global:
+///   Since this driver will only ever produce one instance of the Private Data
+///   protocol you are not required to dynamically allocate the PrivateData.
+///
+LEGACY_REGION_INSTANCE        mPrivateData;
+
+UINT8                         mRomData[16] = {
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00,
+  0x00
+};
+
+UINT8                         mLockData[16] = {
+  0x01,
+  0x10,
+  0x01,
+  0x10,
+  0x01,
+  0x10,
+  0x01,
+  0x10,
+  0x01,
+  0x10,
+  0x01,
+  0x10, /// it was: 0x03, 0x30, 0x03, 0x30, <-- why ??
+  0x10,
+  0x10,
+  0x10,
+  0x10
+};
+
+UINT8                         mUnlockData[16] = {
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x30,
+  0x30,
+  0x30,
+  0x30
+};
+
+UINT8                         mMaskData[16] = {
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x30,
+  0x03,
+  0x00,
+  0x00,
+  0x00,
+  0x00
+};
+
+UINT8                         mReg[16] = {
+  R_SA_PAM1,
+  R_SA_PAM1,
+  R_SA_PAM2,
+  R_SA_PAM2,
+  R_SA_PAM3,
+  R_SA_PAM3,
+  R_SA_PAM4,
+  R_SA_PAM4,
+  R_SA_PAM5,
+  R_SA_PAM5,
+  R_SA_PAM6,
+  R_SA_PAM6,
+  R_SA_PAM0,
+  R_SA_PAM0,
+  R_SA_PAM0,
+  R_SA_PAM0
+};
+
+EFI_STATUS
+LegacyRegionManipulation (
+  IN  EFI_LEGACY_REGION_PROTOCOL  * This,
+  IN  UINT32                      Start,
+  IN  UINT32                      Length,
+  IN  UINT32                      Mode,
+  OUT UINT32                      *Granularity OPTIONAL
+  )
+/**
+  Modify PAM registers for region specified to MODE state.
+
+  @param[in] This    - Pointer to EFI_LEGACY_REGION_PROTOCOL instance.
+  @param[in] Start   - Starting address of a memory region covered by PAM registers (C0000h - FFFFFh).
+  @param[in] Length  - Memory region length.
+  @param[in] Mode    - Action to perform on a PAM region: UNLOCK, LOCK or BOOTLOCK.
+  @param[out] Granularity - Granularity of region in bytes.
+
+  @retval EFI_SUCCESS - PAM region(s) state modified as requested.
+  @retval EFI_INVALID_PARAMETER - parameter out of boundary
+**/
+{
+  UINT8                           Index;
+  UINT8                           Data;
+  UINT8                           TempData;
+  UINT32                          TempAddr;
+  UINT32                          NewStartAddr;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *Pci;
+  EFI_CPU_ARCH_PROTOCOL           *CpuArch;
+  UINT64                          PciAddress;
+  UINT64                          Attributes;
+  EFI_STATUS                      Status;
+
+  Pci                     = mPrivateData.PciRootBridgeIo;
+  if ((Start  < 0xC0000) ||
+      (Start  > 0xFFFFF) ||
+      (Length > 0x40000) ||
+      ((Start + Length - 1) > 0xFFFFF)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  TempAddr                = Start;
+  Index                   = (UINT8) ((TempAddr - 0xC0000) / PAM_GRANULARITY);
+  NewStartAddr            = TempAddr = (TempAddr / PAM_GRANULARITY) * PAM_GRANULARITY;
+  while (TempAddr <= (Start + Length - 1)) {
+    if (Index >= 16) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if ((Mode == LOCK) || (Mode == BOOTLOCK)) {
+      Data = mLockData[Index];
+    } else {
+      if (Mode == UNLOCK) {
+        Data = mUnlockData[Index];
+      } else {
+        Data = mRomData[Index];
+      }
+    }
+
+    PciAddress = EFI_PCI_ADDRESS (0, 0, 0, mReg[Index]);
+    Pci->Pci.Read (Pci, EfiPciWidthUint8, PciAddress, 1, &TempData);
+    TempData = (UINT8) ((TempData & mMaskData[Index]) | Data);
+    Pci->Pci.Write (Pci, EfiPciWidthUint8, PciAddress, 1, &TempData);
+    TempAddr += PAM_GRANULARITY;
+    Index++;
+  }
+
+  if (Granularity != NULL) {
+    *Granularity = PAM_GRANULARITY;
+  }
+  ///
+  ///  Program the MTRRs
+  ///
+  switch (Mode) {
+
+  case UNLOCK:
+    Attributes = EFI_MEMORY_WT;
+    break;
+
+  case LOCK:
+    Attributes = EFI_MEMORY_WP;
+    break;
+
+  case BOOTLOCK:
+    Attributes = EFI_MEMORY_WP;
+    break;
+
+  default:
+    Attributes = EFI_MEMORY_UC;
+
+  }
+
+  Status = gBS->LocateProtocol (
+                  &gEfiCpuArchProtocolGuid,
+                  NULL,
+                  (VOID **) &CpuArch
+                  );
+  ASSERT_EFI_ERROR (Status);
+  Status = CpuArch->SetMemoryAttributes (
+                      CpuArch,
+                      NewStartAddr,
+                      TempAddr - NewStartAddr,
+                      Attributes
+                      );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+LegacyRegionDecode (
+  IN  EFI_LEGACY_REGION_PROTOCOL   *This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  IN  BOOLEAN                      *On
+  )
+/**
+  Enable/Disable decoding of the given region
+
+  @param[in] This    - Pointer to EFI_LEGACY_REGION_PROTOCOL instance.
+  @param[in] Start  - Starting address of region.
+  @param[in] Length - Length of region in bytes.
+  @param[in] On     - 0 = Disable decoding, 1 = Enable decoding.
+
+  @retval EFI_SUCCESS - Decoding change affected.
+**/
+{
+  UINT32  Granularity;
+  if (*On) {
+    return LegacyRegionManipulation (This, Start, Length, UNLOCK, &Granularity);
+  } else {
+    return LegacyRegionManipulation (This, Start, Length, LEGACY_REGION_DECODE_ROM, &Granularity);
+  }
+}
+
+EFI_STATUS
+EFIAPI
+LegacyRegionBootLock (
+  IN  EFI_LEGACY_REGION_PROTOCOL   * This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  OUT UINT32                       *Granularity OPTIONAL
+  )
+/**
+  Make the indicated region read from RAM / write to ROM.
+
+  @param[in] This        - Pointer to EFI_LEGACY_REGION_PROTOCOL instance.
+  @param[in] Start       - Starting address of region.
+  @param[in] Length      - Length of region in bytes.
+  @param[out] Granularity - Granularity of region in bytes.
+
+  @retval EFI_SUCCESS - Region locked or made R/O.
+**/
+{
+  return LegacyRegionManipulation (This, Start, Length, BOOTLOCK, Granularity);
+}
+
+EFI_STATUS
+EFIAPI
+LegacyRegionLock (
+  IN  EFI_LEGACY_REGION_PROTOCOL   * This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  OUT UINT32                       *Granularity OPTIONAL
+  )
+/**
+  Make the indicated region read from RAM / write to ROM.
+
+  @param[in] This    - Pointer to EFI_LEGACY_REGION_PROTOCOL instance.
+  @param[in] Start       - Starting address of region.
+  @param[in] Length      - Length of region in bytes.
+  @param[out] Granularity - Granularity of region in bytes.
+
+  @retval EFI_SUCCESS - Region locked or made R/O.
+**/
+{
+  return LegacyRegionManipulation (This, Start, Length, LOCK, Granularity);
+}
+
+EFI_STATUS
+EFIAPI
+LegacyRegionUnlock (
+  IN  EFI_LEGACY_REGION_PROTOCOL   * This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  OUT UINT32                       *Granularity OPTIONAL
+  )
+/**
+  Make the indicated region read from RAM / write to RAM.
+
+  @param[in] This    - Pointer to EFI_LEGACY_REGION_PROTOCOL instance.
+  @param[in] Start       - Starting address of region.
+  @param[in] Length      - Length of region in bytes.
+  @param[out] Granularity - Granularity of region in bytes.
+
+  @retval EFI_SUCCESS - Region unlocked or made R/W.
+**/
+{
+  return LegacyRegionManipulation (This, Start, Length, UNLOCK, Granularity);
+}
+
+EFI_STATUS
+LegacyRegionInstall (
+  IN EFI_HANDLE           ImageHandle
+  )
+/**
+  Install Driver to produce Legacy Region protocol.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+
+  @retval EFI_SUCCESS - Legacy Region protocol installed
+  @retval Other       - No protocol installed, unload driver.
+**/
+{
+  EFI_STATUS              Status;
+  LEGACY_REGION_INSTANCE  *Private;
+  Private = &mPrivateData;
+
+  ///
+  /// Grab a copy of all the protocols we depend on. Any error would
+  /// be a dispatcher bug!.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  NULL,
+                  (VOID **) &Private->PciRootBridgeIo
+                  );
+  Private->Signature              = LEGACY_REGION_INSTANCE_SIGNATURE;
+  Private->LegacyRegion.Decode    = LegacyRegionDecode;
+  Private->LegacyRegion.Lock      = LegacyRegionLock;
+  Private->LegacyRegion.BootLock  = LegacyRegionBootLock;
+  Private->LegacyRegion.UnLock    = LegacyRegionUnlock;
+  Private->ImageHandle            = ImageHandle;
+
+  ///
+  /// Make a new handle and install the protocol
+  ///
+  Private->Handle = NULL;
+  return gBS->InstallProtocolInterface (
+                &Private->Handle,
+                &gEfiLegacyRegionProtocolGuid,
+                EFI_NATIVE_INTERFACE,
+                &Private->LegacyRegion
+                );
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/LegacyRegion.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/LegacyRegion.h
new file mode 100644
index 0000000..d2e9b2d
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/LegacyRegion.h
@@ -0,0 +1,76 @@
+/** @file
+  This code supports a private implementation of the Legacy Region protocol.
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _LEGACY_REGION_H_
+#define _LEGACY_REGION_H_
+
+#include "EdkIIGlueDxe.h"
+#include "Pci22.h"
+
+#include EFI_PROTOCOL_PRODUCER (LegacyRegion)
+#include EFI_PROTOCOL_DEFINITION (PciRootBridgeIo)
+#define LEGACY_REGION_INSTANCE_SIGNATURE  EFI_SIGNATURE_32 ('R', 'E', 'G', 'N')
+
+#include EFI_ARCH_PROTOCOL_CONSUMER (Cpu)
+#define LEGACY_REGION_DECODE_ROM  3
+
+#include "SaAccess.h"
+
+///
+/// PAM registers granularity is 16 KB
+///
+#define PAM_GRANULARITY 0x4000
+#define UNLOCK          0x0000
+#define LOCK            0x0001
+#define BOOTLOCK        0x0002
+
+typedef struct {
+  UINT32                          Signature;
+  EFI_HANDLE                      Handle;
+  EFI_LEGACY_REGION_PROTOCOL      LegacyRegion;
+  EFI_HANDLE                      ImageHandle;
+
+  ///
+  /// Protocol for PAM register access
+  ///
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+} LEGACY_REGION_INSTANCE;
+
+#define LEGACY_REGION_INSTANCE_FROM_THIS  (this) CR (this, \
+                                                     LEGACY_REGION_INSTANCE, \
+                                                     LegacyRegion, \
+                                                     LEGACY_REGION_INSTANCE_SIGNATURE \
+        )
+
+/**
+  Install Driver to produce Legacy Region protocol.
+
+  @param[in] ImageHandle             Handle for the image of this driver
+
+  @retval EFI_SUCCESS - Legacy Region protocol installed
+  @retval Other       - No protocol installed, unload driver.
+**/
+EFI_STATUS
+LegacyRegionInstall (
+  IN EFI_HANDLE           ImageHandle
+  )
+;
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PciExpressInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PciExpressInit.c
new file mode 100644
index 0000000..921ae94
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PciExpressInit.c
@@ -0,0 +1,483 @@
+/** @file
+  This driver does SA PCI Express initialization.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "PciExpressInit.h"
+
+extern SYSTEM_AGENT_GLOBAL_NVS_AREA_PROTOCOL  mSaGlobalNvsAreaProtocol;
+
+EFI_STATUS
+PciExpressInit (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  PCI Express Dxe Initialization.
+  Run before PCI Bus Init, where assignment of Bus, Memory,
+    and I/O Resources are assigned.
+
+  @param[in] DxePlatformSaPolicy -     SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS     - Pci Express successfully started and ready to be used
+  @exception EFI_UNSUPPORTED - Pci Express can't be initialized
+**/
+{
+  EFI_STATUS  Status;
+  UINT64      EgressPortBar;
+  UINT64      MchBar;
+  UINT64      PciExpressBar;
+  UINT64      DmiBar;
+  UINT32      PchRootComplexBar;
+  CPU_FAMILY  CpuFamilyId;
+  CPU_STEPPING CpuSteppingId;
+
+  CpuFamilyId = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  ///
+  /// The SA Represents Component ID 1 (CID1), while the PCH represents
+  /// Component ID 2 (CID2).  This code will completely configure both Components
+  /// CID1 Integrated Ports:
+  ///    Egress Port = Port 0
+  ///    DMI Port    = Port 1
+  ///    Peg Port10  = Port 2
+  ///    Peg Port11  = Port 3
+  ///    Peg Port12  = Port 4
+  /// CID2 Integated Ports:
+  ///    Egress Port = Port 0
+  ///    Root Port 1 = Port 1
+  ///    Root Port 2 = Port 2
+  ///    Root Port 3 = Port 3
+  ///    Root Port 4 = Port 4
+  ///    Azalia      = Port 5
+  ///
+  ///
+  /// Get BAR registers
+  ///
+  PchRootComplexBar = MmPci32 (0, 0, 31, 0, 0xF0) &~BIT0;
+  PciExpressBar     = McD0PciCfg64 (R_SA_PCIEXBAR) & (B_SA_PCIEXBAR_PCIEXBAR_MASK | B_SA_PCIEXBAR_ADMSK128_MASK | B_SA_PCIEXBAR_ADMSK64_MASK);
+  EgressPortBar     = McD0PciCfg64 (R_SA_PXPEPBAR) &~BIT0;
+  MchBar            = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+  DmiBar            = McD0PciCfg64 (R_SA_DMIBAR) &~BIT0;
+
+  ///
+  /// Configure the Egress Port (0) in CID1
+  ///
+  Status = Cid1EgressPort0Init (EgressPortBar);
+
+#ifdef PEG_FLAG
+  if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuCrw)) {
+    ///
+    /// Configure the PEG Ports in CID1
+    ///
+    Status = Cid1PegPortInit (DxePlatformSaPolicy);
+  }
+#endif // PEG_FLAG
+
+  ///
+  /// Configure the SA DMI Port (1) in CID1 and the PCH DMI Port (0) in CID2.
+  /// Note that both links must be fully configured before the link
+  /// should be checked for negotiation between the 2 CIDs.
+  ///
+  DEBUG ((EFI_D_INFO, " Going to call Cid1Cid2DmiPortInit\n"));
+  Status = Cid1Cid2DmiPortInit (DmiBar, DxePlatformSaPolicy);
+
+  ///
+  /// Configure the RootComplex Topology in CID1.
+  ///
+  DEBUG ((EFI_D_INFO, " Going to call Cid1TopologyInit\n"));
+  Status = Cid1TopologyInit (EgressPortBar, DmiBar, PchRootComplexBar);
+
+  ///
+  /// Configure the RootComplex Topology in CID2.
+  ///
+  DEBUG ((EFI_D_INFO, " Going to call Cid2TopologyInit\n"));
+  Status = Cid2TopologyInit (PchRootComplexBar, DmiBar);
+
+  if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId >= EnumHswC0)) ||
+      ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId >= EnumCrwC0))) {
+    if (DxePlatformSaPolicy->Revision >= DXE_SA_PLATFORM_POLICY_PROTOCOL_REVISION_7) {
+      mSaGlobalNvsAreaProtocol.Area->C7Allowed  = DxePlatformSaPolicy->PcieConfig->C7Allowed; //Update the Run-time C7 Allowed for ASL usage
+    }
+  }
+
+  mSaGlobalNvsAreaProtocol.Area->PackageCstateLimit  = (UINT8)(AsmReadMsr64(MSR_PMG_CST_CONFIG) & B_PACKAGE_C_STATE_LIMIT);
+
+
+  return Status;
+}
+
+UINT32
+PcieFindCapId (
+  IN UINT8   Bus,
+  IN UINT8   Device,
+  IN UINT8   Function,
+  IN UINT8   CapId
+  )
+/**
+  Find the Offset to a given Capabilities ID
+  CAPID list:
+    0x01 = PCI Power Management Interface
+    0x04 = Slot Identification
+    0x05 = MSI Capability
+    0x10 = PCI Express Capability
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+  @param[in] Function  -   Pci Function Number
+  @param[in] CapId     -   CAPID to search for
+
+  @retval 0       - CAPID not found
+  @retval Other   - CAPID found, Offset of desired CAPID
+**/
+{
+  UINT8 CapHeader;
+
+  ///
+  /// Always start at Offset 0x34
+  ///
+  CapHeader = MmPci8 (0, Bus, Device, Function, PCI_CAPP);
+  if (CapHeader == 0xFF) {
+    return 0;
+  }
+
+  while (CapHeader != 0) {
+    ///
+    /// Bottom 2 bits of the pointers are reserved per PCI Local Bus Spec 2.2
+    ///
+    CapHeader &= ~(BIT1 + BIT0);
+    ///
+    /// Search for desired CapID
+    ///
+    if (MmPci8 (0, Bus, Device, Function, CapHeader) == CapId) {
+      return CapHeader;
+    }
+
+    CapHeader = MmPci8 (0, Bus, Device, Function, CapHeader + 1);
+  }
+
+  return 0;
+}
+
+UINT32
+PcieFindExtendedCapId (
+  IN UINT8   Bus,
+  IN UINT8   Device,
+  IN UINT8   Function,
+  IN UINT16  CapId
+  )
+/**
+  Search and return the offset of desired Pci Express Capability ID
+  CAPID list:
+    0x0001 = Advanced Error Rreporting Capability
+    0x0002 = Virtual Channel Capability
+    0x0003 = Device Serial Number Capability
+    0x0004 = Power Budgeting Capability
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+  @param[in] Function  -   Pci Function Number
+  @param[in] CapId     -   Extended CAPID to search for
+
+  @retval 0       - CAPID not found
+  @retval Other   - CAPID found, Offset of desired CAPID
+**/
+{
+  UINT16  CapHeaderOffset;
+  UINT16  CapHeaderId;
+
+  ///
+  /// Start to search at Offset 0x100
+  /// Get Capability Header
+  ///
+  CapHeaderId     = 0;
+  CapHeaderOffset = 0x100;
+
+  while (CapHeaderOffset != 0 && CapHeaderId != 0xFFFF) {
+    ///
+    /// Search for desired CapID
+    ///
+    CapHeaderId = MmPci16 (0, Bus, Device, Function, CapHeaderOffset);
+    if (CapHeaderId == CapId) {
+      return CapHeaderOffset;
+    }
+
+    CapHeaderOffset = (MmPci16 (0, Bus, Device, Function, CapHeaderOffset + 2) >> 4);
+  }
+
+  return 0;
+}
+
+VOID
+PcieSetClkreq (
+  IN  UINT8   Bus,
+  IN  UINT8   Device
+  )
+/**
+  This function enables the CLKREQ# PM on all the end point functions
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+**/
+{
+  UINT8   Function;
+  UINT32  CapOffset;
+
+  ///
+  /// Parse through all the functions of the endpoint and find the PCIe Cap ID (offset 10h) and if
+  /// exists then enable the CLKREQ# bit (BIT8) on that function
+  ///
+  for (Function = 0; Function < 8; Function++) {
+    ///
+    /// Find the PCIe Cap Id (offset 10h)
+    ///
+    CapOffset = PcieFindCapId (Bus, Device, Function, EFI_PCI_CAPABILITY_ID_PCIEXP);
+    if (CapOffset == 0) {
+      continue;
+    }
+    ///
+    /// Check if CLKREQ# is supported by the endpoints
+    ///
+    if ((MmPci32 (0, Bus, Device, Function, (CapOffset + 0x0C)) & BIT18) != BIT18) {
+      ///
+      /// CLKREQ# is not supported so dont do anything
+      ///
+      return ;
+    }
+  }
+  ///
+  /// Now enable the CLKREQ#
+  ///
+  for (Function = 0; Function < 8; Function++) {
+    ///
+    /// Find the PCIe Cap Id (offset 10h)
+    ///
+    CapOffset = PcieFindCapId (Bus, Device, Function, EFI_PCI_CAPABILITY_ID_PCIEXP);
+    if (CapOffset == 0) {
+      continue;
+    }
+
+    MmPci16Or (0, Bus, Device, Function, (CapOffset + 0x010), BIT8);
+  }
+}
+
+VOID
+AdditionalDMIProgramStepsBeforeASPM (
+  IN UINT64                          DmiBar,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Additional DMI Programming Steps as in SA BIOS spec
+
+  @param[in] DmiBar              - DMIBAR Address
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+**/
+{
+  UINT32             Data32And;
+  UINT32             Data32Or;
+
+  ///
+  /// Configure the De-emphasis control on DMI
+  ///
+  Data32And = (UINT32) ~BIT6;
+  Data32Or  = (UINT32) ((DxePlatformSaPolicy->PcieConfig->DmiDeEmphasis & 0x1) << 6);
+  Mmio32AndThenOr (DmiBar, R_SA_DMIBAR_LCTL2_OFFSET, Data32And, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + R_SA_DMIBAR_LCTL2_OFFSET),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Set 0x22C[31] to 1 before enabling ASPM
+  ///
+  Data32And = (UINT32) ~(BIT31);
+  Data32Or  = (UINT32)   BIT31;
+  Mmio32AndThenOr (DmiBar, R_SA_DMIBAR_L0SLAT_OFFSET, Data32And, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + R_SA_DMIBAR_L0SLAT_OFFSET),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Set 0x238 bit29 for DMI before enabling ASPM
+  ///
+  Data32And = 0xFFFFFFFF;
+  Data32Or  = BIT29;
+  Mmio32Or (DmiBar, 0x238, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x238),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Set DMI Offset 0xC28 [4:0]
+  ///
+  Data32And = 0xFFFFFFE0;
+  Data32Or  = 0x13;
+  Mmio32AndThenOr (DmiBar, R_SA_PEG_AFE_PM_TMR_OFFSET, Data32And, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + R_SA_PEG_AFE_PM_TMR_OFFSET),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+}
+
+VOID
+AdditionalPEGProgramStepsBeforeASPM (
+  IN  UINT8                          Bus,
+  IN  UINT8                          pegDev,
+  IN  UINT8                          pegFn,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Additional PEG Programming Steps as in SA BIOS spec
+
+  @param[in] Bus         - Pci Bus Number
+  @param[in] pegDev      - Pci Device Number
+  @param[in] pegFn       - Pci Func Number
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+**/
+{
+  UINT32             Data32Or;
+  UINT32             Data32And;
+  UINT32             PegBaseAddress;
+
+  PegBaseAddress = (UINT32) MmPciAddress (0x0, Bus, pegDev, pegFn, 0x0);
+
+  ///
+  /// Permanently set PEGUESEV[CTS](0x1CC bit14) to 1b during BIOS boot for all the PEG controllers
+  ///
+  Data32And = 0xFFFFFFFF;
+  Data32Or  = BIT14;
+  Mmio32Or (PegBaseAddress, 0x1CC, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PegBaseAddress + 0x1CC),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Configure the De-emphasis control on PEG
+  ///
+  Data32And = (UINT32)~BIT6;
+  Data32Or  = (DxePlatformSaPolicy->PcieConfig->PegDeEmphasis[pegFn] & BIT0) << 6;
+  Mmio32AndThenOr (PegBaseAddress, R_SA_PEG_LCTL2_OFFSET, Data32And, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PegBaseAddress + R_SA_PEG_LCTL2_OFFSET),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Set 0x22C[31] to 1 before enabling ASPM
+  ///
+  Data32And = (UINT32) ~(BIT31);
+  Data32Or  = (UINT32)   BIT31;
+  Mmio32AndThenOr (PegBaseAddress, R_SA_PEG_L0SLAT_OFFSET, Data32And, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PegBaseAddress + R_SA_PEG_L0SLAT_OFFSET),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Program 0x250 bit[22:20] to 010b for all the PEG controllers before enabling ASPM
+  ///
+  Data32And = (UINT32)~(BIT22 + BIT21 + BIT20);
+  Data32Or  = 0x02 << 20;
+  Mmio32AndThenOr (PegBaseAddress, 0x250, Data32And, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PegBaseAddress + 0x250),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Set 0x238 bit29 for PEG controller before enabling ASPM
+  ///
+  Data32And = 0xFFFFFFFF;
+  Data32Or  = BIT29;
+  Mmio32Or (PegBaseAddress, 0x238, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PegBaseAddress + 0x238),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Set PEG Offset 0x1F8 bit16
+  ///
+  Data32And = 0xFFFFFFFF;
+  Data32Or  = BIT16;
+  Mmio32Or (PegBaseAddress, 0x1F8, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PegBaseAddress + 0x1F8),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+
+  ///
+  /// Set PEG Offset 0xC28 [4:0]
+  ///
+  Data32And = 0xFFFFFFE0;
+  Data32Or  = 0x13;
+  Mmio32AndThenOr (PegBaseAddress, R_SA_PEG_AFE_PM_TMR_OFFSET, Data32And, Data32Or);
+
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PegBaseAddress + R_SA_PEG_AFE_PM_TMR_OFFSET),
+    &Data32Or,  ///< Data to be ORed
+    &Data32And  ///< Data to be ANDed
+    );
+}
+
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PciExpressInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PciExpressInit.h
new file mode 100644
index 0000000..05037b9
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PciExpressInit.h
@@ -0,0 +1,224 @@
+/** @file
+  Header file for PciExpress Initialization Driver.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _PCIEXPRESS_INITIALIZATION_DRIVER_H_
+#define _PCIEXPRESS_INITIALIZATION_DRIVER_H_
+
+#include "EdkIIGlueDxe.h"
+#include "SaAccess.h"
+#include "PchAccess.h"
+#include "pci23.h"
+#include "EfiScriptLib.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+#include "cpu.h"
+
+#include EFI_PROTOCOL_CONSUMER (ExitPmAuth)
+#include EFI_PROTOCOL_DEPENDENCY (BootScriptSave)
+#include EFI_PROTOCOL_CONSUMER (SaPlatformPolicy)
+#include EFI_PROTOCOL_DEPENDENCY (SaGlobalNvsArea)
+
+#define DISABLED  0
+#define AUTO      1
+#define ENABLED   1
+
+#define GEN1      1
+#define GEN2      2
+
+typedef struct {
+  UINT8 Bus;
+  UINT8 Device;
+  UINT8 Function;
+  UINT8 Slot;
+  UINT8 Bus2;
+  UINT8 Device2;
+  UINT8 Function2;
+} PEG_PORT_DEVICE;
+
+///
+/// Function prototypes
+///
+/**
+  PCI Express Dxe Initialization.
+  Run before PCI Bus Init, where assignment of Bus, Memory,
+    and I/O Resources are assigned.
+
+  @param[in] DxePlatformSaPolicy -     SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS     - Pci Express successfully started and ready to be used
+  @exception EFI_UNSUPPORTED - Pci Express can't be initialized
+**/
+EFI_STATUS
+PciExpressInit (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+
+/**
+  Find the Offset to a given Capabilities ID
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+  @param[in] Function  -   Pci Function Number
+  @param[in] CapId     -   CAPID to search fo
+
+  @retval 0       - CAPID not found
+  @retval Other   - CAPID found, Offset of desired CAPID
+**/
+UINT32
+PcieFindCapId (
+  IN UINT8   Bus,
+  IN UINT8   Device,
+  IN UINT8   Function,
+  IN UINT8   CapId
+  )
+;
+
+/**
+  Search and return the offset of desired Pci Express Capability ID
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+  @param[in] Function  -   Pci Function Number
+  @param[in] CapId     -   Extended CAPID to search for
+
+  @retval 0       - CAPID not found
+  @retval Other   - CAPID found, Offset of desired CAPID
+**/
+UINT32
+PcieFindExtendedCapId (
+  IN UINT8   Bus,
+  IN UINT8   Device,
+  IN UINT8   Function,
+  IN UINT16  CapId
+  )
+;
+
+/**
+  This function enables the CLKREQ# PM on all the end point functions
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+**/
+VOID
+PcieSetClkreq (
+  IN  UINT8   Bus,
+  IN  UINT8   Device
+  )
+;
+
+/**
+  Perform Egress Port 0 Initialization.
+
+  @param[in] EgressPortBar   - EPBAR Address
+**/
+EFI_STATUS
+Cid1EgressPort0Init (
+  IN  UINT64      EgressPortBar
+  )
+;
+
+#ifdef PEG_FLAG
+/**
+  Conditionally perform PEG Port Initialization.
+  bugbug: organize this code in a way that can utilize the
+  PchS3ItemTypeInitPcieRootPortDownstream EFI_PCH_S3_DISPATCH_ITEM_TYPE
+
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+**/
+EFI_STATUS
+Cid1PegPortInit (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+#endif // PEG_FLAG
+
+/**
+  DMI Port Initialization for both CID1 (Port 1 in MCH) and CID2 (Port 0 in ICH).
+
+  @param[in] DmiBar              - DMIBAR Address
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+**/
+EFI_STATUS
+Cid1Cid2DmiPortInit (
+  IN  UINT64                         DmiBar,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+
+/**
+  Perform Root Complex Topology Initialization for CID1.
+
+  @param[in] EgressPortBar     - EPBAR Address
+  @param[in] DmiBar            - DMIBAR Address
+  @param[in] PchRootComplexBar - ICH RCBA Address
+**/
+EFI_STATUS
+Cid1TopologyInit (
+  IN UINT64        EgressPortBar,
+  IN  UINT64       DmiBar,
+  IN  UINT32       PchRootComplexBar
+  )
+;
+
+/**
+  Perform Root Complex Topology Initialization for CID2.
+
+  @param[in] DmiBar            - DMIBAR Address
+  @param[in] PchRootComplexBar - ICH RCBA Address
+**/
+EFI_STATUS
+Cid2TopologyInit (
+  IN UINT32   PchRootComplexBar,
+  IN UINT64   DmiBar
+  )
+;
+
+/**
+  Additional PEG Programming Steps as in SA BIOS spec
+
+  @param[in] Bus         - Bus Number of PEG device
+  @param[in] pegDev      - Dev Number of PEG device
+  @param[in] pegFn       - Func Number of PEG device
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+**/
+VOID
+AdditionalPEGProgramStepsBeforeASPM (
+  IN  UINT8                          Bus,
+  IN  UINT8                          pegDev,
+  IN  UINT8                          pegFn,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+
+/**
+  Additional DMI Programming Steps as in SA BIOS spec
+
+  @param[in] DmiBar              - DMIBAR Address
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+**/
+VOID
+AdditionalDMIProgramStepsBeforeASPM (
+  IN UINT64                          DmiBar,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PcieComplex.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PcieComplex.c
new file mode 100644
index 0000000..bf4b367
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PcieComplex.c
@@ -0,0 +1,1371 @@
+/** @file
+  This file will perform SA PCIE Root Complex initialization.
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "SaBuildFlags.h"
+#include "PciExpressInit.h"
+#include "EdkIIGluePcdPciExpressLib.h"
+#include "SaPcieLib.h"
+#include "PcieComplex.h"
+#include <Token.h>
+///
+/// Global variables
+///
+UINT8                                  HwStrap;
+extern UINT16                          mSaIotrapSmiAddress;
+extern BOOLEAN                         mInitPcieAspmAfterOprom;
+extern DXE_PLATFORM_SA_POLICY_PROTOCOL *mDxePlatformSaPolicy;
+
+#ifdef PEG_FLAG
+PEG_PORT_DEVICE PegDeviceTable[] = {
+  ///
+  ///          Bus,           Device,         Function, Slot, Bus2, Device2, Function2
+  ///
+  { SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM,    1,    2,       0,         0 }
+// AMI_OVERRID >>  
+#if RC_PEG_1 == 1
+  ,{ SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM,    2,    3,       0,         0 }
+#endif
+#if RC_PEG_2 == 1
+  ,{ SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM,    3,    4,       0,         0 }
+#endif
+// AMI_OVERRID <<
+};
+#endif // PEG_FLAG
+
+///
+/// Functions
+///
+VOID
+EFIAPI
+SaLateInitSmiCallback (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+/**
+    This function gets registered as a callback to perform all SA late initialization
+
+    @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.
+**/
+{
+  if (mSaIotrapSmiAddress != 0) {
+    DEBUG ((EFI_D_INFO, "[SA] Issue IOTRAP SMI %X\n", mSaIotrapSmiAddress));
+    IoWrite8 (mSaIotrapSmiAddress, 0);
+  }
+  if (Event != NULL) {
+    gBS->CloseEvent(Event);
+  }
+  return;
+}
+
+EFI_STATUS
+Cid1EgressPort0Init (
+  IN  UINT64      EgressPortBar
+  )
+/**
+    Perform Egress Port 0 Initialization.
+
+    @param[in] EgressPortBar   - EPBAR Address
+
+    @retval EFI_SUCCESS        - Egress Port 0 initialization successed.
+**/
+{
+  UINT32  Data32And;
+  UINT32  Data32Or;
+  UINT8   Data8And;
+  UINT8   Data8Or;
+  UINT8   BitMask;
+  UINT8   BitValue;
+
+  ///
+  /// Egress Port Configuration
+  ///
+  /// Egress Port Virtual Channel 0 Configuration
+  ///   System BIOS must insure that only TC0 is mapped to VC0.
+  ///   a. Set the Egress Port Register EPBAR offset 014h[7:1]=0000000b
+  ///
+  Data32And = BIT0;
+  Data32Or  = 0;
+  Mmio32And (EgressPortBar, 0x14, Data32And);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (EgressPortBar + 0x14),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// System BIOS must program the extended VC count field.
+  ///   b. Set the Egress Port Register EPBAR offset 004h[2:0]=001b
+  ///
+  Data8And  = (UINT8)~(0x07);
+  Data8Or   = BIT0;
+  Mmio8AndThenOr (EgressPortBar, 0x4, Data8And, Data8Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint8,
+    (UINTN) (EgressPortBar + 0x4),
+    &Data8Or,
+    &Data8And
+    );
+
+  ///
+  /// Egress Port VC1 Configuration
+  ///   a. Assign Virtual Channel ID 1 to VC1: by programming PXEPBAR Offset 020[26:24] = '001b'
+  ///   b. Select and map TC1 to VC1: by programming PXPEPBAR Offset 020h[7:1] = '0000001b'
+  ///
+  Data32And = (UINT32)~(0x070000FE);
+  Data32Or  = ((0x01 << 24) + BIT1);
+  Mmio32AndThenOr (EgressPortBar, 0x20, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (EgressPortBar + 0x20),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// c. Enable VC1 (no hardware behind this bit, s/w compatibility flag only) BIT31
+  ///    Program EXPEPBAR Offset 020h[31]=1
+  ///
+  Data32And = 0xFFFFFFFF;
+  Data32Or  = BIT31;
+  Mmio32Or (EgressPortBar, 0x20, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (EgressPortBar + 0x20),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// d. Poll the VC1 Negotiation Pending bit until it reads 0:
+  ///    Read the Egress Port Register EPBAR Offset 026h until [1]==0
+  ///
+  BitMask   = (UINT8) BIT1;
+  BitValue  = 0;
+  while ((Mmio8 (EgressPortBar, 0x26) & BitMask) != BitValue) {
+  };
+  SCRIPT_MEM_POLL (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint8,
+    (UINTN) (EgressPortBar + 0x26),
+    &BitMask,
+    &BitValue,
+    50,
+    200000
+    );
+
+  return EFI_SUCCESS;
+}
+
+#ifdef PEG_FLAG
+EFI_STATUS
+Cid1PegPortInit (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+    Conditionally perform PEG Port Initialization.
+    bugbug: organize this code in a way that can utilize the
+    PchS3ItemTypeInitPcieRootPortDownstream EFI_PCH_S3_DISPATCH_ITEM_TYPE
+
+    @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+    @retval EFI_SUCCESS            - PEG Port initialization successed.
+**/
+{
+  UINT32              Data32;
+  UINT32              Data32Or;
+  UINT32              Data32And;
+  UINT16              Data16Or;
+  UINT16              Data16And;
+  UINT8               Data8;
+  UINT32              PegBaseAddress;
+  UINT8               Bus;
+  UINT8               Dev;
+  UINT8               Func;
+  UINT8               Slot;
+  UINT8               Bus2;
+  UINT8               Dev2;
+  UINT8               Func2;
+  UINT8               PegComplete;
+  CPU_FAMILY          CpuFamilyId;
+  CPU_STEPPING        CpuSteppingId;
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  ///
+  /// Read HwStrap Register - PEG1CFGSEL D1.R 504h [17:16]
+  ///
+  HwStrap = (UINT8) ((McD1PciCfg32 (R_SA_PEG_FUSESCMN_OFFSET) & (BIT17 + BIT16)) >> 16);
+
+  ///
+  /// HSW - Scan/initialize PEG devices based on HW strapping.
+  ///
+  for (PegComplete = 0; PegComplete < ((sizeof (PegDeviceTable)) / (sizeof (PEG_PORT_DEVICE))); PegComplete++) {
+    ///
+    /// Get Peg Device BDF, Slot# and EndPoint(Temporary)
+    ///
+    Bus             = PegDeviceTable[PegComplete].Bus;
+    Dev             = PegDeviceTable[PegComplete].Device;
+    Func            = PegDeviceTable[PegComplete].Function;
+    Slot            = PegDeviceTable[PegComplete].Slot;
+    Bus2            = PegDeviceTable[PegComplete].Bus2;
+    Dev2            = PegDeviceTable[PegComplete].Device2;
+    Func2           = PegDeviceTable[PegComplete].Function2;
+
+    PegBaseAddress  = (UINT32) MmPciAddress (0, Bus, Dev, Func, 0);
+    ///
+    /// Check if the PEG is Enabled. Since Graphics initialization has already
+    /// occurred, simply check for PEG presence.
+    /// bugbug: need to make sure this dependency is captured in the integration guide
+    ///
+    if (McDevFunPciCfg16 (Bus, Dev, Func, PCI_VID) != 0xFFFF) {
+      ///
+      /// PEG port enable and visible
+      ///
+      ///
+      /// Initialize Slot Implemented for PCI Express Port
+      ///
+      Data16And = 0xFFFF;
+      Data16Or  = BIT8;
+
+      McDevFunPciCfg16Or (Bus, Dev, Func, R_SA_PEG_CAP_OFFSET, Data16Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint16,
+        (UINTN) (PegBaseAddress + R_SA_PEG_CAP_OFFSET),
+        &Data16Or,
+        &Data16And
+        );
+
+      ///
+      /// Initialize "Physical Slot Number" for PCI Express Port
+      ///
+      Data32 = McDevFunPciCfg32 (Bus, Dev, Func, R_SA_PEG_SLOTCAP_OFFSET);
+      Data32 &= 0x0007FFFF;
+      ///
+      /// Set [31:19] - Slot # based on Peg Port
+      ///
+      Data32 |= (Slot << 19);
+
+      ///
+      /// Initialize Slot Power Limit for PCI Express Port and Power Limit Scale.
+      /// Note: this register is a write once.
+      ///
+      /// Set [14:7] - 75 Watts (Default)
+      ///
+      Data32 &= 0xFFFE007F;
+      Data32 |= (75 << 7);
+      ///
+      /// [16:15] - 1.0 Watt Scale
+      ///
+      Data32 |= (0 << 15);
+
+      McDevFunPciCfg32 (Bus, Dev, Func, R_SA_PEG_SLOTCAP_OFFSET) = Data32;
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (PegBaseAddress + R_SA_PEG_SLOTCAP_OFFSET),
+        1,
+        &Data32
+        );
+
+      ///
+      /// Additional Programming Steps
+      ///
+      /// Set PEG D.F.R 006h [15:0] = 0FFFFh
+      ///
+      Data32And = ~(0xFFFF0000);
+      Data32Or  = 0xFFFF0000;
+
+      McDevFunPciCfg32AndThenOr (Bus, Dev, Func, 0x4, Data32And, Data32Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (PegBaseAddress + 0x4),
+        &Data32Or,
+        &Data32And
+        );
+
+      ///
+      /// Set PEG D.F.R 01Eh [15:0] = 0FFFFh
+      ///
+      Data32And = ~(0xFFFF0000);
+      Data32Or  = 0xFFFF0000;
+
+      McDevFunPciCfg32AndThenOr (Bus, Dev, Func, 0x1C, Data32And, Data32Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (PegBaseAddress + 0x1C),
+        &Data32Or,
+        &Data32And
+        );
+
+      ///
+      /// Set PEG D.F.R 0AAh [15:0] = 0FFFFh
+      ///
+      Data32And = 0xFFFF;
+      Data32Or  = 0xFFFF;
+
+      McDevFunPciCfg16Or (Bus, Dev, Func, 0xAA, Data32Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint16,
+        (UINTN) (PegBaseAddress + 0xAA),
+        &Data32Or,
+        &Data32And
+        );
+
+      ///
+      /// Set PEG D.F.R 1C4h [31:0] = 0FFFFFFFFh
+      ///
+      Data32 = 0xFFFFFFFF;
+
+      McDevFunPciCfg32Or (Bus, Dev, Func, R_SA_PEG_PEGUESTS_OFFSET, Data32);
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (PegBaseAddress + R_SA_PEG_PEGUESTS_OFFSET),
+        1,
+        &Data32
+        );
+
+      ///
+      /// Set PEG D.F.R 1D0h [31:0] = 0FFFFFFFFh
+      ///
+      Data32 = 0xFFFFFFFF;
+
+      McDevFunPciCfg32Or (Bus, Dev, Func, R_SA_PEG_PEGCESTS_OFFSET, Data32);
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (PegBaseAddress + R_SA_PEG_PEGCESTS_OFFSET),
+        1,
+        &Data32
+        );
+
+      ///
+      /// Set PEG D.F.R 1F0h [31:0] = 0FFFFFFFFh
+      ///
+      Data32 = 0xFFFFFFFF;
+
+      McDevFunPciCfg32Or (Bus, Dev, Func, 0x1F0, Data32);
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (PegBaseAddress + 0x1F0),
+        1,
+        &Data32
+        );
+      ///
+      /// If HSW CPU steppingId >= B0 or CRW, set BIT19 of DCAP2 register of the PEG port,to enable OBFF support using WAKE# signaling
+      ///
+      if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId >= EnumHswB0)) ||
+           (CpuFamilyId == EnumCpuCrw)) {
+        Data32 = (UINT32) BIT19;
+        McDevFunPciCfg32Or (Bus, Dev, Func, R_SA_PEG_DCAP2_OFFSET, Data32);
+        SCRIPT_MEM_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (PegBaseAddress + R_SA_PEG_DCAP2_OFFSET),
+          1,
+          &Data32
+        );
+      }
+      ///
+      /// Complete Common Port and Endpoint Configuration
+      ///
+      ///
+      /// Virtual Channel Configuration of PCI Express Port
+      /// Set the VC0RCTL register D1:F0 Offset 114h [7:1] = 7Fh
+      ///
+      Data32And = 0xFFFFFF01;
+      Data32Or  = BIT7 | BIT6 | BIT5 | BIT4 | BIT3 | BIT2 | BIT1;
+
+      McDevFunPciCfg32AndThenOr (Bus, Dev, Func, R_SA_PEG_VC0RCTL0_OFFSET, Data32And, Data32Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (PegBaseAddress + R_SA_PEG_VC0RCTL0_OFFSET),
+        &Data32Or,
+        &Data32And
+        );
+      ///
+      /// 6.8 Additional Programming Steps before Enabling ASPM for PEG device
+      ///
+      AdditionalPEGProgramStepsBeforeASPM (Bus, Dev, Func, DxePlatformSaPolicy);
+
+      ///
+      /// 6.10 Interrupt Routing for PCI Express*
+      /// It is recommened to re-route the legacy interrupts (INTA -> INTB,C,D)
+      /// to avoid overcrowded INTA. ACPI PRT needs update.
+      /// The ACPI _PRT() methods for PEG controllers must match the legacy interrupt routing.
+      ///
+      if ((Dev == 1) && (Func == 1)) {
+        Data32And = (UINT32)~(BIT25 | BIT24);
+        Data32Or  = BIT24 | BIT25;
+        McDevFunPciCfg32AndThenOr (Bus, Dev, Func, R_SA_PEG_CFG4_OFFSET, Data32And, Data32Or);
+        SCRIPT_MEM_READ_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (PegBaseAddress + R_SA_PEG_CFG4_OFFSET),
+          &Data32Or,
+          &Data32And
+          );
+      }
+
+      if ((Dev == 1) && (Func == 2)) {
+        Data32And = (UINT32)~(BIT25 | BIT24);
+        Data32Or  = BIT25;
+        McDevFunPciCfg32AndThenOr (Bus, Dev, Func, R_SA_PEG_CFG4_OFFSET, Data32And, Data32Or);
+        SCRIPT_MEM_READ_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (PegBaseAddress + R_SA_PEG_CFG4_OFFSET),
+          &Data32Or,
+          &Data32And
+          );
+      }
+    }
+
+    ///
+    /// Lock offset 3Dh for Interrupt Pin
+    ///
+    Data8 = McDevFunPciCfg8 (Bus, Dev, Func, PCI_INTPIN);
+    McDevFunPciCfg8 (Bus, Dev, Func, PCI_INTPIN) = Data8;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint8,
+      (UINTN) (PegBaseAddress + PCI_INTPIN),
+      1,
+      &Data8
+      );
+
+    ///
+    /// Lock DCAP register
+    ///
+    Data32 = McDevFunPciCfg32 (Bus, Dev, Func, R_SA_PEG_DCAP_OFFSET);
+    McDevFunPciCfg32 (Bus, Dev, Func, R_SA_PEG_DCAP_OFFSET) = Data32;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (PegBaseAddress + R_SA_PEG_DCAP_OFFSET),
+      1,
+      &Data32
+      );
+
+    if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuCrw)
+        ){
+      if ((Bus == 0) && (Dev == 1) && (Func == 0)) {
+        Data32  = McDevFunPciCfg32 (Bus, Dev, Func, 0xCD0);
+        Data32 |= BIT11;
+        McDevFunPciCfg32 (Bus, Dev, Func, 0xCD0) = Data32;
+        SCRIPT_MEM_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (PegBaseAddress + 0xCD0),
+          1,
+          &Data32
+          );
+       }
+    }
+
+    if ((HwStrap == SA_PEG_x16_x0_x0) && (PegComplete == 0)) {
+      break;
+    }
+    if ((HwStrap == SA_PEG_x8_x8_x0) && (PegComplete == 1)) {
+      break;
+    }
+    if ((HwStrap == SA_PEG_x8_x4_x4) && (PegComplete == 2)) {
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+#endif // PEG_FLAG
+
+EFI_STATUS
+Cid1Cid2DmiPortInit (
+  IN  UINT64                         DmiBar,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+    DMI Port Initialization for both CID1 (Port 1 in MCH) and CID2 (Port 0 in PCH).
+
+    @param[in] DmiBar              - DMIBAR Address
+    @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+    @retval EFI_SUCCESS            - DMI Port initialization successed.
+**/
+{
+  UINT32     Data32Or;
+#ifdef DMI_FLAG
+  UINT8      Data8And;
+  UINT8      Data8Or;
+  CPU_FAMILY CpuFamilyId;
+
+  CpuFamilyId = GetCpuFamily();
+#endif // DMI_FLAG
+
+#ifdef DMI_FLAG
+  if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuCrw)) {
+    ///
+    /// Additional Programming Steps
+    ///
+    AdditionalDMIProgramStepsBeforeASPM (DmiBar, DxePlatformSaPolicy);
+  }
+#endif // DMI_FLAG
+
+  ///
+  /// Set DMIBAR Offset 1C4h [31:0] = 0FFFFFFFFh
+  ///
+  Data32Or                = 0xFFFFFFFF;
+  Mmio32 (DmiBar, 0x1C4)  = 0xFFFFFFFF;
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x1C4),
+    1,
+    &Data32Or
+    );
+
+  ///
+  /// Set DMIBAR Offset 1D0h [31:0] = 0FFFFFFFFh
+  ///
+  Data32Or                = 0xFFFFFFFF;
+  Mmio32 (DmiBar, 0x1D0)  = 0xFFFFFFFF;
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x1D0),
+    1,
+    &Data32Or
+    );
+
+#ifdef DMI_FLAG
+  if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuCrw)) {
+    ///
+    /// Enable `Active State PM'. DMILCTL register at DMIBAR 088h [1:0] = '11b'.
+    /// Based on the policy:
+    ///
+    if (DxePlatformSaPolicy->PcieConfig->DmiAspm == PcieAspmAutoConfig ||
+        DxePlatformSaPolicy->PcieConfig->DmiAspm == PcieAspmL0sL1
+        ) {
+      ///
+      /// Enable ASPM = L0s and L1 Entry
+      ///
+      Data8And  = 0xFC;
+      Data8Or   = 0x03;
+      Mmio8Or (DmiBar, 0x88, Data8Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint8,
+        (UINTN) (DmiBar + 0x088),
+        &Data8Or,
+        &Data8And
+        );
+    } else if (DxePlatformSaPolicy->PcieConfig->DmiAspm == PcieAspmL0s) {
+      ///
+      /// Enable ASPM = L0s
+      ///
+      Data8And  = 0xFC;
+      Data8Or   = 0x01;
+      Mmio8Or (DmiBar, 0x88, Data8Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint8,
+        (UINTN) (DmiBar + 0x088),
+        &Data8Or,
+        &Data8And
+        );
+    } else if (DxePlatformSaPolicy->PcieConfig->DmiAspm == PcieAspmL1) {
+      ///
+      /// Enable ASPM = L1
+      ///
+      Data8And  = 0xFC;
+      Data8Or   = 0x02;
+      Mmio8Or (DmiBar, 0x88, Data8Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint8,
+        (UINTN) (DmiBar + 0x088),
+        &Data8Or,
+        &Data8And
+        );
+    }
+
+    if (DxePlatformSaPolicy->PcieConfig->DmiExtSync == ENABLED) {
+      ///
+      /// Enable Extended Synch
+      ///
+      Data8And  = 0xFF;
+      Data8Or   = 0x10;
+      Mmio8Or (DmiBar, 0x88, Data8Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint8,
+        (UINTN) (DmiBar + 0x088),
+        &Data8Or,
+        &Data8And
+        );
+    }
+
+    if (DxePlatformSaPolicy->PcieConfig->DmiIot == ENABLED) {
+      ///
+      /// if DMI Iot is enabled, set DMIBAR offset 0xD34 = 0x44
+      ///
+      Data8Or               = 0x44;
+      Mmio8 (DmiBar, 0xD34) = 0x44;
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint8,
+        (UINTN) (DmiBar + 0xD34),
+        1,
+        (VOID *) (UINTN) (DmiBar + 0xD34)
+        );
+    }
+  }
+#endif // DMI_FLAG
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+Cid1TopologyInit (
+  IN UINT64        EgressPortBar,
+  IN  UINT64       DmiBar,
+  IN  UINT32       PchRootComplexBar
+  )
+/**
+    Perform Root Complex Topology Initialization for CID1.
+
+    @param[in] EgressPortBar     - EPBAR Address
+    @param[in] DmiBar            - DMIBAR Address
+    @param[in] PchRootComplexBar - PCH RCBA Address
+
+    @retval EFI_SUCCESS          - Root Complex topology initialization for CID1 successed.
+**/
+{
+  UINT32  Data32;
+  UINT32  Data32And;
+  UINT32  Data32Or;
+  UINT32  DwordReg;
+  UINT64  McD1Base;
+  UINT64  McD1F1Base;
+  UINT64  McD1F2Base;
+
+  McD1Base    = MmPciAddress (0, 0, 1, 0, 0);
+  McD1F1Base  = MmPciAddress (0, 0, 1, 1, 0);
+  McD1F2Base  = MmPciAddress (0, 0, 1, 2, 0);
+
+  ///
+  /// Set the CID1 Egress Port 0 Topology
+  ///
+  ///
+  /// Step 1, Set the SA Component ID = 1.
+  ///
+  Data32And = 0xFF00FFFF;
+  Data32Or  = BIT16;
+  Mmio32AndThenOr (EgressPortBar, 0x44, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (EgressPortBar + 0x44),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// Step 2, Set link 1 Target Component ID and valid (Bit 0 = 1b).
+  /// Set the Link 1 TCID = 1 (Bits 23:16 = 01h).
+  ///
+  Data32And = 0xFF00FFFF;
+  Data32Or  = (BIT16 | BIT0);
+  Mmio32AndThenOr (EgressPortBar, 0x50, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (EgressPortBar + 0x50),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// Step 3, Set Link 1 to Reference the DMI RCRB (Bits 31:0 = DMI Base).
+  ///
+  Data32 = (UINT32) DmiBar;
+  MmioWrite32 ((UINTN) EgressPortBar + 0x58, Data32);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (EgressPortBar + 0x58),
+    1,
+    &Data32
+    );
+  Data32 = (UINT32) 0x00;
+  MmioWrite32 ((UINTN) EgressPortBar + 0x58 + 4, Data32);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (EgressPortBar + 0x58 + 4),
+    1,
+    &Data32
+    );
+
+#ifdef PEG_FLAG
+  ///
+  /// CID1 Egress port Root Topology and PciExpress Port (PEG Ports) Topology
+  /// Programming only if PEG devices are enabled.
+  ///
+  /// Step 1 PCI Express Enabled Check
+  /// Check and Configure CID1 root and Device 1 function 0
+  ///
+  if (McD1PciCfg16 (PCI_VID) != 0xFFFF) {
+    ///
+    /// Step 4, Set Link 2 to Reference the Device 1 function 0.
+    ///
+    Data32 = (UINT32) 0x8000;
+    MmioWrite32 ((UINTN) EgressPortBar + 0x68, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x68),
+      1,
+      &Data32
+      );
+    Data32 = (UINT32) 0x00;
+    MmioWrite32 ((UINTN) EgressPortBar + 0x68 + 4, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x68 + 4),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Step 5, Set link 2 Target Component ID and valid bit(Offset 60h, Bit 0 = 1b)
+    /// Set the Link 2 TCID = 1 (Bits 23:16 = 01h).
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = (BIT16 | BIT0);
+    Mmio32AndThenOr (EgressPortBar, 0x60, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x60),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Set the CID1 PCI Express* Port Root Topology
+    ///
+    /// Step 2 Set the Link 1 CID = 1 144h(23:16 = 01h).
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = BIT16;
+    MmioAndThenOr32 ((UINTN) McD1Base + 0x144, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1Base + 0x144),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 3. Set Link 1 to Reference the SA EP RCRB (Bits 31:0 = EPBAR).
+    ///
+    Data32 = (UINT32) EgressPortBar;
+    MmioWrite32 ((UINTN) McD1Base + 0x158, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1Base + 0x158),
+      1,
+      &Data32
+      );
+    Data32 = (UINT32) 0x00;
+    MmioWrite32 ((UINTN) McD1Base + 0x158 + 4, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1Base + 0x158 + 4),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Step 4 Set link 1 Target Component ID and valid bit(Offset 150h, Bit 0 = 1b)
+    /// Set the Link 1 TCID = 1 (Bits 23:16 = 01h).
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = (BIT16 | BIT0);
+    MmioAndThenOr32 ((UINTN) McD1Base + 0x150, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1Base + 0x150),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 5 Program Read-Only Write-Once Registers
+    /// D1.F0.R 0C4h [31:0]
+    ///
+    Data32And             = 0xFFFFFFFF;
+    Data32Or              = (UINT32) 0x00;
+    Data32                = McD1PciCfg32 (0xC4);
+    McD1PciCfg32 (0xC4)   = Data32;
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1Base + 0xC4),
+      &Data32Or,
+      &Data32And
+      );
+  }
+  ///
+  /// Check and Configure CID1 root and Device 1 function 1
+  ///
+  if (McD1F1PciCfg16 (PCI_VID) != 0xFFFF) {
+    ///
+    /// Step 6, Set Link 3 to Reference the Device 1 function 1.
+    ///
+    Data32 = (UINT32) 0x9000;
+    MmioWrite32 ((UINTN) EgressPortBar + 0x78, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x78),
+      1,
+      &Data32
+      );
+    Data32 = (UINT32) 0x00;
+    MmioWrite32 ((UINTN) EgressPortBar + 0x78 + 4, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x78 + 4),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Step 7. Set the Link 3 Target Component ID and Valid Bit 70h [0]
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = (BIT16 | BIT0);
+    Mmio32AndThenOr (EgressPortBar, 0x70, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x70),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 2 Set the Link 1 CID = 1 144h(23:16 = 01h).
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = BIT16;
+    MmioAndThenOr32 ((UINTN) McD1F1Base + 0x144, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F1Base + 0x144),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 3 Set Link 1 to Reference the IMC EP RCRB (Bits 31:0 = EPBAR).
+    ///
+    Data32 = (UINT32) EgressPortBar;
+    MmioWrite32 ((UINTN) McD1F1Base + 0x158, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F1Base + 0x158),
+      1,
+      &Data32
+      );
+    Data32 = (UINT32) 0x00;
+    MmioWrite32 ((UINTN) McD1F1Base + 0x158 + 4, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F1Base + 0x158 + 4),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Step 4 Set link 1 Target Component ID and valid bit(Offset 150h, Bit 0 = 1b)
+    /// Set the Link 1 TCID = 1 (Bits 23:16 = 01h).
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = (BIT16 | BIT0);
+    MmioAndThenOr32 ((UINTN) McD1F1Base + 0x150, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F1Base + 0x150),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 5 Program Read-Only Write-Once Registers
+    ///
+    Data32And               = 0xFFFFFFFF;
+    Data32Or                = (UINT32) 0x00;
+    Data32                  = McD1F1PciCfg32 (0xC4);
+    McD1F1PciCfg32 (0xC4)   = Data32;
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F1Base + 0xC4),
+      &Data32Or,
+      &Data32And
+      );
+  }
+  ///
+  /// Check and Configure CID1 root and Device 1 function 2
+  ///
+  if (McD1F2PciCfg16 (PCI_VID) != 0xFFFF) {
+    ///
+    /// Step 8, Set Link 4 to Reference the Device 1 function 2.
+    ///
+    Data32 = (UINT32) 0xA000;
+    MmioWrite32 ((UINTN) EgressPortBar + 0x88, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x88),
+      1,
+      &Data32
+      );
+    Data32 = (UINT32) 0x00;
+    MmioWrite32 ((UINTN) EgressPortBar + 0x88 + 4, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x88 + 4),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Step 9, Set the Link 4 Target Component ID and Valid Bit 80h [0]
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = (BIT16 | BIT0);
+    Mmio32AndThenOr (EgressPortBar, 0x80, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (EgressPortBar + 0x80),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 2. Set the Link 1 CID = 1 144h(23:16 = 01h).
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = BIT16;
+    MmioAndThenOr32 ((UINTN) McD1F2Base + 0x144, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F2Base + 0x144),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 3. Set Link 1 to Reference the IMC EP RCRB (Bits 31:0 = EPBAR).
+    ///
+    Data32 = (UINT32) EgressPortBar;
+    MmioWrite32 ((UINTN) McD1F2Base + 0x158, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F2Base + 0x158),
+      1,
+      &Data32
+      );
+    Data32 = (UINT32) 0x00;
+    MmioWrite32 ((UINTN) McD1F2Base + 0x158 + 4, Data32);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F2Base + 0x158 + 4),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Step 4. Set link 1 Target Component ID and valid bit(Offset 150h, Bit 0 = 1b)
+    /// Set the Link 1 TCID = 1 (Bits 23:16 = 01h).
+    ///
+    Data32And = 0xFF00FFFF;
+    Data32Or  = (BIT16 | BIT0);
+    MmioAndThenOr32 ((UINTN) McD1F2Base + 0x150, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F2Base + 0x150),
+      &Data32Or,
+      &Data32And
+      );
+
+    ///
+    /// Step 5. Program Read-Only Write-Once Registers
+    ///
+    Data32And               = 0xFFFFFFFF;
+    Data32Or                = (UINT32) 0x00;
+    Data32                  = McD1F2PciCfg32 (0xC4);
+    McD1F2PciCfg32 (0xC4)   = Data32;
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (McD1F2Base + 0xC4),
+      &Data32Or,
+      &Data32And
+      );
+  }
+#endif // PEG_FLAG
+
+  ///
+  /// Set the CID1 DMI Port Root Topology
+  ///
+  /// Step 1 Set the CID = 1 ( Bits 23:16 = 01h).
+  ///
+  Data32And = 0xFF00FFFF;
+  Data32Or  = BIT16;
+  Mmio32AndThenOr (DmiBar, 0x44, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x44),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// Step 2.  Set Link 1 Target Port Number = 0 (Bits 31:24 = 00h).
+  /// Step 3.  Set Link 1 TCID = 2 (Bits 23:16 = 02h).
+  /// Step 4.  Set Link 1 as valid (Bit 0 = 1b).
+  ///
+  Data32And = 0x0000FFFF;
+  Data32Or  = (BIT17 + BIT0);
+  Mmio32AndThenOr (DmiBar, 0x50, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x50),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// Step 5, Set Link 1 to Reference the PCH RCRB
+  ///
+  Data32                  = PchRootComplexBar;
+  Mmio32 (DmiBar, 0x58)   = PchRootComplexBar;
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x58),
+    1,
+    &Data32
+    );
+  Data32                      = (UINT32) 0x00;
+  Mmio32 (DmiBar, 0x58 + 4)   = Data32;
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x58 + 4),
+    1,
+    &Data32
+    );
+
+  ///
+  /// Step 6, Set Link 2 to Reference the IMC EP (Bits 31:0 = EP).
+  ///
+  Data32 = (UINT32) EgressPortBar;
+  MmioWrite32 ((UINTN) DmiBar + 0x68, Data32);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x68),
+    1,
+    &Data32
+    );
+  Data32 = (UINT32) 0x00;
+  MmioWrite32 ((UINTN) DmiBar + 0x68 + 4, Data32);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x68 + 4),
+    1,
+    &Data32
+    );
+
+  ///
+  /// Step 7, Set link 2 Target Component ID and valid Bit(Bit 0 = 1b)
+  /// Set the Link 2 TCID = 1 (Bits 23:16 = 01h).
+  ///
+  Data32And = 0xFF00FFFF;
+  Data32Or  = (BIT16 | BIT0);
+  Mmio32AndThenOr (DmiBar, 0x60, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (DmiBar + 0x60),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// Step 8. Program RO and Write-Once Registers
+  /// DMIBAR Offset 004h [31:0]
+  /// DMIBAR Offset 084h [31:0]
+  ///
+  DwordReg                = Mmio32 (DmiBar, 0x4);
+  Mmio32 (DmiBar, 0x4)    = DwordReg;
+
+  DwordReg                = Mmio32 (DmiBar, 0x84);
+  Mmio32 (DmiBar, 0x84)   = DwordReg;
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+Cid2TopologyInit (
+  IN UINT32   PchRootComplexBar,
+  IN UINT64   DmiBar
+  )
+/**
+  Perform Root Complex Topology Initialization for CID2.
+  Note: This sequence follows PCH BIOS specification Ver 0.5 section 8.3
+        Root Complex Topology Programming
+
+  @param[in] PchRootComplexBar - PCH RCBA Address
+  @param[in] DmiBar            - DMIBAR Address
+
+  @retval EFI_SUCCESS          - Root Complex topology initialization for CID2 successed.
+**/
+{
+  UINT32  Data32;
+  UINT32  Data32And;
+  UINT32  Data32Or;
+
+  ///
+  /// PCH BIOS specification Ver 0.5 section 8.3 Note 1,2
+  /// program a value into this Component ID field to determine the Component ID of
+  /// ICH8, and this value must be different from the Component ID value of the MCH.
+  /// Set the CID = 2 (Offset 104h, Bits 23:16 = 02h).
+  ///
+  Data32And = 0xFF00FFFF;
+  Data32Or  = BIT17;
+  MmioAndThenOr32 (PchRootComplexBar + 0x104, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PchRootComplexBar + 0x104),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// PCH BIOS specification Ver 0.5 section 8.3 Note 3,4
+  /// Note 3: This Target Port # field must be filled in by System BIOS with the Port
+  /// # of the RCRB DMI link in the MCH.
+  /// Note 4: This Target CID field must be filled in by System BIOS with the
+  /// Component ID of the MCH.
+  /// Set the Link 1 Target Port Number = 1 (Offset 110h, bits 31:24 = 01h).
+  /// Set the Link 1 Target Component ID = 1 (Offset 110h, bits 23:16 = 01h).
+  ///
+  Data32And = 0x0000FFFF;
+  Data32Or  = (BIT24 | BIT16);
+  MmioAndThenOr32 (PchRootComplexBar + 0x110, Data32And, Data32Or);
+  SCRIPT_MEM_READ_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PchRootComplexBar + 0x110),
+    &Data32Or,
+    &Data32And
+    );
+
+  ///
+  /// PCH BIOS specification Ver 0.5 section 8.3 Note 5
+  /// Fill the Base Address field with the same base address of the RCRB DMI link in
+  /// the MCH, This register field is located at RCBA+ 0118h[63:0],
+  /// and will be locked once written until the next reset.
+  ///
+  Data32 = (UINT32) DmiBar;
+  MmioWrite32 (PchRootComplexBar + 0x118, Data32);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (PchRootComplexBar + 0x118),
+    1,
+    &Data32
+    );
+
+  Data32 = *((UINT32 *) (&DmiBar) + 1);
+  MmioWrite32 (PchRootComplexBar + 0x118 + 4, Data32);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint8,
+    (UINTN) (PchRootComplexBar + 0x118 + 4),
+    1,
+    &Data32
+    );
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+PegInitBeforeExitPmAuth (
+  VOID
+  )
+/**
+  This function performs Peg initialization before ExitPmAuth.
+
+  @retval EFI_SUCCESS   - Always.
+**/
+{
+#if SA_PCIE_ASPM_IN_SMM == 1
+  EFI_EVENT                       ReadyToBoot;
+  EFI_STATUS                      Status;
+#endif
+  BOOLEAN                         AspmHasBeenHandled;
+
+  DEBUG ((EFI_D_ERROR, "[SA] Pcie before ExitPmAuth callback.\n"));
+  AspmHasBeenHandled = FALSE;
+  ///
+  /// SMM mode ASPM handling
+  /// Check if supported and enabled
+  ///
+#if SA_PCIE_ASPM_IN_SMM == 1
+  if ((mInitPcieAspmAfterOprom == 1) && (mSaIotrapSmiAddress != 0)) {
+    ///
+    /// Do the Phase 1 SMI callback
+    /// This will enumerate PCIe downstream devices
+    ///
+    SaLateInitSmiCallback (NULL, NULL);
+
+    ///
+    /// Create an ReadyToBoot call back event to do the Phase 3 SMI callback
+    /// This will handle PEG ASPM programming after OROM execution
+    ///
+    Status = EfiCreateEventReadyToBootEx (
+               EFI_TPL_NOTIFY,
+               SaLateInitSmiCallback,
+               NULL,
+               &ReadyToBoot
+               );
+    ASSERT_EFI_ERROR (Status);
+    AspmHasBeenHandled = TRUE;
+  }
+#endif
+
+  ///
+  /// DXE mode ASPM handling
+  /// Check if SMM mode already taken care all things
+  /// TRUE to skip DXE mode task. Otherwise do DXE mode ASPM initialization
+  ///
+#if SA_PCIE_ASPM_IN_DXE == 1
+  if (AspmHasBeenHandled == FALSE) {
+    ///
+    /// Initialize ASPM before OpROM, S3 Save script supported
+    /// First initialize all policy settings
+    /// Initialize module global variables - Stepping ID and Platform Policy
+    ///
+    SaPcieInitPolicy (mDxePlatformSaPolicy);
+    ///
+    /// Do initialization
+    ///
+    SaPcieEnumCallback ();
+    SaPcieConfigAfterOpRom ();
+  }
+#endif
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+SaSecurityInit (
+  VOID
+  )
+/**
+  This function performs SA Security locking in ExitPmAuth callback
+
+  @retval EFI_SUCCESS     - Security lock has done
+  @retval EFI_UNSUPPORTED - Security lock not done successfully
+**/
+{
+  BOOLEAN                         SecurityHasBeenHandled;
+  SecurityHasBeenHandled = FALSE;
+#if SA_PCIE_ASPM_IN_SMM == 1
+  if ((mInitPcieAspmAfterOprom == 1) && (mSaIotrapSmiAddress != 0)) {
+    ///
+    /// Generate the Phase 2 of SA SMI to do security lock
+    ///
+    SaLateInitSmiCallback (NULL, NULL);
+
+    SecurityHasBeenHandled = TRUE;
+  }
+#endif
+
+  ///
+  /// Check if SMM mode already taken care this task
+  ///
+#if SA_PCIE_ASPM_IN_DXE == 1
+  if (SecurityHasBeenHandled == FALSE) {
+    SaSaveRestorePlatform (TRUE);
+    SaSecurityLock ();
+    SecurityHasBeenHandled = TRUE;
+  }
+#endif
+  ///
+  /// Security locking is important so fail to do this will be an ERROR.
+  ///
+  if (SecurityHasBeenHandled == TRUE) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PcieComplex.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PcieComplex.h
new file mode 100644
index 0000000..8a52de9
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/PcieComplex.h
@@ -0,0 +1,43 @@
+/** @file
+  This is header file for SA PCIE Root Complex initialization.
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+EFI_STATUS
+PegInitBeforeExitPmAuth (
+  VOID
+  )
+/**
+  This function performs Peg initialization before ExitPmAuth.
+
+  @retval EFI_SUCCESS   - Always.
+**/
+;
+
+EFI_STATUS
+SaSecurityInit (
+  VOID
+  )
+/**
+  This function performs SA Security locking in ExitPmAuth callback
+
+  @retval EFI_SUCCESS     - Security lock has done
+  @retval EFI_UNSUPPORTED - Security lock not done successfully
+**/
+;
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaFvi.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaFvi.c
new file mode 100644
index 0000000..d88a5e7
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaFvi.c
@@ -0,0 +1,105 @@
+/** @file
+  SA Firmware Version Info implementation.
+
+@copyright
+  Copyright (c) 2011 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#include "SaInit.h"
+
+FVI_ELEMENT_AND_FUNCTION      mSaFviElementsData[] = {
+  {
+    DEFAULT_FVI_ELEMENT_DATA(SA),
+    NULL
+  },
+  {
+    {
+      1,
+      0,
+      MEM_RC_VERSION,
+      MEM_FVI_STRING,
+      {
+        0
+      },
+    },
+    NULL
+  },
+  {
+    {
+      1,
+      0,
+      PCIE_RC_VERSION,
+      PCIE_FVI_STRING,
+      {
+        0
+      },
+    },
+    NULL
+  },
+  {
+    {
+      1,
+      2,
+      SA_CRID_VERSION,
+      SA_CRID_STATUS,
+      SA_CRID_DISABLED,
+    },
+    NULL
+  },
+  {
+    {
+      1,
+      0,
+      SA_CRID_VERSION,
+      SA_CRID_ORIGINAL_VALUE,
+      {
+        0
+      },
+    },
+    NULL
+  },
+  {
+    {
+      1,
+      0,
+      SA_CRID_VERSION,
+      SA_CRID_NEW_VALUE,
+      {
+        0
+      },
+    },
+    NULL
+  },
+  {
+    {
+      1,
+      0,
+      VBIOS_RC_VERSION,
+      VBIOS_FVI_STRING,
+      {
+        0
+      },
+    },
+    NULL
+  }
+};
+
+FVI_DATA_HUB_CALLBACK_CONTEXT mSaFviVersionData = {
+  MISC_SUBCLASS_FVI_HEADER_ENTRY(SA),
+  mSaFviElementsData,
+};
+
+UINTN                         mSaFviElements = sizeof (mSaFviElementsData) / sizeof (FVI_ELEMENT_AND_FUNCTION);
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.c
new file mode 100644
index 0000000..af4d45a
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.c
@@ -0,0 +1,842 @@
+/** @file
+  This is the driver that initializes the Intel System Agent.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "SaInit.h"
+#include EFI_GUID_DEFINITION (SaDataHob)
+#include <Protocol/PciEnumerationComplete.h>
+#include EFI_PROTOCOL_DEPENDENCY (SaGlobalNvsArea)
+#include EFI_GUID_DEFINITION (SaSsdtTableStorage)
+
+///
+/// Global Variables
+///
+SYSTEM_AGENT_GLOBAL_NVS_AREA_PROTOCOL  mSaGlobalNvsAreaProtocol;
+DXE_PLATFORM_SA_POLICY_PROTOCOL        *mDxePlatformSaPolicy;
+UINT16                                 mSaIotrapSmiAddress;
+BOOLEAN                                mInitPcieAspmAfterOprom;
+EFI_GUID                               gEfiPciEnumerationCompleteProtocolGuid = EFI_PCI_ENUMERATION_COMPLETE_GUID;
+
+EFI_STATUS
+InitializeSaSsdtAcpiTables (
+  VOID
+  )
+/**
+@brief
+  Initialize System Agent SSDT ACPI tables
+
+  @retval EFI_SUCCESS    ACPI tables are initialized successfully
+  @retval EFI_NOT_FOUND  ACPI tables not found
+**/
+{
+  EFI_STATUS                    Status;
+  EFI_HANDLE                    *HandleBuffer;
+  UINTN                         NumberOfHandles;
+  EFI_FV_FILETYPE               FileType;
+  UINT32                        FvStatus;
+  EFI_FV_FILE_ATTRIBUTES        Attributes;
+  UINTN                         Size;
+  UINTN                         i;
+  EFI_FIRMWARE_VOLUME_PROTOCOL  *FwVol;
+  INTN                          Instance;
+  EFI_ACPI_COMMON_HEADER        *CurrentTable;
+  UINTN                         AcpiTableKey;
+  UINT8                         *CurrPtr;
+  UINT8                         *EndPtr;
+  UINT32                        *Signature;
+  EFI_ACPI_DESCRIPTION_HEADER   *SaAcpiTable;
+  EFI_ACPI_TABLE_PROTOCOL       *AcpiTable;
+
+  FwVol               = NULL;
+  SaAcpiTable = NULL;
+
+  ///
+  /// Locate ACPI Table protocol
+  ///
+  DEBUG ((EFI_D_INFO, "Init SA SSDT table\n"));
+  Status = gBS->LocateProtocol (&gEfiAcpiTableProtocolGuid, NULL, &AcpiTable);
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "Fail to locate EfiAcpiTableProtocol.\n"));
+    return EFI_NOT_FOUND;
+  }
+
+  ///
+  /// Locate protocol.
+  /// There is little chance we can't find an FV protocol
+  ///
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolumeProtocolGuid,
+                  NULL,
+                  &NumberOfHandles,
+                  &HandleBuffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+  ///
+  /// Looking for FV with ACPI storage file
+  ///
+  for (i = 0; i < NumberOfHandles; i++) {
+    ///
+    /// Get the protocol on this handle
+    /// This should not fail because of LocateHandleBuffer
+    ///
+    Status = gBS->HandleProtocol (
+                    HandleBuffer[i],
+                    &gEfiFirmwareVolumeProtocolGuid,
+                    &FwVol
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    ///
+    /// See if it has the ACPI storage file
+    ///
+    Size      = 0;
+    FvStatus  = 0;
+    Status = FwVol->ReadFile (
+                      FwVol,
+                      &gSaSsdtAcpiTableStorageGuid,
+                      NULL,
+                      &Size,
+                      &FileType,
+                      &Attributes,
+                      &FvStatus
+                      );
+
+    ///
+    /// If we found it, then we are done
+    ///
+    if (Status == EFI_SUCCESS) {
+      break;
+    }
+  }
+  ///
+  /// Free any allocated buffers
+  ///
+  FreePool (HandleBuffer);
+
+  ///
+  /// Sanity check that we found our data file
+  ///
+  ASSERT (FwVol != NULL);
+  if (FwVol == NULL) {
+    DEBUG ((EFI_D_INFO, "SA Global NVS table not found\n"));
+    return EFI_NOT_FOUND;
+  }
+  ///
+  /// Our exit status is determined by the success of the previous operations
+  /// If the protocol was found, Instance already points to it.
+  /// Read tables from the storage file.
+  ///
+  Instance      = 0;
+  CurrentTable  = NULL;
+  while (Status == EFI_SUCCESS) {
+    Status = FwVol->ReadSection (
+                      FwVol,
+                      &gSaSsdtAcpiTableStorageGuid,
+                      EFI_SECTION_RAW,
+                      Instance,
+                      &CurrentTable,
+                      &Size,
+                      &FvStatus
+                      );
+
+    if (!EFI_ERROR (Status)) {
+      ///
+      /// Check the table ID to modify the table
+      ///
+      if (((EFI_ACPI_DESCRIPTION_HEADER *) CurrentTable)->OemTableId == EFI_SIGNATURE_64 ('S', 'a', 'S', 's', 'd', 't', ' ', 0)) {
+        SaAcpiTable = (EFI_ACPI_DESCRIPTION_HEADER *) CurrentTable;
+        ///
+        /// Locate the SSDT package
+        ///
+        CurrPtr = (UINT8 *) SaAcpiTable;
+        EndPtr  = CurrPtr + SaAcpiTable->Length;
+
+        for (; CurrPtr <= EndPtr; CurrPtr++) {
+          Signature = (UINT32 *) (CurrPtr + 3);
+          if (*Signature == EFI_SIGNATURE_32 ('S', 'A', 'N', 'V')) {
+            ASSERT_EFI_ERROR (*(UINT32 *) (CurrPtr + 3 + sizeof (*Signature) + 2) == 0xFFFF0000);
+            ASSERT_EFI_ERROR (*(UINT16 *) (CurrPtr + 3 + sizeof (*Signature) + 2 + sizeof (UINT32) + 1) == 0xAA55);
+            ///
+            /// SA Global NVS Area address
+            ///
+            *(UINT32 *) (CurrPtr + 3 + sizeof (*Signature) + 2) = (UINT32) (UINTN) mSaGlobalNvsAreaProtocol.Area;
+            ///
+            /// SA Global NVS Area size
+            ///
+            *(UINT16 *) (CurrPtr + 3 + sizeof (*Signature) + 2 + sizeof (UINT32) + 1) =
+            sizeof (SYSTEM_AGENT_GLOBAL_NVS_AREA);
+
+            AcpiTableKey = 0;
+            Status = AcpiTable->InstallAcpiTable (
+                                  AcpiTable,
+                                  SaAcpiTable,
+                                  SaAcpiTable->Length,
+                                  &AcpiTableKey
+                                  );
+            ASSERT_EFI_ERROR (Status);
+            return EFI_SUCCESS;
+          }
+        }
+      }
+      ///
+      /// Increment the instance
+      ///
+      Instance++;
+      CurrentTable = NULL;
+    }
+  }
+
+  return Status;
+
+}
+
+EFI_STATUS
+EFIAPI
+SaInitEntryPoint (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+/**
+  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                Status;
+  VOID                      *Registration;
+  SA_INSTANCE_PRIVATE_DATA  *SaInstance;
+  UINTN                     MCHBAR_BASE;
+  SA_DATA_HOB               *SaDataHob;
+
+  DEBUG ((EFI_D_INFO, "SaInitDxe Start\n"));
+
+  INITIALIZE_SCRIPT (ImageHandle, SystemTable);
+  SaInstance  = NULL;
+
+  MCHBAR_BASE = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+
+  ///
+  /// Get the platform setup policy.
+  ///
+  Status = gBS->LocateProtocol (&gDxePlatformSaPolicyGuid, NULL, (VOID **) &mDxePlatformSaPolicy);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Dump SA Platform Policy
+  ///
+  SaDxePolicyDump (mDxePlatformSaPolicy);
+
+  ///
+  /// Get SaDataHob HOB and see if ASPM should be programmed before or after OpROM.
+  ///
+  mSaIotrapSmiAddress    = 0;
+  mInitPcieAspmAfterOprom = 0;
+  SaDataHob              = NULL;
+  SaDataHob              = (SA_DATA_HOB *)GetFirstGuidHob (&gSaDataHobGuid);
+  if (SaDataHob != NULL) {
+    mSaIotrapSmiAddress    = SaDataHob->SaIotrapSmiAddress;
+    mInitPcieAspmAfterOprom = SaDataHob->InitPcieAspmAfterOprom;
+  }
+
+  ///
+  /// If there was no DXE ASPM code, always executes SMM code
+  ///
+#if SA_PCIE_ASPM_IN_DXE == 0
+  mInitPcieAspmAfterOprom = 1;
+#endif
+  ///
+  /// If there was no SMM mode supported, always enable DXE mode
+  ///
+#if SA_PCIE_ASPM_IN_SMM == 0
+  mInitPcieAspmAfterOprom = 0;
+#endif
+
+  ///
+  /// Install System Agent Global NVS protocol
+  ///
+  DEBUG ((EFI_D_INFO, "Install SA GNVS protocol\n"));
+  Status = (gBS->AllocatePool) (EfiACPIMemoryNVS, sizeof (SYSTEM_AGENT_GLOBAL_NVS_AREA), &mSaGlobalNvsAreaProtocol.Area);
+  ASSERT_EFI_ERROR (Status);
+  ZeroMem ((VOID *) mSaGlobalNvsAreaProtocol.Area, sizeof (SYSTEM_AGENT_GLOBAL_NVS_AREA));
+  mSaGlobalNvsAreaProtocol.Area->SaRcRevision = SA_RC_VERSION;
+  mSaGlobalNvsAreaProtocol.Area->XPcieCfgBaseAddress  = (UINT32) (MmPciAddress (0, 0, 0, 0, 0x0));
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ImageHandle,
+                  &gSaGlobalNvsAreaProtocolGuid,
+                  &mSaGlobalNvsAreaProtocol,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// PciExpress Dxe Initialization
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing PciExpress (Dxe)\n"));
+  PciExpressInit (mDxePlatformSaPolicy);
+
+  ///
+  /// Internal devices and Misc configurations.
+  ///
+  DEBUG ((EFI_D_INFO, "Internal Device and Misc Configurations\n"));
+  DeviceConfigure (mDxePlatformSaPolicy);
+  ProgramSvidSid (mDxePlatformSaPolicy);
+
+  ///
+  /// LegacyRegion Driver
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing Legacy Region\n"));
+  LegacyRegionInstall (ImageHandle);
+
+  ///
+  /// GtPostInit Initialization
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing GT PowerManagement and other GT POST related\n"));
+  GraphicsInit (ImageHandle, mDxePlatformSaPolicy);
+
+  ///
+  /// Audio (dHDA) Initialization
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing CPU Audio (dHDA) device\n"));
+  AudioInit (ImageHandle, mDxePlatformSaPolicy);
+
+  //
+  // @todo remove this VP SKIP and prepare for PO settings to skip this code.
+  //
+  ///
+  /// Vtd Initialization
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing VT-d\n"));
+  VtdInit (mDxePlatformSaPolicy);
+
+  ///
+  /// IgdOpRegion Install Initialization
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing IGD OpRegion\n"));
+  IgdOpRegionInit ();
+
+
+  ///
+  /// Create PCI Enumeration Completed callback for SA
+  ///
+  EfiCreateProtocolNotifyEvent (
+    &gEfiPciEnumerationCompleteProtocolGuid,
+    EFI_TPL_CALLBACK,
+    SaPciEnumCompleteCallback,
+    NULL,
+    &Registration
+    );
+
+  ///
+  /// Create ExitPmAuth callback for SA
+  ///
+  EfiCreateProtocolNotifyEvent (
+    &gExitPmAuthProtocolGuid,
+    EFI_TPL_CALLBACK,
+    SaExitPmAuthCallback,
+    NULL,
+    &Registration
+    );
+
+  ///
+  /// Install SA_INFO_PROTOCOL
+  ///
+  SaInstance = AllocateZeroPool (sizeof (SA_INSTANCE_PRIVATE_DATA));
+  if (SaInstance == NULL) {
+    ASSERT (FALSE);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  SaInstance->SaInfo.Revision = SA_INFO_PROTOCOL_REVISION_1;
+  ///
+  /// RCVersion[31:0] is the release number.
+  /// For example:
+  /// SaFramework 0.6.0.01 should be 00 06 00 01 (0x00060001)
+  /// SaFramework 0.6.2    should be 00 06 02 00 (0x00060200)
+  ///
+  SaInstance->SaInfo.RCVersion = SA_RC_VERSION;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ImageHandle,
+                  &gEfiSaInfoProtocolGuid,
+                  &(SaInstance->SaInfo),
+                  NULL
+                  );
+
+  ///
+  /// Install System Agent Global NVS ACPI table
+  ///
+#ifndef Remove_SaSsdt_Data_To_Dsdt 
+  Status = InitializeSaSsdtAcpiTables ();
+#endif //AMI_OVERRIDE
+
+#ifdef SG_SUPPORT
+
+  DEBUG ((EFI_D_INFO, "Initializing Switchable Graphics (Dxe)\n"));
+  SwitchableGraphicsInit (ImageHandle, SystemTable, mDxePlatformSaPolicy);
+
+#endif
+
+  DEBUG ((EFI_D_INFO, "SaInitDxe End\n"));
+
+  return EFI_SUCCESS;
+}
+
+VOID
+DeviceConfigure (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Initialize GT Post Routines.
+
+    @param[in] ImageHandle         -     Handle for the image of this driver
+    @param[in] DxePlatformSaPolicy -     SA DxePlatformPolicy protocol
+**/
+{
+  UINT64  MchBar;
+
+  MchBar = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+
+  ///
+  /// Enable/Disable CHAP device (B0,D7,F0).
+  ///
+  if (DxePlatformSaPolicy->MiscConfig->ChapDeviceEnable) {
+    McD0PciCfg32Or (R_SA_DEVEN, B_SA_DEVEN_D7EN_MASK);
+  } else {
+    McD0PciCfg32And (R_SA_DEVEN, ~(B_SA_DEVEN_D7EN_MASK));
+  }
+  ///
+  /// Enable/Disable Thermal device (B0,D4,F0).
+  ///
+  if (DxePlatformSaPolicy->MiscConfig->Device4Enable) {
+    McD0PciCfg32Or (R_SA_DEVEN, B_SA_DEVEN_D4EN_MASK);
+  } else {
+    McD0PciCfg32And (R_SA_DEVEN, ~(B_SA_DEVEN_D4EN_MASK));
+  }
+  ///
+  /// Enable/Disable Audio device (B0,D3,F0).
+  ///
+  if ((DxePlatformSaPolicy->MiscConfig->AudioEnable) && (McD2PciCfg16 (R_SA_IGD_VID) != 0xFFFF)) {
+    McD0PciCfg32Or (R_SA_DEVEN, B_SA_DEVEN_D3EN_MASK);
+  } else {
+    McD0PciCfg32And (R_SA_DEVEN, ~(B_SA_DEVEN_D3EN_MASK));
+  }
+
+  return ;
+}
+
+VOID
+ProgramSvidSid (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+    Program SA devices Subsystem Vendor Identifier (SVID) and Subsystem Identifier (SID).
+
+    @param[in] DxePlatformSaPolicy   The SAPlatform Policy protocol instance
+**/
+{
+  UINT8                         Index;
+  UINT8                         BusNumber;
+  UINTN                         PciEAddressBase;
+  UINT8                         DeviceNumber;
+  UINT8                         FunctionNumber;
+  UINT8                         SvidRegOffset;
+  UINT16                        Data16;
+  STATIC SA_SVID_SID_INIT_ENTRY SvidSidInitTable[] = {
+    {
+      0,
+      0,
+      PCI_SVID_OFFSET
+    },
+    {
+      1,
+      0,
+      R_SA_PEG_SS_OFFSET
+    },
+    {
+      1,
+      1,
+      R_SA_PEG_SS_OFFSET
+    },
+    {
+      1,
+      2,
+      R_SA_PEG_SS_OFFSET
+    },
+    {
+      2,
+      0,
+      PCI_SVID_OFFSET
+    },
+    {
+      3,
+      0,
+      PCI_SVID_OFFSET
+    },
+    {
+      4,
+      0,
+      PCI_SVID_OFFSET
+    },
+    {
+      7,
+      0,
+      PCI_SVID_OFFSET
+    }
+  };
+
+  if ((DxePlatformSaPolicy->MiscConfig->DefaultSvidSid->SubSystemVendorId != 0) ||
+      (DxePlatformSaPolicy->MiscConfig->DefaultSvidSid->SubSystemId != 0)
+      ) {
+    for (Index = 0; Index < (sizeof (SvidSidInitTable) / sizeof (SA_SVID_SID_INIT_ENTRY)); Index++) {
+      BusNumber       = 0;
+      DeviceNumber    = SvidSidInitTable[Index].DeviceNumber;
+      FunctionNumber  = SvidSidInitTable[Index].FunctionNumber;
+      SvidRegOffset   = SvidSidInitTable[Index].SvidRegOffset;
+      PciEAddressBase = (UINT32) MmPciAddress (0, BusNumber, DeviceNumber, FunctionNumber, 0);
+      ///
+      /// Skip if the device is disabled
+      ///
+      if (MmioRead16 (PciEAddressBase + PCI_VID) == 0xFFFF) {
+        continue;
+      }
+      ///
+      /// Program SA devices Subsystem Vendor Identifier (SVID)
+      ///
+      Data16 = DxePlatformSaPolicy->MiscConfig->DefaultSvidSid->SubSystemVendorId;
+      MmioWrite16 (
+        (UINTN) (PciEAddressBase + SvidRegOffset),
+        Data16
+        );
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint16,
+        (UINTN) (PciEAddressBase + SvidRegOffset),
+        1,
+        (VOID *) (UINTN) (PciEAddressBase + SvidRegOffset)
+        );
+
+      ///
+      /// Program SA devices Subsystem Identifier (SID)
+      ///
+      Data16 = DxePlatformSaPolicy->MiscConfig->DefaultSvidSid->SubSystemId;
+      MmioWrite16 (
+        (UINTN) (PciEAddressBase + SvidRegOffset + 2),
+        Data16
+        );
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint16,
+        (UINTN) (PciEAddressBase + SvidRegOffset + 2),
+        1,
+        (VOID *) (UINTN) (PciEAddressBase + SvidRegOffset + 2)
+        );
+    }
+  }
+
+  return;
+}
+
+VOID
+SaFviExitPmAuth(
+  VOID
+  )
+/**
+  This function populates the SA FVI version numbers
+**/
+{
+  const UINT8               StrEnabled[sizeof (SA_CRID_ENABLED)]    = SA_CRID_ENABLED;
+  const UINT8               StrDisabled[sizeof (SA_CRID_DISABLED)]  = SA_CRID_DISABLED;
+  const CodeVersion         PcieRcVersion = {
+#include "SaPcieVersion.h"
+  };
+  const CodeVersion         MemRcVersion = {
+#include "MrcVersion.h"
+  };
+  EFI_STATUS                Status;
+  EFI_IA32_REGISTER_SET     RegSet;
+  UINT16                    VbiosBuildNum;
+  EFI_LEGACY_BIOS_PROTOCOL  *LegacyBios;
+
+  if (mDxePlatformSaPolicy->MiscConfig->FviReport) {
+    InitFviDataHubCbContext (
+      mDxePlatformSaPolicy->MiscConfig->FviSmbiosType,
+      (UINT8) mSaFviElements,
+      &mSaFviVersionData
+      );
+    mSaFviElementsData[MEM_RC_VER].Element.Version.MajorVersion = (UINT8) MemRcVersion.Major;
+    mSaFviElementsData[MEM_RC_VER].Element.Version.MinorVersion = (UINT8) MemRcVersion.Minor;
+    mSaFviElementsData[MEM_RC_VER].Element.Version.Revision     = (UINT8) MemRcVersion.Rev;
+    mSaFviElementsData[MEM_RC_VER].Element.Version.BuildNum     = (UINT16) MemRcVersion.Build;
+    mSaFviElementsData[PCIE_VER].Element.Version.MajorVersion   = (UINT8) PcieRcVersion.Major;
+    mSaFviElementsData[PCIE_VER].Element.Version.MinorVersion   = (UINT8) PcieRcVersion.Minor;
+    mSaFviElementsData[PCIE_VER].Element.Version.Revision       = (UINT8) PcieRcVersion.Rev;
+    mSaFviElementsData[PCIE_VER].Element.Version.BuildNum       = (UINT16) PcieRcVersion.Build;
+    mSaFviElementsData[CRID_ORIGINAL].Element.Version.BuildNum  = (UINT16) McD0PciCfg8 (PCI_REVISION_ID_OFFSET);
+
+    if (mDxePlatformSaPolicy->MiscConfig->CridEnable == TRUE) {
+      CopyMem (mSaFviElementsData[CRID_STATUS].Element.VerString, StrEnabled, sizeof (StrEnabled));
+    } else {
+      CopyMem (mSaFviElementsData[CRID_STATUS].Element.VerString, StrDisabled, sizeof (StrDisabled));
+    }
+
+    mSaFviElementsData[CRID_NEW].Element.Version.BuildNum = (UINT16) McD0PciCfg8 (PCI_REVISION_ID_OFFSET);
+    //
+    // Check IGFX device
+    //
+    VbiosBuildNum = 0xFFFF;
+    Status = gBS->LocateProtocol (&gEfiLegacyBiosProtocolGuid, NULL, (void **) &LegacyBios);
+    if (Status == EFI_SUCCESS) {
+      RegSet.X.AX   = 0x5f01;
+      Status        = LegacyBios->Int86 (LegacyBios, 0x15, &RegSet);
+      VbiosBuildNum = (((RegSet.X.DX & 0x0F00) << 4) | ((RegSet.X.DX & 0x000F) << 8) | ((RegSet.X.BX & 0x0F00) >> 4) | (RegSet.X.BX & 0x000F));
+    }
+    mSaFviElementsData[VBIOS_VER].Element.Version.BuildNum = VbiosBuildNum;
+
+    CreateRcFviDatahub (&mSaFviVersionData);
+  }
+}
+
+VOID
+EFIAPI
+SaPciEnumCompleteCallback (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+/**
+  This function gets registered as a callback to perform SA 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.
+**/
+{
+  EFI_STATUS          Status;
+  VOID                *ProtocolPointer;
+
+  ///
+  /// 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, &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+     return;
+  }
+
+  gBS->CloseEvent (Event);
+
+  Status = PegInitBeforeExitPmAuth ();
+  if (EFI_SUCCESS != Status) {
+    DEBUG ((EFI_D_ERROR, "[SA] Pcie initialization before ExitPmAuth Error, Status = %x \n", Status));
+    ASSERT_EFI_ERROR (Status);
+  }
+}
+
+VOID
+EFIAPI
+SaExitPmAuthCallback (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+/**
+  This function gets registered as a callback to perform SA configuration security lock
+
+  @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.
+**/
+{
+  EFI_STATUS          Status;
+  VOID                *ProtocolPointer;
+
+  ///
+  /// 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 (&gExitPmAuthProtocolGuid, NULL, &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+     return;
+  }
+
+  gBS->CloseEvent (Event);
+
+  if (McD2PciCfg16 (R_SA_IGD_VID) != 0xFFFF) {
+    Status = PostPmInitExitPmAuth();
+    if (EFI_SUCCESS != Status) {
+      DEBUG ((EFI_D_ERROR, "[SA] ExitPmAuth GraphicsInit Error, Status = %x \n", Status));
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  UpdateDmarExitPmAuth();
+
+  if (McD2PciCfg16 (R_SA_IGD_VID) != 0xFFFF) {
+    Status = GetVBiosVbtExitPmAuth();
+    if (EFI_SUCCESS != Status) {
+      DEBUG ((EFI_D_ERROR, "[SA] ExitPmAuth Op Region Error, Status = %x \n", Status));
+    }
+
+    Status = UpdateIgdOpRegionExitPmAuth();
+    if (EFI_SUCCESS != Status) {
+      DEBUG ((EFI_D_ERROR, "[SA] ExitPmAuth Update Op Region Error, Status = %x \n", Status));
+    }
+  }
+
+  SaFviExitPmAuth();
+
+  Status = SaSecurityInit ();
+  if (EFI_SUCCESS != Status) {
+    DEBUG ((EFI_D_ERROR, "[SA] Security lock Error, Status = %x \n", Status));
+    ASSERT_EFI_ERROR (Status);
+  }
+  return;
+}
+
+VOID
+SaDxePolicyDump (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *mDxePlatformSaPolicy
+  )
+/**
+  This function prints the DXE phase platform policy.
+
+  @param[in] mDxePlatformSaPolicy - SA DxePlatformPolicy protocol
+**/
+{
+#ifdef EFI_DEBUG
+  INTN  i;
+
+  DEBUG ((EFI_D_INFO, "\n------------------------ SA Platform Policy (DXE) dump BEGIN -----------------\n"));
+  DEBUG ((EFI_D_INFO, "Revision : %x\n", mDxePlatformSaPolicy->Revision));
+  DEBUG ((EFI_D_INFO, "------------------------ SA_VTD_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " VtdEnable : %x\n", mDxePlatformSaPolicy->Vtd->VtdEnable));
+  DEBUG ((EFI_D_INFO, " RmrrUsbBaseAddress : %x\n", mDxePlatformSaPolicy->Vtd->RmrrUsbBaseAddress));
+
+  DEBUG ((EFI_D_INFO, " VtdBaseAddress[%d] :", SA_VTD_ENGINE_NUMBER));
+  for (i = 0; i < SA_VTD_ENGINE_NUMBER; i++) {
+    DEBUG ((EFI_D_INFO, " %x", mDxePlatformSaPolicy->Vtd->BaseAddress[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, "------------------------ SA_MEMORY_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " SpdAddressTable[%d] :", SA_MC_MAX_SOCKETS));
+  for (i = 0; i < SA_MC_MAX_SOCKETS; i++) {
+    DEBUG ((EFI_D_INFO, " %x", mDxePlatformSaPolicy->MemoryConfig->SpdAddressTable[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " ChannelASlotMap : %x\n", mDxePlatformSaPolicy->MemoryConfig->ChannelASlotMap));
+  DEBUG ((EFI_D_INFO, " ChannelBSlotMap : %x\n", mDxePlatformSaPolicy->MemoryConfig->ChannelBSlotMap));
+  DEBUG ((EFI_D_INFO, " RmtBdatEnable : %x\n", mDxePlatformSaPolicy->MemoryConfig->RmtBdatEnable));
+  DEBUG ((EFI_D_INFO, " MrcTimeMeasure : %x\n", mDxePlatformSaPolicy->MemoryConfig->MrcTimeMeasure));
+  DEBUG ((EFI_D_INFO, " MrcFastBoot : %x\n", mDxePlatformSaPolicy->MemoryConfig->MrcFastBoot));
+
+  DEBUG ((EFI_D_INFO, "------------------------ SA_PCIE_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " DmiAspm : %x\n", mDxePlatformSaPolicy->PcieConfig->DmiAspm));
+
+  DEBUG ((EFI_D_INFO, " PegAspm[%d] :", SA_PEG_MAX_FUN));
+  for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+    DEBUG ((EFI_D_INFO, " %x", mDxePlatformSaPolicy->PcieConfig->PegAspm[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " PegAspmL0s[%d] :", SA_PEG_MAX_FUN));
+  for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+    DEBUG ((EFI_D_INFO, " %x", mDxePlatformSaPolicy->PcieConfig->PegAspmL0s[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " PegDeEmphasis[%d] :", SA_PEG_MAX_FUN));
+  for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+    DEBUG ((EFI_D_INFO, " %x", mDxePlatformSaPolicy->PcieConfig->PegDeEmphasis[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " DmiExtSync : %x\n", mDxePlatformSaPolicy->PcieConfig->DmiExtSync));
+  DEBUG ((EFI_D_INFO, " DmiDeEmphasis : %x\n", mDxePlatformSaPolicy->PcieConfig->DmiDeEmphasis));
+  DEBUG ((EFI_D_INFO, " DmiIot : %x\n", mDxePlatformSaPolicy->PcieConfig->DmiIot));
+
+  if (mDxePlatformSaPolicy->Revision >= DXE_SA_PLATFORM_POLICY_PROTOCOL_REVISION_2) {
+    if (mDxePlatformSaPolicy->PcieConfig->PcieAspmDevsOverride != NULL) {
+      DEBUG ((EFI_D_INFO, "------------------------ PCIE_ASPM_DEV_INFO -----------------\n"));
+      DEBUG ((EFI_D_INFO, " VendorId DeviceId RevId RootApmcMask EndpointApmcMask\n"));
+      i = 0;
+      while ((mDxePlatformSaPolicy->PcieConfig->PcieAspmDevsOverride[i].VendorId != SA_PCIE_DEV_END_OF_TABLE) &&
+             (i < MAX_PCIE_ASPM_OVERRIDE)) {
+        DEBUG ((EFI_D_INFO, " %04x     %04x     %02x    %01x            %01x\n",
+          mDxePlatformSaPolicy->PcieConfig->PcieAspmDevsOverride[i].VendorId,
+          mDxePlatformSaPolicy->PcieConfig->PcieAspmDevsOverride[i].DeviceId,
+          mDxePlatformSaPolicy->PcieConfig->PcieAspmDevsOverride[i].RevId,
+          mDxePlatformSaPolicy->PcieConfig->PcieAspmDevsOverride[i].RootApmcMask,
+          mDxePlatformSaPolicy->PcieConfig->PcieAspmDevsOverride[i].EndpointApmcMask));
+        i++;
+      }
+      DEBUG ((EFI_D_INFO, "------------------------ END_OF_TABLE -----------------------\n"));
+    }
+    if (mDxePlatformSaPolicy->PcieConfig->PcieLtrDevsOverride != NULL) {
+      DEBUG ((EFI_D_INFO, "------------------------ PCIE_LTR_DEV_INFO -----------------\n"));
+      DEBUG ((EFI_D_INFO, " VendorId DeviceId RevId SnoopLatency NonSnoopLatency\n"));
+      i = 0;
+      while ((mDxePlatformSaPolicy->PcieConfig->PcieLtrDevsOverride[i].VendorId != SA_PCIE_DEV_END_OF_TABLE) &&
+             (i < MAX_PCIE_LTR_OVERRIDE)) {
+        DEBUG ((EFI_D_INFO, " %04x     %04x     %02x    %01x            %01x\n",
+          mDxePlatformSaPolicy->PcieConfig->PcieLtrDevsOverride[i].VendorId,
+          mDxePlatformSaPolicy->PcieConfig->PcieLtrDevsOverride[i].DeviceId,
+          mDxePlatformSaPolicy->PcieConfig->PcieLtrDevsOverride[i].RevId,
+          mDxePlatformSaPolicy->PcieConfig->PcieLtrDevsOverride[i].SnoopLatency,
+          mDxePlatformSaPolicy->PcieConfig->PcieLtrDevsOverride[i].NonSnoopLatency));
+        i++;
+      }
+      DEBUG ((EFI_D_INFO, "------------------------ END_OF_TABLE ----------------------\n"));
+    }
+  }
+
+  for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+    DEBUG ((EFI_D_INFO, " PegPwrOpt[%d].LtrEnable            : %x\n", i, mDxePlatformSaPolicy->PcieConfig->PegPwrOpt[i].LtrEnable));
+    DEBUG ((EFI_D_INFO, " PegPwrOpt[%d].LtrMaxSnoopLatency   : %x\n", i, mDxePlatformSaPolicy->PcieConfig->PegPwrOpt[i].LtrMaxSnoopLatency));
+    DEBUG ((EFI_D_INFO, " PegPwrOpt[%d].ObffEnable           : %x\n", i, mDxePlatformSaPolicy->PcieConfig->PegPwrOpt[i].ObffEnable));
+    DEBUG ((EFI_D_INFO, " PegPwrOpt[%d].LtrMaxNoSnoopLatency : %x\n", i, mDxePlatformSaPolicy->PcieConfig->PegPwrOpt[i].LtrMaxNoSnoopLatency));
+  }
+
+  if (mDxePlatformSaPolicy->Revision >= DXE_SA_PLATFORM_POLICY_PROTOCOL_REVISION_3) {
+    DEBUG ((EFI_D_INFO, "------------------------ SA_SG_VBIOS_CONFIGURATION -----------------\n"));
+    DEBUG ((EFI_D_INFO, " LoadVbios : %x\n", mDxePlatformSaPolicy->VbiosConfig->LoadVbios));
+    DEBUG ((EFI_D_INFO, " ExecuteVbios : %x\n", mDxePlatformSaPolicy->VbiosConfig->ExecuteVbios));
+    DEBUG ((EFI_D_INFO, " VbiosSource : %x\n", mDxePlatformSaPolicy->VbiosConfig->VbiosSource));
+  }
+
+
+  DEBUG ((EFI_D_INFO, "------------------------ SA_IGD_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " RenderStandby : %x\n", mDxePlatformSaPolicy->IgdConfig->RenderStandby));
+  DEBUG ((EFI_D_INFO, "------------------------ SA_MISC_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " ChapDeviceEnable : %x\n", mDxePlatformSaPolicy->MiscConfig->ChapDeviceEnable));
+  DEBUG ((EFI_D_INFO, " Device4Enable : %x\n", mDxePlatformSaPolicy->MiscConfig->Device4Enable));
+  DEBUG ((EFI_D_INFO, " SubSystemVendorId : %x\n", mDxePlatformSaPolicy->MiscConfig->DefaultSvidSid->SubSystemVendorId));
+  DEBUG ((EFI_D_INFO, " SubSystemId : %x\n", mDxePlatformSaPolicy->MiscConfig->DefaultSvidSid->SubSystemId));
+  DEBUG ((EFI_D_INFO, " CridEnable : %x\n", mDxePlatformSaPolicy->MiscConfig->CridEnable));
+  DEBUG ((EFI_D_INFO, " AudioEnable : %x\n", mDxePlatformSaPolicy->MiscConfig->AudioEnable));
+  DEBUG ((EFI_D_INFO, " FviReport : %x\n", mDxePlatformSaPolicy->MiscConfig->FviReport));
+  DEBUG ((EFI_D_INFO, " FviSmbiosType : %x\n", mDxePlatformSaPolicy->MiscConfig->FviSmbiosType));
+  DEBUG ((EFI_D_INFO, "\n------------------------ SA Platform Policy (DXE) dump END -----------------\n"));
+#endif
+  return;
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.dxs b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.dxs
new file mode 100644
index 0000000..03c23ef
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.dxs
@@ -0,0 +1,65 @@
+/** @file
+  @todo ADD DESCRIPTION
+
+@copyright
+  Copyright (c)  1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains a 'Sample Driver' and is licensed as such
+  under the terms of your license agreement with Intel or your
+  vendor.  This file may be modified by the user, subject to
+  the additional terms of the license agreement
+
+**/
+
+//
+// Common for R8 and R9 codebase
+//
+#include "AutoGen.h"
+#include "DxeDepex.h"
+//
+// BUILD_WITH_GLUELIB and BUILD_WITH_EDKII_GLUE_LIB are both "defined" in R8 codebase;
+// BUILD_WITH_EDKII_GLUE_LIB is defined in Edk-Dev-Snapshot-20070228 and later version
+// BUILD_WITH_GLUELIB and BUILD_WITH_EDKII_GLUE_LIB are "not defined" in R9 codebase.
+//
+#if defined (BUILD_WITH_GLUELIB) || defined (BUILD_WITH_EDKII_GLUE_LIB)
+#include "EfiDepex.h"
+#endif
+
+#include EFI_PROTOCOL_DEPENDENCY      (SaPlatformPolicy)
+#include EFI_PROTOCOL_DEPENDENCY      (PciRootBridgeIo)
+#include EFI_PROTOCOL_DEPENDENCY      (BootScriptSave)
+#include EFI_PROTOCOL_DEPENDENCY      (AcpiSupport)
+#include EFI_PROTOCOL_DEPENDENCY      (PciHostBridgeResourceAllocation)
+#include EFI_PROTOCOL_DEPENDENCY      (CpuIo)
+#include EFI_PROTOCOL_DEPENDENCY      (DataHub)
+#include EFI_PROTOCOL_DEPENDENCY      (PowerMgmtInitDone)
+#if (EFI_SPECIFICATION_VERSION >= 0x0002000A)
+#include EFI_PROTOCOL_DEPENDENCY      (HiiDatabase)
+#else
+#include EFI_PROTOCOL_DEPENDENCY      (HII)
+#endif
+#include EFI_PROTOCOL_DEPENDENCY (FirmwareVolume)
+
+DEPENDENCY_START
+  EFI_ACPI_SUPPORT_GUID                                 AND
+  EFI_DATA_HUB_PROTOCOL_GUID                            AND
+  EFI_POWER_MGMT_INIT_DONE_PROTOCOL_GUID                AND
+  EFI_FIRMWARE_VOLUME_PROTOCOL_GUID                     AND
+  DXE_PLATFORM_SA_POLICY_GUID                           AND
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_GUID                  AND
+  EFI_BOOT_SCRIPT_SAVE_PROTOCOL_GUID                    AND
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL_GUID AND
+  EFI_CPU_IO_PROTOCOL_GUID                              AND
+#if (EFI_SPECIFICATION_VERSION >= 0x0002000A)
+    EFI_HII_DATABASE_PROTOCOL_GUID
+#else
+    EFI_HII_PROTOCOL_GUID
+#endif
+DEPENDENCY_END
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.h
new file mode 100644
index 0000000..a8c2b8f
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.h
@@ -0,0 +1,194 @@
+/** @file
+  Header file for SA Initialization Driver.
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _SA_INITIALIZATION_DRIVER_H_
+#define _SA_INITIALIZATION_DRIVER_H_
+
+///
+/// External include files do NOT need to be explicitly specified in real EDKII
+/// environment
+///
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "EfiScriptLib.h"
+#include "SaBuildFlags.h"
+#include "LegacyRegion.h"
+#include "Vtd.h"
+#include "GraphicsInit.h"
+#include "IgdOpregion.h"
+#include "PciExpressInit.h"
+#include "AudioInit.h"
+#include "RcFviDxeLib.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+#include "PcieComplex.h"
+#include "VTd.h"
+#include "SwitchableGraphicsInit.h"
+
+///
+/// Driver Consumed Protocol Prototypes
+///
+#include EFI_PROTOCOL_DEFINITION (SaPlatformPolicy)
+#include EFI_PROTOCOL_PRODUCER (SaInfo)
+#endif
+///
+/// Data definitions
+///
+#define CRID_DATA 0x69
+#define CRID_LOCK 0x17
+
+typedef struct {
+  UINT64  BaseAddr;
+  UINT32  Offset;
+  UINT32  AndMask;
+  UINT32  OrMask;
+} BOOT_SCRIPT_REGISTER_SETTING;
+
+typedef struct {
+  UINT64                Address;
+  EFI_BOOT_SCRIPT_WIDTH Width;
+  UINT32                Value;
+} BOOT_SCRIPT_PCI_REGISTER_SAVE;
+
+typedef struct {
+  EFI_SA_INFO_PROTOCOL  SaInfo;
+} SA_INSTANCE_PRIVATE_DATA;
+
+typedef struct {
+  UINT8 DeviceNumber;
+  UINT8 FunctionNumber;
+  UINT8 SvidRegOffset;
+} SA_SVID_SID_INIT_ENTRY;
+
+#define SA_FVI_STRING          "Reference Code - SA - System Agent"
+#define SA_FVI_SMBIOS_TYPE     0xDD  ///< Default value
+#define SA_FVI_SMBIOS_INSTANCE 0x06
+#define MEM_FVI_STRING         "Reference Code - MRC"
+#define MEM_RC_VERSION \
+  { \
+    0xFF, 0xFF, 0xFF, 0xFFFF \
+  }
+#define PCIE_FVI_STRING "SA - PCIe Version"
+#define PCIE_RC_VERSION \
+  { \
+    0xFF, 0xFF, 0xFF, 0xFFFF \
+  }
+#define SA_CRID_STATUS          "SA-CRID Status"
+#define SA_CRID_ORIGINAL_VALUE  "SA-CRID Original Value"
+#define SA_CRID_NEW_VALUE       "SA-CRID New Value"
+#define SA_CRID_ENABLED         "Enabled "
+#define SA_CRID_DISABLED        "Disabled"
+#define SA_CRID_VERSION \
+  { \
+    0xFF, 0xFF, 0xFF, 0xFFFF \
+  }
+#define VBIOS_FVI_STRING        "OPROM - VBIOS"
+#define VBIOS_RC_VERSION \
+  { \
+    0xFF, 0xFF, 0xFF, 0xFFFF \
+  }
+
+enum {
+  SA_RC_VER     = 0,
+  MEM_RC_VER,
+  PCIE_VER,
+  CRID_STATUS,
+  CRID_ORIGINAL,
+  CRID_NEW,
+  VBIOS_VER
+} SA_FVI_INDEX;
+
+extern FVI_ELEMENT_AND_FUNCTION       mSaFviElementsData[];
+extern FVI_DATA_HUB_CALLBACK_CONTEXT  mSaFviVersionData;
+extern UINTN                          mSaFviElements;
+
+///
+/// Function Prototype
+///
+VOID
+EFIAPI
+SaPciEnumCompleteCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+/**
+  This function gets registered as a callback to perform SA 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.
+
+  @retval EFI_SUCCESS   - Always.
+
+  **/
+;
+
+VOID
+EFIAPI
+SaExitPmAuthCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+/**
+  This function gets registered as a callback to perform SA configuration security lock
+
+  @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.
+
+  @retval EFI_SUCCESS   - Always.
+
+  **/
+;
+
+VOID
+DeviceConfigure (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  This function performs SA internal devices enabling/disabling
+
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  **/
+;
+
+VOID
+ProgramSvidSid (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+    Program SA devices Subsystem Vendor Identifier (SVID) and Subsystem Identifier (SID).
+
+    @param[in] DxePlatformSaPolicy   The SAPlatform Policy protocol instance
+**/
+;
+
+VOID
+SaDxePolicyDump (
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *mDxePlatformSaPolicy
+  )
+/**
+  This function prints the DXE phase platform policy.
+
+  @param[in] mDxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  **/
+;
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.inf b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.inf
new file mode 100644
index 0000000..638f6da
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInit.inf
@@ -0,0 +1,137 @@
+## @file
+#  Component description file for SystemAgent Initialization driver
+#
+#@copyright
+#  Copyright (c)  1999 - 2013 Intel Corporation. All rights reserved
+#  This software and associated documentation (if any) is furnished
+#  under a license and may only be used or copied in accordance
+#  with the terms of the license. Except as permitted by such
+#  license, no part of this software or documentation may be
+#  reproduced, stored in a retrieval system, or transmitted in any
+#  form or by any means without the express written consent of
+#  Intel Corporation.
+#
+#  This file contains a 'Sample Driver' and is licensed as such
+#  under the terms of your license agreement with Intel or your
+#  vendor.  This file may be modified by the user, subject to
+#  the additional terms of the license agreement
+#
+##
+
+[defines]
+BASE_NAME            = SaInit
+FILE_GUID            = DE23ACEE-CF55-4fb6-AA77-984AB53DE811
+COMPONENT_TYPE       = BS_DRIVER
+
+[sources.common]
+  SaInit.h
+  SaInit.c
+  LegacyRegion.h
+  LegacyRegion.c
+  Vtd.c
+  Vtd.h
+  IgdOpRegion.h
+  IgdOpRegion.c
+  GraphicsInit.h
+  GraphicsInit.c
+  PciExpressInit.h
+  PciExpressInit.c
+  PcieComplex.c
+  PcieComplex.h
+  AudioInit.c
+  AudioInit.h
+  SaFvi.c
+  SwitchableGraphicsInit.c
+  SwitchableGraphicsInit.h
+
+#
+# Edk II Glue Driver Entry Point
+#
+  EdkIIGlueDxeDriverEntryPoint.c
+
+[includes.common]
+  .
+  ../Common
+  $(EDK_SOURCE)/Foundation
+  $(EDK_SOURCE)/Foundation/Efi
+  $(EDK_SOURCE)/Foundation/Library
+  $(EDK_SOURCE)/Foundation/Efi/Include
+  $(EDK_SOURCE)/Foundation/core/Dxe
+  $(EDK_SOURCE)/Foundation/Include
+  $(EDK_SOURCE)/Foundation/Include/IndustryStandard
+  $(EDK_SOURCE)/Foundation/Framework
+  $(EDK_SOURCE)/Foundation/Framework/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include/pcd
+  $(EDK_SOURCE)/Foundation/Library/Dxe/Include
+  $(EDK_SOURCE)/Foundation/Cpu/Pentium/Include
+  $(EDK_SOURCE)/Foundation/Library/Dxe/UefiEfiIfrSupportLib
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Protocol
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/$(PROJECT_SA_MRC)/Pei/Source/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Library/SaPcieLib/Common
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include/Library
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Protocol
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include/Library
+  $(EFI_SOURCE)
+  $(EFI_SOURCE)/../MdePkg/Include
+
+#
+# Typically, the sample code referenced will be available in the code base already.
+# So, keep this include at the end to defer to the source base definition
+# and only use the sample code definition if source base does not include these files.
+#
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/SampleCode
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/SampleCode/Include
+
+[libraries.common]
+  EfiProtocolLib
+  EdkGuidLib
+  EfiCommonLib
+  EfiScriptLib
+  EdkIIGlueBaseLib
+  EdkIIGlueBaseIoLibIntrinsic
+  EdkIIGlueBaseMemoryLib
+  EdkIIGlueBasePciExpressLib
+  EdkIIGlueDxeReportStatusCodeLib
+  EdkIIGlueDxeDebugLibReportStatusCode
+  EdkIIGlueUefiBootServicesTableLib
+  EdkIIGlueUefiRuntimeServicesTableLib
+  EdkIIGlueDxeServicesTableLib
+  EdkIIGlueDxeHobLib
+  EdkIIGlueUefiLib
+  EdkIIGlueBasePciLibPciExpress
+  $(PROJECT_SA_FAMILY)ProtocolLib
+  $(PROJECT_PCH_FAMILY)ProtocolLib
+  RcFviDxeLib
+  PchPlatformLib
+  UefiEfiIfrSupportLib
+  SaGuidLib
+  EdkProtocolLib
+  CpuPlatformLib
+#
+# Comment out SaPcieDxeLib if ASPM initialization in DXE phase was not supported
+#
+  SaPcieDxeLib
+
+[nmake.common]
+  IMAGE_ENTRY_POINT = _ModuleEntryPoint
+  DPX_SOURCE = SaInit.dxs
+
+  C_FLAGS = $(C_FLAGS)  -D"__EDKII_GLUE_MODULE_ENTRY_POINT__=SaInitEntryPoint" \
+                        -D __EDKII_GLUE_BASE_IO_LIB_INTRINSIC__ \
+                        -D __EDKII_GLUE_BASE_LIB__ \
+                        -D __EDKII_GLUE_BASE_MEMORY_LIB__ \
+                        -D __EDKII_GLUE_DXE_REPORT_STATUS_CODE_LIB__ \
+                        -D __EDKII_GLUE_DXE_SERVICES_TABLE_LIB__ \
+                        -D __EDKII_GLUE_DXE_DEBUG_LIB_REPORT_STATUS_CODE__ \
+                        -D __EDKII_GLUE_UEFI_BOOT_SERVICES_TABLE_LIB__\
+                        -D __EDKII_GLUE_UEFI_RUNTIME_SERVICES_TABLE_LIB__ \
+                        -D __EDKII_GLUE_DXE_HOB_LIB__ \
+                        -D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+                        -D __EDKII_GLUE_UEFI_LIB__
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.cif b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.cif
new file mode 100644
index 0000000..f705e96
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.cif
@@ -0,0 +1,30 @@
+<component>
+	name = "SaInitDxe"
+	category = ModulePart
+	LocalRoot = "ReferenceCode\Chipset\SystemAgent\SaInit\Dxe"
+	RefName = "SaInitDxe"
+[files]
+"SaInitDxe.sdl"	
+"SaInitDxe.mak"	
+"SaInit.c"
+"SaInit.dxs"
+"SaInit.h"
+"SaInit.inf"
+"graphicsinit.c"
+"graphicsinit.h"
+"igdopregion.c"
+"igdopregion.h"
+"LegacyRegion.c"
+"LegacyRegion.h"
+"PcieComplex.c"
+"PcieComplex.h"
+"PciExpressInit.c"
+"PciExpressInit.h"
+"VTd.c"
+"VTd.h"
+"AudioInit.c"
+"AudioInit.h"
+"SaFvi.c"
+"SwitchableGraphicsInit.c"
+"SwitchableGraphicsInit.h"
+<endComponent>
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.mak b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.mak
new file mode 100644
index 0000000..809ba05
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.mak
@@ -0,0 +1,86 @@
+#---------------------------------------------------------------------------
+#   Create SaInitDxe Driver
+#---------------------------------------------------------------------------
+EDK : SaInitDxe
+
+SaInitDxe: $(BUILD_DIR)\SaInitDxe.mak SaInitDxeBin
+
+$(BUILD_DIR)\SaInitDxe.mak : $(SaInitDxe_DIR)\$(@B).cif $(SaInitDxe_DIR)\$(@B).mak $(BUILD_RULES)
+  $(CIF2MAK) $(SaInitDxe_DIR)\$(@B).cif $(CIF2MAK_DEFAULTS)
+
+SaInitDxe_INCLUDES=\
+  $(EdkIIGlueLib_INCLUDES)\
+  $(INTEL_MCH_INCLUDES) \
+  $(INTEL_PCH_INCLUDES)\
+  $(IndustryStandard_INCLUDES)\
+  $(INTEL_PLATFORM_PROTOCOL_INCLUDES)\
+  $(PROJECT_CPU_INCLUDES)\
+  /I$(INTEL_SYSTEM_AGENT_DIR)\Library\SaPcieLib\Common\
+  /I$(INTEL_SYSTEM_AGENT_DIR)\SampleCode\
+
+SaInitDxe_DEFINES =$(MY_DEFINES)\
+  /D "__EDKII_GLUE_MODULE_ENTRY_POINT__=SaInitEntryPoint" \
+  /D __EDKII_GLUE_BASE_IO_LIB_INTRINSIC__ \
+  /D __EDKII_GLUE_BASE_LIB__ \
+  /D __EDKII_GLUE_BASE_MEMORY_LIB__ \
+  /D __EDKII_GLUE_DXE_REPORT_STATUS_CODE_LIB__ \
+  /D __EDKII_GLUE_DXE_SERVICES_TABLE_LIB__ \
+  /D __EDKII_GLUE_DXE_DEBUG_LIB_REPORT_STATUS_CODE__ \
+  /D __EDKII_GLUE_UEFI_BOOT_SERVICES_TABLE_LIB__\
+  /D __EDKII_GLUE_DXE_HOB_LIB__ \
+  /D __EDKII_GLUE_UEFI_LIB__\
+  /D __EDKII_GLUE_UEFI_RUNTIME_SERVICES_TABLE_LIB__ \
+  /D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+
+SaInitDxe_LIB_LINKS =\
+    $(EFIPROTOCOLLIB)\
+    $(EDKGUIDLIB)\
+    $(EFICOMMONLIB)\
+    $(EFISCRIPTLIB)\
+    $(EdkIIGlueBaseLib_LIB)\
+!IF "$(x64_BUILD)"=="1"
+    $(EdkIIGlueBaseLibX64_LIB)\
+!ELSE
+    $(EdkIIGlueBaseLibIA32_LIB)\
+!ENDIF
+    $(EdkIIGlueBaseIoLibIntrinsic_LIB)\
+    $(EdkIIGlueBaseMemoryLib_LIB)\
+    $(EdkIIGlueDxeReportStatusCodeLib_LIB)\
+    $(EdkIIGlueDxeDebugLibReportStatusCode_LIB)\
+    $(EdkIIGlueUefiBootServicesTableLib_LIB)\
+    $(EdkIIGlueDxeServicesTableLib_LIB)\
+    $(EdkIIGlueDxeHobLib_LIB)\
+    $(EdkIIGlueUefiLib_LIB)\
+    $(INTEL_SA_PROTOCOL_LIB)\
+    $(INTEL_PCH_PROTOCOL_LIB)\
+    $(IntelPlatformProtocolLib_LIB)\
+!IF $(EFI_SPECIFICATION_VERSION) >= 0x0002000A
+    $(UEFIEFIIFRSUPPORTLIB)\
+!ELSE
+    $(EFIIFRSUPPORTLIB) \
+!ENDIF
+    $(SaGuidLib_LIB)\
+  	$(EdkIIGlueBasePciExpressLib_LIB)\
+    $(EdkIIGlueUefiRuntimeServicesTableLib_LIB)\
+    $(EdkIIGlueBasePciLibPciExpress_LIB)\
+    $(EDKPROTOCOLLIB)\
+    $(RcFviDxeLib_LIB)\
+    $(PchPlatformDxeLib_LIB)\
+    $(CpuPlatformLib_LIB)\
+#
+# Comment out SaPcieDxeLib if ASPM initialization in DXE phase was not supported
+#
+    $(SaPcieDxeLib_LIB)\
+
+SaInitDxeBin: $(SaInitDxe_LIB_LINKS)
+    $(MAKE) /$(MAKEFLAGS) $(EDKIIGLUE_DEFAULTS)\
+    /f $(BUILD_DIR)\SaInitDxe.mak all \
+    "MY_INCLUDES=$(SaInitDxe_INCLUDES)"\
+    "MY_DEFINES=$(SaInitDxe_DEFINES)"\
+    GUID=DE23ACEE-CF55-4fb6-AA77-984AB53DE811\
+    ENTRY_POINT=_ModuleEntryPoint \
+    TYPE=BS_DRIVER\
+    EDKIIModule=DXEDRIVER\
+    DEPEX1=$(SaInitDxe_DIR)\SaInit.dxs\
+    DEPEX1_TYPE=EFI_SECTION_DXE_DEPEX\
+    COMPRESS=1
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.sdl b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.sdl
new file mode 100644
index 0000000..e4a95b4
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SaInitDxe.sdl
@@ -0,0 +1,24 @@
+TOKEN
+  Name  = "SaInitDxe_SUPPORT"
+  Value  = "1"
+  Help  = "Main switch to enable SaInitDxe support in Project"
+  TokenType = Boolean
+  TargetEQU = Yes
+  TargetMAK = Yes
+  Master = Yes
+End
+
+PATH
+  Name  = "SaInitDxe_DIR"
+End
+
+MODULE
+  File  = "SaInitDxe.mak"
+  Help  = "Includes SaInitDxe.mak to Project"
+End
+
+ELINK
+  Name  = "$(BUILD_DIR)\SaInitDxe.ffs"
+  Parent  = "FV_MAIN"
+  InvokeOrder = AfterParent
+End
\ No newline at end of file
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SwitchableGraphicsInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SwitchableGraphicsInit.c
new file mode 100644
index 0000000..599a01a
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SwitchableGraphicsInit.c
@@ -0,0 +1,958 @@
+/** @file
+  SwitchableGraphics Dxe driver.
+  This DXE driver loads SwitchableGraphics acpi tables
+  for the platform.
+
+@copyright
+  Copyright (c) 2010 - 2013 Intel Corporation. All rights reserved.
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+
+#include "SwitchableGraphicsInit.h"
+#include EFI_PROTOCOL_CONSUMER (ExitPmAuth)
+#include <Token.h>  //<< AMI_OVERRIDE >>
+
+extern DXE_PLATFORM_SA_POLICY_PROTOCOL *mDxePlatformSaPolicy;
+
+EFI_BOOT_SERVICES     *gBS;
+SA_DATA_HOB           *SaDataHob;
+
+EFI_GUID              gSaDataHobGuid = SA_DATA_HOB_GUID;
+EFI_GUID              gSgAcpiTableStorageGuid = SG_ACPI_TABLE_STORAGE_GUID;
+EFI_GUID              gSgAcpiTablePchStorageGuid = SG_ACPI_TABLE_PCH_STORAGE_GUID;
+
+VOID                  *VbiosAddress = NULL;
+BOOLEAN               DgpuOpRomCopied;
+UINT32                VbiosSize;
+
+UINT8                 EndpointBus;
+UINT16                GpioBaseAddress;
+UINT8                 GpioSupport;
+
+UINT8                 RootPortDev;
+UINT8                 RootPortFun;
+
+CPU_FAMILY            CpuFamilyId;
+
+// AMI MODIFY BEGIN
+EFI_STATUS
+LoadTpvAcpiTables(
+  VOID
+  );
+// AMI MODIFY END
+
+/**
+  Initialize the SwitchableGraphics support (DXE).
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS         - SwitchableGraphics initialization complete
+  @retval EFI_OUT_OF_RESOURCES - Unable to allocated memory
+  @retval EFI_NOT_FOUND        - SA DataHob not found
+  @retval EFI_DEVICE_ERROR     - Error Accessing SG GPIO
+**/
+EFI_STATUS
+SwitchableGraphicsInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN EFI_SYSTEM_TABLE                *SystemTable,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Registration;
+  UINTN       PciD31F0RegBase;
+  UINT32      RootComplexBar;
+  UINT32      RpFn;
+
+  CpuFamilyId = GetCpuFamily();
+
+  if (CpuFamilyId == EnumCpuHswUlt) {
+      /// For SwitchableGraphics support the dGPU is present on PCH RootPort
+      RootPortDev = PCI_DEVICE_NUMBER_PCH_PCIE_ROOT_PORTS;
+    //AMI override begin
+      RootPortFun = SG_ULT_PORT_FUNC;
+	//RootPortFun = PCI_FUNCTION_NUMBER_PCH_PCIE_ROOT_PORT_5;
+    //AMI override end
+
+      PciD31F0RegBase = MmPciAddress (
+                          0,
+                          DEFAULT_PCI_BUS_NUMBER_PCH,
+                          PCI_DEVICE_NUMBER_PCH_LPC,
+                          PCI_FUNCTION_NUMBER_PCH_LPC,
+                          0
+                          );
+      RootComplexBar  = MmioRead32 (PciD31F0RegBase + R_PCH_LPC_RCBA) & B_PCH_LPC_RCBA_BAR;
+      RpFn = MmioRead32 (RootComplexBar + R_PCH_RCRB_RPFN);
+      /// dGPU sits on Root Port 5 [1-based]
+      /// Root Port 5 Function Number (RP5FN) = RPFN[18:16]
+      //AMI override begin
+      //RootPortFun = (UINT8) ((RpFn >> (4 * S_PCH_RCRB_PRFN_RP_FIELD)) & B_PCH_RCRB_RPFN_RP1FN);
+      RootPortFun = (UINT8) ((RpFn >> (RootPortFun * S_PCH_RCRB_PRFN_RP_FIELD)) & B_PCH_RCRB_RPFN_RP1FN);
+      ////AMI override end
+  } else {
+      /// Assume: For SwitchableGraphics support the dGPU is present on PEG RootPort by default
+      RootPortDev = SA_PEG10_DEV_NUM;
+      RootPortFun = SA_PEG10_FUN_NUM;
+  }
+
+  DEBUG ((EFI_D_INFO, "dGPU Rootport info[B/D/F] : [0x00/0x%x/0x%x]\n", RootPortDev, RootPortFun));
+
+  gBS = SystemTable->BootServices;
+
+  ///
+  /// Get SG GPIO info from SA HOB.
+  ///
+  Status = EfiGetSystemConfigurationTable (&gEfiHobListGuid, (VOID **) &SaDataHob);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SaDataHob = GetNextGuidHob (&gSaDataHobGuid, SaDataHob);
+  if (SaDataHob == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  GpioSupport = SaDataHob->SgInfo.SgGpioSupport;
+
+  ///
+  /// Read GPIO base
+  ///
+  GpioBaseAddress = McDevFunPciCfg16 (0, PCI_DEVICE_NUMBER_PCH_LPC, 0, R_PCH_LPC_GPIO_BASE) &~BIT0;
+  if (GpioBaseAddress == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ///
+  /// Update GlobalNvs data for runtime usage
+  ///
+  Status = UpdateGlobalNvsData (SaDataHob->SgInfo, DxePlatformSaPolicy);
+// AMI MODIFY BEGIN
+  DEBUG ((EFI_D_INFO, "SG:: OEM SSDT start"));
+#if SGOEMSSDT_SUPPORT
+     // Load OEM SSDT 
+  if (SaDataHob->SgInfo.SgMode == SgModeDgpu) { // In PEG mode
+
+  DEBUG ((EFI_D_INFO, "SG:: OEM SSDT start1"));
+    Status = LoadAndExecuteDgpuVbios (mDxePlatformSaPolicy->VbiosConfig);
+      return Status;
+   }
+#endif
+// AMI MODIFY END
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Load Intel SG SSDT tables
+  ///
+  Status = LoadAcpiTables ();
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Check to see if Switchable Graphics Mode is enabled
+  ///
+  if (SaDataHob->SgInfo.SgMode == SgModeMuxless) {
+  ///
+  /// Create ReadyToBoot callback for SG
+  ///
+  EfiCreateProtocolNotifyEvent (
+    &gExitPmAuthProtocolGuid,
+    EFI_TPL_CALLBACK,
+    SgExitPmAuthCallback,
+    NULL,
+    &Registration
+    );
+  }
+
+  return Status;
+}
+
+/**
+  Initialize the runtime SwitchableGraphics support data for ACPI tables in GlobalNvs.
+
+  @param[in] SaDataHob->SgInfo   - Pointer to Hob for SG system details.
+  @param[in] DxePlatformSaPolicy - Pointer to the loaded image protocol for this driver.
+
+  @retval EFI_SUCCESS - The data updated successfully.
+**/
+EFI_STATUS
+UpdateGlobalNvsData (
+  IN SG_INFO_HOB                     SgInfo,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+{
+  SYSTEM_AGENT_GLOBAL_NVS_AREA_PROTOCOL  *SaGlobalNvsArea;
+  UINT8                                  CapOffset;
+  UINT16                                 ExtendedCapOffset;
+  EFI_STATUS                             Status;
+  UINT32                                 Data32;
+
+  ///
+  ///  Locate the SA Global NVS Protocol.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gSaGlobalNvsAreaProtocolGuid,
+                  NULL,
+                  (VOID **) &SaGlobalNvsArea
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ///
+  /// SG Mode for ASL usage
+  ///
+  SaGlobalNvsArea->Area->SgMode               |= SaDataHob->SgInfo.SgMode;
+
+  SaGlobalNvsArea->Area->PXFixedDynamicMode   = SaDataHob->SgInfo.PXFixedDynamicMode; // AMI_OVERRIDE_FOR ATI 5.0 Fixed/Dynamic
+
+  SaGlobalNvsArea->Area->GpioBaseAddress      = GpioBaseAddress;
+
+  SaGlobalNvsArea->Area->SgGPIOSupport = SaDataHob->SgInfo.SgGpioSupport;
+
+  DEBUG ((EFI_D_INFO, "SG:: Switchable Graphics Mode : 0x%x\n", SaDataHob->SgInfo.SgMode));
+
+  if (SaDataHob->SgInfo.SgMode == SgModeMuxless) {
+    ///
+    /// SG Feature List for ASL usage
+    ///
+    //SaGlobalNvsArea->Area->SgFeatureList |= DxePlatformSaPolicy->FeatureList->WirelessDisplay;
+
+    if (SaDataHob->SgInfo.SgGpioSupport) {
+      ///
+      /// GPIO Assignment for ASL usage
+      ///
+      SaGlobalNvsArea->Area->SgDgpuPwrOK      = SaDataHob->SgInfo.SgDgpuPwrOK;
+      SaGlobalNvsArea->Area->SgDgpuHoldRst    = SaDataHob->SgInfo.SgDgpuHoldRst;
+      SaGlobalNvsArea->Area->SgDgpuPwrEnable  = SaDataHob->SgInfo.SgDgpuPwrEnable;
+      SaGlobalNvsArea->Area->SgDgpuPrsnt      = SaDataHob->SgInfo.SgDgpuPrsnt;
+
+      DEBUG ((EFI_D_INFO, "SG:: dGPU_PWROK GPIO   GPIO assigned = %d\n", SaDataHob->SgInfo.SgDgpuPwrOK & 0x7f));
+      DEBUG ((EFI_D_INFO, "SG:: dGPU_HOLD_RST#    GPIO assigned = %d\n", SaDataHob->SgInfo.SgDgpuHoldRst & 0x7f));
+      DEBUG ((EFI_D_INFO, "SG:: dGPU_PWR_EN#      GPIO assigned = %d\n", SaDataHob->SgInfo.SgDgpuPwrEnable & 0x7f));
+      DEBUG ((EFI_D_INFO, "SG:: dGPU_PRSNT#       GPIO assigned = %d\n", SaDataHob->SgInfo.SgDgpuPrsnt & 0x7f));
+    }
+
+    DEBUG ((EFI_D_INFO, "SG:: VBIOS Configurations:\n"));
+    DEBUG (
+      (
+      EFI_D_INFO, "SG:: Load VBIOS    (0=No Vbios;1=Load VBIOS)         =%d\n", DxePlatformSaPolicy->VbiosConfig->
+      LoadVbios
+      )
+      );
+    DEBUG (
+      (
+      EFI_D_INFO, "SG:: Execute VBIOS (0=Do not execute;1=Execute Vbios)  =%d\n", DxePlatformSaPolicy->VbiosConfig->
+      ExecuteVbios
+      )
+      );
+    DEBUG (
+      (
+      EFI_D_INFO, "SG:: VBIOS Source  (0=PCIE Card;1=FW Volume)         =%d\n", DxePlatformSaPolicy->VbiosConfig->
+      VbiosSource
+      )
+      );
+
+    ///
+    /// PEG Endpoint Base Addresses and Capability Structure Offsets for ASL usage
+    ///
+
+    ///
+    /// Save bus numbers on the PEG/PCH bridge.
+    ///
+    Data32 = MmPci32 (0, 0, RootPortDev, RootPortFun, PCI_PBUS);
+    Data32 &= 0x00FFFF00;
+
+    ///
+    /// Set PEG/PCH PortBus = 1 to Read Endpoint.
+    ///
+    MmPci32AndThenOr (0, 0, RootPortDev, RootPortFun, PCI_PBUS, 0xFF0000FF, 0x00010100);
+
+    ///
+    /// A config write is required in order for the device to re-capture the Bus number,
+    /// according to PCI Express Base Specification, 2.2.6.2
+    /// Write to a read-only register VendorID to not cause any side effects.
+    ///
+    McDevFunPciCfg16 (1, 0, 0, PCI_VID) = 0;
+
+    EndpointBus = MmPci8 (0, 0, RootPortDev, RootPortFun, PCI_SBUS);
+
+    if (EndpointBus != 0xFF) {
+      //AMI override begin
+      SaGlobalNvsArea->Area->EndpointBaseAddress = (UINT32) (MmPciAddress (0, EndpointBus, 0, 0, 0x0));
+      //AMI override end
+      SaGlobalNvsArea->Area->CapStrPresence = 0;
+
+      CapOffset = (UINT8) PcieFindCapId (EndpointBus, 0, 0, PEG_CAP_ID);
+      SaGlobalNvsArea->Area->EndpointPcieCapOffset = CapOffset;
+      DEBUG ((EFI_D_INFO, "SG:: Endpoint PCI Express Capability Offset : 0x%x\n", SaGlobalNvsArea->Area->EndpointPcieCapOffset));
+
+      ExtendedCapOffset = (UINT16) PcieFindExtendedCapId (EndpointBus, 0, 0, PEG_CAP_VER);
+      if (ExtendedCapOffset != 0) {
+        SaGlobalNvsArea->Area->CapStrPresence |= BIT0;
+        SaGlobalNvsArea->Area->EndpointVcCapOffset = ExtendedCapOffset;
+        DEBUG ((EFI_D_INFO, "SG:: Endpoint Virtual Channel Capability Offset : 0x%x\n", SaGlobalNvsArea->Area->EndpointVcCapOffset));
+      }
+   }
+
+    ///
+    /// Restore bus numbers on the PEG/PCH bridge.
+    ///
+    MmPci32AndThenOr (0, 0, RootPortDev, RootPortFun, PCI_PBUS, 0xFF0000FF, Data32);
+  } else {
+    DEBUG ((EFI_D_ERROR, "SG:: Switchable Graphics Mode disabled!!!\n"));
+    Status = EFI_LOAD_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  Load and execute the dGPU VBIOS.
+
+  @param[in] VbiosConfig - Pointer to VbiosConfig policy for Load/Execute and VBIOS Source.
+      LoadVbios    - 0 = Do Not Load   ; 1 = Load VBIOS
+      ExecuteVbios - 0 = Do Not Execute; 1 = Execute VBIOS
+      VbiosSource  - 0 = PCIE Device   ; 1 = FirmwareVolume => TBD
+
+  @retval EFI_SUCCESS     - Load and execute successful.
+  @exception EFI_UNSUPPORTED - Secondary VBIOS not loaded.
+**/
+EFI_STATUS
+LoadAndExecuteDgpuVbios (
+  IN SA_SG_VBIOS_CONFIGURATION    *VbiosConfig
+  )
+{
+  EFI_HANDLE                *HandleBuffer;
+  UINTN                     HandleCount;
+  UINTN                     Index;
+  VBIOS_PCIR_STRUCTURE      *PcirBlockPtr;
+  EFI_STATUS                Status;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  VBIOS_OPTION_ROM_HEADER   *VBiosRomImage;
+  EFI_LEGACY_BIOS_PROTOCOL  *LegacyBios;
+  EFI_IA32_REGISTER_SET     RegSet;
+  EFI_PHYSICAL_ADDRESS      ImageLocation;
+  UINTN                     Offset;
+
+  HandleBuffer = NULL;
+  DgpuOpRomCopied = FALSE;
+
+  DEBUG ((EFI_D_INFO, "SG:: LoadAndExecuteDgpuVbios\n"));
+
+  ///
+  /// Endpoint Device Bus#
+  ///
+  EndpointBus = MmPci8 (0, 0, RootPortDev, RootPortFun, PCI_SBUS);
+
+  ///
+  ///  Endpoint Device Not found
+  ///
+  if (EndpointBus == 0xFF) {
+    DEBUG ((EFI_D_ERROR, "SG:: 0x00/0x%x/0x%x Rootport's Endpoint Device Not found\n", RootPortDev, RootPortFun));
+    return EFI_UNSUPPORTED;
+  }
+
+  ///
+  /// Check Policy setting for loading VBIOS
+  ///
+  if (VbiosConfig->LoadVbios != 0) {
+
+    DEBUG ((EFI_D_INFO, "SG:: Start to load dGPU VBIOS if available\n"));
+
+    ///
+    /// Set as if an umcompressed video BIOS image was not obtainable.
+    ///
+    VBiosRomImage = NULL;
+
+    ///
+    /// Get all PCI IO protocols
+    ///
+    Status = gBS->LocateHandleBuffer (
+                    ByProtocol,
+                    &gEfiPciIoProtocolGuid,
+                    NULL,
+                    &HandleCount,
+                    &HandleBuffer
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    ///
+    /// Find the video BIOS by checking each PCI IO handle for DGPU video
+    /// BIOS OPROM.
+    ///
+    for (Index = 0; Index < HandleCount; Index++) {
+      Status = gBS->HandleProtocol (
+                      HandleBuffer[Index],
+                      &gEfiPciIoProtocolGuid,
+                      (VOID **) &PciIo
+                      );
+      if (EFI_ERROR (Status) || (PciIo->RomImage == NULL)) {
+        ///
+        /// If this PCI device doesn't have a ROM image, skip to the next device.
+        ///
+        continue;
+      }
+
+      VBiosRomImage = PciIo->RomImage;
+
+      ///
+      /// Get pointer to PCIR structure
+      ///
+      PcirBlockPtr = (VBIOS_PCIR_STRUCTURE *) ((UINTN) VBiosRomImage + VBiosRomImage->PcirOffset);
+      //AMI override begin
+      if (( PcirBlockPtr->VendorId != 0x10DE) && (PcirBlockPtr->VendorId != 0x1002)){
+        continue;
+      }
+      //AMI override end
+      ///
+      /// Check if we have an video BIOS OPROM for DGPU.
+      ///
+      if ((VBiosRomImage->Signature == OPTION_ROM_SIGNATURE) &&
+          (McDevFunPciCfg16 (EndpointBus, 0, 0, PCI_VID) == PcirBlockPtr->VendorId) &&
+          (PcirBlockPtr->ClassCode[2] == 0x03)
+          ) {
+
+        DEBUG ((EFI_D_INFO, "SG:: Loading dGPU VBIOS...\n"));
+
+        ///
+        /// Allocate space for copying Oprom
+        ///
+        VbiosSize = (PcirBlockPtr->ImageLength) * 512;
+        Status    = (gBS->AllocatePool) (EfiBootServicesData, VbiosSize, &VbiosAddress);
+        if (EFI_ERROR (Status)) {
+          break;
+        }
+	  
+        ///
+        /// Execute VBIOS based on Policy setting
+        ///
+        if (VbiosConfig->ExecuteVbios) {
+          DEBUG ((EFI_D_INFO, "SG:: Execute dGPU VBIOS...\n"));
+          ///
+          /// Memory IO Bus Master needs to be enabled when we execute the vbios
+          ///
+          ///
+          /// Enable Memory Access, IO Access Bus Master enable on PEG/PCH ROOT PORT
+          ///
+          MmPci16Or (0, 0, RootPortDev, RootPortFun, PCI_CMD, BIT0 + BIT1 + BIT2);
+
+          ///
+          /// Enable Memory Access, IO Access Bus Master enable and Rom Enable on Peg/PCH Endpoint device
+          ///
+          McDevFunPciCfg16Or (EndpointBus, 0, 0, PCI_CMD, BIT0 + BIT1 + BIT2);
+
+          ///
+          /// Allocate 64kb under 1MB memory region
+          ///
+          Status = AllocateLegacyMemory (
+                    AllocateMaxAddress,
+                    CONVENTIONAL_MEMORY_TOP,
+                    (BIN_FILE_SIZE_MAX / 4096),
+                    &ImageLocation
+                    );
+          if (!EFI_ERROR (Status)) {
+            (gBS->SetMem) ((VOID *) (UINTN) ImageLocation, BIN_FILE_SIZE_MAX, 0);
+
+            ///
+            /// After allocation copy VBIOS to buffer
+            ///
+            (gBS->CopyMem) ((VOID *) (UINTN) ImageLocation, PciIo->RomImage, VbiosSize);
+
+            Status = gBS->LocateProtocol (
+                            &gEfiLegacyBiosProtocolGuid,
+                            NULL,
+                            (VOID **) &LegacyBios
+                            );
+            if (!EFI_ERROR (Status)) {
+              (gBS->SetMem) (&RegSet, sizeof (EFI_IA32_REGISTER_SET), 0);
+
+              RegSet.H.AH = MmPci8 (0, 0, RootPortDev, RootPortFun, PCI_SBUS);
+              Offset      = MemoryRead16 ((UINTN) ImageLocation + 0x40);
+              LegacyBios->FarCall86 (
+                            LegacyBios,
+                            ((UINT16) RShiftU64 ((ImageLocation & 0x000FFFF0),
+                          4)),
+                            ((UINT16) Offset),
+                            &RegSet,
+                            NULL,
+                            0
+                            );
+
+              Offset = MemoryRead16 ((UINTN) ImageLocation + 0x42) + (UINTN) ImageLocation;
+              if (MemoryRead16 ((UINTN) ImageLocation + 0x44) == 0x0) {
+                VbiosSize = MemoryRead8 ((UINTN) ImageLocation + 0x2) * 512;
+              } else {
+                VbiosSize = MemoryRead16 ((UINTN) ImageLocation + 0x44) * 512;
+              }
+              ///
+              /// Copy Oprom to allocated space for the following scenario:
+              /// # Load vbios and Execute vbios policy setting
+              ///
+              DEBUG ((EFI_D_INFO, "Copy Oprom to allocated space: Load & Execute policy satisfied\n"));
+              (gBS->CopyMem) (VbiosAddress, (VOID *) Offset, VbiosSize);
+              DgpuOpRomCopied = TRUE;
+              (gBS->SetMem) ((VOID *) (UINTN) ImageLocation, BIN_FILE_SIZE_MAX, 0);
+            }
+            (gBS->FreePages) (ImageLocation, (BIN_FILE_SIZE_MAX / 4096));
+          }
+
+          ///
+          /// Disable Memory Access, IO Access Bus Master enable and Rom Enable on PEG/PCH Endpoint device
+          ///
+          McDevFunPciCfg16And (EndpointBus, 0, 0, PCI_CMD, ~(BIT0 + BIT1 + BIT2));
+
+          ///
+          /// Disable Memory Access, IO Access Bus Master enable on PEG/PCH Root Port
+          ///
+          MmPci16And (0, 0, RootPortDev, RootPortFun, PCI_CMD, ~(BIT0 + BIT1 + BIT2));
+        } 
+		
+        ///
+        /// Copy Oprom to allocated space  for the following scenario:
+        /// # Load vbios and Execute vbios policy setting in which dGPU execution is not called
+        /// # Load vbios but don't Execute vbios policy setting
+        ///
+        if ((VbiosAddress!=NULL) && (!DgpuOpRomCopied)) {
+          DEBUG ((EFI_D_INFO, "Copy Oprom to allocated space: Load policy satisfied\n"));
+          (gBS->CopyMem) (VbiosAddress, PciIo->RomImage, VbiosSize);
+          DgpuOpRomCopied = TRUE;
+        }
+
+    // AMI MODIFY BEGIN
+    //
+    //
+#ifdef AMI_SgTpv_SUPPORT
+    if (!EFI_ERROR (Status)) {
+      Status = LoadTpvAcpiTables ();
+    }
+#endif // AMI_SgTpv_SUPPORT
+    //
+    // AMI MODIFY ENDS
+    //
+        break;
+      }
+    }
+
+  }
+  
+
+  if (VbiosAddress!=NULL) {
+    (gBS->FreePool) (VbiosAddress);
+  }
+
+  if (HandleBuffer!=NULL) {
+    (gBS->FreePool) (HandleBuffer);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read SG GPIO value
+
+  @param[in] Value - PCH GPIO number and Active value
+      Bit0 to Bit7    - PCH GPIO Number
+      Bit8            - GPIO Active value (0 = Active Low; 1 = Active High)
+
+  @retval GPIO read value.
+**/
+BOOLEAN
+GpioRead (
+  IN UINT8 Value
+  )
+{
+  BOOLEAN Active;
+  UINT32  Data;
+  UINT16  BitOffset=0;
+  UINT16  Offset=0;
+
+  ///
+  /// Check if SG GPIOs are supported
+  ///
+  if (GpioSupport == 0) {
+    return FALSE;
+  }
+  ///
+  /// Extract GPIO number and Active value
+  ///
+  Active = (BOOLEAN) (Value >> 7);
+  Value &= 0x7F;
+
+  if (CpuFamilyId == EnumCpuHswUlt) {
+      Offset    = R_PCH_GP_N_CONFIG0 + (Value * 0x08);
+      BitOffset = 30; //GPI_LVL
+  } else {
+    if (Value < 0x20) {
+      Offset    = R_PCH_GPIO_LVL;
+      BitOffset = Value;
+    } else if (Value < 0x40) {
+      Offset    = R_PCH_GPIO_LVL2;
+      BitOffset = Value - 0x20;
+    } else {
+      Offset    = R_PCH_GPIO_LVL3;
+      BitOffset = Value - 0x40;
+    }
+  }
+
+  ///
+  /// Read specified value GPIO
+  ///
+  Data = IoRead32 (GpioBaseAddress + Offset);
+  Data >>= BitOffset;
+
+  if (Active == 0) {
+    Data = ~Data;
+  }
+
+  return (BOOLEAN) (Data & 0x1);
+}
+
+/**
+  Write SG GPIO value
+
+  @param[in] Value - PCH GPIO number and Active value
+      Bit0 to Bit7    - PCH GPIO Number
+      Bit8            - GPIO Active value (0 = Active Low; 1 = Active High)
+  @param[in] Level - Write data (0 = Disable; 1 = Enable)
+**/
+VOID
+GpioWrite (
+  IN UINT8   Value,
+  IN BOOLEAN Level
+  )
+{
+  BOOLEAN Active;
+  UINT32  Data;
+  UINT16  BitOffset=0;
+  UINT16  Offset=0;
+
+  ///
+  /// Check if SG GPIOs are supported
+  ///
+  if (GpioSupport == 0) {
+    return ;
+  }
+
+  Active = (BOOLEAN) (Value >> 7);
+  Value &= 0x7F;
+
+  if (Active == 0) {
+    Level = (~Level) & 0x1;
+  }
+
+  if (CpuFamilyId == EnumCpuHswUlt) {
+      Offset    = R_PCH_GP_N_CONFIG0 + (Value * 0x08);
+      BitOffset = 31; //GPO_LVL
+  } else {
+    if (Value < 0x20) {
+      Offset    = R_PCH_GPIO_LVL;
+      BitOffset = Value;
+    } else if (Value < 0x40) {
+      Offset    = R_PCH_GPIO_LVL2;
+      BitOffset = Value - 0x20;
+    } else {
+      Offset    = R_PCH_GPIO_LVL3;
+      BitOffset = Value - 0x40;
+    }
+  }
+
+  Data = IoRead32 (GpioBaseAddress + Offset);
+  Data &= ~(0x1 << BitOffset);
+  Data |= (Level << BitOffset);
+  IoWrite32 (GpioBaseAddress + Offset, Data);
+
+  return ;
+}
+
+/**
+  Do an AllocatePages () of type AllocateMaxAddress for EfiBootServicesCode
+  memory.
+
+  @param[in] AllocateType     - Allocated Legacy Memory Type
+  @param[in] StartPageAddress - Start address of range
+  @param[in] Pages            - Number of pages to allocate
+  @param[in, out] Result      - Result of allocation
+
+  @retval EFI_SUCCESS - Legacy16 code loaded
+  @retval Other       - No protocol installed, unload driver.
+**/
+EFI_STATUS
+AllocateLegacyMemory (
+  IN  EFI_ALLOCATE_TYPE         AllocateType,
+  IN  EFI_PHYSICAL_ADDRESS      StartPageAddress,
+  IN  UINTN                     Pages,
+  IN OUT  EFI_PHYSICAL_ADDRESS  *Result
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  MemPage;
+
+  ///
+  /// Allocate Pages of memory less <= StartPageAddress
+  ///
+  MemPage = (EFI_PHYSICAL_ADDRESS) (UINTN) StartPageAddress;
+  Status  = (gBS->AllocatePages) (AllocateType, EfiBootServicesCode, Pages, &MemPage);
+
+  ///
+  /// Do not ASSERT on Status error but let caller decide since some cases
+  /// memory is already taken but that is ok.
+  ///
+  if (!EFI_ERROR (Status)) {
+    *Result = (EFI_PHYSICAL_ADDRESS) (UINTN) MemPage;
+  }
+
+  return Status;
+}
+
+/**
+  Load Intel SG SSDT Tables
+
+  @param[in] None
+
+  @retval EFI_SUCCESS - SG SSDT Table load successful.
+**/
+EFI_STATUS
+LoadAcpiTables (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_HANDLE                    *HandleBuffer;
+  BOOLEAN                       LoadTable;
+  UINTN                         NumberOfHandles;
+  UINTN                         Index;
+  INTN                          Instance;
+  UINTN                         Size;
+  UINT32                        FvStatus;
+  UINTN                         TableHandle;
+  EFI_GUID                      AcpiTableGuid;
+  EFI_FV_FILETYPE               FileType;
+  EFI_FV_FILE_ATTRIBUTES        Attributes;
+  EFI_FIRMWARE_VOLUME_PROTOCOL  *FwVol;
+  EFI_ACPI_TABLE_PROTOCOL       *AcpiTable;
+  EFI_ACPI_TABLE_VERSION        Version;
+  EFI_ACPI_DESCRIPTION_HEADER   *TableHeader;
+  EFI_ACPI_COMMON_HEADER        *Table;
+
+  FwVol         = NULL;
+  Table         = NULL;
+
+  AcpiTableGuid = gSgAcpiTableStorageGuid;
+  if (CpuFamilyId == EnumCpuHswUlt) {
+    AcpiTableGuid = gSgAcpiTablePchStorageGuid;
+  }
+
+  DEBUG ((EFI_D_INFO, "SG:: Loading ACPI Tables...\n"));
+
+  ///
+  /// Locate FV protocol.
+  ///
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolumeProtocolGuid,
+                  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],
+                    &gEfiFirmwareVolumeProtocolGuid,
+                    (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,
+                      &AcpiTableGuid,
+                      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
+  ///
+  (gBS->FreePool) (HandleBuffer);
+
+  ///
+  /// Sanity check that we found our data file
+  ///
+  ASSERT (FwVol);
+
+  ///
+  /// By default, a table belongs in all ACPI table versions published.
+  ///
+  Version = EFI_ACPI_TABLE_VERSION_1_0B | EFI_ACPI_TABLE_VERSION_2_0 | EFI_ACPI_TABLE_VERSION_3_0;
+
+  ///
+  /// 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,
+                      &AcpiTableGuid,
+                      EFI_SECTION_RAW,
+                      Instance,
+                      (VOID **) &Table,
+                      &Size,
+                      &FvStatus
+                      );
+    if (!EFI_ERROR (Status)) {
+      ///
+      /// check and load SwitchableGraphics SSDT table
+      ///
+      LoadTable   = FALSE;
+      TableHeader = (EFI_ACPI_DESCRIPTION_HEADER *) Table;
+
+      if (((EFI_ACPI_DESCRIPTION_HEADER *) TableHeader)->OemTableId == EFI_SIGNATURE_64 (
+          'S',
+          'g',
+          'P',
+          'e',
+          'g',
+          0,
+          0,
+          0
+          )
+          ) {
+        ///
+        /// This is SG SSDT [dGPU is present on PEG RootPort]
+        ///
+        DEBUG ((EFI_D_INFO, "SG:: ---- SG SSDT ----\n"));
+        DEBUG ((EFI_D_INFO, "SG:: Found out SSDT:SgPeg [SgSsdt.asl]. dGPU is present on PEG RootPort.\n"));
+        LoadTable = TRUE;
+      } 
+	  
+	  if (((EFI_ACPI_DESCRIPTION_HEADER *) TableHeader)->OemTableId == EFI_SIGNATURE_64 (
+          'S',
+          'g',
+          'P',
+          'c',
+          'h',
+          0,
+          0,
+          0
+          )
+          ) {
+        ///
+        /// This is SG SSDT [dGPU is present on PCH RootPort]
+        ///
+        DEBUG ((EFI_D_INFO, "SG:: ---- SG SSDT ----\n"));
+        DEBUG ((EFI_D_INFO, "SG:: Found out SSDT:SgPch [SgSsdtPch.asl]. dGPU is present on PCH RootPort.\n"));
+        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;
+}
+
+
+VOID
+EFIAPI
+SgExitPmAuthCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *ProtocolPointer;
+
+  ///
+  /// 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 (&gExitPmAuthProtocolGuid, NULL, &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+     return;
+  }
+
+  gBS->CloseEvent (Event);
+
+  DEBUG ((EFI_D_INFO, "SG:: ExitPmAuth Callback\n"));
+  ///
+  /// Load and Execute dGPU VBIOS
+  ///
+  Status = LoadAndExecuteDgpuVbios (mDxePlatformSaPolicy->VbiosConfig);
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SwitchableGraphicsInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SwitchableGraphicsInit.h
new file mode 100644
index 0000000..4d32f8f
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/SwitchableGraphicsInit.h
@@ -0,0 +1,267 @@
+/** @file
+  Header file for the SwitchableGraphics Dxe driver.
+  This driver loads SwitchableGraphics ACPI tables.
+
+@copyright
+  Copyright (c) 2010 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+
+#ifndef _SWITCHABLE_GRAPHICS_DXE_H_
+#define _SWITCHABLE_GRAPHICS_DXE_H_
+
+//
+// External include files do NOT need to be explicitly specified in real EDKII
+// environment
+//
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "EfiScriptLib.h"
+#endif
+
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+
+#include "PchAccess.h"
+#include "Acpi3_0.h"
+#include "SaAccess.h"
+
+#include EFI_PROTOCOL_DEPENDENCY (PciIo)
+#include EFI_PROTOCOL_DEPENDENCY (AcpiTable)
+#include EFI_PROTOCOL_DEPENDENCY (FirmwareVolume)
+#include EFI_PROTOCOL_DEPENDENCY (SaGlobalNvsArea)
+#include EFI_GUID_DEFINITION (SaDataHob)
+#include EFI_PROTOCOL_DEFINITION (SaPlatformPolicy)
+
+///
+/// SG ACPI table data storage file
+///
+#include EFI_GUID_DEFINITION (SgAcpiTableStorage)
+
+///
+/// Switchable Graphics defines.
+///
+#define CONVENTIONAL_MEMORY_TOP 0xA0000 ///< 640 KB
+#define BIN_FILE_SIZE_MAX       0x10000
+
+#define OPTION_ROM_SIGNATURE    0xAA55
+
+#define MemoryRead16(Address)   * (UINT16 *) (Address)
+#define MemoryRead8(Address)    * (UINT8 *) (Address)
+
+///
+/// PEG Capability Equates
+///
+#define PEG_CAP_ID  0x10
+#define PEG_CAP_VER 0x2
+
+#pragma pack(1)
+typedef struct {
+  UINT16  Signature;  ///< 0xAA55
+  UINT8   Reserved[22];
+  UINT16  PcirOffset;
+} VBIOS_OPTION_ROM_HEADER;
+#pragma pack()
+
+#pragma pack(1)
+typedef struct {
+  UINT32  Signature;  ///< "PCIR"
+  UINT16  VendorId;
+  UINT16  DeviceId;
+  UINT16  Reserved0;
+  UINT16  Length;
+  UINT8   Revision;
+  UINT8   ClassCode[3];
+  UINT16  ImageLength;
+  UINT16  CodeRevision;
+  UINT8   CodeType;
+  UINT8   Indicator;
+  UINT16  Reserved1;
+} VBIOS_PCIR_STRUCTURE;
+#pragma pack()
+
+/**
+  Initialize the SwitchableGraphics support.
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS         - SwitchableGraphics initialization complete
+  @retval EFI_OUT_OF_RESOURCES - Unable to allocated memory
+**/
+EFI_STATUS
+SwitchableGraphicsInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN EFI_SYSTEM_TABLE                *SystemTable,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+
+/**
+  Load and execute the dGPU VBIOS.
+
+  @param[in] VbiosData - Pointer to VbiosData policy for Load/Execute and VBIOS Source.
+      LoadVbios    - 0 = Do Not Load   ; 1 = Load VBIOS
+      ExecuteVbios - 0 = Do Not Execute; 1 = Execute VBIOS
+      VbiosSource  - 0 = PCIE Device   ; 1 = FirmwareVolume => TBD
+
+  @retval EFI_SUCCESS     - Load and execute successful.
+  @exception EFI_UNSUPPORTED - Secondary VBIOS not loaded.
+**/
+EFI_STATUS
+LoadAndExecuteDgpuVbios (
+  IN SA_SG_VBIOS_CONFIGURATION  *VbiosConfig
+  )
+;
+
+/**
+  Initialize the runtime SwitchableGraphics support data for ACPI tables in GlobalNvs.
+  @param[in] SgInfoDataHob   - Pointer to Hob for SG system details.
+  @param[in] DxePlatformSgPolicy - Pointer to the loaded image protocol for this driver.
+
+  @retval EFI_SUCCESS - The data updated successfully.
+**/
+EFI_STATUS
+UpdateGlobalNvsData (
+  IN SG_INFO_HOB                     SgInfo,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+;
+
+/**
+  Do an AllocatePages () of type AllocateMaxAddress for EfiBootServicesCode
+  memory.
+
+  @param[in] AllocateType     - Allocated Legacy Memory Type
+  @param[in] StartPageAddress - Start address of range
+  @param[in] Pages            - Number of pages to allocate
+  @param[in, out] Result      - Result of allocation
+
+  @retval EFI_SUCCESS - Legacy16 code loaded
+  @retval Other       - No protocol installed, unload driver.
+**/
+EFI_STATUS
+AllocateLegacyMemory (
+  IN  EFI_ALLOCATE_TYPE         AllocateType,
+  IN  EFI_PHYSICAL_ADDRESS      StartPageAddress,
+  IN  UINTN                     Pages,
+  IN OUT  EFI_PHYSICAL_ADDRESS  *Result
+  )
+;
+
+/**
+  Search and return the offset of desired Pci Express Capability ID
+    CAPID list:
+      0x0001 = Advanced Error Rreporting Capability
+      0x0002 = Virtual Channel Capability
+      0x0003 = Device Serial Number Capability
+      0x0004 = Power Budgeting Capability
+
+    @param[in] Bus       -   Pci Bus Number
+    @param[in] Device    -   Pci Device Number
+    @param[in] Function  -   Pci Function Number
+    @param[in] CapId     -   Extended CAPID to search for
+
+    @retval 0       - CAPID not found
+    @retval Other   - CAPID found, Offset of desired CAPID
+**/
+UINT32
+PcieFindExtendedCapId (
+  IN UINT8  Bus,
+  IN UINT8  Device,
+  IN UINT8  Function,
+  IN UINT16 CapId
+  )
+;
+
+/**
+  Find the Offset to a given Capabilities ID
+    CAPID list:
+      0x01 = PCI Power Management Interface
+      0x04 = Slot Identification
+      0x05 = MSI Capability
+      0x10 = PCI Express Capability
+
+    @param[in] Bus       -   Pci Bus Number
+    @param[in] Device    -   Pci Device Number
+    @param[in] Function  -   Pci Function Number
+    @param[in] CapId     -   CAPID to search for
+
+    @retval 0       - CAPID not found
+    @retval Other   - CAPID found, Offset of desired CAPID
+**/
+UINT32
+PcieFindCapId (
+  IN UINT8 Bus,
+  IN UINT8 Device,
+  IN UINT8 Function,
+  IN UINT8 CapId
+  )
+;
+
+/**
+  Read SG GPIO value
+
+  @param[in] Value - PCH GPIO number and Active value
+      Bit0 to Bit7    - PCH GPIO Number
+      Bit8            - GPIO Active value (0 = Active Low; 1 = Active High)
+
+  @retval GPIO read value.
+**/
+BOOLEAN
+GpioRead (
+  IN UINT8 Value
+  )
+;
+
+/**
+  Write SG GPIO value
+
+  @param[in] Value - PCH GPIO number and Active value
+      Bit0 to Bit7    - PCH GPIO Number
+      Bit8            - GPIO Active value (0 = Active Low; 1 = Active High)
+  @param[in] Level - Write data (0 = Disable; 1 = Enable)
+**/
+VOID
+GpioWrite (
+  IN UINT8   Value,
+  IN BOOLEAN Level
+  )
+;
+
+/**
+  Load Intel SG SSDT Tables
+
+  @param[in] None
+
+  @retval EFI_SUCCESS - SG SSDT Table load successful.
+**/
+EFI_STATUS
+LoadAcpiTables (
+  VOID
+  )
+;
+
+
+VOID
+EFIAPI
+SgExitPmAuthCallback (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/VTd.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/VTd.c
new file mode 100644
index 0000000..1dea621
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/VTd.c
@@ -0,0 +1,934 @@
+/** @file
+  This code provides a initialization of intel VT-d (Virtualization Technology for Directed I/O).
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+@todo assumption is ANDD table will use device path to be loaded dynamically, need to use pchplatformpolicy if assumption is incorrect.
+**/
+#include "SaInit.h"
+#include "VTd.h"
+
+UINT32                           mPchRootComplexBar;
+DXE_PLATFORM_SA_POLICY_PROTOCOL  *mDxePlatformSaPolicy;
+DXE_PCH_PLATFORM_POLICY_PROTOCOL *mDxePlatformPchPolicy;
+PCH_SERIES                       mPchSeries;
+
+/**
+For device that specified by Device Num and Function Num,
+mDevEnMap is used to check device presence.
+  0x80 means use Device ID to detemine presence
+
+ The structure is used to check if device scope is valid when update DMAR table
+**/
+UINT16  mDevEnMap[][2] = {
+  {
+    0x0200,
+    0x80
+  },  ///< D2F0
+  {
+    0x1D00,
+    0x80
+  },  ///< D29F0
+  {
+    0x1A00,
+    0x80
+  },  ///< D26F0
+  {
+    0x1400,
+    0x80
+  }   ///< D20F0
+};
+
+BOOLEAN mInterruptRemappingSupport;
+
+UINT16
+GetFunDisableBit (
+  UINT8 DevNum,
+  UINT8 FunNum
+  )
+/**
+    Get the corresponding device Enable/Disable bit according DevNum and FunNum
+
+    @param[in] DevNum  - Device Number
+    @param[in] FunNum  - Function Number
+
+    @retval If the device is found, return disable/Enable bit in FD/Deven reigster
+    @retval If not found return 0xFF
+**/
+{
+  UINTN Index;
+
+  for (Index = 0; Index < sizeof (mDevEnMap) / 4; Index++) {
+    if (mDevEnMap[Index][0] == ((DevNum << 0x08) | FunNum)) {
+      return mDevEnMap[Index][1];
+    }
+  }
+
+  return 0xFF;
+}
+
+VOID
+UpdateDRHD (
+  IN OUT VOID *DrhdEnginePtr
+  )
+/**
+    Update the DRHD structure
+
+    @param[in, out] DrhdEnginePtr       - A pointer to DRHD structure
+**/
+{
+  UINT16                        Length;
+  UINT16                        DisableBit;
+  UINTN                         DeviceScopeNum;
+  BOOLEAN                       NeedRemove;
+  EFI_ACPI_DRHD_ENGINE1_STRUCT  *DrhdEngine;
+
+  //
+  // Convert DrhdEnginePtr to EFI_ACPI_DRHD_ENGINE1_STRUCT Pointer
+  //
+  DrhdEngine      = (EFI_ACPI_DRHD_ENGINE1_STRUCT *) DrhdEnginePtr;
+
+  Length          = DrhdEngine->Length;
+  DeviceScopeNum  = (DrhdEngine->Length - EFI_ACPI_DRHD_ENGINE_HEADER_LENGTH) / sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE);
+  DisableBit = GetFunDisableBit (
+                DrhdEngine->DeviceScope[0].PciPath[0],
+                DrhdEngine->DeviceScope[0].PciPath[1]
+                );
+  NeedRemove = FALSE;
+  if ((DisableBit == 0xFF) ||
+      (DrhdEngine->RegisterBaseAddress == 0) ||
+      ((DisableBit == 0x80) &&
+      (MmPci32 (0, 0, DrhdEngine->DeviceScope[0].PciPath[0], DrhdEngine->DeviceScope[0].PciPath[1], 0x00) == 0xFFFFFFFF))
+      ){
+        NeedRemove = TRUE;
+  }
+  if (NeedRemove) {
+    Length -= sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE);
+  }
+  ///
+  /// If no devicescope is left, we set the structure length as 0x00
+  ///
+  if ((Length > EFI_ACPI_DRHD_ENGINE_HEADER_LENGTH) || (DrhdEngine->Flags == 0x01)) {
+    DrhdEngine->Length = Length;
+  } else {
+    DrhdEngine->Length = 0;
+  }
+}
+
+UINT8
+GetIoApicID (
+  VOID
+  )
+/**
+    Get IOAPIC ID from LPC
+
+    @retval APIC ID
+**/
+{
+  UINT8 VOLATILE  *IoapicIndex;
+  UINT32 VOLATILE *IoapicData;
+  UINT32          Data32;
+
+  ///
+  /// Get IOAPIC base
+  ///
+  IoapicIndex = (UINT8 *) (UINTN) (R_PCH_IO_APIC_INDEX + ((PchMmRcrb16 (R_PCH_RCRB_OIC) & 0x0ff) << 12));
+  IoapicData  = (UINT32 *) (UINTN) (R_PCH_IO_APIC_DATA + ((PchMmRcrb16 (R_PCH_RCRB_OIC) & 0x0ff) << 12));
+
+  ///
+  /// Get APIC ID from Identification Register (Index 0)
+  ///
+  *IoapicIndex  = 0;
+  Data32        = (*IoapicData & 0x0F000000) >> 24;
+
+  return (UINT8) Data32;
+}
+
+VOID
+UpdateDRHD2 (
+  IN OUT VOID *DrhdEnginePtr
+  )
+/**
+    Update the second DRHD structure
+
+    @param[in, out] DrhdEnginePtr       - A pointer to DRHD structure
+**/
+{
+  UINT16                        Length;
+  UINTN                         DeviceScopeNum;
+  UINTN                         ValidDeviceScopeNum;
+  UINT16                        Data16;
+  UINT16                        HpetReg;
+  UINT16                        Index;
+  UINT8                         Bus;
+  UINT8                         Path[2] = { 0, 0 };
+  BOOLEAN                       NeedRemove;
+  EFI_ACPI_DRHD_ENGINE2_STRUCT  *DrhdEngine;
+
+  ///
+  /// Convert DrhdEnginePtr to EFI_ACPI_DRHD_ENGINE2_STRUCT Pointer
+  ///
+  DrhdEngine      = (EFI_ACPI_DRHD_ENGINE2_STRUCT *) DrhdEnginePtr;
+
+  Length          = DrhdEngine->Length;
+  DeviceScopeNum  = (DrhdEngine->Length - EFI_ACPI_DRHD_ENGINE_HEADER_LENGTH) / sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE);
+  Data16          = 0;
+  Bus             = 0;
+  HpetReg         = R_PCH_LPC_HPET0;
+  ValidDeviceScopeNum = 0;
+
+  for (Index = 0; Index < DeviceScopeNum; Index++) {
+    NeedRemove = FALSE;
+    /**
+      For HPET and APIC, update device scope if Interrupt remapping is supported. remove device scope
+      if interrupt remapping is not supported.
+      - Index = 0 - IOAPIC
+      - Index = 1 - HPET
+      For Serial IO devices, they do not appear in PCI space, use platform policy to determine existence, also remove if PCH not LP
+      - Index = 2 - I2C0
+      - Index = 3 - I2C1
+      - Index = 4 - SPI0
+      - Index = 5 - SPI1
+      - Index = 6 - UART0
+      - Index = 7 - UART1
+      - Index = 8 - SDIO
+    **/
+    if (mInterruptRemappingSupport) {
+      if (Index == 0) {
+        ///
+        /// Update source id for IoApic's device scope entry
+        ///
+        Data16  = PchLpcPciCfg16 (R_PCH_LPC_IOXAPIC);
+        Bus     = (UINT8) (Data16 >> 8);
+        if (Bus != 0x00) {
+          Path[0] = (UINT8) ((Data16 & 0xff) >> 3);
+          Path[1] = (UINT8) (Data16 & 0x7);
+        } else {
+          //
+          // BUGBUG: Here we just hardcode, because in this version, R_PCH_LPC_IOXAPIC is initialized AFTER Vtd run.  We can NOT get proper setting from PCH
+          // We can NOT get proper setting from PCH
+          /// @todo check if code still needed
+          //
+          DEBUG ((EFI_D_WARN, "BUGBUG: UpdateApicHpet use hardcode value - To be fixed!\n"));
+          Bus     = 0xF0;
+          Path[0] = 0x1F;
+          Path[1] = 0x0;
+        }
+        DrhdEngine->DeviceScope[Index].StartBusNumber = Bus;
+        //
+        // Update APIC ID
+        //
+        DrhdEngine->DeviceScope[Index].EnumId = GetIoApicID ();
+      }
+      if (Index == 1) {
+        ///
+        /// Update source id for HPET's device scope entry
+        ///
+        Data16  = PchLpcPciCfg16 (HpetReg);
+        Bus     = (UINT8) (Data16 >> 8);
+        Path[0] = (UINT8) ((Data16 & 0xFF) >> 3);
+        Path[1] = (UINT8) (Data16 & 0x7);
+        DrhdEngine->DeviceScope[Index].StartBusNumber = Bus;
+      }
+    } else {
+      if ((Index == 0) || (Index == 1)) {
+        NeedRemove = TRUE;
+      }
+    }
+    /*
+    Pch removed device from PCI space and it is visible by ACPI only, we use platform policy to check
+    if device is present. If Pch is 2 chip, remove all serialio devices.
+    */
+#ifdef SERIAL_IO_FLAG
+    if (mPchSeries == PchLp){
+      if (Index == 2){
+        if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoI2c0 == 0){
+          NeedRemove = TRUE;
+        }
+      }
+      if (Index == 3){
+        if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoI2c1 == 0){
+          NeedRemove = TRUE;
+        }
+      }
+      if (Index == 4){
+        if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoSpi0 == 0){
+          NeedRemove = TRUE;
+        }
+      }
+      if (Index == 5){
+        if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoSpi1 == 0){
+          NeedRemove = TRUE;
+        }
+      }
+      if (Index == 6){
+        if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoUart0 == 0){
+          NeedRemove = TRUE;
+        }
+      }
+      if (Index == 7){
+        if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoUart1 == 0){
+          NeedRemove = TRUE;
+        }
+      }
+      if (Index == 8){
+        if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoSdio == 0){
+          NeedRemove = TRUE;
+        }
+      }
+    } else {
+#endif
+      if ((Index >= 2) && (Index <= 8)){
+        NeedRemove = TRUE;
+      }
+#ifdef SERIAL_IO_FLAG
+    }
+#endif
+    CopyMem (
+      &DrhdEngine->DeviceScope[ValidDeviceScopeNum],
+      &DrhdEngine->DeviceScope[Index],
+      sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE)
+      );
+    if (NeedRemove) {
+      Length -= sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE);
+    } else {
+      ValidDeviceScopeNum++;
+    }
+  }
+  ///
+  /// If no devicescope is left, we set the structure length as 0x00
+  ///
+  if ((Length > EFI_ACPI_DRHD_ENGINE_HEADER_LENGTH) || (DrhdEngine->Flags == 0x01)) {
+    DrhdEngine->Length = Length;
+  } else {
+    DrhdEngine->Length = 0;
+  }
+}
+
+VOID
+UpdateRMRR (
+  IN OUT VOID *RmrrPtr
+  )
+/**
+    Update the RMRR structure
+
+    @param[in, out] RmrrPtr             - A pointer to RMRR structure
+**/
+{
+  UINT16                  Length;
+  UINT16                  DisableBit;
+  UINTN                   DeviceScopeNum;
+  UINTN                   ValidDeviceScopeNum;
+  UINTN                   Index;
+  BOOLEAN                 NeedRemove;
+  EFI_ACPI_RMRR_USB_STRUC *Rmrr;
+
+  ///
+  /// To make sure all devicescope can be checked,
+  /// we convert the RmrrPtr to EFI_ACPI_RMRR_USB_STRUC pointer
+  ///
+  Rmrr                = (EFI_ACPI_RMRR_USB_STRUC *) RmrrPtr;
+  Length              = Rmrr->Length;
+  ValidDeviceScopeNum = 0;
+  DeviceScopeNum      = (Rmrr->Length - EFI_ACPI_RMRR_HEADER_LENGTH) / sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE);
+  for (Index = 0; Index < DeviceScopeNum; Index++) {
+    ///
+    /// here we assume Device will exist on PCH if Device Number is greater than 0x0F
+    ///
+    DisableBit = GetFunDisableBit (
+                  Rmrr->DeviceScope[Index].PciPath[0],
+                  Rmrr->DeviceScope[Index].PciPath[1]
+                  );
+    NeedRemove = FALSE;
+    if ((DisableBit == 0xFF) ||
+        ((DisableBit == 0x80) &&
+        (MmPci32 (0, 0, Rmrr->DeviceScope[Index].PciPath[0], Rmrr->DeviceScope[Index].PciPath[1], 0x00) == 0xFFFFFFFF))
+        ){
+      NeedRemove = TRUE;
+    }
+    CopyMem (
+      &Rmrr->DeviceScope[ValidDeviceScopeNum],
+      &Rmrr->DeviceScope[Index],
+      sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE)
+      );
+    if (NeedRemove) {
+      Length -= sizeof (EFI_ACPI_DEV_SCOPE_STRUCTURE);
+    } else {
+      ValidDeviceScopeNum++;
+    }
+  }
+  ///
+  /// If No deviceScope is left, set length as 0x00
+  ///
+  if (Length > EFI_ACPI_RMRR_HEADER_LENGTH) {
+    Rmrr->Length = Length;
+  } else {
+    Rmrr->Length = 0;
+  }
+}
+
+VOID
+DmarTableUpdate (
+  IN OUT   EFI_ACPI_DESCRIPTION_HEADER       *TableHeader,
+  IN OUT   EFI_ACPI_TABLE_VERSION            *Version
+  )
+/**
+  Update the DMAR table
+
+  @param[in, out] TableHeader         - The table to be set
+  @param[in, out] Version             - Version to publish
+**/
+{
+  EFI_ACPI_DMAR_TABLE *DmarTable;
+  EFI_ACPI_DMAR_TABLE TempDmarTable;
+  UINTN               Offset;
+  UINTN               StructureLen;
+  UINT16              IgdMode;
+  UINT16              GttMode;
+  UINT32              IgdMemSize;
+  UINT32              GttMemSize;
+
+  IgdMemSize  = 0;
+  GttMemSize  = 0;
+  DmarTable = (EFI_ACPI_DMAR_TABLE *) TableHeader;
+
+  if (mInterruptRemappingSupport) {
+    DmarTable->Flags |= BIT0; ///< Set INTR_REMAP bit (BIT 0) if interrupt remapping is supported
+  }
+  ///
+  /// Find IGD memsize
+  ///
+  IgdMode = (McD0PciCfg16 (R_SA_GGC) & B_SA_GGC_GMS_MASK) >> 3;
+  if (IgdMode <= V_SA_GGC_GMS_512MB) {
+    IgdMemSize = IgdMode * 32 * (1024) * (1024);
+  } else {
+    IgdMemSize = 0;
+  }
+  ///
+  /// Find GTT mem size
+  ///
+  GttMode = (McD0PciCfg16 (R_SA_GGC) & B_SA_GGC_GGMS_MASK) >> 8;
+  if (GttMode <= V_SA_GGC_GGMS_2MB) {
+    GttMemSize = GttMode * (1024) * (1024);
+  } else {
+    GttMemSize = 0;
+  }
+
+  DmarTable->RmrrIgd.RmrBaseAddress   = (McD0PciCfg32 (R_SA_TOLUD) &~(0x01)) - IgdMemSize - GttMemSize;
+  DmarTable->RmrrIgd.RmrLimitAddress  = DmarTable->RmrrIgd.RmrBaseAddress + IgdMemSize + GttMemSize - 1;
+  DEBUG ((EFI_D_INFO, "RMRR Base  address IGD %016lX\n", DmarTable->RmrrIgd.RmrBaseAddress));
+  DEBUG ((EFI_D_INFO, "RMRR Limit address IGD %016lX\n", DmarTable->RmrrIgd.RmrLimitAddress));
+
+  DmarTable->RmrrUsb.RmrBaseAddress   = mDxePlatformSaPolicy->Vtd->RmrrUsbBaseAddress[0];
+  DmarTable->RmrrUsb.RmrLimitAddress  = mDxePlatformSaPolicy->Vtd->RmrrUsbBaseAddress[1];
+
+  ///
+  /// Convert to 4KB alignment.
+  ///
+  DmarTable->RmrrUsb.RmrBaseAddress &= ~0xFFF;
+  DmarTable->RmrrUsb.RmrLimitAddress &= ~0xFFF;
+  DmarTable->RmrrUsb.RmrLimitAddress += 0x1000 - 1;
+
+  DEBUG ((EFI_D_INFO, "RMRR Base  address USB %016lX\n", DmarTable->RmrrUsb.RmrBaseAddress));
+  DEBUG ((EFI_D_INFO, "RMRR Limit address USB %016lX\n", DmarTable->RmrrUsb.RmrLimitAddress));
+
+  ///
+  /// @todo check if this check is still needed.
+  ///
+  if (DmarTable->RmrrUsb.RmrBaseAddress == 0) {
+    DEBUG ((EFI_D_WARN, "BUGBUG:  RmrrUsb.RmrBaseAddress is 0 - To be fixed\n"));
+  }
+  ///
+  /// Update DRHD structures of DmarTable
+  ///
+  DmarTable->DrhdEngine1.RegisterBaseAddress = (McMmio32 (R_SA_MCHBAR_VTD1_OFFSET) &~1);
+  DmarTable->DrhdEngine2.RegisterBaseAddress = (McMmio32 (R_SA_MCHBAR_VTD2_OFFSET) &~1);
+
+  DEBUG ((EFI_D_INFO, "VTD base address1 %x\n", DmarTable->DrhdEngine1.RegisterBaseAddress));
+  DEBUG ((EFI_D_INFO, "VTD base address2 %x\n", DmarTable->DrhdEngine2.RegisterBaseAddress));
+  ///
+  /// copy DmarTable to TempDmarTable to be processed
+  ///
+  CopyMem (&TempDmarTable, DmarTable, sizeof (EFI_ACPI_DMAR_TABLE));
+
+  ///
+  /// Update DRHD structures of temp DMAR table
+  ///
+  UpdateDRHD (&TempDmarTable.DrhdEngine1);
+  UpdateDRHD2 (&TempDmarTable.DrhdEngine2);
+
+  ///
+  /// Update RMRR structures of temp DMAR table
+  ///
+  UpdateRMRR ((VOID *) &TempDmarTable.RmrrUsb);
+  UpdateRMRR ((VOID *) &TempDmarTable.RmrrIgd);
+
+  ///
+  /// Remove unused device scope or entire DRHD structures
+  ///
+  Offset = (UINTN) (&TempDmarTable.DrhdEngine1);
+  if (TempDmarTable.DrhdEngine1.Length != 0) {
+    Offset += TempDmarTable.DrhdEngine1.Length;
+  }
+  if (TempDmarTable.DrhdEngine2.Length != 0) {
+    StructureLen = TempDmarTable.DrhdEngine2.Length;
+    CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.DrhdEngine2, TempDmarTable.DrhdEngine2.Length);
+    Offset += StructureLen;
+  }
+  ///
+  /// Remove unused device scope or entire RMRR structures
+  ///
+  if (TempDmarTable.RmrrUsb.Length != 0) {
+    StructureLen = TempDmarTable.RmrrUsb.Length;
+    CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.RmrrUsb, TempDmarTable.RmrrUsb.Length);
+    Offset += StructureLen;
+  }
+  if (TempDmarTable.RmrrIgd.Length != 0) {
+    StructureLen = TempDmarTable.RmrrIgd.Length;
+    CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.RmrrIgd, TempDmarTable.RmrrIgd.Length);
+    Offset += StructureLen;
+  }
+#ifdef SERIAL_IO_FLAG
+  ///
+  /// Include necessary ANDD structures. If not PchLp, remove all ANDD.
+  ///
+  if (mPchSeries == PchLp){
+    if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoI2c0 != 0) {
+      StructureLen = TempDmarTable.AnddI2C0.Length;
+      CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.AnddI2C0, TempDmarTable.AnddI2C0.Length);
+      Offset += StructureLen;
+    }
+    if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoI2c1 != 0) {
+      StructureLen = TempDmarTable.AnddI2C1.Length;
+      CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.AnddI2C1, TempDmarTable.AnddI2C1.Length);
+      Offset += StructureLen;
+    }
+    if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoSpi0 != 0) {
+      StructureLen = TempDmarTable.AnddSpi0.Length;
+      CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.AnddSpi0, TempDmarTable.AnddSpi0.Length);
+      Offset += StructureLen;
+    }
+    if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoSpi1 != 0) {
+      StructureLen = TempDmarTable.AnddSpi1.Length;
+      CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.AnddSpi1, TempDmarTable.AnddSpi1.Length);
+      Offset += StructureLen;
+    }
+    if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoUart0 != 0) {
+      StructureLen = TempDmarTable.AnddUa00.Length;
+      CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.AnddUa00, TempDmarTable.AnddUa00.Length);
+      Offset += StructureLen;
+    }
+    if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoUart1 != 0) {
+      StructureLen = TempDmarTable.AnddUa01.Length;
+      CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.AnddUa01, TempDmarTable.AnddUa01.Length);
+      Offset += StructureLen;
+    }
+    if (mDxePlatformPchPolicy->DeviceEnabling->SerialIoSdio != 0) {
+      StructureLen = TempDmarTable.AnddSdhc.Length;
+      CopyMem ((VOID *) Offset, (VOID *) &TempDmarTable.AnddSdhc, TempDmarTable.AnddSdhc.Length);
+      Offset += StructureLen;
+    }
+  }
+#endif
+  Offset = Offset - (UINTN) &TempDmarTable;
+  TempDmarTable.Header.Checksum = (UINT8) (TempDmarTable.Header.Checksum + TempDmarTable.Header.Length - Offset);
+  TempDmarTable.Header.Length = (UINT32) Offset;
+  ///
+  /// Replace DMAR table with rebuilt table TempDmarTable
+  ///
+  CopyMem ((VOID *) DmarTable, (VOID *) &TempDmarTable, TempDmarTable.Header.Length);
+}
+
+EFI_STATUS
+WaForVc0RemappingEngine (
+  UINT64                      MchBar
+  )
+/**
+  Workaround for VC0 remapping engine
+
+  @param[in] MchBar             - MCHBAR address
+
+  @retval EFI_SUCCESS           - successed.
+**/
+{
+  UINT16              DeviceId;
+  UINT32              Vc0RemapEngineBase;
+  UINT32              Data32Or;
+  UINT32              Data32And;
+
+  DeviceId  = PchLpcPciCfg16(R_PCH_LPC_DEVICE_ID);
+  Vc0RemapEngineBase = Mmio32(MchBar, R_SA_MCHBAR_VTD2_OFFSET) & 0xFFFFFFFE;
+
+  ///
+  /// Disable VTD SuperPage policy when iGfx is enabled
+  ///
+  if (McD2PciCfg16(R_SA_IGD_VID) != 0xFFFF) {
+    Data32And = (UINT32)~(BIT25);
+    Data32Or  = 0;
+    Mmio32And(Vc0RemapEngineBase, 0xFF0, Data32And);
+    SCRIPT_MEM_READ_WRITE
+      (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (Vc0RemapEngineBase + 0xFF0),
+        &Data32Or,  ///< Data to be ORed
+        &Data32And  ///< Data to be ANDed
+      );
+  }
+
+  return EFI_SUCCESS;
+}
+
+VOID
+UpdateDmarExitPmAuth (
+  VOID
+  )
+/**
+  ExitPmAuth routine for update DMAR
+**/
+{
+  EFI_STATUS                      Status;
+  EFI_HANDLE                      *HandleBuffer;
+  UINTN                           NumberOfHandles;
+  EFI_FV_FILETYPE                 FileType;
+  UINT32                          FvStatus;
+  EFI_FV_FILE_ATTRIBUTES          Attributes;
+  UINTN                           Size;
+  UINTN                           i;
+  INTN                            Instance;
+  EFI_ACPI_TABLE_VERSION          Version;
+  EFI_ACPI_COMMON_HEADER          *CurrentTable;
+  UINTN                           AcpiTableHandle;
+  EFI_FIRMWARE_VOLUME_PROTOCOL    *FwVol;
+  EFI_ACPI_TABLE_PROTOCOL         *AcpiTable;
+  EFI_ACPI_DESCRIPTION_HEADER     *VtdAcpiTable;
+  STATIC BOOLEAN                  Triggered = FALSE;
+
+  if (Triggered) {
+    return;
+  }
+
+  Triggered     = TRUE;
+
+  FwVol         = NULL;
+  AcpiTable     = NULL;
+  VtdAcpiTable  = NULL;
+
+  ///
+  /// Locate PCH platform policy protocol and PCH series to support feature enabling/disabling
+  ///
+  Status = gBS->LocateProtocol (
+                  &gDxePchPlatformPolicyProtocolGuid,
+                  NULL,
+                  (VOID**) &mDxePlatformPchPolicy
+                  );
+  ASSERT_EFI_ERROR (Status);
+  mPchSeries  = GetPchSeries();
+
+  if ((!mDxePlatformSaPolicy->Vtd->VtdEnable) || (McD0PciCfg32 (R_SA_MC_CAPID0_A_OFFSET) & BIT23)) {
+      DEBUG ((EFI_D_INFO, "Vtd Disabled, skip DMAR Table install\n"));
+
+      return;
+  }
+
+  ///
+  /// Locate ACPI support protocol
+  ///
+  Status = gBS->LocateProtocol (&gEfiAcpiTableProtocolGuid, NULL, (VOID **) &AcpiTable);
+
+  ///
+  /// Locate protocol.
+  /// There is little chance we can't find an FV protocol
+  ///
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolumeProtocolGuid,
+                  NULL,
+                  &NumberOfHandles,
+                  &HandleBuffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Looking for FV with ACPI storage file
+  ///
+  for (i = 0; i < NumberOfHandles; i++) {
+    ///
+    /// Get the protocol on this handle
+    /// This should not fail because of LocateHandleBuffer
+    ///
+    Status = gBS->HandleProtocol (
+                    HandleBuffer[i],
+                    &gEfiFirmwareVolumeProtocolGuid,
+                    (VOID **) &FwVol
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    ///
+    /// See if it has the ACPI storage file
+    ///
+    Size      = 0;
+    FvStatus  = 0;
+    Status = FwVol->ReadFile (
+                      FwVol,
+                      &gSaAcpiTableStorageGuid,
+                      NULL,
+                      &Size,
+                      &FileType,
+                      &Attributes,
+                      &FvStatus
+                      );
+
+    ///
+    /// If we found it, then we are done
+    ///
+    if (Status == EFI_SUCCESS) {
+      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);
+  if (FwVol == NULL) {
+    return ;
+  }
+  ///
+  /// By default, a table belongs in all ACPI table versions published.
+  ///
+  Version = EFI_ACPI_TABLE_VERSION_1_0B | EFI_ACPI_TABLE_VERSION_2_0 | EFI_ACPI_TABLE_VERSION_3_0;
+
+  ///
+  /// Read tables from the storage file.
+  ///
+  Instance      = 0;
+  CurrentTable  = NULL;
+
+  while (Status == EFI_SUCCESS) {
+    Status = FwVol->ReadSection (
+                      FwVol,
+                      &gSaAcpiTableStorageGuid,
+                      EFI_SECTION_RAW,
+                      Instance,
+                      (VOID **) &CurrentTable,
+                      &Size,
+                      &FvStatus
+                      );
+
+    if (!EFI_ERROR (Status)) {
+      ///
+      /// Check the Signature ID to modify the table
+      ///
+      switch (((EFI_ACPI_DESCRIPTION_HEADER *) CurrentTable)->Signature) {
+
+      case EFI_ACPI_VTD_DMAR_TABLE_SIGNATURE:
+        VtdAcpiTable = (EFI_ACPI_DESCRIPTION_HEADER *) CurrentTable;
+        DmarTableUpdate (VtdAcpiTable, &Version);
+        break;
+
+      default:
+        break;
+      }
+      ///
+      /// Increment the instance
+      ///
+      Instance++;
+      CurrentTable = NULL;
+    }
+  }
+  ///
+  /// Update the VTD table in the ACPI tables.
+  ///
+  AcpiTableHandle = 0;
+  if (VtdAcpiTable != NULL) {
+    Status = AcpiTable->InstallAcpiTable (
+                          AcpiTable,
+                          VtdAcpiTable,
+                          VtdAcpiTable->Length,
+                          &AcpiTableHandle
+                          );
+  }
+}
+
+EFI_STATUS
+VtdInit (
+  IN  DXE_PLATFORM_SA_POLICY_PROTOCOL    *DxePlatformSaPolicy
+  )
+/**
+@brief
+  Locate the VT-d ACPI tables data file and read ACPI SSDT tables.
+  Publish the appropriate SSDT based on current configuration and capabilities.
+
+  @param[in] DxePlatformSaPolicy  -  SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS     - Vtd initialization complete
+  @exception EFI_UNSUPPORTED - Vtd is not enabled by policy
+**/
+{
+  EFI_STATUS   Status;
+  UINTN        i;
+  UINT64       MchBar;
+  UINT32       Data32Or;
+  UINT32       Data32And;
+  UINT32       VtdBase;
+  UINT32       VtBarReg [SA_VTD_ENGINE_NUMBER];
+  CPU_FAMILY   CpuFamilyId;
+  CPU_STEPPING CpuStepping;
+
+  mInterruptRemappingSupport  = FALSE;
+  mPchRootComplexBar          = MmPci32 (0, 0, 31, 0, 0xF0) &~BIT0;
+  MchBar                      = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+
+  VtBarReg[0]                 = R_SA_MCHBAR_VTD1_OFFSET;
+  VtBarReg[1]                 = R_SA_MCHBAR_VTD2_OFFSET;
+  mDxePlatformSaPolicy        = DxePlatformSaPolicy;
+
+  ///
+  /// Check SA supports VTD and VTD is enabled in setup menu
+  ///
+  if ((!mDxePlatformSaPolicy->Vtd->VtdEnable) || (McD0PciCfg32 (R_SA_MC_CAPID0_A_OFFSET) & BIT23)) {
+    DEBUG ((EFI_D_WARN, "VTd disabled or no capability!\n"));
+    return EFI_UNSUPPORTED;
+  }
+  DEBUG ((EFI_D_INFO, "VTd enabled\n"));
+
+  ///
+  /// 14.1 Program Remap Engine Base Address
+  /// Configure VTD1 BAR
+  ///
+  i = 0;
+
+  ///
+  /// Skip GFXVTBAR if IGD is disabled
+  ///
+  if (McD2PciCfg16 (R_SA_IGD_VID) != 0xFFFF) {
+    Data32Or = mDxePlatformSaPolicy->Vtd->BaseAddress[i];
+    Data32Or |= 0x1;
+    Mmio32 (MchBar, R_SA_MCHBAR_VTD1_OFFSET) = Data32Or;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (MchBar + R_SA_MCHBAR_VTD1_OFFSET),
+      1,
+      &Data32Or
+      );
+    i++;
+  }
+
+  ///
+  /// Configure VTD2 BAR
+  ///
+  Data32Or = mDxePlatformSaPolicy->Vtd->BaseAddress[i];
+  Data32Or |= 0x1;
+  Mmio32 (MchBar, R_SA_MCHBAR_VTD2_OFFSET) = Data32Or;
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (MchBar + R_SA_MCHBAR_VTD2_OFFSET),
+    1,
+    &Data32Or
+    );
+
+  ///
+  /// Workaround for VC0 remapping engine
+  ///
+  Status = WaForVc0RemappingEngine (MchBar);
+  ASSERT_EFI_ERROR (Status);
+
+  for (i = 0; i < SA_VTD_ENGINE_NUMBER; i++) {
+    VtdBase = Mmio32 (MchBar, VtBarReg[i]) & 0xfffffffe;
+
+    ///
+    /// skip if the VT bar is 0
+    ///
+    if (VtdBase == 0) {
+      continue;
+    }
+
+    CpuFamilyId = GetCpuFamily();
+    CpuStepping = GetCpuStepping();
+
+    if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuHswUlt) || (CpuFamilyId == EnumCpuCrw)
+        ) {
+      Data32And = (UINT32) ~(BIT15+BIT16+BIT17+BIT18+BIT19); ///< mask out 19:15
+      Data32Or  = BIT15;
+      if (i == 1) {
+        Mmio32AndThenOr (VtdBase, 0xF04, Data32And, Data32Or);
+        SCRIPT_MEM_READ_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (VtdBase + 0xF04),
+          &Data32Or,  ///< Data to be ORed
+          &Data32And  ///< Data to be ANDed
+        );
+      }
+    }
+    ///
+    /// 14.2 Set the remap engine policy bits
+    ///
+    Data32And = 0x0; ///< mask out all bits
+    Data32Or  = 0;
+
+
+    if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuHswUlt) || (CpuFamilyId == EnumCpuCrw)) {
+      if (i == 0) {
+        Data32Or |= 0x02100000;
+      }
+      if (i == 1) {
+        if (McD2PciCfg16 (R_SA_IGD_VID) != 0xFFFF) {
+          Data32Or |= 0x000A5003;
+        } else {
+          Data32Or |= 0x020A5003;
+        }
+      }
+    }
+
+    ///
+    /// Set lock bit
+    ///
+    Data32Or |= BIT31;
+
+
+    Mmio32AndThenOr (VtdBase, 0xFF0, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (VtdBase + 0xFF0),
+      &Data32Or,  ///< Data to be ORed
+      &Data32And  ///< Data to be ANDed
+      );
+
+    ///
+    /// Check IR status
+    ///
+    if ((Mmio32 (VtdBase, VTD_ECAP_REG) & IR) && !(mInterruptRemappingSupport)) {
+      mInterruptRemappingSupport = TRUE;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/VTd.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/VTd.h
new file mode 100644
index 0000000..2d9daa4
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/VTd.h
@@ -0,0 +1,70 @@
+/** @file
+  This code provides a initialization of intel VT-d (Virtualization Technology for Directed I/O).
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _VT_D_H_
+#define _VT_D_H_
+
+///
+/// Include files
+///
+#include "DmaRemappingTable.h"
+#include "SaAccess.h"
+#include "PchAccess.h"
+
+#include EFI_PROTOCOL_DEPENDENCY (AcpiTable)
+#include EFI_PROTOCOL_DEFINITION (SaPlatformPolicy)
+#include EFI_PROTOCOL_DEFINITION (PchPlatformPolicy)
+
+///
+/// SA ACPI table data storage file
+///
+#include EFI_GUID_DEFINITION (SaAcpiTableStorage)
+
+#define VTD_ECAP_REG  0x10
+#define IR            BIT3
+
+#define VTD_GCMD_REG  0x18
+#define QIE           BIT26
+#define IRE           BIT25
+
+EFI_STATUS
+VtdInit (
+  IN  DXE_PLATFORM_SA_POLICY_PROTOCOL    *DxePlatformSaPolicy
+  )
+/**
+  Locate the VT-d ACPI tables data file and read ACPI SSDT tables.
+  Publish the appropriate SSDT based on current configuration and capabilities.
+
+  @param[in] DxePlatformSaPolicy            SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS - Vtd initialization complete
+  @retval Other       - No Vtd function initiated
+**/
+;
+
+VOID
+UpdateDmarExitPmAuth (
+  VOID
+  )
+/**
+  ExitPmAuth routine for update DMAR
+**/
+;
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/graphicsinit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/graphicsinit.c
new file mode 100644
index 0000000..2b1af13
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/graphicsinit.c
@@ -0,0 +1,1094 @@
+/** @file
+  DXE driver for Initializing SystemAgent Graphics initialization.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "GraphicsInit.h"
+#include EFI_PROTOCOL_DEFINITION (LegacyBios)
+#include EFI_PROTOCOL_DEFINITION (GopComponentName2)
+#ifndef AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+#if (defined(IntelSaGopDriver_SUPPORT) && (IntelSaGopDriver_SUPPORT == 1))
+#include "Include\Protocol\IntelSaGopDriver.h"
+#endif
+#endif
+UINT64                          GTTMMADR;
+UINTN                           MCHBAR_BASE;
+UINT8                           rP1GraphicsFreq;
+EFI_EVENT                       mExitPmAuthEvent;
+
+DXE_PLATFORM_SA_POLICY_PROTOCOL *mDxePlatformSaPolicy;
+GOP_COMPONENT_NAME2_PROTOCOL *GopComponentName2Protocol = NULL;
+
+///
+/// RC6 Settings
+///
+BOOT_SCRIPT_REGISTER_SETTING  gSaGtRC6Registers[] = {
+  0x0,
+  0xA090,
+  0xFFFFFFFF,
+  0x0,
+  ///
+  /// RC1e - RC6/6p - RC6pp Wake Rate Limits
+  ///
+  0x0,
+  0xA098,
+  0xFFFFFFFF,
+  0x3E80000,
+  0x0,
+  0xA09C,
+  0xFFFFFFFF,
+  0x00280000,
+  0x0,
+  0xA0A8,
+  0xFFFFFFFF,
+  0x1E848,
+  0x0,
+  0xA0AC,
+  0xFFFFFFFF,
+  0x19,
+  ///
+  /// Render/Video/Blitter Idle Max Count
+  ///
+  0x0,
+  0x2054,
+  0x0,
+  0xA,
+  0x0,
+  0x12054,
+  0x0,
+  0xA,
+  0x0,
+  0x22054,
+  0x0,
+  0xA,
+  0x0,
+  0x1a054,
+  0x0,
+  0xA,
+  ///
+  /// RC Sleep / RCx Thresholds
+  ///
+  0x0,
+  0xA0B0,
+  0xFFFFFFFF,
+  0,
+  0x0,
+  0xA0B4,
+  0xFFFFFFFF,
+  0x3E8,
+  0x0,
+  0xA0B8,
+  0xFFFFFFFF,
+  0xC350,
+  ///
+  /// RP Settings
+  ///
+  0x0,
+  0xA010,
+  0xFFFFFFFF,
+  0xF4240,
+  0x0,
+  0xA014,
+  0xFFFFFFFF,
+  0x12060000,
+  0x0,
+  0xA02C,
+  0xFFFFFFFF,
+  0x0000E808,
+  0x0,
+  0xA030,
+  0xFFFFFFFF,
+  0x0003BD08,
+  0x0,
+  0xA068,
+  0xFFFFFFFF,
+  0x000101D0,
+  0x0,
+  0xA06C,
+  0xFFFFFFFF,
+  0x00055730,
+  0x0,
+  0xA070,
+  0xFFFFFFFF,
+  0xA
+};
+
+///
+/// PM Lock Settings
+///
+BOOT_SCRIPT_REGISTER_SETTING  gSaGtPmLockBits[] = {
+  0x0,
+  0xA248,
+  0xFFFFFFFF,
+  BIT31,
+  0x0,
+  0xA004,
+  0xFFFFFFFF,
+  BIT4,
+  0x0,
+  0xA080,
+  0xFFFFFFFF,
+  BIT2,
+  0x0,
+  0xA180,
+  0xFFFFFFFF,
+  BIT31
+};
+
+EFI_STATUS
+PmInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Initialize GT PowerManagement of SystemAgent.
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS       - GT Power Management initialization complete
+**/
+{
+  UINT8             i;
+  UINT32            RegOffset;
+  UINT32            Data32;
+  UINT32            Data32And;
+  UINT32            Data32Or;
+  UINT32            Data32Mask;
+  UINT32            Result;
+  CPU_STEPPING      CpuSteppingId;
+  CPU_FAMILY        CpuFamilyId;
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+      ///
+      /// Multi Threaded Force Wake
+      ///
+      RegOffset                     = 0xA180;
+      Data32                        = BIT5;
+      Mmio32 (GTTMMADR, RegOffset)  = Data32;
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + RegOffset),
+        1,
+        &Data32
+        );
+
+      RegOffset                     = 0xA188;
+      Data32                        = 0x00010001;
+      Mmio32 (GTTMMADR, RegOffset)  = Data32;
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + RegOffset),
+        1,
+        &Data32
+        );
+      ///
+      /// Force Wake Acknowledge Bit
+      ///
+      RegOffset                     = 0x130044;
+      Data32Mask                    = BIT0;
+      Result                        = 1;
+      PollGtReady (GTTMMADR, RegOffset, Data32Mask, Result);
+      SCRIPT_MEM_POLL (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + RegOffset),
+        &Data32Mask,
+        &Result,
+        50,
+        200000
+        );
+
+    ///
+    /// Enable counters except Power Meter
+    ///
+    Data32And = 0xFFFFFFFF;
+    Data32Or  = 0x16;
+    Mmio32AndThenOr (GTTMMADR, 0xA248, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0xA248),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+
+      ///
+      /// GFXPAUSE Settings
+      ///
+      Data32 = 0x70020;
+
+
+      if (((CpuFamilyId == EnumCpuHsw)    && (CpuSteppingId < EnumHswC0)   ) ||
+          ((CpuFamilyId == EnumCpuHswUlt) && (CpuSteppingId < EnumHswUltC0)) ||
+          ((CpuFamilyId == EnumCpuCrw)    && (CpuSteppingId < EnumCrwC0)   )) {
+        Data32 |= 0xFFFF;
+      }
+
+      Mmio32 (GTTMMADR, 0xA000) = Data32;
+
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + 0xA000),
+        1,
+        &Data32
+        );
+
+
+    ///
+    /// ECO Settings
+    /// BIT28 = 1 GFX will be blocked from accessing memory (go=0) during CPD enter
+    /// BIT26 = 1 indicates to PCU that we are doing a fifo block due to RC6 and not CPD
+    /// BIT[24:22] = 100 RC6 Control
+    ///
+    RegOffset = 0xA180;
+    Data32And = 0xFFFFFFFF;
+    Data32Or  = BIT28 + BIT26;
+    if (DxePlatformSaPolicy->IgdConfig->RenderStandby) {
+      Data32And = (UINT32)~(BIT23 + BIT22);
+      Data32Or |= BIT24;
+    }
+
+    ///
+    /// Force CPD Non-IA. for steppings less than C0 for HSW/CRW
+    ///
+    if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId < EnumHswC0)) ||
+        ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId < EnumCrwC0))) {
+        Data32Or |= BIT30;
+    }
+    ///
+    /// Force CPD Block memory bits. for stepping less than C0 for HSW/ULT/CRW
+    ///
+    if (((CpuFamilyId == EnumCpuHsw)    && (CpuSteppingId < EnumHswC0)   ) ||
+        ((CpuFamilyId == EnumCpuHswUlt) && (CpuSteppingId < EnumHswUltC0)) ||
+        ((CpuFamilyId == EnumCpuCrw)    && (CpuSteppingId < EnumCrwC0)   )) {
+        Data32And &= (UINT32)~(BIT28);
+        Data32Or  &= (UINT32)~(BIT28);
+    }
+
+    Mmio32AndThenOr (GTTMMADR, RegOffset, Data32And, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + RegOffset),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+
+    ///
+    /// Clock Gating Settings
+    ///
+    Data32 = 0x3FD;
+    Mmio32 (GTTMMADR, 0x9424) = Data32;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x9424),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Enable Unit Level Clock Gates
+    ///
+    Data32 = 0x80;
+    Mmio32 (GTTMMADR, 0x9400) = Data32;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x9400),
+      1,
+      &Data32
+      );
+
+    Data32 = 0x40401000;
+    Mmio32 (GTTMMADR, 0x9404) = Data32;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x9404),
+      1,
+      &Data32
+      );
+
+    Data32 = 0;
+    Mmio32 (GTTMMADR, 0x9408) = Data32;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x9408),
+      1,
+      &Data32
+      );
+
+    Data32 = 0x02000001;
+    Mmio32 (GTTMMADR, 0x940c) = Data32;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x940c),
+      1,
+      &Data32
+      );
+
+    Data32 = 0x08000000;
+    Mmio32 (GTTMMADR, 0xA008) = Data32;
+
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0xA008),
+      1,
+      &Data32
+      );
+    ///
+    /// RC6 Settings
+    ///
+    for (i = 0; i < sizeof (gSaGtRC6Registers) / sizeof (BOOT_SCRIPT_REGISTER_SETTING); ++i) {
+      RegOffset                     = gSaGtRC6Registers[i].Offset;
+      Data32And                     = gSaGtRC6Registers[i].AndMask;
+      Data32Or                      = gSaGtRC6Registers[i].OrMask;
+
+      Mmio32AndThenOr (GTTMMADR, RegOffset, Data32And, Data32Or);
+
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + RegOffset),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+    }
+
+    ///
+    /// RP Control
+    ///
+    Data32                    = 0xB92;
+    Mmio32 (GTTMMADR, 0xA024) = Data32;
+
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0xA024),
+      1,
+      &Data32
+      );
+
+    ///
+    /// HW RC6 Control Settings
+    ///
+    Data32 = 0;
+
+    if (DxePlatformSaPolicy->IgdConfig->RenderStandby) {
+      Data32 = 0x88040000;
+    }
+
+    Mmio32 (GTTMMADR, 0xA090) = Data32;
+
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0xA090),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Video frequency request
+    ///
+    Data32  = 0x08000000;
+    Mmio32 (GTTMMADR, 0xA00C) = Data32;
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0xA00C),
+      1,
+      &Data32
+      );
+
+    ///
+    /// RC6 Settings
+    ///
+    ///
+    /// Wait for Mailbox ready
+    ///
+    Data32Mask  = BIT31;
+    Result      = 0;
+
+    PollGtReady (GTTMMADR, 0x138124, Data32Mask, Result);
+
+    SCRIPT_MEM_POLL (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x138124),
+      &Data32Mask,
+      &Result,
+      50,
+      200000
+      );
+
+    ///
+    /// Mailbox Data  - RC6 VIDS
+    ///
+    Data32 = 0x0;
+    Mmio32 (GTTMMADR, 0x138128) = Data32;
+
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x138128),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Mailbox Command
+    ///
+    Data32                      = 0x80000004;
+    Mmio32 (GTTMMADR, 0x138124) = Data32;
+
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x138124),
+      1,
+      &Data32
+      );
+
+    ///
+    /// Wait for Mailbox ready
+    ///
+    Data32Mask  = BIT31;
+    Result      = 0;
+
+    PollGtReady (GTTMMADR, 0x138124, Data32Mask, Result);
+
+    SCRIPT_MEM_POLL (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x138124),
+      &Data32Mask,
+      &Result,
+      50,
+      200000
+      );
+
+    ///
+    /// Enable PM Interrupts
+    ///
+    Data32                      = 0x3000076;
+    Mmio32 (GTTMMADR, 0x4402C)  = Data32;
+
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x4402C),
+      1,
+      &Data32
+      );
+
+    ///
+    /// RC6 setting
+    ///
+    if (mDxePlatformSaPolicy->IgdConfig->RenderStandby) {
+      ///
+      /// Software RC state - RC6
+      ///
+      Data32                    = 0x40000;
+      Mmio32 (GTTMMADR, 0xA094) = Data32;
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint8,
+        (UINTN) (GTTMMADR + 0xA094),
+        1,
+        &Data32
+        );
+    }
+
+      ///
+      /// PM Lock Settings
+      ///
+      for (i = 0; i < sizeof (gSaGtPmLockBits) / sizeof (BOOT_SCRIPT_REGISTER_SETTING); ++i) {
+        RegOffset = gSaGtPmLockBits[i].Offset;
+        Data32And = gSaGtPmLockBits[i].AndMask;
+        Data32Or  = gSaGtPmLockBits[i].OrMask;
+
+        Mmio32AndThenOr (GTTMMADR, RegOffset, Data32And, Data32Or);
+
+        SCRIPT_MEM_READ_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (GTTMMADR + RegOffset),
+          &Data32Or,  /// Data to be ORed
+          &Data32And  /// Data to be ANDed
+          );
+      }
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+PavpInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Initialize PAVP feature of SystemAgent.
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS - PAVP initialization complete
+**/
+{
+
+  UINT32  DwordData;
+  UINT32  PcmBase;
+  CPU_STEPPING      CpuSteppingId;
+  CPU_FAMILY        CpuFamilyId;
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+    McD0PciCfg32And (R_SA_PAVPC, ~(B_SA_PAVPC_HVYMODSEL_MASK | B_SA_PAVPC_PCMBASE_MASK | B_SA_PAVPC_PAVPE_MASK | B_SA_PAVPC_PCME_MASK));
+    McD0PciCfg16Or (R_SA_PAVPC, B_SA_PAVPC_PCME_MASK | B_SA_PAVPC_PAVPE_MASK);
+    PcmBase = ((UINT32) RShiftU64 ((McD0PciCfg32 (R_SA_TOLUD)), 20)) - PAVP_PCM_SIZE_1_MB;
+    McD0PciCfg32Or (R_SA_PAVPC, (UINT32) LShiftU64 (PcmBase, 20));
+
+    if ((CpuFamilyId == EnumCpuHsw) ||
+        (CpuFamilyId == EnumCpuHswUlt) ||
+        (CpuFamilyId == EnumCpuCrw)
+    ){
+      McD0PciCfg32Or (R_SA_PAVPC, (BIT4));
+    }
+
+  DwordData = McD0PciCfg32 (R_SA_PAVPC);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (MmPciAddress (0,
+            0,
+            0,
+            0,
+            R_SA_PAVPC)),
+    1,
+    &DwordData
+    );
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+PostPmInitExitPmAuth (
+  VOID
+  )
+/**
+    Do Post GT PM Init Steps after VBIOS Initialization.
+
+  @retval EFI_SUCCESS          Succeed.
+**/
+{
+  UINT32                    RegOffset;
+  UINT32                    Data32;
+  EFI_STATUS                Status;
+  UINT32                    Data32Mask;
+  UINT32                    Result;
+  UINT16                    Data16;
+  CHAR16                    *DriverVersion;
+  UINTN                     Index;
+  EFI_LEGACY_BIOS_PROTOCOL  *LegacyBios = NULL;
+
+  ///
+  /// Get the platform setup policy.
+  ///
+  Status = gBS->LocateProtocol (&gDxePlatformSaPolicyGuid, NULL, (VOID **) &mDxePlatformSaPolicy);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// only 32bit read/write is legal for device 0:2:0
+  ///
+  GTTMMADR  = (UINT64) (McD2PciCfg32 (R_SA_IGD_GTTMMADR));
+  GTTMMADR  = LShiftU64 ((UINT64) McD2PciCfg32 (R_SA_IGD_GTTMMADR + 4), 32) | (GTTMMADR);
+
+  ///
+  /// Save the current GTMMADR value into S3 resume script before other S3 resume items in this function.
+  /// GTTMMADR may have been modified by the PCI enumeration code at this point,
+  /// but not saved into the S3 resume script yet.
+  ///
+  ///
+  /// only 32bit read/write is legal for device 0:2:0
+  ///
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (MmPciAddress (0,
+            0,
+            2,
+            0,
+            R_SA_IGD_GTTMMADR)),
+    1,
+    &GTTMMADR
+    );
+
+  ///
+  /// only 32bit read/write is legal for device 0:2:0
+  ///
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (MmPciAddress (0,
+            0,
+            2,
+            0,
+            R_SA_IGD_GTTMMADR + 4)),
+    1,
+    (&(UINT32) GTTMMADR) + 1
+    );
+
+  GTTMMADR = GTTMMADR &~(BIT2 | BIT1);
+
+  ///
+  /// Enable Bus Master, I/O and Memory access on 0:2:0
+  ///
+  McD2PciCfg16Or (R_SA_IGD_CMD, (BIT2 | BIT1));
+  Data16 = McD2PciCfg16(R_SA_IGD_CMD);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint16,
+    (UINTN) (MmPciAddress (0, 0, 2, 0, R_SA_IGD_CMD)),
+    1,
+    &Data16
+    );
+
+      ///
+      /// Deassert Force Wake
+      RegOffset                     = 0xA188;
+      Data32                        = 0x00010000;
+      Mmio32 (GTTMMADR, RegOffset)  = Data32;
+
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + RegOffset),
+        1,
+        &Data32
+        );
+
+      RegOffset                     = 0x130044;
+      Data32Mask                    = BIT0;
+      Result                        = 0;
+      PollGtReady (GTTMMADR, RegOffset, Data32Mask, Result);
+
+      SCRIPT_MEM_POLL (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + RegOffset),
+        &Data32Mask,
+        &Result,
+        50,
+        200000
+        );
+
+      RegOffset                     = 0xA188;
+      Data32                        = 0x00000001;
+      Mmio32 (GTTMMADR, RegOffset)  = Data32;
+      SCRIPT_MEM_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + RegOffset),
+        1,
+        &Data32
+        );
+
+
+  Status = gBS->LocateProtocol (
+                  &gEfiLegacyBiosProtocolGuid,
+                  NULL,
+                  (VOID **) &LegacyBios
+                  );
+
+
+#ifndef AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+#if (defined(IntelSaGopDriver_SUPPORT) && (IntelSaGopDriver_SUPPORT == 1))
+{
+  EFI_PHYSICAL_ADDRESS          VbtAddress;
+  UINT32                        VbtSize;
+  PLATFORM_GOP_POLICY_PROTOCOL  *PlatformGOPPolicy;
+  EFI_GUID PlatformGOPPolicyGuid = EFI_PLATFORM_GOP_POLICY_PROTOCOL_GUID;
+  EFI_STATUS                    Status2 = EFI_SUCCESS;
+  //
+  // Locate Platform GOP policy protocol
+  //
+  Status = gBS->LocateProtocol (&PlatformGOPPolicyGuid, NULL, &PlatformGOPPolicy);
+  if (!EFI_ERROR(Status)) {
+    Status2 = PlatformGOPPolicy->GetVbtData(&VbtAddress, &VbtSize);
+    if (!EFI_ERROR(Status2)) LegacyBios = NULL;
+  }
+}
+#endif
+#endif // AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+
+  if (!LegacyBios) {
+    Status = gBS->LocateProtocol (&gGopComponentName2ProtocolGuid, NULL, (VOID **)&GopComponentName2Protocol);
+    if (!EFI_ERROR (Status)) {
+      Status = GopComponentName2Protocol->GetDriverVersion (
+                                    GopComponentName2Protocol,
+                                    "en-US",
+                                    &DriverVersion
+                                    );
+      if (!EFI_ERROR (Status)) {
+        for (Index = 0; (DriverVersion[Index] != '\0'); Index++) {
+        }
+        Index = (Index+1)*2;
+        CopyMem(mDxePlatformSaPolicy->IgdConfig->GopVersion, DriverVersion, Index);
+      }
+    }
+  }
+
+  ///
+  /// Return final status
+  ///
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+GraphicsInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+Initialize GT Post Routines.
+
+  @param[in] ImageHandle         -     Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy -     SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS   - GT POST initialization complete
+  @retval EFI_NOT_FOUND - Dxe System Table not found.
+**/
+{
+  EFI_PHYSICAL_ADDRESS  MemBaseAddress;
+  UINT32                LoGTBaseAddress;
+  UINT32                HiGTBaseAddress;
+  UINT32                Data32And;
+  UINT32                Data32Or;
+  EFI_STATUS            Status;
+  UINTN                 DwordData;
+  UINT32                Data32Mask;
+  UINT32                Result;
+  UINT16                Data16;
+  CPU_FAMILY            CpuFamilyId;
+  UINT8                 CpuSteppingId;
+  UINT32                Data32;
+  UINT16                IsUlx;
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  GTTMMADR    = 0;
+  Status      = EFI_SUCCESS;
+  MCHBAR_BASE = McD0PciCfg64 (0x48) &~BIT0;
+
+  ///
+  /// Read the RP1 Graphics Frequency
+  ///
+  rP1GraphicsFreq = (UINT8) ((Mmio32 (MCHBAR_BASE, 0x5998) >> 8) & 0xFF);
+
+  ///
+  /// If device 0:2:0 (Internal Graphics Device, or GT) is enabled, then Program GTTMMADR,
+  ///
+  if (McD2PciCfg16 (R_SA_IGD_VID) != 0xFFFF) {
+    gDS     = NULL;
+    Status  = EfiGetSystemConfigurationTable (&gEfiDxeServicesTableGuid, (VOID **) &gDS);
+    ASSERT_EFI_ERROR (Status);
+    ASSERT(gDS != NULL);
+    if (gDS == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    ///
+    /// Means Allocate 4MB for GTTMADDR
+    ///
+    MemBaseAddress = 0x0ffffffffffffffff;
+
+    Status = gDS->AllocateMemorySpace (
+                    EfiGcdAllocateAnySearchBottomUp,
+                    EfiGcdMemoryTypeMemoryMappedIo,
+                    GTT_MEM_ALIGN,
+                    GTTMMADR_SIZE_4MB,
+                    &MemBaseAddress,
+                    ImageHandle,
+                    NULL
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    ///
+    /// Program GT PM Settings if GTTMMADR allocation is Successful
+    ///
+    GTTMMADR                          = (UINTN) MemBaseAddress;
+
+    LoGTBaseAddress                   = (UINT32) (MemBaseAddress & 0xFFFFFFFF);
+    HiGTBaseAddress                   = (UINT32) RShiftU64 ((MemBaseAddress & 0xFFFFFFFF00000000), 32);
+    McD2PciCfg32 (R_SA_IGD_GTTMMADR)     = LoGTBaseAddress | BIT2;
+    McD2PciCfg32 (R_SA_IGD_GTTMMADR + 4) = HiGTBaseAddress;
+
+    DwordData                         = McD2PciCfg32 (R_SA_IGD_GTTMMADR);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (MmPciAddress (0,
+              0,
+              2,
+              0,
+              R_SA_IGD_GTTMMADR)),
+      1,
+      &DwordData
+      );
+
+    DwordData = McD2PciCfg32 (R_SA_IGD_GTTMMADR + 4);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (MmPciAddress (0,
+              0,
+              2,
+              0,
+              R_SA_IGD_GTTMMADR + 4)),
+      1,
+      &DwordData
+      );
+
+    ///
+    /// Enable Bus Master, I/O and Memory access on 0:2:0
+    ///
+    McD2PciCfg16Or (R_SA_IGD_CMD, (BIT2 | BIT1));
+    Data16 = McD2PciCfg16 (R_SA_IGD_CMD);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint16,
+      (UINTN) (MmPciAddress (0, 0, 2, 0, R_SA_IGD_CMD)),
+      1,
+      &Data16
+      );
+
+    ///
+    /// PAVP Initialization
+    ///
+      DEBUG ((EFI_D_INFO, "Initializing PAVP\n"));
+      PavpInit (ImageHandle, DxePlatformSaPolicy);
+    ///
+    /// PmInit Initialization
+    ///
+    DEBUG ((EFI_D_INFO, "Initializing GT PowerManagement\n"));
+    PmInit (ImageHandle, DxePlatformSaPolicy);
+
+    ///
+    /// Enable PowerWell for DP and Audio: set Dev2 mmio 45400 bit 31 and poll bit30 (1 means enabled)
+    ///
+    Data32And = 0xFFFFFFFF;
+    Data32Or  = (UINT32) (BIT31);
+    Mmio32Or (GTTMMADR, 0x45400, Data32Or);
+    SCRIPT_MEM_READ_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x45400),
+      &Data32Or,  /// Data to be ORed
+      &Data32And  /// Data to be ANDed
+      );
+
+    Data32Mask  = BIT30;
+    Result      = BIT30;
+    PollGtReady (GTTMMADR, 0x45400, Data32Mask, Result);
+    SCRIPT_MEM_POLL (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (GTTMMADR + 0x45400),
+      &Data32Mask,
+      &Result,
+      50,
+      200000
+      );
+
+    if (DxePlatformSaPolicy->Revision >=4) {
+      IsUlx = 0;
+      Data16 = McD2PciCfg16 (0x2);
+      if ((Data16 == 0xA0E) || (Data16 == 0xA1E)) {
+        IsUlx = 1;
+      }
+      ///
+      /// BIOS to get supported CD frequency
+      ///
+      if (((MmioRead32 (GTTMMADR + 0x42014) & 0x1000000) != 0) || (IsUlx == 1) || (CpuFamilyId == EnumCpuHswUlt)) {
+        ///fixed frequency
+        mDxePlatformSaPolicy->IgdConfig->CdClkVar = 0;
+      } else {
+        ///choice of varying frequency
+        mDxePlatformSaPolicy->IgdConfig->CdClkVar = 2;
+      }
+
+      if (IsUlx == 1) {
+        Data32And = 0xF7FFFFFF;
+        Data32Or  = 0x4000000;
+      } else {
+        if ((mDxePlatformSaPolicy->IgdConfig->CdClk) != 1) {
+          if ((mDxePlatformSaPolicy->IgdConfig->CdClkVar != 0)) {
+            ///540 or 337.5Mhz
+            Data32And = 0xF7FFFFFF;
+            Data32Or  = 0x4000000;
+          }
+        } else {
+          ///450 Mhz
+          Data32And = 0xF3FFFFFF;
+          Data32Or  = 0;
+        }
+      }
+      Mmio32AndThenOr (GTTMMADR, 0x130040, Data32And, Data32Or);
+      SCRIPT_MEM_READ_WRITE (
+        EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+        EfiBootScriptWidthUint32,
+        (UINTN) (GTTMMADR + 0x130040),
+        &Data32Or,  /// Data to be ORed
+        &Data32And  /// Data to be ANDed
+        );
+
+      if ((CpuFamilyId == EnumCpuHswUlt)||(IsUlx == 1)) {
+        Data32 = 1;
+        if (((mDxePlatformSaPolicy->IgdConfig->CdClk) == 1) && (IsUlx == 0)) {
+          Data32 = 0;
+        }
+
+        Mmio32 (GTTMMADR, 0x138128) = Data32;
+        SCRIPT_MEM_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (GTTMMADR + 0x138128),
+          1,
+          &Data32
+          );
+
+        Data32                      = 0x0;
+        Mmio32 (GTTMMADR, 0x13812c) = Data32;
+        SCRIPT_MEM_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (GTTMMADR + 0x13812c),
+          1,
+          &Data32
+          );
+
+        Data32                      = 0x80000017;
+        Mmio32 (GTTMMADR, 0x138124) = Data32;
+        SCRIPT_MEM_WRITE (
+          EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+          EfiBootScriptWidthUint32,
+          (UINTN) (GTTMMADR + 0x138124),
+          1,
+          &Data32
+          );
+      }
+    }
+
+    McD2PciCfg16And (R_SA_IGD_CMD, ~(BIT2 | BIT1));
+    Data16 = McD2PciCfg16(R_SA_IGD_CMD);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint16,
+      (UINTN) (MmPciAddress (0, 0, 2, 0, R_SA_IGD_CMD)),
+      1,
+      &Data16
+      );
+
+    McD2PciCfg64 (R_SA_IGD_GTTMMADR) = 0;
+    DwordData = McD2PciCfg64(R_SA_IGD_GTTMMADR);
+    SCRIPT_MEM_WRITE (
+      EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+      EfiBootScriptWidthUint32,
+      (UINTN) (MmPciAddress (0, 0, 2, 0, R_SA_IGD_GTTMMADR)),
+      1,
+      &DwordData
+      );
+
+    ///
+    /// Free allocated resources
+    ///
+    gDS->FreeMemorySpace (MemBaseAddress, GTTMMADR_SIZE_4MB);
+  }
+
+  ///
+  /// Lock PAVPC Register
+  ///
+  McD0PciCfg32Or (R_SA_PAVPC, B_SA_PAVPC_PAVPLCK_MASK);
+
+  DwordData = McD0PciCfg32 (R_SA_PAVPC);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (MmPciAddress (0,
+                           0,
+                           0,
+                           0,
+                           R_SA_PAVPC)),
+    1,
+    &DwordData
+    );
+
+  return EFI_SUCCESS;
+}
+
+VOID
+PollGtReady (
+  UINT64 Base,
+  UINT32 Offset,
+  UINT32 Mask,
+  UINT32 Result
+  )
+/**
+  "Poll Status" for GT Readiness
+
+  @param[in] Base            - Base address of MMIO
+  @param[in] Offset          - MMIO Offset
+  @param[in] Mask            - Mask
+  @param[in] Result          - Value to wait for
+**/
+{
+  UINT32  GtStatus;
+  UINT16  StallCount;
+
+  StallCount = 0;
+
+  ///
+  /// Register read
+  ///
+  GtStatus = Mmio32 (Base, Offset);
+
+  while (((GtStatus & Mask) != Result) && (StallCount < GT_WAIT_TIMEOUT)) {
+
+    GtStatus = Mmio32 (Base, Offset);
+    ///
+    /// 1mSec wait
+    ///
+    gBS->Stall (1000);
+    StallCount = StallCount + 1;
+  }
+
+  ASSERT ((StallCount != GT_WAIT_TIMEOUT));
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/graphicsinit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/graphicsinit.h
new file mode 100644
index 0000000..d5696e3
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/graphicsinit.h
@@ -0,0 +1,132 @@
+/** @file
+  Header file for initialization of GT PowerManagement
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _GRAPHICS_INIT_H_
+#define _GRAPHICS_INIT_H_
+
+#include "EdkIIGlueDxe.h"
+#include "SaAccess.h"
+#include "EfiScriptLib.h"
+#include EFI_PROTOCOL_DEFINITION (SaPlatformPolicy)
+#include EFI_PROTOCOL_DEFINITION (PciHostBridgeResourceAllocation)
+#include EFI_PROTOCOL_CONSUMER (ExitPmAuth)
+#include "SaInit.h"
+#include "PchAccess.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+
+///
+/// Data definitions
+///
+///
+/// GT RELATED EQUATES
+///
+#define GTT_MEM_ALIGN     22
+#define GTTMMADR_SIZE_4MB 0x400000
+#define GT_WAIT_TIMEOUT   3000     ///< ~3 seconds
+
+///
+/// PAVP Modes
+///
+#define PAVP_LITE_MODE      1
+#define PAVP_SERPENT_MODE   2
+#define PAVP_PCM_SIZE_1_MB  1
+
+EFI_STATUS
+PavpInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Initialize PAVP feature of SystemAgent.
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS         - PAVP initialization complete
+**/
+;
+
+/**
+  Initialize PAVP.
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS - PAVP initialization complete
+**/
+EFI_STATUS
+PmInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Initialize GT PowerManagement of SystemAgent.
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS         - GT Power Management initialization complete
+**/
+;
+
+EFI_STATUS
+GraphicsInit (
+  IN EFI_HANDLE                      ImageHandle,
+  IN DXE_PLATFORM_SA_POLICY_PROTOCOL *DxePlatformSaPolicy
+  )
+/**
+  Initialize GT PowerManagement of SystemAgent.
+
+  @param[in] ImageHandle         - Handle for the image of this driver
+  @param[in] DxePlatformSaPolicy - SA DxePlatformPolicy protocol
+
+  @retval EFI_SUCCESS         - GT Power Management initialization complete
+**/
+;
+
+VOID
+PollGtReady (
+  UINT64 Base,
+  UINT32 Offset,
+  UINT32 Mask,
+  UINT32 Result
+  )
+/**
+  "Poll Status" for GT Readiness
+
+  @param[in] Base            - Base address of MMIO
+  @param[in] Offset          - MMIO Offset
+  @param[in] Mask            - Mask
+  @param[in] Result          - Value to wait for
+**/
+;
+
+EFI_STATUS
+PostPmInitExitPmAuth (
+  VOID
+  )
+/**
+  Do Post GT PM Init Steps after VBIOS Initialization.
+
+  @retval EFI_SUCCESS          Succeed.
+**/
+;
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/igdopregion.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/igdopregion.c
new file mode 100644
index 0000000..0774807
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/igdopregion.c
@@ -0,0 +1,955 @@
+/** @file
+  This is part of the implementation of an Intel Graphics drivers OpRegion /
+  Software SCI interface between system BIOS, ASL code, and Graphics drivers.
+  The code in this file will load the driver and initialize the interface
+
+  Supporting Specifiction: OpRegion / Software SCI SPEC 0.70
+
+  Acronyms:
+    IGD:        Internal Graphics Device
+    NVS:        ACPI Non Volatile Storage
+    OpRegion:   ACPI Operational Region
+    VBT:        Video BIOS Table (OEM customizable data)
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+
+///
+/// Include files
+///
+#include "IgdOpRegion.h"
+#include "CpuIA32.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+#include <Protocol\SaPlatformPolicy\SaPlatformPolicy.h>
+#ifndef AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+#include "Token.h"
+#if (defined(IntelSaGopDriver_SUPPORT) && (IntelSaGopDriver_SUPPORT == 1))
+#include "Include\Protocol\IntelSaGopDriver.h"
+#endif
+#endif
+
+///
+/// Global variables
+///
+IGD_OPREGION_PROTOCOL                  mIgdOpRegion;
+EFI_GUID                               mMiscSubClass = EFI_MISC_SUBCLASS_GUID;
+EFI_EVENT                              mExitPmAuthEvent;
+BOOLEAN                                mRunExitPmAuthRoutine;
+SYSTEM_AGENT_GLOBAL_NVS_AREA_PROTOCOL  *mSaGlobalNvsArea;
+DXE_PLATFORM_SA_POLICY_PROTOCOL        *mDxePlatformSaPolicy;
+
+///
+/// Function implementations.
+///
+EFI_STATUS
+GetIntegratedIntelVbtPtr (
+  OUT VBIOS_VBT_STRUCTURE **VbtFileBuffer
+  )
+/**
+  Get VBT data using SaPlaformPolicy
+
+  @param[in] VbtFileBuffer    Pointer to VBT data buffer.
+
+  @retval EFI_SUCCESS      VBT data was returned.
+  @retval EFI_NOT_FOUND    VBT data not found.
+  @exception EFI_UNSUPPORTED  Invalid signature in VBT data.
+**/
+{
+  EFI_STATUS                    Status;
+  EFI_PHYSICAL_ADDRESS          VbtAddress;
+  UINT32                        Size;
+
+  ///
+  /// Get the platform SA policy.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gDxePlatformSaPolicyGuid,
+                  NULL,
+                  (VOID **) &mDxePlatformSaPolicy
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  VbtAddress = mDxePlatformSaPolicy->IgdConfig->VbtAddress;
+  Size       = mDxePlatformSaPolicy->IgdConfig->Size ;
+
+  if (VbtAddress == 0x00000000) {
+    return EFI_NOT_FOUND;
+  } else {
+    ///
+    /// Check VBT signature
+    ///
+    *VbtFileBuffer  = NULL;
+    *VbtFileBuffer = (VBIOS_VBT_STRUCTURE *) (UINTN) VbtAddress;
+    if ((*((UINT32 *) ((*VbtFileBuffer)->HeaderSignature))) != VBT_SIGNATURE) {
+      FreePool (*VbtFileBuffer);
+      *VbtFileBuffer = NULL;
+      return EFI_UNSUPPORTED;
+    }
+  }
+  if (Size == 0) {
+    return EFI_NOT_FOUND;
+  } else {
+    ///
+    /// Check VBT size
+    ///
+    if ((*VbtFileBuffer)->HeaderVbtSize > Size) {
+      (*VbtFileBuffer)->HeaderVbtSize = (UINT16) Size;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+GetIntegratedIntelVBiosPtr (
+  OUT INTEL_VBIOS_OPTION_ROM_HEADER **VBiosImage
+  )
+/**
+  Get a pointer to an uncompressed image of the Intel video BIOS.
+
+  Note: This function would only be called if the video BIOS at 0xC000 is
+        missing or not an Intel video BIOS.  It may not be an Intel video BIOS
+        if the Intel graphic contoller is considered a secondary adapter.
+
+  @param[in] VBiosImage   - Pointer to an uncompressed Intel video BIOS.  This pointer must
+                            be set to NULL if an uncompressed image of the Intel Video BIOS
+                            is not obtainable.
+
+  @retval EFI_SUCCESS        - VBiosPtr is updated.
+  @exception EFI_UNSUPPORTED - No Intel video BIOS found.
+**/
+
+{
+  EFI_HANDLE                    *HandleBuffer;
+  UINTN                         HandleCount;
+  UINTN                         Index;
+  INTEL_VBIOS_PCIR_STRUCTURE    *PcirBlockPtr;
+  EFI_STATUS                    Status;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  INTEL_VBIOS_OPTION_ROM_HEADER *VBiosRomImage;
+  ///
+  /// Set as if an umcompressed Intel video BIOS image was not obtainable.
+  ///
+  VBiosRomImage = NULL;
+
+  ///
+  /// Get all PCI IO protocols
+  ///
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiPciIoProtocolGuid,
+                  NULL,
+                  &HandleCount,
+                  &HandleBuffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Find the video BIOS by checking each PCI IO handle for an Intel video
+  /// BIOS OPROM.
+  ///
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = gBS->HandleProtocol (
+                    HandleBuffer[Index],
+                    &gEfiPciIoProtocolGuid,
+                    (VOID **) &PciIo
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    VBiosRomImage = PciIo->RomImage;
+
+    ///
+    /// If this PCI device doesn't have a ROM image, skip to the next device.
+    ///
+    if (!VBiosRomImage) {
+      continue;
+    }
+    ///
+    /// Get pointer to PCIR structure
+    ///
+    PcirBlockPtr = (INTEL_VBIOS_PCIR_STRUCTURE *) ((UINT8 *) VBiosRomImage + VBiosRomImage->PcirOffset);
+
+    ///
+    /// Check if we have an Intel video BIOS OPROM.
+    ///
+    if ((VBiosRomImage->Signature == OPTION_ROM_SIGNATURE) &&
+        (PcirBlockPtr->VendorId == V_SA_MC_VID) &&
+        (PcirBlockPtr->ClassCode[0] == 0x00) &&
+        (PcirBlockPtr->ClassCode[1] == 0x00) &&
+        (PcirBlockPtr->ClassCode[2] == 0x03)
+        ) {
+      ///
+      /// Found Intel video BIOS.
+      ///
+      *VBiosImage = VBiosRomImage;
+      return EFI_SUCCESS;
+    }
+  }
+  ///
+  /// No Intel video BIOS found.
+  ///
+  ///
+  /// Free any allocated buffers
+  ///
+  FreePool (HandleBuffer);
+  return EFI_UNSUPPORTED;
+}
+
+EFI_STATUS
+GetVBiosVbtExitPmAuth (
+  VOID
+  )
+/**
+  Get Intel video BIOS VBT information (i.e. Pointer to VBT and VBT size).
+  The VBT (Video BIOS Table) is a block of customizable data that is built
+  within the video BIOS and edited by customers.
+
+  @retval EFI_SUCCESS            - Video BIOS VBT information returned.
+  @exception EFI_UNSUPPORTED     - Could not find VBT information (*VBiosVbtPtr = NULL).
+**/
+{
+  INTEL_VBIOS_PCIR_STRUCTURE    *PcirBlockPtr;
+  UINT16                        PciVenderId;
+  INTEL_VBIOS_OPTION_ROM_HEADER *VBiosPtr;
+  VBIOS_VBT_STRUCTURE           *VBiosVbtPtr;
+  EFI_LEGACY_BIOS_PROTOCOL      *LegacyBios;
+  EFI_STATUS                    Status;
+  VBIOS_VBT_STRUCTURE           *VbtFileBuffer;
+  UINTN                         Index;
+#ifndef AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+#if (defined(IntelSaGopDriver_SUPPORT) && (IntelSaGopDriver_SUPPORT == 1))
+  EFI_PHYSICAL_ADDRESS          VbtAddress;
+  UINT32                        VbtSize;
+  PLATFORM_GOP_POLICY_PROTOCOL  *PlatformGOPPolicy;
+  EFI_GUID PlatformGOPPolicyGuid = EFI_PLATFORM_GOP_POLICY_PROTOCOL_GUID;
+  EFI_STATUS                    Status2 = EFI_SUCCESS;
+#endif
+#endif // AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+
+  if (!mRunExitPmAuthRoutine) {
+    return EFI_SUCCESS;
+  }
+
+  ///
+  /// Get the platform SA policy.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gDxePlatformSaPolicyGuid,
+                  NULL,
+                  (VOID **) &mDxePlatformSaPolicy
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  LegacyBios = NULL;
+  VBiosPtr = NULL;
+
+  Status = gBS->LocateProtocol (&gEfiLegacyBiosProtocolGuid, NULL, (VOID **) &LegacyBios);
+
+#ifndef AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+#if (defined(IntelSaGopDriver_SUPPORT) && (IntelSaGopDriver_SUPPORT == 1))
+  //
+  // Locate Platform GOP policy protocol
+  //
+  Status = gBS->LocateProtocol (&PlatformGOPPolicyGuid, NULL, &PlatformGOPPolicy);
+  if (!EFI_ERROR(Status)) {
+    Status2 = PlatformGOPPolicy->GetVbtData(&VbtAddress, &VbtSize);
+    if (!EFI_ERROR(Status2)) LegacyBios = NULL;
+  }
+#endif
+#endif // AMI_OVERRIDE_FOR_INTEL_GOP_SUPPORT
+
+  if (LegacyBios) {
+    VBiosPtr      = (INTEL_VBIOS_OPTION_ROM_HEADER *) (UINTN) (VBIOS_LOCATION_PRIMARY);
+    PcirBlockPtr  = (INTEL_VBIOS_PCIR_STRUCTURE *) ((UINT8 *) VBiosPtr + VBiosPtr->PcirOffset);
+    PciVenderId   = PcirBlockPtr->VendorId;
+    ///
+    /// If the video BIOS is not at 0xC0000 or it is not an Intel video BIOS get
+    /// the integrated Intel video BIOS (must be uncompressed).
+    ///
+    if ((VBiosPtr->Signature != OPTION_ROM_SIGNATURE) || (PciVenderId != V_SA_MC_VID)) {
+      GetIntegratedIntelVBiosPtr (&VBiosPtr);
+      if (VBiosPtr != NULL) {
+        ///
+        /// Video BIOS found.
+        ///
+        PcirBlockPtr  = (INTEL_VBIOS_PCIR_STRUCTURE *) ((UINT8 *) VBiosPtr + VBiosPtr->PcirOffset);
+        PciVenderId   = PcirBlockPtr->VendorId;
+
+        if ((VBiosPtr->Signature != OPTION_ROM_SIGNATURE) || (PciVenderId != V_SA_MC_VID)) {
+          ///
+          /// Intel video BIOS not found.
+          ///
+          VBiosVbtPtr = NULL;
+          return EFI_UNSUPPORTED;
+        }
+      }
+    }
+  } else {
+    ///
+    /// No Video BIOS found, try to get VBT from FV.
+    ///
+    GetIntegratedIntelVbtPtr (&VbtFileBuffer);
+    if (VbtFileBuffer != NULL) {
+      ///
+      /// Video BIOS not found, use VBT from SaPlatformPolicy
+      ///
+      DEBUG ((EFI_D_INFO, "VBT data found\n"));
+      for (Index = 0; (mDxePlatformSaPolicy->IgdConfig->GopVersion[Index] != '\0'); Index++) {
+      }
+      Index = (Index+1)*2;
+      CopyMem (mIgdOpRegion.OpRegion->Header.DVER, mDxePlatformSaPolicy->IgdConfig->GopVersion, Index);
+      (gBS->CopyMem) (mIgdOpRegion.OpRegion->VBT.GVD1, VbtFileBuffer, VbtFileBuffer->HeaderVbtSize);
+      FreePool (VbtFileBuffer);
+      return EFI_SUCCESS;
+    }
+  }
+
+  if (VBiosPtr == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  DEBUG ((EFI_D_INFO, "VBIOS found at 0x%X\n", VBiosPtr));
+  VBiosVbtPtr = (VBIOS_VBT_STRUCTURE *) ((UINT8 *) VBiosPtr + VBiosPtr->VbtOffset);
+
+  if ((*((UINT32 *) (VBiosVbtPtr->HeaderSignature))) != VBT_SIGNATURE) {
+    return EFI_UNSUPPORTED;
+  }
+  ///
+  /// No PlatformGopPolicy.h in EDK II code
+  ///
+#if 0
+  GetSVER (mIgdOpRegion.OpRegion->Header.SVER);
+#endif
+  DEBUG ((EFI_D_INFO, "System BIOS ID is %a\n", mIgdOpRegion.OpRegion->Header.SVER));
+
+  ///
+  /// Initialize Video BIOS version with its build number.
+  ///
+  mIgdOpRegion.OpRegion->Header.VVER[0] = VBiosVbtPtr->CoreBlockBiosBuild[0];
+  mIgdOpRegion.OpRegion->Header.VVER[1] = VBiosVbtPtr->CoreBlockBiosBuild[1];
+  mIgdOpRegion.OpRegion->Header.VVER[2] = VBiosVbtPtr->CoreBlockBiosBuild[2];
+  mIgdOpRegion.OpRegion->Header.VVER[3] = VBiosVbtPtr->CoreBlockBiosBuild[3];
+  (gBS->CopyMem) (mIgdOpRegion.OpRegion->VBT.GVD1, VBiosVbtPtr, VBiosVbtPtr->HeaderVbtSize);
+
+  ///
+  /// Return final status
+  ///
+  return EFI_SUCCESS;
+}
+///
+/// No PlatformGopPolicy.h in EDK II code
+///
+#if 0
+EFI_STATUS
+GetSVER (
+  OUT UINT8 *SVER
+  )
+/**
+  Set the SVER (system BIOS ID) string with the system BIOS build number.
+
+  @param[in] SVER  String to populate with system BIOS build number.
+
+  @retval EFI_SUCCESS     The SVER string is populated.
+  @exception EFI_UNSUPPORTED The SVER string is not populated.
+**/
+{
+  EFI_SUBCLASS_TYPE1_HEADER *DataHeader;
+  EFI_DATA_HUB_PROTOCOL     *DataHub;
+  EFI_MISC_BIOS_VENDOR      *BiosVendor;
+  UINTN                     Length;
+  UINT64                    MonotonicCount;
+  CHAR16                    *NewString;
+  EFI_DATA_RECORD_HEADER    *Record;
+  EFI_STATUS                Status;
+
+  ///
+  ///  Locate the data hub protocol.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gEfiDataHubProtocolGuid,
+                  NULL,
+                  &DataHub
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  ///  Get the BIOS ID from the data hub.
+  ///
+  MonotonicCount  = 0;
+  Record          = NULL;
+
+  do {
+    ///
+    /// Check each data class record
+    ///
+    Status = DataHub->GetNextRecord (DataHub, &MonotonicCount, NULL, &Record);
+    if (Record->DataRecordClass == EFI_DATA_RECORD_CLASS_DATA) {
+      ///
+      /// Check for BIOS vendor information
+      ///
+      DataHeader = (EFI_SUBCLASS_TYPE1_HEADER *) (Record + 1);
+      if (CompareGuid (&Record->DataRecordGuid, &mMiscSubClass) &&
+          (DataHeader->RecordType == EFI_MISC_BIOS_VENDOR_RECORD_NUMBER)
+          ) {
+        BiosVendor = (EFI_MISC_BIOS_VENDOR *) (DataHeader + 1);
+
+        ///
+        /// Get the BIOS ID string from the HII database
+        ///
+        Status = GetStringFromToken (&Record->ProducerName, BiosVendor->BiosVersion, &NewString);
+        ASSERT_EFI_ERROR (Status);
+
+        ///
+        /// Convert from Unicode to ASCII
+        ///
+        if (NewString != NULL) {
+          Length = StrLen (NewString);
+          ASSERT (Length <= SVER_SIZE);
+          ASPrint (SVER, Length, "%s", NewString);
+          return EFI_SUCCESS;
+        }
+      }
+    }
+  } while ((!EFI_ERROR (Status)) && (MonotonicCount != 0));
+
+  ///
+  /// We assume BIOS ID is present and required.
+  /// If this is not the case, BIOS ID will need to be determined in another way.
+  ///
+  ASSERT_EFI_ERROR (EFI_UNSUPPORTED);
+  return EFI_UNSUPPORTED;
+}
+
+#if (EFI_SPECIFICATION_VERSION < 0x0002000A)
+EFI_STATUS
+GetStringFromToken (
+  IN      EFI_GUID                  *ProducerGuid,
+  IN      STRING_REF                Token,
+  OUT     CHAR16                    **String
+  )
+/**
+  Acquire the string associated with the ProducerGuid and return it.
+
+  @param[in] ProducerGuid - The Guid to search the HII database for
+  @param[in] Token - The token value of the string to extract
+  @param[in] String - The string that is extracted
+
+  @retval EFI_SUCCESS       The function completed successfully
+  @retval EFI_NOT_FOUND     The requested string was not found
+**/
+{
+  EFI_STATUS        Status;
+  UINT16            HandleBufferLength;
+  EFI_HII_HANDLE    *HiiHandleBuffer;
+  UINTN             StringBufferLength;
+  UINTN             NumberOfHiiHandles;
+  UINTN             Index;
+  UINT16            Length;
+  EFI_GUID          HiiGuid;
+  EFI_HII_PROTOCOL  *Hii;
+
+  HandleBufferLength  = 0x1000;
+  HiiHandleBuffer     = NULL;
+
+  ///
+  /// Locate HII protocol
+  ///
+  Status = gBS->LocateProtocol (&gEfiHiiProtocolGuid, NULL, &Hii);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Get all the Hii handles
+  ///
+  HiiHandleBuffer = AllocateZeroPool (HandleBufferLength);
+
+  Status          = Hii->FindHandles (Hii, &HandleBufferLength, HiiHandleBuffer);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Get the Hii Handle that matches the StructureNode->ProducerName
+  ///
+  NumberOfHiiHandles = HandleBufferLength / sizeof (EFI_HII_HANDLE);
+  for (Index = 0; Index < NumberOfHiiHandles; Index++) {
+    Length = 0;
+    Status = ExtractDataFromHiiHandle (
+              HiiHandleBuffer[Index],
+              &Length,
+              NULL,
+              &HiiGuid
+              );
+    if (CompareGuid (ProducerGuid, &HiiGuid)) {
+      break;
+    }
+  }
+  ///
+  /// Find the string based on the current language
+  ///
+  StringBufferLength  = 0x100;
+  *String             = AllocateZeroPool (0x100);
+  Status = Hii->GetString (
+                  Hii,
+                  HiiHandleBuffer[Index],
+                  Token,
+                  FALSE,
+                  NULL,
+                  &StringBufferLength,
+                  *String
+                  );
+
+  (gBS->FreePool) (HiiHandleBuffer);
+
+  if (EFI_ERROR (Status)) {
+    (gBS->FreePool) (*String);
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+#endif
+#endif
+
+EFI_STATUS
+IgdOpRegionInit (
+  VOID
+  )
+/**
+  Graphics OpRegion / Software SCI driver installation function.
+
+  @param[in] void         - None
+  @retval EFI_SUCCESS     - The driver installed without error.
+  @retval EFI_ABORTED     - The driver encountered an error and could not complete
+                            installation of the ACPI tables.
+**/
+
+{
+  EFI_HANDLE                    Handle;
+  EFI_STATUS                    Status;
+  ///
+  ///  VBIOS_VBT_STRUCTURE           *VBiosVbtPtr;
+  ///
+  UINT32                        DwordData;
+  EFI_CPU_IO_PROTOCOL           *CpuIo;
+  UINT16                        Data16;
+  UINT32                        Data32;
+  UINT16                        PchAcpiBaseAddress;
+  UINT16                        DeviceId;
+  PCH_SERIES                    PchSeries;
+  CPU_FAMILY                    CpuFamilyId;
+  EFI_GUID                      DxePlatformSaPolicyGuid = DXE_PLATFORM_SA_POLICY_GUID;
+  DXE_PLATFORM_SA_POLICY_PROTOCOL  *DxePlatformSaPolicy;
+
+  CpuFamilyId = GetCpuFamily();
+
+  ///
+  /// Get the platform SA policy.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gDxePlatformSaPolicyGuid,
+                  NULL,
+                  (VOID **) &mDxePlatformSaPolicy
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ///
+  ///  Locate the SA Global NVS Protocol.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gSaGlobalNvsAreaProtocolGuid,
+                  NULL,
+                  (VOID **) &mSaGlobalNvsArea
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Allocate an ACPI NVS memory buffer as the IGD OpRegion, zero initialize
+  /// the first 1K, and set the IGD OpRegion pointer in the Global NVS
+  /// area structure.
+  ///
+  Status = (gBS->AllocatePool) (EfiACPIMemoryNVS, sizeof (IGD_OPREGION_STRUC), (VOID **) &mIgdOpRegion.OpRegion);
+  ASSERT_EFI_ERROR (Status);
+
+  (gBS->SetMem) (mIgdOpRegion.OpRegion, 0x2000, 0);
+  mSaGlobalNvsArea->Area->IgdOpRegionAddress = (UINT32) (UINTN) (mIgdOpRegion.OpRegion);
+
+  ///
+  /// If IGD is disabled return
+  ///
+  mRunExitPmAuthRoutine = TRUE;
+  if (IgdMmPci32 (0) == 0xFFFFFFFF) {
+    mRunExitPmAuthRoutine = FALSE;
+    return EFI_SUCCESS;
+  }
+
+  PchSeries = GetPchSeries();
+  ///
+  /// Initialize OpRegion Header
+  ///
+  (gBS->CopyMem) (mIgdOpRegion.OpRegion->Header.SIGN, HEADER_SIGNATURE, sizeof (HEADER_SIGNATURE));
+  ///
+  /// Set OpRegion Size in KBs
+  ///
+  mIgdOpRegion.OpRegion->Header.SIZE = HEADER_SIZE / 1024;
+  mIgdOpRegion.OpRegion->Header.OVER = (UINT32) (LShiftU64 (HEADER_OPREGION_VER, 16) + LShiftU64 (HEADER_OPREGION_REV, 8));
+
+  ///
+  /// Get CPU Flavor by reading System Agent's Device ID (B0:D1F:F0:R02)
+  ///
+  DeviceId = McD0PciCfg16 (R_SA_MC_DEVICE_ID);
+  if (IS_SA_DEVICE_ID_MOBILE (DeviceId)) {
+    ///
+    /// For Mobile, all Mailbox are supported.
+    ///
+    mIgdOpRegion.OpRegion->Header.MBOX = HEADER_MBOX_SUPPORT_MOBILE;
+  } else if (IS_SA_DEVICE_ID_DESKTOP (DeviceId) || IS_SA_DEVICE_ID_SERVER (DeviceId)) {
+    ///
+    /// For Desktop and Server, Mailbox 1/3/5 are not supported.
+    ///
+    mIgdOpRegion.OpRegion->Header.MBOX = HEADER_MBOX_SUPPORT_DESKTOP;
+  } else {
+    DEBUG ((EFI_D_ERROR, "Hang if unknown System Agent\n"));
+    ASSERT (FALSE);
+    ///
+    /// Hang if unknown System Agent
+    ///
+  }
+  ///
+  /// Initialize OpRegion Mailbox 1 (Public ACPI Methods).
+  ///
+  /// @todo:  The initial setting of mailbox 1 fields is implementation specific.
+  /// Adjust them as needed many even coming from user setting in setup.
+  ///
+  ///
+  /// Initialize OpRegion Mailbox 3 (ASLE Interrupt and Power Conservation).
+  ///
+  /// @todo:  The initial setting of mailbox 3 fields is implementation specific.
+  /// Adjust them as needed many even coming from user setting in setup.
+  ///
+  ///
+  /// Do not initialize TCHE. This field is written by the graphics driver only.
+  ///
+  ///
+  /// The ALSI field is generally initialized by ASL code by reading the embedded controller.
+  ///
+  if (mDxePlatformSaPolicy->Revision >= 5) {
+    mIgdOpRegion.OpRegion->Header.PCON = mDxePlatformSaPolicy->IgdConfig->PlatformConfig;
+    if (CpuFamilyId == EnumCpuHswUlt) {
+      mIgdOpRegion.OpRegion->Header.PCON = mIgdOpRegion.OpRegion->Header.PCON | 0x2;
+    }
+  }
+  mIgdOpRegion.OpRegion->MBox3.BCLP = BACKLIGHT_BRIGHTNESS;
+
+  mIgdOpRegion.OpRegion->MBox3.PFIT = (FIELD_VALID_BIT | PFIT_STRETCH);
+
+  ///
+  /// Reporting to driver for VR IMON Calibration. Bits [5-1] values supported 14A to 31A.
+  ///
+  mIgdOpRegion.OpRegion->MBox3.PCFT = (mSaGlobalNvsArea->Area->GfxTurboIMON << 1) & 0x003E;
+
+  ///
+  /// Set Initial current Brightness
+  ///
+  mIgdOpRegion.OpRegion->MBox3.CBLV = (INIT_BRIGHT_LEVEL | FIELD_VALID_BIT);
+
+  ///
+  /// <EXAMPLE> Create a static Backlight Brightness Level Duty cycle Mapping Table
+  /// Possible 20 entries (example used 10), each 16 bits as follows:
+  /// [15] = Field Valid bit, [14:08] = Level in Percentage (0-64h), [07:00] = Desired duty cycle (0 - FFh).
+  ///
+  mIgdOpRegion.OpRegion->MBox3.BCLM[0] = (0x0000 + WORD_FIELD_VALID_BIT);  ///< 0%
+  ///
+  mIgdOpRegion.OpRegion->MBox3.BCLM[1] = (0x0A19 + WORD_FIELD_VALID_BIT);  ///< 10%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[2] = (0x1433 + WORD_FIELD_VALID_BIT);  ///< 20%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[3] = (0x1E4C + WORD_FIELD_VALID_BIT);  ///< 30%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[4] = (0x2866 + WORD_FIELD_VALID_BIT);  ///< 40%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[5] = (0x327F + WORD_FIELD_VALID_BIT);  ///< 50%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[6] = (0x3C99 + WORD_FIELD_VALID_BIT);  ///< 60%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[7] = (0x46B2 + WORD_FIELD_VALID_BIT);  ///< 70%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[8] = (0x50CC + WORD_FIELD_VALID_BIT);  ///< 80%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[9] = (0x5AE5 + WORD_FIELD_VALID_BIT);  ///< 90%
+  mIgdOpRegion.OpRegion->MBox3.BCLM[10] = (0x64FF + WORD_FIELD_VALID_BIT);  ///< 100%
+
+  mIgdOpRegion.OpRegion->MBox3.IUER = 0x00;
+
+  Status = gBS->LocateProtocol (&DxePlatformSaPolicyGuid, NULL, &DxePlatformSaPolicy);
+ 
+  if (!EFI_ERROR(Status)) {
+    mIgdOpRegion.OpRegion->MBox3.IUER =  DxePlatformSaPolicy->IgdConfig->IuerStatusVal;
+  }
+
+  ///
+  /// Initialize hardware state:
+  ///   Set ASLS Register to the OpRegion physical memory address.
+  ///   Set SWSCI register bit 15 to a "1" to activate SCI interrupts.
+  ///
+  IgdMmPci32 (R_SA_IGD_ASLS_OFFSET) = (UINT32) (UINTN) (mIgdOpRegion.OpRegion);
+  IgdMmPci16AndThenOr (R_SA_IGD_SWSCI_OFFSET, ~(BIT0), BIT15);
+
+  DwordData = IgdMmPci32 (R_SA_IGD_ASLS_OFFSET);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (MmPciAddress (0x0,
+            SA_IGD_BUS,
+            SA_IGD_DEV,
+            SA_IGD_FUN_0,
+            R_SA_IGD_ASLS_OFFSET)),
+    1,
+    &DwordData
+    );
+  DwordData = IgdMmPci32 (R_SA_IGD_SWSCI_OFFSET);
+  SCRIPT_MEM_WRITE (
+    EFI_ACPI_S3_RESUME_SCRIPT_TABLE,
+    EfiBootScriptWidthUint32,
+    (UINTN) (MmPciAddress (0x0,
+            SA_IGD_BUS,
+            SA_IGD_DEV,
+            SA_IGD_FUN_0,
+            R_SA_IGD_SWSCI_OFFSET)),
+    1,
+    &DwordData
+    );
+  PchAcpiBaseAddress = MmPci16 (
+                        0,
+                        DEFAULT_PCI_BUS_NUMBER_PCH,
+                        PCI_DEVICE_NUMBER_PCH_LPC,
+                        PCI_FUNCTION_NUMBER_PCH_LPC,
+                        R_PCH_LPC_ACPI_BASE
+                        ) &~BIT0;
+
+  ///
+  /// Find the CPU I/O Protocol.  ASSERT if not found.
+  ///
+  Status = gBS->LocateProtocol (
+                  &gEfiCpuIoProtocolGuid,
+                  NULL,
+                  (VOID **) &CpuIo
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  CpuIo->Io.Read (
+              CpuIo,
+              EfiCpuIoWidthUint16,
+              PchAcpiBaseAddress + PCH_TCO_BASE + R_PCH_TCO1_STS,
+              1,
+              &Data16
+              );
+  ///
+  /// Clear the B_DMISCI_STS bit in R_TCO1_STS by writing a '1'.
+  ///
+  Data16 &= B_PCH_TCO1_STS_DMISCI;
+
+  CpuIo->Io.Write (
+              CpuIo,
+              EfiCpuIoWidthUint16,
+              PchAcpiBaseAddress + PCH_TCO_BASE + R_PCH_TCO1_STS,
+              1,
+              &Data16
+              );
+
+  if (PchSeries == PchLp) {
+    ///
+    /// Clear the ACPI TCO status.
+    ///
+    Data32 = B_PCH_ACPI_GPE0_STS_127_96_TC0SCI;
+    CpuIo->Io.Write (
+                CpuIo,
+                EfiCpuIoWidthUint32,
+                (UINT64) (PchAcpiBaseAddress + R_PCH_ACPI_GPE0_STS_127_96),
+                1,
+                &Data32
+                );
+
+    ///
+    /// Enable ACPI TCO SCI's.
+    ///
+    CpuIo->Io.Read (
+                CpuIo,
+                EfiCpuIoWidthUint16,
+                (UINT64) (PchAcpiBaseAddress + R_PCH_ACPI_GPE0_EN_127_96),
+                1,
+                &Data16
+                );
+    Data16 |= B_PCH_ACPI_GPE0_EN_127_96_TC0SCI;
+    CpuIo->Io.Write (
+                CpuIo,
+                EfiCpuIoWidthUint16,
+                (UINT64) (PchAcpiBaseAddress + R_PCH_ACPI_GPE0_EN_127_96),
+                1,
+                &Data16
+                );
+  } else if (PchSeries == PchH) {
+    ///
+    /// Clear the ACPI TCO status.
+    ///
+    Data32 = B_PCH_ACPI_GPE0a_STS_TC0SCI;
+    CpuIo->Io.Write (
+                CpuIo,
+                EfiCpuIoWidthUint32,
+                (UINT64) (PchAcpiBaseAddress + R_PCH_ACPI_GPE0a_STS),
+                1,
+                &Data32
+                );
+
+    ///
+    /// Enable ACPI TCO SCI's.
+    ///
+    CpuIo->Io.Read (
+                CpuIo,
+                EfiCpuIoWidthUint16,
+                (UINT64) (PchAcpiBaseAddress + R_PCH_ACPI_GPE0a_EN),
+                1,
+                &Data16
+                );
+    Data16 |= B_PCH_ACPI_GPE0a_EN_TC0SCI;
+    CpuIo->Io.Write (
+                CpuIo,
+                EfiCpuIoWidthUint16,
+                (UINT64) (PchAcpiBaseAddress + R_PCH_ACPI_GPE0a_EN),
+                1,
+                &Data16
+                );
+  }
+
+  ///
+  /// Install OpRegion / Software SCI protocol
+  ///
+  Handle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gIgdOpRegionProtocolGuid,
+                  &mIgdOpRegion,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Return final status
+  ///
+  return EFI_SUCCESS;
+}
+
+
+EFI_STATUS
+UpdateIgdOpRegionExitPmAuth (
+  VOID
+  )
+/**
+  Update Graphics OpRegion after PCI enumeration.
+
+  @param[in] void         - None
+  @retval EFI_SUCCESS     - The function completed successfully.
+**/
+{
+  EFI_STATUS                    Status;
+  UINTN                         HandleCount;
+  EFI_HANDLE                    *HandleBuffer;
+  UINTN                         Index;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  PCI_TYPE00                    Pci;
+  UINTN                         Segment;
+  UINTN                         Bus;
+  UINTN                         Device;
+  UINTN                         Function;
+
+  Bus      = 0;
+  Device   = 0;
+  Function = 0;
+
+  DEBUG ((EFI_D_INFO, "UpdateIgdOpRegionExitPmAuth\n"));
+
+  mIgdOpRegion.OpRegion->Header.PCON |= BIT8; //Set External Gfx Adapter field is valid
+  mIgdOpRegion.OpRegion->Header.PCON &= (UINT32) (~BIT7); //Assume No External Gfx Adapter 
+
+  ///
+  /// Get all PCI IO protocols handles
+  ///
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiPciIoProtocolGuid,
+                  NULL,
+                  &HandleCount,
+                  &HandleBuffer
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; Index < HandleCount; Index++) {
+      ///
+      /// Get the PCI IO Protocol Interface corresponding to each handle
+      ///
+      Status = gBS->HandleProtocol (
+                      HandleBuffer[Index],
+                      &gEfiPciIoProtocolGuid,
+                      (VOID **) &PciIo
+                      );
+
+      if (!EFI_ERROR (Status)) {
+        ///
+        /// Read the PCI configuration space
+        ///
+        Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint32,
+                          0,
+                          sizeof (Pci) / sizeof (UINT32),
+                          &Pci
+                          );
+
+        ///
+        /// Find the display controllers devices
+        ///
+        if (!EFI_ERROR (Status) && IS_PCI_DISPLAY (&Pci)) {
+          Status = PciIo->GetLocation (
+                            PciIo,
+                            &Segment,
+                            &Bus,
+                            &Device,
+                            &Function
+                            );
+
+          //
+          // Assumption: Onboard devices will be sits on Bus no 0, while external devices will be sits on Bus no > 0
+          //
+          if (!EFI_ERROR (Status) && (Bus > 0)) {
+            //External Gfx Adapter Detected and Available
+            DEBUG ((EFI_D_INFO, "PCON - External Gfx Adapter Detected and Available\n"));
+            mIgdOpRegion.OpRegion->Header.PCON |= BIT7;
+            break;
+		  }
+        }
+      }
+    }
+  }
+
+  ///
+  /// Free any allocated buffers
+  ///
+  if (HandleBuffer != NULL) {
+    FreePool (HandleBuffer);
+  }
+
+  ///
+  /// Return final status
+  ///
+  return Status;
+}
\ No newline at end of file
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/igdopregion.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/igdopregion.h
new file mode 100644
index 0000000..085c665
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Dxe/igdopregion.h
@@ -0,0 +1,281 @@
+/** @file
+  This is part of the implementation of an Intel Graphics drivers OpRegion /
+  Software SCI interface between system BIOS, ASL code, and Graphics drivers.
+
+  Supporting Specifiction: OpRegion / Software SCI SPEC 0.70
+
+  Acronyms:
+    IGD:        Internal Graphics Device
+    NVS:        ACPI Non Volatile Storage
+    OpRegion:   ACPI Operational Region
+    VBT:        Video BIOS Table (OEM customizable data)
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _IGD_OPREGION_H_
+#define _IGD_OPREGION_H_
+
+///
+/// Statements that include other header files.
+///
+#include "EdkIIGlueDxe.h"
+#include "EfiScriptLib.h"
+#include "PchAccess.h"
+#include "PchPlatformLib.h"
+#include "SaAccess.h"
+#include "EfiMgmtModeRuntimeLib.h"
+#include "EfiApi.h"
+#include "pci22.h"
+
+///
+/// Driver Consumed Protocol Prototypes
+///
+#include EFI_GUID_DEFINITION (DataHubRecords)
+#include EFI_PROTOCOL_DEFINITION (SaPlatformPolicy)
+
+#include EFI_PROTOCOL_CONSUMER (PciIo)
+#include EFI_PROTOCOL_CONSUMER (PciRootBridgeIo)
+
+#include EFI_PROTOCOL_DEPENDENCY (CpuIo)
+#include EFI_PROTOCOL_DEPENDENCY (DataHub)
+#include EFI_PROTOCOL_DEPENDENCY (SaGlobalNvsArea)
+#include EFI_PROTOCOL_CONSUMER (ExitPmAuth)
+
+///
+/// Driver Produced Protocol Prototypes
+///
+#include EFI_PROTOCOL_PRODUCER (IgdOpRegion)
+
+///
+///
+/// OpRegion (Miscellaneous) #defines.
+///
+/// OpRegion Header #defines.
+///
+#define HEADER_SIGNATURE            "IntelGraphicsMem"
+#define HEADER_SIZE                 0x2000
+#define HEADER_OPREGION_VER         0x0200
+#define HEADER_OPREGION_REV         0x00
+#define HEADER_MBOX_SUPPORT         (HD_MBOX5 + HD_MBOX4 + HD_MBOX3 + HD_MBOX2 + HD_MBOX1)
+#define HEADER_MBOX_SUPPORT_MOBILE  HEADER_MBOX_SUPPORT
+#define HEADER_MBOX_SUPPORT_DESKTOP HEADER_MBOX_SUPPORT
+#define HD_MBOX1                    BIT0
+#define HD_MBOX2                    BIT1
+#define HD_MBOX3                    BIT2
+#define HD_MBOX4                    BIT3
+#define HD_MBOX5                    BIT4
+#define SVER_SIZE                   32
+
+///
+/// OpRegion Mailbox 1 EQUates.
+///
+/// OpRegion Mailbox 3 EQUates.
+///
+#define ALS_ENABLE            BIT0
+#define BLC_ENABLE            BIT1
+#define BACKLIGHT_BRIGHTNESS  0xFF
+#define FIELD_VALID_BIT       BIT31
+#define WORD_FIELD_VALID_BIT  BIT15
+#define PFIT_ENABLE           BIT2
+#define PFIT_OPRN_AUTO        0x00000000
+#define PFIT_OPRN_SCALING     0x00000007
+#define PFIT_OPRN_OFF         0x00000000
+#define PFIT_SETUP_AUTO       0
+#define PFIT_SETUP_SCALING    1
+#define PFIT_SETUP_OFF        2
+#define INIT_BRIGHT_LEVEL     0x64
+#define PFIT_STRETCH          6
+
+///
+/// GMCH PCI register access #defines.
+///
+#define IgdMmPci32(Register)            MmPci32 (0, SA_IGD_BUS, SA_IGD_DEV, SA_IGD_FUN_0, Register)
+#define IgdMmPci16Or(Register, OrData)  MmPci16Or (0, SA_IGD_BUS, SA_IGD_DEV, SA_IGD_FUN_0, Register, OrData)
+#define IgdMmPci16AndThenOr(Register, AndData, OrData) \
+  MmPci16AndThenOr ( \
+  0, \
+  SA_IGD_BUS, \
+  SA_IGD_DEV, \
+  SA_IGD_FUN_0, \
+  Register, \
+  AndData, \
+  OrData \
+  )
+
+///
+/// Video BIOS / VBT #defines
+///
+#define OPTION_ROM_SIGNATURE    0xAA55
+#define VBIOS_LOCATION_PRIMARY  0xC0000
+
+#define VBT_SIGNATURE           EFI_SIGNATURE_32 ('$', 'V', 'B', 'T')
+///
+/// Typedef stuctures
+///
+#pragma pack(1)
+typedef struct {
+  UINT16  Signature;  /// 0xAA55
+  UINT8   Size512;
+  UINT8   Reserved[21];
+  UINT16  PcirOffset;
+  UINT16  VbtOffset;
+} INTEL_VBIOS_OPTION_ROM_HEADER;
+#pragma pack()
+
+#pragma pack(1)
+typedef struct {
+  UINT32  Signature;  /// "PCIR"
+  UINT16  VendorId;   /// 0x8086
+  UINT16  DeviceId;
+  UINT16  Reserved0;
+  UINT16  Length;
+  UINT8   Revision;
+  UINT8   ClassCode[3];
+  UINT16  ImageLength;
+  UINT16  CodeRevision;
+  UINT8   CodeType;
+  UINT8   Indicator;
+  UINT16  Reserved1;
+} INTEL_VBIOS_PCIR_STRUCTURE;
+#pragma pack()
+
+#pragma pack(1)
+typedef struct {
+  UINT8   HeaderSignature[20];
+  UINT16  HeaderVersion;
+  UINT16  HeaderSize;
+  UINT16  HeaderVbtSize;
+  UINT8   HeaderVbtCheckSum;
+  UINT8   HeaderReserved;
+  UINT32  HeaderOffsetVbtDataBlock;
+  UINT32  HeaderOffsetAim1;
+  UINT32  HeaderOffsetAim2;
+  UINT32  HeaderOffsetAim3;
+  UINT32  HeaderOffsetAim4;
+  UINT8   DataHeaderSignature[16];
+  UINT16  DataHeaderVersion;
+  UINT16  DataHeaderSize;
+  UINT16  DataHeaderDataBlockSize;
+  UINT8   CoreBlockId;
+  UINT16  CoreBlockSize;
+  UINT16  CoreBlockBiosSize;
+  UINT8   CoreBlockBiosType;
+  UINT8   CoreBlockReleaseStatus;
+  UINT8   CoreBlockHWSupported;
+  UINT8   CoreBlockIntegratedHW;
+  UINT8   CoreBlockBiosBuild[4];
+  UINT8   CoreBlockBiosSignOn[155];
+} VBIOS_VBT_STRUCTURE;
+#pragma pack()
+///
+/// Driver Private Function definitions
+///
+EFI_STATUS
+GetSVER (
+  OUT UINT8 *SVER
+  )
+/**
+  Set the SVER (system BIOS ID) string with the system BIOS build number.
+
+  @param[in] SVER  String to populate with system BIOS build number.
+
+  @retval EFI_SUCCESS     The SVER string is populated.
+  @exception EFI_UNSUPPORTED The SVER string is not populated.
+**/
+;
+
+EFI_STATUS
+GetStringFromToken (
+  IN      EFI_GUID                  *ProducerGuid,
+  IN      STRING_REF                Token,
+  OUT     CHAR16                    **String
+  )
+/**
+  Acquire the string associated with the ProducerGuid and return it.
+
+  @param[in] ProducerGuid - The Guid to search the HII database for
+  @param[in] Token - The token value of the string to extract
+  @param[in] String - The string that is extracted
+
+  @retval EFI_SUCCESS       The function completed successfully
+  @retval EFI_NOT_FOUND     The requested string was not found
+**/
+;
+
+EFI_STATUS
+IgdOpRegionInit (
+  void
+  )
+/**
+  Graphics OpRegion / Software SCI driver installation function.
+
+  @param[in] void         - None
+  @retval EFI_SUCCESS     - The driver installed without error.
+  @retval EFI_ABORTED     - The driver encountered an error and could not complete
+                            installation of the ACPI tables.
+**/
+;
+
+EFI_STATUS
+ExtractDataFromHiiHandle (
+  IN      EFI_HII_HANDLE      HiiHandle,
+  IN OUT  UINT16              *ImageLength,
+  OUT     UINT8               *DefaultImage,
+  OUT     EFI_GUID            *Guid
+  )
+/**
+  Extract information pertaining to the HiiHandle
+
+  @param[in] HiiHandle       - Hii handle
+  @param[in] ImageLength     - For input, length of DefaultImage;
+                    For output, length of actually required
+  @param[in] DefaultImage    - Image buffer prepared by caller
+  @param[in] Guid            - Guid information about the form
+
+  @retval EFI_OUT_OF_RESOURCES    - No enough buffer to allocate
+  @retval EFI_BUFFER_TOO_SMALL    - DefualtImage has no enough ImageLength
+  @retval EFI_SUCCESS             - Successfully extract data from Hii database.
+**/
+;
+
+EFI_STATUS
+GetVBiosVbtExitPmAuth (
+  VOID
+  )
+/**
+  Get Intel video BIOS VBT information (i.e. Pointer to VBT and VBT size).
+  The VBT (Video BIOS Table) is a block of customizable data that is built
+  within the video BIOS and edited by customers.
+
+  @retval EFI_SUCCESS            - Video BIOS VBT information returned.
+  @exception EFI_UNSUPPORTED     - Could not find VBT information (*VBiosVbtPtr = NULL).
+**/
+;
+
+EFI_STATUS
+UpdateIgdOpRegionExitPmAuth (
+  VOID
+  )
+/**
+  Update Graphics OpRegion after PCI enumeration.
+
+  @param[in] void         - None
+  @retval EFI_SUCCESS     - The function completed successfully.
+**/
+;
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/GraphicsInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/GraphicsInit.c
new file mode 100644
index 0000000..ab08eb3
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/GraphicsInit.c
@@ -0,0 +1,929 @@
+/** @file
+  PEIM to initialize both IGD, PEG and PCI graphics card.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "GraphicsInit.h"
+#include "CpuIA32.h"
+#include <Token.h>
+#define IGD_ENABLE  1
+#define IGD_DISABLE 0
+
+/**
+  GraphicsInit: Initialize the IGD if no other external graphics is present
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+**/
+VOID
+GraphicsInit (
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI          *SaPlatformPolicyPpi
+  )
+{
+  DISPLAY_DEVICE  PrimaryDisplay;
+  UINT8           GMSData;
+  UINT32          PciMmioLength;
+  UINT32          PchPcieMmioLength;
+  UINT32          PegMmioLength;
+  UINT32          IGfxMmioLength;
+  UINT32          TotalMmioLength;
+  BOOLEAN         IGfxSupported;
+  UINT32          LoGTBaseAddress;
+  UINT32          HiGTBaseAddress;
+  UINT32          RegOffset;
+  UINT32          Data32And;
+  UINT32          Data32Or;
+  UINT32          Data32Mask;
+  UINT32          Result;
+  PEI_STALL_PPI   *StallPpi;
+  EFI_STATUS      Status;
+  UINT64          GttMmAdr;
+  UINTN           MchBarBase;
+  CPU_FAMILY      CpuFamilyId;
+
+  PciMmioLength     = 0;
+  PchPcieMmioLength = 0;
+  PegMmioLength     = 0;
+  IGfxMmioLength    = 0;
+  GttMmAdr          = 0;
+  MchBarBase        = McD0PciCfg32 (0x48) &~BIT0;
+  CpuFamilyId       = GetCpuFamily();
+
+  ///
+  /// Set the VGA Decode bits to a good known starting point where both PEG and
+  /// IGD VGA Decode Bits are both disabled.
+  ///
+  McD0PciCfg16Or (R_SA_GGC, B_SA_GGC_IVD_MASK);
+  PrimaryDisplay = IGD;
+
+  ///
+  /// Check if IGfx is supported
+  ///
+  IGfxSupported = (BOOLEAN) (McD2PciCfg16 (R_SA_IGD_VID) != 0xFFFF);
+
+  ///
+  /// Check external VGA devices
+  ///
+  CheckOffboardPcieVga (PeiServices, &PchPcieMmioLength, &PrimaryDisplay);
+  CheckAndInitializePegVga (
+    PeiServices,
+    SaPlatformPolicyPpi->PcieConfig->AlwaysEnablePeg,
+    &PrimaryDisplay,
+    SaPlatformPolicyPpi->GtConfig->PrimaryDisplay,
+    &PegMmioLength
+    );
+
+  ///
+  /// Temporarily program GttMmAdr
+  ///
+  GttMmAdr = SaPlatformPolicyPpi->GtConfig->GttMmAdr;
+  
+  ///
+  /// Program GttMmAdr 
+  ///
+  LoGTBaseAddress = (UINT32) (GttMmAdr & 0xFFFFFFFF);
+  HiGTBaseAddress = (UINT32) RShiftU64 ((GttMmAdr & 0xFFFFFFFF00000000), 32);
+  McD2PciCfg32 (R_SA_IGD_GTTMMADR)     = LoGTBaseAddress | BIT2;
+  McD2PciCfg32 (R_SA_IGD_GTTMMADR + 4) = HiGTBaseAddress;
+
+  ///
+  /// Enable Bus Master and Memory access on 0:2:0
+  ///
+  McD2PciCfg16Or (R_SA_IGD_CMD, (BIT2 | BIT1));
+
+  ///
+  /// If primary display device is IGD or no other display detected then enable IGD
+  ///
+  if (IGfxSupported &&
+      (
+        (
+        ((PrimaryDisplay == IGD) || (SaPlatformPolicyPpi->GtConfig->PrimaryDisplay == IGD)) &&
+      (SaPlatformPolicyPpi->GtConfig->InternalGraphics != IGD_DISABLE)
+    ) || (SaPlatformPolicyPpi->GtConfig->InternalGraphics == IGD_ENABLE)
+    )
+      ) {
+
+    DEBUG ((EFI_D_INFO, "IGD enabled.\n"));
+
+    ///
+    /// Program GFX Memory by setting D0.F0.R 050h [7:3]
+    ///
+    McD0PciCfg16And (R_SA_GGC, ~(B_SA_GGC_GMS_MASK));
+
+    GMSData = SaPlatformPolicyPpi->GtConfig->IgdDvmt50PreAlloc;
+
+    McD0PciCfg8Or (R_SA_GGC, (GMSData << N_SA_GGC_GMS_OFFSET));
+
+    ///
+    /// Program Graphics GTT Memory D0:F0:R50h[9:8] = 10b => 2MB of GTT
+    ///
+    McD0PciCfg16AndThenOr (R_SA_GGC, ~(B_SA_GGC_GGMS_MASK), 1 << (N_SA_GGC_GGMS_OFFSET + 1));
+
+    ///
+    /// Set register D2.F0.R 062h [2:1] = `01b' to set a 256MByte aperture.
+    /// This must be done before Device 2 registers are enumerated.
+    ///
+    if (SaPlatformPolicyPpi->GtConfig->ApertureSize == APERTURE_SIZE_128MB) {
+      McD2PciCfg8And (R_SA_IGD_MSAC_OFFSET, ~(BIT2 + BIT1));
+    } else if (SaPlatformPolicyPpi->GtConfig->ApertureSize == APERTURE_SIZE_256MB) {
+      McD2PciCfg8AndThenOr (R_SA_IGD_MSAC_OFFSET, ~(BIT2 + BIT1), BIT1);
+    } else {
+      McD2PciCfg8Or (R_SA_IGD_MSAC_OFFSET, (BIT2 + BIT1));
+    }
+    ///
+    /// Enable IGD VGA Decode.  This is needed so the Class Code will
+    /// be correct for the IGD Device when determining which device
+    /// should be primary.  If disabled, IGD will show up as a non VGA device.
+    ///
+    if ((SaPlatformPolicyPpi->GtConfig->PrimaryDisplay != IGD) && (PrimaryDisplay != IGD)) {
+      ///
+      /// If IGD is forced to be enabled, but is a secondary display, disable IGD VGA Decode
+      ///
+      McD0PciCfg16Or (R_SA_GGC, B_SA_GGC_IVD_MASK);
+      DEBUG ((EFI_D_INFO, "IGD VGA Decode is disabled because it's not a primary display.\n"));
+    } else {
+      McD0PciCfg16And (R_SA_GGC, ~(B_SA_GGC_IVD_MASK));
+    }
+
+    FindPciDeviceMmioLength (0, 2, 0, &IGfxMmioLength);
+
+    if ((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_12) && (SaPlatformPolicyPpi->GtConfig->PanelPowerEnable == 1)) {
+      ///
+      /// Panel Enable VDD bit
+      ///
+      Mmio32Or (GttMmAdr, 0xc7204, 0x8);
+    }
+  } else {
+
+    Status = (*PeiServices)->LocatePpi (PeiServices, &gPeiStallPpiGuid, 0, NULL, &StallPpi);
+    ASSERT_PEI_ERROR (PeiServices, Status);
+
+    if (CpuFamilyId == EnumCpuHswUlt) {
+      ///
+      /// For HSW ULT, Disable LCPLL
+      /// Set LCPLL_CTL (GTTMMADR Offset 0x130040) PLL_disable (Bit 31) to 1b to disable LCPLL.
+      /// Poll for LCPLL_CTL PLL_lock (Bit 30) = 0b to indicate LCPLL lost lock.Timeout and continue after 1 ms.
+      /// Set D_COMP COMP_DISABLE (MCHBAR offset 0x5F0C Bit 0)to 1b.
+      /// Wait > 100 ns for write to complete.
+      /// Poll for D_COMP RCOMP_IN_PROGRESS Bit 9 = 0b.Timeout and continue after 1 ms.
+      ///
+
+      RegOffset = 0x130040;
+      Data32And = 0xFFFFFFFF;
+      Data32Or  = BIT31;
+      Mmio32AndThenOr (GttMmAdr, RegOffset, Data32And, Data32Or);
+
+      if (IGfxSupported) {
+        Data32Mask = BIT30;
+        Result     = 0;
+        PollGtReady (PeiServices, StallPpi, GttMmAdr, RegOffset, Data32Mask, Result);
+      }
+
+      RegOffset = 0x5F0C;
+      Data32And = 0xFFFFFFFF;
+      Data32Or  = BIT0;
+      Mmio32AndThenOr (MchBarBase, RegOffset, Data32And, Data32Or);
+
+      ///
+      /// 1uSec wait
+      ///
+      StallPpi->Stall (PeiServices, StallPpi, 1);
+
+      if (IGfxSupported) {
+        Data32Mask = BIT9;
+        Result     = 0;
+        PollGtReady (PeiServices, StallPpi, MchBarBase, RegOffset, Data32Mask, Result);
+      }
+    }
+
+    DEBUG ((EFI_D_INFO, "Disable IGD Device.\n"));
+    ///
+    /// Disable IGD device
+    ///
+    /// Register D0:F0 Offset 50h [7:3] = '00000b'.
+    /// This prevents UMA memory from being pre-allocated to IGD.
+    /// Set D0:F0 Offset 50h [9:8] = '00b'.
+    /// GTT Graphics Memory Size to 0
+    /// Set VGA Disable (IVD) in D0:F0 Offset 50h [1] = '1b'.
+    ///
+    McD0PciCfg16AndThenOr (R_SA_GGC, ~(B_SA_GGC_GGMS_MASK | B_SA_GGC_GMS_MASK), B_SA_GGC_IVD_MASK);
+    SaPlatformPolicyPpi->GtConfig->GttSize            = 0;
+    SaPlatformPolicyPpi->GtConfig->IgdDvmt50PreAlloc  = 0;
+
+    ///
+    /// Disable IGD. D0.F0.R 054h [4] = '0b'.
+    ///
+    McD0PciCfg8And (R_SA_DEVEN, ~(B_SA_DEVEN_D2EN_MASK));
+  }
+
+  TotalMmioLength = PciMmioLength + PchPcieMmioLength + PegMmioLength + IGfxMmioLength;
+  DEBUG ((EFI_D_INFO, "TotalMmioLength:   0x%08X bytes\n", TotalMmioLength));
+
+  ///
+  /// Disable Bus Master and Memory access on 0:2:0 and clear GTTMMADR
+  ///
+  McD2PciCfg16And (R_SA_IGD_CMD, ~(BIT2 | BIT1));
+  McD2PciCfg32And (R_SA_IGD_GTTMMADR, 0x0);
+  McD2PciCfg32And (R_SA_IGD_GTTMMADR + 0x4, 0x0);
+
+  ///
+  /// Determine MMIO Size for Dynamic Tolud
+  ///
+  if (SaPlatformPolicyPpi->MemConfig->MaxTolud == 0x00) {
+    ///
+    /// if total MMIO need 1GB or over
+    ///
+    if (TotalMmioLength >= 0x40000000) {
+      SaPlatformPolicyPpi->GtConfig->MmioSize = 0x800;
+    }
+    ///
+    /// if total MMIO need 728MB~1GB
+    ///
+    else if (TotalMmioLength >= 0x30000000) {
+      SaPlatformPolicyPpi->GtConfig->MmioSize = 0x700;
+    }
+    ///
+    /// if total MMIO need 512MB~728MB
+    ///
+    else if (TotalMmioLength >= 0x20000000) {
+      SaPlatformPolicyPpi->GtConfig->MmioSize = 0x600;
+    }
+    ///
+    /// if total MMIO need 256MB~512MB
+    ///
+    else if (TotalMmioLength >= 0x10000000) {
+      SaPlatformPolicyPpi->GtConfig->MmioSize = 0x500;
+    }
+    ///
+    /// if total MMIO need less than 256MB
+    ///
+    else {
+      SaPlatformPolicyPpi->GtConfig->MmioSize = 0x400;
+    }
+  }
+}
+
+/**
+  CheckAndInitializePegVga:  Check if PEG card is present and configure accordingly
+
+  @param[in] PeiServices             - Pointer to the PEI services table
+  @param[in] AlwaysEnablePeg         - 0 - Peg is not always enabled.
+  @param[in, out] PrimaryDisplay     - Primary Display - default is IGD
+  @param[in] PrimaryDisplaySelection - Primary display selection from BIOS Setup
+  @param[in, out] PegMmioLength      - Total PEG MMIO length on all PEG ports
+**/
+VOID
+CheckAndInitializePegVga (
+  IN     EFI_PEI_SERVICES                   **PeiServices,
+  IN     UINT8                              AlwaysEnablePeg,
+  IN OUT DISPLAY_DEVICE                     *PrimaryDisplay,
+  IN     UINT8                              PrimaryDisplaySelection,
+  IN OUT UINT32                             *PegMmioLength
+  )
+{
+  UINT8     ClassCode;
+  BOOLEAN   IgdPresent;
+  UINT8     PegBus;
+  UINT8     PegDev;
+  UINT8     PegFunc;
+  UINT16    PegDevenReg;
+  UINT8     PegComplete;
+  UINT16    PegEnable;
+  BOOLEAN   CardDetect;
+  UINT32    MmioLength;
+  UINT8     Func;
+  UINT8     MaxFunction;
+  UINT8     HeaderType;
+  UINT8     PegVgaFunc;
+
+  PEG_DEVEN PegDevenTable[] = {
+    ///
+    ///  Bus, Device, Function, DevenMask
+    ///
+    {
+      SA_PEG_BUS_NUM,
+      SA_PEG10_DEV_NUM,
+      SA_PEG10_FUN_NUM,
+      BIT3
+    }
+// AMI_OVERRID >>
+#if RC_PEG_1 == 1
+,
+    {
+      SA_PEG_BUS_NUM,
+      SA_PEG11_DEV_NUM,
+      SA_PEG11_FUN_NUM,
+      BIT2
+    }
+#endif
+#if RC_PEG_2 == 1
+,
+    {
+      SA_PEG_BUS_NUM,
+      SA_PEG12_DEV_NUM,
+      SA_PEG12_FUN_NUM,
+      BIT1
+    }
+#endif
+// AMI_OVERRID <<
+  };
+
+  MmioLength = 0;
+  CardDetect = FALSE;
+  PegVgaFunc = 0xFF;
+
+  ///
+  /// Read the DEVEN register for PEG0/1/2 controllers configuration
+  ///
+  PegDevenReg = MmPci16 (0, SA_MC_BUS, 0, 0, R_SA_DEVEN) & (BIT3 + BIT2 + BIT1);
+
+  ///
+  /// If IGD is disabled
+  /// or not present IgdPresent is set to false
+  ///
+  if (McD2PciCfg16 (PCI_VID) == 0xFFFF) {
+    IgdPresent = FALSE;
+  } else {
+    IgdPresent = TRUE;
+  }
+  ///
+  /// Scan PEG device vs DEVEN register for PEG controllers configuration
+  ///
+  for (PegComplete = 0; PegComplete < ((sizeof (PegDevenTable)) / (sizeof (PEG_DEVEN))); PegComplete++) {
+
+    PegBus    = PegDevenTable[PegComplete].Bus;
+    PegDev    = PegDevenTable[PegComplete].Device;
+    PegFunc   = PegDevenTable[PegComplete].Function;
+    PegEnable = PegDevenTable[PegComplete].DevenMask;
+
+    if ((PegDevenReg & PegEnable) == 0) {
+      continue;
+    }
+    ///
+    /// Check for a card presence in the PEG slot.
+    /// We don't know if it's a graphics card yet.
+    ///
+    if ((MmPci8 (0, PegBus, PegDev, PegFunc, R_SA_PEG_SLOTSTS_OFFSET) & BIT6) == 0) {
+      continue;
+    }
+    ///
+    /// Set PEG PortBus = 1 to Read Endpoint.
+    ///
+    MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF, 0x00010100);
+
+    ///
+    /// A config write is required in order for the device to re-capture the Bus number,
+    /// according to PCI Express Base Specification, 2.2.6.2
+    /// Write to a read-only register VendorID to not cause any side effects.
+    ///
+    MmPci16 (0, 1, 0, 0, PCI_VID) = 0;
+
+    ///
+    /// Read Vendor ID to check if endpoint exists
+    /// if no device exists, then check next device
+    ///
+    if (MmPci16 (0, 1, 0, 0, PCI_VID) == 0xFFFF) {
+      continue;
+    }
+    ///
+    /// Check for a multifunction device
+    ///
+    HeaderType = MmPci8 (0, 1, 0, 0, PCI_HEADER_TYPE_OFFSET);
+    if ((HeaderType & HEADER_TYPE_MULTI_FUNCTION) != 0) {
+      MaxFunction = 7;
+    } else {
+      MaxFunction = 0;
+    }
+    ///
+    /// Calculate total PEG MMIO length on all functions of the endpoint
+    ///
+    for (Func = 0; Func <= MaxFunction; Func++) {
+      if (MmPci16 (0, 1, 0, Func, PCI_VID) == 0xFFFF) {
+        continue;
+      }
+
+      FindPciDeviceMmioLength (1, 0, Func, &MmioLength);
+      *PegMmioLength += MmioLength;
+    }
+    ///
+    /// Perform PEG Endpoint Class Code Check.  If the Endpoint Class Code is
+    /// not GFX, then the Port is being used as a standard PCI Express Port.
+    ///
+    ClassCode = MmPci8 (0, 1, 0, 0, PCI_BCC);
+    if (ClassCode == 0x03) {
+      ///
+      /// Disable PEG if IGD or PCI VGA take precedence.
+      ///
+      if (AlwaysEnablePeg == 0) {
+        ///
+        /// If IGD is present and selected as primary, skip the PEG VGA enabling
+        ///
+        if (IgdPresent && (PrimaryDisplaySelection == IGD)) {
+          MmPci32And (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF);
+          continue;
+        }
+        ///
+        /// If PCI video card was detected, skip the PEG VGA enabling
+        ///
+        if (*PrimaryDisplay == PCI) {
+          MmPci32And (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF);
+          continue;
+        }
+      }
+      ///
+      /// Enable PEG video and Execute 16-bit address decodes on VGA I/O accesses
+      ///
+      /// Check if PEG VGA already detected
+      ///
+      if (*PrimaryDisplay != PEG) {
+        MmPci16Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_BCTRL_OFFSET, (BIT3 + BIT4));
+        *PrimaryDisplay = PEG;
+        PegVgaFunc = PegFunc;
+        DEBUG ((EFI_D_INFO, "PCIe card on PEG%x%x (%x:%x:%x) enabled as VGA.\n", PegDev, PegFunc, PegBus, PegDev, PegFunc));
+      }
+    }
+
+    if (ClassCode == 0x06) {
+      CardDetect = EnumerateBridgeDevice (PegBus, PegDev, PegFunc, PegMmioLength);
+      if (CardDetect == TRUE) {
+        ///
+        /// Check if PEG VGA already detected
+        ///
+        if (*PrimaryDisplay != PEG) {
+          MmPci16Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_BCTRL_OFFSET, (BIT3 + BIT4));
+          *PrimaryDisplay = PEG;
+          PegVgaFunc = PegFunc;
+          DEBUG ((EFI_D_INFO, "PCIe card on PEG%x%x (%x:%x:%x) enabled as VGA.\n", PegDev, PegFunc, PegBus, PegDev, PegFunc));
+        }
+      }
+    }
+    ///
+    /// Restore bus numbers on the PEG bridge.
+    ///
+    MmPci32And (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF);
+  } // End of the for Loop
+
+  ///
+  /// If a PEG device is used for primary graphics, set the ISAEN bit on all other PEG ports.
+  ///
+  if (PegVgaFunc != 0xFF) {
+    for (PegComplete = 0; PegComplete < ((sizeof (PegDevenTable)) / (sizeof (PEG_DEVEN))); PegComplete++) {
+      if (PegVgaFunc == PegComplete) {
+        continue;
+      }
+      PegBus    = PegDevenTable[PegComplete].Bus;
+      PegDev    = PegDevenTable[PegComplete].Device;
+      PegFunc   = PegDevenTable[PegComplete].Function;
+      MmPci16Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_BCTRL_OFFSET, BIT2);
+      DEBUG ((EFI_D_INFO, "PEG%x%x (%x:%x:%x) ISAEN has been set.\n", PegDev, PegFunc, PegBus, PegDev, PegFunc));
+    }
+  }
+}
+
+/**
+  Find the MMIO size that a given PCI device requires
+
+  @param[in] BusNum     - Bus number of the device
+  @param[in] DevNum     - device Number of the device
+  @param[in] FunNum     - Function number of the device
+  @param[out] MmioLength - MMIO Length in bytes
+**/
+VOID
+FindPciDeviceMmioLength (
+  IN UINT32  BusNum,
+  IN UINT32  DevNum,
+  IN UINT32  FunNum,
+  OUT UINT32 *MmioLength
+  )
+{
+  UINT32  CurrentMmioLength;
+  UINT32  SavedBAR;
+  UINT32  i;
+  UINT64  BarAlign;
+  UINT8   ClassCode;
+
+  *MmioLength = 0;
+  BarAlign    = PCI_BAR_OLD_ALIGN;
+
+  ClassCode   = MmPci8 (0, BusNum, DevNum, FunNum, PCI_BCC);
+  if (ClassCode == PCI_CLASS_BRIDGE) {
+    return ;
+  }
+
+  for (i = PCI_BAR0; i <= PCI_BAR5; i += 4) {
+    SavedBAR = MmPci32 (0, BusNum, DevNum, FunNum, i);
+    ///
+    /// Check BAR is read-only or not
+    ///
+    MmPci32And (0, BusNum, DevNum, FunNum, i, (UINT32) PCI_BAR_NOCHANGE);
+    MmPci32Or (0, BusNum, DevNum, FunNum, i, BarAlign);
+    if (SavedBAR == MmPci32 (0, BusNum, DevNum, FunNum, i)) {
+      ///
+      /// Restore BAR as original value
+      ///
+      MmPci32 (0, BusNum, DevNum, FunNum, i) = SavedBAR;
+      continue;
+    }
+    ///
+    /// If BAR is not memory map, skip it
+    ///
+    if ((SavedBAR & BIT0) != 0) {
+      ///
+      /// Restore BAR as original value
+      ///
+      MmPci32 (0, BusNum, DevNum, FunNum, i) = SavedBAR;
+      continue;
+    }
+    ///
+    /// Calculate the MMIO length through BAR
+    ///
+    CurrentMmioLength = ~(MmPci32 (0, BusNum, DevNum, FunNum, i) &~0xF) + 1;
+    *MmioLength += CurrentMmioLength;
+
+    ///
+    /// Restore BAR as original value
+    ///
+    MmPci32 (0, BusNum, DevNum, FunNum, i) = SavedBAR;
+    ///
+    /// Skip next index if BAR is 64bit address
+    ///
+    if ((SavedBAR & (BIT1 + BIT2)) == 0x4) {
+      i += 4;
+    }
+  }
+}
+
+/**
+  CheckOffboardPcieVga: Check if off board PCIe graphics Card is present
+
+  @param[in] PeiServices             - Pointer to the PEI services table
+  @param[in, out] PchPcieMmioLength  - Total PCIe MMIO length on all PCH root ports
+  @param[in, out] PrimaryDisplay     - Primary Display - default is IGD
+**/
+VOID
+CheckOffboardPcieVga (
+  IN     EFI_PEI_SERVICES     **PeiServices,
+  IN OUT UINT32               *PchPcieMmioLength,
+  IN OUT DISPLAY_DEVICE       *PrimaryDisplay
+  )
+{
+  UINT8   PortNo;
+  UINT32  PcieBusNum;
+  UINT8   Bus;
+  UINT8   Dev;
+  UINT8   Func;
+  UINT8   MaxFunction;
+  UINT8   SubBusNum;
+  UINT8   HeaderType;
+  UINT16  Buffer16;
+  BOOLEAN CardDetect;
+  UINT32  MmioLength;
+
+  MmioLength = 0;
+
+  ///
+  /// Initialize Secondary and Subordinate bus number for first Pcie root port
+  ///
+  PcieBusNum  = 0x00010100;
+
+  SubBusNum   = 0;
+
+  CardDetect  = FALSE;
+
+  for (PortNo = 0; PortNo < GetPchMaxPciePortNum (); PortNo++) {
+    ///
+    /// Check if root port exists
+    ///
+    if (MmPci16 (0, 0, 0x1C, PortNo, PCI_VID) == 0xFFFF) {
+      continue;
+    }
+
+    MmPci32 (0, 0, 0x1c, PortNo, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET)  = PcieBusNum;
+    Bus = MmPci8 (0, 0, 0x1c, PortNo, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET);
+
+    ///
+    /// Assign temporary subordinate bus number so that device this bridge can be seen
+    ///
+    MmPci8 (0, 0, 0x1c, PortNo, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET) = 0xFF;
+
+    ///
+    /// A config write is required in order for the device to re-capture the Bus number,
+    /// according to PCI Express Base Specification, 2.2.6.2
+    /// Write to a read-only register VendorID to not cause any side effects.
+    ///
+    MmPci16 (0, Bus, 0, 0, PCI_VID) = 0;
+
+    SubBusNum                       = EnumerateDownstream (Bus);
+    ///
+    /// Update the actual subordinate bus number
+    ///
+    MmPci8 (0, 0, 0x1c, PortNo, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET) = SubBusNum;
+    PcieBusNum = (SubBusNum + 1) << 8;
+  }
+
+  for (Bus = 1; Bus <= SubBusNum; Bus++) {
+    for (Dev = 0; Dev < 32; Dev++) {
+      ///
+      /// Read Vendor ID to check if device exists
+      /// if no device exists, then check next device
+      ///
+      if (MmPci16 (0, Bus, Dev, 0, PCI_VID) == 0xFFFF) {
+        continue;
+      }
+      ///
+      /// Check for a multifunction device
+      ///
+      HeaderType = MmPci8 (0, Bus, Dev, 0, PCI_HDR);
+      if ((HeaderType & HEADER_TYPE_MULTI_FUNCTION) != 0) {
+        MaxFunction = 7;
+      } else {
+        MaxFunction = 0;
+      }
+
+      for (Func = 0; Func <= MaxFunction; Func++) {
+        if (MmPci16 (0, Bus, Dev, Func, PCI_VID) == 0xFFFF) {
+          continue;
+        }
+
+        FindPciDeviceMmioLength (Bus, Dev, Func, &MmioLength);
+        *PchPcieMmioLength += MmioLength;
+
+        ///
+        /// Video cards can have Base Class 0 with Sub-class 1
+        /// or Base Class 3.
+        ///
+        if (MmPci16 (0, Bus, Dev, Func, PCI_SCC) == 0x0300) {
+          if (CardDetect != TRUE) {
+            *PrimaryDisplay = PCI;
+            DEBUG ((EFI_D_INFO, "PCH PCIe Graphics Card enabled.\n"));
+            CardDetect = TRUE;
+          }
+        }
+      }
+    }
+  }
+  ///
+  /// Clear bus number on all the bridges that we have opened so far.
+  /// We have to do it in the reverse Bus number order.
+  ///
+  for (Bus = SubBusNum; Bus >= 1; Bus--) {
+    for (Dev = 0; Dev < 32; Dev++) {
+      ///
+      /// Read Vendor ID to check if device exists
+      /// if no device exists, then check next device
+      ///
+      if (MmPci16 (0, Bus, Dev, 0, PCI_VID) == 0xFFFF) {
+        continue;
+      }
+
+      Buffer16 = MmPci16 (0, Bus, Dev, 0, PCI_SCC);
+      ///
+      /// Clear Bus Number for PCI/PCI Bridge Device
+      ///
+      if (Buffer16 == 0x0604) {
+        MmPci32 (0, Bus, Dev, 0, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET) = 0;
+      }
+    }
+  }
+
+  for (PortNo = 0; PortNo < GetPchMaxPciePortNum (); PortNo++) {
+    ///
+    /// Clear bus numbers so that PCIe slots are hidden
+    ///
+    MmPci32 (0, 0, 0x1c, PortNo, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET) = 0;
+  }
+}
+
+/**
+  This function enumerate all downstream bridge.
+
+  @param[in] BusNum  - Primary bus number of current bridge.
+
+  @retval BusNum: return current bus number if current bus is an enpoint device.
+  @retval SubBus: return subordinate bus number if current bus is a bridge.
+**/
+UINT8
+EnumerateDownstream (
+  IN UINT8  BusNum
+  )
+{
+  UINT8   DevNum;
+  UINT16  Buffer16;
+  UINT8   SubBus;
+  UINT8   SecBus;
+
+  SubBus  = 0;
+
+  SecBus  = BusNum;
+
+  for (DevNum = 0; DevNum < 32; DevNum++) {
+    ///
+    /// Read Vendor ID to check if device exists
+    /// if no device exists, then check next device
+    ///
+    if (MmPci16 (0, BusNum, DevNum, 0, PCI_VID) == 0xFFFF) {
+      continue;
+    }
+
+    Buffer16 = MmPci16 (0, BusNum, DevNum, 0, PCI_SCC);
+    ///
+    /// Check for PCI/PCI Bridge Device Base Class 6 with subclass 4
+    ///
+    if (Buffer16 == 0x0604) {
+      SecBus++;
+      MmPci8 (0, BusNum, DevNum, 0, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET)   = BusNum;
+      MmPci8 (0, BusNum, DevNum, 0, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET) = SecBus;
+      ///
+      /// Assign temporary subordinate bus number so that device behind this bridge can be seen
+      ///
+      MmPci8 (0, BusNum, DevNum, 0, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET) = 0xFF;
+
+      ///
+      /// A config write is required in order for the device to re-capture the Bus number,
+      /// according to PCI Express Base Specification, 2.2.6.2
+      /// Write to a read-only register VendorID to not cause any side effects.
+      ///
+      MmPci16 (0, SecBus, 0, 0, PCI_VID) = 0;
+
+      ///
+      /// Enumerate bus behind this bridge by calling this funstion recursively
+      ///
+      SubBus = EnumerateDownstream (SecBus);
+      ///
+      /// Update the correct subordinate bus number
+      ///
+      MmPci8 (0, BusNum, DevNum, 0, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET) = SubBus;
+      SecBus = SubBus;
+    }
+  }
+
+  if (SubBus == 0) {
+    return BusNum;
+  } else {
+    return SubBus;
+  }
+}
+
+/**
+  This function enumerate the bridge on the device
+
+  @param[in] PegBus  - Particular Bus number
+  @param[in] PegDev  - Particular Device number
+  @param[in] PegFunc  - Particular Func number
+  @param[in, out] PegMmioLength - PEG MMIO length
+
+  @retval CardDetect : TRUE if current bridge device has a Graphics card.
+  @retval CardDetect : FALSE if current bridge device has no Graphics card.
+**/
+BOOLEAN
+EnumerateBridgeDevice (
+  IN UINT8      PegBus,
+  IN UINT8      PegDev,
+  IN UINT8      PegFunc,
+  IN OUT UINT32 *PegMmioLength
+  )
+{
+
+  UINT8   Bus;
+  UINT8   Dev;
+  UINT8   SubBusNum;
+  UINT16  Buffer16;
+  BOOLEAN CardDetect;
+
+  CardDetect = FALSE;
+
+  ///
+  /// Temporarily program the secondary and subordinate bus numbers
+  /// of PEG bridge to (1, 0xFF) so that devices behind the bridge can be seen
+  ///
+  Bus = 1;
+  MmPci8 (0, PegBus, PegDev, PegFunc, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET)   = Bus;
+  MmPci8 (0, PegBus, PegDev, PegFunc, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET) = 0xFF;
+
+  ///
+  /// A config write is required in order for the device to re-capture the Bus number,
+  /// according to PCI Express Base Specification, 2.2.6.2
+  /// Write to a read-only register VendorID to not cause any side effects.
+  ///
+  MmPci16 (0, Bus, 0, 0, PCI_VID) = 0;
+
+  SubBusNum                       = EnumerateDownstream (Bus);
+
+  for (Bus = 1; Bus <= SubBusNum; Bus++) {
+    for (Dev = 0; Dev < 32; Dev++) {
+      ///
+      /// Read Vendor ID to check if device exists
+      /// if no device exists, then check next device
+      ///
+      if (MmPci16 (0, Bus, Dev, 0, PCI_VID) == 0xFFFF) {
+        continue;
+
+      }
+      ///
+      /// Video cards can have Base Class 0 with Sub-class 1
+      /// or Base Class 3.
+      ///
+      if (MmPci16 (0, Bus, Dev, 0, PCI_SCC) == 0x0300) {
+        FindPciDeviceMmioLength (Bus, Dev, 0, PegMmioLength);
+        CardDetect = TRUE;
+        break;
+      }
+    }
+
+    if (CardDetect == TRUE) {
+      break;
+    }
+  }
+  ///
+  /// Clear bus number on all the bridges that we have opened so far.
+  /// We have to do it in the reverse Bus number order.
+  ///
+  for (Bus = SubBusNum; Bus >= 1; Bus--) {
+    for (Dev = 0; Dev < 32; Dev++) {
+      ///
+      /// Read Vendor ID to check if device exists
+      /// if no device exists, then check next device
+      ///
+      if (MmPci16 (0, Bus, Dev, 0, PCI_VID) == 0xFFFF) {
+        continue;
+      }
+
+      Buffer16 = MmPci16 (0, Bus, Dev, 0, PCI_SCC);
+      ///
+      /// Clear Bus Number for PCI/PCI Bridge Device
+      ///
+      if (Buffer16 == 0x0604) {
+        MmPci32 (0, Bus, Dev, 0, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET) = 0;
+      }
+    }
+  }
+  ///
+  /// Clear the bus numbers on the PEG bridge
+  ///
+  MmPci32 (0, PegBus, PegDev, PegFunc, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET) = 0;
+
+  return CardDetect;
+}
+
+/**
+
+  "Poll Status" for GT Readiness
+
+  @param[in] PeiServices     - Pointer to the PEI services table
+  @param[in] StallPpi        - Pointer to Stall PPI
+  @param[in] Base            - Base address of MMIO
+  @param[in] Offset          - MMIO Offset
+  @param[in] Mask            - Mask
+  @param[in] Result          - Value to wait for
+
+
+**/
+VOID
+PollGtReady (
+  IN EFI_PEI_SERVICES     **PeiServices,
+  IN PEI_STALL_PPI        *StallPpi,
+  UINT64 Base,
+  UINT32 Offset,
+  UINT32 Mask,
+  UINT32 Result
+  )
+{
+  UINT32  GtStatus;
+  UINT16  StallCount;
+
+  StallCount = 0;
+
+  ///
+  /// Register read
+  ///
+  GtStatus = Mmio32 (Base, Offset);
+
+  while (((GtStatus & Mask) != Result) && (StallCount < GT_WAIT_TIMEOUT)) {
+
+    GtStatus = Mmio32 (Base, Offset);
+    ///
+    /// 1mSec wait
+    ///
+    StallPpi->Stall (PeiServices, StallPpi, 1000);
+    StallCount = StallCount + 1;
+  }
+
+  ASSERT ((StallCount != GT_WAIT_TIMEOUT));
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/GraphicsInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/GraphicsInit.h
new file mode 100644
index 0000000..6d060cb
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/GraphicsInit.h
@@ -0,0 +1,219 @@
+/** @file
+  Graphics header file
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _GRAPHICS_INIT_H_
+#define _GRAPHICS_INIT_H_
+
+#include "EdkIIGluePeim.h"
+#include "SaAccess.h"
+#include "PchAccess.h"
+#include "EdkIIGluePcdPciExpressLib.h"
+#include "EdkIIGlueConfig.h"
+#include "Pci22.h"
+#include "PchPlatformLib.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+
+///
+/// Driver Consumed PPI Prototypes
+///
+#include EFI_PPI_DEPENDENCY (SaPlatformPolicy)
+
+typedef struct {
+  UINT8   Bus;
+  UINT8   Device;
+  UINT8   Function;
+  UINT16  DevenMask;
+} PEG_DEVEN;
+
+typedef enum {
+  IGD               = 0,
+  PEG,
+  PCI,
+  DISPLAY_DEVICE_MAX
+} DISPLAY_DEVICE;
+
+typedef enum {
+  VBIOS_DEFAULT     = 0,
+  CRT,
+  LFP,
+  CRT_LFP,
+  TV,
+  LFPSDVO,
+  EFP,
+  TVSDVO,
+  CRT_LFPSDVO,
+  CRT_EFP,
+  IGD_BOOT_TYPE_MAX
+} IGD_BOOT_TYPE;
+
+typedef enum {
+  GMS_FIXED         = 0,
+  GMS_DVMT,
+  GMS_FIXED_DVMT,
+  GMS_MAX
+} GRAPHICS_MEMORY_SELECTION;
+
+typedef enum {
+  GM_32M            = 1,
+  GM_64M            = 2,
+  GM_128M           = 4,
+  GM_MAX
+} STOLEN_MEMORY;
+
+typedef enum {
+  PAVP_DISABLED     = 0,
+  PAVP_LITE,
+  PAVP_HIGH
+} PAVP_MODE;
+
+#define GTTMMADR_SIZE_4MB   0x400000
+#define GTT_SIZE_2MB        2
+#define GT_WAIT_TIMEOUT     3000     ///< ~3 seconds
+
+#define APERTURE_SIZE_128MB 1
+#define APERTURE_SIZE_256MB 2
+#define APERTURE_SIZE_512MB 3
+
+/**
+  CheckAndInitializePegVga:  Check if PEG card is present and configure accordingly
+
+  @param[in] PeiServices             - Pointer to the PEI services table
+  @param[in] AlwaysEnablePeg         - 0 - Peg is not always enabled.
+  @param[in, out] PrimaryDisplay     - Primary Display - default is IGD
+  @param[in] PrimaryDisplaySelection - Primary display selection from BIOS Setup
+  @param[in, out] PegMmioLength      - Total PEG MMIO length on all PEG ports
+**/
+VOID
+CheckAndInitializePegVga (
+  IN     EFI_PEI_SERVICES                   **PeiServices,
+  IN     UINT8                              AlwaysEnablePeg,
+  IN OUT DISPLAY_DEVICE                     *PrimaryDisplay,
+  IN     UINT8                              PrimaryDisplaySelection,
+  IN OUT UINT32                             *PegMmioLength
+  )
+;
+
+/**
+  CheckOffboardPciVga: Check if off board PCI graphics Card is present
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in, out] PciMmioLength   - PCI MMIO length
+  @param[in, out] PrimaryDisplay  - Primary Display - default is IGD
+**/
+VOID
+CheckOffboardPciVga (
+  IN     EFI_PEI_SERVICES                   **PeiServices,
+  IN OUT UINT32                             *PciMmioLength,
+  IN OUT DISPLAY_DEVICE                     *PrimaryDisplay
+  )
+;
+
+/**
+  CheckOffboardPcieVga: Check if off board PCIe graphics Card is present
+
+  @param[in] PeiServices             - Pointer to the PEI services table
+  @param[in, out] PchPcieMmioLength  - Total PCIe MMIO length on all PCH root ports
+  @param[in, out] PrimaryDisplay     - Primary Display - default is IGD
+**/
+VOID
+CheckOffboardPcieVga (
+  IN     EFI_PEI_SERVICES     **PeiServices,
+  IN OUT UINT32               *PchPcieMmioLength,
+  IN OUT DISPLAY_DEVICE       *PrimaryDisplay
+  )
+;
+
+/**
+  Find the MMIO size that a given PCI device requires
+
+  @param[in] BusNum      - Bus number of the device
+  @param[in] DevNum      - device Number of the device
+  @param[in] FunNum      - Function number of the device
+  @param[out] MmioLength - MMIO Length in bytes
+**/
+VOID
+FindPciDeviceMmioLength (
+  IN UINT32  BusNum,
+  IN UINT32  DevNum,
+  IN UINT32  FunNum,
+  OUT UINT32 *MmioLength
+  )
+;
+
+/**
+  This function enumerate all downstream bridge.
+
+  @param[in] BusNum  - Primary bus number of current bridge
+
+  @retval Current bus number: if current bus is an enpoint device
+  @retval subordinate bus number: if current bus is a bridge
+**/
+UINT8
+EnumerateDownstream (
+  IN UINT8 BusNum
+  )
+;
+
+/**
+  This function enumerate the bridge on the device
+
+  @param[in] PegBus             - Particular Bus number
+  @param[in] PegDev             - Particular Device number
+  @param[in] PegFunc            - Particular Func number
+  @param[in, out] PegMmioLength - PEG MMIO length
+
+  @retval CardDetect : TRUE if current bridge device has a Graphics card.
+  @retval CardDetect : FALSE if current bridge device has no Graphics card.
+**/
+BOOLEAN
+EnumerateBridgeDevice (
+  IN UINT8                                  PegBus,
+  IN UINT8                                  PegDev,
+  IN UINT8                                  PegFunc,
+  IN OUT UINT32                             *PegMmioLength
+  )
+;
+
+/**
+
+  "Poll Status" for GT Readiness
+
+  @param[in] PeiServices     - Pointer to the PEI services table
+  @param[in] StallPpi        - Pointer to Stall PPI
+  @param[in] Base            - Base address of MMIO
+  @param[in] Offset          - MMIO Offset
+  @param[in] Mask            - Mask
+  @param[in] Result          - Value to wait for
+
+
+**/
+VOID
+PollGtReady (
+  IN EFI_PEI_SERVICES     **PeiServices,
+  IN PEI_STALL_PPI        *StallPpi,
+  UINT64 Base,
+  UINT32 Offset,
+  UINT32 Mask,
+  UINT32 Result
+  )
+;
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PciExpressInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PciExpressInit.c
new file mode 100644
index 0000000..bead6ce
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PciExpressInit.c
@@ -0,0 +1,2987 @@
+/** @file
+  PEI Function to initialize SA PciExpress.
+
+@copyright
+  Copyright (c) 1999 - 2014 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+
+#include "PciExpressInit.h"
+#include "PcieTraining.h"
+#include "PchPlatformLib.h"
+
+#include <Token.h> //AMI_OVERRIDE
+
+#ifdef PEG_FLAG
+#include EFI_GUID_DEFINITION (SaDataHob)
+
+#define SA_PEG_VMARGIN_UP   0
+#define SA_PEG_VMARGIN_DOWN 1
+
+#define SA_PEG_LINK_DISABLE_MAXWAIT 100*10
+
+VOID
+ReportPcieLinkStatus (
+  IN UINT8             PegBus,
+  IN UINT8             PegDev,
+  IN UINT8             PegFunc
+)
+/**
+ This function reports a PEG controller's link status
+
+  @param[in] PegBus       - Peg Bus
+  @param[in] PegDev       - Peg Device
+  @param[in] PegFunc      - Peg Function
+
+  @retval None
+**/
+{
+  UINT32 Deven;
+  UINT16 LinkStatus;
+  UINT8  LinkWidth;
+  UINT8  LinkSpeed;
+  UINT16 Vc0Pending;
+
+  Deven  = MmPci32 (0, SA_MC_BUS, SA_MC_DEV, SA_MC_FUN, R_SA_DEVEN);
+  Deven = (Deven >> 1) & 0x7;
+  DEBUG ((EFI_D_INFO, "PEG%x%x (%x:%x:%x) - ", PegDev, PegFunc, PegBus, PegDev, PegFunc));
+  if (((Deven >> (SA_PEG_MAX_FUN - 1 - PegFunc)) & 0x1) == 0x1) {
+    LinkStatus = MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS_OFFSET);
+    LinkWidth  = (LinkStatus >> 4) & 0x3F;
+    LinkSpeed  = LinkStatus & 0xF;
+    Vc0Pending = ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0RSTS_OFFSET)) >> 1) & 0x1;
+    DEBUG ((EFI_D_INFO, "Trained to x%d at Gen%d.", LinkWidth, LinkSpeed));
+    DEBUG ((EFI_D_INFO, " VC0 Negotiation Pending = %d.", Vc0Pending));
+    DEBUG ((EFI_D_INFO, "\n"));
+  } else {
+    DEBUG ((EFI_D_INFO, "Disabled.\n"));
+  }
+}
+
+VOID
+WaitForVc0Negotiation (
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN PEI_STALL_PPI     *StallPpi,
+  IN UINT8             PegBus,
+  IN UINT8             PegDev,
+  IN UINT8             PegFunc
+)
+/**
+  This function prints the time required for VC0 Negotiation Pending to be cleared. Quits after 100 msec.
+
+  @param[in] PeiServices  - Pointer to the PEI services table
+  @param[in] StallPpi     - Pointer to PEI_STALL_PPI
+  @param[in] PegBus       - Peg Bus
+  @param[in] PegDev       - Peg Device
+  @param[in] PegFunc      - Peg Function
+
+  @retval None
+**/
+{
+  UINT32 Deven;
+  UINT32 MsecWait;
+  UINT16 Vc0Pending;
+
+  Deven  = MmPci32 (0, SA_MC_BUS, SA_MC_DEV, SA_MC_FUN, R_SA_DEVEN);
+  Deven = (Deven >> 1) & 0x7;
+  if (((Deven >> (SA_PEG_MAX_FUN - 1 - PegFunc)) & 0x1) == 0x1) {
+    MsecWait = 0;
+    Vc0Pending = ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0RSTS_OFFSET)) >> 1) & 0x1;
+
+    while (Vc0Pending && (MsecWait < 100)) {
+      MsecWait++;
+      StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MILLI_SECOND);
+      Vc0Pending = ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0RSTS_OFFSET)) >> 1) & 0x1;
+    }
+    DEBUG ((EFI_D_INFO, "PEG%x%x (%x:%x:%x) - VC0 Negotiation Pending = %x after %d msec.\n",
+      PegDev, PegFunc, PegBus, PegDev, PegFunc, Vc0Pending, MsecWait));
+  }
+}
+
+VOID
+GracefulLinkStatusStall (
+  IN EFI_PEI_SERVICES                   **PeiServices,
+  IN PEI_STALL_PPI                      *StallPpi,
+  IN SA_PLATFORM_POLICY_PPI             *SaPlatformPolicyPpi,
+  IN SA_DATA_HOB                        *SaDataHob,
+  IN UINT8                              HwStrap
+  )
+/**
+  If we have a record of links that did not train from last boot, then do not stall for them
+  For links that did train last boot, stall until they train or 100ms pass
+
+  @param[in] PeiServices         - Pointer to the PEI services table
+  @param[in] StallPpi            - Pointer to PEI_STALL_PPI
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] SaDataHob           - Pointer to SA_DATA_HOB
+  @param[in] HwStrap             - HwStrap configuration from FUSESCMN
+
+  @retval None
+**/
+{
+  UINT8          LinkStatusGood;
+  UINT8          PegLinkFailMask;
+  UINT8          i;
+  UINT8          PegDev;
+  UINT8          PegFunc;
+  UINT8          CurrentPegFuncBit;
+  UINT8          SkipFuncMask;
+  EFI_STATUS     Status;
+  EFI_BOOT_MODE  BootMode;
+
+  PegLinkFailMask = 0;
+  ///
+  /// If this is S3 Resume or Warm Boot, check if PEG delay can be skipped when no PEG devices populated.
+  ///
+  Status = (*PeiServices)->GetBootMode (PeiServices, &BootMode);
+  ASSERT_EFI_ERROR (Status);
+  if ((BootMode == BOOT_ON_S3_RESUME) || (GetPchPmStatus (WarmBoot) == TRUE)) {
+    if ((SaDataHob != NULL) && (SaDataHob->PegDataValid)) {
+      if (SaDataHob->PegData.PegLinkFailMask != 0) {
+        PegLinkFailMask = SaDataHob->PegData.PegLinkFailMask;
+        DEBUG ((EFI_D_INFO, "Previous Link Training Fail Mask 0x%2.2X\n", SaDataHob->PegData.PegLinkFailMask));
+      } else {
+        PegLinkFailMask = 0xFF;
+        for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+          PegLinkFailMask &= (UINT8) ~(0x1 << i);
+        }
+      }
+    }
+  }
+  DEBUG ((EFI_D_INFO, "New Link Training Fail Mask 0x%2.2X\n", PegLinkFailMask));
+
+  switch (HwStrap) {
+    case SA_PEG_x16_x0_x0:
+      SkipFuncMask = BIT2 | BIT1;
+      break;
+
+    case SA_PEG_x8_x8_x0:
+      SkipFuncMask = BIT2;
+      break;
+
+    default:
+    case SA_PEG_x8_x4_x4:
+      SkipFuncMask = 0;
+      break;
+  }
+
+  LinkStatusGood = 0;
+  PegDev = 1;
+  for (i = 0; i < 100; i++) {
+    ///
+    /// Poll endpoints
+    /// Skip endpoints that failed last boot
+    /// Skip endpoints that trained good this boot
+    /// Loop in 1-ms increments until all endpoints are:
+    ///  reported bad from last boot, or
+    ///  have trained good on this boot
+    ///
+    for (PegFunc = 0, CurrentPegFuncBit = 1; PegFunc < SA_PEG_MAX_FUN; PegFunc++, CurrentPegFuncBit <<= 1) {
+      if (CurrentPegFuncBit == (CurrentPegFuncBit & SkipFuncMask)) {
+        if (i == 0) {
+          DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - skipping due to furcation\n", PegDev, PegFunc, SA_PEG_BUS_NUM, PegDev, PegFunc));
+        }
+        continue;
+      }
+      ///
+      /// If configuration change, then break out of loop.
+      /// Test if current PEG is neither in fail mask nor previously tested good.
+      ///
+      if (CurrentPegFuncBit != (CurrentPegFuncBit & (UINT8) (PegLinkFailMask | LinkStatusGood))) {
+        DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - checking\n", PegDev, PegFunc, SA_PEG_BUS_NUM, PegDev, PegFunc));
+        ///
+        /// VC negotiation is complete
+        ///
+        if ((BIT1 & MmPci16(0, SA_PEG_BUS_NUM, PegDev, PegFunc, R_SA_PEG_VC0RSTS_OFFSET)) != BIT1) {
+          DEBUG ((EFI_D_INFO, "  VC negotiation is complete\n"));
+          ///
+          /// Record as a good link
+          ///
+          LinkStatusGood |= CurrentPegFuncBit;
+        }
+      }
+    }
+    ///
+    /// If all links accounted for, then exit.
+    ///
+    if (((UINT8) (LinkStatusGood | PegLinkFailMask | SkipFuncMask)) == 0xFF) {
+      break;
+    }
+    StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MILLI_SECOND);
+  } ///< End of stall loop
+
+  DEBUG ((EFI_D_INFO, "Total Stall: %d msec\n", i));
+  if (SaDataHob != NULL) {
+    SaDataHob->PegData.PegLinkFailMask = (UINT8) ~LinkStatusGood;
+    DEBUG ((EFI_D_INFO, "Returned PegLinkFailMask 0x%2.2X\n", SaDataHob->PegData.PegLinkFailMask));
+  }
+
+}
+
+UINT32
+PcieFindCapId (
+  IN UINT8   Bus,
+  IN UINT8   Device,
+  IN UINT8   PegFunc,
+  IN UINT8   CapId
+  )
+/**
+  Find the Offset to a given Capabilities ID
+  CAPID list:
+    0x01 = PCI Power Management Interface
+    0x04 = Slot Identification
+    0x05 = MSI Capability
+    0x10 = PCI Express Capability
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+  @param[in] PegFunc   -   Pci Function Number
+  @param[in] CapId     -   CAPID to search for
+
+  @retval 0       - CAPID not found
+  @retval Other   - CAPID found, Offset of desired CAPID
+**/
+{
+  UINT8 CapHeader;
+
+  ///
+  /// Always start at Offset 0x34
+  ///
+  CapHeader = MmPci8 (0, Bus, Device, PegFunc, PCI_CAPBILITY_POINTER_OFFSET);
+  if (CapHeader == 0xFF) {
+    return 0;
+  }
+
+  while (CapHeader != 0) {
+    ///
+    /// Bottom 2 bits of the pointers are reserved per PCI Local Bus Spec 2.2
+    ///
+    CapHeader &= ~(BIT1 | BIT0);
+    ///
+    /// Search for desired CapID
+    ///
+    if (MmPci8 (0, Bus, Device, PegFunc, CapHeader) == CapId) {
+      return CapHeader;
+    }
+
+    CapHeader = MmPci8 (0, Bus, Device, PegFunc, CapHeader + 1);
+  }
+
+  return 0;
+}
+
+
+VOID
+SetLoadBus (
+  IN UINT32 DmiBar,
+  IN UINTN  Dev,
+  IN UINTN  Lane,
+  IN UINT32 LoadSel,
+  IN UINT32 LoadData,
+  IN UINT8  CpuSteppingId
+  )
+/**
+  Set Load Bus
+
+  @param[in] DmiBar        - DMIBAR address
+  @param[in] Dev           - Device Number
+  @param[in] Lane          - Number of Lane
+  @param[in] LoadSel       - Load selection value
+  @param[in] LoadData      - Load Data
+  @param[in] CpuSteppingId - CPUID.1.EAX[3:0], CPU stepping ID
+
+  @retval None
+**/
+{
+  UINT32  lbcvalue;
+  UINT32  lbcdata;
+  UINT32  lbclnsel;
+  UINT32  lbcldsel;
+  UINT32  lbcaddr;
+
+  lbcaddr   = R_SA_PEG_LOADBUSCTL0_OFFSET + BUNDLE_STEP * (Lane >> 1);
+  lbcvalue  = 0x70000000;
+
+  lbclnsel  = ((Lane & 1) == 0) ? 0x80000 : 0x100000;
+  lbcldsel  = (LoadSel & 0x3f) << 21;
+  lbcdata   = (LoadData << 1) & 0x7FFF;
+
+  lbcvalue  = lbcvalue | lbclnsel | lbcldsel | lbcdata;
+
+  if (Dev == 0) {
+    Mmio32 (DmiBar, lbcaddr) = lbcvalue;
+  } else {
+    MmPci32 (0, SA_PEG_BUS_NUM, Dev, SA_PEG10_FUN_NUM, lbcaddr) = lbcvalue;
+  }
+
+  return;
+}
+
+UINT32
+GetMonBus (
+  IN UINT32 DmiBar,
+  IN UINTN  Dev,
+  IN UINTN  Lane,
+  IN UINT32 LoadSel,
+  IN UINT8  CpuSteppingId
+  )
+/**
+  Get monitor bus from the lane selected
+
+  @param[in] DmiBar        - DMIBAR address
+  @param[in] Dev           - Device number
+  @param[in] Lane          - Number of Lane
+  @param[in] LoadSel       - Load selection value
+  @param[in] LoadData      - Load selecttion data
+  @param[in] CpuSteppingId - CPUID.1.EAX[3:0], CPU stepping ID
+
+  @retval UINT32 - Load bus address
+**/
+{
+  UINT32  monvalue;
+  UINT32  prevalue;
+  UINT32  lbclnsel;
+  UINT32  lbcldsel;
+  UINT32  lbcaddr;
+  UINT32  Result;
+
+  lbcaddr   = R_SA_PEG_LOADBUSCTL0_OFFSET + BUNDLE_STEP * (Lane >> 1);
+  monvalue  = 0x068008000;
+
+  if (Dev == 0) {
+    prevalue = Mmio32 (DmiBar, lbcaddr);
+  } else {
+    prevalue = MmPci32 (0, SA_PEG_BUS_NUM, Dev, SA_PEG10_FUN_NUM, lbcaddr);
+  }
+  monvalue  = monvalue | (prevalue & 0x7FFE);
+
+  lbclnsel  = ((Lane & 1) == 0) ? 0x80000 : 0x100000;
+  lbcldsel  = (LoadSel & 0x3f) << 21;
+  monvalue  = monvalue | lbclnsel | lbcldsel;
+
+  if (Dev == 0) {
+    Mmio32 (DmiBar, lbcaddr)  = monvalue;
+    Result                    = Mmio32 (DmiBar, lbcaddr);
+  } else {
+    MmPci32 (0, SA_PEG_BUS_NUM, Dev, SA_PEG10_FUN_NUM, lbcaddr) = monvalue;
+    Result = MmPci32 (0, SA_PEG_BUS_NUM, Dev, SA_PEG10_FUN_NUM, lbcaddr);
+  }
+
+  return (Result >> 1) & 0x3FFF;
+}
+
+VOID
+ProgramPreset (
+  IN UINT8  Direction,
+  IN UINT8  PresetValue,
+  IN UINT8  PegFunc,
+  IN UINT8  Lane
+  )
+/**
+  Program PEG Gen3 preset value
+
+  @param[in] Direction     - 0 = Root Port, 1 = End Point
+  @param[in] PresetValue   - Preset value to program
+  @param[in] PegFunc       - Peg function number to be configured
+  @param[in] Lane          - Lane to be configured
+
+  @retval None
+**/
+{
+  UINT32  Data32Or;
+  UINT32  Data32And;
+  UINT8   OriginalLane;
+
+  OriginalLane = Lane;
+  switch (PegFunc) {
+    case 1:
+      if (Lane < 8) {
+        DEBUG ((EFI_D_WARN, "Invalid input to ProgramPreset() function!  PegFunc=%d, Lane=%d\n", PegFunc, Lane));
+        return;
+      } else {
+        Lane -= 8;
+      }
+      break;
+    case 2:
+      if (Lane < 12) {
+        DEBUG ((EFI_D_WARN, "Invalid input to ProgramPreset() function!  PegFunc=%d, Lane=%d\n", PegFunc, Lane));
+        return;
+      } else {
+        Lane -= 12;
+      }
+      break;
+    default:
+      break;
+  }
+  ///
+  /// RP preset goes to bits [3:0]  for even lane and [19:16] for odd lane
+  /// EP preset goes to bits [11:8] for even lane and [27:24] for odd lane
+  ///
+  if (Direction != 0) {
+    if ((Lane % 2) == 0) {
+      Data32And = 0xFFFFF0FF;
+      Data32Or  = PresetValue << 8;
+    } else {
+      Data32And = 0xF0FFFFFF;
+      Data32Or  = PresetValue << 24;
+    }
+  } else {
+    if (OriginalLane >= 8) {
+      if ((Lane % 2) == 0) {
+        Data32And = 0xFFF0FFFF;
+        Data32Or  = PresetValue << 16;
+      } else {
+            Data32And = 0xFFFFFFF0;
+            Data32Or  = PresetValue;
+      }
+    } else {
+      if ((Lane % 2) == 0) {
+        Data32And = 0xFFFFFFF0;
+        Data32Or  = PresetValue;
+      } else {
+        Data32And = 0xFFF0FFFF;
+        Data32Or  = PresetValue << 16;
+      }
+    }
+  }
+
+  MmPci32AndThenOr (0, SA_PEG_BUS_NUM, SA_PEG_DEV_NUM, PegFunc, R_SA_PEG_EQCTL0_1_OFFSET + (Lane / 2) * 4, Data32And, Data32Or);
+
+  return;
+}
+
+VOID
+ProgramPresetPerLane (
+  IN UINT8  *RootPortPreset,
+  IN UINT8  *EndPointPreset,
+  IN UINT8  *EndPointHint,
+  IN UINTN  HwStrap
+  )
+/**
+  Program PEG Gen3 preset values per lane
+
+  @param[in] RootPortPreset  - Array of root port preset values to program
+  @param[in] EndPointPreset  - Array of end point preset values to program
+  @param[in] EndPointHint    - Array of end point hint value to program
+  @param[in] HwStrap         - HwStrap configuration from FUSESCMN
+
+  @retval None
+**/
+{
+  UINTN i;
+
+  for (i = 0; i < SA_PEG_MAX_LANE; i++) {
+    if (RootPortPreset[i] > 9) {
+      RootPortPreset[i] = 8;
+    }
+    if (EndPointPreset[i] > 9) {
+      EndPointPreset[i] = 7;
+    }
+    if (EndPointHint[i] > 6) {
+      EndPointHint[i] = 2;
+    }
+  }
+
+  ///
+  /// RP preset goes to bits [3:0]   and [19:16]
+  /// EP preset goes to bits [11:8]  and [27:24]
+  /// EP hint   goes to bits [14:12] and [30:28]
+  ///
+  switch (HwStrap) {
+    case SA_PEG_x16_x0_x0:
+      ///
+      /// PEG10 x16
+      ///
+
+      for (i = 0; i < 4; ++i) {
+        McD1PciCfg32AndThenOr (R_SA_PEG_EQCTL0_1_OFFSET + i * 4, 0x80F080F0,
+          RootPortPreset[2 * i] | (EndPointPreset[2 * i] << 8) | (EndPointHint[2 * i] << 12) | (RootPortPreset[2 * i + 1] << 16) | (EndPointPreset[2 * i + 1] << 24) | (EndPointHint[2 * i + 1] << 28));
+      }
+      for (i = 4; i < 8; ++i) {
+        McD1PciCfg32AndThenOr (R_SA_PEG_EQCTL0_1_OFFSET + i * 4, 0x80F080F0,
+          RootPortPreset[2 * i + 1] | (EndPointPreset[2 * i] << 8) | (EndPointHint[2 * i] << 12) | (RootPortPreset[2 * i] << 16) | (EndPointPreset[2 * i + 1] << 24) | (EndPointHint[2 * i + 1] << 28));
+      }
+      break;
+
+    case SA_PEG_x8_x8_x0:
+      ///
+      /// PEG10 x8 / PEG11 x8
+      ///
+      for (i = 0; i < 4; ++i) {
+        McD1PciCfg32AndThenOr (R_SA_PEG_EQCTL0_1_OFFSET + i * 4, 0x80F080F0,
+          RootPortPreset[2 * i] | (EndPointPreset[2 * i] << 8) | (EndPointHint[2 * i] << 12) | (RootPortPreset[2 * i + 1] << 16) | (EndPointPreset[2 * i + 1] << 24) | (EndPointHint[2 * i + 1] << 28));
+        McD1F1PciCfg32AndThenOr (R_SA_PEG_EQCTL0_1_OFFSET + i * 4, 0x80F080F0,
+          RootPortPreset[8 + 2 * i + 1] | (EndPointPreset[8 + 2 * i] << 8) | (EndPointHint[8 + 2 * i] << 12) | (RootPortPreset[8 + 2 * i] << 16) | (EndPointPreset[8 + 2 * i + 1] << 24) | (EndPointHint[8 + 2 * i + 1] << 28));
+      }
+      break;
+
+    case SA_PEG_x8_x4_x4:
+      ///
+      /// PEG10 x8 / PEG11 x4 / PEG12 x4
+      ///
+      for (i = 0; i < 4; ++i) {
+        McD1PciCfg32AndThenOr (R_SA_PEG_EQCTL0_1_OFFSET + i * 4, 0x80F080F0,
+          RootPortPreset[2 * i] | (EndPointPreset[2 * i] << 8) | (EndPointHint[2 * i] << 12) | (RootPortPreset[2 * i + 1] << 16) | (EndPointPreset[2 * i + 1] << 24) | (EndPointHint[2 * i + 1] << 28));
+      }
+      for (i = 0; i < 2; ++i) {
+        McD1F1PciCfg32AndThenOr (R_SA_PEG_EQCTL0_1_OFFSET + i * 4, 0x80F080F0,
+          RootPortPreset[8 + 2 * i + 1] | (EndPointPreset[8 + 2 * i] << 8) | (EndPointHint[8 + 2 * i] << 12) | (RootPortPreset[8 + 2 * i] << 16) | (EndPointPreset[8 + 2 * i + 1] << 24) | (EndPointHint[8 + 2 * i + 1] << 28));
+        McD1F2PciCfg32AndThenOr (R_SA_PEG_EQCTL0_1_OFFSET + i * 4, 0x80F080F0,
+          RootPortPreset[12 + 2 * i + 1] | (EndPointPreset[12 + 2 * i] << 8) | (EndPointHint[12 + 2 * i] << 12) | (RootPortPreset[12 + 2 * i] << 16) | (EndPointPreset[12 + 2 * i + 1] << 24) | (EndPointHint[12 + 2 * i + 1] << 28));
+      }
+      break;
+
+    default:
+      break;
+  }
+
+  return;
+}
+
+VOID
+PegGen3Equalization (
+  IN SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi,
+  IN UINT8                   CpuSteppingId,
+  IN UINTN                   HwStrap
+  )
+/**
+  Perform PEG Gen3 Equalization steps
+
+  @param[in] SaPlatformPolicyPpi - pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] CpuSteppingId       - CPUID.1.EAX[3:0], CPU stepping ID
+  @param[in] HwStrap             - HwStrap configuration from FUSESCMN
+
+  @retval None
+**/
+{
+  UINT8               *RootPortPreset;
+  UINT8               *EndPointPreset;
+  UINT8               *EndPointHint;
+
+  ///
+  /// Apply static presets for root port and endpoint
+  ///
+  RootPortPreset = SaPlatformPolicyPpi->PcieConfig->Gen3RootPortPreset;
+  EndPointPreset = SaPlatformPolicyPpi->PcieConfig->Gen3EndPointPreset;
+  EndPointHint   = SaPlatformPolicyPpi->PcieConfig->Gen3EndPointHint;
+
+  ProgramPresetPerLane (RootPortPreset, EndPointPreset, EndPointHint, HwStrap);
+
+  return;
+}
+
+#endif // PEG_FLAG
+
+#if defined(DMI_FLAG) || defined(PEG_FLAG)
+VOID
+PegDmiRecipe (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN UINT32                 MchBar,
+  IN UINT32                 DmiBar,
+  IN UINTN                  Dev,
+  IN UINT8                  SwingControl
+  )
+/**
+  Perform PEG/DMI PCIe Recipe steps
+
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] MchBar              - MCHBAR or zero if called for PEG
+  @param[in] DmiBar              - DMIBAR or zero if called for PEG
+  @param[in] Dev                 - PEG device number: 1 for PEG10, 0 if called for DMI.
+  @param[in] SwingControl        - 1 = Half, 2 = Full
+
+  @retval None
+**/
+{
+  UINTN             i;
+  UINTN             BundlesCount;
+  UINTN             LanesCount;
+  UINT32            Data32;
+  UINT32            Data32And;
+  UINT32            Data32Or;
+  UINT32            VcuAddress;
+  UINT8             VcuReadOp;
+  UINT8             VcuWriteOp;
+  CPU_FAMILY        CpuFamilyId;
+  CPU_STEPPING      CpuSteppingId;
+  BOOLEAN           IsSingleCall;
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  ///
+  /// With both DMI_FLAG and PEG_FLAG defined, this function is called twice: once for DMI and once for PEG.
+  /// Some registers are shared between DMI and PEG, so they are only programmed during one of the calls.
+  /// If only one flag is defined, this function will only be called once, so the shared registers need to be
+  /// programmed during the first/only call.
+  ///
+  IsSingleCall = TRUE;
+#if defined(DMI_FLAG) && defined(PEG_FLAG)
+  IsSingleCall = FALSE;
+#endif
+
+  if (Dev == SA_PEG_DEV_NUM) {
+    ///
+    /// PEG10
+    ///
+    LanesCount = SA_PEG_MAX_LANE;
+  } else {
+    ///
+    /// DMI
+    ///
+    LanesCount = SA_DMI_MAX_LANE;
+  }
+
+  BundlesCount = LanesCount >> 1;
+
+  ///
+  /// g23rxvref = 0xC (DMI & PEG)
+  ///
+  Data32And = (UINT32) ~(BIT4 | BIT3 | BIT2 | BIT1 | BIT0);
+  Data32Or  = 0xC;
+  for (i = 0; i < LanesCount; i++) {
+    SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFELN0CFG0_OFFSET + i * LANE_STEP, Data32And, Data32Or);
+  }
+  ///
+  /// CDRPDDATMODE = 0x1 (DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < LanesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFELN0CFG1_OFFSET + i * LANE_STEP, 0xFFFFFFFF, BIT11);
+    }
+  }
+  ///
+  /// irefctl = 0x2 (PEG only)
+  ///
+  if (DmiBar == 0) {
+    ///
+    /// Switch on
+    /// SwingControl 1 == Half
+    ///              2 == Full (Default)
+    ///
+    switch (SwingControl) {
+      case SA_SWING_HALF:
+        Data32Or = 0x0C;
+        break;
+
+      default:
+      case SA_SWING_FULL:
+        Data32Or = 0x02;
+        break;
+    }
+
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG1_OFFSET + i * BUNDLE_STEP, (UINT32) ~(0x1F), Data32Or);
+    }
+  }
+  ///
+  /// txvrefsel = 0x3 (PEG only)
+  ///
+  if (DmiBar == 0) {
+    ///
+    /// Switch on
+    /// SwingControl 1 == Half
+    ///              2 == Full (Default)
+    ///
+    switch (SwingControl) {
+      case SA_SWING_HALF:
+        Data32Or = 0x0E;
+        break;
+
+      default:
+      case SA_SWING_FULL:
+        Data32Or = 0x03;
+        break;
+    }
+
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG1_OFFSET + i * BUNDLE_STEP, (UINT32) ~(0x1F << 5), Data32Or << 5);
+    }
+  }
+  ///
+  /// igacq = 0x0 (HSW A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG1_OFFSET + i * BUNDLE_STEP, (UINT32) ~(7 << 22), 0);
+    }
+  }
+  ///
+  /// dfegainacq = 0x1 (HSW A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG1_OFFSET + i * BUNDLE_STEP, (UINT32) ~(3 << 29), 1 << 29);
+    }
+  }
+  ///
+  /// PGTRK = 0x9 (DMI & PEG)
+  ///
+  Data32And = (UINT32) ~(BIT10 | BIT9 | BIT8 | BIT7 | BIT6 | BIT5);
+  Data32Or  = 0x9 << 5;
+  for (i = 0; i < BundlesCount; i++) {
+    SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG3_OFFSET + i * BUNDLE_STEP, Data32And, Data32Or);
+  }
+  ///
+  /// igtrk = 0x0 (HSW A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG3_OFFSET + i * BUNDLE_STEP, (UINT32) ~(7 << 17), 0);
+    }
+  }
+  ///
+  /// RXRTBIN = 0x5 (PEG only)
+  ///
+  if (DmiBar == 0) {
+    Data32And = (UINT32) ~(BIT24 | BIT23 | BIT22 | BIT21);
+    Data32Or  = 0x5 << 21;
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG3_OFFSET + i * BUNDLE_STEP, Data32And, Data32Or);
+    }
+  }
+  ///
+  /// G3RXCTLEPEAK = 0x8 (PEG only)
+  ///
+  if (DmiBar == 0) {
+    Data32And = (UINT32) ~(BIT9 | BIT8 | BIT7 | BIT6);
+    for (i = 0; i < BundlesCount; i++) {
+      if (SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_3) {
+        Data32Or = SaPlatformPolicyPpi->PcieConfig->Gen3RxCtleP[i] << 6;
+      } else {
+        Data32Or  = 8 << 6;
+      }
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG4_OFFSET + i * BUNDLE_STEP, Data32And, Data32Or);
+    }
+  }
+  ///
+  /// g2rxctlepeak = 0x0 (DMI & PEG)
+  ///
+  for (i = 0; i < BundlesCount; i++) {
+    SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG4_OFFSET + i * BUNDLE_STEP, (UINT32) ~(0xF << 10), 0);
+  }
+  ///
+  /// AFEBNDSPARE[uneqmm] = 0x4 (HSW == A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG4_OFFSET + i * BUNDLE_STEP, (UINT32) ~(7 << 29), 4 << 29);
+    }
+  }
+  ///
+  /// AGCACQLEN = 0x2 (HSW == A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG5_OFFSET + i * BUNDLE_STEP, (UINT32) ~(3 << 7), 2 << 7);
+    }
+  }
+  ///
+  /// G2DFEC1CTL = 0x0 (HSW == A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG5_OFFSET + i * BUNDLE_STEP, (UINT32) ~(3 << 12), 0);
+    }
+  }
+  ///
+  /// RXSQEXCTL = 0x0 (DMI & PEG)
+  ///
+  Data32And = (UINT32) ~(BIT20 | BIT19 | BIT18);
+  Data32Or  = 0;
+  for (i = 0; i < BundlesCount; i++) {
+    SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG5_OFFSET + i * BUNDLE_STEP, Data32And, Data32Or);
+  }
+  ///
+  /// dfemfc = 0x3 (HSW == A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG5_OFFSET + i * BUNDLE_STEP, (UINT32) ~(3 << 21), 3 << 21);
+    }
+  }
+
+  ///
+  /// ICOMPGAIN = 0x6 (HSW == A0: DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    VcuAddress = R_SA_VCU_AFECMNCFG0_ADDRESS_REV1;
+    VcuReadOp  = V_SA_VCU_OPCODE_READ_MMIO_REV1;
+    VcuWriteOp = V_SA_VCU_OPCODE_WRITE_MMIO_REV1;
+  } else {
+    VcuAddress = R_SA_VCU_AFECMNCFG0_ADDRESS_REV2;
+    VcuReadOp  = V_SA_VCU_OPCODE_READ_MMIO_REV2;
+    VcuWriteOp = V_SA_VCU_OPCODE_WRITE_MMIO_REV2;
+  }
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    if ((DmiBar != 0) || (IsSingleCall)) {
+      Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+      Data32 &= (UINT32) ~(BIT12 | BIT11 | BIT10 | BIT9);
+      Data32 |= 0x6 << 9;
+      SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+    }
+  }
+
+  ///
+  /// PEGVCMSEL = 0x3 (PEG only)
+  ///
+  if (DmiBar == 0) {
+    Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+    Data32 &= (UINT32) ~(BIT29 | BIT28 | BIT27 | BIT26 | BIT25);
+    ///
+    /// Switch on
+    /// SwingControl 1 == Half
+    ///              2 == Full (Default)
+    ///
+    switch (SwingControl) {
+      case SA_SWING_HALF:
+        Data32 |= (UINT32) (0x0E << 25);
+        break;
+
+      default:
+      case SA_SWING_FULL:
+        Data32 |= (UINT32) (0x03 << 25);
+        break;
+    }
+    SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+  }
+
+  ///
+  /// PH3EQFFEKNOBS = 0x8 (DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    VcuAddress = R_SA_VCU_AFECMNCFG2_ADDRESS_REV1;
+  } else {
+    VcuAddress = R_SA_VCU_AFECMNCFG2_ADDRESS_REV2;
+  }
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    if ((DmiBar != 0) || (IsSingleCall)) {
+      Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+      Data32 &= (UINT32) ~(BIT30 | BIT29 | BIT28 | BIT27 | BIT26 | BIT25);
+      Data32 |= 0x8 << 25;
+      SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+    }
+  }
+
+  ///
+  /// FIXUNCORRDATA = 0x0 (DMI & PEG)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    VcuAddress = R_SA_VCU_AFECMNCFG3_ADDRESS_REV1;
+  } else {
+    VcuAddress = R_SA_VCU_AFECMNCFG3_ADDRESS_REV2;
+  }
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    if ((DmiBar != 0) || (IsSingleCall)) {
+      Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+      Data32 &= (UINT32) ~(BIT20);
+      Data32 |= 0;
+      SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+    }
+  }
+  ///
+  /// BLEGCTL = 0x0 (DMI & PEG)
+  ///
+  if ((DmiBar != 0) || (IsSingleCall)) {
+    Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+    Data32 &= (UINT32) ~(BIT28 | BIT27 | BIT26 | BIT25 | BIT24 | BIT23);
+    Data32 |= 0;
+    SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+  }
+  if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) &&
+      ((DmiBar      != 0)          || (IsSingleCall)              )) {
+    VcuAddress = R_SA_VCU_AFECMNCFG7_ADDRESS_REV1;
+    VcuReadOp  = V_SA_VCU_OPCODE_READ_CSR_REV1;
+    VcuWriteOp = V_SA_VCU_OPCODE_WRITE_CSR_REV1;
+
+    Data32 = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+    ///
+    /// VREFRXDET = 0x19 (PEG only)
+    ///
+    if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+      Data32 &= (UINT32) ~(BIT18 | BIT17 | BIT16 | BIT15 | BIT14);
+      Data32 |= 0x19 << 14;
+    }
+    ///
+    /// DFEFIX = 0x4 (PEG only)
+    ///
+    if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+      Data32 &= (UINT32) ~(BIT30 | BIT29 | BIT28);
+      Data32 |= 0x4 << 28;
+    }
+    SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+  }
+
+
+  ///
+  /// RXDETECT_SAMPLE_TIME = 0x4 (PEG only)
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    if (DmiBar == 0) {
+      Data32And = (UINT32) ~(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8 | BIT7);
+      Data32Or  = 0x4 << 7;
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFE_PWRON_OFFSET, Data32And, Data32Or);
+    }
+  }
+
+  ///
+  /// RXL0S_ENTRY_EXIT_TIMER = 0x00 (HSW == A0: DMI & PEG)
+  ///                        = 0x13 (HSW >= B0: DMI & PEG)
+  ///
+  Data32And = (UINT32) ~(BIT4 | BIT3 | BIT2 | BIT1 | BIT0);
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    Data32Or = 0x00;
+  } else {
+    Data32Or = 0x13;
+  }
+  SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFE_PM_TMR_OFFSET, Data32And, Data32Or);
+
+  ///
+  /// Set 0xC38[6] = 0x1 (HSW == A0), 0x0 (HSW >= B0) (DMI & PEG)
+  ///
+  Data32And = (UINT32) ~(BIT6);
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    Data32Or = 0x1 << 6;
+  } else {
+    Data32Or = 0x0 << 6;
+  }
+  if (DmiBar == 0) {
+    McD1PciCfg16AndThenOr   (R_SA_PEG_CMNSPARE_OFFSET, Data32And, Data32Or);
+    McD1F1PciCfg16AndThenOr (R_SA_PEG_CMNSPARE_OFFSET, Data32And, Data32Or);
+    McD1F2PciCfg16AndThenOr (R_SA_PEG_CMNSPARE_OFFSET, Data32And, Data32Or);
+  } else {
+    Mmio32AndThenOr (DmiBar, R_SA_DMIBAR_CMNSPARE_OFFSET, Data32And, Data32Or);
+  }
+
+  ///
+  /// Set 0x260[1:0] = '10b' (DMI & PEG)
+  ///
+  Data32And = (UINT32) ~(BIT1 | BIT0);
+  Data32Or  = 0x2;
+  if (DmiBar == 0) {
+    McD1PciCfg16AndThenOr   (R_SA_PEG_CFG6_OFFSET, Data32And, Data32Or);
+    McD1F1PciCfg16AndThenOr (R_SA_PEG_CFG6_OFFSET, Data32And, Data32Or);
+    McD1F2PciCfg16AndThenOr (R_SA_PEG_CFG6_OFFSET, Data32And, Data32Or);
+  } else {
+    Mmio32AndThenOr (DmiBar, R_SA_DMIBAR_CFG6_OFFSET, Data32And, Data32Or);
+  }
+
+  ///
+  /// setdfelsbsel = 0
+  ///
+  for (i = 0; i < BundlesCount; i++) {
+    SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG0_OFFSET + i * BUNDLE_STEP, (UINT32) ~(3 << 26), 0);
+  }
+  ///
+  /// OFFCORGAIN = 0x3 (HSW >= A0)
+  ///
+  for (i = 0; i < BundlesCount; i++) {
+    SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG1_OFFSET + i * BUNDLE_STEP, (UINT32) ~(3 << 10), 3 << 10);
+  }
+
+  ///
+  /// fixtxrtermoffset = -3; (PEG only)
+  ///
+  if (DmiBar == 0) {
+    for (i = 0; i < BundlesCount; i++) {
+      SaMmio32AndThenOr (DmiBar, Dev, R_SA_PEG_AFEBND0CFG3_OFFSET + i * BUNDLE_STEP, (UINT32) ~(0x1F << 25), ((0x03 << 1) | 0x01) << 25);
+    }
+  }
+
+  ///
+  /// Set BND0SPARE[29:27] = '101b' (PEG)
+  ///
+  Data32And = (UINT32) ~(BIT29 | BIT28 | BIT27);
+  Data32Or  = (UINT32) 0x28000000;
+  for (i = 0; i < BundlesCount; i++) {
+    MmPci32AndThenOr (
+      0,
+      SA_PEG_BUS_NUM,
+      SA_PEG10_DEV_NUM,
+      SA_PEG10_FUN_NUM,
+      R_SA_PEG_BND0SPARE_OFFSET + (i * BUNDLE_STEP),
+      Data32And,
+      Data32Or
+      );
+  }
+  return;
+}
+#endif // DMI_FLAG || PEG_FLAG
+
+#ifdef PEG_FLAG
+VOID
+BubbleSort (
+  IN OUT DATA_SAMPLE Array[]
+  )
+/**
+  Bubble sort from DATA_SAMPLE
+
+  @param[in, out] - Array[]: array of DATA_SAMPLE
+
+  @retval None
+**/
+{
+  UINTN       n;
+  UINTN       i;
+  UINTN       j;
+  DATA_SAMPLE Temp;
+
+  n = (UINTN) Array[0].Count;
+
+  for (i = 1; i <= n; i++) {
+    for (j = n; j > i; j--) {
+      if (Array[j - 1].Data > Array[j].Data) {
+        Temp          = Array[j - 1];
+        Array[j - 1]  = Array[j];
+        Array[j]      = Temp;
+      }
+    }
+  }
+
+  return;
+}
+
+UINT32
+GetMiddleValue (
+  IN OUT DATA_SAMPLE Array[]
+  )
+/**
+  Get Middle Value from DATA_SAMPLE
+
+  @param[in, out] - Array[]: array of DATA_SAMPLE
+
+  @retval UINT32 : Middle Value of DATA_SAMPLE
+**/
+{
+  UINT32  n;
+
+  n = Array[0].Count;
+
+  return Array[n / 2 + 1].Data;
+}
+
+#ifdef EFI_DEBUG
+VOID
+DumpSamplerValues (
+  IN UINT32 DmiBar,
+  IN UINT8  CpuSteppingId,
+  IN UINTN  Dev,
+  IN UINTN  LanesCount
+  )
+/**
+  Dump Sampler Values
+
+  @param[in] DmiBar        - DMIBAR address
+  @param[in] CpuSteppingId - CPUID.1.EAX[3:0], CPU stepping ID
+  @param[in] Dev           - Device number
+  @param[in] LanesCount    - Value of Lanes
+
+  @retval None
+**/
+{
+  UINTN   Lane;
+  UINT32  Sampler;
+  UINT8   SampleData;
+
+  DEBUG ((EFI_D_INFO, "Lane DS0  DS1  ESP0 ESP1 ESM0 ESM1\n"));
+  for (Lane = 0; Lane < LanesCount; Lane++) {
+    DEBUG ((EFI_D_INFO, "%2d:  ", Lane));
+    for (Sampler = 0; Sampler < 6; Sampler++) {
+      SampleData = GetMonBus (DmiBar, Dev, Lane, 0x31 + Sampler, CpuSteppingId) & 0x3F;
+      DEBUG ((EFI_D_INFO, "%02X   ", SampleData));
+    }
+
+    DEBUG ((EFI_D_INFO, "\n"));
+  }
+
+  return;
+}
+#endif // EFI_DEBUG
+
+VOID
+SamplerCalibratePegPort (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN UINT8                  CpuSteppingId,
+  IN UINTN                  Dev,
+  IN UINTN                  LanesCount,
+  IN UINTN                  HwStrap
+  )
+/**
+ This function performs the PEG Sampler Calibration for HSW on a given PEG controller
+ It also calls the Step 2 of PEG Recipe routine.
+
+  @param[in] PeiServices         - Pointer to the PEI services table
+  @param[in] StallPpi            - pointer to PEI_STALL_PPI
+  @param[in] CpuSteppingId       - CPUID.1.EAX[3:0], CPU stepping ID
+  @param[in] Dev                 - Device number
+  @param[in] LanesCount          - Value of Lanes
+  @param[in] HwStrap             - HwStrap configuration from FUSESCMN
+
+  @retval None
+**/
+{
+  UINTN       Iteration;
+  UINTN       PegSamplerIterations;
+  UINTN       Lane;
+  UINTN       i;
+  BOOLEAN     Peg11Present;
+  BOOLEAN     Peg12Present;
+  BOOLEAN     EarlyExit;
+  BOOLEAN     CodeFound;
+  UINT8       SampleData;
+  UINT32      Sampler;
+  UINT16      CodesFound;
+  UINT32      MiddleCode;
+  UINTN       OcDelay;
+
+  ///
+  /// Array of sampling data - 16 lanes max, 6 samplers per lane, up to MAX_CODES per sampler
+  /// Size is 3 * 16 * 6 * 11 = 3168 bytes
+  ///
+  DATA_SAMPLE SampleArray[16][6][MAX_CODES + 1];
+
+  PegSamplerIterations  = SA_PEG_SAMPLER_ITERATIONS;
+  Peg11Present          = FALSE;
+  Peg12Present          = FALSE;
+
+  if (LanesCount == 16) {
+    OcDelay = 10;
+  } else {
+    OcDelay = 20;
+  }
+
+  ZeroMem (&SampleArray, sizeof (SampleArray));
+
+  if (HwStrap == SA_PEG_x8_x8_x0) {
+    Peg11Present = TRUE;
+  }
+
+
+  if (HwStrap == SA_PEG_x8_x4_x4) {
+    Peg11Present  = TRUE;
+    Peg12Present  = TRUE;
+  }
+  ///
+  /// Dump the Sampler values before calibration
+  ///
+  DEBUG ((EFI_D_INFO, "--- Sampler values before calibration ---\n"));
+#ifdef EFI_DEBUG
+  DumpSamplerValues (0, CpuSteppingId, Dev, LanesCount);
+#endif // EFI_DEBUG
+  ///
+  /// Disable the link
+  ///
+  MmPci16Or   (0, SA_PEG_BUS_NUM, Dev, 0, R_SA_PEG_LCTL_OFFSET, BIT4);
+  if (Peg11Present) {
+    MmPci16Or (0, SA_PEG_BUS_NUM, Dev, 1, R_SA_PEG_LCTL_OFFSET, BIT4);
+  }
+  if (Peg12Present) {
+    MmPci16Or (0, SA_PEG_BUS_NUM, Dev, 2, R_SA_PEG_LCTL_OFFSET, BIT4);
+  }
+
+  ///
+  /// Delay 10 ms after link disable
+  ///
+  StallPpi->Stall (PeiServices, StallPpi, 10 * STALL_ONE_MILLI_SECOND);
+
+  ///
+  /// Override L0s and L1 - set bits 11, 13 & 15 of AFEOVR
+  ///
+  MmPci32Or   (0, SA_PEG_BUS_NUM, Dev, 0, R_SA_PEG_AFEOVR_OFFSET, 0xA800);
+  if (Peg11Present) {
+    MmPci32Or (0, SA_PEG_BUS_NUM, Dev, 1, R_SA_PEG_AFEOVR_OFFSET, 0xA800);
+  }
+  if (Peg12Present) {
+    MmPci32Or (0, SA_PEG_BUS_NUM, Dev, 2, R_SA_PEG_AFEOVR_OFFSET, 0xA800);
+  }
+
+  for (Iteration = 0; Iteration < PegSamplerIterations; Iteration++) {
+    PostCode ((UINT8) (Iteration / 100));
+    ///
+    /// First trigger OC on each lane
+    ///
+    for (Lane = 0; Lane < LanesCount; Lane++) {
+      SetLoadBus (0, Dev, Lane, 0x39, 0x1, CpuSteppingId);
+      SetLoadBus (0, Dev, Lane, 0x3A, 0xC, CpuSteppingId);
+    }
+    ///
+    /// Delay to give the OC time to complete
+    ///
+    StallPpi->Stall (PeiServices, StallPpi, OcDelay * STALL_ONE_MICRO_SECOND);
+
+    for (Lane = 0; Lane < LanesCount; Lane++) {
+      for (Sampler = 0; Sampler < 6; Sampler++) {
+        SampleData = GetMonBus (0, Dev, Lane, 0x31 + Sampler, CpuSteppingId) & 0x3F;
+
+        ///
+        /// SampleArray[Lane][Sampler][0].Count holds the number of distinct codes found so far
+        ///
+        CodeFound = FALSE;
+        for (i = 1; i <= SampleArray[Lane][Sampler][0].Count; ++i) {
+          if (SampleArray[Lane][Sampler][i].Data == SampleData) {
+            CodeFound = TRUE;
+            SampleArray[Lane][Sampler][i].Count++;
+            break;
+          }
+        }
+
+        if (!CodeFound) {
+          if (i > MAX_CODES) {
+            DEBUG ((EFI_D_ERROR, "ERROR: PEG dev=%d, lane=%d, sampler=%d, iteration=%d, found more than %d distinct codes!!!\n", Dev, Lane, Sampler, Iteration, MAX_CODES));
+            if (LanesCount == 16) {
+              PostCode ((UINT8) (ERROR_BY_16 >> 8));
+            } else {
+              PostCode ((UINT8) (ERROR_NOT_BY_16 >> 8));
+            }
+
+            EFI_DEADLOOP ();
+          }
+          ///
+          /// Increment the number of distinct codes found for this sampler
+          ///
+          SampleArray[Lane][Sampler][0].Count++;
+
+          ///
+          /// Save the new code
+          ///
+          SampleArray[Lane][Sampler][i].Data  = SampleData;
+          SampleArray[Lane][Sampler][i].Count = 1;
+        }
+      }
+    }
+    ///
+    /// Exit early if all the sampled lanes have more than 5 codes,
+    /// and we covered at least 20% of iterations.
+    /// Ignore the inactive lanes, these will only show one code.
+    ///
+    if (Iteration > PegSamplerIterations / 5) {
+      EarlyExit = TRUE;
+      for (Lane = 0; Lane < LanesCount; Lane++) {
+        for (Sampler = 0; Sampler < 6; Sampler++) {
+          CodesFound = SampleArray[Lane][Sampler][0].Count;
+          if ((CodesFound > 1) && (CodesFound < 5)) {
+            EarlyExit = FALSE;
+            break;
+          }
+        }
+
+        if (!EarlyExit) {
+          break;
+        }
+      }
+
+      if (EarlyExit) {
+        break;
+      }
+    }
+  }
+  ///
+  /// Iteration
+  ///
+  for (Lane = 0; Lane < LanesCount; Lane++) {
+    for (Sampler = 0; Sampler < 6; Sampler++) {
+      ///
+      /// Sort the codes for this Lane / Sampler
+      ///
+      BubbleSort (SampleArray[Lane][Sampler]);
+      ///
+      /// Find the middle value
+      ///
+      MiddleCode = GetMiddleValue (SampleArray[Lane][Sampler]);
+      ///
+      /// Set the middle calibration code
+      ///
+      SetLoadBus (0, Dev, Lane, 0x31 + Sampler, MiddleCode, CpuSteppingId);
+    }
+  }
+  ///
+  /// Restore AFEOVR
+  ///
+  MmPci32   (0, SA_PEG_BUS_NUM, Dev, 0, R_SA_PEG_AFEOVR_OFFSET) = 0;
+  if (Peg11Present) {
+    MmPci32 (0, SA_PEG_BUS_NUM, Dev, 1, R_SA_PEG_AFEOVR_OFFSET) = 0;
+  }
+  if (Peg12Present) {
+    MmPci32 (0, SA_PEG_BUS_NUM, Dev, 2, R_SA_PEG_AFEOVR_OFFSET) = 0;
+  }
+  ///
+  /// Delay 1 ms before link enable
+  ///
+  StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MILLI_SECOND);
+
+  ///
+  /// Enable the link
+  ///
+  MmPci16And   (0, SA_PEG_BUS_NUM, Dev, 0, R_SA_PEG_LCTL_OFFSET, ~BIT4);
+  if (Peg11Present) {
+    MmPci16And (0, SA_PEG_BUS_NUM, Dev, 1, R_SA_PEG_LCTL_OFFSET, ~BIT4);
+  }
+  if (Peg12Present) {
+    MmPci16And (0, SA_PEG_BUS_NUM, Dev, 2, R_SA_PEG_LCTL_OFFSET, ~BIT4);
+  }
+  ///
+  /// Dump the Sampler values after calibration
+  ///
+  DEBUG ((EFI_D_INFO, "--- Sampler values after calibration ---\n"));
+#ifdef EFI_DEBUG
+  DumpSamplerValues (0, CpuSteppingId, Dev, LanesCount);
+#endif // EFI_DEBUG
+  return;
+}
+
+VOID
+PegSamplerCalibration (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN UINT8                  CpuSteppingId,
+  IN UINT8                  HwStrap
+  )
+/**
+ This function performs the PEG Sampler Calibration
+
+  @param[in] PeiServices         - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi - pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] StallPpi            - pointer to PEI_STALL_PPI
+  @param[in] CpuSteppingId       - CPUID.1.EAX[3:0], CPU stepping ID
+  @param[in] HwStrap             - HwStrap configuration from FUSESCMN
+
+  @retval None
+**/
+{
+  UINT32  Data32;
+
+  ///
+  /// Calibrate PEG10/PEG11/PEG12 - 16 lanes total
+  ///
+  if (MmPci16 (0, 0, 1, 0, PCI_VID) != 0xFFFF) {
+    Data32 = MmPci32 (0, 0, 1, 0, R_SA_PEG_PEGSTS_OFFSET);
+    if (((Data32 & 0xFFFF) != 0) && (((Data32 >> 16) & 0x0F) >= 7)) {
+      SamplerCalibratePegPort (PeiServices, StallPpi, CpuSteppingId, SA_PEG10_DEV_NUM, 16, HwStrap);
+    }
+  }
+
+  return;
+}
+
+VOID
+PegGen2AutoSpeedDisable (
+  IN EFI_PEI_SERVICES                   **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI             *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI                      *StallPpi,
+  IN SA_DATA_HOB                        *SaDataHob,
+  IN PEG_PORT                           *PegPortTable,
+  IN UINTN                              PegPortTableSize,
+  IN UINT8                              HwStrap
+  )
+/**
+ This function performs the PEG GEN2 Auto Speed Disable
+
+  @param[in] PeiServices         - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] StallPpi            - Pointer to PEI_STALL_PPI
+  @param[in] SaDataHob           - Pointer to SA_DATA_HOB
+  @param[in] PegPortTable        - Pointer to PEG Port Table
+  @param[in] PegPortTableSize    - Size of PEG Port Table
+  @param[in] HwStrap             - Furcation HW Strap Value
+
+  @retval None
+**/
+{
+  UINTN          PortIndex;
+  EFI_STATUS     Status;
+  EFI_BOOT_MODE  BootMode;
+  UINT8          DisableAutoSpeedUp;
+  UINT8          PegBus;
+  UINT8          PegDev;
+  UINT8          PegFunc;
+
+  DisableAutoSpeedUp = 0;
+  Status = (*PeiServices)->GetBootMode (PeiServices, &BootMode);
+  ASSERT_EFI_ERROR (Status);
+
+  for (PortIndex = 0; PortIndex < PegPortTableSize; PortIndex++) {
+    PegBus  = PegPortTable[PortIndex].Bus;
+    PegDev  = PegPortTable[PortIndex].Device;
+    PegFunc = PegPortTable[PortIndex].Function;
+
+    ///
+    /// If VC0 is still pending
+    /// And presence of an endpoint was detected, enable GEN2 auto speed disable
+    ///
+    if ((MmPci16 (0, PegBus, PegDev, PegFunc, PCI_VID                        ) != 0xFFFF) &&
+       ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0RSTS_OFFSET) & BIT1) != 0)      &&
+       ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_SLOTSTS_OFFSET) & BIT6) != 0)) {
+      DisableAutoSpeedUp |= 1 << PegFunc;
+      MmPci16Or  (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL_OFFSET, BIT4);
+      MmPci32Or  (0, PegBus, PegDev, PegFunc, R_SA_PEG_CFG5_OFFSET, BIT9);
+      MmPci16And (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL_OFFSET, (UINT16)~(BIT4));
+    }
+  }
+  ///
+  /// If needed, reinitialize link after disable
+  ///
+  if (DisableAutoSpeedUp != 0) {
+    if ((BootMode == BOOT_ON_S3_RESUME) || (GetPchPmStatus (WarmBoot) == TRUE)) {
+      if ((SaDataHob != NULL) && (SaDataHob->PegDataValid)) {
+        if (SaDataHob->PegData.PegLinkFailMask != 0) {
+          ///
+          /// Even on warm boot/S3 resume, we still want to force checking of VC0 status
+          ///
+          SaDataHob->PegData.PegLinkFailMask = 0;
+        }
+      }
+    }
+    GracefulLinkStatusStall (PeiServices, StallPpi, SaPlatformPolicyPpi, SaDataHob, HwStrap);
+    for (PortIndex = 0; PortIndex < PegPortTableSize; PortIndex++) {
+      PegBus  = PegPortTable[PortIndex].Bus;
+      PegDev  = PegPortTable[PortIndex].Device;
+      PegFunc = PegPortTable[PortIndex].Function;
+      if (((DisableAutoSpeedUp >> PegFunc) & 1) == 1) {
+        ///
+        /// Retrain to allow link to reach GEN2
+        ///
+        MmPci16Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL_OFFSET, BIT5);
+      }
+    }
+    ///
+    /// Ensure all links are now ready
+    ///
+    if ((BootMode == BOOT_ON_S3_RESUME) || (GetPchPmStatus (WarmBoot) == TRUE)) {
+      if ((SaDataHob != NULL) && (SaDataHob->PegDataValid)) {
+        if (SaDataHob->PegData.PegLinkFailMask != 0) {
+          ///
+          /// Even on warm boot/S3 resume, we still want to force checking of VC0 status
+          ///
+          SaDataHob->PegData.PegLinkFailMask = 0;
+        }
+      }
+    }
+    GracefulLinkStatusStall (PeiServices, StallPpi, SaPlatformPolicyPpi, SaDataHob, HwStrap);
+    DEBUG ((EFI_D_INFO, "PEG Link Status after auto speed disable:\n"));
+    for (PortIndex = 0; PortIndex < PegPortTableSize; PortIndex++) {
+      PegBus  = PegPortTable[PortIndex].Bus;
+      PegDev  = PegPortTable[PortIndex].Device;
+      PegFunc = PegPortTable[PortIndex].Function;
+      ReportPcieLinkStatus (PegBus, PegDev, PegFunc);
+    }
+  }
+}
+
+VOID
+PciExpressInit (
+  IN EFI_PEI_SERVICES                   **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI             *SaPlatformPolicyPpi
+  )
+/**
+  Initialize the SA PciExpress in PEI
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+**/
+{
+  UINT8               PegBus;
+  UINT8               PegDev;
+  UINT8               PegFunc;
+  UINT8               PegIndex;
+  UINT8               HwStrap;
+  UINT64              MchBar;
+  UINT64              DmiBar;
+  BOOLEAN             DisableFun0;
+  BOOLEAN             DisableFun1;
+  BOOLEAN             DisableFun2;
+  BOOLEAN             DisableLinkFunc0;
+  EFI_STATUS          Status;
+  CPU_FAMILY          CpuFamilyId;
+  CPU_STEPPING        CpuSteppingId;
+  UINTN               PegComplete;
+  UINT16              Peg10Speed;
+  UINT16              Peg11Speed;
+  UINT16              Peg12Speed;
+  UINT8               SwingControl;
+  UINT8               Gen3Capable;
+  UINT8               i;
+  PEI_STALL_PPI       *StallPpi;
+  UINT32              Data32;
+  UINT32              CapOffset;
+  BOOLEAN             AnyGen3Endpoint;
+  UINT8               FullSwing;
+  UINT8               PreCursor;
+  UINT8               Cursor;
+  UINT8               PostCursor;
+
+  PEG_PORT            PegPortTable[] = {
+    ///
+    ///  Bus, Device, Function, Index, PresenceDetect, MaxLinkWidth, EndpointMaxLinkSpeed
+    ///
+    { SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, 0, FALSE, 16, 0}
+//AMI_OVERRIDE>>
+#if RC_PEG_1 == 1
+    ,{ SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM, 1, FALSE,  8, 0}
+#endif
+#if RC_PEG_2 == 1
+    ,{ SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM, 2, FALSE,  4, 0}
+#endif
+//AMI_OVERRIDE<<
+  };
+  SA_DATA_HOB         *SaDataHob;
+  BOOLEAN             S3Flow;
+  UINT8               LinkStatusGood;
+  BOOLEAN             FunctionExists;
+  UINT8               UnusedLanes;
+  UINT8               CtrlMaxLinkWidth;
+  UINT8               EpMaxLinkWidth;
+  UINT16              LoopCount;
+  UINT8               MaxBndlPwrdnCount;
+  UINT8               BndlPwrdnCount;
+  UINT8               PwrDnUnusedBundlesSetupData;
+
+  HwStrap                     = SA_PEG_x16_x0_x0;
+  DisableFun0                 = FALSE;
+  DisableFun1                 = FALSE;
+  DisableFun2                 = FALSE;
+  DisableLinkFunc0            = FALSE;
+  MchBar                      = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+  DmiBar                      = McD0PciCfg64 (R_SA_DMIBAR) &~BIT0;
+  PegDev                      = 0x1;
+
+  Peg10Speed                  = 0;
+  Peg11Speed                  = 0;
+  Peg12Speed                  = 0;
+  SwingControl                = SaPlatformPolicyPpi->PcieConfig->PegSwingControl;
+  SaDataHob                   = NULL;
+  S3Flow                      = FALSE;
+  Gen3Capable                 = TRUE;
+  AnyGen3Endpoint             = FALSE;
+  FullSwing                   = 0;
+  CtrlMaxLinkWidth            = 0;
+  EpMaxLinkWidth              = 0;
+  UnusedLanes                 = 0;
+  LoopCount                   = 0;
+  MaxBndlPwrdnCount           = 0;
+  BndlPwrdnCount              = 0;
+  PwrDnUnusedBundlesSetupData = 0xFF;
+
+  ///
+  /// Check to see if PEG exists and leave initialization function if non-existant
+  ///
+  if (McD1PciCfg16 (PCI_VID) == 0xFFFF) {
+    DEBUG ((EFI_D_INFO, "PEG controller not detected\n"));
+    return;
+  }
+
+  ///
+  /// Read the HW straps - Bus 0, Device 1, Fun 0, Reg 0x504 BIT17,16
+  /// Fun 1 & 2 disabled = Pcie 1x16, Fun 2 disabled = Pcie 2x8
+  /// Fun 1 & 2 enabled = Pcie 1x8 + (2+4)
+  ///
+  HwStrap = (UINT8) ((MmPci32 (0, SA_MC_BUS, PegDev, 0, R_SA_PEG_FUSESCMN_OFFSET) >> 16) & 0x03);
+  DEBUG ((EFI_D_INFO, "PEG HW Strap value %x\n", HwStrap));
+
+  Status = (*PeiServices)->LocatePpi (PeiServices, &gPeiStallPpiGuid, 0, NULL, &StallPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Update PEG LCAP.MLW based on PEG port Split configuration.
+  /// This is Write-Once field, so keep the values in the structure,
+  /// and write later together with LCAP.MLS.
+  ///
+  switch (HwStrap) {
+    case SA_PEG_x8_x4_x4:
+      ///
+      /// 0x0: Device 1 functions 1 and 2 enabled
+      /// PEG10: x16->x8, PEG11: x8->x4, PEG12: x4
+      ///
+      PegPortTable[0].MaxLinkWidth  = 8;
+//AMI_OVERRIDE >>
+#if RC_PEG_1 == 1
+      PegPortTable[1].MaxLinkWidth  = 4;
+#endif
+//AMI_OVERRIDE <<
+      break;
+
+
+    case SA_PEG_x8_x8_x0:
+      ///
+      /// 0x2: Device 1 function 1 enabled; function 2 disabled
+      /// PEG10: x16->x8, PEG11: x8, PEG12: N/A
+      ///
+      PegPortTable[0].MaxLinkWidth = 8;
+      break;
+
+    default:
+    case SA_PEG_x16_x0_x0:
+      break;
+  }
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  ///
+  /// Read PEG Gen3 Fuse to see if it should override programming
+  ///
+  if (MmPci32 (0, SA_MC_BUS, SA_MC_DEV, SA_MC_FUN, R_SA_MC_CAPID0_B) & BIT20) {
+    Gen3Capable = FALSE;
+    DEBUG ((EFI_D_INFO, "PEG Gen3 Fused off\n"));
+  }
+
+  ///
+  /// Gen3 Preset Search: 0 = Disabled, 1 = Enabled (default)
+  ///
+  if (SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch == 2) {
+    SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch = 1;
+  }
+
+  ///
+  /// Restore SA Data HOB's PEG data
+  ///
+  if (SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_2) {
+    SaDataHob = (SA_DATA_HOB *) GetFirstGuidHob (&gSaDataHobGuid);
+
+    if (SaDataHob != NULL) {
+      if (SaPlatformPolicyPpi->PcieConfig->PegDataPtr != NULL) {
+        DEBUG ((EFI_D_INFO, "\nRestore SA PEG DATA from previous boot: Size=%X\n", sizeof (SA_PEG_DATA)));
+        CopyMem (&(SaDataHob->PegData), SaPlatformPolicyPpi->PcieConfig->PegDataPtr, sizeof (SA_PEG_DATA));
+        SaDataHob->PegDataValid = TRUE;
+        if ((SaDataHob->PegData.PegGen3PresetSearch != SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch) && (SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch != 1)) {
+          ///
+          /// Zero out previous boot GEN3 Preset data so old data won't be re-used when PegGen3PresetSearch re-enabled later
+          ///
+          DEBUG ((EFI_D_INFO, "\nPegGen3PresetSearch is disabled, Clear old Preset data\n"));
+          for (PegIndex = 0; PegIndex < SA_PEG_MAX_FUN; PegIndex++) {
+            SaDataHob->PegData.EndPointVendorIdDeviceId[PegIndex] = 0;
+            for (i = 0; i < SA_PEG_MAX_LANE; i++) {
+              SaDataHob->PegData.BestPreset[i] = 0;
+            }
+          }
+        }
+      }
+      SaDataHob->PegData.PegGen3PresetSearch = SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch;
+    }
+  }
+
+  ///
+  /// Perform PEG Pre-Detection steps
+  ///
+  for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+    PegBus  = PegPortTable[PegComplete].Bus;
+    PegDev  = PegPortTable[PegComplete].Device;
+    PegFunc = PegPortTable[PegComplete].Function;
+    PegPreDetectionSteps (PegBus, PegDev, PegFunc, SaPlatformPolicyPpi);
+  }
+
+  ///
+  /// Perform PEG Recipe steps
+  ///
+  DEBUG ((EFI_D_INFO, "PEG Recipe...\n"));
+  PegDmiRecipe (SaPlatformPolicyPpi, (UINT32) MchBar, 0, SA_PEG10_DEV_NUM, SwingControl);
+
+  ///
+  /// Perform PEG Gen3 Equalization steps and load preset values
+  ///
+  if (Gen3Capable == TRUE) {
+    if (SaPlatformPolicyPpi->PcieConfig->PegGen3Equalization != 0) {
+      DEBUG ((EFI_D_INFO, "PEG Gen3 Equalization...\n"));
+      PegGen3Equalization (SaPlatformPolicyPpi, CpuSteppingId, HwStrap);
+    }
+  }
+
+  ///
+  /// PEG Sampler Calibration: 0 = Disabled (default), 1 = Enabled
+  ///
+  if (SaPlatformPolicyPpi->PcieConfig->PegSamplerCalibrate == 2) {
+    SaPlatformPolicyPpi->PcieConfig->PegSamplerCalibrate = 0;
+  }
+
+  if (SaPlatformPolicyPpi->PcieConfig->PegSamplerCalibrate == 1) {
+    ///
+    /// Back up the current PEG speed in LCTL2.TLS[3:0]
+    ///
+    Peg10Speed = McD1PciCfg16   (R_SA_PEG_LCTL2_OFFSET) & 0x0F;
+    Peg11Speed = McD1F1PciCfg16 (R_SA_PEG_LCTL2_OFFSET) & 0x0F;
+    Peg12Speed = McD1F2PciCfg16 (R_SA_PEG_LCTL2_OFFSET) & 0x0F;
+    ///
+    /// Set the PEG speed in LCTL2.TLS[3:0] to Gen1 before clearing DEFER_OC,
+    /// in order to run Sampler Calibration at Gen1.
+    ///
+    McD1PciCfg16AndThenOr   (R_SA_PEG_LCTL2_OFFSET, 0xFFF0, 1);
+    McD1F1PciCfg16AndThenOr (R_SA_PEG_LCTL2_OFFSET, 0xFFF0, 1);
+    McD1F2PciCfg16AndThenOr (R_SA_PEG_LCTL2_OFFSET, 0xFFF0, 1);
+  }
+
+  for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+    ///
+    /// Program the PEG speed according to Setup options: Auto/Gen1/2/3
+    /// We have to do it before endpoint enumeration, so that uncompliant card
+    /// can train at a lower speed.
+    ///
+    ConfigurePegGenX (
+      PeiServices,
+      StallPpi,
+      SaPlatformPolicyPpi,
+      PegPortTable,
+      PegComplete,
+      CpuSteppingId,
+      Gen3Capable
+      );
+  }
+
+  ///
+  /// RxCEM Loopback (LPBK) Mode
+  ///
+  if ((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_3) && (SaPlatformPolicyPpi->PcieConfig->RxCEMLoopback == 1)) {
+    McD1PciCfg32AndThenOr (R_SA_PEG_PEGTST_OFFSET, ~(BIT19|BIT18|BIT17|BIT16), (SaPlatformPolicyPpi->PcieConfig->RxCEMLoopbackLane & 0xF) << 16);
+    for (i = 0; i < SA_PEG_MAX_LANE; i++) {
+      if (i == SaPlatformPolicyPpi->PcieConfig->RxCEMLoopbackLane) {
+        McD1PciCfg32And (R_SA_PEG_AFELN0CFG0_OFFSET + LANE_STEP * i, ~BIT9);
+      } else {
+        McD1PciCfg32Or (R_SA_PEG_AFELN0CFG0_OFFSET + LANE_STEP * i, BIT9);
+      }
+    }
+  }
+
+  ///
+  /// Enable 3-OC retry for PEG(0/1/2). HSW/CRW earlier than C0.
+  ///
+  if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId < EnumHswC0)) ||
+      ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId < EnumCrwC0))) {
+    McD1PciCfg32Or    (R_SA_PEG_AFE_PWRON_OFFSET, BIT5);
+    McD1F1PciCfg32Or  (R_SA_PEG_AFE_PWRON_OFFSET, BIT5);
+    McD1F2PciCfg32Or  (R_SA_PEG_AFE_PWRON_OFFSET, BIT5);
+  }
+
+  ///
+  /// Bypass phase2
+  ///
+  Data32  = McD1PciCfg32 (R_SA_PEG_EQCFG_OFFSET);
+  Data32 |= BIT15 | BIT1;
+  McD1PciCfg32 (R_SA_PEG_EQCFG_OFFSET) = Data32;
+
+  ///
+  /// Clear DEFER_OC in offset 0xC24[16] on all PEG controllers to start the PEG training
+  ///
+  McD1PciCfg32And   (R_SA_PEG_AFE_PWRON_OFFSET, ~BIT16);
+  McD1F1PciCfg32And (R_SA_PEG_AFE_PWRON_OFFSET, ~BIT16);
+  McD1F2PciCfg32And (R_SA_PEG_AFE_PWRON_OFFSET, ~BIT16);
+
+  ///
+  /// Delay for 100ms to meet the timing requirements of the PCI Express Base
+  /// Specification, Revision 1.0A, Section 6.6 ("...software must wait at least
+  /// 100 ms from the end of reset of one or more device before it is permitted
+  /// to issue Configuration Requests to those devices").
+  ///
+  GracefulLinkStatusStall (PeiServices, StallPpi, SaPlatformPolicyPpi, SaDataHob, HwStrap);
+
+  PegGen2AutoSpeedDisable (PeiServices,
+                           SaPlatformPolicyPpi,
+                           StallPpi,
+                           SaDataHob,
+                           &(PegPortTable[0]),
+                           ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))),
+                           HwStrap);
+
+  ///
+  /// Read the presence detect bit for each PEG port - must be done before sampler calibration
+  ///
+  for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+    PegBus  = PegPortTable[PegComplete].Bus;
+    PegDev  = PegPortTable[PegComplete].Device;
+    PegFunc = PegPortTable[PegComplete].Function;
+    if (( MmPci16 (0, PegBus, PegDev, PegFunc, PCI_VID                        ) != 0xFFFF) &&
+        ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_SLOTSTS_OFFSET) & BIT6) !=      0)) {
+      PegPortTable[PegComplete].PresenceDetect = TRUE;
+
+      ///
+      /// Read the endpoint's Max Link Speed
+      ///
+      MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF, 0x00010100);
+      MmPci16 (0, 1, 0, 0, PCI_VID) = 0;
+      CapOffset = PcieFindCapId (1, 0, 0, EFI_PCI_CAPABILITY_ID_PCIEXP);
+      if (CapOffset != 0) {
+        Data32 = MmPci32 (0, 1, 0, 0, CapOffset + 0xC);
+        PegPortTable[PegComplete].EndpointMaxLinkSpeed = Data32 & 0xF;
+        if (PegPortTable[PegComplete].EndpointMaxLinkSpeed >= 0x3) {
+          AnyGen3Endpoint = TRUE;
+        }
+      }
+      MmPci32And (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF);
+    }
+  }
+  DEBUG ((EFI_D_INFO, "Presence detect table...\n"));
+  for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+    DEBUG ((EFI_D_INFO, " PEG%d%d PresenceDetect: %x. EndpointMaxLinkSpeed: %x.\n",
+            PegPortTable[PegComplete].Device,
+            PegPortTable[PegComplete].Function,
+            PegPortTable[PegComplete].PresenceDetect,
+            PegPortTable[PegComplete].EndpointMaxLinkSpeed));
+  }
+
+  ///
+  /// If any Gen3 device, setup equalization values and retrain link
+  ///
+  if (AnyGen3Endpoint &&
+      (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId >= EnumHswB0)) ||
+       ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId >= EnumCrwB0)))) {
+    ///
+    /// Program presets based upon endpoint fullswing value
+    ///
+    for (i = 0; i < SA_PEG_MAX_LANE; i++) {
+      switch (i) {
+        case  0:
+          GetLinkPartnerFullSwing (i, &FullSwing);
+          break;
+        case  8:
+          if (PegPortTable[1].PresenceDetect) {
+            GetLinkPartnerFullSwing (i, &FullSwing);
+          }
+          break;
+        case 12:
+          if (PegPortTable[2].PresenceDetect) {
+            GetLinkPartnerFullSwing (i, &FullSwing);
+          }
+          break;
+        default:
+          break;
+      }
+      GetCoefficientsFromPreset (SaPlatformPolicyPpi->PcieConfig->Gen3EndPointPreset[i], FullSwing, &PreCursor, &Cursor, &PostCursor);
+      SetPartnerTxCoefficients (i, &PreCursor, &Cursor, &PostCursor);
+    }
+
+    ///
+    /// Redo EQ
+    ///
+    for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+      if (PegPortTable[PegComplete].PresenceDetect) {
+        PegBus  = PegPortTable[PegComplete].Bus;
+        PegDev  = PegPortTable[PegComplete].Device;
+        PegFunc = PegPortTable[PegComplete].Function;
+        MmPci32Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL3_OFFSET, BIT0);  ///< DOEQ
+        MmPci16Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL_OFFSET, BIT5);   ///< Retrain link
+      }
+    }
+    for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+      if (PegPortTable[PegComplete].PresenceDetect) {
+        WaitForL0 (PeiServices, StallPpi, &(PegPortTable[PegComplete]), FALSE);
+      }
+    }
+  }
+
+  ///
+  /// Sampler Calibration
+  ///
+  if (SaPlatformPolicyPpi->PcieConfig->PegSamplerCalibrate == 1) {
+    DEBUG ((EFI_D_INFO, "PEG SamplerCalibration...\n"));
+    PegSamplerCalibration (PeiServices, SaPlatformPolicyPpi, StallPpi, CpuSteppingId, HwStrap);
+
+    ///
+    /// Restore the PEG speed
+    ///
+    McD1PciCfg16AndThenOr   (R_SA_PEG_LCTL2_OFFSET, 0xFFF0, Peg10Speed);
+    McD1F1PciCfg16AndThenOr (R_SA_PEG_LCTL2_OFFSET, 0xFFF0, Peg11Speed);
+    McD1F2PciCfg16AndThenOr (R_SA_PEG_LCTL2_OFFSET, 0xFFF0, Peg12Speed);
+
+    ///
+    /// Delay 100ms to let endpoint train properly
+    ///
+    StallPpi->Stall (PeiServices, StallPpi, 100 * STALL_ONE_MILLI_SECOND);
+  }
+  ///
+  /// Gen3 Preset Search: 0 = Disabled, 1 = Enabled (default)
+  ///
+  if (SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch == 2) {
+    SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch = 1;
+  }
+
+  if (SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch == 1) {
+    PegGen3PresetSearch (PeiServices, SaPlatformPolicyPpi, StallPpi, SaDataHob);
+  }
+
+  ///
+  /// After last equalization, set PH3 bypass
+  ///
+  McD1PciCfg32Or (R_SA_PEG_EQCFG_OFFSET, BIT15 | BIT14);
+
+  ///
+  /// Scan PEG Ports for device population
+  ///
+  DEBUG ((EFI_D_INFO, "PEG Ports Scanning starts.\n"));
+  for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+
+    PegBus    = PegPortTable[PegComplete].Bus;
+    PegDev    = PegPortTable[PegComplete].Device;
+    PegFunc   = PegPortTable[PegComplete].Function;
+    PegIndex  = PegPortTable[PegComplete].Index;
+
+    ///
+    /// Check for a card presence in the PEG slot, or if the PEG port exists.
+    ///
+    if ((MmPci16 (0, PegBus, PegDev, PegFunc, PCI_VID) == 0xFFFF) ||
+        (PegPortTable[PegComplete].PresenceDetect == FALSE)) {
+      if (SaPlatformPolicyPpi->PcieConfig->AlwaysEnablePeg == 0) {
+        goto PegDisable;
+      }
+    } else {
+      if (SaPlatformPolicyPpi->PcieConfig->AlwaysEnablePeg == 0) {
+        ///
+        /// Set PEG PortBus = 1 to Read Endpoint.
+        ///
+        MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF, 0x00010100);
+
+        ///
+        /// A config write is required in order for the device to re-capture the Bus number,
+        /// according to PCI Express Base Specification, 2.2.6.2
+        /// Write to a read-only register VendorID to not cause any side effects.
+        ///
+        MmPci16 (0, 1, 0, 0, PCI_VID) = 0;
+
+        ///
+        /// Negotiation Done?
+        ///
+        if ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0RSTS_OFFSET) & BIT1) != 0) {
+          goto PegDisable;
+        }
+
+        ///
+        /// Restore bus numbers on the PEG bridge.
+        ///
+        MmPci32And (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF);
+      }
+
+      if (SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_8) {
+        PwrDnUnusedBundlesSetupData = SaPlatformPolicyPpi->PcieConfig->PowerDownUnusedBundles[PegIndex];
+      } else {
+        PwrDnUnusedBundlesSetupData = 0xFF; ///< Forced to AUTO mode for calculating unused bundles to powerdown
+      }
+
+      if (PwrDnUnusedBundlesSetupData == 0xff) { ///< AUTO mode for calculating unused bundles to powerdown
+        ///
+        /// Read the controller's Max Link Width
+        ///
+        CtrlMaxLinkWidth = (UINT8) ((MmPci32 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCAP_OFFSET) >> 4) & 0x3F);
+
+        ///
+        /// Read the endpoint's Max Link Width
+        ///
+
+        ///
+        /// Set PEG PortBus = 1 to Read Endpoint.
+        ///
+        MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF, 0x00010100);
+
+        ///
+        /// A config write is required in order for the device to re-capture the Bus number,
+        /// according to PCI Express Base Specification, 2.2.6.2
+        /// Write to a read-only register VendorID to not cause any side effects.
+        ///
+        MmPci16 (0, 1, 0, 0, PCI_VID) = 0;
+
+        ///
+        /// Check if the device actually got mapped into config space,
+        /// if the device wasn't able to be mapped into config space then
+        /// it's possible that it's a test card or some other device that
+        /// does not support config space.  In that case our only option is
+        /// to assume that the link trains to its max width and use that to
+        /// determine which bundles to power down
+        ///
+        if (MmPci32 (0, 1, 0, 0, PCI_VID) == 0xFFFFFFFF) {
+          EpMaxLinkWidth = (UINT8) ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS_OFFSET) & 0x3f0) >> 4);
+          DEBUG ((EFI_D_INFO,
+                  "PEG%d%d - Endpoint not responding to PCI config space access, assuming negotiated width (X%d) is max width\n",
+                  PegDev,
+                  PegFunc,
+                  EpMaxLinkWidth
+                ));
+        } else {
+          CapOffset = PcieFindCapId (1, 0, 0, EFI_PCI_CAPABILITY_ID_PCIEXP);
+          if (CapOffset != 0) {
+            EpMaxLinkWidth = (UINT8) ((MmPci32 (0, 1, 0, 0, CapOffset + 0xC) >> 4) & 0x3F);
+          }
+        }
+
+        ///
+        /// Restore bus numbers on the PEG bridge.
+        ///
+        MmPci32And (0, PegBus, PegDev, PegFunc, PCI_PBUS, 0xFF0000FF);
+
+        if (CtrlMaxLinkWidth > EpMaxLinkWidth) {
+          UnusedLanes = CtrlMaxLinkWidth - EpMaxLinkWidth;
+        } else {
+          UnusedLanes = 0;
+        }
+
+        BndlPwrdnCount  = (UnusedLanes / 2); 
+
+        DEBUG ((EFI_D_INFO, "CtrlMLW[%d]. EpMLW[%d]. UnusedLanes[%d]. BndlPwrdnCount[%d].\n", CtrlMaxLinkWidth, EpMaxLinkWidth, UnusedLanes, BndlPwrdnCount));
+        } else if (PwrDnUnusedBundlesSetupData != 0) { ///< User selection mode: 1...8 unused bundles
+          BndlPwrdnCount = PwrDnUnusedBundlesSetupData;
+          DEBUG ((EFI_D_INFO, "BndlPwrdnCount[%d].\n", BndlPwrdnCount));
+        }
+
+      ///
+      /// PowerOff unused lanes for PEGs
+      ///
+      if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId >= EnumHswC0)) ||
+          ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId >= EnumCrwC0))) {
+        if (PwrDnUnusedBundlesSetupData != 0) {
+          MaxBndlPwrdnCount = GetMaxBundles(PeiServices, PegFunc, HwStrap);
+          if (BndlPwrdnCount > MaxBndlPwrdnCount) {
+            BndlPwrdnCount = MaxBndlPwrdnCount;
+            DEBUG ((EFI_D_INFO+EFI_D_ERROR, "BndlPwrdnCount violation! Overriding BndlPwrdnCount! BndlPwrdnCount[%d].\n", BndlPwrdnCount));
+          }
+          PowerDownUnusedBundles(PeiServices, PegFunc, HwStrap, BndlPwrdnCount);
+        }
+      }
+
+      ///
+      /// Additional Programming Steps for PEGs
+      ///
+      DEBUG ((EFI_D_INFO, "Run AdditionalPegProgramSteps on PEG%x%x!\n", PegDev, PegFunc));
+      AdditionalPegProgramSteps (SaPlatformPolicyPpi, PegBus, PegDev, PegFunc);
+    }
+
+    if ((HwStrap == SA_PEG_x16_x0_x0) && (PegIndex == 0)) {
+      DisableFun1 = TRUE;
+      DisableFun2 = TRUE;
+      break;
+    }
+    if ((HwStrap == SA_PEG_x8_x8_x0) && (PegIndex == 1)) {
+      DisableFun2 = TRUE;
+      break;
+    }
+    if ((HwStrap == SA_PEG_x8_x4_x4) && (PegIndex == 2)) {
+      break;
+    }
+
+    continue;
+
+PegDisable:
+    ///
+    /// SA_PEG_x16_x0_x0 Mode: in this mode, PEG11 and PEG12 need to be Disabled by BIOS in this driver.
+    /// Only PEG10 needs to be checked (whether has a VGA device on it) and disabled if not.
+    ///
+    /// SA_PEG_x8_x8_x0 Mode: in this mode, PEG12 needs to be disabled, PEG10 and PEG11
+    /// need to be checked and disabled if no device installed.
+    ///
+    ///
+    /// SA_PEG_x8_x4_x4 Mode: in this mode, all PEG10, PEG11 and PEG12 devices
+    /// need to be checked and disabled if no device installed.
+    ///
+    if (HwStrap == SA_PEG_x16_x0_x0) {
+      DisableFun0 = TRUE;
+      DisableFun1 = TRUE;
+      DisableFun2 = TRUE;
+      break;
+    } else if (HwStrap == SA_PEG_x8_x8_x0) {
+      DisableFun2 = TRUE;
+      if (PegIndex == 0) {
+        DisableFun0 = TRUE;
+      } else {
+        DisableFun1 = TRUE;
+        break;
+      }
+    } else if (HwStrap == SA_PEG_x8_x4_x4) {
+      if (PegIndex == 0) {
+        DisableFun0 = TRUE;
+      } else if (PegIndex == 1) {
+        DisableFun1 = TRUE;
+      } else {
+        DisableFun2 = TRUE;
+        break;
+      }
+    }
+  } ///< End of the for Loop
+
+  if (!DisableFun1 || !DisableFun2) {
+    ///
+    /// PEG10 must be enabled if PEG11 and/or PEG12 are enabled
+    ///
+    if (DisableFun0) {
+      DisableLinkFunc0 = TRUE;
+    }
+
+    DisableFun0 = FALSE;
+  }
+
+  if (MmPci16 (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_VID_OFFSET) != 0xFFFF) {
+    FunctionExists = TRUE;
+  } else {
+    FunctionExists = FALSE;
+    DEBUG ((EFI_D_WARN, "PEG10 Disabled.\n"));
+  }
+  if ((DisableFun0 || DisableLinkFunc0) && FunctionExists) {
+    ///
+    /// Set D1.F0.R 224h [8] = 1
+    ///
+    MmPci32Or (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_LTSSMC_OFFSET, BIT8);
+
+    ///
+    /// DisableLink. Set D1.F0.R 0B0h [4] (LD (Link Disable) bit in Link Control Register
+    /// Set D1.F0.R D10h [0].
+    ///
+    MmPci8Or (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_LCTL_OFFSET, BIT4);
+
+    ///
+    /// Poll until D1.F0.R 464h [5:0] = 2
+    ///
+    LoopCount = 0;
+    while(   ((MmPci32 (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_REUT_PH1_PIS_OFFSET) & 0x3F) != 2)
+          && (LoopCount < SA_PEG_LINK_DISABLE_MAXWAIT)) {
+      StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MICRO_SECOND*100); //100usec
+      LoopCount++;
+    }
+
+    ///
+    /// Program AFEOVR.RXSQDETOVR
+    /// PCIe link disable for Switchable GFx
+    /// Additional Power savings: Set 0:1:0 0xC20 BIT4 = 0 & BIT5 = 1
+    ///
+    MmPci8AndThenOr (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_AFEOVR_OFFSET, ~(BIT5 | BIT4), BIT5);
+
+    if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId >= EnumHswC0)) ||
+        ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId >= EnumCrwC0))) {
+        MaxBndlPwrdnCount = GetMaxBundles(PeiServices, 0, HwStrap);
+        PowerDownUnusedBundles(PeiServices, 0, HwStrap, MaxBndlPwrdnCount);
+    }
+
+    if (DisableFun0) {
+      ///
+      /// Set D1.F0.R D20h [30] to power off PEG lanes when no device is attached (prvtexdetq=1).
+      /// Clear D0.F0.R 054h (DEVEN) enable bit.
+      ///
+      MmPci32Or (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_PEGCOMLCGCTRL_OFFSET, BIT30);
+      MmPci8And (0, SA_MC_BUS, 0, 0, R_SA_DEVEN, ~B_SA_DEVEN_D1F0EN_MASK);
+      DEBUG ((EFI_D_WARN, "PEG10 Disabled.\n"));
+    }
+  }
+
+  if (MmPci16 (0, SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM, R_SA_PEG_VID_OFFSET) != 0xFFFF) {
+    FunctionExists = TRUE;
+  } else {
+    FunctionExists = FALSE;
+    DEBUG ((EFI_D_WARN, "PEG11 Disabled.\n"));
+  }
+  if (DisableFun1 && FunctionExists) {
+    ///
+    /// Set D1.F1.R 224h [8] = 1
+    ///
+    MmPci32Or (0, SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM, R_SA_PEG_LTSSMC_OFFSET, BIT8);
+
+    ///
+    /// DisableLink. Set D1.F1.R 0B0h [4] (LD (Link Disable) bit in Link Control Register.
+    /// Set D1.F1.R D10h [0].
+    /// Set D1.F1.R D20h [30] to power off PEG lanes when no device is attached (prvtexdetq=1).
+    /// Clear B0,D0,F0 054h (DEVEN) enable bit.
+    ///
+    MmPci8Or (0, SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM, R_SA_PEG_LCTL_OFFSET, BIT4);
+
+    ///
+    /// Poll until D1.F0.R 464h [13:8] = 2
+    ///
+    LoopCount = 0;
+    while(   (((MmPci32 (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_REUT_PH1_PIS_OFFSET) >> 8) & 0x3F) != 2)
+          && (LoopCount < SA_PEG_LINK_DISABLE_MAXWAIT)) {
+      StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MICRO_SECOND*100); //100usec
+      LoopCount++;
+    }
+
+    ///
+    /// Program AFEOVR.RXSQDETOVR
+    /// PCIe link disable for Switchable GFx
+    /// Additional Power savings: Set 0:1:1 0xC20 BIT4 = 0 & BIT5 = 1
+    ///
+    MmPci8AndThenOr (0, SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM, R_SA_PEG_AFEOVR_OFFSET, ~(BIT5 | BIT4), BIT5);
+
+    if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId >= EnumHswC0)) ||
+        ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId >= EnumCrwC0))) {
+        MaxBndlPwrdnCount = GetMaxBundles(PeiServices, 1, HwStrap);
+        PowerDownUnusedBundles(PeiServices, 1, HwStrap, MaxBndlPwrdnCount);
+    }
+
+    MmPci32Or (0, SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM, R_SA_PEG_PEGCOMLCGCTRL_OFFSET, BIT30);
+    MmPci8And (0, SA_MC_BUS, 0, 0, R_SA_DEVEN, ~B_SA_DEVEN_D1F1EN_MASK);
+    DEBUG ((EFI_D_WARN, "PEG11 Disabled.\n"));
+  }
+
+  if (MmPci16 (0, SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM, R_SA_PEG_VID_OFFSET) != 0xFFFF) {
+    FunctionExists = TRUE;
+  } else {
+    FunctionExists = FALSE;
+    DEBUG ((EFI_D_WARN, "PEG12 Disabled.\n"));
+  }
+  if (DisableFun2 && FunctionExists) {
+    ///
+    /// Set D1.F2.R 224h [8] = 1
+    ///
+    MmPci32Or (0, SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM, R_SA_PEG_LTSSMC_OFFSET, BIT8);
+
+    ///
+    /// DisableLink. Set D1.F2.R 0B0h [4] (LD (Link Disable) bit in Link Control Register.
+    /// Set D1.F2.R D10h [0].
+    /// Set D1.F2.R D20h [30] to power off PEG lanes when no device is attached (prvtexdetq=1).
+    /// Clear B0,D0,F0 054h (DEVEN) enable bit.
+    ///
+    MmPci8Or (0, SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM, R_SA_PEG_LCTL_OFFSET, BIT4);
+
+    ///
+    /// Poll until D1.F0.R 464h [21:16] = 2
+    ///
+    LoopCount = 0;
+    while(   (((MmPci32 (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_REUT_PH1_PIS_OFFSET) >> 16) & 0x3F) != 2)
+          && (LoopCount < SA_PEG_LINK_DISABLE_MAXWAIT)) {
+      StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MICRO_SECOND*100); //100usec
+      LoopCount++;
+    }
+
+    ///
+    /// Program AFEOVR.RXSQDETOVR
+    /// PCIe link disable for Switchable GFx
+    /// Additional Power savings: Set 0:1:2 0xC20 BIT4 = 0 & BIT5 = 1
+    ///
+    MmPci8AndThenOr (0, SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM, R_SA_PEG_AFEOVR_OFFSET, ~(BIT5 | BIT4), BIT5);
+
+    if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId >= EnumHswC0)) ||
+        ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId >= EnumCrwC0))) {
+        MaxBndlPwrdnCount = GetMaxBundles(PeiServices, 2, HwStrap);
+        PowerDownUnusedBundles(PeiServices, 2, HwStrap, MaxBndlPwrdnCount);
+    }
+
+    MmPci32Or (0, SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM, R_SA_PEG_PEGCOMLCGCTRL_OFFSET, BIT30);
+    MmPci8And (0, SA_MC_BUS, 0, 0, R_SA_DEVEN, ~B_SA_DEVEN_D1F2EN_MASK);
+    DEBUG ((EFI_D_WARN, "PEG12 Disabled.\n"));
+  }
+
+  for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+    PegBus  = PegPortTable[PegComplete].Bus;
+    PegDev  = PegPortTable[PegComplete].Device;
+    PegFunc = PegPortTable[PegComplete].Function;
+    WaitForVc0Negotiation(PeiServices, StallPpi, PegBus, PegDev, PegFunc);
+    ReportPcieLinkStatus(PegBus, PegDev, PegFunc);
+  }
+
+  ///
+  /// Re-check Link again and see if PegLinkFailMask in SaDataHob needed update
+  ///
+  if (SaDataHob != NULL) {
+    LinkStatusGood = 0;
+    if ((BIT1 & MmPci16(0, 0, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_VC0RSTS_OFFSET)) != BIT1) {
+      LinkStatusGood |= BIT0;
+    }
+    if ((BIT1 & MmPci16(0, 0, SA_PEG10_DEV_NUM, SA_PEG11_FUN_NUM, R_SA_PEG_VC0RSTS_OFFSET)) != BIT1) {
+      LinkStatusGood |= BIT1;
+    }
+    if ((BIT1 & MmPci16(0, 0, SA_PEG10_DEV_NUM, SA_PEG12_FUN_NUM, R_SA_PEG_VC0RSTS_OFFSET)) != BIT1) {
+      LinkStatusGood |= BIT2;
+    }
+    if (SaDataHob->PegData.PegLinkFailMask != (UINT8) (~LinkStatusGood)) {
+      DEBUG ((EFI_D_INFO, "Original PegLinkFailMask=%X, Final PegLinkFailMask=%X\n", SaDataHob->PegData.PegLinkFailMask, (UINT8) (~LinkStatusGood)));
+      SaDataHob->PegData.PegLinkFailMask = (UINT8) (~LinkStatusGood);
+    }
+  }
+
+  if ((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_11) &&
+      (SaPlatformPolicyPpi->PcieConfig->PegComplianceTestingMode == 1)) {
+    for (PegComplete = 0; PegComplete < ((sizeof (PegPortTable)) / (sizeof (PEG_PORT))); PegComplete++) {
+      PegBus  = PegPortTable[PegComplete].Bus;
+      PegDev  = PegPortTable[PegComplete].Device;
+      PegFunc = PegPortTable[PegComplete].Function;
+      MmPci32Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_CFG5_OFFSET, BIT0);
+    }
+  }
+
+  ///
+  /// Maximize the dedicated credits for the PEG controllers
+  ///
+  MaximizeSharedCredits();
+  RebalancePegPerformanceCredits (DisableFun0, DisableFun1, DisableFun2);
+  return;
+}
+
+VOID
+ConfigurePegGenX (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEG_PORT               *PegPortTable,
+  IN UINTN                  TableIndex,
+  IN UINT8                  CpuSteppingId,
+  IN UINT8                  Gen3Capable
+  )
+/**
+  Configure PEG GenX mode
+
+  @param[in] PeiServices         - Pointer to the PEI services table
+  @param[in] StallPpi            - Pointer to PEI_STALL_PPI
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] PegPortTable        - Pointer to PEG_PORT array
+  @param[in] TableIndex          - Index in PEG_PORT array
+  @param[in] CpuSteppingId       - CPU stepping
+  @param[in] Gen3Capable         - Selected PEG_PORT is Gen3 capable
+
+  @retval None
+**/
+{
+  UINT8   PegPortGenx;
+  UINT8   PegBus;
+  UINT8   PegDev;
+  UINT8   PegFunc;
+  UINT8   PegIndex;
+  UINT8   MaxLinkWidth;
+  UINT16  LinkSpeed;
+
+  PegBus        = PegPortTable[TableIndex].Bus;
+  PegDev        = PegPortTable[TableIndex].Device;
+  PegFunc       = PegPortTable[TableIndex].Function;
+  PegIndex      = PegPortTable[TableIndex].Index;
+  MaxLinkWidth  = PegPortTable[TableIndex].MaxLinkWidth;
+
+  ///
+  /// Check if this port exists
+  ///
+  if (MmPci16 (0, PegBus, PegDev, PegFunc, PCI_VID) == 0xFFFF) {
+    return;
+  }
+
+  ///
+  /// PegPortGenx: 0 = Auto, 1 = Gen1, 2 = Gen2, 3 = Gen3
+  ///
+  PegPortGenx = SaPlatformPolicyPpi->PcieConfig->PegGenx[PegIndex];
+
+  if (PegPortGenx == PEG_AUTO) {
+    DEBUG ((EFI_D_ERROR, "Auto\n"));
+    LinkSpeed = (UINT16)(MmPci32(0, PegBus, PegDev, PegFunc, R_SA_PEG_LCAP_OFFSET) & 0x0F);
+  } else {
+    LinkSpeed = SaPlatformPolicyPpi->PcieConfig->PegGenx[PegIndex];
+    DEBUG ((EFI_D_ERROR, "Speed From Setup %x\n", LinkSpeed));
+  }
+  ///
+  /// If Gen3 is fused off, limit is Gen2
+  ///
+  if (Gen3Capable == FALSE) {
+    if (LinkSpeed > 2) {
+      LinkSpeed = 2;
+    }
+  }
+  ///
+  /// Set the requested speed in Max Link Speed in LCAP[3:0] and Target Link Speed in LCTL2[3:0].
+  /// Update LCAP.MLW in the same write as it's a Write-Once field
+  ///
+  DEBUG ((EFI_D_INFO, "PEG%x%x (%x:%x:%x) - Max Link Speed = Gen%d\n", PegDev, PegFunc, PegBus, PegDev, PegFunc, LinkSpeed));
+  MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCAP_OFFSET, 0xFFFFFC00, ((UINT32) MaxLinkWidth << 4) | LinkSpeed);
+  MmPci16AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL2_OFFSET, ~(0x0F), LinkSpeed);
+
+  return;
+}
+
+VOID
+AdditionalPegProgramSteps (
+  IN SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi,
+  IN  UINT8                  PegBus,
+  IN  UINT8                  PegDev,
+  IN  UINT8                  PegFunc
+  )
+/**
+  Additional PEG Programming Steps at PEI
+
+  @param[in] SaPlatformPolicyPpi - pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] PegBus      - Pci Bus Number
+  @param[in] PegDev      - Pci Device Number
+  @param[in] PegFunc     - Pci Func Number
+
+  @retval None
+**/
+{
+  UINT32              Data32And;
+  UINT32              Data32Or;
+
+
+  ///
+  /// Set L0SLAT[15:0] to 0x2020
+  ///
+  Data32And = (UINT32) ~(0xFFFF);
+  Data32Or  = 0x00002020;
+  MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_L0SLAT_OFFSET, Data32And, Data32Or);
+
+  ///
+  /// Disable PEG Debug Align Message - set 258[29] = '1b'
+  ///
+  Data32And = (UINT32) ~BIT29;
+  Data32Or  = BIT29;
+  MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_CFG4_OFFSET, Data32And, Data32Or);
+
+  ///
+  /// Retrain the link only if VC0 negotiation is complete at this point.
+  /// This is to support CLB card together with "Aways Enable PEG" option
+  ///
+  if ((MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0RSTS_OFFSET) & BIT1) == 0) {
+    ///
+    /// Set LCTL.RL (0xb0 bit 5) to initiate link retrain
+    ///
+    MmPci8Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL_OFFSET, BIT5);
+    ///
+    /// Wait for Link training complete
+    ///
+    while (MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS_OFFSET) & BIT11) {
+    };
+  }
+
+  return;
+}
+
+VOID
+MaximizeSharedCredits (
+  )
+/**
+  Maximize the dedicated credits for the PEG controllers
+**/
+{
+
+  UINT64 MchBar;
+  UINT32 Crdtctl0;
+  UINT32 Crdtctl1;
+  UINT32 Crdtctl2;
+  UINT32 Crdtctl3;
+  UINT8  Data8;
+  UINT8  Iotrk;
+  UINT8  Rrtrk;
+  BOOL   CommitUpdates;
+  UINT8  i;
+
+  Iotrk = 40;
+  Rrtrk = 71;
+  CommitUpdates = FALSE;
+
+  MchBar = McD0PciCfg64 (R_SA_MCHBAR) & ~BIT0;
+  Crdtctl0 = Mmio32 (MchBar, R_SA_MCHBAR_CRDTCTL0_OFFSET);
+  Crdtctl1 = Mmio32 (MchBar, R_SA_MCHBAR_CRDTCTL1_OFFSET);
+  Crdtctl2 = Mmio32 (MchBar, R_SA_MCHBAR_CRDTCTL2_OFFSET);
+  Crdtctl3 = Mmio32 (MchBar, R_SA_MCHBAR_CRDTCTL3_OFFSET);
+
+  Data8 = 0;
+  for (i = 0; i < 24; i += 3) {
+    Data8 += (Crdtctl0 >> i) & 0x7;
+    Data8 += (Crdtctl1 >> i) & 0x7;
+  }
+  if (Data8 > Iotrk) {
+    DEBUG ((EFI_D_ERROR, "ERROR: Attempted to reserve > %d IOTRK (Attempt = %d)! Skipping programming.\n", Iotrk, Data8));
+    CommitUpdates = FALSE;
+  } else {
+    Iotrk -= Data8;
+    DEBUG ((EFI_D_INFO, "IOTRK: Reserved = %d. Shared = %d. Total = %d.\n", Data8, Iotrk, Data8 + Iotrk));
+  }
+
+  Data8 = 0;
+  for (i = 0; i < 24; i += 3) {
+    Data8 += (Crdtctl2 >> i) & 0x7;
+  }
+  Data8 += (Crdtctl2 >> 24) & 0x3F;
+  if (Data8 > Rrtrk) {
+    DEBUG ((EFI_D_ERROR, "ERROR: Attempted to reserve > %d RRTRK (Attempt = %d)! Skipping programming.\n", Rrtrk, Data8));
+    CommitUpdates = FALSE;
+  } else {
+    Rrtrk -= Data8;
+    DEBUG ((EFI_D_INFO, "RRTRK: Reserved = %d. Shared = %d. Total = %d.\n", Data8, Rrtrk, Data8 + Rrtrk));
+  }
+
+  if (CommitUpdates) {
+    Crdtctl3 = ((Rrtrk & 0x7F) << 6) | (Iotrk & 0x3F);
+    Mmio32AndThenOr (MchBar, R_SA_MCHBAR_CRDTCTL3_OFFSET, (UINT32) ~(0x00001FFF), Crdtctl3);
+  }
+
+  return;
+}
+
+VOID
+RebalancePegPerformanceCredits (
+  IN  BOOLEAN   DisablePeg10,
+  IN  BOOLEAN   DisablePeg11,
+  IN  BOOLEAN   DisablePeg12
+  )
+/**
+  Rebalance Credits when PEG controllers so that no starvation occurs
+
+  @param[in] DisablePeg10     - Peg10 disable/enable status
+  @param[in] DisablePeg11     - Peg11 disable/enable status
+  @param[in] DisablePeg12     - Peg12 disable/enable status
+
+  @retval None
+**/
+{
+  UINT64 MchBar;
+  UINT32 Crdtctl4;
+  UINT32 Crdtctl6;
+  UINT32 Crdtctl8;
+  UINT16 PegLinkWidth10;
+  UINT16 PegLinkWidth11;
+  UINT16 PegLinkWidth12;
+
+  MchBar = McD0PciCfg64 (R_SA_MCHBAR) & ~BIT0;
+  Crdtctl4 = Mmio32 (MchBar, R_SA_MCHBAR_CRDTCTL4_OFFSET);
+  Crdtctl6 = Mmio32 (MchBar, R_SA_MCHBAR_CRDTCTL6_OFFSET);
+  Crdtctl8 = Mmio32 (MchBar, R_SA_MCHBAR_CRDTCTL8_OFFSET);
+
+  DEBUG ((EFI_D_INFO, "Crdtctl4 Crdtctl6 Crdtctl8 Before = %x %x %x\n", Crdtctl4, Crdtctl6, Crdtctl8));
+
+  PegLinkWidth10 = 0;
+  PegLinkWidth11 = 0;
+  PegLinkWidth12 = 0;
+
+  if (!DisablePeg10) {
+    PegLinkWidth10 = (MmPci16 (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_LSTS_OFFSET) & 0x3F0) >> 4;
+  }
+  if (!DisablePeg11) {
+    PegLinkWidth11 = (MmPci16 (0, SA_PEG_BUS_NUM, SA_PEG11_DEV_NUM, SA_PEG11_FUN_NUM, R_SA_PEG_LSTS_OFFSET) & 0x3F0) >> 4;
+  }
+  if (!DisablePeg12) {
+    PegLinkWidth12 = (MmPci16 (0, SA_PEG_BUS_NUM, SA_PEG12_DEV_NUM, SA_PEG12_FUN_NUM, R_SA_PEG_LSTS_OFFSET) & 0x3F0) >> 4;
+  }
+
+  DEBUG ((EFI_D_INFO, "PEG10: LinkDisabled = %x. Width = %x\n", DisablePeg10, PegLinkWidth10));
+  DEBUG ((EFI_D_INFO, "PEG11: LinkDisabled = %x. Width = %x\n", DisablePeg11, PegLinkWidth11));
+  DEBUG ((EFI_D_INFO, "PEG12: LinkDisabled = %x. Width = %x\n", DisablePeg12, PegLinkWidth12));
+
+  ///
+  /// PEG10 = x8 and PEG11 = x8
+  ///
+  if (!DisablePeg10 && !DisablePeg11 && (PegLinkWidth10 == 8) && (PegLinkWidth11 == 8)) {
+    Crdtctl4 &= ~0x3E0;
+    Crdtctl4 |= (Crdtctl4 & 0x7C00) >> 5;
+    Crdtctl6 &= ~0x3E0;
+    Crdtctl6 |= (Crdtctl6 & 0x7C00) >> 5;
+    Crdtctl8 &= ~0xFC0;
+    Crdtctl8 |= (Crdtctl8 & 0x3F000) >> 6;
+  }
+
+  ///
+  /// PEG12 = x4
+  ///
+  if (!DisablePeg12 && (PegLinkWidth12 == 4)) {
+    ///
+    /// PEG10 = x4
+    ///
+    if (!DisablePeg10 && (PegLinkWidth10 == 4)) {
+      Crdtctl4 &= ~0x3E0;
+      Crdtctl4 |= (Crdtctl4 & 0xF8000) >> 10;
+      Crdtctl6 &= ~0x3E0;
+      Crdtctl6 |= (Crdtctl6 & 0xF8000) >> 10;
+      Crdtctl8 &= ~0xFC0;
+      Crdtctl8 |= (Crdtctl8 & 0xFC0000) >> 12;
+    }
+    ///
+    /// PEG11 = x4
+    ///
+    if (!DisablePeg11 && (PegLinkWidth11 == 4)) {
+      Crdtctl4 &= ~0x7C00;
+      Crdtctl4 |= (Crdtctl4 & 0xF8000) >> 5;
+      Crdtctl6 &= ~0x7C00;
+      Crdtctl6 |= (Crdtctl6 & 0xF8000) >> 5;
+      Crdtctl8 &= ~0x3F000;
+      Crdtctl8 |= (Crdtctl8 & 0xFC0000) >> 6;
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "Crdtctl4 Crdtctl6 Crdtctl8 After = %x %x %x\n", Crdtctl4, Crdtctl6, Crdtctl8));
+
+  Mmio32AndThenOr (MchBar, R_SA_MCHBAR_CRDTCTL4_OFFSET, (UINT32) ~0x01FFFFFF, Crdtctl4);
+  Mmio32AndThenOr (MchBar, R_SA_MCHBAR_CRDTCTL6_OFFSET, (UINT32) ~0x01FFFFFF, Crdtctl6);
+  Mmio32AndThenOr (MchBar, R_SA_MCHBAR_CRDTCTL8_OFFSET, (UINT32) ~0x3FFFFFFF, Crdtctl8);
+
+  return;
+}
+
+VOID
+PegPreDetectionSteps (
+  IN  UINT8                   PegBus,
+  IN  UINT8                   PegDev,
+  IN  UINT8                   PegFunc,
+  IN SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  Additional PEG Programming Steps before PEG detection at PEI
+
+  @param[in] PegBus              - Pci Bus Number
+  @param[in] PegDev              - Pci Device Number
+  @param[in] PegFunc             - Pci Func Number
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+**/
+{
+  UINT32        Data32;
+  UINT32        Data32And;
+  UINT32        Data32Or;
+  UINT32        i;
+  CPU_STEPPING  CpuSteppingId;
+  CPU_FAMILY    CpuFamilyId;
+
+  CpuSteppingId = GetCpuStepping();
+  CpuFamilyId   = GetCpuFamily();
+
+
+  Data32Or = (UINT32) (BIT4 | BIT3 | BIT2 | BIT1 | BIT0);
+  Data32And = (UINT32) ~(BIT8);
+  MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_LTSSMC_OFFSET, Data32And, Data32Or);
+
+  ///
+  /// Set PPCIE_CR_REUT_OVR_CTL_0_1_0_MMR.GRCLKGTDIS [28] to 1 (for PCIE Margin Test, Default is kept 0)
+  ///
+  if ((PegDev == 1) && (PegFunc == 0)) {
+    Data32And = (UINT32) ~BIT28;
+    Data32Or  = 0;
+    MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_REUT_OVR_CTL_OFFSET, Data32And, Data32Or);
+  }
+
+  if ((PegDev == 1) && (PegFunc == 0)) {
+    ///
+    /// DCBLNC = 0
+    ///
+    Data32And = (UINT32) ~(BIT3 | BIT2);
+    Data32Or  = 0;
+    for (i = 0; i < SA_PEG_MAX_BUNDLE; i++) {
+      MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_G3CTL0_OFFSET + i * BUNDLE_STEP, Data32And, Data32Or);
+    }
+  }
+
+  ///
+  /// DEBUP3[4] = 1
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuCrw)) {
+    Data32And = (UINT32) ~(BIT4);
+    Data32Or  = BIT4;
+    MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_DEBUP3_OFFSET, Data32And, Data32Or);
+  }
+
+  ///
+  /// FCLKGTTLLL[2] = 1
+  ///
+  if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId < EnumHswC0)) ||
+      ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId < EnumCrwC0))) {
+    Data32And = (UINT32) ~(BIT2);
+    Data32Or  = BIT2;
+    MmPci32AndThenOr (0, PegBus, PegDev, PegFunc, R_SA_PEG_FCLKGTTLLL_OFFSET, Data32And, Data32Or);
+  }
+
+  ///
+  /// Program Read-Only Write-Once Registers
+  ///   R 308h [31:0]
+  ///   R 314h [31:0]
+  ///   R 32Ch [31:0]
+  ///   R 330h [31:0]
+  ///
+  Data32 = MmPci32 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0PRCA_OFFSET);
+  MmPci32          (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0PRCA_OFFSET)  = Data32;
+  Data32 = MmPci32 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0NPRCA_OFFSET);
+  MmPci32          (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC0NPRCA_OFFSET) = Data32;
+  Data32 = MmPci32 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC1PRCA_OFFSET);
+  MmPci32          (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC1PRCA_OFFSET)  = Data32;
+  Data32 = MmPci32 (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC1NPRCA_OFFSET);
+  MmPci32          (0, PegBus, PegDev, PegFunc, R_SA_PEG_VC1NPRCA_OFFSET) = Data32;
+
+  return;
+}
+#endif // PEG_FLAG
+
+#if defined(DMI_FLAG) || defined(PEG_FLAG)
+UINT32
+SendVcuApiSequence (
+  IN    UINT32  MchBar,
+  IN    UINT32  Address,
+  IN    UINT16  OpCode,
+  IN    UINT32  WriteData
+  )
+/**
+  Send one sequence to VCU MailBox
+
+  @param[in] MchBar              - MCHBAR value
+  @param[in] Address             - Target address
+  @param[in] OpCode              - OpCode number
+  @param[in] WriteData           - Data value (only used if OpCode is a write)
+**/
+{
+  BOOL              IsWrite;
+  BOOL              IsCsr;
+  UINT32            DataOpCode;
+  UINT32            SequenceId;
+  UINT32            VcuData;
+  CPU_FAMILY        CpuFamilyId;
+  CPU_STEPPING      CpuSteppingId;
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    if ((OpCode == V_SA_VCU_OPCODE_WRITE_CSR_REV1) || (OpCode == V_SA_VCU_OPCODE_WRITE_MMIO_REV1)) {
+      IsWrite = TRUE;
+    } else {
+      IsWrite = FALSE;
+    }
+
+    if ((OpCode == V_SA_VCU_OPCODE_READ_CSR_REV1) || (OpCode == V_SA_VCU_OPCODE_WRITE_CSR_REV1)) {
+      IsCsr = TRUE;
+    } else {
+      IsCsr = FALSE;
+    }
+  } else {
+    if ((OpCode == V_SA_VCU_OPCODE_WRITE_CSR_REV2) || (OpCode == V_SA_VCU_OPCODE_WRITE_MMIO_REV2)) {
+      IsWrite = TRUE;
+    } else {
+      IsWrite = FALSE;
+    }
+
+    if ((OpCode == V_SA_VCU_OPCODE_READ_CSR_REV2) || (OpCode == V_SA_VCU_OPCODE_WRITE_CSR_REV2)) {
+      IsCsr = TRUE;
+    } else {
+      IsCsr = FALSE;
+    }
+  }
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    if (IsWrite) {
+      DataOpCode = V_SA_VCU_OPCODE_WRITE_DATA_REV1;
+      if (IsCsr) {
+        SequenceId = V_SA_VCU_SEQID_WRITE_CSR_REV1;
+      } else {
+        SequenceId = V_SA_VCU_SEQID_WRITE_MMIO_REV1;
+      }
+    } else {
+      DataOpCode = V_SA_VCU_OPCODE_READ_DATA_REV1;
+      if (IsCsr) {
+        SequenceId = V_SA_VCU_SEQID_READ_CSR_REV1;
+      } else {
+        SequenceId = V_SA_VCU_SEQID_READ_MMIO_REV1;
+      }
+    }
+  } else {
+    if (IsWrite) {
+      DataOpCode = V_SA_VCU_OPCODE_WRITE_DATA_REV2;
+      if (IsCsr) {
+        SequenceId = V_SA_VCU_SEQID_WRITE_CSR_REV2;
+      } else {
+        SequenceId = V_SA_VCU_SEQID_WRITE_MMIO_REV2;
+      }
+    } else {
+      DataOpCode = V_SA_VCU_OPCODE_READ_DATA_REV2;
+      if (IsCsr) {
+        SequenceId = V_SA_VCU_SEQID_READ_CSR_REV2;
+      } else {
+        SequenceId = V_SA_VCU_SEQID_READ_MMIO_REV2;
+      }
+    }
+  }
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_OPEN_SEQ_REV1, SequenceId);
+  } else {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_OPEN_SEQ_REV2, SequenceId);
+  }
+  SendVcuApiCmd (MchBar, OpCode, Address);
+  SendVcuApiCmd (MchBar, DataOpCode, WriteData);
+  VcuData = Mmio32 (MchBar, R_SA_MCHBAR_VCU_MAILBOX_DATA_OFFSET);
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_CLOSE_SEQ_REV1, SequenceId);
+  } else {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_CLOSE_SEQ_REV2, SequenceId);
+  }
+
+  return VcuData;
+}
+
+VOID
+SendVcuApiCmd (
+  IN    UINT32  MchBar,
+  IN    UINT32  Interface,
+  IN    UINT32  Data
+  )
+/**
+  Send one command to VCU MailBox
+
+  @param[in] MchBar              - MCHBAR value
+  @param[in] Interface           - Interface number
+  @param[in] Data                - Data value
+**/
+{
+  UINT32  ResponseCounter;
+  UINT16  ResponseCode;
+  UINT32  BusyCounter;
+  UINT32  RunBusy;
+  BOOL    BusyStatus;
+
+  ResponseCode = V_SA_VCU_RESPONSE_SUCCESS;
+  for (ResponseCounter = 0; ResponseCounter < V_SA_VCU_RESPONSE_RETRY_LIMIT; ResponseCounter++) {
+    Mmio32 (MchBar, R_SA_MCHBAR_VCU_MAILBOX_DATA_OFFSET) = Data;
+    Mmio32 (MchBar, R_SA_MCHBAR_VCU_MAILBOX_INTERFACE_OFFSET) = (Interface | B_SA_MCHBAR_VCU_STATUS_RUN_BUSY);
+    BusyStatus = FALSE;
+    for (BusyCounter = 0; BusyCounter < V_SA_VCU_STATUS_BUSY_LIMIT; BusyCounter++) {
+      RunBusy     = Mmio32 (MchBar, R_SA_MCHBAR_VCU_MAILBOX_INTERFACE_OFFSET);
+      BusyStatus  = (RunBusy & B_SA_MCHBAR_VCU_STATUS_RUN_BUSY) ? TRUE : FALSE;
+      if (BusyStatus == FALSE) {
+        break;
+      }
+    }
+
+    if (BusyStatus) {
+      DEBUG ((EFI_D_INFO, "VCU Busy Timeout after %d tries: MCHBAR=%8.8X. Interface=%8.8X. Data=%4.4X.\n", BusyCounter, MchBar, Interface, Data));
+    }
+    ResponseCode = Mmio16 (MchBar, R_SA_MCHBAR_VCU_MAILBOX_INTERFACE_OFFSET);
+    if (ResponseCode == V_SA_VCU_RESPONSE_SUCCESS) {
+      break;
+    }
+  }
+
+  if (ResponseCode != V_SA_VCU_RESPONSE_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "ERROR: VCU Response Error after %d tries: MCHBAR=%8.8X. Interface=%4.4X. Data=%8.8X. ResponseCode=%4.4X\n", ResponseCounter, MchBar, Interface, Data, ResponseCode));
+  }
+
+  return;
+}
+#endif // DMI_FLAG || PEG_FLAG
+
+#ifdef PEG_FLAG
+
+UINT8
+GetMaxBundles (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN UINT8                     PegFunc,
+  IN UINT8                     HwStrap
+  )
+/**
+  GetMaxBundles: Get the maximum bundle numbers for the corresponding PEG
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] PegFunc              - Points to PEG0/PEG1/PEG2/...
+  @param[in] HwStrap              - Points to PEG configuration information [x16_x0_x0/x8_x8_x0/x8_x4_x4/...]
+
+  @retval                         - MaxBndlPwrdnCount [Maximun number of bundles for this HW configuration]
+**/
+{
+  UINT8  MaxBndlPwrdnCount;
+
+  MaxBndlPwrdnCount  = 0;
+
+  DEBUG ((EFI_D_INFO, "In GetMaxBundles procedure\n"));
+
+  if (PegFunc == 0) { // PEG10
+    if (HwStrap == SA_PEG_x16_x0_x0) {
+      MaxBndlPwrdnCount = 8;
+    } else {
+      MaxBndlPwrdnCount = 4;
+    }
+  } else if (PegFunc == 1) { // PEG11
+    if (HwStrap == SA_PEG_x8_x8_x0) {
+      MaxBndlPwrdnCount = 4;
+    } else if (HwStrap == SA_PEG_x8_x4_x4) {
+      MaxBndlPwrdnCount = 2;
+    }
+  } else if (PegFunc == 2) { // PEG12
+    if (HwStrap == SA_PEG_x8_x4_x4) {
+      MaxBndlPwrdnCount = 2;
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "MaxBndlPwrdnCount = %d\n", MaxBndlPwrdnCount));
+  return MaxBndlPwrdnCount;
+}
+
+VOID
+PowerDownUnusedBundles (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN UINT8                     PegFunc,
+  IN UINT8                     HwStrap,
+  IN UINT8                     BndlPwrdnCount
+  )
+/**
+  PowerDownUnusedBundles: Program the PEG BundleSpare registers for power on sequence [PowerOff unused bundles for PEGs]
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] PegFunc              - Points to PEG0/PEG1/PEG2/...
+  @param[in] HwStrap              - Points to PEG configuration information [x16_x0_x0/x8_x8_x0/x8_x4_x4/...]
+  @param[in] BndlPwrdnCount       - Points to how many bundles are unused and should be powered down
+**/
+{
+  BOOLEAN  PegLaneReversal;
+  UINT8    BndlPwrdnFirst;
+
+  UINT8    i;
+  UINT8    j;
+
+  PegLaneReversal = FALSE;
+  BndlPwrdnFirst  = 0;
+
+  DEBUG ((EFI_D_INFO, "In PowerDownUnusedBundles sequence\n"));
+
+  if (BndlPwrdnCount == 0) {
+    ///
+    /// If all lanes are used. Do nothing
+    ///
+    DEBUG ((EFI_D_INFO, "All lanes are used. Do nothing.\n"));
+    return;
+  }
+
+  if ((MmPci32 (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_PEGTST_OFFSET) & BIT20) != 0) {
+    DEBUG ((EFI_D_INFO, "PegLaneReversal is true\n"));
+    PegLaneReversal = TRUE;
+  }
+
+  if (PegFunc == 0) { // PEG10
+    if (HwStrap == SA_PEG_x16_x0_x0) {
+      if (!PegLaneReversal) {
+        BndlPwrdnFirst = 8 - BndlPwrdnCount;
+      } else {
+        BndlPwrdnFirst = 0;
+      }
+    } else {
+      if (!PegLaneReversal) {
+        BndlPwrdnFirst = 4 - BndlPwrdnCount;
+      } else {
+        BndlPwrdnFirst = 4;
+      }
+    }
+  } else if (PegFunc == 1) { // PEG11
+    if (HwStrap == SA_PEG_x8_x8_x0) {
+      if (!PegLaneReversal) {
+        BndlPwrdnFirst = 8 - BndlPwrdnCount;
+      } else {
+        BndlPwrdnFirst = 0;
+      }
+    } else if (HwStrap == SA_PEG_x8_x4_x4) {
+      if (!PegLaneReversal) {
+        BndlPwrdnFirst = 6 - BndlPwrdnCount;
+      } else {
+        BndlPwrdnFirst = 2;
+      }
+    }
+  } else if (PegFunc == 2) { // PEG12
+    if (HwStrap == SA_PEG_x8_x4_x4) {
+      if (!PegLaneReversal) {
+        BndlPwrdnFirst = 8 - BndlPwrdnCount;
+      } else {
+        BndlPwrdnFirst = 0;
+      }
+    }
+  }
+
+  ///
+  /// Power down unused lanes per request
+  ///
+  DEBUG ((EFI_D_INFO, "BNDL_PWRDN PEG%d%d[%d:%d]\n", 0, PegFunc, BndlPwrdnFirst, (BndlPwrdnFirst+BndlPwrdnCount-1)));
+  for (i = BndlPwrdnFirst, j=1; j <= BndlPwrdnCount; i++, j++) {
+    MmPci32Or (0, SA_PEG_BUS_NUM, SA_PEG10_DEV_NUM, SA_PEG10_FUN_NUM, R_SA_PEG_BND0SPARE_OFFSET + (i * BUNDLE_STEP), BIT31);
+  }
+
+  return;
+}
+#endif // PEG_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PciExpressInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PciExpressInit.h
new file mode 100644
index 0000000..2ca1bbe
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PciExpressInit.h
@@ -0,0 +1,452 @@
+/** @file
+  PciExpressInit header file
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#ifndef _PCIEXPRESS_INIT_H_
+#define _PCIEXPRESS_INIT_H_
+
+#include "EdkIIGluePeim.h"
+#include "SaAccess.h"
+#include "PchAccess.h"
+#include "EdkIIGluePcdPciExpressLib.h"
+#include "EdkIIGlueConfig.h"
+#include "Pci30.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+
+///
+/// Driver Consumed PPI Prototypes
+///
+#include EFI_PPI_DEPENDENCY (Stall)
+#include EFI_PPI_DEPENDENCY (SaPlatformPolicy)
+
+#define PEG_AUTO    0
+#define PEG_GEN1    1
+#define PEG_GEN2    2
+#define PEG_GEN3    3
+
+#define DMI_GEN1    1
+#define DMI_GEN2    2
+
+#define LANE_STEP   0x10
+#define BUNDLE_STEP 0x20
+
+typedef struct {
+  UINT8   Bus;
+  UINT8   Device;
+  UINT8   Function;
+  UINT8   Index;
+  BOOLEAN PresenceDetect;
+  UINT8   MaxLinkWidth;
+  UINT8   EndpointMaxLinkSpeed;
+} PEG_PORT;
+
+///
+/// Data structure used in Sampler Calibration
+///
+#pragma pack(1)
+typedef struct _SAMPLE {
+  UINT8   Data;
+  UINT16  Count;
+} DATA_SAMPLE;
+#pragma pack()
+
+#define MAX_CODES 10
+
+///
+/// Data structure used in Preset Search
+///
+typedef struct _PRESET_DATA {
+  UINT8 Preset;
+  UINTN TimingMargin[SA_PEG_MAX_BUNDLE];
+  UINTN VoltageUpMargin[SA_PEG_MAX_BUNDLE];
+  UINTN VoltageDownMargin[SA_PEG_MAX_BUNDLE];
+} PRESET_DATA;
+
+#define MAX_PRESETS 3
+
+#define SA_PEG_SAMPLER_ITERATIONS  500
+///
+/// Common register access macros - use either DMIBAR or PEG10
+///
+#define SaMmio32AndThenOr(BaseAddr, Device, Register, AndData, OrData) \
+  if (BaseAddr != 0) { \
+    Mmio32AndThenOr (BaseAddr, Register, AndData, OrData); \
+  } else { \
+    MmPci32AndThenOr (0, 0, Device, 0, Register, AndData, OrData); \
+  }
+
+#ifdef PEG_FLAG
+VOID
+BubbleSort (
+  IN OUT DATA_SAMPLE Array[]
+  )
+/**
+  Bubble sort from DATA_SAMPLE
+
+  @param[in, out] - Array[]: array of DATA_SAMPLE
+
+  @retval None
+**/
+;
+
+UINT32
+GetMiddleValue (
+  IN OUT DATA_SAMPLE Array[]
+  )
+/**
+  Get Middle Value from DATA_SAMPLE
+
+  @param[in, out] - Array[]: array of DATA_SAMPLE
+
+  @retval UINT32 : Middle Value of DATA_SAMPLE
+**/
+;
+
+VOID
+SetLoadBus (
+  IN UINT32 DmiBar,
+  IN UINTN  Dev,
+  IN UINTN  Lane,
+  IN UINT32 LoadSel,
+  IN UINT32 LoadData,
+  IN UINT8  CpuSteppingId
+  )
+/**
+  Set Load Bus
+
+  @param[in] DmiBar        - DMIBAR address
+  @param[in] Dev           - Device Number
+  @param[in] Lane          - Number of Lane
+  @param[in] LoadSel       - Load selection value
+  @param[in] LoadData      - Load Data
+  @param[in] CpuSteppingId - CPUID.1.EAX[3:0], CPU stepping ID
+
+  @retval None
+**/
+;
+
+UINT32
+GetMonBus (
+  IN UINT32 DmiBar,
+  IN UINTN  Dev,
+  IN UINTN  Lane,
+  IN UINT32 LoadSel,
+  IN UINT8  CpuSteppingId
+  )
+/**
+  Get monitor bus from the lane selected
+
+  @param[in] DmiBar        - DMIBAR address
+  @param[in] Dev           - Device number
+  @param[in] Lane          - Number of Lane
+  @param[in] LoadSel       - Load selection value
+  @param[in] LoadData      - Load selecttion data
+  @param[in] CpuSteppingId - CPUID.1.EAX[3:0], CPU stepping ID
+
+  @retval UINT32 - Load bus address
+**/
+;
+
+VOID
+DumpSamplerValues (
+  IN UINT32 DmiBar,
+  IN UINT8  CpuSteppingId,
+  IN UINTN  Dev,
+  IN UINTN  LanesCount
+  )
+/**
+  Dump Sampler Values
+
+  @param[in] DmiBar        - DMIBAR address
+  @param[in] CpuSteppingId - CPUID.1.EAX[3:0], CPU stepping ID
+  @param[in] Dev           - Device number
+  @param[in] LanesCount    - Value of Lanes
+
+  @retval None
+**/
+;
+#endif // PEG_FLAG
+
+#if defined(DMI_FLAG) || defined(PEG_FLAG)
+VOID
+PegDmiRecipe (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN UINT32                 MchBar,
+  IN UINT32                 DmiBar,
+  IN UINTN                  Dev,
+  IN UINT8                  SwingControl
+  )
+/**
+  Perform PEG/DMI PCIe Recipe steps
+
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] MchBar              - MCHBAR or zero if called for PEG
+  @param[in] DmiBar              - DMIBAR or zero if called for PEG
+  @param[in] Dev                 - PEG device number: 1 for PEG10, 0 if called for DMI.
+  @param[in] SwingControl        - 1 = Half, 2 = Full
+
+  @retval None
+**/
+;
+#endif // DMI_FLAG || PEG_FLAG
+
+#ifdef PEG_FLAG
+VOID
+ConfigurePegGenX (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEG_PORT               *PegPortTable,
+  IN UINTN                  TableIndex,
+  IN UINT8                  CpuSteppingId,
+  IN UINT8                  Gen3Capable
+  )
+/**
+  Configure PEG GenX mode
+
+  @param[in] PeiServices         - Pointer to the PEI services table
+  @param[in] StallPpi            - Pointer to PEI_STALL_PPI
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] PegPortTable        - Pointer to PEG_PORT array
+  @param[in] TableIndex          - Index in PEG_PORT array
+  @param[in] CpuSteppingId       - CPU stepping
+  @param[in] Gen3Capable         - Selected PEG_PORT is Gen3 capable
+
+  @retval None
+**/
+;
+
+VOID
+AdditionalPegProgramSteps (
+  IN SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi,
+  IN  UINT8                  PegBus,
+  IN  UINT8                  PegDev,
+  IN  UINT8                  PegFunc
+  )
+/**
+  Additional PEG Programming Steps at PEI
+
+  @param[in] SaPlatformPolicyPpi - SaPlatformPolicyPpi to access the GtConfig related information
+  @param[in] PegBus      - Pci Bus Number
+  @param[in] PegDev      - Pci Device Number
+  @param[in] PegFunc     - Pci Func Number
+**/
+;
+
+VOID
+MaximizeSharedCredits (
+  )
+/**
+  Maximize the dedicated credits for the PEG controllers
+**/
+;
+
+VOID
+RebalancePegPerformanceCredits (
+  IN  BOOLEAN   DisablePeg10,
+  IN  BOOLEAN   DisablePeg11,
+  IN  BOOLEAN   DisablePeg12
+  )
+/**
+  Rebalance Credits when PEG controllers so that no stavation occurs
+
+  @param[in] DisablePeg10     - Peg10 disable/enable status
+  @param[in] DisablePeg11     - Peg11 disable/enable status
+  @param[in] DisablePeg12     - Peg12 disable/enable status
+
+  @retval None
+**/
+;
+
+VOID
+PegPreDetectionSteps (
+  IN  UINT8                   PegBus,
+  IN  UINT8                   PegDev,
+  IN  UINT8                   PegFunc,
+  IN SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  Additional PEG Programming Steps before PEG detection at PEI
+
+  @param[in] PegBus              - Pci Bus Number
+  @param[in] PegDev              - Pci Device Number
+  @param[in] PegFunc             - Pci Func Number
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+**/
+;
+#endif // PEG_FLAG
+
+#if defined(DMI_FLAG) || defined(PEG_FLAG)
+UINT32
+SendVcuApiSequence (
+  IN    UINT32  MchBar,
+  IN    UINT32  Address,
+  IN    UINT16  OpCode,
+  IN    UINT32  WriteData
+  )
+/**
+  Send one sequence to VCU MailBox
+
+  @param[in] MchBar              - MCHBAR value
+  @param[in] Address             - Target address
+  @param[in] OpCode              - OpCode number
+  @param[in] WriteData           - Data value (only used if OpCode is a write)
+**/
+;
+
+VOID
+SendVcuApiCmd (
+  IN    UINT32  MchBar,
+  IN    UINT32  Interface,
+  IN    UINT32  Data
+  )
+/**
+  Send one command to VCU MailBox
+
+  @param[in] MchBar              - MCHBAR value
+  @param[in] Interface           - Interface number
+  @param[in] Data                - Data value
+**/
+;
+
+VOID
+ReportPcieLinkStatus (
+  IN UINT8             PegBus,
+  IN UINT8             PegDev,
+  IN UINT8             PegFunc
+)
+/**
+  This function reports a PEG controller's link status
+
+  @param[in] PegBus       - Peg Bus
+  @param[in] PegDev       - Peg Device
+  @param[in] PegFunc      - Peg Function
+
+  @retval None
+**/
+;
+
+VOID
+WaitForVc0Negotiation (
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN PEI_STALL_PPI     *StallPpi,
+  IN UINT8             PegBus,
+  IN UINT8             PegDev,
+  IN UINT8             PegFunc
+)
+/**
+  This function prints the time required for VC0 Negotiation Pending to be cleared. Quits after 100 msec.
+
+  @param[in] PeiServices  - Pointer to the PEI services table
+  @param[in] StallPpi     - Pointer to PEI_STALL_PPI
+  @param[in] PegBus       - Peg Bus
+  @param[in] PegDev       - Peg Device
+  @param[in] PegFunc      - Peg Function
+
+  @retval None
+**/
+;
+
+UINT32
+PcieFindCapId (
+  IN UINT8   Bus,
+  IN UINT8   Device,
+  IN UINT8   PegFunc,
+  IN UINT8   CapId
+  )
+/**
+  Find the Offset to a given Capabilities ID
+  CAPID list:
+    0x01 = PCI Power Management Interface
+    0x04 = Slot Identification
+    0x05 = MSI Capability
+    0x10 = PCI Express Capability
+
+  @param[in] Bus       -   Pci Bus Number
+  @param[in] Device    -   Pci Device Number
+  @param[in] PegFunc   -   Pci Function Number
+  @param[in] CapId     -   CAPID to search for
+
+  @retval 0       - CAPID not found
+  @retval Other   - CAPID found, Offset of desired CAPID
+**/
+;
+
+VOID
+ProgramPreset (
+  IN UINT8  Direction,
+  IN UINT8  PresetValue,
+  IN UINT8  PegFunc,
+  IN UINT8  Lane
+  )
+/**
+  Program PEG Gen3 preset value
+
+  @param[in] Direction     - 0 = Root Port, 1 = End Point
+  @param[in] PresetValue   - Preset value to program
+  @param[in] PegFunc       - Peg function number to be configured
+  @param[in] Lane          - Lane to be configured
+
+  @retval None
+**/
+;
+
+#endif // DMI_FLAG || PEG_FLAG
+
+#ifdef PEG_FLAG
+UINT8
+GetMaxBundles (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN UINT8                     PegFunc,
+  IN UINT8                     HwStrap
+  )
+/**
+  GetMaxBundles: Get the maximum bundle numbers for the corresponding PEG
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] PegFunc              - Points to PEG0/PEG1/PEG2/...
+  @param[in] HwStrap              - Points to PEG configuration information [x16_x0_x0/x8_x8_x0/x8_x4_x4/...]
+
+  @retval                         - MaxBndlPwrdnCount [Maximun number of bundles for this HW configuration]
+**/
+;
+
+VOID
+PowerDownUnusedBundles (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN UINT8                     PegFunc,
+  IN UINT8                     HwStrap,
+  IN UINT8                     BndlPwrdnCount
+  )
+/**
+  PowerDownUnusedBundles: Program the PEG BundleSpare registers for power on sequence [PowerOff unused bundles for PEGs]
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] PegFunc              - Points to PEG0/PEG1/PEG2/...
+  @param[in] HwStrap              - Points to PEG configuration information [x16_x0_x0/x8_x8_x0/x8_x4_x4/...]
+  @param[in] BndlPwrdnCount       - Points to how many bundles are unused and should be powered down
+
+  @retval                         - None
+**/
+;
+#endif // PEG_FLAG
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTraining.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTraining.c
new file mode 100644
index 0000000..26f16be
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTraining.c
@@ -0,0 +1,628 @@
+/** @file
+  This driver trains the PEG interface.
+
+@copyright
+  Copyright (c) 2012 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+#include "PcieTraining.h"
+
+#ifdef PEG_FLAG
+
+UINT16
+GetErrorTarget (
+  IN  SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  )
+{
+  UINT16  ErrorTarget;
+
+  if ((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_3) &&
+      (SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchErrorTarget >= 1)) {
+    ErrorTarget = SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchErrorTarget;
+  } else {
+    ErrorTarget = 4;
+  }
+
+  return ErrorTarget;
+}
+
+VOID
+GetPortInfo (
+  OUT PORT_INFO *PortInfoList,
+  OUT UINT8     *PortInfoListLength,
+  OUT BOOLEAN   *SkipBundle0
+  )
+{
+  UINT32            HwStrap;
+  UINT8             PegBus;
+  UINT8             PegDev;
+  UINT8             PcieController;
+  UINT8             Index;
+  UINT8             Lane;
+  UINT8             LaneIndex;
+  UINT8             FurcationSetup[SA_PEG_MAX_FUN];
+  UINT8             PcieControllerList[SA_PEG_MAX_FUN];
+  UINT8             NumberToCheck;
+  UINT8             StartLane;
+  UINT8             Width;
+  UINT32            Lcap;
+  UINT32            CapOffset;
+  CPU_FAMILY        CpuFamilyId;
+  CPU_STEPPING      CpuSteppingId;
+
+  PegBus        = SA_MC_BUS;
+  PegDev        = 1;
+  HwStrap       = (McD1PciCfg32(R_SA_PEG_FUSESCMN_OFFSET) >> 16) & 0x3;
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  switch(HwStrap) {
+    case SA_PEG_x8_x4_x4:
+      FurcationSetup[0] = 8;
+      FurcationSetup[1] = 4;
+      FurcationSetup[2] = 4;
+      NumberToCheck     = 3;
+      break;
+    case SA_PEG_x8_x8_x0:
+      FurcationSetup[0] = 8;
+      FurcationSetup[1] = 8;
+      NumberToCheck     = 2;
+      break;
+    default:
+    case SA_PEG_x16_x0_x0:
+      FurcationSetup[0] = 16;
+      NumberToCheck     = 1;
+      break;
+  }
+
+  ///
+  /// Figure out which PcieControllers are enabled
+  ///
+  (*PortInfoListLength) = 0;
+  for (PcieController = 0; PcieController < NumberToCheck; PcieController++) {
+    ///
+    /// Sanity check to make sure width > 0
+    ///
+    if (FurcationSetup[PcieController] == 0) {
+      continue;
+    }
+
+    ///
+    /// Check to make sure the Root Port Exists
+    ///
+    if (MmPci16 (0, PegBus, PegDev, PcieController, PCI_VID) == 0xFFFF) {
+      continue;
+    }
+
+    ///
+    /// Add the PcieController to the list of enabled controllers
+    ///
+    PcieControllerList[(*PortInfoListLength)] = PcieController;
+    (*PortInfoListLength)++;
+  }
+
+  ///
+  /// If needed, skip Bundle 0's preset search and use Bundle 1's preset instead.
+  ///
+  (*SkipBundle0) = FALSE;
+  if (((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId < EnumHswC0)) ||
+      ((CpuFamilyId == EnumCpuCrw) && (CpuSteppingId < EnumCrwC0))) {
+    (*SkipBundle0) = TRUE;
+  }
+  if ((*SkipBundle0)) {
+    DEBUG ((EFI_D_INFO, "Skipping each controller's Lane 0-1 preset searches; using their Lane 2 preset instead.\n"));
+  }
+
+  StartLane = 0;
+  for (Index = 0; Index < (*PortInfoListLength); Index++) {
+    PcieController = PcieControllerList[Index];
+
+    ///
+    /// Get information for the current port
+    ///
+    (PortInfoList[Index]).EnableMargin                 = TRUE;
+    (PortInfoList[Index]).FoundUsablePreset            = FALSE;
+    (PortInfoList[Index]).PegPort.Bus                  = PegBus;
+    (PortInfoList[Index]).PegPort.Device               = PegDev;
+    (PortInfoList[Index]).PegPort.Function             = PcieController;
+    (PortInfoList[Index]).PegPort.Index                = PcieController;
+    (PortInfoList[Index]).PegPort.EndpointMaxLinkSpeed = 0;
+    ReportPcieLinkStatus (PegBus, PegDev, PcieController);
+    Width                                              = GetNegotiatedWidth(&((PortInfoList[Index]).PegPort));
+    (PortInfoList[Index]).LaneListLength               = Width;
+    for (Lane = 0, LaneIndex = 0; Lane < Width; Lane++) {
+      if ((*SkipBundle0)) {
+        if ((Lane == 0) || (Lane == 1)) {
+          (PortInfoList[Index]).LaneListLength--;
+          continue;
+        }
+      }
+      if (LaneIndex < SA_PEG_MAX_LANE) {
+        (PortInfoList[Index]).LaneList[LaneIndex] = (Lane + StartLane);
+      }
+      LaneIndex++;
+    }
+
+    ///
+    /// Check that both root port and endpoint support Gen3
+    ///
+    Lcap = MmPci32 (0, PegBus, PegDev, PcieController, R_SA_PEG_LCAP_OFFSET);
+    if ((Lcap & 0x0F) != 3) {
+      DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - Root Port is not Gen3-capable. Max Link Speed = %d.\n",
+        PegDev, PcieController, PegBus, PegDev, PcieController, Lcap & 0x0F));
+      (PortInfoList[Index]).LinkIsGen3Capable = FALSE;
+    } else {
+
+      DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - Root Port is Gen3-capable.\n",
+        PegDev, PcieController, PegBus, PegDev, PcieController));
+
+      ///
+      /// Set PEG PortBus = 1 to Read Endpoint.
+      ///
+      MmPci32AndThenOr (0, PegBus, PegDev, PcieController, PCI_PBUS, 0xFF0000FF, 0x00010100);
+
+      ///
+      /// A config write is required in order for the device to re-capture the Bus number,
+      /// according to PCI Express Base Specification, 2.2.6.2
+      /// Write to a read-only register VendorID to not cause any side effects.
+      ///
+      MmPci16 (0, 1, 0, 0, PCI_VID) = 0;
+
+      ///
+      /// Save end point vendor id and device id
+      ///
+      (PortInfoList[Index]).EndPointVendorIdDeviceId = MmPci32 (0, 1, 0, 0, 0);
+
+      ///
+      /// Negotiation Done?
+      ///
+      if ((MmPci16 (0, PegBus, PegDev, PcieController, R_SA_PEG_VC0RSTS_OFFSET) & BIT1) != 0) {
+        (PortInfoList[Index]).LinkIsGen3Capable = FALSE;
+        DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - VC0 negotiation is pending! Skipping endpoint.\n",
+          PegDev, PcieController, PegBus, PegDev, PcieController, Lcap & 0x0F));
+        ReportPcieLinkStatus (PegBus, PegDev, PcieController);
+      } else {
+        ///
+        /// Get the pointer to the Port PCI Express Capability Structure.
+        ///
+        CapOffset = PcieFindCapId (1, 0, 0, EFI_PCI_CAPABILITY_ID_PCIEXP);
+        if (CapOffset == 0) {
+          (PortInfoList[Index]).LinkIsGen3Capable = FALSE;
+          DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - Endpoint is not Gen3-capable. No PCIe Capability found.\n",
+            PegDev, PcieController, PegBus, PegDev, PcieController, Lcap & 0x0F));
+        } else {
+          Lcap = MmPci32 (0, 1, 0, 0, CapOffset + 0x0C);
+          (PortInfoList[Index]).PegPort.EndpointMaxLinkSpeed = Lcap & 0x0F;
+          if ((Lcap & 0x0F) < 3) {
+            (PortInfoList[Index]).LinkIsGen3Capable = FALSE;
+            DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - Endpoint is not Gen3-capable. Max Link Speed = %d.\n",
+              PegDev, PcieController, PegBus, PegDev, PcieController, Lcap & 0x0F));
+          } else {
+            (PortInfoList[Index]).LinkIsGen3Capable = TRUE;
+            DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - Endpoint is Gen3-capable\n",
+              PegDev, PcieController, PegBus, PegDev, PcieController, Lcap & 0x0F));
+          }
+        }
+      }
+      ///
+      /// Restore bus numbers on the PEG bridge.
+      ///
+      MmPci32And (0, PegBus, PegDev, PcieController, PCI_PBUS, 0xFF0000FF);
+    }
+
+    StartLane += FurcationSetup[PcieController];
+  } ///< End of for each port
+
+  return;
+}
+
+EFI_STATUS
+RunMarginTest (
+  IN  EFI_PEI_SERVICES        **PeiServices,
+  IN  SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi,
+  IN  SA_DATA_HOB             *SaDataHob,
+  IN  PEI_STALL_PPI           *StallPpi,
+  IN  UINT32                  MonitorPort,
+  IN  PORT_INFO               *PortInfoList,
+  IN  UINT8                   PortInfoListLength,
+  IN  MARGIN_TEST_TYPE        MarginTest,
+  OUT INT32                   *Margins
+  )
+{
+  EFI_STATUS Status;
+  UINT8      *LaneList;
+  UINT8      Lane;
+  UINT8      PortListIndex;
+  UINT8      OriginalSpeed;
+  UINT8      OriginalWidth;
+  UINT8      LaneListLength;
+  PEG_PORT   *PegPort;
+
+  Status = EFI_SUCCESS;
+
+  ///
+  /// Initialize Margins to -1.  Since -1 is an invalid value, we know that lane wasn't tested if its margin == -1
+  ///
+  for (Lane = 0; Lane < SA_PEG_MAX_LANE; Lane++) {
+    Margins[Lane] = -1;
+  }
+
+  for (PortListIndex = 0; PortListIndex < PortInfoListLength; PortListIndex++) {
+    ///
+    /// Test all lanes associated with this the current port
+    ///
+    LaneList       = &((PortInfoList[PortListIndex]).LaneList[0]);
+    LaneListLength =   (PortInfoList[PortListIndex]).LaneListLength;
+    PegPort        = &((PortInfoList[PortListIndex]).PegPort);
+    if ((PortInfoList[PortListIndex]).EnableMargin == FALSE ||
+        (PortInfoList[PortListIndex]).SkipMargin   == TRUE) {
+      continue;
+    }
+
+    if ((PortInfoList[PortListIndex]).LinkIsGen3Capable) {
+      OriginalSpeed = 3;
+    } else {
+      OriginalSpeed = GetLinkSpeed (PegPort);
+    }
+    OriginalWidth = GetNegotiatedWidth (PegPort);
+
+    switch (MarginTest) {
+      case LaneLevelRxJitter:
+        Status = LaneLevelJitterTest (
+                 PeiServices,
+                 SaPlatformPolicyPpi,
+                 SaDataHob,
+                 StallPpi,
+                 MonitorPort,
+                 LaneList,
+                 LaneListLength,
+                 PegPort,
+                 OriginalSpeed,
+                 OriginalWidth,
+                 FALSE,
+                 Margins
+                 );
+        break;
+      default:
+        DEBUG ((EFI_D_WARN, "Invalid Margin Test Requested.\n"));
+        break;
+    }
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+LaneLevelJitterTest (
+  IN  EFI_PEI_SERVICES       **PeiServices,
+  IN  SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN  SA_DATA_HOB            *SaDataHob,
+  IN  PEI_STALL_PPI          *StallPpi,
+  IN  UINT32                 MonitorPort,
+  IN  UINT8                  *LaneList,
+  IN  UINT8                  LaneListLength,
+  IN  PEG_PORT               *PegPort,
+  IN  UINT8                  OriginalLinkSpeed,
+  IN  UINT8                  OriginalLinkWidth,
+  IN  BOOLEAN                TxJitterTest,
+  OUT INT32                  *Margins
+  )
+{
+  EFI_STATUS Status;
+  UINT8     LaneListIndex;
+  INT8      Jitter;
+  UINT32    Errors;
+  UINT32    PreviousErrors;
+  UINT8     Lane;
+  UINT32    RecoveryCount;
+  BOOLEAN   AbortMargin;
+  UINT16    ErrorTarget;
+  UINT8     ConvergenceCounter;
+  INT32     LastMargin;
+  INT32     MarginDifference;
+  INT32     StartJitter;
+  INT8      MarginDirection;
+  UINT8     RepeatCount;
+
+  ErrorTarget = GetErrorTarget (SaPlatformPolicyPpi);
+
+  for (LaneListIndex = 0; LaneListIndex < LaneListLength; LaneListIndex++) {
+    Lane            = LaneList[LaneListIndex];
+    Errors          = 0;
+    AbortMargin     = FALSE;
+    Margins[Lane]   = 0;
+    MarginDirection = 1;
+
+    if (TxJitterTest) {
+      ConfigureTxJitterMux (Lane, SaPlatformPolicyPpi->PlatformData->MchBar);
+      EnableTxJitterInjection (Lane, TRUE);
+    }
+
+    ///
+    /// Determine value to start at
+    ///
+    if (LaneListIndex == 0) {
+      StartJitter = 0;
+    } else {
+      StartJitter = (Margins[LaneList[LaneListIndex - 1]] / 100) - JITTER_MARGIN_INITIAL_OFFSET;
+      if (StartJitter < 0) {
+        StartJitter = 0;
+      }
+    }
+
+    ConvergenceCounter = 0;
+    LastMargin         = -1;
+    RepeatCount        = 0;
+    while (ConvergenceCounter < MARGIN_CONVERGANCE_MIN_MATCHES && RepeatCount < MARGIN_CONVERGANCE_MAX_REPEATS) {
+      RepeatCount++;
+      ///
+      /// Determine whether to go up or down from starting point
+      ///
+      AbortMargin   = FALSE;
+      RecoveryCount = SaPcieGetErrorCount (MonitorPort, PegPort->Function);
+      Status        = SetJitterTolerance (&Lane, 1, (UINT8) StartJitter);
+      ASSERT_EFI_ERROR (Status);
+
+      Errors = SaPciePointTest (PeiServices, SaPlatformPolicyPpi, StallPpi, MonitorPort, PegPort, RecoveryCount);
+      if (Errors >= ErrorTarget) {
+        if (StartJitter == 0) {
+          Margins[Lane] = 0;
+          AbortMargin   = TRUE;
+        } else {
+          MarginDirection = -1;
+        }
+      } else {
+        MarginDirection = 1;
+      }
+      for (Jitter =  (INT8) (StartJitter + MarginDirection);
+           Jitter <  JITTER_LENGTH &&
+           Jitter >= 0             &&
+           (!AbortMargin);
+           Jitter = (INT8) (Jitter + MarginDirection)) {
+        ///
+        /// Check for a link downgrade
+        ///
+        AbortMargin = LinkIsDowngraded (PegPort, OriginalLinkSpeed, OriginalLinkWidth);
+        if (AbortMargin) {
+          if (MarginDirection < 0) {
+            LastMargin         = -1;
+            ConvergenceCounter = 0;
+            StartJitter        = 0;
+            MarginDirection    = 1;
+          } else {
+            if (Jitter == 0) {
+              Margins[Lane] = 0;
+            } else {
+              Margins[Lane] = (Jitter - 1) * 100;
+            }
+          }
+          break;
+        }
+
+        ///
+        /// Get initial recovery count
+        ///
+        RecoveryCount = SaPcieGetErrorCount (MonitorPort, PegPort->Function);
+        Status        = SetJitterTolerance (&Lane, 1, Jitter);
+        ASSERT_EFI_ERROR (Status);
+
+        PreviousErrors = Errors;
+        Errors         = SaPciePointTest (PeiServices, SaPlatformPolicyPpi, StallPpi, MonitorPort, PegPort, RecoveryCount);
+        if (MarginDirection < 0) {
+          if (Errors < ErrorTarget) { ///< Downward direction has started passing
+            Margins[Lane] = InterpolateMargin (ErrorTarget, PreviousErrors, Errors, (INT32) Jitter);
+            break;
+          }
+        } else {
+          if (Errors >= ErrorTarget) { ///< Upward direction has started failing
+            Margins[Lane] = InterpolateMargin (ErrorTarget, Errors, PreviousErrors, (INT32) Jitter);
+            break;
+          }
+        }
+      } ///< End of for loop
+
+      ///
+      /// Check if we never reached the error target
+      ///
+      if (MarginDirection < 0) {
+        if ((Errors >= ErrorTarget) && (!AbortMargin)) {
+          Margins[Lane] = 0;
+        }
+      } else {
+        if ((Errors < ErrorTarget) && (!AbortMargin)) {
+          Margins[Lane] = JITTER_LENGTH * 100;
+        }
+      }
+
+      ///
+      /// Compute the next margin point to start at
+      ///
+      StartJitter = (Margins[Lane] / 100) - JITTER_MARGIN_INITIAL_OFFSET;
+      if (StartJitter < 0) {
+        StartJitter = 0;
+      }
+
+      ///
+      /// Check for convergance
+      ///
+      if (LastMargin == -1) {
+        LastMargin = Margins[Lane];
+      } else {
+        MarginDifference = CalculateMarginDifference (LastMargin, Margins[Lane]);
+        if (MarginDifference <= MARGIN_CONVERGANCE_ALLOWED_DELTA) {
+          ConvergenceCounter++;
+        } else {
+          ConvergenceCounter = 0;
+        }
+        LastMargin = Margins[Lane];
+      }
+
+      if (LinkIsDowngraded (PegPort, OriginalLinkSpeed, OriginalLinkWidth)) {
+        Status = SetJitterTolerance (&Lane, 1, 0);
+        ASSERT_EFI_ERROR (Status);
+      }
+
+      ///
+      /// If the link degraded in any way, bring it back to functional state
+      ///
+      Status = EnsureLinkIsHealthy (PeiServices, SaPlatformPolicyPpi, SaDataHob, StallPpi, PegPort, OriginalLinkSpeed, OriginalLinkWidth);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    } ///< End of repeat while loop
+
+    ///
+    /// Remove Jitter in preparation for testing the next lane
+    ///
+    Status = SetJitterTolerance (&Lane, 1, 0);
+    ASSERT_EFI_ERROR (Status);
+    if (TxJitterTest) {
+      EnableTxJitterInjection (Lane, FALSE);
+    }
+  } ///< End of for each lane loop
+
+  return EFI_SUCCESS;
+}
+
+UINT32
+SaPciePointTest (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN UINT32                 MonitorPort,
+  IN PEG_PORT               *PegPort,
+  IN UINT32                 InitialRecoveryCount
+  )
+{
+  UINT32  Data32;
+
+  StallPpi->Stall (PeiServices, StallPpi, SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchDwellTime);
+  Data32 = SaPcieGetErrorCount (MonitorPort, PegPort->Function) - InitialRecoveryCount;
+
+  return Data32;
+}
+
+INT32
+CalculateMarginDifference (
+  IN INT32                   Margin1,
+  IN INT32                   Margin2
+  )
+{
+  if (Margin1 < Margin2) {
+    return Margin2 - Margin1;
+  } else {
+    return Margin1 - Margin2;
+  }
+}
+
+INT32
+InterpolateMargin (
+  IN UINT32 ErrorTarget,
+  IN UINT32 CurrentErrorCount,
+  IN UINT32 PreviousErrorCount,
+  IN INT32  FailingPoint
+  )
+{
+  UINT32 LnErrorTarget;
+  UINT32 LnCurrentErrorCount;
+  UINT32 LnPreviousErrorCount;
+  INT32  Margin;
+
+  if (ErrorTarget > 40000) {
+    ErrorTarget = 40000;
+  }
+  if (CurrentErrorCount > 40000) {
+    CurrentErrorCount = 40000;
+  }
+  if (PreviousErrorCount > 40000) {
+    PreviousErrorCount = 40000;
+  }
+
+  LnErrorTarget        = NaturalLog (ErrorTarget * 100);
+  LnCurrentErrorCount  = NaturalLog (CurrentErrorCount * 100);
+  LnPreviousErrorCount = NaturalLog (PreviousErrorCount * 100);
+
+  if (FailingPoint >= 0) {
+    if ((LnCurrentErrorCount - LnPreviousErrorCount) == 0) {
+      Margin = (FailingPoint - 1) * 100;
+    } else {
+      Margin = ((LnErrorTarget - LnPreviousErrorCount) * 100) /
+               (LnCurrentErrorCount - LnPreviousErrorCount)   +
+               ((FailingPoint - 1) * 100);
+    }
+    if (Margin < 0) {
+      Margin = 0;
+    }
+  } else {
+    if ((LnCurrentErrorCount - LnPreviousErrorCount) == 0) {
+      Margin = (FailingPoint + 1) * 100;
+    } else {
+      Margin = ((FailingPoint + 1) * 100)                     -
+               ((LnErrorTarget - LnPreviousErrorCount) * 100) /
+               (LnCurrentErrorCount - LnPreviousErrorCount);
+    }
+    if (Margin > 0) {
+      Margin = 0;
+    }
+  }
+
+  return Margin;
+}
+
+
+UINT32
+NaturalLog (
+  IN UINT32 Input
+  )
+  /*++
+
+  Routine Description:
+
+    This function calculates the Natural Log of the Input parameter using integers
+
+  Arguments:
+
+    Input           - 100 times a number to get the Natural log from.
+                    - Max Input Number is 40,000 (without 100x)
+
+  Returns:
+
+    Output          - 100 times the actual result. Accurate within +/- 2
+
+  --*/
+{
+  UINT32  Output;
+
+  ///
+  ///Special case - treat 0 recoveries as 1 recovery for interpolation purposes
+  ///
+  if (Input == 0) {
+    return 0;
+  }
+
+  Output = 0;
+  while (Input > 271) {
+    Input   = (Input * 1000) / 2718;
+    Output += 100;
+  }
+
+  Output += ((-16 * Input * Input + 11578 * Input - 978860) / 10000);
+
+  return Output;
+}
+
+#endif // PEG_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTraining.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTraining.h
new file mode 100644
index 0000000..5c7ee57
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTraining.h
@@ -0,0 +1,425 @@
+/** @file
+  Header file for PcieTraining Initialization Driver.
+
+@copyright
+  Copyright (c) 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+#ifndef _PCIETRAINING_H_
+#define _PCIETRAINING_H_
+
+#include "EdkIIGluePeim.h"
+#include "SaAccess.h"
+#include "PciExpressInit.h"
+
+#include EFI_PPI_DEPENDENCY (SaPlatformPolicy)
+#include EFI_GUID_DEFINITION (SaDataHob)
+
+///
+/// Data structures
+///
+
+typedef struct {
+  PEG_PORT PegPort;
+  UINT32   EndPointVendorIdDeviceId;
+  BOOLEAN  LinkIsGen3Capable;
+  UINT8    LaneList[SA_PEG_MAX_LANE];
+  UINT8    LaneListLength;
+  BOOLEAN  EnableMargin;
+  BOOLEAN  SkipMargin;
+  BOOLEAN  FoundUsablePreset;
+} PORT_INFO;
+
+typedef struct {
+  INT8  Depth;
+  UINT8 Step;
+  UINT8 ReportedMargin;
+  UINT8 DoubleMargin;
+} JITTER_SETTING;
+
+typedef struct {
+  UINT8  Lane;
+  UINT32 InitialDs0Dac;
+  UINT32 InitialDs0Value;
+  INT32  Ds0MarginOffset;
+  UINT32 InitialDs1Dac;
+  UINT32 InitialDs1Value;
+  INT32  Ds1MarginOffset;
+  INT32  MaxUpMargin;
+  INT32  MaxDownMargin;
+} VOC_STATE;
+
+typedef enum {
+  LaneLevelRxJitter,
+  VocUp,
+  VocDown
+} MARGIN_TEST_TYPE;
+
+#define JITTER_LENGTH                         25
+#define JITTER_MARGIN_INITIAL_OFFSET          1
+#define MARGIN_CONVERGANCE_ALLOWED_DELTA      100
+#define MARGIN_CONVERGANCE_MIN_MATCHES        2
+#define MARGIN_CONVERGANCE_MAX_REPEATS        30
+
+#define SA_PEI_MONITOR_OFFSET                 0xFED85000
+
+///
+/// Register Definitions
+///
+#define B_SA_PEG_LTSSMC_WIDTH_MASK            0xFFFFFFE0
+
+#define R_SA_PEG_REUT_PH_CTR_OFFSET           0x444
+#define B_SA_PEG_REUT_PH_CTR_PHYRESET_MASK    0x1
+#define B_SA_PEG_REUT_PH_CTR_RESETMOD_MASK    0x2
+#define B_SA_PEG_REUT_PH_CTR_AUTOCOMP_MASK    0x2000
+
+#define R_SA_PEG_REUT_PH1_PIS_OFFSET          0x464
+#define B_SA_PEG_REUT_PH1_PIS_ST_MASK         0x3F
+#define B_SA_PEG_REUT_PH1_PIS_ST_STEP         0x8
+
+#define B_SA_PEG_BCTRL_SRESET_MASK            BIT6
+
+#define V_SA_VCU_OPCODE_SET_TXJITTER_MUX      0x3002
+#define V_SA_VCU_SEQID_SET_TXJITTER_MUX       0x00030003
+
+#define R_SA_VCU_REUT_PH_CTR_ADDRESS_REV1     0x04448808
+#define R_SA_VCU_REUT_PH_CTR_ADDRESS_REV2     0x04448080
+
+#define R_SA_VCU_REUT_PH1_PIS_ADDRESS_REV1    0x04648808
+#define R_SA_VCU_REUT_PH1_PIS_ADDRESS_REV2    0x04648080
+
+///
+/// Function Prototypes
+///
+
+
+
+UINT16
+GetErrorTarget (
+  IN  SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  );
+
+VOID GetPortInfo (
+  OUT PORT_INFO *PortInfoList,
+  OUT UINT8     *PortInfoListLength,
+  OUT BOOLEAN   *SkipBundle0
+  );
+
+EFI_STATUS
+RunMarginTest (
+  IN  EFI_PEI_SERVICES        **PeiServices,
+  IN  SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi,
+  IN  SA_DATA_HOB             *SaDataHob,
+  IN  PEI_STALL_PPI           *StallPpi,
+  IN  UINT32                  MonitorPort,
+  IN  PORT_INFO               *PortInfoList,
+  IN  UINT8                   PortInfoListLength,
+  IN  MARGIN_TEST_TYPE        MarginTest,
+  OUT INT32                   *Margins
+  );
+
+EFI_STATUS
+LaneLevelJitterTest (
+  IN  EFI_PEI_SERVICES       **PeiServices,
+  IN  SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN  SA_DATA_HOB            *SaDataHob,
+  IN  PEI_STALL_PPI          *StallPpi,
+  IN  UINT32                 MonitorPort,
+  IN  UINT8                  *LaneList,
+  IN  UINT8                  LaneListLength,
+  IN  PEG_PORT               *PegPort,
+  IN  UINT8                  OriginalLinkSpeed,
+  IN  UINT8                  OriginalLinkWidth,
+  IN  BOOLEAN                TxJitterTest,
+  OUT INT32                  *Margins
+  );
+
+UINT32
+SaPciePointTest (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN UINT32                 MonitorPort,
+  IN PEG_PORT               *PegPort,
+  IN UINT32                 InitialRecoveryCount
+  );
+
+INT32
+CalculateMarginDifference (
+  IN INT32                   Margin1,
+  IN INT32                   Margin2
+  );
+
+INT32
+InterpolateMargin (
+  IN UINT32 ErrorTarget,
+  IN UINT32 CurrentErrorCount,
+  IN UINT32 PreviousErrorCount,
+  IN INT32  FailingPoint
+  );
+
+
+UINT32
+NaturalLog (
+  IN UINT32 Input
+  );
+
+
+VOID
+PegGen3PresetSearch (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI          *StallPpi,
+  SA_DATA_HOB               *SaDataHob
+  );
+
+BOOLEAN
+SaPolicyEnablesGen3 (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  );
+
+
+EFI_STATUS
+EnsureLinkIsHealthy (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN SA_DATA_HOB            *SaDataHob,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN PEG_PORT               *PegPort,
+  IN UINT8                  OriginalLinkSpeed,
+  IN UINT8                  OriginalLinkWidth
+  );
+
+EFI_STATUS
+WaitForL0 (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort,
+  IN BOOLEAN          UseVcu
+  );
+
+EFI_STATUS
+TogglePegSlotReset (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  );
+
+EFI_STATUS
+AssertPegSlotReset (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  );
+
+EFI_STATUS
+DeassertPegSlotReset (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  );
+
+EFI_STATUS
+RecoverLinkFailure (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN SA_DATA_HOB            *SaDataHob,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN PEG_PORT               *PegPort,
+  IN UINT8                  OriginalLinkSpeed,
+  IN UINT8                  OriginalLinkWidth
+  );
+
+BOOLEAN
+LinkIsDowngraded (
+  IN PEG_PORT *PegPort,
+  IN UINT8    OriginalLinkSpeed,
+  IN UINT8    OriginalLinkWidth
+  );
+
+EFI_STATUS
+SecondaryBusReset (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort
+  );
+
+EFI_STATUS
+ResetPhyLayer (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort
+  );
+
+EFI_STATUS
+RetrainLink (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort
+  );
+
+UINT8
+GetNegotiatedWidth (
+  IN PEG_PORT *PegPort
+  );
+
+EFI_STATUS
+RecoverLinkWidth (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort,
+  IN UINT8            OriginalLinkWidth
+  );
+
+UINT8
+GetLinkSpeed (
+  IN PEG_PORT *PegPort
+  );
+
+EFI_STATUS
+RecoverLinkSpeed (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort,
+  IN UINT8            OriginalLinkSpeed
+  );
+
+VOID
+PcieTrainingWarmReset (
+  IN EFI_PEI_SERVICES **PeiServices
+  );
+
+
+EFI_STATUS
+SetJitterTolerance (
+  IN UINT8   *LaneList,
+  IN UINT8   LaneListLength,
+  IN UINT8   ReportedMargin
+  );
+
+EFI_STATUS
+SetRawJitterTolerance (
+  IN UINT8   *LaneList,
+  IN UINT8   LaneListLength,
+  IN UINT8   Step,
+  IN UINT8   Depth,
+  IN UINT8   DoubleMargin,
+  IN BOOLEAN EnableJitter
+  );
+
+VOID
+EnableTxJitterInjection (
+  IN UINT8    Lane,
+  IN BOOLEAN  EnableTxJitter
+  );
+
+VOID
+ConfigureTxJitterMux (
+  IN UINT8    Lane,
+  IN UINT32   MchBar
+  );
+
+EFI_STATUS
+GetBundleList (
+  IN  UINT8 *LaneList,
+  IN  UINT8 LaneListLength,
+  OUT UINT8 *BundleList,
+  OUT UINT8 *BundleListLength
+  );
+
+
+UINT32
+OpenMonitor (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI          *StallPpi
+  );
+
+VOID
+CloseMonitor (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN UINT32                  MonitorPort
+  );
+
+UINT32
+SaPcieGetErrorCount (
+  IN UINT32 MonitorPort,
+  IN UINT8  PcieController
+  );
+
+VOID
+SaPcieClearErrorCount (
+  IN UINT32           MonitorPort,
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi
+  );
+
+VOID
+InitMonitor (
+  IN UINT32 MchBar,
+  IN UINT32 GdxcBar
+  );
+
+VOID
+TearDownMonitor (
+  IN UINT32 MchBar,
+  IN UINT32 GdxcBar
+  );
+
+UINT32
+EnableMonitor (
+  VOID
+  );
+
+VOID
+DisableMonitor (
+  VOID
+  );
+
+VOID
+FullMonitorReset (
+  IN UINT32 MonitorPort
+  );
+
+VOID
+ProgramMonitor (
+  IN UINT32           MonitorPort,
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi
+  );
+
+
+VOID
+GetLinkPartnerFullSwing (
+  IN  UINT8 Lane,
+  OUT UINT8 *FullSwing
+  );
+
+VOID
+GetCoefficientsFromPreset (
+  IN  UINT8 Preset,
+  IN  UINT8 FullSwing,
+  OUT UINT8 *PreCursor,
+  OUT UINT8 *Cursor,
+  OUT UINT8 *PostCursor
+  );
+
+VOID
+SetPartnerTxCoefficients (
+  IN  UINT8 Lane,
+  IN  UINT8 *PreCursor,
+  IN  UINT8 *Cursor,
+  IN  UINT8 *PostCursor
+  );
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingEqSettings.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingEqSettings.c
new file mode 100644
index 0000000..dc4515e
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingEqSettings.c
@@ -0,0 +1,166 @@
+/*++ @file
+  This file adds equalization setting support.
+
+@copyright
+  Copyright (c) 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+--*/
+
+#include "PcieTraining.h"
+#include "PciExpressInit.h"
+
+#ifdef PEG_FLAG
+
+VOID
+GetLinkPartnerFullSwing (
+  IN  UINT8 Lane,
+  OUT UINT8 *FullSwing
+  )
+{
+  UINT32  Data32;
+
+  Data32 = BIT25 | BIT23 | (Lane << 19) | BIT18;
+  McD1PciCfg32(R_SA_PEG_EQPH3_OFFSET) = Data32;
+  Data32 = McD1PciCfg32(R_SA_PEG_EQPH3_OFFSET);
+  McD1PciCfg32(R_SA_PEG_EQPH3_OFFSET) = 0;
+
+  *FullSwing = (Data32 >> 6) & 0x3F;
+
+  return;
+}
+
+VOID
+GetCoefficientsFromPreset (
+  IN  UINT8 Preset,
+  IN  UINT8 FullSwing,
+  OUT UINT8 *PreCursor,
+  OUT UINT8 *Cursor,
+  OUT UINT8 *PostCursor
+  )
+{
+  INT32   PreCursorMilli;
+  INT32   PostCursorMilli;
+
+  PreCursorMilli  = 0;
+  PostCursorMilli = 0;
+
+  ///
+  /// Get starting values from Table 4-16 of the PCIe Base Spec v3.0
+  ///
+  switch (Preset) {
+    case  0:
+      PreCursorMilli  =    0;
+      PostCursorMilli = -250;
+      break;
+
+    case  1:
+      PreCursorMilli  =    0;
+      PostCursorMilli = -167;
+      break;
+
+    case  2:
+      PreCursorMilli  =    0;
+      PostCursorMilli = -200;
+      break;
+
+    case  3:
+      PreCursorMilli  =    0;
+      PostCursorMilli = -125;
+      break;
+
+    case  4:
+      PreCursorMilli  =    0;
+      PostCursorMilli =    0;
+      break;
+
+    case  5:
+      PreCursorMilli  = -100;
+      PostCursorMilli =    0;
+      break;
+
+    case  6:
+      PreCursorMilli  = -125;
+      PostCursorMilli =    0;
+      break;
+
+    case  7:
+      PreCursorMilli  = -100;
+      PostCursorMilli = -200;
+      break;
+
+    case  8:
+      PreCursorMilli  = -125;
+      PostCursorMilli = -125;
+      break;
+
+    case  9:
+      PreCursorMilli  = -166;
+      PostCursorMilli =    0;
+      break;
+
+    case 10:  ///< P10 is unsupported
+    default:
+      PreCursorMilli  = -100;
+      PostCursorMilli = -200;
+      DEBUG ((EFI_D_WARN, "GetCoefficientsFromPreset(): Unsupported Preset Requested: P%d. Using P7.\n", Preset));
+      break;
+  }
+
+  ///
+  /// Convert to absolute values
+  ///
+  if (PreCursorMilli < 0) {
+    PreCursorMilli *= -1;
+  }
+  if (PostCursorMilli < 0) {
+    PostCursorMilli *= -1;
+  }
+
+  ///
+  /// Apply FullSwing
+  ///
+  PreCursorMilli  *= FullSwing;
+  PostCursorMilli *= FullSwing;
+
+  ///
+  /// Convert to integers
+  ///
+  *PreCursor  = (( PreCursorMilli % 1000) >= 500) ? (UINT8) (( PreCursorMilli / 1000) + 1) : (UINT8) ( PreCursorMilli / 1000);
+  *PostCursor = ((PostCursorMilli % 1000) >= 500) ? (UINT8) ((PostCursorMilli / 1000) + 1) : (UINT8) (PostCursorMilli / 1000);
+  *Cursor     = FullSwing - (*PreCursor) - (*PostCursor);
+
+  return;
+}
+
+VOID
+SetPartnerTxCoefficients (
+  IN  UINT8 Lane,
+  IN  UINT8 *PreCursor,
+  IN  UINT8 *Cursor,
+  IN  UINT8 *PostCursor
+  )
+{
+  UINT32  Data32;
+
+  Data32 = (Lane << 19) | BIT18 | (*Cursor << 12) | (*PreCursor << 6) | (*PostCursor);
+  McD1PciCfg32(R_SA_PEG_EQPH3_OFFSET) = Data32;
+  McD1PciCfg32(R_SA_PEG_EQPH3_OFFSET) = 0;
+
+  return;
+}
+
+#endif // PEG_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingErrorCount.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingErrorCount.c
new file mode 100644
index 0000000..d1fb107
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingErrorCount.c
@@ -0,0 +1,195 @@
+/** @file
+  Error Counting for PEG training.
+
+@copyright
+  Copyright (c) 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+#include "PcieTraining.h"
+
+#ifdef PEG_FLAG
+
+UINT32
+OpenMonitor (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI          *StallPpi
+  )
+{
+  UINT32 MonitorPort;
+
+  InitMonitor (SaPlatformPolicyPpi->PlatformData->MchBar, SaPlatformPolicyPpi->PlatformData->GdxcBar);
+  MonitorPort = EnableMonitor ();
+  FullMonitorReset (MonitorPort);
+  ProgramMonitor (MonitorPort, PeiServices, StallPpi);
+
+  return MonitorPort;
+}
+
+VOID
+CloseMonitor (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN UINT32                 MonitorPort
+  )
+{
+  FullMonitorReset (MonitorPort);
+  DisableMonitor ();
+  TearDownMonitor (SaPlatformPolicyPpi->PlatformData->MchBar, SaPlatformPolicyPpi->PlatformData->GdxcBar);
+
+  return;
+}
+
+UINT32
+SaPcieGetErrorCount (
+  IN UINT32 MonitorPort,
+  IN UINT8  PcieController
+  )
+{
+  UINT32 Data32;
+
+  Data32 = Mmio32 (MonitorPort, (0xC + (PcieController * 0x10)));
+
+  return Data32;
+}
+
+VOID
+SaPcieClearErrorCount (
+  IN UINT32           MonitorPort,
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi
+  )
+{
+  FullMonitorReset (MonitorPort);
+  ProgramMonitor (MonitorPort, PeiServices, StallPpi);
+
+  return;
+}
+
+VOID
+InitMonitor (
+  IN UINT32 MchBar,
+  IN UINT32 GdxcBar
+  )
+{
+  Mmio32 (MchBar, 0x6430)  = 0x3;
+  Mmio32 (MchBar, 0x6434)  = 0x76543210;
+  McD1PciCfg32 (0x630)     = 0xB;
+  McD1PciCfg32 (0x600)     = 0x60B;
+  McD1PciCfg32 (0x604)     = 0x76543980;
+  McD1F1PciCfg32 (0x600)   = 0x60B;
+  McD1F1PciCfg32 (0x604)   = 0x76543280;
+  McD1F2PciCfg32 (0x600)   = 0xB;
+  McD1F2PciCfg32 (0x604)   = 0x76543210;
+  Mmio32 (MchBar, 0x6438)  = 0x680000;
+  Mmio32 (GdxcBar, 0xA04)  = 0xA;
+
+  return;
+}
+
+VOID
+TearDownMonitor (
+  IN UINT32 MchBar,
+  IN UINT32 GdxcBar
+  )
+{
+  Mmio32 (MchBar, 0x6430)  = 0x0;
+  Mmio32 (MchBar, 0x6434)  = 0x0;
+  McD1PciCfg32 (0x630)     = 0x0;
+  McD1PciCfg32 (0x600)     = 0x0;
+  McD1PciCfg32 (0x604)     = 0x0;
+  McD1F1PciCfg32 (0x600)   = 0x0;
+  McD1F1PciCfg32 (0x604)   = 0x0;
+  McD1F2PciCfg32 (0x600)   = 0x0;
+  McD1F2PciCfg32 (0x604)   = 0x0;
+  Mmio32 (MchBar, 0x6438)  = 0x0;
+  Mmio32 (GdxcBar, 0xA04)   = 0x0;
+
+  return;
+}
+
+UINT32
+EnableMonitor (
+  VOID
+  )
+{
+  UINT32 Data32;
+
+  Data32 = (McD0PciCfg32 (0140) & ~(07));
+  Mmio32Or (Data32, 0124, 040000);
+  Mmio64 (Data32, 0700020) = (UINT64) SA_PEI_MONITOR_OFFSET;
+  Mmio16Or (Data32, 0700004, 02);
+
+  return SA_PEI_MONITOR_OFFSET;
+}
+
+VOID
+DisableMonitor (
+  VOID
+  )
+{
+  UINT32 Data32;
+
+  Data32 = (McD0PciCfg32 (0140) & ~(07));
+  Mmio16And (Data32, 0700004, 0177775);
+  Mmio64And (Data32, 0700020, 07777);
+  Mmio32And (Data32, 0124, 037777737777);
+
+  return;
+}
+
+VOID
+FullMonitorReset (
+  IN UINT32 MonitorPort
+  )
+{
+  Mmio32 (MonitorPort, 0x0)  = 0x40000;
+  Mmio32 (MonitorPort, 0x4)  = 0x0;
+  Mmio32 (MonitorPort, 0x10) = 0x40000;
+  Mmio32 (MonitorPort, 0x14) = 0x0;
+  Mmio32 (MonitorPort, 0x20) = 0x40000;
+  Mmio32 (MonitorPort, 0x24) = 0x0;
+  Mmio32 (MonitorPort, 0x8)  = 0xFF000000;
+  Mmio32 (MonitorPort, 0x18) = 0xFF000000;
+  Mmio32 (MonitorPort, 0x28) = 0xFF000000;
+  Mmio32 (MonitorPort, 0xC)  = 0x0;
+  Mmio32 (MonitorPort, 0x1C) = 0x0;
+  Mmio32 (MonitorPort, 0x2C) = 0x0;
+
+  return;
+}
+
+VOID
+ProgramMonitor (
+  IN UINT32           MonitorPort,
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi
+  )
+{
+  Mmio32 (MonitorPort, 0x4)  = 0xA100;
+  Mmio32 (MonitorPort, 0x14) = 0xA101;
+  Mmio32 (MonitorPort, 0x24) = 0xA102;
+  Mmio32 (MonitorPort, 0x0)  = 0x110000;
+  StallPpi->Stall (PeiServices, StallPpi, 1 * STALL_ONE_MILLI_SECOND);
+
+  Mmio32 (MonitorPort, 0x0)  = 0x22100;
+  Mmio32 (MonitorPort, 0x10) = 0x22101;
+  Mmio32 (MonitorPort, 0x20) = 0x22102;
+
+  return;
+}
+
+#endif // PEG_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingLinkRecovery.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingLinkRecovery.c
new file mode 100644
index 0000000..1480bc7
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingLinkRecovery.c
@@ -0,0 +1,721 @@
+/*++ @file
+  This driver recovers the PEG link.
+
+@copyright
+  Copyright (c) 2012 - 2014 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+--*/
+
+#include "PcieTraining.h"
+#include "PciExpressInit.h"
+#include EFI_PPI_CONSUMER (PchReset)
+
+#ifdef PEG_FLAG
+
+EFI_STATUS
+EnsureLinkIsHealthy (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN SA_DATA_HOB            *SaDataHob,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN PEG_PORT               *PegPort,
+  IN UINT8                  OriginalLinkSpeed,
+  IN UINT8                  OriginalLinkWidth
+  )
+{
+  EFI_STATUS Status;
+
+  Status = EFI_SUCCESS;
+
+  ///
+  /// Check Link Status and Recover the Link if needed
+  ///
+  Status = RecoverLinkWidth (PeiServices, StallPpi, PegPort, OriginalLinkWidth);
+  if (EFI_ERROR (Status)) {
+    Status = RecoverLinkFailure (PeiServices, SaPlatformPolicyPpi, SaDataHob, StallPpi, PegPort, OriginalLinkSpeed, OriginalLinkWidth);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  Status = RecoverLinkSpeed (PeiServices, StallPpi, PegPort, OriginalLinkSpeed);
+  if (EFI_ERROR (Status)) {
+    Status = RecoverLinkFailure (PeiServices, SaPlatformPolicyPpi, SaDataHob, StallPpi, PegPort, OriginalLinkSpeed, OriginalLinkWidth);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  Status = WaitForL0 (PeiServices, StallPpi, PegPort, FALSE);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "DOWNGRADE, Link is not in L0\n"));
+    Status = RetrainLink (PeiServices, StallPpi, PegPort);
+    if (EFI_ERROR (Status)) {
+      Status = RecoverLinkFailure (PeiServices, SaPlatformPolicyPpi, SaDataHob, StallPpi, PegPort, OriginalLinkSpeed, OriginalLinkWidth);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Wait until link is up.
+
+  @param[in] PeiServices  - Pointer to the PEI services table
+  @param[in] StallPpi     - Pointer to PEI_STALL_PPI
+  @param[in] PegPort      - Pointer PEG Port
+  @param[in] UseVcu       - If TRUE, use VCU to determine link state.  If FALSE, use MMIO CFG to determine link state.
+
+  @retval EFI_SUCCESS     - Completed successfully before timeout
+  @retval EFI_TIMEOUT     - Timed out
+**/
+EFI_STATUS
+WaitForL0 (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort,
+  IN BOOLEAN          UseVcu
+  )
+{
+  UINT32        MchBar;
+  UINT32        i;
+  EFI_STATUS    Status;
+  UINT32        VcuAddress;
+  UINT8         VcuReadOp;
+  UINT8         VcuWriteOp;
+  CPU_FAMILY    CpuFamilyId;
+  CPU_STEPPING  CpuSteppingId;
+  BOOLEAN       CheckEq;
+  BOOLEAN       CompletedEq;
+  UINT32        EqStatus;
+  UINT32        LinkStatus;
+
+  Status         = EFI_TIMEOUT;
+  CheckEq        = (PegPort->EndpointMaxLinkSpeed >= 0x3) ? TRUE : FALSE;
+  CompletedEq    = FALSE;
+  i              = 0;
+  MchBar         = McD0PciCfg64 (R_SA_MCHBAR) & ~BIT0;
+  CpuFamilyId    = GetCpuFamily();
+  CpuSteppingId  = GetCpuStepping();
+
+#ifndef AMI_OVERRIDE_FOR_ULT_FASTBOOT
+  if (CpuFamilyId == EnumCpuHswUlt) return EFI_UNSUPPORTED;
+#endif // AMI_OVERRIDE_FOR_ULT_FASTBOOT
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    VcuAddress = R_SA_VCU_REUT_PH1_PIS_ADDRESS_REV1;
+    VcuReadOp  = V_SA_VCU_OPCODE_READ_CSR_REV1;
+    VcuWriteOp = V_SA_VCU_OPCODE_WRITE_CSR_REV1;
+  } else {
+    VcuAddress = R_SA_VCU_REUT_PH1_PIS_ADDRESS_REV2;
+    VcuReadOp  = V_SA_VCU_OPCODE_READ_CSR_REV2;
+    VcuWriteOp = V_SA_VCU_OPCODE_WRITE_CSR_REV2;
+  }
+
+  ///
+  /// If endpoint's LCAP.MLS (Spec section 7.8.6) indicated Gen3 capability, first wait for equalization to complete.
+  /// Check equalization status LSTS2.EC (Spec section 7.8.20) until Gen3 equalization successfully completed.
+  ///
+  if (CheckEq && !UseVcu) {
+    for (; i < 100; i++) {
+      EqStatus = MmPci16 (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_LSTS2_OFFSET);
+      EqStatus = (EqStatus >> 1) & 0x1;
+      if (EqStatus == 0x1) {
+        CompletedEq = TRUE;
+        break;
+      }
+      StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MILLI_SECOND);
+    }
+  }
+
+  ///
+  /// Check for L0 status.  If !UseVcu, check PEGSTS.
+  /// Continue up to 100 msec of combined delay.
+  /// Skip if equalization was needed but didn't successfully complete.
+  ///
+  if ((CheckEq && CompletedEq) || !CheckEq || UseVcu) {
+    for (; i < 100; i++) {
+      if (UseVcu) {
+        LinkStatus = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+        LinkStatus = (LinkStatus >> (PegPort->Function * B_SA_PEG_REUT_PH1_PIS_ST_STEP)) & B_SA_PEG_REUT_PH1_PIS_ST_MASK;
+        if (LinkStatus == 0x10) {
+          Status = EFI_SUCCESS;
+          break;
+        }
+      } else {
+        LinkStatus = MmPci32 (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_PEGSTS_OFFSET);
+        LinkStatus = (LinkStatus >> 16) & 0xF;
+        if (LinkStatus == 0x7) {
+          Status = EFI_SUCCESS;
+          break;
+        }
+      }
+      StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MILLI_SECOND);
+    }
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+SetPchGpio (
+  IN UINT8    GpioNumber,
+  IN UINT8    Level
+  )
+/**
+  This function sets a GPIO to a particular level.
+
+  @param[in] GpioNumber          - PCH GPIO number
+  @param[in] Level               - 0 = Low, 1 = High
+
+  @retval EFI_SUCCESS            - Did toggle GPIO
+  @retval EFI_UNSUPPORTED        - Didn't toggle GPIO
+  @retval EFI_INVALID_PARAMETER  - Didn't toggle GPIO
+**/
+{
+  UINT32      Data32;
+  UINT16      LpcDeviceId;
+  UINT16      GpioBase;
+  UINT16      UseSelOffset;
+  UINT16      IoSelOffset;
+  UINT16      LvlOffset;
+  UINT8       GpioBit;
+  EFI_STATUS  Status;
+
+  Level       &= 0x1;
+  Status       = EFI_SUCCESS;
+  GpioBase     = 0;
+  UseSelOffset = 0;
+  IoSelOffset  = 0;
+  LvlOffset    = 0;
+  GpioBit      = 0;
+
+  LpcDeviceId = McDevFunPciCfg16 (
+                  DEFAULT_PCI_BUS_NUMBER_PCH,
+                  PCI_DEVICE_NUMBER_PCH_LPC,
+                  PCI_FUNCTION_NUMBER_PCH_LPC,
+                  R_PCH_LPC_DEVICE_ID
+                  );
+  if (!IS_PCH_LPT_LPC_DEVICE_ID (LpcDeviceId)) {
+    Status = EFI_UNSUPPORTED;
+    return Status;
+  }
+
+  GpioBase = McDevFunPciCfg16 (
+               DEFAULT_PCI_BUS_NUMBER_PCH,
+               PCI_DEVICE_NUMBER_PCH_LPC,
+               PCI_FUNCTION_NUMBER_PCH_LPC,
+               R_PCH_LPC_GPIO_BASE
+               ) & B_PCH_LPC_GPIO_BASE_BAR;
+  if (GpioBase == 0) {
+    Status = EFI_UNSUPPORTED;
+    return Status;
+  }
+
+  if (GpioNumber < 0x20) {
+    UseSelOffset = R_PCH_GPIO_USE_SEL;
+    IoSelOffset  = R_PCH_GPIO_IO_SEL;
+    LvlOffset    = R_PCH_GPIO_LVL;
+    GpioBit      = GpioNumber;
+  } else if (GpioNumber < 0x40) {
+    UseSelOffset = R_PCH_GPIO_USE_SEL2;
+    IoSelOffset  = R_PCH_GPIO_IO_SEL2;
+    LvlOffset    = R_PCH_GPIO_LVL2;
+    GpioBit      = GpioNumber - 0x20;
+  } else if (GpioNumber < 0x60) {
+    UseSelOffset = R_PCH_GPIO_USE_SEL3;
+    IoSelOffset  = R_PCH_GPIO_IO_SEL3;
+    LvlOffset    = R_PCH_GPIO_LVL3;
+    GpioBit      = GpioNumber - 0x40;
+  } else {
+    Status = EFI_INVALID_PARAMETER;
+    return Status;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "Setting GPIO%d to %x\n", GpioNumber, Level));
+    IoOr32    ((UINTN) (GpioBase + UseSelOffset), (UINT32)  (1 << GpioBit));
+    IoAnd32   ((UINTN) (GpioBase + IoSelOffset),  (UINT32) ~(1 << GpioBit));
+    Data32   = IoRead32  ((UINTN) (GpioBase + LvlOffset));
+    Data32  &= (UINT32) ~(1     << GpioBit);
+    Data32  |= (UINT32)  (Level << GpioBit);
+    IoWrite32 ((UINTN) (GpioBase + LvlOffset), Data32);
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+TogglePegSlotReset (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  )
+/**
+  This function asserts and deasserts a GPIO that controls PERST#.
+  The specific GPIO and its active level is provided by a policy.
+  The GPIO minimum assertion time, T_PERST (100 usec), is defined in the PCIe CEM Specification.
+
+  @param[in] PeiServices         - Pointer to the PEI services table
+  @param[in] StallPpi            - Pointer to PEI_STALL_PPI
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+
+  @retval EFI_SUCCESS            - Did toggle GPIO
+  @retval EFI_UNSUPPORTED        - Didn't toggle GPIO
+  @retval EFI_INVALID_PARAMETER  - Didn't toggle GPIO
+**/
+{
+  EFI_STATUS  Status;
+  UINT8       i;
+
+  DEBUG ((EFI_D_INFO, "Toggling PEG slot reset.\n"));
+  for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+    MmPci16Or (0, SA_PEG_BUS_NUM, SA_PEG_DEV_NUM, i, R_SA_PEG_LCTL_OFFSET, BIT4);
+  }
+  Status = AssertPegSlotReset (SaPlatformPolicyPpi);
+  if (!EFI_ERROR (Status)) {
+    StallPpi->Stall (PeiServices, StallPpi, 100 * STALL_ONE_MICRO_SECOND);
+    for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+      MmPci16And (0, SA_PEG_BUS_NUM, SA_PEG_DEV_NUM, i, R_SA_PEG_LCTL_OFFSET, (UINT16) ~(BIT4));
+    }
+    Status = DeassertPegSlotReset (SaPlatformPolicyPpi);
+  } else {
+    for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+      MmPci16And (0, SA_PEG_BUS_NUM, SA_PEG_DEV_NUM, i, R_SA_PEG_LCTL_OFFSET, (UINT16) ~(BIT4));
+    }
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+AssertPegSlotReset (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  )
+/**
+  This function asserts a GPIO that controls PERST#.
+  The specific GPIO and its active level is provided by a policy.
+  The GPIO minimum assertion time, T_PERST (100 usec), is defined in the PCIe CEM Specification.
+
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+
+  @retval EFI_SUCCESS            - Did assert GPIO
+  @retval EFI_UNSUPPORTED        - Didn't assert GPIO
+  @retval EFI_INVALID_PARAMETER  - Didn't assert GPIO
+**/
+{
+  EFI_STATUS  Status;
+  UINT8       GpioNumber;
+  UINT8       AssertLevel;
+
+  Status = EFI_SUCCESS;
+
+  DEBUG ((EFI_D_INFO, "Asserting PEG slot reset.\n"));
+
+  if (!((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_3) &&
+        (SaPlatformPolicyPpi->PcieConfig->PegGpioData->GpioSupport == TRUE))) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    GpioNumber  = (UINT8) (SaPlatformPolicyPpi->PcieConfig->PegGpioData->SaPegReset->Value);
+    AssertLevel = (UINT8) (SaPlatformPolicyPpi->PcieConfig->PegGpioData->SaPegReset->Active & 0x1);
+    Status      = SetPchGpio (GpioNumber, AssertLevel);
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+DeassertPegSlotReset (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  )
+/**
+  This function deasserts a GPIO that controls PERST#.
+  The specific GPIO and its active level is provided by a policy.
+  The GPIO minimum assertion time, T_PERST (100 usec), is defined in the PCIe CEM Specification.
+
+  @param[in] SaPlatformPolicyPpi - Pointer to SA_PLATFORM_POLICY_PPI
+
+  @retval EFI_SUCCESS            - Did deassert GPIO
+  @retval EFI_UNSUPPORTED        - Didn't deassert GPIO
+  @retval EFI_INVALID_PARAMETER  - Didn't deassert GPIO
+**/
+{
+  EFI_STATUS  Status;
+  UINT8       GpioNumber;
+  UINT8       DeassertLevel;
+
+  Status = EFI_SUCCESS;
+
+  DEBUG ((EFI_D_INFO, "Deasserting PEG slot reset.\n"));
+
+  if (!((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_3) &&
+        (SaPlatformPolicyPpi->PcieConfig->PegGpioData->GpioSupport == TRUE))) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    GpioNumber    = (UINT8)  (SaPlatformPolicyPpi->PcieConfig->PegGpioData->SaPegReset->Value);
+    DeassertLevel = (UINT8) ((SaPlatformPolicyPpi->PcieConfig->PegGpioData->SaPegReset->Active & 0x1) ^ 0x1);
+    SetPchGpio (GpioNumber, DeassertLevel);
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+RecoverLinkFailure (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN SA_DATA_HOB            *SaDataHob,
+  IN PEI_STALL_PPI          *StallPpi,
+  IN PEG_PORT               *PegPort,
+  IN UINT8                  OriginalLinkSpeed,
+  IN UINT8                  OriginalLinkWidth
+  )
+{
+  EFI_STATUS Status;
+  UINT8      CurrentLinkWidth;
+  UINT8      CurrentLinkSpeed;
+  UINT8      PegBus;
+  UINT8      PegDev;
+  UINT8      PegFunc;
+
+  ///
+  /// A platform reset should be done after presets are saved in NVRAM
+  ///
+  if (SaDataHob != NULL) {
+    if ((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_2) &&
+        (SaPlatformPolicyPpi->PcieConfig->PegGen3ForcePresetSearch == 0)) {
+      SaDataHob->PegPlatformResetRequired = TRUE;
+    } else if (SaPlatformPolicyPpi->Revision < SA_PLATFORM_POLICY_PPI_REVISION_2) {
+      SaDataHob->PegPlatformResetRequired = TRUE;
+    }
+  }
+
+  ///
+  /// Bypass phase2 and assert slot reset
+  ///
+  McD1PciCfg32Or (R_SA_PEG_EQCFG_OFFSET, BIT15);
+  Status = TogglePegSlotReset (PeiServices, StallPpi, SaPlatformPolicyPpi);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "Error 0x%x.\n", Status));
+  } else {
+    DEBUG ((EFI_D_INFO, "Success.\n"));
+  }
+  PegBus  = PegPort->Bus;
+  PegDev  = PegPort->Device;
+  PegFunc = PegPort->Function;
+  ///
+  /// Wait for Equalization Done
+  ///
+  while (((MmPci32 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS2_OFFSET) >> 1) & 0x1) != 0x1);
+  ///
+  /// Wait for flow control credits exchange
+  ///
+  WaitForVc0Negotiation (PeiServices, StallPpi, PegBus, PegDev, PegFunc);
+
+  CurrentLinkWidth = GetNegotiatedWidth (PegPort);
+  if (CurrentLinkWidth < OriginalLinkWidth) {
+    DEBUG ((EFI_D_ERROR, "Link Width DOWNGRADED!\n"));
+    Status = EFI_TIMEOUT;
+  }
+  CurrentLinkSpeed = GetLinkSpeed (PegPort);
+  if (CurrentLinkSpeed < OriginalLinkSpeed) {
+    DEBUG ((EFI_D_ERROR, "Link Speed DOWNGRADED!\n"));
+    Status = EFI_TIMEOUT;
+  }
+
+  return Status;
+}
+
+BOOLEAN
+LinkIsDowngraded (
+  IN PEG_PORT *PegPort,
+  IN UINT8    OriginalLinkSpeed,
+  IN UINT8    OriginalLinkWidth
+  )
+{
+  BOOLEAN   IsDowngraded;
+
+  IsDowngraded = FALSE;
+
+  if (OriginalLinkSpeed != GetLinkSpeed (PegPort)) {
+    DEBUG ((EFI_D_INFO, "Link speed downgrade detected\n"));
+    IsDowngraded = TRUE;
+  }
+  if (OriginalLinkWidth != GetNegotiatedWidth (PegPort)) {
+    DEBUG ((EFI_D_INFO, "Link width downgrade detected\n"));
+    IsDowngraded = TRUE;
+  }
+  return IsDowngraded;
+}
+
+EFI_STATUS
+SecondaryBusReset (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort
+  )
+{
+  EFI_STATUS Status;
+
+  Status = EFI_SUCCESS;
+  DEBUG ((EFI_D_INFO, "SECONDARY BUS RESET!\n"));
+  MmPci16Or (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_BCTRL_OFFSET, B_SA_PEG_BCTRL_SRESET_MASK);
+  MmPci16And (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_BCTRL_OFFSET,~(B_SA_PEG_BCTRL_SRESET_MASK));
+  Status = WaitForL0 (PeiServices, StallPpi, PegPort, FALSE);
+  DEBUG ((EFI_D_INFO, "Reset Complete\n"));
+
+  return Status;
+}
+
+EFI_STATUS
+ResetPhyLayer (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort
+  )
+{
+  EFI_STATUS        Status;
+  UINT32            MchBar;
+  UINT32            Data32;
+  UINT32            VcuAddress;
+  UINT8             VcuReadOp;
+  UINT8             VcuWriteOp;
+  BOOLEAN           UseVcu;
+  CPU_FAMILY        CpuFamilyId;
+  CPU_STEPPING      CpuSteppingId;
+
+  UseVcu        = TRUE;
+  Status        = EFI_SUCCESS;
+  MchBar        = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+#ifndef AMI_OVERRIDE_FOR_ULT_FASTBOOT
+  if (CpuFamilyId == EnumCpuHswUlt) return EFI_UNSUPPORTED;
+#endif // AMI_OVERRIDE_FOR_ULT_FASTBOOT
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    VcuAddress = R_SA_VCU_REUT_PH_CTR_ADDRESS_REV1;
+    VcuReadOp  = V_SA_VCU_OPCODE_READ_CSR_REV1;
+    VcuWriteOp = V_SA_VCU_OPCODE_WRITE_CSR_REV1;
+  } else {
+    VcuAddress = R_SA_VCU_REUT_PH_CTR_ADDRESS_REV2;
+    VcuReadOp  = V_SA_VCU_OPCODE_READ_CSR_REV2;
+    VcuWriteOp = V_SA_VCU_OPCODE_WRITE_CSR_REV2;
+  }
+
+  DEBUG ((EFI_D_INFO, "PHY LAYER RESET!\n"));
+  if (UseVcu) {
+    Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+    Data32 &= (UINT32) ~(BIT1);
+    SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+  } else {
+    McD1PciCfg32And (R_SA_PEG_REUT_PH_CTR_OFFSET, ~(B_SA_PEG_REUT_PH_CTR_RESETMOD_MASK));
+  }
+
+  if (UseVcu) {
+    Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+    Data32 |= (UINT32) BIT0;
+    SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+  } else {
+    McD1PciCfg32Or (R_SA_PEG_REUT_PH_CTR_OFFSET, B_SA_PEG_REUT_PH_CTR_PHYRESET_MASK);
+  }
+
+  if (UseVcu) {
+    Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+    Data32 |= (UINT32) BIT13;
+    SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+  } else {
+    McD1PciCfg32Or (R_SA_PEG_REUT_PH_CTR_OFFSET, B_SA_PEG_REUT_PH_CTR_AUTOCOMP_MASK);
+  }
+
+  if (UseVcu) {
+    Data32  = SendVcuApiSequence (MchBar, VcuAddress, VcuReadOp, 0);
+    Data32 &= (UINT32) ~(BIT0);
+    SendVcuApiSequence (MchBar, VcuAddress, VcuWriteOp, Data32);
+  } else {
+    McD1PciCfg32And (R_SA_PEG_REUT_PH_CTR_OFFSET, ~(B_SA_PEG_REUT_PH_CTR_PHYRESET_MASK));
+  }
+
+  Status = WaitForL0 (PeiServices, StallPpi, PegPort, TRUE);
+  DEBUG ((EFI_D_INFO, "Reset Complete\n"));
+
+  return Status;
+}
+
+EFI_STATUS
+RetrainLink (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort
+  )
+{
+  EFI_STATUS Status;
+
+  ///
+  /// Initiate Link Retrain
+  ///
+  MmPci16Or (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_LCTL_OFFSET, 0x20);
+
+  Status = WaitForL0 (PeiServices, StallPpi, PegPort, FALSE);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "Link retrain FAILED!!!\n"));
+  }
+
+  return Status;
+}
+
+UINT8
+GetNegotiatedWidth (
+  IN PEG_PORT *PegPort
+  )
+{
+  UINT16 Lsts;
+
+  Lsts = MmPci16 (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_LSTS_OFFSET);
+
+  return (UINT8)((Lsts >> 4) & 0x3F);
+}
+
+EFI_STATUS
+RecoverLinkWidth (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort,
+  IN UINT8            OriginalLinkWidth
+  )
+{
+  EFI_STATUS Status;
+  UINT8      CurrentLinkWidth;
+
+  Status = EFI_SUCCESS;
+
+  CurrentLinkWidth = GetNegotiatedWidth (PegPort);
+  if (CurrentLinkWidth < OriginalLinkWidth) {
+    DEBUG ((EFI_D_INFO, "DOWNGRADE from x%d to x%d detected\n", OriginalLinkWidth, CurrentLinkWidth));
+    MmPci32AndThenOr (
+      0,
+      PegPort->Bus,
+      PegPort->Device,
+      PegPort->Function,
+      R_SA_PEG_LTSSMC_OFFSET,
+      B_SA_PEG_LTSSMC_WIDTH_MASK,
+      OriginalLinkWidth
+    );
+    MmPci16Or (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_LCTL_OFFSET, 0x10);
+    StallPpi->Stall (PeiServices, StallPpi, STALL_ONE_MICRO_SECOND);
+    MmPci16And (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_LCTL_OFFSET, 0xFFEF);
+    Status = WaitForL0 (PeiServices, StallPpi, PegPort, FALSE);
+    MmPci32Or (
+      0,
+      PegPort->Bus,
+      PegPort->Device,
+      PegPort->Function,
+      R_SA_PEG_LTSSMC_OFFSET,
+      0x1F
+    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    CurrentLinkWidth = GetNegotiatedWidth (PegPort);
+    if (CurrentLinkWidth == OriginalLinkWidth) {
+      DEBUG ((EFI_D_INFO, "Width Recovery Successful\n"));
+      Status = EFI_SUCCESS;
+    } else {
+      DEBUG ((EFI_D_INFO, "Width Recovery FAILED!\n"));
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  return Status;
+}
+
+UINT8
+GetLinkSpeed (
+  IN PEG_PORT *PegPort
+  )
+{
+  UINT16 Lsts;
+
+  Lsts = MmPci16 (0, PegPort->Bus, PegPort->Device, PegPort->Function, R_SA_PEG_LSTS_OFFSET);
+
+  return (UINT8)(Lsts & 0xF);
+}
+
+EFI_STATUS
+RecoverLinkSpeed (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *StallPpi,
+  IN PEG_PORT         *PegPort,
+  IN UINT8            OriginalLinkSpeed
+  )
+{
+  EFI_STATUS Status;
+  UINT8      CurrentLinkSpeed;
+
+  Status = EFI_SUCCESS;
+
+  CurrentLinkSpeed = GetLinkSpeed (PegPort);
+  if (CurrentLinkSpeed < OriginalLinkSpeed) {
+    DEBUG ((EFI_D_INFO, "DOWNGRADE from Gen %d to Gen %d detected\n", OriginalLinkSpeed, CurrentLinkSpeed));
+
+    Status = RetrainLink (PeiServices, StallPpi, PegPort);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "Link Speed Recovery FAILED!\n"));
+      return Status;
+    }
+
+    CurrentLinkSpeed = GetLinkSpeed (PegPort);
+    if (CurrentLinkSpeed < OriginalLinkSpeed) {
+      DEBUG ((EFI_D_INFO, "Link Speed Recovery FAILED!\n"));
+      Status = EFI_DEVICE_ERROR;
+    } else {
+      DEBUG ((EFI_D_INFO, "Link Speed Recovery Successful\n"));
+    }
+  }
+
+  return Status;
+}
+
+VOID
+PcieTrainingWarmReset (
+  IN EFI_PEI_SERVICES **PeiServices
+  )
+{
+  EFI_STATUS      Status;
+  PCH_RESET_PPI   *PchResetPpi;
+
+  Status = (*PeiServices)->LocatePpi (
+                              PeiServices,
+                              &gPchResetPpiGuid,
+                              0,
+                              NULL,
+                              &PchResetPpi
+                              );
+  ASSERT_EFI_ERROR (Status);
+  PchResetPpi->Reset (PchResetPpi, WarmReset);
+
+  return;
+}
+
+#endif // PEG_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingMargining.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingMargining.c
new file mode 100644
index 0000000..7d2f63d
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingMargining.c
@@ -0,0 +1,223 @@
+/** @file
+  Margining for PEG Training
+
+@copyright
+  Copyright (c) 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+#include "PcieTraining.h"
+
+#ifdef PEG_FLAG
+
+///
+/// Jitter Calculations
+///
+JITTER_SETTING Jitter_Gen3[/* 083.333 */] = {{1, 64, 1, 0}, {1, 33, 2, 0}, {3, 64, 3, 0}, {4, 80, 4, 0}, {5, 97, 5, 0}, {6, 128, 6, 0}, {7, 129, 7, 0}, {8, 161, 8, 0}, {9, 192, 9, 0}, {10, 193, 10, 0}, {11, 225, 11, 0}, {12, 128, 12, 1}, {13, 129, 13, 1}, {14, 145, 14, 1}, {15, 160, 15, 1}, {16, 161, 16, 1}, {17, 177, 17, 1}, {18, 192, 18, 1}, {19, 193, 19, 1}, {20, 209, 20, 1}, {21, 224, 21, 1}, {22, 225, 22, 1}, {23, 241, 23, 1}, {24, 246, 24, 1}};
+
+EFI_STATUS
+SetJitterTolerance (
+  IN UINT8 *LaneList,
+  IN UINT8 LaneListLength,
+  IN UINT8 ReportedMargin
+  )
+{
+  JITTER_SETTING Settings = {0, 0, 0, 0};
+  UINT8         i;
+  BOOLEAN       Found;
+
+  if (ReportedMargin > 0) {
+    Found = FALSE;
+    for (i = 0; i < (JITTER_LENGTH - 1); ++i) {
+      Settings = Jitter_Gen3[i];
+      if (Settings.ReportedMargin == ReportedMargin) {
+        Found = TRUE;
+        break;
+      }
+    }
+
+    if (Found) {
+      return SetRawJitterTolerance (
+                                      LaneList,
+                                      LaneListLength,
+                                      Settings.Step,
+                                      Settings.Depth,
+                                      Settings.DoubleMargin,
+                                      TRUE
+                                    );
+    } else {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    return SetRawJitterTolerance (
+                                   LaneList,
+                                   LaneListLength,
+                                   Settings.Step,
+                                   Settings.Depth,
+                                   Settings.DoubleMargin,
+                                   FALSE
+                                 );
+  }
+}
+
+EFI_STATUS
+SetRawJitterTolerance (
+  IN UINT8   *LaneList,
+  IN UINT8   LaneListLength,
+  IN UINT8   Step,
+  IN UINT8   Depth,
+  IN UINT8   DoubleMargin,
+  IN BOOLEAN EnableJitter
+  )
+{
+  UINT8 BundleList[8];
+  UINT8 BundleListLength;
+  UINT8 i;
+  UINT8 Lane;
+  UINT8 Bundle;
+
+  BundleListLength = 0;
+
+  GetBundleList (LaneList, LaneListLength, &BundleList[0], &BundleListLength);
+
+  for (i = 0; i < LaneListLength; ++i) {
+    Lane = LaneList[i];
+
+    McD1PciCfg32And (R_SA_PEG_AFELN0CFG0_OFFSET + (Lane * LANE_STEP), 0xFFFFFBFF);
+  }
+
+  for (i = 0; i < BundleListLength; ++i) {
+    Bundle = BundleList[i];
+
+    McD1PciCfg32AndThenOr (
+      R_SA_PEG_AFEBND0CFG0_OFFSET + (Bundle * BUNDLE_STEP),
+      (UINT32) ~(BIT28 | BIT17 | BIT16 | BIT15 | BIT14 | BIT13 | BIT12 | BIT11 | BIT10),
+      (UINT32)  ((DoubleMargin << 28) | (Step << 10))
+    );
+
+    McD1PciCfg32AndThenOr (
+      R_SA_PEG_AFEBND0CFG3_OFFSET + (Bundle * BUNDLE_STEP),
+      (UINT32) ~(BIT16 | BIT15 | BIT14 | BIT13 | BIT12 | BIT11),
+      (UINT32)  (Depth << 11)
+    );
+  }
+
+  if (EnableJitter) {
+    for (i = 0; i < LaneListLength; ++i) {
+      Lane = LaneList[i];
+      McD1PciCfg32Or (R_SA_PEG_AFELN0CFG0_OFFSET + (Lane * LANE_STEP), BIT10);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+VOID
+EnableTxJitterInjection (
+  IN UINT8   Lane,
+  IN BOOLEAN EnableTxJitter
+  )
+{
+  UINT8 Bundle;
+
+  Bundle = Lane / 2;
+
+  if (EnableTxJitter) {
+    McD1PciCfg32AndThenOr (
+      R_SA_PEG_AFEBND0CFG0_OFFSET + (Bundle * BUNDLE_STEP),
+      ~(BIT1 | BIT2),
+      0x6
+    );
+  } else {
+    McD1PciCfg32And (
+      R_SA_PEG_AFEBND0CFG0_OFFSET + (Bundle * BUNDLE_STEP),
+      ~(BIT1 | BIT2)
+    );
+  }
+}
+
+
+VOID
+ConfigureTxJitterMux (
+  IN UINT8  Lane,
+  IN UINT32 MchBar
+  )
+{
+  CPU_FAMILY        CpuFamilyId;
+  CPU_STEPPING      CpuSteppingId;
+  UINT32            SequenceId;
+
+  CpuFamilyId   = GetCpuFamily ();
+  CpuSteppingId = GetCpuStepping ();
+  SequenceId    = V_SA_VCU_SEQID_SET_TXJITTER_MUX;
+
+#ifndef AMI_OVERRIDE_FOR_ULT_FASTBOOT
+  if (CpuFamilyId == EnumCpuHswUlt) return;
+#endif // AMI_OVERRIDE_FOR_ULT_FASTBOOT
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_OPEN_SEQ_REV1, SequenceId);
+  } else {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_OPEN_SEQ_REV2, SequenceId);
+  }
+
+  SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_SET_TXJITTER_MUX, Lane);
+
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_CLOSE_SEQ_REV1, 0);
+  } else {
+    SendVcuApiCmd (MchBar, V_SA_VCU_OPCODE_CLOSE_SEQ_REV2, 0);
+  }
+
+  return;
+}
+
+EFI_STATUS
+GetBundleList (
+  IN  UINT8 *LaneList,
+  IN  UINT8 LaneListLength,
+  OUT UINT8 *BundleList,
+  OUT UINT8 *BundleListLength
+  )
+{
+  UINT8   Bundle;
+  BOOLEAN HasBundle;
+  UINT8   i;
+  UINT8   j;
+
+  (*BundleListLength) = 0;
+
+  for (i = 0; i < LaneListLength; ++i) {
+    Bundle = LaneList[i] / 2;
+
+    HasBundle = FALSE;
+    for (j = 0; j < (*BundleListLength); ++j) {
+      if (BundleList[j] == Bundle) {
+          HasBundle = TRUE;
+          break;
+      }
+    }
+
+    if (!HasBundle) {
+      BundleList[*BundleListLength] = Bundle;
+      ++(*BundleListLength);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+#endif // PEG_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingPhase3.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingPhase3.c
new file mode 100644
index 0000000..6123968
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/PcieTrainingPhase3.c
@@ -0,0 +1,657 @@
+/** @file
+  This driver trains the PEG interface.
+
+@copyright
+  Copyright (c) 2012 - 2014 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+#include "PcieTraining.h"
+
+#ifdef PEG_FLAG
+
+#include EFI_PPI_DEFINITION (PchMeUma)
+#include "PchMeUma.h"
+
+typedef struct _MARGIN_DATA {
+  UINT8 Preset;
+  INT32 TimingMargin[SA_PEG_MAX_LANE];
+} MARGIN_DATA;
+
+UINT8
+SelectBestPresetForLane (
+  IN  PORT_INFO               *PortInfo,
+  IN  UINT8                   Lane,
+  IN  SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi,
+  IN  SA_DATA_HOB             *SaDataHob,
+  IN  UINT8                   TrainingPreset
+  )
+{
+  UINT8   BestPreset;
+
+  ///
+  /// Init to EV default
+  ///
+  BestPreset = 7;
+
+  ///
+  /// EnableMargin    FoundUsablePreset    SaDataHob
+  ///            Y                    Y        !NULL  -> Use training results; update SaDataHob
+  ///            Y                    Y         NULL  -> Use training results
+  ///            Y                    N        !NULL  -> Use Policy value; update SaDataHob
+  ///            X                    N         NULL  -> Use Policy value
+  ///            N                    X        !NULL  -> Restore from SaDataHob
+  ///
+  if (Lane < SA_PEG_MAX_LANE) {
+    DEBUG ((EFI_D_INFO, "Preset for Lane %2d: ", Lane));
+    if ((PortInfo->EnableMargin) && (PortInfo->FoundUsablePreset)) {
+      ///
+      /// Use the best preset found during training
+      ///
+      BestPreset = TrainingPreset;
+      DEBUG ((EFI_D_INFO, "Search Result: P%d", BestPreset));
+      if (SaDataHob != NULL) {
+        DEBUG ((EFI_D_INFO, ". Saving value for next boot."));
+        SaDataHob->PegData.BestPreset[Lane] = BestPreset;
+      }
+    } else if ((PortInfo->EnableMargin) && (!PortInfo->FoundUsablePreset) && (SaDataHob != NULL)) {
+      ///
+      /// Use the policy value; update SaDataHob
+      ///
+      BestPreset = (UINT8) SaPlatformPolicyPpi->PcieConfig->Gen3EndPointPreset[Lane];
+      DEBUG ((EFI_D_INFO, "Applying Policy value: P%d. Saving value for next boot.", BestPreset));
+      SaDataHob->PegData.BestPreset[Lane] = BestPreset;
+    } else if ((!PortInfo->FoundUsablePreset) && (SaDataHob == NULL)) {
+      ///
+      /// Use the policy value
+      ///
+      BestPreset = (UINT8) SaPlatformPolicyPpi->PcieConfig->Gen3EndPointPreset[Lane];
+      DEBUG ((EFI_D_INFO, "Applying Policy value: P%d", BestPreset));
+    } else if ((!PortInfo->EnableMargin) && (SaDataHob != NULL)) {
+      ///
+      /// Use the preset found on a previous boot
+      ///
+      BestPreset = (UINT8) SaDataHob->PegData.BestPreset[Lane];
+      DEBUG ((EFI_D_INFO, "Restoring previous value: P%d", BestPreset));
+    }
+    DEBUG ((EFI_D_INFO, "\n"));
+  } else {
+    ///
+    /// Error: Non-existent lane
+    ///
+    DEBUG ((EFI_D_ERROR, "Illegal Lane: %d", Lane));
+  }
+
+  return BestPreset;
+}
+
+VOID
+PegGen3PresetSearch (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi,
+  IN PEI_STALL_PPI          *StallPpi,
+  SA_DATA_HOB               *SaDataHob
+  )
+{
+  UINT8                 Index;
+  INT32                 MarginScore;
+  UINT8                 PegBus;
+  UINT8                 PegDev;
+  UINT8                 PegFunc;
+  UINT16                LinkStatus;
+  BOOLEAN               LoadedSavedPreset;
+  BOOLEAN               EndpointDeviceChanged;
+  UINT8                 TempIndex;
+  BOOLEAN               SkipBundle0;
+  CPU_FAMILY            CpuFamilyId;
+  CPU_STEPPING          CpuSteppingId;
+  MARGIN_DATA           MarginData[MAX_PRESETS];
+  PORT_INFO             PortInfoList[SA_PEG_MAX_FUN];
+  INT32                 LaneScores[SA_PEG_MAX_LANE];
+  INT32                 BestScores[SA_PEG_MAX_LANE];
+  UINT8                 BestPresets[SA_PEG_MAX_LANE];
+  EFI_STATUS            Status;
+  UINT32                MonitorPort;
+  UINT8                 PortInfoListLength;
+  UINT8                 PortIndex;
+  UINT8                 Lane;
+  BOOLEAN               AnyGen3CapableLinks;
+  BOOLEAN               AnyMarginingNeeded;
+  UINT8                 PreCursor;
+  UINT8                 Cursor;
+  UINT8                 PostCursor;
+  UINT8                 FullSwing;
+  UINT32                NominalRecoveryCount;
+  UINT8                 FirstSkippedLane;
+  UINT8                 LastSkippedLane;
+  UINT32                Data32;
+  BOOLEAN               SlotResetNeeded;
+  PCH_ME_UMA_PPI        *PchMeUmaPpi;
+  UINT8                 DetectedReplacedCpu;
+
+  AnyMarginingNeeded = FALSE;
+  CpuFamilyId        = GetCpuFamily();
+  CpuSteppingId      = GetCpuStepping();
+
+  ///
+  /// Setup default presets to search
+  ///
+  MarginData[0].Preset = 7;
+  MarginData[1].Preset = 3;
+  MarginData[2].Preset = 5;
+
+  ///
+  /// Initialize Arrays
+  ///
+  for (Lane = 0; Lane < SA_PEG_MAX_LANE; Lane++) {
+    LaneScores[Lane]  = -1;
+    BestScores[Lane]  = -1;
+    BestPresets[Lane] = MarginData[0].Preset;
+  }
+
+  DEBUG ((EFI_D_INFO, "PEG Gen3 Preset Search\n"));
+
+  if (!SaPolicyEnablesGen3 (SaPlatformPolicyPpi)) {
+    DEBUG ((EFI_D_INFO, " Gen3 is disabled by policy\n"));
+    return;
+  }
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId < EnumHswB0)) {
+    DEBUG ((EFI_D_WARN, " Gen3 preset search is not supported on this stepping\n"));
+    return;
+  }
+  Data32 = (McD1PciCfg32 (R_SA_PEG_PEGTST_OFFSET) & BIT20) >> 20;
+  if (Data32 != 0) {
+    DEBUG ((EFI_D_WARN, " Gen3 preset search does not support lane reversal\n"));
+    return;
+  }
+
+  ///
+  /// If ME is supported and the CPU has been replaced, redo the Preset Search.
+  /// Note that calling CpuReplacementCheck() can induce an ME-required warm reset.
+  ///
+  DetectedReplacedCpu = 0;
+  Status              = EFI_SUCCESS;
+
+  if ((SaDataHob != NULL) && (SaDataHob->PegDataValid)) {
+    Status = (*PeiServices)->LocatePpi (PeiServices, &gPchMeUmaPpiGuid, 0, NULL, &PchMeUmaPpi);
+    ASSERT_EFI_ERROR (Status);
+    DEBUG ((EFI_D_INFO, "Calling CpuReplacementCheck\n"));
+    Status = PchMeUmaPpi->CpuReplacementCheck (PeiServices, NULL, &DetectedReplacedCpu);
+    ASSERT_EFI_ERROR (Status);
+    DEBUG ((EFI_D_INFO, " ME reported CPU Replacement value: %x\n", DetectedReplacedCpu));
+  }
+
+  ///
+  /// Get the furcation setup and port information
+  ///
+  GetPortInfo (&(PortInfoList[0]), &PortInfoListLength, &SkipBundle0);
+
+  ///
+  /// Make sure we at Gen3 before starting, if not attempt reset and see if that helps
+  ///
+  SlotResetNeeded = FALSE;
+  for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+    PegBus  = (PortInfoList[PortIndex]).PegPort.Bus;
+    PegDev  = (PortInfoList[PortIndex]).PegPort.Device;
+    PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+    LinkStatus = MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS_OFFSET);
+    if (((LinkStatus & 0x0F) != 3) && ((PortInfoList[PortIndex]).LinkIsGen3Capable)) {
+      SlotResetNeeded = TRUE;
+      break;
+    }
+  }
+  if (SlotResetNeeded) {
+    ///
+    /// Bypass phase2 and assert slot reset
+    ///
+    McD1PciCfg32Or (R_SA_PEG_EQCFG_OFFSET, BIT15);
+    Status = TogglePegSlotReset (PeiServices, StallPpi, SaPlatformPolicyPpi);
+    if (!EFI_ERROR (Status)) {
+      for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+        PegBus  = (PortInfoList[PortIndex]).PegPort.Bus;
+        PegDev  = (PortInfoList[PortIndex]).PegPort.Device;
+        PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+        ///
+        /// Wait for Equalization Done
+        ///
+        while (((MmPci32 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS2_OFFSET) >> 1) & 0x1) != 0x1);
+        ///
+        /// Wait for flow control credits exchange
+        ///
+        WaitForVc0Negotiation (PeiServices, StallPpi, PegBus, PegDev, PegFunc);
+      }
+    }
+  }
+
+  ///
+  /// Determine which PEG ports require testing
+  ///
+  AnyGen3CapableLinks = FALSE;
+  for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+    PegBus  = (PortInfoList[PortIndex]).PegPort.Bus;
+    PegDev  = (PortInfoList[PortIndex]).PegPort.Device;
+    PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+
+    ///
+    /// If we already have Best Preset value from previous boot, use it and skip PresetSearch when end point device no change
+    ///
+    LoadedSavedPreset     = FALSE;
+    EndpointDeviceChanged = TRUE;
+    if (SaDataHob != NULL) {
+      if (SaDataHob->PegDataValid) {
+        LoadedSavedPreset = TRUE;
+        if (SaDataHob->PegData.EndPointVendorIdDeviceId[PegFunc] == (PortInfoList[PortIndex]).EndPointVendorIdDeviceId) {
+          EndpointDeviceChanged = FALSE;
+        }
+      }
+      if (EndpointDeviceChanged) {
+        ///
+        /// Save new device ID vendor ID
+        ///
+        SaDataHob->PegData.EndPointVendorIdDeviceId[PegFunc] = (PortInfoList[PortIndex]).EndPointVendorIdDeviceId;
+      }
+    }
+    DEBUG ((EFI_D_INFO, " PEG%x%x (%x:%x:%x) - LoadedSavedPreset = %d. EndpointDeviceChanged = %d.\n",
+      PegDev, PegFunc, PegBus, PegDev, PegFunc, LoadedSavedPreset, EndpointDeviceChanged));
+
+    ReportPcieLinkStatus (PegBus, PegDev, PegFunc);
+
+    if ( (!LoadedSavedPreset)         ||
+         (EndpointDeviceChanged)      ||
+         (DetectedReplacedCpu   != 0) ||
+         ((SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_2) &&
+          (SaPlatformPolicyPpi->PcieConfig->PegGen3ForcePresetSearch == 1    ))) {
+      (PortInfoList[PortIndex]).EnableMargin = TRUE;
+    } else {
+      (PortInfoList[PortIndex]).EnableMargin = FALSE;
+    }
+
+    if (!(PortInfoList[PortIndex]).LinkIsGen3Capable) {
+      (PortInfoList[PortIndex]).EnableMargin = FALSE;
+    } else {
+      AnyGen3CapableLinks = TRUE;
+    }
+  } ///< PegFunc Loop end
+
+  if (!AnyGen3CapableLinks) {
+    DEBUG ((EFI_D_INFO, "Skipping Preset Search - No Gen3 capable links\n"));
+    return;
+  }
+
+  ///
+  /// Determine if any ports need to be trained.
+  /// If any ports are trained, the corresponding endpoint should also be reset with PERST#.
+  ///
+  for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+    if ((PortInfoList[PortIndex]).EnableMargin == TRUE) {
+      AnyMarginingNeeded = TRUE;
+      break;
+    }
+  }
+
+  if (AnyMarginingNeeded) {
+    MonitorPort = OpenMonitor (PeiServices, SaPlatformPolicyPpi, StallPpi);
+
+    McD1PciCfg32Or (R_SA_PEG_REUT_PH_CTR_OFFSET, B_SA_PEG_REUT_PH_CTR_AUTOCOMP_MASK);
+
+    ///
+    /// Presets Loop start
+    ///
+    for (Index = 0; Index < MAX_PRESETS; Index++) {
+      ///
+      /// Clear out old values
+      ///
+      for (TempIndex = 0; TempIndex < SA_PEG_MAX_LANE; TempIndex++) {
+        MarginData[Index].TimingMargin[TempIndex]      = 0;
+      } ///< End of for each Lane
+
+
+      for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+        (PortInfoList[PortIndex]).SkipMargin = FALSE;
+        PegBus  = (PortInfoList[PortIndex]).PegPort.Bus;
+        PegDev  = (PortInfoList[PortIndex]).PegPort.Device;
+        PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+
+        if (!(PortInfoList[PortIndex]).LinkIsGen3Capable) {
+          DEBUG ((EFI_D_INFO, "Skipping PEG%d%d - Not Gen3 capable\n", PegDev, PegFunc));
+          continue;
+        }
+
+        ///
+        /// Find first lane of the port for coefficient programming
+        ///
+        switch (PegFunc) {
+          default:
+          case 0:
+            Lane             = 0;
+            FirstSkippedLane = 0;
+            LastSkippedLane  = 1;
+            break;
+          case 1:
+            Lane             = 8;
+            FirstSkippedLane = 8;
+            LastSkippedLane  = 9;
+            break;
+          case 2:
+            Lane             = 12;
+            FirstSkippedLane = 12;
+            LastSkippedLane  = 13;
+            break;
+        }
+
+        ///
+        /// Get FullSwing
+        ///
+        GetLinkPartnerFullSwing (Lane, &FullSwing);
+
+        ///
+        /// Get Coefficients
+        ///
+        GetCoefficientsFromPreset (MarginData[Index].Preset, FullSwing, &PreCursor, &Cursor, &PostCursor);
+
+        ///
+        /// Set Lane's Coefficients
+        ///
+        if (SkipBundle0) {
+          for (TempIndex = FirstSkippedLane; TempIndex <= LastSkippedLane; TempIndex++) {
+            SetPartnerTxCoefficients (TempIndex, &PreCursor, &Cursor, &PostCursor);
+            ProgramPreset (1, MarginData[Index].Preset, PegFunc, TempIndex);
+          }
+        }
+        for (TempIndex = 0; TempIndex < (PortInfoList[PortIndex]).LaneListLength; TempIndex++) {
+          SetPartnerTxCoefficients ((PortInfoList[PortIndex]).LaneList[TempIndex], &PreCursor, &Cursor, &PostCursor);
+          ProgramPreset (1, MarginData[Index].Preset, PegFunc, (PortInfoList[PortIndex]).LaneList[TempIndex]);
+        }
+
+        ///
+        /// Set DOEQ bit
+        ///
+        MmPci32Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL3_OFFSET, BIT0);
+        RetrainLink (PeiServices, StallPpi, &((PortInfoList[PortIndex]).PegPort));
+
+        ///
+        /// Get the current link status
+        ///
+        LinkStatus = MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS_OFFSET);
+        ///
+        /// No need to margin if couldn't get to Gen3 with this preset
+        ///
+        if ((LinkStatus & 0x0F) != 3) {
+          RecoverLinkFailure (PeiServices, SaPlatformPolicyPpi, SaDataHob, StallPpi,
+                              &((PortInfoList[PortIndex]).PegPort), 3,
+                              GetNegotiatedWidth (&((PortInfoList[PortIndex]).PegPort)));
+          LinkStatus = MmPci16 (0, PegBus, PegDev, PegFunc, R_SA_PEG_LSTS_OFFSET);
+          if ((LinkStatus & 0x0F) != 3) {
+            (PortInfoList[PortIndex]).SkipMargin = TRUE;
+            continue;
+          }
+        }
+
+        NominalRecoveryCount = SaPcieGetErrorCount (MonitorPort, PegFunc);
+        StallPpi->Stall (PeiServices, StallPpi, SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchDwellTime * STALL_ONE_MICRO_SECOND);
+        NominalRecoveryCount = SaPcieGetErrorCount (MonitorPort, PegFunc) - NominalRecoveryCount;
+        if (NominalRecoveryCount > 0) {
+          (PortInfoList[PortIndex]).SkipMargin = TRUE;
+          continue;
+        }
+        (PortInfoList[PortIndex]).FoundUsablePreset = TRUE;
+      } ///< End of for each Port
+
+      Status = RunMarginTest (
+                 PeiServices,
+                 SaPlatformPolicyPpi,
+                 SaDataHob,
+                 StallPpi,
+                 MonitorPort,
+                 &(PortInfoList[0]),
+                 PortInfoListLength,
+                 LaneLevelRxJitter,
+                 MarginData[Index].TimingMargin
+                 );
+
+      if (EFI_ERROR (Status)) {
+        for (TempIndex = 0; TempIndex < SA_PEG_MAX_LANE; TempIndex++) {
+          MarginData[Index].TimingMargin[TempIndex] = 0;
+        }
+      }
+    } ///< Presets Loop end
+
+    McD1PciCfg32And (R_SA_PEG_REUT_PH_CTR_OFFSET, ~B_SA_PEG_REUT_PH_CTR_AUTOCOMP_MASK);
+    CloseMonitor (SaPlatformPolicyPpi, MonitorPort);
+
+    ///
+    /// Find the preset with the maximum margin (largest of all margin values)
+    ///
+    for (Index = 0; Index < sizeof (MarginData) / sizeof (MarginData[0]); Index++) {
+      for (Lane = 0; Lane < SA_PEG_MAX_LANE; Lane++) {
+        LaneScores[Lane] = -1;
+      }
+      for (Lane = 0; Lane < SA_PEG_MAX_LANE; Lane++) {
+        if (MarginData[Index].TimingMargin[Lane] != -1) {
+          MarginScore = MarginData[Index].TimingMargin[Lane];
+          LaneScores[Lane] = MarginScore;
+        }
+        if (LaneScores[Lane] > BestScores[Lane]) {
+          BestScores[Lane]  = LaneScores[Lane];
+          BestPresets[Lane] = MarginData[Index].Preset;
+        }
+      } ///< End of for each Lane
+    } ///< End of for each Preset
+
+    ///
+    /// If Lanes 0-1 were skipped, copy the values from Lane 2
+    ///
+    if (SkipBundle0) {
+      for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+        PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+        switch (PegFunc) {
+          default:
+          case 0:
+            Lane             = 2;
+            FirstSkippedLane = 0;
+            LastSkippedLane  = 1;
+            break;
+          case 1:
+            Lane             = 10;
+            FirstSkippedLane = 8;
+            LastSkippedLane  = 9;
+            break;
+          case 2:
+            Lane             = 14;
+            FirstSkippedLane = 12;
+            LastSkippedLane  = 13;
+            break;
+        }
+        if (BestScores[Lane] != -1) {
+          DEBUG ((EFI_D_INFO, "Using Lane %2d's Best Preset for Lanes %2d-%2d.\n", Lane, FirstSkippedLane, LastSkippedLane));
+          for (TempIndex = FirstSkippedLane; TempIndex <= LastSkippedLane; TempIndex++) {
+            BestScores[TempIndex]  = BestScores[Lane];
+            BestPresets[TempIndex] = BestPresets[Lane];
+          }
+        }
+      } ///< End of for each port
+    } ///< End of SkipBundle0
+  } ///< End of AnyMarginingNeeded
+
+  for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+    PegBus  = (PortInfoList[PortIndex]).PegPort.Bus;
+    PegDev  = (PortInfoList[PortIndex]).PegPort.Device;
+    PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+    if (!(PortInfoList[PortIndex]).LinkIsGen3Capable) {
+      continue;
+    }
+    if (SkipBundle0) {
+      switch (PegFunc) {
+        default:
+        case 0:
+          FirstSkippedLane = 0;
+          LastSkippedLane  = 1;
+          break;
+        case 1:
+          FirstSkippedLane = 8;
+          LastSkippedLane  = 9;
+          break;
+        case 2:
+          FirstSkippedLane = 12;
+          LastSkippedLane  = 13;
+          break;
+      }
+      for (TempIndex = FirstSkippedLane; TempIndex <= LastSkippedLane; TempIndex++) {
+        BestPresets[TempIndex] = SelectBestPresetForLane (
+                                   &(PortInfoList[PortIndex]),
+                                   TempIndex,
+                                   SaPlatformPolicyPpi,
+                                   SaDataHob,
+                                   BestPresets[TempIndex]
+                                   );
+      }
+    }
+    for (TempIndex = 0; TempIndex < (PortInfoList[PortIndex]).LaneListLength; TempIndex++) {
+      BestPresets[(PortInfoList[PortIndex]).LaneList[TempIndex]] = SelectBestPresetForLane (
+                                                                     &(PortInfoList[PortIndex]),
+                                                                     (PortInfoList[PortIndex]).LaneList[TempIndex],
+                                                                     SaPlatformPolicyPpi,
+                                                                     SaDataHob,
+                                                                     BestPresets[(PortInfoList[PortIndex]).LaneList[TempIndex]]
+                                                                     );
+    }
+  }
+
+  ///
+  /// Program the presets.  If any link was margined, also reset the
+  /// endpoints in order to return the endpoints to a known-good state.
+  ///
+  if (AnyMarginingNeeded) {
+    for (TempIndex = 0; TempIndex < SA_PEG_MAX_FUN; TempIndex++) {
+      MmPci16Or (0, SA_PEG_BUS_NUM, SA_PEG_DEV_NUM, TempIndex, R_SA_PEG_LCTL_OFFSET, BIT4);
+    }
+    AssertPegSlotReset (SaPlatformPolicyPpi);
+  }
+  for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+    PegBus  = (PortInfoList[PortIndex]).PegPort.Bus;
+    PegDev  = (PortInfoList[PortIndex]).PegPort.Device;
+    PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+    if (!(PortInfoList[PortIndex]).LinkIsGen3Capable) {
+      DEBUG ((EFI_D_INFO, "PEG%d%d - Not Gen3 capable, skip coefficient programming.\n", PegDev, PegFunc));
+      continue;
+    }
+    ///
+    /// Find first lane of the port for coefficient programming
+    ///
+    switch (PegFunc) {
+      default:
+      case 0:
+        Lane             = 0;
+        FirstSkippedLane = 0;
+        LastSkippedLane  = 1;
+        break;
+      case 1:
+        Lane             = 8;
+        FirstSkippedLane = 8;
+        LastSkippedLane  = 9;
+        break;
+      case 2:
+        Lane             = 12;
+        FirstSkippedLane = 12;
+        LastSkippedLane  = 13;
+        break;
+    }
+    ///
+    /// Get FullSwing
+    ///
+    GetLinkPartnerFullSwing (Lane, &FullSwing);
+
+    if (SkipBundle0) {
+      for (TempIndex = FirstSkippedLane; TempIndex <= LastSkippedLane; TempIndex++) {
+        GetCoefficientsFromPreset (BestPresets[TempIndex], FullSwing, &PreCursor, &Cursor, &PostCursor);
+        SetPartnerTxCoefficients (TempIndex, &PreCursor, &Cursor, &PostCursor);
+        ProgramPreset (1, BestPresets[TempIndex], PegFunc, TempIndex);
+      }
+    }
+    for (TempIndex = 0; TempIndex < (PortInfoList[PortIndex]).LaneListLength; TempIndex++) {
+      ///
+      /// Get Coefficients
+      ///
+      GetCoefficientsFromPreset (BestPresets[((PortInfoList[PortIndex]).LaneList[TempIndex])], FullSwing, &PreCursor, &Cursor, &PostCursor);
+
+      ///
+      /// Set Lane's Coefficients
+      ///
+      SetPartnerTxCoefficients ((PortInfoList[PortIndex]).LaneList[TempIndex], &PreCursor, &Cursor, &PostCursor);
+
+      ///
+      /// Set Phase 1 Presets
+      ///
+      ProgramPreset (1, BestPresets[((PortInfoList[PortIndex]).LaneList[TempIndex])], PegFunc, (PortInfoList[PortIndex]).LaneList[TempIndex]);
+    }
+
+    MmPci32Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL3_OFFSET, BIT0);   ///< DOEQ
+    MmPci16Or (0, PegBus, PegDev, PegFunc, R_SA_PEG_LCTL_OFFSET,  BIT5);   ///< Retrain link
+  }
+
+  if (AnyMarginingNeeded) {
+    StallPpi->Stall (PeiServices, StallPpi, 100 * STALL_ONE_MICRO_SECOND);
+    for (TempIndex = 0; TempIndex < SA_PEG_MAX_FUN; TempIndex++) {
+      MmPci16And (0, SA_PEG_BUS_NUM, SA_PEG_DEV_NUM, TempIndex, R_SA_PEG_LCTL_OFFSET, (UINT16) ~(BIT4));
+    }
+    DeassertPegSlotReset (SaPlatformPolicyPpi);
+  }
+
+  for (PortIndex = 0; PortIndex < PortInfoListLength; PortIndex++) {
+    PegBus  = (PortInfoList[PortIndex]).PegPort.Bus;
+    PegDev  = (PortInfoList[PortIndex]).PegPort.Device;
+    PegFunc = (PortInfoList[PortIndex]).PegPort.Function;
+    WaitForL0 (PeiServices, StallPpi, &(PortInfoList[PortIndex].PegPort), FALSE);
+    ReportPcieLinkStatus (PegBus, PegDev, PegFunc);
+  }
+
+  DEBUG ((EFI_D_INFO, "PEG Gen3 Preset Search done\n\n"));
+
+  return;
+}
+
+BOOLEAN
+SaPolicyEnablesGen3 (
+  IN SA_PLATFORM_POLICY_PPI *SaPlatformPolicyPpi
+  )
+{
+  UINTN   PegPortGenx;
+  BOOLEAN Gen3Enabled;
+  UINT8   Index;
+
+  Gen3Enabled = FALSE;
+
+  ///
+  /// Check if Gen3 is enabled on PEG10/11/12
+  ///
+  for (Index = 0; Index < SA_PEG_MAX_FUN; Index++) {
+    ///
+    /// PegPortGenx: 0 = Auto, 1 = Gen1, 2 = Gen2, 3 = Gen3
+    ///
+    PegPortGenx = SaPlatformPolicyPpi->PcieConfig->PegGenx[Index];
+
+    ///
+    /// Check if the root port is present and the speed is not limited to Gen1/Gen2
+    ///
+    if ((PegPortGenx == PEG_AUTO) || (PegPortGenx == PEG_GEN3)) {
+      Gen3Enabled = TRUE;
+      break;
+    }
+  }
+  return Gen3Enabled;
+}
+
+#endif // PEG_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaDmiPeim.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaDmiPeim.c
new file mode 100644
index 0000000..ea817d8
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaDmiPeim.c
@@ -0,0 +1,450 @@
+/** @file
+  SA Dmi PEI Initialization library
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "SaDmiPeim.h"
+#include "PciExpressInit.h"
+
+///
+/// Functions
+///
+/**
+  Initialize DMI Tc/Vc mapping through SA-PCH.
+
+  @param[in] PeiServices          - General purpose services available to every PEIM.
+  @param[in] SaPlatformPolicyPpi  - Instance of SA_PLATFORM_POLICY_PPI
+
+  @retval EFI_SUCCESS
+**/
+EFI_STATUS
+SaDmiTcVcInit (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+{
+  EFI_STATUS        Status;
+  UINT64            MchBar;
+  UINT64            DmiBar;
+  PCH_INIT_PPI      *PchInitPpi;
+  PCH_DMI_TC_VC_PPI *PchDmiTcVcMapPpi;
+  CPU_FAMILY        CpuFamilyId;
+  UINT8             i;
+
+  MchBar  = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+  DmiBar  = McD0PciCfg64 (R_SA_DMIBAR) &~BIT0;
+  CpuFamilyId = GetCpuFamily();
+
+  ///
+  /// Locate PchInitPpi and PchDmiTcVcMapPpi
+  ///
+  Status = (*PeiServices)->LocatePpi (PeiServices, &gPchInitPpiGuid, 0, NULL, (VOID **) &PchInitPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = (*PeiServices)->LocatePpi (PeiServices, &gPchDmiTcVcMapPpiGuid, 0, NULL, (VOID **) &PchDmiTcVcMapPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// SA OPI Initialization
+  ///
+  if (CpuFamilyId == EnumCpuHswUlt) {
+    MmioOr8 ((UINTN) (DmiBar + 0xA78), BIT1);
+  }
+
+  ///
+  /// Update DmiTcVcMapping based on Policy
+  ///
+  PchDmiTcVcMapPpi->DmiVc[DmiVcTypeVc1].Enable  = (BOOLEAN) SaPlatformPolicyPpi->PcieConfig->DmiVc1;
+  PchDmiTcVcMapPpi->DmiVc[DmiVcTypeVcp].Enable  = (BOOLEAN) SaPlatformPolicyPpi->PcieConfig->DmiVcp;
+  PchDmiTcVcMapPpi->DmiVc[DmiVcTypeVcm].Enable  = (BOOLEAN) SaPlatformPolicyPpi->PcieConfig->DmiVcm;
+
+  for (i = 0; i < DmiTcTypeMax; i++) {
+    if (((PchDmiTcVcMapPpi->DmiTc[i].Vc == DmiVcTypeVc1) && (PchDmiTcVcMapPpi->DmiVc[DmiVcTypeVc1].Enable == FALSE)) ||
+        ((PchDmiTcVcMapPpi->DmiTc[i].Vc == DmiVcTypeVcp) && (PchDmiTcVcMapPpi->DmiVc[DmiVcTypeVcp].Enable == FALSE)) ||
+        ((PchDmiTcVcMapPpi->DmiTc[i].Vc == DmiVcTypeVcm) && (PchDmiTcVcMapPpi->DmiVc[DmiVcTypeVcm].Enable == FALSE))
+        ) {
+      PchDmiTcVcMapPpi->DmiTc[i].Vc = DmiVcTypeVc0;
+    }
+  }
+  ///
+  /// Program NB TC/VC mapping
+  ///
+  SaSetDmiTcVcMapping (PchDmiTcVcMapPpi, DmiBar);
+
+  ///
+  /// Call PchDmiTcVcProgPoll
+  ///
+  Status = PchInitPpi->DmiTcVcProgPoll (PeiServices);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Poll NB negotiation completion
+  ///
+  SaPollDmiVcStatus (PchDmiTcVcMapPpi, DmiBar);
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Map SA DMI TCs to VC
+
+  @param[in] PchDmiTcVcMapPpi     - Instance of PCH_DMI_TC_VC_PPI
+  @param[in] DmiBar               - DMIBAR address
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Wrong phase parameter passed in.
+**/
+EFI_STATUS
+SaSetDmiTcVcMapping (
+  IN    PCH_DMI_TC_VC_PPI   *PchDmiTcVcMapPpi,
+  IN    UINT64              DmiBar
+  )
+{
+  UINT32  Data32And;
+  UINT32  Data32Or;
+  UINT8   Data8And;
+  UINT8   Data8Or;
+  UINT8   Index;
+  UINT16  Register;
+  UINT8   VcId;
+  UINT8   VcMap[DmiVcTypeMax] = { 0 };
+
+  ///
+  /// Set the TC/VC mappings
+  ///
+  for (Index = 0; Index < DmiTcTypeMax; Index++) {
+    VcMap[PchDmiTcVcMapPpi->DmiTc[Index].Vc] |= (BIT0 << Index);
+  }
+  ///
+  /// System BIOS must perform the following steps for VC0 configuration.
+  ///   Program the TCs/VC0 map by setting DMIBAR offset 014h [7:1] = '0111 101b'.
+  ///
+  /// Private Virtual Channel Configuration
+  /// Step1. Assign Virtual Channel ID 2 to VCp:
+  ///    Programming the DMIVCPRCTL DMI Port Register DMIBAR Offset 02Ch[26:24] = '010b'.
+  ///
+  /// Step2. Set TC2 to VCp:
+  ///    Program the DMIVCPRCTL DMI Port Register DMIBAR offset 02Ch [7:1] = '0000 010b'.
+  ///
+  /// Step3. Enable VCp by programming the DMIVCPRCTL DMI Port Register DMIBAR Offset 02Ch[31] = '1b'.
+  ///
+  /// Virtual Channel for ME (VCm) Configuration
+  /// This is configured by ConfigMemMe
+  ///
+  /// Step1. Assign Virtual Channel ID 7 to VCm:
+  ///    Programming the DMIVCMRCTL DMI Port Register DMIBAR Offset 038h[26:24] = '111b'.
+  ///
+  /// Step2. Enable VCm:
+  ///    Programming the DMIVMPRCTL DMI Port Register DMIBAR Offset 038h[31] = '1b'.
+  ///
+  /// Step3. Enable VCm by programming the DMIVCMRCTL DMI Port Register DMIBAR Offset 038h[31] = '1b'.
+  ///
+  for (Index = 0; Index < DmiVcTypeMax; Index++) {
+    if (PchDmiTcVcMapPpi->DmiVc[Index].Enable == PCH_DEVICE_ENABLE) {
+      ///
+      /// Map TCs to VC, Set the VC ID, Enable VC
+      ///
+      VcId = PchDmiTcVcMapPpi->DmiVc[Index].VcId,
+
+      Data32And = (UINT32) (~(V_SA_DMIBAR_DMIVCCTL_ID | B_SA_DMIBAR_DMIVCCTL_TVM_MASK));
+      Data32Or = VcId << N_SA_DMIBAR_DMIVCCTL_ID;
+      Data32Or |= VcMap[Index];
+      Data32Or |= N_SA_DMIBAR_DMIVCCTL_EN;
+
+      switch (Index) {
+      case DmiVcTypeVc0:
+        Register = R_SA_DMIBAR_DMIVC0RCTL_OFFSET;
+        break;
+
+      case DmiVcTypeVc1:
+        Register = R_SA_DMIBAR_DMIVC1RCTL_OFFSET;
+        break;
+
+      case DmiVcTypeVcp:
+        Register = R_SA_DMIBAR_DMIVCPRCTL_OFFSET;
+        break;
+
+      case DmiVcTypeVcm:
+        Register = R_SA_DMIBAR_DMIVCMRCTL_OFFSET;
+        break;
+
+      default:
+        return EFI_INVALID_PARAMETER;
+      }
+
+      MmioAndThenOr32 ((UINTN) (DmiBar + Register), Data32And, Data32Or);
+    }
+  }
+  ///
+  /// System BIOS must program the extended VC Count:
+  ///    Set the DMI Port Register DMIBAR Offset 004h[2:0]=001b
+  ///
+  Data8And = (UINT8) (~0x07);
+  if (PchDmiTcVcMapPpi->DmiVc[DmiVcTypeVc1].Enable == TRUE) {
+    Data8Or = 1;
+  } else {
+    Data8Or = 0;
+  }
+
+  MmioAndThenOr8 ((UINTN) (DmiBar + R_SA_DMIBAR_DMIPVCCAP1_OFFSET), Data8And, Data8Or);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Poll SA DMI negotiation completion
+
+  @param[in] PchDmiTcVcMapPpi     - Instance of PCH_DMI_TC_VC_PPI
+  @param[in] DmiBar               - DMIBAR address
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Wrong phase parameter passed in.
+**/
+EFI_STATUS
+SaPollDmiVcStatus (
+  IN    PCH_DMI_TC_VC_PPI   *PchDmiTcVcMapPpi,
+  IN    UINT64              DmiBar
+  )
+{
+  UINT8   Index;
+  UINT16  Register;
+
+  ///
+  /// 6.2.3.2 - Step 4, Poll until VC1 has been negotiated
+  ///    Read the DMIVC1RSTS DMI Port Register Offset 026h until [1]==0
+  ///
+  /// 6.2.3.3 - Step4. Poll the VCp Negotiation Pending bit until it reads 0:
+  ///    Read the DMIVCPRSTS DMI Port Register Offset 032h until [1]==0
+  ///
+  /// 6.2.3.4 - Step4. Poll the VCm Negotiation Pending bit until it reads 0:
+  ///    Read the DMIVCMRSTS DMI Port Register Offset 03Eh until [1]==0
+  ///
+  for (Index = 0; Index < DmiVcTypeMax; Index++) {
+    if (PchDmiTcVcMapPpi->DmiVc[Index].Enable == PCH_DEVICE_ENABLE) {
+      switch (Index) {
+      case DmiVcTypeVc0:
+        Register = R_SA_DMIBAR_DMIVC0RSTS_OFFSET;
+        break;
+
+      case DmiVcTypeVc1:
+        Register = R_SA_DMIBAR_DMIVC1RSTS_OFFSET;
+        break;
+
+      case DmiVcTypeVcp:
+        Register = R_SA_DMIBAR_DMIVCPRSTS_OFFSET;
+        break;
+
+      case DmiVcTypeVcm:
+        Register = R_SA_DMIBAR_DMIVCMRSTS_OFFSET;
+        break;
+
+      default:
+        return EFI_INVALID_PARAMETER;
+      }
+      ///
+      /// Wait for negotiation to complete
+      ///
+      while ((MmioRead16 ((UINTN) (DmiBar + Register)) & B_SA_DMIBAR_DMISTS_NP) != 0);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+#ifdef DMI_FLAG
+/**
+  Initialize DMI.
+
+  @param[in] PeiServices          - General purpose services available to every PEIM.
+  @param[in] SaPlatformPolicyPpi  - Instance of SA_PLATFORM_POLICY_PPI
+
+  @retval EFI_SUCCESS
+**/
+EFI_STATUS
+DmiInit (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+{
+  EFI_STATUS              Status;
+  UINT64                  MchBar;
+  UINT64                  DmiBar;
+  PCH_INIT_PPI            *PchInitPpi;
+  PCH_PLATFORM_POLICY_PPI *PchPlatformPolicyPpi;
+  UINT8                   CpuSteppingId;
+  BOOLEAN                 DmiGen2Enable;
+  UINT16                  LinkStatus;
+  UINT32                  Data32Or;
+
+  ///
+  /// Read the CPU stepping
+  ///
+  CpuSteppingId = GetCpuStepping();
+
+  ///
+  /// BridgeId = (UINT8) (McD0PciCfg16 (R_MC_DEVICE_ID) & 0xF0);
+  /// BridgeSteppingId = BridgeId + CpuSteppingId;
+  ///
+  MchBar  = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+  DmiBar  = McD0PciCfg64 (R_SA_DMIBAR) &~BIT0;
+
+  ///
+  /// Get RCBA through the PchPlatformPolicy PPI
+  ///
+  Status = (**PeiServices).LocatePpi (
+                            PeiServices,
+                            &gPchPlatformPolicyPpiGuid,
+                            0,
+                            NULL,
+                            &PchPlatformPolicyPpi
+                            );
+  ASSERT_EFI_ERROR (Status);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Can't locate PchPlatformPolicy PPI - exiting.\n"));
+    return Status;
+  }
+  ///
+  /// Perform DMI Recipe steps
+  ///
+  DEBUG ((EFI_D_INFO, "DMI Recipe...\n"));
+  PegDmiRecipe (SaPlatformPolicyPpi, (UINT32) MchBar, (UINT32) DmiBar, 0, 0);
+
+  ///
+  /// Additional DMI steps. See SA BIOS Spec.
+  ///
+  DEBUG ((EFI_D_INFO, "Run AdditionalDmiProgramSteps!\n"));
+  AdditionalDmiProgramSteps (SaPlatformPolicyPpi, (UINT32) MchBar, (UINT32) DmiBar);
+
+  DmiGen2Enable = TRUE;
+  if ((SaPlatformPolicyPpi->PcieConfig->DmiGen2 == 0) ||
+      ((MmioRead8 ((UINTN) PchPlatformPolicyPpi->Rcba + R_PCH_RCRB_LCAP) & (BIT0 | BIT1 | BIT2 | BIT3)) == 0x1) ||
+      (McDevFunPciCfg32 (0, 0, 0, R_SA_MC_CAPID0_A_OFFSET) & BIT22)
+     ) {
+    DEBUG ((EFI_D_WARN, "DMI Gen2 is Disabled or not capable, staying at Gen1 !\n"));
+    DmiGen2Enable = FALSE;
+  }
+
+  if (DmiGen2Enable) {
+    ///
+    /// Locate PchInitPpi
+    ///
+    Status = (*PeiServices)->LocatePpi (PeiServices, &gPchInitPpiGuid, 0, NULL, &PchInitPpi);
+    ASSERT_EFI_ERROR (Status);
+
+    ///
+    /// Program PCH TLS to Gen 2
+    ///
+    PchInitPpi->DmiGen2Prog (PeiServices);
+
+    ///
+    /// Program CPU Max Link Speed to Gen 2
+    ///
+    MmioAndThenOr32 ((UINTN) (DmiBar + R_SA_DMIBAR_LCAP_OFFSET), (UINT32)~0xF, 2);
+  }
+  Data32Or = (MmioRead32 ((UINTN) (DmiBar + R_SA_DMIBAR_LCAP_OFFSET)) & (BIT3 | BIT2 | BIT1 | BIT0));
+  MmioAndThenOr32 ((UINTN) (DmiBar + R_SA_DMIBAR_LCTL2_OFFSET), (UINT32)~(BIT3 | BIT2 | BIT1 | BIT0), Data32Or);
+
+  ///
+  /// Retrain link
+  ///
+  DmiLinkTrain (DmiBar);
+
+  ///
+  /// Retrain link if it is GEN2 Capable and it is not yet set to GEN2
+  ///
+  if (DmiGen2Enable &&
+      ((((MmioRead16 ((UINTN) (DmiBar + R_SA_DMIBAR_LSTS_OFFSET))) & 0x0F) != DMI_GEN2) ||
+       (((MmioRead16 ((UINTN) PchPlatformPolicyPpi->Rcba + R_PCH_RCRB_LSTS)) & 0x0F) != DMI_GEN2))
+     ) {
+    DEBUG ((EFI_D_INFO, "DMI Link re-train to set GEN2\n"));
+    DmiLinkTrain (DmiBar);
+  }
+  ///
+  /// Get the current link status
+  ///
+  LinkStatus = MmioRead16 ((UINTN) (DmiBar + R_SA_DMIBAR_LSTS_OFFSET));
+  DEBUG ((EFI_D_INFO, "DMI trained to x%d at Gen%d\n", (LinkStatus >> 4) & 0x3F, LinkStatus & 0x0F));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  DMI link training
+
+  @param[in] DmiBar - DMIBAR address
+**/
+VOID
+DmiLinkTrain (
+  IN  UINT64 DmiBar
+  )
+{
+  ///
+  /// Retrain link
+  ///
+  MmioOr8 ((UINTN) (DmiBar + R_SA_DMIBAR_LCTL_OFFSET), BIT5);
+
+  ///
+  /// Wait for link training complete
+  ///
+  while ((MmioRead16 ((UINTN) (DmiBar + R_SA_DMIBAR_LSTS_OFFSET)) & BIT11) != 0)
+    ;
+}
+
+/**
+  Additional DMI Programming Steps at PEI
+
+  @param[in] SaPlatformPolicyPpi - pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] MchBar              - MCHBAR address
+  @param[in] DmiBar              - DMIBAR address
+**/
+VOID
+AdditionalDmiProgramSteps (
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi,
+  IN UINT32                    MchBar,
+  IN UINT32                    DmiBar
+  )
+{
+  UINT32  Data32And;
+  UINT32  Data32Or;
+
+  ///
+  /// Disable DMI and PEG Debug Align Message - set 0x258[29] = '1b'
+  ///
+  Data32And = (UINT32) ~BIT29;
+  Data32Or  = BIT29;
+  Mmio32AndThenOr (DmiBar, R_SA_DMIBAR_CFG4_OFFSET, Data32And, Data32Or);
+
+  ///
+  /// Overwrite DMICC (DMIBAR offset 0x208) to 0x6B5
+  ///
+  Data32And = (UINT32)~(BIT10 | BIT9 | BIT8 | BIT7 | BIT6 | BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0);
+  Data32Or  = 0x6B5;
+  Mmio32AndThenOr (DmiBar, R_SA_DMIBAR_DMICC_OFFSET, Data32And, Data32Or);
+
+  ///
+  /// Set L0SLAT[15:0] to 0x2020
+  ///
+  Data32And = (UINT32) ~(0xFFFF);
+  Data32Or  = 0x00002020;
+  Mmio32AndThenOr (DmiBar, R_SA_DMIBAR_L0SLAT_OFFSET, Data32And, Data32Or);
+}
+#endif // DMI_FLAG
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaDmiPeim.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaDmiPeim.h
new file mode 100644
index 0000000..799eb13
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaDmiPeim.h
@@ -0,0 +1,94 @@
+/** @file
+  Header file for the SA Dmi Init library.
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _SA_DMI_PEIM_H_
+#define _SA_DMI_PEIM_H_
+
+#include "EdkIIGluePeim.h"
+#include "SaAccess.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+
+///
+/// Driver Consumed PPI Prototypes
+///
+#include EFI_PPI_DEPENDENCY (SaPlatformPolicy)
+#include EFI_PPI_DEPENDENCY (PchPlatformPolicy)
+#include EFI_PPI_DEPENDENCY (PchInit)
+#include EFI_PPI_DEPENDENCY (PchDmiTcVcMap)
+
+/**
+  Map SA DMI TCs to VC
+
+  @param[in] PchDmiTcVcMapPpi     - Instance of PCH_DMI_TC_VC_PPI
+  @param[in] DmiBar               - DMIBAR address
+
+  @retval EFI_SUCCESS
+**/
+EFI_STATUS
+SaSetDmiTcVcMapping (
+  IN    PCH_DMI_TC_VC_PPI  *PchDmiTcVcMapPpi,
+  IN    UINT64             DmiBar
+  )
+;
+
+/**
+  Poll SA DMI negotiation completion
+
+  @param[in] PchDmiTcVcMapPpi     - Instance of PCH_DMI_TC_VC_PPI
+  @param[in] DmiBar               - DMIBAR address
+
+  @retval EFI_SUCCESS
+**/
+EFI_STATUS
+SaPollDmiVcStatus (
+  IN    PCH_DMI_TC_VC_PPI  *PchDmiTcVcMapPpi,
+  IN    UINT64             DmiBar
+  )
+;
+
+#ifdef DMI_FLAG
+/**
+  DMI link training
+
+  @param[in] DmiBar - DMIBAR address
+**/
+VOID
+DmiLinkTrain (
+  IN  UINT64 DmiBar
+  )
+;
+
+/**
+  Additional DMI Programming Steps at PEI
+
+  @param[in] SaPlatformPolicyPpi - pointer to SA_PLATFORM_POLICY_PPI
+  @param[in] MchBar              - MCHBAR address
+  @param[in] DmiBar              - DMIBAR address
+**/
+VOID
+AdditionalDmiProgramSteps (
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi,
+  IN UINT32                    MchBar,
+  IN UINT32                    DmiBar
+  )
+;
+#endif // DMI_FLAG
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.c
new file mode 100644
index 0000000..28fc22e
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.c
@@ -0,0 +1,689 @@
+/** @file
+  The PEIM implements the SA PEI Initialization.
+
+@copyright
+  Copyright (c) 1999 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+#include "SaInitPeim.h"
+#ifdef RAPID_START_FLAG
+#include EFI_PPI_CONSUMER (RapidStart)
+#endif
+
+static EFI_PEI_PPI_DESCRIPTOR     mSaPeiInitPpi[] = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gSaPeiInitPpiGuid,
+  NULL
+};
+
+STATIC EFI_PEI_NOTIFY_DESCRIPTOR  mSaResetCompleteNotifyDesc = {
+  (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gMemoryInitHobGuid,
+  SaResetComplete
+};
+
+EFI_GUID  gEfiPeiEndOfPeiPhasePpiGuid = EFI_PEI_END_OF_PEI_PHASE_PPI_GUID;
+STATIC EFI_PEI_NOTIFY_DESCRIPTOR  mSaS3ResumeNotifyDesc = {
+  (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiPeiEndOfPeiPhasePpiGuid,
+  SaS3ResumeAtEndOfPei
+};
+
+#ifdef RAPID_START_FLAG
+STATIC EFI_PEI_NOTIFY_DESCRIPTOR  mSaOnRapidStartPpiNotifyDesc = {
+  (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gRapidStartPpiGuid,
+  SaCheckRapidStartMode
+};
+#endif
+
+///
+/// Functions
+///
+static
+EFI_STATUS
+SaS3ResumeAtEndOfPei (
+  IN      EFI_PEI_SERVICES          **PeiServices,
+  IN      EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN      VOID                      *Ppi
+  )
+/**
+  This function handles SA S3 resume task
+
+  @param[in] PeiServices   - Pointer to PEI Services Table.
+  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
+                             caused this function to execute.
+  @param[in] Ppi           - Pointer to the PPI data associated with this function.
+
+  @retval EFI_STATUS       - Always return EFI_SUCCESS
+**/
+{
+  SA_DATA_HOB                 *SaDataHob;
+  CPU_STEPPING                CpuSteppingId;
+  CPU_FAMILY                  CpuFamilyId;
+
+  CpuSteppingId = GetCpuStepping();
+  CpuFamilyId   = GetCpuFamily();
+  ///
+  /// Get SaPegDataHob HOB
+  ///
+  SaDataHob = NULL;
+  SaDataHob = (SA_DATA_HOB *)GetFirstGuidHob (&gSaDataHobGuid);
+  if (SaDataHob != NULL) {
+     ///
+     /// If there was no DXE mode supported, always enable SMM mode
+     ///
+#if SA_PCIE_ASPM_IN_DXE == 0
+    SaDataHob->InitPcieAspmAfterOprom = 1;
+#endif
+     ///
+     /// If there was no SMM mode supported, always enable DXE mode
+     ///
+#if SA_PCIE_ASPM_IN_SMM == 0
+    SaDataHob->InitPcieAspmAfterOprom = 0;
+#endif
+
+    if ((SaDataHob->SaIotrapSmiAddress != 0) && (SaDataHob->InitPcieAspmAfterOprom == 1)) {
+#if SA_PCIE_ASPM_IN_SMM == 1
+      ///
+      /// Always generate SA IO TRAP SMI when supported
+      /// The SMI handler will directly return if not PCIe ASPM init after Oprom was enabled
+      ///
+      DEBUG ((EFI_D_INFO, "Generate SA IOTRAP SMI port=%X\n", SaDataHob->SaIotrapSmiAddress));
+      IoWrite8 (SaDataHob->SaIotrapSmiAddress, 0);
+#endif
+    }
+  }
+#if SA_PCIE_ASPM_IN_DXE == 1
+  if ((SaDataHob == NULL) || (SaDataHob->InitPcieAspmAfterOprom == 0)) {
+    ///
+    /// When SaDataHob not preset/corrupted or InitPcieAspmAfterOprom set to 0,
+    /// try to do DXE mode S3 resume task.
+    ///
+    ///
+    /// Lock processor/chipset BAR registers
+    /// Other save/restore were done by S3 Save script table
+    ///
+    if (!(((CpuFamilyId == EnumCpuHsw)    && (CpuSteppingId <= EnumHswB0)   ) ||
+          ((CpuFamilyId == EnumCpuHswUlt) && (CpuSteppingId <= EnumHswUltB0)) ||
+          ((CpuFamilyId == EnumCpuCrw)    && (CpuSteppingId <= EnumCrwB0)   ) )) {
+      AsmMsrOr64 (0x2e7, 1);
+    }
+  }
+#endif
+  return EFI_SUCCESS;
+}
+
+#ifdef RAPID_START_FLAG
+static
+EFI_STATUS
+SaCheckRapidStartMode (
+  IN      EFI_PEI_SERVICES          **PeiServices,
+  IN      EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN      VOID                      *Ppi
+  )
+/**
+  This function will check Rapid Start mode and install SaS3Resume callback notify if it was Rapid Start Resume
+
+  @param[in] PeiServices   - Pointer to PEI Services Table.
+  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
+                             caused this function to execute.
+  @param[in] Ppi           - Pointer to the PPI data associated with this function.
+
+  @retval EFI_STATUS       - Always return EFI_SUCCESS
+**/
+{
+  RAPID_START_PPI     *RapidStartPpi;
+  EFI_STATUS          Status;
+
+  ///
+  /// When Rapid Start implemented and in the Rapid Start Resume transition, this SA S3 resume Notify should happen early than Rapid Start End-Of-Pei callback
+  /// This is because Rapid Start End-Of-Pei callback will generate ACPI_ENABLE SMI which should be after SA S3 resume completed.
+  ///
+  Status = PeiServicesLocatePpi (&gRapidStartPpiGuid, 0, NULL, &RapidStartPpi);
+  ASSERT_EFI_ERROR (Status);
+  if (RapidStartPpi->GetMode (RapidStartPpi) == RapidStartExit) {
+    DEBUG ((EFI_D_INFO, "[SA] Install Notify callback for Rapid Start Resume\n"));
+    Status = PeiServicesNotifyPpi (&mSaS3ResumeNotifyDesc);
+    ASSERT_EFI_ERROR (Status);
+  }
+  return EFI_SUCCESS;
+}
+#endif
+
+EFI_STATUS
+EFIAPI
+SaInitPeiEntryPoint (
+  IN EFI_FFS_FILE_HEADER       *FfsHeader,
+  IN EFI_PEI_SERVICES          **PeiServices
+  )
+/**
+  SA PEI Initialization.
+
+  @param[in] FfsHeader    - Pointer to Firmware File System file header.
+  @param[in] PeiServices  - General purpose services available to every PEIM.
+
+  @retval EFI_SUCCESS
+**/
+{
+  EFI_STATUS              Status;
+  SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi;
+  EFI_BOOT_MODE           BootMode;
+#ifdef RAPID_START_FLAG
+  RAPID_START_PPI         *RapidStartPpi;
+#endif
+#if defined(DMI_FLAG) || defined(PEG_FLAG)
+  CPU_FAMILY              CpuFamilyId;
+
+  CpuFamilyId = GetCpuFamily();
+#endif // DMI_FLAG || PEG_FLAG
+
+  ///
+  /// Get platform policy settings through the SaPlatformPolicy PPI
+  ///
+  Status = (**PeiServices).LocatePpi (
+                            PeiServices,
+                            &gSaPlatformPolicyPpiGuid,
+                            0,
+                            NULL,
+                            (VOID **) &SaPlatformPolicyPpi
+                            );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Dump SA Platform Policy
+  ///
+  SaPeiPolicyDump (SaPlatformPolicyPpi);
+
+  ///
+  /// Program SA Bar Registers
+  ///
+  DEBUG ((EFI_D_INFO, "Programming SA Bars\n"));
+  ProgramSaBars (SaPlatformPolicyPpi);
+
+  ///
+  /// Install SA HOBs
+  ///
+  InstallSaHob (PeiServices, SaPlatformPolicyPpi);
+
+  ///
+  /// Report SA PCIe code version
+  ///
+  DEBUG ((EFI_D_INFO, "Reporting SA PCIe code version\n"));
+  ReportPcieVersion (SaPlatformPolicyPpi);
+
+#ifdef DMI_FLAG
+  if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuCrw)) {
+    ///
+    /// Initialize DMI
+    ///
+    DEBUG ((EFI_D_INFO, "Initializing DMI\n"));
+    DmiInit (PeiServices, SaPlatformPolicyPpi);
+  }
+#endif // DMI_FLAG
+
+#ifdef SG_SUPPORT
+  ///
+  /// Initialize SwitchableGraphics
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing SwitchableGraphics\n"));
+  SwitchableGraphicsInit (PeiServices, SaPlatformPolicyPpi);
+#endif
+
+#ifdef PEG_FLAG
+  if ((CpuFamilyId == EnumCpuHsw) || (CpuFamilyId == EnumCpuCrw)) {
+    ///
+    /// Initialize SA PCIe
+    ///
+    DEBUG ((EFI_D_INFO, "Initializing SA PCIe\n"));
+    PciExpressInit (PeiServices, SaPlatformPolicyPpi);
+  }
+#endif // PEG_FLAG
+
+  ///
+  /// Initialize Graphics (IGD/External)
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing Graphics\n"));
+  GraphicsInit (PeiServices, SaPlatformPolicyPpi);
+
+  ///
+  /// Initialize Overclocking
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing System Agent Overclocking\n"));
+  SaOcInit(PeiServices, SaPlatformPolicyPpi);
+
+  ///
+  /// Initialize DMI Tc/Vc mapping setting
+  ///
+  DEBUG ((EFI_D_INFO, "Initializing DMI Tc/Vc mapping\n"));
+  SaDmiTcVcInit (PeiServices, SaPlatformPolicyPpi);
+
+  ///
+  /// Early BIOS POST Programming
+  ///
+  DEBUG ((EFI_D_INFO, "Early BIOS POST Programming\n"));
+  EarlyBiosPostProgramming(SaPlatformPolicyPpi);
+
+  ///
+  /// Install Notify
+  ///
+  Status = PeiServicesNotifyPpi (&mSaResetCompleteNotifyDesc);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Install SA S3 resume Notify only when booting from S3 resume
+  ///
+  Status = PeiServicesGetBootMode (&BootMode);
+  DEBUG ((EFI_D_INFO, "[SA] BootMode = %X\n", BootMode));
+  if ((Status == EFI_SUCCESS) && (BootMode == BOOT_ON_S3_RESUME)) {
+    DEBUG ((EFI_D_INFO, "[SA] Install SA S3 Notify callback\n"));
+    Status = PeiServicesNotifyPpi (&mSaS3ResumeNotifyDesc);
+    ASSERT_EFI_ERROR (Status);
+  }
+#ifdef RAPID_START_FLAG
+  else {
+    ///
+    /// When Rapid Start implemented and in the Rapid Start Resume transition, this SA S3 resume Notify should happen early than Rapid Start End-Of-Pei callback
+    /// This is because Rapid Start End-Of-Pei callback will generate ACPI_ENABLE SMI which should be after SA S3 resume completed.
+    ///
+    Status = PeiServicesLocatePpi (&gRapidStartPpiGuid, 0, NULL, &RapidStartPpi);
+    if (Status == EFI_SUCCESS) {
+      DEBUG ((EFI_D_INFO, "[SA] Check Rapid Start transition mode\n"));
+      if (RapidStartPpi->GetMode (RapidStartPpi) == RapidStartExit) {
+        DEBUG ((EFI_D_INFO, "[SA] Install Notify callback for Rapid Start Resume\n"));
+        Status = PeiServicesNotifyPpi (&mSaS3ResumeNotifyDesc);
+        ASSERT_EFI_ERROR (Status);
+      }
+    } else {
+      DEBUG ((EFI_D_INFO, "[SA] Postpone Rapid Start mode checking after RapidStartPpi installed\n"));
+      Status = PeiServicesNotifyPpi (&mSaOnRapidStartPpiNotifyDesc);
+    }
+  }
+#endif
+
+  ///
+  /// Install Ppi with SaInitPeim complete
+  ///
+  Status = (**PeiServices).InstallPpi (PeiServices, mSaPeiInitPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+SaResetComplete (
+  IN      EFI_PEI_SERVICES          **PeiServices,
+  IN      EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN      VOID                      *Ppi
+  )
+/**
+  BIOS_RESET_CPL bit is set for processor to activate the power and thermal management
+  features on the platform.
+
+  @param[in] PeiServices   - Pointer to PEI Services Table.
+  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
+                  caused this function to execute.
+  @param[in] Ppi           - Pointer to the PPI data associated with this function.
+
+  @retval EFI_SUCCESS
+**/
+{
+  EFI_STATUS              Status;
+  UINT64                  MchBar;
+  UINT32                  Data32And;
+  UINT32                  Data32Or;
+  SA_PLATFORM_POLICY_PPI  *SaPlatformPolicyPpi;
+
+  MchBar = McD0PciCfg64 (R_SA_MCHBAR) &~BIT0;
+
+  ///
+  /// Get platform policy settings through the SaPlatformPolicy PPI
+  ///
+  Status = PeiServicesLocatePpi (&gSaPlatformPolicyPpiGuid, 0, NULL, (VOID **) &SaPlatformPolicyPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Set MCHBAR Offset 5F00h [10:9] = 11b
+  ///
+  Data32And = (UINT32)~(BIT10 | BIT9);
+  Data32Or  = 0x3 << 9;
+  Mmio32AndThenOr (MchBar, R_SA_MCHBAR_SAPMCTL_OFFSET, Data32And, Data32Or);
+
+  ///
+  /// Set BIOS_RESET_CPL
+  ///
+  DEBUG ((EFI_D_INFO, "Set BIOS_RESET_CPL to indicate all configurations complete\n"));
+  Mmio8Or ((UINTN) MchBar, R_SA_MCHBAR_BIOS_RESET_CPL_OFFSET, BIT0 | BIT1);
+
+  return EFI_SUCCESS;
+}
+
+VOID
+ProgramSaBars (
+  IN    SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  Programs SA Bars
+
+  @param[in] SaPlatformPolicyPpi - Instance of SA_PLATFORM_POLICY_PPI
+**/
+{
+  CPU_FAMILY  CpuFamilyId;
+
+  CpuFamilyId = GetCpuFamily();
+
+  ///
+  /// Program SA MchBar, DmiBar and EpBar
+  ///
+  McD0PciCfg64 (R_SA_MCHBAR)   = (UINT64) (SaPlatformPolicyPpi->PlatformData->MchBar        | BIT0);
+  McD0PciCfg64 (R_SA_DMIBAR)   = (UINT64) (SaPlatformPolicyPpi->PlatformData->DmiBar        | BIT0);
+  McD0PciCfg64 (R_SA_PXPEPBAR) = (UINT64) (SaPlatformPolicyPpi->PlatformData->EpBar         | BIT0);
+
+  ///
+  /// Program SA GdxcBar
+  ///
+  Mmio64 ((UINTN) (SaPlatformPolicyPpi->PlatformData->MchBar), R_SA_MCHBAR_GDXCBAR_OFFSET) = (UINT64)
+    (SaPlatformPolicyPpi->PlatformData->GdxcBar | BIT0);
+
+  if (CpuFamilyId == EnumCpuCrw) {
+    ///
+    /// Program SA EdramBar
+    ///
+    Mmio64 ((UINTN) (SaPlatformPolicyPpi->PlatformData->MchBar), R_SA_MCHBAR_EDRAMBAR_OFFSET) = (UINT64)
+      (SaPlatformPolicyPpi->PlatformData->EdramBar | BIT0);
+  }
+}
+
+VOID
+EarlyBiosPostProgramming (
+  IN    SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  Do Early BIOS POST Programming
+
+  @param[in] SaPlatformPolicyPpi - Instance of SA_PLATFORM_POLICY_PPI
+**/
+{
+  /// SA BS 11.1 Early BIOS POST Programming
+  /// 1.  Enable System Agent Clock Gating by setting the MCHBAR offset 5F00h [0] = '1b'.
+  MmioOr32 ((UINTN) (SaPlatformPolicyPpi->PlatformData->MchBar + 0x5F00), BIT0);
+}
+
+EFI_STATUS
+InstallSaHob (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+/**
+  Init and Install SA Hob
+
+  @param[in] PeiServices  - General purpose services available to every PEIM.
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+
+  @retval EFI_SUCCESS
+**/
+{
+  EFI_STATUS                  Status;
+  SA_DATA_HOB                 *SaDataHob;
+  PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicy;
+
+  ///
+  /// Create HOB for SA Data
+  ///
+  Status = (**PeiServices).CreateHob (
+                             PeiServices,
+                             EFI_HOB_TYPE_GUID_EXTENSION,
+                             sizeof (SA_DATA_HOB),
+                             (VOID **) &SaDataHob
+                             );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Initialize default HOB data
+  ///
+  SaDataHob->EfiHobGuidType.Name = gSaDataHobGuid;
+  ZeroMem (&(SaDataHob->DprDirectory[EnumDprDirectoryTxt]), sizeof (DPR_DIRECTORY_ENTRY));
+  ZeroMem (&(SaDataHob->DprDirectory[EnumDprDirectoryPfat]), sizeof (DPR_DIRECTORY_ENTRY));
+  ZeroMem (&(SaDataHob->PegData), sizeof (SA_PEG_DATA));
+  SaDataHob->PegDataValid             = FALSE;
+  SaDataHob->PegPlatformResetRequired = FALSE;
+  if (SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_2) {
+    SaDataHob->SaIotrapSmiAddress = SaPlatformPolicyPpi->PcieConfig->SaIotrapSmiAddress;
+    SaDataHob->InitPcieAspmAfterOprom = SaPlatformPolicyPpi->PcieConfig->InitPcieAspmAfterOprom;
+  } else {
+    SaDataHob->SaIotrapSmiAddress = 0;
+    SaDataHob->InitPcieAspmAfterOprom = 0;
+  }
+
+  ///
+  /// Get platform policy settings through the SaPlatformPolicy PPI
+  ///
+  Status = (**PeiServices).LocatePpi (
+                            PeiServices,
+                            &gPeiCpuPlatformPolicyPpiGuid,
+                            0,
+                            NULL,
+                            (VOID **) &CpuPlatformPolicy
+                            );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// TXT DPR Directory Entry
+  ///
+#if defined(TXT_SUPPORT_FLAG) && (TXT_SUPPORT_FLAG == 1)
+  SaDataHob->DprDirectory[EnumDprDirectoryTxt].Type = DPR_DIRECTORY_TYPE_TXT;
+  if (CpuPlatformPolicy->CpuConfig->Txt) {
+    SaDataHob->DprDirectory[EnumDprDirectoryTxt].Size = (UINT8) RShiftU64 (CpuPlatformPolicy->SecurityConfig->TxtConfig->TxtDprMemorySize, 20);
+  }
+#endif
+
+  ///
+  /// PFAT Directory Entry
+  ///
+  SaDataHob->DprDirectory[EnumDprDirectoryPfat].Type = DPR_DIRECTORY_TYPE_PFAT;
+  if (CpuPlatformPolicy->CpuConfig->Pfat) {
+    SaDataHob->DprDirectory[EnumDprDirectoryPfat].Size = CpuPlatformPolicy->SecurityConfig->PfatConfig->PfatMemSize;
+  }
+
+  DEBUG ((EFI_D_INFO, "SA Data HOB installed\n"));
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+ReportPcieVersion (
+  IN    SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  Report the SA PCIe initialization code version.
+
+  @param[in] SaPlatformPolicyPpi - Instance of SA_PLATFORM_POLICY_PPI
+
+  @retval EFI_SUCCESS
+**/
+{
+  UINT32            Version;
+  CPU_FAMILY        CpuFamilyId;
+  CPU_STEPPING      CpuSteppingId;
+  const CodeVersion SaPcieCodeVersion = {
+#include "SaPcieVersion.h"
+  };
+
+  DEBUG ((EFI_D_INFO, "***************** System Agent PCIe code version *****************\n"));
+  DEBUG ((EFI_D_INFO, "** Major version number is: %3d                                 **\n", SaPcieCodeVersion.Major));
+  DEBUG ((EFI_D_INFO, "** Minor version number is: %3d                                 **\n", SaPcieCodeVersion.Minor));
+  DEBUG ((EFI_D_INFO, "** Rev version number is:   %3d                                 **\n", SaPcieCodeVersion.Rev));
+  DEBUG ((EFI_D_INFO, "** Build number is:         %3d                                 **\n", SaPcieCodeVersion.Build));
+  DEBUG ((EFI_D_INFO, "******************************************************************\n"));
+
+  Version = (((UINT32) SaPcieCodeVersion.Major) << 24) | (((UINT32) SaPcieCodeVersion.Minor) << 16) | (((UINT32) SaPcieCodeVersion.Rev) << 8) | (((UINT32) SaPcieCodeVersion.Build));
+
+  CpuFamilyId   = GetCpuFamily();
+  CpuSteppingId = GetCpuStepping();
+
+  ///
+  /// Store SA Reference Code version and SA PCIe code version in scrachpad registers
+  ///
+  if ((CpuFamilyId == EnumCpuHsw) && (CpuSteppingId == EnumHswA0)) {
+    Mmio32 ((UINTN) (SaPlatformPolicyPpi->PlatformData->MchBar), R_SA_MCHBAR_PCIE_CODE_VERSION_OFFSET_HSW) = Version;
+  } else {
+    Mmio32 ((UINTN) (SaPlatformPolicyPpi->PlatformData->DmiBar), R_SA_DMIBAR_SCRATCHPAD0_OFFSET) = SaPlatformPolicyPpi->Revision;
+    Mmio32 ((UINTN) (SaPlatformPolicyPpi->PlatformData->DmiBar), R_SA_DMIBAR_SCRATCHPAD1_OFFSET) = Version;
+  }
+  return EFI_SUCCESS;
+}
+
+VOID
+SaPeiPolicyDump (
+  IN    SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  This function prints the PEI phase platform policy.
+
+  @param[in] SaPlatformPolicyPpi - Instance of SA_PLATFORM_POLICY_PPI
+**/
+{
+#ifdef EFI_DEBUG
+  INTN   i;
+
+  DEBUG ((EFI_D_INFO, "\n------------------------ SA Platform Policy (PEI) dump BEGIN -----------------\n"));
+  DEBUG ((EFI_D_INFO, "Revision : %x\n", SaPlatformPolicyPpi->Revision));
+
+  DEBUG ((EFI_D_INFO, "------------------------ SA_PLATFORM_DATA -----------------\n"));
+
+  DEBUG ((EFI_D_INFO, " SpdAddressTable[%d] :", SA_MC_MAX_SOCKETS));
+  for (i = 0; i < SA_MC_MAX_SOCKETS; i++) {
+    DEBUG ((EFI_D_INFO, " %x", SaPlatformPolicyPpi->PlatformData->SpdAddressTable[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " MchBar : %x\n", SaPlatformPolicyPpi->PlatformData->MchBar));
+  DEBUG ((EFI_D_INFO, " DmiBar : %x\n", SaPlatformPolicyPpi->PlatformData->DmiBar));
+  DEBUG ((EFI_D_INFO, " EpBar : %x\n", SaPlatformPolicyPpi->PlatformData->EpBar));
+  DEBUG ((EFI_D_INFO, " PciExpressBar : %x\n", SaPlatformPolicyPpi->PlatformData->PciExpressBar));
+  DEBUG ((EFI_D_INFO, " SmbusBar : %x\n", SaPlatformPolicyPpi->PlatformData->SmbusBar));
+  DEBUG ((EFI_D_INFO, " GdxcBar : %x\n", SaPlatformPolicyPpi->PlatformData->GdxcBar));
+  DEBUG ((EFI_D_INFO, " TsegSize : %x\n", SaPlatformPolicyPpi->PlatformData->TsegSize));
+  DEBUG ((EFI_D_INFO, " UserBd : %x\n", SaPlatformPolicyPpi->PlatformData->UserBd));
+  DEBUG ((EFI_D_INFO, " FastBoot : %x\n", SaPlatformPolicyPpi->PlatformData->FastBoot));
+  DEBUG ((EFI_D_INFO, " EdramBar : %x\n", SaPlatformPolicyPpi->PlatformData->EdramBar));
+
+  DEBUG ((EFI_D_INFO, "------------------------ GT_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " MmioSize : %x MB\n", SaPlatformPolicyPpi->GtConfig->MmioSize));
+  DEBUG ((EFI_D_INFO, " GttSize : %x MB\n", SaPlatformPolicyPpi->GtConfig->GttSize));
+  DEBUG ((EFI_D_INFO, " IgdDvmt50PreAlloc : %x\n", SaPlatformPolicyPpi->GtConfig->IgdDvmt50PreAlloc));
+  DEBUG ((EFI_D_INFO, " InternalGraphics : %x\n", SaPlatformPolicyPpi->GtConfig->InternalGraphics));
+  DEBUG ((EFI_D_INFO, " PrimaryDisplay : %x\n", SaPlatformPolicyPpi->GtConfig->PrimaryDisplay));
+  DEBUG ((EFI_D_INFO, " ApertureSize : %x\n", SaPlatformPolicyPpi->GtConfig->ApertureSize));
+  DEBUG ((EFI_D_INFO, " GttMmAdr : %x\n", SaPlatformPolicyPpi->GtConfig->GttMmAdr));
+
+  DEBUG ((EFI_D_INFO, "------------------------ PCIE_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " DmiVc1 : %x\n", SaPlatformPolicyPpi->PcieConfig->DmiVc1));
+  DEBUG ((EFI_D_INFO, " DmiVcp : %x\n", SaPlatformPolicyPpi->PcieConfig->DmiVcp));
+  DEBUG ((EFI_D_INFO, " DmiVcm : %x\n", SaPlatformPolicyPpi->PcieConfig->DmiVcm));
+  DEBUG ((EFI_D_INFO, " DmiGen2 : %x\n", SaPlatformPolicyPpi->PcieConfig->DmiGen2));
+  DEBUG ((EFI_D_INFO, " AlwaysEnablePeg : %x\n", SaPlatformPolicyPpi->PcieConfig->AlwaysEnablePeg));
+
+  DEBUG ((EFI_D_INFO, " PegGenx[%d] :", SA_PEG_MAX_FUN));
+  for (i = 0; i < SA_PEG_MAX_FUN; i++) {
+    DEBUG ((EFI_D_INFO, " %x", SaPlatformPolicyPpi->PcieConfig->PegGenx[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " PegGen3Equalization : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3Equalization));
+
+  DEBUG ((EFI_D_INFO, " Gen3RootPortPreset[%d] :", SA_PEG_MAX_LANE));
+  for (i = 0; i < SA_PEG_MAX_LANE; i++) {
+    DEBUG ((EFI_D_INFO, " %x", SaPlatformPolicyPpi->PcieConfig->Gen3RootPortPreset[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " Gen3EndPointPreset[%d] :", SA_PEG_MAX_LANE));
+  for (i = 0; i < SA_PEG_MAX_LANE; i++) {
+    DEBUG ((EFI_D_INFO, " %x", SaPlatformPolicyPpi->PcieConfig->Gen3EndPointPreset[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " Gen3EndPointHint[%d] :", SA_PEG_MAX_LANE));
+  for (i = 0; i < SA_PEG_MAX_LANE; i++) {
+    DEBUG ((EFI_D_INFO, " %x", SaPlatformPolicyPpi->PcieConfig->Gen3EndPointHint[i]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  DEBUG ((EFI_D_INFO, " PegSamplerCalibrate : %x\n", SaPlatformPolicyPpi->PcieConfig->PegSamplerCalibrate));
+  DEBUG ((EFI_D_INFO, " PegGen3EqualizationPhase2 : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3EqualizationPhase2));
+  DEBUG ((EFI_D_INFO, " PegGen3PresetSearch : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearch));
+  DEBUG ((EFI_D_INFO, " PegGen3PresetSearchDwellTime : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchDwellTime));
+  DEBUG ((EFI_D_INFO, " PegGen3PresetSearchMarginSteps : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchMarginSteps));
+  DEBUG ((EFI_D_INFO, " PegGen3PresetSearchStartMargin : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchStartMargin));
+  DEBUG ((EFI_D_INFO, " PegSwingControl : %x\n", SaPlatformPolicyPpi->PcieConfig->PegSwingControl));
+  if (SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_2) {
+    DEBUG ((EFI_D_INFO, " PegGen3PresetSearchVoltageMarginSteps : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchVoltageMarginSteps));
+    DEBUG ((EFI_D_INFO, " PegGen3PresetSearchVoltageStartMargin : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchVoltageStartMargin));
+    DEBUG ((EFI_D_INFO, " PegGen3PresetSearchFavorTiming : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchFavorTiming));
+    DEBUG ((EFI_D_INFO, " PegDataPtr : %p\n", SaPlatformPolicyPpi->PcieConfig->PegDataPtr));
+    DEBUG ((EFI_D_INFO, " PegGen3ForcePresetSearch : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3ForcePresetSearch));
+    DEBUG ((EFI_D_INFO, " InitPcieAspmAfterOprom : %x\n", SaPlatformPolicyPpi->PcieConfig->InitPcieAspmAfterOprom));
+    DEBUG ((EFI_D_INFO, " SaIotrapSmiAddress : %x\n", SaPlatformPolicyPpi->PcieConfig->SaIotrapSmiAddress));
+  }
+  if (SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_3) {
+    DEBUG ((EFI_D_INFO, " PegGpioData : %p\n", SaPlatformPolicyPpi->PcieConfig->PegGpioData));
+    if (SaPlatformPolicyPpi->PcieConfig->PegGpioData != NULL) {
+      DEBUG ((EFI_D_INFO, " PegGpioData->GpioSupport : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGpioData->GpioSupport));
+      if (SaPlatformPolicyPpi->PcieConfig->PegGpioData->SaPegReset != NULL) {
+        DEBUG ((EFI_D_INFO, " PegGpioData->SaPegReset->Value : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGpioData->SaPegReset->Value));
+        DEBUG ((EFI_D_INFO, " PegGpioData->SaPegReset->Active : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGpioData->SaPegReset->Active));
+      }
+    }
+    DEBUG ((EFI_D_INFO, " PegGen3PresetSearchErrorTarget : %x\n", SaPlatformPolicyPpi->PcieConfig->PegGen3PresetSearchErrorTarget));
+    DEBUG ((EFI_D_INFO, " RxCEMLoopback : %x\n", SaPlatformPolicyPpi->PcieConfig->RxCEMLoopback));
+    DEBUG ((EFI_D_INFO, " RxCEMLoopbackLane : %x\n", SaPlatformPolicyPpi->PcieConfig->RxCEMLoopbackLane));
+
+    DEBUG ((EFI_D_INFO, " Gen3RxCtleP[%d] :", SA_PEG_MAX_BUNDLE));
+    for (i = 0; i < SA_PEG_MAX_BUNDLE; i++) {
+      DEBUG ((EFI_D_INFO, " %x", SaPlatformPolicyPpi->PcieConfig->Gen3RxCtleP[i]));
+     }
+    DEBUG ((EFI_D_INFO, "\n"));
+  }
+
+  DEBUG ((EFI_D_INFO, "------------------------ OVERCLOCKING_CONFIGURATION -----------------\n"));
+  DEBUG ((EFI_D_INFO, " GtVoltageOffset : %x\n", SaPlatformPolicyPpi->OcConfig->GtVoltageOffset));
+  DEBUG ((EFI_D_INFO, " GtVoltageOverride : %x\n", SaPlatformPolicyPpi->OcConfig->GtVoltageOverride));
+  DEBUG ((EFI_D_INFO, " GtExtraTurboVoltage : %x\n", SaPlatformPolicyPpi->OcConfig->GtExtraTurboVoltage));
+  DEBUG ((EFI_D_INFO, " GtMaxOcTurboRatio : %x\n", SaPlatformPolicyPpi->OcConfig->GtMaxOcTurboRatio));
+  DEBUG ((EFI_D_INFO, " SaVoltageOffset : %x\n", SaPlatformPolicyPpi->OcConfig->SaVoltageOffset));
+  DEBUG ((EFI_D_INFO, " GtVoltageMode : %x\n", SaPlatformPolicyPpi->OcConfig->GtVoltageMode));
+  DEBUG ((EFI_D_INFO, " OcSupport : %x\n", SaPlatformPolicyPpi->OcConfig->OcSupport));
+  DEBUG ((EFI_D_INFO, " IoaVoltageOffset : %x\n", SaPlatformPolicyPpi->OcConfig->IoaVoltageOffset));
+  DEBUG ((EFI_D_INFO, " IodVoltageOffset : %x\n", SaPlatformPolicyPpi->OcConfig->IodVoltageOffset));
+
+  DEBUG ((EFI_D_INFO, "------------------------ Misc -----------------\n"));
+  DEBUG ((EFI_D_INFO, " S3DataPtr : %x\n", SaPlatformPolicyPpi->S3DataPtr));
+
+  if (SaPlatformPolicyPpi->Revision >= SA_PLATFORM_POLICY_PPI_REVISION_3) {
+    DEBUG ((EFI_D_INFO, "------------------------ SG_GPIO_DATA -----------------\n"));
+    DEBUG ((EFI_D_INFO, " SgGpioData : %x\n", SaPlatformPolicyPpi->SgGpioData));
+    if (SaPlatformPolicyPpi->SgGpioData != NULL) {
+      DEBUG ((EFI_D_INFO, " SgGpioData->GpioSupport : %x\n", SaPlatformPolicyPpi->SgGpioData->GpioSupport));
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "\n------------------------ SA Platform Policy (PEI) dump END -----------------\n"));
+#endif
+  return;
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.cif b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.cif
new file mode 100644
index 0000000..c60de34
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.cif
@@ -0,0 +1,30 @@
+<component>
+	name = "SaInitPeim"
+	category = ModulePart
+	LocalRoot = "ReferenceCode\Chipset\SystemAgent\SaInit\Pei"
+	RefName = "SaInitPeim"
+[files]
+"SaInitPeim.sdl"	
+"SaInitPeim.mak"	
+"SaInitPeim.h"	
+"SaInitPeim.c"	
+"SaInitPeim.dxs"	
+"SaInitPeim.inf"	
+"GraphicsInit.h"	
+"GraphicsInit.c"	
+"SaDmiPeim.h"	
+"SaDmiPeim.c"	
+"PciExpressInit.h"	
+"PciExpressInit.c"	
+"PcieTraining.c"	
+"PcieTraining.h"	
+"PcieTrainingEqSettings.c"	
+"PcieTrainingErrorCount.c"	
+"PcieTrainingLinkRecovery.c"	
+"PcieTrainingMargining.c"	
+"PcieTrainingPhase3.c"	
+"SaOcInit.c"	
+"SaOcInit.h"	
+"SwitchableGraphicsInit.c"	
+"SwitchableGraphicsInit.h"	
+<endComponent>
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.dxs b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.dxs
new file mode 100644
index 0000000..4eedf37
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.dxs
@@ -0,0 +1,49 @@
+/** @file
+  Dependency expression source file.
+
+@copyright
+  Copyright (c)  2009 - 2013 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains a 'Sample Driver' and is licensed as such
+  under the terms of your license agreement with Intel or your
+  vendor.  This file may be modified by the user, subject to
+  the additional terms of the license agreement
+
+**/
+  
+
+//
+// Common for R8 and R9 codebase
+//
+#include "AutoGen.h"
+#include "PeimDepex.h"
+
+//
+// BUILD_WITH_GLUELIB and BUILD_WITH_EDKII_GLUE_LIB are both "defined" in R8 codebase;
+// BUILD_WITH_EDKII_GLUE_LIB is defined in Edk-Dev-Snapshot-20070228 and later version
+// BUILD_WITH_GLUELIB and BUILD_WITH_EDKII_GLUE_LIB are "not defined" in R9 codebase.
+//
+#if defined (BUILD_WITH_GLUELIB) || defined (BUILD_WITH_EDKII_GLUE_LIB)
+#include "EfiDepex.h"
+#endif
+
+#include EFI_PPI_DEPENDENCY (SaPlatformPolicy)
+#include EFI_PPI_DEPENDENCY (CpuPlatformPolicy)
+#include EFI_PPI_DEPENDENCY (PchInit)
+#include EFI_PPI_DEPENDENCY (Stall)
+#include EFI_PPI_DEPENDENCY (PchMeUma)
+
+DEPENDENCY_START
+  SA_PLATFORM_POLICY_PPI_GUID      AND
+  PCH_INIT_PPI_GUID                AND
+  PEI_STALL_PPI_GUID               AND
+  PEI_CPU_PLATFORM_POLICY_PPI_GUID
+  AND PCH_ME_UMA_PPI_GUID
+DEPENDENCY_END
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.h
new file mode 100644
index 0000000..2f3263c
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.h
@@ -0,0 +1,260 @@
+/** @file
+  Header file for the SA Init PEIM
+
+@copyright
+  Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _SA_INIT_PEIM_H_
+#define _SA_INIT_PEIM_H_
+
+///
+/// External include files do NOT need to be explicitly specified in real EDKII
+/// environment
+///
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGluePeim.h"
+#include "SaAccess.h"
+#include "MemInfoHob.h"
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+#include "SaOcInit.h"
+
+#include EFI_PPI_DEPENDENCY  (SaPlatformPolicy)
+#include EFI_PPI_DEPENDENCY  (CpuPlatformPolicy)
+#include EFI_PPI_PRODUCER    (SaPeiInit)
+#include EFI_GUID_DEFINITION (SaDataHob)
+#endif
+///
+/// Data definitions & structures
+///
+
+EFI_GUID  gMemoryInitHobGuid = EFI_PEI_PERMANENT_MEMORY_INSTALLED_PPI_GUID;
+
+///
+/// Functions
+///
+EFI_STATUS
+EFIAPI
+SaInitPeiEntryPoint (
+  IN EFI_FFS_FILE_HEADER       *FfsHeader,
+  IN EFI_PEI_SERVICES          **PeiServices
+  )
+/**
+  SA PEI Initialization.
+
+  @param[in] FfsHeader    - Pointer to Firmware File System file header.
+  @param[in] PeiServices  - General purpose services available to every PEIM.
+
+  @retval EFI_SUCCESS
+**/
+;
+
+VOID
+ProgramSaBars (
+  IN    SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+/**
+  Programs Sa Bars
+
+  @param[in] SaPlatformPolicyPpi  - Instance of SA_PLATFORM_POLICY_PPI
+**/
+;
+
+VOID
+SwitchableGraphicsInit (
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI       *SaPlatformPolicyPpi
+  )
+/**
+  SwitchableGraphicsInit: Initialize the Switchable Graphics if enabled
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the SgConfig related information
+**/
+;
+
+VOID
+EarlyBiosPostProgramming (
+  IN    SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  Do Early BIOS POST Programming
+
+  @param[in] SaPlatformPolicyPpi - Instance of SA_PLATFORM_POLICY_PPI
+**/
+;
+
+VOID
+GraphicsInit (
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI       *SaPlatformPolicyPpi
+  )
+/**
+  GraphicsInit: Initialize the IGD if no other external graphics is present
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+**/
+;
+
+EFI_STATUS
+SaDmiTcVcInit (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+/**
+  Initialize DMI Tc/Vc mapping through SA-PCH.
+
+  @param[in] PeiServices          - General purpose services available to every PEIM.
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+
+  @retval EFI_SUCCESS
+**/
+;
+
+#ifdef DMI_FLAG
+EFI_STATUS
+DmiInit (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+/**
+  Initialize DMI.
+
+  @param[in] PeiServices          - General purpose services available to every PEIM.
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+
+  @retval EFI_SUCCESS
+**/
+;
+#endif // DMI_FLAG
+
+#ifdef PEG_FLAG
+VOID
+PciExpressInit (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+/**
+  GraphicsInit: Initialize the IGD if no other external graphics is present
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+**/
+;
+#endif // PEG_FLAG
+
+EFI_STATUS
+InstallSaHob (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI    *SaPlatformPolicyPpi
+  )
+/**
+  Init and Install SA Hob
+
+  @param[in] PeiServices  - General purpose services available to every PEIM.
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+
+  @retval EFI_SUCCESS
+**/
+;
+
+EFI_STATUS
+ReportPcieVersion (
+  IN    SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  Report the SA PCIe initialization code version.
+
+  @param[in] SaPlatformPolicyPpi - Instance of SA_PLATFORM_POLICY_PPI
+
+  @retval EFI_SUCCESS
+**/
+;
+
+STATIC
+EFI_STATUS
+SaResetComplete (
+  IN      EFI_PEI_SERVICES          **PeiServices,
+  IN      EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN      VOID                      *Ppi
+  )
+/**
+  BIOS_CPL_BIT is set for processor to activate the power and thermal management
+  features on the platform.
+
+  @param[in] PeiServices   - Pointer to PEI Services Table.
+  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
+                  caused this function to execute.
+  @param[in] Ppi           - Pointer to the PPI data associated with this function.
+
+  @retval EFI_SUCCESS
+**/
+;
+
+VOID
+SaPeiPolicyDump (
+  IN    SA_PLATFORM_POLICY_PPI   *SaPlatformPolicyPpi
+  )
+/**
+  This function prints the PEI phase platform policy.
+
+  @param[in] SaPlatformPolicyPpi - Instance of SA_PLATFORM_POLICY_PPI
+**/
+;
+
+static
+EFI_STATUS
+SaS3ResumeAtEndOfPei (
+  IN      EFI_PEI_SERVICES          **PeiServices,
+  IN      EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN      VOID                      *Ppi
+  )
+/**
+  This function handles SA S3 resume task
+
+  @param[in] PeiServices   - Pointer to PEI Services Table.
+  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
+                             caused this function to execute.
+  @param[in] Ppi           - Pointer to the PPI data associated with this function.
+
+  @retval EFI_STATUS       - Always return EFI_SUCCESS
+**/
+;
+
+#ifdef RAPID_START_FLAG
+static
+EFI_STATUS
+SaCheckRapidStartMode (
+  IN      EFI_PEI_SERVICES          **PeiServices,
+  IN      EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN      VOID                      *Ppi
+  )
+/**
+  This function will check Rapid Start mode and install SaS3Resume callback notify if it was Rapid Start Resume
+
+  @param[in] PeiServices   - Pointer to PEI Services Table.
+  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
+                             caused this function to execute.
+  @param[in] Ppi           - Pointer to the PPI data associated with this function.
+
+  @retval EFI_STATUS       - Always return EFI_SUCCESS
+**/
+;
+#endif // RAPID_START_FLAG
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.inf b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.inf
new file mode 100644
index 0000000..8ca75a4
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.inf
@@ -0,0 +1,132 @@
+## @file
+#  Component description file for the SA Init PEIM.
+#
+#@copyright
+#  Copyright (c)  2010 - 2014 Intel Corporation. All rights reserved
+#  This software and associated documentation (if any) is furnished
+#  under a license and may only be used or copied in accordance
+#  with the terms of the license. Except as permitted by such
+#  license, no part of this software or documentation may be
+#  reproduced, stored in a retrieval system, or transmitted in any
+#  form or by any means without the express written consent of
+#  Intel Corporation.
+#
+#  This file contains a 'Sample Driver' and is licensed as such
+#  under the terms of your license agreement with Intel or your
+#  vendor.  This file may be modified by the user, subject to
+#  the additional terms of the license agreement
+#
+
+[defines]
+BASE_NAME            = SaInitPeim
+FILE_GUID            = FD236AE7-0791-48c4-B29E-29BDEEE1A811
+COMPONENT_TYPE       = PE32_PEIM
+
+[sources.common]
+  SaInitPeim.h
+  SaInitPeim.c
+  GraphicsInit.h
+  GraphicsInit.c
+  SaDmiPeim.h
+  SaDmiPeim.c
+  PciExpressInit.h
+  PciExpressInit.c
+  SwitchableGraphicsInit.c
+  SwitchableGraphicsInit.h
+  PcieTraining.h
+  PcieTraining.c
+  PcieTrainingEqSettings.c
+  PcieTrainingErrorCount.c
+  PcieTrainingMargining.c
+  PcieTrainingLinkRecovery.c
+  PcieTrainingPhase3.c
+  SaOcInit.h
+  SaOcInit.c
+#
+# Edk II Glue Driver Entry Point
+#
+  EdkIIGluePeimEntryPoint.c
+
+
+[includes.common]
+  .
+  ../Common
+  $(EDK_SOURCE)/Foundation/Efi
+  $(EDK_SOURCE)/Foundation/Include
+  $(EDK_SOURCE)/Foundation/Efi/Include
+  $(EDK_SOURCE)/Foundation/Framework/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_RAPID_START_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Library
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Library/OverclockingLib
+
+#
+# EDK II Glue Library utilizes some standard headers from EDK
+#
+  $(EFI_SOURCE)
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)
+  $(EDK_SOURCE)/Foundation
+  $(EDK_SOURCE)/Foundation/Framework
+  $(EDK_SOURCE)/Foundation/Include/IndustryStandard
+  $(EDK_SOURCE)/Foundation/Core/Dxe
+  $(EDK_SOURCE)/Foundation/Include/Pei
+  $(EDK_SOURCE)/Foundation/Library/Pei/Include
+  $(EDK_SOURCE)/Foundation/Library/Dxe/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include/Library
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include/Library
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include/Pcd
+  $(EDK_SOURCE)/Foundation/Cpu/Pentium/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/$(PROJECT_SA_MRC)/Pei/Source/Api
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/$(PROJECT_SA_MRC)/Pei/Source/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/$(PROJECT_SA_MRC)/Pei/Source/Include/MrcRegisters
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/$(PROJECT_SA_MRC)/Pei
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include/Library
+  $(EFI_SOURCE)/$(PROJECT_ME_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_ME_ROOT)/PchMeUma
+
+[libraries.common]
+  $(PROJECT_PCH_FAMILY)PpiLib
+  EdkFrameworkPpiLib
+  EdkIIGlueBaseIoLibIntrinsic
+  EdkIIGlueBaseMemoryLib
+  EdkIIGluePeiDebugLibReportStatusCode
+  EdkIIGluePeiReportStatusCodeLib
+  EdkIIGluePeiServicesLib
+  EdkIIGluePeiMemoryAllocationLib
+  EdkIIGlueBasePciExpressLib
+  EdkIIGlueBasePostCodeLibPort80
+  PeiLib
+  $(PROJECT_SA_FAMILY)PpiLib
+  SAGuidLib
+  EdkPpiLib
+  CpuPpiLib
+  CpuPlatformLib
+  EdkIIGluePeiHobLib
+  PchPlatformLib
+  OverclockingLib
+  MeLibPpi
+#
+# Uncomment all the RapidStart include directories and library if RapidStart is supported
+#
+#  RapidStartPpiLib
+
+
+[nmake.common]
+  IMAGE_ENTRY_POINT = _ModuleEntryPoint
+  DPX_SOURCE        = SaInitPeim.dxs
+#
+# Module Entry Point
+#
+  C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_MODULE_ENTRY_POINT__=SaInitPeiEntryPoint
+  C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_BASE_IO_LIB_INTRINSIC__ \
+                       -D __EDKII_GLUE_BASE_MEMORY_LIB__ \
+                       -D __EDKII_GLUE_PEI_DEBUG_LIB_REPORT_STATUS_CODE__ \
+                       -D __EDKII_GLUE_PEI_REPORT_STATUS_CODE_LIB__ \
+                       -D __EDKII_GLUE_PEI_SERVICES_LIB__ \
+                       -D __EDKII_GLUE_PEI_MEMORY_ALLOCATION_LIB__ \
+                       -D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.mak b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.mak
new file mode 100644
index 0000000..935d761
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.mak
@@ -0,0 +1,72 @@
+#---------------------------------------------------------------------------
+#   Create SaInitPeim module
+#---------------------------------------------------------------------------
+all : SaInitPeim
+SaInitPeim : $(BUILD_DIR)\SaInitPeim.mak SaInitPeimBin
+
+$(BUILD_DIR)\SaInitPeim.mak : $(SaInitPeim_DIR)\$(@B).cif $(SaInitPeim_DIR)\$(@B).mak $(BUILD_RULES)
+  $(CIF2MAK) $(SaInitPeim_DIR)\$(@B).cif $(CIF2MAK_DEFAULTS)
+
+SaInitPeim_INCLUDES=\
+            $(INTEL_MCH_INCLUDES)\
+            $(INTEL_PCH_INCLUDES)\
+            $(EdkIIGlueLib_INCLUDES)\
+            $(PROJECT_CPU_INCLUDES)\
+            $(RAPIDSTART_INCLUDES)\
+            $(PchMeUma_INCLUDES)\
+            /I$(PROJECT_CPU_ROOT)\Library\OverclockingLib \
+
+SaInitPeim_DEFINES = $(MY_DEFINES)\
+            /D"__EDKII_GLUE_MODULE_ENTRY_POINT__=SaInitPeiEntryPoint"\
+            /D __EDKII_GLUE_BASE_LIB__ \
+            /D __EDKII_GLUE_BASE_MEMORY_LIB__ \
+            /D __EDKII_GLUE_BASE_IO_LIB_INTRINSIC__ \
+            /D __EDKII_GLUE_PEI_DEBUG_LIB_REPORT_STATUS_CODE__ \
+            /D __EDKII_GLUE_PEI_REPORT_STATUS_CODE_LIB__ \
+            /D __EDKII_GLUE_PEI_SERVICES_LIB__ \
+            /D __EDKII_GLUE_PEI_MEMORY_ALLOCATION_LIB__ \
+            /D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+
+SaInitPeim_LIB_LINKS =\
+            $(INTEL_SA_PPI_LIB) \
+            $(IntelPchPpiLib_LIB)\
+            $(EDKFRAMEWORKPPILIB) \
+            $(EdkIIGlueBaseLib_LIB)\
+            $(EdkIIGlueBaseLibIA32_LIB)\
+            $(EdkIIGlueBaseIoLibIntrinsic_LIB) \
+            $(EdkIIGluePeiDebugLibReportStatusCode_LIB) \
+            $(EdkIIGluePeiReportStatusCodeLib_LIB) \
+            $(EdkIIGluePeiServicesLib_LIB) \
+            $(EdkIIGluePeiMemoryAllocationLib_LIB) \
+            $(EdkIIGlueBasePciLibPciExpress_LIB) \
+            $(EdkIIGlueBasePciExpressLib_LIB)\
+            $(PEILIB)\
+            $(SaGuidLib_LIB)\
+            $(EdkIIGlueBasePciExpressLib_LIB)\
+            $(EdkIIGlueBasePostCodeLibPort80_LIB)\
+            $(CPU_PPI_LIB)\
+            $(CpuPlatformLib_LIB)\
+            $(PchPlatformPeiLib_LIB)\
+            $(EdkIIGluePeiHobLib_LIB)\
+            $(OcPlatformLib_LIB)\
+#
+# Uncomment all the RapidStart include directories and library if RapidStart is supported
+#
+#  RapidStartPpiLib
+            $(RapidStartPpiLib_LIB)\
+            $(MeLibPpi_LIB)
+
+SaInitPeimBin: $(SaInitPeim_LIB_LINKS)  
+            $(MAKE) /$(MAKEFLAGS) $(EDKIIGLUE_DEFAULTS)\
+            /f $(BUILD_DIR)\SaInitPeim.mak all \
+            "MY_INCLUDES=$(SaInitPeim_INCLUDES)"\
+            "MY_DEFINES=$(SaInitPeim_DEFINES)"\
+            NAME=SaInitPeim\
+            MAKEFILE=$(BUILD_DIR)\SaInitPeim.mak \
+            GUID=FD236AE7-0791-48c4-B29E-29BDEEE1A811\
+            ENTRY_POINT=_ModuleEntryPoint \
+            TYPE=PEIM \
+            EDKIIModule=PEIM\
+            DEPEX1=$(SaInitPeim_DIR)\SaInitPeim.dxs\
+            DEPEX1_TYPE=EFI_SECTION_PEI_DEPEX\
+            COMPRESS=0
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.sdl b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.sdl
new file mode 100644
index 0000000..38ceadb
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaInitPeim.sdl
@@ -0,0 +1,39 @@
+TOKEN
+  Name  = "SaInitPeim_SUPPORT"
+  Value = "1"
+  Help  = "Main switch to enable SaInitPeim support in Project"
+  TokenType = Boolean
+  TargetEQU = Yes
+  TargetMAK = Yes
+  Master = Yes
+End
+
+TOKEN
+	Name  = "SA_DEBUG_INFO"
+	Value  = "1"
+	TokenType = Boolean
+	Range  = "0-1 "
+	TargetMAK = Yes
+End
+
+ELINK
+	Name  = "/D SA_DEBUG_FLAG"
+	Parent  = "INTEL_SA_RC_FLAGS"
+	Token = "SA_DEBUG_INFO" "=" "1"
+	InvokeOrder = AfterParent
+End
+
+PATH
+  Name  = "SaInitPeim_DIR"
+End
+
+MODULE
+  File  = "SaInitPeim.mak"
+  Help  = "Includes SaInitPeim.mak to Project"
+End
+
+ELINK
+  Name  = "$(BUILD_DIR)\SaInitPeim.ffs"
+  Parent  = "FV_BB"
+  InvokeOrder = AfterParent
+End
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaOcInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaOcInit.c
new file mode 100644
index 0000000..80a1ffb
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaOcInit.c
@@ -0,0 +1,189 @@
+/** @file
+  OC System Agent Early Post initializations.
+
+@copyright
+  Copyright (c) 2011 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+///
+/// External include files do NOT need to be explicitly specified in real EDKII
+/// environment
+///
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGluePeim.h"
+#include "SaOcInit.h"
+#endif
+
+EFI_STATUS
+SaOcInit (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI      *SaPlatformPolicyPpi
+  )
+/**
+  Initializes Overclocking settings in the processor.
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] OverclockingtConfig      Pointer to Policy protocol instance
+
+  @retval EFI_SUCCESS
+**/
+{
+  EFI_STATUS    Status;
+  OC_CAPABILITIES_ITEM OcCaps;
+  VOLTAGE_FREQUENCY_ITEM CurrentVfItem;
+  VOLTAGE_FREQUENCY_ITEM RequestedVfItem;
+  UINT32      LibStatus;
+  UINT8       DomainId;
+  BOOLEAN     VfUpdateNeeded;
+  WDT_PPI     *gWdtPei;
+
+  LibStatus = 0;
+  VfUpdateNeeded = FALSE;
+
+  if (SaPlatformPolicyPpi->OcConfig->OcSupport == 0){
+    ///
+    /// Overclocking is disabled
+    ///
+    DEBUG ((EFI_D_ERROR, "(OC) Overclocking is disabled. Bypassing SA overclocking flow.\n"));
+    return EFI_SUCCESS;
+  }
+
+  Status = EFI_SUCCESS;
+  ZeroMem(&CurrentVfItem,sizeof(CurrentVfItem));
+  ZeroMem(&RequestedVfItem,sizeof(RequestedVfItem));
+
+  //
+  // Locate WDT_PPI (ICC WDT PPI)
+  //
+  Status = PeiServicesLocatePpi (
+             &gWdtPpiGuid,
+             0,
+             NULL,
+             (VOID **) &gWdtPei
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// We will loop on the CPU domains to manage the voltage/frequency settings
+  ///
+  for (DomainId = OC_LIB_DOMAIN_ID_GT; DomainId <= OC_LIB_DOMAIN_ID_IOD; DomainId++) {
+    ///
+    /// Only GT, Uncore, IOA, and IOD are valid for System Agent
+    ///
+    if ((DomainId == OC_LIB_DOMAIN_ID_GT) ||(DomainId == OC_LIB_DOMAIN_ID_UNCORE) ||
+        (DomainId == OC_LIB_DOMAIN_ID_IOA) || (DomainId == OC_LIB_DOMAIN_ID_IOD)){
+      ///
+      /// Get OC Capabilities of the domain
+      ///
+      ZeroMem(&OcCaps,sizeof(OcCaps));
+      OcCaps.DomainId = DomainId;
+      Status = GetOcCapabilities(&OcCaps,&LibStatus);
+
+      if (LibStatus == OC_LIB_COMPLETION_CODE_SUCCESS){
+        ///
+        /// If any OC is supported on this domain, then proceed
+        ///
+        if (OcCaps.RatioOcSupported || OcCaps.VoltageOverridesSupported || OcCaps.VoltageOffsetSupported){
+          ///
+          /// Need to populate the user requested settings from the platform policy
+          /// to determine if OC changes are desired.
+          ///
+          ZeroMem(&CurrentVfItem,sizeof(CurrentVfItem));
+          CurrentVfItem.DomainId = DomainId;
+
+          ///
+          /// Get a copy of the current domain VfSettings from the Mailbox Library
+          ///
+          Status = GetVoltageFrequencyItem(&CurrentVfItem,&LibStatus);
+          if ((Status != EFI_SUCCESS) || (LibStatus != OC_LIB_COMPLETION_CODE_SUCCESS)){
+            continue;
+          }
+
+          ///
+          /// Populate the user requested VfSettings struct
+          ///
+          ZeroMem(&RequestedVfItem,sizeof(RequestedVfItem));
+          RequestedVfItem.DomainId = DomainId;
+          if (DomainId == OC_LIB_DOMAIN_ID_GT){
+            RequestedVfItem.VfSettings.MaxOcRatio = (UINT8) SaPlatformPolicyPpi->OcConfig->GtMaxOcTurboRatio;
+
+            ///
+            /// VoltageTarget has 2 uses and we need to update the target based
+            /// on the voltagemode requested
+            ///
+            RequestedVfItem.VfSettings.VoltageTargetMode = SaPlatformPolicyPpi->OcConfig->GtVoltageMode;
+            if (RequestedVfItem.VfSettings.VoltageTargetMode == OC_LIB_OFFSET_ADAPTIVE){
+              RequestedVfItem.VfSettings.VoltageTarget = SaPlatformPolicyPpi->OcConfig->GtExtraTurboVoltage;
+            }
+            else {
+              RequestedVfItem.VfSettings.VoltageTarget = SaPlatformPolicyPpi->OcConfig->GtVoltageOverride;
+            }
+            RequestedVfItem.VfSettings.VoltageOffset = SaPlatformPolicyPpi->OcConfig->GtVoltageOffset;
+
+            VfUpdateNeeded = (BOOLEAN)CompareMem((VOID*)&RequestedVfItem,(VOID*)&CurrentVfItem,sizeof(VOLTAGE_FREQUENCY_ITEM));
+          }
+          else if ((DomainId == OC_LIB_DOMAIN_ID_UNCORE) || (DomainId == OC_LIB_DOMAIN_ID_IOA) || (DomainId == OC_LIB_DOMAIN_ID_IOD)){
+            ///
+            /// Uncore,IOA, and IOD domains only supports voltage offset, other settings are ignored
+            ///
+            switch (DomainId) {
+              case OC_LIB_DOMAIN_ID_UNCORE:
+                RequestedVfItem.VfSettings.VoltageOffset = SaPlatformPolicyPpi->OcConfig->SaVoltageOffset;
+              break;
+
+              case OC_LIB_DOMAIN_ID_IOA:
+                RequestedVfItem.VfSettings.VoltageOffset = SaPlatformPolicyPpi->OcConfig->IoaVoltageOffset;
+              break;
+
+              case OC_LIB_DOMAIN_ID_IOD:
+                RequestedVfItem.VfSettings.VoltageOffset = SaPlatformPolicyPpi->OcConfig->IodVoltageOffset;
+              break;
+            }
+
+            if (RequestedVfItem.VfSettings.VoltageOffset != CurrentVfItem.VfSettings.VoltageOffset)
+              VfUpdateNeeded = TRUE;
+          }
+
+          if (VfUpdateNeeded){
+            VfUpdateNeeded = FALSE;
+
+            ///
+            /// Arm watchdog timer for OC changes
+            ///
+            Status = gWdtPei->ReloadAndStart (WDT_TIMEOUT_BETWEEN_PEI_DXE);
+
+            ///
+            /// Need to update the requested voltage/frequency values
+            ///
+            Status = SetVoltageFrequencyItem(RequestedVfItem,&LibStatus);
+            if ((Status != EFI_SUCCESS) || (LibStatus != OC_LIB_COMPLETION_CODE_SUCCESS)){
+              DEBUG ((EFI_D_ERROR, "(OC) Set Voltage Frequency failed. EFI Status = %X, Library Status = %X\n", Status, LibStatus));
+            }
+          }
+        }
+        else {
+          DEBUG ((EFI_D_INFO, "(OC) No OC support for this Domain = %X\n", DomainId));
+        }
+      }
+      else {
+        DEBUG ((EFI_D_ERROR, "(OC) GetOcCapabilities message failed. Library Status = %X, Domain = %X\n", LibStatus, DomainId));
+      }
+    }
+  }
+
+  return Status;
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaOcInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaOcInit.h
new file mode 100644
index 0000000..0ebff94
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SaOcInit.h
@@ -0,0 +1,56 @@
+/** @file
+  Describes the functions visible to the rest of the OcInit.
+
+@copyright
+  Copyright (c) 2011 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+#ifndef _SA_INIT_H_
+#define _SA_INIT_H_
+
+#include "OverclockingLibrary.h"
+#include EFI_PPI_DEPENDENCY (CpuPlatformPolicy)
+#include EFI_PPI_DEPENDENCY (SaPlatformPolicy)
+#include EFI_PPI_CONSUMER (Wdt)
+
+#ifdef USE_WDT_IN_DEBUG_BIOS
+//
+// MRC takes a lot of time to execute in debug mode
+//
+#define WDT_TIMEOUT_BETWEEN_PEI_DXE 120
+#else
+#define WDT_TIMEOUT_BETWEEN_PEI_DXE 60
+#endif
+
+///
+/// Function Prototypes
+///
+EFI_STATUS
+SaOcInit (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI      *SaPlatformPolicyPpi
+  )
+/**
+  Initializes Overclocking settings in the processor.
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] OverclockingtConfig      Pointer to Policy protocol instance
+
+  @retval EFI_SUCCESS
+**/
+;
+
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SwitchableGraphicsInit.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SwitchableGraphicsInit.c
new file mode 100644
index 0000000..cbb8748
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SwitchableGraphicsInit.c
@@ -0,0 +1,288 @@
+/** @file
+  SwitchableGraphics Pei driver.
+  This Pei driver initialize GPIO programming
+  for the platform.
+
+@copyright
+  Copyright (c) 2010 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+
+**/
+
+#include "SwitchableGraphicsInit.h"
+#include EFI_GUID_DEFINITION (SaDataHob)
+
+#ifdef SG_SUPPORT
+
+/**
+  Initialize the SwitchableGraphics support (PEI).
+
+  @param[in] PeiServices          - Pointer to the PEI services table
+  @param[in] SaPlatformPolicyPpi  - SaPlatformPolicyPpi to access the GtConfig related information
+**/
+VOID
+SwitchableGraphicsInit (
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN SA_PLATFORM_POLICY_PPI       *SaPlatformPolicyPpi
+  )
+{
+  EFI_STATUS              Status;
+  PEI_STALL_PPI           *StallPpi;
+  EFI_GUID                StallPpiGuid             = PEI_STALL_PPI_GUID;
+  UINT16                  GpioAddress;
+  SA_DATA_HOB             *SaDataHob;
+  BOOLEAN                 SgDgpuPrsntGpioIsValid   = TRUE;
+  CPU_FAMILY              CpuFamilyId;
+
+  CpuFamilyId = GetCpuFamily();
+
+  ///
+  /// Get SaDataHob HOB
+  ///
+  SaDataHob = NULL;
+  SaDataHob = (SA_DATA_HOB *) GetFirstGuidHob (&gSaDataHobGuid);
+
+  if (SaDataHob != NULL) {
+    SaDataHob->SgInfo.SgMode = SaPlatformPolicyPpi->PlatformData->SgMode;
+    SaDataHob->SgInfo.PXFixedDynamicMode = SaPlatformPolicyPpi->PlatformData->PXFixedDynamicMode; // AMI_OVERRIDE_FOR ATI 5.0 Fixed/Dynamic 
+
+    ///
+    /// GPIO Assigned from policy
+    ///
+    SaDataHob->SgInfo.SgGpioSupport  = SaPlatformPolicyPpi->SgGpioData->GpioSupport;
+
+    if (SaPlatformPolicyPpi->SgGpioData->GpioSupport == 1) {
+      ///
+      /// Get GPIO Base Address
+      ///
+      GpioAddress = MmPci16 (0, 0, PCI_DEVICE_NUMBER_PCH_LPC, 0, R_PCH_LPC_GPIO_BASE) &~BIT0;
+
+      SaDataHob->SgInfo.SgDgpuPwrOK     = SaPlatformPolicyPpi->SgGpioData->SgDgpuPwrOK->Value;
+      SaDataHob->SgInfo.SgDgpuHoldRst   = SaPlatformPolicyPpi->SgGpioData->SgDgpuHoldRst->Value;
+      SaDataHob->SgInfo.SgDgpuPwrEnable = SaPlatformPolicyPpi->SgGpioData->SgDgpuPwrEnable->Value;
+      SaDataHob->SgInfo.SgDgpuPrsnt     = SaPlatformPolicyPpi->SgGpioData->SgDgpuPrsnt->Value;
+
+      ///
+      /// Set Bit7 as indicator for GPIO Active Low/High
+      ///
+      SaDataHob->SgInfo.SgDgpuPwrOK     |= (SaPlatformPolicyPpi->SgGpioData->SgDgpuPwrOK->Active     << 7);
+      SaDataHob->SgInfo.SgDgpuHoldRst   |= (SaPlatformPolicyPpi->SgGpioData->SgDgpuHoldRst->Active   << 7);
+      SaDataHob->SgInfo.SgDgpuPwrEnable |= (SaPlatformPolicyPpi->SgGpioData->SgDgpuPwrEnable->Active << 7);
+      SaDataHob->SgInfo.SgDgpuPrsnt     |= (SaPlatformPolicyPpi->SgGpioData->SgDgpuPrsnt->Active     << 7);
+
+      if (CpuFamilyId == EnumCpuHswUlt) {
+        PEI_DEBUG ((PeiServices, EFI_D_INFO, "SgDgpuPrsntGpioIsValid = FALSE\n"));
+        SgDgpuPrsntGpioIsValid = FALSE;
+      }
+
+      ///
+      /// Locate PPI stall service
+      ///
+      Status = (**PeiServices).LocatePpi (
+                                PeiServices,
+                                &StallPpiGuid,
+                                0,
+                                NULL,
+                                &StallPpi
+                                );
+      if (!EFI_ERROR (Status)) {
+        ///
+        /// if DGPU PRSNT is Disabled, it means that MXM card was not detected, and
+        /// DGPU HOLD RST must be driven high to allow the board to support a normal PEG card
+        ///
+        if (    (SgDgpuPrsntGpioIsValid)
+             && (GpioRead (PeiServices, CpuFamilyId, GpioAddress, SaDataHob->SgInfo.SgDgpuPrsnt) == GP_DISABLE)) {
+          GpioWrite (PeiServices, CpuFamilyId, GpioAddress, SaDataHob->SgInfo.SgDgpuHoldRst, GP_DISABLE);
+            ///
+            /// Set SG mode as disabled
+            ///
+            SaDataHob->SgInfo.SgMode = SgModeDisabled;
+        } else {
+          ///
+          /// DGPU PRSNT Enabled. MXM is present.
+          ///      If PEG Mode or SG Muxless
+          ///              Power on MXM
+          ///              Configure GPIOs to drive MXM in PEG mode or SG Muxless
+          ///      else
+          ///              Do Nothing
+          ///
+          if ((SaPlatformPolicyPpi->PlatformData->SgMode == SgModeMuxless) ||
+              (SaPlatformPolicyPpi->PlatformData->SgMode == SgModeDgpu)) {
+            PEI_DEBUG ((PeiServices, EFI_D_INFO, "Configure GPIOs for driving the dGPU.\n"));
+            ///
+            ///  Drive DGPU HOLD RST Enable to make sure we hold reset
+            ///
+            GpioWrite (PeiServices, CpuFamilyId, GpioAddress, SaDataHob->SgInfo.SgDgpuHoldRst, GP_ENABLE);
+            ///
+            /// wait 100ms
+            ///
+            StallPpi->Stall (
+                        PeiServices,
+                        StallPpi,
+                        SG_DELAY_HOLD_RST
+                        );
+
+            ///
+            /// Drive DGPU PWR EN to Power On MXM
+            ///
+            GpioWrite (PeiServices, CpuFamilyId, GpioAddress, SaDataHob->SgInfo.SgDgpuPwrEnable, GP_ENABLE);
+
+            ///
+            /// wait 300ms
+            ///
+            StallPpi->Stall (
+                        PeiServices,
+                        StallPpi,
+                        SG_DELAY_PWR_ENABLE
+                        );
+
+            ///
+            /// Drive DGPU HOLD RST Disabled to remove reset
+            ///
+            GpioWrite (PeiServices, CpuFamilyId, GpioAddress, SaDataHob->SgInfo.SgDgpuHoldRst, GP_DISABLE);
+
+            ///
+            /// wait 100ms
+            ///
+            StallPpi->Stall (
+                      PeiServices,
+                      StallPpi,
+                      SG_DELAY_HOLD_RST
+                      );
+          }
+        }
+      }
+    }
+  }
+  ///
+  /// Program SubsystemID for IGFX
+  ///
+  PEI_DEBUG ((PeiServices, EFI_D_INFO, "Program SDID [Subsystem ID] for IGFX: 0x%x\n", SaPlatformPolicyPpi->PlatformData->SgSubSystemId));
+  McD2PciCfg16Or (PCI_SID, SaPlatformPolicyPpi->PlatformData->SgSubSystemId);
+
+}
+
+/**
+  SG GPIO Read
+
+  @param[in] PeiServices - General purpose services available to every PEIM
+  @param[in] CpuFamilyId - Specifies the CPU family
+  @param[in] GpioAddress - GPIO base address
+  @param[in] Value       - PCH GPIO number
+
+  @retval GPIO read value (0/1)
+**/
+BOOLEAN
+GpioRead (
+  EFI_PEI_SERVICES          **PeiServices,
+  CPU_FAMILY                CpuFamilyId,
+  IN UINT16                 GpioAddress,
+  IN UINT8                  Value
+  )
+{
+  BOOLEAN Active;
+  UINT16  BitOffset=0;
+  UINT32  Data;
+
+  Active = (BOOLEAN) (Value >> 7);
+  Value &= 0x7F;
+
+  ASSERT (GpioAddress != 0);
+
+  if (CpuFamilyId == EnumCpuHswUlt) {
+      GpioAddress += R_PCH_GP_N_CONFIG0 + (Value * 0x08);
+      BitOffset = 30; //GPI_LVL
+  } else {
+    if (Value < 0x20) {
+      GpioAddress += R_PCH_GPIO_LVL;
+      BitOffset = Value;
+    } else if (Value < 0x40) {
+      GpioAddress += R_PCH_GPIO_LVL2;
+      BitOffset = Value - 0x20;
+    } else {
+      GpioAddress += R_PCH_GPIO_LVL3;
+      BitOffset = Value - 0x40;
+    }
+  }
+
+  Data = IoRead32 (GpioAddress);
+  Data >>= BitOffset;
+
+  if (Active == 0) {
+    Data = ~Data;
+  }
+
+  return (BOOLEAN) (Data & 0x1);
+}
+
+/**
+  SG GPIO Write
+
+  @param[in] PeiServices - General purpose services available to every PEIM
+  @param[in] CpuFamilyId - Specifies the CPU family
+  @param[in] GpioAddress - GPIO base address
+  @param[in] Value       - PCH GPIO number
+  @param[in] Level       - Write SG GPIO value (0/1)
+
+  @retval none
+**/
+VOID
+GpioWrite (
+  EFI_PEI_SERVICES          **PeiServices,
+  CPU_FAMILY                CpuFamilyId,
+  IN UINT16                 GpioAddress,
+  IN UINT8                  Value,
+  IN BOOLEAN                Level
+  )
+{
+  BOOLEAN Active;
+  UINT32  Data;
+  UINT16  BitOffset=0;
+
+  Active = (BOOLEAN) (Value >> 7);
+  Value &= 0x7F;
+
+  if (Active == 0) {
+    Level = (~Level) & 0x1;
+  }
+
+  ASSERT (GpioAddress != 0);
+
+  if (CpuFamilyId == EnumCpuHswUlt) {
+      GpioAddress += R_PCH_GP_N_CONFIG0 + (Value * 0x08);
+      BitOffset = 31; //GPO_LVL
+  } else {
+    if (Value < 0x20) {
+      GpioAddress += R_PCH_GPIO_LVL;
+      BitOffset = Value;
+    } else if (Value < 0x40) {
+      GpioAddress += R_PCH_GPIO_LVL2;
+      BitOffset = Value - 0x20;
+    } else {
+      GpioAddress += R_PCH_GPIO_LVL3;
+      BitOffset = Value - 0x40;
+    }
+  }
+
+  Data = IoRead32 (GpioAddress);
+  Data &= ~(0x1 << BitOffset);
+  Data |= (Level << BitOffset);
+
+  IoWrite32 (GpioAddress, Data);
+  return ;
+}
+
+#endif //SG_SUPPORT
\ No newline at end of file
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SwitchableGraphicsInit.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SwitchableGraphicsInit.h
new file mode 100644
index 0000000..7126b19
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Pei/SwitchableGraphicsInit.h
@@ -0,0 +1,93 @@
+/** @file
+  Header file for the SwitchableGraphics Pei driver.
+
+@copyright
+  Copyright (c) 2010 - 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+#ifndef _SWITCHABLE_GRAPHICS_PEI_H_
+#define _SWITCHABLE_GRAPHICS_PEI_H_
+
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGluePeim.h"
+#include "EdkIIGlueIoLib.h"
+#endif
+
+#include "CpuRegs.h"
+#include "CpuPlatformLib.h"
+
+#include "PchAccess.h"
+#include "SaAccess.h"
+
+#include EFI_PPI_DEPENDENCY (SaPlatformPolicy)
+#include EFI_PPI_DEPENDENCY (Stall)
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#ifndef HIGH
+#define HIGH  1
+#endif
+
+#ifndef LOW
+#define LOW 0
+#endif
+
+#define SG_DELAY_HOLD_RST    100 * STALL_ONE_MILLI_SECOND
+#define SG_DELAY_PWR_ENABLE  300 * STALL_ONE_MILLI_SECOND
+
+/**
+  SG GPIO Write
+
+  @param[in] PeiServices - General purpose services available to every PEIM
+  @param[in] CpuFamilyId - Specifies the CPU family
+  @param[in] GpioAddress - GPIO base address
+  @param[in] Value       - PCH GPIO number
+  @param[in] Level       - Write SG GPIO value (0/1)
+
+  @retval none
+**/
+VOID
+GpioWrite (
+  EFI_PEI_SERVICES          **PeiServices,
+  CPU_FAMILY                CpuFamilyId,
+  IN UINT16                 GpioAddress,
+  IN UINT8                  Value,
+  IN BOOLEAN                Level
+  )
+;
+
+/**
+  SG GPIO Read
+
+  @param[in] PeiServices - General purpose services available to every PEIM
+  @param[in] CpuFamilyId - Specifies the CPU family
+  @param[in] GpioAddress - GPIO base address
+  @param[in] Value       - PCH GPIO number
+
+  @retval GPIO read value (0/1)
+**/
+BOOLEAN
+GpioRead (
+  EFI_PEI_SERVICES          **PeiServices,
+  CPU_FAMILY                CpuFamilyId,
+  IN UINT16                 GpioAddress,
+  IN UINT8                  Value
+  )
+;
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.c b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.c
new file mode 100644
index 0000000..45ce0af
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.c
@@ -0,0 +1,204 @@
+/** @file
+  This SMM driver will handle SA relevant late initialization
+
+@copyright
+  Copyright (c) 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+///
+/// External include files do NOT need to be explicitly specified in real EDKII
+/// environment
+///
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "SaBuildFlags.h"
+#include "EdkIIGlueDxe.h"
+#include "SaLateInitSmm.h"
+#include "SaRegs.h"
+#include "SaAccess.h"
+#include "PchAccess.h"
+#include "CpuIA32.h"
+#include "SaPcieLib.h"
+#include EFI_PROTOCOL_DEPENDENCY (SmmIoTrapDispatch)
+#include EFI_GUID_DEFINITION (SaDataHob)
+#endif
+
+typedef enum {
+  EnumSaSmiCallbackForMaxPayLoad,
+  EnumSaSmiCallbackForSecurityLock,
+  EnumSaSmiCallbackForLateInit,
+  EnumSaSmiCallbackForS3resume,
+  EnumSaSmiCallbackMax
+} SMI_OPERATION;
+
+UINT8                           mSaSmiCallbackPhase = EnumSaSmiCallbackForMaxPayLoad;
+
+/**
+  A SMI callback to do SA relevant late initialization
+
+  @param[in] DispatchHandle  - The handle of this callback, obtained when registering
+  @param[in] DispatchContext - Pointer to the EFI_SMM_IO_TRAP_DISPATCH_CALLBACK_CONTEXT
+
+  @retval None
+**/
+VOID
+EFIAPI
+SaIoTrapSmiCallback (
+  IN  EFI_HANDLE                                  DispatchHandle,
+  IN  EFI_SMM_IO_TRAP_DISPATCH_CALLBACK_CONTEXT   *CallbackContext
+  )
+{
+
+  if (mSaSmiCallbackPhase == EnumSaSmiCallbackMax) {
+    return;
+  }
+  if (mSaSmiCallbackPhase == EnumSaSmiCallbackForMaxPayLoad) {
+    SaPcieEnumCallback ();
+    ///
+    /// Switch to next phase
+    ///
+    mSaSmiCallbackPhase = EnumSaSmiCallbackForSecurityLock;
+  } else if (mSaSmiCallbackPhase == EnumSaSmiCallbackForSecurityLock) {
+    ///
+    /// Save platform registers including IGFX BAR & COMMAND registers and PAM
+    ///
+    SaSaveRestorePlatform (TRUE);
+    SaSecurityLock ();
+    ///
+    /// Switch to next phase
+    ///
+    mSaSmiCallbackPhase = EnumSaSmiCallbackForLateInit;
+  } else if (mSaSmiCallbackPhase == EnumSaSmiCallbackForLateInit) {
+    ///
+    /// Expected to execute in ReadyToBoot point (after OROM)
+    ///
+    SaPcieConfigAfterOpRom ();
+    ///
+    /// Switch to next phase
+    ///
+    mSaSmiCallbackPhase = EnumSaSmiCallbackForS3resume;
+  } else if (mSaSmiCallbackPhase == EnumSaSmiCallbackForS3resume) {
+    ///
+    /// Expected to execute in end of S3 resume flow
+    ///
+    SaS3ResumeCallback ();
+  }
+}
+
+/**
+  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  - DxePlatformSaPolicy not found
+**/
+EFI_STATUS
+SaLateInitSmmEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_SMM_IO_TRAP_DISPATCH_PROTOCOL         *PchIoTrap;
+  EFI_HANDLE                                PchIoTrapHandle;
+  EFI_SMM_IO_TRAP_DISPATCH_REGISTER_CONTEXT PchIoTrapContext;
+  SA_DATA_HOB                               *SaDataHob;
+  DXE_PLATFORM_SA_POLICY_PROTOCOL           *DxePlatformSaPolicy;
+  BOOLEAN                                   InitPcieAspmAfterOprom;
+  EFI_PHYSICAL_ADDRESS                      IotrapAddress;
+
+  DEBUG ((EFI_D_INFO, "SaLateInitSmmEntryPoint()\n"));
+
+  SaDataHob = NULL;
+  SaDataHob = (SA_DATA_HOB *)GetFirstGuidHob (&gSaDataHobGuid);
+  Status = EFI_NOT_FOUND;
+  if ((SaDataHob != NULL)) {
+    ///
+    /// Locate the PCH Trap dispatch protocol
+    ///
+    Status = gBS->LocateProtocol (&gEfiSmmIoTrapDispatchProtocolGuid, NULL, (VOID **) &PchIoTrap);
+    ASSERT_EFI_ERROR (Status);
+    if ((Status == EFI_SUCCESS) && (SaDataHob->SaIotrapSmiAddress != 0)) {
+      ///
+      /// Allocate 16 byte range from GCD for this IO trap address
+      ///
+      IotrapAddress = SaDataHob->SaIotrapSmiAddress;
+      DEBUG ((EFI_D_INFO, "Iotrap address=%X\n", SaDataHob->SaIotrapSmiAddress));
+      Status = gDS->AllocateIoSpace (
+                      EfiGcdAllocateAddress,
+                      EfiGcdIoTypeIo,
+                      0,
+                      0x10,
+                      &IotrapAddress,
+                      ImageHandle,
+                      NULL
+                      );
+      ASSERT_EFI_ERROR (Status);
+      if (Status == EFI_SUCCESS) {
+        PchIoTrapContext.Type     = ReadWriteTrap;
+        PchIoTrapContext.Length   = 4;
+        PchIoTrapContext.Address  = SaDataHob->SaIotrapSmiAddress;
+        PchIoTrapContext.Context  = NULL;
+        PchIoTrapContext.MergeDisable = FALSE;
+        Status = PchIoTrap->Register (
+                              PchIoTrap,
+                              SaIoTrapSmiCallback,
+                              &PchIoTrapContext,
+                              &PchIoTrapHandle
+                              );
+        ASSERT_EFI_ERROR (Status);
+        if (Status == EFI_SUCCESS) {
+          InitPcieAspmAfterOprom = SaDataHob->InitPcieAspmAfterOprom;
+#if SA_PCIE_ASPM_IN_DXE == 0
+          ///
+          /// There is no DXE ASPM code so always executes SMM code
+          ///
+          InitPcieAspmAfterOprom = 1;
+#endif
+          if (InitPcieAspmAfterOprom == 1) {
+            ///
+            /// Initialize module global variables - Stepping ID and Platform Policy for runtime SMI handler
+            /// Get the platform setup policy.
+            ///
+            Status = gBS->LocateProtocol (&gDxePlatformSaPolicyGuid, NULL, (VOID **) &DxePlatformSaPolicy);
+            ASSERT_EFI_ERROR (Status);
+            if (DxePlatformSaPolicy != NULL) {
+              SaPcieInitPolicy (DxePlatformSaPolicy);
+            }
+          } else {
+            ///
+            /// InitPcieAspmAfterOprom was not available or disabled, make this SMI handler directly return.
+            ///
+            mSaSmiCallbackPhase = EnumSaSmiCallbackMax;
+          }
+        }
+      }
+    }
+  }
+
+  ///
+  /// For security consideration, if this SMM driver was compiled/executed, the IOTRAP SMI handler must be registered successfully.
+  /// If not, hang system here
+  ///
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "Failed to register SaIotrapSmiCallback! System halt!\n"));
+    EFI_DEADLOOP ();
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.cif b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.cif
new file mode 100644
index 0000000..6fbd4ba
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.cif
@@ -0,0 +1,13 @@
+<component>
+	name = "SaLateInitSmm"
+	category = ModulePart
+	LocalRoot = "ReferenceCode\Chipset\SystemAgent\SaInit\Smm\"
+	RefName = "SaLateInitSmm"
+[files]
+"SaLateInitSmm.sdl"	
+"SaLateInitSmm.mak"	
+"SaLateInitSmm.h"	
+"SaLateInitSmm.c"	
+"SaLateInitSmm.dxs"	
+"SaLateInitSmm.inf"	
+<endComponent>
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.dxs b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.dxs
new file mode 100644
index 0000000..8890d7d
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.dxs
@@ -0,0 +1,45 @@
+/** @file
+  Dependency expression source file.
+
+@copyright
+  Copyright (c)  2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains a 'Sample Driver' and is licensed as such
+  under the terms of your license agreement with Intel or your
+  vendor.  This file may be modified by the user, subject to
+  the additional terms of the license agreement
+
+**/
+ 
+
+//
+// Common for R8 and R9 codebase
+//
+#include "AutoGen.h"
+#include "DxeDepex.h"
+
+//
+// BUILD_WITH_GLUELIB and BUILD_WITH_EDKII_GLUE_LIB are both "defined" in R8 codebase;
+// BUILD_WITH_EDKII_GLUE_LIB is defined in Edk-Dev-Snapshot-20070228 and later version
+// BUILD_WITH_GLUELIB and BUILD_WITH_EDKII_GLUE_LIB are "not defined" in R9 codebase.
+//
+#if defined (BUILD_WITH_GLUELIB) || defined (BUILD_WITH_EDKII_GLUE_LIB)
+#include "EfiDepex.h"
+
+#include EFI_PROTOCOL_DEPENDENCY (SmmBase)
+#include EFI_PROTOCOL_DEPENDENCY (SmmIoTrapDispatch)
+#include EFI_PROTOCOL_DEPENDENCY (SaPlatformPolicy)
+#endif
+
+DEPENDENCY_START
+  EFI_SMM_BASE_PROTOCOL_GUID AND
+  EFI_SMM_IO_TRAP_DISPATCH_PROTOCOL_GUID AND
+  DXE_PLATFORM_SA_POLICY_GUID
+DEPENDENCY_END
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.h b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.h
new file mode 100644
index 0000000..71ff7e4
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.h
@@ -0,0 +1,39 @@
+/** @file
+  Header file for SA SMM Handler
+
+@copyright
+  Copyright (c) 2012 Intel Corporation. All rights reserved
+  This software and associated documentation (if any) is furnished
+  under a license and may only be used or copied in accordance
+  with the terms of the license. Except as permitted by such
+  license, no part of this software or documentation may be
+  reproduced, stored in a retrieval system, or transmitted in any
+  form or by any means without the express written consent of
+  Intel Corporation.
+
+  This file contains an 'Intel Peripheral Driver' and uniquely
+  identified as "Intel Reference Module" and is
+  licensed for Intel CPUs and chipsets under the terms of your
+  license agreement with Intel or your vendor.  This file may
+  be modified by the user, subject to additional terms of the
+  license agreement
+**/
+
+#ifndef _SaLateInitSmm_H_
+#define _SaLateInitSmm_H_
+
+///
+/// External include files do NOT need to be explicitly specified in real EDKII
+/// environment
+///
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+///
+/// Driver Consumed Protocol Prototypes
+///
+#include EFI_PROTOCOL_DEPENDENCY (SmmBase)
+#include EFI_PROTOCOL_DEPENDENCY (SmmIchnDispatch)
+#include EFI_PROTOCOL_DEPENDENCY (SaPlatformPolicy)
+
+#endif
+#endif
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.inf b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.inf
new file mode 100644
index 0000000..3fd8846
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.inf
@@ -0,0 +1,97 @@
+## @file
+#  Component description file for the SA late initialization SMM module.
+#
+#@copyright
+#  Copyright (c)  2012 Intel Corporation. All rights reserved
+#  This software and associated documentation (if any) is furnished
+#  under a license and may only be used or copied in accordance
+#  with the terms of the license. Except as permitted by such
+#  license, no part of this software or documentation may be
+#  reproduced, stored in a retrieval system, or transmitted in any
+#  form or by any means without the express written consent of
+#  Intel Corporation.
+#
+#  This file contains a 'Sample Driver' and is licensed as such
+#  under the terms of your license agreement with Intel or your
+#  vendor.  This file may be modified by the user, subject to
+#  the additional terms of the license agreement
+#
+
+[defines]
+BASE_NAME            = SaLateInitSmm
+FILE_GUID            = 2D1E361C-7B3F-4d15-8B1F-66E551FABDC7
+COMPONENT_TYPE       = RT_DRIVER
+
+[sources.common]
+  SaLateInitSmm.c
+  SaLateInitSmm.h
+
+#
+# Edk II Glue Driver Entry Point
+#
+  EdkIIGlueSmmDriverEntryPoint.c
+
+[includes.common]
+  $(EDK_SOURCE)/Foundation
+  $(EDK_SOURCE)/Foundation/Efi
+  $(EDK_SOURCE)/Foundation/Framework
+  $(EDK_SOURCE)/Foundation/Include
+  $(EDK_SOURCE)/Foundation/Efi/Include
+  $(EDK_SOURCE)/Foundation/Framework/Include
+  $(EDK_SOURCE)/Foundation/Include/IndustryStandard
+  $(EDK_SOURCE)/Foundation/Core/Dxe
+  $(EDK_SOURCE)/Foundation/Core/Dxe/Include
+  $(EDK_SOURCE)/Foundation/Library/Dxe/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include/Pcd
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include/Library
+  $(EDK_SOURCE)/Foundation/Cpu/Pentium/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Library/SaPcieLib/Common
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)
+
+
+[libraries.common]
+  EdkProtocolLib
+  ArchProtocolLib
+  EdkFrameworkProtocolLib
+  EdkIIGlueBaseLib
+  EdkIIGlueBaseIoLibIntrinsic
+  EdkIIGlueBaseMemoryLib
+  EdkIIGlueDxeMemoryAllocationLib
+  EdkIIGlueDxeDebugLibReportStatusCode
+  EdkIIGlueSmmRuntimeDxeReportStatusCodeLib
+  EdkIIGlueBasePciLibPciExpress
+  EdkIIGlueUefiLib
+  EdkIIGlueUefiBootServicesTableLib
+  EdkIIGlueDxeServicesTableLib
+  EdkIIGlueUefiRuntimeServicesTableLib
+  EdkIIGlueUefiDevicePathLib
+  EfiProtocolLib
+  $(PROJECT_SA_FAMILY)ProtocolLib
+  SaGuidLib
+  EdkIIGlueDxeHobLib
+  CpuPlatformLib
+  SaPcieSmmLib
+  $(PROJECT_PCH_FAMILY)ProtocolLib
+
+[nmake.common]
+  IMAGE_ENTRY_POINT=_ModuleEntryPoint
+  DPX_SOURCE=SaLateInitSmm.dxs
+
+  C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_MODULE_ENTRY_POINT__=SaLateInitSmmEntryPoint
+  C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_BASE_LIB__ \
+                       -D __EDKII_GLUE_BASE_IO_LIB_INTRINSIC__ \
+                       -D __EDKII_GLUE_BASE_MEMORY_LIB__ \
+                       -D __EDKII_GLUE_DXE_MEMORY_ALLOCATION_LIB__ \
+                       -D __EDKII_GLUE_DXE_DEBUG_LIB_REPORT_STATUS_CODE__ \
+                       -D __EDKII_GLUE_SMM_RUNTIME_DXE_REPORT_STATUS_CODE_LIB__ \
+                       -D __EDKII_GLUE_UEFI_LIB__\
+                       -D __EDKII_GLUE_DXE_SERVICES_TABLE_LIB__ \
+                       -D __EDKII_GLUE_UEFI_BOOT_SERVICES_TABLE_LIB__ \
+                       -D __EDKII_GLUE_UEFI_RUNTIME_SERVICES_TABLE_LIB__ \
+                       -D __EDKII_GLUE_UEFI_DEVICE_PATH_LIB__ \
+                       -D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+                       -D __EDKII_GLUE_DXE_HOB_LIB__
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.mak b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.mak
new file mode 100644
index 0000000..dfa035d
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.mak
@@ -0,0 +1,96 @@
+#*************************************************************************
+#*************************************************************************
+#**                                                                     **
+#**        (C)Copyright 1985-2011, American Megatrends, Inc.            **
+#**                                                                     **
+#**                       All Rights Reserved.                          **
+#**                                                                     **
+#**      5555 Oakbrook Parkway, Suite 200, Norcross, GA 30093           **
+#**                                                                     **
+#**                       Phone: (770)-246-8600                         **
+#**                                                                     **
+#*************************************************************************
+#*************************************************************************
+
+#---------------------------------------------------------------------------
+#   Create SaLateInitSmm Driver
+#---------------------------------------------------------------------------
+EDK : SaLateInitSmm
+SaLateInitSmm : $(BUILD_DIR)\SaLateInitSmm.mak SaLateInitSmmBin
+
+
+$(BUILD_DIR)\SaLateInitSmm.mak : $(SaLateInitSmm_DIR)\$(@B).cif $(SaLateInitSmm_DIR)\$(@B).mak $(BUILD_RULES)
+  $(CIF2MAK) $(SaLateInitSmm_DIR)\$(@B).cif $(CIF2MAK_DEFAULTS)
+
+SaLateInitSmm_INCLUDES=\
+        $(EdkIIGlueLib_INCLUDES)\
+        $(INTEL_MCH_INCLUDES)\
+        $(INTEL_PCH_INCLUDES)\
+        /I$(INTEL_SYSTEM_AGENT_DIR)\Library\SaPcieLib\Common\
+
+SaLateInitSmm_DEFINES = $(MY_DEFINES)\
+      /D"__EDKII_GLUE_MODULE_ENTRY_POINT__=SaLateInitSmmEntryPoint"\
+      /D __EDKII_GLUE_BASE_LIB__ \
+      /D __EDKII_GLUE_BASE_IO_LIB_INTRINSIC__ \
+      /D __EDKII_GLUE_BASE_MEMORY_LIB__ \
+      /D __EDKII_GLUE_DXE_MEMORY_ALLOCATION_LIB__ \
+      /D __EDKII_GLUE_DXE_DEBUG_LIB_REPORT_STATUS_CODE__ \
+      /D __EDKII_GLUE_SMM_RUNTIME_DXE_REPORT_STATUS_CODE_LIB__ \
+      /D __EDKII_GLUE_UEFI_LIB__ \
+      /D __EDKII_GLUE_DXE_SERVICES_TABLE_LIB__ \
+      /D __EDKII_GLUE_UEFI_BOOT_SERVICES_TABLE_LIB__ \
+      /D __EDKII_GLUE_UEFI_RUNTIME_SERVICES_TABLE_LIB__ \
+      /D __EDKII_GLUE_UEFI_DEVICE_PATH_LIB__ \
+      /D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+      /D __EDKII_GLUE_DXE_HOB_LIB__ \
+
+SaLateInitSmm_LIB_LINKS =\
+            $(EDKPROTOCOLLIB)\
+            $(ArchProtocolLib)\
+            $(EDKFRAMEWORKPROTOCOLLIB)\
+            $(EdkIIGlueBaseLib_LIB)\
+            $(EdkIIGlueBaseIoLibIntrinsic_LIB)\
+            $(EdkIIGlueBaseMemoryLib_LIB)\
+            $(EdkIIGlueDxeMemoryAllocationLib_LIB)\
+            $(EdkIIGlueDxeDebugLibReportStatusCode_LIB)\
+            $(EdkIIGlueSmmRuntimeDxeReportStatusCodeLib_LIB)\
+            $(EdkIIGlueBasePciLibPciExpress_LIB)\
+            $(EdkIIGlueUefiLib_LIB)\
+            $(EdkIIGlueSmmFirmwarePerformanceLib_LIB)\
+            $(EdkIIGlueDxeServicesTableLib_LIB)\
+            $(EdkIIGlueUefiRuntimeServicesTableLib_LIB)\
+            $(EdkIIGlueUefiDevicePathLib_LIB)\
+            $(EFIPROTOCOLLIB)\
+            $(INTEL_SA_PROTOCOL_LIB)\
+            $(SaGuidLib_LIB)\
+            $(EdkIIGlueDxeHobLib_LIB)\
+            $(CpuPlatformLib_LIB)\
+            $(PchPlatformDxeLib_LIB)\
+            $(SaPcieSmmLib_LIB)\
+            $(INTEL_PCH_PROTOCOL_LIB)\
+
+SaLateInitSmmBin: $(COMPILERSTUB) $(SaLateInitSmm_LIB_LINKS)
+            $(MAKE) /$(MAKEFLAGS) $(EDKIIGLUE_DEFAULTS)\
+            /f  $(BUILD_DIR)\SaLateInitSmm.mak all \
+            "MY_INCLUDES=$(SaLateInitSmm_INCLUDES)" \
+            "MY_DEFINES=$(SaLateInitSmm_DEFINES)" \
+            GUID=2D1E361C-7B3F-4d15-8B1F-66E551FABDC7\
+            ENTRY_POINT=_ModuleEntryPoint \
+            TYPE=BS_DRIVER\
+            EDKIIModule=SMMDRIVER\
+            DEPEX1=$(SaLateInitSmm_DIR)\SaLateInitSmm.dxs\
+            DEPEX1_TYPE=EFI_SECTION_DXE_DEPEX  \
+            COMPRESS=1
+#*************************************************************************
+#*************************************************************************
+#**                                                                     **
+#**        (C)Copyright 1985-2011, American Megatrends, Inc.            **
+#**                                                                     **
+#**                       All Rights Reserved.                          **
+#**                                                                     **
+#**      5555 Oakbrook Parkway, Suite 200, Norcross, GA 30093           **
+#**                                                                     **
+#**                       Phone: (770)-246-8600                         **
+#**                                                                     **
+#*************************************************************************
+#*************************************************************************
diff --git a/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.sdl b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.sdl
new file mode 100644
index 0000000..285ea16
--- /dev/null
+++ b/ReferenceCode/Chipset/SystemAgent/SaInit/Smm/SaLateInitSmm.sdl
@@ -0,0 +1,26 @@
+TOKEN
+	Name  = "SaLateInitSmm_SUPPORT"
+	Value  = "1"
+	Help  = "Main switch to enable SaLateInitSmm support in Project"
+	TokenType = Boolean
+	TargetEQU = Yes
+	TargetMAK = Yes
+	TargetH = Yes
+	Master = Yes
+End
+
+PATH
+	Name  = "SaLateInitSmm_DIR"
+	Help  = "SaLateInitSmm file source directory"
+End
+
+MODULE
+	Help  = "Includes SaLateInitSmm.mak to Project"
+	File  = "SaLateInitSmm.mak"
+End
+
+ELINK
+	Name  = "$(BUILD_DIR)\SaLateInitSmm.ffs"
+	Parent  = "FV_MAIN"
+	InvokeOrder = AfterParent
+End
\ No newline at end of file