init. 1AQQW051HEADmaster

12 files changed, 2728 insertions, 0 deletions

diff --git a/ReferenceCode/Haswell/CpuS3/Pei/CpuAsm.asm b/ReferenceCode/Haswell/CpuS3/Pei/CpuAsm.asm
new file mode 100644
index 0000000..f396d99
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/CpuAsm.asm
@@ -0,0 +1,147 @@
+;
+; 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               
+;
+;------------------------------------------------------------------------------
+;
+; Copyright (c) 2006 - 2011 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.
+;
+;
+; Module Name:
+;
+;   CpuAsm.asm
+; 
+; Abstract:
+; 
+;   This is the code that supports IA32 CPU architectural protocol
+;
+;------------------------------------------------------------------------------
+
+include MpEqu.inc
+
+.686p
+.model  flat        
+.code
+
+PAUSE32   MACRO
+            DB      0F3h
+            DB      090h
+            ENDM
+
+EnableMce  proc near C public
+
+  mov     eax, cr4
+  or      eax, 40h
+  mov     cr4, eax
+
+  ret
+
+EnableMce  endp
+
+MpMtrrSynchUpEntry  PROC    NEAR C PUBLIC
+    ;
+    ; Enter no fill cache mode, CD=1(Bit30), NW=0 (Bit29)
+    ;
+    mov eax, cr0
+    and eax, 0DFFFFFFFh
+    or  eax, 040000000h
+    mov cr0, eax
+    ;
+    ; Flush cache
+    ;
+    wbinvd
+    ;
+    ; Clear PGE flag Bit 7
+    ;
+    mov eax, cr4
+    mov edx, eax
+    and eax, 0FFFFFF7Fh
+    mov cr4, eax
+    ;
+    ; Flush all TLBs
+    ;
+    mov eax, cr3
+    mov cr3, eax
+    
+    mov eax, edx
+    
+    ret
+    
+MpMtrrSynchUpEntry  ENDP
+    
+MpMtrrSynchUpExit  PROC    NEAR C  PUBLIC
+
+    push    ebp             ; C prolog
+    mov     ebp, esp
+    ;
+    ; Flush all TLBs the second time
+    ;
+    mov eax, cr3
+    mov cr3, eax
+    ;
+    ; Enable Normal Mode caching CD=NW=0, CD(Bit30), NW(Bit29)
+    ;
+    mov eax, cr0
+    and eax, 09FFFFFFFh
+    mov cr0, eax
+    ;
+    ; Set PGE Flag in CR4 if set
+    ;
+    mov eax, dword ptr [ebp + 8]
+    mov cr4, eax
+    
+    pop ebp
+    
+    ret
+
+MpMtrrSynchUpExit  ENDP
+
+;-------------------------------------------------------------------------------
+;  AsmAcquireMPLock (&Lock);
+;-------------------------------------------------------------------------------
+AsmAcquireMPLock   PROC  near C  PUBLIC
+
+        pushad
+        mov         ebp,esp
+
+        mov         al, NotVacantFlag
+        mov         ebx, dword ptr [ebp+24h]
+TryGetLock:
+        lock xchg   al, byte ptr [ebx]
+        cmp         al, VacantFlag
+        jz          LockObtained
+
+        PAUSE32
+        jmp         TryGetLock       
+
+LockObtained:
+        popad
+        ret
+AsmAcquireMPLock   ENDP
+
+;-------------------------------------------------------------------------------
+;  AsmReleaseMPLock (&Lock);                                                    
+;-------------------------------------------------------------------------------------
+AsmReleaseMPLock   PROC  near C  PUBLIC
+
+        pushad
+        mov         ebp,esp
+
+        mov         al, VacantFlag
+        mov         ebx, dword ptr [ebp+24h]
+        lock xchg   al, byte ptr [ebx]
+        
+        popad
+        ret
+AsmReleaseMPLock   ENDP
+
+END
\ No newline at end of file
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.c b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.c
new file mode 100644
index 0000000..ddf8ba1
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.c
@@ -0,0 +1,1035 @@
+/** @file
+  Cpu driver running on S3 boot path
+
+@copyright
+  Copyright (c) 2005 - 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
+
+**/
+
+///
+/// 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 "MpCommon.h"
+#include "BootGuardLibrary.h"
+#include EFI_PPI_DEFINITION (SmmAccess)
+#include EFI_PPI_DEPENDENCY (Stall)
+#include EFI_PPI_DEFINITION (CpuPlatformPolicy)
+
+#include EFI_GUID_DEFINITION (HOB)
+#include EFI_GUID_DEFINITION (StatusCodeDataTypeId)
+#include EFI_GUID_DEFINITION (SmramCpuDataHeader)
+#endif
+
+#define PEI_ACPI_CPU_DATA_HOB_GUID \
+  { 0x7682bbef, 0xb0b6, 0x4939, 0xae, 0x66, 0x1b, 0x3d, 0xf2, 0xf6, 0xaa, 0xf3};
+
+EFI_GUID gPeiAcpiCpuDataGuid = PEI_ACPI_CPU_DATA_HOB_GUID;
+typedef VOID (*S3_AP_PROCEDURE)(
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  UINT64               *MtrrValues
+  );
+
+/**
+  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
+**/
+static
+EFI_STATUS
+CpuS3ResumeAtEndOfPei (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN VOID                      *Ppi
+  );
+
+EFI_GUID                         gEfiPeiEndOfPeiPhasePpiGuid = EFI_PEI_END_OF_PEI_PHASE_PPI_GUID;
+STATIC EFI_PEI_NOTIFY_DESCRIPTOR mCpuS3ResumeNotifyDesc = {
+  (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiPeiEndOfPeiPhasePpiGuid,
+  CpuS3ResumeAtEndOfPei
+};
+
+/**
+  Get APIC ID of processor
+
+  @retval APIC ID of processor
+**/
+UINT32
+GetApicID (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  return (UINT32) (CpuidRegisters.RegEbx >> 24);
+}
+
+/**
+  Send interrupt to CPU
+
+  @param[in] BroadcastMode - interrupt broadcast mode
+  @param[in] ApicID        - APIC ID for sending interrupt
+  @param[in] VectorNumber  - Vector number
+  @param[in] DeliveryMode  - Interrupt delivery mode
+  @param[in] TriggerMode   - Interrupt trigger mode
+  @param[in] Assert        - Interrupt pin polarity
+  @param[in] PeiServices   - Indirect reference to the PEI Services Table
+  @param[in] PeiStall      - EFI_PEI_STALL_PPI to stall for some interval
+
+  @retval EFI_INVALID_PARAMETER - input parameter not correct
+  @retval EFI_NOT_READY         - there was a pending interrupt
+  @retval EFI_SUCCESS           - interrupt sent successfully
+**/
+EFI_STATUS
+SendInterrupt (
+  IN UINT32            BroadcastMode,
+  IN UINT32            ApicID,
+  IN UINT32            VectorNumber,
+  IN UINT32            DeliveryMode,
+  IN UINT32            TriggerMode,
+  IN BOOLEAN           Assert,
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN EFI_PEI_STALL_PPI *PeiStall
+  )
+{
+  UINT64               ApicBaseReg;
+  EFI_PHYSICAL_ADDRESS ApicBase;
+  UINT32               ICRLow;
+  UINT32               ICRHigh;
+
+  ///
+  /// Initialze ICR high dword, since P6 family processor needs
+  /// the destination field to be 0x0F when it is a broadcast
+  ///
+  ICRHigh = 0x0f000000;
+  ICRLow  = VectorNumber | (DeliveryMode << 8);
+
+  if (TriggerMode == TRIGGER_MODE_LEVEL) {
+    ICRLow |= 0x8000;
+  }
+
+  if (Assert) {
+    ICRLow |= 0x4000;
+  }
+
+  switch (BroadcastMode) {
+    case BROADCAST_MODE_SPECIFY_CPU:
+      ICRHigh = ApicID << 24;
+      break;
+
+    case BROADCAST_MODE_ALL_INCLUDING_SELF:
+      ICRLow |= 0x80000;
+      break;
+
+    case BROADCAST_MODE_ALL_EXCLUDING_SELF:
+      ICRLow |= 0xC0000;
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+  }
+
+  ApicBaseReg = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  ApicBase    = ApicBaseReg & 0xffffff000ULL;
+
+  *(volatile UINT32 *) (UINTN) (ApicBase + APIC_REGISTER_ICR_HIGH_OFFSET) = ICRHigh;
+  *(volatile UINT32 *) (UINTN) (ApicBase + APIC_REGISTER_ICR_LOW_OFFSET)  = ICRLow;
+
+  PeiStall->Stall (
+          PeiServices,
+          PeiStall,
+          10 * STALL_ONE_MICRO_SECOND
+          );
+
+  ICRLow = *(volatile UINT32 *) (UINTN) (ApicBase + APIC_REGISTER_ICR_LOW_OFFSET);
+  if (ICRLow & 0x1000) {
+    return EFI_NOT_READY;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Programs XAPIC registers.
+
+  @param[in] BSP             - Is this BSP?
+**/
+VOID
+ProgramXApic (
+  BOOLEAN BSP
+  )
+{
+  UINT64               ApicBaseReg;
+  EFI_PHYSICAL_ADDRESS ApicBase;
+  volatile UINT32      *EntryAddress;
+  UINT32               EntryValue;
+
+  ApicBaseReg = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  ApicBase    = ApicBaseReg & 0xffffff000ULL;
+
+  ///
+  /// Program the Spurious Vectore entry
+  ///
+  EntryAddress  = (UINT32 *) (UINTN) (ApicBase + APIC_REGISTER_SPURIOUS_VECTOR_OFFSET);
+  EntryValue    = *EntryAddress;
+  EntryValue &= 0xFFFFFD0F;
+  EntryValue |= 0x10F;
+  *EntryAddress = EntryValue;
+
+  ///
+  /// Program the LINT1 vector entry as extINT
+  ///
+  EntryAddress  = (UINT32 *) (UINTN) (ApicBase + APIC_REGISTER_LINT0_VECTOR_OFFSET);
+  EntryValue    = *EntryAddress;
+
+  if (BSP) {
+    EntryValue &= 0xFFFE00FF;
+    EntryValue |= 0x700;
+  } else {
+    EntryValue |= 0x10000;
+  }
+
+  *EntryAddress = EntryValue;
+
+  ///
+  /// Program the LINT1 vector entry as NMI
+  ///
+  EntryAddress  = (UINT32 *) (UINTN) (ApicBase + APIC_REGISTER_LINT1_VECTOR_OFFSET);
+  EntryValue    = *EntryAddress;
+  EntryValue &= 0xFFFE00FF;
+  if (BSP) {
+    EntryValue |= 0x400;
+  } else {
+    EntryValue |= 0x10400;
+  }
+
+  *EntryAddress = EntryValue;
+
+}
+
+/**
+  Restore all MSR settings
+
+  @param[in] ScriptTable - contain the MSR settings that will be restored
+**/
+VOID
+InitializeFeatures (
+  IN MP_CPU_S3_SCRIPT_DATA *ScriptTable
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+  UINT32             ApicId;
+  UINT8              SkipMsr;
+  ///
+  /// Restore all the MSRs for processor
+  ///
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  ApicId = (CpuidRegisters.RegEbx >> 24);
+
+  while (ScriptTable->MsrIndex != 0) {
+    if (ApicId == ScriptTable->ApicId) {
+      SkipMsr = 0;
+      if ((ScriptTable->MsrIndex == MSR_PMG_CST_CONFIG) && (AsmReadMsr64 (MSR_PMG_CST_CONFIG) & B_CST_CONTROL_LOCK)) {
+        SkipMsr = 1;
+      }
+      if (SkipMsr == 0) {
+        AsmWriteMsr64 (ScriptTable->MsrIndex, ScriptTable->MsrValue);
+      }
+    }
+    ScriptTable++;
+  }
+}
+
+/**
+  Restore all MSR settings with debug message output
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] ScriptTable - Script table contain all MSR settings that will be restored
+**/
+VOID
+InitializeFeaturesLog (
+  IN EFI_PEI_SERVICES      **PeiServices,
+  IN MP_CPU_S3_SCRIPT_DATA *ScriptTable
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+  UINT32             ApicId;
+  BOOLEAN            SkipMsr;
+
+  ///
+  /// Restore all the MSRs for processor
+  ///
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  ApicId = (CpuidRegisters.RegEbx >> 24);
+
+  while (ScriptTable->MsrIndex != 0) {
+    if (ApicId == ScriptTable->ApicId) {
+      DEBUG ((EFI_D_INFO, "MSR Index - %x, MSR value - %x\n", ScriptTable->MsrIndex, ScriptTable->MsrValue));
+      SkipMsr = FALSE;
+      if ((ScriptTable->MsrIndex == MSR_PMG_CST_CONFIG) && (AsmReadMsr64 (MSR_PMG_CST_CONFIG) & B_CST_CONTROL_LOCK)) {
+        SkipMsr = TRUE;
+      }
+      if (ScriptTable->MsrIndex == MSR_IA32_DEBUG_INTERFACE) {
+        /* Access to MSR_IA32_DEBUG_INTERFACE is supported on
+  HSW B0, HSWULT B0 and CRW B0 Stepping
+  HSW stepping >= C0, HSWULT Stepping >= C0 and CRW stepping >= C0 stepping, if CPUID (EAX=1): ECX[11] = 1
+        */
+        switch (CpuidRegisters.RegEax) {
+          case (EnumCpuHsw + EnumHswA0):
+            DEBUG ((EFI_D_INFO,"MSR_IA32_DEBUG_INTERFACE is not supported on Ax CPU stepping\n"));
+            break;
+          case (EnumCpuHsw    + EnumHswB0):
+          case (EnumCpuHswUlt + EnumHswUltB0):
+          case (EnumCpuCrw    + EnumCrwB0):
+            if ((AsmReadMsr64 (MSR_IA32_DEBUG_INTERFACE) & B_DEBUG_INTERFACE_LOCK)) {
+  SkipMsr = TRUE;
+            }
+            break;
+          default:
+            if (CpuidRegisters.RegEcx & BIT11) {
+              if ((AsmReadMsr64 (MSR_IA32_DEBUG_INTERFACE) & B_DEBUG_INTERFACE_LOCK)) {
+                SkipMsr = TRUE;
+              }
+            }
+            break;
+        }
+      }
+      if (!SkipMsr) {
+        AsmWriteMsr64 (ScriptTable->MsrIndex, ScriptTable->MsrValue);
+      }
+    }
+    ScriptTable++;
+  }
+}
+
+/**
+  Initialize prefetcher settings
+
+  @param[in] MlcStreamerprefecterEnabled - Enable/Disable MLC streamer prefetcher
+  @param[in] MlcSpatialPrefetcherEnabled - Enable/Disable MLC spatial prefetcher
+**/
+VOID
+ProcessorsPrefetcherInitialization (
+  IN UINTN MlcStreamerprefecterEnabled,
+  IN UINTN MlcSpatialPrefetcherEnabled
+  )
+{
+  UINT64 MsrValue;
+  MsrValue = AsmReadMsr64 (MISC_FEATURE_CONTROL);
+
+  if (MlcStreamerprefecterEnabled == CPU_FEATURE_DISABLE) {
+    MsrValue |= B_MISC_FEATURE_CONTROL_MLC_STRP;
+  }
+
+  if (MlcSpatialPrefetcherEnabled == CPU_FEATURE_DISABLE) {
+    MsrValue |= B_MISC_FEATURE_CONTROL_MLC_SPAP;
+  }
+
+  if ((MsrValue & (B_MISC_FEATURE_CONTROL_MLC_STRP | B_MISC_FEATURE_CONTROL_MLC_SPAP)) != 0) {
+    AsmWriteMsr64 (MISC_FEATURE_CONTROL, MsrValue);
+  }
+
+  return;
+}
+
+/**
+  AP initialization
+
+  @param[in] ExchangeInfo  - Pointer to the exchange info buffer for output.
+  @param[in] MtrrValues    - buffer contains MTRR settings
+**/
+VOID
+MPRendezvousProcedure (
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  UINT64               *MtrrValues
+  )
+{
+  UINT32                 FailedRevision;
+  ACPI_CPU_DATA          *AcpiCpuData;
+  EFI_PEI_SERVICES       **PeiServices;
+  EFI_PEI_STALL_PPI      *PeiStall;
+  MP_CPU_S3_DATA_POINTER *CpuS3DataPtr;
+
+  ///
+  /// Switch AP speed to BSP speed
+  ///
+  AsmWriteMsr64 (MSR_IA32_PERF_CTRL, ExchangeInfo->CpuPerfCtrlValue);
+
+  AcpiCpuData  = (ACPI_CPU_DATA *) (ExchangeInfo->AcpiCpuDataAddress);
+  PeiServices   = ExchangeInfo->PeiServices;
+  PeiStall      = ExchangeInfo->PeiStall;
+
+  ProgramXApic (FALSE);
+
+  InitializeMicrocode (
+          ExchangeInfo,
+          (EFI_CPU_MICROCODE_HEADER **) (UINTN) ExchangeInfo->MicrocodePointer,
+          &FailedRevision
+          );
+
+  ProcessorsPrefetcherInitialization (
+          ExchangeInfo->CpuPlatformPolicyPpi->CpuConfig->MlcStreamerPrefetcher,
+          ExchangeInfo->CpuPlatformPolicyPpi->CpuConfig->MlcSpatialPrefetcher
+          );
+
+  ///
+  /// wait till all CPUs done the Microcode Load
+  ///
+  while (ExchangeInfo->McuLoadCount < AcpiCpuData->NumberOfCpus) {
+    CpuPause ();
+  }
+
+  MpMtrrSynchUp (MtrrValues);
+
+  InitializeFeatures (ExchangeInfo->S3BootScriptTable);
+
+  InterlockedIncrement (&(ExchangeInfo->FinishedCount));
+
+  ///
+  /// Sempahore check loop executed in memory
+  ///
+  (*ExchangeInfo->SemaphoreCheck)(&ExchangeInfo->FinishedCount);
+
+  InterlockedIncrement (&(ExchangeInfo->WakeupCount));
+
+  ///
+  /// Restore the MTRR programmed before OS boot
+  ///
+  /// MpMtrrSynchUp (MtrrValues);
+  ///
+  CpuS3DataPtr = (MP_CPU_S3_DATA_POINTER *) (UINTN) AcpiCpuData->CpuPrivateData;
+  SetMtrrRegisters (PeiServices, (EFI_MTRR_VALUES *) CpuS3DataPtr->S3BspMtrrTable);
+
+  while (ExchangeInfo->WakeupCount < AcpiCpuData->NumberOfCpus - 1) {
+    CpuPause ();
+  }
+
+  InterlockedIncrement (&(ExchangeInfo->FinishedCount));
+
+}
+
+/**
+  Wake up all the application processors
+
+  @param[in] PeiServices   - Indirect reference to the PEI Services Table
+  @param[in] PeiStall      - EFI_PEI_STALL_PPI to stall for some interval
+  @param[in] AcpiCpuData   - pointer to ACPI_CPU_DATA structure
+  @param[in] MtrrValues    - pointer to a buffer which stored MTRR settings
+
+  @retval EFI_SUCCESS - APs are successfully waked up
+**/
+EFI_STATUS
+WakeUpAPs (
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN EFI_PEI_STALL_PPI *PeiStall,
+  ACPI_CPU_DATA        *AcpiCpuData,
+  UINT64               *MtrrValues,
+  S3_AP_PROCEDURE      Function
+  )
+{
+  EFI_PHYSICAL_ADDRESS   WakeUpBuffer;
+  MP_CPU_EXCHANGE_INFO   *ExchangeInfo;
+  MP_CPU_S3_DATA_POINTER *CpuS3DataPtr;
+  UINTN                  CpuPpiAddrDelta;
+  PEI_CPU_PLATFORM_POLICY_PPI *CpuPolicyPpi;
+
+  WakeUpBuffer = AcpiCpuData->WakeUpBuffer;
+  CopyMem ((VOID *) (UINTN) WakeUpBuffer, AsmGetAddressMap (), 0x1000 - 0x200);
+
+  ExchangeInfo                        = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeUpBuffer + (0x1000 - 0x200));
+
+  ExchangeInfo->Lock                  = 0;
+  ExchangeInfo->StackStart            = (UINTN) AcpiCpuData->StackAddress;
+  ExchangeInfo->StackSize             = STACK_SIZE_PER_PROC;
+  ExchangeInfo->ApFunction            = (UINT32) Function;
+  ExchangeInfo->BufferStart           = (UINT32) WakeUpBuffer;
+  ExchangeInfo->PmodeOffset           = (UINT32) (AsmGetPmodeOffset ());
+  ExchangeInfo->SemaphoreCheck        = (VOID (*)(UINT32 *))(AsmGetSemaphoreCheckOffset () + (UINT32) WakeUpBuffer);
+  ExchangeInfo->AcpiCpuDataAddress    = (UINT32) AcpiCpuData;
+  ExchangeInfo->MtrrValuesAddress     = (UINT32) MtrrValues;
+  ExchangeInfo->FinishedCount         = (UINT32) 0;
+  ExchangeInfo->SerializeLock         = (UINT32) 0;
+  ExchangeInfo->MicrocodePointer      = (UINT32) (UINTN) AcpiCpuData->MicrocodePointerBuffer;
+  ExchangeInfo->StartState            = (UINT32) 0;
+
+  CpuS3DataPtr                        = (MP_CPU_S3_DATA_POINTER *) (UINTN) AcpiCpuData->CpuPrivateData;
+  ExchangeInfo->S3BootScriptTable     = (MP_CPU_S3_SCRIPT_DATA *) (UINTN) CpuS3DataPtr->S3BootScriptTable;
+  ExchangeInfo->VirtualWireMode       = CpuS3DataPtr->VirtualWireMode;
+  ExchangeInfo->PeiServices           = PeiServices;
+  ExchangeInfo->PeiStall              = PeiStall;
+  ExchangeInfo->CpuPerfCtrlValue      = AsmReadMsr64 (MSR_IA32_PERF_CTRL);
+
+  ExchangeInfo->CpuPlatformPolicyPpi  = NULL;
+  PeiServicesLocatePpi (
+          &gPeiCpuPlatformPolicyPpiGuid,
+          0,
+          NULL,
+          (VOID **) &(ExchangeInfo->CpuPlatformPolicyPpi)
+          );
+
+  //
+  // Calculate delta from cache address to memory address
+  //  
+  CpuPolicyPpi = ExchangeInfo->CpuPlatformPolicyPpi;
+  CpuPpiAddrDelta = CpuPolicyPpi->CpuPlatformPpiPtr - (UINTN)(CpuPolicyPpi);                                               
+  //
+  // Calculate new address in memory for each pointer in PEI_CPU_PLATFORM_POLICY_PPI
+  //  
+  CpuPolicyPpi->CpuConfig                       = (CPU_CONFIG_PPI *)((UINTN)CpuPolicyPpi->CpuConfig - CpuPpiAddrDelta);
+  CpuPolicyPpi->PowerMgmtConfig                 = (POWER_MGMT_CONFIG_PPI *)((UINTN)CpuPolicyPpi->PowerMgmtConfig - CpuPpiAddrDelta);
+  CpuPolicyPpi->SecurityConfig                  = (SECURITY_CONFIG_PPI *)((UINTN)CpuPolicyPpi->SecurityConfig - CpuPpiAddrDelta);
+  CpuPolicyPpi->SecurityConfig->PfatConfig      = (PFAT_CONFIG *)((UINTN)CpuPolicyPpi->SecurityConfig->PfatConfig - CpuPpiAddrDelta);
+  CpuPolicyPpi->SecurityConfig->TxtConfig       = (TXT_CONFIG *)((UINTN)CpuPolicyPpi->SecurityConfig->TxtConfig - CpuPpiAddrDelta);
+  CpuPolicyPpi->SecurityConfig->BootGuardConfig = (BOOT_GUARD_CONFIG *)((UINTN)CpuPolicyPpi->SecurityConfig->BootGuardConfig - CpuPpiAddrDelta);
+  CpuPolicyPpi->OverclockingConfig              = (OVERCLOCKING_CONFIG_PPI *)((UINTN)CpuPolicyPpi->OverclockingConfig - CpuPpiAddrDelta);
+  
+  CopyMem (
+          (VOID *) (UINTN) &ExchangeInfo->GdtrProfile,
+          (VOID *) (UINTN) AcpiCpuData->GdtrProfile,
+          sizeof (PSEUDO_DESCRIPTOR)
+          );
+  CopyMem (
+          (VOID *) (UINTN) &ExchangeInfo->IdtrProfile,
+          (VOID *) (UINTN) AcpiCpuData->IdtrProfile,
+          sizeof (PSEUDO_DESCRIPTOR)
+          );
+
+  DEBUG ((EFI_D_INFO, "Cpu S3 Bootscript at %08X\n", (UINT32) ExchangeInfo->S3BootScriptTable));
+
+#ifdef BOOT_GUARD_SUPPORT_FLAG
+#if BOOT_GUARD_SUPPORT_FLAG == 1
+  //
+  // Disable PBET before send IPI to APs
+  //
+  StopPbeTimer ();
+#endif
+#endif
+
+  ///
+  /// Don't touch MPCPU private data
+  /// Here we use ExchangeInfo instead
+  ///
+  ///
+  /// Send INIT IPI - SIPI to all APs
+  ///
+  SendInterrupt (
+          BROADCAST_MODE_ALL_EXCLUDING_SELF,
+          0,
+          0,
+          DELIVERY_MODE_INIT,
+          TRIGGER_MODE_EDGE,
+          TRUE,
+          PeiServices,
+          PeiStall
+          );
+
+  PeiStall->Stall (
+          PeiServices,
+          PeiStall,
+          10 * STALL_ONE_MILLI_SECOND ///< 10ms
+          );
+
+  SendInterrupt (
+          BROADCAST_MODE_ALL_EXCLUDING_SELF,
+          0,
+          (UINT32) RShiftU64 (WakeUpBuffer,
+                              12),
+          DELIVERY_MODE_SIPI,
+          TRIGGER_MODE_EDGE,
+          TRUE,
+          PeiServices,
+          PeiStall
+          );
+
+  PeiStall->Stall (
+          PeiServices,
+          PeiStall,
+          200 * STALL_ONE_MICRO_SECOND ///< 200us
+          );
+
+  SendInterrupt (
+          BROADCAST_MODE_ALL_EXCLUDING_SELF,
+          0,
+          (UINT32) RShiftU64 (WakeUpBuffer,
+                              12),
+          DELIVERY_MODE_SIPI,
+          TRIGGER_MODE_EDGE,
+          TRUE,
+          PeiServices,
+          PeiStall
+          );
+
+  PeiStall->Stall (
+          PeiServices,
+          PeiStall,
+          200 * STALL_ONE_MICRO_SECOND ///< 200us
+          );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine is used to search SMRAM and get SmramCpuData point.
+
+  @param[in] PeiServices  - PEI services global pointer
+  @param[in] SmmAccessPpi - SmmAccess PPI instance
+
+  @retval SmramCpuData - The pointer of CPU information in SMRAM.
+**/
+STATIC
+SMRAM_CPU_DATA *
+GetSmmCpuData (
+  IN EFI_PEI_SERVICES   **PeiServices,
+  IN PEI_SMM_ACCESS_PPI *SmmAccessPpi
+  )
+{
+  EFI_SMRAM_DESCRIPTOR *SmramRanges;
+  UINTN                SmramRangeCount;
+  UINTN                Size;
+  EFI_STATUS           Status;
+  UINT32               Address;
+  SMRAM_CPU_DATA       *SmramCpuData;
+
+        ///
+        /// Get all SMRAM range
+        ///
+  Size    = 0;
+  Status  = SmmAccessPpi->GetCapabilities (PeiServices, SmmAccessPpi, &Size, NULL);
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+
+  Status = PeiServicesAllocatePool (
+                        Size,
+                        (VOID **) &SmramRanges
+                        );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = SmmAccessPpi->GetCapabilities (PeiServices, SmmAccessPpi, &Size, SmramRanges);
+  ASSERT_EFI_ERROR (Status);
+
+  Size /= sizeof (*SmramRanges);
+  SmramRangeCount = Size;
+
+        ///
+        ///  BUGBUG: We assume TSEG is the last range of SMRAM in SmramRanges
+        ///
+  SmramRanges += SmramRangeCount - 1;
+
+  DEBUG ((EFI_D_INFO, "TsegBase - %x\n", SmramRanges->CpuStart));
+  DEBUG ((EFI_D_INFO, "TsegTop  - %x\n", SmramRanges->CpuStart + SmramRanges->PhysicalSize));
+
+        ///
+        /// Search SMRAM on page alignment for the SMMNVS signature
+        ///
+        for (Address = (UINT32) (SmramRanges->CpuStart + SmramRanges->PhysicalSize - EFI_PAGE_SIZE);
+             Address >= (UINT32) SmramRanges->CpuStart;
+             Address -= EFI_PAGE_SIZE
+             ) {
+                SmramCpuData = (SMRAM_CPU_DATA *) (UINTN) Address;
+                if (CompareGuid (&SmramCpuData->HeaderGuid, &gSmramCpuDataHeaderGuid) 
+                && SmramCpuData->AcpiCpuData.NumberOfCpus == (UINT32)(AsmReadMsr64(0x35) & 0xffff)) { //(AMI_CHG)
+                        ///
+                        /// Find it
+                        ///
+                        return SmramCpuData;
+    }
+  }
+
+  ASSERT (FALSE);
+
+        return NULL;
+}
+
+/**
+  This routine is restore the CPU information from SMRAM to original reserved memory region.
+
+  @param[in] PeiServices  - PEI services global pointer
+
+  @retval AcpiCpuData - The pointer of CPU information in reserved memory.
+**/
+ACPI_CPU_DATA *
+RestoreSmramCpuData (
+  IN EFI_PEI_SERVICES **PeiServices
+  )
+{
+  PEI_SMM_ACCESS_PPI       *SmmAccessPpi;
+  SMRAM_CPU_DATA           *SmramCpuData;
+  EFI_STATUS               Status;
+  ACPI_CPU_DATA            *AcpiCpuData;
+  MP_CPU_S3_DATA_POINTER   *CpuS3DataPtr;
+  PSEUDO_DESCRIPTOR        *Idtr;
+  PSEUDO_DESCRIPTOR        *Gdtr;
+  UINTN                    MicrocodeSize;
+  EFI_CPU_MICROCODE_HEADER **Microcode;
+  EFI_CPU_MICROCODE_HEADER *LockBoxMicrocode;
+  UINTN                    Index;
+
+  Status = PeiServicesLocatePpi (
+                        &gPeiSmmAccessPpiGuid,
+                        0,
+                        NULL,
+                        (VOID **) &SmmAccessPpi
+                        );
+  ASSERT_EFI_ERROR (Status);
+
+        ///
+        /// Open all SMM regions
+        ///
+  Index = 0;
+        do {
+                Status = SmmAccessPpi->Open (PeiServices, SmmAccessPpi, Index);
+                Index++;
+  } while (!EFI_ERROR (Status));
+
+  SmramCpuData = GetSmmCpuData (PeiServices, SmmAccessPpi);
+  DEBUG ((EFI_D_INFO, "CpuS3 SmramCpuData                         - 0x%x \n", SmramCpuData));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->GdtrProfileSize              - %x\n", SmramCpuData->GdtrProfileSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->GdtSize                      - %x\n", SmramCpuData->GdtSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->IdtrProfileSize              - %x\n", SmramCpuData->IdtrProfileSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->IdtSize                      - %x\n", SmramCpuData->IdtSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->CpuPrivateDataSize           - %x\n", SmramCpuData->CpuPrivateDataSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->S3BootScriptTableSize        - %x\n", SmramCpuData->S3BootScriptTableSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->S3BspMtrrTableSize           - %x\n", SmramCpuData->S3BspMtrrTableSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->MicrocodePointerBufferSize   - %x\n", SmramCpuData->MicrocodePointerBufferSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->MicrocodeDataBufferSize      - %x\n", SmramCpuData->MicrocodeDataBufferSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->GdtrProfileOffset            - %x\n", SmramCpuData->GdtrProfileOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->GdtOffset                    - %x\n", SmramCpuData->GdtOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->IdtrProfileOffset            - %x\n", SmramCpuData->IdtrProfileOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->IdtOffset                    - %x\n", SmramCpuData->IdtOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->CpuPrivateDataOffset         - %x\n", SmramCpuData->CpuPrivateDataOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->S3BootScriptTableOffset      - %x\n", SmramCpuData->S3BootScriptTableOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->S3BspMtrrTableOffset         - %x\n", SmramCpuData->S3BspMtrrTableOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->MicrocodePointerBufferOffset - %x\n", SmramCpuData->MicrocodePointerBufferOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData->MicrocodeDataBufferOffset    - %x\n", SmramCpuData->MicrocodeDataBufferOffset));
+
+        ///
+        /// Start restore data to NVS
+        ///
+  AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) SmramCpuData->AcpiCpuPointer;
+  CopyMem (AcpiCpuData, &SmramCpuData->AcpiCpuData, sizeof (ACPI_CPU_DATA));
+
+  CopyMem (
+                (VOID *) (UINTN) AcpiCpuData->GdtrProfile,
+                (UINT8 *) SmramCpuData + SmramCpuData->GdtrProfileOffset,
+                SmramCpuData->GdtrProfileSize
+                );
+  Gdtr = (PSEUDO_DESCRIPTOR *) (UINTN) AcpiCpuData->GdtrProfile;
+  CopyMem (
+                (VOID *) (UINTN) Gdtr->Base,
+                (UINT8 *) SmramCpuData + SmramCpuData->GdtOffset,
+                SmramCpuData->GdtSize
+                );
+  CopyMem (
+                (VOID *) (UINTN) AcpiCpuData->IdtrProfile,
+                (UINT8 *) SmramCpuData + SmramCpuData->IdtrProfileOffset,
+                SmramCpuData->IdtrProfileSize
+                );
+  Idtr = (PSEUDO_DESCRIPTOR *) (UINTN) AcpiCpuData->IdtrProfile;
+  CopyMem (
+                (VOID *) (UINTN) Idtr->Base,
+                (UINT8 *) SmramCpuData + SmramCpuData->IdtOffset,
+                SmramCpuData->IdtSize
+                );
+
+  CopyMem (
+                (VOID *) (UINTN) AcpiCpuData->CpuPrivateData,
+                (UINT8 *) SmramCpuData + SmramCpuData->CpuPrivateDataOffset,
+                SmramCpuData->CpuPrivateDataSize
+                );
+  CpuS3DataPtr = (MP_CPU_S3_DATA_POINTER *) (UINTN) AcpiCpuData->CpuPrivateData;
+  CopyMem (
+                (VOID *) (UINTN) CpuS3DataPtr->S3BootScriptTable,
+                (UINT8 *) SmramCpuData + SmramCpuData->S3BootScriptTableOffset,
+                SmramCpuData->S3BootScriptTableSize
+                );
+  CopyMem (
+                (VOID *) (UINTN) CpuS3DataPtr->S3BspMtrrTable,
+                (UINT8 *) SmramCpuData + SmramCpuData->S3BspMtrrTableOffset,
+                SmramCpuData->S3BspMtrrTableSize
+                );
+
+  CopyMem (
+                (VOID *) (UINTN) AcpiCpuData->MicrocodePointerBuffer,
+                (UINT8 *) SmramCpuData + SmramCpuData->MicrocodePointerBufferOffset,
+                SmramCpuData->MicrocodePointerBufferSize
+                );
+        ///
+        /// Restore Microcode Data
+        ///
+  Microcode         = (VOID *) (UINTN) AcpiCpuData->MicrocodePointerBuffer;
+  LockBoxMicrocode  = (EFI_CPU_MICROCODE_HEADER *) ((UINT8 *) SmramCpuData + SmramCpuData->MicrocodeDataBufferOffset);
+        if (Microcode != NULL) {
+                Index         = 0;
+                MicrocodeSize = 0;
+                while (Microcode[Index] != NULL) {
+                        if (LockBoxMicrocode->DataSize == 0) {
+                                MicrocodeSize = 2048;
+      } else {
+                                MicrocodeSize = LockBoxMicrocode->TotalSize;
+      }
+
+                        CopyMem (Microcode[Index], LockBoxMicrocode, MicrocodeSize);
+                        LockBoxMicrocode = (EFI_CPU_MICROCODE_HEADER *) ((UINT8 *) LockBoxMicrocode + MicrocodeSize);
+                        Index++;
+    }
+  }
+        ///
+        /// Close all SMM regions
+        ///
+  Index = 0;
+        do {
+                Status = SmmAccessPpi->Close (PeiServices, SmmAccessPpi, Index);
+                Index++;
+  } while (!EFI_ERROR (Status));
+
+        return AcpiCpuData;
+}
+
+/**
+  Initialize multiple processors.
+
+  @param[in] FfsHeader     - Pointer to an alleged FFS file.
+  @param[in] PeiServices   - Indirect reference to the PEI Services Table
+
+  @retval EFI_SUCCESS - Multiple processors are intialized successfully
+**/
+EFI_STATUS
+InitializeCpu (
+  IN EFI_FFS_FILE_HEADER *FfsHeader,
+  IN EFI_PEI_SERVICES    **PeiServices
+  )
+{
+  EFI_STATUS             Status;
+  EFI_PEI_STALL_PPI      *PeiStall;
+  ACPI_CPU_DATA          *AcpiCpuData;
+  EFI_BOOT_MODE          BootMode;
+  UINT64                 *MtrrValues;
+  MP_CPU_S3_DATA_POINTER *CpuS3DataPtr;
+  UINTN                  VariableMtrrNumber;
+  EFI_PHYSICAL_ADDRESS   WakeUpBuffer;
+  MP_CPU_EXCHANGE_INFO   *ExchangeInfo;
+  UINT32                 FailedRevision;
+  VOID                   *Hob;
+
+  DEBUG ((EFI_D_INFO, "Cpu S3 dispatch\n"));
+
+  Status = PeiServicesGetBootMode (&BootMode);
+        if (EFI_ERROR (Status)) {
+                ///
+                /// If not in S3 boot path. do nothing
+                ///
+                return EFI_SUCCESS;
+  }
+
+        if (BootMode != BOOT_ON_S3_RESUME) {
+                return EFI_SUCCESS;
+  }
+
+  DEBUG ((EFI_D_INFO, "Cpu S3 Entrypoint\n"));
+
+        ///
+        /// Restore ACPI Nvs data from SMRAM
+        ///
+  AcpiCpuData = RestoreSmramCpuData (PeiServices);
+  DEBUG ((EFI_D_INFO, "CpuS3 RestoreSmramCpuData - 0x%x \n", AcpiCpuData));
+
+  AcpiCpuData->S3BootPath = TRUE;
+
+  Status = PeiServicesLocatePpi (
+                        &gEfiPeiStallPpiGuid,
+                        0,
+                        NULL,
+                        (VOID **) &PeiStall
+                        );
+  ASSERT_EFI_ERROR (Status);
+
+  CpuS3DataPtr        = (MP_CPU_S3_DATA_POINTER *) (UINTN) AcpiCpuData->CpuPrivateData;
+
+  VariableMtrrNumber  = (UINTN) ((UINT64) AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT) * 2;
+  Status = PeiServicesAllocatePool (
+                        (FixedMtrrNumber + MtrrDefTypeNumber + VariableMtrrNumber) * sizeof (UINT64),
+                        (VOID **) &MtrrValues
+                        );
+  ASSERT_EFI_ERROR (Status);
+  ReadMtrrRegisters (PeiServices, MtrrValues);
+
+  WakeUpBuffer                = AcpiCpuData->WakeUpBuffer;
+  ExchangeInfo                = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeUpBuffer + (0x1000 - 0x200));
+  ExchangeInfo->WakeupCount   = (UINT32) 0;
+  ExchangeInfo->FinishedCount = (UINT32) 0;
+
+        ///
+        /// Restore AP configuration
+        ///
+  WakeUpAPs (PeiServices, PeiStall, AcpiCpuData, MtrrValues, MPRendezvousProcedure);
+
+        ///
+        /// Program XApic register
+        ///
+  ProgramXApic (
+                TRUE
+                );
+
+  InitializeMicrocode (
+                ExchangeInfo,
+                (EFI_CPU_MICROCODE_HEADER **) (UINTN) AcpiCpuData->MicrocodePointerBuffer,
+                &FailedRevision
+                );
+
+        ///
+        /// Restore features for BSP
+        ///
+  InitializeFeaturesLog (
+                PeiServices,
+                (MP_CPU_S3_SCRIPT_DATA *) CpuS3DataPtr->S3BootScriptTable
+                );
+
+        ///
+        /// Save acpi cpu data into one hob, it will be used by a callback when End of Pei Signal installed.
+        ///
+  Hob = BuildGuidDataHob (
+                        &gPeiAcpiCpuDataGuid,
+                        (VOID *)(UINTN)AcpiCpuData,
+                        (UINTN) sizeof (ACPI_CPU_DATA)
+                        );
+  ASSERT (Hob != NULL);
+  
+  //(AMI_CHG)>
+  //DEBUG ((EFI_D_INFO, "Install CPU S3 Notify callback\n"));
+  //Status = PeiServicesNotifyPpi (&mCpuS3ResumeNotifyDesc);
+  //ASSERT_EFI_ERROR (Status);
+  //(AMI_CHG)<
+
+        ///
+        /// Wait for all APs to complete
+        ///
+        while (ExchangeInfo->FinishedCount < AcpiCpuData->NumberOfCpus - 1) {
+                CpuPause ();
+  }
+
+        return EFI_SUCCESS;
+}
+
+/**
+  This function handles CPU 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
+**/
+STATIC
+EFI_STATUS
+CpuS3ResumeAtEndOfPei (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
+  IN VOID                      *Ppi
+  )
+{
+
+  EFI_STATUS             Status;
+  EFI_PEI_STALL_PPI      *PeiStall;
+  ACPI_CPU_DATA          *AcpiCpuData;
+  EFI_PHYSICAL_ADDRESS   WakeUpBuffer;
+  MP_CPU_S3_DATA_POINTER *CpuS3DataPtr;
+  MP_CPU_EXCHANGE_INFO   *ExchangeInfo;
+  VOID                   *Hob;
+
+  DEBUG ((EFI_D_INFO, "Cpu S3 callback Entry\n"));
+
+        ///
+        /// Find the saved acpi cpu data from HOB.
+        ///
+  AcpiCpuData = NULL;
+  Hob = GetFirstGuidHob (&gPeiAcpiCpuDataGuid);
+        if (Hob != NULL) {
+                AcpiCpuData = (ACPI_CPU_DATA *) ((UINTN) Hob + sizeof (EFI_HOB_GUID_TYPE));
+                ASSERT (AcpiCpuData != NULL);
+                if (AcpiCpuData == NULL) {
+                        return EFI_UNSUPPORTED;
+    }
+  } else {
+                return EFI_UNSUPPORTED;
+  }
+
+  WakeUpBuffer                = AcpiCpuData->WakeUpBuffer;
+  ExchangeInfo                = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeUpBuffer + (0x1000 - 0x200));
+        ///
+        /// Have APs to continue the task - Restore S3BspMtrrTable
+        ///
+  ExchangeInfo->WakeupCount   = (UINT32) 0;
+  ExchangeInfo->FinishedCount = (UINT32) 0;
+
+  PeiStall = NULL;
+  Status = PeiServicesLocatePpi (
+                        &gEfiPeiStallPpiGuid,
+                        0,
+                        NULL,
+                        (VOID **) &PeiStall
+                        );
+  ASSERT_EFI_ERROR (Status);
+
+        ///
+        /// Set MTRR to the final values
+        /// Do not do it too early so as to avoid performance penalty
+        ///
+  CpuS3DataPtr = (MP_CPU_S3_DATA_POINTER *) (UINTN) AcpiCpuData->CpuPrivateData;
+
+#ifdef EFI_DEBUG
+  ShowMtrrRegisters (PeiServices, (EFI_MTRR_VALUES *) CpuS3DataPtr->S3BspMtrrTable);
+#endif
+  SetMtrrRegisters (PeiServices, (EFI_MTRR_VALUES *) CpuS3DataPtr->S3BspMtrrTable);
+
+  PeiStall->Stall (
+                PeiServices,
+                PeiStall,
+                1 * STALL_ONE_MILLI_SECOND ///< 1ms
+                );
+
+        while (ExchangeInfo->FinishedCount < AcpiCpuData->NumberOfCpus - 1) {
+                CpuPause ();
+  }
+
+        return EFI_SUCCESS;
+
+}
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.cif b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.cif
new file mode 100644
index 0000000..2cdadb9
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.cif
@@ -0,0 +1,18 @@
+<component>
+	name = "CpuS3Peim"
+	category = ModulePart
+	LocalRoot = "ReferenceCode\Haswell\CpuS3\Pei"
+	RefName = "CpuS3Peim"
+[files]
+"CpuAsm.asm"
+"CpuS3Peim.c"
+"CpuS3Peim.dxs"
+"CpuS3Peim.inf"
+"CpuS3Peim.mak"
+"CpuS3Peim.sdl"
+"Microcode.c"
+"MpCommon.h"
+"MtrrSync.c"
+"MpFuncs.asm"
+"MpEqu.inc"
+<endComponent>
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.dxs b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.dxs
new file mode 100644
index 0000000..45d6061
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.dxs
@@ -0,0 +1,31 @@
+/** @file
+  Dependency expression source file.
+
+@copyright
+  Copyright (c)  2005 - 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
+
+**/
+  
+
+#include "EfiDepex.h"
+#include EFI_PPI_DEFINITION (BootMode)
+#include EFI_PPI_CONSUMER (MemoryDiscovered)
+#include EFI_PPI_DEFINITION (SmmAccess)
+
+DEPENDENCY_START
+  PEI_MASTER_BOOT_MODE_PEIM_PPI AND
+  PEI_PERMANENT_MEMORY_INSTALLED_PPI_GUID AND
+  PEI_SMM_ACCESS_PPI_GUID
+DEPENDENCY_END
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.inf b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.inf
new file mode 100644
index 0000000..bb2c9c0
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.inf
@@ -0,0 +1,102 @@
+## @file
+#  @todo ADD DESCRIPTION
+#
+#@copyright
+#  Copyright (c)  2005 - 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            = CpuS3Peim
+FILE_GUID            = C866BD71-7C79-4BF1-A93B-066B830D8F9A
+COMPONENT_TYPE       = PE32_PEIM
+
+[sources.common]
+  CpuS3Peim.c
+  MtrrSync.c
+  Microcode.c
+
+  MpFuncs.asm
+  CpuAsm.asm
+
+#
+# Edk II Glue Driver Entry Point
+#
+ EdkIIGluePeimEntryPoint.c
+
+[includes.common]
+  $(EDK_SOURCE)/Foundation/Framework
+  $(EDK_SOURCE)/Foundation
+  $(EDK_SOURCE)/Foundation/Efi
+  $(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/Library/Dxe/Include
+  $(EDK_SOURCE)/Foundation/Include/Pei
+  $(EDK_SOURCE)/Foundation/Library/Pei/Include
+  $(EDK_SOURCE)/Foundation/Cpu/Pentium/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include/Library
+
+#
+# Edk II Glue Library, some hearder are included by R9 header so have to include
+#
+  $(EFI_SOURCE)
+  $(EFI_SOURCE)/Framework
+  $(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/Dxe/Include
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include/Library
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/SampleCode
+
+
+[libraries.common]
+  EdkFrameworkPpiLib
+  EdkIIGlueBaseLib
+  EdkIIGlueBaseMemoryLib
+  EdkIIGluePeiDebugLibReportStatusCode
+  EdkIIGluePeiReportStatusCodeLib
+  EdkIIGluePeiServicesLib
+  EdkPpiLib
+  CpuPpiLib
+  $(PROJECT_SA_FAMILY)SampleCodePpiLib
+  CpuGuidLib
+  CpuIA32Lib
+  EdkIIGlueBaseTimerLibLocalApic
+  EdkIIGluePeiHobLib
+  CpuPlatformLib
+  BootGuardLib
+
+[nmake.common]
+  IMAGE_ENTRY_POINT=_ModuleEntryPoint
+  DPX_SOURCE=CpuS3Peim.dxs
+
+#
+# Module Entry Point
+#
+ C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_MODULE_ENTRY_POINT__=InitializeCpu
+ C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_BASE_LIB__ \
+                      -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__
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.mak b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.mak
new file mode 100644
index 0000000..1fe6836
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.mak
@@ -0,0 +1,76 @@
+# MAK file for the eModule: CpuS3Peim
+
+EDK : CpuS3Peim
+
+BUILD_CpuS3Peim_DIR = $(BUILD_DIR)\$(CpuS3Peim_DIR)
+
+$(BUILD_DIR)\CpuS3Peim.mak : $(CpuS3Peim_DIR)\CpuS3Peim.cif $(BUILD_RULES)
+    $(CIF2MAK) $(CpuS3Peim_DIR)\CpuS3Peim.cif $(CIF2MAK_DEFAULTS)
+
+CpuS3Peim : $(BUILD_DIR)\CpuS3Peim.MAK CpuS3PeimBin 
+
+CpuS3Peim_OBJECTS = \
+    $(BUILD_CpuS3Peim_DIR)\CpuS3Peim.obj \
+    $(BUILD_CpuS3Peim_DIR)\MtrrSync.obj \
+    $(BUILD_CpuS3Peim_DIR)\Microcode.obj \
+    $(BUILD_CpuS3Peim_DIR)\MpFuncs.obj \
+    $(BUILD_CpuS3Peim_DIR)\CpuAsm.obj
+
+CpuS3Peim_MY_INCLUDES= \
+    $(EDK_INCLUDES) \
+    /I$(PROJECT_CPU_ROOT) \
+    /I$(PROJECT_CPU_ROOT)\Include \
+    $(PROJECT_CPU_INCLUDES)\
+    /I$(INTEL_SYSTEM_AGENT_DIR) \
+    /I$(INTEL_SYSTEM_AGENT_DIR)\Include \
+    /I$(INTEL_SYSTEM_AGENT_DIR)\SampleCode \
+
+#-    $(CPU_INCLUDES)\
+
+CpuS3Peim_DEFINES = $(MY_DEFINES)\
+    /D"__EDKII_GLUE_MODULE_ENTRY_POINT__=InitializeCpu"\
+    /D __EDKII_GLUE_BASE_LIB__ \
+    /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_SERVICES_TABLE_POINTER_LIB_MM7__ \
+
+
+CpuS3Peim_LIBS =\
+    $(EDKFRAMEWORKPPILIB)\
+    $(EdkIIGlueBaseLib_LIB)\
+    $(EdkIIGlueBaseMemoryLib_LIB) \
+    $(EdkIIGluePeiDebugLibReportStatusCode_LIB) \
+    $(EdkIIGluePeiReportStatusCodeLib_LIB)\
+    $(EdkIIGluePeiServicesLib_LIB)\
+    $(EDKPPILIB)\
+    $(EdkIIGlueBaseTimerLibLocalApic_LIB)\
+    $(EdkIIGluePeiHobLib_LIB) \
+    $(CPU_PPI_LIB)\
+    $(INTEL_SA_PPI_LIB)\
+    $(CpuGuidLib_LIB)\
+    $(CPUIA32LIB)\
+    $(EdkIIGlueBasePciLibPciExpress_LIB)\
+    $(EdkIIGlueBasePciExpressLib_LIB)\
+    $(PchPlatformPeiLib_LIB)\
+    $(CpuPlatformLib_LIB)\
+    $(BootGuardLib_LIB)\
+    $(IntelSaSampleCodePpiLib_LIB)\
+  
+CpuS3PeimBin : $(CpuS3Peim_LIBS)
+    $(MAKE) /$(MAKEFLAGS) $(EDKIIGLUE_DEFAULTS)\
+    /f $(BUILD_DIR)\CpuS3Peim.mak all\
+    NAME=CpuS3Peim\
+    MAKEFILE=$(BUILD_DIR)\CpuS3Peim.mak \
+    "MY_INCLUDES=$(CpuS3Peim_MY_INCLUDES)" \
+    "MY_DEFINES=$(CpuS3Peim_DEFINES)"\
+    OBJECTS="$(CpuS3Peim_OBJECTS)" \
+    GUID=C866BD71-7C79-4BF1-A93B-066B830D8F9A\
+    ENTRY_POINT=_ModuleEntryPoint \
+    TYPE=PEIM \
+    EDKIIModule=PEIM\
+    DEPEX1=$(CpuS3Peim_DIR)\CpuS3Peim.DXS \
+    DEPEX1_TYPE=EFI_SECTION_PEI_DEPEX  \
+    COMPRESS=0
+#-----------------------------------------------------------------------
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.sdl b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.sdl
new file mode 100644
index 0000000..011eee3
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/CpuS3Peim.sdl
@@ -0,0 +1,37 @@
+TOKEN
+	Name  = "CpuS3Peim_SUPPORT"
+	Value  = "1"
+	Help  = "Main switch to enable Cpu Pei init support in Project"
+	TokenType = Boolean
+	TargetEQU = Yes
+	TargetMAK = Yes
+	TargetH = Yes
+	Master = Yes
+End
+
+PATH
+	Name  = "CpuS3Peim_DIR"
+End
+
+MODULE
+	Help  = "Includes CpuS3Peim.mak to Project"
+	File  = "CpuS3Peim.mak"
+End
+
+#-ELINK
+#-	Name  = "CpuS3Peim_INCLUDES"
+#-	InvokeOrder = ReplaceParent
+#-End
+#-
+#-ELINK
+#-	Name  = "/I$(CpuS3Peim_DIR)"
+#-	Parent  = "CpuS3Peim_INCLUDES"
+#-	InvokeOrder = AfterParent
+#-End
+
+ELINK
+	Name  = "$(BUILD_DIR)\CpuS3Peim.ffs"
+	Parent  = "FV_BB"
+	InvokeOrder = AfterParent
+End
+
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/Microcode.c b/ReferenceCode/Haswell/CpuS3/Pei/Microcode.c
new file mode 100644
index 0000000..4d596d3
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/Microcode.c
@@ -0,0 +1,472 @@
+/** @file
+  CPU microcode update 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 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
+
+**/
+
+///
+/// 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 "MpCommon.h"
+#define SKIP_MICROCODE_CHECKSUM_CHECK 1
+#endif
+///
+/// Array of pointers which each points to 1 microcode update binary (in memory)
+///
+EFI_CPU_MICROCODE_HEADER **mMicrocodePointerBuffer;
+
+///
+/// Function declarations
+///
+EFI_STATUS
+FindLoadMicrocode (
+  IN UINT32                   Cpuid,
+  IN EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  IN OUT UINT32               *Revision
+  );
+
+/**
+  Check number of cores in the package.
+
+  @retval Number of cores in the package.
+**/
+UINT8
+GetCoreNumber (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER Cpuid;
+  AsmCpuidEx (
+          4,
+          0,
+          &Cpuid.RegEax,
+          NULL,
+          NULL,
+          NULL
+          );
+  return (UINT8) ((RShiftU64 (Cpuid.RegEax, 26) & 0x3f) + 1);
+}
+
+/**
+  Check if this is non-core processor - HT AP thread
+
+  @retval TRUE if this is HT AP thread
+  @retval FALSE if this is core thread
+**/
+BOOLEAN
+IsSecondaryThread (
+  VOID
+  )
+{
+  UINT32             ApicID;
+  EFI_CPUID_REGISTER CpuidRegisters;
+  UINT8              CpuCount;
+  UINT8              CoreCount;
+  UINT8              CpuPerCore;
+  UINT32             Mask;
+
+  ApicID = GetApicID ();
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  if ((CpuidRegisters.RegEdx & 0x10000000) == 0) {
+    return FALSE;
+  }
+
+  CpuCount = (UINT8) ((CpuidRegisters.RegEbx >> 16) & 0xff);
+  if (CpuCount == 1) {
+    return FALSE;
+  }
+
+  AsmCpuid (
+          CPUID_SIGNATURE,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  if (CpuidRegisters.RegEax > 3) {
+
+    CoreCount = GetCoreNumber ();
+  } else {
+    CoreCount = 1;
+  }
+  ///
+  /// Assumes there is symmetry across core boundary, i.e. each core within a package has the same number of logical processors
+  ///
+  if (CpuCount == CoreCount) {
+    return FALSE;
+  }
+
+  CpuPerCore = CpuCount / CoreCount;
+
+  ///
+  /// Assume 1 Core  has no more than 8 threads
+  ///
+  if (CpuPerCore == 2) {
+    Mask = 0x1;
+  } else if (CpuPerCore <= 4) {
+    Mask = 0x3;
+  } else {
+    Mask = 0x7;
+  }
+
+  if ((ApicID & Mask) == 0) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+}
+
+/**
+  Returns the processor microcode revision of the processor installed in the system.
+
+  @retval Processor Microcode Revision
+**/
+UINT32
+GetCpuUcodeRevision (
+  VOID
+  )
+{
+  AsmWriteMsr64 (MSR_IA32_BIOS_SIGN_ID, 0);
+  EfiCpuid (CPUID_VERSION_INFO, NULL);
+  return (UINT32) RShiftU64 (AsmReadMsr64 (MSR_IA32_BIOS_SIGN_ID), 32);
+}
+
+/**
+  Wait till all primary threads done the microcode load
+
+  @param[in] ExchangeInfo      - Pointer to the exchange info buffer for output.
+**/
+VOID
+WaitForPrimaryThreadMcuUpdate (
+  IN MP_CPU_EXCHANGE_INFO *ExchangeInfo
+  )
+{
+  UINTN CoreNumber;
+  CoreNumber = (UINTN) ((RShiftU64 (AsmReadMsr64 (MSR_CORE_THREAD_COUNT), 16)) & 0xffff);
+  if (IsSecondaryThread ()) {
+    while (ExchangeInfo->McuLoadCount < CoreNumber) {
+      CpuPause ();
+    }
+  }
+}
+
+/**
+  This function checks the MCU revision to decide if BIOS needs to load
+  microcode.
+
+  @param[in] MicrocodePointer - Microcode in memory
+  @param[in] Revision         - Current CPU microcode revision
+
+  @retval EFI_SUCCESS - BIOS needs to load microcode
+  @retval EFI_ABORTED - Don't need to update microcode
+**/
+EFI_STATUS
+CheckMcuRevision (
+  IN EFI_CPU_MICROCODE_HEADER *MicrocodePointer,
+  IN UINT32                   Revision
+  )
+{
+  EFI_STATUS Status;
+  Status = EFI_ABORTED;
+
+  if (Revision == 0) {
+    Status = EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+/**
+  This will locate a processor microcode and if it finds a newer revision, it will
+  load it to the processor.
+
+  @param[in] ExchangeInfo           - Pointer to the exchange info buffer for output.
+  @param[in] MicrocodePointerBuffer - The Array of pointers which each points to 1 microcode update binary (in memory)
+  @param[in] FailedRevision         - The microcode revision that fails to be loaded
+
+  @retval EFI_SUCCESS           - A new microcode update is loaded
+  @retval Other                 - Due to some reason, no new microcode update is loaded
+**/
+EFI_STATUS
+InitializeMicrocode (
+  IN MP_CPU_EXCHANGE_INFO     *ExchangeInfo,
+  IN EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  OUT UINT32                  *FailedRevision
+  )
+{
+  EFI_STATUS         Status;
+  EFI_CPUID_REGISTER Cpuid;
+  UINT32             UcodeRevision;
+  ACPI_CPU_DATA      *mAcpiCpuData;
+
+  mAcpiCpuData = (ACPI_CPU_DATA *) (ExchangeInfo->AcpiCpuDataAddress);
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &Cpuid.RegEax,
+          &Cpuid.RegEbx,
+          &Cpuid.RegEcx,
+          &Cpuid.RegEdx
+          );
+
+  WaitForPrimaryThreadMcuUpdate (ExchangeInfo);
+  UcodeRevision = GetCpuUcodeRevision ();
+  Status = FindLoadMicrocode (
+                  Cpuid.RegEax,
+                  MicrocodePointerBuffer,
+                  &UcodeRevision
+                  );
+  *FailedRevision = UcodeRevision;
+
+  InterlockedIncrement (&(ExchangeInfo->McuLoadCount));
+  return Status;
+}
+
+/**
+  This will load the microcode to the processors.
+
+  @param[in] MicrocodeEntryPoint - The microcode update pointer
+  @param[in] Revision            - The current (before load this microcode update) microcode revision
+
+  @retval EFI_SUCCESS           - Microcode loaded
+  @retval EFI_LOAD_ERROR        - Microcode not loaded
+**/
+EFI_STATUS
+LoadMicrocode (
+  IN EFI_CPU_MICROCODE_HEADER *MicrocodeEntryPoint,
+  IN UINT32                   *Revision
+  )
+{
+  ///
+  /// Load the Processor Microcode
+  ///
+  AsmWriteMsr64 (
+          MSR_IA32_BIOS_UPDT_TRIG,
+          (UINT64) ((UINTN) MicrocodeEntryPoint + sizeof (EFI_CPU_MICROCODE_HEADER))
+          );
+
+  ///
+  /// Verify that the microcode has been loaded
+  ///
+  if (GetCpuUcodeRevision () == *Revision) {
+    return EFI_LOAD_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Verify the DWORD type checksum
+
+  @param[in] ChecksumAddr  - The start address to be checkumed
+  @param[in] ChecksumLen   - The length of data to be checksumed
+
+  @retval EFI_SUCCESS           - Checksum correct
+  @retval EFI_CRC_ERROR         - Checksum incorrect
+**/
+EFI_STATUS
+Checksum32Verify (
+  IN UINT32 *ChecksumAddr,
+  IN UINT32 ChecksumLen
+  )
+{
+#if SKIP_MICROCODE_CHECKSUM_CHECK
+  return EFI_SUCCESS;
+#else
+  UINT32 Checksum;
+  UINT32 Index;
+
+  Checksum = 0;
+
+  for (Index = 0; Index < ChecksumLen; Index++) {
+    Checksum += ChecksumAddr[Index];
+  }
+
+  return (Checksum == 0) ? EFI_SUCCESS : EFI_CRC_ERROR;
+#endif
+}
+
+/**
+  Find microcode data
+
+  @param[in] Cpuid                   - processor CPUID
+  @param[in] MicrocodePointerBuffer  - the pointer for microcode buffer
+  @param[in] Revision                - revision of microcode
+
+  @retval The pointer of microcode header
+**/
+EFI_CPU_MICROCODE_HEADER *
+FindMicrocode (
+  IN UINTN                    Cpuid,
+  IN EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  IN UINT32                   *Revision
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_CPU_MICROCODE_HEADER                *MicrocodeEntryPoint;
+  UINT8                                   Index;
+  UINT8                                   ExtendedIndex;
+  UINT8                                   MsrPlatform;
+  UINT32                                  ExtendedTableLength;
+  UINT32                                  ExtendedTableCount;
+  BOOLEAN                                 CorrectMicrocode;
+  EFI_CPU_MICROCODE_EXTENDED_TABLE        *ExtendedTable;
+  EFI_CPU_MICROCODE_EXTENDED_TABLE_HEADER *ExtendedTableHeader;
+
+  Status              = EFI_NOT_FOUND;
+  ExtendedTableLength = 0;
+  MicrocodeEntryPoint = NULL;
+  CorrectMicrocode    = FALSE;
+
+  ///
+  /// The index of platform information resides in bits 50:52 of MSR IA32_PLATFORM_ID
+  ///
+  MsrPlatform = (UINT8) (RShiftU64 (AsmReadMsr64 (MSR_IA32_PLATFORM_ID), 50) & 0x07);
+
+  Index       = 0;
+  while (MicrocodePointerBuffer[Index] != NULL) {
+    MicrocodeEntryPoint = MicrocodePointerBuffer[Index];
+    CorrectMicrocode    = FALSE;
+    ///
+    /// Check if the microcode is for the Cpu and the version is newer
+    /// and the update can be processed on the platform
+    ///
+    if ((MicrocodeEntryPoint->HeaderVersion == 0x00000001) &&
+        !EFI_ERROR (CheckMcuRevision (MicrocodeEntryPoint, *Revision))
+        ) {
+      if ((MicrocodeEntryPoint->ProcessorId == Cpuid) && (MicrocodeEntryPoint->ProcessorFlags & (1 << MsrPlatform))) {
+        if (MicrocodeEntryPoint->DataSize == 0) {
+          Status = Checksum32Verify ((UINT32 *) MicrocodeEntryPoint, 2048 / sizeof (UINT32));
+        } else {
+          Status = Checksum32Verify (
+                          (UINT32 *) MicrocodeEntryPoint,
+                          (MicrocodeEntryPoint->DataSize + sizeof (EFI_CPU_MICROCODE_HEADER)) / sizeof (UINT32)
+                          );
+        }
+
+        if (!EFI_ERROR (Status)) {
+          CorrectMicrocode = TRUE;
+        }
+      } else if ((MicrocodeEntryPoint->DataSize != 0)) {
+        ///
+        /// Check the  Extended Signature if the entended signature exist
+        /// Only the data size != 0 the extended signature may exist
+        ///
+        ExtendedTableLength = MicrocodeEntryPoint->TotalSize - (MicrocodeEntryPoint->DataSize + sizeof (EFI_CPU_MICROCODE_HEADER));
+        if (ExtendedTableLength != 0) {
+          ///
+          /// Extended Table exist, check if the CPU in support list
+          ///
+          ExtendedTableHeader = (EFI_CPU_MICROCODE_EXTENDED_TABLE_HEADER *) ((UINT8 *) (MicrocodeEntryPoint) + MicrocodeEntryPoint->DataSize + 48);
+          ///
+          /// Calulate Extended Checksum
+          ///
+          if ((ExtendedTableLength % 4) == 0) {
+            Status = Checksum32Verify ((UINT32 *) ExtendedTableHeader, ExtendedTableLength / sizeof (UINT32));
+            if (!EFI_ERROR (Status)) {
+              ///
+              /// Checksum correct
+              ///
+              ExtendedTableCount  = ExtendedTableHeader->ExtendedSignatureCount;
+              ExtendedTable       = (EFI_CPU_MICROCODE_EXTENDED_TABLE *) (ExtendedTableHeader + 1);
+              for (ExtendedIndex = 0; ExtendedIndex < ExtendedTableCount; ExtendedIndex++) {
+                ///
+                /// Verify Header
+                ///
+                if ((ExtendedTable->ProcessorSignature == Cpuid) && (ExtendedTable->ProcessorFlag & (1 << MsrPlatform))) {
+                  Status = Checksum32Verify (
+                                  (UINT32 *) ExtendedTable,
+                                  sizeof (EFI_CPU_MICROCODE_EXTENDED_TABLE) / sizeof (UINT32)
+                                  );
+                  if (!EFI_ERROR (Status)) {
+                    ///
+                    /// Find one
+                    ///
+                    CorrectMicrocode = TRUE;
+                    break;
+                  }
+                }
+
+  ExtendedTable++;
+              }
+            }
+          }
+        }
+      }
+    }
+
+    if (CorrectMicrocode) {
+      break;
+    }
+
+    Index += 2;
+  }
+
+  if (!CorrectMicrocode) {
+    MicrocodeEntryPoint = NULL;
+  }
+
+  return MicrocodeEntryPoint;
+}
+
+/**
+  This will locate a processor microcode and if it finds a newer revision, it will
+  load it to the processor.
+
+  @param[in] Cpuid                  - Data returned by cpuid instruction
+  @param[in] MicrocodePointerBuffer - The Array of pointers which each points to 1 microcode update binary (in memory)
+  @param[in] Revision               - As input parameter, the current microcode revision;
+                           as output parameter, the microcode revision after microcode update is loaded
+
+  @retval EFI_SUCCESS           - A new microcode update is loaded
+  @retval Other                 - Due to some reason, no new microcode update is loaded
+**/
+EFI_STATUS
+FindLoadMicrocode (
+  IN UINT32                   Cpuid,
+  IN EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  IN OUT UINT32               *Revision
+  )
+{
+  EFI_STATUS               Status;
+  EFI_CPU_MICROCODE_HEADER *MicrocodeEntryPoint;
+
+  Status = EFI_NOT_FOUND;
+
+  MicrocodeEntryPoint = FindMicrocode (
+                  Cpuid,
+                  MicrocodePointerBuffer,
+                  Revision
+                  );
+
+  if (MicrocodeEntryPoint != NULL) {
+    Status    = LoadMicrocode (MicrocodeEntryPoint, Revision);
+    *Revision = MicrocodeEntryPoint->UpdateRevision;
+  }
+
+  return Status;
+}
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/MpCommon.h b/ReferenceCode/Haswell/CpuS3/Pei/MpCommon.h
new file mode 100644
index 0000000..63a28d7
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/MpCommon.h
@@ -0,0 +1,275 @@
+/** @file
+  some definitions for MP and HT driver.
+
+@copyright
+  Copyright (c) 2005 - 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
+**/
+#ifndef _MP_COMMON_
+#define _MP_COMMON_
+
+#include "Tiano.h"
+#include "EfiCombinationLib.h"
+#include "CpuAccess.h"
+#include "CpuPlatformLib.h"
+#include "EdkIIGlueBaseLib.h"
+
+///
+/// Protocol produced by this driver
+///
+/// #include EFI_PROTOCOL_PRODUCER (MpService)
+///
+/// Protocol consumed by this driver
+///
+#include EFI_PPI_DEPENDENCY (Stall)
+#include EFI_GUID_DEFINITION (SmramCpuDataHeader)
+#include EFI_PPI_DEFINITION (CpuPlatformPolicy)
+
+#define VacantFlag          0x00
+#define NotVacantFlag       0xff
+
+#define MICROSECOND         10
+
+#define MAXIMUM_CPU_NUMBER  0x40
+#define STACK_SIZE_PER_PROC 0x8000
+
+#define MAXIMUM_CPU_S3_TABLE_SIZE     0x1000
+
+#define IO_APIC_INDEX_REGISTER        0xFEC00000
+#define IO_APIC_DATA_REGISTER         0xFEC00010
+extern UINTN FixedMtrrNumber;
+extern UINTN MtrrDefTypeNumber;
+extern UINTN VariableMtrrNumber;
+
+typedef struct {
+  UINT16 Index;
+  UINT64 Value;
+} EFI_MTRR_VALUES;
+
+typedef struct {
+  UINT32 ApicId;
+  UINT32 MsrIndex;
+  UINT64 MsrValue;
+} MP_CPU_S3_SCRIPT_DATA;
+
+typedef struct {
+  UINT32 S3BootScriptTable;
+  UINT32 S3BspMtrrTable;
+  UINT32 VirtualWireMode;
+} MP_CPU_S3_DATA_POINTER;
+
+typedef struct {
+  UINT32                Lock;
+  UINT32                StackStart;
+  UINT32                StackSize;
+  UINT32                ApFunction;
+  PSEUDO_DESCRIPTOR     GdtrProfile;
+  PSEUDO_DESCRIPTOR     IdtrProfile;
+  UINT32                BufferStart;
+  UINT32                PmodeOffset;
+  UINT32                AcpiCpuDataAddress;
+  UINT32                MtrrValuesAddress;
+  UINT32                FinishedCount;
+  UINT32                WakeupCount;
+  UINT32                SerializeLock;
+  UINT32                MicrocodePointer;
+  MP_CPU_S3_SCRIPT_DATA *S3BootScriptTable;
+  UINT32                StartState;
+  UINT32                VirtualWireMode;
+  VOID (*SemaphoreCheck)(
+          UINT32 *SemaphoreAddress
+          );
+  UINT32                      McuLoadCount;
+  EFI_PEI_SERVICES            **PeiServices;
+  PEI_STALL_PPI               *PeiStall;
+  UINT64                      CpuPerfCtrlValue;
+  PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi;
+} MP_CPU_EXCHANGE_INFO;
+
+/**
+  Get protected mode code offset
+
+  @retval Offset of protected mode code
+**/
+VOID *
+AsmGetPmodeOffset (
+  VOID
+  );
+
+/**
+  Get code offset of SemaphoreCheck
+
+  @retval Offset of SemaphoreCheck
+**/
+UINT32
+AsmGetSemaphoreCheckOffset (
+  VOID
+  );
+
+/**
+  Read MTRR settings
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] MtrrValues  - buffer to store MTRR settings
+**/
+VOID
+ReadMtrrRegisters (
+  IN EFI_PEI_SERVICES **PeiServices,
+  UINT64              *MtrrValues
+  );
+
+/**
+  Syncup MTRR settings between all processors
+
+  @param[in] MtrrValues  - buffer to store MTRR settings
+**/
+VOID
+MpMtrrSynchUp (
+  UINT64 *MtrrValues
+  );
+
+/**
+  Set MTRR registers
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] MtrrArray   - buffer with MTRR settings
+**/
+VOID
+SetMtrrRegisters (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN EFI_MTRR_VALUES  *MtrrArray
+  );
+
+#ifdef EFI_DEBUG
+/**
+  Print MTRR settings in debug build BIOS
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] MtrrArray   - buffer with MTRR settings
+**/
+VOID
+ShowMtrrRegisters (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN EFI_MTRR_VALUES  *MtrrArray
+  );
+#endif
+
+/**
+  This will locate a processor microcode and if it finds a newer revision, it will
+  load it to the processor.
+
+  @param[in] ExchangeInfo           - Pointer to the exchange info buffer for output.
+  @param[in] MicrocodePointerBuffer - The Array of pointers which each points to 1 microcode update binary (in memory)
+  @param[in] FailedRevision         - The microcode revision that fails to be loaded
+
+  @retval EFI_SUCCESS           - A new microcode update is loaded
+  @retval Other                 - Due to some reason, no new microcode update is loaded
+**/
+EFI_STATUS
+InitializeMicrocode (
+  IN MP_CPU_EXCHANGE_INFO     *ExchangeInfo,
+  IN EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  OUT UINT32                  *FailedRevision
+  );
+
+///
+/// Functions shared in MP/HT drivers
+///
+/**
+  Send interrupt to CPU
+
+  @param[in] BroadcastMode - interrupt broadcast mode
+  @param[in] ApicID        - APIC ID for sending interrupt
+  @param[in] VectorNumber  - Vector number
+  @param[in] DeliveryMode  - Interrupt delivery mode
+  @param[in] TriggerMode   - Interrupt trigger mode
+  @param[in] Assert        - Interrupt pin polarity
+  @param[in] PeiServices   - Indirect reference to the PEI Services Table
+  @param[in] PeiStall      - EFI_PEI_STALL_PPI to stall for some interval
+
+  @retval EFI_INVALID_PARAMETER - input parameter not correct
+  @retval EFI_NOT_READY         - there was a pending interrupt
+  @retval EFI_SUCCESS           - interrupt sent successfully
+**/
+EFI_STATUS
+SendInterrupt (
+  IN UINT32           BroadcastMode,
+  IN UINT32           ApicID,
+  IN UINT32           VectorNumber,
+  IN UINT32           DeliveryMode,
+  IN UINT32           TriggerMode,
+  IN BOOLEAN          Assert,
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_STALL_PPI    *PeiStall
+  );
+
+/**
+  Get APIC ID of processor
+
+  @retval APIC ID of processor
+**/
+UINT32
+GetApicID (
+  VOID
+  );
+
+/**
+  Programs Local APIC registers for virtual wire mode.
+
+  @param[in] BSP             - Is this BSP?
+**/
+VOID
+ProgramXApic (
+  BOOLEAN BSP
+  );
+
+/**
+  Lock APs
+
+  @param[in] Lock  - Lock state
+**/
+VOID
+AsmAcquireMPLock (
+  IN UINT8 *Lock
+  );
+
+/**
+  Release APs
+
+  @param[in] Lock  - Lock state
+**/
+VOID
+AsmReleaseMPLock (
+  IN UINT8 *Lock
+  );
+
+/**
+  Get address map of RendezvousFunnelProc.
+
+  @retval AddressMap  - Output buffer for address map information
+**/
+VOID *
+AsmGetAddressMap (
+  VOID
+  );
+
+/**
+  Do CLI and HALT processor
+**/
+VOID
+AsmCliHltLoop (
+  VOID
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/MpEqu.inc b/ReferenceCode/Haswell/CpuS3/Pei/MpEqu.inc
new file mode 100644
index 0000000..da4b873
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/MpEqu.inc
@@ -0,0 +1,51 @@
+;@file
+;  This is the equates file for HT (Hyper-threading) support
+;
+;@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
+;
+
+VacantFlag             Equ   00h
+NotVacantFlag          Equ   0ffh
+
+LockLocation                  equ        1000h - 0200h
+StackStartAddressLocation     equ        LockLocation + 04h
+StackSizeLocation             equ        LockLocation + 08h
+CProcedureLocation            equ        LockLocation + 0Ch
+GdtrLocation                  equ        LockLocation + 10h
+IdtrLocation                  equ        LockLocation + 16h
+BufferStartLocation           equ        LockLocation + 1Ch
+PmodeOffsetLocation           equ        LockLocation + 20h
+AcpiCpuDataAddressLocation    equ        LockLocation + 24h
+MtrrValuesAddressLocation     equ        LockLocation + 28h
+FinishedCountAddressLocation  equ        LockLocation + 2Ch
+WakeupCountAddressLocation    equ        LockLocation + 30h
+SerializeLockAddressLocation  equ        LockLocation + 34h
+MicrocodeAddressLocation      equ        LockLocation + 38h
+BootScriptAddressLocation     equ        LockLocation + 3Ch
+StartStateLocation            equ        LockLocation + 40h
+VirtualWireMode               equ        LockLocation + 44h
+SemaphoreCheck                equ        LockLocation + 48h
+PeiServices                   equ        LockLocation + 4Ch
+PeiStall                      equ        LockLocation + 50h
+CpuPerfCtrlValue              equ        LockLocation + 54h
+CpuPlatformPolicyPpi          equ        LockLocation + 5Ch
+
+PAUSE32   MACRO
+            DB      0F3h
+            DB      090h
+            ENDM
+
+;-------------------------------------------------------------------------------
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/MpFuncs.asm b/ReferenceCode/Haswell/CpuS3/Pei/MpFuncs.asm
new file mode 100644
index 0000000..80064c2
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/MpFuncs.asm
@@ -0,0 +1,196 @@
+;
+; 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               
+;
+;-------------------------------------------------------------------------------
+;
+; Copyright (c)  1999 - 2011 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.
+;
+;
+; Module Name:
+;
+;   MpFuncs32.asm
+; 
+; Abstract:
+; 
+;   This is the assembly code for MP support
+;
+;-------------------------------------------------------------------------------
+
+include  MpEqu.inc 
+
+;-------------------------------------------------------------------------------------
+;RendezvousFunnelProc  procedure follows. All APs execute their procedure. This
+;procedure serializes all the AP processors through an Init sequence. It must be
+;noted that APs arrive here very raw...ie: real mode, no stack.
+;ALSO THIS PROCEDURE IS EXECUTED BY APs ONLY ON 16 BIT MODE. HENCE THIS PROC
+;IS IN MACHINE CODE.
+;-------------------------------------------------------------------------------------
+;RendezvousFunnelProc (&WakeUpBuffer,MemAddress);
+
+.686p
+.model  flat        
+.code        
+
+
+RendezvousFunnelProc   PROC  PUBLIC
+RendezvousFunnelProcStart::
+
+;Step-1: Grab a lock. At this point CS = 0x(vv00) and ip= 0x0.
+
+        db 8ch,0c8h                                ; mov        ax,cs
+        db 8eh,0d8h                                ; mov        ds,ax
+        db 8eh,0c0h                                ; mov        es,ax
+        db 8eh,0d0h                                ; mov        ss,ax 
+        db 33h,0c0h                                ; xor        ax,ax
+        db 8eh,0e0h                                ; mov        fs,ax
+        db 8eh,0e8h                                ; mov        gs,ax
+
+        db 0BEh                                    ; opcode of mov si, mem16
+        dw BufferStartLocation                     ; mov        si, BufferStartLocation
+        db 66h,  8Bh, 1Ch                          ; mov        ebx,dword ptr [si]
+
+        db 0BFh                                    ; opcode of mov di, mem16
+        dw PmodeOffsetLocation                     ; mov        di, PmodeOffsetLocation
+        db 66h,  8Bh, 05h                          ; mov        eax,dword ptr [di]
+        db 8Bh,  0F8h                              ; mov        di, ax
+        db 83h,  0EFh,06h                          ; sub        di, 06h
+        db 66h,  03h, 0C3h                         ; add        eax, ebx
+        db 66h,  89h, 05h                          ; mov        dword ptr [di],eax
+
+        db 0BEh                                    ; opcode of mov si, mem16
+        dw GdtrLocation                            ; mov        si, GdtrLocation
+        db 66h                                     ; db         66h
+        db 2Eh,  0Fh, 01h, 14h                     ; lgdt       fword ptr cs:[si]
+
+        db 33h,  0C0h                              ; xor        ax,  ax
+        db 8Eh,  0D8h                              ; mov        ds,  ax
+        
+        db 0Fh,  20h, 0C0h                         ; mov        eax, cr0                        ;Get control register 0
+        db 66h,  83h, 0C8h, 03h                    ; or         eax, 000000003h                 ;Set PE bit (bit #0) & MP
+        db 0Fh,  22h, 0C0h                         ; mov        cr0, eax
+
+        db 66h,  67h, 0EAh                         ; far jump
+        dd 0h                                      ; 32-bit offset
+        dw 20h                                     ; 16-bit selector
+
+NemInit::                                          ; protected mode entry point
+
+        db 66h,  0B8h, 18h, 00h                    ; mov        ax, 18h
+        db 66h,  8Eh,  0D8h		                   ; mov        ds, ax
+        db 66h,  8Eh,  0C0h		                   ; mov        es, ax
+        db 66h,  8Eh,  0E0h		                   ; mov        fs, ax
+        db 66h,  8Eh,  0E8h		                   ; mov        gs, ax
+        db 66h,  8Eh,  0D0h		                   ; mov        ss, ax            ; Flat mode setup.
+
+        mov  esi, ebx
+        
+        mov  edi, esi
+        add  edi, StartStateLocation
+        mov  eax, 1
+        mov  dword ptr [edi], eax
+
+        mov  edi, esi
+        add  edi, LockLocation
+        mov  eax, NotVacantFlag
+TestLock::
+        xchg dword ptr [edi], eax
+        cmp  eax, NotVacantFlag
+        jz   TestLock
+
+ProgramStack::
+
+        mov  edi, esi
+        add  edi, StackSizeLocation
+        mov  eax, dword ptr [edi]
+        mov  edi, esi
+        add  edi, StackStartAddressLocation
+        add  eax, dword ptr [edi]
+        mov  esp, eax
+        mov  dword ptr [edi], eax
+
+Releaselock::
+
+        mov  eax, VacantFlag
+        mov  edi, esi
+        add  edi, LockLocation
+        xchg dword ptr [edi], eax
+
+CProcedureInvoke::
+
+        mov  edi, esi
+        add  edi, MtrrValuesAddressLocation
+        mov  eax, dword ptr [edi]
+        push eax
+        
+        mov  eax, esi
+        add  eax, LockLocation
+        push eax
+        
+        mov  edi, esi
+        add  edi, CProcedureLocation
+        mov  eax, dword ptr [edi]
+
+        call eax
+        add  esp, 8
+        
+        cli
+        hlt
+        jmp  $-2
+
+RendezvousFunnelProc   ENDP
+
+SemaphoreStartAddress PROC C, SemaphoreAddress:PTR DWORD
+        mov  eax, SemaphoreAddress
+@@:
+        cmp  dword ptr [eax], 0
+        jz   @F
+
+        PAUSE32               
+        jmp  @B
+             
+@@:        
+        ret
+SemaphoreStartAddress ENDP
+
+RendezvousFunnelProcEnd::
+
+
+;-------------------------------------------------------------------------------------
+;  AsmGetAddressMap (&AddressMap);
+;-------------------------------------------------------------------------------------
+AsmGetAddressMap   PROC  near C  PUBLIC
+
+        mov eax, RendezvousFunnelProcStart
+        ret
+        
+AsmGetAddressMap   ENDP
+
+AsmGetPmodeOffset PROC  near C PUBLIC
+
+        mov eax, NemInit - RendezvousFunnelProcStart
+        ret
+        
+AsmGetPmodeOffset ENDP 
+
+AsmGetSemaphoreCheckOffset PROC  near C PUBLIC
+        mov eax, SemaphoreStartAddress - RendezvousFunnelProcStart
+        ret
+AsmGetSemaphoreCheckOffset ENDP 
+
+AsmCliHltLoop PROC near C PUBLIC
+        cli
+        hlt
+        jmp $-2
+AsmCliHltLoop ENDP
+
+END
diff --git a/ReferenceCode/Haswell/CpuS3/Pei/MtrrSync.c b/ReferenceCode/Haswell/CpuS3/Pei/MtrrSync.c
new file mode 100644
index 0000000..96e47ee
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuS3/Pei/MtrrSync.c
@@ -0,0 +1,288 @@
+/** @file
+  Synchronization of MTRRs on S3 boot path.
+
+@copyright
+  Copyright (c) 2005 - 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
+**/
+
+///
+/// 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 "CpuAccess.h"
+#include "MpCommon.h"
+#endif
+
+UINTN
+MpMtrrSynchUpEntry (
+  VOID
+  );
+
+VOID
+MpMtrrSynchUpExit (
+  UINTN Cr4
+  );
+
+UINT16 mFixedMtrrIndex[] = {
+  IA32_MTRR_FIX64K_00000,
+  IA32_MTRR_FIX16K_80000,
+  IA32_MTRR_FIX16K_A0000,
+  IA32_MTRR_FIX4K_C0000,
+  IA32_MTRR_FIX4K_C8000,
+  IA32_MTRR_FIX4K_D0000,
+  IA32_MTRR_FIX4K_D8000,
+  IA32_MTRR_FIX4K_E0000,
+  IA32_MTRR_FIX4K_E8000,
+  IA32_MTRR_FIX4K_F0000,
+  IA32_MTRR_FIX4K_F8000,
+};
+
+UINT16 mMtrrDefType[] = { CACHE_IA32_MTRR_DEF_TYPE };
+
+UINT16 mVariableMtrrIndex[] = {
+  CACHE_VARIABLE_MTRR_BASE,
+  CACHE_VARIABLE_MTRR_BASE + 1,
+  CACHE_VARIABLE_MTRR_BASE + 2,
+  CACHE_VARIABLE_MTRR_BASE + 3,
+  CACHE_VARIABLE_MTRR_BASE + 4,
+  CACHE_VARIABLE_MTRR_BASE + 5,
+  CACHE_VARIABLE_MTRR_BASE + 6,
+  CACHE_VARIABLE_MTRR_BASE + 7,
+  CACHE_VARIABLE_MTRR_BASE + 8,
+  CACHE_VARIABLE_MTRR_BASE + 9,
+  CACHE_VARIABLE_MTRR_BASE + 10,
+  CACHE_VARIABLE_MTRR_BASE + 11,
+  CACHE_VARIABLE_MTRR_BASE + 12,
+  CACHE_VARIABLE_MTRR_BASE + 13,
+  CACHE_VARIABLE_MTRR_BASE + 14,
+  CACHE_VARIABLE_MTRR_BASE + 15,
+  CACHE_VARIABLE_MTRR_BASE + 16,
+  CACHE_VARIABLE_MTRR_BASE + 17,
+  CACHE_VARIABLE_MTRR_BASE + 18,
+  CACHE_VARIABLE_MTRR_BASE + 19,
+  ///
+  /// CACHE_VARIABLE_MTRR_END,
+  ///
+};
+
+UINTN FixedMtrrNumber   = sizeof (mFixedMtrrIndex) / sizeof (UINT16);
+UINTN MtrrDefTypeNumber = sizeof (mMtrrDefType) / sizeof (UINT16);
+
+/**
+  Save the MTRR registers to global variables
+
+  @param[in] PeiServices   - Indirect reference to the PEI Services Table
+  @param[in] MtrrValues    - pointer to the buffer which stores MTRR settings
+**/
+VOID
+ReadMtrrRegisters (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN UINT64           *MtrrValues
+  )
+{
+  UINT32 Index;
+  UINT32 VariableMtrrNumber;
+  ///
+  /// Read all Mtrrs
+  ///
+  for (Index = 0; Index < FixedMtrrNumber; Index++) {
+    *MtrrValues = AsmReadMsr64 (mFixedMtrrIndex[Index]);
+    MtrrValues++;
+  }
+
+  for (Index = 0; Index < MtrrDefTypeNumber; Index++) {
+    *MtrrValues = AsmReadMsr64 (mMtrrDefType[Index]);
+    MtrrValues++;
+  }
+
+  VariableMtrrNumber = ((UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT));
+  if (VariableMtrrNumber > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrNumber = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+
+  DEBUG ((EFI_D_INFO, "MTRR: VariableMtrrNumber = %x\n", VariableMtrrNumber));
+  for (Index = 0; Index < VariableMtrrNumber * 2; Index++) {
+    *MtrrValues = AsmReadMsr64 (mVariableMtrrIndex[Index]);
+    MtrrValues++;
+    DEBUG (
+            (EFI_D_INFO,
+             "MTRR: Index = %x   AsmReadMsr64(%x) = %x\n",
+             Index,
+             mVariableMtrrIndex[Index],
+             AsmReadMsr64 (mVariableMtrrIndex[Index]))
+            );
+  }
+
+  return;
+}
+
+/**
+  Synch up the MTRR values for all processors
+
+  @param[in] MtrrValues    - pointer to the buffer which stores MTRR settings
+**/
+VOID
+MpMtrrSynchUp (
+  UINT64 *MtrrValues
+  )
+{
+  UINT32             Index;
+  UINT32             VariableMtrrNumber;
+  UINTN              Cr4;
+  UINT64             *FixedMtrr;
+  UINT64             *MtrrDefType;
+  UINT64             *VariableMtrr;
+  UINT64             ValidMtrrAddressMask;
+  EFI_CPUID_REGISTER FeatureInfo;
+  EFI_CPUID_REGISTER FunctionInfo;
+  UINT8              PhysicalAddressBits;
+
+  ///
+  /// Get physical CPU MTRR width in case of difference from BSP
+  ///
+  AsmCpuid (
+          CPUID_EXTENDED_FUNCTION,
+          &FunctionInfo.RegEax,
+          &FunctionInfo.RegEbx,
+          &FunctionInfo.RegEcx,
+          &FunctionInfo.RegEdx
+          );
+  PhysicalAddressBits = 36;
+  if (FunctionInfo.RegEax >= CPUID_VIR_PHY_ADDRESS_SIZE) {
+    AsmCpuid (
+            CPUID_VIR_PHY_ADDRESS_SIZE,
+            &FeatureInfo.RegEax,
+            &FeatureInfo.RegEbx,
+            &FeatureInfo.RegEcx,
+            &FeatureInfo.RegEdx
+            );
+    PhysicalAddressBits = (UINT8) FeatureInfo.RegEax;
+  }
+
+  ValidMtrrAddressMask  = (LShiftU64 (1, PhysicalAddressBits) - 1) & 0xfffffffffffff000ULL;
+
+  FixedMtrr             = MtrrValues;
+  MtrrDefType           = MtrrValues + FixedMtrrNumber;
+  VariableMtrr          = MtrrValues + FixedMtrrNumber + MtrrDefTypeNumber;
+
+  ///
+  /// ASM code to setup processor register before synching up the MTRRs
+  ///
+  Cr4 = MpMtrrSynchUpEntry ();
+
+  ///
+  /// Disable Fixed Mtrrs
+  ///
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, MtrrDefType[0] & 0xFFFFF7FF);
+
+  ///
+  /// Update Fixed Mtrrs
+  ///
+  for (Index = 0; Index < FixedMtrrNumber; Index++) {
+    AsmWriteMsr64 (mFixedMtrrIndex[Index], FixedMtrr[Index]);
+  }
+  ///
+  /// Synchup Base Variable Mtrr
+  ///
+  VariableMtrrNumber = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  if (VariableMtrrNumber > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrNumber = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+
+  for (Index = 0; Index < VariableMtrrNumber * 2; Index++) {
+    AsmWriteMsr64 (
+            mVariableMtrrIndex[Index],
+            (VariableMtrr[Index] & 0x0FFF) | (VariableMtrr[Index] & ValidMtrrAddressMask)
+            );
+  }
+  ///
+  /// Synchup def type Fixed Mtrrs
+  ///
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, MtrrDefType[0]);
+
+  ///
+  /// ASM code to setup processor register after synching up the MTRRs
+  ///
+  MpMtrrSynchUpExit (Cr4);
+
+  return;
+}
+
+/**
+  Set MTRR registers
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] MtrrArray   - buffer with MTRR settings
+**/
+VOID
+SetMtrrRegisters (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN EFI_MTRR_VALUES  *MtrrArray
+  )
+{
+  UINT32 Index;
+  UINTN  Cr4;
+
+  ///
+  /// ASM code to setup processor register before synching up the MTRRs
+  ///
+  Cr4   = MpMtrrSynchUpEntry ();
+
+  Index = 0;
+  while ((MtrrArray[Index].Index != 0) && (MtrrArray[Index].Index >= CACHE_VARIABLE_MTRR_BASE)) {
+    AsmWriteMsr64 (MtrrArray[Index].Index, MtrrArray[Index].Value);
+    Index++;
+  }
+  ///
+  /// ASM code to setup processor register after synching up the MTRRs
+  ///
+  MpMtrrSynchUpExit (Cr4);
+}
+
+#ifdef EFI_DEBUG
+/**
+  Print MTRR settings in debug build BIOS
+
+  @param[in] PeiServices - General purpose services available to every PEIM.
+  @param[in] MtrrArray   - buffer with MTRR settings
+**/
+VOID
+ShowMtrrRegisters (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN EFI_MTRR_VALUES  *MtrrArray
+  )
+{
+  UINT32 Index;
+
+  Index = 0;
+  while ((MtrrArray[Index].Index != 0) && (MtrrArray[Index].Index >= CACHE_VARIABLE_MTRR_BASE)) {
+
+    DEBUG ((EFI_D_INFO, "MTRR: MtrrArray Index = %x\n", Index));
+    DEBUG (
+            (EFI_D_INFO,
+             "MTRR: MtrrArray[%x].Index = %x    MtrrArray[%x].Value = %x\n",
+             Index,
+             MtrrArray[Index].Index,
+             Index,
+             MtrrArray[Index].Value)
+            );
+    Index++;
+  }
+
+  DEBUG ((EFI_D_INFO, "MTRR: Total Index = %x\n", Index));
+}
+#endif