init. 1AQQW051HEADmaster

54 files changed, 22201 insertions, 0 deletions

diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CacheData.c b/ReferenceCode/Haswell/CpuInit/Dxe/CacheData.c
new file mode 100644
index 0000000..f68c151
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CacheData.c
@@ -0,0 +1,515 @@
+/** @file
+  Processor Cache data records.
+
+@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 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 "EdkIIGlueDxe.h"
+#include "CpuAccess.h"
+#include "CacheData.h"
+#endif
+///
+/// This is the VFR compiler generated header file which defines the
+/// string identifiers.
+///
+#include "CpuInitDxeStrDefs.h"
+
+extern EFI_DATA_HUB_PROTOCOL     *mDataHub;
+extern EFI_SUBCLASS_TYPE1_HEADER mCpuDataRecordHeader;
+
+EFI_CACHE_CONVERTER mCacheConverter[] = {
+  {
+    1,
+    0x09,
+    32,
+    EfiCacheAssociativity4Way,
+    EfiCacheTypeInstruction
+  },
+  {
+    1,
+    0x0D,
+    16,
+    EfiCacheAssociativity4Way,
+    EfiCacheTypeData
+  },
+  {
+    2,
+    0x21,
+    256,
+    EfiCacheAssociativity8Way,
+    EfiCacheTypeUnified
+  },
+  {
+    1,
+    0x2C,
+    32,
+    EfiCacheAssociativity8Way,
+    EfiCacheTypeData
+  },
+  {
+    2,
+    0x40,
+    0,
+    EfiCacheAssociativityUnknown,
+    EfiCacheTypeUnknown
+  },
+  {
+    3,
+    0xD0,
+    512,
+    EfiCacheAssociativity4Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD1,
+    1024,
+    EfiCacheAssociativity4Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD2,
+    2048,
+    EfiCacheAssociativity4Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD3,
+    4096,
+    EfiCacheAssociativity4Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD4,
+    8192,
+    EfiCacheAssociativity4Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD6,
+    1024,
+    EfiCacheAssociativity8Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD7,
+    2048,
+    EfiCacheAssociativity8Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD8,
+    4096,
+    EfiCacheAssociativity8Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xD9,
+    8192,
+    EfiCacheAssociativity8Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xDA,
+    12288,
+    EfiCacheAssociativity8Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xDC,
+    1536,
+    EfiCacheAssociativity12Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xDD,
+    3072,
+    EfiCacheAssociativity12Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xDE,
+    6144,
+    EfiCacheAssociativity12Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xDF,
+    12288,
+    EfiCacheAssociativity12Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xE0,
+    18432,
+    EfiCacheAssociativity12Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xE2,
+    2048,
+    EfiCacheAssociativity16Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xE3,
+    4096,
+    EfiCacheAssociativity16Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xE4,
+    8192,
+    EfiCacheAssociativity16Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xE5,
+    16384,
+    EfiCacheAssociativity16Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xE6,
+    24576,
+    EfiCacheAssociativity16Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xEA,
+    12288,
+    EfiCacheAssociativity24Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xEB,
+    18432,
+    EfiCacheAssociativity24Way,
+    EfiCacheTypeUnified
+  },
+  {
+    3,
+    0xEC,
+    24567,
+    EfiCacheAssociativity24Way,
+    EfiCacheTypeUnified
+  },
+  {
+    0,
+    0xFF,
+    0,
+    0,
+    0
+  }
+};
+
+///
+/// Convert Cache Type Field to SMBIOS format
+///
+#define SMBIOS_CACHE_TYPE_MAX 5
+UINT8 SmbiosCacheTypeFieldConverter[SMBIOS_CACHE_TYPE_MAX] = {
+  EfiCacheTypeUnknown,
+  EfiCacheTypeData,
+  EfiCacheTypeInstruction,
+  EfiCacheTypeUnified,
+  EfiCacheTypeOther
+};
+
+UINT8 mCacheInstance[EFI_CACHE_LMAX] = { 0, 0, 0, 0 };
+
+EFI_SUBCLASS_TYPE1_HEADER mCacheDataRecordHeader = {
+  EFI_CACHE_SUBCLASS_VERSION,         ///< Version
+  sizeof (EFI_SUBCLASS_TYPE1_HEADER), ///< Header Size
+  0,                                  ///< Instance, Initialize later
+  0,                                  ///< SubInstance, Initialize later to Cache Level
+  0                                   ///< RecordType, Initialize later
+};
+
+/**
+  This function gets called with the processor number and will log all cache data to data hub
+  pertaining to this processor.
+
+  @param[in] CpuNumber - Processor Number
+  @param[in] CacheInformation - Cache information get from cpuid instruction
+
+  @retval EFI_OUT_OF_RESOURCES - Failed to allocate required POOL for record buffer.
+  @retval EFI_SUCCESS - successful to update cache data
+**/
+EFI_STATUS
+InitializeCacheData (
+  IN UINTN              CpuNumber,
+  IN EFI_CPUID_REGISTER *CacheInformation
+  )
+{
+  EFI_STATUS                   Status;
+  UINT32                       HeaderSize;
+  UINT32                       TotalSize;
+  CPU_CACHE_DATA_RECORD_BUFFER RecordBuffer;
+  UINT8                        Index1;
+  UINT8                        CacheLevel;
+  UINT8                        LxCacheType;
+  UINT32                       Ways;
+  UINT32                       Partitions;
+  UINT32                       LineSets;
+  UINT32                       Sets;
+  UINT32                       LxCacheSize;
+  EFI_CPUID_REGISTER           CpuidRegisters;
+
+  ///
+  /// Only log CPU socket level information.
+  ///
+  if (CpuNumber != 0) {
+    return EFI_SUCCESS;
+  }
+
+  mCacheDataRecordHeader.Instance = (UINT16) (CpuNumber + 1);
+
+  HeaderSize                      = sizeof (EFI_SUBCLASS_TYPE1_HEADER);
+  RecordBuffer.Raw                = AllocatePool (HeaderSize + CPU_CACHE_DATA_MAXIMUM_LENGTH);
+  if (RecordBuffer.Raw == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  for (Index1 = 0;; Index1++) {
+    EfiCpuidExt (CPUID_CACHE_PARAMS, Index1, &CpuidRegisters);
+    if ((CpuidRegisters.RegEax & V_CPUID_CACHE_TYPE_MASK) == 0) {
+      break;
+    }
+
+    Ways        = ((CpuidRegisters.RegEbx >> B_CPUID_CACHE_PARAMS_WAYS_SHIFT) & 0x3FF) + 1;
+    Partitions  = ((CpuidRegisters.RegEbx >> B_CPUID_CACHE_PARAMS_PARTITIONS_SHIFT) & 0x3FF) + 1;
+    LineSets    = (CpuidRegisters.RegEbx & 0xFFF) + 1;
+    Sets        = CpuidRegisters.RegEcx + 1;
+
+    CacheLevel  = (UINT8) (CpuidRegisters.RegEax & V_CPUID_CACHE_LEVEL_MASK) >> B_CPUID_CACHE_LEVEL_SHIFT;
+    LxCacheSize = (Ways * Partitions * LineSets * Sets) / 1024;
+    LxCacheType = (UINT8) (CpuidRegisters.RegEax & V_CPUID_CACHE_TYPE_MASK);
+
+    CopyMem (RecordBuffer.Raw, &mCacheDataRecordHeader, HeaderSize);
+
+    mCacheInstance[CacheLevel - 1]++;
+    RecordBuffer.DataRecord->DataRecordHeader.Instance    = mCacheInstance[CacheLevel - 1];
+    RecordBuffer.DataRecord->DataRecordHeader.SubInstance = CacheLevel;
+
+    ///
+    ///  Record Type 1
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType        = CacheSizeRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_SIZE_DATA);
+    RecordBuffer.DataRecord->VariableRecord.CacheSize.Value = (UINT16) LxCacheSize;
+    RecordBuffer.DataRecord->VariableRecord.CacheSize.Exponent = 10;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 2
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType              = MaximumSizeCacheRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_MAXIMUM_CACHE_SIZE_DATA);
+    RecordBuffer.DataRecord->VariableRecord.MaximumCacheSize.Value = (UINT16) LxCacheSize;
+    RecordBuffer.DataRecord->VariableRecord.MaximumCacheSize.Exponent = 10;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 3
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType        = CacheSpeedRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_SPEED_DATA);
+    RecordBuffer.DataRecord->VariableRecord.CacheSpeed.Exponent = 0;
+    RecordBuffer.DataRecord->VariableRecord.CacheSpeed.Value = 0;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 4
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType  = CacheSocketRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_SOCKET_DATA);
+    RecordBuffer.DataRecord->VariableRecord.CacheSocket = 0;
+    ///
+    /// CacheDesignation[Index1];
+    ///
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 5
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType  = CacheSramTypeRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_SRAM_TYPE_DATA);
+    ZeroMem (
+            &RecordBuffer.DataRecord->VariableRecord.CacheSramType,
+            sizeof (EFI_CACHE_SRAM_TYPE_DATA)
+            );
+    RecordBuffer.DataRecord->VariableRecord.CacheSramType.Synchronous = TRUE;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 6, since record same as Type 5
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType  = CacheInstalledSramTypeRecordType;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 7
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType    = CacheErrorTypeRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_ERROR_TYPE_DATA);
+    RecordBuffer.DataRecord->VariableRecord.CacheErrorType = EfiCacheErrorSingleBit;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 8
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType  = CacheTypeRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_TYPE_DATA);
+    ///
+    /// If cache type is larger or equal than 5, this is undefined type so mark it as "Other" Cache type.
+    ///
+    if (LxCacheType >= SMBIOS_CACHE_TYPE_MAX) {
+      LxCacheType = SMBIOS_CACHE_TYPE_MAX - 1;
+    }
+    RecordBuffer.DataRecord->VariableRecord.CacheType = SmbiosCacheTypeFieldConverter[LxCacheType];
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 9
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType        = CacheAssociativityRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_ASSOCIATIVITY_DATA);
+    ///
+    /// Convert Associativity Ways to SMBIOS format
+    ///
+    switch (Ways) {
+      case 2:
+        Ways = EfiCacheAssociativity2Way;
+        break;
+      case 4:
+        Ways = EfiCacheAssociativity4Way;
+        break;
+      case 8:
+        Ways = EfiCacheAssociativity8Way;
+        break;
+      case 12:
+        Ways = EfiCacheAssociativity12Way;
+        break;
+      case 16:
+        Ways = EfiCacheAssociativity16Way;
+        break;
+      case 24:
+        Ways = EfiCacheAssociativity24Way;
+        break;
+      case 32:
+        Ways = EfiCacheAssociativity32Way;
+        break;
+      case 48:
+        Ways = EfiCacheAssociativity48Way;
+        break;
+      case 64:
+        Ways = EfiCacheAssociativity64Way;
+        break;
+      default:
+        Ways = EfiCacheAssociativityOther;
+        break;
+    }
+    RecordBuffer.DataRecord->VariableRecord.CacheAssociativity = Ways;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    ///  Record Type 10
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType                = CacheConfigRecordType;
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_CONFIGURATION_DATA);
+    RecordBuffer.DataRecord->VariableRecord.CacheConfig.Level = CacheLevel;
+    RecordBuffer.DataRecord->VariableRecord.CacheConfig.Socketed = EFI_CACHE_NOT_SOCKETED;
+    RecordBuffer.DataRecord->VariableRecord.CacheConfig.Reserved2 = 0;
+    RecordBuffer.DataRecord->VariableRecord.CacheConfig.Location = EfiCacheInternal;
+    RecordBuffer.DataRecord->VariableRecord.CacheConfig.Enable = EFI_CACHE_ENABLED;
+    RecordBuffer.DataRecord->VariableRecord.CacheConfig.OperationalMode = EfiCacheWriteBack;
+    RecordBuffer.DataRecord->VariableRecord.CacheConfig.Reserved1 = 0;
+    Status = LogCacheData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Cache Association. Processor Record Type 17
+    ///
+    TotalSize = HeaderSize + sizeof (EFI_CACHE_ASSOCIATION_DATA);
+    RecordBuffer.DataRecord->VariableRecord.CacheAssociation.ProducerName = gEfiProcessorProducerGuid;
+    ///
+    /// RecordBuffer.DataRecord->VariableRecord.CacheAssociation.ProducerInstance = (UINT16)Instance;
+    ///
+    RecordBuffer.DataRecord->VariableRecord.CacheAssociation.Instance = RecordBuffer.DataRecord->DataRecordHeader.Instance;
+    RecordBuffer.DataRecord->VariableRecord.CacheAssociation.SubInstance = RecordBuffer.DataRecord->DataRecordHeader.SubInstance;
+    CopyMem (RecordBuffer.Raw, &mCpuDataRecordHeader, HeaderSize);
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType  = CacheAssociationRecordType;
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+  }
+
+  FreePool (RecordBuffer.Raw);
+  return EFI_SUCCESS;
+}
+
+/**
+  Log cache data into data hub
+
+  @param[in] DataHub - Pointer to the DataHub protocol that will be updated
+  @param[in] Buffer  - Data buffer which will be updated into DataHub
+  @param[in] Size    - The size of data buffer
+
+  @retval EFI_STATUS - status code for logging data into dat hub
+**/
+EFI_STATUS
+LogCacheData (
+  EFI_DATA_HUB_PROTOCOL *DataHub,
+  UINT8                 *Buffer,
+  UINT32                Size
+  )
+{
+  EFI_STATUS Status;
+
+  Status = DataHub->LogData (
+                  DataHub,
+                  &gEfiCacheSubClassGuid,
+                  &gEfiProcessorProducerGuid,
+                  EFI_DATA_RECORD_CLASS_DATA,
+                  Buffer,
+                  Size
+                  );
+  return Status;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CacheData.h b/ReferenceCode/Haswell/CpuInit/Dxe/CacheData.h
new file mode 100644
index 0000000..57579eb
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CacheData.h
@@ -0,0 +1,91 @@
+/** @file
+  Header file for CPU Data 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 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 _CACHE_DATA_H_
+#define _CACHE_DATA_H_
+
+#include "CpuInitDxe.h"
+
+///
+/// Platform-specific definitions
+///
+#define CPU_CACHE_DATA_MAXIMUM_LENGTH 0x100
+
+typedef union {
+  EFI_CACHE_DATA_RECORD *DataRecord;
+  UINT8                 *Raw;
+} CPU_CACHE_DATA_RECORD_BUFFER;
+
+typedef struct {
+  UINT8                        CacheLevel;
+  UINT8                        CacheDescriptor;
+  UINT16                       CacheSizeinKB;
+  EFI_CACHE_ASSOCIATIVITY_DATA Associativity;
+  EFI_CACHE_TYPE_DATA          Type;
+} EFI_CACHE_CONVERTER;
+
+/**
+  This function gets called with the processor number and will log all cache data to data hub
+  pertaining to this processor.
+
+  @param[in] CpuNumber - Processor Number
+  @param[in] CacheInformation - Cache information get from cpuid instruction
+
+  @retval EFI_OUT_OF_RESOURCES - Failed to allocate required POOL for record buffer.
+  @retval EFI_SUCCESS - successful to update cache data
+**/
+EFI_STATUS
+InitializeCacheData (
+  IN UINTN              CpuNumber,
+  IN EFI_CPUID_REGISTER *CacheInformation
+  );
+
+/**
+  Log cache data into data hub
+
+  @param[in] DataHub - Pointer to the DataHub protocol that will be updated
+  @param[in] Buffer  - Data buffer which will be updated into DataHub
+  @param[in] Size    - The size of data buffer
+
+  @retval EFI_STATUS - status code for logging data into dat hub
+**/
+EFI_STATUS
+LogCacheData (
+  EFI_DATA_HUB_PROTOCOL *DataHub,
+  UINT8                 *Buffer,
+  UINT32                Size
+  );
+
+/**
+  Log CPU data into data hub
+
+  @param[in] DataHub - point to data hub that will be updated
+  @param[in] Buffer  - the buffer which will be updated into data hub
+  @param[in] Size    - size of the buffer
+
+  @retval EFI_STATUS - status returned when updating Data hub
+**/
+EFI_STATUS
+LogCpuData (
+  EFI_DATA_HUB_PROTOCOL *DataHub,
+  UINT8                 *Buffer,
+  UINT32                Size
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuFvi.c b/ReferenceCode/Haswell/CpuInit/Dxe/CpuFvi.c
new file mode 100644
index 0000000..7559980
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuFvi.c
@@ -0,0 +1,62 @@
+/** @file
+  CPU 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
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxe.h"
+#endif
+
+FVI_ELEMENT_AND_FUNCTION mCpuFviElementsData[] = {
+  {
+    DEFAULT_FVI_ELEMENT_DATA(CPU),
+    NULL
+  },
+  {
+    {
+      1,
+      0,
+      UCODE_VERSION,
+      UCODE_FVI_STRING,
+      {
+        0
+      },
+    },
+    NULL
+  },
+  {
+    {
+      1,
+      0,
+      TXT_VERSION,
+      TXT_FVI_STRING,
+      {
+        0
+      },
+    },
+    NULL
+  }
+};
+
+FVI_DATA_HUB_CALLBACK_CONTEXT mCpuFviVersionData = {
+  MISC_SUBCLASS_FVI_HEADER_ENTRY(CPU),
+  mCpuFviElementsData,
+};
+
+UINTN mCpuFviElements = sizeof (mCpuFviElementsData) / sizeof (FVI_ELEMENT_AND_FUNCTION);
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.c b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.c
new file mode 100644
index 0000000..93ec58e
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.c
@@ -0,0 +1,1085 @@
+/** @file
+  Cpu driver, which initializes CPU and implements CPU Architecture
+  Protocol as defined in Framework specification.
+
+@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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxe.h"
+#include "ProcessorData.h"
+#include "CacheData.h"
+#include "Exception.h"
+#include "PfatDefinitions.h"
+#include "BootGuardLibrary.h"
+#include "BootGuardRevocationLib.h"
+
+#define SAMPLE_TICK_COUNT 1000
+
+#endif
+
+#include <token.h>
+extern UINT64                   mValidMtrrAddressMask;
+extern UINT64                   mValidMtrrBitsMask;
+extern EFI_CPU_MICROCODE_HEADER **mMicrocodePointerBuffer;
+extern UINT8                    CpuInitDxeStrings[];
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+EFI_HANDLE                mDriverHandle;
+EFI_HII_DATABASE_PROTOCOL *mHiiDatabase;
+#else
+EFI_HII_PROTOCOL *mHii;
+#endif
+EFI_SMM_BASE_PROTOCOL            *mSmmBaseProtocol = NULL;
+VOID                             *mSmmBaseRegistration;
+EFI_METRONOME_ARCH_PROTOCOL      *mMetronome;
+DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu = NULL;
+EFI_HII_HANDLE                   mStringHandle;
+BOOLEAN                          mIsFlushingGCD = TRUE;
+UINT8                            mSmmbaseSwSmiNumber;
+BOOLEAN                          mVariableMtrrChanged;
+BOOLEAN                          mFixedMtrrChanged;
+UINT64                           mCpuFrequency = 0;
+
+EFI_EVENT                 gReadyToBootEvent;
+EFI_CPU_INTERRUPT_HANDLER mExternalVectorTable[0x100];
+BOOLEAN                   mInterruptState = FALSE;
+
+///
+/// The Cpu Architectural Protocol that this Driver produces
+///
+EFI_CPU_ARCH_PROTOCOL gCpu = {
+  FlushCpuDataCache,
+  EnableInterrupt,
+  DisableInterrupt,
+  CpuGetInterruptState,
+  Init,
+  RegisterInterruptHandler,
+  GetTimerValue,
+  SetMemoryAttributes,
+  1, ///< NumberOfTimers
+  4, ///< DmaBufferAlignment
+};
+
+/**
+  Decide if the CPU is executing in SMM mode
+
+  @retval TRUE  - The CPU is executing in SMM mode
+  @retval FALSE - The CPU is not executing in SMM mode
+**/
+BOOLEAN
+ExecutionInSmm (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  BOOLEAN    InSmm;
+
+  if (mSmmBaseProtocol == NULL) {
+    return FALSE;
+  }
+
+  Status = mSmmBaseProtocol->InSmm (mSmmBaseProtocol, &InSmm);
+  ASSERT_EFI_ERROR (Status);
+  return InSmm;
+}
+
+/**
+  Flush CPU data cache. If the instruction cache is fully coherent
+  with all DMA operations then function can just return EFI_SUCCESS.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] Start               - Physical address to start flushing from.
+  @param[in] Length              - Number of bytes to flush. Round up to chipset
+                                   granularity.
+  @param[in] FlushType           - Specifies the type of flush operation to perform.
+
+  @retval EFI_SUCCESS           - If cache was flushed
+  @exception EFI_UNSUPPORTED       - If flush type is not supported.
+  @retval EFI_DEVICE_ERROR      - If requested range could not be flushed.
+**/
+EFI_STATUS
+EFIAPI
+FlushCpuDataCache (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                Length,
+  IN EFI_CPU_FLUSH_TYPE    FlushType
+  )
+{
+  if (FlushType == EfiCpuFlushTypeWriteBackInvalidate) {
+    AsmWbinvd ();
+    return EFI_SUCCESS;
+  } else if (FlushType == EfiCpuFlushTypeInvalidate) {
+    EfiInvd ();
+    return EFI_SUCCESS;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
+
+/**
+  Enables CPU interrupts.
+
+  @param[in] This                - Protocol instance structure
+
+  @retval EFI_SUCCESS           - If interrupts were enabled in the CPU
+  @retval EFI_DEVICE_ERROR      - If interrupts could not be enabled on the CPU.
+**/
+EFI_STATUS
+EFIAPI
+EnableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  )
+{
+  if (!ExecutionInSmm ()) {
+    CpuEnableInterrupt ();
+  }
+
+  mInterruptState = TRUE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Disables CPU interrupts.
+
+  @param[in] This                - Protocol instance structure
+
+  @retval EFI_SUCCESS           - If interrupts were disabled in the CPU.
+**/
+EFI_STATUS
+EFIAPI
+DisableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  )
+{
+  CpuDisableInterrupt ();
+
+  mInterruptState = FALSE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Return the state of interrupts.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] State               - Pointer to the CPU's current interrupt state
+
+  @retval EFI_SUCCESS           - If interrupts were disabled in the CPU.
+  @retval EFI_INVALID_PARAMETER - State is NULL.
+**/
+EFI_STATUS
+EFIAPI
+CpuGetInterruptState (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  OUT BOOLEAN              *State
+  )
+{
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *State = mInterruptState;
+  return EFI_SUCCESS;
+}
+
+/**
+  Generates an INIT to the CPU
+
+  @param[in] This                - Protocol instance structure
+  @param[in] InitType            - Type of CPU INIT to perform
+
+  @retval EFI_SUCCESS           - If CPU INIT occurred. This value should never be seen
+  @retval EFI_DEVICE_ERROR      - If CPU INIT failed.
+  @exception EFI_UNSUPPORTED    - Requested type of CPU INIT not supported.
+**/
+EFI_STATUS
+EFIAPI
+Init (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_CPU_INIT_TYPE     InitType
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Registers a function to be called from the CPU interrupt handler.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] InterruptType       - Defines which interrupt to hook.
+                                   IA-32 valid range is 0x00 through 0xFF
+  @param[in] InterruptHandler    - A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER
+                                   that is called when a processor interrupt occurs.
+                                   A null pointer is an error condition.
+
+  @retval EFI_SUCCESS            - If handler installed or uninstalled.
+  @retval EFI_ALREADY_STARTED    - InterruptHandler is not NULL, and a handler for
+                                   InterruptType was previously installed
+  @retval EFI_INVALID_PARAMETER  - InterruptHandler is NULL, and a handler for
+                                   InterruptType was not previously installed.
+  @exception EFI_UNSUPPORTED     - The interrupt specified by InterruptType is not supported.
+**/
+EFI_STATUS
+EFIAPI
+RegisterInterruptHandler (
+  IN EFI_CPU_ARCH_PROTOCOL     *This,
+  IN EFI_EXCEPTION_TYPE        InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER InterruptHandler
+  )
+{
+  if (InterruptType < 0 || InterruptType > 0xff) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (InterruptHandler == NULL && mExternalVectorTable[InterruptType] == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (InterruptHandler != NULL && mExternalVectorTable[InterruptType] != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  mExternalVectorTable[InterruptType] = InterruptHandler;
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+GetTimerValue (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN UINT32                TimerIndex,
+  OUT UINT64               *TimerValue,
+  OUT UINT64 *TimerPeriod  OPTIONAL
+  )
+/**
+  Returns a timer value from one of the CPU's internal timers. There is no
+  inherent time interval between ticks but is a function of the CPU frequency.
+
+  @param[in] This                - Protocol instance structure.
+  @param[in] TimerIndex          - Specifies which CPU timer is requested.
+  @param[in] TimerValue          - Pointer to the returned timer value.
+  @param[in] TimerPeriod         - A pointer to the amount of time that passes in femtoseconds (10-15) for each
+                                   increment of TimerValue. If TimerValue does not increment at a predictable
+                                   rate, then 0 is returned. The amount of time that has passed between two calls to
+                                   GetTimerValue() can be calculated with the formula
+                                   (TimerValue2 - TimerValue1) * TimerPeriod. This parameter is optional and may be NULL.
+
+  @retval EFI_SUCCESS            - If the CPU timer count was returned.
+  @exception EFI_UNSUPPORTED     - If the CPU does not have any readable timers.
+  @retval EFI_DEVICE_ERROR       - If an error occurred while reading the timer.
+  @retval EFI_INVALID_PARAMETER  - TimerIndex is not valid or TimerValue is NULL.
+**/
+{
+  UINT64 Actual;
+
+  if (TimerValue == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (TimerIndex != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *TimerValue = EfiReadTsc ();
+
+  if (TimerPeriod != NULL) {
+    GetActualFrequency (mMetronome, &Actual);
+    *TimerPeriod = DivU64x32 (1000000000, (UINT32) Actual);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set memory cacheability attributes for given range of memeory
+
+  @param[in] This                - Protocol instance structure
+  @param[in] BaseAddress         - Specifies the start address of the memory range
+  @param[in] Length              - Specifies the length of the memory range
+  @param[in] Attributes          - The memory cacheability for the memory range
+
+  @retval EFI_SUCCESS            - If the cacheability of that memory range is set successfully
+  @exception EFI_UNSUPPORTED     - If the desired operation cannot be done
+  @retval EFI_INVALID_PARAMETER  - The input parameter is not correct, such as Length = 0
+**/
+EFI_STATUS
+EFIAPI
+SetMemoryAttributes (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Attributes
+  )
+{
+  EFI_STATUS Status;
+  UINT64     TempQword;
+  UINT32     MsrNum;
+  UINTN      MtrrNumber;
+  BOOLEAN    Positive;
+  BOOLEAN    OverLap;
+#define SKIP_ALIGN_CHECK  0
+#if SKIP_ALIGN_CHECK
+  UINT32 Remainder;
+#endif
+  EFI_MP_SERVICES_PROTOCOL *MpService;
+  EFI_STATUS               Status1;
+  UINT32                   VariableMtrrLimit;
+  UINT32                   SmiEnVal;
+  BOOLEAN                  InterruptState;  
+  UINT8                    MasterIrq,SlaveIrq;     
+    
+  mFixedMtrrChanged     = FALSE;
+  mVariableMtrrChanged  = FALSE;
+
+  VariableMtrrLimit     = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+
+  if (mIsFlushingGCD) {
+    return EFI_SUCCESS;
+  }
+
+  TempQword = 0;
+
+  ///
+  /// Check for invalid parameter
+  ///
+  if (Length == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((BaseAddress &~mValidMtrrAddressMask) != 0 || (Length &~mValidMtrrAddressMask) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  switch (Attributes) {
+    case EFI_MEMORY_UC:
+      Attributes = CACHE_UNCACHEABLE;
+      break;
+
+    case EFI_MEMORY_WC:
+      Attributes = CACHE_WRITECOMBINING;
+      break;
+
+    case EFI_MEMORY_WT:
+      Attributes = CACHE_WRITETHROUGH;
+      break;
+
+    case EFI_MEMORY_WP:
+      Attributes = CACHE_WRITEPROTECTED;
+      break;
+
+    case EFI_MEMORY_WB:
+      Attributes = CACHE_WRITEBACK;
+      break;
+
+    default:
+      return EFI_UNSUPPORTED;
+  }
+
+  ///
+  /// Check if Fixed MTRR
+  ///
+  Status = EFI_SUCCESS;
+  while ((BaseAddress < (1 << 20)) && (Length > 0) && Status == EFI_SUCCESS) {
+    Status = CalculateFixedMtrr (Attributes, &BaseAddress, &Length);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  MasterIrq = IoRead8(0x21);            //save current irq mask
+  SlaveIrq = IoRead8(0xA1);
+  IoWrite8(0x21,0xFF);
+  IoWrite8(0xA1,0xFF);
+
+  CpuGetInterruptState (&gCpu, &InterruptState);
+  if (InterruptState) {
+    DisableInterrupt (&gCpu);
+  }
+  SmiEnVal = IoRead32(PM_BASE_ADDRESS + 0x30); // Save SMI Control and Enable Reg.
+  IoWrite32(PM_BASE_ADDRESS + 0x30, 0); // Disable SMI
+
+  if (mFixedMtrrChanged) {
+    ProgramFixedMtrr ();
+  }
+
+  if (Length == 0) {
+    ///
+    /// Just Fixed MTRR. NO need to go through Variable MTRR
+    ///
+    goto Done;
+  }
+
+  ///
+  /// since mem below 1m will be override by fixed mtrr, we can set it to 0 to save mtrr.
+  ///
+  if (BaseAddress == 0x100000) {
+    BaseAddress = 0;
+    Length += 0x100000;
+  }
+
+  ///
+  /// Check memory base address alignment
+  ///
+#if SKIP_ALIGN_CHECK
+  DivU64x32Remainder (BaseAddress, (UINT32) Power2MaxMemory (LShiftU64 (Length, 1)), &Remainder);
+  if (Remainder != 0) {
+    DivU64x32Remainder (BaseAddress, (UINT32) Power2MaxMemory (Length), &Remainder);
+    if (Remainder != 0) {
+      Status = EFI_UNSUPPORTED;
+      goto Done;
+    }
+  }
+#endif
+
+  ///
+  /// Check overlap
+  ///
+  GetMemoryAttribute ();
+  OverLap = CheckMemoryAttributeOverlap (BaseAddress, BaseAddress + Length - 1);
+  if (OverLap) {
+    Status = CombineMemoryAttribute (Attributes, &BaseAddress, &Length);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    if (Length == 0) {
+      ///
+      /// combine successfully
+      ///
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+  } else {
+    if (Attributes == CACHE_UNCACHEABLE) {
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+  }
+
+  ///
+  /// Program Variable MTRRs
+  ///
+  if (mUsedMtrr >= VariableMtrrLimit) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  ///
+  /// Find first unused MTRR
+  ///
+  for (MsrNum = CACHE_VARIABLE_MTRR_BASE; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1); MsrNum += 2) {
+    if ((AsmReadMsr64 (MsrNum + 1) & B_CACHE_MTRR_VALID) == 0) {
+      break;
+    }
+  }
+
+  TempQword = Length;
+  if (TempQword == Power2MaxMemory (TempQword)) {
+    ProgramVariableMtrr (
+            MsrNum,
+            BaseAddress,
+            Length,
+            Attributes
+            );
+  } else {
+    GetDirection (TempQword, &MtrrNumber, &Positive);
+    if ((mUsedMtrr + MtrrNumber) > VariableMtrrLimit) {
+      goto Done;
+    }
+    if (!Positive) {
+      Length = Power2MaxMemory (LShiftU64 (TempQword, 1));
+      ProgramVariableMtrr (
+              MsrNum,
+              BaseAddress,
+              Length,
+              Attributes
+              );
+      BaseAddress += TempQword;
+      TempQword   = Length - TempQword;
+      Attributes  = CACHE_UNCACHEABLE;
+    }
+    do {
+      ///
+      /// Find unused MTRR
+      ///
+      for (; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1); MsrNum += 2) {
+        if ((AsmReadMsr64 (MsrNum + 1) & B_CACHE_MTRR_VALID) == 0) {
+          break;
+        }
+      }
+      Length = Power2MaxMemory (TempQword);
+      ProgramVariableMtrr (
+              MsrNum,
+              BaseAddress,
+              Length,
+              Attributes
+              );
+      BaseAddress += Length;
+      TempQword -= Length;
+    } while (TempQword);
+  }
+
+Done:
+  Status1 = gBS->LocateProtocol (
+                  &gEfiMpServiceProtocolGuid,
+                  NULL,
+                  (VOID **) &MpService
+                  );
+
+  if (!EFI_ERROR (Status1)) {
+    if (mVariableMtrrChanged || mFixedMtrrChanged) {
+      ///
+      /// PERF_START (NULL, L"CacheSync", NULL, 0);
+      ///
+      ReadMtrrRegisters ();
+      Status1 = MpService->StartupAllAPs (
+                      MpService,
+                      MpMtrrSynchUp,
+                      FALSE,
+                      NULL,
+                      0,
+                      NULL,
+                      NULL
+                      );
+      ///
+      /// PERF_END (NULL, L"CacheSync", NULL, 0);
+      ///
+    }
+  }
+
+  IoWrite32(PM_BASE_ADDRESS + 0x30, SmiEnVal); // Restore SMI Control and Enable Reg.
+  if (InterruptState) {
+    EnableInterrupt (&gCpu);
+  }
+  IoWrite8(0x21,MasterIrq);     //restore irq mask
+  IoWrite8(0xA1,SlaveIrq);
+  return Status;
+}
+
+/**
+  Initialize the SmmBase pointer when SmmBase protocol get installed
+
+  @param[in] Event   - Event whose notification function is being invoked.
+  @param[in] Context - Pointer to the notification functions context, which is implementation dependent.
+**/
+VOID
+EFIAPI
+InitializeSmmBasePtr (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS Status;
+
+  Status = gBS->LocateProtocol (&gEfiSmmBaseProtocolGuid, NULL, (VOID **) &mSmmBaseProtocol);
+  if (EFI_ERROR (Status)) {
+    mSmmBaseProtocol = NULL;
+  }
+}
+
+/**
+  Call back function to publish Hii data
+
+  @param[in] Event   - The triggered event.
+  @param[in] Context - Context for this event.
+**/
+VOID
+EFIAPI
+HiiCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+  EFI_HII_PACKAGE_LIST_HEADER *PackageList;
+#else
+  EFI_HII_PACKAGES *PackageList;
+#endif
+  EFI_STATUS Status;
+
+  ///
+  /// Initialize strings to HII database
+  ///
+#if (EFI_SPECIFICATION_VERSION >= 0x0002000A)
+
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  &mHiiDatabase
+                  );
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  ///
+  /// Create driver handle used by HII database
+  ///
+  Status = CreateHiiDriverHandle (&mDriverHandle);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  ///
+  /// Publish our HII data
+  ///
+  PackageList = PreparePackageList (1, &gProcessorProducerGuid, CpuInitDxeStrings);
+  if (PackageList == NULL) {
+    return;
+  }
+  Status = mHiiDatabase->NewPackageList (
+                  mHiiDatabase,
+                  PackageList,
+                  mDriverHandle,
+                  &mStringHandle
+                  );
+  ASSERT_EFI_ERROR (Status);
+#else
+  ///
+  /// There should only be one HII protocol
+  ///
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiProtocolGuid,
+                  NULL,
+                  (VOID **) &mHii
+                  );
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  PackageList = PreparePackages (1, &gProcessorProducerGuid, CpuInitDxeStrings);
+  Status      = mHii->NewPack (mHii, PackageList, &mStringHandle);
+  FreePool (PackageList);
+#endif
+}
+
+/**
+  Create SMBIOS Table type - FviSmbiosType, when ExitPmAuth event is signaled
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+  **/
+VOID
+InitializeFviDataCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS Status;
+  VOID       *ProtocolPointer;
+  UINT32     uCodeRevision;
+
+  Status = gBS->LocateProtocol (&gExitPmAuthProtocolGuid, NULL, &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+    return;
+  }
+
+  gBS->CloseEvent (Event);
+
+  if (mPlatformCpu->CpuConfig->FviReport) {
+    InitFviDataHubCbContext (
+            mPlatformCpu->CpuConfig->FviSmbiosType,
+            (UINT8) mCpuFviElements,
+            &mCpuFviVersionData
+            );
+
+    uCodeRevision = GetCpuUcodeRevision ();
+    mCpuFviElementsData[UCODE_VER].Element.Version.MajorVersion = (UINT8) ((uCodeRevision & 0xFF000000) >> 24);
+    mCpuFviElementsData[UCODE_VER].Element.Version.MinorVersion = (UINT8) ((uCodeRevision & 0x00FF0000) >> 16);
+    mCpuFviElementsData[UCODE_VER].Element.Version.Revision     = (UINT8) ((uCodeRevision & 0x0000FF00) >> 8);
+    mCpuFviElementsData[UCODE_VER].Element.Version.BuildNum     = (UINT16) (uCodeRevision & 0x000000FF);
+
+    CreateRcFviDatahub (&mCpuFviVersionData);
+  }
+  return;
+}
+
+/**
+  Initialize the state information for the CPU Architectural Protocol
+
+  @param[in] ImageHandle - Image handle of the loaded driver
+  @param[in] SystemTable - Pointer to the System Table
+
+  @retval EFI_SUCCESS           - thread can be successfully created
+  @retval EFI_OUT_OF_RESOURCES  - cannot allocate protocol data structure
+  @retval EFI_DEVICE_ERROR      - cannot create the thread
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpu (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  )
+{
+  EFI_STATUS Status;
+  EFI_HANDLE NewHandle1;
+  VOID       *Registration;
+
+  ///
+  /// Initialize the Global Descriptor Table
+  ///
+  InitializeSelectors ();
+
+  ///
+  /// Setup Cache attributes and Interrupt Tables
+  ///
+  PrepareMemory ();
+
+  ///
+  /// Initialize Exception Handlers
+  ///
+  InitializeException (&gCpu);
+
+  ///
+  /// Install CPU Architectural Protocol
+  ///
+  NewHandle1  = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &NewHandle1,
+                  &gEfiCpuArchProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &gCpu
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Refresh memory space attributes according to MTRRs
+  ///
+  Status          = RefreshGcdMemoryAttributes ();
+  mIsFlushingGCD  = FALSE;
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Locate DxeCpuPlatformPolicy protocol instance and assign it to a global variable
+  ///
+  Status = gBS->LocateProtocol (&gDxeCpuPlatformPolicyProtocolGuid, NULL, (VOID **) &mPlatformCpu);
+  if (EFI_ERROR (Status)) {
+    DEBUG((EFI_D_ERROR,"Failed to locate DxeCpuPlatformPolicy Protocol\n"));
+    return Status;
+  }
+
+  //
+  // Dump Cpu platform policy
+  //
+  CpuDxePolicyDump();
+
+  ///
+  /// Initialize the global SmmBase SWSMI number
+  ///
+  mSmmbaseSwSmiNumber = mPlatformCpu->CpuConfig->SmmbaseSwSmiNumber;
+
+  ///
+  /// Load the microcode if needed
+  ///
+  Status  = LoadAllMicrocodeUpdates ();
+
+  Status  = gBS->LocateProtocol (&gEfiMetronomeArchProtocolGuid, NULL, (VOID **) &mMetronome);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+  EfiLibCreateProtocolNotifyEvent (
+          &gEfiHiiDatabaseProtocolGuid,
+          EFI_TPL_CALLBACK,
+          HiiCallback,
+          NULL,
+          &Registration
+          );
+#else
+  EfiLibCreateProtocolNotifyEvent (
+          &gEfiHiiProtocolGuid,
+          EFI_TPL_CALLBACK,
+          HiiCallback,
+          NULL,
+          &Registration
+          );
+#endif
+
+  ///
+  /// Create an SmmBase protocol call back event to initialize
+  /// Smm Base pointer, during SMM mode
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gEfiSmmBaseProtocolGuid,
+          TPL_CALLBACK,
+          InitializeSmmBasePtr,
+          NULL,
+          &mSmmBaseRegistration
+          );
+
+  ///
+  /// Initialize MP Support if necessary
+  ///
+  Status = InitializeMpSupport ();
+  if (EFI_ERROR(Status)) {
+    DEBUG((EFI_D_ERROR,"Failed to initialize MPs\n"));
+  }
+
+  ///
+  /// Create an ExitPmAuth protocol callback event to generate SMBIOS table - FviSmbiosType
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gExitPmAuthProtocolGuid,
+          EFI_TPL_CALLBACK,
+          InitializeFviDataCallback,
+          NULL,
+          &Registration
+          );
+
+  ///
+  /// Create an ExitPmAuth protocol callback event for PFAT.
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gExitPmAuthProtocolGuid,
+          EFI_TPL_CALLBACK,
+          InitializePfatToolsIntCallback,
+          NULL,
+          &Registration
+          );
+
+  ///
+  /// Verify if Boot Guard is supported
+  ///
+  if (IsBootGuardSupported()){
+    ///
+    /// Identify if Revocation is requested by Boot Guard ACM
+    ///
+    if (AsmReadMsr64 (MSR_BOOT_GUARD_SACM_INFO) & BIT7) {
+    ///
+    /// Create an ExitPmAuth protocol call back event if one or more of Boot Guard modules are revoked.
+    ///
+    EfiCreateProtocolNotifyEvent (
+      &gExitPmAuthProtocolGuid,
+      TPL_CALLBACK,
+      BootGuardRevocationCallback,
+      NULL,
+      &Registration
+      );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns the actual CPU core frequency in MHz.
+
+  @param[in] Metronome           - Metronome protocol
+  @param[in] Frequency           - Pointer to the CPU core frequency
+
+  @retval EFI_SUCCESS            - If the frequency is returned successfully
+  @retval EFI_INVALID_PARAMETER  - If the input parameter is wrong
+**/
+EFI_STATUS
+GetActualFrequency (
+  IN EFI_METRONOME_ARCH_PROTOCOL *Metronome,
+  OUT UINT64                     *Frequency
+  )
+{
+  UINT64     BeginValue;
+  UINT64     EndValue;
+  UINT64     TotalValue;
+  UINT32     TickCount;
+  BOOLEAN    InterruptState;
+  EFI_STATUS Status;
+
+  if (Metronome == NULL || Frequency == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mCpuFrequency == 0) {
+    ///
+    /// In order to calculate the actual CPU frequency, we keep track of the CPU Tsc value (which
+    /// increases by 1 for every cycle) for a know period of time. The Metronome is not accurate
+    /// for the 1st tick, so I choose to wait for 1000 ticks, thus the error can be control to be
+    /// lower than 1%.
+    ///
+    TickCount = SAMPLE_TICK_COUNT;
+    CpuGetInterruptState (&gCpu, &InterruptState);
+    if (InterruptState) {
+      DisableInterrupt (&gCpu);
+    }
+    ///
+    /// In DxeCis-0.91 specs.
+    /// Metronome->WaitForTick is possible for interrupt processing,
+    /// or exception processing to interrupt the execution of the WaitForTick() function.
+    /// Depending on the hardware source for the ticks, it is possible for a tick to be missed.
+    /// This function cannot guarantee that ticks will not be missed.
+    ///
+    while (TRUE) {
+      BeginValue  = EfiReadTsc ();
+      Status      = Metronome->WaitForTick (Metronome, TickCount);
+      EndValue    = EfiReadTsc ();
+      if (!EFI_ERROR (Status)) {
+        TotalValue = EndValue - BeginValue;
+        break;
+      }
+    }
+
+    if (InterruptState) {
+      EnableInterrupt (&gCpu);
+    }
+
+    mCpuFrequency = MultU64x32 (TotalValue, 10);
+    mCpuFrequency = DivU64x32 (mCpuFrequency, Metronome->TickPeriod * TickCount);
+  }
+
+  *Frequency = mCpuFrequency;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump RC CPU and PPM platform policies
+**/
+VOID
+CpuDxePolicyDump (
+  VOID
+  )
+{
+#ifdef EFI_DEBUG
+  CPU_CONFIG        *CpuConfig;
+  POWER_MGMT_CONFIG *PowerMgmtConfig;
+  SECURITY_CONFIG   *SecurityConfig;
+
+  CpuConfig = mPlatformCpu->CpuConfig;
+  PowerMgmtConfig = mPlatformCpu->PowerMgmtConfig;
+  SecurityConfig = mPlatformCpu->SecurityConfig;
+  //
+  // Dump Cpu Platform policy
+  //
+  DEBUG ((EFI_D_INFO, "\n------------------------ DXE CpuPlatformPolicy Dump Begin -----------------\n"));
+  //
+  // Cpu config
+  //
+  DEBUG ((EFI_D_INFO, " CPU:: HtState : 0x%X \n", CpuConfig->HtState));
+  DEBUG ((EFI_D_INFO, " CPU:: LimitCpuidMaximumValue : 0x%X\n", CpuConfig->LimitCpuidMaximumValue));
+  DEBUG ((EFI_D_INFO, " CPU:: ExecuteDisableBit : 0x%X\n", CpuConfig->ExecuteDisableBit));
+  DEBUG ((EFI_D_INFO, " CPU:: VmxEnable : 0x%X\n", CpuConfig->VmxEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: SmxEnable : 0x%X\n", CpuConfig->SmxEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: MachineCheckEnable : 0x%X\n", CpuConfig->MachineCheckEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: MonitorMwaitEnable : 0x%X\n", CpuConfig->MonitorMwaitEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: XapicEnable : 0x%X\n", CpuConfig->XapicEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: IsColdReset : 0x%X\n", CpuConfig->IsColdReset));
+  DEBUG ((EFI_D_INFO, " CPU:: MlcStreamerPrefetcher : 0x%X\n", CpuConfig->MlcStreamerPrefetcher));
+  DEBUG ((EFI_D_INFO, " CPU:: EnableDts : 0x%X\n", CpuConfig->EnableDts));
+  DEBUG ((EFI_D_INFO, " CPU:: FviReport : 0x%X\n", CpuConfig->FviReport));
+  DEBUG ((EFI_D_INFO, " CPU:: AesEnable : 0x%X\n", CpuConfig->AesEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: DebugInterfaceEnable : 0x%X\n", CpuConfig->DebugInterfaceEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: DebugInterfaceLockEnable : 0x%X\n", CpuConfig->DebugInterfaceLockEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: ApIdleManner : 0x%X\n", CpuConfig->ApIdleManner));
+  DEBUG ((EFI_D_INFO, " CPU:: ApHandoffManner : 0x%X\n", CpuConfig->ApHandoffManner));
+  DEBUG ((EFI_D_INFO, " CPU:: BspSelection : 0x%X\n", CpuConfig->BspSelection));
+  DEBUG ((EFI_D_INFO, " CPU:: SmmbaseSwSmiNumber : 0x%X\n", CpuConfig->SmmbaseSwSmiNumber));
+  DEBUG ((EFI_D_INFO, " CPU:: FviSmbiosType; : 0x%X\n", CpuConfig->FviSmbiosType));
+  //
+  // SECURITY_CONFIG : TXT_FUNCTION_CONFIG
+  //
+  DEBUG ((EFI_D_INFO, " CPU:: SECURITY_CONFIG:TXT_FUNCTION_CONFIG : ResetAux : 0x%X\n", SecurityConfig->TxtFunctionConfig->ResetAux));
+  DEBUG ((EFI_D_INFO, "\n------------------------ DXE CpuPlatformPolicy Dump End -----------------\n"));
+#endif
+}
+
+/**
+  Initialize the global DataHub pointer
+
+  @param[in] DataHub   - Pointer to the DataHub protocol as output
+
+  @retval EFI_SUCCESS  - If the DataHub pointer is initialized successfully
+**/
+EFI_STATUS
+InitializeDataHubPtr (
+  OUT EFI_DATA_HUB_PROTOCOL **DataHub
+  )
+{
+  EFI_STATUS Status;
+
+  Status = gBS->LocateProtocol (&gEfiDataHubProtocolGuid, NULL, (VOID **) DataHub);
+  return Status;
+}
+
+/**
+  Drop into SMM to register IOTRAP for pfat tools interface
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+InitializePfatToolsIntCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS           Status;
+  VOID                 *ProtocolPointer;
+  EFI_PHYSICAL_ADDRESS BaseAddress;
+  PFAT_HOB             *PfatHobPtr;
+  EFI_GUID             PfatHobGuid = PFAT_HOB_GUID;
+
+  Status = gBS->LocateProtocol (&gExitPmAuthProtocolGuid, NULL, &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+    return;
+  }
+
+  PfatHobPtr = GetFirstGuidHob (&PfatHobGuid);
+  if (PfatHobPtr != NULL) {
+    BaseAddress = (EFI_PHYSICAL_ADDRESS) PfatHobPtr->PfatToolsIntIoTrapAdd;
+    ///
+    /// IOTRAP TO SMM
+    ///
+    IoRead8 (BaseAddress);
+  }
+  return;
+}
+
+/**
+  Register callback function for Boot Guard revocation flow.
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+BootGuardRevocationCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *ProtocolPointer;
+
+  //
+  // Check whether this is real ExitPmAuth notification, or just a SignalEvent
+  //
+  Status = gBS->LocateProtocol (&gExitPmAuthProtocolGuid, NULL, (VOID **) &ProtocolPointer);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  BootGuardOemRevocationHook();
+  
+  //
+  // Closed the event to avoid call twice
+  //
+  gBS->CloseEvent (Event);
+
+  return;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.cif b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.cif
new file mode 100644
index 0000000..f533f7c
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.cif
@@ -0,0 +1,46 @@
+<component>
+	name = "CpuInitDxe"
+	category = ModulePart
+	LocalRoot = "ReferenceCode\Haswell\CpuInit\Dxe"
+	RefName = "CpuInitDxe"
+[files]
+"CacheData.h"	
+"CacheData.c"
+"CpuInitDxe.c"	
+"CpuInitDxeDbgr.c"
+"CpuInitDxe.h"	
+"CpuInitDxeDbgr.h"
+"CpuFvi.c"
+"Exception.h"
+"MachineCheck.c"	
+"MachineCheck.h"	
+"MemoryAttribute.c"	
+"MemoryAttribute.h"	
+"Microcode.c"	
+"MpCommon.c"	
+"MpCommon.h"	
+"MpService.c"	
+"MpService.h"	
+"PiMpService.c"	
+"PiMpService.h"	
+"MtrrSync.c"	
+"Features.c"	
+"Features.h"	
+"ProcessorData.c"	
+"ProcessorData.h"
+"CpuInitDxeStrings.uni"	
+"CpuInitDxe.inf"	
+"CpuInitDxe.dxs"	
+"CpuInitDxe.sdl"	
+"CpuInitDxe.mak"	
+"x64\Cpu.asm"
+"x64\MpFuncs.asm"	
+"x64\MemoryOperation.c"	
+"x64\MemoryOperationDbgr.c"
+"x64\Exception.c"	
+"x64\MpCpu.c"	
+"x64\CpuLib.h"	
+"x64\ProcessorDef.h"	
+"x64\VirtualMemory.h"	
+"x64\MpEqu.inc"
+<endComponent>
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.dxs b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.dxs
new file mode 100644
index 0000000..5298ba3
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.dxs
@@ -0,0 +1,57 @@
+/** @file
+  This is the Dependency expression for the CPU architectural 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
+
+**/
+
+//
+// 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_DEFINITION (CpuIo)
+#if (EFI_SPECIFICATION_VERSION>=0x2000A)
+#include EFI_PROTOCOL_DEFINITION (HiiDatabase)
+#else
+#include EFI_PROTOCOL_DEFINITION (Hii)
+#endif
+#include EFI_ARCH_PROTOCOL_DEFINITION (Variable)
+#include EFI_ARCH_PROTOCOL_DEFINITION (Metronome)
+#include EFI_PROTOCOL_DEFINITION (CpuPlatformPolicy)
+#endif
+
+DEPENDENCY_START
+  EFI_METRONOME_ARCH_PROTOCOL_GUID AND
+  EFI_CPU_IO_PROTOCOL_GUID AND
+#if (EFI_SPECIFICATION_VERSION>=0x2000A)
+  EFI_HII_DATABASE_PROTOCOL_GUID AND
+#else
+  EFI_HII_PROTOCOL_GUID AND
+#endif
+  EFI_VARIABLE_ARCH_PROTOCOL_GUID AND
+  DXE_CPU_PLATFORM_POLICY_PROTOCOL_GUID
+DEPENDENCY_END
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.h b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.h
new file mode 100644
index 0000000..a7be681
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.h
@@ -0,0 +1,546 @@
+/** @file
+  Private data structures
+
+@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 _CPU_INIT_DXE_H
+#define _CPU_INIT_DXE_H
+
+#include "CpuAccess.h"
+#include "MemoryAttribute.h"
+#include "RcFviDxeLib.h"
+#include "EdkIIGlueBaseLib.h"
+
+#include EFI_PROTOCOL_DEFINITION (CpuPlatformPolicy)
+#include EFI_PROTOCOL_CONSUMER (ExitPmAuth)
+#include EFI_PROTOCOL_DEFINITION (CpuInfo)
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+#include "UefiIfrLibrary.h"
+#endif
+
+#define INTERRUPT_VECTOR_NUMBER     256
+#define INTERRUPT_GATE_ATTRIBUTE    0x8e00
+#define NUMBER_OF_MICROCODE_UPDATE  10
+
+extern UINT8 mSmmbaseSwSmiNumber;
+
+#define CPU_FVI_STRING          "Reference Code - CPU"
+#define CPU_FVI_SMBIOS_TYPE     0xDD  ///< Default value
+#define CPU_FVI_SMBIOS_INSTANCE 0x02
+#define UCODE_FVI_STRING        "uCode Version"
+#define UCODE_VERSION \
+  { \
+    0xFF, 0xFF, 0xFF, 0xFFFF \
+  }
+#define TXT_FVI_STRING          "TXT ACM version"
+#define TXT_ACM_MAJOR_VERSION   0x1
+#define TXT_ACM_MINOR_VERSION   0x3
+#define TXT_ACM_REVERSION       0x0
+#define TXT_ACM_BUILD_NUMBER    0x1
+#define TXT_VERSION \
+  { \
+    TXT_ACM_MAJOR_VERSION, TXT_ACM_MINOR_VERSION, TXT_ACM_REVERSION, TXT_ACM_BUILD_NUMBER \
+  }
+enum {
+  CPU_RC_VER= 0,
+  UCODE_VER,
+  TXT_VER
+} CPU_FVI_INDEX;
+
+extern FVI_ELEMENT_AND_FUNCTION      mCpuFviElementsData[];
+extern FVI_DATA_HUB_CALLBACK_CONTEXT mCpuFviVersionData;
+extern UINTN                         mCpuFviElements;
+
+/**
+  Adjust length to a paragraph boundry
+
+  @param[in] MemoryLength  -  Input memory length.
+
+  @retval Returned Maximum length.
+**/
+UINT64
+Power2MaxMemory (
+  IN UINT64 MemoryLength
+  );
+
+/**
+  Disable cache and its mtrr
+
+  @param[in] OldMtrr - To return the Old MTRR value
+**/
+VOID
+EfiDisableCacheMtrr (
+  IN UINT64 *OldMtrr
+  );
+
+/**
+  Recover cache MTRR
+
+  @param[in] EnableMtrr - Whether to enable the MTRR
+  @param[in] OldMtrr    - The saved old MTRR value to restore when not to
+                          enable the MTRR
+**/
+VOID
+EfiRecoverCacheMtrr (
+  IN BOOLEAN EnableMtrr,
+  IN UINT64  OldMtrr
+  );
+
+typedef struct _ALIGNED_DWORD {
+  UINT32 High;
+  UINT32 Low;
+} ALIGNED_DWORD;
+
+typedef union _ALIGNED {
+  UINT64        AlignedQword;
+  ALIGNED_DWORD AlignedDword;
+} ALIGNED;
+
+/**
+  Initialize the state information for the CPU Architectural Protocol
+
+  @param[in] ImageHandle - Image handle of the loaded driver
+  @param[in] SystemTable - Pointer to the System Table
+
+  @retval EFI_SUCCESS           - thread can be successfully created
+  @retval EFI_OUT_OF_RESOURCES  - cannot allocate protocol data structure
+  @retval EFI_DEVICE_ERROR      - cannot create the thread
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpu (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  );
+
+/**
+  Prepare memory for essential system tables.
+
+  @retval EFI_SUCCESS  - Memory successfully prepared.
+**/
+EFI_STATUS
+PrepareMemory (
+  VOID
+  );
+
+/**
+  Flush CPU data cache. If the instruction cache is fully coherent
+  with all DMA operations then function can just return EFI_SUCCESS.
+
+  @param[in] This             - Protocol instance structure
+  @param[in] Start            - Physical address to start flushing from.
+  @param[in] Length           - Number of bytes to flush. Round up to chipset granularity.
+  @param[in] FlushType        - Specifies the type of flush operation to perform.
+
+  @retval EFI_SUCCESS         - If cache was flushed
+  @exception EFI_UNSUPPORTED  - If flush type is not supported.
+  @retval EFI_DEVICE_ERROR    - If requested range could not be flushed.
+**/
+EFI_STATUS
+EFIAPI
+FlushCpuDataCache (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                Length,
+  IN EFI_CPU_FLUSH_TYPE    FlushType
+  );
+
+/**
+  Enables CPU interrupts.
+
+  @param[in] This            - Protocol instance structure
+
+  @retval EFI_SUCCESS        - If interrupts were enabled in the CPU
+  @retval EFI_DEVICE_ERROR   - If interrupts could not be enabled on the CPU.
+**/
+EFI_STATUS
+EFIAPI
+EnableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  );
+
+/**
+  Disables CPU interrupts.
+
+  @param[in] This           - Protocol instance structure
+
+  @retval EFI_SUCCESS       - If interrupts were disabled in the CPU.
+  @retval EFI_DEVICE_ERROR  - If interrupts could not be disabled on the CPU.
+**/
+EFI_STATUS
+EFIAPI
+DisableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  );
+
+/**
+  Return the state of interrupts.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] State               - Pointer to the CPU's current interrupt state
+
+  @retval EFI_SUCCESS            - If interrupts were disabled in the CPU.
+  @retval EFI_INVALID_PARAMETER  - State is NULL.
+**/
+EFI_STATUS
+EFIAPI
+CpuGetInterruptState (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  OUT BOOLEAN              *State
+  );
+
+/**
+  Generates an INIT to the CPU
+
+  @param[in] This             - Protocol instance structure
+  @param[in] InitType         - Type of CPU INIT to perform
+
+  @retval EFI_SUCCESS         - If CPU INIT occurred. This value should never be seen.
+  @retval EFI_DEVICE_ERROR    - If CPU INIT failed.
+  @exception EFI_UNSUPPORTED  - Requested type of CPU INIT not supported.
+**/
+EFI_STATUS
+EFIAPI
+Init (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_CPU_INIT_TYPE     InitType
+  );
+
+/**
+  Registers a function to be called from the CPU interrupt handler.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] InterruptType       - Defines which interrupt to hook.
+                                   IA-32 valid range is 0x00 through 0xFF
+  @param[in] InterruptHandler    - A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER
+                                   that is called when a processor interrupt occurs.
+                                   A null pointer is an error condition.
+
+  @retval EFI_SUCCESS            - If handler installed or uninstalled.
+  @retval EFI_ALREADY_STARTED    - InterruptHandler is not NULL, and a handler for
+                                   InterruptType was previously installed
+  @retval EFI_INVALID_PARAMETER  - InterruptHandler is NULL, and a handler for
+                                   InterruptType was not previously installed.
+  @exception EFI_UNSUPPORTED     - The interrupt specified by InterruptType is not supported.
+**/
+EFI_STATUS
+EFIAPI
+RegisterInterruptHandler (
+  IN EFI_CPU_ARCH_PROTOCOL     *This,
+  IN EFI_EXCEPTION_TYPE        InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER InterruptHandler
+  );
+
+/**
+  Returns a timer value from one of the CPU's internal timers. There is no
+  inherent time interval between ticks but is a function of the CPU frequency.
+
+  @param[in] This                - Protocol instance structure.
+  @param[in] TimerIndex          - Specifies which CPU timer is requested.
+  @param[in] TimerValue          - Pointer to the returned timer value.
+  @param[in] TimerPeriod         - A pointer to the amount of time that passes in femtoseconds (10-15) for each
+                                   increment of TimerValue. If TimerValue does not increment at a predictable
+                                   rate, then 0 is returned. The amount of time that has passed between two calls to
+                                   GetTimerValue() can be calculated with the formula
+                                   (TimerValue2 - TimerValue1) * TimerPeriod. This parameter is optional and may be NULL.
+
+  @retval EFI_SUCCESS            - If the CPU timer count was returned.
+  @exception EFI_UNSUPPORTED     - If the CPU does not have any readable timers.
+  @retval EFI_DEVICE_ERROR       - If an error occurred while reading the timer.
+  @retval EFI_INVALID_PARAMETER  - TimerIndex is not valid or TimerValue is NULL.
+**/
+EFI_STATUS
+EFIAPI
+GetTimerValue (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN UINT32                TimerIndex,
+  OUT UINT64               *TimerValue,
+  OUT UINT64 *TimerPeriod  OPTIONAL
+  );
+
+/**
+  Set memory cacheability attributes for given range of memeory
+
+  @param[in] This                - Protocol instance structure
+  @param[in] BaseAddress         - Specifies the start address of the memory range
+  @param[in] Length              - Specifies the length of the memory range
+  @param[in] Attributes          - The memory cacheability for the memory range
+
+  @retval EFI_SUCCESS            - If the cacheability of that memory range is set successfully
+  @exception EFI_UNSUPPORTED     - If the desired operation cannot be done
+  @retval EFI_INVALID_PARAMETER  - The input parameter is not correct, such as Length = 0
+**/
+EFI_STATUS
+EFIAPI
+SetMemoryAttributes (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Attributes
+  );
+
+/**
+  Init Global Descriptor table
+**/
+VOID
+InitializeSelectors (
+  VOID
+  );
+
+typedef struct {
+  VOID  *Start;
+  UINTN Size;
+  UINTN FixOffset;
+} INTERRUPT_HANDLER_TEMPLATE_MAP;
+
+typedef union {
+  EFI_CPU_DATA_RECORD *DataRecord;
+  UINT8               *Raw;
+} EFI_CPU_DATA_RECORD_BUFFER;
+
+///
+/// This constant defines the maximum length of the CPU brand string. According to the
+/// IA manual, the brand string is in EAX through EDX (thus 16 bytes) after executing
+/// the CPUID instructions with EAX as 80000002, 80000003, 80000004.
+///
+#define MAXIMUM_CPU_BRAND_STRING_LENGTH 48
+
+typedef struct {
+  BOOLEAN                    StringValid;
+  CHAR16                     BrandString[MAXIMUM_CPU_BRAND_STRING_LENGTH + 1];
+  EFI_PROCESSOR_VERSION_DATA StringRef;
+} PROCESSOR_VERSION_INFORMATION;
+
+///
+/// The constant defines how many times the Cpuid instruction should be executed
+/// in order to get all the cache information. For Pentium 4 processor, 1 is enough
+///
+#define CPU_CPUID_EXECTION_COUNT  2
+
+typedef struct {
+  UINT64                                 IntendCoreFrequency;
+  UINT64                                 IntendFsbFrequency;
+  PROCESSOR_VERSION_INFORMATION          Version;
+  EFI_PROCESSOR_MANUFACTURER_DATA        Manufacturer;
+  EFI_PROCESSOR_ID_DATA                  CpuidData;
+  EFI_PROCESSOR_FAMILY_DATA              Family;
+  INT16                                  Voltage;
+  EFI_PROCESSOR_APIC_BASE_ADDRESS_DATA   ApicBase;
+  EFI_PROCESSOR_APIC_ID_DATA             ApicID;
+  EFI_PROCESSOR_APIC_VERSION_NUMBER_DATA ApicVersion;
+  UINT32                                 MicrocodeRevision;
+  EFI_PROCESSOR_STATUS_DATA              Status; ///< Need to update this field before report
+  CPU_PHYSICAL_LOCATION                  Location;
+  EFI_MP_HEALTH_FLAGS                    Health;
+  EFI_CPUID_REGISTER                     CacheInformation[CPU_CPUID_EXECTION_COUNT];
+} CPU_DATA_FOR_DATAHUB;
+
+///
+/// Type, FamilyId and Model values for the different processors
+/// [0:7]   - Model
+/// [8:23]  - FamilyId
+/// [24:31] - Type
+///
+#define RESERVED  0x00
+
+/**
+  This will locate a processor microcode and if it finds a newer revision, it will
+  load it to the processor.
+
+  @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 EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  OUT UINT32                  *FailedRevision
+  );
+
+/**
+  To indicate the first microcode load is done
+**/
+VOID
+McuFirstLoadDone (
+  VOID
+  );
+
+/**
+  Initializes MP support in the system.
+
+  @retval EFI_SUCCESS           - Multiple processors are initialized successfully.
+  @retval EFI_NOT_FOUND         - The ACPI variable is not found in S3 boot path.
+  @retval EFI_OUT_OF_RESOURCES  - No enough resoruces (such as out of memory).
+**/
+EFI_STATUS
+InitializeMpSupport (
+  VOID
+  );
+
+/**
+  Save the MTRR registers to global variables
+**/
+VOID
+ReadMtrrRegisters (
+  VOID
+  );
+
+/**
+  Synch up the MTRR values for all processors
+**/
+VOID
+EFIAPI
+MpMtrrSynchUp (
+  IN VOID *Buffer
+  );
+
+/**
+  Copy Global MTRR data to S3
+**/
+VOID
+SaveBspMtrrForS3 (
+  VOID
+  );
+
+/**
+  Load all microcode updates to memory. Since in S3 resume boot path, CPUs should be
+  patched again, these microcode updates are copied to OS reserved memory.
+
+  @retval EFI_SUCCESS           - All microcode updates are loaded to memory successfully
+  @retval EFI_OUT_OF_RESOURCES  - Not enough memory to accomodate all the microcode updates
+**/
+EFI_STATUS
+LoadAllMicrocodeUpdates (
+  VOID
+  );
+
+/**
+  Returns the actual CPU core frequency in MHz.
+
+  @param[in] Metronome           - Metronome protocol
+  @param[in] Frequency           - Pointer to the CPU core frequency
+
+  @retval EFI_SUCCESS            - If the frequency is returned successfully
+  @retval EFI_INVALID_PARAMETER  - If the input parameter is wrong
+**/
+EFI_STATUS
+GetActualFrequency (
+  IN EFI_METRONOME_ARCH_PROTOCOL *Metronome,
+  OUT UINT64                     *Frequency
+  );
+
+/**
+  Dump RC CPU and PPM platform policies
+**/
+VOID
+CpuDxePolicyDump (
+  VOID
+  );
+
+/**
+  Initialize the global DataHub pointer
+
+  @param[in] DataHub   - Pointer to the DataHub protocol as output
+
+  @retval EFI_SUCCESS  - If the DataHub pointer is initialized successfully
+**/
+EFI_STATUS
+InitializeDataHubPtr (
+  OUT EFI_DATA_HUB_PROTOCOL **DataHub
+  );
+
+/**
+  Check if loading microcode update fails, if so, report proper status code
+
+  @param[in] CpuNumber       - The CPU number
+  @param[in] Status          - The return value of InitializeMicrocode()
+  @param[in] FailedRevision  - The revision of the microcode update that failed to be loaded
+
+  @retval EFI_SUCCESS        - The status is check and proper status code is reported
+**/
+EFI_STATUS
+CheckMicrocodeUpdate (
+  IN UINTN      CpuNumber,
+  IN EFI_STATUS Status,
+  IN UINT32     FailedRevision
+  );
+
+/**
+  Enable Cpu Interrupt
+**/
+VOID
+CpuEnableInterrupt (
+  VOID
+  );
+
+/**
+  Disable Cpu Interrupt
+**/
+VOID
+CpuDisableInterrupt (
+  VOID
+  );
+
+/**
+  Get code segment
+
+  @retval Code segmnet value
+**/
+UINT16
+GetCodeSegment (
+  VOID
+  );
+
+/**
+  Initialize Cpu float point unit
+**/
+VOID
+CpuInitFloatPointUnit (
+  VOID
+  );
+
+/**
+  Drop into SMM to register IOTRAP handler for pfat tools interface
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+InitializePfatToolsIntCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  Register callback function for Boot Guard revocation flow.
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+BootGuardRevocationCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.inf b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.inf
new file mode 100644
index 0000000..bd66192
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.inf
@@ -0,0 +1,171 @@
+## @file
+#  Component description file for MP Cpu module.
+#
+#@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
+#
+
+[defines]
+BASE_NAME            = CpuInitDxe
+FILE_GUID            = 62D171CB-78CD-4480-8678-C6A2A797A8DE
+COMPONENT_TYPE       = BS_DRIVER
+
+[sources.ia32]
+
+[sources.x64]
+  x64/Cpu.asm
+  x64/MpFuncs.asm
+  x64/MemoryOperation.c
+  x64/Exception.c
+  x64/MpCpu.c
+  x64/CpuLib.h
+  x64/ProcessorDef.h
+  x64/VirtualMemory.h
+
+[sources.common]
+  CacheData.h
+  CacheData.c
+  CpuInitDxe.c
+  CpuInitDxe.h
+  Exception.h
+  MachineCheck.c
+  MachineCheck.h
+  MemoryAttribute.c
+  MemoryAttribute.h
+  Microcode.c
+  MpCommon.c
+  MpCommon.h
+  MpService.c
+  MpService.h
+  PiMpService.c
+  PiMpService.h
+  MtrrSync.c
+  Features.c
+  Features.h
+  ProcessorData.c
+  ProcessorData.h
+  CpuInitDxeStrings.uni
+  CpuFvi.c
+
+#
+# Edk II Glue Driver Entry Point
+#
+ EdkIIGlueDxeDriverEntryPoint.c
+
+[includes.common]
+  $(DEST_DIR)
+  $(EDK_SOURCE)/Foundation
+  $(EDK_SOURCE)/Foundation/Core/Dxe
+  $(EDK_SOURCE)/Foundation/Efi
+  $(EDK_SOURCE)/Foundation/Include
+  $(EDK_SOURCE)/Foundation/Include/Pei
+  $(EDK_SOURCE)/Foundation/Efi/Include
+  $(EDK_SOURCE)/Foundation/FrameWork
+  $(EDK_SOURCE)/Foundation/FrameWork/Include
+  $(EDK_SOURCE)/Foundation/Library/Dxe/Include
+  $(EDK_SOURCE)/Foundation/Library/Pei/Include
+  $(EDK_SOURCE)/Foundation/Include/IndustryStandard
+  $(EDK_SOURCE)/Foundation/Cpu/Pentium/Include
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/CpuInit/Dxe
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/CpuInit/Dxe/x64
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include/Library
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include/Library
+
+#
+# if (EFI_SPECIFICATION_VERSION < 0x0002000A), use EfiIfrSupportLib
+# if (EFI_SPECIFICATION_VERSION >= 0x0002000A), use UefiEfiIfrSupportLib
+#
+# $(EDK_SOURCE)/Foundation/Library/Dxe/EfiIfrSupportLib
+  $(EDK_SOURCE)/Foundation/Library/Dxe/UefiEfiIfrSupportLib
+
+#
+# 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
+  $(EDK_SOURCE)/Foundation/Library/EdkIIGlueLib/Include/Library
+#
+# 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_CPU_ROOT)/SampleCode
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/SampleCode/Include
+
+[libraries.common]
+  EfiGuidLib
+  EdkFrameworkProtocolLib
+  EdkProtocolLib
+  EdkIIGlueBaseIoLibIntrinsic
+  EdkIIGlueBaseLib
+  EdkIIGlueBaseMemoryLib
+  EdkIIGlueDxeReportStatusCodeLib
+  EdkIIGlueDxeServicesTableLib
+  EdkIIGlueDxeDebugLibReportStatusCode
+  EdkIIGlueUefiBootServicesTableLib
+  EdkIIGlueUefiRuntimeServicesTableLib
+  EdkIIGlueUefiLib
+  EdkIIGlueBasePciLibPciExpress
+  EdkIIGlueDxeMemoryAllocationLib
+  EdkIIGlueDxeHobLib
+  EdkIIGlueHiiLib
+  EdkIIGlueBaseTimerLibLocalApic
+
+#
+# if (EFI_SPECIFICATION_VERSION < 0x0002000A), use EfiIfrSupportLib
+# if (EFI_SPECIFICATION_VERSION >= 0x0002000A), use UefiEfiIfrSupportLib, EfiDriverLib
+#
+#  EfiIfrSupportLib
+  EfiDriverLib
+  UefiEfiIfrSupportLib
+  CpuProtocolLib
+  CpuGuidLib
+  CpuIA32Lib
+  RcFviDxeLib
+  CpuPlatformLib
+  BootGuardLib
+  BootGuardRevocationLib
+
+[nmake.common]
+  IMAGE_ENTRY_POINT=_ModuleEntryPoint
+  DPX_SOURCE=CpuInitDxe.dxs
+#
+# Module Entry Point
+#
+ C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_MODULE_ENTRY_POINT__=InitializeCpu
+ C_FLAGS = $(C_FLAGS) -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__
+ C_FLAGS = $(C_FLAGS) -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_UEFI_LIB__ \
+                      -D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+                      -D __EDKII_GLUE_DXE_MEMORY_ALLOCATION_LIB__ \
+                      -D __EDKII_GLUE_DXE_HOB_LIB__
+
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.mak b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.mak
new file mode 100644
index 0000000..f3d5922
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.mak
@@ -0,0 +1,140 @@
+# MAK file for the eModule:PowerManagement
+
+EDK : CpuInitDxe
+
+BUILD_CpuInitDxe_DIR = $(BUILD_DIR)\$(CpuInitDxe_DIR)
+
+$(BUILD_DIR)\CpuInitDxe.mak : $(CpuInitDxe_DIR)\CpuInitDxe.cif $(BUILD_RULES)
+    $(CIF2MAK) $(CpuInitDxe_DIR)\CpuInitDxe.cif $(CIF2MAK_DEFAULTS)
+
+CpuInitDxe : $(BUILD_DIR)\CpuInitDxe.MAK CpuInitDxeBin 
+
+CpuInitDxe_OBJECTS = \
+    $(BUILD_CpuInitDxe_DIR)\CacheData.obj \
+    $(BUILD_CpuInitDxe_DIR)\CpuFvi.obj \
+    $(BUILD_CpuInitDxe_DIR)\MachineCheck.obj \
+    $(BUILD_CpuInitDxe_DIR)\MemoryAttribute.obj \
+    $(BUILD_CpuInitDxe_DIR)\Microcode.obj \
+    $(BUILD_CpuInitDxe_DIR)\MpCommon.obj \
+    $(BUILD_CpuInitDxe_DIR)\MpService.obj \
+    $(BUILD_CpuInitDxe_DIR)\PiMpService.obj \
+    $(BUILD_CpuInitDxe_DIR)\MtrrSync.obj \
+    $(BUILD_CpuInitDxe_DIR)\Features.obj \
+    $(BUILD_CpuInitDxe_DIR)\ProcessorData.obj \
+    $(BUILD_CpuInitDxe_DIR)\x64\Cpu.obj \
+    $(BUILD_CpuInitDxe_DIR)\x64\MpFuncs.obj \
+    $(BUILD_CpuInitDxe_DIR)\x64\Exception.obj \
+    $(BUILD_CpuInitDxe_DIR)\x64\MpCpu.obj \
+!IFDEF PeiDebugger_SUPPORT
+!IF $(PeiDebugger_SUPPORT)
+    $(BUILD_CpuInitDxe_DIR)\CpuInitDxeDbgr.obj \
+    $(BUILD_CpuInitDxe_DIR)\x64\MemoryOperationDbgr.obj \
+!ELSE
+    $(BUILD_CpuInitDxe_DIR)\CpuInitDxe.obj \
+    $(BUILD_CpuInitDxe_DIR)\x64\MemoryOperation.obj \
+!ENDIF
+!ELSE
+    $(BUILD_CpuInitDxe_DIR)\CpuInitDxe.obj \
+    $(BUILD_CpuInitDxe_DIR)\x64\MemoryOperation.obj \
+!ENDIF
+    $(BUILD_DIR)\CpuInitDxeStrings.obj \
+
+CpuInitDxe_MY_INCLUDES= \
+    $(EdkIIGlueLib_INCLUDES)\
+    $(EDK_INCLUDES)\
+    $(PROJECT_CPU_INCLUDES)\
+    $(INTEL_PCH_INCLUDES)\
+    /I$(PROJECT_CPU_ROOT)\CpuInit\Dxe\
+    /I$(PROJECT_CPU_ROOT)\CpuInit\Dxe\x64\
+    /I$(PROJECT_CPU_ROOT)\CpuSmm\Include\
+    /I$(UefiEfiIfrSupportLib_DIR)\
+    /I$(PROJECT_CPU_ROOT)\
+    /I$(PROJECT_CPU_ROOT)\Include\
+    /IBoard\EM\Platform\SMBIOSUpdateData
+
+CpuInitDxe_DEFINES = $(MY_DEFINES)\
+    /D"__EDKII_GLUE_MODULE_ENTRY_POINT__=InitializeCpu"\
+    /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_LIB__ \
+    /D __EDKII_GLUE_UEFI_DEVICE_PATH_LIB__ \
+    /D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+    /D __EDKII_GLUE_DXE_MEMORY_ALLOCATION_LIB__ \
+    /D __EDKII_GLUE_DXE_HOB_LIB__ \
+    /D REQUEST_EBDA_SIZE="$(REQUEST_EBDA_SIZE)" \
+!IFDEF AMI_PEI_DEBUG_SUPPORT
+!IF $(AMI_PEI_DEBUG_SUPPORT)
+    /D __AMI_PEI_DEBUG_SUPPORT__
+!ENDIF
+!ENDIF    
+!IF $(CPU_MODULE_CREATE_SMBIOS_TABLES)
+    /D__CREATE_CPU_SMBIOS__
+!ENDIF    
+    
+CpuInitDxe_LIBS =\
+    $(EFIGUIDLIB)\
+    $(EDKFRAMEWORKPROTOCOLLIB)\
+    $(EDKPROTOCOLLIB)\
+    $(EdkIIGlueBaseIoLibIntrinsic_LIB)\
+    $(EdkIIGlueBaseLib_LIB)\
+    $(EdkIIGlueBaseMemoryLib_LIB)\
+    $(EdkIIGlueDxeReportStatusCodeLib_LIB)\
+    $(EdkIIGlueDxeServicesTableLib_LIB)\
+    $(EdkIIGlueDxeDebugLibReportStatusCode_LIB)\
+    $(EdkIIGlueUefiBootServicesTableLib_LIB)\
+    $(EdkIIGlueUefiLib_LIB)\
+    $(EdkIIGlueBasePciLibPciExpress_LIB)\
+    $(EdkIIGlueDxeMemoryAllocationLib_LIB)\
+    $(EdkIIGlueBaseTimerLibLocalApic_LIB)\
+    $(EdkIIGlueDxeHobLib_LIB)\
+    $(EdkIIGlueHiiLib_LIB)\
+    $(EFIDRIVERLIB)\
+    $(UEFIEFIIFRSUPPORTLIB)\
+    $(CpuProtocolLib_LIB)\
+    $(CpuGuidLib_LIB)\
+    $(CPUIA32LIB)\
+    $(CpuPlatformLib_LIB)\
+    $(PchPlatformDxeLib_LIB)\
+    $(RcFviDxeLib_LIB)\
+    $(BootGuardRevocationLib_LIB)\
+    $(BootGuardLib_LIB)
+
+
+CpuInitDxeBin : $(CpuInitDxe_LIBS)
+    $(CC) $(CFLAGS) /Fo$(BUILD_DIR)\ $(BUILD_DIR)\CpuInitDxeStrings.c
+    $(MAKE) /$(MAKEFLAGS) $(EDKIIGLUE_DEFAULTS)\
+    /f $(BUILD_DIR)\CpuInitDxe.mak all\
+    NAME=CpuInitDxe\
+    MAKEFILE=$(BUILD_DIR)\CpuInitDxe.mak \
+    "MY_INCLUDES=$(CpuInitDxe_MY_INCLUDES)" \
+    "MY_DEFINES=$(CpuInitDxe_DEFINES)"\
+    OBJECTS="$(CpuInitDxe_OBJECTS)" \
+    GUID=62D171CB-78CD-4480-8678-C6A2A797A8DE\
+    ENTRY_POINT=_ModuleEntryPoint \
+    TYPE=RT_DRIVER \
+    EDKIIModule=DXEDRIVER\
+    DEPEX1=$(CpuInitDxe_DIR)\CpuInitDxe.DXS \
+    DEPEX1_TYPE=EFI_SECTION_DXE_DEPEX  \
+    COMPRESS=1
+
+#---------------------------------------------------------------------------
+#    Create Cpu SDB data
+#---------------------------------------------------------------------------
+SetupSdbs : CpuDxeSDB
+
+CpuDxeSDB : $(BUILD_DIR)\CpuInitDxe.mak 
+    $(MAKE) /$(MAKEFLAGS) $(BUILD_DEFAULTS)\
+    /f $(BUILD_DIR)\CpuInitDxe.mak all\
+    TYPE=SDB NAME=CpuInitDxe
+    $(STRGATHER) -dump -lang $(SUPPORTED_LANGUAGES: = -lang )\
+    -db $(BUILD_DIR)\CpuInitDxe.sdb\
+    -oh $(BUILD_DIR)\CpuInitDxeStrDefs.h\
+    -bn CpuInitDxeStrings\
+    -oc $(BUILD_DIR)\CpuInitDxeStrings.c
+#    $(CC) $(CFLAGS) /Fo$(BUILD_DIR)\ $(BUILD_DIR)\CpuInitDxeStrings.c
+
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.sdl b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.sdl
new file mode 100644
index 0000000..e50eae9
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxe.sdl
@@ -0,0 +1,45 @@
+TOKEN
+	Name  = "CpuInitDxe_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
+
+TOKEN
+	Name  = "REQUEST_EBDA_SIZE"
+	Value  = "0x1000"
+	Help  = "Default value 0x1000. If we need to use EBDA, please change the token value to 0x2000. For example, Using mini-pcie card."
+	TokenType = Integer
+	TargetMAK = Yes
+End
+
+PATH
+	Name  = "CpuInitDxe_DIR"
+End
+
+MODULE
+	Help  = "Includes CpuPeiInit.mak to Project"
+	File  = "CpuInitDxe.mak"
+End
+
+ELINK
+	Name  = "CpuInitDxe_INCLUDES"
+	InvokeOrder = ReplaceParent
+End
+
+ELINK
+	Name  = "/I$(CpuInitDxe_DIR)"
+	Parent  = "CpuInitDxe_INCLUDES"
+	InvokeOrder = AfterParent
+End
+
+ELINK
+	Name  = "$(BUILD_DIR)\CpuInitDxe.ffs"
+	Parent  = "FV_MAIN"
+	InvokeOrder = AfterParent
+End
+
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeDbgr.c b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeDbgr.c
new file mode 100644
index 0000000..308329e
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeDbgr.c
@@ -0,0 +1,1112 @@
+/** @file
+  Cpu driver, which initializes CPU and implements CPU Architecture
+  Protocol as defined in Framework specification.
+
+@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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxeDbgr.h"
+#include "ProcessorData.h"
+#include "CacheData.h"
+#include "Exception.h"
+#include "PfatDefinitions.h"
+#include "BootGuardLibrary.h"
+#include "BootGuardRevocationLib.h"
+//(AMI_CHG+)>
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+EFI_GUID gAmiDebuggerCpuProtocolGuid  = AMI_DEBUGGER_CPU_PROTOCOL_GUID;
+AMI_DEBUGGER_CPU_PROTOCOL  *mAmiDebuggerCpuProtocol;
+VOID* AptioInterruptHandlerHalt;
+UINT32 AptioInterruptHandlerHalt1;
+VOID 
+EFIAPI CommonExceptionHandler ( 
+  IN EFI_EXCEPTION_TYPE   InterruptType, 
+  IN EFI_SYSTEM_CONTEXT   SystemContext
+  );
+#endif  
+//<(AMI_CHG+)
+
+#define SAMPLE_TICK_COUNT 1000
+
+#endif
+
+extern UINT64                   mValidMtrrAddressMask;
+extern UINT64                   mValidMtrrBitsMask;
+extern EFI_CPU_MICROCODE_HEADER **mMicrocodePointerBuffer;
+extern UINT8                    CpuInitDxeStrings[];
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+EFI_HANDLE                mDriverHandle;
+EFI_HII_DATABASE_PROTOCOL *mHiiDatabase;
+#else
+EFI_HII_PROTOCOL *mHii;
+#endif
+EFI_SMM_BASE_PROTOCOL            *mSmmBaseProtocol = NULL;
+VOID                             *mSmmBaseRegistration;
+EFI_METRONOME_ARCH_PROTOCOL      *mMetronome;
+DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu = NULL;
+EFI_HII_HANDLE                   mStringHandle;
+BOOLEAN                          mIsFlushingGCD = TRUE;
+UINT8                            mSmmbaseSwSmiNumber;
+BOOLEAN                          mVariableMtrrChanged;
+BOOLEAN                          mFixedMtrrChanged;
+UINT64                           mCpuFrequency = 0;
+
+EFI_EVENT                 gReadyToBootEvent;
+EFI_CPU_INTERRUPT_HANDLER mExternalVectorTable[0x100];
+BOOLEAN                   mInterruptState = FALSE;
+
+///
+/// The Cpu Architectural Protocol that this Driver produces
+///
+EFI_CPU_ARCH_PROTOCOL gCpu = {
+  FlushCpuDataCache,
+  EnableInterrupt,
+  DisableInterrupt,
+  CpuGetInterruptState,
+  Init,
+  RegisterInterruptHandler,
+  GetTimerValue,
+  SetMemoryAttributes,
+  1, ///< NumberOfTimers
+  4, ///< DmaBufferAlignment
+};
+
+/**
+  Decide if the CPU is executing in SMM mode
+
+  @retval TRUE  - The CPU is executing in SMM mode
+  @retval FALSE - The CPU is not executing in SMM mode
+**/
+BOOLEAN
+ExecutionInSmm (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  BOOLEAN    InSmm;
+
+  if (mSmmBaseProtocol == NULL) {
+    return FALSE;
+  }
+
+  Status = mSmmBaseProtocol->InSmm (mSmmBaseProtocol, &InSmm);
+  ASSERT_EFI_ERROR (Status);
+  return InSmm;
+}
+
+/**
+  Flush CPU data cache. If the instruction cache is fully coherent
+  with all DMA operations then function can just return EFI_SUCCESS.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] Start               - Physical address to start flushing from.
+  @param[in] Length              - Number of bytes to flush. Round up to chipset
+                                   granularity.
+  @param[in] FlushType           - Specifies the type of flush operation to perform.
+
+  @retval EFI_SUCCESS           - If cache was flushed
+  @exception EFI_UNSUPPORTED       - If flush type is not supported.
+  @retval EFI_DEVICE_ERROR      - If requested range could not be flushed.
+**/
+EFI_STATUS
+EFIAPI
+FlushCpuDataCache (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                Length,
+  IN EFI_CPU_FLUSH_TYPE    FlushType
+  )
+{
+  if (FlushType == EfiCpuFlushTypeWriteBackInvalidate) {
+    AsmWbinvd ();
+    return EFI_SUCCESS;
+  } else if (FlushType == EfiCpuFlushTypeInvalidate) {
+    EfiInvd ();
+    return EFI_SUCCESS;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
+
+/**
+  Enables CPU interrupts.
+
+  @param[in] This                - Protocol instance structure
+
+  @retval EFI_SUCCESS           - If interrupts were enabled in the CPU
+  @retval EFI_DEVICE_ERROR      - If interrupts could not be enabled on the CPU.
+**/
+EFI_STATUS
+EFIAPI
+EnableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  )
+{
+  if (!ExecutionInSmm ()) {
+    CpuEnableInterrupt ();
+  }
+
+  mInterruptState = TRUE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Disables CPU interrupts.
+
+  @param[in] This                - Protocol instance structure
+
+  @retval EFI_SUCCESS           - If interrupts were disabled in the CPU.
+**/
+EFI_STATUS
+EFIAPI
+DisableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  )
+{
+  CpuDisableInterrupt ();
+
+  mInterruptState = FALSE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Return the state of interrupts.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] State               - Pointer to the CPU's current interrupt state
+
+  @retval EFI_SUCCESS           - If interrupts were disabled in the CPU.
+  @retval EFI_INVALID_PARAMETER - State is NULL.
+**/
+EFI_STATUS
+EFIAPI
+CpuGetInterruptState (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  OUT BOOLEAN              *State
+  )
+{
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *State = mInterruptState;
+  return EFI_SUCCESS;
+}
+
+/**
+  Generates an INIT to the CPU
+
+  @param[in] This                - Protocol instance structure
+  @param[in] InitType            - Type of CPU INIT to perform
+
+  @retval EFI_SUCCESS           - If CPU INIT occurred. This value should never be seen
+  @retval EFI_DEVICE_ERROR      - If CPU INIT failed.
+  @exception EFI_UNSUPPORTED    - Requested type of CPU INIT not supported.
+**/
+EFI_STATUS
+EFIAPI
+Init (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_CPU_INIT_TYPE     InitType
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Registers a function to be called from the CPU interrupt handler.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] InterruptType       - Defines which interrupt to hook.
+                                   IA-32 valid range is 0x00 through 0xFF
+  @param[in] InterruptHandler    - A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER
+                                   that is called when a processor interrupt occurs.
+                                   A null pointer is an error condition.
+
+  @retval EFI_SUCCESS            - If handler installed or uninstalled.
+  @retval EFI_ALREADY_STARTED    - InterruptHandler is not NULL, and a handler for
+                                   InterruptType was previously installed
+  @retval EFI_INVALID_PARAMETER  - InterruptHandler is NULL, and a handler for
+                                   InterruptType was not previously installed.
+  @exception EFI_UNSUPPORTED     - The interrupt specified by InterruptType is not supported.
+**/
+EFI_STATUS
+EFIAPI
+RegisterInterruptHandler (
+  IN EFI_CPU_ARCH_PROTOCOL     *This,
+  IN EFI_EXCEPTION_TYPE        InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER InterruptHandler
+  )
+{
+//(AMI_CHG+)>    
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+  EFI_STATUS Status;
+#endif  	
+//<(AMI_CHG+)
+  
+  if (InterruptType < 0 || InterruptType > 0xff) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (InterruptHandler == NULL && mExternalVectorTable[InterruptType] == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (InterruptHandler != NULL && mExternalVectorTable[InterruptType] != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  mExternalVectorTable[InterruptType] = InterruptHandler;
+  
+//AMI_CHG>  
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+    if ((UINT32) InterruptHandler == (UINT32)CommonExceptionHandler)
+        Status = mAmiDebuggerCpuProtocol->DebuggerIsDebuggerIrqHadler(InterruptType,AptioInterruptHandlerHalt);
+    else
+        Status = mAmiDebuggerCpuProtocol->DebuggerIsDebuggerIrqHadler(InterruptType,InterruptHandler);
+#endif  
+//<AMI_CHG
+  
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+GetTimerValue (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN UINT32                TimerIndex,
+  OUT UINT64               *TimerValue,
+  OUT UINT64 *TimerPeriod  OPTIONAL
+  )
+/**
+  Returns a timer value from one of the CPU's internal timers. There is no
+  inherent time interval between ticks but is a function of the CPU frequency.
+
+  @param[in] This                - Protocol instance structure.
+  @param[in] TimerIndex          - Specifies which CPU timer is requested.
+  @param[in] TimerValue          - Pointer to the returned timer value.
+  @param[in] TimerPeriod         - A pointer to the amount of time that passes in femtoseconds (10-15) for each
+                                   increment of TimerValue. If TimerValue does not increment at a predictable
+                                   rate, then 0 is returned. The amount of time that has passed between two calls to
+                                   GetTimerValue() can be calculated with the formula
+                                   (TimerValue2 - TimerValue1) * TimerPeriod. This parameter is optional and may be NULL.
+
+  @retval EFI_SUCCESS            - If the CPU timer count was returned.
+  @exception EFI_UNSUPPORTED     - If the CPU does not have any readable timers.
+  @retval EFI_DEVICE_ERROR       - If an error occurred while reading the timer.
+  @retval EFI_INVALID_PARAMETER  - TimerIndex is not valid or TimerValue is NULL.
+**/
+{
+  UINT64 Actual;
+
+  if (TimerValue == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (TimerIndex != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *TimerValue = EfiReadTsc ();
+
+  if (TimerPeriod != NULL) {
+    GetActualFrequency (mMetronome, &Actual);
+    *TimerPeriod = DivU64x32 (1000000000, (UINT32) Actual);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set memory cacheability attributes for given range of memeory
+
+  @param[in] This                - Protocol instance structure
+  @param[in] BaseAddress         - Specifies the start address of the memory range
+  @param[in] Length              - Specifies the length of the memory range
+  @param[in] Attributes          - The memory cacheability for the memory range
+
+  @retval EFI_SUCCESS            - If the cacheability of that memory range is set successfully
+  @exception EFI_UNSUPPORTED     - If the desired operation cannot be done
+  @retval EFI_INVALID_PARAMETER  - The input parameter is not correct, such as Length = 0
+**/
+EFI_STATUS
+EFIAPI
+SetMemoryAttributes (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Attributes
+  )
+{
+  EFI_STATUS Status;
+  UINT64     TempQword;
+  UINT32     MsrNum;
+  UINTN      MtrrNumber;
+  BOOLEAN    Positive;
+  BOOLEAN    OverLap;
+#define SKIP_ALIGN_CHECK  0
+#if SKIP_ALIGN_CHECK
+  UINT32 Remainder;
+#endif
+  EFI_MP_SERVICES_PROTOCOL *MpService;
+  EFI_STATUS               Status1;
+  UINT32                   VariableMtrrLimit;
+
+  mFixedMtrrChanged     = FALSE;
+  mVariableMtrrChanged  = FALSE;
+
+  VariableMtrrLimit     = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+
+  if (mIsFlushingGCD) {
+    return EFI_SUCCESS;
+  }
+
+  TempQword = 0;
+
+  ///
+  /// Check for invalid parameter
+  ///
+  if (Length == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((BaseAddress &~mValidMtrrAddressMask) != 0 || (Length &~mValidMtrrAddressMask) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  switch (Attributes) {
+    case EFI_MEMORY_UC:
+      Attributes = CACHE_UNCACHEABLE;
+      break;
+
+    case EFI_MEMORY_WC:
+      Attributes = CACHE_WRITECOMBINING;
+      break;
+
+    case EFI_MEMORY_WT:
+      Attributes = CACHE_WRITETHROUGH;
+      break;
+
+    case EFI_MEMORY_WP:
+      Attributes = CACHE_WRITEPROTECTED;
+      break;
+
+    case EFI_MEMORY_WB:
+      Attributes = CACHE_WRITEBACK;
+      break;
+
+    default:
+      return EFI_UNSUPPORTED;
+  }
+
+  ///
+  /// Check if Fixed MTRR
+  ///
+  Status = EFI_SUCCESS;
+  while ((BaseAddress < (1 << 20)) && (Length > 0) && Status == EFI_SUCCESS) {
+    Status = CalculateFixedMtrr (Attributes, &BaseAddress, &Length);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if (mFixedMtrrChanged) {
+    ProgramFixedMtrr ();
+  }
+
+  if (Length == 0) {
+    ///
+    /// Just Fixed MTRR. NO need to go through Variable MTRR
+    ///
+    goto Done;
+  }
+
+  ///
+  /// since mem below 1m will be override by fixed mtrr, we can set it to 0 to save mtrr.
+  ///
+  if (BaseAddress == 0x100000) {
+    BaseAddress = 0;
+    Length += 0x100000;
+  }
+
+  ///
+  /// Check memory base address alignment
+  ///
+#if SKIP_ALIGN_CHECK
+  DivU64x32Remainder (BaseAddress, (UINT32) Power2MaxMemory (LShiftU64 (Length, 1)), &Remainder);
+  if (Remainder != 0) {
+    DivU64x32Remainder (BaseAddress, (UINT32) Power2MaxMemory (Length), &Remainder);
+    if (Remainder != 0) {
+      Status = EFI_UNSUPPORTED;
+      goto Done;
+    }
+  }
+#endif
+
+  ///
+  /// Check overlap
+  ///
+  GetMemoryAttribute ();
+  OverLap = CheckMemoryAttributeOverlap (BaseAddress, BaseAddress + Length - 1);
+  if (OverLap) {
+    Status = CombineMemoryAttribute (Attributes, &BaseAddress, &Length);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    if (Length == 0) {
+      ///
+      /// combine successfully
+      ///
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+  } else {
+    if (Attributes == CACHE_UNCACHEABLE) {
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+  }
+
+  ///
+  /// Program Variable MTRRs
+  ///
+  if (mUsedMtrr >= VariableMtrrLimit) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  ///
+  /// Find first unused MTRR
+  ///
+  for (MsrNum = CACHE_VARIABLE_MTRR_BASE; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1); MsrNum += 2) {
+    if ((AsmReadMsr64 (MsrNum + 1) & B_CACHE_MTRR_VALID) == 0) {
+      break;
+    }
+  }
+
+  TempQword = Length;
+  if (TempQword == Power2MaxMemory (TempQword)) {
+    ProgramVariableMtrr (
+            MsrNum,
+            BaseAddress,
+            Length,
+            Attributes
+            );
+  } else {
+    GetDirection (TempQword, &MtrrNumber, &Positive);
+    if ((mUsedMtrr + MtrrNumber) > VariableMtrrLimit) {
+      goto Done;
+    }
+    if (!Positive) {
+      Length = Power2MaxMemory (LShiftU64 (TempQword, 1));
+      ProgramVariableMtrr (
+              MsrNum,
+              BaseAddress,
+              Length,
+              Attributes
+              );
+      BaseAddress += TempQword;
+      TempQword   = Length - TempQword;
+      Attributes  = CACHE_UNCACHEABLE;
+    }
+    do {
+      ///
+      /// Find unused MTRR
+      ///
+      for (; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1); MsrNum += 2) {
+        if ((AsmReadMsr64 (MsrNum + 1) & B_CACHE_MTRR_VALID) == 0) {
+          break;
+        }
+      }
+      Length = Power2MaxMemory (TempQword);
+      ProgramVariableMtrr (
+              MsrNum,
+              BaseAddress,
+              Length,
+              Attributes
+              );
+      BaseAddress += Length;
+      TempQword -= Length;
+    } while (TempQword);
+  }
+
+Done:
+  Status1 = gBS->LocateProtocol (
+                  &gEfiMpServiceProtocolGuid,
+                  NULL,
+                  (VOID **) &MpService
+                  );
+
+  if (!EFI_ERROR (Status1)) {
+    if (mVariableMtrrChanged || mFixedMtrrChanged) {
+      ///
+      /// PERF_START (NULL, L"CacheSync", NULL, 0);
+      ///
+      ReadMtrrRegisters ();
+      Status1 = MpService->StartupAllAPs (
+                      MpService,
+                      MpMtrrSynchUp,
+                      FALSE,
+                      NULL,
+                      0,
+                      NULL,
+                      NULL
+                      );
+      ///
+      /// PERF_END (NULL, L"CacheSync", NULL, 0);
+      ///
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Initialize the SmmBase pointer when SmmBase protocol get installed
+
+  @param[in] Event   - Event whose notification function is being invoked.
+  @param[in] Context - Pointer to the notification functions context, which is implementation dependent.
+**/
+VOID
+EFIAPI
+InitializeSmmBasePtr (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS Status;
+
+  Status = gBS->LocateProtocol (&gEfiSmmBaseProtocolGuid, NULL, (VOID **) &mSmmBaseProtocol);
+  if (EFI_ERROR (Status)) {
+    mSmmBaseProtocol = NULL;
+  }
+}
+
+/**
+  Call back function to publish Hii data
+
+  @param[in] Event   - The triggered event.
+  @param[in] Context - Context for this event.
+**/
+VOID
+EFIAPI
+HiiCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+  EFI_HII_PACKAGE_LIST_HEADER *PackageList;
+#else
+  EFI_HII_PACKAGES *PackageList;
+#endif
+  EFI_STATUS Status;
+
+  ///
+  /// Initialize strings to HII database
+  ///
+#if (EFI_SPECIFICATION_VERSION >= 0x0002000A)
+
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  &mHiiDatabase
+                  );
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  ///
+  /// Create driver handle used by HII database
+  ///
+  Status = CreateHiiDriverHandle (&mDriverHandle);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  ///
+  /// Publish our HII data
+  ///
+  PackageList = PreparePackageList (1, &gProcessorProducerGuid, CpuInitDxeStrings);
+  if (PackageList == NULL) {
+    return;
+  }
+  Status = mHiiDatabase->NewPackageList (
+                  mHiiDatabase,
+                  PackageList,
+                  mDriverHandle,
+                  &mStringHandle
+                  );
+  ASSERT_EFI_ERROR (Status);
+#else
+  ///
+  /// There should only be one HII protocol
+  ///
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiProtocolGuid,
+                  NULL,
+                  (VOID **) &mHii
+                  );
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  PackageList = PreparePackages (1, &gProcessorProducerGuid, CpuInitDxeStrings);
+  Status      = mHii->NewPack (mHii, PackageList, &mStringHandle);
+  FreePool (PackageList);
+#endif
+}
+
+/**
+  Create SMBIOS Table type - FviSmbiosType, when ExitPmAuth event is signaled
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+  **/
+VOID
+InitializeFviDataCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS Status;
+  VOID       *ProtocolPointer;
+  UINT32     uCodeRevision;
+
+  Status = gBS->LocateProtocol (&gExitPmAuthProtocolGuid, NULL, &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+    return;
+  }
+
+  gBS->CloseEvent (Event);
+
+  if (mPlatformCpu->CpuConfig->FviReport) {
+    InitFviDataHubCbContext (
+            mPlatformCpu->CpuConfig->FviSmbiosType,
+            (UINT8) mCpuFviElements,
+            &mCpuFviVersionData
+            );
+
+    uCodeRevision = GetCpuUcodeRevision ();
+    mCpuFviElementsData[UCODE_VER].Element.Version.MajorVersion = (UINT8) ((uCodeRevision & 0xFF000000) >> 24);
+    mCpuFviElementsData[UCODE_VER].Element.Version.MinorVersion = (UINT8) ((uCodeRevision & 0x00FF0000) >> 16);
+    mCpuFviElementsData[UCODE_VER].Element.Version.Revision     = (UINT8) ((uCodeRevision & 0x0000FF00) >> 8);
+    mCpuFviElementsData[UCODE_VER].Element.Version.BuildNum     = (UINT16) (uCodeRevision & 0x000000FF);
+
+    CreateRcFviDatahub (&mCpuFviVersionData);
+  }
+  return;
+}
+
+/**
+  Initialize the state information for the CPU Architectural Protocol
+
+  @param[in] ImageHandle - Image handle of the loaded driver
+  @param[in] SystemTable - Pointer to the System Table
+
+  @retval EFI_SUCCESS           - thread can be successfully created
+  @retval EFI_OUT_OF_RESOURCES  - cannot allocate protocol data structure
+  @retval EFI_DEVICE_ERROR      - cannot create the thread
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpu (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  )
+{
+  EFI_STATUS Status;
+  EFI_HANDLE NewHandle1;
+  VOID       *Registration;
+
+  ///
+  /// Initialize the Global Descriptor Table
+  ///
+//(AMI_CHG+)>
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+  Status = gBS->LocateProtocol(
+                    &gAmiDebuggerCpuProtocolGuid,
+                    NULL,
+                    &mAmiDebuggerCpuProtocol
+                  );
+                  
+  if (!EFI_ERROR(Status)){
+    Status = mAmiDebuggerCpuProtocol->
+            DebuggerGetAptioIntHandler((VOID*)&AptioInterruptHandlerHalt1);                 
+  
+    (UINT32)AptioInterruptHandlerHalt = AptioInterruptHandlerHalt1;
+  
+    Status = mAmiDebuggerCpuProtocol->DebuggerSetupExceptionHandler();            
+  }
+#endif  
+  
+  InitializeSelectors ();
+
+  ///
+  /// Initialize Exception Handlers
+  ///
+  InitializeException (&gCpu);
+
+  ///
+  /// Setup Cache attributes and Interrupt Tables
+  ///  
+  PrepareMemory ();
+  
+//<(AMI_CHG+)
+
+  ///
+  /// Install CPU Architectural Protocol
+  ///
+  NewHandle1  = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &NewHandle1,
+                  &gEfiCpuArchProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &gCpu
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Refresh memory space attributes according to MTRRs
+  ///
+  Status          = RefreshGcdMemoryAttributes ();
+  mIsFlushingGCD  = FALSE;
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Locate DxeCpuPlatformPolicy protocol instance and assign it to a global variable
+  ///
+  Status = gBS->LocateProtocol (&gDxeCpuPlatformPolicyProtocolGuid, NULL, (VOID **) &mPlatformCpu);
+  if (EFI_ERROR (Status)) {
+    DEBUG((EFI_D_ERROR,"Failed to locate DxeCpuPlatformPolicy Protocol\n"));
+    return Status;
+  }
+
+  //
+  // Dump Cpu platform policy
+  //
+  CpuDxePolicyDump();
+
+  ///
+  /// Initialize the global SmmBase SWSMI number
+  ///
+  mSmmbaseSwSmiNumber = mPlatformCpu->CpuConfig->SmmbaseSwSmiNumber;
+
+  ///
+  /// Load the microcode if needed
+  ///
+  Status  = LoadAllMicrocodeUpdates ();
+
+  Status  = gBS->LocateProtocol (&gEfiMetronomeArchProtocolGuid, NULL, (VOID **) &mMetronome);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+  EfiLibCreateProtocolNotifyEvent (
+          &gEfiHiiDatabaseProtocolGuid,
+          EFI_TPL_CALLBACK,
+          HiiCallback,
+          NULL,
+          &Registration
+          );
+#else
+  EfiLibCreateProtocolNotifyEvent (
+          &gEfiHiiProtocolGuid,
+          EFI_TPL_CALLBACK,
+          HiiCallback,
+          NULL,
+          &Registration
+          );
+#endif
+
+  ///
+  /// Create an SmmBase protocol call back event to initialize
+  /// Smm Base pointer, during SMM mode
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gEfiSmmBaseProtocolGuid,
+          TPL_CALLBACK,
+          InitializeSmmBasePtr,
+          NULL,
+          &mSmmBaseRegistration
+          );
+
+  ///
+  /// Initialize MP Support if necessary
+  ///
+  Status = InitializeMpSupport ();
+  if (EFI_ERROR(Status)) {
+    DEBUG((EFI_D_ERROR,"Failed to initialize MPs\n"));
+  }
+
+  ///
+  /// Create an ExitPmAuth protocol callback event to generate SMBIOS table - FviSmbiosType
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gExitPmAuthProtocolGuid,
+          EFI_TPL_CALLBACK,
+          InitializeFviDataCallback,
+          NULL,
+          &Registration
+          );
+
+  ///
+  /// Create an ExitPmAuth protocol callback event for PFAT.
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gExitPmAuthProtocolGuid,
+          EFI_TPL_CALLBACK,
+          InitializePfatToolsIntCallback,
+          NULL,
+          &Registration
+          );
+
+  ///
+  /// Verify if Boot Guard is supported
+  ///
+  if (IsBootGuardSupported()){
+    ///
+    /// Identify if Revocation is requested by Boot Guard ACM
+    ///
+    if (AsmReadMsr64 (MSR_BOOT_GUARD_SACM_INFO) & BIT7) {
+    ///
+    /// Create an ExitPmAuth protocol call back event if one or more of Boot Guard modules are revoked.
+    ///
+    EfiCreateProtocolNotifyEvent (
+      &gExitPmAuthProtocolGuid,
+      TPL_CALLBACK,
+      BootGuardRevocationCallback,
+      NULL,
+      &Registration
+      );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns the actual CPU core frequency in MHz.
+
+  @param[in] Metronome           - Metronome protocol
+  @param[in] Frequency           - Pointer to the CPU core frequency
+
+  @retval EFI_SUCCESS            - If the frequency is returned successfully
+  @retval EFI_INVALID_PARAMETER  - If the input parameter is wrong
+**/
+EFI_STATUS
+GetActualFrequency (
+  IN EFI_METRONOME_ARCH_PROTOCOL *Metronome,
+  OUT UINT64                     *Frequency
+  )
+{
+  UINT64     BeginValue;
+  UINT64     EndValue;
+  UINT64     TotalValue;
+  UINT32     TickCount;
+  BOOLEAN    InterruptState;
+  EFI_STATUS Status;
+
+  if (Metronome == NULL || Frequency == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mCpuFrequency == 0) {
+    ///
+    /// In order to calculate the actual CPU frequency, we keep track of the CPU Tsc value (which
+    /// increases by 1 for every cycle) for a know period of time. The Metronome is not accurate
+    /// for the 1st tick, so I choose to wait for 1000 ticks, thus the error can be control to be
+    /// lower than 1%.
+    ///
+    TickCount = SAMPLE_TICK_COUNT;
+    CpuGetInterruptState (&gCpu, &InterruptState);
+    if (InterruptState) {
+      DisableInterrupt (&gCpu);
+    }
+    ///
+    /// In DxeCis-0.91 specs.
+    /// Metronome->WaitForTick is possible for interrupt processing,
+    /// or exception processing to interrupt the execution of the WaitForTick() function.
+    /// Depending on the hardware source for the ticks, it is possible for a tick to be missed.
+    /// This function cannot guarantee that ticks will not be missed.
+    ///
+    while (TRUE) {
+      BeginValue  = EfiReadTsc ();
+      Status      = Metronome->WaitForTick (Metronome, TickCount);
+      EndValue    = EfiReadTsc ();
+      if (!EFI_ERROR (Status)) {
+        TotalValue = EndValue - BeginValue;
+        break;
+      }
+    }
+
+    if (InterruptState) {
+      EnableInterrupt (&gCpu);
+    }
+
+    mCpuFrequency = MultU64x32 (TotalValue, 10);
+    mCpuFrequency = DivU64x32 (mCpuFrequency, Metronome->TickPeriod * TickCount);
+  }
+
+  *Frequency = mCpuFrequency;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump RC CPU and PPM platform policies
+**/
+VOID
+CpuDxePolicyDump (
+  VOID
+  )
+{
+#ifdef EFI_DEBUG
+  CPU_CONFIG        *CpuConfig;
+  POWER_MGMT_CONFIG *PowerMgmtConfig;
+  SECURITY_CONFIG   *SecurityConfig;
+
+  CpuConfig = mPlatformCpu->CpuConfig;
+  PowerMgmtConfig = mPlatformCpu->PowerMgmtConfig;
+  SecurityConfig = mPlatformCpu->SecurityConfig;
+  //
+  // Dump Cpu Platform policy
+  //
+  DEBUG ((EFI_D_INFO, "\n------------------------ DXE CpuPlatformPolicy Dump Begin -----------------\n"));
+  //
+  // Cpu config
+  //
+  DEBUG ((EFI_D_INFO, " CPU:: HtState : 0x%X \n", CpuConfig->HtState));
+  DEBUG ((EFI_D_INFO, " CPU:: LimitCpuidMaximumValue : 0x%X\n", CpuConfig->LimitCpuidMaximumValue));
+  DEBUG ((EFI_D_INFO, " CPU:: ExecuteDisableBit : 0x%X\n", CpuConfig->ExecuteDisableBit));
+  DEBUG ((EFI_D_INFO, " CPU:: VmxEnable : 0x%X\n", CpuConfig->VmxEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: SmxEnable : 0x%X\n", CpuConfig->SmxEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: MachineCheckEnable : 0x%X\n", CpuConfig->MachineCheckEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: MonitorMwaitEnable : 0x%X\n", CpuConfig->MonitorMwaitEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: XapicEnable : 0x%X\n", CpuConfig->XapicEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: IsColdReset : 0x%X\n", CpuConfig->IsColdReset));
+  DEBUG ((EFI_D_INFO, " CPU:: MlcStreamerPrefetcher : 0x%X\n", CpuConfig->MlcStreamerPrefetcher));
+  DEBUG ((EFI_D_INFO, " CPU:: EnableDts : 0x%X\n", CpuConfig->EnableDts));
+  DEBUG ((EFI_D_INFO, " CPU:: FviReport : 0x%X\n", CpuConfig->FviReport));
+  DEBUG ((EFI_D_INFO, " CPU:: AesEnable : 0x%X\n", CpuConfig->AesEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: DebugInterfaceEnable : 0x%X\n", CpuConfig->DebugInterfaceEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: DebugInterfaceLockEnable : 0x%X\n", CpuConfig->DebugInterfaceLockEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: ApIdleManner : 0x%X\n", CpuConfig->ApIdleManner));
+  DEBUG ((EFI_D_INFO, " CPU:: ApHandoffManner : 0x%X\n", CpuConfig->ApHandoffManner));
+  DEBUG ((EFI_D_INFO, " CPU:: BspSelection : 0x%X\n", CpuConfig->BspSelection));
+  DEBUG ((EFI_D_INFO, " CPU:: SmmbaseSwSmiNumber : 0x%X\n", CpuConfig->SmmbaseSwSmiNumber));
+  DEBUG ((EFI_D_INFO, " CPU:: FviSmbiosType; : 0x%X\n", CpuConfig->FviSmbiosType));
+  //
+  // SECURITY_CONFIG : TXT_FUNCTION_CONFIG
+  //
+  DEBUG ((EFI_D_INFO, " CPU:: SECURITY_CONFIG:TXT_FUNCTION_CONFIG : ResetAux : 0x%X\n", SecurityConfig->TxtFunctionConfig->ResetAux));
+  DEBUG ((EFI_D_INFO, "\n------------------------ DXE CpuPlatformPolicy Dump End -----------------\n"));
+#endif
+}
+
+/**
+  Initialize the global DataHub pointer
+
+  @param[in] DataHub   - Pointer to the DataHub protocol as output
+
+  @retval EFI_SUCCESS  - If the DataHub pointer is initialized successfully
+**/
+EFI_STATUS
+InitializeDataHubPtr (
+  OUT EFI_DATA_HUB_PROTOCOL **DataHub
+  )
+{
+  EFI_STATUS Status;
+
+  Status = gBS->LocateProtocol (&gEfiDataHubProtocolGuid, NULL, (VOID **) DataHub);
+  return Status;
+}
+
+/**
+  Drop into SMM to register IOTRAP for pfat tools interface
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+InitializePfatToolsIntCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS           Status;
+  VOID                 *ProtocolPointer;
+  EFI_PHYSICAL_ADDRESS BaseAddress;
+  PFAT_HOB             *PfatHobPtr;
+  EFI_GUID             PfatHobGuid = PFAT_HOB_GUID;
+
+  Status = gBS->LocateProtocol (&gExitPmAuthProtocolGuid, NULL, &ProtocolPointer);
+  if (EFI_SUCCESS != Status) {
+    return;
+  }
+
+  PfatHobPtr = GetFirstGuidHob (&PfatHobGuid);
+  if (PfatHobPtr != NULL) {
+    BaseAddress = (EFI_PHYSICAL_ADDRESS) PfatHobPtr->PfatToolsIntIoTrapAdd;
+    ///
+    /// IOTRAP TO SMM
+    ///
+    IoRead8 (BaseAddress);
+  }
+  return;
+}
+
+/**
+  Register callback function for Boot Guard revocation flow.
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+BootGuardRevocationCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *ProtocolPointer;
+
+  //
+  // Check whether this is real ExitPmAuth notification, or just a SignalEvent
+  //
+  Status = gBS->LocateProtocol (&gExitPmAuthProtocolGuid, NULL, (VOID **) &ProtocolPointer);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  BootGuardOemRevocationHook();
+  
+  //
+  // Closed the event to avoid call twice
+  //
+  gBS->CloseEvent (Event);
+
+  return;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeDbgr.h b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeDbgr.h
new file mode 100644
index 0000000..86316bb
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeDbgr.h
@@ -0,0 +1,602 @@
+/** @file
+  Private data structures
+
+@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 _CPU_INIT_DXE_H
+#define _CPU_INIT_DXE_H
+
+#include "CpuAccess.h"
+#include "MemoryAttribute.h"
+#include "RcFviDxeLib.h"
+#include "EdkIIGlueBaseLib.h"
+
+#include EFI_PROTOCOL_DEFINITION (CpuPlatformPolicy)
+#include EFI_PROTOCOL_CONSUMER (ExitPmAuth)
+#include EFI_PROTOCOL_DEFINITION (CpuInfo)
+
+#if (EFI_SPECIFICATION_VERSION >= 0x2000A)
+#include "UefiIfrLibrary.h"
+#endif
+
+#define INTERRUPT_VECTOR_NUMBER     256
+#define INTERRUPT_GATE_ATTRIBUTE    0x8e00
+#define NUMBER_OF_MICROCODE_UPDATE  10
+
+extern UINT8 mSmmbaseSwSmiNumber;
+
+//(AMI_CHG+)
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+
+#define AMI_DEBUGGER_CPU_PROTOCOL_GUID \
+  { 0xab21acc3, 0xba33, 0xee2c, 0x66, 0xbc, 0x12, 0x56, 0x77, 0x11, 0x1a, 0xb2 }
+  
+
+typedef struct _AMI_DEBUGGER_CPU_PROTOCOL AMI_DEBUGGER_CPU_PROTOCOL;  
+
+#pragma pack(1)
+
+typedef struct {
+  UINT16  Offset15To0;
+  UINT16  SegmentSelector;
+  UINT16  Attributes;
+  UINT16  Offset31To16;
+  UINT32  Offset63To32;
+  UINT32  Reserved;
+} DEBUGGER_INTERRUPT_GATE_DESCRIPTOR;
+
+#pragma pack()
+
+typedef
+EFI_STATUS
+(EFIAPI *DEBUGGER_GET_APTIO_INT_HANDLER) (
+    IN OUT UINT32*   InterruptHandlerHaltAddr  
+  );
+
+typedef
+EFI_STATUS
+(EFIAPI *DEBUGGER_FIXUP_PEI_EXCEPTION_HANDLER) (
+  IN DEBUGGER_INTERRUPT_GATE_DESCRIPTOR*   IdtEntry,
+  IN UINT32    i  
+  );
+  
+typedef
+EFI_STATUS
+(EFIAPI *DEBUGGER_SETUP_EXCEPTION_HANDLER) (VOID);
+
+typedef
+EFI_STATUS
+(EFIAPI *DEBUGGER_IS_DEBUGGER_IRQ_HANDLER) (
+  IN EFI_EXCEPTION_TYPE           InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER    InterruptHandler
+  );
+
+typedef struct _AMI_DEBUGGER_CPU_PROTOCOL {
+    DEBUGGER_GET_APTIO_INT_HANDLER       DebuggerGetAptioIntHandler;
+	DEBUGGER_FIXUP_PEI_EXCEPTION_HANDLER DebuggerFixUpPEIExceptionHandlers;
+	DEBUGGER_SETUP_EXCEPTION_HANDLER     DebuggerSetupExceptionHandler;
+	DEBUGGER_IS_DEBUGGER_IRQ_HANDLER     DebuggerIsDebuggerIrqHadler;	
+};
+  
+#endif
+//<(AMI_CHG+)
+
+#define CPU_FVI_STRING      "Reference Code - CPU"
+#define CPU_FVI_SMBIOS_TYPE 0xDD  ///< Default value
+#define CPU_FVI_SMBIOS_INSTANCE 0x02
+#define UCODE_FVI_STRING    "uCode Version"
+#define UCODE_VERSION \
+  { \
+    0xFF, 0xFF, 0xFF, 0xFFFF \
+  }
+#define TXT_FVI_STRING          "TXT ACM version"
+#define TXT_ACM_MAJOR_VERSION   0x1
+#define TXT_ACM_MINOR_VERSION   0x0
+#define TXT_ACM_REVERSION       0x0
+#define TXT_ACM_BUILD_NUMBER    0x0
+#define TXT_VERSION \
+  { \
+    TXT_ACM_MAJOR_VERSION, TXT_ACM_MINOR_VERSION, TXT_ACM_REVERSION, TXT_ACM_BUILD_NUMBER \
+  }
+enum {
+  CPU_RC_VER= 0,
+  UCODE_VER,
+  TXT_VER
+} CPU_FVI_INDEX;
+
+extern FVI_ELEMENT_AND_FUNCTION      mCpuFviElementsData[];
+extern FVI_DATA_HUB_CALLBACK_CONTEXT mCpuFviVersionData;
+extern UINTN                         mCpuFviElements;
+
+/**
+  Adjust length to a paragraph boundry
+
+  @param[in] MemoryLength  -  Input memory length.
+
+  @retval Returned Maximum length.
+**/
+UINT64
+Power2MaxMemory (
+  IN UINT64 MemoryLength
+  );
+
+/**
+  Disable cache and its mtrr
+
+  @param[in] OldMtrr - To return the Old MTRR value
+**/
+VOID
+EfiDisableCacheMtrr (
+  IN UINT64 *OldMtrr
+  );
+
+/**
+  Recover cache MTRR
+
+  @param[in] EnableMtrr - Whether to enable the MTRR
+  @param[in] OldMtrr    - The saved old MTRR value to restore when not to
+                          enable the MTRR
+**/
+VOID
+EfiRecoverCacheMtrr (
+  IN BOOLEAN EnableMtrr,
+  IN UINT64  OldMtrr
+  );
+
+typedef struct _ALIGNED_DWORD {
+  UINT32 High;
+  UINT32 Low;
+} ALIGNED_DWORD;
+
+typedef union _ALIGNED {
+  UINT64        AlignedQword;
+  ALIGNED_DWORD AlignedDword;
+} ALIGNED;
+
+/**
+  Initialize the state information for the CPU Architectural Protocol
+
+  @param[in] ImageHandle - Image handle of the loaded driver
+  @param[in] SystemTable - Pointer to the System Table
+
+  @retval EFI_SUCCESS           - thread can be successfully created
+  @retval EFI_OUT_OF_RESOURCES  - cannot allocate protocol data structure
+  @retval EFI_DEVICE_ERROR      - cannot create the thread
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpu (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  );
+
+/**
+  Prepare memory for essential system tables.
+
+  @retval EFI_SUCCESS  - Memory successfully prepared.
+**/
+EFI_STATUS
+PrepareMemory (
+  VOID
+  );
+
+/**
+  Flush CPU data cache. If the instruction cache is fully coherent
+  with all DMA operations then function can just return EFI_SUCCESS.
+
+  @param[in] This             - Protocol instance structure
+  @param[in] Start            - Physical address to start flushing from.
+  @param[in] Length           - Number of bytes to flush. Round up to chipset granularity.
+  @param[in] FlushType        - Specifies the type of flush operation to perform.
+
+  @retval EFI_SUCCESS         - If cache was flushed
+  @exception EFI_UNSUPPORTED  - If flush type is not supported.
+  @retval EFI_DEVICE_ERROR    - If requested range could not be flushed.
+**/
+EFI_STATUS
+EFIAPI
+FlushCpuDataCache (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                Length,
+  IN EFI_CPU_FLUSH_TYPE    FlushType
+  );
+
+/**
+  Enables CPU interrupts.
+
+  @param[in] This            - Protocol instance structure
+
+  @retval EFI_SUCCESS        - If interrupts were enabled in the CPU
+  @retval EFI_DEVICE_ERROR   - If interrupts could not be enabled on the CPU.
+**/
+EFI_STATUS
+EFIAPI
+EnableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  );
+
+/**
+  Disables CPU interrupts.
+
+  @param[in] This           - Protocol instance structure
+
+  @retval EFI_SUCCESS       - If interrupts were disabled in the CPU.
+  @retval EFI_DEVICE_ERROR  - If interrupts could not be disabled on the CPU.
+**/
+EFI_STATUS
+EFIAPI
+DisableInterrupt (
+  IN EFI_CPU_ARCH_PROTOCOL *This
+  );
+
+/**
+  Return the state of interrupts.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] State               - Pointer to the CPU's current interrupt state
+
+  @retval EFI_SUCCESS            - If interrupts were disabled in the CPU.
+  @retval EFI_INVALID_PARAMETER  - State is NULL.
+**/
+EFI_STATUS
+EFIAPI
+CpuGetInterruptState (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  OUT BOOLEAN              *State
+  );
+
+/**
+  Generates an INIT to the CPU
+
+  @param[in] This             - Protocol instance structure
+  @param[in] InitType         - Type of CPU INIT to perform
+
+  @retval EFI_SUCCESS         - If CPU INIT occurred. This value should never be seen.
+  @retval EFI_DEVICE_ERROR    - If CPU INIT failed.
+  @exception EFI_UNSUPPORTED  - Requested type of CPU INIT not supported.
+**/
+EFI_STATUS
+EFIAPI
+Init (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_CPU_INIT_TYPE     InitType
+  );
+
+/**
+  Registers a function to be called from the CPU interrupt handler.
+
+  @param[in] This                - Protocol instance structure
+  @param[in] InterruptType       - Defines which interrupt to hook.
+                                   IA-32 valid range is 0x00 through 0xFF
+  @param[in] InterruptHandler    - A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER
+                                   that is called when a processor interrupt occurs.
+                                   A null pointer is an error condition.
+
+  @retval EFI_SUCCESS            - If handler installed or uninstalled.
+  @retval EFI_ALREADY_STARTED    - InterruptHandler is not NULL, and a handler for
+                                   InterruptType was previously installed
+  @retval EFI_INVALID_PARAMETER  - InterruptHandler is NULL, and a handler for
+                                   InterruptType was not previously installed.
+  @exception EFI_UNSUPPORTED     - The interrupt specified by InterruptType is not supported.
+**/
+EFI_STATUS
+EFIAPI
+RegisterInterruptHandler (
+  IN EFI_CPU_ARCH_PROTOCOL     *This,
+  IN EFI_EXCEPTION_TYPE        InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER InterruptHandler
+  );
+
+/**
+  Returns a timer value from one of the CPU's internal timers. There is no
+  inherent time interval between ticks but is a function of the CPU frequency.
+
+  @param[in] This                - Protocol instance structure.
+  @param[in] TimerIndex          - Specifies which CPU timer is requested.
+  @param[in] TimerValue          - Pointer to the returned timer value.
+  @param[in] TimerPeriod         - A pointer to the amount of time that passes in femtoseconds (10-15) for each
+                                   increment of TimerValue. If TimerValue does not increment at a predictable
+                                   rate, then 0 is returned. The amount of time that has passed between two calls to
+                                   GetTimerValue() can be calculated with the formula
+                                   (TimerValue2 - TimerValue1) * TimerPeriod. This parameter is optional and may be NULL.
+
+  @retval EFI_SUCCESS            - If the CPU timer count was returned.
+  @exception EFI_UNSUPPORTED     - If the CPU does not have any readable timers.
+  @retval EFI_DEVICE_ERROR       - If an error occurred while reading the timer.
+  @retval EFI_INVALID_PARAMETER  - TimerIndex is not valid or TimerValue is NULL.
+**/
+EFI_STATUS
+EFIAPI
+GetTimerValue (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN UINT32                TimerIndex,
+  OUT UINT64               *TimerValue,
+  OUT UINT64 *TimerPeriod  OPTIONAL
+  );
+
+/**
+  Set memory cacheability attributes for given range of memeory
+
+  @param[in] This                - Protocol instance structure
+  @param[in] BaseAddress         - Specifies the start address of the memory range
+  @param[in] Length              - Specifies the length of the memory range
+  @param[in] Attributes          - The memory cacheability for the memory range
+
+  @retval EFI_SUCCESS            - If the cacheability of that memory range is set successfully
+  @exception EFI_UNSUPPORTED     - If the desired operation cannot be done
+  @retval EFI_INVALID_PARAMETER  - The input parameter is not correct, such as Length = 0
+**/
+EFI_STATUS
+EFIAPI
+SetMemoryAttributes (
+  IN EFI_CPU_ARCH_PROTOCOL *This,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Attributes
+  );
+
+/**
+  Init Global Descriptor table
+**/
+VOID
+InitializeSelectors (
+  VOID
+  );
+
+typedef struct {
+  VOID  *Start;
+  UINTN Size;
+  UINTN FixOffset;
+} INTERRUPT_HANDLER_TEMPLATE_MAP;
+
+typedef union {
+  EFI_CPU_DATA_RECORD *DataRecord;
+  UINT8               *Raw;
+} EFI_CPU_DATA_RECORD_BUFFER;
+
+///
+/// This constant defines the maximum length of the CPU brand string. According to the
+/// IA manual, the brand string is in EAX through EDX (thus 16 bytes) after executing
+/// the CPUID instructions with EAX as 80000002, 80000003, 80000004.
+///
+#define MAXIMUM_CPU_BRAND_STRING_LENGTH 48
+
+typedef struct {
+  BOOLEAN                    StringValid;
+  CHAR16                     BrandString[MAXIMUM_CPU_BRAND_STRING_LENGTH + 1];
+  EFI_PROCESSOR_VERSION_DATA StringRef;
+} PROCESSOR_VERSION_INFORMATION;
+
+///
+/// The constant defines how many times the Cpuid instruction should be executed
+/// in order to get all the cache information. For Pentium 4 processor, 1 is enough
+///
+#define CPU_CPUID_EXECTION_COUNT  2
+
+typedef struct {
+  UINT64                                 IntendCoreFrequency;
+  UINT64                                 IntendFsbFrequency;
+  PROCESSOR_VERSION_INFORMATION          Version;
+  EFI_PROCESSOR_MANUFACTURER_DATA        Manufacturer;
+  EFI_PROCESSOR_ID_DATA                  CpuidData;
+  EFI_PROCESSOR_FAMILY_DATA              Family;
+  INT16                                  Voltage;
+  EFI_PROCESSOR_APIC_BASE_ADDRESS_DATA   ApicBase;
+  EFI_PROCESSOR_APIC_ID_DATA             ApicID;
+  EFI_PROCESSOR_APIC_VERSION_NUMBER_DATA ApicVersion;
+  UINT32                                 MicrocodeRevision;
+  EFI_PROCESSOR_STATUS_DATA              Status; ///< Need to update this field before report
+  CPU_PHYSICAL_LOCATION                  Location;
+  EFI_MP_HEALTH_FLAGS                    Health;
+  EFI_CPUID_REGISTER                     CacheInformation[CPU_CPUID_EXECTION_COUNT];
+} CPU_DATA_FOR_DATAHUB;
+
+///
+/// Type, FamilyId and Model values for the different processors
+/// [0:7]   - Model
+/// [8:23]  - FamilyId
+/// [24:31] - Type
+///
+#define RESERVED  0x00
+
+/**
+  This will locate a processor microcode and if it finds a newer revision, it will
+  load it to the processor.
+
+  @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 EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  OUT UINT32                  *FailedRevision
+  );
+
+/**
+  To indicate the first microcode load is done
+**/
+VOID
+McuFirstLoadDone (
+  VOID
+  );
+
+/**
+  Initializes MP support in the system.
+
+  @retval EFI_SUCCESS           - Multiple processors are initialized successfully.
+  @retval EFI_NOT_FOUND         - The ACPI variable is not found in S3 boot path.
+  @retval EFI_OUT_OF_RESOURCES  - No enough resoruces (such as out of memory).
+**/
+EFI_STATUS
+InitializeMpSupport (
+  VOID
+  );
+
+/**
+  Save the MTRR registers to global variables
+**/
+VOID
+ReadMtrrRegisters (
+  VOID
+  );
+
+/**
+  Synch up the MTRR values for all processors
+**/
+VOID
+EFIAPI
+MpMtrrSynchUp (
+  IN VOID *Buffer
+  );
+
+/**
+  Copy Global MTRR data to S3
+**/
+VOID
+SaveBspMtrrForS3 (
+  VOID
+  );
+
+/**
+  Load all microcode updates to memory. Since in S3 resume boot path, CPUs should be
+  patched again, these microcode updates are copied to OS reserved memory.
+
+  @retval EFI_SUCCESS           - All microcode updates are loaded to memory successfully
+  @retval EFI_OUT_OF_RESOURCES  - Not enough memory to accomodate all the microcode updates
+**/
+EFI_STATUS
+LoadAllMicrocodeUpdates (
+  VOID
+  );
+
+/**
+  Returns the actual CPU core frequency in MHz.
+
+  @param[in] Metronome           - Metronome protocol
+  @param[in] Frequency           - Pointer to the CPU core frequency
+
+  @retval EFI_SUCCESS            - If the frequency is returned successfully
+  @retval EFI_INVALID_PARAMETER  - If the input parameter is wrong
+**/
+EFI_STATUS
+GetActualFrequency (
+  IN EFI_METRONOME_ARCH_PROTOCOL *Metronome,
+  OUT UINT64                     *Frequency
+  );
+
+/**
+  Dump RC CPU and PPM platform policies
+**/
+VOID
+CpuDxePolicyDump (
+  VOID
+  );
+
+/**
+  Initialize the global DataHub pointer
+
+  @param[in] DataHub   - Pointer to the DataHub protocol as output
+
+  @retval EFI_SUCCESS  - If the DataHub pointer is initialized successfully
+**/
+EFI_STATUS
+InitializeDataHubPtr (
+  OUT EFI_DATA_HUB_PROTOCOL **DataHub
+  );
+
+/**
+  Check if loading microcode update fails, if so, report proper status code
+
+  @param[in] CpuNumber       - The CPU number
+  @param[in] Status          - The return value of InitializeMicrocode()
+  @param[in] FailedRevision  - The revision of the microcode update that failed to be loaded
+
+  @retval EFI_SUCCESS        - The status is check and proper status code is reported
+**/
+EFI_STATUS
+CheckMicrocodeUpdate (
+  IN UINTN      CpuNumber,
+  IN EFI_STATUS Status,
+  IN UINT32     FailedRevision
+  );
+
+/**
+  Enable Cpu Interrupt
+**/
+VOID
+CpuEnableInterrupt (
+  VOID
+  );
+
+/**
+  Disable Cpu Interrupt
+**/
+VOID
+CpuDisableInterrupt (
+  VOID
+  );
+
+/**
+  Get code segment
+
+  @retval Code segmnet value
+**/
+UINT16
+GetCodeSegment (
+  VOID
+  );
+
+/**
+  Initialize Cpu float point unit
+**/
+VOID
+CpuInitFloatPointUnit (
+  VOID
+  );
+
+/**
+  Drop into SMM to register IOTRAP handler for pfat tools interface
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+InitializePfatToolsIntCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  Register callback function for Boot Guard revocation flow.
+
+  @param[in] Event     - A pointer to the Event that triggered the callback.
+  @param[in] Context   - A pointer to private data registered with the callback function.
+**/
+VOID
+EFIAPI
+BootGuardRevocationCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeStrings.uni b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeStrings.uni
new file mode 100644
index 0000000..9dee7a4
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/CpuInitDxeStrings.uni
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/Exception.h b/ReferenceCode/Haswell/CpuInit/Dxe/Exception.h
new file mode 100644
index 0000000..b8a1eac
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/Exception.h
@@ -0,0 +1,92 @@
+/** @file
+  IA32 Exception Includes.
+
+@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 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 _IA32EXCEPTION_H
+#define _IA32EXCEPTION_H
+
+//
+// Driver Consumed Protocol Prototypes
+//
+#include EFI_ARCH_PROTOCOL_DEFINITION (Cpu)
+
+#define INTERRUPT_HANDLER_DIVIDE_ZERO           0x00
+#define INTERRUPT_HANDLER_DEBUG                 0x01
+#define INTERRUPT_HANDLER_NMI                   0x02
+#define INTERRUPT_HANDLER_BREAKPOINT            0x03
+#define INTERRUPT_HANDLER_OVERFLOW              0x04
+#define INTERRUPT_HANDLER_BOUND                 0x05
+#define INTERRUPT_HANDLER_INVALID_OPCODE        0x06
+#define INTERRUPT_HANDLER_DEVICE_NOT_AVAILABLE  0x07
+#define INTERRUPT_HANDLER_DOUBLE_FAULT          0x08
+#define INTERRUPT_HANDLER_COPROCESSOR_OVERRUN   0x09
+#define INTERRUPT_HANDLER_INVALID_TSS           0x0A
+#define INTERRUPT_HANDLER_SEGMENT_NOT_PRESENT   0x0B
+#define INTERRUPT_HANDLER_STACK_SEGMENT_FAULT   0x0C
+#define INTERRUPT_HANDLER_GP_FAULT              0x0D
+#define INTERRUPT_HANDLER_PAGE_FAULT            0x0E
+#define INTERRUPT_HANDLER_RESERVED              0x0F
+#define INTERRUPT_HANDLER_MATH_FAULT            0x10
+#define INTERRUPT_HANDLER_ALIGNMENT_FAULT       0x11
+#define INTERRUPT_HANDLER_MACHINE_CHECK         0x12
+#define INTERRUPT_HANDLER_STREAMING_SIMD        0x13
+
+typedef struct {
+  EFI_STATUS_CODE_DATA Header;
+  union {
+    EFI_SYSTEM_CONTEXT_IA32 SystemContextIa32;
+    EFI_SYSTEM_CONTEXT_X64  SystemContextX64;
+  } SystemContext;
+} CPU_STATUS_CODE_TEMPLATE;
+
+/**
+  Common exception handler
+
+  @param[in] InterruptType - Exception type
+  @param[in] SystemContext - EFI_SYSTEM_CONTEXT
+**/
+VOID
+EFIAPI
+CommonExceptionHandler (
+  IN EFI_EXCEPTION_TYPE InterruptType,
+  IN EFI_SYSTEM_CONTEXT SystemContext
+  );
+
+/**
+  Install the IA-32 EM64T Exception Handler.
+  The current operation (which likely will change) will uninstall all the
+  pertinent exception handlers (0-7, 10-14, 16-19) except for Int8 which the timer
+  is currently sitting on (or soon will be).
+
+  It then installs all the appropriate handlers for each exception.
+
+  The handler then calls gRT->ReportStatusCode with a specific progress code.  The
+  progress codes for now start at 0x200 for IA-32 processors. See Status Code
+  Specification for details. The Status code Specification uses the enumeration from
+  the EFI 1.1 Debug Support Protocol.
+
+  @param[in] CpuProtocol - Instance of CPU Arch Protocol
+
+  @retval EFI_SUCCESS - This function always return success after registering handlers.
+**/
+EFI_STATUS
+InitializeException (
+  IN EFI_CPU_ARCH_PROTOCOL *CpuProtocol
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/Features.c b/ReferenceCode/Haswell/CpuInit/Dxe/Features.c
new file mode 100644
index 0000000..2561850
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/Features.c
@@ -0,0 +1,1093 @@
+/** @file
+  CPU feature control module
+
+@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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxe.h"
+#include "Features.h"
+#include "MachineCheck.h"
+#include "Mpcommon.h"
+#include "CpuPlatformLib.h"
+#endif
+
+extern DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu;
+
+UINT8 mFeatureLock;
+UINTN mCommonFeatures;
+UINTN mSetupFeatures;
+
+static UINT8              mLock;
+static LEAST_FEATURE_PROC mLeastFeatureProcessor;
+
+/**
+  Write 64bits MSR with script
+
+  @param[in] Index - MSR index that will be written
+  @param[in] Value - value written to MSR
+**/
+VOID
+AsmWriteMsr64WithScript (
+  IN UINT32 Index,
+  IN UINT64 Value
+  )
+{
+  AsmWriteMsr64 (Index, Value);
+  WriteMsr64ToScript (Index, Value);
+}
+
+/**
+  Write 64bits MSR to script
+
+  @param[in] Index - MSR index that will be written
+  @param[in] Value - value written to MSR
+**/
+VOID
+WriteMsr64ToScript (
+  IN UINT32 Index,
+  IN UINT64 Value
+  )
+{
+  UINTN TableIndex;
+
+  ASSERT (mMPSystemData != NULL);
+
+  ///
+  /// Save it into script
+  ///
+  AsmAcquireMPLock (&(mMPSystemData->S3BootScriptLock));
+  TableIndex = mMPSystemData->S3BootScriptCount++;
+  AsmReleaseMPLock (&(mMPSystemData->S3BootScriptLock));
+
+  ASSERT (TableIndex < MAX_CPU_S3_TABLE_SIZE - 1);
+  mMPSystemData->S3BootScriptTable[TableIndex].ApicId   = GetApicID (NULL, NULL);
+  mMPSystemData->S3BootScriptTable[TableIndex].MsrIndex = Index;
+  mMPSystemData->S3BootScriptTable[TableIndex].MsrValue = Value;
+}
+
+/**
+  Provide access to the CPU misc enables MSR
+
+  @param[in] Enable  - Enable or Disable Misc Features
+  @param[in] BitMask - The register bit offset of MSR MSR_IA32_MISC_ENABLE
+**/
+VOID
+CpuMiscEnable (
+  BOOLEAN Enable,
+  UINT64  BitMask
+  )
+{
+  UINT64 MsrValue;
+
+  MsrValue = AsmReadMsr64 (MSR_IA32_MISC_ENABLE);
+  if (Enable) {
+    MsrValue |= BitMask;
+  } else {
+    MsrValue &= ~BitMask;
+  }
+
+  AsmWriteMsr64 (MSR_IA32_MISC_ENABLE, MsrValue);
+}
+
+/**
+  Calculate how many bits are one from given number
+
+  @param[in] Value - number that will be calculated bits
+
+  @retval Number of bits
+**/
+UINT32
+GetBitsNumberOfOne (
+  UINT32 Value
+  )
+{
+  UINT32 Result;
+
+  Result = 0;
+  while (Value) {
+    if (Value & 1) {
+      Result++;
+    }
+    Value >>= 1;
+  }
+  return Result;
+}
+
+///
+/// DCA contains processor code and chipset code
+/// CPU driver has the following assumption on the initialization flow
+/// 1. Chipset pre-initialization should detect DCA support per chipset capability after CpuPlatformPolicy
+/// 2. If not support, it should update CpuPlatformPolicy DCA to disable state
+/// 3. If support, it should enable the DCA related registers
+/// 4. CPU initialization for DCA (CPU may change CpuPlatformPolicy DCA states per CPU capability)
+/// 5. Normal chipset driver (IOH) should look at CpuPlatformPolicy DCA policy again in PCI enumeratoin
+/// 6. Chipset enable or disable DCA according to CpuPlatformPolicy DCA state
+///
+/**
+  Detect DCA supported or not
+
+  @retval DCA_SUPPORT if supported or 0 if not supported
+**/
+UINTN
+IsDcaSupported (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+  BOOLEAN            Support;
+
+  Support = 0;
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  if ((CpuidRegisters.RegEcx & B_CPUID_VERSION_INFO_ECX_DCA) != 0) {
+    ///
+    /// Execute Disable Bit feature is not supported on this processor.
+    ///
+    Support = DCA_SUPPORT;
+  }
+  return Support;
+}
+
+/**
+  Detect HT supported or not
+
+  @retval HT_SUPPORT if supported or 0 if not supported
+**/
+UINTN
+IsHTSupported (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+  UINTN              Support;
+
+  Support = 0;
+
+  AsmCpuidEx (
+          CPUID_CORE_TOPOLOGY,
+          0,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  if ((CpuidRegisters.RegEbx & 0x00FF) == 2) {
+    Support = HT_SUPPORT;
+  }
+  return Support;
+
+}
+
+/**
+  Detect if AES supported or not
+
+  @retval AES_SUPPORT if supported or 0 if not supported
+**/
+UINTN
+IsAesSupported (
+  VOID
+  )
+{
+
+  EFI_CPUID_REGISTER CpuidRegisters;
+  UINTN              Support;
+
+  Support = 0;
+  EfiCpuid (CPUID_VERSION_INFO, &CpuidRegisters);
+  if ((CpuidRegisters.RegEcx & B_CPUID_VERSION_INFO_ECX_AES) != 0) {
+    Support = AES_SUPPORT;
+  }
+  return Support;
+}
+
+/**
+  Detect if XD supported or not
+
+  @retval XD_SUPPORT if supported or 0 if not supported
+**/
+UINTN
+IsXdSupported (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+  BOOLEAN            Support;
+
+  Support = 0;
+  AsmCpuid (
+          CPUID_EXTENDED_FUNCTION,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  if (CpuidRegisters.RegEax > CPUID_EXTENDED_FUNCTION) {
+    AsmCpuid (
+            CPUID_EXTENDED_CPU_SIG,
+            &CpuidRegisters.RegEax,
+            &CpuidRegisters.RegEbx,
+            &CpuidRegisters.RegEcx,
+            &CpuidRegisters.RegEdx
+            );
+    if ((CpuidRegisters.RegEdx & B_CPUID_VERSION_INFO_EDX_XD) != 0) {
+      ///
+      /// Execute Disable Bit feature is not supported on this processor.
+      ///
+      Support = XD_SUPPORT;
+    }
+  }
+  return Support;
+}
+
+/**
+  Enable XD if supported or disable it if not supported
+
+  @param[in] Support  - bitmap that indicate XD supported or not
+**/
+VOID
+EnableDisableXd (
+  IN UINTN Support
+  )
+{
+  BOOLEAN XdSupport;
+
+  if ((mCommonFeatures & XD_SUPPORT) == 0) {
+    return;
+  }
+  XdSupport = (BOOLEAN)((Support & XD_SUPPORT) == XD_SUPPORT);
+  ///
+  /// MSR MISC_ENABLE[34] has negative logic: 0 - XD Enabled, 1 - XD Disabled
+  ///
+  CpuMiscEnable (!XdSupport, B_MSR_IA32_MISC_ENABLE_XD);
+}
+
+/**
+  Check on the processor if VMX/TXT is supported.
+
+  @retval VMX_SUPPORT and TXT_SUPPORT if supported or 0 if not supported
+**/
+UINTN
+IsVmxSupported (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER CpuIdRegister;
+  UINTN              Support;
+
+  Support = 0;
+
+  ///
+  /// Get CPUID to check if the processor supports Vanderpool Technology.
+  ///
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuIdRegister.RegEax,
+          &CpuIdRegister.RegEbx,
+          &CpuIdRegister.RegEcx,
+          &CpuIdRegister.RegEdx
+          );
+  if ((CpuIdRegister.RegEcx & B_CPUID_VERSION_INFO_ECX_VME) != 0) {
+    ///
+    /// VT is supported.
+    ///
+    Support |= VMX_SUPPORT;
+  }
+  if ((CpuIdRegister.RegEcx & B_CPUID_VERSION_INFO_ECX_SME) != 0) {
+    ///
+    /// TXT is supported.
+    ///
+    Support |= TXT_SUPPORT;
+  }
+  return Support;
+}
+
+/**
+  Enable VMX/TXT on the processor.
+
+  @param[in] Support  - To enable or disable VMX/TXT feature.
+**/
+VOID
+EnableDisableVmx (
+  IN UINTN Support
+  )
+{
+  UINT64 Ia32FeatCtrl;
+  UINT64 NewFeatCtrl;
+
+  Ia32FeatCtrl = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
+  Ia32FeatCtrl &= ~((UINT64) OPTION_FEATURE_RESERVED_MASK);
+
+  NewFeatCtrl = Ia32FeatCtrl;
+
+  ///
+  /// If Vmx is Disabled, Enable it.
+  ///
+  if (mCommonFeatures & VMX_SUPPORT) {
+    if (Support & VMX_SUPPORT) {
+      NewFeatCtrl |= B_MSR_IA32_FEATURE_CONTROL_EVT;
+    } else {
+      NewFeatCtrl &= ~B_MSR_IA32_FEATURE_CONTROL_EVT;
+    }
+  }
+  if (mCommonFeatures & TXT_SUPPORT) {
+    if (Support & TXT_SUPPORT) {
+      ///
+      /// MSR Lock will be done in later.
+      ///
+      NewFeatCtrl |= (B_MSR_IA32_FEATURE_CONTROL_SLFE | B_MSR_IA32_FEATURE_CONTROL_SGE);
+      if (mCommonFeatures & VMX_SUPPORT) {
+        ///
+        /// Bit [1] can only be set if CPU is both VMX and TXT capable
+        ///
+        NewFeatCtrl |= B_MSR_IA32_FEATURE_CONTROL_ELT;
+      }
+    } else {
+      NewFeatCtrl &= ~(B_MSR_IA32_FEATURE_CONTROL_ELT | B_MSR_IA32_FEATURE_CONTROL_SLFE | B_MSR_IA32_FEATURE_CONTROL_SGE);
+    }
+  }
+
+  ///
+  /// Check the Feature Lock Bit.
+  /// If it is already set, which indicates we are executing POST
+  /// due to a warm RESET (i.e., PWRGOOD was not de-asserted).
+  ///
+  if ((Ia32FeatCtrl & B_MSR_IA32_FEATURE_CONTROL_LOCK) == 0) {
+    AsmWriteMsr64WithScript (MSR_IA32_FEATURE_CONTROL, NewFeatCtrl);
+  } else {
+    ///
+    /// if Lock bit is set
+    ///
+    NewFeatCtrl &= ~(B_MSR_IA32_FEATURE_CONTROL_LOCK);
+    WriteMsr64ToScript (MSR_IA32_FEATURE_CONTROL, NewFeatCtrl);
+  }
+}
+
+/**
+  Enable / Disable AES on the processor.
+
+  @param[in] Support  - To enable or disable AES feature.
+**/
+VOID
+EnableDisableAes (
+  IN UINTN Support
+  )
+{
+  UINT64 MsrValue;
+
+  if (!(mCommonFeatures & AES_SUPPORT) || (IsSecondaryThread ())) {
+    return;
+  }
+
+  ///
+  /// The processor was manufacted with AES-NI feature
+  ///
+  MsrValue = AsmReadMsr64 (MSR_IA32_FEATURE_CONFIG);
+
+  ///
+  /// Check the Feature Lock Bit.
+  /// If it is already set, which indicates we are executing POST
+  /// due to a warm RESET (i.e., PWRGOOD was not de-asserted).
+  ///
+  if ((MsrValue & B_IA32_FEATURE_CONFIG_LOCK) == 0) {
+    if (Support & AES_SUPPORT) {
+      ///
+      /// Enabled AES, writes of 00b, 01b pr 10b to the MSR will result in AES enable.
+      /// Should lock this MSR always, so write 01b to the MSR.
+      ///
+      MsrValue &= ~B_IA32_FEATURE_CONFIG_AES_DIS;
+      MsrValue |= B_IA32_FEATURE_CONFIG_LOCK;
+    } else {
+      ///
+      /// To disable AES, system BIOS must write 11b to this MSR.
+      ///
+      MsrValue |= (B_IA32_FEATURE_CONFIG_AES_DIS + B_IA32_FEATURE_CONFIG_LOCK);
+    }
+    AsmWriteMsr64WithScript (MSR_IA32_FEATURE_CONFIG, MsrValue);
+  }
+  return;
+}
+
+/**
+  Check on the processor if Debug Interface is supported.
+
+  @retval DEBUG_SUPPORT and DEBUG_LOCK_SUPPORT if supported or 0 if not supported
+**/
+UINTN
+IsDebugInterfaceSupported (
+  VOID
+  )
+{
+  UINTN              Support;
+  EFI_CPUID_REGISTER CpuIdRegister;
+
+  Support = 0;
+
+  ///
+  /// 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
+  ///
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuIdRegister.RegEax,
+          &CpuIdRegister.RegEbx,
+          &CpuIdRegister.RegEcx,
+          &CpuIdRegister.RegEdx
+          );
+  switch (CpuIdRegister.RegEax) {
+    case (EnumCpuHsw + EnumHswA0):
+      //DEBUG ((EFI_D_INFO,"MSR_IA32_DEBUG_INTERFACE is not supported on Ax CPU stepping\n")); (AMI_CHG+)
+      break;
+    case (EnumCpuHsw    + EnumHswB0):
+    case (EnumCpuHswUlt + EnumHswUltB0):
+    case (EnumCpuCrw    + EnumCrwB0):
+      Support |= DEBUG_SUPPORT;
+      Support |= DEBUG_LOCK_SUPPORT;
+      break;
+    default:
+      if (CpuIdRegister.RegEcx & BIT11) {
+        Support |= DEBUG_SUPPORT;
+        Support |= DEBUG_LOCK_SUPPORT;
+      }
+      break;
+  }
+  return Support;
+}
+
+/**
+  Enable/Disable Debug Interfaces in the processor.
+
+  @param[in] Support  - To enable or disable Debug Interface feature.
+**/
+VOID
+EnableDisableDebugInterface (
+  IN UINTN Support
+  )
+{
+  UINT64  Ia32DebugInterface;
+  BOOLEAN IsBsp;
+
+  ///
+  /// IA32_DEBUG_INTERFACE_MSR scope is "Package", program on BSP only
+  ///
+  IsBsp = (AsmReadMsr64 (MSR_IA32_APIC_BASE) & B_MSR_IA32_APIC_BASE_BSP) ? TRUE : FALSE;
+  if (!(mCommonFeatures & DEBUG_SUPPORT) || (IsBsp == FALSE)) {
+    return;
+  }
+
+  ///
+  /// Check if the processor supports debug interface
+  ///
+  if (IsDebugInterfaceSupported()) {
+    Ia32DebugInterface = AsmReadMsr64 (MSR_IA32_DEBUG_INTERFACE);
+    if (!(Ia32DebugInterface &  B_DEBUG_INTERFACE_LOCK)) {
+      if (Support & DEBUG_SUPPORT) {
+        ///
+        /// Enable Debug Interface (MSR 0xC80.Bit0 = 1)
+        ///
+        Ia32DebugInterface |= B_DEBUG_INTERFACE_ENABLE;
+        DEBUG ((EFI_D_ERROR, "Enable MSR_IA32_DEBUG_INTERFACE\n"));
+      } else {
+        ///
+        /// Disable Debug Interface (MSR 0xC80.Bit0 = 0)
+        ///
+        Ia32DebugInterface &= ~B_DEBUG_INTERFACE_ENABLE;
+        DEBUG ((EFI_D_ERROR, "Disable MSR_IA32_DEBUG_INTERFACE\n"));
+      }
+      if (Support & DEBUG_LOCK_SUPPORT) {
+        Ia32DebugInterface &= ~B_DEBUG_INTERFACE_LOCK;
+      }
+      DEBUG ((EFI_D_ERROR, "Set MSR_IA32_DEBUG_INTERFACE to %x\n",Ia32DebugInterface));
+      AsmWriteMsr64WithScript (MSR_IA32_DEBUG_INTERFACE, Ia32DebugInterface);
+    }
+  }
+  return;
+}
+
+/**
+  Lock VMX/TXT feature bits on the processor.
+  Set "CFG Lock" (MSR 0E2h Bit[15]
+
+  @param[in] LockFeatureEnable - TRUE to lock these feature bits and FALSE to not lock
+**/
+VOID
+LockFeatureBit (
+  IN BOOLEAN LockFeatureEnable
+  )
+{
+  UINT64 Ia32FeatCtrl;
+  UINT32 MsrValue;
+
+  if (!LockFeatureEnable) {
+    return;
+  }
+
+  ///
+  /// MSR 3Ah for VMX/TXT Lock
+  ///
+  Ia32FeatCtrl = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
+  Ia32FeatCtrl &= ~((UINT64) OPTION_FEATURE_RESERVED_MASK);
+
+  if ((Ia32FeatCtrl & B_MSR_IA32_FEATURE_CONTROL_LOCK) == 0) {
+    ///
+    /// Set Feature Lock bits.
+    ///
+    Ia32FeatCtrl |= B_MSR_IA32_FEATURE_CONTROL_LOCK;
+    AsmWriteMsr64WithScript (MSR_IA32_FEATURE_CONTROL, Ia32FeatCtrl);
+  } else {
+    WriteMsr64ToScript (MSR_IA32_FEATURE_CONTROL, Ia32FeatCtrl);
+  }
+
+  MsrValue = GetCsrDesiredCores ();
+  if ((MsrValue & BIT16) == 0) {
+    ///
+    /// Set Lock
+    ///
+    SetLockCsrDesiredCores ();
+  }
+  return;
+}
+
+/**
+  Detect if X2APIC supported or not
+
+  @retval XAPIC_SUPPORT if supported or 0 if not supported
+**/
+UINTN
+IsXapicSupported (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+  UINTN              Support;
+  UINT64             MsrValue;
+
+  Support   = 0;
+
+  MsrValue  = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  if ((CpuidRegisters.RegEcx & B_CPUID_VERSION_INFO_ECX_XAPIC) != 0) {
+    if (MsrValue & B_MSR_IA32_APIC_BASE_G_XAPIC) {
+      ///
+      /// XAPIC Mode feature is supported on this processor.
+      ///
+      Support = XAPIC_SUPPORT;
+    }
+  }
+  return Support;
+}
+
+/**
+  Enable / Disable X2APIC on the processor.
+
+  @param[in] Support  - To enable or disable X2APIC feature.
+**/
+VOID
+EnableDisableXAPIC (
+  IN UINTN Support
+  )
+{
+  UINT64 MsrValue;
+
+  if (!(mCommonFeatures & XAPIC_SUPPORT)) {
+    return;
+  }
+  MsrValue = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  if (Support & XAPIC_SUPPORT) {
+    MsrValue |= B_MSR_IA32_APIC_BASE_M_XAPIC;
+  } else {
+    MsrValue &= ~B_MSR_IA32_APIC_BASE_G_XAPIC;
+    MsrValue &= ~B_MSR_IA32_APIC_BASE_M_XAPIC;
+    AsmWriteMsr64 (MSR_IA32_APIC_BASE, MsrValue);
+    MsrValue |= B_MSR_IA32_APIC_BASE_G_XAPIC;
+  }
+  AsmWriteMsr64 (MSR_IA32_APIC_BASE, MsrValue);
+}
+
+/**
+  Initialize other processor functions (TPR messaging, floating point)
+**/
+VOID
+InitializeMiscProcessorFunctions (
+  VOID
+  )
+{
+  UINT64 MsrValue;
+
+  ///
+  /// Enable TPR Update messages,if supported (see section 2.5)
+  ///
+  MsrValue = AsmReadMsr64 (PIC_THREAD_CONTROL);
+  MsrValue &= (~B_PIC_THREAD_CONTROL_TPR_DIS);
+  AsmWriteMsr64WithScript (PIC_THREAD_CONTROL, MsrValue);
+
+  ///
+  /// Enable the Save Floating Point feature on every logical processors in the
+  /// platform when available. The BIOS must verify the SMM SAVE CONTROL capability
+  /// bit is set to 1 in PLATFORM_INFO MSR CEh [16] (see Section 2.16) before setting
+  /// the SMM_SAVE_CONTROL MSR 3Eh [0] to 1.
+  ///
+  MsrValue = AsmReadMsr64 (MSR_PLATFORM_INFO);
+  if ((MsrValue & B_PLATFORM_INFO_SMM_SAVE_CONTROL) != 0) {
+    MsrValue = AsmReadMsr64 (MSR_IA32_SMM_SAVE_CONTROL);
+    MsrValue |= B_MSR_IA32_SMM_SAVE_CONTROL_SFPPE;
+    AsmWriteMsr64WithScript (MSR_IA32_SMM_SAVE_CONTROL, MsrValue);
+  }
+
+}
+
+/**
+  Create feature control structure which will be used to program each feature on each core.
+
+  @param[in] MPSystemData - MP_SYSTEM_DATA global variable that contains platform policy protocol settings of each features.
+**/
+VOID
+InitializeFeaturePerSetup (
+  IN MP_SYSTEM_DATA *MPSystemData
+  )
+{
+  mFeatureLock    = VacantFlag;
+  mCommonFeatures = (UINTN) -1;
+  mSetupFeatures  = (UINTN) -1;
+
+  if (!MPSystemData->VmxEnable) {
+    mSetupFeatures &= ~VMX_SUPPORT;
+  }
+  if (!MPSystemData->TxtEnable) {
+    mSetupFeatures &= ~TXT_SUPPORT;
+  }
+  if (!MPSystemData->ExecuteDisableBit) {
+    mSetupFeatures &= ~XD_SUPPORT;
+  }
+  if (!MPSystemData->XapicEnable) {
+    mSetupFeatures &= ~XAPIC_SUPPORT;
+  }
+  if (!MPSystemData->AesEnable) {
+    mSetupFeatures &= ~AES_SUPPORT;
+  }
+  if (!MPSystemData->DebugInterfaceEnable) {
+    mSetupFeatures &= ~DEBUG_SUPPORT;
+  }
+  if (!MPSystemData->DebugInterfaceLockEnable) {
+    mSetupFeatures &= ~DEBUG_LOCK_SUPPORT;
+  }
+}
+
+/**
+  Detect each processor feature and log all supported features
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+**/
+VOID
+CollectProcessorFeature (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  )
+{
+  UINTN Support;
+
+  Support = 0;
+  Support |= IsXdSupported ();
+  Support |= IsVmxSupported ();
+  Support |= IsDcaSupported ();
+  Support |= IsAesSupported ();
+  Support |= IsXapicSupported ();
+  Support |= IsHTSupported ();
+  Support |= IsDebugInterfaceSupported ();
+
+  AsmAcquireMPLock (&mFeatureLock);
+  mCommonFeatures &= Support;
+  AsmReleaseMPLock (&mFeatureLock);
+
+  return;
+}
+
+/**
+  Program all processor features basing on desired settings
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+**/
+VOID
+ProgramProcessorFeature (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  )
+{
+  UINTN   Supported;
+  BOOLEAN IsBsp;
+
+  Supported = mCommonFeatures & mSetupFeatures;
+
+  ///
+  /// Configure features such as Xd, Vmx, Smx, XAPIC, AES, DebugInterface
+  ///
+  EnableDisableXd (Supported);
+  EnableDisableVmx (Supported);
+  EnableDisableXAPIC (Supported);
+  EnableDisableAes (Supported);
+  EnableDisableDebugInterface (Supported);
+
+  ///
+  /// Lock Proceesor Features
+  /// Make sure all CPU Features configuration are set before lock
+  ///
+  LockFeatureBit (1);
+
+  ///
+  /// Programe XApic register
+  ///
+  IsBsp = (AsmReadMsr64 (MSR_IA32_APIC_BASE) & B_MSR_IA32_APIC_BASE_BSP) ? TRUE : FALSE;
+  ProgramXApic (IsBsp);
+
+  ///
+  /// Initialize MonitorMWait register
+  ///
+  CpuMiscEnable (mMPSystemData->MonitorMwaitEnable, B_MSR_IA32_MISC_ENABLE_MONITOR);
+
+  ///
+  /// Initialize Machine Check registers
+  ///
+  InitializeMachineCheckRegisters (NULL, mMPSystemData->MachineCheckEnable);
+
+  //
+  // Misc functions
+  //
+  InitializeMiscProcessorFunctions();
+
+  return;
+}
+
+/**
+  Program CPUID Limit before booting to OS
+
+  @param[in] MpServices  - MP Services Protocol entry
+**/
+VOID
+ProgramCpuidLimit (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  )
+{
+  UINT64 MsrValue;
+
+  ///
+  /// Move Limit CPUID Maxval configuration here to not impact the BOOT
+  /// After setting this, no code can execute CPUID function > 3.
+  ///
+  CpuMiscEnable (mMPSystemData->LimitCpuidMaximumValue, B_MSR_IA32_MISC_ENABLE_CPUID_MAX);
+
+  ///
+  /// Finally record the MISC MSR into CPU S3 script table
+  /// to avoid access for multiple times
+  ///
+  MsrValue = AsmReadMsr64 (MSR_IA32_MISC_ENABLE);
+  AsmWriteMsr64WithScript (MSR_IA32_MISC_ENABLE, MsrValue);
+
+  return;
+}
+
+/**
+  Initialize prefetcher settings
+
+  @param[in] MlcStreamerprefecterEnabled - Enable/Disable MLC streamer prefetcher
+  @param[in] MlcSpatialPrefetcherEnabled - Enable/Disable MLC spatial prefetcher
+**/
+VOID
+InitializeProcessorsPrefetcher (
+  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;
+}
+
+/**
+  Get processor feature
+
+  @param[in] Features  - pointer to a buffer which stores feature information
+**/
+VOID
+GetProcessorFeatures (
+  IN UINT32 *Features
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  Features[0] = CpuidRegisters.RegEcx;
+  Features[1] = CpuidRegisters.RegEdx;
+  AsmCpuid (
+          CPUID_EXTENDED_CPU_SIG,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  Features[2] = CpuidRegisters.RegEax;
+  Features[3] = CpuidRegisters.RegEbx;
+  Features[4] = CpuidRegisters.RegEcx;
+  Features[5] = CpuidRegisters.RegEdx;
+
+  return;
+}
+
+/**
+  Update some processor info into LEAST_FEATURE_PROC data structure.
+
+  @param[in] Index                 - indicate which processor calling this routine
+  @param[in] LeastFeatureProcessor - the data structure that will be updated
+**/
+VOID
+UpdateProcessorInfo (
+  IN UINTN              Index,
+  IN LEAST_FEATURE_PROC *LeastFeatureProcessor
+  )
+{
+  UINT16 FamilyId;
+  UINT8  Model;
+  UINT8  SteppingId;
+
+  EfiCpuVersion (&FamilyId, &Model, &SteppingId, NULL);
+  LeastFeatureProcessor->Index    = Index;
+  LeastFeatureProcessor->ApicId   = GetApicID (NULL, NULL);
+  LeastFeatureProcessor->Version  = EfiMakeCpuVersion (FamilyId, Model, SteppingId);
+}
+
+/**
+  Get processor feature delta
+
+  @param[in] FeaturesInput   - Supported features for input processor
+  @param[in] CommonFeatures  - Supported features for processor (subset of FeaturesInput)
+
+  @retval The least of processor features
+**/
+UINT32
+GetProcessorFeatureDelta (
+  IN UINT32 *FeaturesInput,
+  IN UINT32 *CommonFeatures
+  )
+{
+  UINT32 Delta;
+  UINTN  Index;
+
+  ///
+  /// CommonFeatures is the subset of FeaturesInput
+  ///
+  Delta = 0;
+  for (Index = 0; Index < MAX_FEATURE_NUM; Index++) {
+    Delta += GetBitsNumberOfOne (FeaturesInput[Index] - CommonFeatures[Index]);
+  }
+
+  return 0;
+}
+
+/**
+  Find out the common features supported by all core/threads
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+**/
+VOID
+GetProcessorCommonFeature (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  )
+{
+  UINTN  Index;
+  UINT32 Features[MAX_FEATURE_NUM];
+
+  GetProcessorFeatures (Features);
+
+  AsmAcquireMPLock (&mLock);
+  for (Index = 0; Index < MAX_FEATURE_NUM; Index++) {
+    mLeastFeatureProcessor.Features[Index] &= Features[Index];
+  }
+  AsmReleaseMPLock (&mLock);
+  return;
+}
+
+/**
+  Get the processor data with least features
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+**/
+VOID
+GetProcessorWithLeastFeature (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  )
+{
+  EFI_STATUS         Status;
+  UINTN              CurrentProcessor;
+  LEAST_FEATURE_PROC LeastFeatureProcessor;
+
+  Status = MpServices->WhoAmI (
+                  MpServices,
+                  &CurrentProcessor
+                  );
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  GetProcessorFeatures (LeastFeatureProcessor.Features);
+  LeastFeatureProcessor.FeatureDelta = GetProcessorFeatureDelta (
+                  LeastFeatureProcessor.Features,
+                  mLeastFeatureProcessor.Features
+                  );
+
+  AsmAcquireMPLock (&mLock);
+  if (LeastFeatureProcessor.FeatureDelta < mLeastFeatureProcessor.FeatureDelta) {
+    mLeastFeatureProcessor.FeatureDelta = LeastFeatureProcessor.FeatureDelta;
+    UpdateProcessorInfo (CurrentProcessor, &mLeastFeatureProcessor);
+  } else if (LeastFeatureProcessor.FeatureDelta == mLeastFeatureProcessor.FeatureDelta) {
+    UpdateProcessorInfo (CurrentProcessor, &LeastFeatureProcessor);
+    if (LeastFeatureProcessor.Version < mLeastFeatureProcessor.Version) {
+      UpdateProcessorInfo (CurrentProcessor, &mLeastFeatureProcessor);
+    } else if (LeastFeatureProcessor.Version == mLeastFeatureProcessor.Version) {
+      if (LeastFeatureProcessor.ApicId < mLeastFeatureProcessor.ApicId) {
+        UpdateProcessorInfo (CurrentProcessor, &mLeastFeatureProcessor);
+      }
+    }
+  }
+
+  AsmReleaseMPLock (&mLock);
+
+  return;
+}
+
+/**
+  Switch BSP to the processor which has least features
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+
+  @retval EFI_STATUS  - status code returned from each sub-routines
+**/
+EFI_STATUS
+SwitchToLowestFeatureProcess (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  )
+{
+  EFI_STATUS Status;
+  UINTN      CurrentProcessor;
+  UINTN      NewBsp;
+  UINT32     Features[MAX_FEATURE_NUM];
+
+  Status = MpServices->WhoAmI (
+                  MpServices,
+                  &CurrentProcessor
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ReportStatusCode (
+          EFI_PROGRESS_CODE,
+          EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_HP_PC_BSP_SELECT
+          );
+
+  ///
+  /// Take current BSP as the least feature
+  ///
+  UpdateProcessorInfo (CurrentProcessor, &mLeastFeatureProcessor);
+  GetProcessorFeatures (mLeastFeatureProcessor.Features);
+  CopyMem (Features, mLeastFeatureProcessor.Features, sizeof (Features));
+  Status = MpServices->StartupAllAPs (
+                  MpServices,
+                  GetProcessorCommonFeature,
+                  FALSE,
+                  NULL,
+                  100000,
+                  (VOID *) MpServices,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Go through all processors to find the processor with least features
+  ///
+  mLeastFeatureProcessor.FeatureDelta = GetProcessorFeatureDelta (Features, mLeastFeatureProcessor.Features);
+  Status = MpServices->StartupAllAPs (
+                  MpServices,
+                  GetProcessorWithLeastFeature,
+                  FALSE,
+                  NULL,
+                  100000,
+                  (VOID *) MpServices,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Switch BSP according
+  ///
+  if (mPlatformCpu->CpuConfig->BspSelection == 16) {
+    ///
+    /// Enable least feature SBSP selection
+    ///
+    NewBsp = mLeastFeatureProcessor.Index;
+  } else {
+    ///
+    /// Do not change the current BSP
+    /// made by SEC
+    ///
+    NewBsp = CurrentProcessor;
+  }
+
+  if (NewBsp != CurrentProcessor) {
+
+    DEBUG ((EFI_D_INFO, "Switch BSP from %d ==> %d\n", CurrentProcessor, NewBsp));
+    Status = MpServices->SwitchBSP (
+                    MpServices,
+                    NewBsp,
+                    TRUE
+                    );
+  }
+
+  return Status;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/Features.h b/ReferenceCode/Haswell/CpuInit/Dxe/Features.h
new file mode 100644
index 0000000..207ddde
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/Features.h
@@ -0,0 +1,145 @@
+/** @file
+  Header file of CPU feature control module
+
+@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 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 _FEATURES_H_
+#define _FEATURES_H_
+
+#include "CpuInitDxe.h"
+#include "MpService.h"
+
+#define OPTION_FEATURE_RESERVED_MASK        0xFFFF00F8  ///< bits 30:16,  7:3
+#define OPTION_FEATURE_CONFIG_RESERVED_MASK 0xFFFFFFFC  ///< bits 2:31
+/**
+  Create feature control structure which will be used to program each feature on each core.
+
+  @param[in] MPSystemData - MP_SYSTEM_DATA global variable that contains platform policy protocol settings of each features.
+**/
+VOID
+InitializeFeaturePerSetup (
+  IN MP_SYSTEM_DATA *MPSystemData
+  );
+
+/**
+  Program all processor features basing on desired settings
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+**/
+VOID
+ProgramProcessorFeature (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  );
+
+/**
+  Program CPUID Limit before booting to OS
+
+  @param[in] MpServices  - MP Services Protocol entry
+**/
+VOID
+ProgramCpuidLimit (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  );
+
+/**
+  Initialize prefetcher settings
+
+  @param[in] MlcStreamerprefecterEnabled - Enable/Disable MLC streamer prefetcher
+  @param[in] MlcSpatialPrefetcherEnabled - Enable/Disable MLC spatial prefetcher
+**/
+VOID
+InitializeProcessorsPrefetcher (
+  IN UINTN MlcStreamerprefecterEnabled,
+  IN UINTN MlcSpatialPrefetcherEnabled
+  );
+
+/**
+  Detect each processor feature and log all supported features
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+**/
+VOID
+CollectProcessorFeature (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  );
+
+/**
+  Lock VMX/TXT feature bits on the processor.
+  Set "CFG Lock" (MSR 0E2h Bit[15]
+
+  @param[in] LockFeatureEnable - TRUE to lock these feature bits and FALSE to not lock
+**/
+VOID
+LockFeatureBit (
+  IN BOOLEAN LockFeatureEnable
+  );
+
+/**
+  Function to get desired core number by CSR register
+
+  @retval Desired core number
+**/
+UINT32
+GetCsrDesiredCores (
+  VOID
+  );
+
+/**
+  Function to set desired core numbers or SMT lock
+**/
+VOID
+SetLockCsrDesiredCores (
+  VOID
+  );
+
+/**
+  Write 64bits MSR with script
+
+  @param[in] Index - MSR index that will be written
+  @param[in] Value - value written to MSR
+**/
+VOID
+AsmWriteMsr64WithScript (
+  IN UINT32 Index,
+  IN UINT64 Value
+  );
+
+/**
+  Write 64bits MSR to script
+
+  @param[in] Index - MSR index that will be written
+  @param[in] Value - value written to MSR
+**/
+VOID
+WriteMsr64ToScript (
+  IN UINT32 Index,
+  IN UINT64 Value
+  );
+
+/**
+  Provide access to the CPU misc enables MSR
+
+  @param[in] Enable  - Enable or Disable Misc Features
+  @param[in] BitMask - The register bit offset of MSR MSR_IA32_MISC_ENABLE
+**/
+VOID
+CpuMiscEnable (
+  BOOLEAN Enable,
+  UINT64  BitMask
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MachineCheck.c b/ReferenceCode/Haswell/CpuInit/Dxe/MachineCheck.c
new file mode 100644
index 0000000..b7485fa
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MachineCheck.c
@@ -0,0 +1,133 @@
+/** @file
+  Machine check register 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 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
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "MachineCheck.h"
+#include "Features.h"
+
+#include EFI_GUID_DEFINITION (PowerOnHob)
+#include EFI_PROTOCOL_DEFINITION (MpService)
+#include EFI_PROTOCOL_DEFINITION (CpuPlatformPolicy)
+#endif
+
+extern DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu;
+
+/**
+  Initialize all the Machine-Check registers.
+
+  @param[in] Buffer      - Pointer to private data. Not Used.
+  @param[in] MchkEnable  - Enable or disable Mchk.
+**/
+VOID
+InitializeMachineCheckRegisters (
+  IN VOID    *Buffer,
+  IN BOOLEAN MchkEnable
+  )
+{
+  EFI_CPUID_REGISTER            CpuidRegisters;
+  CPU_FEATURE                   Feature;
+  CPU_IA32_MCG_CAP_LOW_REGISTER *MCGCap;
+  UINT64                        MCGCapValue;
+  UINT8                         Count;
+  UINT8                         Index;
+  UINT8                         StartIndex;
+  UINT64                        Value;
+
+  if (!MchkEnable) {
+    ///
+    /// Do not enable MCHK
+    ///
+    return;
+  }
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  *(UINT32 *) (&Feature) = CpuidRegisters.RegEdx;
+
+  if (Feature.MCE && Feature.MCA) {
+
+    MCGCapValue = AsmReadMsr64 (IA32_MCG_CAP);
+    MCGCap      = (CPU_IA32_MCG_CAP_LOW_REGISTER *) &MCGCapValue;
+
+    Count       = (UINT8) MCGCap->Count;
+
+    StartIndex  = 0;
+    for (Index = StartIndex; Index < Count; Index++) {
+      Value = (UINT64) -1;
+      AsmWriteMsr64WithScript (IA32_MC0_CTL + Index * 4, Value);
+    }
+    for (Index = StartIndex; Index < Count; Index++) {
+      if (Index <= 4) {
+        ///
+        /// Clean MC0-MC4 Status when system is cold reset, but no boot script.  S3 is treated as warm reset.
+        ///
+        if (mPlatformCpu->CpuConfig->IsColdReset == CPU_FEATURE_ENABLE) {
+          AsmWriteMsr64 (IA32_MC0_STATUS + Index * 4, 0);
+        }
+        continue;
+      }
+      AsmWriteMsr64WithScript (IA32_MC0_STATUS + Index * 4, 0);
+    }
+    EnableMce ();
+  }
+
+  return;
+}
+
+/**
+  Enable MCE feature for current CPU.
+
+  @param[in] MchkEnable  - Enable Mchk or not.
+**/
+VOID
+InitializeMce (
+  IN BOOLEAN MchkEnable
+  )
+{
+  EFI_CPUID_REGISTER CpuidRegisters;
+  CPU_FEATURE        Feature;
+
+  if (!MchkEnable) {
+    ///
+    /// Do not enable MCHK
+    ///
+    return;
+  }
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  *(UINT32 *) (&Feature) = CpuidRegisters.RegEdx;
+
+  if (Feature.MCE && Feature.MCA) {
+    EnableMce ();
+  }
+
+  return;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MachineCheck.h b/ReferenceCode/Haswell/CpuInit/Dxe/MachineCheck.h
new file mode 100644
index 0000000..107c849
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MachineCheck.h
@@ -0,0 +1,109 @@
+/** @file
+  The header file for Machine check register 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 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 _MACHINE_CHECK_INIT_H
+#define _MACHINE_CHECK_INIT_H
+
+#include "CpuInitDxe.h"
+#include "MpCommon.h"
+
+#pragma pack(1)
+
+typedef struct {
+  UINT32 FPU : 1;
+  UINT32 VME : 1;
+  UINT32 DE  : 1;
+  UINT32 PSE : 1;
+  UINT32 TSC : 1;
+  UINT32 MSR : 1;
+  UINT32 PAE : 1;
+  UINT32 MCE : 1;
+
+  UINT32 CX8       : 1;
+  UINT32 APIC      : 1;
+  UINT32 Reserved0 : 1;
+  UINT32 SEP       : 1;
+  UINT32 MTRR      : 1;
+  UINT32 PGE       : 1;
+  UINT32 MCA       : 1;
+  UINT32 CMOV      : 1;
+
+  UINT32 PAT       : 1;
+  UINT32 PSE_36    : 1;
+  UINT32 PSN       : 1;
+  UINT32 CLFSH     : 1;
+  UINT32 Reserved1 : 1;
+  UINT32 DS        : 1;
+  UINT32 ACPI      : 1;
+  UINT32 MMX       : 1;
+
+  UINT32 FXSR      : 1;
+  UINT32 SSE       : 1;
+  UINT32 SSE2      : 1;
+  UINT32 SS        : 1;
+  UINT32 HTT       : 1;
+  UINT32 TM        : 1;
+  UINT32 Reserved2 : 1;
+  UINT32 PBE       : 1;
+} CPU_FEATURE;
+
+typedef struct {
+  UINT32 Count        : 8;
+  UINT32 MCG_CTL_P    : 1;
+  UINT32 MCG_EXT_P    : 1;
+  UINT32 EXT_CORR_ERR : 1;
+  UINT32 MCG_TES_P    : 1;
+  UINT32 Reserved0    : 4;
+  UINT32 MCG_EXT_CNT  : 8;
+  UINT32 Reserved1    : 8;
+} CPU_IA32_MCG_CAP_LOW_REGISTER;
+
+#pragma pack()
+
+/**
+  Initialize all the Machine-Check registers.
+
+  @param[in] Buffer      - Pointer to private data. Not Used.
+  @param[in] MchkEnable  - Enable Mchk or not.
+**/
+VOID
+InitializeMachineCheckRegisters (
+  IN VOID    *Buffer,
+  IN BOOLEAN MchkEnable
+  );
+
+/**
+  Enable MCE feature for current CPU.
+
+  @param[in] MchkEnable  - Enable Mchk or not.
+**/
+VOID
+InitializeMce (
+  IN BOOLEAN MchkEnable
+  );
+
+/**
+  Enable "Machine Check Enable"
+**/
+VOID
+EnableMce (
+  VOID
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MemoryAttribute.c b/ReferenceCode/Haswell/CpuInit/Dxe/MemoryAttribute.c
new file mode 100644
index 0000000..9132580
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MemoryAttribute.c
@@ -0,0 +1,895 @@
+/** @file
+  CPU MTRR programming driver
+
+Revision History
+
+@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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "MpService.h"
+#include "MemoryAttribute.h"
+#include "CpuInitDxe.h"
+#endif
+
+FIXED_MTRR mFixedMtrrTable[V_FIXED_MTRR_NUMBER];
+
+FIXED_MTRR mFixedMtrrTable[] = {
+  {
+    IA32_MTRR_FIX64K_00000,
+    0,
+    0x10000
+  },
+  {
+    IA32_MTRR_FIX16K_80000,
+    0x80000,
+    0x4000
+  },
+  {
+    IA32_MTRR_FIX16K_A0000,
+    0xA0000,
+    0x4000
+  },
+  {
+    IA32_MTRR_FIX4K_C0000,
+    0xC0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_C8000,
+    0xC8000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_D0000,
+    0xD0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_D8000,
+    0xD8000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_E0000,
+    0xE0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_E8000,
+    0xE8000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_F0000,
+    0xF0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_F8000,
+    0xF8000,
+    0x1000
+  },
+};
+
+MTRR_VALUE mFixedMtrrValueTable[] = {
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+  {
+    0,
+    FALSE
+  },
+};
+
+VARIABLE_MTRR  mVariableMtrr[V_MAXIMUM_VARIABLE_MTRR_NUMBER];
+UINT32         mUsedMtrr;
+UINT8          mDefaultMemoryType = EFI_MEMORY_UC;
+extern UINT64  mValidMtrrAddressMask;
+extern UINT64  mValidMtrrBitsMask;
+extern BOOLEAN mVariableMtrrChanged;
+extern BOOLEAN mFixedMtrrChanged;
+
+/**
+  Disable Cache MTRR
+**/
+VOID
+PreMtrrChange (
+  VOID
+  )
+{
+  UINT64 TempQword;
+
+  EfiDisableCache ();
+  ///
+  /// Disable Cache MTRR
+  ///
+  TempQword = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+  TempQword = TempQword &~B_CACHE_MTRR_VALID &~B_CACHE_FIXED_MTRR_VALID;
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, TempQword);
+
+  return;
+}
+
+/**
+  Enable Cache MTRR
+**/
+VOID
+PostMtrrChange (
+  VOID
+  )
+{
+  UINT64 TempQword;
+
+  TempQword = 0;
+  ///
+  /// Enable Cache MTRR
+  ///
+  TempQword = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, TempQword | B_CACHE_MTRR_VALID | B_CACHE_FIXED_MTRR_VALID);
+
+  EfiEnableCache ();
+  return;
+}
+
+/**
+  Calculate fixed MTRR
+
+  @param[in] MemoryCacheType - Cache type for this memory range
+  @param[in] Base            - Memory range base address
+  @param[in] Length          - Memory range length
+
+  @exception EFI_UNSUPPORTED - Fixed MTRR number not enough or not present
+  @retval EFI_SUCCESS - Fixed MTRR settings calculated successfully
+**/
+EFI_STATUS
+CalculateFixedMtrr (
+  IN UINT64 MemoryCacheType,
+  IN UINT64 *Base,
+  IN UINT64 *Length
+  )
+{
+  UINT32 MsrNum;
+  UINT32 ByteShift;
+  UINT64 TempQword;
+  UINT64 OrMask;
+  UINT64 ClearMask;
+
+  TempQword = 0;
+  OrMask    = 0;
+  ClearMask = 0;
+
+  for (MsrNum = 0; MsrNum < V_FIXED_MTRR_NUMBER; MsrNum++) {
+    if ((*Base >= mFixedMtrrTable[MsrNum].BaseAddress) &&
+        (*Base < (mFixedMtrrTable[MsrNum].BaseAddress + 8 * mFixedMtrrTable[MsrNum].Length))
+        ) {
+      break;
+    }
+  }
+
+  if (MsrNum == V_FIXED_MTRR_NUMBER) {
+    return EFI_UNSUPPORTED;
+  }
+  ///
+  /// We found the fixed MTRR to be programmed
+  ///
+  for (ByteShift = 0; ByteShift < 8; ByteShift++) {
+    if (*Base == (mFixedMtrrTable[MsrNum].BaseAddress + ByteShift * mFixedMtrrTable[MsrNum].Length)) {
+      break;
+    }
+  }
+
+  if (ByteShift == 8) {
+    return EFI_UNSUPPORTED;
+  }
+
+  for (; ((ByteShift < 8) && (*Length >= mFixedMtrrTable[MsrNum].Length)); ByteShift++) {
+    OrMask |= LShiftU64 ((UINT64) MemoryCacheType, (UINT32) (ByteShift * 8));
+    ClearMask |= LShiftU64 ((UINT64) 0xFF, (UINT32) (ByteShift * 8));
+    *Length -= mFixedMtrrTable[MsrNum].Length;
+    *Base += mFixedMtrrTable[MsrNum].Length;
+    mFixedMtrrChanged = TRUE;
+  }
+
+  if (ByteShift < 8 && (*Length != 0)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  TempQword = AsmReadMsr64 (mFixedMtrrTable[MsrNum].Msr) &~ClearMask | OrMask;
+  mFixedMtrrValueTable[MsrNum].MsrValue = TempQword;
+  mFixedMtrrValueTable[MsrNum].Changed  = TRUE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Program fixed MTRR
+**/
+VOID
+ProgramFixedMtrr (
+  VOID
+  )
+{
+  UINT32 MsrNum;
+  PreMtrrChange ();
+  for (MsrNum = 0; MsrNum < V_FIXED_MTRR_NUMBER; MsrNum++) {
+    if (mFixedMtrrValueTable[MsrNum].Changed) {
+      AsmWriteMsr64 (mFixedMtrrTable[MsrNum].Msr, mFixedMtrrValueTable[MsrNum].MsrValue);
+      mFixedMtrrValueTable[MsrNum].Changed = FALSE;
+    }
+  }
+
+  PostMtrrChange ();
+}
+
+/**
+  Get all information about memory cache registers
+
+  @retval EFI_SUCCESS - always success
+**/
+EFI_STATUS
+GetMemoryAttribute (
+  VOID
+  )
+{
+  UINTN  Index;
+  UINT32 MsrNum;
+  UINT64 MsrValue;
+  UINT32 VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+
+  ///
+  /// Get Default Mtrr Type
+  ///
+  MsrValue            = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+  mDefaultMemoryType  = (UINT8) MsrValue;
+
+  ///
+  /// Get Variable Mtrr
+  ///
+  ZeroMem (mVariableMtrr, sizeof (VARIABLE_MTRR) * V_MAXIMUM_VARIABLE_MTRR_NUMBER);
+  mUsedMtrr = 0;
+
+  for (MsrNum = CACHE_VARIABLE_MTRR_BASE, Index = 0;
+       ((MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1)) && (Index < V_MAXIMUM_VARIABLE_MTRR_NUMBER));
+       MsrNum += 2
+       ) {
+    if ((AsmReadMsr64 (MsrNum + 1) & B_CACHE_MTRR_VALID) != 0) {
+      mVariableMtrr[Index].Msr          = MsrNum;
+      mVariableMtrr[Index].BaseAddress  = (AsmReadMsr64 (MsrNum) & mValidMtrrAddressMask);
+      mVariableMtrr[Index].Length       = ((~((AsmReadMsr64 (MsrNum + 1) & mValidMtrrAddressMask))) & mValidMtrrBitsMask) + 1;
+      mVariableMtrr[Index].Type         = (AsmReadMsr64 (MsrNum) & 0x0ff);
+      mVariableMtrr[Index].Valid        = TRUE;
+      mUsedMtrr++;
+      Index++;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if different memory attribute range overlapping with each other
+
+  @param[in] Start - start of memory range that will be checking
+  @param[in] End   - end of memory range address that will be checking
+
+  @retval TRUE if overlapping found
+  @retval FALSE if not found
+**/
+BOOLEAN
+CheckMemoryAttributeOverlap (
+  IN EFI_PHYSICAL_ADDRESS Start,
+  IN EFI_PHYSICAL_ADDRESS End
+  )
+{
+  UINT32 Index;
+
+  for (Index = 0; Index < V_MAXIMUM_VARIABLE_MTRR_NUMBER; Index++) {
+    if (mVariableMtrr[Index].Valid &&
+        !(
+                Start > (mVariableMtrr[Index].BaseAddress + mVariableMtrr[Index].Length - 1) ||
+                (End < mVariableMtrr[Index].BaseAddress)
+                )
+        ) {
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Combine current memory attribute range to existing memory attribute range
+
+  @param[in] Attributes  - Cache type
+  @param[in] Base        - Base address of memory range that will be combined into existing one.
+  @param[in] Length      - Length of the memory range that will be combined into existing one.
+
+  @retval EFI_SUCCESS - Memory combined successfully
+  @retval EFI_ACCESS_DENIED - memory type that is not allowed to overlap
+**/
+EFI_STATUS
+CombineMemoryAttribute (
+  IN UINT64 Attributes,
+  IN UINT64 *Base,
+  IN UINT64 *Length
+  )
+{
+  UINT32  Index;
+  UINT64  CombineStart;
+  UINT64  CombineEnd;
+  UINT64  MtrrEnd;
+  UINT64  EndAddress;
+  BOOLEAN InvalidMTRRs[V_MAXIMUM_VARIABLE_MTRR_NUMBER];
+
+  EndAddress = *Base +*Length - 1;
+
+  for (Index = 0; Index < V_MAXIMUM_VARIABLE_MTRR_NUMBER; Index++) {
+    InvalidMTRRs[Index] = FALSE;
+  }
+
+  Index = 0;
+  while (Index < V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    MtrrEnd = mVariableMtrr[Index].BaseAddress + mVariableMtrr[Index].Length - 1;
+
+    ///
+    /// The MTRR is marked invalid or the ranges are not intersected.
+    ///
+    if (InvalidMTRRs[Index] ||
+        !mVariableMtrr[Index].Valid ||
+        (*Base > (MtrrEnd) || (EndAddress < mVariableMtrr[Index].BaseAddress))
+        ) {
+      Index++;
+      continue;
+    }
+    ///
+    /// if the requested range contains MTRR range, invalidate this MTRR
+    ///
+    if (mVariableMtrr[Index].BaseAddress >= *Base && MtrrEnd <= EndAddress) {
+      InvalidMTRRs[Index] = TRUE;
+      Index++;
+      continue;
+    }
+
+    if (Attributes == mVariableMtrr[Index].Type) {
+      ///
+      /// if the Mtrr range contain the request range, return EFI_SUCCESS
+      ///
+      if (mVariableMtrr[Index].BaseAddress <= *Base && MtrrEnd >= EndAddress) {
+        *Length = 0;
+        return EFI_SUCCESS;
+      }
+      ///
+      /// invalid this MTRR, and program the combine range
+      ///
+      CombineStart  = (*Base) < mVariableMtrr[Index].BaseAddress ? (*Base) : mVariableMtrr[Index].BaseAddress;
+      CombineEnd    = EndAddress > MtrrEnd ? EndAddress : MtrrEnd;
+
+      ///
+      /// Record this MTRR as invalid
+      ///
+      InvalidMTRRs[Index] = TRUE;
+
+      ///
+      /// The range is modified, retry from the first MTRR
+      ///
+      if (*Base != CombineStart || *Length != CombineEnd - CombineStart + 1) {
+        Index = 0;
+      } else {
+        Index++;
+      }
+
+      *Base       = CombineStart;
+      *Length     = CombineEnd - CombineStart + 1;
+      EndAddress  = CombineEnd;
+      continue;
+    }
+
+    if ((Attributes == CACHE_UNCACHEABLE) ||
+        (Attributes == CACHE_WRITETHROUGH && mVariableMtrr[Index].Type == CACHE_WRITEBACK) ||
+        (Attributes == CACHE_WRITEBACK && mVariableMtrr[Index].Type == CACHE_WRITETHROUGH) ||
+        (Attributes == CACHE_WRITETHROUGH && mVariableMtrr[Index].Type == CACHE_UNCACHEABLE) ||
+        (Attributes == CACHE_WRITEBACK && mVariableMtrr[Index].Type == CACHE_UNCACHEABLE)
+        ) {
+      Index++;
+      continue;
+    }
+    ///
+    /// Other type memory overlap is invalid
+    ///
+    return EFI_ACCESS_DENIED;
+  }
+  ///
+  /// Finally invalidate recorded MTRRs
+  ///
+  for (Index = 0; Index < V_MAXIMUM_VARIABLE_MTRR_NUMBER; Index++) {
+    if (InvalidMTRRs[Index]) {
+      InvariableMtrr (mVariableMtrr[Index].Msr, Index);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Given the input, check if the number of MTRR is lesser
+    if positive or subtractive
+
+    @param[in] Input - Length of Memory to program MTRR
+    @param[in] MtrrNumber - return needed Mtrr number
+    @param[in] Direction  - TRUE: do positive
+                            FALSE: do subtractive
+
+    @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+GetDirection (
+  IN UINT64  Input,
+  IN UINTN   *MtrrNumber,
+  IN BOOLEAN *Direction
+  )
+{
+  UINT64 TempQword;
+  UINT32 Positive;
+  UINT32 Subtractive;
+
+  TempQword   = Input;
+  Positive    = 0;
+  Subtractive = 0;
+
+  do {
+    TempQword -= Power2MaxMemory (TempQword);
+    Positive++;
+
+  } while (TempQword != 0);
+
+  TempQword = Power2MaxMemory (LShiftU64 (Input, 1)) - Input;
+  Subtractive++;
+  do {
+    TempQword -= Power2MaxMemory (TempQword);
+    Subtractive++;
+
+  } while (TempQword != 0);
+
+  if (Positive <= Subtractive) {
+    *Direction  = TRUE;
+    *MtrrNumber = Positive;
+  } else {
+    *Direction  = FALSE;
+    *MtrrNumber = Subtractive;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Calculate max memory of power 2
+
+  @param[in] MemoryLength  - Memory length that will be calculated
+
+  @retval Max memory
+**/
+UINT64
+Power2MaxMemory (
+  IN UINT64 MemoryLength
+  )
+{
+  UINT64 Result;
+  UINT32 *ResultPointer;
+  UINT32 *MemoryLengthPointer;
+  MemoryLengthPointer = (UINT32 *) &MemoryLength;
+  ResultPointer       = (UINT32 *) &Result;
+  Result              = 0;
+  if (MemoryLengthPointer[1] != 0) {
+    ResultPointer[1] = GetPowerOfTwo32 (MemoryLengthPointer[1]);
+  } else {
+    ResultPointer[0] = GetPowerOfTwo32 (MemoryLengthPointer[0]);
+  }
+
+  return Result;
+}
+
+/**
+  Clear MTRR
+
+  @param[in] MtrrNumber  - MTRR register that will be cleared
+  @param[in] Index       - index of MTRR register
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+InvariableMtrr (
+  IN UINTN MtrrNumber,
+  IN UINTN Index
+  )
+{
+  PreMtrrChange ();
+  mVariableMtrr[Index].Valid = FALSE;
+  AsmWriteMsr64 ((UINT32) MtrrNumber, 0);
+  AsmWriteMsr64 ((UINT32) (MtrrNumber + 1), 0);
+  mUsedMtrr--;
+  PostMtrrChange ();
+  return EFI_SUCCESS;
+}
+
+/**
+  Programm VARIABLE MTRR
+
+  @param[in] MtrrNumber      - Variable MTRR register
+  @param[in] BaseAddress     - Memory base address
+  @param[in] Length          - Memory length
+  @param[in] MemoryCacheType - Cache type
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+ProgramVariableMtrr (
+  IN UINTN                MtrrNumber,
+  IN EFI_PHYSICAL_ADDRESS BaseAddress,
+  IN UINT64               Length,
+  IN UINT64               MemoryCacheType
+  )
+{
+  UINT64 TempQword;
+
+  PreMtrrChange ();
+
+  ///
+  /// MTRR Physical Base
+  ///
+  TempQword = (BaseAddress & mValidMtrrAddressMask) | MemoryCacheType;
+  AsmWriteMsr64 ((UINT32) MtrrNumber, TempQword);
+
+  ///
+  /// MTRR Physical Mask
+  ///
+  TempQword = ~(Length - 1);
+  AsmWriteMsr64 ((UINT32) (MtrrNumber + 1), (TempQword & mValidMtrrAddressMask) | B_CACHE_MTRR_VALID);
+
+  ///
+  /// Variable MTRR is updated
+  ///
+  mVariableMtrrChanged = TRUE;
+
+  PostMtrrChange ();
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get GCD Mem Space type from Mtrr Type
+
+  @param[in] MtrrAttributes - Mtrr type
+
+  @retval GCD Mem Space typed (64-bit)
+  @retval EFI_MEMORY_UC - input MTRR type is Uncacheable
+  @retval EFI_MEMORY_WC - input MTRR type is Write Combining
+  @retval EFI_MEMORY_WT - input MTRR type is Write-through
+  @retval EFI_MEMORY_WP - input MTRR type is Write-protected
+  @retval EFI_MEMORY_WB - input MTRR type is Write Back
+**/
+UINT64
+GetMemorySpaceAttributeFromMtrrType (
+  IN UINT8 MtrrAttributes
+  )
+{
+  switch (MtrrAttributes) {
+    case CACHE_UNCACHEABLE:
+      return EFI_MEMORY_UC;
+
+    case CACHE_WRITECOMBINING:
+      return EFI_MEMORY_WC;
+
+    case CACHE_WRITETHROUGH:
+      return EFI_MEMORY_WT;
+
+    case CACHE_WRITEPROTECTED:
+      return EFI_MEMORY_WP;
+
+    case CACHE_WRITEBACK:
+      return EFI_MEMORY_WB;
+
+    default:
+      return 0;
+  }
+}
+
+/**
+  Refresh the GCD Memory Space Attributes according to MTRRs
+
+  @retval EFI_STATUS - status returned from each sub-routine
+**/
+EFI_STATUS
+RefreshGcdMemoryAttributes (
+  VOID
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           Index;
+  UINTN                           SubIndex;
+  UINT64                          RegValue;
+  EFI_PHYSICAL_ADDRESS            BaseAddress;
+  UINT64                          Length;
+  UINT64                          Attributes;
+  UINT64                          CurrentAttributes;
+  UINT8                           MtrrType;
+  UINTN                           NumberOfDescriptors;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap;
+  UINT64                          DefaultAttributes;
+
+  MemorySpaceMap  = NULL;
+
+  Status          = GetMemoryAttribute ();
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = gDS->GetMemorySpaceMap (
+                  &NumberOfDescriptors,
+                  &MemorySpaceMap
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  DefaultAttributes = GetMemorySpaceAttributeFromMtrrType (mDefaultMemoryType);
+
+  ///
+  /// Set default attributes to all spaces.
+  ///
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    if (MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeNonExistent) {
+      continue;
+    }
+
+    gDS->SetMemorySpaceAttributes (
+            MemorySpaceMap[Index].BaseAddress,
+            MemorySpaceMap[Index].Length,
+            (MemorySpaceMap[Index].Attributes &~EFI_MEMORY_CACHETYPE_MASK) |
+            (MemorySpaceMap[Index].Capabilities & DefaultAttributes)
+            );
+  }
+  ///
+  /// Go for variable MTRRs, WB first, Other types second
+  ///
+  for (Index = 0; Index < 6; Index++) {
+    if (mVariableMtrr[Index].Valid && mVariableMtrr[Index].Type == CACHE_WRITEBACK) {
+      SetGcdMemorySpaceAttributes (
+              MemorySpaceMap,
+              NumberOfDescriptors,
+              mVariableMtrr[Index].BaseAddress,
+              mVariableMtrr[Index].Length,
+              EFI_MEMORY_WB
+              );
+    }
+  }
+
+  for (Index = 0; Index < 6; Index++) {
+    if (mVariableMtrr[Index].Valid && mVariableMtrr[Index].Type != CACHE_WRITEBACK) {
+      Attributes = GetMemorySpaceAttributeFromMtrrType ((UINT8) mVariableMtrr[Index].Type);
+      SetGcdMemorySpaceAttributes (
+              MemorySpaceMap,
+              NumberOfDescriptors,
+              mVariableMtrr[Index].BaseAddress,
+              mVariableMtrr[Index].Length,
+              Attributes
+              );
+    }
+  }
+  ///
+  /// Go for fixed MTRRs
+  ///
+  Attributes  = 0;
+  BaseAddress = 0;
+  Length      = 0;
+  for (Index = 0; Index < V_FIXED_MTRR_NUMBER; Index++) {
+    RegValue = AsmReadMsr64 (mFixedMtrrTable[Index].Msr);
+    for (SubIndex = 0; SubIndex < 8; SubIndex++) {
+      MtrrType          = (UINT8) RShiftU64 (RegValue, SubIndex * 8);
+      CurrentAttributes = GetMemorySpaceAttributeFromMtrrType (MtrrType);
+      if (Length == 0) {
+        Attributes = CurrentAttributes;
+      } else {
+        if (CurrentAttributes != Attributes) {
+          SetGcdMemorySpaceAttributes (
+                  MemorySpaceMap,
+                  NumberOfDescriptors,
+                  BaseAddress,
+                  Length,
+                  Attributes
+                  );
+          BaseAddress = mFixedMtrrTable[Index].BaseAddress + mFixedMtrrTable[Index].Length * SubIndex;
+          Length      = 0;
+          Attributes  = CurrentAttributes;
+        }
+      }
+
+      Length += mFixedMtrrTable[Index].Length;
+    }
+  }
+  ///
+  /// handle the last region
+  ///
+  SetGcdMemorySpaceAttributes (
+          MemorySpaceMap,
+          NumberOfDescriptors,
+          BaseAddress,
+          Length,
+          Attributes
+          );
+
+Done:
+  if (MemorySpaceMap != NULL) {
+    FreePool (MemorySpaceMap);
+  }
+
+  return Status;
+}
+
+/**
+  Search into the Gcd Memory Space for descriptors (from StartIndex
+  to EndIndex) that contains the memory range specified by BaseAddress
+  and Length.
+
+  @param[in] MemorySpaceMap      - Gcd Memory Space Map as array
+  @param[in] NumberOfDescriptors - Number of descriptors in map
+  @param[in] BaseAddress         - BaseAddress for the requested range
+  @param[in] Length              - Length for the requested range
+  @param[in] StartIndex          - Start index into the Gcd Memory Space Map
+  @param[in] EndIndex            - End index into the Gcd Memory Space Map
+
+  @retval EFI_SUCCESS   - Search successfully
+  @retval EFI_NOT_FOUND - The requested descriptors not exist
+**/
+EFI_STATUS
+SearchGcdMemorySpaces (
+  IN EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap,
+  IN UINTN                           NumberOfDescriptors,
+  IN EFI_PHYSICAL_ADDRESS            BaseAddress,
+  IN UINT64                          Length,
+  OUT UINTN                          *StartIndex,
+  OUT UINTN                          *EndIndex
+  )
+{
+  UINTN Index;
+
+  *StartIndex = 0;
+  *EndIndex   = 0;
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    if (BaseAddress >= MemorySpaceMap[Index].BaseAddress &&
+        BaseAddress < MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length
+        ) {
+      *StartIndex = Index;
+    }
+
+    if (BaseAddress + Length - 1 >= MemorySpaceMap[Index].BaseAddress &&
+        BaseAddress + Length - 1 < MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length
+        ) {
+      *EndIndex = Index;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Set the attributes for a specified range in Gcd Memory Space Map.
+
+  @param[in] MemorySpaceMap      - Gcd Memory Space Map as array
+  @param[in] NumberOfDescriptors - Number of descriptors in map
+  @param[in] BaseAddress         - BaseAddress for the range
+  @param[in] Length              - Length for the range
+  @param[in] Attributes          - Attributes to set
+
+  @retval EFI_SUCCESS   - Set successfully
+  @retval EFI_NOT_FOUND - The specified range does not exist in Gcd Memory Space
+**/
+EFI_STATUS
+SetGcdMemorySpaceAttributes (
+  IN EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap,
+  IN UINTN                           NumberOfDescriptors,
+  IN EFI_PHYSICAL_ADDRESS            BaseAddress,
+  IN UINT64                          Length,
+  IN UINT64                          Attributes
+  )
+{
+  EFI_STATUS           Status;
+  UINTN                Index;
+  UINTN                StartIndex;
+  UINTN                EndIndex;
+  EFI_PHYSICAL_ADDRESS RegionStart;
+  UINT64               RegionLength;
+
+  Status = SearchGcdMemorySpaces (
+                  MemorySpaceMap,
+                  NumberOfDescriptors,
+                  BaseAddress,
+                  Length,
+                  &StartIndex,
+                  &EndIndex
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  for (Index = StartIndex; Index <= EndIndex; Index++) {
+    if (MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeNonExistent) {
+      continue;
+    }
+
+    if (BaseAddress >= MemorySpaceMap[Index].BaseAddress) {
+      RegionStart = BaseAddress;
+    } else {
+      RegionStart = MemorySpaceMap[Index].BaseAddress;
+    }
+
+    if (BaseAddress + Length - 1 < MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length) {
+      RegionLength = BaseAddress + Length - RegionStart;
+    } else {
+      RegionLength = MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length - RegionStart;
+    }
+
+    gDS->SetMemorySpaceAttributes (
+            RegionStart,
+            RegionLength,
+            (MemorySpaceMap[Index].Attributes &~EFI_MEMORY_CACHETYPE_MASK) | (MemorySpaceMap[Index].Capabilities & Attributes)
+            );
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MemoryAttribute.h b/ReferenceCode/Haswell/CpuInit/Dxe/MemoryAttribute.h
new file mode 100644
index 0000000..8ce2a74
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MemoryAttribute.h
@@ -0,0 +1,259 @@
+/** @file
+  Header file for CPU MTRR programming driver
+
+Revision History:
+
+@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 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 _EFI_MEMORY_ATTRIB_H
+#define _EFI_MEMORY_ATTRIB_H
+
+extern UINT32 mUsedMtrr;
+
+typedef struct {
+  UINT32 Msr;
+  UINT32 BaseAddress;
+  UINT32 Length;
+} FIXED_MTRR;
+
+typedef struct {
+  UINT64  MsrValue;
+  BOOLEAN Changed;
+} MTRR_VALUE;
+
+typedef struct {
+  UINT64  BaseAddress;
+  UINT64  Length;
+  UINT64  Type;
+  UINT32  Msr;
+  BOOLEAN Valid;
+} VARIABLE_MTRR;
+
+#define EFI_MEMORY_CACHETYPE_MASK (EFI_MEMORY_UC | EFI_MEMORY_WC | EFI_MEMORY_WT | EFI_MEMORY_WB | EFI_MEMORY_UCE)
+
+/**
+  Calculate fixed MTRR
+
+  @param[in] MemoryCacheType - Cache type for this memory range
+  @param[in] Base            - Memory range base address
+  @param[in] Length          - Memory range length
+
+  @exception EFI_UNSUPPORTED - Fixed MTRR number not enough or not present
+  @retval EFI_SUCCESS - Fixed MTRR settings calculated successfully
+**/
+EFI_STATUS
+CalculateFixedMtrr (
+  IN UINT64 MemoryCacheType,
+  IN UINT64 *Base,
+  IN UINT64 *Length
+  );
+
+/**
+  Disable Cache MTRR
+**/
+VOID
+PreMtrrChange (
+  VOID
+  );
+/**
+  Enable Cache MTRR
+**/
+VOID
+PostMtrrChange (
+  VOID
+  );
+
+/**
+  Program fixed MTRR
+**/
+VOID
+ProgramFixedMtrr (
+  VOID
+  );
+
+/**
+  Get all information about memory cache registers
+
+  @retval EFI_SUCCESS - always success
+**/
+EFI_STATUS
+GetMemoryAttribute (
+  VOID
+  );
+
+/**
+  Check if different memory attribute range overlapping with each other
+
+  @param[in] Start - start of memory range that will be checking
+  @param[in] End   - end of memory range address that will be checking
+
+  @retval TRUE if overlapping found
+  @retval FALSE if not found
+**/
+BOOLEAN
+CheckMemoryAttributeOverlap (
+  IN EFI_PHYSICAL_ADDRESS Start,
+  IN EFI_PHYSICAL_ADDRESS End
+  );
+
+/**
+  Combine current memory attribute range to existing memory attribute range
+
+  @param[in] Attribute   - Cache type
+  @param[in] Base        - Base address of memory range that will be combined into existing one.
+  @param[in] Length      - Length of the memory range that will be combined into existing one.
+
+  @retval EFI_SUCCESS - Memory combined successfully
+  @retval EFI_ACCESS_DENIED - memory type that is not allowed to overlap
+**/
+EFI_STATUS
+CombineMemoryAttribute (
+  IN UINT64 Attribute,
+  IN UINT64 *Base,
+  IN UINT64 *Length
+  );
+
+/**
+  Given the input, check if the number of MTRR is lesser
+    if positive or subtractive
+
+    @param[in] Input - Length of Memory to program MTRR
+    @param[in] MtrrNumber - return needed Mtrr number
+    @param[in] Direction  - TRUE: do positive
+                            FALSE: do subtractive
+
+    @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+GetDirection (
+  IN UINT64  Input,
+  IN UINTN   *MtrrNumber,
+  IN BOOLEAN *Direction
+  );
+
+/**
+  Calculate max memory of power 2
+
+  @param[in] MemoryLength  - Memory length that will be calculated
+
+  @retval Max memory
+**/
+UINT64
+Power2MaxMemory (
+  IN UINT64 MemoryLength
+  );
+
+/**
+  Clear MTRR
+
+  @param[in] MtrrNumber  - MTRR register that will be cleared
+  @param[in] Index       - index of MTRR register
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+InvariableMtrr (
+  IN UINTN MtrrNumber,
+  IN UINTN Index
+  );
+
+/**
+  Programm VARIABLE MTRR
+
+  @param[in] MtrrNumber      - Variable MTRR register
+  @param[in] BaseAddress     - Memory base address
+  @param[in] Length          - Memory length
+  @param[in] MemoryCacheType - Cache type
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+ProgramVariableMtrr (
+  IN UINTN                MtrrNumber,
+  IN EFI_PHYSICAL_ADDRESS BaseAddress,
+  IN UINT64               Length,
+  IN UINT64               MemoryCacheType
+  );
+
+/**
+  Get GCD Mem Space type from Mtrr Type
+
+  @param[in] MtrrAttribute - Mtrr type
+
+  @retval GCD Mem Space typed (64-bit)
+**/
+UINT64
+GetMemorySpaceAttributeFromMtrrType (
+  IN UINT8 MtrrAttribute
+  );
+
+/**
+  Refresh the GCD Memory Space Attributes according to MTRRs
+
+  @retval EFI_STATUS - status returned from each sub-routine
+**/
+EFI_STATUS
+RefreshGcdMemoryAttributes (
+  VOID
+  );
+
+/**
+  Search into the Gcd Memory Space for descriptors (from StartIndex
+  to EndIndex) that contains the memory range specified by BaseAddress
+  and Length.
+
+  @param[in] MemorySpaceMap      - Gcd Memory Space Map as array
+  @param[in] NumberOfDescriptors - Number of descriptors in map
+  @param[in] BaseAddress         - BaseAddress for the requested range
+  @param[in] Length              - Length for the requested range
+  @param[in] StartIndex          - Start index into the Gcd Memory Space Map
+  @param[in] EndIndex            - End index into the Gcd Memory Space Map
+
+  @retval EFI_SUCCESS   - Search successfully
+  @retval EFI_NOT_FOUND - The requested descriptors not exist
+**/
+EFI_STATUS
+SearchGcdMemorySpaces (
+  IN EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap,
+  IN UINTN                           NumberOfDescriptors,
+  IN EFI_PHYSICAL_ADDRESS            BaseAddress,
+  IN UINT64                          Length,
+  OUT UINTN                          *StartIndex,
+  OUT UINTN                          *EndIndex
+  );
+
+/**
+  Set the attributes for a specified range in Gcd Memory Space Map.
+
+  @param[in] MemorySpaceMap      - Gcd Memory Space Map as array
+  @param[in] NumberOfDescriptors - Number of descriptors in map
+  @param[in] BaseAddress         - BaseAddress for the range
+  @param[in] Length              - Length for the range
+  @param[in] Attributes          - Attributes to set
+
+  @retval EFI_SUCCESS   - Set successfully
+  @retval EFI_NOT_FOUND - The specified range does not exist in Gcd Memory Space
+**/
+EFI_STATUS
+SetGcdMemorySpaceAttributes (
+  IN EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap,
+  IN UINTN                           NumberOfDescriptors,
+  IN EFI_PHYSICAL_ADDRESS            BaseAddress,
+  IN UINT64                          Length,
+  IN UINT64                          Attributes
+  );
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/Microcode.c b/ReferenceCode/Haswell/CpuInit/Dxe/Microcode.c
new file mode 100644
index 0000000..3f54a7d
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/Microcode.c
@@ -0,0 +1,547 @@
+/** @file
+  CPU Microcode update 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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxe.h"
+#include "ProcessorData.h"
+#include "MpCommon.h"
+#include "MpService.h"
+#endif
+
+extern DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu;
+
+///
+/// static type so this variable only can be accessed in this file
+///
+static UINT32 mMcuFirstLoadDone = FALSE;
+UINT32        mMcuLoadCount;
+
+///
+/// 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
+  );
+
+/**
+  To indicate the first microcode load is done
+**/
+VOID
+McuFirstLoadDone (
+  VOID
+  )
+{
+  mMcuFirstLoadDone = TRUE;
+}
+
+/**
+  Wait till all primary threads done the microcode load
+**/
+VOID
+WaitForPrimaryThreadMcuUpdate (
+  VOID
+  )
+{
+  UINTN CoreNumber;
+  CoreNumber = RShiftU64 (AsmReadMsr64 (MSR_CORE_THREAD_COUNT), 16) & 0xffff;
+  if (IsSecondaryThread ()) {
+    while (mMcuLoadCount < 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 (mMcuFirstLoadDone) {
+    if (MicrocodePointer->UpdateRevision & 0x80000000 ||
+        (MicrocodePointer->UpdateRevision > 0 && MicrocodePointer->UpdateRevision > *Revision)
+        ) {
+      Status = EFI_SUCCESS;
+    }
+  } else {
+    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] 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 EFI_CPU_MICROCODE_HEADER **MicrocodePointerBuffer,
+  OUT UINT32                  *FailedRevision
+  )
+{
+  EFI_STATUS         Status;
+  EFI_CPUID_REGISTER Cpuid;
+  UINT32             UcodeRevision;
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &Cpuid.RegEax,
+          &Cpuid.RegEbx,
+          &Cpuid.RegEcx,
+          &Cpuid.RegEdx
+          );
+
+  WaitForPrimaryThreadMcuUpdate ();
+  UcodeRevision = GetCpuUcodeRevision ();
+  Status = FindLoadMicrocode (
+                  Cpuid.RegEax,
+                  MicrocodePointerBuffer,
+                  &UcodeRevision
+                  );
+  *FailedRevision = UcodeRevision;
+
+  InterlockedIncrement (&mMcuLoadCount);
+
+  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 checksumed
+  @param[in] ChecksumLen  - The length of data that will be checksumed
+
+  @retval EFI_SUCCESS           - Checksum correct
+  @retval EFI_CRC_ERROR         - Checksum incorrect
+**/
+EFI_STATUS
+Checksum32Verify (
+  IN UINT32 *ChecksumAddr,
+  IN UINT32 ChecksumLen
+  )
+{
+  UINT32 Checksum;
+  UINT32 Index;
+
+  Checksum = 0;
+
+  for (Index = 0; Index < ChecksumLen; Index++) {
+    Checksum += ChecksumAddr[Index];
+  }
+
+  return (Checksum == 0) ? EFI_SUCCESS : EFI_CRC_ERROR;
+}
+
+/**
+  Check if this microcode is correct one for processor
+
+  @param[in] Cpuid               - processor CPUID
+  @param[in] MicrocodeEntryPoint - entry point of microcode
+  @param[in] Revision            - revision of microcode
+
+  @retval CorrectMicrocode if this microcode is correct
+**/
+BOOLEAN
+CheckMicrocode (
+  IN UINTN                    Cpuid,
+  IN EFI_CPU_MICROCODE_HEADER *MicrocodeEntryPoint,
+  IN UINT32                   *Revision
+  )
+{
+  EFI_STATUS                              Status;
+  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;
+  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);
+
+  ///
+  /// 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++;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  return CorrectMicrocode;
+}
+
+/**
+  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;
+  BOOLEAN                  CorrectMicrocode;
+
+  Status              = EFI_NOT_FOUND;
+  MicrocodeEntryPoint = NULL;
+  CorrectMicrocode    = FALSE;
+
+  Index               = 0;
+  while (MicrocodePointerBuffer[Index] != NULL) {
+    MicrocodeEntryPoint = MicrocodePointerBuffer[Index];
+    CorrectMicrocode    = CheckMicrocode (Cpuid, MicrocodeEntryPoint, Revision);
+
+    if (CorrectMicrocode) {
+      break;
+    }
+
+    Index++;
+  }
+
+  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;
+}
+
+/**
+  Load all microcode updates to memory. Since in S3 resume boot path, CPUs should be
+  patched again, these microcode updates are copied to OS reserved memory.
+
+  @retval EFI_SUCCESS           - All microcode updates are loaded to memory successfully
+  @retval EFI_OUT_OF_RESOURCES  - Not enough memory to accomodate all the microcode updates
+**/
+EFI_STATUS
+LoadAllMicrocodeUpdates (
+  VOID
+  )
+{
+  EFI_STATUS               Status;
+  EFI_CPU_MICROCODE_HEADER *MicrocodeEntryPoint;
+  EFI_CPU_MICROCODE_HEADER *MicrocodeBuffer;
+  UINTN                    MicrocodeNumber;
+  UINTN                    Index;
+  UINTN                    TotalSize[NUMBER_OF_MICROCODE_UPDATE + 1];
+  EFI_CPUID_REGISTER       Cpuid;
+  UINT32                   UcodeRevision;
+
+  AsmCpuid (
+          CPUID_VERSION_INFO,
+          &Cpuid.RegEax,
+          &Cpuid.RegEbx,
+          &Cpuid.RegEcx,
+          &Cpuid.RegEdx
+          );
+
+  UcodeRevision   = 0;
+  MicrocodeNumber = 0;
+  Status = (gBS->AllocatePool)
+          (
+                  EfiReservedMemoryType, sizeof (EFI_CPU_MICROCODE_HEADER *) * (NUMBER_OF_MICROCODE_UPDATE + 1), (VOID *)
+                  (&mMicrocodePointerBuffer)
+          );
+  ASSERT_EFI_ERROR (Status);
+
+  ZeroMem (mMicrocodePointerBuffer, sizeof (EFI_CPU_MICROCODE_HEADER *) * (NUMBER_OF_MICROCODE_UPDATE + 1));
+
+  MicrocodeEntryPoint = NULL;
+  while (TRUE) {
+    if (MicrocodeNumber > NUMBER_OF_MICROCODE_UPDATE) {
+      DEBUG ((EFI_D_INFO, "CPU Too Many Microcode available > %d\n", (UINTN) NUMBER_OF_MICROCODE_UPDATE));
+      Status = EFI_SUCCESS;
+      break;
+    }
+    ///
+    /// Initialize it to 0
+    ///
+    TotalSize[MicrocodeNumber] = 0;
+    ///
+    /// Continue to try to find patch
+    ///
+    Status = mPlatformCpu->CpuConfig->RetrieveMicrocode (mPlatformCpu, (VOID *) &MicrocodeEntryPoint);
+
+    if (EFI_ERROR (Status)) {
+      if (Status == EFI_NOT_FOUND) {
+        Status = EFI_SUCCESS;
+      }
+      break;
+
+    } else {
+      if (!CheckMicrocode (Cpuid.RegEax, MicrocodeEntryPoint, &UcodeRevision)) {
+        continue;
+      }
+
+      if (MicrocodeEntryPoint->DataSize == 0) {
+        TotalSize[MicrocodeNumber] = 2048;
+      } else {
+        TotalSize[MicrocodeNumber] = MicrocodeEntryPoint->TotalSize;
+      }
+
+      Status = AllocateReservedMemoryBelow4G (
+                      TotalSize[MicrocodeNumber],
+                      &MicrocodeBuffer
+                      );
+      if (EFI_ERROR (Status)) {
+        break;
+      }
+
+      CopyMem (MicrocodeBuffer, MicrocodeEntryPoint, TotalSize[MicrocodeNumber]);
+      mMicrocodePointerBuffer[MicrocodeNumber] = MicrocodeBuffer;
+      MicrocodeNumber++;
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    Index = 0;
+    while ((Index <= NUMBER_OF_MICROCODE_UPDATE) && (mMicrocodePointerBuffer[Index] != NULL)) {
+      (gBS->FreePages)((EFI_PHYSICAL_ADDRESS) (UINTN) mMicrocodePointerBuffer[Index], EFI_SIZE_TO_PAGES (TotalSize[Index]));
+      mMicrocodePointerBuffer[Index] = NULL;
+      Index++;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Check if loading microcode update fails, if so, report proper status code
+
+  @param[in] CpuNumber       - The CPU number
+  @param[in] Status          - The return value of InitializeMicrocode()
+  @param[in] FailedRevision  - The revision of the microcode update that failed to be loaded
+
+  @retval EFI_SUCCESS - The status is check and proper status code is reported
+**/
+EFI_STATUS
+CheckMicrocodeUpdate (
+  IN UINTN      CpuNumber,
+  IN EFI_STATUS Status,
+  IN UINT32     FailedRevision
+  )
+{
+  EFI_STATUS_CODE_VALUE StatusCode;
+
+  if (!EFI_ERROR (Status)) {
+    return EFI_SUCCESS;
+  }
+  if (Status == EFI_LOAD_ERROR) {
+    StatusCode = EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_HP_EC_MICROCODE_UPDATE;
+  } else if (Status == EFI_NOT_FOUND) {
+    if (GetCpuUcodeRevision () != 0) {
+      ///
+      /// Some other driver (for example, SEC) already updated CPU microcode
+      ///
+      return EFI_SUCCESS;
+    }
+    StatusCode = EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_HP_EC_NO_MICROCODE_UPDATE;
+  } else {
+    return Status;
+  }
+  ///
+  /// ReportStatusCode UEFI service can't be called by AP currently, so call by BSP only
+  ///
+  if (CpuNumber == mMPSystemData->BSP) {
+    return ReportStatusCode (
+                   EFI_ERROR_MINOR | EFI_ERROR_CODE,
+                   StatusCode
+                   );
+  } else {
+    return Status;
+  }
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MpCommon.c b/ReferenceCode/Haswell/CpuInit/Dxe/MpCommon.c
new file mode 100644
index 0000000..dd9a55e
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MpCommon.c
@@ -0,0 +1,1186 @@
+/** @file
+  Code which support multi-processor
+
+@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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "MpCommon.h"
+#include "CpuInitDxe.h"
+#include "Features.h"
+#include EFI_PROTOCOL_DEFINITION (ExitPmAuth)
+#endif
+
+extern MP_SYSTEM_DATA *mMPSystemData;
+
+extern EFI_PHYSICAL_ADDRESS             mOriginalBuffer;
+extern EFI_PHYSICAL_ADDRESS             mBackupBuffer;
+extern EFI_METRONOME_ARCH_PROTOCOL      *mMetronome;
+extern DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu;
+volatile UINTN                          mSwitchToLegacyRegionCount = 0;
+
+EFI_PHYSICAL_ADDRESS mLegacyRegion;
+//(AMI_CHG+)>
+#if (REQUEST_EBDA_SIZE != 0x1000)
+UINTN mEbdaOffset = 0;
+#endif
+//<(AMI_CHG+) 
+
+/**
+  Check if X2APIC is enabled
+
+  @retval TRUE if enabled
+  @retval FALSE if not enabled
+**/
+BOOLEAN
+IsXapicEnabled (
+  VOID
+  )
+{
+  UINT64 MsrValue;
+
+  MsrValue = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  if (MsrValue & B_MSR_IA32_APIC_BASE_G_XAPIC) {
+    if (MsrValue & B_MSR_IA32_APIC_BASE_M_XAPIC) {
+      return TRUE;
+    } else {
+      return FALSE;
+    }
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Function to get APIC register from MSR or MMIO
+
+  @param[in] XapicEnabled    - x2APIC enabled or not
+  @param[in] MsrIndex        - MSR index of APIC register
+  @param[in] MemoryMappedIo  - MMIO address for APIC register
+
+  @retval The value of APIC register
+**/
+UINT64
+ReadApicMsrOrMemory (
+  BOOLEAN XapicEnabled,
+  UINT32  MsrIndex,
+  UINT64  MemoryMappedIo
+  )
+{
+  UINT64 Value;
+
+  if (XapicEnabled) {
+    Value = AsmReadMsr64 (MsrIndex);
+  } else {
+    Value = (UINT64) *(volatile UINT32 *) (UINTN) MemoryMappedIo;
+  }
+
+  return Value;
+}
+
+/**
+  Function to write APIC register by MSR or MMIO
+
+  @param[in] XapicEnabled    - x2APIC enabled or not
+  @param[in] MsrIndex        - MSR index of APIC register
+  @param[in] MemoryMappedIo  - MMIO address for APIC register
+  @param[in] Value           - Value that will be written to APIC register
+**/
+VOID
+WriteApicMsrOrMemory (
+  BOOLEAN XapicEnabled,
+  UINT32  MsrIndex,
+  UINT64  MemoryMappedIo,
+  UINT64  Value
+  )
+{
+  if (XapicEnabled) {
+    AsmWriteMsr64 (MsrIndex, Value);
+  } else {
+    if (MsrIndex == EXT_XAPIC_ICR) {
+      *(volatile UINT32 *) (UINTN) (MemoryMappedIo - APIC_REGISTER_ICR_LOW_OFFSET + APIC_REGISTER_ICR_HIGH_OFFSET) = (UINT32) (Value >> 32);
+    }
+    *(volatile UINT32 *) (UINTN) MemoryMappedIo = (UINT32) Value;
+  }
+}
+
+/**
+  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
+
+  @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
+  )
+{
+  UINT64               ApicBaseReg;
+  EFI_PHYSICAL_ADDRESS ApicBase;
+  UINT32               ICRLow;
+  UINT32               ICRHigh;
+  BOOLEAN              XapicEnabled;
+
+  ///
+  /// 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;
+  }
+
+  XapicEnabled = IsXapicEnabled ();
+
+  switch (BroadcastMode) {
+    case BROADCAST_MODE_SPECIFY_CPU:
+      if (XapicEnabled) {
+        ICRHigh = (UINT32) ApicID;
+      } else {
+        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 & 0xffffff000;
+
+  ///
+  /// According Nehalem BWG, if Extended XAPIC Mode is enabled,
+  /// legacy xAPIC is no longer working.
+  /// So, previous MMIO offset must be transferred to MSR offset R/W.
+  /// ----------------------------------------------------------------
+  /// MMIO Offset     MSR Offset     Register Name
+  /// ----------------------------------------------------------------
+  ///  300h-310h        830h         Interrupt Command Register [63:0]
+  ///                   831h         [Reserved]
+  /// ----------------------------------------------------------------
+  ///
+  WriteApicMsrOrMemory (
+          XapicEnabled,
+          EXT_XAPIC_ICR,
+          ApicBase + APIC_REGISTER_ICR_LOW_OFFSET,
+          (((UINT64) ICRHigh << 32) | (UINT64) ICRLow)
+          );
+
+  gBS->Stall (10);
+
+  ICRLow = (UINT32) ReadApicMsrOrMemory (XapicEnabled, EXT_XAPIC_ICR, ApicBase + APIC_REGISTER_ICR_LOW_OFFSET);
+
+  if (ICRLow & BIT12) {
+    return EFI_NOT_READY;
+  }
+
+  gBS->Stall (100);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  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;
+}
+
+/**
+  Get APIC ID of processor
+
+  @param[in] ApicBase    - APIC base
+  @param[in] ApicVersion - APIC version
+
+  @retval APIC ID of processor
+**/
+UINT32
+GetApicID (
+  OUT EFI_PHYSICAL_ADDRESS *ApicBase OPTIONAL,
+  OUT UINT32 *ApicVersion            OPTIONAL
+  )
+{
+  UINT64  ApicBaseReg;
+  UINT32  ApicID;
+  UINT32  LocalApicVersion;
+  UINT64  LocalApicBase;
+  UINTN   MsrValue;
+  BOOLEAN XapicEnabled;
+
+  XapicEnabled = IsXapicEnabled ();
+
+  if (XapicEnabled) {
+    ///
+    /// According Nehalem BWG, if Extended XAPIC Mode
+    /// is enabled, legacy xAPIC is no longer working.
+    /// So, previous MMIO offset must be transfered
+    /// to MSR offset R/W.
+    /// MMIO Offset     MSR Offset     Register Name
+    ///  020h             802h         EXT_XAPIC_LOGICAL_APIC_ID
+    ///  030h             803h         EXT_XAPIC_VERSION
+    ///
+    MsrValue      = (UINTN) AsmReadMsr64 (EXT_XAPIC_VERSION);
+    *ApicVersion  = (UINT32) (MsrValue & 0xff);
+    *ApicBase     = 0;
+
+    MsrValue      = (UINTN) AsmReadMsr64 (EXT_XAPIC_LOGICAL_APIC_ID);
+    ApicID        = (UINT32) MsrValue;
+    return ApicID;
+  }
+
+  ApicBaseReg   = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  LocalApicBase = ApicBaseReg & 0xffffff000;
+  if (ApicBase) {
+    *ApicBase = LocalApicBase;
+  }
+
+  ///
+  /// if Apic is not enabled yet, enable it here
+  ///
+  if ((ApicBaseReg & 0x800) == 0) {
+    ApicBaseReg |= 0x800;
+    AsmWriteMsr64 (MSR_IA32_APIC_BASE, ApicBaseReg);
+  }
+
+  if (ApicVersion) {
+    LocalApicVersion  = *(volatile UINT32 *) (UINTN) (LocalApicBase + APIC_REGISTER_APIC_VERSION_OFFSET);
+    *ApicVersion      = LocalApicVersion & 0xff;
+  }
+
+  ApicID = *(volatile UINT32 *) (UINTN) (LocalApicBase + APIC_REGISTER_LOCAL_ID_OFFSET);
+  return (ApicID >> 24) & 0x0ff;
+}
+
+/**
+  Programs Local APIC registers.
+
+  @param[in] BSP  - Is this BSP?
+**/
+VOID
+ProgramXApic (
+  BOOLEAN BSP
+  )
+{
+  UINT64               ApicBaseReg;
+  EFI_PHYSICAL_ADDRESS ApicBase;
+  UINT64               EntryValue;
+  BOOLEAN              XapicEnabled;
+
+  ApicBaseReg   = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  ApicBase      = ApicBaseReg & 0xffffff000;
+
+  XapicEnabled  = IsXapicEnabled ();
+
+  ///
+  /// Program the Spurious Vector entry if XAPIC is enabled
+  ///
+  EntryValue = ReadApicMsrOrMemory (XapicEnabled, EXT_XAPIC_SVR, ApicBase + APIC_REGISTER_SPURIOUS_VECTOR_OFFSET);
+  EntryValue &= 0xFFFFFD0F;
+  EntryValue |= 0x10F;
+  WriteApicMsrOrMemory (XapicEnabled, EXT_XAPIC_SVR, ApicBase + APIC_REGISTER_SPURIOUS_VECTOR_OFFSET, EntryValue);
+
+  ///
+  /// Double check if it is BSP
+  ///
+  if (!BSP) {
+    CpuDisableInterrupt ();
+  }
+
+  ///
+  /// Program the LINT0 vector entry as EntInt
+  ///
+  EntryValue = ReadApicMsrOrMemory (XapicEnabled, EXT_XAPIC_LVT_LINT0, ApicBase + APIC_REGISTER_LINT0_VECTOR_OFFSET);
+  if (BSP) {
+    EntryValue &= 0xFFFE00FF;
+    EntryValue |= 0x700;
+  } else {
+    EntryValue |= 0x10000;
+    ///
+    /// set bit 16 as mask for LINT0
+    ///
+  }
+
+  WriteApicMsrOrMemory (XapicEnabled, EXT_XAPIC_LVT_LINT0, ApicBase + APIC_REGISTER_LINT0_VECTOR_OFFSET, EntryValue);
+
+  ///
+  /// Program the LINT1 vector entry as NMI
+  ///
+  EntryValue = ReadApicMsrOrMemory (XapicEnabled, EXT_XAPIC_LVT_LINT1, ApicBase + APIC_REGISTER_LINT1_VECTOR_OFFSET);
+  EntryValue &= 0xFFFE00FF;
+  if (BSP) {
+    EntryValue |= 0x400;
+  } else {
+    EntryValue |= 0x10400;
+  }
+
+  WriteApicMsrOrMemory (XapicEnabled, EXT_XAPIC_LVT_LINT1, ApicBase + APIC_REGISTER_LINT1_VECTOR_OFFSET, EntryValue);
+
+}
+
+/**
+  Allocate a temporary memory under 1MB for MP Init to perform INIT-SIPI.
+  This buffer also provides memory for stack/data for MP running
+
+  @param[in] WakeUpBuffer  - Return buffer location
+
+  @retval EFI_SUCCESS if ok to get a memory under 1MB for MP running.
+**/
+EFI_STATUS
+AllocateWakeUpBuffer (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer
+  )
+{
+  EFI_STATUS Status;
+
+  Status = EFI_SUCCESS;
+  for (*WakeUpBuffer = 0x58000; *WakeUpBuffer >= 0x2000; *WakeUpBuffer -= 0x1000) {
+    Status = (gBS->AllocatePages)(AllocateAddress, EfiReservedMemoryType, 1, WakeUpBuffer);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Allocate Reserved Memory
+
+  @param[in] Size       - Memory Size
+  @param[in] Alignment  - Alignment size
+  @param[in] Pointer    - return memory location
+
+  @retval EFI_SUCCESS   - Allocate a reserved memory successfully
+**/
+EFI_STATUS
+AllocateAlignedReservedMemory (
+  IN UINTN Size,
+  IN UINTN Alignment,
+  OUT VOID **Pointer
+  )
+{
+  EFI_STATUS Status;
+  UINTN      PointerValue;
+
+  Status = AllocateReservedMemoryBelow4G (
+                  Size + Alignment - 1,
+                  Pointer
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PointerValue  = (UINTN) *Pointer;
+  PointerValue  = (PointerValue + Alignment - 1) / Alignment * Alignment;
+
+  *Pointer      = (VOID *) PointerValue;
+  return EFI_SUCCESS;
+}
+
+/**
+  Fill in the CPU location information
+
+  @param[in] Location  - CPU location information
+
+  @retval EFI_SUCCESS  - always return success
+**/
+EFI_STATUS
+FillInCpuLocation (
+  IN CPU_PHYSICAL_LOCATION *Location
+  )
+{
+  UINT32             ApicId;
+  EFI_CPUID_REGISTER RegsInfo;
+  UINT32             LevelType;
+  UINT32             LevelBits;
+  UINT8              Shift;
+  UINT8              Bits;
+  UINT32             Mask;
+  BOOLEAN            HyperThreadingEnabled;
+
+  AsmCpuid (CPUID_VERSION_INFO, &RegsInfo.RegEax, &RegsInfo.RegEbx, &RegsInfo.RegEcx, &RegsInfo.RegEdx);
+  ApicId = (RegsInfo.RegEbx >> 24);
+
+  AsmCpuid (CPUID_SIGNATURE, &RegsInfo.RegEax, &RegsInfo.RegEbx, &RegsInfo.RegEcx, &RegsInfo.RegEdx);
+  if (RegsInfo.RegEax >= CPUID_CORE_TOPOLOGY) {
+    LevelBits = 0;
+    LevelType = 0;
+    do {
+      AsmCpuidEx (
+              CPUID_CORE_TOPOLOGY,
+              LevelType,
+              &RegsInfo.RegEax,
+              &RegsInfo.RegEbx,
+              &RegsInfo.RegEcx,
+              &RegsInfo.RegEdx
+              );
+      LevelType = ((RegsInfo.RegEcx >> 8) & 0xFF);
+      switch (LevelType) {
+        case 1:
+          ///
+          /// Thread
+          ///
+          Location->Thread  = ApicId & ((1 << (RegsInfo.RegEax & 0x0F)) - 1);
+          LevelBits         = RegsInfo.RegEax & 0x0F;
+          break;
+
+        case 2:
+          ///
+          /// Core
+          ///
+          Location->Core  = ApicId >> LevelBits;
+          LevelBits       = RegsInfo.RegEax & 0x0F;
+          break;
+
+        default:
+          ///
+          /// End of Level
+          ///
+          Location->Die     = 0;
+          Location->Package = ApicId >> LevelBits;
+          break;
+      }
+    } while (!(RegsInfo.RegEax == 0 && RegsInfo.RegEbx == 0));
+  } else {
+
+    AsmCpuid (CPUID_VERSION_INFO, &RegsInfo.RegEax, &RegsInfo.RegEbx, &RegsInfo.RegEcx, &RegsInfo.RegEdx);
+    Bits  = 0;
+    Shift = (UINT8) ((RegsInfo.RegEbx >> 16) & 0xFF);
+
+    Mask  = Shift - 1;
+    while (Shift > 1) {
+      Shift >>= 1;
+      Bits++;
+    }
+
+    HyperThreadingEnabled = FALSE;
+    AsmCpuidEx (CPUID_CACHE_PARAMS, 0, &RegsInfo.RegEax, &RegsInfo.RegEbx, &RegsInfo.RegEcx, &RegsInfo.RegEdx);
+    if (Mask > (RegsInfo.RegEax >> 26)) {
+      HyperThreadingEnabled = TRUE;
+    }
+
+    Location->Package = (ApicId >> Bits);
+    Location->Die     = 0;
+    if (HyperThreadingEnabled) {
+      Location->Core    = (ApicId & Mask) >> 1;
+      Location->Thread  = (ApicId & Mask) & 1;
+    } else {
+      Location->Core    = (ApicId & Mask);
+      Location->Thread  = 0;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Fill in CPU relevant information into data hub
+
+  @param[in] CpuNumber          - CPU number
+  @param[in] CpuDataforDatahub  - pointer to data hub that will be updated
+
+  @retval EFI_SUCCESS           - always return success
+**/
+EFI_STATUS
+FillinDataforDataHub (
+  IN UINTN                 CpuNumber,
+  OUT CPU_DATA_FOR_DATAHUB *CpuDataforDatahub
+  )
+{
+  ZeroMem (CpuDataforDatahub, sizeof (*CpuDataforDatahub));
+
+  ///
+  /// Read Cpu Frequency from MSR instead
+  ///
+  CpuDataforDatahub->IntendCoreFrequency =
+                  (
+                    100 *
+                    (((UINT32) EfiReadMsr (MSR_IA32_PERF_STS) >> N_IA32_PERF_STSP_STATE_TARGET) & B_IA32_PERF_STSP_STATE_MASK)
+                  );
+
+  GetProcessorVersion (&CpuDataforDatahub->Version);
+  CpuDataforDatahub->Manufacturer = GetProcessorManufacturer ();
+
+  EfiCpuid (CPUID_VERSION_INFO, (EFI_CPUID_REGISTER *) &CpuDataforDatahub->CpuidData);
+
+  CpuDataforDatahub->Family   = GetProcessorFamily ();
+  CpuDataforDatahub->Voltage  = GetProcessorVoltage ();
+  CpuDataforDatahub->ApicID = GetApicID (
+                  &CpuDataforDatahub->ApicBase,
+                  &CpuDataforDatahub->ApicVersion
+                  );
+
+  CpuDataforDatahub->MicrocodeRevision = GetCpuUcodeRevision ();
+  EfiCpuid (CPUID_CACHE_INFO, CpuDataforDatahub->CacheInformation);
+
+  ///
+  /// Status field will be updated later, after calling CpuPlatformPolicy protocol to override
+  ///
+  CpuDataforDatahub->Status = GetProcessorStatus (CpuNumber);;
+
+  FillInCpuLocation (&CpuDataforDatahub->Location);
+
+  ///
+  /// Health field will be filled in else where
+  ///
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate EfiReservedMemoryType below 4G memory address.
+
+  @param[in] Size      - Size of memory to allocate.
+  @param[in] Buffer    - Allocated address for output.
+
+  @retval EFI_SUCCESS  - Memory successfully allocated.
+  @retval Other        - Other errors occur.
+**/
+EFI_STATUS
+AllocateReservedMemoryBelow4G (
+  IN UINTN Size,
+  OUT VOID **Buffer
+  )
+{
+  UINTN                Pages;
+  EFI_PHYSICAL_ADDRESS Address;
+  EFI_STATUS           Status;
+
+  Pages   = EFI_SIZE_TO_PAGES (Size);
+  Address = 0xffffffff;
+
+  Status  = (gBS->AllocatePages)(AllocateMaxAddress, EfiReservedMemoryType, Pages, &Address);
+
+  *Buffer = (VOID *) (UINTN) Address;
+
+  return Status;
+}
+
+/**
+  This function is invoked when SMM_BASE protocol is installed, then we
+  allocate SMRAM and save all information there.
+
+  @param[in] Event   - The triggered event.
+  @param[in] Context - Context for this event.
+**/
+VOID
+EFIAPI
+InitializeSmramDataContent (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_SMM_BASE_PROTOCOL    *SmmBase;
+  SMRAM_CPU_DATA           SmramCpuDataTemplate;
+  UINTN                    LockBoxSize;
+  UINT8                    *LockBoxData;
+  PSEUDO_DESCRIPTOR        *Idtr;
+  PSEUDO_DESCRIPTOR        *Gdtr;
+  UINTN                    MicrocodeSize;
+  EFI_CPU_MICROCODE_HEADER **Microcode;
+  UINT8                    *LockBoxMicrocode;
+  UINTN                    Index;
+  EFI_STATUS               Status;
+  EFI_SMM_CONTROL_PROTOCOL *SmmControl;
+  UINT8                    *SmramCpuData;
+  UINTN                    VarSize;
+  SMRAM_CPU_DATA_ADDRESS   SmramCpuDataAddr;
+  UINTN                    ArgBufferSize;
+  UINT8                    ArgBuffer;
+  EFI_SMM_CONTROL_REGISTER SmiRegister;
+
+  DEBUG ((EFI_D_INFO, "InitializeSmramDataContent\n"));
+
+  Status = gBS->LocateProtocol (&gEfiSmmBaseProtocolGuid, NULL, (VOID **) &SmmBase);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->LocateProtocol (&gEfiSmmControlProtocolGuid, NULL, (VOID **) &SmmControl);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Init
+  ///
+  CopyMem (&SmramCpuDataTemplate.HeaderGuid, &gSmramCpuDataHeaderGuid, sizeof (EFI_GUID));
+  SmramCpuDataTemplate.AcpiCpuPointer = (EFI_PHYSICAL_ADDRESS) (UINTN) mAcpiCpuData;
+  CopyMem (&SmramCpuDataTemplate.AcpiCpuData, mAcpiCpuData, sizeof (ACPI_CPU_DATA));
+
+  ///
+  /// Calculate size
+  ///
+  SmramCpuDataTemplate.GdtrProfileSize        = sizeof (PSEUDO_DESCRIPTOR);
+  Gdtr = (PSEUDO_DESCRIPTOR *) (UINTN) mAcpiCpuData->GdtrProfile;
+  SmramCpuDataTemplate.GdtSize = Gdtr->Limit + 1;
+  SmramCpuDataTemplate.IdtrProfileSize = sizeof (PSEUDO_DESCRIPTOR);
+  Idtr = (PSEUDO_DESCRIPTOR *) (UINTN) mAcpiCpuData->GdtrProfile;
+  SmramCpuDataTemplate.IdtSize = Idtr->Limit + 1;
+  SmramCpuDataTemplate.CpuPrivateDataSize = sizeof (MP_CPU_S3_DATA_POINTER);
+  SmramCpuDataTemplate.S3BootScriptTableSize = sizeof (mMPSystemData->S3BootScriptTable);
+  SmramCpuDataTemplate.S3BspMtrrTableSize = sizeof (mMPSystemData->S3BspMtrrTable);
+  ///
+  /// Record best match for each CPU Microcode and NULL for end
+  ///
+  SmramCpuDataTemplate.MicrocodePointerBufferSize = sizeof (UINT32) * (mAcpiCpuData->NumberOfCpus + 1);
+  ///
+  /// Calculate Microcode DataSize
+  ///
+  SmramCpuDataTemplate.MicrocodeDataBufferSize  = 0;
+  Microcode = (VOID *) (UINTN) mAcpiCpuData->MicrocodePointerBuffer;
+  if (Microcode != NULL) {
+    Index         = 0;
+    MicrocodeSize = 0;
+    while (Microcode[Index] != NULL) {
+      if (Microcode[Index]->DataSize == 0) {
+        MicrocodeSize = 2048;
+      } else {
+        MicrocodeSize = Microcode[Index]->TotalSize;
+      }
+
+      SmramCpuDataTemplate.MicrocodeDataBufferSize += (UINT32) MicrocodeSize;
+      Index++;
+    }
+  }
+
+  SmramCpuDataTemplate.GdtrProfileOffset    = sizeof (SMRAM_CPU_DATA);
+  SmramCpuDataTemplate.GdtOffset            = SmramCpuDataTemplate.GdtrProfileOffset + SmramCpuDataTemplate.GdtrProfileSize;
+  SmramCpuDataTemplate.IdtrProfileOffset    = SmramCpuDataTemplate.GdtOffset + SmramCpuDataTemplate.GdtSize;
+  SmramCpuDataTemplate.IdtOffset            = SmramCpuDataTemplate.IdtrProfileOffset + SmramCpuDataTemplate.IdtrProfileSize;
+  SmramCpuDataTemplate.CpuPrivateDataOffset = SmramCpuDataTemplate.IdtOffset + SmramCpuDataTemplate.IdtSize;
+  SmramCpuDataTemplate.S3BootScriptTableOffset = SmramCpuDataTemplate.CpuPrivateDataOffset + SmramCpuDataTemplate.CpuPrivateDataSize;
+  SmramCpuDataTemplate.S3BspMtrrTableOffset = SmramCpuDataTemplate.S3BootScriptTableOffset + SmramCpuDataTemplate.S3BootScriptTableSize;
+  SmramCpuDataTemplate.MicrocodePointerBufferOffset = SmramCpuDataTemplate.S3BspMtrrTableOffset + SmramCpuDataTemplate.S3BspMtrrTableSize;
+  SmramCpuDataTemplate.MicrocodeDataBufferOffset = SmramCpuDataTemplate.MicrocodePointerBufferOffset + SmramCpuDataTemplate.MicrocodePointerBufferSize;
+
+  LockBoxSize = sizeof (SMRAM_CPU_DATA) +
+          SmramCpuDataTemplate.GdtrProfileSize +
+          SmramCpuDataTemplate.GdtSize +
+          SmramCpuDataTemplate.IdtrProfileSize +
+          SmramCpuDataTemplate.IdtSize +
+          SmramCpuDataTemplate.CpuPrivateDataSize +
+          SmramCpuDataTemplate.S3BootScriptTableSize +
+          SmramCpuDataTemplate.S3BspMtrrTableSize +
+          SmramCpuDataTemplate.MicrocodePointerBufferSize +
+          SmramCpuDataTemplate.MicrocodeDataBufferSize;
+
+  DEBUG ((EFI_D_INFO, "LockBoxSize                               - %x\n", LockBoxSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.GdtrProfileSize              - %x\n", SmramCpuDataTemplate.GdtrProfileSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.GdtSize                      - %x\n", SmramCpuDataTemplate.GdtSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.IdtrProfileSize              - %x\n", SmramCpuDataTemplate.IdtrProfileSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.IdtSize                      - %x\n", SmramCpuDataTemplate.IdtSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.CpuPrivateDataSize           - %x\n", SmramCpuDataTemplate.CpuPrivateDataSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.S3BootScriptTableSize        - %x\n", SmramCpuDataTemplate.S3BootScriptTableSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.S3BspMtrrTableSize           - %x\n", SmramCpuDataTemplate.S3BspMtrrTableSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.MicrocodePointerBufferSize   - %x\n", SmramCpuDataTemplate.MicrocodePointerBufferSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.MicrocodeDataBufferSize      - %x\n", SmramCpuDataTemplate.MicrocodeDataBufferSize));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.GdtrProfileOffset            - %x\n", SmramCpuDataTemplate.GdtrProfileOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.GdtOffset                    - %x\n", SmramCpuDataTemplate.GdtOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.IdtrProfileOffset            - %x\n", SmramCpuDataTemplate.IdtrProfileOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.IdtOffset                    - %x\n", SmramCpuDataTemplate.IdtOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.CpuPrivateDataOffset         - %x\n", SmramCpuDataTemplate.CpuPrivateDataOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.S3BootScriptTableOffset      - %x\n", SmramCpuDataTemplate.S3BootScriptTableOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.S3BspMtrrTableOffset         - %x\n", SmramCpuDataTemplate.S3BspMtrrTableOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.MicrocodePointerBufferOffset - %x\n", SmramCpuDataTemplate.MicrocodePointerBufferOffset));
+  DEBUG ((EFI_D_INFO, "SmramCpuData.MicrocodeDataBufferOffset    - %x\n", SmramCpuDataTemplate.MicrocodeDataBufferOffset));
+
+  ///
+  /// Allocate Normal Memory
+  ///
+  Status = (gBS->AllocatePool)(EfiBootServicesData, LockBoxSize, &SmramCpuData);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Allocate SMRAM
+  ///
+  Status = SmmBase->SmmAllocatePool (
+                  SmmBase,
+                  EfiRuntimeServicesData,
+                  LockBoxSize + EFI_PAGE_SIZE,
+                  &LockBoxData
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Let it page aligned
+  ///
+  LockBoxData = (UINT8 *) (((UINTN) LockBoxData + EFI_PAGE_SIZE - 1) &~(EFI_PAGE_SIZE - 1));
+  DEBUG ((EFI_D_INFO, "CPU SMRAM NVS Data - %x\n", LockBoxData));
+
+  ///
+  /// Copy data buffer
+  ///
+  CopyMem (SmramCpuData, &SmramCpuDataTemplate, sizeof (SmramCpuDataTemplate));
+
+  CopyMem (
+          SmramCpuData + SmramCpuDataTemplate.GdtrProfileOffset,
+          (VOID *) (UINTN) mAcpiCpuData->GdtrProfile,
+          SmramCpuDataTemplate.GdtrProfileSize
+          );
+  CopyMem (
+          SmramCpuData + SmramCpuDataTemplate.GdtOffset,
+          (VOID *) (UINTN) Gdtr->Base,
+          SmramCpuDataTemplate.GdtSize
+          );
+  CopyMem (
+          SmramCpuData + SmramCpuDataTemplate.IdtrProfileOffset,
+          (VOID *) (UINTN) mAcpiCpuData->IdtrProfile,
+          SmramCpuDataTemplate.IdtrProfileSize
+          );
+  CopyMem (
+          SmramCpuData + SmramCpuDataTemplate.IdtOffset,
+          (VOID *) (UINTN) Idtr->Base,
+          SmramCpuDataTemplate.IdtSize
+          );
+  CopyMem (
+          SmramCpuData + SmramCpuDataTemplate.CpuPrivateDataOffset,
+          (VOID *) (UINTN) mAcpiCpuData->CpuPrivateData,
+          SmramCpuDataTemplate.CpuPrivateDataSize
+          );
+  CopyMem (
+          SmramCpuData + SmramCpuDataTemplate.S3BootScriptTableOffset,
+          (VOID *) (UINTN) mMPSystemData->S3DataPointer.S3BootScriptTable,
+          SmramCpuDataTemplate.S3BootScriptTableSize
+          );
+  CopyMem (
+          SmramCpuData + SmramCpuDataTemplate.S3BspMtrrTableOffset,
+          (VOID *) (UINTN) mMPSystemData->S3DataPointer.S3BspMtrrTable,
+          SmramCpuDataTemplate.S3BspMtrrTableSize
+          );
+  Microcode = (VOID *) (UINTN) mAcpiCpuData->MicrocodePointerBuffer;
+  if (Microcode != NULL) {
+    ///
+    /// Copy Microcode Pointer Buffer
+    ///
+    CopyMem (
+            SmramCpuData + SmramCpuDataTemplate.MicrocodePointerBufferOffset,
+            Microcode,
+            SmramCpuDataTemplate.MicrocodePointerBufferSize
+            );
+    ///
+    /// Copy Microcode Data
+    ///
+    Index             = 0;
+    MicrocodeSize     = 0;
+    LockBoxMicrocode  = SmramCpuData + SmramCpuDataTemplate.MicrocodeDataBufferOffset;
+    while (Microcode[Index] != NULL) {
+      if (Microcode[Index]->DataSize == 0) {
+        MicrocodeSize = 2048;
+      } else {
+        MicrocodeSize = Microcode[Index]->TotalSize;
+      }
+
+      CopyMem (LockBoxMicrocode, Microcode[Index], MicrocodeSize);
+      LockBoxMicrocode += MicrocodeSize;
+      Index++;
+    }
+  }
+
+  ///
+  /// Copy to SMRAM
+  ///
+  ///
+  /// We have to use SMI to copy SMRAM, because we can not access SMRAM after SMRR enabled.
+  /// SMM_ACCESS.Open () takes no effect.
+  ///
+  VarSize                       = sizeof (SmramCpuDataAddr);
+  SmramCpuDataAddr.LockBoxData  = (UINT64) (UINTN) LockBoxData;
+  SmramCpuDataAddr.SmramCpuData = (UINT64) (UINTN) SmramCpuData;
+  SmramCpuDataAddr.LockBoxSize  = (UINT64) LockBoxSize;
+
+  Status = gRT->SetVariable (
+                  SMRAM_CPU_DATA_VARIABLE,
+                  &gSmramCpuDataVariableGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                  VarSize,
+                  &SmramCpuDataAddr
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Fill SMI data port
+  ///
+  Status = SmmControl->GetRegisterInfo (SmmControl, &SmiRegister);
+  ASSERT_EFI_ERROR (Status);
+  IoWrite8 (SmiRegister.SmiDataRegister, SMM_FROM_CPU_DRIVER_SAVE_INFO);
+
+  ///
+  /// Trigger SMI
+  ///
+  ArgBufferSize = sizeof (ArgBuffer);
+  ArgBuffer     = mSmmbaseSwSmiNumber;
+  Status        = SmmControl->Trigger (SmmControl, (INT8 *) &ArgBuffer, &ArgBufferSize, FALSE, 0);
+  Status        = SmmControl->Clear (SmmControl, 0);
+  return;
+}
+
+/**
+  This function is invoked when LegacyBios protocol is installed, we must
+  allocate reserved memory under 1M for AP.
+
+  @param[in] Event    - The triggered event.
+  @param[in] Context  - Context for this event.
+**/
+VOID
+EFIAPI
+ReAllocateEbdaMemoryForAP (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_LEGACY_BIOS_PROTOCOL *LegacyBios;
+  EFI_PHYSICAL_ADDRESS     EbdaOld;
+  EFI_PHYSICAL_ADDRESS     EbdaNew;
+  UINTN                    EbdaSize;
+  EFI_STATUS               Status;
+
+  ///
+  /// Check whether this is real LegacyBios notification
+  ///
+  Status = gBS->LocateProtocol (&gEfiLegacyBiosProtocolGuid, NULL, (VOID **) &LegacyBios);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  ///
+  /// PLEASE NOTE:
+  /// For legacy implementation, we have reserved 0x9F000 to 0x9FFFF for S3 usage in CSM,
+  /// No don't need to allocate it again
+  /// This range will be used for MpS3 driver and S3Resume driver on S3 boot path
+  /// The base needs to be aligned to 4K to satisfy the AP vector requirement
+  /// The original implementation requires 8K from legacy memory form E/F segment,
+  /// which needs lock/unlock and makes lots of code chipset dependent on S3 boot path
+  /// Here we just use normal low memory to eliminate the dependency
+  /// In this case, EBDA will start from 0x9F000 - sizeof (EBDA) in CSM definition
+  /// CSM EBDA base and memory size in BDA area needs to be consistent with this
+  ///
+  ///
+  /// Get EDBA address/length and turn it into the S3 reserved address
+  /// The length of this range is limited so we need to keep the real mode code small
+  ///
+  EbdaOld                     = (EFI_PHYSICAL_ADDRESS) (*(UINT16 *) (UINTN) 0x40E) << 4;;
+  EbdaSize                    = (UINTN) (*((UINT8 *) (UINTN) EbdaOld));
+  
+//(AMI_CHG+)>
+#if (REQUEST_EBDA_SIZE == 0x1000)
+  mLegacyRegion               = EbdaOld + (EbdaSize << 10);
+  mLegacyRegion               = (mLegacyRegion - 0x1000) & 0xFFFFF000;
+  EbdaNew                     = mLegacyRegion - (EbdaSize << 10);
+#else
+  *(UINT8 *) ((UINTN) EbdaOld)  = (UINT8)(EbdaSize + 8);
+  mLegacyRegion               = EbdaOld + (EbdaSize << 10);
+  mLegacyRegion               = (mLegacyRegion - REQUEST_EBDA_SIZE) & 0xFFFFF000;
+  EbdaNew                     = mLegacyRegion - (EbdaSize << 10);
+  mEbdaOffset                 = EbdaSize << 10;
+#endif 
+//<(AMI_CHG+)
+
+  (*(UINT16 *) (UINTN) 0x40E) = (UINT16) (EbdaNew >> 4);
+  CopyMem ((VOID *) (UINTN) EbdaNew, (VOID *) (UINTN) EbdaOld, EbdaSize << 10);
+
+  ///
+  /// Update 40:13 with the new size of available base memory
+  ///
+  *(UINT16 *) (UINTN) 0x413 = (*(UINT16 *) (UINTN) 0x413) - (UINT16) (((EbdaOld - EbdaNew) >> 10));
+
+  ///
+  /// Free the Wake-up buffer and re-declare it as Reserved Memory
+  ///
+  DEBUG ((EFI_D_INFO, "Legacy region freed before re-allocation:  %X\n", mLegacyRegion));
+  Status  = (gBS->FreePages) (mLegacyRegion, 1);
+  ASSERT_EFI_ERROR (Status);
+  DEBUG ((EFI_D_INFO, "Allocate and reserve the 4K buffer for Legacy Region\n"));
+  Status = (gBS->AllocatePages)(AllocateAddress, EfiReservedMemoryType, 1, &mLegacyRegion);
+  ASSERT_EFI_ERROR (Status);
+  DEBUG ((EFI_D_INFO, "mLegacyRegion CSM - %x\n", mLegacyRegion));
+}
+
+/**
+  This function is invoked when LegacyBios protocol is installed, we must
+  allocate reserved memory under 1M for AP.
+
+  @param[in] Event    - The triggered event.
+  @param[in] Context  - Context for this event.
+**/
+VOID
+EFIAPI
+ReAllocateMemoryForAP (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  EFI_LEGACY_BIOS_PROTOCOL *LegacyBios;
+  EFI_STATUS               Status;
+  EFI_PHYSICAL_ADDRESS     LegacyRegion;
+  MP_CPU_EXCHANGE_INFO     *ExchangeInfo;
+  MONITOR_MWAIT_DATA       *MonitorAddr;
+  UINTN                    Index;
+  UINT64                   MaxCstate;
+  UINT64                   CStateLimit;
+  UINT32                   SubStates;
+  EFI_CPUID_REGISTER       MwaitInfo;
+  BOOLEAN                  HasCsm;
+  EFI_MP_SERVICES_PROTOCOL *MpService;
+
+  VOID           *ExitPmAuth;
+  STATIC BOOLEAN InitDone = FALSE;
+
+  ///
+  /// Check whether this is real ExitPmAuth notification
+  ///
+  Status = gBS->LocateProtocol (&gExitPmAuthProtocolGuid, NULL, &ExitPmAuth);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  ///
+  /// Make sure it is invoked only once.
+  ///
+  if (InitDone) {
+    return;
+  }
+
+  InitDone  = TRUE;
+
+  Status    = gBS->LocateProtocol (&gEfiLegacyBiosProtocolGuid, NULL, (VOID **) &LegacyBios);
+  if (EFI_ERROR (Status)) {
+    HasCsm = FALSE;
+  } else {
+    HasCsm = TRUE;
+  }
+
+  while (ApRunning ()) {
+    CpuPause ();
+  }
+  ///
+  /// Re-load microcode patch here!!!
+  ///
+  ReLoadMicrocodeBeforeBoot ();
+
+  if (HasCsm) {
+//(AMI_CHG+)>
+#if (REQUEST_EBDA_SIZE == 0x1000)
+    LegacyRegion = mLegacyRegion;
+#else
+	EFI_PHYSICAL_ADDRESS     CurEbda;
+    
+    CurEbda      = (EFI_PHYSICAL_ADDRESS) (*(UINT16 *) (UINTN) 0x40E) << 4;
+    LegacyRegion = (EFI_PHYSICAL_ADDRESS) ((UINTN)CurEbda + mEbdaOffset);
+    LegacyRegion += 0x1000;
+    LegacyRegion &= 0xffff000;
+#endif 
+//<(AMI_CHG+)
+    DEBUG ((EFI_D_INFO, "Using LegacyRegion CSM - %x\n", LegacyRegion));
+  } else {
+    ///
+    /// The BackBuffer is 4k. Allocate 0x2000 bytes from below 640K memory to ensure 4k aligned spaces of 0x1000 bytes,
+    /// since Alignment argument does not work.
+    ///
+    LegacyRegion = 0x9FFFF;
+    Status = (gBS->AllocatePages)(AllocateMaxAddress, EfiReservedMemoryType, EFI_SIZE_TO_PAGES (0x2000), &LegacyRegion);
+    ASSERT_EFI_ERROR (Status);
+    DEBUG ((EFI_D_INFO, "LegacyRegion NonCSM - %x\n", LegacyRegion));
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+  }
+  ///
+  /// This address should be less than A seg.
+  /// And it should be aligned to 4K
+  ///
+  ASSERT (!((UINTN) LegacyRegion & 0x0FFF) && ((UINTN) LegacyRegion < 0xA0000));
+
+  mAcpiCpuData->WakeUpBuffer  = (EFI_PHYSICAL_ADDRESS) LegacyRegion;
+  mAcpiCpuData->WakeUpBuffer  = (mAcpiCpuData->WakeUpBuffer + 0x0fff) & 0x0fffff000;
+
+  ExchangeInfo                = (MP_CPU_EXCHANGE_INFO *) (UINTN) (mBackupBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+  ExchangeInfo->BufferStart   = (UINT32) mAcpiCpuData->WakeUpBuffer;
+  CopyMem (
+          (VOID *) (UINTN) mAcpiCpuData->WakeUpBuffer,
+          (VOID *) (UINTN) mBackupBuffer,
+          EFI_PAGE_SIZE
+          );
+  RedirectFarJump ();
+
+  if (HasCsm) {
+    Status = LegacyBios->CopyLegacyRegion (
+                    LegacyBios,
+                    sizeof (MP_CPU_EXCHANGE_INFO),
+                    (VOID *) (UINTN) (mAcpiCpuData->WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET),
+                    (VOID *) (UINTN) (mBackupBuffer + MP_CPU_EXCHANGE_INFO_OFFSET)
+                    );
+  }
+
+  ///
+  /// Set all APs to deepest C-State before ready to boot for better power saving,
+  /// if boot to DOS/EFI_SHARE or any operating system that running only single thread.
+  ///
+  ExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (mAcpiCpuData->WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+  if (mPlatformCpu->CpuConfig->ApHandoffManner != WakeUpApPerHltLoop) {
+    ///
+    /// Based on HSW BWG 17.2.7, BIOS should use CPUID.(EAX=5) Monitor/Mwait Leaf and also check MSR E2h[3:0] Package C-state limit to determine
+    /// if the processor supports MONITOR/MWAIT extensions for various Haswell specific C-states and sub C-states.
+    ///
+    CStateLimit = AsmReadMsr64 (MSR_PMG_CST_CONFIG) & B_PACKAGE_C_STATE_LIMIT;
+    AsmCpuid (5, &MwaitInfo.RegEax, &MwaitInfo.RegEbx, &MwaitInfo.RegEcx, &MwaitInfo.RegEdx);
+    MaxCstate = 0;
+    SubStates = 0;
+    if (MwaitInfo.RegEcx & BIT0) {
+      switch (CStateLimit) {
+        case V_CSTATE_LIMIT_C10:
+          SubStates = (MwaitInfo.RegEdx & (BIT31 | BIT30 | BIT29 | BIT28)) >> 28;
+          MaxCstate = 0x60 | (SubStates - 1);
+          break;
+
+        case V_CSTATE_LIMIT_C9:
+          SubStates = (MwaitInfo.RegEdx & (BIT27 | BIT26 | BIT25 | BIT24)) >> 24;
+          MaxCstate = 0x50 | (SubStates - 1);
+          break;
+
+        case V_CSTATE_LIMIT_C8:
+          SubStates = (MwaitInfo.RegEdx & (BIT23 | BIT22 | BIT21 | BIT20)) >> 20;
+          MaxCstate = 0x40 | (SubStates - 1);
+          break;
+
+        case V_CSTATE_LIMIT_C7S:
+          SubStates = (MwaitInfo.RegEdx & (BIT19 | BIT18 | BIT17 | BIT16)) >> 16;
+          MaxCstate = 0x30 | (SubStates - 1);
+          break;
+
+        case V_CSTATE_LIMIT_C7:
+          SubStates = (MwaitInfo.RegEdx & (BIT19 | BIT18 | BIT17 | BIT16)) >> 16;
+          MaxCstate = 0x30 | (SubStates - 1);
+          break;
+
+        case V_CSTATE_LIMIT_C6:
+          SubStates = (MwaitInfo.RegEdx & (BIT15 | BIT14 | BIT13 | BIT12)) >> 12;
+          MaxCstate = 0x20 | (SubStates - 1);
+          break;
+
+        case V_CSTATE_LIMIT_C3:
+          SubStates = (MwaitInfo.RegEdx & (BIT11 | BIT10 | BIT9 | BIT8)) >> 8;
+          MaxCstate = 0x10 | (SubStates - 1);
+          break;
+
+        case V_CSTATE_LIMIT_C1:
+          SubStates = (MwaitInfo.RegEdx & (BIT7 | BIT6 | BIT5 | BIT4)) >> 4;
+          MaxCstate = 0x00 | (SubStates - 1);
+          break;
+
+        default:
+          break;
+      }
+    }
+
+    ///
+    /// Use WakeUpApPerMwaitLoop32 if CR4 paging table entities are not allocated as RESERVED MEMORY TYPE in 64-bits mode.
+    ///
+    ExchangeInfo->WakeUpApManner  = WakeUpApPerMwaitLoop32;
+    for (Index = 0; Index < mMPSystemData->NumberOfCpus; Index++) {
+      MonitorAddr = (MONITOR_MWAIT_DATA *) ((UINT8 *) ExchangeInfo->StackStart + (Index + 1) * ExchangeInfo->StackSize - MONITOR_FILTER_SIZE);
+      MonitorAddr->WakeUpApVectorChangeFlag = TRUE;
+      MonitorAddr->MwaitTargetCstate        = MaxCstate;
+    }
+  } else {
+    ExchangeInfo->WakeUpApManner = WakeUpApPerHltLoop;
+  }
+
+  ///
+  /// Locate MpServices protocol
+  ///
+  Status = gBS->LocateProtocol (
+                  &gEfiMpServiceProtocolGuid,
+                  NULL,
+                  (VOID **) &MpService
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Move Limit CPUID Maxval configuration here to not impact the BOOT
+  /// After setting this, no code can execute CPUID function > 3.
+  ///
+  ProgramCpuidLimit (MpService);
+  Status = MpService->StartupAllAPs (
+                  MpService,
+                  ProgramCpuidLimit,
+                  FALSE,
+                  NULL,
+                  0,
+                  MpService,
+                  NULL
+                  );
+
+  ///
+  /// Invoke the InitializeSmram directly, since it is in ExitPmAuth event.
+  ///
+  InitializeSmramDataContent (NULL, NULL);
+}
+
+/**
+  This function is invoked by EFI_EVENT_SIGNAL_LEGACY_BOOT.
+  Before booting to legacy OS, reset AP's wakeup buffer address,
+  preparing for S3 usage.
+
+  @param[in] Event    - The triggered event.
+  @param[in] Context  - Context for this event.
+**/
+VOID
+ResetAPs (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  return;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MpCommon.h b/ReferenceCode/Haswell/CpuInit/Dxe/MpCommon.h
new file mode 100644
index 0000000..7677e80
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MpCommon.h
@@ -0,0 +1,606 @@
+/** @file
+  Some definitions for MP and HT 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 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 _MP_COMMON_
+#define _MP_COMMON_
+
+#include "ProcessorData.h"
+#include "CacheData.h"
+#include "Exception.h"
+#include "ProcessorDef.h"
+
+//
+// Protocol produced by this driver
+//
+#include EFI_PROTOCOL_PRODUCER (MpService)
+
+//
+// Protocol consumed by this driver
+//
+#include EFI_PROTOCOL_DEFINITION (CpuPlatformPolicy)
+
+//
+// GUID definitions
+//
+#include EFI_GUID_DEFINITION (HtBistHOB)
+#include EFI_GUID_DEFINITION (SmramCpuDataVariable)
+#include EFI_GUID_DEFINITION (SmramCpuDataHeader)
+
+#define VacantFlag          0x00
+#define NotVacantFlag       0xff
+#define MICROSECOND         10
+#define MAXIMUM_CPU_NUMBER  0x40
+#define STACK_SIZE_PER_PROC 0x8000
+
+#define IO_APIC_INDEX_REGISTER        0xFEC00000
+#define IO_APIC_DATA_REGISTER         0xFEC00010
+
+///
+/// Data structure used in MP/HT driver
+///
+#define MP_CPU_EXCHANGE_INFO_OFFSET     (0x1000 - 0x400)
+#define MP_CPU_LEGACY_RESET_INFO_OFFSET (0x100 - 0x20)
+
+#define SMM_FROM_CPU_DRIVER_SAVE_INFO   0x81
+
+#pragma pack(1)
+#define SIZE_OF_MCE_HANDLER 16
+
+typedef struct {
+  UINT16 LimitLow;
+  UINT16 BaseLow;
+  UINT8  BaseMiddle;
+  UINT16 Attributes;
+  UINT8  BaseHigh;
+} SEGMENT_DESCRIPTOR;
+
+#pragma pack()
+
+#define BREAK_TO_RUN_AP_SIGNAL  0x6E755200
+#define MONITOR_FILTER_SIZE     0x40
+
+typedef enum {
+  WakeUpApCounterInit   = 0,
+  WakeUpApPerHltLoop    = 1,
+  WakeUpApPerMwaitLoop  = 2,
+  WakeUpApPerRunLoop    = 3,
+  WakeUpApPerMwaitLoop32= 4,
+  WakeUpApPerRunLoop32  = 5
+} WAKEUP_AP_MANNER;
+
+typedef struct {
+  UINTN BreakToRunApSignal;
+  UINTN HltLoopBreakCounter;
+  UINTN MwaitLoopBreakCounter;
+  UINTN RunLoopBreakCounter;
+  UINTN MwaitLoopBreakCounter32;
+  UINTN RunLoopBreakCounter32;
+  UINTN WakeUpApVectorChangeFlag;
+  UINTN MwaitTargetCstate;
+} MONITOR_MWAIT_DATA;
+
+typedef struct {
+  UINT32 Number;
+  UINT32 BIST;
+} BIST_INFO;
+
+typedef struct {
+  UINTN             Lock;
+  VOID              *StackStart;
+  UINTN             StackSize;
+  VOID              *ApFunction;
+  PSEUDO_DESCRIPTOR GdtrProfile;
+  PSEUDO_DESCRIPTOR IdtrProfile;
+  UINT32            BufferStart;
+  UINT32            Cr3;
+  UINT32            InitFlag;
+  WAKEUP_AP_MANNER  WakeUpApManner;
+  BIST_INFO         BistBuffer[MAXIMUM_CPU_NUMBER];
+} MP_CPU_EXCHANGE_INFO;
+
+extern ACPI_CPU_DATA *mAcpiCpuData;
+
+//
+// Protocol interface functions
+//
+/**
+  Get general MP information
+
+  @param[in] This                  - EFI_MP_SERVICE_PROTOCOL
+  @param[in] NumberOfCPUs          - Number of processors
+  @param[in] MaxiumNumberOfCPUs    - Max supported number of processors
+  @param[in] NumberOfEnabledCPUs   - Number of processors enabled
+  @param[in] RendezvousIntNumber   - number of Rendezvous procedure
+  @param[in] RendezvousProcLength  - length of Rendezvous procedure
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+EFIAPI
+GetGeneralMPInfo (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                   *NumberOfCPUs,
+  OUT UINTN                   *MaxiumNumberOfCPUs,
+  OUT UINTN                   *NumberOfEnabledCPUs,
+  OUT UINTN                   *RendezvousIntNumber,
+  OUT UINTN                   *RendezvousProcLength
+  );
+
+/**
+  Get processor context
+
+  @param[in] This                    - EFI_MP_SERVICE_PROTOCOL
+  @param[in] ProcessorNumber         - Cpu number
+  @param[in] BufferLength            - buffer length
+  @param[in] ProcessorContextBuffer  - pointer to the buffer that will be updated
+
+  @retval EFI_INVALID_PARAMETER - buffer is NULL or CpuNumber our of range
+  @retval EFI_BUFFER_TOO_SMALL - buffer too small
+  @retval EFI_SUCCESS - got processor context successfully
+**/
+EFI_STATUS
+EFIAPI
+GetProcessorContext (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    ProcessorNumber,
+  IN OUT UINTN                *BufferLength,
+  IN OUT EFI_MP_PROC_CONTEXT  *ProcessorContextBuffer
+  );
+
+/**
+  MP Service to get specified application processor (AP)
+  to execute a caller-provided code stream.
+
+  @param[in] This                - Pointer to MP Service Protocol
+  @param[in] Procedure           - The procedure to be assigned to AP.
+  @param[in] ProcessorNumber     - Cpu number
+  @param[in] WaitEvent           - If timeout, the event to be triggered after this AP finishes.
+  @param[in] TimeoutInMicroSecs  - The timeout value in microsecond. Zero means infinity.
+  @param[in] ProcArguments       - Argument for Procedure.
+
+  @retval EFI_INVALID_PARAMETER - Procudure is NULL.
+  @retval EFI_INVALID_PARAMETER - Number of CPU out of range, or it belongs to BSP.
+  @retval EFI_INVALID_PARAMETER - Specified CPU is not idle.
+  @retval EFI_SUCCESS           - The AP has finished.
+  @retval EFI_TIMEOUT           - Time goes out before the AP has finished.
+**/
+EFI_STATUS
+EFIAPI
+StartupThisAP (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE         Procedure,
+  IN UINTN                    ProcessorNumber,
+  IN EFI_EVENT WaitEvent      OPTIONAL,
+  IN UINTN TimeoutInMicroSecs OPTIONAL,
+  IN OUT VOID *ProcArguments  OPTIONAL
+  );
+
+/**
+  MP Service to get all the available application processors (APs)
+  to execute a caller-provided code stream.
+
+  @param[in] This                - Pointer to MP Service Protocol
+  @param[in] Procedure           - The procedure to be assigned to APs.
+  @param[in] SingleThread        - If true, all APs execute in block mode.
+                        Otherwise, all APs exceute in non-block mode.
+  @param[in] WaitEvent           - If timeout, the event to be triggered after all APs finish.
+  @param[in] TimeoutInMicroSecs  - The timeout value in microsecond. Zero means infinity.
+  @param[in] ProcArguments       - Argument for Procedure.
+  @param[in] FailedCPUList       - If not NULL, all APs that fail to start will be recorded in the list.
+
+  @retval EFI_INVALID_PARAMETER - Procudure is NULL.
+  @retval EFI_SUCCESS           - Only 1 logical processor exists.
+  @retval EFI_SUCCESS           - All APs have finished.
+  @retval EFI_TIMEOUT           - Time goes out before all APs have finished.
+**/
+EFI_STATUS
+EFIAPI
+StartupAllAPs (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE         Procedure,
+  IN BOOLEAN                  SingleThread,
+  IN EFI_EVENT WaitEvent      OPTIONAL,
+  IN UINTN TimeoutInMicroSecs OPTIONAL,
+  IN OUT VOID *ProcArguments  OPTIONAL,
+  OUT UINTN *FailedCPUList    OPTIONAL
+  );
+
+/**
+    MP Service to makes the current BSP into an AP and then switches the
+    designated AP into the AP. This procedure is usually called after a CPU
+    test that has found that BSP is not healthy to continue it's responsbilities.
+
+    @param[in] This            - Pointer to MP Service Protocol.
+    @param[in] ProcessorNumber - The handle number of processor.
+    @param[in] OldBSPState     - Whether to enable or disable the original BSP.
+
+    @retval EFI_INVALID_PARAMETER - Number for Specified AP out of range.
+    @retval EFI_INVALID_PARAMETER - Number of specified CPU belongs to BSP.
+    @retval EFI_NOT_READY         - Specified AP is not idle.
+    @retval EFI_SUCCESS           - BSP successfully switched.
+**/
+EFI_STATUS
+EFIAPI
+SwitchBSP (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    ProcessorNumber,
+  IN BOOLEAN                  OldBSPState
+  );
+
+/**
+  This procedure sends an IPI to the designated processor in
+  the requested delivery mode with the requested vector.
+
+  @param[in] This         - Pointer to MP Service Protocol.
+  @param[in] ProcessorNumber - The handle number of processor.
+  @param[in] VectorNumber - Vector number.
+  @param[in] DeliveryMode - I/O APIC Interrupt Deliver Modes
+
+  @retval EFI_INVALID_PARAMETER - Input paramters were not correct.
+  @retval Other status          - Status returned by SendInterrupt ()
+**/
+EFI_STATUS
+EFIAPI
+SendIPI (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    ProcessorNumber,
+  IN UINTN                    VectorNumber,
+  IN UINTN                    DeliveryMode
+  );
+
+/**
+  This procedure enables or disables APs.
+
+  @param[in] This            - Pointer to MP Service Protocol.
+  @param[in] ProcessorNumber - The handle number of processor.
+  @param[in] NewAPState      - Indicate new desired AP state
+  @param[in] HealthState     - If not NULL, it points to the value that specifies the new health status of the AP.
+                               If it is NULL, this parameter is ignored.
+
+  @retval EFI_INVALID_PARAMETER - Input paramters were not correct.
+  @retval EFI_SUCCESS           - Function completed successfully
+**/
+EFI_STATUS
+EFIAPI
+EnableDisableAP (
+  IN EFI_MP_SERVICES_PROTOCOL   *This,
+  IN UINTN                      ProcessorNumber,
+  IN BOOLEAN                    NewAPState,
+  IN EFI_MP_HEALTH *HealthState OPTIONAL
+  );
+
+/**
+  Implementation of WhoAmI() service of MP Services Protocol.
+
+  This service lets the caller processor get its handle number.
+  This service may be called from the BSP and APs.
+
+  @param[in] This                 - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber      - Pointer to the handle number of AP.
+
+  @retval EFI_SUCCESS           - Processor number successfully returned.
+  @retval EFI_INVALID_PARAMETER - ProcessorNumber is NULL
+**/
+EFI_STATUS
+EFIAPI
+WhoAmI (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                   *ProcessorNumber
+  );
+
+///
+/// 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
+
+  @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
+  );
+
+/**
+  Get APIC ID of processor
+
+  @param[in] ApicBase          - APIC base
+  @param[in] ApicVersionNumber - APIC version
+
+  @retval APIC ID of processor
+**/
+UINT32
+GetApicID (
+  OUT EFI_PHYSICAL_ADDRESS *ApicBase OPTIONAL,
+  OUT UINT32 *ApicVersionNumber      OPTIONAL
+  );
+
+/**
+  Programs XAPIC registers.
+
+  @param[in] BSP             - Is this BSP
+**/
+VOID
+ProgramXApic (
+  IN BOOLEAN BSP
+  );
+
+/**
+  Allocate a temporary memory under 1MB for MP Init to perform INIT-SIPI.
+  This buffer also provides memory for stack/data for MP running.
+
+  @param[in] WakeUpBuffer  - Return buffer location
+
+  @retval EFI_SUCCESS if ok to get a memory under 1MB for MP running.
+**/
+EFI_STATUS
+AllocateWakeUpBuffer (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer
+  );
+
+///
+/// Assembly functions implemented in MP/HT drivers
+///
+/**
+  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 GDTR and IDTR
+
+  @param[in] Gdt - will be stored GDTR
+  @param[in] Idt - will be stored IDTR
+**/
+VOID
+AsmGetGdtrIdtr (
+  OUT PSEUDO_DESCRIPTOR **Gdt,
+  OUT PSEUDO_DESCRIPTOR **Idt
+  );
+
+/**
+  Prepare GDTR and IDTR for AP
+
+  @param[in] GDTR  - The GDTR profile
+  @param[in] IDTR  - The IDTR profile
+
+  @retval EFI_STATUS  - status returned by each sub-routine
+  @retval EFI_SUCCESS - GDTR and IDTR has been prepared for AP
+**/
+EFI_STATUS
+PrepareGdtIdtForAP (
+  OUT PSEUDO_DESCRIPTOR *GDTR,
+  OUT PSEUDO_DESCRIPTOR *IDTR
+  );
+
+/**
+  Allocate Reserved Memory
+
+  @param[in] Size      - Memory Size
+  @param[in] Alignment - Alignment size
+  @param[in] Pointer   - return memory location
+
+  @retval EFI_SUCCESS - Allocate a reserved memory successfully
+**/
+EFI_STATUS
+AllocateAlignedReservedMemory (
+  IN UINTN Size,
+  IN UINTN Alignment,
+  OUT VOID **Pointer
+  );
+
+/**
+  Fill in CPU relevant information into data hub
+
+  @param[in] CpuNumber         - CPU number
+  @param[in] CpuDataForDatahub - pointer to data hub that will be updated
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+FillinDataforDataHub (
+  IN UINTN                 CpuNumber,
+  OUT CPU_DATA_FOR_DATAHUB *CpuDataForDatahub
+  );
+
+/**
+  This function is invoked when LegacyBios protocol is installed, we must
+  allocate reserved memory under 1M for AP.
+
+  @param[in] Event   - The triggered event.
+  @param[in] Context - Context for this event.
+**/
+VOID
+EFIAPI
+ReAllocateEbdaMemoryForAP (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  This function is invoked when LegacyBios protocol is installed, we must
+  allocate reserved memory under 1M for AP.
+
+  @param[in] Event   - The triggered event.
+  @param[in] Context - Context for this event.
+**/
+VOID
+EFIAPI
+ReAllocateMemoryForAP (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  This function is invoked by EFI_EVENT_SIGNAL_LEGACY_BOOT.
+  Before booting to legacy OS, reset it with memory allocated
+  by ReAllocateMemoryForAp() and set local APIC correctly.
+
+  @param[in] Event   - The triggered event.
+  @param[in] Context - Context for this event.
+**/
+VOID
+ResetAPs (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  Prepare Wakeup Buffer and stack for APs.
+
+  @param[in] WakeUpBuffer             - Pointer to the address of wakeup buffer for output.
+  @param[in] StackAddressStart        - Pointer to the stack address of APs for output.
+  @param[in] MaximumCPUsForThisSystem - Maximum CPUs in this system.
+
+  @retval EFI_SUCCESS              - Memory successfully prepared for APs.
+  @retval Other                    - Error occurred while allocating memory.
+**/
+EFI_STATUS
+PrepareMemoryForAPs (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer,
+  OUT VOID                 **StackAddressStart,
+  IN UINTN                 MaximumCPUsForThisSystem
+  );
+
+/**
+  Prepare exchange information for APs.
+
+  @param[in] ExchangeInfo      - Pointer to the exchange info buffer for output.
+  @param[in] StackAddressStart - Start address of APs' stacks.
+  @param[in] ApFunction        - Address of function assigned to AP.
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer.
+
+  @retval EFI_SUCCESS       - Exchange Info successfully prepared for APs.
+  @retval Other             - Error occurred while allocating memory.
+**/
+EFI_STATUS
+PrepareExchangeInfo (
+  OUT MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  IN VOID                  *StackAddressStart,
+  IN VOID                  *ApFunction,
+  IN EFI_PHYSICAL_ADDRESS  WakeUpBuffer
+  );
+
+/**
+  Prepare Wakeup Buffer and stack for APs during S3.
+
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer for output.
+  @param[in] StackAddressStart - Pointer to the stack address of APs for output.
+
+  @retval EFI_SUCCESS       - Memory successfully prepared for APs.
+**/
+EFI_STATUS
+S3PrepareMemoryForAPs (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer,
+  OUT VOID                 **StackAddressStart
+  );
+
+/**
+  Prepare exchange information for APs during S3.
+
+  @param[in] ExchangeInfo      - Pointer to the exchange info for output.
+  @param[in] StackAddressStart - Start address of APs' stacks.
+  @param[in] ApFunction        - Address of function assigned to AP.
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer.
+
+  @retval EFI_SUCCESS       - Exchange Info successfully prepared for APs.
+**/
+EFI_STATUS
+S3PrepareExchangeInfo (
+  OUT MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  IN VOID                  *StackAddressStart,
+  IN VOID                  *ApFunction,
+  IN EFI_PHYSICAL_ADDRESS  WakeUpBuffer
+  );
+
+/**
+  Check whether any AP is running for assigned task.
+
+  @retval TRUE  - Some APs are running.
+  @retval FALSE - No AP is running.
+**/
+BOOLEAN
+ApRunning (
+  VOID
+  );
+
+/**
+  Wrapper function for all procedures assigned to AP via MP service protocol.
+  It controls states of AP and invokes assigned precedure.
+**/
+VOID
+ApProcWrapper (
+  VOID
+  );
+
+/**
+  Allocate EfiReservedMemoryType below 4G memory address.
+
+  @param[in] Size   - Size of memory to allocate.
+  @param[in] Buffer - Allocated address for output.
+
+  @retval EFI_SUCCESS - Memory successfully allocated.
+  @retval Other       - Other errors occur.
+**/
+EFI_STATUS
+AllocateReservedMemoryBelow4G (
+  IN UINTN Size,
+  OUT VOID **Buffer
+  );
+
+/**
+  Dynamically write the far jump destination in APs' wakeup buffer,
+  in order to refresh APs' CS registers for mode switching.
+**/
+VOID
+RedirectFarJump (
+  VOID
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MpService.c b/ReferenceCode/Haswell/CpuInit/Dxe/MpService.c
new file mode 100644
index 0000000..7979618
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MpService.c
@@ -0,0 +1,2194 @@
+/** @file
+  Code which support multi-processor
+
+@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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "MpService.h"
+#include "PiMpService.h"
+#include "CpuInitDxe.h"
+#include "MachineCheck.h"
+#include "Features.h"
+#include "BootGuardLibrary.h"
+#endif
+
+#include EFI_PROTOCOL_DEFINITION (GenericMemoryTest)
+#include EFI_GUID_DEFINITION (GlobalVariable)
+#include EFI_PROTOCOL_DEFINITION (ExitPmAuth)
+
+extern DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu;
+extern EFI_METRONOME_ARCH_PROTOCOL      *mMetronome;
+extern MP_SYSTEM_DATA                   *mMPSystemData;
+extern UINTN                            mCommonFeatures;
+extern volatile UINTN                   mSwitchToLegacyRegionCount;
+extern EFI_CPU_MICROCODE_HEADER         **mMicrocodePointerBuffer;
+extern EFI_DATA_HUB_PROTOCOL            *mDataHub;
+
+static EFI_HANDLE        mHandle         = NULL;
+static UINT32            mFinishedCount  = 0;
+extern UINT32            mMcuLoadCount;
+STATIC UINT64            mCpuPerfCtrlValue;
+EFI_MP_SERVICES_PROTOCOL mMpService = {
+  GetGeneralMPInfo,
+  GetProcessorContext,
+  StartupAllAPs,
+  StartupThisAP,
+  SwitchBSP,
+  SendIPI,
+  EnableDisableAP,
+  WhoAmI
+};
+
+EFI_PHYSICAL_ADDRESS  mOriginalBuffer;
+EFI_PHYSICAL_ADDRESS  mBackupBuffer;
+DXE_CPU_INFO_PROTOCOL mCpuInfo;
+
+/**
+  Initialize MP services by MP Service Protocol
+**/
+VOID
+EFIAPI
+InitializeMpServices (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  EFI_EVENT  LegacyBootEvent;
+  EFI_EVENT  ExitBootServicesEvent;
+  VOID       *Registration;
+
+  LegacyBootEvent       = NULL;
+  ExitBootServicesEvent = NULL;
+
+  ///
+  /// Save Mtrr Registers in global data areas
+  ///
+  ReadMtrrRegisters ();
+
+  ///
+  /// Initialize and collect MP related data
+  ///
+  Status = InitializeMpSystemData ();
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  ///
+  /// Register protocol notifacation function for ExitPmAuth protocol
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gExitPmAuthProtocolGuid,
+          TPL_NOTIFY,
+          ReAllocateMemoryForAP,
+          NULL,
+          &Registration
+          );
+
+  ///
+  /// Register protocol notifaction function to allocate memory in EBDA as early as possible
+  ///
+  EfiCreateProtocolNotifyEvent (
+          &gEfiLegacyBiosProtocolGuid,
+          TPL_NOTIFY,
+          ReAllocateEbdaMemoryForAP,
+          NULL,
+          &Registration
+          );
+
+  ///
+  /// Create legacy boot and EFI boot events to reset APs before OS handoff
+  ///
+  Status = EfiCreateEventLegacyBootEx (
+                  EFI_TPL_CALLBACK,
+                  ResetAPs,
+                  mMPSystemData,
+                  &LegacyBootEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVENT_SIGNAL_EXIT_BOOT_SERVICES,
+                  TPL_CALLBACK,
+                  ResetAPs,
+                  mMPSystemData,
+                  &ExitBootServicesEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  ///
+  /// Create timer event to check AP state for non-blocking execution.
+  ///
+  Status = gBS->CreateEvent (
+                  EFI_EVENT_TIMER | EFI_EVENT_NOTIFY_SIGNAL,
+                  EFI_TPL_CALLBACK,
+                  CheckAPsStatus,
+                  NULL,
+                  &mMPSystemData->CheckAPsEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Now install the Frameowrk & PI MP services protocol.
+  ///
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mHandle,
+                  &gEfiMpServiceProtocolGuid,
+                  &mMpService,
+                  &gEfiPiMpServiceProtocolGuid,
+                  &mPiMpService,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->SetTimer (
+                  mMPSystemData->CheckAPsEvent,
+                  TimerPeriodic,
+                  10000 * MICROSECOND
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  if (EFI_ERROR (Status)) {
+
+Done:
+    if (LegacyBootEvent != NULL) {
+      gBS->CloseEvent (LegacyBootEvent);
+    }
+
+    if (ExitBootServicesEvent != NULL) {
+      gBS->CloseEvent (ExitBootServicesEvent);
+    }
+
+    FreePool (mMPSystemData);
+  }
+}
+
+/**
+  Get general MP information
+
+  @param[in] This                  - EFI_MP_SERVICE_PROTOCOL
+  @param[in] NumberOfCPUs          - Number of processors
+  @param[in] MaximumNumberOfCPUs   - Max supported number of processors
+  @param[in] NumberOfEnabledCPUs   - Number of processors enabled
+  @param[in] RendezvousIntNumber   - number of Rendezvous procedure
+  @param[in] RendezvousProcLength  - length of Rendezvous procedure
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+EFIAPI
+GetGeneralMPInfo (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                   *NumberOfCPUs,
+  OUT UINTN                   *MaximumNumberOfCPUs,
+  OUT UINTN                   *NumberOfEnabledCPUs,
+  OUT UINTN                   *RendezvousIntNumber,
+  OUT UINTN                   *RendezvousProcLength
+  )
+{
+  UINTN          Index;
+  CPU_DATA_BLOCK *CpuData;
+
+  if (NumberOfCPUs) {
+    *NumberOfCPUs = mMPSystemData->NumberOfCpus;
+  }
+
+  if (MaximumNumberOfCPUs) {
+    *MaximumNumberOfCPUs = mMPSystemData->MaximumCpusForThisSystem;
+  }
+
+  if (RendezvousProcLength) {
+    *RendezvousProcLength = RENDEZVOUS_PROC_LENGTH;
+  }
+
+  if (RendezvousIntNumber) {
+    *RendezvousIntNumber = 0;
+  }
+
+  if (NumberOfEnabledCPUs) {
+    *NumberOfEnabledCPUs = 0;
+    for (Index = 0; Index < mMPSystemData->NumberOfCpus; Index++) {
+      CpuData = &mMPSystemData->CpuData[Index];
+      if (mMPSystemData->EnableSecondaryCpu) {
+        if (CpuData->State != CPU_STATE_DISABLED) {
+          (*NumberOfEnabledCPUs)++;
+        }
+      } else {
+        if (CpuData->State != CPU_STATE_DISABLED && !mMPSystemData->CpuData[Index].SecondaryCpu) {
+          (*NumberOfEnabledCPUs)++;
+        }
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get processor context
+
+  @param[in] This                    - EFI_MP_SERVICE_PROTOCOL
+  @param[in] CpuNumber               - Cpu number
+  @param[in] BufferLength            - buffer length
+  @param[in] ProcessorContextBuffer  - pointer to the buffer that will be updated
+
+  @retval EFI_INVALID_PARAMETER - buffer is NULL or CpuNumber our of range
+  @retval EFI_BUFFER_TOO_SMALL - buffer too small
+  @retval EFI_SUCCESS - got processor context successfully
+**/
+EFI_STATUS
+EFIAPI
+GetProcessorContext (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    CpuNumber,
+  IN OUT UINTN                *BufferLength,
+  IN OUT EFI_MP_PROC_CONTEXT  *ProcessorContextBuffer
+  )
+{
+  EFI_MP_PROC_CONTEXT *ProcessorBuffer;
+  CPU_DATA_BLOCK      *CpuData;
+
+  if (BufferLength == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*BufferLength < sizeof (EFI_MP_PROC_CONTEXT)) {
+    *BufferLength = sizeof (EFI_MP_PROC_CONTEXT);
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  if ((mMPSystemData->NumberOfCpus <= CpuNumber) || (ProcessorContextBuffer == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CpuData                   = &mMPSystemData->CpuData[CpuNumber];
+
+  *BufferLength             = sizeof (EFI_MP_PROC_CONTEXT);
+  ProcessorBuffer           = ProcessorContextBuffer;
+
+  ProcessorBuffer->ApicID   = CpuData->ApicID;
+
+  ProcessorBuffer->Enabled  = TRUE;
+  if (!mMPSystemData->EnableSecondaryCpu) {
+    if (CpuData->SecondaryCpu) {
+      ProcessorBuffer->Enabled = FALSE;
+    }
+  }
+
+  if (CpuData->State == CPU_STATE_DISABLED) {
+    ProcessorBuffer->Enabled = FALSE;
+  }
+
+  if (CpuNumber == mMPSystemData->BSP) {
+    ProcessorBuffer->Designation = EfiCpuBSP;
+  } else {
+    ProcessorBuffer->Designation = EfiCpuAP;
+  }
+
+  ProcessorBuffer->Health.Flags       = CpuData->Health;
+  ProcessorBuffer->Health.TestStatus  = 0;
+
+  ProcessorBuffer->PackageNumber      = CpuData->CpuDataforDatahub.Location.Package;
+  ProcessorBuffer->NumberOfCores      = CpuData->CpuDataforDatahub.Location.Core;
+  ProcessorBuffer->NumberOfThreads    = CpuData->CpuDataforDatahub.Location.Thread;
+  ProcessorBuffer->ProcessorTestMask  = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  MP Service to get specified application processor (AP)
+  to execute a caller-provided code stream.
+
+  @param[in] This                - Pointer to MP Service Protocol
+  @param[in] Procedure           - The procedure to be assigned to AP.
+  @param[in] CpuNumber           - Number of the specified processor.
+  @param[in] WaitEvent           - If timeout, the event to be triggered after this AP finishes.
+  @param[in] TimeoutInMicroSecs  - The timeout value in microsecond. Zero means infinity.
+  @param[in] ProcArguments       - Argument for Procedure.
+
+  @retval EFI_INVALID_PARAMETER - Procudure is NULL.
+  @retval EFI_INVALID_PARAMETER - Number of CPU out of range, or it belongs to BSP.
+  @retval EFI_INVALID_PARAMETER - Specified CPU is not idle.
+  @retval EFI_SUCCESS           - The AP has finished.
+  @retval EFI_TIMEOUT           - Time goes out before the AP has finished.
+**/
+EFI_STATUS
+EFIAPI
+StartupThisAP (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE         Procedure,
+  IN UINTN                    CpuNumber,
+  IN EFI_EVENT WaitEvent      OPTIONAL,
+  IN UINTN TimeoutInMicroSecs OPTIONAL,
+  IN OUT VOID *ProcArguments  OPTIONAL
+  )
+{
+  EFI_STATUS     Status;
+  CPU_DATA_BLOCK *CpuData;
+  UINT64         ExpectedTime;
+
+  ///
+  /// Check for invalid CPU number
+  ///
+  if ((CpuNumber >= mMPSystemData->NumberOfCpus) || CpuNumber == mMPSystemData->BSP) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Procedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+  ///
+  /// As a first step, check if processor is OK to start up code stream.
+  ///
+  if (CpuData->State != CPU_STATE_IDLE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ExpectedTime                = CalculateTimeout (TimeoutInMicroSecs);
+
+  mMPSystemData->StartCount   = 1;
+  mMPSystemData->FinishCount  = 0;
+
+  WakeUpAp (
+          CpuData,
+          Procedure,
+          ProcArguments
+          );
+
+  while (TRUE) {
+    AsmAcquireMPLock (&CpuData->StateLock);
+    if (CpuData->State == CPU_STATE_FINISHED) {
+      CpuData->State = CPU_STATE_IDLE;
+      AsmReleaseMPLock (&CpuData->StateLock);
+      break;
+    }
+
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    if (CheckTimeout (ExpectedTime)) {
+      ///
+      /// Save data into private data structure, and create timer to poll AP state before exiting
+      ///
+      mMPSystemData->WaitEvent        = WaitEvent;
+      Status = gBS->SetTimer (
+                      CpuData->CheckThisAPEvent,
+                      TimerPeriodic,
+                      CPU_CHECK_AP_INTERVAL * MICROSECOND
+                      );
+      return EFI_TIMEOUT;
+    }
+
+    gBS->Stall (CPU_CHECK_AP_INTERVAL);
+  }
+
+  if (WaitEvent) gBS->SignalEvent (WaitEvent);  //(AMI_CHG)
+  return EFI_SUCCESS;
+}
+
+/**
+  MP Service to get all the available application processors (APs)
+  to execute a caller-provided code stream.
+
+  @param[in] This                - Pointer to MP Service Protocol
+  @param[in] Procedure           - The procedure to be assigned to APs.
+  @param[in] SingleThread        - If true, all APs execute in block mode.
+                        Otherwise, all APs exceute in non-block mode.
+  @param[in] WaitEvent           - If timeout, the event to be triggered after all APs finish.
+  @param[in] TimeoutInMicroSecs  - The timeout value in microsecond. Zero means infinity.
+  @param[in] ProcArguments       - Argument for Procedure.
+  @param[in] FailedCPUList       - If not NULL, all APs that fail to start will be recorded in the list.
+
+  @retval EFI_INVALID_PARAMETER - Procudure is NULL.
+  @retval EFI_SUCCESS           - Only 1 logical processor exists.
+  @retval EFI_SUCCESS           - All APs have finished.
+  @retval EFI_TIMEOUT           - Time goes out before all APs have finished.
+**/
+EFI_STATUS
+EFIAPI
+StartupAllAPs (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE         Procedure,
+  IN BOOLEAN                  SingleThread,
+  IN EFI_EVENT WaitEvent      OPTIONAL,
+  IN UINTN TimeoutInMicroSecs OPTIONAL,
+  IN OUT VOID *ProcArguments  OPTIONAL,
+  OUT UINTN *FailedCPUList    OPTIONAL
+  )
+{
+  EFI_STATUS     Status;
+  CPU_DATA_BLOCK *CpuData;
+  CPU_DATA_BLOCK *NextCpuData;
+  UINTN          ListIndex;
+  UINTN          CpuNumber;
+  UINTN          NextCpuNumber;
+  UINT64         ExpectedTime;
+  CPU_STATE      APInitialState;
+  CPU_STATE      CpuState;
+
+  ///
+  /// Check for valid procedure for APs
+  ///
+  if (Procedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mMPSystemData->NumberOfCpus == 1) {
+    if (WaitEvent) gBS->SignalEvent (WaitEvent); //(AMI_CHG)
+    return EFI_SUCCESS;
+  }
+
+// AMI Override: Add
+  //
+  // Check whether all enabled APs are idle.
+  // If any enabled AP is not idle, return EFI_NOT_READY.
+  //
+  for (CpuNumber = 0; CpuNumber <  mMPSystemData->NumberOfCpus; CpuNumber++) {
+
+    CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+    //
+    // Skip BSP and disabled APs.
+    //
+    if (CpuNumber == mMPSystemData->BSP ||
+        CpuData->State == CPU_STATE_DISABLED) {
+      continue;
+    }
+
+    //
+    // If any enabled APs are busy, return EFI_NOT_FOUND.
+    //
+    if (CpuData->State != CPU_STATE_IDLE) {
+      return EFI_NOT_READY;
+    }
+  }
+  //
+  // All enabled APs are idle, we can safely initiate a new session
+  //
+// AMI Override: End
+  ExpectedTime                = CalculateTimeout (TimeoutInMicroSecs);
+
+  ListIndex                   = 0;
+  CpuData                     = NULL;
+
+  mMPSystemData->FinishCount  = 0;
+  mMPSystemData->StartCount   = 0;
+  APInitialState              = CPU_STATE_READY;
+
+  for (CpuNumber = 0; CpuNumber < mMPSystemData->NumberOfCpus; CpuNumber++) {
+    CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+    ///
+    /// Get APs prepared, and put failing APs into FailedCPUList
+    /// If "SingleThread", one AP will be put to ready state.
+    /// Once this AP finishes its task, the next AP is put to Ready state.
+    /// This process continues until all APs are put into Ready State
+    /// if not "SingleThread", all APs are put to ready state at the same time
+    ///
+    if (CpuNumber != mMPSystemData->BSP) {
+      if (CpuData->State == CPU_STATE_IDLE) {
+        mMPSystemData->StartCount++;
+
+        AsmAcquireMPLock (&CpuData->StateLock);
+        CpuData->State = APInitialState;
+        AsmReleaseMPLock (&CpuData->StateLock);
+
+        if (SingleThread) {
+          APInitialState = CPU_STATE_BLOCKED;
+        }
+
+      } else if (FailedCPUList != NULL) {
+        FailedCPUList[ListIndex] = CpuNumber;
+        ListIndex++;
+      }
+    }
+  }
+
+  while (TRUE) {
+    for (CpuNumber = 0; CpuNumber < mMPSystemData->NumberOfCpus; CpuNumber++) {
+      CpuData = &mMPSystemData->CpuData[CpuNumber];
+      if (CpuNumber == mMPSystemData->BSP) {
+        continue;
+      }
+      CpuState = CpuData->State;
+      switch (CpuState) {
+        case CPU_STATE_READY:
+          WakeUpAp (
+                  CpuData,
+                  Procedure,
+                  ProcArguments
+                  );
+          break;
+
+        case CPU_STATE_FINISHED:
+          mMPSystemData->FinishCount++;
+          if (SingleThread) {
+            Status = GetNextBlockedCpuNumber (&NextCpuNumber);
+            if (!EFI_ERROR (Status)) {
+              NextCpuData = &mMPSystemData->CpuData[NextCpuNumber];
+              AsmAcquireMPLock (&NextCpuData->StateLock);
+              NextCpuData->State = CPU_STATE_READY;
+              AsmReleaseMPLock (&NextCpuData->StateLock);
+            }
+          }
+          AsmAcquireMPLock (&CpuData->StateLock);
+          CpuData->State = CPU_STATE_IDLE;
+          AsmReleaseMPLock (&CpuData->StateLock);
+          break;
+
+        default:
+          break;
+      }
+    }
+
+    if (mMPSystemData->FinishCount == mMPSystemData->StartCount) {
+      if (WaitEvent) gBS->SignalEvent (WaitEvent);  //(AMI_CHG)
+      return EFI_SUCCESS;
+    }
+
+    if (CheckTimeout (ExpectedTime)) {
+      ///
+      /// Save data into private data structure, and create timer to poll AP state before exiting
+      ///
+      mMPSystemData->Procedure      = Procedure;
+      mMPSystemData->ProcArguments  = ProcArguments;
+      mMPSystemData->SingleThread   = SingleThread;
+      mMPSystemData->WaitEvent      = WaitEvent;
+
+      Status = gBS->SetTimer (
+                      mMPSystemData->CheckAllAPsEvent,
+                      TimerPeriodic,
+                      CPU_CHECK_AP_INTERVAL * MICROSECOND
+                      );
+      return EFI_TIMEOUT;
+    }
+
+    gBS->Stall (CPU_CHECK_AP_INTERVAL);
+  }
+
+  if (WaitEvent) gBS->SignalEvent (WaitEvent);  //(AMI_CHG)
+  return EFI_SUCCESS;
+}
+
+/**
+    MP Service to makes the current BSP into an AP and then switches the
+    designated AP into the AP. This procedure is usually called after a CPU
+    test that has found that BSP is not healthy to continue it's responsbilities.
+
+    @param[in] This         - Pointer to MP Service Protocol.
+    @param[in] CpuNumber    - The number of the specified AP.
+    @param[in] EnableOldBSP - Whether to enable or disable the original BSP.
+
+    @retval EFI_INVALID_PARAMETER - Number for Specified AP out of range.
+    @retval EFI_INVALID_PARAMETER - Number of specified CPU belongs to BSP.
+    @retval EFI_NOT_READY         - Specified AP is not idle.
+    @retval EFI_SUCCESS           - BSP successfully switched.
+**/
+EFI_STATUS
+EFIAPI
+SwitchBSP (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    CpuNumber,
+  IN BOOLEAN                  EnableOldBSP
+  )
+{
+  EFI_STATUS            Status;
+  EFI_CPU_ARCH_PROTOCOL *CpuArch;
+  BOOLEAN               OldInterruptState;
+  CPU_DATA_BLOCK        *CpuData;
+  CPU_STATE             CpuState;
+
+  ///
+  /// Check if the specified CPU number is valid
+  ///
+  if (CpuNumber >= mMPSystemData->NumberOfCpus) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ///
+  /// Check if the specified CPU is already BSP
+  ///
+  if (CpuNumber == mMPSystemData->BSP) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CpuData = &mMPSystemData->CpuData[CpuNumber];
+  if (CpuData->State != CPU_STATE_IDLE) {
+    return EFI_NOT_READY;
+  }
+  ///
+  /// Before send both BSP and AP to a procedure to exchange their roles,
+  /// interrupt must be disabled. This is because during the exchange role
+  /// process, 2 CPU may use 1 stack. If interrupt happens, the stack will
+  /// be corrputed, since interrupt return address will be pushed to stack
+  /// by hardware.
+  ///
+  CpuArch = mMPSystemData->CpuArch;
+  (CpuArch->GetInterruptState)(CpuArch, &OldInterruptState);
+  if (OldInterruptState) {
+    Status = CpuArch->DisableInterrupt (CpuArch);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  ///
+  /// Unprogram virtual wire mode for the old BSP
+  ///
+  ProgramXApic (FALSE);
+  SetApicBSPBit (FALSE);
+
+  mMPSystemData->BSPInfo.State  = CPU_SWITCH_STATE_IDLE;
+  mMPSystemData->BSPInfo.Lock   = VacantFlag;
+  mMPSystemData->APInfo.State   = CPU_SWITCH_STATE_IDLE;
+  mMPSystemData->APInfo.Lock    = VacantFlag;
+
+  ///
+  /// Need to wakeUp AP (future BSP)
+  ///
+  WakeUpAp (
+          CpuData,
+          FutureBSPProc,
+          mMPSystemData
+          );
+
+  AsmExchangeRole (&mMPSystemData->BSPInfo, &mMPSystemData->APInfo);
+
+  ///
+  /// The new BSP has come out. Since it carries the register value of the AP, need
+  /// to pay attention to variable which are stored in registers (due to optimization)
+  ///
+  SetApicBSPBit (TRUE);
+  ProgramXApic (TRUE);
+
+  if (OldInterruptState) {
+    Status = CpuArch->EnableInterrupt (CpuArch);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  CpuData = &mMPSystemData->CpuData[mMPSystemData->BSP];
+  while (TRUE) {
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuState = CpuData->State;
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    if (CpuState == CPU_STATE_FINISHED) {
+      break;
+    }
+  }
+
+  Status              = ChangeCpuState (mMPSystemData->BSP, EnableOldBSP, CPU_CAUSE_NOT_DISABLED);
+  mMPSystemData->BSP  = CpuNumber;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This procedure sends an IPI to the designated processor in
+  the requested delivery mode with the requested vector.
+
+  @param[in] This         - Pointer to MP Service Protocol.
+  @param[in] CpuNumber    - The number of the specified AP.
+  @param[in] VectorNumber - Vector number.
+  @param[in] DeliveryMode - I/O APIC Interrupt Deliver Modes
+
+  @retval EFI_INVALID_PARAMETER - Input paramters were not correct.
+  @retval Other status          - Status returned by SendInterrupt ()
+**/
+EFI_STATUS
+EFIAPI
+SendIPI (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    CpuNumber,
+  IN UINTN                    VectorNumber,
+  IN UINTN                    DeliveryMode
+  )
+{
+  UINT32               TriggerMode;
+  EFI_STATUS           Status;
+  CPU_DATA_BLOCK       *CpuData;
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+  MONITOR_MWAIT_DATA   *MonitorAddr;
+
+  ///
+  /// Check for valid input parameters.
+  ///
+  if (CpuNumber >= mMPSystemData->NumberOfCpus || CpuNumber == mMPSystemData->BSP) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (VectorNumber >= INTERRUPT_VECTOR_NUMBER) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DeliveryMode >= DELIVERY_MODE_MAX) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CpuData     = &mMPSystemData->CpuData[CpuNumber];
+  TriggerMode = TRIGGER_MODE_EDGE;
+
+  ///
+  /// Fix the vector number for special interrupts like SMI and INIT.
+  ///
+  if (DeliveryMode == DELIVERY_MODE_SMI || DeliveryMode == DELIVERY_MODE_INIT) {
+    VectorNumber = 0x0;
+  }
+
+  ///
+  /// If current waking manner is not HLT loop and some other DXE driver
+  /// like as TXT needs to send INIT command per this MpService routine,
+  /// then we need to set vector change flag for the specific AP to back
+  /// to MWAIT or RUN loop procedure, so additional INIT-SIPI command
+  /// will be sent in WakeUpAp routine for AP tasks to work later.
+  ///
+  ExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (mAcpiCpuData->WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+  if (ExchangeInfo->WakeUpApManner != WakeUpApPerHltLoop && DeliveryMode == DELIVERY_MODE_INIT) {
+    MonitorAddr = (MONITOR_MWAIT_DATA *)
+            (
+                    (UINT8 *) ExchangeInfo->StackStart +
+                    (ExchangeInfo->BistBuffer[CpuData->ApicID].Number + 1) *
+                    ExchangeInfo->StackSize -
+                    MONITOR_FILTER_SIZE
+            );
+    MonitorAddr->WakeUpApVectorChangeFlag = TRUE;
+  }
+
+  Status = SendInterrupt (
+                  BROADCAST_MODE_SPECIFY_CPU,
+                  CpuData->ApicID,
+                  (UINT32) VectorNumber,
+                  (UINT32) DeliveryMode,
+                  TriggerMode,
+                  TRUE
+                  );
+
+  return Status;
+}
+
+/**
+  This procedure enables Or disables APs.
+
+  @param[in] This         - Pointer to MP Service Protocol.
+  @param[in] CpuNumber    - The number of the specified AP.
+  @param[in] NewAPState   - Indicate new desired AP state
+  @param[in] HealthState  - If not NULL, it points to the value that specifies
+                            the new health status of the AP.  If it is NULL,
+                            this parameter is ignored.
+
+  @retval EFI_INVALID_PARAMETER - Input paramters were not correct.
+  @retval EFI_SUCCESS           - Function completed successfully
+**/
+EFI_STATUS
+EFIAPI
+EnableDisableAP (
+  IN EFI_MP_SERVICES_PROTOCOL   *This,
+  IN UINTN                      CpuNumber,
+  IN BOOLEAN                    NewAPState,
+  IN EFI_MP_HEALTH *HealthState OPTIONAL
+  )
+{
+  EFI_STATUS     Status;
+  CPU_DATA_BLOCK *CpuData;
+
+  ///
+  /// Check for valid input parameters.
+  ///
+  if (CpuNumber >= mMPSystemData->NumberOfCpus || CpuNumber == mMPSystemData->BSP) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CpuData = &mMPSystemData->CpuData[CpuNumber];
+  Status  = ChangeCpuState (CpuNumber, NewAPState, CPU_CAUSE_USER_SELECTION);
+
+  if (HealthState != NULL) {
+    CopyMem (&CpuData->Health, HealthState, sizeof (EFI_MP_HEALTH));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This procedure returns the calling CPU handle.
+
+  @param[in] This         - Pointer to MP Service Protocol.
+  @param[in] CpuNumber    - The number of the specified AP.
+
+  @retval EFI_SUCCESS           - Function completed successfully
+**/
+EFI_STATUS
+EFIAPI
+WhoAmI (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                   *CpuNumber
+  )
+{
+  UINTN ApicID;
+  UINTN NumOfCpus;
+  UINTN Index;
+
+  ApicID    = GetApicID (NULL, NULL);
+
+  NumOfCpus = mMPSystemData->NumberOfCpus;
+
+  for (Index = 0; Index < NumOfCpus; Index++) {
+    if (ApicID == mMPSystemData->CpuData[Index].ApicID) {
+      break;
+    }
+  }
+
+  *CpuNumber = Index;
+  return EFI_SUCCESS;
+}
+
+/**
+  Searches the HOB list provided by the core to find
+  if a MP guided HOB list exists or not. If it does, it copies it to the driver
+  data area, else returns 0
+
+  @param[in] MPSystemData - Pointer to an MP_SYSTEM_DATA structure
+
+  @retval EFI_SUCCESS  - Success
+  @retval EFI_NOT_FOUND - HOB not found or else
+**/
+EFI_STATUS
+GetMpBistStatus (
+  IN MP_SYSTEM_DATA *MPSystemData
+  )
+{
+  VOID                 *HobList;
+  VOID                 *DataInHob;
+  EFI_PEI_HOB_POINTERS Hob;
+  UINTN                DataSize;
+
+  ///
+  /// Check for MP Data Hob.
+  ///
+  HobList = GetFirstGuidHob (&gEfiHtBistHobGuid);
+
+  if (HobList == NULL) {
+    DEBUG ((EFI_D_ERROR, "No HOBs found\n"));
+    return EFI_NOT_FOUND;
+  }
+
+  DataInHob   = (VOID *) ((UINTN) HobList + sizeof (EFI_HOB_GUID_TYPE));
+
+  Hob.Header  = HobList;
+  DataSize    = Hob.Header->HobLength - sizeof (EFI_HOB_GUID_TYPE);
+
+  ///
+  /// This is the MP HOB. So, copy all the data
+  ///
+  if (HobList != NULL) {
+    if (NULL == MPSystemData->BistHobData) {
+      (gBS->AllocatePool)(EfiReservedMemoryType, DataSize, (VOID **) &MPSystemData->BistHobData);
+    }
+
+    CopyMem (MPSystemData->BistHobData, DataInHob, DataSize);
+    MPSystemData->BistHobSize = DataSize;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if CPUID support large than 3
+
+  @retval TRUE if CPUID support large than 3
+  @retval FALSE if not
+**/
+BOOLEAN
+IsCpuidSupportOver3 (
+  VOID
+  )
+{
+  BOOLEAN            RetVal;
+  EFI_CPUID_REGISTER CpuidRegisters;
+  RetVal = FALSE;
+  AsmCpuid (
+          CPUID_SIGNATURE,
+          &CpuidRegisters.RegEax,
+          &CpuidRegisters.RegEbx,
+          &CpuidRegisters.RegEcx,
+          &CpuidRegisters.RegEdx
+          );
+  if (CpuidRegisters.RegEax > 3) {
+    RetVal = TRUE;
+  }
+
+  return RetVal;
+}
+
+/**
+  Allocate data pool for MP information and fill data in it.
+
+  @param[in] WakeUpBuffer             - The address of wakeup buffer.
+  @param[in] StackAddressStart        - The start address of APs's stacks.
+  @param[in] MaximumCPUsForThisSystem - Maximum CPUs in this system.
+
+  @retval EFI_SUCCESS              - Function successfully executed.
+  @retval Other                    - Error occurred while allocating memory.
+**/
+EFI_STATUS
+FillMPData (
+  OUT EFI_PHYSICAL_ADDRESS WakeUpBuffer,
+  OUT VOID                 *StackAddressStart,
+  IN UINTN                 MaximumCPUsForThisSystem
+  )
+{
+  EFI_STATUS Status;
+
+  ///
+  /// First check if the MP data structures and AP rendezvous routine have been
+  /// supplied by the PEIMs that executed in early boot stage.
+  ///
+  ///
+  /// Clear the data structure area first.
+  ///
+  ZeroMem (mMPSystemData, sizeof (MP_SYSTEM_DATA));
+
+  Status                        = GetMpBistStatus (mMPSystemData);
+
+  mAcpiCpuData->CpuPrivateData  = (EFI_PHYSICAL_ADDRESS) (UINTN) (&(mMPSystemData->S3DataPointer));
+  mAcpiCpuData->APState         = mPlatformCpu->CpuConfig->HtState;
+  mAcpiCpuData->WakeUpBuffer    = WakeUpBuffer;
+  mAcpiCpuData->StackAddress    = (EFI_PHYSICAL_ADDRESS) StackAddressStart;
+
+  Status = PrepareGdtIdtForAP (
+                  (PSEUDO_DESCRIPTOR *) (UINTN) mAcpiCpuData->GdtrProfile,
+                  (PSEUDO_DESCRIPTOR *) (UINTN) mAcpiCpuData->IdtrProfile
+                  );
+
+  ///
+  /// First BSP fills and inits all known values, including it's own records.
+  ///
+  mMPSystemData->APSerializeLock    = VacantFlag;
+  mMPSystemData->NumberOfCpus       = 1;
+  mMPSystemData->EnableSecondaryCpu = mPlatformCpu->CpuConfig->HtState;
+
+  ///
+  /// Record these CPU configuration data (both for normal boot and for S3 use)
+  ///
+  if (IsCpuidSupportOver3 ()) {
+    mMPSystemData->LimitCpuidMaximumValue = mPlatformCpu->CpuConfig->LimitCpuidMaximumValue;
+  } else {
+    mMPSystemData->LimitCpuidMaximumValue = FALSE;
+  }
+
+  mMPSystemData->ExecuteDisableBit                = mPlatformCpu->CpuConfig->ExecuteDisableBit;
+  mMPSystemData->VmxEnable                        = mPlatformCpu->CpuConfig->VmxEnable;
+  mMPSystemData->TxtEnable                        = mPlatformCpu->CpuConfig->SmxEnable;
+  mMPSystemData->MonitorMwaitEnable               = mPlatformCpu->CpuConfig->MonitorMwaitEnable;
+  mMPSystemData->MachineCheckEnable               = mPlatformCpu->CpuConfig->MachineCheckEnable;
+  mMPSystemData->XapicEnable                      = mPlatformCpu->CpuConfig->XapicEnable;
+  mMPSystemData->AesEnable                        = mPlatformCpu->CpuConfig->AesEnable;
+  mMPSystemData->DebugInterfaceEnable             = 0;
+  mMPSystemData->DebugInterfaceLockEnable         = 1;
+  if (mPlatformCpu->Revision >= DXE_PLATFORM_CPU_POLICY_PROTOCOL_REVISION_2) {
+    mMPSystemData->DebugInterfaceEnable             = mPlatformCpu->CpuConfig->DebugInterfaceEnable;
+    mMPSystemData->DebugInterfaceLockEnable         = mPlatformCpu->CpuConfig->DebugInterfaceLockEnable;
+  }
+
+  mMPSystemData->S3DataPointer.S3BootScriptTable  = (UINT32) (UINTN) mMPSystemData->S3BootScriptTable;
+  mMPSystemData->S3DataPointer.S3BspMtrrTable     = (UINT32) (UINTN) mMPSystemData->S3BspMtrrTable;
+
+  mMPSystemData->CpuArch                          = NULL;
+  gBS->LocateProtocol (&gEfiCpuArchProtocolGuid, NULL, (VOID **) &mMPSystemData->CpuArch);
+
+  mMPSystemData->MaximumCpusForThisSystem = MaximumCPUsForThisSystem;
+
+  mMPSystemData->BSP                      = 0;
+
+  ///
+  /// Save Mtrr Register for S3 resume
+  ///
+  SaveBspMtrrForS3 ();
+
+  FillInProcessorInformation (mMPSystemData, TRUE, 0);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wake up APs for the first time to count their number and collect BIST data.
+
+  @param[in] WakeUpBuffer      - Address of the wakeup buffer.
+
+  @retval EFI_SUCCESS       - Function successfully finishes.
+**/
+EFI_STATUS
+CountApNumberAndCollectBist (
+  IN EFI_PHYSICAL_ADDRESS WakeUpBuffer
+  )
+{
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+  UINTN                Index;
+  UINT64               MsrValue;
+  UINT64               ProcessorThreadCount;
+  UINT32               ResponseProcessorCount;
+  UINTN                TimeoutTime;
+
+  ///
+  /// Send INIT IPI - SIPI to all APs
+  ///
+  SendInterrupt (
+          BROADCAST_MODE_ALL_EXCLUDING_SELF,
+          0,
+          0,
+          DELIVERY_MODE_INIT,
+          TRIGGER_MODE_EDGE,
+          TRUE
+          );
+  gBS->Stall (10 * STALL_ONE_MILLI_SECOND);
+  SendInterrupt (
+          BROADCAST_MODE_ALL_EXCLUDING_SELF,
+          0,
+          (UINT32) RShiftU64 (WakeUpBuffer,
+                              12),
+          DELIVERY_MODE_SIPI,
+          TRIGGER_MODE_EDGE,
+          TRUE
+          );
+  gBS->Stall (200 * STALL_ONE_MICRO_SECOND);
+  SendInterrupt (
+          BROADCAST_MODE_ALL_EXCLUDING_SELF,
+          0,
+          (UINT32) RShiftU64 (WakeUpBuffer,
+                              12),
+          DELIVERY_MODE_SIPI,
+          TRIGGER_MODE_EDGE,
+          TRUE
+          );
+  gBS->Stall (200 * STALL_ONE_MICRO_SECOND);
+  ExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+
+  ///
+  /// Get thread count
+  ///
+  MsrValue              = AsmReadMsr64 (MSR_CORE_THREAD_COUNT);
+  ProcessorThreadCount  = MsrValue & 0xffff;
+
+  ///
+  /// Only support MAXIMUM_CPU_NUMBER threads so far
+  ///
+  ASSERT (ProcessorThreadCount <= MAXIMUM_CPU_NUMBER);
+  if (ProcessorThreadCount > MAXIMUM_CPU_NUMBER) {
+    ProcessorThreadCount = MAXIMUM_CPU_NUMBER;
+  }
+
+  for (TimeoutTime = 0; TimeoutTime <= CPU_WAIT_FOR_TASK_TO_BE_COMPLETED; TimeoutTime += CPU_CHECK_AP_INTERVAL) {
+    ///
+    /// Wait for task to complete and then exit.
+    ///
+    gBS->Stall (CPU_CHECK_AP_INTERVAL);
+    for (Index = 1, ResponseProcessorCount = 1; Index < MAXIMUM_CPU_NUMBER; Index++) {
+      if (ExchangeInfo->BistBuffer[Index].Number == 1) {
+        ResponseProcessorCount++;
+      }
+    }
+
+    if (ResponseProcessorCount == ProcessorThreadCount) {
+      break;
+    }
+  }
+
+  for (Index = 0; Index < MAXIMUM_CPU_NUMBER; Index++) {
+    if (ExchangeInfo->BistBuffer[Index].Number == 1) {
+      ExchangeInfo->BistBuffer[Index].Number = (UINT32) mMPSystemData->NumberOfCpus++;
+    }
+  }
+
+  mAcpiCpuData->NumberOfCpus  = (UINT32) mMPSystemData->NumberOfCpus;
+
+  ExchangeInfo->InitFlag      = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wake up APs for the second time to collect detailed information.
+
+  @param[in] WakeUpBuffer      - Address of the wakeup buffer.
+
+  @retval EFI_SUCCESS       - Function successfully finishes.
+**/
+EFI_STATUS
+PollForInitialization (
+  IN EFI_PHYSICAL_ADDRESS WakeUpBuffer
+  )
+{
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+  UINT32               FailedRevision;
+  EFI_STATUS           Status;
+
+  ExchangeInfo              = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+
+  ExchangeInfo->ApFunction  = (VOID *) (UINTN) DetailedMpInitialization;
+
+  CpuInitFloatPointUnit ();
+
+  ///
+  /// Update microcode for BSP
+  ///
+  mMcuLoadCount = 0;
+  Status = InitializeMicrocode (mMicrocodePointerBuffer, &FailedRevision);
+  if (Status == EFI_LOAD_ERROR) {
+    DEBUG((EFI_D_INFO,"Failed to load microcode patch\n"));
+  }
+
+  ///
+  /// Wait until all APs finish
+  ///
+  while (mFinishedCount < mAcpiCpuData->NumberOfCpus - 1) {
+    CpuPause ();
+  }
+
+  McuFirstLoadDone ();
+  return EFI_SUCCESS;
+}
+
+/**
+  Callback function to initialize MP data hub
+
+  @param[in] Event   - Event whose notification function is being invoked.
+  @param[in] Context - Pointer to the notification functions context, which is implementation dependent.
+**/
+VOID
+EFIAPI
+MpDataHubCallback (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  CPU_DATA_FOR_DATAHUB *CpuDataforDatahub;
+  UINTN                Index;
+  EFI_STATUS           Status;
+
+  Status = InitializeDataHubPtr (&mDataHub);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  for (Index = 0; Index < mMPSystemData->NumberOfCpus; Index++) {
+    CpuDataforDatahub = &mMPSystemData->CpuData[Index].CpuDataforDatahub;
+    UpdateDataforDatahub (Index, CpuDataforDatahub);
+  }
+
+}
+
+/**
+  Initialize multiple processors and collect MP related data
+
+  @retval EFI_SUCCESS   - Multiple processors get initialized and data collected successfully
+  @retval Other         - The operation failed and appropriate error status will be returned
+**/
+EFI_STATUS
+InitializeMpSystemData (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  UINT32     MaxThreadsPerCore;
+  UINT32     MaxCoresPerDie;
+  UINT32     MaxDiesPerPackage;
+  UINT32     MaxPackages;
+
+  VOID                 *StackAddressStart;
+  EFI_PHYSICAL_ADDRESS WakeUpBuffer;
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+  UINTN                Index;
+
+  EFI_CPU_ARCH_PROTOCOL *CpuArch;
+  BOOLEAN               mInterruptState;
+  CPU_DATA_BLOCK        *CpuData;
+  UINTN                 MaximumCPUsForThisSystem;
+  VOID                  *Registration;
+  EFI_HANDLE            Handle;
+
+  Handle                = NULL;
+
+  ///
+  /// Program Local APIC registers
+  ///
+  ProgramXApic (TRUE);
+
+  ///
+  /// Get information on threads, cores, dies and package for the CPU(s) on this platform
+  ///
+  Status = mPlatformCpu->CpuConfig->GetMaxCount (
+                  mPlatformCpu,
+                  &MaxThreadsPerCore,
+                  &MaxCoresPerDie,
+                  &MaxDiesPerPackage,
+                  &MaxPackages
+                  );
+  ///
+  /// Get the total CPU count
+  ///
+  if (!EFI_ERROR (Status)) {
+    MaximumCPUsForThisSystem = MaxThreadsPerCore * MaxCoresPerDie * MaxDiesPerPackage * MaxPackages;
+  } else {
+    MaximumCPUsForThisSystem = MAXIMUM_CPU_NUMBER;
+  }
+
+  ///
+  /// Prepare Wakeup Buffer and Stack for APs
+  ///
+  Status = PrepareMemoryForAPs (
+                  &WakeUpBuffer,
+                  &StackAddressStart,
+                  MaximumCPUsForThisSystem
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  mOriginalBuffer = WakeUpBuffer;
+  Status          = (gBS->AllocatePages)(AllocateAnyPages, EfiBootServicesData, 1, &mBackupBuffer);
+
+  ///
+  /// Fill MP Data
+  ///
+  FillMPData (
+          WakeUpBuffer,
+          StackAddressStart,
+          MaximumCPUsForThisSystem
+          );
+
+  ///
+  /// Prepare exchange information for APs
+  ///
+  ExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+  PrepareExchangeInfo (
+          ExchangeInfo,
+          StackAddressStart,
+          NULL,
+          WakeUpBuffer
+          );
+
+  ReportStatusCode (
+          EFI_PROGRESS_CODE,
+          EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_HP_PC_AP_INIT
+          );
+
+  CpuArch = mMPSystemData->CpuArch;
+  (CpuArch->GetInterruptState)(CpuArch, &mInterruptState);
+  CpuArch->DisableInterrupt (CpuArch);
+
+  ///
+  /// Get BSP Performance Control setting
+  ///
+  mCpuPerfCtrlValue = AsmReadMsr64 (MSR_IA32_PERF_CTRL);
+
+#ifdef BOOT_GUARD_SUPPORT_FLAG
+  //
+  // Disable PBET before send IPI to APs
+  //
+  StopPbeTimer ();
+#endif
+
+  ///
+  /// First INIT-SIPI-SIPI and reset AP waking counters
+  ///
+  CountApNumberAndCollectBist (WakeUpBuffer);
+  ExchangeInfo->WakeUpApManner = WakeUpApCounterInit;
+
+  ///
+  /// Assign AP function to initialize FPU MCU MTRR and get detail info
+  ///
+  PollForInitialization (WakeUpBuffer);
+  ///
+  /// Assign WakeUpApManner (WakeUpApPerHltLoop/WakeUpApPerMwaitLoop/WakeUpApPerRunLoop)
+  ///
+  ExchangeInfo->WakeUpApManner = mPlatformCpu->CpuConfig->ApIdleManner;
+  ///
+  /// Assign AP function to ApProcWrapper for StartAllAps/StartThisAp calling
+  ///
+  ExchangeInfo->ApFunction = (VOID *) (UINTN) ApProcWrapper;
+
+  if (mInterruptState) {
+    CpuArch->EnableInterrupt (CpuArch);
+  }
+
+  for (Index = 1; Index < mMPSystemData->NumberOfCpus; Index++) {
+    CpuData = &mMPSystemData->CpuData[Index];
+    if (CpuData->Health.Uint32 != 0) {
+      ReportStatusCode (
+              EFI_ERROR_MAJOR | EFI_ERROR_CODE,
+              EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_HP_EC_SELF_TEST
+              );
+    }
+
+    Status = CheckMicrocodeUpdate (Index, CpuData->MicrocodeStatus, CpuData->FailedRevision);
+    if (Status == EFI_LOAD_ERROR) {
+      DEBUG((EFI_D_INFO,"Failed to load microcode patch\n"));
+    }
+  }
+
+  Status = gBS->CreateEvent (
+                  EFI_EVENT_TIMER | EFI_EVENT_NOTIFY_SIGNAL,
+                  EFI_TPL_CALLBACK,
+                  CheckAllAPsStatus,
+                  NULL,
+                  &mMPSystemData->CheckAllAPsEvent
+                  );
+  for (Index = 0; Index < mMPSystemData->NumberOfCpus; Index++) {
+    CpuData = &mMPSystemData->CpuData[Index];
+    if (Index == mMPSystemData->BSP) {
+      continue;
+    }
+    Status = gBS->CreateEvent (
+                    EFI_EVENT_TIMER | EFI_EVENT_NOTIFY_SIGNAL,
+                    EFI_TPL_CALLBACK,
+                    CheckThisAPStatus,
+                    (VOID *) CpuData,
+                    &CpuData->CheckThisAPEvent
+                    );
+  }
+  ///
+  /// Switch BSP to Lowest Feature Processor (LFP)
+  ///
+  if (mPlatformCpu->CpuConfig->BspSelection == 16) {
+    Status = SwitchToLowestFeatureProcess (&mMpService);
+  }
+
+  ///
+  /// Initialize feature control structure
+  ///
+  InitializeFeaturePerSetup (mMPSystemData);
+
+  ///
+  /// Detect and log all processor supported features
+  ///
+  CollectProcessorFeature (&mMpService);
+  Status = mMpService.StartupAllAPs (
+                  &mMpService,
+                  CollectProcessorFeature,
+                  FALSE,
+                  NULL,
+                  0,
+                  (VOID *) &mMpService,
+                  NULL
+                  );
+
+  ProgramProcessorFeature (&mMpService);
+  Status = mMpService.StartupAllAPs (
+                  &mMpService,
+                  ProgramProcessorFeature,
+                  FALSE,
+                  NULL,
+                  0,
+                  (VOID *) &mMpService,
+                  NULL
+                  );
+
+  ///
+  /// Install CPU info protocol
+  ///
+  mCpuInfo.Revision           = DXE_CPU_INFO_REVISION_1;
+  mCpuInfo.RcVersion          = CPU_RC_VERSION;
+  mCpuInfo.CpuCommonFeatures  = mCommonFeatures;
+
+  gBS->InstallMultipleProtocolInterfaces (
+          &Handle,
+          &gDxeCpuInfoProtocolGuid,
+          &mCpuInfo,
+          NULL
+          );
+  DEBUG ((EFI_D_INFO, "mCommonFeatures = %x\n", mCommonFeatures));
+
+  EfiCreateProtocolNotifyEvent (
+          &gEfiDataHubProtocolGuid,
+          EFI_TPL_CALLBACK,
+          MpDataHubCallback,
+          NULL,
+          &Registration
+          );
+
+  CopyMem ((VOID *) (UINTN) mBackupBuffer, (VOID *) (UINTN) mOriginalBuffer, EFI_PAGE_SIZE);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wrapper function for all procedures assigned to AP via MP service protocol.
+  It controls states of AP and invokes assigned precedure.
+**/
+VOID
+ApProcWrapper (
+  VOID
+  )
+{
+  EFI_AP_PROCEDURE     Procedure;
+  VOID                 *Parameter;
+  UINTN                CpuNumber;
+  CPU_DATA_BLOCK       *CpuData;
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+  MONITOR_MWAIT_DATA   *MonitorAddr;
+
+  ///
+  /// Initialize MCE for CR4.
+  ///
+  InitializeMce (mMPSystemData->MachineCheckEnable);
+
+  WhoAmI (&mMpService, &CpuNumber);
+  CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+  ///
+  /// Now let us check it out.
+  ///
+  Procedure = CpuData->Procedure;
+  Parameter = CpuData->Parameter;
+
+  if (Procedure != NULL) {
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuData->State = CPU_STATE_BUSY;
+    AsmReleaseMPLock (&CpuData->StateLock);
+    Procedure (Parameter);
+
+    ///
+    /// if BSP is switched to AP, it continue execute from here, but it carries register state
+    /// of the old AP, so need to reload CpuData (might be stored in a register after compiler
+    /// optimization) to make sure it points to the right data
+    ///
+    WhoAmI (&mMpService, &CpuNumber);
+    CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+    AsmAcquireMPLock (&CpuData->ProcedureLock);
+    CpuData->Procedure = NULL;
+    AsmReleaseMPLock (&CpuData->ProcedureLock);
+
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuData->State = CPU_STATE_FINISHED;
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    ///
+    /// Check AP wakeup manner, update signal and relating counter once finishing AP task
+    ///
+    ExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (mAcpiCpuData->WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+    MonitorAddr = (MONITOR_MWAIT_DATA *)
+            (
+                    (UINT8 *) ExchangeInfo->StackStart +
+                    (ExchangeInfo->BistBuffer[CpuData->ApicID].Number + 1) *
+                    ExchangeInfo->StackSize -
+                    MONITOR_FILTER_SIZE
+            );
+
+    switch (ExchangeInfo->WakeUpApManner) {
+      case WakeUpApPerHltLoop:
+        MonitorAddr->HltLoopBreakCounter += 1;
+        break;
+
+      case WakeUpApPerMwaitLoop:
+        MonitorAddr->MwaitLoopBreakCounter += 1;
+        break;
+
+      case WakeUpApPerRunLoop:
+        MonitorAddr->RunLoopBreakCounter += 1;
+        break;
+
+      case WakeUpApPerMwaitLoop32:
+        MonitorAddr->MwaitLoopBreakCounter32 += 1;
+        break;
+
+      case WakeUpApPerRunLoop32:
+        MonitorAddr->RunLoopBreakCounter32 += 1;
+        break;
+
+      default:
+        break;
+    }
+
+    MonitorAddr->BreakToRunApSignal = 0;
+  }
+}
+
+/**
+  Procedure for detailed initialization of APs. It will be assigned to all APs
+  after first INIT-SIPI-SIPI finishing CPU number counting and BIST collection.
+**/
+VOID
+DetailedMpInitialization (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  UINT32     FailedRevision;
+
+  ///
+  /// Program APs CPU Performance Control setting
+  ///
+  AsmWriteMsr64 (MSR_IA32_PERF_CTRL, mCpuPerfCtrlValue);
+
+  CpuInitFloatPointUnit ();
+
+  Status = InitializeMicrocode (
+                  (EFI_CPU_MICROCODE_HEADER **) (UINTN) mAcpiCpuData->MicrocodePointerBuffer,
+                  &FailedRevision
+                  );
+
+  ///
+  /// Init MLC Streamer Prefetcher and MLC Spatial Prefetcher
+  ///
+  InitializeProcessorsPrefetcher (
+          mPlatformCpu->CpuConfig->MlcStreamerPrefetcher,
+          mPlatformCpu->CpuConfig->MlcSpatialPrefetcher
+          );
+
+  ///
+  /// Init Cache after all MCU is loaded
+  ///
+  while (mMcuLoadCount < mAcpiCpuData->NumberOfCpus) {
+    CpuPause ();
+  }
+
+  ///
+  /// Save Mtrr Registers in global data areas
+  ///
+  MpMtrrSynchUp (NULL);
+  ProgramXApic (FALSE);
+  FillInProcessorInformation (mMPSystemData, FALSE, 0);
+  InterlockedIncrement (&mFinishedCount);
+}
+
+/**
+  Switch current BSP processor to AP
+
+  @param[in] MPSystemData  - Pointer to the data structure containing MP related data
+**/
+VOID
+FutureBSPProc (
+  IN MP_SYSTEM_DATA *MPSystemData
+  )
+{
+  AsmExchangeRole (&MPSystemData->APInfo, &MPSystemData->BSPInfo);
+  return;
+}
+
+/**
+  This function is called by all processors (both BSP and AP) once and collects MP related data
+
+  @param[in] MPSystemData  - Pointer to the data structure containing MP related data
+  @param[in] BSP           - TRUE if the CPU is BSP
+  @param[in] BistParam     - BIST (build-in self test) data for the processor. This data
+                             is only valid for processors that are waked up for the 1ast
+                             time in this CPU DXE driver.
+
+  @retval EFI_SUCCESS   - Data for the processor collected and filled in
+**/
+EFI_STATUS
+FillInProcessorInformation (
+  IN MP_SYSTEM_DATA *MPSystemData,
+  IN BOOLEAN        BSP,
+  IN UINT32         BistParam
+  )
+{
+  UINT32               Health;
+  UINT32               ApicID;
+  CPU_DATA_BLOCK       *CpuData;
+  UINT32               BIST;
+  UINTN                CpuNumber;
+  UINTN                Index;
+  UINTN                Count;
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+
+  ApicID  = GetApicID (NULL, NULL);
+  BIST    = 0;
+
+  if (BSP) {
+    CpuNumber = 0;
+    BIST      = BistParam;
+  } else {
+    ExchangeInfo  = (MP_CPU_EXCHANGE_INFO *) (UINTN) (mAcpiCpuData->WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+    CpuNumber     = ExchangeInfo->BistBuffer[ApicID].Number;
+    BIST          = ExchangeInfo->BistBuffer[ApicID].BIST;
+  }
+
+  CpuData                 = &MPSystemData->CpuData[CpuNumber];
+  CpuData->SecondaryCpu   = IsSecondaryThread ();
+  CpuData->ApicID         = ApicID;
+  CpuData->Procedure      = NULL;
+  CpuData->Parameter      = NULL;
+  CpuData->StateLock      = VacantFlag;
+  CpuData->ProcedureLock  = VacantFlag;
+  CpuData->State          = CPU_STATE_IDLE;
+
+  Health                  = BIST;
+  Count                   = MPSystemData->BistHobSize / sizeof (BIST_HOB_DATA);
+  for (Index = 0; Index < Count; Index++) {
+    if (ApicID == MPSystemData->BistHobData[Index].ApicId) {
+      Health = MPSystemData->BistHobData[Index].Health.Uint32;
+    }
+  }
+
+  if (Health > 0) {
+    CpuData->State                        = CPU_STATE_DISABLED;
+    MPSystemData->DisableCause[CpuNumber] = CPU_CAUSE_SELFTEST_FAILURE;
+
+  } else {
+    MPSystemData->DisableCause[CpuNumber] = CPU_CAUSE_NOT_DISABLED;
+  }
+
+  FillinDataforDataHub (CpuNumber, &CpuData->CpuDataforDatahub);
+  CpuData->CpuDataforDatahub.Health.Uint32 = Health;
+
+  CopyMem (&CpuData->PhysicalLocation, &CpuData->CpuDataforDatahub.Location, sizeof (PHYSICAL_LOCATION));
+  return EFI_SUCCESS;
+}
+
+/**
+  Set APIC BSP bit
+
+  @param[in] Enable  - enable as BSP or not
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+SetApicBSPBit (
+  IN BOOLEAN Enable
+  )
+{
+  UINT64 ApicBaseReg;
+
+  ApicBaseReg = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+
+  if (Enable) {
+    ApicBaseReg |= 0x100;
+  } else {
+    ApicBaseReg &= 0xfffffffffffffe00;
+  }
+
+  AsmWriteMsr64 (MSR_IA32_APIC_BASE, ApicBaseReg);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Change CPU state
+
+  @param[in] CpuNumber - CPU number
+  @param[in] NewState  - the new state that will be changed to
+  @param[in] Cause     - Cause
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+ChangeCpuState (
+  IN UINTN                  CpuNumber,
+  IN BOOLEAN                NewState,
+  IN CPU_STATE_CHANGE_CAUSE Cause
+  )
+{
+  CPU_DATA_BLOCK                             *CpuData;
+  EFI_COMPUTING_UNIT_CPU_DISABLED_ERROR_DATA ErrorData;
+
+  CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+  mMPSystemData->DisableCause[CpuNumber] = Cause;
+
+  if (!NewState) {
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuData->State = CPU_STATE_DISABLED;
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    ErrorData.DataHeader.HeaderSize = sizeof (EFI_STATUS_CODE_DATA);
+    ErrorData.DataHeader.Size       = sizeof (EFI_COMPUTING_UNIT_CPU_DISABLED_ERROR_DATA) - sizeof (EFI_STATUS_CODE_DATA);
+    CopyMem (
+            &ErrorData.DataHeader.Type,
+            &gEfiStatusCodeSpecificDataGuid,
+            sizeof (EFI_GUID)
+            );
+    ErrorData.Cause             = Cause;
+    ErrorData.SoftwareDisabled  = TRUE;
+//(AMI_CHG)>
+/*    ReportStatusCode (
+            EFI_ERROR_MINOR | EFI_ERROR_CODE,
+            EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_EC_DISABLED
+            );*/
+//<(AMI_CHG)
+  } else {
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuData->State = CPU_STATE_IDLE;
+    AsmReleaseMPLock (&CpuData->StateLock);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  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 (NULL, NULL);
+
+  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;
+  }
+}
+
+/**
+  If timeout occurs in StartupAllAps(), a timer is set, which invokes this
+  procedure periodically to check whether all APs have finished.
+
+  @param[in] Event   - Event triggered.
+  @param[in] Context - Parameter passed with the event.
+**/
+VOID
+CheckAllAPsStatus (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  UINTN          CpuNumber;
+  UINTN          NextCpuNumber;
+  CPU_DATA_BLOCK *CpuData;
+  CPU_DATA_BLOCK *NextCpuData;
+  EFI_STATUS     Status;
+  CPU_STATE      CpuState;
+
+  for (CpuNumber = 0; CpuNumber < mMPSystemData->NumberOfCpus; CpuNumber++) {
+    CpuData = &mMPSystemData->CpuData[CpuNumber];
+    if (CpuNumber == mMPSystemData->BSP) {
+      continue;
+    }
+
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuState = CpuData->State;
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    switch (CpuState) {
+      case CPU_STATE_READY:
+        WakeUpAp (
+                CpuData,
+                mMPSystemData->Procedure,
+                mMPSystemData->ProcArguments
+                );
+        break;
+
+      case CPU_STATE_FINISHED:
+        if (mMPSystemData->SingleThread) {
+          Status = GetNextBlockedCpuNumber (&NextCpuNumber);
+          if (!EFI_ERROR (Status)) {
+            NextCpuData = &mMPSystemData->CpuData[NextCpuNumber];
+
+            AsmAcquireMPLock (&NextCpuData->StateLock);
+            NextCpuData->State = CPU_STATE_READY;
+            AsmReleaseMPLock (&NextCpuData->StateLock);
+
+            WakeUpAp (
+                    NextCpuData,
+                    mMPSystemData->Procedure,
+                    mMPSystemData->ProcArguments
+                    );
+          }
+        }
+
+        AsmAcquireMPLock (&CpuData->StateLock);
+        CpuData->State = CPU_STATE_IDLE;
+        AsmReleaseMPLock (&CpuData->StateLock);
+
+        mMPSystemData->FinishCount++;
+        break;
+
+      default:
+        break;
+    }
+  }
+
+  if (mMPSystemData->FinishCount == mMPSystemData->StartCount) {
+    gBS->SetTimer (
+            mMPSystemData->CheckAllAPsEvent,
+            TimerCancel,
+            0
+            );
+    Status = gBS->SignalEvent (mMPSystemData->WaitEvent);
+  }
+
+  return;
+}
+
+/**
+  Check if this AP has finished task
+
+  @param[in] Event   - Event triggered.
+  @param[in] Context - Parameter passed with the event.
+**/
+VOID
+CheckThisAPStatus (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  CPU_DATA_BLOCK *CpuData;
+  EFI_STATUS     Status;
+  CPU_STATE      CpuState;
+
+  CpuData = (CPU_DATA_BLOCK *) Context;
+
+  AsmAcquireMPLock (&CpuData->StateLock);
+  CpuState = CpuData->State;
+  AsmReleaseMPLock (&CpuData->StateLock);
+
+  if (CpuState == CPU_STATE_FINISHED) {
+    gBS->SetTimer (
+          CpuData->CheckThisAPEvent,
+          TimerCancel,
+          0
+          );
+    Status = gBS->SignalEvent (mMPSystemData->WaitEvent);
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuData->State = CPU_STATE_IDLE;
+    AsmReleaseMPLock (&CpuData->StateLock);
+  }
+
+  return;
+}
+
+/**
+  Convert the timeout value to TSC value
+
+  @param[in] TimeoutInMicroSecs  - how many microseconds the timeout is
+
+  @retval expected TSC value for timeout
+**/
+UINT64
+CalculateTimeout (
+  IN UINTN TimeoutInMicroSecs
+  )
+{
+  UINT64     CurrentTsc;
+  UINT64     ExpectedTsc;
+  UINT64     Frequency;
+  EFI_STATUS Status;
+
+  if (TimeoutInMicroSecs == 0) {
+    return 0xffffffffffff;
+  }
+
+  CurrentTsc  = EfiReadTsc ();
+
+  Status      = GetActualFrequency (mMetronome, &Frequency);
+
+  ExpectedTsc = CurrentTsc + MultU64x32 (Frequency, (UINT32) TimeoutInMicroSecs);
+
+  return ExpectedTsc;
+}
+
+/**
+  Check if timeout happened
+
+  @param[in] ExpectedTsc - the TSC value for timeout
+
+  @retval TRUE if timeout happened
+  @retval FALSE if timeout not yet happened
+**/
+BOOLEAN
+CheckTimeout (
+  IN UINT64 ExpectedTsc
+  )
+{
+  UINT64 CurrentTsc;
+
+  CurrentTsc = EfiReadTsc ();
+  if (CurrentTsc >= ExpectedTsc) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get the next blocked processor
+
+  @param[in] NextCpuNumber - that will be updated for next blocked CPU number
+
+  @retval EFI_SUCCESS - The next blocked CPU found
+  @retval EFI_NOT_FOUND - cannot find blocked CPU
+**/
+EFI_STATUS
+GetNextBlockedCpuNumber (
+  OUT UINTN *NextCpuNumber
+  )
+{
+  UINTN          CpuNumber;
+  CPU_STATE      CpuState;
+  CPU_DATA_BLOCK *CpuData;
+
+  for (CpuNumber = 0; CpuNumber < mMPSystemData->NumberOfCpus; CpuNumber++) {
+    if (CpuNumber == mMPSystemData->BSP) {
+      continue;
+    }
+
+    CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuState = CpuData->State;
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    if (CpuState == CPU_STATE_BLOCKED) {
+      *NextCpuNumber = CpuNumber;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Update data hub for processor status
+
+  @param[in] CpuNumber         - CPU number
+  @param[in] CpuDataforDatahub - the data hub that will be updated
+**/
+VOID
+UpdateDataforDatahub (
+  IN UINTN                 CpuNumber,
+  OUT CPU_DATA_FOR_DATAHUB *CpuDataforDatahub
+  )
+{
+  CpuDataforDatahub->Status = GetProcessorStatus (CpuNumber);
+  InitializeProcessorData (CpuNumber, CpuDataforDatahub);
+  InitializeCacheData (CpuNumber, CpuDataforDatahub->CacheInformation);
+
+  return;
+}
+
+/**
+  Function to wake up a specified AP and assign procedure to it.
+
+  @param[in] CpuData       - CPU data block for the specified AP.
+  @param[in] Procedure     - Procedure to assign.
+  @param[in] ProcArguments - Argument for Procedure.
+**/
+VOID
+WakeUpAp (
+  IN CPU_DATA_BLOCK   *CpuData,
+  IN EFI_AP_PROCEDURE Procedure,
+  IN VOID             *ProcArguments
+  )
+{
+  MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+  MONITOR_MWAIT_DATA   *MonitorAddr;
+
+  AsmAcquireMPLock (&CpuData->ProcedureLock);
+  CpuData->Parameter  = ProcArguments;
+  CpuData->Procedure  = Procedure;
+  AsmReleaseMPLock (&CpuData->ProcedureLock);
+
+  ExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (mAcpiCpuData->WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+  MonitorAddr = (MONITOR_MWAIT_DATA *)
+          (
+                  (UINT8 *) ExchangeInfo->StackStart +
+                  (ExchangeInfo->BistBuffer[CpuData->ApicID].Number + 1) *
+                  ExchangeInfo->StackSize -
+                  MONITOR_FILTER_SIZE
+          );
+
+  if (MonitorAddr->WakeUpApVectorChangeFlag == TRUE || ExchangeInfo->WakeUpApManner == WakeUpApPerHltLoop) {
+    SendInterrupt (
+            BROADCAST_MODE_SPECIFY_CPU,
+            CpuData->ApicID,
+            0,
+            DELIVERY_MODE_INIT,
+            TRIGGER_MODE_EDGE,
+            TRUE
+            );
+    SendInterrupt (
+            BROADCAST_MODE_SPECIFY_CPU,
+            CpuData->ApicID,
+            (UINT32) RShiftU64 (mAcpiCpuData->WakeUpBuffer,
+                                12),
+            DELIVERY_MODE_SIPI,
+            TRIGGER_MODE_EDGE,
+            TRUE
+            );
+    MonitorAddr->WakeUpApVectorChangeFlag = FALSE;
+
+    //
+    // Clear StateLock to 0 to avoid AP locking it then entering SMM and getting INIT-SIPI here could cause dead-lock
+    //
+    CpuData->StateLock = 0;
+  }
+
+  MonitorAddr->BreakToRunApSignal = (UINTN) (BREAK_TO_RUN_AP_SIGNAL | CpuData->ApicID);
+}
+
+/**
+  Check whether any AP is running for assigned task.
+
+  @retval TRUE  - Some APs are running.
+  @retval FALSE - No AP is running.
+**/
+BOOLEAN
+ApRunning (
+  VOID
+  )
+{
+  CPU_DATA_BLOCK *CpuData;
+  UINTN          CpuNumber;
+
+  for (CpuNumber = 0; CpuNumber < mMPSystemData->NumberOfCpus; CpuNumber++) {
+    CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+    if (CpuNumber != mMPSystemData->BSP) {
+      if (CpuData->State == CPU_STATE_READY || CpuData->State == CPU_STATE_BUSY) {
+        return TRUE;
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Re-load microcode patch before boot.
+
+  @param[in] MpServices - point to EFI_MP_SERVICES_PROTOCOL
+
+  @retval EFI_SUCCESS           - one processor re-loads microcode patch
+**/
+EFI_STATUS
+ReLoadMicrocodePatch (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  )
+{
+  UINT32     FailedRevision;
+  UINTN      CpuNumber;
+  EFI_STATUS Status;
+
+  Status = InitializeMicrocode (
+                  mMicrocodePointerBuffer,
+                  &FailedRevision
+                  );
+  WhoAmI (
+          &mMpService,
+          &CpuNumber
+          );
+  Status = CheckMicrocodeUpdate (
+                  CpuNumber,
+                  Status,
+                  FailedRevision
+                  );
+  return Status;
+}
+
+/**
+  Re-load microcode patch before boot.
+
+  @retval EFI_SUCCESS           - Multiple processors re-load microcode patch
+**/
+EFI_STATUS
+ReLoadMicrocodeBeforeBoot (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+
+  ///
+  /// Added code to handle microcode patch reload here!!!
+  ///
+  mMcuLoadCount = 0;
+  Status = ReLoadMicrocodePatch (&mMpService);
+  if (!EFI_ERROR (Status)) {
+    Status = mMpService.StartupAllAPs (
+                    &mMpService,
+                    ReLoadMicrocodePatch,
+                    FALSE,
+                    NULL,
+                    0,
+                    (VOID *) &mMpService,
+                    NULL
+                    );
+  }
+  return Status;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MpService.h b/ReferenceCode/Haswell/CpuInit/Dxe/MpService.h
new file mode 100644
index 0000000..7fa7544
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MpService.h
@@ -0,0 +1,670 @@
+/** @file
+  Some definitions for MP services 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 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 _MP_SERVICE_H_
+#define _MP_SERVICE_H_
+
+#include "MpCommon.h"
+
+///
+/// Driver Produced Protocol.
+///
+#include EFI_PROTOCOL_PRODUCER (MpService)
+#include EFI_PROTOCOL_PRODUCER (PiMpService)
+
+//
+// Constant definitions
+//
+#define FOURGB                            0x100000000
+#define ONEPAGE                           0x1000
+
+#define RENDEZVOUS_PROC_LENGTH            0x1000
+#define STACK_SIZE_PER_PROC               0x8000
+#define MAX_CPU_S3_MTRR_ENTRY             0x0020
+#define MAX_CPU_S3_TABLE_SIZE             0x0400
+
+#define AP_HALT_CODE_SIZE                 10
+
+#define CPU_CHECK_AP_INTERVAL             10     // multiply to microseconds for gBS->SetTimer in 100nsec.
+#define CPU_WAIT_FOR_TASK_TO_BE_COMPLETED 100000 // microseconds
+///
+///  The MP data structure follows.
+///
+#define CPU_SWITCH_STATE_IDLE   0
+#define CPU_SWITCH_STATE_STORED 1
+#define CPU_SWITCH_STATE_LOADED 2
+
+#define MSR_L3_CACHE_DISABLE    0x40
+
+typedef struct {
+  UINT8             Lock;         ///< offset 0
+  UINT8             State;        ///< offset 1
+  UINTN             StackPointer; ///< offset 4 / 8
+  PSEUDO_DESCRIPTOR Gdtr;         ///< offset 8 / 16
+  PSEUDO_DESCRIPTOR Idtr;         ///< offset 14 / 26
+} CPU_EXCHANGE_ROLE_INFO;
+
+//
+// MTRR table definitions
+//
+typedef struct {
+  UINT16 Index;
+  UINT64 Value;
+} EFI_MTRR_VALUES;
+
+typedef enum {
+  CPU_STATE_IDLE,
+  CPU_STATE_BLOCKED,
+  CPU_STATE_READY,
+  CPU_STATE_BUSY,
+  CPU_STATE_FINISHED,
+  CPU_STATE_DISABLED
+} CPU_STATE;
+
+//
+// Define CPU feature information
+//
+#define MAX_FEATURE_NUM 6
+typedef struct {
+  UINTN  Index;
+  UINT32 ApicId;
+  UINT32 Version;
+  UINT32 FeatureDelta;
+  UINT32 Features[MAX_FEATURE_NUM];
+} LEAST_FEATURE_PROC;
+
+///
+/// Define Individual Processor Data block.
+///
+typedef struct {
+  UINT32               ApicID;
+  EFI_AP_PROCEDURE     Procedure;
+  VOID                 *Parameter;
+  UINT8                StateLock;
+  UINT8                ProcedureLock;
+  EFI_MP_HEALTH_FLAGS  Health;
+  BOOLEAN              SecondaryCpu;
+  UINTN                NumberOfCores;
+  UINTN                NumberOfThreads;
+  UINT64               ActualFsbFrequency;
+  EFI_STATUS           MicrocodeStatus;
+  UINT32               FailedRevision;
+  PHYSICAL_LOCATION    PhysicalLocation;
+  CPU_STATE            State;
+  CPU_DATA_FOR_DATAHUB CpuDataforDatahub;
+
+  //
+  // for PI MP Services Protocol
+  //
+  BOOLEAN   *Finished;
+  UINT64    ExpectedTime;
+  EFI_EVENT WaitEvent;
+  EFI_EVENT             CheckThisAPEvent;
+} CPU_DATA_BLOCK;
+
+typedef struct {
+  UINT32 ApicId;
+  UINT32 MsrIndex;
+  UINT64 MsrValue;
+} MP_CPU_S3_SCRIPT_DATA;
+
+typedef struct {
+  UINT32 S3BootScriptTable;
+  UINT32 S3BspMtrrTable;
+} MP_CPU_S3_DATA_POINTER;
+
+#pragma pack(1)
+typedef struct {
+  UINT32              ApicId;
+  EFI_MP_HEALTH_FLAGS Health;
+} BIST_HOB_DATA;
+#pragma pack()
+
+///
+/// Define MP data block which consumes individual processor block.
+///
+typedef struct {
+  UINT8 APSerializeLock;
+
+  BOOLEAN LimitCpuidMaximumValue;           ///< make processor look like < F40
+  BOOLEAN VmxEnable;
+  BOOLEAN ProcessorVmxEnable;
+  BOOLEAN ProcessorBistEnable;
+  BOOLEAN TxtEnable;
+  BOOLEAN MonitorMwaitEnable;
+  BOOLEAN ExecuteDisableBit;
+  BOOLEAN MachineCheckEnable;
+  BOOLEAN XapicEnable;
+  BOOLEAN AesEnable;
+  BOOLEAN DebugInterfaceEnable;
+  BOOLEAN DebugInterfaceLockEnable;
+  BOOLEAN EnableSecondaryCpu;
+  UINTN   NumberOfCpus;
+  UINTN   MaximumCpusForThisSystem;
+
+  CPU_EXCHANGE_ROLE_INFO BSPInfo;
+  CPU_EXCHANGE_ROLE_INFO APInfo;
+
+  EFI_CPU_ARCH_PROTOCOL *CpuArch;
+  EFI_EVENT             CheckAllAPsEvent;
+  EFI_EVENT             WaitEvent;
+  UINTN                 BSP;
+  BIST_HOB_DATA         *BistHobData;
+  UINTN                 BistHobSize;
+
+  UINTN            FinishCount;
+  UINTN            StartCount;
+  EFI_AP_PROCEDURE Procedure;
+  VOID             *ProcArguments;
+  BOOLEAN          SingleThread;
+  UINTN            StartedCpuNumber;
+
+  CPU_DATA_BLOCK         CpuData[MAXIMUM_CPU_NUMBER];
+  CPU_STATE_CHANGE_CAUSE DisableCause[MAXIMUM_CPU_NUMBER];
+
+  UINT8                  S3BootScriptLock;
+  UINT32                 S3BootScriptCount;
+  MP_CPU_S3_DATA_POINTER S3DataPointer;
+  MP_CPU_S3_SCRIPT_DATA  S3BootScriptTable[MAX_CPU_S3_TABLE_SIZE];
+  EFI_MTRR_VALUES        S3BspMtrrTable[MAX_CPU_S3_MTRR_ENTRY];
+
+  ///
+  /// for PI MP Services Protocol
+  ///
+  BOOLEAN   CpuList[MAXIMUM_CPU_NUMBER];
+  UINTN     **FailedCpuList;
+  UINT64    ExpectedTime;
+  EFI_EVENT CheckAPsEvent;
+} MP_SYSTEM_DATA;
+
+typedef struct {
+  ACPI_CPU_DATA            AcpiCpuData;
+  MP_SYSTEM_DATA           MPSystemData;
+  PSEUDO_DESCRIPTOR        GdtrProfile;
+  PSEUDO_DESCRIPTOR        IdtrProfile;
+  EFI_CPU_MICROCODE_HEADER *MicrocodePointerBuffer[NUMBER_OF_MICROCODE_UPDATE + 1];
+} MP_CPU_RESERVED_DATA;
+
+extern MP_SYSTEM_DATA              *mMPSystemData;
+extern ACPI_CPU_DATA               *mAcpiCpuData;
+extern EFI_MP_SERVICES_PROTOCOL    mMpService;
+extern EFI_PI_MP_SERVICES_PROTOCOL mPiMpService;
+
+///
+/// Prototypes.
+///
+
+/**
+  Get general MP information
+
+  @param[in] This                  - EFI_MP_SERVICE_PROTOCOL
+  @param[in] NumberOfCPUs          - Number of processors
+  @param[in] MaxiumNumberOfCPUs    - Max supported number of processors
+  @param[in] NumberOfEnabledCPUs   - Number of processors enabled
+  @param[in] RendezvousIntNumber   - number of Rendezvous procedure
+  @param[in] RendezvousProcLength  - length of Rendezvous procedure
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+EFIAPI
+GetGeneralMPInfo (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                   *NumberOfCPUs,
+  OUT UINTN                   *MaxiumNumberOfCPUs,
+  OUT UINTN                   *NumberOfEnabledCPUs,
+  OUT UINTN                   *RendezvousIntNumber,
+  OUT UINTN                   *RendezvousProcLength
+  );
+
+/**
+  Get processor context
+
+  @param[in] This                    - EFI_MP_SERVICE_PROTOCOL
+  @param[in] ProcessorNumber         - The handle number of processor.
+  @param[in] BufferLength            - buffer length
+  @param[in] ProcessorContextBuffer  - pointer to the buffer that will be updated
+
+  @retval EFI_INVALID_PARAMETER - buffer is NULL or CpuNumber our of range
+  @retval EFI_BUFFER_TOO_SMALL - buffer too small
+  @retval EFI_SUCCESS - got processor context successfully
+**/
+EFI_STATUS
+EFIAPI
+GetProcessorContext (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    ProcessorNumber,
+  IN OUT UINTN                *BufferLength,
+  IN OUT EFI_MP_PROC_CONTEXT  *ProcessorContextBuffer
+  );
+
+/**
+  MP Service to get specified application processor (AP)
+  to execute a caller-provided code stream.
+
+  @param[in] This                - Pointer to MP Service Protocol
+  @param[in] Procedure           - The procedure to be assigned to AP.
+  @param[in] ProcessorNumber     - The handle number of processor.
+  @param[in] WaitEvent           - If timeout, the event to be triggered after this AP finishes.
+  @param[in] TimeoutInMicroSecs  - The timeout value in microsecond. Zero means infinity.
+  @param[in] ProcArguments       - Argument for Procedure.
+
+  @retval EFI_INVALID_PARAMETER - Procudure is NULL.
+  @retval EFI_INVALID_PARAMETER - Number of CPU out of range, or it belongs to BSP.
+  @retval EFI_INVALID_PARAMETER - Specified CPU is not idle.
+  @retval EFI_SUCCESS           - The AP has finished.
+  @retval EFI_TIMEOUT           - Time goes out before the AP has finished.
+**/
+EFI_STATUS
+EFIAPI
+StartupThisAP (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE         Procedure,
+  IN UINTN                    ProcessorNumber,
+  IN EFI_EVENT WaitEvent      OPTIONAL,
+  IN UINTN TimeoutInMicroSecs OPTIONAL,
+  IN OUT VOID *ProcArguments  OPTIONAL
+  );
+
+/**
+  MP Service to get all the available application processors (APs)
+  to execute a caller-provided code stream.
+
+  @param[in] This                - Pointer to MP Service Protocol
+  @param[in] Procedure           - The procedure to be assigned to APs.
+  @param[in] SingleThread        - If true, all APs execute in block mode.
+                        Otherwise, all APs exceute in non-block mode.
+  @param[in] WaitEvent           - If timeout, the event to be triggered after all APs finish.
+  @param[in] TimeoutInMicroSecs  - The timeout value in microsecond. Zero means infinity.
+  @param[in] ProcArguments       - Argument for Procedure.
+  @param[in] FailedCPUList       - If not NULL, all APs that fail to start will be recorded in the list.
+
+  @retval EFI_INVALID_PARAMETER - Procudure is NULL.
+  @retval EFI_SUCCESS           - Only 1 logical processor exists.
+  @retval EFI_SUCCESS           - All APs have finished.
+  @retval EFI_TIMEOUT           - Time goes out before all APs have finished.
+**/
+EFI_STATUS
+EFIAPI
+StartupAllAPs (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE         Procedure,
+  IN BOOLEAN                  SingleThread,
+  IN EFI_EVENT WaitEvent      OPTIONAL,
+  IN UINTN TimeoutInMicroSecs OPTIONAL,
+  IN OUT VOID *ProcArguments  OPTIONAL,
+  OUT UINTN *FailedCPUList    OPTIONAL
+  );
+
+/**
+    MP Service to makes the current BSP into an AP and then switches the
+    designated AP into the AP. This procedure is usually called after a CPU
+    test that has found that BSP is not healthy to continue it's responsbilities.
+
+    @param[in] This         - Pointer to MP Service Protocol.
+    @param[in] ProcessorNumber - The handle number of processor.
+    @param[in] EnableOldBSPState - Whether to enable or disable the original BSP.
+
+    @retval EFI_INVALID_PARAMETER - Number for Specified AP out of range.
+    @retval EFI_INVALID_PARAMETER - Number of specified CPU belongs to BSP.
+    @retval EFI_NOT_READY         - Specified AP is not idle.
+    @retval EFI_SUCCESS           - BSP successfully switched.
+**/
+EFI_STATUS
+EFIAPI
+SwitchBSP (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    ProcessorNumber,
+  IN BOOLEAN                  EnableOldBSPState
+  );
+
+/**
+  This procedure sends an IPI to the designated processor in
+  the requested delivery mode with the requested vector.
+
+  @param[in] This         - Pointer to MP Service Protocol.
+  @param[in] ProcessorNumber - The handle number of processor.
+  @param[in] VectorNumber - Vector number.
+  @param[in] DeliveryMode - I/O APIC Interrupt Deliver Modes
+
+  @retval EFI_INVALID_PARAMETER - Input paramters were not correct.
+  @retval Other status          - Status returned by SendInterrupt ()
+**/
+EFI_STATUS
+EFIAPI
+SendIPI (
+  IN EFI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                    ProcessorNumber,
+  IN UINTN                    VectorNumber,
+  IN UINTN                    DeliveryMode
+  );
+
+/**
+  Implementation of EnableDisableAP() service of MP Services Protocol.
+
+  This service lets the caller enable or disable an AP.
+  This service may only be called from the BSP.
+
+  @param[in] This                  - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber       - The handle number of processor.
+  @param[in] NewAPState            - Indicates whether the newstate of the AP is enabled or disabled.
+  @param[in] HealthState           - Indicates new health state of the AP..
+
+  @retval EFI_SUCCESS            - AP successfully enabled or disabled.
+  @retval EFI_DEVICE_ERROR       - Caller processor is AP.
+  @retval EFI_NOT_FOUND          - Processor with the handle specified by ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETERS - ProcessorNumber specifies the BSP.
+**/
+EFI_STATUS
+EFIAPI
+EnableDisableAP (
+  IN EFI_MP_SERVICES_PROTOCOL   *This,
+  IN UINTN                      ProcessorNumber,
+  IN BOOLEAN                    NewAPState,
+  IN EFI_MP_HEALTH *HealthState OPTIONAL
+  );
+
+/**
+  Initialize multiple processors and collect MP related data
+
+  @retval EFI_SUCCESS           - Multiple processors get initialized and data collected successfully
+  @retval Other                 - The operation failed due to some reason
+**/
+EFI_STATUS
+InitializeMpSystemData (
+  VOID
+  );
+
+/**
+  Wake up all the application processors
+
+  @param[in] ImageHandle   - The firmware allocated handle for the EFI image.
+  @param[in] SystemTable   - A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS - APs are successfully waked up
+**/
+EFI_STATUS
+WakeUpAPs (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  );
+
+/**
+  Exchange 2 processors (BSP to AP or AP to BSP)
+
+  @param[in] MyInfo      - CPU info for current processor
+  @param[in] OthersInfo  - CPU info that will be exchanged with
+**/
+VOID
+AsmExchangeRole (
+  IN CPU_EXCHANGE_ROLE_INFO *MyInfo,
+  IN CPU_EXCHANGE_ROLE_INFO *OthersInfo
+  );
+
+/**
+  Switch current BSP processor to AP
+
+  @param[in] MPSystemData  - Pointer to the data structure containing MP related data
+**/
+VOID
+FutureBSPProc (
+  IN MP_SYSTEM_DATA *MPSystemData
+  );
+
+/**
+  Searches the HOB list provided by the core to find
+  if a MP guided HOB list exists or not. If it does, it copies it to the driver
+  data area, else returns 0
+
+  @param[in] MPSystemData - Pointer to an MP_SYSTEM_DATA structure
+
+  @retval EFI_SUCCESS  - Success
+  @retval EFI_NOT_FOUND - HOB not found or else
+**/
+EFI_STATUS
+GetMpBistStatus (
+  IN MP_SYSTEM_DATA *MPSystemData
+  );
+
+/**
+  Initialize the state information for the MP DXE Protocol.
+**/
+VOID
+EFIAPI
+InitializeMpServices (
+  VOID
+  );
+
+/**
+  Prepare for MTRR synchronization.
+
+  @retval CR4 value before changing.
+**/
+UINTN
+MpMtrrSynchUpEntry (
+  VOID
+  );
+
+/**
+  Restoration after MTRR synchronization.
+
+  @param[in] Cr4 - CR4 value before changing.
+**/
+VOID
+MpMtrrSynchUpExit (
+  UINTN Cr4
+  );
+
+/**
+  Copy Global MTRR data to S3
+**/
+VOID
+SaveBspMtrrForS3 (
+  VOID
+  );
+
+/**
+  This function is called by all processors (both BSP and AP) once and collects MP related data
+
+  @param[in] MPSystemData  - Pointer to the data structure containing MP related data
+  @param[in] BSP           - TRUE if the CPU is BSP
+  @param[in] BistParam     - BIST (build-in self test) data for the processor. This data
+                             is only valid for processors that are waked up for the 1st
+                             time in this CPU DXE driver.
+
+  @retval EFI_SUCCESS   - Data for the processor collected and filled in
+**/
+EFI_STATUS
+FillInProcessorInformation (
+  IN MP_SYSTEM_DATA *MPSystemData,
+  IN BOOLEAN        BSP,
+  IN UINT32         BistParam
+  );
+
+/**
+  Set APIC BSP bit
+
+  @param[in] Enable  - enable as BSP or not
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+SetApicBSPBit (
+  IN BOOLEAN Enable
+  );
+
+/**
+  Switch BSP to the processor which has least features
+
+  @param[in] MpServices  - EFI_MP_SERVICES_PROTOCOL
+
+  @retval EFI_STATUS  - status code returned from each sub-routines
+**/
+EFI_STATUS
+SwitchToLowestFeatureProcess (
+  IN EFI_MP_SERVICES_PROTOCOL *MpServices
+  );
+
+/**
+  Change CPU state
+
+  @param[in] CpuNumber - CPU number
+  @param[in] NewState  - the new state that will be changed to
+  @param[in] Cause     - Cause
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+ChangeCpuState (
+  IN UINTN                  CpuNumber,
+  IN BOOLEAN                NewState,
+  IN CPU_STATE_CHANGE_CAUSE Cause
+  );
+
+/**
+  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
+  );
+
+/**
+  If timeout occurs in StartupAllAps(), a timer is set, which invokes this
+  procedure periodically to check whether all APs have finished.
+
+  @param[in] Event   - Event triggered.
+  @param[in] Context - Parameter passed with the event.
+**/
+VOID
+CheckAllAPsStatus (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  Check if this AP has finished task
+
+  @param[in] Event   - Event triggered.
+  @param[in] Context - Parameter passed with the event.
+**/
+VOID
+CheckThisAPStatus (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  Convert the timeout value to TSC value
+
+  @param[in] TimeoutInMicroSecs  - how many microseconds the timeout is
+
+  @retval expected TSC value for timeout
+**/
+UINT64
+CalculateTimeout (
+  IN UINTN TimeoutInMicroSecs
+  );
+
+/**
+  Check if timeout happened
+
+  @param[in] ExpectedTsc - the TSC value for timeout
+
+  @retval TRUE if timeout happened
+  @retval FALSE if timeout not yet happened
+**/
+BOOLEAN
+CheckTimeout (
+  IN UINT64 ExpectedTsc
+  );
+
+/**
+  Get the next blocked processor
+
+  @param[in] NextCpuNumber - that will be updated for next blocked CPU number
+
+  @retval EFI_SUCCESS - The next blocked CPU found
+  @retval EFI_NOT_FOUND - cannot find blocked CPU
+**/
+EFI_STATUS
+GetNextBlockedCpuNumber (
+  OUT UINTN *NextCpuNumber
+  );
+
+/**
+  Update data hub for processor status
+
+  @param[in] CpuNumber         - CPU number
+  @param[in] CpuDataforDatahub - the data hub that will be updated
+**/
+VOID
+UpdateDataforDatahub (
+  IN UINTN                 CpuNumber,
+  OUT CPU_DATA_FOR_DATAHUB *CpuDataforDatahub
+  );
+
+/**
+  Procedure for detailed initialization of APs. It will be assigned to all APs while
+  they are waken up for the second time.
+**/
+VOID
+DetailedMpInitialization (
+  VOID
+  );
+
+/**
+  Function to wake up a specified AP and assign procedure to it.
+
+  @param[in] CpuData       - CPU data block for the specified AP.
+  @param[in] Procedure     - Procedure to assign.
+  @param[in] Parameter     - Argument for Procedure.
+**/
+VOID
+WakeUpAp (
+  IN CPU_DATA_BLOCK   *CpuData,
+  IN EFI_AP_PROCEDURE Procedure,
+  IN VOID             *Parameter
+  );
+
+/**
+  Check number of cores in the package.
+
+  @retval Number of cores in the package.
+**/
+UINT8
+GetCoreNumber (
+  VOID
+  );
+
+/**
+  Re-load microcode patch before boot.
+
+  @retval EFI_SUCCESS           - Multiple processors re-load microcode patch
+**/
+EFI_STATUS
+ReLoadMicrocodeBeforeBoot (
+  VOID
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/MtrrSync.c b/ReferenceCode/Haswell/CpuInit/Dxe/MtrrSync.c
new file mode 100644
index 0000000..a696e6b
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/MtrrSync.c
@@ -0,0 +1,338 @@
+/** @file
+  Code which support multi-processor
+
+@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 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
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "MpService.h"
+#endif
+
+extern MP_SYSTEM_DATA *mMPSystemData;
+
+EFI_MTRR_VALUES mFixedMtrrValues[] = {
+  {
+    IA32_MTRR_FIX64K_00000,
+    0
+  },
+  {
+    IA32_MTRR_FIX16K_80000,
+    0
+  },
+  {
+    IA32_MTRR_FIX16K_A0000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_C0000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_C8000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_D0000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_D8000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_E0000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_E8000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_F0000,
+    0
+  },
+  {
+    IA32_MTRR_FIX4K_F8000,
+    0
+  }
+};
+
+EFI_MTRR_VALUES mMtrrDefType[] = { { CACHE_IA32_MTRR_DEF_TYPE, 0 } };
+
+///
+/// Pre-defined Variable MTRR number to 20
+///
+EFI_MTRR_VALUES mVariableMtrrValues[] = {
+  {
+    CACHE_VARIABLE_MTRR_BASE,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 1,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 2,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 3,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 4,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 5,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 6,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 7,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 8,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 9,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 10,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 11,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 12,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 13,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 14,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 15,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 16,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 17,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 18,
+    0
+  },
+  {
+    CACHE_VARIABLE_MTRR_BASE + 19,
+    0
+  }
+};
+
+/**
+  Save the MTRR registers to global variables
+**/
+VOID
+ReadMtrrRegisters (
+  VOID
+  )
+{
+
+  UINT32 Index;
+  UINT32 VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  ///
+  /// Only support MAXIMUM_VARIABLE_MTRR_NUMBER variable MTRR
+  ///
+  ASSERT (VariableMtrrLimit <= V_MAXIMUM_VARIABLE_MTRR_NUMBER);
+  if (VariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+  ///
+  /// Read Fixed Mtrrs
+  ///
+  for (Index = 0; Index < sizeof (mFixedMtrrValues) / sizeof (EFI_MTRR_VALUES); Index++) {
+    mFixedMtrrValues[Index].Value = AsmReadMsr64 (mFixedMtrrValues[Index].Index);
+  }
+  ///
+  /// Read def type Fixed Mtrrs
+  ///
+  mMtrrDefType[0].Value = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+
+  ///
+  /// Read Variable Mtrr
+  ///
+  for (Index = 0; Index < VariableMtrrLimit * 2; Index++) {
+    mVariableMtrrValues[Index].Value = AsmReadMsr64 (mVariableMtrrValues[Index].Index);
+  }
+
+  return;
+}
+
+/**
+  Synch up the MTRR values for all processors
+
+  @param[in] Buffer - Not used.
+**/
+VOID
+EFIAPI
+MpMtrrSynchUp (
+  IN VOID *Buffer
+  )
+{
+  UINT32             Index;
+  UINTN              Cr4;
+  UINT64             MsrValue;
+  UINT64             ValidMtrrAddressMask;
+  EFI_CPUID_REGISTER FeatureInfo;
+  EFI_CPUID_REGISTER FunctionInfo;
+  UINT8              PhysicalAddressBits;
+  UINT32             VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  ///
+  /// Only support MAXIMUM_VARIABLE_MTRR_NUMBER variable MTRR
+  ///
+  ASSERT (VariableMtrrLimit <= V_MAXIMUM_VARIABLE_MTRR_NUMBER);
+  if (VariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+  ///
+  /// ASM code to setup processor register before synching up the MTRRs
+  ///
+  Cr4 = MpMtrrSynchUpEntry ();
+
+  ///
+  /// 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 = ((((UINT64) 1) << PhysicalAddressBits) - 1) & 0xfffffffffffff000;
+
+  ///
+  /// Disable Fixed Mtrrs
+  ///
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, mMtrrDefType[0].Value & 0xFFFFF7FF);
+
+  ///
+  /// Update Fixed Mtrrs
+  ///
+  for (Index = 0; Index < sizeof (mFixedMtrrValues) / sizeof (EFI_MTRR_VALUES); Index++) {
+    AsmWriteMsr64 (mFixedMtrrValues[Index].Index, mFixedMtrrValues[Index].Value);
+  }
+  ///
+  /// Synchup def type Fixed Mtrrs
+  ///
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, mMtrrDefType[0].Value);
+
+  ///
+  /// Synchup Base Variable Mtrr
+  ///
+  for (Index = 0; Index < VariableMtrrLimit * 2 - 1; Index += 2) {
+    MsrValue = (mVariableMtrrValues[Index].Value & 0x0FFF) | (mVariableMtrrValues[Index].Value & ValidMtrrAddressMask);
+    AsmWriteMsr64 (mVariableMtrrValues[Index].Index, MsrValue);
+  }
+  ///
+  /// Synchup Mask Variable Mtrr including valid bit
+  ///
+  for (Index = 1; Index < VariableMtrrLimit * 2; Index += 2) {
+    MsrValue = (mVariableMtrrValues[Index].Value & 0x0FFF) | (mVariableMtrrValues[Index].Value & ValidMtrrAddressMask);
+    AsmWriteMsr64 (mVariableMtrrValues[Index].Index, MsrValue);
+  }
+  ///
+  /// ASM code to setup processor register after synching up the MTRRs
+  ///
+  MpMtrrSynchUpExit (Cr4);
+
+  return;
+}
+
+/**
+  Copy Global MTRR data to S3
+**/
+VOID
+SaveBspMtrrForS3 (
+  VOID
+  )
+{
+  UINTN  Index;
+  UINTN  TableIndex;
+  UINT32 VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  ///
+  /// Only support MAXIMUM_VARIABLE_MTRR_NUMBER variable MTRR
+  ///
+  ASSERT (VariableMtrrLimit <= V_MAXIMUM_VARIABLE_MTRR_NUMBER);
+  if (VariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+
+  TableIndex = 0;
+  for (Index = 0; Index < sizeof (mFixedMtrrValues) / sizeof (EFI_MTRR_VALUES); Index++) {
+    mMPSystemData->S3BspMtrrTable[TableIndex].Index = mFixedMtrrValues[Index].Index;
+    mMPSystemData->S3BspMtrrTable[TableIndex].Value = mFixedMtrrValues[Index].Value;
+    TableIndex++;
+  }
+
+  for (Index = 0; Index < VariableMtrrLimit * 2; Index++) {
+    mMPSystemData->S3BspMtrrTable[TableIndex].Index = mVariableMtrrValues[Index].Index;
+    mMPSystemData->S3BspMtrrTable[TableIndex].Value = mVariableMtrrValues[Index].Value;
+    TableIndex++;
+  }
+
+  mMPSystemData->S3BspMtrrTable[TableIndex].Index = CACHE_IA32_MTRR_DEF_TYPE;
+  mMPSystemData->S3BspMtrrTable[TableIndex].Value = mMtrrDefType[0].Value;
+
+  ASSERT (TableIndex < MAX_CPU_S3_MTRR_ENTRY);
+
+  return;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/PiMpService.c b/ReferenceCode/Haswell/CpuInit/Dxe/PiMpService.c
new file mode 100644
index 0000000..a285f65
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/PiMpService.c
@@ -0,0 +1,957 @@
+/** @file
+  Implementation of PI MP Services Protocol
+
+@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 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
+
+@par Revision Reference:
+  - PI Version 1.2
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "PiMpService.h"
+#endif
+
+EFI_PI_MP_SERVICES_PROTOCOL mPiMpService = {
+  GetNumberOfProcessors,
+  GetProcessorInfo,
+  PiStartupAllAPs,
+  PiStartupThisAP,
+  PiSwitchBSP,
+  PiEnableDisableAP,
+  PiWhoAmI
+};
+
+BOOLEAN mStopCheckAPsStatus = FALSE;
+
+/**
+  Abort any task on the AP and reset the AP to be in idle state.
+
+  @param[in] ProcessorNumber - Processor index of an AP.
+**/
+VOID
+ResetProcessorToIdleState (
+  UINTN ProcessorNumber
+  )
+{
+  CPU_DATA_BLOCK *CpuData;
+
+  CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+  SendIPI (
+          &mMpService,
+          ProcessorNumber,
+          0,
+          DELIVERY_MODE_INIT
+          );
+
+  AsmAcquireMPLock (&CpuData->StateLock);
+  CpuData->State = CPU_STATE_IDLE;
+  AsmReleaseMPLock (&CpuData->StateLock);
+}
+
+/**
+  Searches for the next waiting AP.
+
+  Search for the next AP that is put in waiting state by single-threaded StartupAllAPs().
+
+  @param[in] NextProcessorNumber  - Pointer to the processor number of the next waiting AP.
+
+  @retval EFI_SUCCESS          - The next waiting AP has been found.
+  @retval EFI_NOT_FOUND        - No waiting AP exists.
+**/
+EFI_STATUS
+GetNextWaitingProcessorNumber (
+  OUT UINTN *NextProcessorNumber
+  )
+{
+  UINTN ProcessorNumber;
+
+  for (ProcessorNumber = 0; ProcessorNumber < mMPSystemData->NumberOfCpus; ProcessorNumber++) {
+
+    if (mMPSystemData->CpuList[ProcessorNumber]) {
+      *NextProcessorNumber = ProcessorNumber;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Checks status of all APs.
+
+  This function checks whether all APs have finished task assigned by StartupAllAPs(),
+  and whether timeout expires.
+
+  @retval EFI_SUCCESS    - All APs have finished task assigned by StartupAllAPs().
+  @retval EFI_TIMEOUT    - The timeout expires.
+  @retval EFI_NOT_READY  - APs have not finished task and timeout has not expired.
+**/
+EFI_STATUS
+CheckAllAPs (
+  VOID
+  )
+{
+  UINTN          ProcessorNumber;
+  UINTN          NextProcessorNumber;
+  UINTN          ListIndex;
+  EFI_STATUS     Status;
+  CPU_STATE      CpuState;
+  CPU_DATA_BLOCK *CpuData;
+
+  NextProcessorNumber = 0;
+
+  ///
+  /// Go through all APs that are responsible for the StartupAllAPs().
+  ///
+  for (ProcessorNumber = 0; ProcessorNumber < mMPSystemData->NumberOfCpus; ProcessorNumber++) {
+    if (!mMPSystemData->CpuList[ProcessorNumber]) {
+      continue;
+    }
+
+    CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+    ///
+    ///  Check the CPU state of AP. If it is CPU_STATE_FINISHED, then the AP has finished its task.
+    ///  Only BSP and corresponding AP access this unit of CPU Data. This means the AP will not modify the
+    ///  value of state after setting the it to CPU_STATE_FINISHED, so BSP can safely make use of its value.
+    ///
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuState = CpuData->State;
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    if (CpuState == CPU_STATE_FINISHED) {
+      mMPSystemData->FinishCount++;
+      mMPSystemData->CpuList[ProcessorNumber] = FALSE;
+
+      AsmAcquireMPLock (&CpuData->StateLock);
+      CpuData->State = CPU_STATE_IDLE;
+      AsmReleaseMPLock (&CpuData->StateLock);
+
+      ///
+      /// If in Single Thread mode, then search for the next waiting AP for execution.
+      ///
+      if (mMPSystemData->SingleThread) {
+        Status = GetNextWaitingProcessorNumber (&NextProcessorNumber);
+
+        if (!EFI_ERROR (Status)) {
+          WakeUpAp (
+                  &mMPSystemData->CpuData[NextProcessorNumber],
+                  mMPSystemData->Procedure,
+                  mMPSystemData->ProcArguments
+                  );
+        }
+      }
+    }
+  }
+  ///
+  /// If all APs finish, return EFI_SUCCESS.
+  ///
+  if (mMPSystemData->FinishCount == mMPSystemData->StartCount) {
+    return EFI_SUCCESS;
+  }
+  ///
+  /// If timeout expires, report timeout.
+  ///
+  if (CheckTimeout (mMPSystemData->ExpectedTime)) {
+    ///
+    /// If FailedCpuList is not NULL, record all failed APs in it.
+    ///
+    if (mMPSystemData->FailedCpuList != NULL) {
+      Status = (gBS->AllocatePool)
+              (
+                      EfiBootServicesData,
+                      ((mMPSystemData->StartCount - mMPSystemData->FinishCount + 1) * sizeof (UINTN)),
+                      (VOID **) mMPSystemData->FailedCpuList
+              );
+      ASSERT_EFI_ERROR (Status);
+    }
+
+    ListIndex = 0;
+
+    for (ProcessorNumber = 0; ProcessorNumber < mMPSystemData->NumberOfCpus; ProcessorNumber++) {
+      ///
+      /// Check whether this processor is responsible for StartupAllAPs().
+      ///
+      if (mMPSystemData->CpuList[ProcessorNumber]) {
+        ///
+        /// Reset failed APs to idle state
+        ///
+        ResetProcessorToIdleState (ProcessorNumber);
+        mMPSystemData->CpuList[ProcessorNumber] = FALSE;
+        if (mMPSystemData->FailedCpuList != NULL) {
+          (*mMPSystemData->FailedCpuList)[ListIndex++] = ProcessorNumber;
+        }
+      }
+    }
+
+    if (mMPSystemData->FailedCpuList != NULL) {
+      (*mMPSystemData->FailedCpuList)[ListIndex] = END_OF_CPU_LIST;
+    }
+
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_NOT_READY;
+}
+
+/**
+  Checks status of specified AP.
+
+  This function checks whether specified AP has finished task assigned by StartupThisAP(),
+  and whether timeout expires.
+
+  @param[in] ProcessorNumber - The handle number of processor.
+
+  @retval EFI_SUCCESS     - Specified AP has finished task assigned by StartupThisAPs().
+  @retval EFI_TIMEOUT     - The timeout expires.
+  @retval EFI_NOT_READY   - Specified AP has not finished task and timeout has not expired.
+**/
+EFI_STATUS
+CheckThisAP (
+  UINTN ProcessorNumber
+  )
+{
+  CPU_DATA_BLOCK *CpuData;
+  CPU_STATE      CpuState;
+
+  CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+  ///
+  ///  Check the CPU state of AP. If it is CPU_STATE_FINISHED, then the AP has finished its task.
+  ///  Only BSP and corresponding AP access this unit of CPU Data. This means the AP will not modify the
+  ///  value of state after setting the it to CPU_STATE_FINISHED, so BSP can safely make use of its value.
+  ///
+  AsmAcquireMPLock (&CpuData->StateLock);
+  CpuState = CpuData->State;
+  AsmReleaseMPLock (&CpuData->StateLock);
+
+  ///
+  /// If the APs finishes for StartupThisAP(), return EFI_SUCCESS.
+  ///
+  if (CpuState == CPU_STATE_FINISHED) {
+
+    AsmAcquireMPLock (&CpuData->StateLock);
+    CpuData->State = CPU_STATE_IDLE;
+    AsmReleaseMPLock (&CpuData->StateLock);
+
+    if (CpuData->Finished != NULL) {
+      *(CpuData->Finished) = TRUE;
+    }
+
+    return EFI_SUCCESS;
+  } else {
+    ///
+    /// If timeout expires for StartupThisAP(), report timeout.
+    ///
+    if (CheckTimeout (CpuData->ExpectedTime)) {
+
+      if (CpuData->Finished != NULL) {
+        *(CpuData->Finished) = FALSE;
+      }
+      ///
+      /// Reset failed AP to idle state
+      ///
+      ResetProcessorToIdleState (ProcessorNumber);
+
+      return EFI_TIMEOUT;
+    }
+  }
+
+  return EFI_NOT_READY;
+}
+
+/**
+  Checks APs' status periodically.
+
+  This function is triggerred by timer perodically to check the
+  state of APs for StartupAllAPs() and StartupThisAP() executed
+  in non-blocking mode.
+
+  @param[in] Event   - Event triggered.
+  @param[in] Context - Parameter passed with the event.
+**/
+VOID
+EFIAPI
+CheckAPsStatus (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  UINTN          ProcessorNumber;
+  CPU_DATA_BLOCK *CpuData;
+  EFI_STATUS     Status;
+
+  ///
+  /// If CheckAPsStatus() is stopped, then return immediately.
+  ///
+  if (mStopCheckAPsStatus) {
+    return;
+  }
+  ///
+  /// First, check whether pending StartupAllAPs() exists.
+  ///
+  if (mMPSystemData->WaitEvent != NULL) {
+
+    Status = CheckAllAPs ();
+    ///
+    /// If all APs finish for StartupAllAPs(), signal the WaitEvent for it..
+    ///
+    if (Status != EFI_NOT_READY) {
+      Status                    = gBS->SignalEvent (mMPSystemData->WaitEvent);
+      mMPSystemData->WaitEvent  = NULL;
+    }
+  }
+  ///
+  /// Second, check whether pending StartupThisAPs() callings exist.
+  ///
+  for (ProcessorNumber = 0; ProcessorNumber < mMPSystemData->NumberOfCpus; ProcessorNumber++) {
+
+    CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+    if (CpuData->WaitEvent == NULL) {
+      continue;
+    }
+
+    Status = CheckThisAP (ProcessorNumber);
+
+    if (Status != EFI_NOT_READY) {
+      gBS->SignalEvent (CpuData->WaitEvent);
+      CpuData->WaitEvent = NULL;
+    }
+  }
+
+  return;
+}
+
+/**
+  Implementation of GetNumberOfProcessors() service of MP Services Protocol.
+
+  This service retrieves the number of logical processor in the platform
+  and the number of those logical processors that are enabled on this boot.
+  This service may only be called from the BSP.
+
+  @param[in] This                      - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] NumberOfProcessors        - Pointer to the total number of logical processors in the system,
+                              including the BSP and disabled APs.
+  @param[in] NumberOfEnabledProcessors - Pointer to the number of enabled logical processors that exist
+                              in system, including the BSP.
+
+  @retval EFI_SUCCESS            - Number of logical processors and enabled logical processors retrieved..
+  @retval EFI_DEVICE_ERROR       - Caller processor is AP.
+  @retval EFI_INVALID_PARAMETER  - NumberOfProcessors is NULL.
+  @retval EFI_INVALID_PARAMETER  - NumberOfEnabledProcessors is NULL.
+**/
+EFI_STATUS
+EFIAPI
+GetNumberOfProcessors (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                      *NumberOfProcessors,
+  OUT UINTN                      *NumberOfEnabledProcessors
+  )
+{
+  EFI_STATUS Status;
+  UINTN      CallerNumber;
+
+  ///
+  /// Check whether caller processor is BSP
+  ///
+  WhoAmI (&mMpService, &CallerNumber);
+  if (CallerNumber != mMPSystemData->BSP) {
+    return EFI_DEVICE_ERROR;
+  }
+  ///
+  /// Check parameter NumberOfProcessors and NumberOfEnabledProcessors
+  ///
+  if (NumberOfProcessors == NULL || NumberOfEnabledProcessors == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = GetGeneralMPInfo (
+                  &mMpService,
+                  NumberOfProcessors,
+                  NULL,
+                  NumberOfEnabledProcessors,
+                  NULL,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Implementation of GetNumberOfProcessors() service of MP Services Protocol.
+
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made. This service may only be called from the BSP.
+
+  @param[in] This                - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber     -  The handle number of processor.
+  @param[in] ProcessorInfoBuffer - A pointer to the buffer where information for the requested processor is deposited.
+
+  @retval EFI_SUCCESS           - Processor information successfully returned.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_INVALID_PARAMETER - ProcessorInfoBuffer is NULL
+  @retval EFI_NOT_FOUND         - Processor with the handle specified by ProcessorNumber does not exist.
+**/
+EFI_STATUS
+EFIAPI
+GetProcessorInfo (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                       ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
+  )
+{
+  EFI_STATUS          Status;
+  UINTN               CallerNumber;
+  UINTN               BufferSize;
+  EFI_MP_PROC_CONTEXT ProcessorContextBuffer;
+
+  ///
+  /// Check whether caller processor is BSP
+  ///
+  WhoAmI (&mMpService, &CallerNumber);
+  if (CallerNumber != mMPSystemData->BSP) {
+    return EFI_DEVICE_ERROR;
+  }
+  ///
+  /// Check parameter ProcessorInfoBuffer
+  ///
+  if (ProcessorInfoBuffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  ///
+  /// Check whether processor with the handle specified by ProcessorNumber exists
+  ///
+  if (ProcessorNumber >= mMPSystemData->NumberOfCpus) {
+    return EFI_NOT_FOUND;
+  }
+
+  BufferSize = sizeof (EFI_MP_PROC_CONTEXT);
+  Status = GetProcessorContext (
+                  &mMpService,
+                  ProcessorNumber,
+                  &BufferSize,
+                  &ProcessorContextBuffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ProcessorInfoBuffer->ProcessorId = (UINT64) ProcessorContextBuffer.ApicID;
+
+  ///
+  /// Get Status Flag of specified processor
+  ///
+  ProcessorInfoBuffer->StatusFlag = 0;
+
+  if (ProcessorContextBuffer.Enabled) {
+    ProcessorInfoBuffer->StatusFlag |= PROCESSOR_ENABLED_BIT;
+  }
+
+  if (ProcessorContextBuffer.Designation == EfiCpuBSP) {
+    ProcessorInfoBuffer->StatusFlag |= PROCESSOR_AS_BSP_BIT;
+  }
+
+  if (ProcessorContextBuffer.Health.Flags.Uint32 == 0) {
+    ProcessorInfoBuffer->StatusFlag |= PROCESSOR_HEALTH_STATUS_BIT;
+  }
+
+  ProcessorInfoBuffer->Location.Package = (UINT32) ProcessorContextBuffer.PackageNumber;
+  ProcessorInfoBuffer->Location.Core    = (UINT32) ProcessorContextBuffer.NumberOfCores;
+  ProcessorInfoBuffer->Location.Thread  = (UINT32) ProcessorContextBuffer.NumberOfThreads;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Implementation of StartupAllAPs() service of MP Services Protocol.
+
+  This service lets the caller get all enabled APs to execute a caller-provided function.
+  This service may only be called from the BSP.
+
+  @param[in] This                  - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] Procedure             - A pointer to the function to be run on enabled APs of the system.
+  @param[in] SingleThread          - Indicates whether to execute the function simultaneously or one by one..
+  @param[in] WaitEvent             - The event created by the caller.
+                                     If it is NULL, then execute in blocking mode.
+                                     If it is not NULL, then execute in non-blocking mode.
+  @param[in] TimeoutInMicroSeconds - The time limit in microseconds for this AP to finish the function.
+                                     Zero means infinity.
+  @param[in] ProcedureArgument     - Pointer to the optional parameter of the assigned function.
+  @param[in] FailedCpuList         - The list of processor numbers that fail to finish the function before
+                                     TimeoutInMicrosecsond expires.
+
+  @retval EFI_SUCCESS           - In blocking mode, all APs have finished before the timeout expired.
+  @retval EFI_SUCCESS           - In non-blocking mode, function has been dispatched to all enabled APs.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_NOT_STARTED       - No enabled AP exists in the system.
+  @retval EFI_NOT_READY         - Any enabled AP is busy.
+  @retval EFI_TIMEOUT           - In blocking mode, The timeout expired before all enabled APs have finished.
+  @retval EFI_INVALID_PARAMETER - Procedure is NULL.
+**/
+EFI_STATUS
+EFIAPI
+PiStartupAllAPs (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE            Procedure,
+  IN BOOLEAN                     SingleThread,
+  IN EFI_EVENT WaitEvent         OPTIONAL,
+  IN UINTN                       TimeoutInMicroSeconds,
+  IN VOID *ProcedureArgument     OPTIONAL,
+  OUT UINTN **FailedCpuList      OPTIONAL
+  )
+{
+  EFI_STATUS     Status;
+  UINTN          ProcessorNumber;
+  CPU_DATA_BLOCK *CpuData;
+  BOOLEAN        Blocking;
+
+  if (FailedCpuList != NULL) {
+    *FailedCpuList = NULL;
+  }
+  ///
+  /// Check whether caller processor is BSP
+  ///
+  WhoAmI (&mMpService, &ProcessorNumber);
+  if (ProcessorNumber != mMPSystemData->BSP) {
+    return EFI_DEVICE_ERROR;
+  }
+  ///
+  /// Check parameter Procedure
+  ///
+  if (Procedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  ///
+  /// Temporarily suppress CheckAPsStatus()
+  ///
+  mStopCheckAPsStatus = TRUE;
+
+  ///
+  /// Check whether all enabled APs are idle.
+  /// If any enabled AP is not idle, return EFI_NOT_READY.
+  ///
+  for (ProcessorNumber = 0; ProcessorNumber < mMPSystemData->NumberOfCpus; ProcessorNumber++) {
+
+    CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+    mMPSystemData->CpuList[ProcessorNumber] = FALSE;
+    ///
+    /// Skip BSP and disabled APs.
+    ///
+    if (ProcessorNumber == mMPSystemData->BSP || CpuData->State == CPU_STATE_DISABLED) {
+      continue;
+    }
+    ///
+    /// If any enabled APs are busy, return EFI_NOT_FOUND.
+    ///
+    if (CpuData->State != CPU_STATE_IDLE) {
+      mStopCheckAPsStatus = FALSE;
+      return EFI_NOT_READY;
+    } else {
+      ///
+      /// Mark this processor as responsible for current calling.
+      ///
+      mMPSystemData->CpuList[ProcessorNumber] = TRUE;
+    }
+  }
+  ///
+  /// All enabled APs are idle, we can safely initiate a new session
+  ///
+  mMPSystemData->FinishCount  = 0;
+  mMPSystemData->StartCount   = 0;
+  Blocking                    = FALSE;
+  ///
+  /// Go through all enabled APs to wakeup them for Procedure.
+  /// If in Single Thread mode, then only one AP is woken up, and others are waiting.
+  ///
+  for (ProcessorNumber = 0; ProcessorNumber < mMPSystemData->NumberOfCpus; ProcessorNumber++) {
+
+    CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+    ///
+    /// Check whether this processor is responsible for current calling.
+    ///
+    if (mMPSystemData->CpuList[ProcessorNumber]) {
+
+      mMPSystemData->StartCount++;
+
+      AsmAcquireMPLock (&CpuData->StateLock);
+      CpuData->State = CPU_STATE_READY;
+      AsmReleaseMPLock (&CpuData->StateLock);
+
+      if (!Blocking) {
+        WakeUpAp (
+                CpuData,
+                Procedure,
+                ProcedureArgument
+                );
+      }
+
+      if (SingleThread) {
+        Blocking = TRUE;
+      }
+    }
+  }
+  ///
+  /// If no enabled AP exists, return EFI_NOT_STARTED.
+  ///
+  if (mMPSystemData->StartCount == 0) {
+    mStopCheckAPsStatus = FALSE;
+    return EFI_NOT_STARTED;
+  }
+  ///
+  /// If WaitEvent is not NULL, execute in non-blocking mode.
+  /// BSP saves data for CheckAPsStatus(), and returns EFI_SUCCESS.
+  /// CheckAPsStatus() will check completion and timeout periodically.
+  ///
+  mMPSystemData->Procedure      = Procedure;
+  mMPSystemData->ProcArguments  = ProcedureArgument;
+  mMPSystemData->SingleThread   = SingleThread;
+  mMPSystemData->FailedCpuList  = FailedCpuList;
+  mMPSystemData->ExpectedTime   = CalculateTimeout (TimeoutInMicroSeconds);
+  mMPSystemData->WaitEvent      = WaitEvent;
+
+  ///
+  /// Allow CheckAPsStatus()
+  ///
+  mStopCheckAPsStatus = FALSE;
+
+  if (WaitEvent != NULL) {
+    return EFI_SUCCESS;
+  }
+  ///
+  /// If WaitEvent is NULL, execute in blocking mode.
+  /// BSP checks APs'state until all APs finish or TimeoutInMicrosecsond expires.
+  ///
+  do {
+    Status = CheckAllAPs ();
+  } while (Status == EFI_NOT_READY);
+
+  return Status;
+}
+
+/**
+  Implementation of StartupThisAP() service of MP Services Protocol.
+
+  This service lets the caller get one enabled AP to execute a caller-provided function.
+  This service may only be called from the BSP.
+
+  @param[in] This                  - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] Procedure             - A pointer to the function to be run on the designated AP.
+  @param[in] ProcessorNumber       - The handle number of AP..
+  @param[in] WaitEvent             - The event created by the caller.
+                                     If it is NULL, then execute in blocking mode.
+                                     If it is not NULL, then execute in non-blocking mode.
+  @param[in] TimeoutInMicroseconds - The time limit in microseconds for this AP to finish the function.
+                                     Zero means infinity.
+  @param[in] ProcedureArgument     - Pointer to the optional parameter of the assigned function.
+  @param[in] Finished              - Indicates whether AP has finished assigned function.
+                                     In blocking mode, it is ignored.
+
+  @retval EFI_SUCCESS           - In blocking mode, specified AP has finished before the timeout expires.
+  @retval EFI_SUCCESS           - In non-blocking mode, function has been dispatched to specified AP.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_TIMEOUT           - In blocking mode, the timeout expires before specified AP has finished.
+  @retval EFI_NOT_READY         - Specified AP is busy.
+  @retval EFI_NOT_FOUND         - Processor with the handle specified by ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER - ProcessorNumber specifies the BSP or disabled AP.
+  @retval EFI_INVALID_PARAMETER - Procedure is NULL.
+**/
+EFI_STATUS
+EFIAPI
+PiStartupThisAP (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE            Procedure,
+  IN UINTN                       ProcessorNumber,
+  IN EFI_EVENT WaitEvent         OPTIONAL,
+  IN UINTN                       TimeoutInMicroseconds,
+  IN VOID *ProcedureArgument     OPTIONAL,
+  OUT BOOLEAN *Finished          OPTIONAL
+  )
+{
+  CPU_DATA_BLOCK *CpuData;
+  UINTN          CallerNumber;
+  EFI_STATUS     Status;
+
+  if (Finished != NULL) {
+    *Finished = TRUE;
+  }
+  ///
+  /// Check whether caller processor is BSP
+  ///
+  WhoAmI (&mMpService, &CallerNumber);
+  if (CallerNumber != mMPSystemData->BSP) {
+    return EFI_DEVICE_ERROR;
+  }
+  ///
+  /// Check whether processor with the handle specified by ProcessorNumber exists
+  ///
+  if (ProcessorNumber >= mMPSystemData->NumberOfCpus) {
+    return EFI_NOT_FOUND;
+  }
+  ///
+  /// Check whether specified processor is BSP
+  ///
+  if (ProcessorNumber == mMPSystemData->BSP) {
+    return EFI_INVALID_PARAMETER;
+  }
+  ///
+  /// Check parameter Procedure
+  ///
+  if (Procedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+  ///
+  /// Temporarily suppress CheckAPsStatus()
+  ///
+  mStopCheckAPsStatus = TRUE;
+
+  ///
+  /// Check whether specified AP is disabled
+  ///
+  if (CpuData->State == CPU_STATE_DISABLED) {
+    mStopCheckAPsStatus = FALSE;
+    return EFI_INVALID_PARAMETER;
+  }
+  ///
+  /// Check whether specified AP is busy
+  ///
+  if (CpuData->State != CPU_STATE_IDLE) {
+    mStopCheckAPsStatus = FALSE;
+    return EFI_NOT_READY;
+  }
+  ///
+  /// Wakeup specified AP for Procedure.
+  ///
+  AsmAcquireMPLock (&CpuData->StateLock);
+  CpuData->State = CPU_STATE_READY;
+  AsmReleaseMPLock (&CpuData->StateLock);
+
+  WakeUpAp (
+          CpuData,
+          Procedure,
+          ProcedureArgument
+          );
+
+  ///
+  /// If WaitEvent is not NULL, execute in non-blocking mode.
+  /// BSP saves data for CheckAPsStatus(), and returns EFI_SUCCESS.
+  /// CheckAPsStatus() will check completion and timeout periodically.
+  ///
+  CpuData->WaitEvent    = WaitEvent;
+  CpuData->Finished     = Finished;
+  CpuData->ExpectedTime = CalculateTimeout (TimeoutInMicroseconds);
+
+  ///
+  /// Allow CheckAPsStatus()
+  ///
+  mStopCheckAPsStatus = FALSE;
+
+  if (WaitEvent != NULL) {
+    return EFI_SUCCESS;
+  }
+  ///
+  /// If WaitEvent is NULL, execute in blocking mode.
+  /// BSP checks AP's state until it finishes or TimeoutInMicrosecsond expires.
+  ///
+  do {
+    Status = CheckThisAP (ProcessorNumber);
+  } while (Status == EFI_NOT_READY);
+
+  return Status;
+}
+
+/**
+  Implementation of SwitchBSP() service of MP Services Protocol.
+
+  This service switches the requested AP to be the BSP from that point onward.
+  This service may only be called from the current BSP.
+
+  @param[in] This                 - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber      - The handle number of processor.
+  @param[in] EnableOldBSP         - Whether to enable or disable the original BSP.
+
+  @retval EFI_SUCCESS           - BSP successfully switched.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_NOT_FOUND         - Processor with the handle specified by ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER - ProcessorNumber specifies the BSP or disabled AP.
+  @retval EFI_NOT_READY         - Specified AP is busy.
+**/
+EFI_STATUS
+EFIAPI
+PiSwitchBSP (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                       ProcessorNumber,
+  IN BOOLEAN                     EnableOldBSP
+  )
+{
+  EFI_STATUS     Status;
+  CPU_DATA_BLOCK *CpuData;
+  UINTN          CallerNumber;
+
+  ///
+  /// Check whether caller processor is BSP
+  ///
+  WhoAmI (&mMpService, &CallerNumber);
+  if (CallerNumber != mMPSystemData->BSP) {
+    return EFI_DEVICE_ERROR;
+  }
+  ///
+  /// Check whether processor with the handle specified by ProcessorNumber exists
+  ///
+  if (ProcessorNumber >= mMPSystemData->NumberOfCpus) {
+    return EFI_NOT_FOUND;
+  }
+  ///
+  /// Check whether specified processor is BSP
+  ///
+  if (ProcessorNumber == mMPSystemData->BSP) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+  ///
+  /// Check whether specified AP is disabled
+  ///
+  if (CpuData->State == CPU_STATE_DISABLED) {
+    return EFI_INVALID_PARAMETER;
+  }
+  ///
+  /// Check whether specified AP is busy
+  ///
+  if (CpuData->State != CPU_STATE_IDLE) {
+    return EFI_NOT_READY;
+  }
+
+  Status = SwitchBSP (
+                  &mMpService,
+                  ProcessorNumber,
+                  EnableOldBSP
+                  );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
+
+/**
+  Implementation of EnableDisableAP() service of MP Services Protocol.
+
+  This service lets the caller enable or disable an AP.
+  This service may only be called from the BSP.
+
+  @param[in] This                  - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber       - The handle number of processor.
+  @param[in] EnableAP              - Indicates whether the newstate of the AP is enabled or disabled.
+  @param[in] HealthFlag            - Indicates new health state of the AP..
+
+  @retval EFI_SUCCESS            - AP successfully enabled or disabled.
+  @retval EFI_DEVICE_ERROR       - Caller processor is AP.
+  @retval EFI_NOT_FOUND          - Processor with the handle specified by ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETERS - ProcessorNumber specifies the BSP.
+**/
+EFI_STATUS
+EFIAPI
+PiEnableDisableAP (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                       ProcessorNumber,
+  IN BOOLEAN                     EnableAP,
+  IN UINT32 *HealthFlag          OPTIONAL
+  )
+{
+  EFI_STATUS    Status;
+  UINTN         CallerNumber;
+  EFI_MP_HEALTH HealthState;
+  EFI_MP_HEALTH *HealthStatePointer;
+
+  ///
+  /// Check whether caller processor is BSP
+  ///
+  WhoAmI (&mMpService, &CallerNumber);
+  if (CallerNumber != mMPSystemData->BSP) {
+    return EFI_DEVICE_ERROR;
+  }
+  ///
+  /// Check whether processor with the handle specified by ProcessorNumber exists
+  ///
+  if (ProcessorNumber >= mMPSystemData->NumberOfCpus) {
+    return EFI_NOT_FOUND;
+  }
+  ///
+  /// Check whether specified processor is BSP
+  ///
+  if (ProcessorNumber == mMPSystemData->BSP) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (HealthFlag == NULL) {
+    HealthStatePointer = NULL;
+  } else {
+    if ((*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT) == 0) {
+      HealthState.Flags.Uint32 = 1;
+    } else {
+      HealthState.Flags.Uint32 = 0;
+    }
+
+    HealthState.TestStatus  = 0;
+
+    HealthStatePointer      = &HealthState;
+  }
+
+  Status = EnableDisableAP (
+                  &mMpService,
+                  ProcessorNumber,
+                  EnableAP,
+                  HealthStatePointer
+                  );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
+
+/**
+  Implementation of WhoAmI() service of MP Services Protocol.
+
+  This service lets the caller processor get its handle number.
+  This service may be called from the BSP and APs.
+
+  @param[in] This                 - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber      - Pointer to the handle number of AP.
+
+  @retval EFI_SUCCESS           - Processor number successfully returned.
+  @retval EFI_INVALID_PARAMETER - ProcessorNumber is NULL
+**/
+EFI_STATUS
+EFIAPI
+PiWhoAmI (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                      *ProcessorNumber
+  )
+{
+  EFI_STATUS Status;
+
+  if (ProcessorNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = WhoAmI (&mMpService, ProcessorNumber);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/PiMpService.h b/ReferenceCode/Haswell/CpuInit/Dxe/PiMpService.h
new file mode 100644
index 0000000..0e2bccf
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/PiMpService.h
@@ -0,0 +1,242 @@
+/** @file
+  some definitions for PI MP services Protocol.
+
+@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 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 _PI_MP_SERVICE_H_
+#define _PI_MP_SERVICE_H_
+
+#include "MpService.h"
+
+///
+/// Driver Produced Protocol.
+///
+#include EFI_PROTOCOL_PRODUCER (PiMpService)
+
+/**
+  Implementation of GetNumberOfProcessors() service of MP Services Protocol.
+
+  This service retrieves the number of logical processor in the platform
+  and the number of those logical processors that are enabled on this boot.
+  This service may only be called from the BSP.
+
+  @param[in] This                      - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] NumberOfProcessors        - Pointer to the total number of logical processors in the system,
+                              including the BSP and disabled APs.
+  @param[in] NumberOfEnabledProcessors - Pointer to the number of enabled logical processors that exist
+                              in system, including the BSP.
+
+  @retval EFI_SUCCESS            - Number of logical processors and enabled logical processors retrieved..
+  @retval EFI_DEVICE_ERROR       - Caller processor is AP.
+  @retval EFI_INVALID_PARAMETER  - NumberOfProcessors is NULL.
+  @retval EFI_INVALID_PARAMETER  - NumberOfEnabledProcessors is NULL.
+**/
+EFI_STATUS
+EFIAPI
+GetNumberOfProcessors (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                      *NumberOfProcessors,
+  OUT UINTN                      *NumberOfEnabledProcessors
+  );
+
+/**
+  Implementation of GetNumberOfProcessors() service of MP Services Protocol.
+
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made. This service may only be called from the BSP.
+
+  @param[in] This                - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber     -  The handle number of processor.
+  @param[in] ProcessorInfoBuffer - A pointer to the buffer where information for the requested processor is deposited.
+
+  @retval EFI_SUCCESS           - Processor information successfully returned.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_INVALID_PARAMETER - ProcessorInfoBuffer is NULL
+  @retval EFI_NOT_FOUND         - Processor with the handle specified by ProcessorNumber does not exist.
+**/
+EFI_STATUS
+EFIAPI
+GetProcessorInfo (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                       ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
+  );
+
+/**
+  Implementation of StartupThisAP() service of MP Services Protocol.
+
+  This service lets the caller get one enabled AP to execute a caller-provided function.
+  This service may only be called from the BSP.
+
+  @param[in] This                  - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] Procedure             - A pointer to the function to be run on the designated AP.
+  @param[in] ProcessorNumber       - The handle number of AP..
+  @param[in] WaitEvent             - The event created by the caller.
+                                     If it is NULL, then execute in blocking mode.
+                                     If it is not NULL, then execute in non-blocking mode.
+  @param[in] TimeoutInMicroseconds - The time limit in microseconds for this AP to finish the function.
+                                     Zero means infinity.
+  @param[in] ProcedureArgument     - Pointer to the optional parameter of the assigned function.
+  @param[in] Finished              - Indicates whether AP has finished assigned function.
+                                     In blocking mode, it is ignored.
+
+  @retval EFI_SUCCESS           - In blocking mode, specified AP has finished before the timeout expires.
+  @retval EFI_SUCCESS           - In non-blocking mode, function has been dispatched to specified AP.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_TIMEOUT           - In blocking mode, the timeout expires before specified AP has finished.
+  @retval EFI_NOT_READY         - Specified AP is busy.
+  @retval EFI_NOT_FOUND         - Processor with the handle specified by ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER - ProcessorNumber specifies the BSP or disabled AP.
+  @retval EFI_INVALID_PARAMETER - Procedure is NULL.
+**/
+EFI_STATUS
+EFIAPI
+PiStartupThisAP (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE            Procedure,
+  IN UINTN                       ProcessorNumber,
+  IN EFI_EVENT WaitEvent         OPTIONAL,
+  IN UINTN                       TimeoutInMicroseconds,
+  IN VOID *ProcedureArgument     OPTIONAL,
+  OUT BOOLEAN *Finished          OPTIONAL
+  );
+
+/**
+  Implementation of StartupAllAPs() service of MP Services Protocol.
+
+  This service lets the caller get all enabled APs to execute a caller-provided function.
+  This service may only be called from the BSP.
+
+  @param[in] This                  - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] Procedure             - A pointer to the function to be run on enabled APs of the system.
+  @param[in] SingleThread          - Indicates whether to execute the function simultaneously or one by one..
+  @param[in] WaitEvent             - The event created by the caller.
+                                     If it is NULL, then execute in blocking mode.
+                                     If it is not NULL, then execute in non-blocking mode.
+  @param[in] TimeoutInMicroSeconds - The time limit in microseconds for this AP to finish the function.
+                                     Zero means infinity.
+  @param[in] ProcedureArgument     - Pointer to the optional parameter of the assigned function.
+  @param[in] FailedCpuList         - The list of processor numbers that fail to finish the function before
+                                     TimeoutInMicrosecsond expires.
+
+  @retval EFI_SUCCESS           - In blocking mode, all APs have finished before the timeout expired.
+  @retval EFI_SUCCESS           - In non-blocking mode, function has been dispatched to all enabled APs.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_NOT_STARTED       - No enabled AP exists in the system.
+  @retval EFI_NOT_READY         - Any enabled AP is busy.
+  @retval EFI_TIMEOUT           - In blocking mode, The timeout expired before all enabled APs have finished.
+  @retval EFI_INVALID_PARAMETER - Procedure is NULL.
+**/
+EFI_STATUS
+EFIAPI
+PiStartupAllAPs (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN EFI_AP_PROCEDURE            Procedure,
+  IN BOOLEAN                     SingleThread,
+  IN EFI_EVENT WaitEvent         OPTIONAL,
+  IN UINTN                       TimeoutInMicroSeconds,
+  IN VOID *ProcedureArgument     OPTIONAL,
+  OUT UINTN **FailedCpuList      OPTIONAL
+  );
+
+/**
+  Implementation of SwitchBSP() service of MP Services Protocol.
+
+  This service switches the requested AP to be the BSP from that point onward.
+  This service may only be called from the current BSP.
+
+  @param[in] This                 - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber      - The handle number of processor.
+  @param[in] EnableOldBSP         - Whether to enable or disable the original BSP.
+
+  @retval EFI_SUCCESS           - BSP successfully switched.
+  @retval EFI_DEVICE_ERROR      - Caller processor is AP.
+  @retval EFI_NOT_FOUND         - Processor with the handle specified by ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER - ProcessorNumber specifies the BSP or disabled AP.
+  @retval EFI_NOT_READY         - Specified AP is busy.
+**/
+EFI_STATUS
+EFIAPI
+PiSwitchBSP (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                       ProcessorNumber,
+  IN BOOLEAN                     EnableOldBSP
+  );
+
+/**
+  Implementation of EnableDisableAP() service of MP Services Protocol.
+
+  This service lets the caller enable or disable an AP.
+  This service may only be called from the BSP.
+
+  @param[in] This                  - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber       - The handle number of processor.
+  @param[in] EnableAP              - Indicates whether the newstate of the AP is enabled or disabled.
+  @param[in] HealthFlag            - Indicates new health state of the AP..
+
+  @retval EFI_SUCCESS            - AP successfully enabled or disabled.
+  @retval EFI_DEVICE_ERROR       - Caller processor is AP.
+  @retval EFI_NOT_FOUND          - Processor with the handle specified by ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETERS - ProcessorNumber specifies the BSP.
+**/
+EFI_STATUS
+EFIAPI
+PiEnableDisableAP (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  IN UINTN                       ProcessorNumber,
+  IN BOOLEAN                     EnableAP,
+  IN UINT32 *HealthFlag          OPTIONAL
+  );
+
+/**
+  Implementation of WhoAmI() service of MP Services Protocol.
+
+  This service lets the caller processor get its handle number.
+  This service may be called from the BSP and APs.
+
+  @param[in] This                 - A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber      - Pointer to the handle number of AP.
+
+  @retval EFI_SUCCESS           - Processor number successfully returned.
+  @retval EFI_INVALID_PARAMETER - ProcessorNumber is NULL
+**/
+EFI_STATUS
+EFIAPI
+PiWhoAmI (
+  IN EFI_PI_MP_SERVICES_PROTOCOL *This,
+  OUT UINTN                      *ProcessorNumber
+  );
+
+/**
+  Checks APs' status periodically.
+
+  This function is triggerred by timer perodically to check the
+  state of APs for StartupAllAPs() and StartupThisAP() executed
+  in non-blocking mode.
+
+  @param[in] Event   - Event triggered.
+  @param[in] Context - Parameter passed with the event.
+**/
+VOID
+EFIAPI
+CheckAPsStatus (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/ProcessorData.c b/ReferenceCode/Haswell/CpuInit/Dxe/ProcessorData.c
new file mode 100644
index 0000000..f3a47a5
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/ProcessorData.c
@@ -0,0 +1,795 @@
+/** @file
+  Produces CPU data records.
+
+Revision History
+
+@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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "ProcessorData.h"
+#include "MpCommon.h"
+#include "MpService.h"
+
+///
+/// This is the VFR compiler generated header file which defines the
+/// string identifiers.
+///
+#include "CpuInitDxeStrDefs.h"
+
+#endif
+
+extern MP_SYSTEM_DATA                   *mMPSystemData;
+extern DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu;
+
+#if (EFI_SPECIFICATION_VERSION < 0x2000A)
+extern EFI_HII_PROTOCOL *mHii;
+#endif
+extern EFI_DATA_HUB_PROTOCOL            *mDataHub;
+extern DXE_CPU_PLATFORM_POLICY_PROTOCOL *mPlatformCpu;
+extern EFI_HII_HANDLE                   mStringHandle;
+
+EFI_SUBCLASS_TYPE1_HEADER mCpuDataRecordHeader = {
+  EFI_PROCESSOR_SUBCLASS_VERSION,       ///< Version
+  sizeof (EFI_SUBCLASS_TYPE1_HEADER),   ///< Header Size
+  0,                                    ///< Instance, Initialize later
+  EFI_SUBCLASS_INSTANCE_NON_APPLICABLE, ///< SubInstance
+  0                                     ///< RecordType, Initialize later
+};
+
+EFI_STATUS
+LogCpuData (
+  EFI_DATA_HUB_PROTOCOL *DataHub,
+  UINT8                 *Buffer,
+  UINT32                Size
+  );
+
+typedef struct _PROCESSOR_FAMILY_FIELD {
+  CHAR8  ProcessorFamily[48];
+  UINT16 ProcessorEnum;
+} PROCESSOR_FAMILY_FIELD;
+
+PROCESSOR_FAMILY_FIELD ProcessorFamilyField[] = {
+  {
+    "Intel(R) Core(TM) i7",
+    0xC6
+  },
+  {
+    "Intel(R) Core(TM) i5",
+    0xCD
+  },
+  {
+    "Intel(R) Core(TM) i3",
+    0xCE
+  },
+  {
+    "Intel(R) Xeon(R) CPU",
+    0xB3
+  },
+  {
+    "Intel(R) Pentium(R) CPU",
+    0x0B
+  },
+  {
+    "Intel(R) Celeron(R) CPU",
+    0x0F
+  },
+};
+
+/**
+  Converts an ascii string to unicode string 16 chars at a time.
+
+  @param[in] AsciiString   - Pointer to input Ascii string.
+  @param[in] UnicodeString - Pointer to output Unicode string buffer.
+**/
+VOID
+AsciiToUnicode (
+  IN CHAR8      *AsciiString,
+  IN OUT CHAR16 *UnicodeString
+  )
+{
+  UINT8 Index;
+
+  for (Index = 0; Index < 16; Index++) {
+    UnicodeString[Index] = (CHAR16) AsciiString[Index];
+  }
+
+  return;
+}
+///
+/// Processor-specific routines
+///
+/**
+  Returns the procesor version string token installed in the system.
+
+  @retval Processor Version string token
+**/
+VOID
+GetProcessorVersion (
+  OUT PROCESSOR_VERSION_INFORMATION *Version
+  )
+{
+  CHAR16             BrandIdString[MAXIMUM_CPU_BRAND_STRING_LENGTH + 1];
+  EFI_CPUID_REGISTER CpuExtendedSupport;
+  EFI_CPUID_REGISTER CpuBrandString;
+  UINT8              Index;
+
+  ///
+  /// Create the string using Brand ID String.
+  ///
+  Version->StringValid = FALSE;
+
+  if (IsIntelProcessor ()) {
+    Version->StringRef = STRING_TOKEN (STR_INTEL_GENUINE_PROCESSOR);
+
+    AsmCpuid (
+            CPUID_EXTENDED_FUNCTION,
+            &CpuExtendedSupport.RegEax,
+            &CpuExtendedSupport.RegEbx,
+            &CpuExtendedSupport.RegEcx,
+            &CpuExtendedSupport.RegEdx
+            );
+    AsmCpuid (
+            CPUID_BRAND_STRING1,
+            &CpuBrandString.RegEax,
+            &CpuBrandString.RegEbx,
+            &CpuBrandString.RegEcx,
+            &CpuBrandString.RegEdx
+            );
+    ///
+    /// Check if Brand ID String is supported or filled up
+    ///
+    if (CpuExtendedSupport.RegEax != 0 && CpuBrandString.RegEax != 0) {
+      AsciiToUnicode ((CHAR8 *) &CpuBrandString, (CHAR16 *) &BrandIdString[0]);
+      AsmCpuid (
+              CPUID_BRAND_STRING2,
+              &CpuBrandString.RegEax,
+              &CpuBrandString.RegEbx,
+              &CpuBrandString.RegEcx,
+              &CpuBrandString.RegEdx
+              );
+      AsciiToUnicode ((CHAR8 *) &CpuBrandString, (CHAR16 *) &BrandIdString[16]);
+      AsmCpuid (
+              CPUID_BRAND_STRING3,
+              &CpuBrandString.RegEax,
+              &CpuBrandString.RegEbx,
+              &CpuBrandString.RegEcx,
+              &CpuBrandString.RegEdx
+              );
+      AsciiToUnicode ((CHAR8 *) &CpuBrandString, (CHAR16 *) &BrandIdString[32]);
+
+      ///
+      /// Remove preceeding spaces
+      ///
+      Index = 0;
+      while (BrandIdString[Index] == 0x20) {
+        Index++;
+        if (Index >= MAXIMUM_CPU_BRAND_STRING_LENGTH) {
+          break;
+        }
+      }
+
+      CopyMem (
+              Version->BrandString,
+              &BrandIdString[Index],
+              (MAXIMUM_CPU_BRAND_STRING_LENGTH - Index) * sizeof (CHAR16)
+              );
+      Version->BrandString[MAXIMUM_CPU_BRAND_STRING_LENGTH - Index] = 0;
+      Version->StringValid = TRUE;
+    }
+  } else {
+    Version->StringRef = STRING_TOKEN (STR_UNKNOWN);
+  }
+}
+
+/**
+  Returns the procesor manufaturer string token installed in the system.
+
+  @retval Processor Manufacturer string token
+**/
+EFI_PROCESSOR_MANUFACTURER_DATA
+GetProcessorManufacturer (
+  VOID
+  )
+{
+
+  if (IsIntelProcessor ()) {
+    return STRING_TOKEN (STR_INTEL_CORPORATION);
+  } else {
+    return STRING_TOKEN (STR_UNKNOWN);
+  }
+}
+
+/**
+  Returns if processor is Intel or not.
+
+  @retval TRUE - Intel Processor.
+  @retval FALSE - Not Intel Processor.
+**/
+BOOLEAN
+IsIntelProcessor (
+  VOID
+  )
+{
+  EFI_CPUID_REGISTER Reg;
+
+  AsmCpuid (CPUID_SIGNATURE, &Reg.RegEax, &Reg.RegEbx, &Reg.RegEcx, &Reg.RegEdx);
+
+  if ((Reg.RegEbx != 'uneG') || (Reg.RegEdx != 'Ieni') || (Reg.RegEcx != 'letn')) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+}
+
+/**
+  Returns the processor family of the processor installed in the system.
+
+  @retval Processor Family
+**/
+EFI_PROCESSOR_FAMILY_DATA
+GetProcessorFamily (
+  VOID
+  )
+{
+
+  UINTN              j;
+  UINTN              Index;
+  EFI_CPUID_REGISTER CpuExtendedSupport;
+  EFI_CPUID_REGISTER CpuBrandString;
+  CHAR8              BrandString[MAXIMUM_CPU_BRAND_STRING_LENGTH + 1];
+  UINT16             ProcessorFamily;
+  ProcessorFamily = EfiProcessorFamilyUnknown;
+  Index           = 0;
+
+  if (IsIntelProcessor ()) {
+    ///
+    /// Get Brand string
+    ///
+    AsmCpuid (
+            CPUID_EXTENDED_FUNCTION,
+            &CpuExtendedSupport.RegEax,
+            &CpuExtendedSupport.RegEbx,
+            &CpuExtendedSupport.RegEcx,
+            &CpuExtendedSupport.RegEdx
+            );
+    AsmCpuid (
+            CPUID_BRAND_STRING1,
+            &CpuBrandString.RegEax,
+            &CpuBrandString.RegEbx,
+            &CpuBrandString.RegEcx,
+            &CpuBrandString.RegEdx
+            );
+
+    if (CpuExtendedSupport.RegEax != 0 && CpuBrandString.RegEax != 0) {
+      EfiAsciiStrCpy (&BrandString[0], (CHAR8 *) &CpuBrandString);
+      AsmCpuid (
+              CPUID_BRAND_STRING2,
+              &CpuBrandString.RegEax,
+              &CpuBrandString.RegEbx,
+              &CpuBrandString.RegEcx,
+              &CpuBrandString.RegEdx
+              );
+      EfiAsciiStrCpy (&BrandString[16], (CHAR8 *) &CpuBrandString);
+      AsmCpuid (
+              CPUID_BRAND_STRING3,
+              &CpuBrandString.RegEax,
+              &CpuBrandString.RegEbx,
+              &CpuBrandString.RegEcx,
+              &CpuBrandString.RegEdx
+              );
+      EfiAsciiStrCpy (&BrandString[32], (CHAR8 *) &CpuBrandString);
+
+      ///
+      /// Remove preceeding spaces
+      ///
+      while (BrandString[Index] == 0x20) {
+        Index++;
+      }
+
+      BrandString[MAXIMUM_CPU_BRAND_STRING_LENGTH - Index] = 0;
+    }
+
+    ProcessorFamily = 0xC6;
+
+    for (j = 0; j < sizeof (ProcessorFamilyField) / sizeof (PROCESSOR_FAMILY_FIELD); j++) {
+      if (EfiAsciiStrnCmp (
+                  &BrandString[Index],
+                  ProcessorFamilyField[j].ProcessorFamily,
+                  (EfiAsciiStrLen (ProcessorFamilyField[j].ProcessorFamily))
+                  ) == 0) {
+        ProcessorFamily = ProcessorFamilyField[j].ProcessorEnum;
+        break;
+      }
+    }
+  } else {
+    ProcessorFamily = EfiProcessorFamilyOther;
+  }
+
+  return ProcessorFamily;
+}
+
+/**
+  Returns the processor voltage of the processor installed in the system.
+
+  @retval Processor Voltage
+**/
+INT16
+GetProcessorVoltage (
+  VOID
+  )
+{
+  INT16  VoltageInmV;
+  UINT64 MsrValue;
+
+  VoltageInmV = 0;
+  MsrValue = 0;
+
+  ///
+  /// Core voltage = (float) IA32_PERF_STS(47:32) * (float) 1/(2^13)
+  ///
+  MsrValue = AsmReadMsr64 (MSR_IA32_PERF_STS);
+  MsrValue = RShiftU64(MsrValue, 32);
+  MsrValue &= 0x0FFFF;
+
+  ///
+  /// Convert unit to mV
+  ///
+  MsrValue = MultU64x32(MsrValue, 1000);
+  MsrValue = RShiftU64 (MsrValue, 13);
+  VoltageInmV = (UINT16) MsrValue;
+
+  return VoltageInmV;
+}
+
+/**
+  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);
+}
+
+/**
+  Get processor status by specific CPU number
+
+  @param[in] CpuNumber - indicate which CPU status are requested
+
+  @retval EFI_PROCESSOR_STATUS_DATA  for specific CPU number
+**/
+EFI_PROCESSOR_STATUS_DATA
+GetProcessorStatus (
+  IN UINTN CpuNumber
+  )
+{
+  EFI_PROCESSOR_STATUS_DATA ProcessorStatus;
+  CPU_DATA_BLOCK            *CpuData;
+
+  CpuData                   = &mMPSystemData->CpuData[CpuNumber];
+
+  ProcessorStatus.Reserved1 = 0;
+  ProcessorStatus.Reserved2 = 0;
+  ProcessorStatus.Reserved3 = 0;
+
+  ProcessorStatus.CpuStatus = EfiCpuStatusEnabled;
+  if (CpuData->State == CPU_STATE_DISABLED) {
+    switch (mMPSystemData->DisableCause[CpuNumber]) {
+      case CPU_CAUSE_USER_SELECTION:
+      case CPU_CAUSE_BY_ASSOCIATION:
+        ProcessorStatus.CpuStatus = EfiCpuStatusDisabledByUser;
+        break;
+
+      case CPU_CAUSE_INTERNAL_ERROR:
+      case CPU_CAUSE_THERMAL_ERROR:
+      case CPU_CAUSE_SELFTEST_FAILURE:
+      case CPU_CAUSE_PREBOOT_TIMEOUT:
+      case CPU_CAUSE_CONFIG_ERROR:
+        ProcessorStatus.CpuStatus = EfiCpuStatusDisabledbyBios;
+        break;
+
+      case CPU_CAUSE_FAILED_TO_START:
+      case CPU_CAUSE_UNSPECIFIED:
+      default:
+        ProcessorStatus.CpuStatus = EfiCpuStatusOther;
+        break;
+    }
+  }
+
+  ProcessorStatus.SocketPopulated = TRUE;
+  ProcessorStatus.ApicEnable      = 1;
+
+  if (mMPSystemData->BSP == CpuNumber) {
+    ProcessorStatus.BootApplicationProcessor = 1;
+  } else {
+    ProcessorStatus.BootApplicationProcessor = 0;
+  }
+
+  return ProcessorStatus;
+}
+
+/**
+  This function gets called with the processor number and will log all data to data hub
+  pertaining to this processor.
+
+  @param[in] CpuNumber         - Processor Number
+  @param[in] CpuDataForDatahub - Contains CPU data which is collected for data hub
+
+  @retval EFI_OUT_OF_RESOURCES - failed to allocate pool for CPU data
+  @retval EFI_SUCCESS          - CPU data Hub has been created successfully
+**/
+EFI_STATUS
+InitializeProcessorData (
+  IN UINTN                CpuNumber,
+  IN CPU_DATA_FOR_DATAHUB *CpuDataForDatahub
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_CPU_DATA_RECORD_BUFFER RecordBuffer;
+  UINT32                     HeaderSize;
+  UINT32                     TotalSize;
+  PLATFORM_CPU_INFORMATION   PlatformCpuInfo;
+  STRING_REF                 Token;
+  UINT64                     MsrValue;
+  UINT64                     ProcessorCoreCount;
+  UINT64                     ProcessorThreadCount;
+
+  mCpuDataRecordHeader.Instance = (UINT16) (CpuNumber + 1);
+
+  HeaderSize                    = sizeof (EFI_SUBCLASS_TYPE1_HEADER);
+  RecordBuffer.Raw              = AllocatePool (HeaderSize + EFI_CPU_DATA_MAXIMUM_LENGTH);
+  if (RecordBuffer.Raw == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ZeroMem (&PlatformCpuInfo, sizeof (PLATFORM_CPU_INFORMATION));
+  CopyMem (RecordBuffer.Raw, &mCpuDataRecordHeader, HeaderSize);
+
+  ///
+  /// Record following data by socket base
+  ///
+  if (CpuNumber == 0) {
+    ///
+    /// Record Type 1
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType = ProcessorCoreFrequencyRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorCoreFrequency.Value = (UINT16) CpuDataForDatahub->IntendCoreFrequency;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorCoreFrequency.Exponent = 6;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_CORE_FREQUENCY_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 2
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType                    = ProcessorFsbFrequencyRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorFsbFrequency.Value     = 100;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorFsbFrequency.Exponent  = 6;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_FSB_FREQUENCY_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 3
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType = ProcessorVersionRecordType;
+    if (CpuDataForDatahub->Version.StringValid) {
+      Token = 0;
+#if (EFI_SPECIFICATION_VERSION >= 0x0002000A)
+      Status = IfrLibNewString (mStringHandle, &Token, CpuDataForDatahub->Version.BrandString);
+#else
+      Status = mHii->NewString (mHii, NULL, mStringHandle, &Token, CpuDataForDatahub->Version.BrandString);
+#endif
+      if (EFI_ERROR (Status)) {
+        Token = CpuDataForDatahub->Version.StringRef;
+      }
+    } else {
+      Token = CpuDataForDatahub->Version.StringRef;
+    }
+
+    RecordBuffer.DataRecord->VariableRecord.ProcessorVersion  = Token;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_VERSION_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 4
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType          = ProcessorManufacturerRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorManufacturer = CpuDataForDatahub->Manufacturer;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_MANUFACTURER_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 6.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType = ProcessorIdRecordType;
+    CopyMem (
+            &RecordBuffer.DataRecord->VariableRecord.ProcessorId,
+            &CpuDataForDatahub->CpuidData,
+            sizeof (CpuDataForDatahub->CpuidData)
+            );
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_ID_DATA);
+    Status    = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 7.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType  = ProcessorTypeRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorType = EfiCentralProcessor;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_TYPE_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 8.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType    = ProcessorFamilyRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorFamily = CpuDataForDatahub->Family;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_FAMILY_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 9.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType              = ProcessorVoltageRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorVoltage.Value    = CpuDataForDatahub->Voltage;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorVoltage.Exponent = -3;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_VOLTAGE_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+  }
+  ///
+  /// log data by socket base
+  ///
+  /// Record Type 10.
+  ///
+  RecordBuffer.DataRecord->DataRecordHeader.RecordType      = ProcessorApicBaseAddressRecordType;
+  RecordBuffer.DataRecord->VariableRecord.ProcessorApicBase = CpuDataForDatahub->ApicBase;
+  TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_APIC_BASE_ADDRESS_DATA);
+  Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+  ///
+  /// Record Type 11.
+  ///
+  RecordBuffer.DataRecord->DataRecordHeader.RecordType    = ProcessorApicIdRecordType;
+  RecordBuffer.DataRecord->VariableRecord.ProcessorApicId = CpuDataForDatahub->ApicID;
+  TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_APIC_ID_DATA);
+  Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+  ///
+  /// Record Type 12.
+  ///
+  RecordBuffer.DataRecord->DataRecordHeader.RecordType                = ProcessorApicVersionNumberRecordType;
+  RecordBuffer.DataRecord->VariableRecord.ProcessorApicVersionNumber  = CpuDataForDatahub->ApicVersion;
+  TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_APIC_VERSION_NUMBER_DATA);
+  Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+  ///
+  /// Record Type 13.
+  ///
+  RecordBuffer.DataRecord->DataRecordHeader.RecordType  = CpuUcodeRevisionDataRecordType;
+  RecordBuffer.DataRecord->VariableRecord.CpuUcodeRevisionData.ProcessorMicrocodeType = EfiProcessorIa32Microcode;
+  RecordBuffer.DataRecord->VariableRecord.CpuUcodeRevisionData.ProcessorMicrocodeRevisionNumber = CpuDataForDatahub->MicrocodeRevision;
+  TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_MICROCODE_REVISION_DATA);
+  Status    = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+  ///
+  /// Record following data by socket base
+  ///
+  if (CpuNumber == 0) {
+    ///
+    /// Record Type 14.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType    = ProcessorStatusRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorStatus = CpuDataForDatahub->Status;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_STATUS_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 17(0x11). Set in Cache File
+    ///
+    ///
+    /// Record Type 21(0x15). zero based
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType            = ProcessorPackageNumberRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorPackageNumber  = CpuDataForDatahub->Location.Package;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_PACKAGE_NUMBER_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 24(0x18).
+    /// Health definition in DataHub spec is not the same as BIST format, so add 1 to convert
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType          = ProcessorHealthStatusRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorHealthStatus = CpuDataForDatahub->Health.Uint32 + 1;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_HEALTH_STATUS);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+  }
+  ///
+  /// log data by socket base
+  ///
+  /// The following record data comes from platform driver:
+  ///
+  PlatformCpuInfo.StringHandle  = mStringHandle;
+  PlatformCpuInfo.ApicID        = CpuDataForDatahub->ApicID;
+  Status = mPlatformCpu->CpuConfig->GetCpuInfo (
+                  mPlatformCpu,
+                  &CpuDataForDatahub->Location,
+                  &PlatformCpuInfo
+                  );
+
+  ///
+  /// Record following data by socket base
+  ///
+  if (CpuNumber == 0) {
+    ///
+    /// Record Type 5. (Processor Serial Number: this feature is only available on PIII, not support here)
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType      = ProcessorSerialNumberRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorAssetTag = PlatformCpuInfo.SerialNumber;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_SERIAL_NUMBER_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 15.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType        = ProcessorSocketTypeRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorSocketType = PlatformCpuInfo.SocketType;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_SOCKET_TYPE_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 16(0x10).
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType        = ProcessorSocketNameRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorSocketName = PlatformCpuInfo.SocketName;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_SOCKET_NAME_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 18(0x12).
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType              = ProcessorMaxCoreFrequencyRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorMaxCoreFrequency = PlatformCpuInfo.MaxCoreFrequency;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_MAX_CORE_FREQUENCY_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 19(0x13).
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType      = ProcessorAssetTagRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorAssetTag = PlatformCpuInfo.AssetTag;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_ASSET_TAG_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+  }
+  ///
+  /// log data by socket base
+  ///
+  /// Record Type 20(0x14).
+  ///
+  RecordBuffer.DataRecord->DataRecordHeader.RecordType              = ProcessorMaxFsbFrequencyRecordType;
+  RecordBuffer.DataRecord->VariableRecord.ProcessorMaxFsbFrequency  = PlatformCpuInfo.MaxFsbFrequency;
+  TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_MAX_FSB_FREQUENCY_DATA);
+  Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+  ///
+  /// Record Type 22(0x16).
+  ///
+  RecordBuffer.DataRecord->DataRecordHeader.RecordType                = ProcessorCoreFrequencyListRecordType;
+  RecordBuffer.DataRecord->VariableRecord.ProcessorCoreFrequencyList  = PlatformCpuInfo.PlatformCoreFrequencyList;
+  TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_CORE_FREQUENCY_LIST_DATA);
+  Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+  ///
+  /// Record Type 23(0x17).
+  ///
+  RecordBuffer.DataRecord->DataRecordHeader.RecordType              = ProcessorFsbFrequencyListRecordType;
+  RecordBuffer.DataRecord->VariableRecord.ProcessorFsbFrequencyList = PlatformCpuInfo.PlatformFsbFrequencyList;
+  TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_FSB_FREQUENCY_LIST_DATA);
+  Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+  ///
+  /// Record following data by socket base
+  ///
+  if (CpuNumber == 0) {
+    ///
+    /// Record Type 30. (Processor Part Number)
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType      = ProcessorPartNumberRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorAssetTag = PlatformCpuInfo.PartNumber;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_PART_NUMBER_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 28.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType                          = ProcessorCharacteristicsRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorCharacteristics.Reserved2    = 0;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorCharacteristics.Reserved     = 0;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorCharacteristics.Unknown      = 0;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorCharacteristics.Capable64Bit = 1;
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_CHARACTERISTICS_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    MsrValue = AsmReadMsr64 (MSR_CORE_THREAD_COUNT);
+    ProcessorThreadCount = MsrValue & 0xffff;
+    ProcessorCoreCount = (MsrValue >> 16) & 0xffff;
+
+    ///
+    /// Record Type 25.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType        = ProcessorCoreCountRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorCoreCount  = (UINT8) (ProcessorCoreCount);
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_CORE_COUNT_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 26.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType              = ProcessorEnabledCoreCountRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorEnabledCoreCount = (UINT8) (ProcessorCoreCount);
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_ENABLED_CORE_COUNT_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+
+    ///
+    /// Record Type 27.
+    ///
+    RecordBuffer.DataRecord->DataRecordHeader.RecordType          = ProcessorThreadCountRecordType;
+    RecordBuffer.DataRecord->VariableRecord.ProcessorThreadCount  = (UINT8) (ProcessorThreadCount);
+    TotalSize = HeaderSize + sizeof (EFI_PROCESSOR_THREAD_COUNT_DATA);
+    Status = LogCpuData (mDataHub, RecordBuffer.Raw, TotalSize);
+  }
+  ///
+  /// log data by socket base
+  ///
+  FreePool (RecordBuffer.Raw);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Log CPU data into data hub
+
+  @param[in] DataHub - point to data hub that will be updated
+  @param[in] Buffer  - the buffer which will be updated into data hub
+  @param[in] Size    - size of the buffer
+
+  @retval EFI_STATUS - status returned when updating Data hub
+**/
+EFI_STATUS
+LogCpuData (
+  EFI_DATA_HUB_PROTOCOL *DataHub,
+  UINT8                 *Buffer,
+  UINT32                Size
+  )
+{
+  EFI_STATUS Status;
+
+  Status = DataHub->LogData (
+                  DataHub,
+                  &gEfiProcessorSubClassGuid,
+                  &gEfiProcessorProducerGuid,
+                  EFI_DATA_RECORD_CLASS_DATA,
+                  Buffer,
+                  Size
+                  );
+  return Status;
+
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/ProcessorData.h b/ReferenceCode/Haswell/CpuInit/Dxe/ProcessorData.h
new file mode 100644
index 0000000..fea963e
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/ProcessorData.h
@@ -0,0 +1,136 @@
+/** @file
+  Header file for CPU Data File
+
+Revision History
+
+@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 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 _PROCESSOR_DATA_H_
+#define _PROCESSOR_DATA_H_
+
+#include "CpuInitDxe.h"
+
+#define EFI_PROCESSOR_TYPE_SIZE 6
+
+typedef struct {
+  UINT8      Index;
+  UINT8      ProcessorFamily;
+  STRING_REF ProcessorVersionToken;
+} EFI_PROCESSOR_VERSION;
+
+typedef struct {
+  UINT32 Dword1;
+  UINT32 Dword2;
+  UINT32 Dword3;
+  UINT32 Dword4;
+} EFI_QDWORD;
+
+///
+/// Platform-specific definitions
+///
+#define EFI_CPU_DATA_MAXIMUM_LENGTH 0x100
+
+/**
+  This function gets called with the processor number and will log all data to data hub
+  pertaining to this processor.
+
+  @param[in] CpuNumber         - Processor Number
+  @param[in] CpuDataForDatahub - Contains CPU data which is collected for data hub
+
+  @retval EFI_OUT_OF_RESOURCES - failed to allocate pool for CPU data
+  @retval EFI_SUCCESS          - CPU data Hub has been created successfully
+**/
+EFI_STATUS
+InitializeProcessorData (
+  IN UINTN                CpuNumber,
+  IN CPU_DATA_FOR_DATAHUB *CpuDataForDatahub
+  );
+
+/**
+  Returns the processor voltage of the processor installed in the system.
+
+  @retval Processor Voltage
+**/
+INT16
+GetProcessorVoltage (
+  VOID
+  );
+
+/**
+  Returns the processor microcode revision of the processor installed in the system.
+
+  @retval Processor Microcode Revision
+**/
+UINT32
+GetCpuUcodeRevision (
+  VOID
+  );
+
+/**
+  Get processor status by specific CPU number
+
+  @param[in] CpuNumber - indicate which CPU status are requested
+
+  @retval EFI_PROCESSOR_STATUS_DATA  for specific CPU number
+**/
+EFI_PROCESSOR_STATUS_DATA
+GetProcessorStatus (
+  UINTN CpuNumber
+  );
+
+/**
+  Returns the procesor version string token installed in the system.
+
+  @retval Processor Version string token
+**/
+VOID
+GetProcessorVersion (
+  OUT PROCESSOR_VERSION_INFORMATION *Version
+  );
+
+/**
+  Returns the processor family of the processor installed in the system.
+
+  @retval Processor Family
+**/
+EFI_PROCESSOR_FAMILY_DATA
+GetProcessorFamily (
+  VOID
+  );
+
+/**
+  Returns the procesor manufaturer string token installed in the system.
+
+  @retval Processor Manufacturer string token
+**/
+EFI_PROCESSOR_MANUFACTURER_DATA
+GetProcessorManufacturer (
+  VOID
+  );
+
+/**
+  Returns if processor is Intel or not.
+
+  @retval TRUE - Intel Processor.
+  @retval FALSE - Not Intel Processor.
+**/
+BOOLEAN
+IsIntelProcessor (
+  VOID
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/Cpu.asm b/ReferenceCode/Haswell/CpuInit/Dxe/x64/Cpu.asm
new file mode 100644
index 0000000..9b28b54
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/Cpu.asm
@@ -0,0 +1,626 @@
+;
+; This file contains an 'Intel Peripheral Driver' 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                                           
+;
+      TITLE   Cpu.asm: Assembly code for the IA-32 resources
+
+;-------------------------------------------------------------------------------
+;
+; 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.
+;
+;
+; Module Name:
+;
+;   Cpu.asm
+; 
+; Abstract:
+; 
+;
+;-------------------------------------------------------------------------------
+
+text    SEGMENT
+
+EXTRN mErrorCodeFlag:DWORD ; Error code flags for exceptions
+
+ExternalVectorTablePtr QWORD 0 ; point to the external interrupt vector table
+
+;
+; Float control word initial value: 
+; all exceptions masked, double-extended-precision, round-to-nearest
+;
+mFpuControlWord       DW      037Fh
+;
+; Multimedia-extensions control word:
+; all exceptions masked, round-to-nearest, flush to zero for masked underflow
+;
+mMmxControlWord       DD      01F80h 
+
+
+InitializeExternalVectorTablePtr PROC PUBLIC
+  mov  ExternalVectorTablePtr, rcx
+  ret
+InitializeExternalVectorTablePtr ENDP
+;
+;
+;
+;------------------------------------------------------------------------------
+;  Generic IDT Vector Handlers for the Host. They are all the same so they
+;  will compress really well.
+;
+;  By knowing the return address for Vector 00 you can can calculate the
+;  vector number by looking at the call CommonInterruptEntry return address.
+;  (return address - (AsmIdtVector00 + 5))/8 == IDT index
+;
+;------------------------------------------------------------------------------
+
+ALIGN   8
+
+PUBLIC	AsmIdtVector00
+        
+AsmIdtVector00 LABEL BYTE
+REPEAT  256
+        call  CommonInterruptEntry
+        dw ( $ - AsmIdtVector00 - 5 ) / 8 ; vector number
+        nop
+ENDM
+
+
+;---------------------------------------;
+; CommonInterruptEntry                  ;
+;---------------------------------------;
+; The follow algorithm is used for the common interrupt routine.
+
+;
+; +---------------------+ <-- 16-byte aligned ensured by processor
+; +    Old SS           +
+; +---------------------+
+; +    Old RSP          +
+; +---------------------+
+; +    RFlags           +
+; +---------------------+
+; +    CS               +
+; +---------------------+
+; +    RIP              +
+; +---------------------+
+; +    Error Code       +
+; +---------------------+
+; + RCX / Vector Number +
+; +---------------------+
+; +    RBP              +
+; +---------------------+ <-- RBP, 16-byte aligned
+;
+
+CommonInterruptEntry PROC  PUBLIC  
+  cli
+  cld
+  ;
+  ; All interrupt handlers are invoked through interrupt gates, so
+  ; IF flag automatically cleared at the entry point
+  ;
+  ;
+  ; Calculate vector number
+  ;
+  xchg    rcx, [rsp] ; get the return address of call, actually, it is the address of vector number.
+  movzx   ecx, word ptr [rcx]        
+  cmp     ecx, 32         ; Intel reserved vector for exceptions?
+  jae     NoErrorCode
+  bt      mErrorCodeFlag, ecx
+  jc      @F
+
+NoErrorCode:
+  ;
+  ; Push a dummy error code on the stack
+  ; to maintain coherent stack map
+  ;
+  push    [rsp]
+  mov     qword ptr [rsp + 8], 0
+@@:       
+  push    rbp
+  mov     rbp, rsp
+
+  ;
+  ; Since here the stack pointer is 16-byte aligned, so
+  ; EFI_FX_SAVE_STATE_X64 of EFI_SYSTEM_CONTEXT_x64
+  ; is 16-byte aligned
+  ;       
+
+;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+  push r15
+  push r14
+  push r13
+  push r12
+  push r11
+  push r10
+  push r9
+  push r8
+  push rax
+  push qword ptr [rbp + 8]   ; RCX
+  push rdx
+  push rbx
+  push qword ptr [rbp + 48]  ; RSP
+  push qword ptr [rbp]       ; RBP
+  push rsi
+  push rdi
+
+;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
+  movzx   rax, word ptr [rbp + 56]
+  push    rax                      ; for ss
+  movzx   rax, word ptr [rbp + 32]
+  push    rax                      ; for cs
+  mov     rax, ds
+  push    rax
+  mov     rax, es
+  push    rax
+  mov     rax, fs
+  push    rax
+  mov     rax, gs
+  push    rax
+
+  mov     [rbp + 8], rcx               ; save vector number
+
+;; UINT64  Rip;
+  push    qword ptr [rbp + 24]
+
+;; UINT64  Gdtr[2], Idtr[2];
+  sub     rsp, 16
+  sidt    fword ptr [rsp]
+  sub     rsp, 16
+  sgdt    fword ptr [rsp]
+
+;; UINT64  Ldtr, Tr;
+  xor     rax, rax
+  str     ax
+  push    rax
+  sldt    ax
+  push    rax
+
+;; UINT64  RFlags;
+  push    qword ptr [rbp + 40]
+
+;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+  mov     rax, cr8
+  push    rax
+  mov     rax, cr4
+  or      rax, 208h
+  mov     cr4, rax
+  push    rax
+  mov     rax, cr3
+  push    rax
+  mov     rax, cr2
+  push    rax
+  xor     rax, rax
+  push    rax
+  mov     rax, cr0
+  push    rax
+
+;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+  mov     rax, dr7
+  push    rax
+;; clear Dr7 while executing debugger itself
+  xor     rax, rax
+  mov     dr7, rax
+
+  mov     rax, dr6
+  push    rax
+;; insure all status bits in dr6 are clear...
+  xor     rax, rax
+  mov     dr6, rax
+
+  mov     rax, dr3
+  push    rax
+  mov     rax, dr2
+  push    rax
+  mov     rax, dr1
+  push    rax
+  mov     rax, dr0
+  push    rax
+
+;; FX_SAVE_STATE_X64 FxSaveState;
+
+  sub rsp, 512
+  mov rdi, rsp
+  db 0fh, 0aeh, 00000111y ;fxsave [rdi]
+
+;; UINT32  ExceptionData;
+  push    qword ptr [rbp + 16]
+
+;; call into exception handler
+  mov     rcx, [rbp + 8]
+  mov     rax, ExternalVectorTablePtr  ; get the interrupt vectors base
+  mov     rax, [rax + rcx * 8]       
+  or      rax, rax                        ; NULL?
+
+  je    nonNullValue;
+
+;; Prepare parameter and call
+;  mov     rcx, [rbp + 8]
+  mov     rdx, rsp
+  ;
+  ; Per X64 calling convention, allocate maximum parameter stack space
+  ; and make sure RSP is 16-byte aligned
+  ;
+  sub     rsp, 4 * 8 + 8
+  call    rax
+  add     rsp, 4 * 8 + 8
+
+nonNullValue:
+  cli
+;; UINT64  ExceptionData;
+  add     rsp, 8
+
+;; FX_SAVE_STATE_X64 FxSaveState;
+
+  mov rsi, rsp
+  db 0fh, 0aeh, 00001110y ; fxrstor [rsi]
+  add rsp, 512
+
+;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+  pop     rax
+  mov     dr0, rax
+  pop     rax
+  mov     dr1, rax
+  pop     rax
+  mov     dr2, rax
+  pop     rax
+  mov     dr3, rax
+;; skip restore of dr6.  We cleared dr6 during the context save.
+  add     rsp, 8
+  pop     rax
+  mov     dr7, rax
+
+;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+  pop     rax
+  mov     cr0, rax
+  add     rsp, 8   ; not for Cr1
+  pop     rax
+  mov     cr2, rax
+  pop     rax
+  mov     cr3, rax
+  pop     rax
+  mov     cr4, rax
+  pop     rax
+  mov     cr8, rax
+
+;; UINT64  RFlags;
+  pop     qword ptr [rbp + 40]
+
+;; UINT64  Ldtr, Tr;
+;; UINT64  Gdtr[2], Idtr[2];
+;; Best not let anyone mess with these particular registers...
+  add     rsp, 48
+
+;; UINT64  Rip;
+  pop     qword ptr [rbp + 24]
+
+;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;
+  pop     rax
+  ; mov     gs, rax ; not for gs
+  pop     rax
+  ; mov     fs, rax ; not for fs
+  ; (X64 will not use fs and gs, so we do not restore it)
+  pop     rax
+  mov     es, rax
+  pop     rax
+  mov     ds, rax
+  pop     qword ptr [rbp + 32]  ; for cs
+  pop     qword ptr [rbp + 56]  ; for ss
+
+;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+  pop     rdi
+  pop     rsi
+  add     rsp, 8               ; not for rbp
+  pop     qword ptr [rbp + 48] ; for rsp
+  pop     rbx
+  pop     rdx
+  pop     rcx
+  pop     rax
+  pop     r8
+  pop     r9
+  pop     r10
+  pop     r11
+  pop     r12
+  pop     r13
+  pop     r14
+  pop     r15
+
+  mov     rsp, rbp
+  pop     rbp
+  add     rsp, 16
+  iretq
+
+CommonInterruptEntry ENDP
+
+
+LongMode PROC  PUBLIC
+
+in_long_mode::
+    ;
+    ; Debug Stop
+    ;
+    jmp   in_long_mode
+
+    ;
+    ; We're in long mode, so marshall the arguments to call the
+    ; passed in function pointers
+    ; Recall
+    ;         [ebp][10h] = HobStart
+    ;         [ebp][18h] = Stack
+    ;         [ebp][20h] = PpisNeededByDxeIplEntryPoint <--- Call this first (for each call, pass HOB pointer)
+    ;         [ebp][28h] = DxeCoreEntryPoint            <--- Call this second
+    ;
+    mov rbx, [rbp+18h]        ; Setup the stack
+    mov rsp, rbx              ; On a new stack now
+
+    mov rcx, [rbp+10h]        ; Pass Hob Start in RCX
+    mov rax, [rbp+20h]        ; Get the function pointer for 
+                              ; PpisNeededByDxeIplEntryPoint into EAX
+    call fword ptr [rax]      ; Make the call into PpisNeededByDxeIplEntryPoint
+
+    mov ecx, [rbp+10h]        ; Pass Hob Start in RCX
+    mov eax, [rbp+28h]        ; Get the function pointer for 
+                              ; DxeCoreEntryPoint into EAX
+    call fword ptr [rax]      ; Make the call into Dxe Core
+
+    call CommonInterruptEntry
+
+    mov rdi, CommonInterruptEntry 
+
+    lgdt  fword ptr [rdi]
+
+    lidt  fword ptr [rdi]
+
+    call near ptr [rax]      ; Make the call into PpisNeededByDxeIplEntryPoint
+
+    call rax
+
+    ;
+    ; Should never get here.
+    ;
+no_long_mode:
+    jmp   no_long_mode
+    ;
+    ; WE SHOULD NEVER GET HERE!!!!!!!!!!!!!
+    ;
+LongMode endp
+
+EnableMce  proc  public
+
+  mov     rax, cr4
+  or      rax, 40h
+  mov     cr4, rax
+
+  ret
+
+EnableMce  endp
+
+MpMtrrSynchUpEntry  PROC        PUBLIC
+    ;
+    ; Enter no fill cache mode, CD=1(Bit30), NW=0 (Bit29)
+    ;
+    mov rax, cr0
+    and rax, 0DFFFFFFFh
+    or  rax, 040000000h
+    mov cr0, rax
+    ;
+    ; Flush cache
+    ;
+    wbinvd
+    ;
+    ; Clear PGE flag Bit 7
+    ;
+    mov rax, cr4
+    mov rdx, rax
+    and rax, 0FFFFFF7Fh
+    mov cr4, rax
+    ;
+    ; Flush all TLBs
+    ;
+    mov rax, cr3
+    mov cr3, rax
+    
+    mov rax, rdx
+    
+    ret
+    
+MpMtrrSynchUpEntry  ENDP
+    
+MpMtrrSynchUpExit  PROC        PUBLIC
+    ;
+    ; Flush all TLBs the second time
+    ;
+    mov rax, cr3
+    mov cr3, rax
+    ;
+    ; Enable Normal Mode caching CD=NW=0, CD(Bit30), NW(Bit29)
+    ;
+    mov rax, cr0
+    and rax, 09FFFFFFFh
+    mov cr0, rax
+    ;
+    ; Set PGE Flag in CR4 if set
+    ;
+    mov cr4, rcx
+    ret
+
+MpMtrrSynchUpExit  ENDP
+
+;
+; Initializes floating point units for requirement of UEFI specification.
+;
+; This function initializes floating-point control word to 0x037F (all exceptions
+; masked,double-extended-precision, round-to-nearest) and multimedia-extensions control word
+; (if supported) to 0x1F80 (all exceptions masked, round-to-nearest, flush to zero
+; for masked underflow).
+;
+CpuInitFloatPointUnit  PROC        PUBLIC
+    ;
+    ; Initialize floating point units
+    ;
+    ; The following opcodes stand for instruction 'finit' 
+    ; to be supported by some 64-bit assemblers
+    ;
+    DB      9Bh, 0DBh, 0E3h
+    fldcw   mFpuControlWord
+
+    ;
+    ; Set OSFXSR bit 9 in CR4
+    ;
+    mov     rax, cr4
+    or      rax, 200h
+    mov     cr4, rax
+
+    ldmxcsr mMmxControlWord
+    ret
+CpuInitFloatPointUnit  ENDP
+
+CpuDisableInterrupt PROC    PUBLIC
+
+    cli
+    ret
+    
+CpuDisableInterrupt  ENDP
+
+CpuEnableInterrupt PROC    PUBLIC
+
+    sti
+    ret
+    
+CpuEnableInterrupt  ENDP
+
+MAX_NR_BUS                    EQU 0FFh
+CSR_DESIRED_CORES             EQU 080h  ; CSR D0:F0:R80
+
+GetCsrDesiredCores PROC    PUBLIC
+
+  push rbx
+
+  ;
+  ; get Bus number from CPUID[1] EBX[31:24]
+  ;
+  mov     eax, 1                        ; bus 0
+  cpuid
+  bswap   ebx
+  shr     bl, 4
+  movzx   eax, bl
+  ;
+  ; Compute CPU Bus Num
+  ; Bus Num = (MAX_NB_BUS - socket ID)
+  ;
+  xor     eax, MAX_NR_BUS               ; bus number = MAX_NR_BUS - socket ID
+  ;
+  ; eax = bus number
+  ; out 0CF8h, GQ1_CR_PCIEXBAR OR (Bus Num shl 16)
+  ;
+  or      eax, 8000h
+  shl     eax, 16
+  or      eax, CSR_DESIRED_CORES        ; D0:F1:R80h
+  mov     dx, 0CF8h
+  out     dx, eax
+  mov     dx, 0CFCh
+  in      eax, dx
+
+  pop  rbx
+
+  ret
+
+GetCsrDesiredCores  ENDP
+
+SetLockCsrDesiredCores PROC    PUBLIC
+
+  push rbx
+
+  ;
+  ; get Bus number from CPUID[1] EBX[31:24]
+  ;
+  mov     eax, 1                        ; bus 0
+  cpuid
+  bswap   ebx
+  shr     bl, 4
+  movzx   eax, bl
+  ;
+  ; Compute CPU Bus Num
+  ; Bus Num = (MAX_NB_BUS - socket ID)
+  ;
+  xor     eax, MAX_NR_BUS               ; bus number = MAX_NR_BUS - socket ID
+  ;
+  ; eax = bus number
+  ; out 0CF8h, GQ1_CR_PCIEXBAR OR (Bus Num shl 16)
+  ;
+  or      eax, 8000h
+  shl     eax, 16
+  or      eax, CSR_DESIRED_CORES        ; D0:F1:R80h
+  mov     dx, 0CF8h
+  out     dx, eax
+  mov     dx, 0CFCh
+  in      eax, dx
+  or      eax, 10000h                   ; Bit[16] = Lock
+  out     dx, eax
+
+  pop  rbx
+  ret
+
+SetLockCsrDesiredCores  ENDP
+
+;------------------------------------------------------------------------------
+;  UINTN
+;  CpuFlushTlb (
+;    VOID
+;    )
+;------------------------------------------------------------------------------
+CpuFlushTlb PROC        PUBLIC
+    mov   rax, cr3
+    mov   cr3, rax
+    ret
+CpuFlushTlb  ENDP
+
+;------------------------------------------------------------------------------
+; UINT16
+; CpuCodeSegment (
+;   VOID
+;   );
+;------------------------------------------------------------------------------
+CpuCodeSegment PROC        PUBLIC  
+    xor   eax, eax
+    mov   eax, cs
+    ret
+CpuCodeSegment  ENDP
+
+;------------------------------------------------------------------------------
+; VOID
+; CpuLoadGlobalDescriptorTable (
+;   VOID  *Table16ByteAligned
+;   );
+;------------------------------------------------------------------------------
+CpuLoadGlobalDescriptorTable PROC   PUBLIC
+    lgdt  FWORD PTR [rcx]
+    ret
+CpuLoadGlobalDescriptorTable  ENDP
+
+;------------------------------------------------------------------------------
+; VOID
+; CpuLoadInterruptDescriptorTable (
+;   VOID  *Table16ByteAligned
+;   );
+;------------------------------------------------------------------------------
+CpuLoadInterruptDescriptorTable PROC   PUBLIC
+    lidt  FWORD PTR [rcx]
+    ret
+CpuLoadInterruptDescriptorTable  ENDP
+
+
+text  ENDS
+END
+
+
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/CpuLib.h b/ReferenceCode/Haswell/CpuInit/Dxe/x64/CpuLib.h
new file mode 100644
index 0000000..14699c1
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/CpuLib.h
@@ -0,0 +1,168 @@
+/** @file
+  Library functions that can be called in both PEI and DXE phase
+
+@copyright
+  Copyright (c) 2004 - 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 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 _CPU_LIB_H_
+#define _CPU_LIB_H_
+
+#if 0
+UINTN
+CpuReadCr0 (
+  VOID
+  )
+/**
+@brief
+  Get CR0 value
+
+  @param[in] None
+
+  @retval CR0 value
+**/
+;
+
+VOID
+CpuWriteCr0 (
+  UINTN   Value
+  )
+/**
+@brief
+  Write CR0 register
+
+  @param[in] Value - Value that will be written into CR0 register
+**/
+;
+
+UINTN
+CpuReadCr3 (
+  VOID
+  )
+/**
+@brief
+  Get CR3 register value
+
+  @param[in] None
+
+  @retval CR3 register value
+**/
+;
+
+VOID
+CpuWriteCr3 (
+  UINTN   Value
+  )
+/**
+@brief
+  Write CR3 register
+
+  @param[in] Value - Value that will be written to CR3 register
+**/
+;
+
+UINTN
+CpuSetPower2 (
+  IN  UINTN   Input
+  )
+/**
+@brief
+  Calculate the power 2 value from the Input value
+
+  @param[in] Input - The number that will be calculated
+
+  @retval Power 2 value after calculated
+**/
+;
+
+UINT64
+CpuReadTsc (
+  VOID
+  )
+/**
+@brief
+  Read CPU TSC value
+
+  @param[in] None
+
+  @retval TSC value
+**/
+;
+
+VOID
+CpuBreak (
+  VOID
+  )
+/**
+@brief
+  Break by INT3
+
+  @param[in] None
+**/
+;
+
+VOID
+CpuInitSelectors (
+  VOID
+  )
+/**
+@brief
+  Initialize selectors by calling INT 68
+
+  @param[in] None
+**/
+;
+
+#endif
+
+UINT16
+CpuCodeSegment (
+  VOID
+  )
+/**
+@brief
+  Return code segment address - CS
+
+  @param[in] None
+
+  @retval Code segment address
+**/
+;
+
+VOID
+CpuLoadGlobalDescriptorTable (
+  VOID  *Table16ByteAligned
+  )
+/**
+@brief
+  Get current GDT descriptor
+
+  @param[in] Table16ByteAligned  - the table buffer that will store current GDT table descriptor
+**/
+;
+
+VOID
+CpuLoadInterruptDescriptorTable (
+  VOID  *Table16ByteAligned
+  )
+/**
+@brief
+  Get current IDT descriptor
+
+  @param[in] Table16ByteAligned  - the table buffer that will store current GDT table descriptor
+**/
+;
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/Exception.c b/ReferenceCode/Haswell/CpuInit/Dxe/x64/Exception.c
new file mode 100644
index 0000000..bcd306b
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/Exception.c
@@ -0,0 +1,372 @@
+/** @file
+  EM64T Exception Handler
+
+@copyright
+  Copyright (c) 2006 - 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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxe.h"
+#include "MpCommon.h"
+#include "Exception.h"
+#endif
+
+typedef
+VOID
+(*EFI_INSTALL_EXCEPTION)(
+  IN UINT32 InterruptType,
+  IN VOID   *SystemContext
+  );
+
+typedef struct {
+  UINT32 ErrorMessage;
+  UINT8 Interrupt;
+} EFI_EXCEPTION_HANDLER;
+
+///
+/// Error code flag indicating whether or not an error code will be
+/// pushed on the stack if an exception occurs.
+///
+/// 1 means an error code will be pushed, otherwise 0
+///
+/// bit 0 - exception 0
+/// bit 1 - exception 1
+/// etc.
+///
+UINT32 mErrorCodeFlag = 0x00027d00;
+
+///
+/// Local Table
+///
+EFI_EXCEPTION_HANDLER mExceptionTable[] = {
+  {
+    EFI_SW_EC_IA32_DIVIDE_ERROR,
+    INTERRUPT_HANDLER_DIVIDE_ZERO
+  },
+  {
+    EFI_SW_EC_IA32_DEBUG,
+    INTERRUPT_HANDLER_DEBUG
+  },
+  {
+    EFI_SW_EC_IA32_NMI,
+    INTERRUPT_HANDLER_NMI
+  },
+  {
+    EFI_SW_EC_IA32_BREAKPOINT,
+    INTERRUPT_HANDLER_BREAKPOINT
+  },
+  {
+    EFI_SW_EC_IA32_OVERFLOW,
+    INTERRUPT_HANDLER_OVERFLOW
+  },
+  {
+    EFI_SW_EC_IA32_BOUND,
+    INTERRUPT_HANDLER_BOUND
+  },
+  {
+    EFI_SW_EC_IA32_INVALID_OPCODE,
+    INTERRUPT_HANDLER_INVALID_OPCODE
+  },
+  ///
+  /// Interrupt 7, 9, 15 not defined in the debug support protocol. Hence no status codes for them!
+  ///
+  {
+    EFI_SW_EC_IA32_DOUBLE_FAULT,
+    INTERRUPT_HANDLER_DOUBLE_FAULT
+  },
+  {
+    EFI_SW_EC_IA32_INVALID_TSS,
+    INTERRUPT_HANDLER_INVALID_TSS
+  },
+  {
+    EFI_SW_EC_IA32_SEG_NOT_PRESENT,
+    INTERRUPT_HANDLER_SEGMENT_NOT_PRESENT
+  },
+  {
+    EFI_SW_EC_IA32_STACK_FAULT,
+    INTERRUPT_HANDLER_STACK_SEGMENT_FAULT
+  },
+  {
+    EFI_SW_EC_IA32_GP_FAULT,
+    INTERRUPT_HANDLER_GP_FAULT
+  },
+  {
+    EFI_SW_EC_IA32_PAGE_FAULT,
+    INTERRUPT_HANDLER_PAGE_FAULT
+  },
+  {
+    EFI_SW_EC_IA32_FP_ERROR,
+    INTERRUPT_HANDLER_MATH_FAULT
+  },
+  {
+    EFI_SW_EC_IA32_ALIGNMENT_CHECK,
+    INTERRUPT_HANDLER_ALIGNMENT_FAULT
+  },
+  {
+    EFI_SW_EC_IA32_MACHINE_CHECK,
+    INTERRUPT_HANDLER_MACHINE_CHECK
+  },
+  {
+    EFI_SW_EC_IA32_SIMD,
+    INTERRUPT_HANDLER_STREAMING_SIMD
+  }
+};
+
+UINTN mExceptionNumber = sizeof (mExceptionTable) / sizeof (EFI_EXCEPTION_HANDLER);
+
+CPU_STATUS_CODE_TEMPLATE mStatusCodeData = {
+  {
+    sizeof (EFI_STATUS_CODE_DATA),
+    sizeof (EFI_SYSTEM_CONTEXT_X64),
+    EFI_STATUS_CODE_DATA_TYPE_EXCEPTION_HANDLER_GUID
+  },
+  {
+    0
+  }
+};
+
+UINT8 mExceptionLock = 0;
+
+/**
+  Report StatusCode for Exception
+
+  @param[in] InterruptType - Interrupt type
+  @param[in] SystemContext - EFI_SYSTEM_CONTEXT
+
+  @retval EFI_SUCCESS
+**/
+EFI_STATUS
+ReportData (
+  IN EFI_EXCEPTION_TYPE InterruptType,
+  IN EFI_SYSTEM_CONTEXT SystemContext
+  )
+{
+  UINT32 ErrorMessage;
+  UINT32 Index;
+
+  CopyMem (
+          &mStatusCodeData.SystemContext.SystemContextX64,
+          SystemContext.SystemContextX64,
+          sizeof (EFI_SYSTEM_CONTEXT_X64)
+          );
+
+  ErrorMessage = EFI_SOFTWARE_DXE_BS_DRIVER;
+  for (Index = 0; Index < mExceptionNumber; Index++) {
+    if (mExceptionTable[Index].Interrupt == InterruptType) {
+      ErrorMessage |= mExceptionTable[Index].ErrorMessage;
+      break;
+    }
+  }
+
+  ReportStatusCode (
+          (EFI_ERROR_CODE | EFI_ERROR_UNRECOVERED),
+          EFI_SOFTWARE_UNSPECIFIED | ErrorMessage
+          );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Common exception handler
+
+  @param[in] InterruptType - Exception type
+  @param[in] SystemContext - EFI_SYSTEM_CONTEXT
+**/
+VOID
+EFIAPI
+CommonExceptionHandler (
+  IN EFI_EXCEPTION_TYPE InterruptType,
+  IN EFI_SYSTEM_CONTEXT SystemContext
+  )
+{
+  AsmAcquireMPLock (&mExceptionLock);
+
+  DEBUG (
+          (EFI_D_ERROR,
+           "!!!! X64 Exception Type - %016lx    CPU Apic ID - %08x!!!!\n",
+           InterruptType,
+           GetApicID (NULL,
+                      NULL))
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "RIP - %016lx, CS - %016lx, RFLAGS - %016lx\n",
+           SystemContext.SystemContextX64->Rip,
+           SystemContext.SystemContextX64->Cs,
+           SystemContext.SystemContextX64->Rflags)
+          );
+  if (mErrorCodeFlag & (1 << InterruptType)) {
+    DEBUG (
+            (EFI_D_ERROR,
+             "ExceptionData - %016lx\n",
+             SystemContext.SystemContextX64->ExceptionData)
+            );
+  }
+
+  DEBUG (
+          (EFI_D_ERROR,
+           "RAX - %016lx, RCX - %016lx, RDX - %016lx\n",
+           SystemContext.SystemContextX64->Rax,
+           SystemContext.SystemContextX64->Rcx,
+           SystemContext.SystemContextX64->Rdx)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "RBX - %016lx, RSP - %016lx, RBP - %016lx\n",
+           SystemContext.SystemContextX64->Rbx,
+           SystemContext.SystemContextX64->Rsp,
+           SystemContext.SystemContextX64->Rbp)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "RSI - %016lx, RDI - %016lx\n",
+           SystemContext.SystemContextX64->Rsi,
+           SystemContext.SystemContextX64->Rdi)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "R8 - %016lx, R9 - %016lx, R10 - %016lx\n",
+           SystemContext.SystemContextX64->R8,
+           SystemContext.SystemContextX64->R9,
+           SystemContext.SystemContextX64->R10)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "R11 - %016lx, R12 - %016lx, R13 - %016lx\n",
+           SystemContext.SystemContextX64->R11,
+           SystemContext.SystemContextX64->R12,
+           SystemContext.SystemContextX64->R13)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "R14 - %016lx, R15 - %016lx\n",
+           SystemContext.SystemContextX64->R14,
+           SystemContext.SystemContextX64->R15)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "DS - %016lx, ES - %016lx, FS - %016lx\n",
+           SystemContext.SystemContextX64->Ds,
+           SystemContext.SystemContextX64->Es,
+           SystemContext.SystemContextX64->Fs)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "GS - %016lx, SS - %016lx\n",
+           SystemContext.SystemContextX64->Gs,
+           SystemContext.SystemContextX64->Ss)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "GDTR - %016lx %016lx, LDTR - %016lx\n",
+           SystemContext.SystemContextX64->Gdtr[0],
+           SystemContext.SystemContextX64->Gdtr[1],
+           SystemContext.SystemContextX64->Ldtr)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "IDTR - %016lx %016lx, TR - %016lx\n",
+           SystemContext.SystemContextX64->Idtr[0],
+           SystemContext.SystemContextX64->Idtr[1],
+           SystemContext.SystemContextX64->Tr)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "CR0 - %016lx, CR2 - %016lx, CR3 - %016lx\n",
+           SystemContext.SystemContextX64->Cr0,
+           SystemContext.SystemContextX64->Cr2,
+           SystemContext.SystemContextX64->Cr3)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "CR4 - %016lx, CR8 - %016lx\n",
+           SystemContext.SystemContextX64->Cr4,
+           SystemContext.SystemContextX64->Cr8)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "DR0 - %016lx, DR1 - %016lx, DR2 - %016lx\n",
+           SystemContext.SystemContextX64->Dr0,
+           SystemContext.SystemContextX64->Dr1,
+           SystemContext.SystemContextX64->Dr2)
+          );
+  DEBUG (
+          (EFI_D_ERROR,
+           "DR3 - %016lx, DR6 - %016lx, DR7 - %016lx\n",
+           SystemContext.SystemContextX64->Dr3,
+           SystemContext.SystemContextX64->Dr6,
+           SystemContext.SystemContextX64->Dr7)
+          );
+
+  ///
+  /// Report Status Code
+  ///
+  ReportData (InterruptType, SystemContext);
+  AsmReleaseMPLock (&mExceptionLock);
+
+  ///
+  /// Use this macro to hang so that the compiler does not optimize out
+  /// the following RET instructions. This allows us to return if we
+  /// have a debugger attached.
+  ///
+  EFI_DEADLOOP ();
+
+  return;
+}
+
+/**
+  Install the IA-32 EM64T Exception Handler.
+  The current operation (which likely will change) will uninstall all the
+  pertinent exception handlers (0-7, 10-14, 16-19) except for Int8 which the timer
+  is currently sitting on (or soon will be).
+
+  It then installs all the appropriate handlers for each exception.
+
+  The handler then calls gRT->ReportStatusCode with a specific progress code.  The
+  progress codes for now start at 0x200 for IA-32 processors. See Status Code
+  Specification for details. The Status code Specification uses the enumeration from
+  the EFI 1.1 Debug Support Protocol.
+
+  @param[in] CpuProtocol - Instance of CPU Arch Protocol
+
+  @retval EFI_SUCCESS - This function always return success after registering handlers.
+**/
+EFI_STATUS
+InitializeException (
+  IN EFI_CPU_ARCH_PROTOCOL *CpuProtocol
+  )
+{
+  EFI_STATUS Status;
+  UINTN      Index;
+
+  CpuProtocol->DisableInterrupt (CpuProtocol);
+
+  for (Index = 0; Index < mExceptionNumber; Index++) {
+    Status = CpuProtocol->RegisterInterruptHandler (CpuProtocol, mExceptionTable[Index].Interrupt, NULL);
+    ///
+    /// Add in our handler
+    ///
+    Status = CpuProtocol->RegisterInterruptHandler (
+                            CpuProtocol,
+                            mExceptionTable[Index].Interrupt,
+                            CommonExceptionHandler
+                            );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/MemoryOperation.c b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MemoryOperation.c
new file mode 100644
index 0000000..5fae506
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MemoryOperation.c
@@ -0,0 +1,729 @@
+/** @file
+  Memory Operation Functions for IA32 Architecture.
+
+@copyright
+  Copyright (c) 2006 - 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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxe.h"
+#include "CpuLib.h"
+#include "MpCommon.h"
+#include "VirtualMemory.h"
+#include "MemoryAttribute.h"
+#endif
+
+VOID
+InitializeExternalVectorTablePtr (
+  EFI_CPU_INTERRUPT_HANDLER *VectorTable
+  );
+
+extern EFI_CPU_INTERRUPT_HANDLER mExternalVectorTable[];
+extern EFI_PHYSICAL_ADDRESS      mBackupBuffer;
+
+UINT8 *mPageStore     = NULL;
+UINTN mPageStoreSize  = 16;
+UINTN mPageStoreIndex = 0;
+
+UINT64 mValidMtrrAddressMask;
+UINT64 mValidMtrrBitsMask;
+
+///
+/// BugBug: Non Portable
+///
+#if defined (__GNUC__)
+#define ALIGN_16BYTE_BOUNDRY  __attribute__ ((aligned (16)))
+#else
+#define ALIGN_16BYTE_BOUNDRY  __declspec (align (16))
+#endif
+
+#pragma pack(1)
+typedef struct {
+  UINT16 LimitLow;
+  UINT16 BaseLow;
+  UINT8 BaseMiddle;
+  UINT8 Attributes1;
+  UINT8 Attributes2;
+  UINT8 BaseHigh;
+} SEGMENT_DESCRIPTOR_x64;
+
+typedef struct {
+  UINT16 Limit;
+  UINTN Base;
+} PSEUDO_DESCRIPTOR_x64;
+
+#pragma pack()
+
+ALIGN_16BYTE_BOUNDRY SEGMENT_DESCRIPTOR_x64 gGdt[] = {
+  {    /// NULL Selector: selector[0]
+    0, /// limit 15:0
+    0, /// base  15:0
+    0, /// base  23:16
+    0, ///
+    0, /// type & limit 19:16
+    0, /// base  31:24
+    ///   0,  /// base  63:32
+    ///   0   /// reserved
+    ///
+  },
+  {         /// Linear Selector: selector[8]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x93,   /// present, ring 0, data, expand-up writable
+    0xcf,   /// type & limit 19:16
+    0,      /// base  31:24
+    ///   0,      /// base  63:32
+    ///   0       /// reserved
+    ///
+  },
+  {         /// Linear code Selector: selector[10]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x9b,   /// present, ring 0, code, expand-up writable
+    0xcf,   /// type & limit 19:16
+    0,      /// base  31:24
+    ///  0,      /// base  63:32
+    ///  0       /// reserved
+    ///
+  },
+  {         /// Compatibility mode data Selector: selector[18]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x93,   /// type & limit 19:16
+    0xcf,
+    0,      /// base  31:24
+    /// 0,      /// base  63:32
+    /// 0       /// reserved
+    ///
+  },
+  {         /// Compatibility code Selector: selector[20]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x9b,   /// type & limit 19:16
+    0xcf,
+    0,      /// base  31:24
+    ///  0,      /// base  63:32
+    ///  0       /// reserved
+    ///
+  },
+  {     /// Spare3 Selector: selector[28]
+    0,  /// limit 15:0
+    0,  /// base  15:0
+    0,  /// base  23:16
+    0,  /// type & limit 19:16
+    0,  /// base  31:24
+    0,
+    ///
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  },
+  {         /// 64-bit data Selector:selector[30]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x93,   /// type & limit 19:16
+    0xcf,
+    0,      /// base  31:24
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  },
+  {         /// 64-bit code Selector: selector[38]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x9b,   /// type & limit 19:16
+    0xaf,
+    0,      /// base  31:24
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  },
+  {     /// Spare3 Selector: selector[40]
+    0,  /// limit 15:0
+    0,  /// base  15:0
+    0,  /// base  23:16
+    0,  /// type & limit 19:16
+    0,  /// base  31:24
+    0,
+    ///
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  }
+};
+
+ALIGN_16BYTE_BOUNDRY PSEUDO_DESCRIPTOR_x64 gGdtPseudoDescriptor = {
+  sizeof (gGdt) - 1,
+  (UINTN) gGdt
+};
+
+INTERRUPT_GATE_DESCRIPTOR gIdtTable[INTERRUPT_VECTOR_NUMBER] = { 0 };
+
+ALIGN_16BYTE_BOUNDRY PSEUDO_DESCRIPTOR_x64 gLidtPseudoDescriptor = {
+  sizeof (gIdtTable) - 1,
+  (UINTN) gIdtTable
+};
+
+/**
+  Init Global Descriptor table
+**/
+VOID
+InitializeSelectors (
+  VOID
+  )
+{
+  CpuLoadGlobalDescriptorTable (&gGdtPseudoDescriptor);
+}
+
+/**
+  Generic IDT Vector Handlers for the Host
+**/
+VOID
+AsmIdtVector00 (
+  VOID
+  );
+
+/**
+  Initialize Interrupt descriptor Tables
+**/
+VOID
+InitializeInterruptTables (
+  VOID
+  )
+{
+  UINT16                    CodeSegment;
+  INTERRUPT_GATE_DESCRIPTOR *IdtEntry;
+  UINT8                     *CurrentHandler;
+  UINT32                    Index;
+
+  CodeSegment     = CpuCodeSegment ();
+
+  IdtEntry        = gIdtTable;
+  CurrentHandler  = (UINT8 *) (UINTN) AsmIdtVector00;
+  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index++) {
+    IdtEntry[Index].Offset15To0     = (UINT16) (UINTN) CurrentHandler;
+    IdtEntry[Index].SegmentSelector = CodeSegment;
+    IdtEntry[Index].Attributes      = INTERRUPT_GATE_ATTRIBUTE;
+    ///
+    /// 8e00;
+    ///
+    IdtEntry[Index].Offset31To16  = (UINT16) ((UINTN) CurrentHandler >> 16);
+    IdtEntry[Index].Offset63To32  = (UINT32) ((UINTN) CurrentHandler >> 32);
+
+    CurrentHandler += 0x8;
+    ///
+  }
+
+  CpuLoadInterruptDescriptorTable (&gLidtPseudoDescriptor);
+
+  return;
+}
+
+/**
+  Initialize cache attributes based on MTRR
+**/
+VOID
+InitailizeCacheAttributes (
+  VOID
+  )
+{
+  EFI_STATUS           Status;
+  EFI_PHYSICAL_ADDRESS Page;
+  EFI_CPUID_REGISTER   FeatureInfo;
+  EFI_CPUID_REGISTER   FunctionInfo;
+  UINT8                PhysicalAddressBits;
+  UINT32               MsrNum;
+  UINT64               TempQword;
+  UINT64               ComplementBits;
+  UINT32               VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+
+  ///
+  /// Allocate 16 pages
+  ///
+  Status            = (gBS->AllocatePages)(AllocateAnyPages, EfiBootServicesData, mPageStoreSize, &Page);
+  ASSERT_EFI_ERROR (Status);
+
+  mPageStore = (UINT8 *) (UINTN) Page;
+
+  ZeroMem (mPageStore, 0x1000 * mPageStoreSize);
+
+  ///
+  /// Check returned value of Eax for extended CPUID functions
+  ///
+  AsmCpuid (
+    CPUID_EXTENDED_FUNCTION,
+    &FunctionInfo.RegEax,
+    &FunctionInfo.RegEbx,
+    &FunctionInfo.RegEcx,
+    &FunctionInfo.RegEdx
+    );
+
+  PhysicalAddressBits = 36;
+
+  ///
+  /// If CPU supports extended functions, get the Physical Address size by reading EAX[7:0]
+  ///
+  if (FunctionInfo.RegEax > CPUID_EXTENDED_FUNCTION) {
+    AsmCpuid (
+      CPUID_VIR_PHY_ADDRESS_SIZE,
+      &FeatureInfo.RegEax,
+      &FeatureInfo.RegEbx,
+      &FeatureInfo.RegEcx,
+      &FeatureInfo.RegEdx
+      );
+    PhysicalAddressBits = (UINT8) FeatureInfo.RegEax;
+  }
+
+  mValidMtrrBitsMask    = (((UINT64) 1) << PhysicalAddressBits) - 1;
+  mValidMtrrAddressMask = mValidMtrrBitsMask & 0xfffffffffffff000;
+
+  ComplementBits        = mValidMtrrBitsMask & 0xfffffff000000000;
+  if (ComplementBits != 0) {
+    ///
+    /// Disable cache and clear the corresponding MTRR bits
+    ///
+    PreMtrrChange ();
+    for (MsrNum = CACHE_VARIABLE_MTRR_BASE;
+         MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1);
+         MsrNum += 2
+         ) {
+      TempQword = AsmReadMsr64 (MsrNum + 1);
+      if ((TempQword & B_CACHE_MTRR_VALID) != 0) {
+        ///
+        /// MTRR Physical Mask
+        ///
+        TempQword = TempQword | ComplementBits;
+        AsmWriteMsr64 (MsrNum + 1, TempQword);
+      }
+    }
+
+    ///
+    /// Enable Cache and set the corresponding MTRR bits
+    ///
+    PostMtrrChange ();
+  }
+}
+
+/**
+  Allocate zeroed pages
+
+  @retval Pointer to the page buffer
+**/
+VOID *
+AllocateZeroedPage (
+  VOID
+  )
+{
+  if (mPageStoreIndex >= mPageStoreSize) {
+    ///
+    /// We are out of space
+    ///
+    return NULL;
+  }
+
+  return (VOID *) (UINTN) &mPageStore[0x1000 * mPageStoreIndex++];
+}
+
+/**
+  Convert 2MB page tables to 4KB page tables
+
+  @param[in] PageAddress             - Page address to convert
+  @param[in] PageDirectoryToConvert  - Page table that will be converted
+**/
+VOID
+Convert2MBPageTo4KPages (
+  IN EFI_PHYSICAL_ADDRESS     PageAddress,
+  IN OUT x64_PAGE_TABLE_ENTRY **PageDirectoryToConvert
+  )
+{
+  UINTN                   Index;
+  EFI_PHYSICAL_ADDRESS    WorkingAddress;
+  x64_PAGE_TABLE_ENTRY_4K *PageTableEntry;
+  x64_PAGE_TABLE_ENTRY    Attributes;
+
+  ///
+  /// Save the attributes of the 2MB table
+  ///
+  Attributes.Page2Mb.Uint64 = (*PageDirectoryToConvert)->Page2Mb.Uint64;
+
+  ///
+  /// Convert PageDirectoryEntry2MB into a 4K Page Directory
+  ///
+  PageTableEntry = AllocateZeroedPage ();
+  if (PageTableEntry == NULL) {
+    return;
+  }
+  (*PageDirectoryToConvert)->Page2Mb.Uint64         = (UINT64) PageTableEntry;
+  (*PageDirectoryToConvert)->Page2Mb.Bits.ReadWrite = 1;
+  (*PageDirectoryToConvert)->Page2Mb.Bits.Present   = 1;
+
+  WorkingAddress = PageAddress;
+  for (Index = 0; Index < 512; Index++, PageTableEntry++, WorkingAddress += 0x1000) {
+    PageTableEntry->Uint64        = (UINT64) WorkingAddress;
+    PageTableEntry->Bits.Present  = 1;
+
+    ///
+    /// Update the new page to have the same attributes as the 2MB page
+    ///
+    PageTableEntry->Bits.ReadWrite      = Attributes.Common.ReadWrite;
+    PageTableEntry->Bits.CacheDisabled  = Attributes.Common.CacheDisabled;
+    PageTableEntry->Bits.WriteThrough   = Attributes.Common.WriteThrough;
+
+    if (WorkingAddress == PageAddress) {
+      ///
+      /// Return back the 4K page that matches the Working addresss
+      ///
+      *PageDirectoryToConvert = (x64_PAGE_TABLE_ENTRY *) PageTableEntry;
+    }
+  }
+}
+
+/**
+  Get current memory mapping information
+
+  @param[in] BaseAddress - get current memory mapping by this Base address
+  @param[in] PageTable   - page table that translated this base address
+  @param[in] Page2MBytes - TRUE if this is 2MBytes page table
+
+  @retval EFI_NOT_FOUND - page table not found
+  @retval EFI_SUCCESS   - page table found
+**/
+EFI_STATUS
+GetCurrentMapping (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  OUT x64_PAGE_TABLE_ENTRY **PageTable,
+  OUT BOOLEAN              *Page2MBytes
+  )
+{
+  UINT64                                    Cr3;
+  x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *PageMapLevel4Entry;
+  x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *PageDirectoryPointerEntry;
+  x64_PAGE_TABLE_ENTRY_2M                   *PageTableEntry2Mb;
+  x64_PAGE_DIRECTORY_ENTRY_4K               *PageDirectoryEntry4k;
+  x64_PAGE_TABLE_ENTRY_4K                   *PageTableEntry4k;
+  UINTN                                     Pml4Index;
+  UINTN                                     PdpIndex;
+  UINTN                                     Pde2MbIndex;
+  UINTN                                     PteIndex;
+
+  Cr3                 = AsmReadCr3 ();
+
+  PageMapLevel4Entry  = (x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *) (Cr3 & 0x000ffffffffff000);
+
+  Pml4Index           = (UINTN) RShiftU64 (BaseAddress, 39) & 0x1ff;
+  if (PageMapLevel4Entry[Pml4Index].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  PageDirectoryPointerEntry = (x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *) (PageMapLevel4Entry[Pml4Index].Uint64 & 0x000ffffffffff000);
+  PdpIndex = (UINTN) RShiftU64 (BaseAddress, 30) & 0x1ff;
+  if (PageDirectoryPointerEntry[PdpIndex].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  PageTableEntry2Mb = (x64_PAGE_TABLE_ENTRY_2M *) (PageDirectoryPointerEntry[PdpIndex].Uint64 & 0x000ffffffffff000);
+  Pde2MbIndex       = (UINTN) RShiftU64 (BaseAddress, 21) & 0x1ff;
+  if (PageTableEntry2Mb[Pde2MbIndex].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (PageTableEntry2Mb[Pde2MbIndex].Bits.MustBe1 == 1) {
+    ///
+    /// We found a 2MByte page so lets return it
+    ///
+    *Page2MBytes  = TRUE;
+    *PageTable    = (x64_PAGE_TABLE_ENTRY *) &PageTableEntry2Mb[Pde2MbIndex].Uint64;
+    return EFI_SUCCESS;
+  }
+
+  ///
+  /// 4K page so keep walking
+  ///
+  PageDirectoryEntry4k  = (x64_PAGE_DIRECTORY_ENTRY_4K *) &PageTableEntry2Mb[Pde2MbIndex].Uint64;
+
+  PageTableEntry4k      = (x64_PAGE_TABLE_ENTRY_4K *) (PageDirectoryEntry4k[Pde2MbIndex].Uint64 & 0x000ffffffffff000);
+  PteIndex              = (UINTN) RShiftU64 (BaseAddress, 12) & 0x1ff;
+  if (PageTableEntry4k[PteIndex].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Page2MBytes  = FALSE;
+  *PageTable    = (x64_PAGE_TABLE_ENTRY *) &PageTableEntry4k[PteIndex];
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare memory for essential system tables.
+
+  @retval EFI_SUCCESS              - Memory successfully prepared.
+**/
+EFI_STATUS
+PrepareMemory (
+  VOID
+  )
+{
+  ///
+  /// Allocate space to convert 2MB page tables to 4K tables.
+  ///  This can not be done at call time as the TPL level will
+  ///  not be correct.
+  ///
+  InitailizeCacheAttributes ();
+
+  InitializeExternalVectorTablePtr (mExternalVectorTable);
+  ///
+  /// Initialize the Interrupt Descriptor Table
+  ///
+  InitializeInterruptTables ();
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare Wakeup Buffer and stack for APs.
+
+  @param[in] WakeUpBuffer             - Pointer to the address of wakeup buffer for output.
+  @param[in] StackAddressStart        - Pointer to the stack address of APs for output.
+  @param[in] MaximumCPUsForThisSystem - Maximum CPUs in this system.
+
+  @retval EFI_SUCCESS              - Memory successfully prepared for APs.
+  @retval Other                    - Error occurred while allocating memory.
+**/
+EFI_STATUS
+PrepareMemoryForAPs (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer,
+  OUT VOID                 **StackAddressStart,
+  IN UINTN                 MaximumCPUsForThisSystem
+  )
+{
+  EFI_STATUS              Status;
+  MP_ASSEMBLY_ADDRESS_MAP AddressMap;
+
+  ///
+  /// Release All APs with a lock and wait for them to retire to rendezvous procedure.
+  /// We need a page (4KB) of memory for IA-32 to use broadcast APIs, on a temporary basis.
+  ///
+  Status = AllocateWakeUpBuffer (WakeUpBuffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Claim memory for AP stack
+  ///
+  Status = AllocateReservedMemoryBelow4G (
+            MaximumCPUsForThisSystem * STACK_SIZE_PER_PROC,
+            StackAddressStart
+            );
+
+  if (EFI_ERROR (Status)) {
+    (gBS->FreePages)(*WakeUpBuffer, 1);
+    return Status;
+  }
+
+  AsmGetAddressMap (&AddressMap);
+  CopyMem ((VOID *) (UINTN) *WakeUpBuffer, AddressMap.RendezvousFunnelAddress, AddressMap.Size);
+  *(UINT32 *) (UINTN) (*WakeUpBuffer + AddressMap.FlatJumpOffset + 3) = (UINT32) (*WakeUpBuffer + AddressMap.PModeEntryOffset);
+  *(UINT32 *) (UINTN) (*WakeUpBuffer + AddressMap.LongJumpOffset + 2) = (UINT32) (*WakeUpBuffer + AddressMap.LModeEntryOffset);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare exchange information for APs.
+
+  @param[in] ExchangeInfo      - Pointer to the exchange info buffer for output.
+  @param[in] StackAddressStart - Start address of APs' stacks.
+  @param[in] ApFunction        - Address of function assigned to AP.
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer.
+
+  @retval EFI_SUCCESS       - Exchange Info successfully prepared for APs.
+**/
+EFI_STATUS
+PrepareExchangeInfo (
+  OUT MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  IN VOID                  *StackAddressStart,
+  IN VOID                  *ApFunction,
+  IN EFI_PHYSICAL_ADDRESS  WakeUpBuffer
+  )
+{
+  ZeroMem ((VOID *) ExchangeInfo, EFI_PAGE_SIZE - MP_CPU_EXCHANGE_INFO_OFFSET);
+
+  ExchangeInfo->Lock        = VacantFlag;
+  ExchangeInfo->StackStart  = StackAddressStart;
+  ExchangeInfo->StackSize   = STACK_SIZE_PER_PROC;
+  ExchangeInfo->ApFunction  = ApFunction;
+
+  CopyMem (
+          (VOID *) (UINTN) &ExchangeInfo->GdtrProfile,
+          (VOID *) (UINTN) mAcpiCpuData->GdtrProfile,
+          sizeof (PSEUDO_DESCRIPTOR)
+          );
+  CopyMem (
+          (VOID *) (UINTN) &ExchangeInfo->IdtrProfile,
+          (VOID *) (UINTN) mAcpiCpuData->IdtrProfile,
+          sizeof (PSEUDO_DESCRIPTOR)
+          );
+
+  ExchangeInfo->BufferStart = (UINT32) WakeUpBuffer;
+  ExchangeInfo->Cr3         = (UINT32) (AsmReadCr3 ());
+  ExchangeInfo->InitFlag    = 1;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare Wakeup Buffer and stack for APs.
+
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer for output.
+  @param[in] StackAddressStart - Pointer to the stack address of APs for output.
+
+  @retval EFI_SUCCESS       - Memory successfully prepared for APs.
+**/
+EFI_STATUS
+S3PrepareMemoryForAPs (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer,
+  OUT VOID                 **StackAddressStart
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare exchange information for APs.
+
+  @param[in] ExchangeInfo      - Pointer to the exchange info for output.
+  @param[in] StackAddressStart - Start address of APs' stacks.
+  @param[in] ApFunction        - Address of function assigned to AP.
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer.
+
+  @retval EFI_SUCCESS       - Exchange Info successfully prepared for APs.
+**/
+EFI_STATUS
+S3PrepareExchangeInfo (
+  OUT MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  IN VOID                  *StackAddressStart,
+  IN VOID                  *ApFunction,
+  IN EFI_PHYSICAL_ADDRESS  WakeUpBuffer
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Dynamically write the far jump destination in APs' wakeup buffer,
+  in order to refresh APs' CS registers for mode switching.
+**/
+VOID
+RedirectFarJump (
+  VOID
+  )
+{
+  MP_ASSEMBLY_ADDRESS_MAP AddressMap;
+
+  AsmGetAddressMap (&AddressMap);
+  *(UINT32 *) (UINTN) (mAcpiCpuData->WakeUpBuffer + AddressMap.FlatJumpOffset + 3) = (UINT32) (mAcpiCpuData->WakeUpBuffer + AddressMap.PModeEntryOffset);
+  *(UINT32 *) (UINTN) (mAcpiCpuData->WakeUpBuffer + AddressMap.LongJumpOffset + 2) = (UINT32) (mAcpiCpuData->WakeUpBuffer + AddressMap.LModeEntryOffset);
+
+  return;
+}
+
+/**
+  Prepare GDTR and IDTR for AP
+
+  @param[in] Gdtr  - The GDTR profile
+  @param[in] Idtr  - The IDTR profile
+
+  @retval EFI_STATUS  - status returned by each sub-routine
+  @retval EFI_SUCCESS - GDTR and IDTR has been prepared for AP
+**/
+EFI_STATUS
+PrepareGdtIdtForAP (
+  OUT PSEUDO_DESCRIPTOR *Gdtr,
+  OUT PSEUDO_DESCRIPTOR *Idtr
+  )
+{
+  INTERRUPT_GATE_DESCRIPTOR *IdtForAP;
+  SEGMENT_DESCRIPTOR        *GdtForAP;
+
+  PSEUDO_DESCRIPTOR *IdtrForBSP;
+  PSEUDO_DESCRIPTOR *GdtrForBSP;
+
+  UINT16     *MceHandler;
+  EFI_STATUS Status;
+
+  AsmGetGdtrIdtr (&GdtrForBSP, &IdtrForBSP);
+
+  ///
+  /// Allocate reserved memory for IDT
+  ///
+  Status = AllocateAlignedReservedMemory (
+            IdtrForBSP->Limit + 1,
+            8,
+            (VOID **) &IdtForAP
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Allocate reserved memory for GDT
+  ///
+  Status = AllocateAlignedReservedMemory (
+            GdtrForBSP->Limit + 1,
+            8,
+            (VOID **) &GdtForAP
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = (gBS->AllocatePool)(EfiReservedMemoryType, SIZE_OF_MCE_HANDLER, (VOID **) &MceHandler);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// McheHandler content: iret (opcode = 0xcf)
+  ///
+  *MceHandler = 0xCF48;
+
+  CopyMem (GdtForAP, (VOID *) GdtrForBSP->Base, GdtrForBSP->Limit + 1);
+  CopyMem (IdtForAP, (VOID *) IdtrForBSP->Base, IdtrForBSP->Limit + 1);
+
+  IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].Offset15To0   = (UINT16) (UINTN) MceHandler;
+  IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].Offset31To16  = (UINT16) ((UINTN) MceHandler >> 16);
+  IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].Offset63To32  = (UINT32) ((UINTN) MceHandler >> 32);
+
+  ///
+  /// Create Gdtr, IDTR profile
+  ///
+  Gdtr->Base  = (UINTN) GdtForAP;
+  Gdtr->Limit = GdtrForBSP->Limit;
+
+  Idtr->Base  = (UINTN) IdtForAP;
+  Idtr->Limit = IdtrForBSP->Limit;
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/MemoryOperationDbgr.c b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MemoryOperationDbgr.c
new file mode 100644
index 0000000..329a4a5
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MemoryOperationDbgr.c
@@ -0,0 +1,749 @@
+/** @file
+  Memory Operation Functions for IA32 Architecture.
+
+@copyright
+  Copyright (c) 2006 - 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 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "CpuInitDxeDbgr.h"
+#include "CpuLib.h"
+#include "MpCommon.h"
+#include "VirtualMemory.h"
+#include "MemoryAttribute.h"
+#endif
+
+//(AMI_CHG+)>
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+extern AMI_DEBUGGER_CPU_PROTOCOL  *mAmiDebuggerCpuProtocol;
+#endif
+//<(AMI_CHG+)
+
+VOID
+InitializeExternalVectorTablePtr (
+  EFI_CPU_INTERRUPT_HANDLER *VectorTable
+  );
+
+extern EFI_CPU_INTERRUPT_HANDLER mExternalVectorTable[];
+extern EFI_PHYSICAL_ADDRESS      mBackupBuffer;
+
+UINT8 *mPageStore     = NULL;
+UINTN mPageStoreSize  = 16;
+UINTN mPageStoreIndex = 0;
+
+UINT64 mValidMtrrAddressMask;
+UINT64 mValidMtrrBitsMask;
+
+///
+/// BugBug: Non Portable
+///
+#if defined (__GNUC__)
+#define ALIGN_16BYTE_BOUNDRY  __attribute__ ((aligned (16)))
+#else
+#define ALIGN_16BYTE_BOUNDRY  __declspec (align (16))
+#endif
+
+#pragma pack(1)
+typedef struct {
+  UINT16 LimitLow;
+  UINT16 BaseLow;
+  UINT8 BaseMiddle;
+  UINT8 Attributes1;
+  UINT8 Attributes2;
+  UINT8 BaseHigh;
+} SEGMENT_DESCRIPTOR_x64;
+
+typedef struct {
+  UINT16 Limit;
+  UINTN Base;
+} PSEUDO_DESCRIPTOR_x64;
+
+#pragma pack()
+
+ALIGN_16BYTE_BOUNDRY SEGMENT_DESCRIPTOR_x64 gGdt[] = {
+  {    /// NULL Selector: selector[0]
+    0, /// limit 15:0
+    0, /// base  15:0
+    0, /// base  23:16
+    0, ///
+    0, /// type & limit 19:16
+    0, /// base  31:24
+    ///   0,  /// base  63:32
+    ///   0   /// reserved
+    ///
+  },
+  {         /// Linear Selector: selector[8]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x93,   /// present, ring 0, data, expand-up writable
+    0xcf,   /// type & limit 19:16
+    0,      /// base  31:24
+    ///   0,      /// base  63:32
+    ///   0       /// reserved
+    ///
+  },
+  {         /// Linear code Selector: selector[10]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x9b,   /// present, ring 0, code, expand-up writable
+    0xcf,   /// type & limit 19:16
+    0,      /// base  31:24
+    ///  0,      /// base  63:32
+    ///  0       /// reserved
+    ///
+  },
+  {         /// Compatibility mode data Selector: selector[18]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x93,   /// type & limit 19:16
+    0xcf,
+    0,      /// base  31:24
+    /// 0,      /// base  63:32
+    /// 0       /// reserved
+    ///
+  },
+  {         /// Compatibility code Selector: selector[20]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x9b,   /// type & limit 19:16
+    0xcf,
+    0,      /// base  31:24
+    ///  0,      /// base  63:32
+    ///  0       /// reserved
+    ///
+  },
+  {     /// Spare3 Selector: selector[28]
+    0,  /// limit 15:0
+    0,  /// base  15:0
+    0,  /// base  23:16
+    0,  /// type & limit 19:16
+    0,  /// base  31:24
+    0,
+    ///
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  },
+  {         /// 64-bit data Selector:selector[30]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x93,   /// type & limit 19:16
+    0xcf,
+    0,      /// base  31:24
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  },
+  {         /// 64-bit code Selector: selector[38]
+    0xffff, /// limit 15:0
+    0,      /// base  15:0
+    0,      /// base  23:16
+    0x9b,   /// type & limit 19:16
+    0xaf,
+    0,      /// base  31:24
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  },
+  {     /// Spare3 Selector: selector[40]
+    0,  /// limit 15:0
+    0,  /// base  15:0
+    0,  /// base  23:16
+    0,  /// type & limit 19:16
+    0,  /// base  31:24
+    0,
+    ///
+    ///  0,  /// base  63:32
+    ///  0   /// reserved
+    ///
+  }
+};
+
+ALIGN_16BYTE_BOUNDRY PSEUDO_DESCRIPTOR_x64 gGdtPseudoDescriptor = {
+  sizeof (gGdt) - 1,
+  (UINTN) gGdt
+};
+
+INTERRUPT_GATE_DESCRIPTOR gIdtTable[INTERRUPT_VECTOR_NUMBER] = { 0 };
+
+ALIGN_16BYTE_BOUNDRY PSEUDO_DESCRIPTOR_x64 gLidtPseudoDescriptor = {
+  sizeof (gIdtTable) - 1,
+  (UINTN) gIdtTable
+};
+
+/**
+  Init Global Descriptor table
+**/
+VOID
+InitializeSelectors (
+  VOID
+  )
+{
+  CpuLoadGlobalDescriptorTable (&gGdtPseudoDescriptor);
+}
+
+/**
+  Generic IDT Vector Handlers for the Host
+**/
+VOID
+AsmIdtVector00 (
+  VOID
+  );
+
+/**
+  Initialize Interrupt descriptor Tables
+**/
+VOID
+InitializeInterruptTables (
+  VOID
+  )
+{
+  UINT16                    CodeSegment;
+  INTERRUPT_GATE_DESCRIPTOR *IdtEntry;
+  UINT8                     *CurrentHandler;
+  UINT32                    Index;
+
+//(AMI_CHG+)>  
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+  EFI_STATUS Status;
+#endif
+//<(AMI_CHG+)
+
+  CodeSegment     = CpuCodeSegment ();
+
+  IdtEntry        = gIdtTable;
+  CurrentHandler  = (UINT8 *) (UINTN) AsmIdtVector00;
+  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index++) {
+    IdtEntry[Index].Offset15To0     = (UINT16) (UINTN) CurrentHandler;
+    IdtEntry[Index].SegmentSelector = CodeSegment;
+    IdtEntry[Index].Attributes      = INTERRUPT_GATE_ATTRIBUTE;
+    ///
+    /// 8e00;
+    ///
+    IdtEntry[Index].Offset31To16  = (UINT16) ((UINTN) CurrentHandler >> 16);
+    IdtEntry[Index].Offset63To32  = (UINT32) ((UINTN) CurrentHandler >> 32);
+
+//(AMI_CHG+)>    
+#if defined(AMI_PEI_DEBUG_SUPPORT) && AMI_PEI_DEBUG_SUPPORT
+    Status = mAmiDebuggerCpuProtocol->DebuggerFixUpPEIExceptionHandlers(
+    					(DEBUGGER_INTERRUPT_GATE_DESCRIPTOR *)IdtEntry, 
+    					 Index);
+#endif
+//<(AMI_CHG+)
+
+    CurrentHandler += 0x8;
+    ///
+  }
+
+  CpuLoadInterruptDescriptorTable (&gLidtPseudoDescriptor);
+
+  return;
+}
+
+/**
+  Initialize cache attributes based on MTRR
+**/
+VOID
+InitailizeCacheAttributes (
+  VOID
+  )
+{
+  EFI_STATUS           Status;
+  EFI_PHYSICAL_ADDRESS Page;
+  EFI_CPUID_REGISTER   FeatureInfo;
+  EFI_CPUID_REGISTER   FunctionInfo;
+  UINT8                PhysicalAddressBits;
+  UINT32               MsrNum;
+  UINT64               TempQword;
+  UINT64               ComplementBits;
+  UINT32               VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+
+  ///
+  /// Allocate 16 pages
+  ///
+  Status            = (gBS->AllocatePages)(AllocateAnyPages, EfiBootServicesData, mPageStoreSize, &Page);
+  ASSERT_EFI_ERROR (Status);
+
+  mPageStore = (UINT8 *) (UINTN) Page;
+
+  ZeroMem (mPageStore, 0x1000 * mPageStoreSize);
+
+  ///
+  /// Check returned value of Eax for extended CPUID functions
+  ///
+  AsmCpuid (
+    CPUID_EXTENDED_FUNCTION,
+    &FunctionInfo.RegEax,
+    &FunctionInfo.RegEbx,
+    &FunctionInfo.RegEcx,
+    &FunctionInfo.RegEdx
+    );
+
+  PhysicalAddressBits = 36;
+
+  ///
+  /// If CPU supports extended functions, get the Physical Address size by reading EAX[7:0]
+  ///
+  if (FunctionInfo.RegEax > CPUID_EXTENDED_FUNCTION) {
+    AsmCpuid (
+      CPUID_VIR_PHY_ADDRESS_SIZE,
+      &FeatureInfo.RegEax,
+      &FeatureInfo.RegEbx,
+      &FeatureInfo.RegEcx,
+      &FeatureInfo.RegEdx
+      );
+    PhysicalAddressBits = (UINT8) FeatureInfo.RegEax;
+  }
+
+  mValidMtrrBitsMask    = (((UINT64) 1) << PhysicalAddressBits) - 1;
+  mValidMtrrAddressMask = mValidMtrrBitsMask & 0xfffffffffffff000;
+
+  ComplementBits        = mValidMtrrBitsMask & 0xfffffff000000000;
+  if (ComplementBits != 0) {
+    ///
+    /// Disable cache and clear the corresponding MTRR bits
+    ///
+    PreMtrrChange ();
+    for (MsrNum = CACHE_VARIABLE_MTRR_BASE;
+         MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1);
+         MsrNum += 2
+         ) {
+      TempQword = AsmReadMsr64 (MsrNum + 1);
+      if ((TempQword & B_CACHE_MTRR_VALID) != 0) {
+        ///
+        /// MTRR Physical Mask
+        ///
+        TempQword = TempQword | ComplementBits;
+        AsmWriteMsr64 (MsrNum + 1, TempQword);
+      }
+    }
+
+    ///
+    /// Enable Cache and set the corresponding MTRR bits
+    ///
+    PostMtrrChange ();
+  }
+}
+
+/**
+  Allocate zeroed pages
+
+  @retval Pointer to the page buffer
+**/
+VOID *
+AllocateZeroedPage (
+  VOID
+  )
+{
+  if (mPageStoreIndex >= mPageStoreSize) {
+    ///
+    /// We are out of space
+    ///
+    return NULL;
+  }
+
+  return (VOID *) (UINTN) &mPageStore[0x1000 * mPageStoreIndex++];
+}
+
+/**
+  Convert 2MB page tables to 4KB page tables
+
+  @param[in] PageAddress             - Page address to convert
+  @param[in] PageDirectoryToConvert  - Page table that will be converted
+**/
+VOID
+Convert2MBPageTo4KPages (
+  IN EFI_PHYSICAL_ADDRESS     PageAddress,
+  IN OUT x64_PAGE_TABLE_ENTRY **PageDirectoryToConvert
+  )
+{
+  UINTN                   Index;
+  EFI_PHYSICAL_ADDRESS    WorkingAddress;
+  x64_PAGE_TABLE_ENTRY_4K *PageTableEntry;
+  x64_PAGE_TABLE_ENTRY    Attributes;
+
+  ///
+  /// Save the attributes of the 2MB table
+  ///
+  Attributes.Page2Mb.Uint64 = (*PageDirectoryToConvert)->Page2Mb.Uint64;
+
+  ///
+  /// Convert PageDirectoryEntry2MB into a 4K Page Directory
+  ///
+  PageTableEntry = AllocateZeroedPage ();
+  if (PageTableEntry == NULL) {
+    return;
+  }
+  (*PageDirectoryToConvert)->Page2Mb.Uint64         = (UINT64) PageTableEntry;
+  (*PageDirectoryToConvert)->Page2Mb.Bits.ReadWrite = 1;
+  (*PageDirectoryToConvert)->Page2Mb.Bits.Present   = 1;
+
+  WorkingAddress = PageAddress;
+  for (Index = 0; Index < 512; Index++, PageTableEntry++, WorkingAddress += 0x1000) {
+    PageTableEntry->Uint64        = (UINT64) WorkingAddress;
+    PageTableEntry->Bits.Present  = 1;
+
+    ///
+    /// Update the new page to have the same attributes as the 2MB page
+    ///
+    PageTableEntry->Bits.ReadWrite      = Attributes.Common.ReadWrite;
+    PageTableEntry->Bits.CacheDisabled  = Attributes.Common.CacheDisabled;
+    PageTableEntry->Bits.WriteThrough   = Attributes.Common.WriteThrough;
+
+    if (WorkingAddress == PageAddress) {
+      ///
+      /// Return back the 4K page that matches the Working addresss
+      ///
+      *PageDirectoryToConvert = (x64_PAGE_TABLE_ENTRY *) PageTableEntry;
+    }
+  }
+}
+
+/**
+  Get current memory mapping information
+
+  @param[in] BaseAddress - get current memory mapping by this Base address
+  @param[in] PageTable   - page table that translated this base address
+  @param[in] Page2MBytes - TRUE if this is 2MBytes page table
+
+  @retval EFI_NOT_FOUND - page table not found
+  @retval EFI_SUCCESS   - page table found
+**/
+EFI_STATUS
+GetCurrentMapping (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  OUT x64_PAGE_TABLE_ENTRY **PageTable,
+  OUT BOOLEAN              *Page2MBytes
+  )
+{
+  UINT64                                    Cr3;
+  x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *PageMapLevel4Entry;
+  x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *PageDirectoryPointerEntry;
+  x64_PAGE_TABLE_ENTRY_2M                   *PageTableEntry2Mb;
+  x64_PAGE_DIRECTORY_ENTRY_4K               *PageDirectoryEntry4k;
+  x64_PAGE_TABLE_ENTRY_4K                   *PageTableEntry4k;
+  UINTN                                     Pml4Index;
+  UINTN                                     PdpIndex;
+  UINTN                                     Pde2MbIndex;
+  UINTN                                     PteIndex;
+
+  Cr3                 = AsmReadCr3 ();
+
+  PageMapLevel4Entry  = (x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *) (Cr3 & 0x000ffffffffff000);
+
+  Pml4Index           = (UINTN) RShiftU64 (BaseAddress, 39) & 0x1ff;
+  if (PageMapLevel4Entry[Pml4Index].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  PageDirectoryPointerEntry = (x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K *) (PageMapLevel4Entry[Pml4Index].Uint64 & 0x000ffffffffff000);
+  PdpIndex = (UINTN) RShiftU64 (BaseAddress, 30) & 0x1ff;
+  if (PageDirectoryPointerEntry[PdpIndex].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  PageTableEntry2Mb = (x64_PAGE_TABLE_ENTRY_2M *) (PageDirectoryPointerEntry[PdpIndex].Uint64 & 0x000ffffffffff000);
+  Pde2MbIndex       = (UINTN) RShiftU64 (BaseAddress, 21) & 0x1ff;
+  if (PageTableEntry2Mb[Pde2MbIndex].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (PageTableEntry2Mb[Pde2MbIndex].Bits.MustBe1 == 1) {
+    ///
+    /// We found a 2MByte page so lets return it
+    ///
+    *Page2MBytes  = TRUE;
+    *PageTable    = (x64_PAGE_TABLE_ENTRY *) &PageTableEntry2Mb[Pde2MbIndex].Uint64;
+    return EFI_SUCCESS;
+  }
+
+  ///
+  /// 4K page so keep walking
+  ///
+  PageDirectoryEntry4k  = (x64_PAGE_DIRECTORY_ENTRY_4K *) &PageTableEntry2Mb[Pde2MbIndex].Uint64;
+
+  PageTableEntry4k      = (x64_PAGE_TABLE_ENTRY_4K *) (PageDirectoryEntry4k[Pde2MbIndex].Uint64 & 0x000ffffffffff000);
+  PteIndex              = (UINTN) RShiftU64 (BaseAddress, 12) & 0x1ff;
+  if (PageTableEntry4k[PteIndex].Bits.Present == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Page2MBytes  = FALSE;
+  *PageTable    = (x64_PAGE_TABLE_ENTRY *) &PageTableEntry4k[PteIndex];
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare memory for essential system tables.
+
+  @retval EFI_SUCCESS              - Memory successfully prepared.
+**/
+EFI_STATUS
+PrepareMemory (
+  VOID
+  )
+{
+  ///
+  /// Allocate space to convert 2MB page tables to 4K tables.
+  ///  This can not be done at call time as the TPL level will
+  ///  not be correct.
+  ///
+  InitailizeCacheAttributes ();
+
+  InitializeExternalVectorTablePtr (mExternalVectorTable);
+  ///
+  /// Initialize the Interrupt Descriptor Table
+  ///
+  InitializeInterruptTables ();
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare Wakeup Buffer and stack for APs.
+
+  @param[in] WakeUpBuffer             - Pointer to the address of wakeup buffer for output.
+  @param[in] StackAddressStart        - Pointer to the stack address of APs for output.
+  @param[in] MaximumCPUsForThisSystem - Maximum CPUs in this system.
+
+  @retval EFI_SUCCESS              - Memory successfully prepared for APs.
+  @retval Other                    - Error occurred while allocating memory.
+**/
+EFI_STATUS
+PrepareMemoryForAPs (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer,
+  OUT VOID                 **StackAddressStart,
+  IN UINTN                 MaximumCPUsForThisSystem
+  )
+{
+  EFI_STATUS              Status;
+  MP_ASSEMBLY_ADDRESS_MAP AddressMap;
+
+  ///
+  /// Release All APs with a lock and wait for them to retire to rendezvous procedure.
+  /// We need a page (4KB) of memory for IA-32 to use broadcast APIs, on a temporary basis.
+  ///
+  Status = AllocateWakeUpBuffer (WakeUpBuffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Claim memory for AP stack
+  ///
+  Status = AllocateReservedMemoryBelow4G (
+            MaximumCPUsForThisSystem * STACK_SIZE_PER_PROC,
+            StackAddressStart
+            );
+
+  if (EFI_ERROR (Status)) {
+    (gBS->FreePages)(*WakeUpBuffer, 1);
+    return Status;
+  }
+
+  AsmGetAddressMap (&AddressMap);
+  CopyMem ((VOID *) (UINTN) *WakeUpBuffer, AddressMap.RendezvousFunnelAddress, AddressMap.Size);
+  *(UINT32 *) (UINTN) (*WakeUpBuffer + AddressMap.FlatJumpOffset + 3) = (UINT32) (*WakeUpBuffer + AddressMap.PModeEntryOffset);
+  *(UINT32 *) (UINTN) (*WakeUpBuffer + AddressMap.LongJumpOffset + 2) = (UINT32) (*WakeUpBuffer + AddressMap.LModeEntryOffset);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare exchange information for APs.
+
+  @param[in] ExchangeInfo      - Pointer to the exchange info buffer for output.
+  @param[in] StackAddressStart - Start address of APs' stacks.
+  @param[in] ApFunction        - Address of function assigned to AP.
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer.
+
+  @retval EFI_SUCCESS       - Exchange Info successfully prepared for APs.
+**/
+EFI_STATUS
+PrepareExchangeInfo (
+  OUT MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  IN VOID                  *StackAddressStart,
+  IN VOID                  *ApFunction,
+  IN EFI_PHYSICAL_ADDRESS  WakeUpBuffer
+  )
+{
+  ZeroMem ((VOID *) ExchangeInfo, EFI_PAGE_SIZE - MP_CPU_EXCHANGE_INFO_OFFSET);
+
+  ExchangeInfo->Lock        = VacantFlag;
+  ExchangeInfo->StackStart  = StackAddressStart;
+  ExchangeInfo->StackSize   = STACK_SIZE_PER_PROC;
+  ExchangeInfo->ApFunction  = ApFunction;
+
+  CopyMem (
+          (VOID *) (UINTN) &ExchangeInfo->GdtrProfile,
+          (VOID *) (UINTN) mAcpiCpuData->GdtrProfile,
+          sizeof (PSEUDO_DESCRIPTOR)
+          );
+  CopyMem (
+          (VOID *) (UINTN) &ExchangeInfo->IdtrProfile,
+          (VOID *) (UINTN) mAcpiCpuData->IdtrProfile,
+          sizeof (PSEUDO_DESCRIPTOR)
+          );
+
+  ExchangeInfo->BufferStart = (UINT32) WakeUpBuffer;
+  ExchangeInfo->Cr3         = (UINT32) (AsmReadCr3 ());
+  ExchangeInfo->InitFlag    = 1;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare Wakeup Buffer and stack for APs.
+
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer for output.
+  @param[in] StackAddressStart - Pointer to the stack address of APs for output.
+
+  @retval EFI_SUCCESS       - Memory successfully prepared for APs.
+**/
+EFI_STATUS
+S3PrepareMemoryForAPs (
+  OUT EFI_PHYSICAL_ADDRESS *WakeUpBuffer,
+  OUT VOID                 **StackAddressStart
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare exchange information for APs.
+
+  @param[in] ExchangeInfo      - Pointer to the exchange info for output.
+  @param[in] StackAddressStart - Start address of APs' stacks.
+  @param[in] ApFunction        - Address of function assigned to AP.
+  @param[in] WakeUpBuffer      - Pointer to the address of wakeup buffer.
+
+  @retval EFI_SUCCESS       - Exchange Info successfully prepared for APs.
+**/
+EFI_STATUS
+S3PrepareExchangeInfo (
+  OUT MP_CPU_EXCHANGE_INFO *ExchangeInfo,
+  IN VOID                  *StackAddressStart,
+  IN VOID                  *ApFunction,
+  IN EFI_PHYSICAL_ADDRESS  WakeUpBuffer
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Dynamically write the far jump destination in APs' wakeup buffer,
+  in order to refresh APs' CS registers for mode switching.
+**/
+VOID
+RedirectFarJump (
+  VOID
+  )
+{
+  MP_ASSEMBLY_ADDRESS_MAP AddressMap;
+
+  AsmGetAddressMap (&AddressMap);
+  *(UINT32 *) (UINTN) (mAcpiCpuData->WakeUpBuffer + AddressMap.FlatJumpOffset + 3) = (UINT32) (mAcpiCpuData->WakeUpBuffer + AddressMap.PModeEntryOffset);
+  *(UINT32 *) (UINTN) (mAcpiCpuData->WakeUpBuffer + AddressMap.LongJumpOffset + 2) = (UINT32) (mAcpiCpuData->WakeUpBuffer + AddressMap.LModeEntryOffset);
+
+  return;
+}
+
+/**
+  Prepare GDTR and IDTR for AP
+
+  @param[in] Gdtr  - The GDTR profile
+  @param[in] Idtr  - The IDTR profile
+
+  @retval EFI_STATUS  - status returned by each sub-routine
+  @retval EFI_SUCCESS - GDTR and IDTR has been prepared for AP
+**/
+EFI_STATUS
+PrepareGdtIdtForAP (
+  OUT PSEUDO_DESCRIPTOR *Gdtr,
+  OUT PSEUDO_DESCRIPTOR *Idtr
+  )
+{
+  INTERRUPT_GATE_DESCRIPTOR *IdtForAP;
+  SEGMENT_DESCRIPTOR        *GdtForAP;
+
+  PSEUDO_DESCRIPTOR *IdtrForBSP;
+  PSEUDO_DESCRIPTOR *GdtrForBSP;
+
+  UINT16     *MceHandler;
+  EFI_STATUS Status;
+
+  AsmGetGdtrIdtr (&GdtrForBSP, &IdtrForBSP);
+
+  ///
+  /// Allocate reserved memory for IDT
+  ///
+  Status = AllocateAlignedReservedMemory (
+            IdtrForBSP->Limit + 1,
+            8,
+            (VOID **) &IdtForAP
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// Allocate reserved memory for GDT
+  ///
+  Status = AllocateAlignedReservedMemory (
+            GdtrForBSP->Limit + 1,
+            8,
+            (VOID **) &GdtForAP
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = (gBS->AllocatePool)(EfiReservedMemoryType, SIZE_OF_MCE_HANDLER, (VOID **) &MceHandler);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ///
+  /// McheHandler content: iret (opcode = 0xcf)
+  ///
+  *MceHandler = 0xCF48;
+
+  CopyMem (GdtForAP, (VOID *) GdtrForBSP->Base, GdtrForBSP->Limit + 1);
+  CopyMem (IdtForAP, (VOID *) IdtrForBSP->Base, IdtrForBSP->Limit + 1);
+
+  IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].Offset15To0   = (UINT16) (UINTN) MceHandler;
+  IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].Offset31To16  = (UINT16) ((UINTN) MceHandler >> 16);
+  IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].Offset63To32  = (UINT32) ((UINTN) MceHandler >> 32);
+
+  ///
+  /// Create Gdtr, IDTR profile
+  ///
+  Gdtr->Base  = (UINTN) GdtForAP;
+  Gdtr->Limit = GdtrForBSP->Limit;
+
+  Idtr->Base  = (UINTN) IdtForAP;
+  Idtr->Limit = IdtrForBSP->Limit;
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpCpu.c b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpCpu.c
new file mode 100644
index 0000000..806dcc0
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpCpu.c
@@ -0,0 +1,89 @@
+/** @file
+  MP Support functions
+
+@copyright
+  Copyright (c) 2007 - 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 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
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGlueDxe.h"
+#include "MpService.h"
+#include EFI_PROTOCOL_DEFINITION (MpService)
+#endif
+
+extern EFI_CPU_MICROCODE_HEADER **mMicrocodePointerBuffer;
+
+ACPI_CPU_DATA  *mAcpiCpuData;
+MP_SYSTEM_DATA *mMPSystemData;
+
+///
+/// Function declarations
+///
+/**
+  Initializes MP support in the system.
+
+  @retval EFI_SUCCESS          - Multiple processors are initialized successfully.
+  @retval EFI_OUT_OF_RESOURCES - No enough resoruces (such as out of memory).
+**/
+EFI_STATUS
+InitializeMpSupport (
+  VOID
+  )
+{
+  EFI_STATUS           Status;
+  MP_CPU_RESERVED_DATA *MpCpuReservedData;
+
+  ///
+  /// Allocate memory for MP CPU related data below 4G
+  ///
+  Status = AllocateReservedMemoryBelow4G (
+            sizeof (MP_CPU_RESERVED_DATA),
+            (VOID **) &MpCpuReservedData
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ZeroMem (MpCpuReservedData, sizeof (MP_CPU_RESERVED_DATA));
+
+  mMPSystemData = &(MpCpuReservedData->MPSystemData);
+  mAcpiCpuData  = &(MpCpuReservedData->AcpiCpuData);
+
+  ///
+  /// Copy microcode to allocated memory
+  ///
+  CopyMem (
+    MpCpuReservedData->MicrocodePointerBuffer,
+    mMicrocodePointerBuffer,
+    sizeof (EFI_CPU_MICROCODE_HEADER *) * (NUMBER_OF_MICROCODE_UPDATE + 1)
+    );
+
+  ///
+  /// Initialize ACPI_CPU_DATA data
+  ///
+  mAcpiCpuData->CpuPrivateData          = (EFI_PHYSICAL_ADDRESS) (UINTN) (&(mMPSystemData->S3DataPointer));
+  mAcpiCpuData->S3BootPath              = FALSE;
+  mAcpiCpuData->MicrocodePointerBuffer  = (EFI_PHYSICAL_ADDRESS) MpCpuReservedData->MicrocodePointerBuffer;
+  mAcpiCpuData->GdtrProfile             = (EFI_PHYSICAL_ADDRESS) & (MpCpuReservedData->GdtrProfile);
+  mAcpiCpuData->IdtrProfile             = (EFI_PHYSICAL_ADDRESS) & (MpCpuReservedData->IdtrProfile);
+
+  ///
+  /// Initialize MP services
+  ///
+  InitializeMpServices ();
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpEqu.inc b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpEqu.inc
new file mode 100644
index 0000000..f7c97ad
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpEqu.inc
@@ -0,0 +1,51 @@
+;@file
+;  @todo ADD DESCRIPTION
+;
+;@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 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
+;
+
+VacantFlag                    Equ   00h
+NotVacantFlag                 Equ   0ffh
+BreakToRunApSignal            Equ   6E755200h
+MonitorFilterSize             Equ   40h
+WakeUpApCounterInit           Equ   0
+WakeUpApPerHltLoop            Equ   1
+WakeUpApPerMwaitLoop          Equ   2
+WakeUpApPerRunLoop            Equ   3
+WakeUpApPerMwaitLoop32        Equ   4
+WakeUpApPerRunLoop32          Equ   5
+
+LockLocation                  equ        1000h - 0400h
+StackStartAddressLocation     equ        LockLocation + 08h
+StackSizeLocation             equ        LockLocation + 10h
+CProcedureLocation            equ        LockLocation + 18h
+GdtrLocation                  equ        LockLocation + 20h
+IdtrLocation                  equ        LockLocation + 2Ah
+BufferStartLocation           equ        LockLocation + 34h
+Cr3OffsetLocation             equ        LockLocation + 38h
+InitFlagLocation              equ        LockLocation + 3Ch
+WakeUpApManner                equ        LockLocation + 40h
+BistBuffer                    equ        LockLocation + 44h
+
+PAUSE32   MACRO
+            DB      0F3h
+            DB      090h
+            ENDM
+
+;-------------------------------------------------------------------------------
+
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpFuncs.asm b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpFuncs.asm
new file mode 100644
index 0000000..58714f8
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpFuncs.asm
@@ -0,0 +1,618 @@
+;
+; This file contains an 'Intel Peripheral Driver' 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                                           
+;
+;-------------------------------------------------------------------------------
+;
+; 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.
+;
+;
+; Module Name:
+;
+;   MpFuncs.asm
+; 
+; Abstract:
+; 
+;   This is the assembly code for EM64T MP support
+;
+;-------------------------------------------------------------------------------
+
+
+include  MpEqu.inc 
+CpuInitFloatPointUnit PROTO C
+
+;-------------------------------------------------------------------------------------
+
+;-------------------------------------------------------------------------------------
+;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);
+
+text      SEGMENT
+
+RendezvousFunnelProc   PROC  PUBLIC
+RendezvousFunnelProcStart::
+
+; At this point CS = 0x(vv00) and ip= 0x0.
+        db 66h,  08bh, 0e8h           ; mov        ebp, eax
+
+        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
+
+; Get APIC ID
+;
+        db 66h,  0B8h
+        dd 00000001h                  ; mov        eax, 1
+        db 0Fh,  0A2h                 ; cpuid
+        db 66h,  0C1h, 0EBh, 18h      ; shr        ebx, 24
+        db 66h,  81h,  0E3h
+        dd 000000FFh                  ; and        ebx, 0ffh                   ; EBX is APIC ID
+
+; If it is the first time AP wakes up, just record AP's BIST
+; Otherwise, switch to protected mode.
+
+        db 0BEh                       ; opcode of mov si, imm16
+        dw InitFlagLocation           ; mov        si,  InitFlag
+        db 66h,  83h, 3Ch, 00h        ; cmp        dword ptr [si], 0
+        db 74h                        ; opcode of jz
+        db flat32Start - ($ + 1)      ; jz         flat32Start
+
+; Record BIST information
+;        
+        db 0B0h, 08h                  ; mov        al,  8
+        db 0F6h, 0E3h                 ; mul        bl
+
+        db 0BEh                       ; opcode of mov si, imm16
+        dw BistBuffer                 ; mov        si,  BistBuffer
+        db 03h,  0F0h                 ; add        si,  ax
+
+        db 66h,  0C7h, 04h
+        dd 00000001h                  ; mov        dword ptr [si], 1           ; Set Valid Flag
+        db 66h,  89h,  6Ch,  04h      ; mov        dword ptr [si + 4], ebp     ; Store BIST value        
+
+;
+; Switch to flat mode.
+;
+flat32Start::
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw BufferStartLocation        ; mov        di, BufferStartLocation
+        db 66h,  8Bh, 35h             ; mov        esi,dword ptr [di]          ; ESI is keeping the start address of wakeup buffer
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw Cr3OffsetLocation          ; mov        di, Cr3Location
+        db 66h,  8Bh, 0Dh             ; mov        ecx,dword ptr [di]          ; ECX is keeping the value of CR3
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw GdtrLocation               ; mov        di, GdtrProfile
+        db 66h                        ; db         66h
+        db 2Eh,  0Fh, 01h, 15h        ; lgdt       fword ptr cs:[di]
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw IdtrLocation               ; mov        di, IdtrProfile
+        db 66h                        ; db         66h
+        db 2Eh,  0Fh, 01h, 1Dh        ; lidt       fword ptr cs:[di]
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw LongModeStartJump - RendezvousFunnelProcStart                       ; Get offset of LongModeStartJump
+        db 66h,  8Bh, 3Dh             ; mov        edi,dword ptr [di]          ; EDI is keeping the LongModeStart Jump Address 
+
+        db 31h,  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) and MP
+        db 0Fh,  22h, 0C0h            ; mov        cr0, eax
+
+FLAT32_JUMP::
+
+        db 66h,  67h, 0EAh            ; far jump
+        dd 0h                         ; 32-bit offset
+        dw 20h                        ; 16-bit selector
+
+NemInit::                             ; 32-bits 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.
+
+
+PrepareToGoLongMode64::
+
+        db 0Fh,  20h,  0E0h           ; mov        eax, cr4
+        db 66h,  0Dh,  020h, 06h      ; or         ax,  0620h                  ; Set PAE=1, OSFXSR=1, OSXMMEXCPT=1.
+        db 0Fh,  22h,  0E0h           ; mov        cr4, eax
+
+        db 0Fh,  22h,  0D9h           ; mov        cr3, ecx
+
+        db 0B9h
+        dd 0C0000080h                 ; mov        ecx, 0c0000080h             ; EFER MSR number.
+        db 0Fh,  32h                  ; rdmsr                                  ; Read EFER.
+        db 0Fh,  0BAh, 0E8h, 08h      ; bts        eax, 8                      ; Set LME=1.
+        db 0Fh,  30h                  ; wrmsr                                  ; Write EFER.
+
+        db 0Fh,  20h,  0C0h           ; mov        eax, cr0                    ; Read CR0.
+        db 0Fh,  0BAh, 0E8h, 1Fh      ; bts        eax, 31                     ; Set PG=1.
+        db 0Fh,  22h,  0C0h           ; mov        cr0, eax                    ; Write CR0.
+
+LONG_JUMP::
+
+        db 67h,  0EAh                 ; far jump
+
+LongModeStartJump:
+
+        dd 0h                         ; 32-bit offset
+        dw 38h                        ; 16-bit selector
+
+
+LongModeStart::
+
+        mov         ax,  30h
+        mov         ds,  ax
+        mov         es,  ax
+        mov         ss,  ax
+
+WaitFirstApTaskAssigned::
+;
+; First INIT-SIPI-SIPI will loop here until DetailedMpInitialization function assigned for each AP
+;
+        pause
+        cmp         qword ptr [esi+CProcedureLocation], 0
+        jz          WaitFirstApTaskAssigned
+
+;
+; Patch Addresses for jumping between RUN and MONITOR MWAIT loops 32-bits and Long Monde Procedure 64-bits
+; Based on the running address of LongModeStart in physic memory which was actually copied by CPU DXE INIT
+;
+        xor         rdx, rdx
+        mov         eax, edi
+        add         eax, RunLoopAndMwaitLoop32 - LongModeStart
+        mov         edx, edi
+        add         edx, RunLoopAndMwaitLoop32Jump - LongModeStart
+        mov         dword ptr [rdx], eax
+
+        mov         rbp, rdx                          ; RBP = 32-bits compatibility mode FAR JUMP m16:32 operand pointer
+
+        mov         eax, edi
+        add         eax, RunLoopAndMwaitLoop64 - LongModeStart
+        mov         edx, edi
+        add         edx, RunLoopAndMwaitLoop64Jump - LongModeStart
+        mov         dword ptr [rdx], eax
+
+; 
+; ProgramStack
+;
+        xor         rcx, rcx
+        mov         edi, esi
+        add         edi, BistBuffer
+        mov         ecx, dword ptr [edi + 8 * ebx]    ; RCX = CpuNumber
+
+        mov         edi, esi
+        add         edi, StackSizeLocation
+        mov         rax, qword ptr [edi]
+        inc         rcx
+        mul         rcx                               ; RAX = StackSize * (CpuNumber + 1)
+
+        mov         edi, esi
+        add         edi, StackStartAddressLocation
+        mov         rdx, qword ptr [edi]
+        add         rax, rdx                          ; RAX = StackStart + StackSize * (CpuNumber + 1)
+
+        mov         rsp, rax
+        sub         rsp, MonitorFilterSize            ; Reserved Monitor data space
+        or          ebx, BreakToRunApSignal           ; ebx = #Cpu run signature
+
+;
+; Call assembly function to initialize FPU.
+;
+        mov         rax, CpuInitFloatPointUnit
+        sub         rsp, 20h
+        call        rax
+        add         rsp, 20h
+        
+;
+; Load C Function pointer and wakeup manner location
+;
+        mov         edi, esi
+        add         edi, CProcedureLocation
+        add         esi, WakeUpApManner               ; esi = WakeUpApManner Address Location
+
+WakeUpThisAp64::
+
+        mov         rax, qword ptr [edi]
+
+        test        rax, rax
+        jz          CheckWakeUpCounterInit64
+
+        push        rbp
+        push        rbx
+        push        rsi
+        push        rdi
+
+        sub         rsp, 20h
+        call        rax
+        add         rsp, 20h
+
+        pop         rdi
+        pop         rsi
+        pop         rbx
+        pop         rbp
+
+CheckWakeUpCounterInit64::
+
+        cmp         dword ptr [esi], WakeUpApCounterInit
+        jnz         CheckWakeUpManner64
+
+;
+; Initialize MONITOR_MWAIT_DATA data structure per thread
+;
+        xor         rcx, rcx
+        mov         qword ptr [rsp +  0], rcx         ; BreakToRunApSignal
+        mov         qword ptr [rsp +  8], rcx         ; HltLoopBreakCounter
+        mov         qword ptr [rsp + 16], rcx         ; MwaitLoopBreakCounter
+        mov         qword ptr [rsp + 24], rcx         ; RunLoopBreakCounter
+        mov         qword ptr [rsp + 32], rcx         ; MwaitLoopBreakCounter32
+        mov         qword ptr [rsp + 40], rcx         ; RunLoopBreakCounter32
+        mov         qword ptr [rsp + 48], rcx         ; WakeUpApVectorChangeFlag
+        mov         qword ptr [rsp + 56], rcx         ; MwaitTargetCstate
+
+WaitWakeUpMannerAssigned::
+
+        pause
+        cmp         dword ptr [esi], WakeUpApCounterInit
+        jz          WaitWakeUpMannerAssigned
+
+CheckWakeUpManner64::
+
+        pause
+        mov         edx, dword ptr [esi]
+        cmp         edx, WakeUpApPerHltLoop
+        jz          HltApLoop64
+
+        cmp         edx, WakeUpApPerMwaitLoop
+        jz          ApMwaitLoop64
+
+        cmp         edx, WakeUpApPerRunLoop
+        jz          CheckRunSignal64
+
+        jmp         JumpToCompatibility32Mode
+
+ApMwaitLoop64::
+
+        cli
+        mov         rax, rsp                          ; Set Monitor Address
+        xor         rcx, rcx
+        xor         rdx, rdx
+        DB          0fh, 1, 0c8h                      ; MONITOR
+        mov         rax, qword ptr [rsp + 56]         ; Mwait Target C-State per rax[7:4]
+        DB          0fh, 1, 0c9h                      ; MWAIT
+
+CheckRunSignal64::
+
+        cmp         qword ptr [rsp], rbx              ; Check if run signal correct?
+        jnz         CheckWakeUpManner64               ; Unknown break, go checking run manner
+
+        jmp         WakeUpThisAp64                    ; Jmp to execute AP task
+
+HltApLoop64::
+
+        cli
+        hlt
+        jmp         HltApLoop64                       ; Jump to halt loop
+
+
+JumpToCompatibility32Mode::
+
+        db 0FFh, 6Dh, 0     ; jmp pword ptr [rbp+0]   ; Far jump to m16:32 for 32-bits compatibility mode
+
+RunLoopAndMwaitLoop32Jump:
+
+        dd 0h                                         ; m32 part of m16:32
+        dw 20h                                        ; m16 part of m16:32
+
+RunLoopAndMwaitLoop32::
+
+        db 66h,  0B8h, 18h,  00h      ; mov        ax,  18h
+        db 66h,  8Eh,  0D8h           ; mov        ds,  ax
+        db 8eh,  0d0h                 ; mov        ss,  ax 
+
+        db 0Fh,  20h,  0C0h           ; mov        eax, cr0                    ; Read CR0.
+        db 0Fh,  0BAh, 0F0h, 1Fh      ; btr        eax, 31                     ; Reset PG=0.
+        db 0Fh,  22h,  0C0h           ; mov        cr0, eax                    ; Write CR0.
+
+        db 0B9h
+        dd 0C0000080h                 ; mov        ecx, 0c0000080h             ; EFER MSR number.
+        db 0Fh,  32h                  ; rdmsr                                  ; Read EFER.
+        db 0Fh,  0BAh, 0F0h, 08h      ; btr        eax, 8                      ; Reset LME=0.
+        db 0Fh,  30h                  ; wrmsr                                  ; Write EFER.
+
+        db 0Fh,  20h,  0E0h           ; mov        eax, cr4
+        db 24h,  0DFh                 ; and        al,  0DFh                   ; Reset PAE=0 in CR4 bit 5
+        db 0Fh,  22h,  0E0h           ; mov        cr4, eax
+
+CheckWakeUpManner32::
+
+        pause
+        cmp         dword ptr [rsi], WakeUpApPerMwaitLoop32                    ; Use rsi for esi per compling in 64-bits mode
+        jnz         CheckRunSignal32
+
+        cli
+        mov         eax, esp                          ; Set Monitor Address
+        xor         ecx, ecx
+        xor         edx, edx
+        DB          0fh, 1, 0c8h                      ; MONITOR
+        mov         eax, dword ptr [rsp + 56]         ; Mwait Target C-State per eax[7:4]
+        DB          0fh, 1, 0c9h                      ; MWAIT
+
+
+CheckRunSignal32::
+
+        cmp         dword ptr [rsp], ebx              ; Check if run signal correct?
+        jnz         CheckWakeUpManner32               ; Unknown break, go checking run manner
+
+        db 0Fh,  20h,  0E0h           ; mov        eax, cr4
+        db 0Ch,  20h                  ; or         al,  20h                    ; Set PAE=1 in CR4 bit 5
+        db 0Fh,  22h,  0E0h           ; mov        cr4, eax
+
+        db 0B9h
+        dd 0C0000080h                 ; mov        ecx, 0c0000080h             ; EFER MSR number.
+        db 0Fh,  32h                  ; rdmsr                                  ; Read EFER.
+        db 0Fh,  0BAh, 0E8h, 08h      ; bts        eax, 8                      ; Set LME=1.
+        db 0Fh,  30h                  ; wrmsr                                  ; Write EFER.
+
+        db 0Fh,  20h,  0C0h           ; mov        eax, cr0                    ; Read CR0.
+        db 0Fh,  0BAh, 0E8h, 1Fh      ; bts        eax, 31                     ; Set PG=1.
+        db 0Fh,  22h,  0C0h           ; mov        cr0, eax                    ; Write CR0.
+
+        db 67h,  0EAh                 ; far jump back to 64-bits long mode
+
+RunLoopAndMwaitLoop64Jump:
+
+        dd 0h                         ; 32-bit offset
+        dw 38h                        ; 16-bit selector
+
+RunLoopAndMwaitLoop64::
+
+        mov         ax,  30h
+        mov         ds,  ax
+        mov         ss,  ax
+
+        jmp         WakeUpThisAp64
+
+RendezvousFunnelProc   ENDP
+RendezvousFunnelProcEnd::
+
+
+;-------------------------------------------------------------------------------------
+;  AsmGetAddressMap (&AddressMap);
+;-------------------------------------------------------------------------------------
+AsmGetAddressMap   PROC   PUBLIC
+
+        mov         rax, offset RendezvousFunnelProcStart
+        mov         qword ptr [rcx], rax
+        mov         qword ptr [rcx+8h], NemInit - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+10h], FLAT32_JUMP - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+18h], LongModeStart - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+20h], LONG_JUMP - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+28h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
+
+        ret
+
+AsmGetAddressMap   ENDP
+
+AsmGetGdtrIdtr   PROC    PUBLIC
+
+        sgdt        GdtDesc
+        lea         rax, GdtDesc
+        mov         [rcx], rax
+
+        sidt        IdtDesc
+        lea         rax, IdtDesc
+        mov         [rdx], rax
+
+        ret
+
+AsmGetGdtrIdtr   ENDP
+
+AsmAcquireMPLock   PROC    PUBLIC
+
+        mov         al, NotVacantFlag
+TryGetLock:
+        xchg        al, byte ptr [rcx]
+        cmp         al, VacantFlag
+        jz          LockObtained
+
+        pause
+        jmp         TryGetLock       
+
+LockObtained:
+        ret
+
+AsmAcquireMPLock   ENDP
+
+AsmReleaseMPLock   PROC    PUBLIC
+
+        mov         al, VacantFlag
+        xchg        al, byte ptr [rcx]
+
+        ret
+
+AsmReleaseMPLock   ENDP
+
+;-------------------------------------------------------------------------------------
+;AsmExchangeRole procedure follows. This procedure executed by current BSP, that is 
+;about to become an AP. It switches it'stack with the current AP.
+;AsmExchangeRole (IN   CPU_EXCHANGE_INFO    *MyInfo, IN   CPU_EXCHANGE_INFO    *OthersInfo);
+;-------------------------------------------------------------------------------------
+CPU_SWITCH_STATE_IDLE          equ        0
+CPU_SWITCH_STATE_STORED        equ        1
+CPU_SWITCH_STATE_LOADED        equ        2
+
+AsmExchangeRole   PROC    PUBLIC
+        ; DO NOT call other functions in this function, since 2 CPU may use 1 stack
+        ; at the same time. If 1 CPU try to call a functiosn, stack will be corrupted.
+
+        push        rax
+        push        rbx
+        push        rcx
+        push        rdx
+        push        rsi
+        push        rdi
+        push        rbp
+        push        r8
+        push        r9
+        push        r10
+        push        r11
+        push        r12
+        push        r13
+        push        r14
+        push        r15
+
+        mov         rax, cr0
+        push        rax
+
+        mov         rax, cr4
+        push        rax
+
+        ; rsi contains MyInfo pointer
+        mov         rsi, rcx
+
+        ; rdi contains OthersInfo pointer
+        mov         rdi, rdx
+
+        ;Store EFLAGS, GDTR and IDTR regiter to stack
+        pushfq
+        sgdt        fword ptr [rsi + 16]
+        sidt        fword ptr [rsi + 26]
+
+        ; Store the its StackPointer
+        mov         qword ptr [rsi + 8], rsp
+
+        ; update its switch state to STORED
+        mov         al, NotVacantFlag
+TryLock1:
+        lock xchg   al, byte ptr [rsi]
+        cmp         al, VacantFlag
+        jz          LockObtained1
+        pause
+        jmp         TryLock1
+
+LockObtained1:
+        mov         byte ptr [rsi + 1], CPU_SWITCH_STATE_STORED
+        lock xchg   al, byte ptr [rsi]
+
+WaitForOtherStored::        
+        ; wait until the other CPU finish storing its state
+        mov         al, NotVacantFlag
+TryLock2:
+        lock xchg   al, byte ptr [rdi]
+        cmp         al, VacantFlag
+        jz          LockObtained2
+        PAUSE32
+        jmp         TryLock2
+
+LockObtained2:
+        mov         bl, byte ptr [rdi + 1]
+        lock xchg   al, byte ptr [rdi]
+        cmp         bl, CPU_SWITCH_STATE_STORED
+        jb          WaitForOtherStored
+
+        ; Since another CPU already stored its state, load them
+        ; load GDTR value
+        lgdt        fword ptr [rdi + 16]
+
+        ; load IDTR value
+        lidt        fword ptr [rdi + 26]
+
+        ; load its future StackPointer
+        mov         rsp, qword ptr [rdi + 8]
+
+        ; update its switch state to LOADED
+        mov         al, NotVacantFlag
+TryLock3:
+        lock xchg   al, byte ptr [rsi]
+        cmp         al, VacantFlag
+        jz          LockObtained3
+        PAUSE32
+        jmp         TryLock3
+
+LockObtained3:
+        mov         byte ptr [rsi+1], CPU_SWITCH_STATE_LOADED
+        lock xchg   al, byte ptr [rsi]
+
+WaitForOtherLoaded::
+        ; wait until the other CPU finish loading new state,
+        ; otherwise the data in stack may corrupt
+        mov         al, NotVacantFlag
+TryLock4:
+        lock xchg   al, byte ptr [rdi]
+        cmp         al, VacantFlag
+        jz          LockObtained4
+        PAUSE32
+        jmp         TryLock4
+
+LockObtained4:
+        mov         bl, byte ptr [rdi+1]
+        lock xchg   al, byte ptr [rdi]
+        cmp         bl, CPU_SWITCH_STATE_LOADED
+        jb          WaitForOtherLoaded
+
+        ; since the other CPU already get the data it want, leave this procedure
+        popfq
+
+        pop         rax
+        mov         cr4, rax
+
+        pop         rax
+        mov         cr0, rax
+
+        pop         r15
+        pop         r14
+        pop         r13
+        pop         r12
+        pop         r11
+        pop         r10
+        pop         r9
+        pop         r8
+        pop         rbp
+        pop         rdi
+        pop         rsi
+        pop         rdx
+        pop         rcx
+        pop         rbx
+        pop         rax
+
+        ret
+AsmExchangeRole   ENDP
+
+GdtDesc             QWORD    0
+                    WORD     0
+
+IdtDesc             QWORD    0
+                    WORD     0
+
+text    ENDS
+
+END
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/ProcessorDef.h b/ReferenceCode/Haswell/CpuInit/Dxe/x64/ProcessorDef.h
new file mode 100644
index 0000000..28e55d7
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/ProcessorDef.h
@@ -0,0 +1,57 @@
+/** @file
+  Definition for EM64T processor
+
+@copyright
+  Copyright (c) 2006 - 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 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 _PROCESSOR_DEF_H
+#define _PROCESSOR_DEF_H
+
+#pragma pack(1)
+
+typedef struct {
+  UINT16  Offset15To0;
+  UINT16  SegmentSelector;
+  UINT16  Attributes;
+  UINT16  Offset31To16;
+  UINT32  Offset63To32;
+  UINT32  Reserved;
+} INTERRUPT_GATE_DESCRIPTOR;
+
+#pragma pack()
+
+typedef struct {
+  UINT8 *RendezvousFunnelAddress;
+  UINTN PModeEntryOffset;
+  UINTN FlatJumpOffset;
+  UINTN LModeEntryOffset;
+  UINTN LongJumpOffset;
+  UINTN Size;
+} MP_ASSEMBLY_ADDRESS_MAP;
+
+VOID
+AsmGetAddressMap (
+  OUT MP_ASSEMBLY_ADDRESS_MAP    *AddressMap
+  )
+/**
+@brief
+  Get address map of RendezvousFunnelProc.
+
+  @param[in] AddressMap  - Output buffer for address map information
+**/
+;
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/VirtualMemory.h b/ReferenceCode/Haswell/CpuInit/Dxe/x64/VirtualMemory.h
new file mode 100644
index 0000000..f9bcc82
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/VirtualMemory.h
@@ -0,0 +1,145 @@
+/** @file
+
+@brief:
+
+  x64 Long Mode Virtual Memory Management Definitions
+
+  References:
+    1) IA-32 Intel(R) Atchitecture Software Developer's Manual Volume 1:Basic Architecture, Intel
+    2) IA-32 Intel(R) Atchitecture Software Developer's Manual Volume 2:Instruction Set Reference, Intel
+    3) IA-32 Intel(R) Atchitecture Software Developer's Manual Volume 3:System Programmer's Guide, Intel
+    4) AMD64 Architecture Programmer's Manual Volume 2: System Programming
+
+@copyright
+  Copyright (c) 2004 - 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 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 _VIRTUAL_MEMORY_H_
+#define _VIRTUAL_MEMORY_H_
+
+#pragma pack(1)
+///
+/// Page-Map Level-4 Offset (PML4) and
+///Page-Directory-Pointer Offset (PDPE) entries 4K & 2MB
+///
+typedef union {
+  struct {
+    UINT64  Present : 1;                /// 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite : 1;              /// 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor : 1;         /// 0 = Supervisor, 1=User
+    UINT64  WriteThrough : 1;           /// 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled : 1;          /// 0 = Cached, 1=Non-Cached
+    UINT64  Accessed : 1;               /// 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Reserved : 1;               /// Reserved
+    UINT64  MustBeZero : 2;             /// Must Be Zero
+    UINT64  Available : 3;              /// Available for use by system software
+    UINT64  PageTableBaseAddress : 40;  /// Page Table Base Address
+    UINT64  AvabilableHigh : 11;        /// Available for use by system software
+    UINT64  Nx : 1;                     /// No Execute bit
+  } Bits;
+  UINT64  Uint64;
+} x64_PAGE_MAP_AND_DIRECTORY_POINTER_2MB_4K;
+
+///
+/// Page-Directory Offset 4K
+///
+typedef union {
+  struct {
+    UINT64  Present : 1;                /// 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite : 1;              /// 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor : 1;         /// 0 = Supervisor, 1=User
+    UINT64  WriteThrough : 1;           /// 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled : 1;          /// 0 = Cached, 1=Non-Cached
+    UINT64  Accessed : 1;               /// 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Reserved : 1;               /// Reserved
+    UINT64  MustBeZero : 1;             /// Must Be Zero
+    UINT64  Reserved2 : 1;              /// Reserved
+    UINT64  Available : 3;              /// Available for use by system software
+    UINT64  PageTableBaseAddress : 40;  /// Page Table Base Address
+    UINT64  AvabilableHigh : 11;        /// Available for use by system software
+    UINT64  Nx : 1;                     /// No Execute bit
+  } Bits;
+  UINT64  Uint64;
+} x64_PAGE_DIRECTORY_ENTRY_4K;
+
+///
+/// Page Table Entry 4K
+///
+typedef union {
+  struct {
+    UINT64  Present : 1;                /// 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite : 1;              /// 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor : 1;         /// 0 = Supervisor, 1=User
+    UINT64  WriteThrough : 1;           /// 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled : 1;          /// 0 = Cached, 1=Non-Cached
+    UINT64  Accessed : 1;               /// 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty : 1;                  /// 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  PAT : 1;                    /// 0 = Ignore Page Attribute Table
+    UINT64  Global : 1;                 /// 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available : 3;              /// Available for use by system software
+    UINT64  PageTableBaseAddress : 40;  /// Page Table Base Address
+    UINT64  AvabilableHigh : 11;        /// Available for use by system software
+    UINT64  Nx : 1;                     /// 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64  Uint64;
+} x64_PAGE_TABLE_ENTRY_4K;
+
+///
+/// Page Table Entry 2MB
+///
+typedef union {
+  struct {
+    UINT64  Present : 1;                /// 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite : 1;              /// 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor : 1;         /// 0 = Supervisor, 1=User
+    UINT64  WriteThrough : 1;           /// 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled : 1;          /// 0 = Cached, 1=Non-Cached
+    UINT64  Accessed : 1;               /// 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty : 1;                  /// 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  MustBe1 : 1;                /// Must be 1
+    UINT64  Global : 1;                 /// 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available : 3;              /// Available for use by system software
+    UINT64  PAT : 1;                    ///
+    UINT64  MustBeZero : 8;             /// Must be zero;
+    UINT64  PageTableBaseAddress : 31;  /// Page Table Base Address
+    UINT64  AvabilableHigh : 11;        /// Available for use by system software
+    UINT64  Nx : 1;                     /// 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64  Uint64;
+} x64_PAGE_TABLE_ENTRY_2M;
+
+typedef union {
+  UINT64  Present : 1;                  /// 0 = Not present in memory, 1 = Present in memory
+  UINT64  ReadWrite : 1;                /// 0 = Read-Only, 1= Read/Write
+  UINT64  UserSupervisor : 1;           /// 0 = Supervisor, 1=User
+  UINT64  WriteThrough : 1;             /// 0 = Write-Back caching, 1=Write-Through caching
+  UINT64  CacheDisabled : 1;            /// 0 = Cached, 1=Non-Cached
+  UINT64  Accessed : 1;                 /// 0 = Not accessed, 1 = Accessed (set by CPU)
+  UINT64  Dirty : 1;                    /// 0 = Not Dirty, 1 = written by processor on access to page
+  UINT64  Reserved : 57;
+} x64_PAGE_TABLE_ENTRY_COMMON;
+
+typedef union {
+  x64_PAGE_TABLE_ENTRY_4K     Page4k;
+  x64_PAGE_TABLE_ENTRY_2M     Page2Mb;
+  x64_PAGE_TABLE_ENTRY_COMMON Common;
+} x64_PAGE_TABLE_ENTRY;
+
+///
+/// BugBug: x64 New stuff
+///
+#pragma pack()
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/BootGuardInit.c b/ReferenceCode/Haswell/CpuInit/Pei/BootGuardInit.c
new file mode 100644
index 0000000..f877784
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/BootGuardInit.c
@@ -0,0 +1,111 @@
+/** @file
+  EFI 2.0 PEIM to initialize the cache and load the BSP microcode
+
+@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 Pre-EFI 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 "CpuInitPeim.h"
+#include "CpuAccess.h"
+#include "BootGuardLibrary.h"
+#include "MeAccess.h"
+#include "HeciRegs.h"
+#endif
+
+VOID
+BootGuardInit (
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN PEI_CPU_PLATFORM_POLICY_PPI  *CpuPlatformPolicyPpi
+  )
+/**
+
+  Perform the platform spefific initializations.
+
+  @param[in] PeiServices          - Indirect reference to the PEI Services Table.
+  @param[in] CpuPlatformPolicyPpi - Platform Policy PPI
+
+**/
+{
+  UINT32  MsrValue;
+  UINT32  MeFwSts4;
+  UINT32  BootGuardAcmStatus;
+
+  if (CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig == NULL) {
+    return;
+  }
+
+  ///
+  /// Check if System Supports Boot Guard
+  ///
+  if( IsBootGuardSupported() ) {
+    CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->BootGuardSupport = TRUE;
+
+    BootGuardAcmStatus  = *(UINT32 *) (UINTN) (TXT_PUBLIC_BASE + R_CPU_BOOT_GUARD_ACM_STATUS);
+    DEBUG ((EFI_D_INFO, "Boot Guard ACM Status = %x\n", BootGuardAcmStatus));
+
+    ///
+    /// Check Bit 12 in ME FWSTS4 to check if TPM_DISCONNECT_ALL bit is set
+    /// or ENF Shutdown path is taken by ME FW.
+    /// Also Identify any failures in ACM
+    ///
+    MeFwSts4 = HeciPciRead32(R_ME_HFS_4);
+    DEBUG ((EFI_D_INFO, "ME FW STS 4 = %x\n", MeFwSts4));
+    if((MeFwSts4 & (B_TPM_DISCONNECT | B_BOOT_GUARD_ENF_MASK)) || (BootGuardAcmStatus & B_BOOT_GUARD_ACM_ERRORCODE_MASK)) {
+      DEBUG ((EFI_D_INFO, "All TPM's on Platform are Disconnected\n"));
+      CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->DisconnectAllTpms = TRUE;
+    }
+    CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->BypassTpmInit = FALSE;
+    CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->MeasuredBoot = FALSE;
+
+    if(MeFwSts4 & BIT10) {
+      DEBUG ((EFI_D_INFO, "Sx Resume Type Identified - TPM Event Log not required for ACM Measurements\n"));
+      CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->ByPassTpmEventLog = TRUE;
+    }
+    ///
+    /// Check bit 0 of BOOT_GUARD_SACM_INFO MSR if system is in Boot Guard boot mode
+    ///
+    MsrValue = (UINT32) AsmReadMsr64 (MSR_BOOT_GUARD_SACM_INFO);
+    DEBUG ((EFI_D_INFO, "MSR_BOOT_GUARD_SACM_INFO MSR = %x\n", MsrValue));
+    if ( (MsrValue & B_NEM_INIT) == 0 ) {
+      DEBUG ((EFI_D_INFO, "NEM is not initiated by Boot Guard ACM\n"));
+    }
+    if (MsrValue & B_MEASURED) {
+      CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->MeasuredBoot = TRUE;
+      ///
+      /// if measured bit is set, BIOS needs to bypass startup command
+      ///
+      if (MsrValue & B_TPM_SUCCESS) {
+        CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->BypassTpmInit = TRUE;
+      }
+      ///
+      /// Read present TPM type
+      ///
+      CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->TpmType = (TPM_TYPE) ( (MsrValue & V_TPM_PRESENT_MASK) >> 1 );
+      DEBUG ((EFI_D_INFO, "TPM Type is %x\n", CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->TpmType));
+    }
+  } else {
+    CpuPlatformPolicyPpi->SecurityConfig->BootGuardConfig->BootGuardSupport = FALSE;
+  }
+
+  return;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CachePeim.c b/ReferenceCode/Haswell/CpuInit/Pei/CachePeim.c
new file mode 100644
index 0000000..959c58e
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CachePeim.c
@@ -0,0 +1,1045 @@
+/** @file
+  EFI 2.0 PEIM to initialize the cache and load the BSP microcode
+
+@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 Pre-EFI 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
+
+**/
+#if !defined(EDK_RELEASE_VERSION) || (EDK_RELEASE_VERSION < 0x00020000)
+#include "EdkIIGluePeim.h"
+#include "CpuAccess.h"
+#include "CpuInitPeim.h"
+#include EFI_PPI_PRODUCER (Cache)
+#define ALIGNED_SEED  0x01010101
+#endif
+
+INT8
+CheckDirection (
+  IN UINT64 Input
+  );
+
+UINT64
+PeiPower2MaxMemory (
+  IN UINT64 MemoryLength
+  );
+
+VOID
+EfiDisableCacheMtrr (
+  IN UINT64 *OldMtrr
+  );
+
+VOID
+EfiRecoverCacheMtrr (
+  IN BOOLEAN EnableMtrr,
+  IN UINT64  OldMtrr
+  );
+
+VOID
+EfiProgramMtrr (
+  IN PEI_CACHE_PPI         *This,
+  IN UINTN                 MtrrNumber,
+  IN EFI_PHYSICAL_ADDRESS  MemoryAddress,
+  IN UINT64                MemoryLength,
+  IN EFI_MEMORY_CACHE_TYPE MemoryCacheType,
+  IN UINT64                ValidMtrrAddressMask
+  );
+
+EFI_STATUS
+EFIAPI
+PeiResetCacheAttributes (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_CACHE_PPI    *This
+  );
+
+EFI_STATUS
+EFIAPI
+PeiActivateCache (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_CACHE_PPI    *This
+  );
+
+EFI_STATUS
+EFIAPI
+PeiSetCacheAttributes (
+  IN EFI_PEI_SERVICES      **PeiServices,
+  IN PEI_CACHE_PPI         *This,
+  IN EFI_PHYSICAL_ADDRESS  MemoryAddress,
+  IN UINT64                MemoryLength,
+  IN EFI_MEMORY_CACHE_TYPE MemoryCacheType
+  );
+
+EFI_STATUS
+SearchForExactMtrr (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_CACHE_PPI          *This,
+  IN EFI_PHYSICAL_ADDRESS   MemoryAddress,
+  IN UINT64                 MemoryLength,
+  IN UINT64                 ValidMtrrAddressMask,
+  OUT UINT32                *UsedMsrNum,
+  OUT EFI_MEMORY_CACHE_TYPE *MemoryCacheType
+  );
+
+BOOLEAN
+IsDefaultType (
+  IN EFI_MEMORY_CACHE_TYPE MemoryCacheType
+  );
+
+EFI_STATUS
+DisableCacheAsRam (
+  VOID
+  );
+
+typedef struct _ALIGNED_DWORD {
+  UINT32 High;
+  UINT32 Low;
+} ALIGNED_DWORD;
+
+typedef union _ALIGNED {
+  UINT64        AlignedQword;
+  ALIGNED_DWORD AlignedDword;
+} ALIGNED;
+
+typedef struct {
+  UINT32 Msr;
+  UINT32 BaseAddress;
+  UINT32 Length;
+} FIXED_MTRR;
+
+FIXED_MTRR mFixedMtrrTable[] = {
+  {
+    IA32_MTRR_FIX64K_00000,
+    0,
+    0x10000
+  },
+  {
+    IA32_MTRR_FIX16K_80000,
+    0x80000,
+    0x4000
+  },
+  {
+    IA32_MTRR_FIX16K_A0000,
+    0xA0000,
+    0x4000
+  },
+  {
+    IA32_MTRR_FIX4K_C0000,
+    0xC0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_C8000,
+    0xC8000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_D0000,
+    0xD0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_D8000,
+    0xD8000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_E0000,
+    0xE0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_E8000,
+    0xE8000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_F0000,
+    0xF0000,
+    0x1000
+  },
+  {
+    IA32_MTRR_FIX4K_F8000,
+    0xF8000,
+    0x1000
+  },
+  {
+    0,
+    0x100000,
+    0
+  }
+};
+
+PEI_CACHE_PPI mCachePpi = {
+  PeiSetCacheAttributes,
+  PeiResetCacheAttributes,
+  PeiActivateCache
+};
+
+/**
+  Update MTRR setting to memory buffer
+
+  @param[in] This                  - Current instance of Pei Cache PPI.
+  @param[in] MsrNum                - offset 0-10 maps to Fixed MTRR table
+                                      offset above 0x200 maps to Variable MTRR table
+  @param[in] UpdateValue           - MTRR setting
+  **/
+VOID
+WriteMsrToBuffer (
+  IN PEI_CACHE_PPI *This,
+  IN UINT32        MsrNum,
+  IN UINT64        UpdateValue
+  )
+{
+  CACHE_PPI_INSTANCE *CachePpiInstance;
+  CachePpiInstance = PEI_CACHE_PPI_INSTANCE_FROM_THIS (This);
+  if (MsrNum >= CACHE_VARIABLE_MTRR_BASE) {
+    if ((MsrNum - CACHE_VARIABLE_MTRR_BASE) >= V_MAXIMUM_VARIABLE_MTRR_NUMBER * 2) {
+      ASSERT (FALSE);
+      return;
+    }
+
+    CachePpiInstance->VariableMtrrChanged = TRUE;
+    CachePpiInstance->VariableMtrrValue[MsrNum - CACHE_VARIABLE_MTRR_BASE].Changed  = TRUE;
+    CachePpiInstance->VariableMtrrValue[MsrNum - CACHE_VARIABLE_MTRR_BASE].MsrValue = UpdateValue;
+  } else {
+    if (MsrNum >= V_FIXED_MTRR_NUMBER) {
+      ASSERT (FALSE);
+      return;
+    }
+
+    CachePpiInstance->FixedMtrrChanged                = TRUE;
+    CachePpiInstance->FixedMtrrValue[MsrNum].Changed  = TRUE;
+    CachePpiInstance->FixedMtrrValue[MsrNum].MsrValue = UpdateValue;
+  }
+}
+
+/**
+  Read MTRR from Buffer. If buffer not ready, read from real MSR instead.
+
+  @param[in] This                  - Current instance of Pei Cache PPI.
+  @param[in] MsrNum                - offset 0-10 maps to Fixed MTRR table
+                                     offset above 0x200 maps to Variable MTRR table
+
+    @retval Return MTRR setting
+  **/
+UINT64
+ReadMsrFromBuffer (
+  IN PEI_CACHE_PPI *This,
+  IN UINT32        MsrNum
+  )
+{
+  UINT64             MtrrVal;
+  CACHE_PPI_INSTANCE *CachePpiInstance;
+  CachePpiInstance = PEI_CACHE_PPI_INSTANCE_FROM_THIS (This);
+  if (MsrNum >= CACHE_VARIABLE_MTRR_BASE) {
+    if ((MsrNum - CACHE_VARIABLE_MTRR_BASE) >= V_MAXIMUM_VARIABLE_MTRR_NUMBER * 2) {
+      ASSERT (FALSE);
+      return 0;
+    }
+
+    if (CachePpiInstance->VariableMtrrValue[MsrNum - CACHE_VARIABLE_MTRR_BASE].Changed) {
+      MtrrVal = CachePpiInstance->VariableMtrrValue[MsrNum - CACHE_VARIABLE_MTRR_BASE].MsrValue;
+    } else {
+      MtrrVal = AsmReadMsr64 (MsrNum);
+    }
+  } else {
+    if (MsrNum >= V_FIXED_MTRR_NUMBER) {
+      ASSERT (FALSE);
+      return 0;
+    }
+
+    if (CachePpiInstance->FixedMtrrValue[MsrNum].Changed) {
+      MtrrVal = CachePpiInstance->FixedMtrrValue[MsrNum].MsrValue;
+    } else {
+      MtrrVal = AsmReadMsr64 (mFixedMtrrTable[MsrNum].Msr);
+    }
+  }
+
+  return MtrrVal;
+}
+
+/**
+    Disable cache and its mtrr
+
+    @param[in] OldMtrr - To return the Old MTRR value
+**/
+VOID
+EfiDisableCacheMtrr (
+  OUT UINT64 *OldMtrr
+  )
+{
+  UINT64 TempQword;
+
+  EfiDisableCache ();
+
+  ///
+  /// Disable Cache MTRR
+  ///
+  *OldMtrr  = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+  TempQword = (*OldMtrr) &~B_CACHE_MTRR_VALID &~B_CACHE_FIXED_MTRR_VALID;
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, TempQword);
+  return;
+}
+
+/**
+    Recover cache MTRR
+
+    @param[in] EnableMtrr - Whether to enable the MTRR
+    @param[in] OldMtrr    - The saved old MTRR value to restore when not to
+                            enable the MTRR
+**/
+VOID
+EfiRecoverCacheMtrr (
+  IN BOOLEAN EnableMtrr,
+  IN UINT64  OldMtrr
+  )
+{
+  UINT64 TempQword;
+
+  TempQword = 0;
+
+  ///
+  /// Enable Cache MTRR
+  ///
+  if (EnableMtrr) {
+    TempQword = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+    TempQword |= (B_CACHE_MTRR_VALID | B_CACHE_FIXED_MTRR_VALID);
+  } else {
+    TempQword = OldMtrr;
+  }
+
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, TempQword);
+
+  EfiEnableCache ();
+  return;
+}
+
+/**
+  Programming MTRR according to Memory address, length, and type.
+
+  @param[in] This                 - Pointer to PEI_CACHE_PPI
+  @param[in] MtrrNumber           - the variable MTRR index number
+  @param[in] MemoryAddress        - the address of target memory
+  @param[in] MemoryLength         - the length of target memory
+  @param[in] MemoryCacheType      - the cache type of target memory
+  @param[in] ValidMtrrAddressMask - the MTRR address mask
+**/
+VOID
+EfiProgramMtrr (
+  IN PEI_CACHE_PPI         *This,
+  IN UINT32                MtrrNumber,
+  IN EFI_PHYSICAL_ADDRESS  MemoryAddress,
+  IN UINT64                MemoryLength,
+  IN EFI_MEMORY_CACHE_TYPE MemoryCacheType,
+  IN UINT64                ValidMtrrAddressMask
+  )
+{
+  UINT64 TempQword;
+
+  ///
+  /// MTRR Physical Base
+  ///
+  TempQword = (MemoryAddress & ValidMtrrAddressMask) | MemoryCacheType;
+  WriteMsrToBuffer (This, MtrrNumber, TempQword);
+
+  ///
+  /// MTRR Physical Mask
+  ///
+  TempQword = ~(MemoryLength - 1);
+  WriteMsrToBuffer (This, MtrrNumber + 1, (TempQword & ValidMtrrAddressMask) | B_CACHE_MTRR_VALID);
+
+  return;
+}
+
+/**
+  Calculate max memory of power 2
+
+  @param[in] MemoryLength  - Memory length that will be calculated
+
+  @retval Max memory
+**/
+UINT64
+PeiPower2MaxMemory (
+  IN UINT64 MemoryLength
+  )
+{
+  UINT64 Result;
+  UINT32 *ResultPointer;
+  UINT32 *MemoryLengthPointer;
+  MemoryLengthPointer = (UINT32 *) &MemoryLength;
+  ResultPointer       = (UINT32 *) &Result;
+  Result              = 0;
+  if (MemoryLengthPointer[1] != 0) {
+    ResultPointer[1] = GetPowerOfTwo32 (MemoryLengthPointer[1]);
+  } else {
+    ResultPointer[0] = GetPowerOfTwo32 (MemoryLengthPointer[0]);
+  }
+
+  return Result;
+}
+
+/**
+  Program the unaligned MTRR register.
+
+  @param[in] This                  - Pointer to PEI_CACHE_PPI
+  @param[in] AlignedQword          - The aligned 64-bit cache type.
+  @param[in] MsrNum                - The index of current MTRR.
+  @param[in] UnalignedBase         - Base Address of the current unaligned MTRR.
+  @param[in] UnalignedLimit        - Limit Address of the current unaligned MTRR.
+
+  @retval EFI_SUCCESS           - The unaligned MTRR is set successfully.
+  @retval EFI_DEVICE_ERROR      - The unaligned address is not the multiple of the basic length of MTRR.
+**/
+EFI_STATUS
+PeiProgramUnalignedMtrr (
+  IN PEI_CACHE_PPI *This,
+  IN UINT64        AlignedQword,
+  IN UINTN         MsrNum,
+  IN UINT32        UnalignedBase,
+  IN UINT32        UnalignedLimit
+  )
+{
+  UINT32 UnalignedOffset;
+  UINT64 TempQword;
+  UINT64 Mask;
+  UINT8  ByteShift;
+
+  UnalignedOffset = UnalignedBase - mFixedMtrrTable[MsrNum].BaseAddress;
+  if (UnalignedOffset % mFixedMtrrTable[MsrNum].Length != 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ByteShift = (UINT8) (UnalignedOffset / mFixedMtrrTable[MsrNum].Length);
+  Mask      = ~(LShiftU64 (1, ByteShift * 8) - 1);
+
+  if (UnalignedLimit < mFixedMtrrTable[MsrNum + 1].BaseAddress) {
+    UnalignedOffset = UnalignedLimit - mFixedMtrrTable[MsrNum].BaseAddress;
+    if (UnalignedOffset % mFixedMtrrTable[MsrNum].Length != 0) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    ByteShift = (UINT8) (UnalignedOffset / mFixedMtrrTable[MsrNum].Length);
+    Mask &= LShiftU64 (1, ByteShift * 8) - 1;
+  }
+
+  TempQword = ReadMsrFromBuffer (This, MsrNum) &~Mask;
+  TempQword |= AlignedQword & Mask;
+  WriteMsrToBuffer (This, MsrNum, TempQword);
+  return EFI_SUCCESS;
+}
+
+/**
+  Given the low memory range ( <= 1MB) and cache type, program the MTRRs.
+
+  @param[in] This                  - Current instance of Pei Cache PPI.
+  @param[in] MemoryCacheType       - Cache Type.
+  @param[in] MemoryBase            - Base Address of Memory to program MTRR.
+  @param[in] MemoryLimit           - Limit Address of Memory to program MTRR.
+
+  @retval EFI_SUCCESS           - Low memory MTRR is set successfully.
+  @retval others                - An error occurs when setting Low memory MTRR.
+**/
+EFI_STATUS
+PeiProgramLowMemoryMtrr (
+  IN PEI_CACHE_PPI         *This,
+  IN EFI_MEMORY_CACHE_TYPE MemoryCacheType,
+  IN UINT32                MemoryBase,
+  IN UINT32                MemoryLimit
+  )
+{
+  EFI_STATUS Status;
+  ALIGNED    Aligned;
+  UINTN      MsrNum;
+
+  Status                    = EFI_SUCCESS;
+
+  Aligned.AlignedDword.High = MemoryCacheType * ALIGNED_SEED;
+  Aligned.AlignedDword.Low  = Aligned.AlignedDword.High;
+
+  for (MsrNum = 0; mFixedMtrrTable[MsrNum].BaseAddress < MemoryBase; MsrNum++) {
+    ;
+  }
+
+  if (MemoryBase < mFixedMtrrTable[MsrNum].BaseAddress) {
+    Status = PeiProgramUnalignedMtrr (This, Aligned.AlignedQword, MsrNum - 1, MemoryBase, MemoryLimit);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+  }
+
+  while (MsrNum < V_FIXED_MTRR_NUMBER && MemoryLimit >= mFixedMtrrTable[MsrNum + 1].BaseAddress) {
+    ///
+    /// Program aligned MTRR
+    ///
+    WriteMsrToBuffer (This, MsrNum, Aligned.AlignedQword);
+    MsrNum++;
+  }
+
+  if (MemoryLimit > mFixedMtrrTable[MsrNum].BaseAddress) {
+    Status = PeiProgramUnalignedMtrr (
+                    This,
+                    Aligned.AlignedQword,
+                    MsrNum,
+                    mFixedMtrrTable[MsrNum].BaseAddress,
+                    MemoryLimit
+                    );
+  }
+
+Done:
+  return Status;
+}
+
+/**
+  Given the memory range and cache type, programs the MTRRs.
+
+  @param[in] PeiServices           - General purpose services available to every PEIM.
+  @param[in] This                  - Current instance of Pei Cache PPI.
+  @param[in] MemoryAddress         - Base Address of Memory to program MTRR.
+  @param[in] MemoryLength          - Length of Memory to program MTRR.
+  @param[in] MemoryCacheType       - Cache Type.
+
+  @retval EFI_SUCCESS           - Mtrr are set successfully.
+  @retval EFI_LOAD_ERROR        - No empty MTRRs to use.
+  @retval EFI_INVALID_PARAMETER - The input parameter is not valid.
+  @retval others                - An error occurs when setting MTTR.
+**/
+EFI_STATUS
+EFIAPI
+PeiSetCacheAttributes (
+  IN EFI_PEI_SERVICES      **PeiServices,
+  IN PEI_CACHE_PPI         *This,
+  IN EFI_PHYSICAL_ADDRESS  MemoryAddress,
+  IN UINT64                MemoryLength,
+  IN EFI_MEMORY_CACHE_TYPE MemoryCacheType
+  )
+{
+  EFI_STATUS            Status;
+  UINT32                MsrNum;
+  UINT64                TempQword;
+  UINT32                UsedMsrNum;
+  EFI_MEMORY_CACHE_TYPE UsedMemoryCacheType;
+  UINT64                ValidMtrrAddressMask;
+  EFI_CPUID_REGISTER    FeatureInfo;
+  UINT64                Power2Length[8];
+  UINT64                LengthArray[8];
+  UINTN                 LengthSize;
+  UINTN                 Index;
+  UINTN                 Count;
+  UINT32                Remainder;
+  UINT32                VariableMtrrLimit;
+  UINT32                *TempQwordPointer;
+  UINT32                *Power2LengthPointer;
+
+  TempQwordPointer  = (UINT32 *) &TempQword;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  if (VariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+  ValidMtrrAddressMask = 0x1000000000ULL;
+
+  AsmCpuid (
+          CPUID_EXTENDED_FUNCTION,
+          &FeatureInfo.RegEax,
+          &FeatureInfo.RegEbx,
+          &FeatureInfo.RegEcx,
+          &FeatureInfo.RegEdx
+          );
+  if (FeatureInfo.RegEax >= CPUID_VIR_PHY_ADDRESS_SIZE) {
+    AsmCpuid (
+            CPUID_VIR_PHY_ADDRESS_SIZE,
+            &FeatureInfo.RegEax,
+            &FeatureInfo.RegEbx,
+            &FeatureInfo.RegEcx,
+            &FeatureInfo.RegEdx
+            );
+    ValidMtrrAddressMask = (LShiftU64 ((UINT64) 1, FeatureInfo.RegEax & 0xFF) - 1) & (~(UINT64) 0x0FFF);
+  }
+
+  ///
+  /// Check for invalid parameter
+  ///
+  if ((MemoryAddress &~ValidMtrrAddressMask) != 0 || (MemoryLength &~ValidMtrrAddressMask) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  switch (MemoryCacheType) {
+    case EfiCacheTypeUncacheable:
+    case EfiCacheTypeWriteCombining:
+    case EfiCacheTypeWriteThrough:
+    case EfiCacheTypeWriteProtected:
+    case EfiCacheTypeWriteBack:
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+  }
+
+  ///
+  /// Check if Fixed MTRR
+  ///
+  if ((MemoryAddress + MemoryLength) <= (1 << 20)) {
+    Status = PeiProgramLowMemoryMtrr (
+                    This,
+                    MemoryCacheType,
+                    (UINT32) MemoryAddress,
+                    (UINT32) (MemoryAddress + MemoryLength)
+                    );
+    return Status;
+  }
+
+  ///
+  /// Special case for 1 MB base address
+  ///
+  if (MemoryAddress == 0x100000) {
+    MemoryAddress = 0;
+    MemoryLength += 0x100000;
+  }
+
+  ///
+  /// Split MemoryLength into a sum of power of 2
+  ///
+  ZeroMem (Power2Length, sizeof (Power2Length));
+  LengthSize  = 0;
+  TempQword   = MemoryLength;
+  do {
+    Power2Length[LengthSize] = PeiPower2MaxMemory (TempQword);
+    TempQword -= Power2Length[LengthSize];
+    LengthSize++;
+  } while (TempQword != 0 && LengthSize < 8);
+  if (TempQword != 0) {
+    return EFI_LOAD_ERROR;
+  }
+
+  ///
+  /// Work out an order of splitted power of 2
+  /// so that Base and Length are suitable for MTRR
+  /// setting constraints.
+  ///
+  Count     = 0;
+  TempQword = MemoryAddress;
+  do {
+    for (Index = 0; Index < LengthSize; Index++) {
+      Power2LengthPointer = (UINT32 *) &Power2Length[Index];
+      if (Power2Length[Index] != 0) {
+        if (Power2LengthPointer[1] != 0) {
+          Remainder = (UINT32) TempQword;
+          if (Remainder == 0) {
+            DivU64x32Remainder (
+                    TempQwordPointer[1],
+                    Power2LengthPointer[1],
+                    &Remainder
+                    );
+          }
+        } else {
+          DivU64x32Remainder (TempQword, (UINT32) Power2Length[Index], &Remainder);
+        }
+
+        if (Remainder == 0) {
+          LengthArray[Count] = Power2Length[Index];
+          TempQword += Power2Length[Index];
+          Power2Length[Index] = 0;
+          Count++;
+          break;
+        }
+      }
+    }
+
+    if (Index == LengthSize) {
+      return EFI_LOAD_ERROR;
+    }
+  } while (Count < LengthSize);
+  ///
+  /// Begin setting the MTRR according to the order
+  ///
+  for (Index = 0; Index < LengthSize; Index++, MemoryAddress += MemoryLength) {
+    MemoryLength = LengthArray[Index];
+    ///
+    /// Search if the range attribute has been set before
+    ///
+    Status = SearchForExactMtrr (
+                    PeiServices,
+                    This,
+                    MemoryAddress,
+                    MemoryLength,
+                    ValidMtrrAddressMask,
+                    &UsedMsrNum,
+                    &UsedMemoryCacheType
+                    );
+
+    if (!EFI_ERROR (Status)) {
+      ///
+      /// Compare if it has the same type as current setting
+      ///
+      if (UsedMemoryCacheType != MemoryCacheType) {
+        ///
+        /// Different type
+        ///
+        ///
+        /// Check if the set type is the same as default type
+        ///
+        if (IsDefaultType (MemoryCacheType)) {
+          ///
+          /// Clear the mtrr
+          ///
+          WriteMsrToBuffer (This, UsedMsrNum, 0);
+          WriteMsrToBuffer (This, UsedMsrNum + 1, 0);
+
+        } else {
+          ///
+          /// Modify the mtrr type
+          ///
+          EfiProgramMtrr (
+                  This,
+                  UsedMsrNum,
+                  MemoryAddress,
+                  MemoryLength,
+                  MemoryCacheType,
+                  ValidMtrrAddressMask
+                  );
+        }
+      }
+
+      continue;
+    }
+
+    ///
+    /// Find first unused MTRR
+    ///
+    for (MsrNum = CACHE_VARIABLE_MTRR_BASE; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2); MsrNum += 2) {
+      if (ReadMsrFromBuffer (This, MsrNum + 1) == 0) {
+        break;
+      }
+    }
+    ///
+    /// Check if we ran out of variable-range MTRRs
+    ///
+    if (MsrNum >= (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2)) {
+      return EFI_LOAD_ERROR;
+    }
+
+    EfiProgramMtrr (
+            This,
+            MsrNum,
+            MemoryAddress,
+            MemoryLength,
+            MemoryCacheType,
+            ValidMtrrAddressMask
+            );
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Update MTRR setting from buffer to MSR.  Disable NEM when NEM is not disabled yet.
+
+  @param[in] PeiServices           - General purpose services available to every PEIM.
+  @param[in] This                  - Current instance of Pei Cache PPI.
+
+  @retval EFI_SUCCESS              - Mtrr are set successfully.
+**/
+EFI_STATUS
+EFIAPI
+PeiActivateCache (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_CACHE_PPI    *This
+  )
+{
+  UINT32             VariableMtrrLimit;
+  UINT32             MsrNum;
+  UINT64             OldMtrr;
+  UINT16             Index;
+  CACHE_PPI_INSTANCE *CachePpiInstance;
+  CachePpiInstance  = PEI_CACHE_PPI_INSTANCE_FROM_THIS (This);
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  if (VariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+
+  ///
+  /// Disable NEM when NEM is not disabled yet
+  ///
+  if (!CachePpiInstance->NemDisabledDone) {
+    DisableCacheAsRam ();
+    CachePpiInstance->NemDisabledDone = TRUE;
+  }
+
+  ///
+  /// Disable/Enable cache only when MTRR configuration is changed in MTRR buffer
+  ///
+  if (CachePpiInstance->FixedMtrrChanged || CachePpiInstance->VariableMtrrChanged) {
+    EfiDisableCacheMtrr (&OldMtrr);
+    if (CachePpiInstance->FixedMtrrChanged) {
+      for (Index = 0; Index < V_FIXED_MTRR_NUMBER; Index++) {
+        if (CachePpiInstance->FixedMtrrValue[Index].Changed) {
+          AsmWriteMsr64 (mFixedMtrrTable[Index].Msr, CachePpiInstance->FixedMtrrValue[Index].MsrValue);
+          CachePpiInstance->FixedMtrrValue[Index].Changed = FALSE;
+        }
+      }
+
+      CachePpiInstance->FixedMtrrChanged = FALSE;
+    }
+
+    if (CachePpiInstance->VariableMtrrChanged) {
+      for (MsrNum = CACHE_VARIABLE_MTRR_BASE; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2); MsrNum++) {
+        if (CachePpiInstance->VariableMtrrValue[MsrNum - CACHE_VARIABLE_MTRR_BASE].Changed) {
+          AsmWriteMsr64 (MsrNum, CachePpiInstance->VariableMtrrValue[MsrNum - CACHE_VARIABLE_MTRR_BASE].MsrValue);
+          CachePpiInstance->VariableMtrrValue[MsrNum - CACHE_VARIABLE_MTRR_BASE].Changed = FALSE;
+        }
+
+        CachePpiInstance->VariableMtrrChanged = FALSE;
+      }
+    }
+
+    EfiRecoverCacheMtrr (TRUE, OldMtrr);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+    Reset all the MTRRs to a known state.
+
+    @param[in] PeiServices - General purpose services available to every PEIM.
+    @param[in] This        - Pointer to the instance of the PEI_CACHE_PPI.
+
+    @retval EFI_SUCCESS - All MTRRs have been reset successfully.
+**/
+EFI_STATUS
+EFIAPI
+PeiResetCacheAttributes (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN PEI_CACHE_PPI    *This
+  )
+{
+  UINT32 MsrNum;
+  UINT16 Index;
+  UINT32 VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  if (VariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+
+  Index = 0;
+
+  ///
+  /// Reset Fixed Mtrrs
+  ///
+  while (mFixedMtrrTable[Index].Msr != 0) {
+    WriteMsrToBuffer (This, Index, 0);
+    Index++;
+  }
+
+  ///
+  /// Reset Variable Mtrrs
+  ///
+  for (MsrNum = CACHE_VARIABLE_MTRR_BASE; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2); MsrNum++) {
+    WriteMsrToBuffer (This, MsrNum, 0);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Search the memory cache type for specific memory from MTRR.
+
+  @param[in] PeiServices          - General purpose services available to every PEIM.
+  @param[in] This                 - Current instance of Pei Cache PPI.
+  @param[in] MemoryAddress        - the address of target memory
+  @param[in] MemoryLength         - the length of target memory
+  @param[in] ValidMtrrAddressMask - the MTRR address mask
+  @param[in] UsedMsrNum           - the used MSR number
+  @param[in] UsedMemoryCacheType  - the cache type for the target memory
+
+  @retval EFI_SUCCESS   - The memory is found in MTRR and cache type is returned
+  @retval EFI_NOT_FOUND - The memory is not found in MTRR
+**/
+EFI_STATUS
+SearchForExactMtrr (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_CACHE_PPI          *This,
+  IN EFI_PHYSICAL_ADDRESS   MemoryAddress,
+  IN UINT64                 MemoryLength,
+  IN UINT64                 ValidMtrrAddressMask,
+  OUT UINT32                *UsedMsrNum,
+  OUT EFI_MEMORY_CACHE_TYPE *UsedMemoryCacheType
+  )
+{
+  UINT32 MsrNum;
+  UINT64 TempQword;
+  UINT32 VariableMtrrLimit;
+
+  VariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+  if (VariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+    VariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+  }
+
+  for (MsrNum = CACHE_VARIABLE_MTRR_BASE; MsrNum < (CACHE_VARIABLE_MTRR_BASE + VariableMtrrLimit * 2 - 1); MsrNum += 2) {
+
+    TempQword = ReadMsrFromBuffer (This, MsrNum + 1);
+
+    if ((TempQword & B_CACHE_MTRR_VALID) == 0) {
+      continue;
+    }
+
+    if ((TempQword & ValidMtrrAddressMask) != ((~(MemoryLength - 1)) & ValidMtrrAddressMask)) {
+      continue;
+    }
+
+    TempQword = ReadMsrFromBuffer (This, MsrNum);
+
+    if ((TempQword & ValidMtrrAddressMask) != (MemoryAddress & ValidMtrrAddressMask)) {
+      continue;
+    }
+
+    *UsedMemoryCacheType  = (EFI_MEMORY_CACHE_TYPE) (TempQword & 0xFF);
+    *UsedMsrNum           = MsrNum;
+
+    return EFI_SUCCESS;
+
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Compares provided Cache type to default type
+
+  @param[in] MemoryCacheType - Memory type for testing
+
+  @retval TRUE  - Memory type instance is the default type
+  @retval FALSE - Memory type instance is not the default type
+**/
+BOOLEAN
+IsDefaultType (
+  IN EFI_MEMORY_CACHE_TYPE MemoryCacheType
+  )
+{
+
+  if ((AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE) & 0xFF) != MemoryCacheType) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+}
+
+/**
+  Install CacheInitPpi
+
+  @retval EFI_OUT_OF_RESOURCES - failed to allocate required pool
+**/
+EFI_STATUS
+CacheInitPpiInit (
+  VOID
+  )
+{
+  EFI_STATUS         Status;
+  CACHE_PPI_INSTANCE *CachePpiInstance;
+
+  CachePpiInstance = AllocateZeroPool (sizeof (CACHE_PPI_INSTANCE));
+  ASSERT (CachePpiInstance != NULL);
+  if (CachePpiInstance == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CachePpiInstance->Ppi.SetCache      = PeiSetCacheAttributes;
+  CachePpiInstance->Ppi.ResetCache    = PeiResetCacheAttributes;
+  CachePpiInstance->Ppi.ActivateCache = PeiActivateCache;
+
+  CachePpiInstance->PpiDesc.Flags     = EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST;
+  CachePpiInstance->PpiDesc.Guid      = &gPeiCachePpiGuid;
+  CachePpiInstance->PpiDesc.Ppi       = &CachePpiInstance->Ppi;
+
+  ///
+  /// Install PPI
+  ///
+  Status = PeiServicesInstallPpi (&CachePpiInstance->PpiDesc);
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
+
+VOID
+CacheInvd (
+  VOID
+  );
+
+/**
+  Disable NEM (cache-as-ram)
+
+  @retval EFI_SUCCESS - always return success
+**/
+EFI_STATUS
+DisableCacheAsRam (
+  VOID
+  )
+{
+  UINT64 CacheAsRamMsr;
+  UINT64 McStatus;
+  UINT32 McIndex;
+  UINT32 McCounter;
+  UINT64 TempQword;
+  UINT64 OldMtrr;
+
+  CacheAsRamMsr = AsmReadMsr64 (NO_EVICT_MODE);
+
+  ///
+  /// Check if CAR has already been disabled. We should not
+  /// execute CacheInvd() after cache has been enabled. This
+  /// check will avoid that.
+  ///
+  if ((CacheAsRamMsr & B_NO_EVICT_MODE_RUN) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  CacheInvd ();
+
+  ///
+  /// Step 3: Disable No-Eviction Mode Run State by clearing
+  ///         NO_EVICT_MODE MSR 2E0h bit [1] = 0
+  ///
+  CacheAsRamMsr &= ~B_NO_EVICT_MODE_RUN;
+  AsmWriteMsr64 (NO_EVICT_MODE, CacheAsRamMsr);
+
+  ///
+  /// Step 4: Disable No-Eviction Mode Setup State by clearing
+  ///         NO_EVICT_MODE MSR 2E0h bit [0] = 0
+  ///
+  CacheAsRamMsr &= ~B_NO_EVICT_MODE_SETUP;
+  AsmWriteMsr64 (NO_EVICT_MODE, CacheAsRamMsr);
+
+  ///
+  /// Disable Cache MTRR by cleaning IA32_MTRR_DEF_TYPE.E or IA32_MTRR_DEF_TYPE.GE
+  ///
+  OldMtrr   = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+  TempQword = OldMtrr &~B_CACHE_MTRR_VALID;
+  AsmWriteMsr64 (CACHE_IA32_MTRR_DEF_TYPE, TempQword);
+
+  ///
+  /// After NEM is disabled, BIOS must clear any Machine Check Bank 5-8 errors that may
+  /// have occurred as the result of ... MLC to to LLC Evictions.
+  ///
+  McStatus  = 0;
+  McCounter = (UINT32) (AsmReadMsr64 (IA32_MCG_CAP) & 0x0f);
+  for (McIndex = 5; McIndex < McCounter; McIndex++) {
+    if (McIndex <= 8) {
+      AsmWriteMsr64 (IA32_MC0_STATUS + McIndex * 4, McStatus);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.cif b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.cif
new file mode 100644
index 0000000..f69c12f
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.cif
@@ -0,0 +1,20 @@
+<component>
+	name = "CpuInitPei"
+	category = ModulePart
+	LocalRoot = "ReferenceCode\Haswell\CpuInit\Pei"
+	RefName = "CpuInitPei"
+[files]
+"CpuInitPeim.c"
+"CachePeim.c"
+"CpuInitPeim.h"
+"PfatInit.c"
+"PfatInit.h"
+"CpuOcInit.c"
+"CpuOcInit.h"
+"CpuInitPeim.dxs"
+"CpuInitPeim.inf"
+"CpuInitPei.mak"
+"CpuInitPei.sdl"
+"Ia32\Cpu.asm"  
+"BootGuardInit.c"
+<endComponent>
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.mak b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.mak
new file mode 100644
index 0000000..7258a7c
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.mak
@@ -0,0 +1,76 @@
+# MAK file for the eModule:PowerManagement
+
+EDK : CpuInitPei
+
+BUILD_CpuInitPei_DIR = $(BUILD_DIR)\$(CpuInitPei_DIR)
+
+$(BUILD_DIR)\CpuInitPei.mak : $(CpuInitPei_DIR)\CpuInitPei.cif $(BUILD_RULES)
+    $(CIF2MAK) $(CpuInitPei_DIR)\CpuInitPei.cif $(CIF2MAK_DEFAULTS)
+
+CpuInitPei : $(BUILD_DIR)\CpuInitPei.MAK CpuInitPeiBin 
+
+CpuInitPei_OBJECTS = \
+    $(BUILD_CpuInitPei_DIR)\CpuInitPeim.obj \
+    $(BUILD_CpuInitPei_DIR)\CachePeim.obj \
+    $(BUILD_CpuInitPei_DIR)\CpuOcInit.obj \
+    $(BUILD_CpuInitPei_DIR)\PfatInit.obj \
+    $(BUILD_CpuInitPei_DIR)\ia32\Cpu.obj \
+    $(BUILD_CpuInitPei_DIR)\BootGuardInit.obj
+
+CpuInitPei_MY_INCLUDES= \
+    $(EDK_INCLUDES)\
+    $(PROJECT_CPU_INCLUDES)\
+    $(INTEL_MCH_INCLUDES)\
+    $(ME_INCLUDES)\
+    $(INTEL_PCH_INCLUDES)
+
+CpuInitPei_DEFINES = $(MY_DEFINES)\
+    /D"__EDKII_GLUE_MODULE_ENTRY_POINT__=PeimInitializeCpu"\
+    /D __EDKII_GLUE_BASE_LIB__ \
+    /D __EDKII_GLUE_PEI_MEMORY_ALLOCATION_LIB__ \
+    /D __EDKII_GLUE_BASE_PCI_LIB_PCI_EXPRESS__ \
+    /D __EDKII_GLUE_PEI_SERVICES_TABLE_POINTER_LIB_MM7__ \
+
+CpuInitPei_LIBS =\
+    $(CPU_PPI_LIB)\
+    $(CPUIA32LIB)\
+    $(EdkIIGluePeiServicesLib_LIB) \
+    $(EdkIIGluePeiReportStatusCodeLib_LIB) \
+    $(EfiRuntimeLib_LIB)\
+    $(INTEL_PCH_PROTOCOL_LIB)\
+    $(EFIRUNTIMELIB)\
+    $(EDKFRAMEWORKPPILIB) \
+    $(EdkIIGluePeiMemoryAllocationLib_LIB)\
+    $(EdkIIGluePeiDebugLibReportStatusCode_LIB)\
+    $(BUILD_DIR)\IA32\EdkIIGlueBaseLib.lib\
+    $(IntelPchPpiLib_LIB)\
+    $(EdkIIGlueBaseLibIA32_LIB)\
+    $(EdkIIGluePeiHobLib_LIB) \
+    $(CpuGuidLib_LIB) \
+    $(PEIHOBLIB) \
+    $(EdkIIGlueBaseIoLibIntrinsic_LIB)\
+    $(EdkIIGlueBasePciLibPciExpress_LIB) \
+    $(EdkIIGlueBasePciExpressLib_LIB)\
+    $(PchPlatformPeiLib_LIB)\
+    $(CpuPlatformLib_LIB)\
+    $(OcPlatformLib_LIB)\
+    $(MeLibPpi_LIB)\
+    $(BootGuardLib_LIB)\
+    $(EFISCRIPTLIB)
+
+CpuInitPeiBin : $(CpuInitPei_LIBS) $(HeciPei_LIBS)
+    $(MAKE) /$(MAKEFLAGS) $(EDKIIGLUE_DEFAULTS)\
+    /f $(BUILD_DIR)\CpuInitPei.mak all\
+    NAME=CpuInitPei\
+    MAKEFILE=$(BUILD_DIR)\CpuInitPei.mak \
+    "MY_INCLUDES=$(CpuInitPei_MY_INCLUDES)" \
+    "MY_DEFINES=$(CpuInitPei_DEFINES)"\
+    OBJECTS="$(CpuInitPei_OBJECTS)" \
+    GUID=01359D99-9446-456d-ADA4-50A711C03ADA\
+    ENTRY_POINT=_ModuleEntryPoint \
+    TYPE=PEIM \
+    EDKIIModule=PEIM\
+    DEPEX1=$(CpuInitPei_DIR)\CpuInitPeim.DXS \
+    DEPEX1_TYPE=EFI_SECTION_PEI_DEPEX  \
+    COMPRESS=0
+#-----------------------------------------------------------------------
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.sdl b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.sdl
new file mode 100644
index 0000000..a1abd10
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPei.sdl
@@ -0,0 +1,37 @@
+TOKEN
+	Name  = "CpuInitPei_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  = "CpuInitPei_DIR"
+End
+
+MODULE
+	Help  = "Includes CpuInitPei.mak to Project"
+	File  = "CpuInitPei.mak"
+End
+
+ELINK
+	Name  = "CpuInitPei_INCLUDES"
+	InvokeOrder = ReplaceParent
+End
+
+ELINK
+	Name  = "/I$(CpuInitPei_DIR)"
+	Parent  = "CpuInitPei_INCLUDES"
+	InvokeOrder = AfterParent
+End
+
+ELINK
+	Name  = "$(BUILD_DIR)\CpuInitPei.ffs"
+	Parent  = "FV_BB"
+	InvokeOrder = AfterParent
+End
+
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.c b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.c
new file mode 100644
index 0000000..739e043
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.c
@@ -0,0 +1,1404 @@
+/** @file
+  EFI 2.0 PEIM to initialize the cache and program for unlock processor
+
+Revision History
+
+@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 Pre-EFI 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 "CpuAccess.h"
+#include "CpuInitPeim.h"
+#include "PchAccess.h"
+#include "SaAccess.h"
+#include "PfatInit.h"
+#include "CpuOcInit.h"
+#include "CpuPlatformLib.h"
+
+#include EFI_PPI_DEFINITION (BootMode)
+#include EFI_PPI_DEFINITION (PchInit)
+#include EFI_PPI_CONSUMER   (PchReset)
+#endif
+
+#include EFI_PPI_CONSUMER (SecPlatformInformation)
+#include EFI_GUID_DEFINITION (HtBistHob)
+
+/**
+  This fuction compare Setup Options of ActiveCores & HyperThreading against the CPU Strap
+  and in case of mismatch requests reset
+
+  @param[in] PeiServices - For printouts
+  @param[in] CpuStrapSet - Strap current setting
+  @param[in] CpuPlatformPolicyPpi - Platform Policy PPI
+
+  @retval CPU_RESET_TYPE
+  @retval CPU_ONLY_RESET - if reset is needed
+  @retval NO_RESET - otherwise
+**/
+CPU_RESET_TYPE
+SetActiveCoresAndSmtEnableDisable (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN CPU_STRAP_SET               *CpuStrapSet,
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+  CPU_RESET_TYPE     ResetType;
+  UINT16             TotalThreadsPerCore;
+  UINT32             Data32;
+  UINT32             NumberOfActiveCores;
+  UINT32             NumberOfActiveThreads;
+  EFI_CPUID_REGISTER Cpuid0B = { 0, 0, 0, 0 };
+
+  ResetType = NO_RESET;
+
+  ///
+  /// Read CPUID(0xB) with ECX=0 for number of logical processors per Core.
+  /// This value does NOT change based on Intel HT Technology disable and core disables.
+  ///
+  Cpuid0B.RegEcx = 0;
+  AsmCpuidEx (CPUID_PROCESSOR_TOPOLOGY, 0, NULL, &Cpuid0B.RegEbx, NULL, NULL);
+  TotalThreadsPerCore = (UINT16) Cpuid0B.RegEbx;
+
+  ///
+  /// CORE_THREAD_COUNT - msr 0x35
+  /// Symbol      Name        MSB:LSB  Description
+  /// CoreCount   CoreCount   31:16    The Core Count reflects the enabled cores based
+  ///                                  on the above thread count and the value of threads_
+  ///                                  available (to determine if HT is on) at reset time.
+  ///
+  /// ThreadCount ThreadCount 15:0     The Thread Count reflects the enabled threads based
+  ///                                  on the factory-configured thread count and the value
+  ///                                  of the CSR_DESIRED_CORES register at reset time.
+  ///
+  ///
+  /// Read MSR for Active Core and Thread Count.
+  ///
+  Data32                = (UINT32) AsmReadMsr64 (MSR_CORE_THREAD_COUNT);
+  NumberOfActiveCores   = (UINT32) ((Data32 >> 16) & 0xFFFF);
+  NumberOfActiveThreads = (UINT32) (Data32 & 0xFFFF);
+
+  DEBUG ((EFI_D_INFO, "Number of Active Cores / Threads = %x / %x\n", NumberOfActiveCores, NumberOfActiveThreads));
+
+  if (TotalThreadsPerCore > 1) {
+    ///
+    /// SMT is supported
+    ///
+    DEBUG ((EFI_D_INFO, "SMT is supported\n"));
+    ///
+    /// Check if the configuration of SMT matches the BIOS Setup option.
+    ///
+    if (CpuStrapSet->HtDisabled == CpuPlatformPolicyPpi->CpuConfig->HyperThreading) {
+      ///
+      /// SMT configuration doesn't match BIOS Setup option; update the Strap Set Data and Issue a Warm reset
+      ///
+      DEBUG ((EFI_D_INFO, "SMT configuration doesn't match the setup value\n"));
+      CpuStrapSet->HtDisabled = !CpuPlatformPolicyPpi->CpuConfig->HyperThreading;
+      ResetType |= WARM_RESET;
+    }
+  } else {
+    ///
+    /// SMT is not supported by default fusing.
+    ///
+    DEBUG ((EFI_D_WARN, "SMT is NOT supported\n"));
+    CpuStrapSet->HtDisabled = 1;
+    CpuPlatformPolicyPpi->CpuConfig->HyperThreading = CPU_FEATURE_DISABLE;
+  }
+  ///
+  /// Check if the configuration of "Active Core" matches the BIOS Setup option.
+  ///
+  if (CpuStrapSet->NumberOfActiveCores != CpuPlatformPolicyPpi->CpuConfig->ActiveCoreCount) {
+    DEBUG (
+            (EFI_D_INFO,
+             "NumberOfActiveCores doesn't match the value from Setup = %x / %x\n",
+             CpuStrapSet->NumberOfActiveCores,
+             CpuPlatformPolicyPpi->CpuConfig->ActiveCoreCount)
+            );
+    CpuStrapSet->NumberOfActiveCores = CpuPlatformPolicyPpi->CpuConfig->ActiveCoreCount;
+    ///
+    /// Haswell CPU doesnt require COLD reset to set number of active cores
+    ///
+    ResetType |= WARM_RESET;
+  }
+
+  return ResetType;
+}
+
+/**
+  This fuction compare Setup Options of BIST against the CPU Strap
+  and in case of mismatch requests reset
+
+  @param[in] PeiServices - For printouts
+  @param[in] CpuStrapSet - Strap current setting
+  @param[in] CpuPlatformPolicyPpi - Platform Policy PPI
+
+  @retval CPU_RESET_TYPE
+  @retval CPU_ONLY_RESET - if reset is needed
+  @retval NO_RESET - otherwise
+**/
+CPU_RESET_TYPE
+BistEnableDisable (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN CPU_STRAP_SET               *CpuStrapSet,
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+  if (CpuStrapSet->Bist == CpuPlatformPolicyPpi->CpuConfig->BistOnReset) {
+    return NO_RESET;
+  } else {
+    CpuStrapSet->Bist = CpuPlatformPolicyPpi->CpuConfig->BistOnReset;
+    DEBUG ((EFI_D_INFO, "BIST configuration doesn't match the setup value\n"));
+    return WARM_RESET;
+  }
+}
+
+/**
+  This fuction compare Setup Options of Processor Flex Multiplier against the CPU Strap
+  and in case of mismatch requests reset
+
+  @param[in] PeiServices - For printouts
+  @param[in] CpuStrapSet - Strap current setting
+  @param[in] CpuPlatformPolicyPpi - Platform Policy PPI
+
+  @retval CPU_RESET_TYPE
+  @retval CPU_ONLY_RESET - if reset is needed
+  @retval NO_RESET - otherwise
+**/
+CPU_RESET_TYPE
+ProgramProcessorFlexMultiplier (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN CPU_STRAP_SET               *CpuStrapSet,
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+  UINT64              FlexRatioData;
+  UINT64              CurrentClockFlexRatio;
+  UINT64              MsrData;
+  CPU_RESET_TYPE      ResetType;
+
+  ResetType         = NO_RESET;
+
+  ///
+  /// Perform Flex Ratio if enabled by user in Setup screen
+  ///
+  if (CpuPlatformPolicyPpi->CpuConfig->CpuRatioOverride) {
+    FlexRatioData = (UINT64) (CpuPlatformPolicyPpi->CpuConfig->CpuRatio);
+  } else {
+    ///
+    /// Flex Ratio was set to disable.  Reset to Max Non-Turbo Ratio
+    ///
+    FlexRatioData = 0x0;
+  }
+  DEBUG ((EFI_D_INFO, "Setup Ratio is 0x%X\n", FlexRatioData));
+
+  ///
+  /// Read and save current Flex Ratio data; disregard enable bit (MSR 194h [15:8])
+  ///
+  MsrData               = AsmReadMsr64 (MSR_FLEX_RATIO);
+  CurrentClockFlexRatio = ((RShiftU64 (MsrData, N_FLEX_RATIO)) & 0xFF);
+  DEBUG ((EFI_D_INFO, "Current Flex Ratio from MSR is 0x%X\n", CurrentClockFlexRatio));
+  DEBUG ((EFI_D_INFO, "Current Flex Ratio from CPU Strap: 0x%X\n", CpuStrapSet->FlexRatio));
+  ///
+  /// Check and set Flex Ratio to requested ratio if possible
+  ///
+  if (FlexRatioData == CpuStrapSet->FlexRatio) {
+    ///
+    /// Do nothing, ratio is already set to requested value and enabled
+    ///
+    DEBUG ((EFI_D_INFO, "No need to set Flex Ratio.\n"));
+  } else {
+    ///
+    /// Set Flex Ratio to user selected value
+    ///
+    /// Write new data to Flex Ratio register
+    /// First clear MSR of any previous value
+    ///
+    MsrData &= RATIO_FLEX_CLEAR_MASK;
+
+    ///
+    /// Enter the new data
+    ///
+    MsrData |= (LShiftU64 ((FlexRatioData & 0xFF), N_FLEX_RATIO));
+    MsrData |= B_FLEX_EN;
+    AsmWriteMsr64 (MSR_FLEX_RATIO, MsrData);
+
+    ///
+    /// Set Soft Reset Data for Flex Ratio
+    ///
+    CpuStrapSet->FlexRatio = (UINT32) FlexRatioData;
+
+    ///
+    /// Set RESET flag
+    ///
+    DEBUG ((EFI_D_INFO, "Setting Flex Ratio to 0x%X\n", CpuStrapSet->FlexRatio));
+    ResetType |= WARM_RESET;
+  }
+
+  return ResetType;
+}
+
+static EFI_PEI_NOTIFY_DESCRIPTOR mNotifyList[] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
+    &gPchInitPpiGuid,
+    SetCpuStrap
+  },
+  {
+    (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+    &gEfiPeiMasterBootModePpiGuid,
+    BuildBistHob
+  }
+};
+
+/**
+  Check if VT is fused and disabled by Setup Option so reset is needed
+
+  @param[in] CpuPlatformPolicyPpi - Platform Policy PPI
+
+  @retval CPU_RESET_TYPE
+  @retval NO_RESET - If no reset is needed
+  @retval COLDRESET - otherwise
+**/
+CPU_RESET_TYPE
+CheckVmxIfNeedReset (
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+  UINT64             MsrIa32FeatureControl;
+  BOOLEAN            CurrentVmxState;
+  EFI_CPUID_REGISTER Cpuid01 = { 0, 0, 0, 0 };
+
+  AsmCpuid (CPUID_VERSION_INFO, &Cpuid01.RegEax, &Cpuid01.RegEbx, &Cpuid01.RegEcx, &Cpuid01.RegEdx);
+
+  ///
+  /// Check if VMX is supported
+  ///
+  if ((Cpuid01.RegEcx & B_CPUID_VERSION_INFO_ECX_VME) == 0) {
+    return NO_RESET;
+  }
+
+  MsrIa32FeatureControl = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
+
+  ///
+  /// If unlocked, no reset needed.
+  ///
+  if ((MsrIa32FeatureControl & B_MSR_IA32_FEATURE_CONTROL_LOCK) == 0) {
+    return NO_RESET;
+  }
+
+  CurrentVmxState = (BOOLEAN) ((MsrIa32FeatureControl & B_MSR_IA32_FEATURE_CONTROL_EVT) != 0);
+
+  ///
+  /// Need to reset only if locked and VMX state has to be changed.
+  ///
+  if ((CurrentVmxState == (CpuPlatformPolicyPpi->CpuConfig->VmxEnable)) == CPU_FEATURE_ENABLE) {
+    return NO_RESET;
+  }
+  ///
+  /// We need a power good reset to unlock MSR_IA32_FEATURE_CONTROL.
+  ///
+  return COLDRESET;
+}
+
+/**
+  Set CPU strap setting for feature change
+
+  @param[in] PeiServices       - Indirect reference to the PEI Services Table.
+  @param[in] NotifyDescriptor  - Address of the notification descriptor data structure. Type
+                      EFI_PEI_NOTIFY_DESCRIPTOR is defined above.
+  @param[in] Ppi               - Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS - Function completed successfully
+**/
+EFI_STATUS
+EFIAPI
+SetCpuStrap (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDescriptor,
+  IN VOID                      *Ppi
+  )
+{
+  EFI_STATUS                  Status;
+  CPU_STRAP_SET               CpuStrapSet = { 0 };
+  UINT16                      CpuStrapSetData;
+  volatile CPU_RESET_TYPE     ResetType;
+  PCH_INIT_PPI                *PchInitPpi;
+  PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi;
+  UINT8                       Data;
+
+  ResetType       = NO_RESET;
+  CpuStrapSetData = 0;
+  Data            = 0;
+
+  Status = PeiServicesLocatePpi (
+                  &gPeiCpuPlatformPolicyPpiGuid,
+                  0,
+                  NULL,
+                  (VOID **) &CpuPlatformPolicyPpi
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Locate PchInit Ppi
+  ///
+  Status = PeiServicesLocatePpi (
+                  &gPchInitPpiGuid,
+                  0,
+                  NULL,
+                  (VOID **) &PchInitPpi
+                  );
+  if (Status == EFI_SUCCESS) {
+    ///
+    /// Get Cpu Strap Set Data
+    ///
+    DEBUG ((EFI_D_INFO, "Set PCH CPU Straps \n"));
+    Status                        = PchInitPpi->CpuStrapSet (PeiServices, GetCpuStrapSetData, &CpuStrapSetData);
+    *((UINT16 *) (&CpuStrapSet))  = CpuStrapSetData;
+    DEBUG ((EFI_D_INFO, "Current CPU Strap Data = 0x%04X\n", CpuStrapSetData));
+
+    if (Status == EFI_SUCCESS) {
+      ResetType |= SetActiveCoresAndSmtEnableDisable (PeiServices, &CpuStrapSet, CpuPlatformPolicyPpi);
+      ResetType |= BistEnableDisable (PeiServices, &CpuStrapSet, CpuPlatformPolicyPpi);
+
+      ///
+      /// Perform Flex Ratio if processor is fused to perform Flex Ratio
+      ///
+      if ((AsmReadMsr64 (MSR_FLEX_RATIO) & B_FLEX_EN) == B_FLEX_EN) {
+        ResetType |= ProgramProcessorFlexMultiplier (PeiServices, &CpuStrapSet, CpuPlatformPolicyPpi);
+      }
+
+      if (ResetType != NO_RESET) {
+        CpuStrapSetData = *((UINT16 *) (&CpuStrapSet));
+        DEBUG ((EFI_D_INFO, "New CPU Strap Data = 0x%04X\n", CpuStrapSetData));
+        Status = PchInitPpi->CpuStrapSet (
+                        PeiServices,
+                        SetCpuStrapSetData,
+                        &CpuStrapSetData
+                        );
+        if (EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_INFO, "NO Reset\n"));
+          ResetType = NO_RESET;
+        }
+      }
+    }
+  }
+
+  ResetType |= CheckVmxIfNeedReset (CpuPlatformPolicyPpi);
+
+  if (ResetType != NO_RESET) {
+    ///
+    /// Perform Warm or Cold Reset
+    ///
+    DEBUG (
+            (EFI_D_INFO,
+             "Reset Required. Performing Cold/Warm Reset to read the new strap values - ResetType = %x\n",
+             ResetType)
+            );
+    PerformWarmORColdReset (PeiServices, ResetType);
+  }
+
+  Status = PchInitPpi->CpuStrapSet (
+                  PeiServices,
+                  LockCpuStrapSetData,
+                  &CpuStrapSetData
+                  );
+  DEBUG ((EFI_D_INFO, "No Reset Required\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  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;
+}
+
+/**
+  Based on ResetType, perform warm or cold reset using PCH Reset PPI
+
+  @param[in] PeiServices       - Indirect reference to the PEI Services Table.
+  @param[in] ResetType         - CPU_RESET_TYPE to indicate which reset shoudl be performed.
+
+  @exception EFI_UNSUPPORTED - Reset type unsupported
+  @retval EFI_SUCCESS     - function successfully (system should already reset)
+**/
+EFI_STATUS
+PerformWarmORColdReset (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN CPU_RESET_TYPE   ResetType
+  )
+{
+  PCH_RESET_PPI *PchResetPpi;
+  EFI_STATUS    Status;
+
+  ///
+  /// Loccate Reset PPI
+  ///
+  Status = (*PeiServices)->LocatePpi (
+                  PeiServices,
+                  &gPchResetPpiGuid,
+                  0,
+                  NULL,
+                  (VOID **) &PchResetPpi
+                  );
+
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Perfrom the requested reset using PCH reset PPI
+  ///
+  Status = EFI_SUCCESS;
+  switch (ResetType) {
+    case WARM_RESET:
+      PchResetPpi->Reset (PchResetPpi, RESET_PPI_WARM_RESET);
+      break;
+
+    case COLDRESET:
+      PchResetPpi->Reset (PchResetPpi, RESET_PPI_COLD_RESET);
+      break;
+
+    default:
+      DEBUG ((EFI_D_ERROR, "CpuInitPeim: PerformWarmORColdReset - ResetType %d not supported: \n", ResetType));
+      Status = EFI_UNSUPPORTED;
+      ASSERT_EFI_ERROR (Status);
+  }
+
+  return Status;
+}
+
+/**
+  Initializes XE OR Overclocking support in the processor.
+
+  @param[in] PowerMgmtConfig      Pointer to Policy protocol instance
+
+  @retval EFI_SUCCESS
+**/
+EFI_STATUS
+XeInit (
+  IN OUT POWER_MGMT_CONFIG_PPI *PowerMgmtConfig,
+  IN OUT OVERCLOCKING_CONFIG_PPI *OcConfig
+  )
+{
+  MSR_REGISTER TurboRatioLimit;
+  MSR_REGISTER FlexRatioMsr;
+  MSR_REGISTER TempMsr;
+  MSR_REGISTER RingRatioMsr;
+  UINT8        CoreCount;
+  UINT8        OverclockingBins;
+  UINT8        OneCoreRatioLimit;
+  UINT8        TwoCoreRatioLimit;
+  UINT8        ThreeCoreRatioLimit;
+  UINT8        FourCoreRatioLimit;
+  UINT8        RatioLimitProgrammble;
+  UINT16       MaxBusRatio;
+
+  if ((PowerMgmtConfig->Xe == 0)) {
+    ///
+    /// XE is disabled
+    ///
+    return EFI_SUCCESS;
+  }
+
+  TurboRatioLimit.Qword = AsmReadMsr64 (MSR_TURBO_RATIO_LIMIT);
+  OneCoreRatioLimit     = (UINT8) (TurboRatioLimit.Dwords.Low & B_MSR_TURBO_RATIO_LIMIT_1C);
+  TwoCoreRatioLimit = (UINT8) RShiftU64 (
+                  (TurboRatioLimit.Dwords.Low & B_MSR_TURBO_RATIO_LIMIT_2C),
+                  N_MSR_TURBO_RATIO_LIMIT_2C
+                  );
+  ThreeCoreRatioLimit = (UINT8) RShiftU64 (
+                  (TurboRatioLimit.Dwords.Low & B_MSR_TURBO_RATIO_LIMIT_3C),
+                  N_MSR_TURBO_RATIO_LIMIT_3C
+                  );
+  FourCoreRatioLimit = (UINT8) RShiftU64 (
+                  (TurboRatioLimit.Dwords.Low & B_MSR_TURBO_RATIO_LIMIT_4C),
+                  N_MSR_TURBO_RATIO_LIMIT_4C
+                  );
+
+  ///
+  /// Check if XE capable
+  ///
+  FlexRatioMsr.Qword = AsmReadMsr64 (MSR_FLEX_RATIO);
+
+  ///
+  /// Overclocking availablity
+  ///
+  OverclockingBins = (UINT8) RShiftU64 ((FlexRatioMsr.Dwords.Low & B_OVERCLOCKING_BINS), N_OVERCLOCKING_BINS);
+
+  ///
+  /// Check if Turbo Ratio Limit is programmable
+  ///  Platform Info MSR (0xCE) [28]
+  ///
+  TempMsr.Qword = AsmReadMsr64 (MSR_PLATFORM_INFO);
+  RatioLimitProgrammble = (UINT8) RShiftU64 (
+                  (TempMsr.Qword & B_PLATFORM_INFO_RATIO_LIMIT),
+                  N_PLATFORM_INFO_RATIO_LIMIT
+                  );
+  ///
+  /// Get Maximum Non-Turbo bus ratio (HFM) from Platform Info MSR Bits[15:8]
+  ///
+  MaxBusRatio = TempMsr.Bytes.SecondByte;
+
+  ///
+  /// Check if processor turbo-ratio can be overriden:
+  ///   BWG Section 14.14.2
+  ///     If PLATFORM INFO MSR [28] == 1 and FLEX_RATIO MSR [19:17] != 0
+  ///       Processor with capability to override turbo-ratio detected (either XE or Overclocking part detected)
+  ///
+  if ((RatioLimitProgrammble == 0) || (OverclockingBins == 0)) {
+    ///
+    /// Neither XE nor Overclocking Capable processor.
+    ///
+    DEBUG ((EFI_D_WARN, "WARNING: Trying to configure XE params on a non-XE processor\n"));
+    return EFI_SUCCESS;
+  }
+  ///
+  /// For Overclocking parts, verify ratio overide is within the allowable limits
+  ///
+  if ((RatioLimitProgrammble) && (OverclockingBins < MAX_OVERCLOCKING_BINS)) {
+    if (PowerMgmtConfig->RatioLimit[0] > (OneCoreRatioLimit + OverclockingBins)) {
+      PowerMgmtConfig->RatioLimit[0] = OneCoreRatioLimit + OverclockingBins;
+    }
+
+    if (PowerMgmtConfig->RatioLimit[1] > (TwoCoreRatioLimit + OverclockingBins)) {
+      PowerMgmtConfig->RatioLimit[1] = TwoCoreRatioLimit + OverclockingBins;
+    }
+
+    if (PowerMgmtConfig->RatioLimit[2] > (ThreeCoreRatioLimit + OverclockingBins)) {
+      PowerMgmtConfig->RatioLimit[2] = ThreeCoreRatioLimit + OverclockingBins;
+    }
+
+    if (PowerMgmtConfig->RatioLimit[3] > (FourCoreRatioLimit + OverclockingBins)) {
+      PowerMgmtConfig->RatioLimit[3] = FourCoreRatioLimit + OverclockingBins;
+    }
+  }
+
+  if ((RatioLimitProgrammble)) {
+    ///
+    /// Now initialize turbo ratio limit MSR
+    /// Find the number of active cores and initialize the ratio limits only if they are available.
+    /// Also program the VID value for the new max Turbo ratio by programming Flex Ratio MSR.
+    ///
+    TempMsr.Qword = AsmReadMsr64 (MSR_CORE_THREAD_COUNT);
+    CoreCount     = (UINT8) RShiftU64 (TempMsr.Dwords.Low, N_CORE_COUNT_OFFSET);
+
+    if (PowerMgmtConfig->RatioLimit[0] >= PowerMgmtConfig->RatioLimit[1] &&
+        PowerMgmtConfig->RatioLimit[0] >= PowerMgmtConfig->RatioLimit[2] &&
+        PowerMgmtConfig->RatioLimit[0] >= PowerMgmtConfig->RatioLimit[3] &&
+        PowerMgmtConfig->RatioLimit[1] >= MaxBusRatio &&
+        PowerMgmtConfig->RatioLimit[2] >= MaxBusRatio &&
+        PowerMgmtConfig->RatioLimit[3] >= MaxBusRatio
+        ) {
+      if (CoreCount >= 1) {
+        TurboRatioLimit.Dwords.Low &= ~B_MSR_TURBO_RATIO_LIMIT_1C;
+        TurboRatioLimit.Dwords.Low |= PowerMgmtConfig->RatioLimit[0];
+      }
+
+      if (CoreCount >= 2) {
+        TurboRatioLimit.Dwords.Low &= ~B_MSR_TURBO_RATIO_LIMIT_2C;
+        TurboRatioLimit.Dwords.Low |= LShiftU64 (PowerMgmtConfig->RatioLimit[1], 8);
+      }
+
+      if (CoreCount >= 3) {
+        TurboRatioLimit.Dwords.Low &= ~B_MSR_TURBO_RATIO_LIMIT_3C;
+        TurboRatioLimit.Dwords.Low |= LShiftU64 (PowerMgmtConfig->RatioLimit[2], 16);
+      }
+
+      if (CoreCount >= 4) {
+        TurboRatioLimit.Dwords.Low &= ~B_MSR_TURBO_RATIO_LIMIT_4C;
+        TurboRatioLimit.Dwords.Low |= LShiftU64 (PowerMgmtConfig->RatioLimit[3], 24);
+      }
+
+      AsmWriteMsr64 (MSR_TURBO_RATIO_LIMIT, TurboRatioLimit.Qword);
+    }
+  }
+
+  ///
+  /// For Overclocking parts, if  a non-default ring ratio is specificed, we need to 
+  /// update the ring ratio limit MSR's max limit
+  ///
+  if ((OverclockingBins != 0) && (OcConfig->ClrMaxOcTurboRatio != 0)) {
+    RingRatioMsr.Qword = AsmReadMsr64 (MSR_RING_RATIO_LIMIT);
+    RingRatioMsr.Bytes.FirstByte &= ~MSR_MAX_RING_RATIO_LIMIT_MASK;
+    RingRatioMsr.Bytes.FirstByte |= OcConfig->ClrMaxOcTurboRatio & MSR_MAX_RING_RATIO_LIMIT_MASK;
+    AsmWriteMsr64 (MSR_RING_RATIO_LIMIT, RingRatioMsr.Qword);
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize performance and power management features
+
+  @param[in] CpuPlatformPolicyPpi    - The CPU Platform Policy PPI instance
+**/
+VOID
+ProcessorsPerfPowerInit (
+  IN OUT PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+  EFI_CPUID_REGISTER    Cpuid = { 0, 0, 0, 0 };
+  MSR_REGISTER          TempMsr;
+  UINT8                 TccActivationOffsetProgrammable;
+  UINT16                MaxBusRatio;
+  UINT16                MinBusRatio;
+  UINT32                Remainder;
+  POWER_MGMT_CONFIG_PPI *PowerMgmtConfig;
+  MSR_REGISTER          TurboRatioLimit;
+  UINT8                 OneCoreRatioLimit;
+  MSR_REGISTER          Ia32MiscEnable;
+  CPU_FAMILY            CpuFamilyId;
+  CPU_STEPPING          CpuStepping;
+  UINT8                 TccActivationOffsetMask;
+  UINT32                WriteFivrSpreadCmd;
+  UINT32                ReadFivrSpreadCmd;
+  UINT32                ReadFivrSpreadData;
+  UINT32                WriteFivrSpreadData;
+  UINT32                MailboxStatus;
+
+  PowerMgmtConfig = CpuPlatformPolicyPpi->PowerMgmtConfig;
+  CpuFamilyId = GetCpuFamily();
+  CpuStepping = GetCpuStepping();
+
+  ///
+  /// Initializes Processor Prefetcher
+  ///
+  ProcessorsPrefetcherInitialization (
+          CpuPlatformPolicyPpi->CpuConfig->MlcStreamerPrefetcher,
+          CpuPlatformPolicyPpi->CpuConfig->MlcSpatialPrefetcher
+          );
+
+  TempMsr.Qword = AsmReadMsr64 (MSR_PLATFORM_INFO);
+
+  ///
+  /// Get Tcc Activation Offset Programmable Setting from Platform Info MSR Bits[30]
+  ///
+  TccActivationOffsetProgrammable = (UINT8) RShiftU64 (
+                  (TempMsr.Qword & B_PLATFORM_INFO_PROG_TCC_ACTIVATION_OFFSET),
+                  N_PLATFORM_INFO_PROG_TCC_ACTIVATION_OFFSET
+                  );
+  ///
+  /// Get Maximum Non-Turbo bus ratio (HFM) from Platform Info MSR Bits[15:8]
+  ///
+  MaxBusRatio = TempMsr.Bytes.SecondByte;
+  ///
+  /// Get Maximum Efficiency bus ratio (LFM) from Platform Info MSR Bits[47:40]
+  ///
+  MinBusRatio = TempMsr.Bytes.SixthByte;
+  ///
+  /// Tcc activation offset in temperature target MSR changes from 4 bits [27:24] to 6 bits [29:24] on ULT C step onwards
+  ///
+  if ((CpuFamilyId == EnumCpuHswUlt) && (CpuStepping >= EnumHswUltC0)) {
+    TccActivationOffsetMask = 0x3F;
+  } else {
+    TccActivationOffsetMask = 0xF;
+  }
+  ///
+  /// If User slects TccActivationOffset greater than supported progam  max supported.
+  ///
+  if(PowerMgmtConfig->TccActivationOffset > TccActivationOffsetMask){
+      PowerMgmtConfig->TccActivationOffset = TccActivationOffsetMask;
+    }
+  ///
+  ///   First check if TCC Activation Offset is programmable PLATFORM INFO MSR [30]
+  ///   If TCC Activation Offset is programable, program the TCC Activation offset value
+  ///   from Policy, and the Tcc activation offset programming should be dependency on RESET_CPL done.
+  ///
+  if (TccActivationOffsetProgrammable) {
+    TempMsr.Qword = AsmReadMsr64 (MSR_TEMPERATURE_TARGET);
+    TempMsr.Dwords.Low &= ~(TccActivationOffsetMask << N_MSR_TEMPERATURE_TARGET_TCC_OFFSET_LIMIT);
+    PowerMgmtConfig->TccActivationOffset &= TccActivationOffsetMask;
+    TempMsr.Dwords.Low |= LShiftU64 (PowerMgmtConfig->TccActivationOffset, N_MSR_TEMPERATURE_TARGET_TCC_OFFSET_LIMIT);
+    AsmWriteMsr64 (MSR_TEMPERATURE_TARGET, TempMsr.Qword);
+  }
+  ///
+  /// Program Voltage regulator Current Limit's
+  ///
+  TempMsr.Qword = AsmReadMsr64 (MSR_VR_CURRENT_CONFIG);
+  TempMsr.Dwords.High &= ~PSIX_THRESHOLD_MASK;
+  if (CpuPlatformPolicyPpi->Revision >= PEI_CPU_PLATFORM_POLICY_PPI_REVISION_5) {
+    TempMsr.Dwords.High |= ((PowerMgmtConfig->Psi3Threshold  << 20) | (PowerMgmtConfig->Psi2Threshold  << 10) | PowerMgmtConfig->Psi1Threshold );
+    if ((CpuFamilyId == EnumCpuHswUlt) && (PowerMgmtConfig->VrPSI4enable)) {
+      TempMsr.Dwords.High |= (0x1 << 30);
+    }
+  }
+  if (PowerMgmtConfig->VrCurrentLimit != 0) {
+    ///
+    /// Check the VR_CURRENT_LOCK bit
+    ///
+    if ((TempMsr.Dwords.Low & B_CURRENT_LIMIT_LOCK) == 0) {
+      TempMsr.Dwords.Low &= ~B_CURRENT_LIMIT_MASK;
+      TempMsr.Dwords.Low |= PowerMgmtConfig->VrCurrentLimit;
+    } else {
+      DEBUG ((EFI_D_INFO, "PPM:: VR Current Limit in MSR_VR_CURRENT_CONFIG is locked\n"));
+    }
+  }
+  ///
+  /// Set CURRENT_LIMIT_LOCK bit in VR_Config_MSR
+  ///
+  if (PowerMgmtConfig->VrCurrentLimitLock) {
+    TempMsr.Dwords.Low |= B_CURRENT_LIMIT_LOCK;
+  }
+
+  AsmWriteMsr64 (MSR_VR_CURRENT_CONFIG, TempMsr.Qword);
+
+  if (CpuPlatformPolicyPpi->Revision >= PEI_CPU_PLATFORM_POLICY_PPI_REVISION_4) {
+    TempMsr.Qword = AsmReadMsr64 (MSR_VR_MISC_CONFIG);
+    TempMsr.Dwords.High = RRotU32 (TempMsr.Dwords.High, 8);
+    if (PowerMgmtConfig->VrMiscIoutSlope <= 1023) {
+      ///
+      /// Update IOUT_SLOPE [49:40]
+      /// Must be 0 to 0x3FF (1023 decimal)
+      ///
+      TempMsr.Dwords.High &= ~0x3FF;
+      TempMsr.Dwords.High |= PowerMgmtConfig->VrMiscIoutSlope;
+    }
+    TempMsr.Dwords.High = LRotU32 (TempMsr.Dwords.High, 8);
+    if ((PowerMgmtConfig->VrMiscIoutOffset != 0) && (PowerMgmtConfig->VrMiscIoutOffset <= 625)) {
+      ///
+      /// Update IOUT_OFFSET[39:32]
+      /// MSR encoding = int(value*2^8+0.0625) for positive offsets
+      ///                inv(int(value*2^8+0.0625))+1 for negative offsets
+      /// +0.0625 could be ignored
+      ///
+      PowerMgmtConfig->VrMiscIoutOffset = (UINT16) DivU64x32Remainder (
+                      (UINT64) MultU64x32 (PowerMgmtConfig->VrMiscIoutOffset, (UINT32) LShiftU64 (1, 8)),
+                      10000,
+                      &Remainder
+                      );
+      if (Remainder >= 5000) {
+        PowerMgmtConfig->VrMiscIoutOffset += 1;
+      }
+      if (PowerMgmtConfig->VrMiscIoutOffsetSign == 1) {
+        ///
+        /// This is negative offset
+        ///
+        PowerMgmtConfig->VrMiscIoutOffset = (UINT8) (~PowerMgmtConfig->VrMiscIoutOffset + 1);
+      }
+      TempMsr.Dwords.High &= ~0xFF;
+      TempMsr.Dwords.High |= PowerMgmtConfig->VrMiscIoutOffset;
+    }
+    AsmWriteMsr64 (MSR_VR_MISC_CONFIG, TempMsr.Qword);
+  }
+
+  if (CpuPlatformPolicyPpi->Revision >= PEI_CPU_PLATFORM_POLICY_PPI_REVISION_3) {
+    TempMsr.Qword = AsmReadMsr64 (MSR_VR_MISC_CONFIG);
+    TempMsr.Dwords.Low = RRotU32 (TempMsr.Dwords.Low, N_MSR_VR_MISC_CONFIG_MIN_VID_OFFSET);
+    ///
+    /// Update MIN_VID [31:24]
+    /// Must be 0 to 0xFF (255 decimal) resolution 10mV
+    /// 0 equal to 0mV; 0xFF (255 decimal) equal to 2550mV
+    ///
+    TempMsr.Dwords.Low &= ~B_MSR_VR_MISC_CONFIG_MIN_VID_MASK;
+    TempMsr.Dwords.Low |= PowerMgmtConfig->VrMiscMinVid;
+    TempMsr.Dwords.Low = LRotU32 (TempMsr.Dwords.Low, N_MSR_VR_MISC_CONFIG_MIN_VID_OFFSET);
+
+    ///
+    /// Update IDLE_EXIT_RAMP_RATE[50]
+    ///
+    TempMsr.Qword &= ~B_MSR_VR_MISC_CONFIG_IDLE_EXIT_RAMP_RATE;
+    if (PowerMgmtConfig->VrMiscIdleExitRampRate) {
+      TempMsr.Qword |= B_MSR_VR_MISC_CONFIG_IDLE_EXIT_RAMP_RATE;
+    }
+
+    ///
+    /// Update IDLE_ENTRY_RAMP_RATE[51]
+    ///
+    TempMsr.Qword &= ~B_MSR_VR_MISC_CONFIG_IDLE_ENTRY_RAMP_RATE;
+    if (PowerMgmtConfig->VrMiscIdleEntryRampRate) {
+      TempMsr.Qword |= B_MSR_VR_MISC_CONFIG_IDLE_ENTRY_RAMP_RATE;
+    }
+
+    ///
+    /// Update IDLE_ENTRY_DECAY_ENABLE[52]
+    ///
+    TempMsr.Qword &= ~B_MSR_VR_MISC_CONFIG_IDLE_ENTRY_DECAY_ENABLE;
+    if (PowerMgmtConfig->VrMiscIdleEntryDecayEnable) {
+      TempMsr.Qword |= B_MSR_VR_MISC_CONFIG_IDLE_ENTRY_DECAY_ENABLE;
+    }
+
+    if (CpuFamilyId == EnumCpuHswUlt) {
+      ///
+      /// Update IDLE_ENTRY_DECAY_ENABLE[52]
+      ///
+      TempMsr.Qword &= ~B_MSR_VR_MISC_CONFIG_SLOW_SLEW_RATE_CONFIG_MASK;
+      switch (PowerMgmtConfig->VrMiscSlowSlewRateConfig) {
+        case 1:
+          TempMsr.Qword |= V_MSR_VR_MISC_CONFIG_SLOW_SLEW_RATE_CONFIG_FAST_4;
+          break;
+        case 2:
+          TempMsr.Qword |= V_MSR_VR_MISC_CONFIG_SLOW_SLEW_RATE_CONFIG_FAST_8;
+          break;
+        case 3:
+          TempMsr.Qword |= V_MSR_VR_MISC_CONFIG_SLOW_SLEW_RATE_CONFIG_FAST_16;
+          break;
+        case 0:
+        default:
+          TempMsr.Qword |= V_MSR_VR_MISC_CONFIG_SLOW_SLEW_RATE_CONFIG_FAST_2;
+          break;
+      }
+    }
+    AsmWriteMsr64 (MSR_VR_MISC_CONFIG, TempMsr.Qword);
+
+    if (CpuFamilyId == EnumCpuHswUlt) {
+      TempMsr.Qword = AsmReadMsr64 (MSR_VR_MISC_CONFIG2);
+      ///
+      /// Update FAST_RAMP_VOLTAGE [7:0]
+      /// Must be 0 to 0xFF (255 decimal) resolution 10mV
+      /// 0 equal to 0mV; 0xFF (255 decimal) equal to 2550mV
+      ///
+      TempMsr.Bytes.FirstByte &= ~B_MSR_VR_MISC_CONFIG2_FAST_RAMP_VOLTAGE_MASK;
+      TempMsr.Bytes.FirstByte |= PowerMgmtConfig->VrMisc2FastRampVoltage;
+
+      ///
+      /// Update MIN_C8_VOLTAGE [15:8]
+      /// Must be 0 to 0xFF (255 decimal) resolution 10mV
+      /// 0 equal to 0mV; 0xFF (255 decimal) equal to 2550mV
+      ///
+      TempMsr.Bytes.SecondByte &= ~B_MSR_VR_MISC_CONFIG2_MIN_C8_VOLTAGE_MASK;
+      TempMsr.Bytes.SecondByte |= PowerMgmtConfig->VrMisc2MinC8Voltage;
+
+      AsmWriteMsr64 (MSR_VR_MISC_CONFIG2, TempMsr.Qword);
+    }
+  }
+  ///
+  /// Initializes XE OR Overclocking support
+  ///
+  XeInit (CpuPlatformPolicyPpi->PowerMgmtConfig, CpuPlatformPolicyPpi->OverclockingConfig);
+
+  ///
+  /// Set processor P state to HFM, LFM or TURBO
+  ///
+  AsmCpuid (CPUID_VERSION_INFO, &Cpuid.RegEax, &Cpuid.RegEbx, &Cpuid.RegEcx, &Cpuid.RegEdx);
+  ///
+  /// Check to see if CPU is GV3 capable - GV3 is fused Enabled
+  ///
+  if ((Cpuid.RegEcx & B_CPUID_VERSION_INFO_ECX_EIST) == B_CPUID_VERSION_INFO_ECX_EIST) {
+    Ia32MiscEnable.Qword = AsmReadMsr64 (MSR_IA32_MISC_ENABLE);
+    Ia32MiscEnable.Qword |= B_MSR_IA32_MISC_ENABLE_EIST;
+    AsmWriteMsr64 (MSR_IA32_MISC_ENABLE, Ia32MiscEnable.Qword);
+
+    TempMsr.Qword = AsmReadMsr64 (MSR_IA32_PERF_CTRL);
+    TempMsr.Qword &= ~B_IA32_PERF_CTRLP_STATE_TARGET;
+
+    if ((CpuPlatformPolicyPpi->Revision >= PEI_CPU_PLATFORM_POLICY_PPI_REVISION_3) && (PowerMgmtConfig->BootInLfm == 2)) {
+      ///
+      /// Set processor P state as TURBO_RATIO_LIMIT_1C if available
+      ///
+      AsmCpuid (CPUID_POWER_MANAGEMENT_PARAMS, &Cpuid.RegEax, &Cpuid.RegEbx, &Cpuid.RegEcx, &Cpuid.RegEdx);
+      if (((Cpuid.RegEax & B_CPUID_POWER_MANAGEMENT_EAX_TURBO)  == B_CPUID_POWER_MANAGEMENT_EAX_TURBO) ||
+          ((Ia32MiscEnable.Qword & B_MSR_IA32_MISC_DISABLE_TURBO) == B_MSR_IA32_MISC_DISABLE_TURBO)) {
+        ///
+        /// Temporarily enable Turbo and it will be reset in DXE phase
+        ///
+        Ia32MiscEnable.Qword &= ~B_MSR_IA32_MISC_DISABLE_TURBO;
+        AsmWriteMsr64 (MSR_IA32_MISC_ENABLE, Ia32MiscEnable.Qword);
+        ///
+        /// Set processor P state as TURBO_RATIO_LIMIT_1C
+        ///
+        TurboRatioLimit.Qword = AsmReadMsr64 (MSR_TURBO_RATIO_LIMIT);
+        OneCoreRatioLimit     = (UINT8) (TurboRatioLimit.Dwords.Low & B_MSR_TURBO_RATIO_LIMIT_1C);
+        TempMsr.Qword |= LShiftU64 (OneCoreRatioLimit, N_IA32_PERF_CTRLP_STATE_TARGET);
+      } else {
+        ///
+        /// Turbo is not available, down to HFM
+        ///
+        DEBUG ((EFI_D_INFO, "CPU: Turbo mode is not available, down to HFM mode.\n"));
+        TempMsr.Qword |= LShiftU64 (MaxBusRatio, N_IA32_PERF_CTRLP_STATE_TARGET);
+      }
+    } else if (PowerMgmtConfig->BootInLfm == CPU_FEATURE_DISABLE) {
+      ///
+      /// Set processor P state as HFM
+      ///
+      TempMsr.Qword |= LShiftU64 (MaxBusRatio, N_IA32_PERF_CTRLP_STATE_TARGET);
+    } else {
+      ///
+      /// Set processor P state as LFM
+      ///
+      TempMsr.Qword |= LShiftU64 (MinBusRatio, N_IA32_PERF_CTRLP_STATE_TARGET);
+    }
+
+    AsmWriteMsr64 (MSR_IA32_PERF_CTRL, TempMsr.Qword);
+  }
+
+  ///
+  /// FIVR Spread Spectrum control is available on C0 steppings onward for all CPU families
+  ///
+  if ((CpuFamilyId == EnumCpuHswUlt && CpuStepping >= EnumHswUltC0) ||
+      (CpuFamilyId == EnumCpuHsw && CpuStepping >= EnumHswC0) ||
+      (CpuFamilyId == EnumCpuCrw && CpuStepping >= EnumCrwC0)){
+
+    ///
+    /// FivrSscPercent is specified in 1/10th percent increments. Range is 0-249.
+    ///
+    if (PowerMgmtConfig->FivrSscPercent > MAX_FIVR_SSC_PERCENT) {
+      PowerMgmtConfig->FivrSscPercent = MAX_FIVR_SSC_PERCENT;
+    }
+
+    ///
+    /// Read current FIVR SSC Spread percentage
+    ///
+    ReadFivrSpreadCmd = 0x8000000E;
+    ReadFivrSpreadData = 0;
+    MailboxRead(MAILBOX_TYPE_PCODE, ReadFivrSpreadCmd, &ReadFivrSpreadData, &MailboxStatus);
+
+//(AMI_CHG)>
+	{
+	  UINT32 i = 0;
+	  while (MailboxStatus == PCODE_MAILBOX_CC_VR_INTERFACE_LOCKED) {
+		  MailboxRead(MAILBOX_TYPE_PCODE, ReadFivrSpreadCmd, &ReadFivrSpreadData, &MailboxStatus);
+		  i++;
+		  if (i == 100) break;
+	  }
+	}
+//<(AMI_CHG)
+
+    if (PowerMgmtConfig->FivrSscEnable) {
+      WriteFivrSpreadData = ( ((PowerMgmtConfig->FivrSscPercent)*1024)/NUM_TENTHS_TO_PERCENTAGE) | FIVR_SSC_LOCK_BIT;
+    }
+    else {
+      ///
+      /// If disabling FIVR SSC, send 0% spread and lock interface
+      ///
+
+      WriteFivrSpreadData = FIVR_SSC_LOCK_BIT;
+    }
+
+    DEBUG ((EFI_D_INFO, "FIVR: Read FIVR Spread = %X, Write FIVR Spread = %X\n", ReadFivrSpreadData, WriteFivrSpreadData));
+    ///
+    /// Only update SSC percentage if needed
+    ///
+    if (ReadFivrSpreadData != WriteFivrSpreadData) {
+      DEBUG ((EFI_D_INFO, "FIVR: Update FIVR Spread = %X\n", WriteFivrSpreadData));
+
+      WriteFivrSpreadCmd = 0x8000000F;
+      MailboxWrite(MAILBOX_TYPE_PCODE, WriteFivrSpreadCmd, WriteFivrSpreadData, &MailboxStatus);
+
+//(AMI_CHG)>
+	  {
+	  	UINT32 i = 0;
+	  	while (MailboxStatus == PCODE_MAILBOX_CC_VR_INTERFACE_LOCKED) {
+		  MailboxWrite(MAILBOX_TYPE_PCODE, WriteFivrSpreadCmd, WriteFivrSpreadData, &MailboxStatus);
+		  i++;
+		  if (i == 100) break;
+	  	}
+	  }
+//<(AMI_CHG)
+
+      if (MailboxStatus != PCODE_MAILBOX_CC_SUCCESS) {
+        DEBUG ((EFI_D_ERROR, "FIVR: Error updating FIVR Spread. Mailbox Status = %X\n", MailboxStatus));
+      }
+    }
+  }
+  else {
+    DEBUG ((EFI_D_ERROR, "FIVR: FIVR Spread adjustment requires C0 stepping or greater. Bypassing FIVR Spread flow.\n"));
+  }
+
+  return;
+}
+
+/**
+  Loads the Processor Microcode & Install the Cache PPI
+
+  @param[in] FfsHeader   - Pointer to an alleged FFS file.
+  @param[in] PeiServices - General purpose services available to every PEIM.
+
+  @retval EFI_STATUS - the status code returned from any sub-routine
+**/
+EFI_STATUS
+PeimInitializeCpu (
+  IN EFI_FFS_FILE_HEADER *FfsHeader,
+  IN EFI_PEI_SERVICES    **PeiServices
+  )
+{
+  EFI_STATUS                  Status;
+  PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi;
+
+  Status = PeiServicesLocatePpi (
+                  &gPeiCpuPlatformPolicyPpiGuid,
+                  0,
+                  NULL,
+                  (VOID **) &CpuPlatformPolicyPpi
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+
+  }
+
+  //
+  // Dump Cpu Platform Policy
+  //
+  CpuPeiPolicyDump(CpuPlatformPolicyPpi);
+
+  ///
+  /// Install Cache PPI
+  ///
+  Status = CacheInitPpiInit ();
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Install Notify
+  ///
+  Status = PeiServicesNotifyPpi (&mNotifyList[0]);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Initializes Overclocking support
+  ///
+  CpuOcInit(PeiServices, CpuPlatformPolicyPpi);
+
+  ///
+  /// Initializes Performance and Power Settings
+  ///
+  ProcessorsPerfPowerInit (CpuPlatformPolicyPpi);
+
+  ///
+  /// Init XMM support
+  ///
+  XmmInit ();
+
+  ///
+  /// Pfat Initialization
+  ///
+  PfatInit (PeiServices, CpuPlatformPolicyPpi);
+
+  ///
+  /// Boot Guard Initializatoin
+  ///
+  BootGuardInit (PeiServices, CpuPlatformPolicyPpi);
+  
+  return Status;
+}
+
+/**
+  Build BIST HOB
+
+  @param[in] PeiServices       - Indirect reference to the PEI Services Table.
+  @param[in] NotifyDescriptor  - Address of the notification descriptor data structure. Type
+                      EFI_PEI_NOTIFY_DESCRIPTOR is defined above.
+  @param[in] Ppi               - Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS - Hob built or not necessary
+**/
+EFI_STATUS
+EFIAPI
+BuildBistHob (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDescriptor,
+  IN VOID                      *Ppi
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_BOOT_MODE                    BootMode;
+  EFI_SEC_PLATFORM_INFORMATION_PPI *SecPlatformInformationPpi;
+  UINT64                           InformationSize;
+  SEC_PLATFORM_INFORMATION_RECORD  *SecPlatformInformation;
+  VOID                             *Hob;
+
+  Status = (**PeiServices).GetBootMode (PeiServices, &BootMode);
+  if (!EFI_ERROR (Status) && (BootMode == BOOT_ON_S3_RESUME)) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesLocatePpi (
+                  &gEfiSecPlatformInformationPpiGuid,   ///< GUID
+                  0,                                    ///< INSTANCE
+                  NULL,                                 ///< EFI_PEI_PPI_DESCRIPTOR
+                  (VOID ** ) &SecPlatformInformationPpi ///< PPI
+                  );
+
+  if (Status == EFI_NOT_FOUND) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  InformationSize         = 0;
+  SecPlatformInformation  = NULL;
+  Status = SecPlatformInformationPpi->PlatformInformation (
+                  PeiServices,
+                  &InformationSize,
+                  SecPlatformInformation
+                  );
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    Status = PeiServicesAllocatePool (
+                    (UINTN) InformationSize,
+                    (VOID **) &SecPlatformInformation
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Status = SecPlatformInformationPpi->PlatformInformation (
+                    PeiServices,
+                    &InformationSize,
+                    SecPlatformInformation
+                    );
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Hob = BuildGuidDataHob (
+                  &gEfiHtBistHobGuid,
+                  SecPlatformInformation,
+                  (UINTN) InformationSize
+                  );
+  ASSERT (Hob != NULL);
+
+  return Status;
+}
+
+/**
+  Dump RC CPU and PPM platform policies
+
+  @param[in] CpuPlatformPolicyPpi  - Address of the cpu platform policy ppi.
+**/
+VOID
+CpuPeiPolicyDump (
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+#ifdef EFI_DEBUG
+  UINTN                 Index;
+  CPU_CONFIG_PPI        *CpuConfig;
+  POWER_MGMT_CONFIG_PPI *PowerMgmtConfig;
+  SECURITY_CONFIG_PPI   *SecurityConfig;
+  OVERCLOCKING_CONFIG_PPI *OverclockingConfig;
+
+  CpuConfig = CpuPlatformPolicyPpi->CpuConfig;
+  PowerMgmtConfig = CpuPlatformPolicyPpi->PowerMgmtConfig;
+  SecurityConfig = CpuPlatformPolicyPpi->SecurityConfig;
+  OverclockingConfig = CpuPlatformPolicyPpi->OverclockingConfig;
+
+  DEBUG ((EFI_D_INFO, "\n------------------------ CpuPlatformPolicyPpi Dump Begin -----------------\n"));
+  //
+  // CPU_CONFIG
+  //
+  DEBUG ((EFI_D_INFO, " CPU:: BistOnReset : 0x%X\n", CpuConfig->BistOnReset));
+  DEBUG ((EFI_D_INFO, " CPU:: HyperThreading : 0x%X\n", CpuConfig->HyperThreading));
+  DEBUG ((EFI_D_INFO, " CPU:: CpuRatioOverride : 0x%X\n", CpuConfig->CpuRatioOverride));
+  DEBUG ((EFI_D_INFO, " CPU:: VmxEnable : 0x%X\n", CpuConfig->VmxEnable));
+  DEBUG ((EFI_D_INFO, " CPU:: Pfat Feature: 0x%X\n", CpuConfig->Pfat));
+  DEBUG ((EFI_D_INFO, " CPU:: MlcStreamerPrefetcher : 0x%X\n", CpuConfig->MlcStreamerPrefetcher));
+  DEBUG ((EFI_D_INFO, " CPU:: MlcSpatialPrefetcher : 0x%X\n", CpuConfig->MlcSpatialPrefetcher));
+  DEBUG ((EFI_D_INFO, " CPU:: ActiveCoreCount : 0x%X\n", CpuConfig->ActiveCoreCount));
+  DEBUG ((EFI_D_INFO, " CPU:: CpuRatio : 0x%X\n", CpuConfig->CpuRatio));
+  DEBUG ((EFI_D_INFO, " CPU:: MaxNonTurboRatio : 0x%X\n", CpuConfig->CpuMaxNonTurboRatio));
+  //
+  // POWER_MGMT_CONFIG
+  //
+  DEBUG ((EFI_D_INFO, " PPM:: BootInLfm : 0x%X\n", PowerMgmtConfig->BootInLfm));
+  DEBUG ((EFI_D_INFO, " PPM:: TccActivationOffset : 0x%X\n", PowerMgmtConfig->TccActivationOffset));
+  DEBUG ((EFI_D_INFO, " PPM:: VrCurrentLimit : 0x%X\n", PowerMgmtConfig->VrCurrentLimit));
+  DEBUG ((EFI_D_INFO, " PPM:: VrCurrentLimitLock : 0x%X\n", PowerMgmtConfig->VrCurrentLimitLock));
+  DEBUG ((EFI_D_INFO, " PPM:: Xe : 0x%X\n", PowerMgmtConfig->Xe));
+  DEBUG ((EFI_D_INFO, " PPM:: RatioLimit[4] : 0x%X , 0x%X , 0x%X , 0x%X \n",  PowerMgmtConfig->RatioLimit[0], \
+          PowerMgmtConfig->RatioLimit[1], \
+          PowerMgmtConfig->RatioLimit[2], \
+          PowerMgmtConfig->RatioLimit[3]));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscIoutSlope : 0x%X\n", PowerMgmtConfig->VrMiscIoutSlope));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscIoutOffsetSign : 0x%X\n", PowerMgmtConfig->VrMiscIoutOffsetSign));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscIoutOffset : 0x%X\n", PowerMgmtConfig->VrMiscIoutOffset));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscMinVid : 0x%X\n", PowerMgmtConfig->VrMiscMinVid));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscIdleExitRampRate : 0x%X\n", PowerMgmtConfig->VrMiscIdleExitRampRate));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscIdleEntryRampRate : 0x%X\n", PowerMgmtConfig->VrMiscIdleEntryRampRate));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscIdleEntryDecayEnable : 0x%X\n", PowerMgmtConfig->VrMiscIdleEntryDecayEnable));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMiscSlowSlewRateConfig : 0x%X\n", PowerMgmtConfig->VrMiscSlowSlewRateConfig));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMisc2FastRampVoltage : 0x%X\n", PowerMgmtConfig->VrMisc2FastRampVoltage));
+  DEBUG ((EFI_D_INFO, " PPM:: VrMisc2MinC8Voltage : 0x%X\n", PowerMgmtConfig->VrMisc2MinC8Voltage));
+  DEBUG ((EFI_D_INFO, " PPM:: FivrSscEnable : 0x%X\n", PowerMgmtConfig->FivrSscEnable));
+  DEBUG ((EFI_D_INFO, " PPM:: FivrSscPercent : 0x%X\n", PowerMgmtConfig->FivrSscPercent));
+
+  //
+  // SECURITY_CONFIG : PFAT_CONFIG
+  //
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : Version : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.Version));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : PlatId[16] :\n"));
+  for (Index = 0; Index < 16; Index++) {
+    if (Index == 15) {
+      DEBUG ((EFI_D_INFO, " 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PlatId[Index]));
+    } else {
+      DEBUG ((EFI_D_INFO, " 0x%X ,", SecurityConfig->PfatConfig->Ppdt.PlatId[Index]));
+    }
+  }
+  DEBUG ((EFI_D_INFO, " \n"));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : PkgAttributes : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.PkgAttributes));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : PslMajorVer : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.PslMajorVer));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : PslMinorVer : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.PslMinorVer));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : ScriptSectionSize : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.ScriptSectionSize));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : DataSectionSize : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.DataSectionSize));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : BiosSvn : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.BiosSvn));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : EcSvn : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.EcSvn));
+  DEBUG ((EFI_D_INFO, " PFAT:: PUP_HEADER : VendorSpecific : 0x%X\n", SecurityConfig->PfatConfig->PupHeader.VendorSpecific));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PpdtSize : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PpdtSize));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PpdtMajVer : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PpdtMajVer));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PpdtMinVer : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PpdtMinVer));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PlatId[16] :\n"));
+  for (Index = 0; Index < 16; Index++) {
+    if (Index == 15) {
+      DEBUG ((EFI_D_INFO, " 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PlatId[Index]));
+    } else {
+      DEBUG ((EFI_D_INFO, " 0x%X ,", SecurityConfig->PfatConfig->Ppdt.PlatId[Index]));
+    }
+  }
+  DEBUG ((EFI_D_INFO, " \n"));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PkeySlot0[32] :\n"));
+  for (Index = 0; Index < 32; Index++) {
+    if ((Index == 15) || (Index == 31)) {
+      DEBUG ((EFI_D_INFO, " 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PkeySlot0[Index]));
+    } else {
+      DEBUG ((EFI_D_INFO, " 0x%X ,", SecurityConfig->PfatConfig->Ppdt.PkeySlot0[Index]));
+    }
+  }
+  DEBUG ((EFI_D_INFO, " \n"));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PkeySlot1[32] :\n"));
+  for (Index = 0; Index < 32; Index++) {
+    if ((Index == 15) || (Index == 31)) {
+      DEBUG ((EFI_D_INFO, " 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PkeySlot1[Index]));
+    } else {
+      DEBUG ((EFI_D_INFO, " 0x%X ,", SecurityConfig->PfatConfig->Ppdt.PkeySlot1[Index]));
+    }
+  }
+  DEBUG ((EFI_D_INFO, " \n"));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PkeySlot2[32] :\n"));
+  for (Index = 0; Index < 32; Index++) {
+    if ((Index == 15)|| (Index == 31)) {
+      DEBUG ((EFI_D_INFO, " 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PkeySlot2[Index]));
+    } else {
+      DEBUG ((EFI_D_INFO, " 0x%X ,", SecurityConfig->PfatConfig->Ppdt.PkeySlot2[Index]));
+    }
+  }
+  DEBUG ((EFI_D_INFO, " \n"));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PfatModSvn : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PfatModSvn));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : BiosSvn : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.BiosSvn));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : ExecLim : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.ExecLim));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : PlatAttr : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.PlatAttr));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : EcCmd : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.EcCmd));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : EcData : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.EcData));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : EcCmdGetSvn : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.EcCmdGetSvn));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : EcCmdOpen : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.EcCmdOpen));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : EcCmdClose : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.EcCmdClose));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : EcCmdPortTest : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.EcCmdPortTest));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : LastSfam : 0x%X\n", SecurityConfig->PfatConfig->Ppdt.LastSfam));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : SfamData[64] :\n"));
+  //
+  // Change the array size according to MAX_SFAM_COUNT
+  //
+  for (Index = 0; Index < 64; Index++) {
+    if ((Index == 15) || (Index == 31) || (Index == 47) || (Index == 63) ) {
+      DEBUG ((EFI_D_INFO, " 0x%X\n", SecurityConfig->PfatConfig->Ppdt.SfamData[Index]));
+    } else {
+      DEBUG ((EFI_D_INFO, " 0x%X ,", SecurityConfig->PfatConfig->Ppdt.SfamData[Index]));
+    }
+  }
+  DEBUG ((EFI_D_INFO, " \n"));
+  DEBUG ((EFI_D_INFO, " PFAT:: PpdtHash[4] : 0x%lX , 0x%lX , 0x%lX , 0x%lX \n",   SecurityConfig->PfatConfig->PpdtHash[0], \
+          SecurityConfig->PfatConfig->PpdtHash[1], \
+          SecurityConfig->PfatConfig->PpdtHash[2], \
+          SecurityConfig->PfatConfig->PpdtHash[3]));
+
+  DEBUG ((EFI_D_INFO, " PFAT:: NumSpiComponents : 0x%x\n", SecurityConfig->PfatConfig->NumSpiComponents));
+  DEBUG ((EFI_D_INFO, " PFAT:: PPDT : ComponentSize[8] :\n"));
+  for (Index = 0; Index < SecurityConfig->PfatConfig->NumSpiComponents; Index++) {
+    if (Index == 7) {
+      DEBUG ((EFI_D_INFO, " 0x%X\n", SecurityConfig->PfatConfig->ComponentSize[Index]));
+    } else {
+      DEBUG ((EFI_D_INFO, " 0x%X ,", SecurityConfig->PfatConfig->ComponentSize[Index]));
+    }
+  }
+  DEBUG ((EFI_D_INFO, " \n"));
+  DEBUG ((EFI_D_INFO, " PFAT:: PfatMemSize : 0x%X\n", SecurityConfig->PfatConfig->PfatMemSize));
+  DEBUG ((EFI_D_INFO, " PFAT:: EcCmdDiscovery : 0x%X\n", SecurityConfig->PfatConfig->EcCmdDiscovery));
+  DEBUG ((EFI_D_INFO, " PFAT:: EcCmdProvisionEav : 0x%X\n", SecurityConfig->PfatConfig->EcCmdProvisionEav));
+  DEBUG ((EFI_D_INFO, " PFAT:: EcCmdLock : 0x%X\n", SecurityConfig->PfatConfig->EcCmdLock));
+  DEBUG ((EFI_D_INFO, " PFAT:: PFATLOG:: Version : 0x%X\n", SecurityConfig->PfatConfig->PfatLog.Version));
+  DEBUG ((EFI_D_INFO, " PFAT:: PFATLOG:: LastPage : 0x%X\n", SecurityConfig->PfatConfig->PfatLog.LastPage));
+  DEBUG ((EFI_D_INFO, " PFAT:: PFATLOG:: LoggingOptions : 0x%X\n", SecurityConfig->PfatConfig->PfatLog.LoggingOptions));
+  DEBUG ((EFI_D_INFO, " PFAT:: PFATLOG:: PfatModSvn : 0x%X\n", SecurityConfig->PfatConfig->PfatLog.PfatModSvn));
+  DEBUG ((EFI_D_INFO, " PFAT:: PFATLOG:: NumOfEntriesInLog : 0x%X\n", SecurityConfig->PfatConfig->PfatLog.NumOfEntriesInLog));
+  //
+  // SECURITY_CONFIG : TXT_CONFIG
+  //
+#if defined(TXT_SUPPORT_FLAG) && (TXT_SUPPORT_FLAG == 1)
+  DEBUG ((EFI_D_INFO, " TXT:: SinitMemorySize : 0x%lX\n", SecurityConfig->TxtConfig->SinitMemorySize));
+  DEBUG ((EFI_D_INFO, " TXT:: TxtHeapMemorySize : 0x%lX\n", SecurityConfig->TxtConfig->TxtHeapMemorySize));
+  DEBUG ((EFI_D_INFO, " TXT:: TxtDprMemoryBase : 0x%lX\n", SecurityConfig->TxtConfig->TxtDprMemoryBase));
+  DEBUG ((EFI_D_INFO, " TXT:: TxtDprMemorySize : 0x%lX\n", SecurityConfig->TxtConfig->TxtDprMemorySize));
+  DEBUG ((EFI_D_INFO, " TXT:: BiosAcmBase : 0x%lX\n", SecurityConfig->TxtConfig->BiosAcmBase));
+  DEBUG ((EFI_D_INFO, " TXT:: BiosAcmSize : 0x%lX\n", SecurityConfig->TxtConfig->BiosAcmSize));
+  DEBUG ((EFI_D_INFO, " TXT:: McuUpdateDataAddr : 0x%lX\n", SecurityConfig->TxtConfig->McuUpdateDataAddr));
+  DEBUG ((EFI_D_INFO, " TXT:: TgaSize : 0x%lX\n", SecurityConfig->TxtConfig->TgaSize));
+  DEBUG ((EFI_D_INFO, " TXT:: TxtLcpPdBase : 0x%lX\n", SecurityConfig->TxtConfig->TxtLcpPdBase));
+  DEBUG ((EFI_D_INFO, " TXT:: TxtLcpPdSize : 0x%lX\n", SecurityConfig->TxtConfig->TxtLcpPdSize));
+#else
+  DEBUG ((EFI_D_INFO, " TXT CONFIG:: UNSUPPORTED \n"));
+#endif
+
+  //
+  // OVERCLOCKING_CONFIG
+  //
+  DEBUG ((EFI_D_INFO, " OC:: CoreVoltageOffset : 0x%X\n", OverclockingConfig->CoreVoltageOffset));
+  DEBUG ((EFI_D_INFO, " OC:: CoreVoltageOverride : 0x%X\n", OverclockingConfig->CoreVoltageOverride));
+  DEBUG ((EFI_D_INFO, " OC:: CoreExtraTurboVoltage : 0x%X\n", OverclockingConfig->CoreExtraTurboVoltage));
+  DEBUG ((EFI_D_INFO, " OC:: CoreMaxOcTurboRatio : 0x%X\n", OverclockingConfig->CoreMaxOcTurboRatio));
+  DEBUG ((EFI_D_INFO, " OC:: ClrVoltageOffset : 0x%X\n", OverclockingConfig->ClrVoltageOffset));
+  DEBUG ((EFI_D_INFO, " OC:: ClrVoltageOverride : 0x%X\n", OverclockingConfig->ClrVoltageOverride));
+  DEBUG ((EFI_D_INFO, " OC:: ClrExtraTurboVoltage : 0x%X\n", OverclockingConfig->ClrExtraTurboVoltage));
+  DEBUG ((EFI_D_INFO, " OC:: ClrMaxOcTurboRatio : 0x%X\n", OverclockingConfig->ClrMaxOcTurboRatio));
+  DEBUG ((EFI_D_INFO, " OC:: SvidVoltageOverride : 0x%X\n", OverclockingConfig->SvidVoltageOverride));
+  DEBUG ((EFI_D_INFO, " OC:: SvidEnable : 0x%X\n", OverclockingConfig->SvidEnable));
+  DEBUG ((EFI_D_INFO, " OC:: FivrFaultsEnable : 0x%X\n", OverclockingConfig->FivrFaultsEnable));
+  DEBUG ((EFI_D_INFO, " OC:: FivrEfficiencyEnable : 0x%X\n", OverclockingConfig->FivrEfficiencyEnable));
+  DEBUG ((EFI_D_INFO, " OC:: CoreVoltageMode : 0x%X\n", OverclockingConfig->CoreVoltageMode));
+  DEBUG ((EFI_D_INFO, " OC:: ClrVoltageMode : 0x%X\n", OverclockingConfig->ClrVoltageMode));
+  DEBUG ((EFI_D_INFO, " OC:: OcSupport : 0x%X\n", OverclockingConfig->OcSupport));
+
+  DEBUG ((EFI_D_INFO, "\n------------------------ CpuPlatformPolicyPpi Dump End -----------------\n\n"));
+#endif
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.dxs b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.dxs
new file mode 100644
index 0000000..62f855b
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.dxs
@@ -0,0 +1,42 @@
+/** @file
+  Dependency expression source file.
+
+@copyright
+  Copyright (c)  2004 - 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 "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"
+#include EFI_PPI_DEFINITION (CpuPlatformPolicy)
+#endif
+
+DEPENDENCY_START
+ 
+  PEI_CPU_PLATFORM_POLICY_PPI_GUID
+
+DEPENDENCY_END
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.h b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.h
new file mode 100644
index 0000000..9d6fac5
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.h
@@ -0,0 +1,212 @@
+/** @file
+  Describes the functions visible to the rest of the CpuPeim.
+
+@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 Pre-EFI 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 _CPU_INIT_PEIM_H_
+#define _CPU_INIT_PEIM_H_
+
+#include EFI_PPI_PRODUCER (Cache)
+#include EFI_PPI_DEFINITION (CpuPlatformPolicy)
+#include "CpuRegs.h"
+
+typedef struct {
+  UINT32 HtDisabled          : 1;
+  UINT32 NumberOfActiveCores : 2;
+  UINT32 Reserved0           : 2;
+  UINT32 Bist                : 1;
+  UINT32 FlexRatio           : 6;
+  UINT32 Reserved1           : 4;
+  UINT32 Reserved2           : 16;
+} CPU_STRAP_SET;
+
+typedef UINT32 CPU_RESET_TYPE;
+
+typedef struct {
+  UINT64  MsrValue;
+  BOOLEAN Changed;
+} MTRR_VALUE;
+
+typedef struct {
+  PEI_CACHE_PPI          Ppi;
+  EFI_PEI_PPI_DESCRIPTOR PpiDesc;
+  MTRR_VALUE             FixedMtrrValue[V_FIXED_MTRR_NUMBER];
+  MTRR_VALUE             VariableMtrrValue[V_MAXIMUM_VARIABLE_MTRR_NUMBER * 2];
+  BOOLEAN                FixedMtrrChanged;
+  BOOLEAN                VariableMtrrChanged;
+  BOOLEAN                NemDisabledDone;
+} CACHE_PPI_INSTANCE;
+
+#define PEI_CACHE_PPI_INSTANCE_FROM_THIS(a) _CR (a, CACHE_PPI_INSTANCE, Ppi)
+
+///
+/// Breaking UINT64 MSR into DWORDs/BYTEs
+///
+#pragma pack(1)
+typedef union _MSR_REGISTER {
+  UINT64 Qword;
+
+  struct _DWORDS {
+    UINT32 Low;
+    UINT32 High;
+  } Dwords;
+
+  struct _BYTES {
+    UINT8 FirstByte;
+    UINT8 SecondByte;
+    UINT8 ThirdByte;
+    UINT8 FouthByte;
+    UINT8 FifthByte;
+    UINT8 SixthByte;
+    UINT8 SeventhByte;
+    UINT8 EighthByte;
+  } Bytes;
+
+} MSR_REGISTER;
+#pragma pack()
+#define NO_RESET              0
+#define CPU_ONLY_RESET        1
+#define WARM_RESET            2
+#define COLDRESET             3
+
+#define RESET_PPI_WARM_RESET  0
+#define RESET_PPI_COLD_RESET  3
+
+#define RESET_PORT            0x0CF9
+#define CLEAR_RESET_BITS      0x0F1
+#define PSIX_THRESHOLD_MASK   0x3FFFFFFF  ///< Bits 61:32 - Mask value respect to Dword.High
+#define PSI1_THRESHOLD_VALUE  0x14
+#define PSI2_THRESHOLD_VALUE  0x05
+#define PSI3_THRESHOLD_VALUE  0x01
+
+#define MAX_OVERCLOCKING_BINS 0x7
+
+/**
+  Set up flags in CR4 for XMM instruction enabling
+**/
+VOID
+XmmInit (
+  VOID
+  );
+
+/**
+  Loads the Processor Microcode & Install the Cache PPI
+
+  @param[in] FfsHeader   - Pointer to an alleged FFS file.
+  @param[in] PeiServices - General purpose services available to every PEIM.
+
+  @retval EFI_STATUS - the status code returned from any sub-routine
+**/
+EFI_STATUS
+PeimInitializeCpu (
+  IN EFI_FFS_FILE_HEADER *FfsHeader,
+  IN EFI_PEI_SERVICES    **PeiServices
+  );
+
+/**
+  Install CacheInitPpi
+
+  @retval EFI_OUT_OF_RESOURCES - failed to allocate required pool
+**/
+EFI_STATUS
+CacheInitPpiInit (
+  VOID
+  );
+
+/**
+  Set CPU strap setting for feature change
+
+  @param[in] PeiServices       - Indirect reference to the PEI Services Table.
+  @param[in] NotifyDescriptor  - Address of the notification descriptor data structure. Type
+                      EFI_PEI_NOTIFY_DESCRIPTOR is defined above.
+  @param[in] Ppi               - Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS - Function completed successfully
+**/
+EFI_STATUS
+EFIAPI
+SetCpuStrap (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDescriptor,
+  IN VOID                      *Ppi
+  );
+
+/**
+  Based on ResetType, perform warm or cold reset using PCH Reset PPI
+
+  @param[in] PeiServices       - Indirect reference to the PEI Services Table.
+  @param[in] ResetType         - CPU_RESET_TYPE to indicate which reset shoudl be performed.
+
+  @exception EFI_UNSUPPORTED - Reset type unsupported
+  @retval EFI_SUCCESS     - function successfully (system should already reset)
+**/
+EFI_STATUS
+PerformWarmORColdReset (
+  IN EFI_PEI_SERVICES **PeiServices,
+  IN CPU_RESET_TYPE   ResetType
+  );
+
+/**
+  Based on ResetType, perform warm or cold reset using PCH Reset PPI
+
+  @param[in] PeiServices
+  @param[in] CPU_RESET_TYPE
+
+  @retval EFI_STATUS
+
+  Build BIST HOB
+
+  @param[in] PeiServices       - Indirect reference to the PEI Services Table.
+  @param[in] NotifyDescriptor  - Address of the notification descriptor data structure. Type
+                      EFI_PEI_NOTIFY_DESCRIPTOR is defined above.
+  @param[in] Ppi               - Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS - Hob built or not necessary
+**/
+EFI_STATUS
+EFIAPI
+BuildBistHob (
+  IN EFI_PEI_SERVICES          **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDescriptor,
+  IN VOID                      *Ppi
+  );
+
+/**
+  Dump RC CPU and PPM platform policies
+
+  @param[in] CpuPlatformPolicyPpi  - Address of the cpu platform policy ppi.
+**/
+VOID
+CpuPeiPolicyDump(
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  );
+
+/**
+
+  Perform the platform spefific initializations.
+
+  @param[in] PeiServices          - Indirect reference to the PEI Services Table.
+  @param[in] CpuPlatformPolicyPpi - Platform Policy PPI
+
+**/ 
+VOID
+BootGuardInit (
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN PEI_CPU_PLATFORM_POLICY_PPI  *CpuPlatformPolicyPpi
+  );
+
+#endif
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.inf b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.inf
new file mode 100644
index 0000000..6110b72
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuInitPeim.inf
@@ -0,0 +1,124 @@
+#
+# This file contains an 'Intel Pre-EFI 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
+#
+#/*++
+#
+# Copyright (c) 2007 - 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.
+
+#
+#  Module Name:
+#
+#    CpuInitPeim.inf
+#
+#  Abstract:
+#
+#    Component description file for CPU module
+#
+#
+#--*/
+
+[defines]
+BASE_NAME            = CpuInitPeim
+FILE_GUID            = 01359D99-9446-456d-ADA4-50A711C03ADA
+COMPONENT_TYPE       = PE32_PEIM
+
+[sources.common]
+  CpuInitPeim.c
+  CachePeim.c
+  PfatInit.c
+  CpuOcInit.c
+  BootGuardInit.c
+  
+#
+# Edk II Glue Driver Entry Point
+#
+  EdkIIGluePeimEntryPoint.c
+
+[sources.ia32]
+  Ia32/Cpu.asm
+
+[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
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include
+#
+# 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)/Library/OverclockingLib
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/Include/Library
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/SampleCode
+  $(EFI_SOURCE)/$(PROJECT_CPU_ROOT)/SampleCode/Include
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_PCH_ROOT)/Include/Library
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)
+  $(EFI_SOURCE)/$(PROJECT_SA_ROOT)/Include
+  $(EFI_SOURCE)/$(PROJECT_ME_ROOT)/Library/MeKernel/Include
+  $(EFI_SOURCE)/$(PROJECT_ME_ROOT)/Heci/Include
+
+[libraries.common]
+  EdkFrameworkPpiLib
+  EdkIIGlueBaseLib
+  EdkIIGlueBaseMemoryLib
+  EdkIIGluePeiDebugLibReportStatusCode
+  EdkIIGluePeiReportStatusCodeLib
+  EdkIIGluePeiServicesLib
+  EdkIIGluePeiMemoryAllocationLib
+  EdkIIGluePeiHobLib
+  EdkPpiLib
+  CpuPpiLib
+  CpuGuidLib
+  $(PROJECT_PCH_FAMILY)PpiLib
+  PeiKscLib
+  CpuPlatformLib
+  OverclockingLib
+  BootGuardLib
+
+[libraries.ia32]
+  CpuIA32Lib
+
+[nmake.common]
+  IMAGE_ENTRY_POINT = _ModuleEntryPoint
+  DPX_SOURCE        = CpuInitPeim.dxs
+
+#
+# Module Entry Point
+#
+ C_FLAGS = $(C_FLAGS) -D __EDKII_GLUE_MODULE_ENTRY_POINT__=PeimInitializeCpu
+ 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/CpuInit/Pei/CpuOcInit.c b/ReferenceCode/Haswell/CpuInit/Pei/CpuOcInit.c
new file mode 100644
index 0000000..8aa7cd7
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuOcInit.c
@@ -0,0 +1,292 @@
+/** @file
+  OC CPU 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 "CpuInitPeim.h"
+#include "CpuAccess.h"
+#include "OverclockingLibrary.h"
+#include "CpuOcInit.h"
+#endif
+
+/**
+  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
+CpuOcInit (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+  EFI_STATUS             Status;
+  OC_CAPABILITIES_ITEM   OcCaps;
+  VOLTAGE_FREQUENCY_ITEM CurrentVfItem;
+  VOLTAGE_FREQUENCY_ITEM RequestedVfItem;
+  GLOBAL_CONFIG_ITEM     CurrentFivrItem;
+  GLOBAL_CONFIG_ITEM     RequestedFivrItem;
+  SVID_CONFIG_ITEM       CurrentSvidItem;
+  SVID_CONFIG_ITEM       RequestedSvidItem;
+  UINT32                 LibStatus;
+  UINT8                  DomainId;
+  UINT8                  ResetRequired;
+  WDT_PPI                *gWdtPei;
+
+  LibStatus = 0; //DEBUG
+  ResetRequired = FALSE;
+  if (CpuPlatformPolicyPpi->OverclockingConfig->OcSupport == 0) {
+    ///
+    /// Overclocking is disabled
+    ///
+    DEBUG ((EFI_D_ERROR, "(OC) Overclocking is disabled. Bypassing CPU core overclocking flow.\n"));
+    return EFI_SUCCESS;
+  }
+
+  Status = EFI_SUCCESS;
+  ZeroMem(&CurrentFivrItem,sizeof(CurrentFivrItem));
+  ZeroMem(&RequestedFivrItem,sizeof(RequestedFivrItem));
+  ZeroMem(&CurrentSvidItem,sizeof(CurrentSvidItem));
+  ZeroMem(&RequestedSvidItem,sizeof(RequestedSvidItem));
+
+  //
+  // 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_IA_CORE; DomainId < OC_LIB_DOMAIN_ID_UNCORE; DomainId++) {
+    ///
+    /// Only IA_CORE and CLR are valid for CPU Core
+    ///
+    if ((DomainId == OC_LIB_DOMAIN_ID_IA_CORE) || (DomainId == OC_LIB_DOMAIN_ID_CLR)) {
+
+      ///
+      /// 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_IA_CORE) {
+            RequestedVfItem.VfSettings.MaxOcRatio = (UINT8) CpuPlatformPolicyPpi->OverclockingConfig->CoreMaxOcTurboRatio;
+          } else if (DomainId == OC_LIB_DOMAIN_ID_CLR) {
+            RequestedVfItem.VfSettings.MaxOcRatio = (UINT8) CpuPlatformPolicyPpi->OverclockingConfig->ClrMaxOcTurboRatio;
+          }
+
+          ///
+          /// VoltageTarget has 2 uses and we need to update the target based
+          /// on the voltagemode requested
+          ///
+          if (DomainId == OC_LIB_DOMAIN_ID_IA_CORE) {
+            RequestedVfItem.VfSettings.VoltageTargetMode = CpuPlatformPolicyPpi->OverclockingConfig->CoreVoltageMode;
+            if (RequestedVfItem.VfSettings.VoltageTargetMode == OC_LIB_OFFSET_ADAPTIVE) {
+              RequestedVfItem.VfSettings.VoltageTarget = CpuPlatformPolicyPpi->OverclockingConfig->CoreExtraTurboVoltage;
+            } else {
+              RequestedVfItem.VfSettings.VoltageTarget = CpuPlatformPolicyPpi->OverclockingConfig->CoreVoltageOverride;
+            }
+            RequestedVfItem.VfSettings.VoltageOffset = CpuPlatformPolicyPpi->OverclockingConfig->CoreVoltageOffset;
+          } else if (DomainId == OC_LIB_DOMAIN_ID_CLR) {
+            RequestedVfItem.VfSettings.VoltageTargetMode = CpuPlatformPolicyPpi->OverclockingConfig->ClrVoltageMode;
+            if (RequestedVfItem.VfSettings.VoltageTargetMode == OC_LIB_OFFSET_ADAPTIVE) {
+              RequestedVfItem.VfSettings.VoltageTarget = CpuPlatformPolicyPpi->OverclockingConfig->ClrExtraTurboVoltage;
+            } else {
+              RequestedVfItem.VfSettings.VoltageTarget = CpuPlatformPolicyPpi->OverclockingConfig->ClrVoltageOverride;
+            }
+            RequestedVfItem.VfSettings.VoltageOffset = CpuPlatformPolicyPpi->OverclockingConfig->ClrVoltageOffset;
+          }
+
+          ///
+          /// Compare current settings with user requested settings to see if changes are needed
+          ///
+          if (CompareMem((VOID *)&RequestedVfItem,(VOID *)&CurrentVfItem,sizeof(VOLTAGE_FREQUENCY_ITEM))) {
+            ///
+            /// Arm watchdog timer for OC changes
+            ///
+            Status = gWdtPei->ReloadAndStart (WDT_TIMEOUT_BETWEEN_PEI_DXE);
+
+            ///
+            /// Need to update the requested voltage/frequency values
+            ///
+            DEBUG ((EFI_D_INFO, "(OC) Set Voltage Frequency for Domain = %X\n", DomainId));
+            DEBUG ((EFI_D_INFO, "(OC) RequestedVfItem.VfSettings.MaxOcRatio     = %X\n", RequestedVfItem.VfSettings.MaxOcRatio));
+            DEBUG ((EFI_D_INFO, "(OC) RequestedVfItem.VfSettings.TargetMode     = %X\n", RequestedVfItem.VfSettings.VoltageTargetMode));
+            DEBUG ((EFI_D_INFO, "(OC) RequestedVfItem.VfSettings.VoltageTarget  = %X\n", RequestedVfItem.VfSettings.VoltageTarget));
+            DEBUG ((EFI_D_INFO, "(OC) RequestedVfItem.VfSettings.VoltageOffset  = %X\n", RequestedVfItem.VfSettings.VoltageOffset));
+            DEBUG ((EFI_D_INFO, "(OC) CurrentVfItem.VfSettings.MaxOcRatio       = %X\n", CurrentVfItem.VfSettings.MaxOcRatio));
+            DEBUG ((EFI_D_INFO, "(OC) CurrentVfItem.VfSettings.TargetMode       = %X\n", CurrentVfItem.VfSettings.VoltageTargetMode));
+            DEBUG ((EFI_D_INFO, "(OC) CurrentVfItem.VfSettings.VoltageTarget    = %X\n", CurrentVfItem.VfSettings.VoltageTarget));
+            DEBUG ((EFI_D_INFO, "(OC) CurrentVfItem.VfSettings.VoltageOffset    = %X\n", CurrentVfItem.VfSettings.VoltageOffset));
+            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_ERROR, "(OC) GetOcCapabilities message failed. Library Status = %X, Domain = %X\n", LibStatus, DomainId));
+      }
+    }
+  }
+
+  ///
+  /// Detect changes to global FIVR settings
+  ///
+  Status = GetFivrConfig(&CurrentFivrItem,&LibStatus);
+
+  if (LibStatus == OC_LIB_COMPLETION_CODE_SUCCESS) {
+    ///
+    /// Populate the requested FIVR settings from platform policy. The platform policy defines
+    /// these bits as 0-Disabled, 1-Enabled. The Mailbox uses the reverse encoding. Need to convert
+    /// the platform policy data to match the mailbox input.
+    ///
+    RequestedFivrItem.DisableFivrFaults = (~CpuPlatformPolicyPpi->OverclockingConfig->FivrFaultsEnable) & BIT0_MASK;
+    RequestedFivrItem.DisableFivrEfficiency = (~CpuPlatformPolicyPpi->OverclockingConfig->FivrEfficiencyEnable) & BIT0_MASK;
+
+    ///
+    /// Compare current FIVR settings with requested FIVR settings to see if changes are needed
+    ///
+    if (CompareMem((VOID *)&RequestedFivrItem,(VOID *)&CurrentFivrItem,sizeof(GLOBAL_CONFIG_ITEM))) {
+      ///
+      /// Arm watchdog timer for OC changes
+      ///
+      Status = gWdtPei->ReloadAndStart (8);
+
+      ///
+      /// Need to update the requested FIVR values
+      ///
+      DEBUG ((EFI_D_INFO, "(OC) Set FIVR Config for Domain = %X\n", DomainId));
+      Status = SetFivrConfig(RequestedFivrItem, &LibStatus);
+      if ((Status != EFI_SUCCESS) || (LibStatus != OC_LIB_COMPLETION_CODE_SUCCESS)) {
+        DEBUG ((EFI_D_ERROR, "(OC) Set FIVR Config failed. EFI Status = %X, Library Status = %X\n", Status, LibStatus));
+      }
+      else {
+        ///
+        /// If Re-enabling Fivr Faults, system needs to perform a cold reset for hardware to take effect
+        ///
+        if ((CurrentFivrItem.DisableFivrFaults == 1) && (RequestedFivrItem.DisableFivrFaults == 0)) {
+          DEBUG ((EFI_D_ERROR, "(OC) FIVR Faults enable detected. Cold Reset required.\n"));
+          ResetRequired = TRUE;
+        }
+      }
+    }
+  }
+  else {
+    DEBUG ((EFI_D_ERROR, "(OC) Get FIVR Config message failed. Library Status = %X\n", LibStatus));
+  }
+
+  ///
+  /// Detect changes to SVID settings
+  ///
+  Status = GetSvidConfig(&CurrentSvidItem,&LibStatus);
+
+  if (LibStatus == OC_LIB_COMPLETION_CODE_SUCCESS) {
+    ///
+    /// Populate the requested SVID settings from platform policy. SvidDisable uses a
+    /// reverse encoding from the platform policy defintion and will need to be converted.
+    ///
+    RequestedSvidItem.VoltageTarget = CpuPlatformPolicyPpi->OverclockingConfig->SvidVoltageOverride;
+    RequestedSvidItem.SvidDisable = ~(CpuPlatformPolicyPpi->OverclockingConfig->SvidEnable) & BIT0_MASK;
+
+    ///
+    /// Compare current SVID settings with requested SVID settings to see if changes are needed
+    ///
+    if (CompareMem((VOID *)&RequestedSvidItem,(VOID *)&CurrentSvidItem,sizeof(SVID_CONFIG_ITEM))) {
+      ///
+      /// Arm watchdog timer for OC changes
+      ///
+      Status = gWdtPei->ReloadAndStart (8);
+
+      ///
+      /// If Re-enabling SVID, system needs to perform a cold reset for hardware to take effect. No write to mailbox needed.
+      ///
+      if ((CurrentSvidItem.SvidDisable == 1) && (RequestedSvidItem.SvidDisable == 0)) {
+        DEBUG ((EFI_D_ERROR, "(OC) SVID Enable detected. Cold Reset required.\n"));
+        ResetRequired = TRUE;
+      }
+      else {
+        ///
+      /// Need to update the requested SVID values
+      ///
+      DEBUG ((EFI_D_INFO, "(OC) Set SVID Config for Domain = %X\n", DomainId));
+      Status = SetSvidConfig(RequestedSvidItem, &LibStatus);
+      if ((Status != EFI_SUCCESS) || (LibStatus != OC_LIB_COMPLETION_CODE_SUCCESS)) {
+        DEBUG ((EFI_D_ERROR, "(OC) Set SVID Config failed. EFI Status = %X, Library Status = %X\n", Status, LibStatus));
+      }
+      }
+    }
+  } else {
+    DEBUG ((EFI_D_ERROR, "(OC) GetFivrConfig message failed. Library Status = %X\n", LibStatus));
+  }
+
+  ///
+  /// Command was successful and SVID config has changed. CPU must perform a reset
+  /// for SVID settings to take effect.
+  ///
+  if (ResetRequired) {
+    DEBUG ((EFI_D_ERROR, "(OC) Perform Cold Reset\n"));
+    PerformWarmORColdReset (PeiServices, COLDRESET);
+  }
+
+  return Status;
+}
+
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/CpuOcInit.h b/ReferenceCode/Haswell/CpuInit/Pei/CpuOcInit.h
new file mode 100644
index 0000000..652c67e
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/CpuOcInit.h
@@ -0,0 +1,55 @@
+/** @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 _OC_INIT_H_
+#define _OC_INIT_H_
+
+#include "OverclockingLibrary.h"
+#include EFI_PPI_DEFINITION (CpuPlatformPolicy)
+#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
+///
+/**
+  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
+CpuOcInit (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  );
+
+#endif
+
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/Ia32/Cpu.asm b/ReferenceCode/Haswell/CpuInit/Pei/Ia32/Cpu.asm
new file mode 100644
index 0000000..e7c812a
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/Ia32/Cpu.asm
@@ -0,0 +1,77 @@
+;
+; This file contains an 'Intel Pre-EFI 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                                           
+;
+;------------------------------------------------------------------------------
+;
+; Copyright (c) 2006 - 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.
+;
+;
+; Module Name:
+;
+;   Cpu.asm
+;
+; Abstract:
+;
+;   Assembly code of Cpu
+;
+;------------------------------------------------------------------------------
+
+    .686p
+    .model  flat
+    .xmm
+
+IA32_CR4_OSFXSR           equ        200h
+IA32_CR4_OSXMMEXCPT       equ        400h
+IA32_CR0_MP               equ        2h
+
+IA32_CPUID_SSE2           equ        02000000h
+IA32_CPUID_SSE2_B         equ        26
+
+    .code
+
+;------------------------------------------------------------------------------
+;   Set up flags in CR4 for XMM instruction enabling
+;------------------------------------------------------------------------------
+XmmInit  PROC C    PUBLIC
+    push    ebx
+
+    ; Check whether SSE2 is supported
+    mov     eax, 1
+    cpuid
+    bt      edx, IA32_CPUID_SSE2_B
+    jnc     @F
+
+    ; Enable XMM
+    mov     eax, cr0
+    or      eax, IA32_CR0_MP
+    mov     cr0, eax
+    mov     eax, cr4
+    or      eax, IA32_CR4_OSFXSR OR IA32_CR4_OSXMMEXCPT
+    mov     cr4, eax
+
+@@:
+    pop     ebx
+    ret
+XmmInit  ENDP
+
+
+;------------------------------------------------------------------------------
+;   Invalidate cache
+;------------------------------------------------------------------------------
+CacheInvd  PROC C    PUBLIC
+    invd
+    ret
+CacheInvd  ENDP
+
+    END
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/PfatInit.c b/ReferenceCode/Haswell/CpuInit/Pei/PfatInit.c
new file mode 100644
index 0000000..8eff986
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/PfatInit.c
@@ -0,0 +1,191 @@
+/** @file
+  PFAT EarlyPost 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 "CpuInitPeim.h"
+#include "CpuAccess.h"
+#include "PfatInit.h"
+#ifdef PFAT_EC_FLAG
+#include EFI_PPI_DEPENDENCY (CpuIo)
+#include EFI_PPI_PRODUCER   (Stall)
+#include "PeiKscLib.h"
+#endif //PFAT_EC_FLAG
+#endif
+
+#ifdef PFAT_EC_FLAG
+/**
+  Gets CPU's random number generator.
+
+  @param[out] UINT32  - Random value
+**/
+UINT32
+RandomNumber (
+        void
+  )
+{
+  UINT32 Random = 0;
+
+  ///
+  /// Assembly instruction to read CPU's random number generator
+  /// Instruction is only available 100k cycles after reset
+  ///
+  __asm {
+tryAgain:
+    ;    rdrand eax
+    ;    db 0Fh, 0C7h, 0F0h
+
+    _emit 0x0F
+    _emit 0xC7
+    _emit 0xF0
+
+    mov Random, eax
+    jnc tryAgain; CF will be set is valid number was generated
+  }
+
+  return (Random);
+}
+#endif //PFAT_EC_FLAG
+
+/**
+  Perform the platform spefific initializations.
+
+  @param[in] PeiServices           - Indirect reference to the PEI Services Table.
+  @param[in] CpuPlatformPolicyPpi  - Platform Policy PPI
+**/
+VOID
+PfatInit (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  )
+{
+  EFI_STATUS  Status;
+  UINT64      MsrValue;
+  BOOLEAN     ResetRequired;
+  PFAT_HOB    *PfatHobPtr;
+  EFI_GUID    PfatHobGuid = PFAT_HOB_GUID;
+  UINT8       i;
+  PFAT_CONFIG *PfatConfig;
+#ifdef PFAT_EC_FLAG
+  volatile UINT32 EphemeralAuthValue;
+  UINT8           EcStatus;
+  PEI_STALL_PPI   *StallPpi;
+  PEI_CPU_IO_PPI  *CpuIo;
+#endif  //PFAT_EC_FLAG
+
+  DEBUG ((EFI_D_INFO, "PfatInit\n"));
+
+  ResetRequired = FALSE;
+  MsrValue = AsmReadMsr64 (MSR_PLATFORM_INFO);
+  if (!(MsrValue & B_MSR_PLATFORM_INFO_PFAT_AVAIL)) {
+    DEBUG ((EFI_D_INFO, "PFAT Feature is not supported\n"));
+    return;
+  }
+
+  if (CpuPlatformPolicyPpi->CpuConfig->Pfat) {
+    DEBUG ((EFI_D_INFO, "PFAT Module is Enable\n"));
+    PfatConfig = CpuPlatformPolicyPpi->SecurityConfig->PfatConfig;
+    ///
+    /// Read PFAT Control Register
+    ///
+    MsrValue = AsmReadMsr64 (MSR_PLAT_FRMW_PROT_CTRL);
+    if (MsrValue & B_MSR_PLAT_FRMW_PROT_CTRL_LK) {
+      if (!(MsrValue & B_MSR_PLAT_FRMW_PROT_CTRL_EN)) {
+        ///
+        /// Reset required as the PFAT CTRL MSR is locked and needs to be toggled
+        ///
+        ResetRequired = TRUE;
+      }
+    } else {
+#ifdef PFAT_EC_FLAG
+      if ((PfatConfig->Ppdt.PlatAttr & EC_PRESENT) && (PfatConfig->Ppdt.PlatAttr & EC_PFAT_PROTECTED)) {
+        DEBUG ((EFI_D_INFO, "EC is Present and EC FW supports PFAT\n"));
+        CpuIo  = (**PeiServices).CpuIo;
+        Status = (*PeiServices)->LocatePpi (PeiServices, &gPeiStallPpiGuid, 0, NULL, &StallPpi);
+        ASSERT_PEI_ERROR (PeiServices, Status);
+        Status = SendKscCommand (PeiServices, CpuIo, StallPpi, PfatConfig->EcCmdProvisionEav);
+        if (Status == EFI_SUCCESS) {
+          EphemeralAuthValue = RandomNumber ();
+          for (i = 0; (i < (sizeof (EphemeralAuthValue))); i++) {
+            Status = SendKscData (PeiServices, CpuIo, StallPpi, (UINT8) ((EphemeralAuthValue >> (i * 8)) & 0xFF));
+          }
+          Status = ReceiveKscData (PeiServices, CpuIo, StallPpi, &EcStatus);
+          if (EcStatus != 0) {
+            ResetRequired = TRUE;
+          }
+          AsmWriteMsr32 (MSR_PLAT_FRMW_PROT_PASSWD, EphemeralAuthValue);
+          EphemeralAuthValue = 0;
+          Status = SendKscCommand (PeiServices, CpuIo, StallPpi, PfatConfig->EcCmdLock);
+        }
+      }
+#endif                  //PFAT_EC_FLAG
+      AsmWriteMsr64 (MSR_PLAT_FRMW_PROT_HASH_0, PfatConfig->PpdtHash[0]);
+      AsmWriteMsr64 (MSR_PLAT_FRMW_PROT_HASH_1, PfatConfig->PpdtHash[1]);
+      AsmWriteMsr64 (MSR_PLAT_FRMW_PROT_HASH_2, PfatConfig->PpdtHash[2]);
+      AsmWriteMsr64 (MSR_PLAT_FRMW_PROT_HASH_3, PfatConfig->PpdtHash[3]);
+      MsrValue |= (B_MSR_PLAT_FRMW_PROT_CTRL_LK | B_MSR_PLAT_FRMW_PROT_CTRL_EN);
+      AsmWriteMsr64 (MSR_PLAT_FRMW_PROT_CTRL, MsrValue);
+    }
+    ///
+    /// Create PFAT HOB
+    ///
+    if (!ResetRequired) {
+      Status = (*PeiServices)->CreateHob (PeiServices, EFI_HOB_TYPE_GUID_EXTENSION, sizeof (PFAT_HOB), (VOID **) &PfatHobPtr);
+      PfatHobPtr->EfiHobGuidType.Name = PfatHobGuid;
+      CopyMem (&PfatHobPtr->Ppdt, &PfatConfig->Ppdt, PfatConfig->Ppdt.PpdtSize);
+      CopyMem (&PfatHobPtr->PupHeader, &PfatConfig->PupHeader, sizeof (PUP_HEADER));
+      CopyMem (&PfatHobPtr->PfatLog, &PfatConfig->PfatLog, sizeof (PFAT_LOG));
+      PfatHobPtr->NumSpiComponents = PfatConfig->NumSpiComponents;
+      for (i = 0; i < PfatConfig->NumSpiComponents; i++) {
+        PfatHobPtr->ComponentSize[i] = PfatConfig->ComponentSize[i];
+      }
+    }
+  } else {
+    DEBUG ((EFI_D_INFO, "PFAT Module is Disabled\n"));
+    MsrValue = AsmReadMsr64 (MSR_PLAT_FRMW_PROT_CTRL);
+    if (MsrValue & B_MSR_PLAT_FRMW_PROT_CTRL_LK) {
+      if (MsrValue & B_MSR_PLAT_FRMW_PROT_CTRL_EN) {
+        ///
+        /// Reset required as the PFAT CTRL MSR is locked and needs to be toggled
+        ///
+        ResetRequired = TRUE;
+      }
+    } else {
+      MsrValue &= ~B_MSR_PLAT_FRMW_PROT_CTRL_EN;
+      MsrValue |= B_MSR_PLAT_FRMW_PROT_CTRL_LK;
+      AsmWriteMsr64 (MSR_PLAT_FRMW_PROT_CTRL, MsrValue);
+    }
+  }
+
+  if (ResetRequired) {
+    ///
+    /// Perform Cold Reset
+    ///
+    DEBUG ((EFI_D_INFO, "Reset Required. Performing Cold Reset to unlock PFAT CONTROL MSR\n"));
+    PerformWarmORColdReset (PeiServices, COLDRESET);
+  }
+
+  return;
+}
diff --git a/ReferenceCode/Haswell/CpuInit/Pei/PfatInit.h b/ReferenceCode/Haswell/CpuInit/Pei/PfatInit.h
new file mode 100644
index 0000000..22e807f
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Pei/PfatInit.h
@@ -0,0 +1,42 @@
+/** @file
+  Describes the functions visible to the rest of the PfatInit.
+
+@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 _PFAT_INIT_H_
+#define _PFAT_INIT_H_
+
+#include "PfatDefinitions.h"
+#include EFI_PPI_DEFINITION (CpuPlatformPolicy)
+
+///
+/// Function Prototypes
+///
+/**
+  Execute Early-Post initialization of PFAT specific MSRs
+
+  @param[in] PeiServices          - Indirect reference to the PEI Services Table.
+  @param[in] CpuPlatformPolicyPpi - Platform Policy PPI
+**/
+VOID
+PfatInit (
+  IN EFI_PEI_SERVICES            **PeiServices,
+  IN PEI_CPU_PLATFORM_POLICY_PPI *CpuPlatformPolicyPpi
+  );
+
+#endif