summaryrefslogtreecommitdiff
path: root/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu
diff options
context:
space:
mode:
authorGuo Mang <mang.guo@intel.com>2016-12-23 10:45:25 +0800
committerGuo Mang <mang.guo@intel.com>2016-12-26 19:15:03 +0800
commit423dd3b025d44e768da358390c06def185f22247 (patch)
treec39a62eed0b9bc82b6b2a230509ab12034e78ca1 /Silicon/BroxtonSoC/BroxtonSiPkg/Cpu
parent53c716058fdca56a2c82efdb4011a84f6eae5746 (diff)
downloadedk2-platforms-423dd3b025d44e768da358390c06def185f22247.tar.xz
BroxtonSiPkg: Add CpuInit
Contributed-under: TianoCore Contribution Agreement 1.0 Signed-off-by: Guo Mang <mang.guo@intel.com>
Diffstat (limited to 'Silicon/BroxtonSoC/BroxtonSiPkg/Cpu')
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.c1004
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.h385
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.inf90
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Exception.h93
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.S763
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.asm735
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Exception.c336
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Htequ.inc41
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.S451
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.asm443
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.S25
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.asm36
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MemoryOperation.c519
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.S113
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.asm102
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCpu.c69
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.S67
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.asm84
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/ProcessorDef.h49
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.c865
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.h285
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.c769
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.h524
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.c2303
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.h663
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MtrrSync.c253
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/Exception.c382
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MemoryOperation.c800
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpCpu.c80
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.h38
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.inc44
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.S610
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.asm591
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/ProcessorDef.h53
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/VirtualMemory.h131
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.S1658
-rw-r--r--Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.asm413
37 files changed, 15867 insertions, 0 deletions
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.c
new file mode 100644
index 0000000000..8b88fce8e0
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.c
@@ -0,0 +1,1004 @@
+/** @file
+ Cpu driver, which initializes CPU and implements CPU Architecture
+ Protocol as defined in Framework specification.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/HobLib.h>
+#include <Library/IoLib.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/MpService.h>
+#include <Protocol/CpuGlobalNvsArea.h>
+#include <Private/CpuInitDataHob.h>
+#include "CpuInitDxe.h"
+#include "Exception.h"
+#include <Private/Library/CpuCommonLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/ConfigBlockLib.h>
+
+//
+// Private GUID for BIOS Guard initializes
+//
+extern EFI_GUID gBiosGuardHobGuid;
+
+GLOBAL_REMOVE_IF_UNREFERENCED CPU_GLOBAL_NVS_AREA_PROTOCOL CpuGlobalNvsAreaProtocol;
+
+
+#ifdef SLE_FLAG
+#define SAMPLE_TICK_COUNT 5
+#else
+#define SAMPLE_TICK_COUNT 1000
+#endif //SLE_FLAG
+
+extern UINT64 mValidMtrrAddressMask;
+extern UINT64 mValidMtrrBitsMask;
+extern UINT8 CpuInitDxeStrings[];
+extern UINT8 mDefaultMemoryType;
+CONST CHAR8 *DefaultVersion = "Intel(R) Genuine processor";
+
+GLOBAL_REMOVE_IF_UNREFERENCED VOID *mSmmBaseRegistration;
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+GLOBAL_REMOVE_IF_UNREFERENCED BOOLEAN mIsFlushingGCD = TRUE;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8 mSmmbaseSwSmiNumber;
+GLOBAL_REMOVE_IF_UNREFERENCED BOOLEAN mVariableMtrrChanged;
+GLOBAL_REMOVE_IF_UNREFERENCED BOOLEAN mFixedMtrrChanged;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT64 mCpuFrequency = 0;
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_EVENT gReadyToBootEvent;
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_CPU_INTERRUPT_HANDLER mExternalVectorTable[0x100];
+GLOBAL_REMOVE_IF_UNREFERENCED BOOLEAN mInterruptState = FALSE;
+
+//
+// The Cpu Init Data Hob
+//
+GLOBAL_REMOVE_IF_UNREFERENCED CPU_INIT_DATA_HOB *mCpuInitDataHob = NULL;
+GLOBAL_REMOVE_IF_UNREFERENCED CPU_CONFIG *CpuConfig = NULL;
+GLOBAL_REMOVE_IF_UNREFERENCED POWER_MGMT_CONFIG *PowerMgmtConfig = NULL;
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED CPU_INFO_PROTOCOL *mCpuInfo;
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN mCommonFeatures;
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN mSiliconFeatures;
+
+//
+// The Cpu Architectural Protocol that this Driver produces
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_CPU_ARCH_PROTOCOL gCpu = {
+ FlushCpuDataCache,
+ EnableInterrupt,
+ DisableInterrupt,
+ CpuGetInterruptState,
+ Init,
+ RegisterInterruptHandler,
+ GetTimerValue,
+ SetMemoryAttributes,
+ 1, ///< NumberOfTimers
+ 4, ///< DmaBufferAlignment
+};
+
+/**
+ 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
+ @retval 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) {
+ AsmInvd ();
+ 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.
+
+**/
+EFI_STATUS
+EFIAPI
+EnableInterrupt (
+ IN EFI_CPU_ARCH_PROTOCOL *This
+ )
+{
+ EnableInterrupts ();
+ 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
+ )
+{
+ DisableInterrupts ();
+ mInterruptState = FALSE;
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Return the state of interrupts.
+
+ @param[in] This Protocol instance structure
+ @param[out] 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.
+ @retval 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.
+ @retval 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
+ )
+{
+ BOOLEAN State;
+
+ 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;
+ }
+
+ //
+ // State is stored with the current interrupt state from CPU Arch Prot.
+ // Keep interrupts disabled until we finish registering.
+ //
+ (This->GetInterruptState) (This, &State);
+ if (State) {
+ This->DisableInterrupt (This);
+ }
+
+ if (InterruptHandler != NULL) {
+ SetInterruptDescriptorTableHandlerAddress ((UINTN)InterruptType);
+ } else {
+ //
+ // Restore the original IDT handler address if InterruptHandler is NULL.
+ //
+ RestoreInterruptDescriptorTableHandlerAddress ((UINTN) InterruptType);
+ }
+
+ mExternalVectorTable[InterruptType] = InterruptHandler;
+
+ if (State) {
+ This->EnableInterrupt (This);
+ }
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ 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[out] TimerValue Pointer to the returned timer value.
+ @param[out] 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.
+ @retval 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
+ )
+{
+ UINT64 Actual;
+
+ if (TimerValue == NULL) {
+ return EFI_INVALID_PARAMETER;
+ }
+
+ if (TimerIndex != 0) {
+ return EFI_INVALID_PARAMETER;
+ }
+
+ *TimerValue = AsmReadTsc ();
+
+ 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
+ @retval 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;
+ 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 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 == mDefaultMemoryType) {
+ 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;
+}
+
+
+VOID
+EFIAPI
+MpIATrust (
+ IN VOID *Buffer
+ )
+{
+ UINT64 Data;
+
+ Data = AsmReadMsr64 (0x120);
+ Data |= BIT6;
+ AsmWriteMsr64 (0x120, Data);
+}
+
+
+EFI_STATUS
+EFIAPI
+SetUntrustedModeExitBootServicesEvent (
+ VOID
+ )
+{
+ EFI_MP_SERVICES_PROTOCOL *MpService;
+ EFI_STATUS Status;
+
+ DEBUG((EFI_D_INFO, "Setting Untrusted Mode\n"));
+
+ Status = gBS->LocateProtocol (
+ &gEfiMpServiceProtocolGuid,
+ NULL,
+ (VOID **) &MpService
+ );
+
+ if (!EFI_ERROR (Status)) {
+ MpIATrust (NULL);
+ Status = MpService->StartupAllAPs (
+ MpService,
+ MpIATrust,
+ FALSE,
+ NULL,
+ 0,
+ NULL,
+ NULL
+ );
+ }
+
+ return Status;
+}
+
+
+VOID
+EFIAPI
+CpuExitBootServicesCallback (
+ IN EFI_EVENT Event,
+ IN VOID *Context
+ )
+{
+ EFI_STATUS Status;
+
+ //
+ // Change Boot Personality At End of Boot (Set Untrusted Mode)
+ //
+ Status = SetUntrustedModeExitBootServicesEvent ();
+ ASSERT_EFI_ERROR (Status);
+}
+
+
+/**
+ 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;
+ EFI_EVENT ExitBootServiceEvent;
+ VOID *Hob;
+
+ //
+ // 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;
+ }
+
+ //
+ // Get CPU Init Data Hob
+ //
+ Hob = GetFirstGuidHob (&gCpuInitDataHobGuid);
+ if (Hob == NULL) {
+ DEBUG ((DEBUG_ERROR, "CPU Data HOB not available\n"));
+ return EFI_NOT_FOUND;
+ }
+ mCpuInitDataHob = (CPU_INIT_DATA_HOB *) ((UINTN) Hob + sizeof (EFI_HOB_GUID_TYPE));
+ CpuConfig = (CPU_CONFIG *) (UINTN) mCpuInitDataHob->CpuConfig;
+ PowerMgmtConfig = (POWER_MGMT_CONFIG *) (UINTN) mCpuInitDataHob->PowerMgmtConfig;
+
+
+ mSiliconFeatures = mCpuInitDataHob->SiliconInfo;
+
+ //
+ // Initialize the global SmmBase SWSMI number
+ //
+ mSmmbaseSwSmiNumber = CpuConfig->SmmbaseSwSmiNumber;
+
+ Status = gBS->LocateProtocol (&gEfiMetronomeArchProtocolGuid, NULL, (VOID **) &mMetronome);
+ if (EFI_ERROR (Status)) {
+ return Status;
+ }
+
+ //
+ // Initialize DxeCpuInfo protocol instance and gather CPU information
+ //
+ Status = InitCpuInfo ();
+ if (EFI_ERROR (Status)) {
+ DEBUG ((DEBUG_ERROR, "Failed to initialize DxeCpuInfo\n"));
+ }
+
+ //
+ // Initialize MP Support if necessary
+ //
+ Status = InitializeMpSupport ();
+ if (EFI_ERROR (Status)) {
+ DEBUG ((DEBUG_ERROR, "Failed to initialize MPs\n"));
+ }
+
+ //
+ // Register the CPU ExitBootServices callback function
+ //
+ Status = gBS->CreateEvent(
+ EVT_SIGNAL_EXIT_BOOT_SERVICES,
+ TPL_CALLBACK,
+ CpuExitBootServicesCallback,
+ NULL,
+ &ExitBootServiceEvent
+ );
+
+ //
+ // Get CPU Global NVS protocol pointer
+ //
+ CpuGlobalNvsAreaProtocol.Area = (CPU_GLOBAL_NVS_AREA *) (UINTN) mCpuInitDataHob->CpuGnvsPointer;
+ CpuGlobalNvsAreaProtocol.Area->DtsAcpiEnable = 0;
+
+ //
+ // Install Cpu Power management GlobalNVS Area protocol
+ //
+ Status = gBS->InstallMultipleProtocolInterfaces (
+ &ImageHandle,
+ &gCpuGlobalNvsAreaProtocolGuid,
+ &CpuGlobalNvsAreaProtocol,
+ NULL
+ );
+ ASSERT_EFI_ERROR (Status);
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Returns the actual CPU core frequency in MHz.
+
+ @param[in] Metronome Metronome protocol
+ @param[out] 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 = AsmReadTsc ();
+ Status = Metronome->WaitForTick (Metronome, TickCount);
+ EndValue = AsmReadTsc ();
+ 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;
+}
+
+
+/**
+ Initialize CPU info.
+
+ @retval EFI_SUCCESS successfully prepared.
+
+**/
+EFI_STATUS
+InitCpuInfo (
+ VOID
+ )
+{
+ CACHE_DESCRIPTOR_INFO *CacheInfo;
+ CHAR8 *BrandString;
+ EFI_CPUID_REGISTER CpuidRegs;
+ UINT32 CpuSignature;
+ UINT8 CacheInfoCount;
+ UINT16 CachePartitions;
+ UINT16 CacheLineSize;
+ UINT32 CacheNumberofSets;
+ UINT8 ThreadsPerCore;
+ UINT64 MsrData;
+ UINT8 Index;
+ EFI_HANDLE Handle;
+ UINT16 MaxEnabledThreadsPerCore;
+ UINT16 MaxEnabledCoresPerDie;
+ UINT16 MaxDiesPerPackage;
+ UINT16 MaxPackages;
+ UINT64 Frequency;
+ Handle = NULL;
+
+ //
+ // Install CPU info protocol
+ //
+ mCpuInfo = AllocateZeroPool (sizeof (CPU_INFO_PROTOCOL));
+ if (mCpuInfo== NULL) {
+ DEBUG ((EFI_D_ERROR, "mCpuInfo is NULL.\n"));
+ return EFI_OUT_OF_RESOURCES;
+ }
+ mCpuInfo->Revision = CPU_INFO_PROTOCOL_REVISION;
+ mCpuInfo->CpuCommonFeatures = mCommonFeatures | (mSiliconFeatures << 10);
+
+ mCpuInfo->CpuInfo = AllocateZeroPool (sizeof (CPU_INFO));
+ if (mCpuInfo->CpuInfo == NULL) {
+ DEBUG ((EFI_D_ERROR, "mCpuInfo->CpuInfo is NULL.\n"));
+ return EFI_OUT_OF_RESOURCES;
+ }
+
+ mCpuInfo->CpuInfo->BrandString = AllocateZeroPool (49 * sizeof (CHAR8));
+ if (mCpuInfo->CpuInfo->BrandString == NULL) {
+ DEBUG ((EFI_D_ERROR, "mCpuInfo->CpuInfo->BrandString is NULL.\n"));
+ return EFI_OUT_OF_RESOURCES;
+ }
+
+ mCpuInfo->SmramCpuInfo = AllocateZeroPool (sizeof (SMRAM_CPU_INFO));
+ if (mCpuInfo->SmramCpuInfo == NULL) {
+ DEBUG ((EFI_D_ERROR, "mCpuInfo->SmramCpuInfo is NULL.\n"));
+ return EFI_OUT_OF_RESOURCES;
+ }
+
+ //
+ // Get Cache Descriptors.
+ //
+ CacheInfoCount = 0;
+ do {
+ AsmCpuidEx (CPUID_FUNCTION_4, CacheInfoCount, &CpuidRegs.RegEax, &CpuidRegs.RegEbx, &CpuidRegs.RegEcx, &CpuidRegs.RegEdx);
+ CacheInfoCount++;
+ } while (((UINT8) CpuidRegs.RegEax & 0x0F)!=0);
+
+ DEBUG ((DEBUG_INFO, "CacheInfoCount = %x\n", CacheInfoCount));
+ CacheInfoCount--;
+ mCpuInfo->CpuInfo->CacheInfo = AllocateZeroPool (CacheInfoCount * sizeof (CACHE_DESCRIPTOR_INFO));
+ if (mCpuInfo->CpuInfo->CacheInfo == NULL) {
+ DEBUG ((EFI_D_ERROR, "mCpuInfo->CpuInfo->CacheInfo is NULL.\n"));
+ return EFI_OUT_OF_RESOURCES;
+ }
+ mCpuInfo->CpuInfo->MaxCacheSupported = CacheInfoCount;
+ mCpuInfo->CpuInfo->SmmbaseSwSmiNumber = mSmmbaseSwSmiNumber;
+
+ BrandString = mCpuInfo->CpuInfo->BrandString;
+ CacheInfo = mCpuInfo->CpuInfo->CacheInfo;
+
+ //
+ // Get Brand string
+ //
+ AsmCpuid (CPUID_BRAND_STRING1, &CpuidRegs.RegEax, &CpuidRegs.RegEbx, &CpuidRegs.RegEcx, &CpuidRegs.RegEdx);
+ if (CpuidRegs.RegEax != 0) {
+ *(UINT32 *) BrandString = CpuidRegs.RegEax; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEbx; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEcx; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEdx; BrandString +=4;
+
+ AsmCpuid (CPUID_BRAND_STRING2, &CpuidRegs.RegEax, &CpuidRegs.RegEbx, &CpuidRegs.RegEcx, &CpuidRegs.RegEdx);
+ *(UINT32 *) BrandString = CpuidRegs.RegEax; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEbx; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEcx; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEdx; BrandString +=4;
+
+ AsmCpuid (CPUID_BRAND_STRING3, &CpuidRegs.RegEax, &CpuidRegs.RegEbx, &CpuidRegs.RegEcx, &CpuidRegs.RegEdx);
+ *(UINT32 *) BrandString = CpuidRegs.RegEax; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEbx; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEcx; BrandString +=4;
+ *(UINT32 *) BrandString = CpuidRegs.RegEdx; BrandString +=4;
+ *BrandString = '\0';
+ } else {
+ CopyMem (BrandString, DefaultVersion, AsciiStrLen (DefaultVersion));
+ }
+ //
+ // Remove leading spaces. After removing leading spaces, the Brand String can not be freed. However, it should never be freed.
+ //
+ while (*mCpuInfo->CpuInfo->BrandString == ' ') {
+ ++mCpuInfo->CpuInfo->BrandString;
+ }
+
+ //
+ // Get information on enabled threads, cores, dies and package for the CPU(s) on this platform
+ //
+ GetEnabledCount (
+ &MaxEnabledThreadsPerCore,
+ &MaxEnabledCoresPerDie,
+ &MaxDiesPerPackage,
+ &MaxPackages
+ );
+
+ //
+ // Gather CPU info
+ //
+ AsmCpuid (CPUID_VERSION_INFO, &CpuSignature, NULL, &CpuidRegs.RegEcx, &CpuidRegs.RegEdx);
+ mCpuInfo->CpuInfo->CpuSignature = CpuSignature;
+ mCpuInfo->CpuInfo->Features = LShiftU64 (CpuidRegs.RegEcx, 32) + CpuidRegs.RegEdx;
+
+ AsmCpuidEx (CPUID_CORE_TOPOLOGY, 0, NULL, &CpuidRegs.RegEbx, NULL, NULL);
+ mCpuInfo->CpuInfo->NumSupportedThreadsPerCore = (UINT8) CpuidRegs.RegEbx;
+ ThreadsPerCore = (UINT8) CpuidRegs.RegEbx;
+
+ AsmCpuidEx (CPUID_CORE_TOPOLOGY, 1, NULL, &CpuidRegs.RegEbx, NULL, NULL);
+ mCpuInfo->CpuInfo->NumSupportedCores = (UINT8) (CpuidRegs.RegEbx / ThreadsPerCore);
+
+ MsrData = AsmReadMsr64 (MSR_CORE_THREAD_COUNT);
+ mCpuInfo->CpuInfo->NumCores = (UINT8) MaxEnabledCoresPerDie;
+ mCpuInfo->CpuInfo->NumHts = (UINT8) MaxEnabledThreadsPerCore;
+ mCpuInfo->CpuInfo->IntendedFreq = (10000 * (((UINT32) AsmReadMsr64 (MSR_PLATFORM_INFO) >> 8) & 0xFF)) /100;
+ GetActualFrequency (mMetronome, &Frequency);
+ //
+ // Units in MHz
+ //
+ mCpuInfo->CpuInfo->ActualFreq = (UINT32) Frequency;
+ mCpuInfo->CpuInfo->Voltage = 0;
+
+ CacheInfoCount = mCpuInfo->CpuInfo->MaxCacheSupported;
+ for (Index=0; Index <= CacheInfoCount; Index++) {
+ AsmCpuidEx (CPUID_FUNCTION_4, Index, &CpuidRegs.RegEax, &CpuidRegs.RegEbx, &CpuidRegs.RegEcx, &CpuidRegs.RegEdx);
+ CacheInfo[Index].Type=(UINT8) (((UINT8) CpuidRegs.RegEax) & CPU_CACHE_TYPE_MASK);
+ CacheInfo[Index].Level=(UINT8) ((((UINT8) CpuidRegs.RegEax) >> 5) & CPU_CACHE_LEVEL_MASK);
+ CacheInfo[Index].Associativity= (UINT16) (((CpuidRegs.RegEbx >> 22) & CPU_CACHE_ASSOCIATIVITY_MASK)+ 1);
+ //
+ // Determine Cache Size in Bytes = (Associativity) * (Partitions + 1) * (Line_Size + 1) * (Sets + 1)= (EBX[31:22] + 1) * (EBX[21:12] + 1) * (EBX[11:0] + 1) * (ECX + 1)
+ //
+ CachePartitions = (UINT16) (((CpuidRegs.RegEbx >> 12)& CPU_CACHE_PARTITION_MASK)+ 1 );
+ CacheLineSize =(UINT16) (((UINT16) CpuidRegs.RegEbx & CPU_CACHE_LINE_SIZE_MASK)+ 1);
+ CacheNumberofSets = CpuidRegs.RegEcx + 1;
+ CacheInfo[Index].Size = (UINT32) ((CacheInfo[Index].Associativity * CachePartitions * CacheLineSize * CacheNumberofSets) / 1024);
+ }
+
+ gBS->InstallMultipleProtocolInterfaces (
+ &Handle,
+ &gCpuInfoProtocolGuid,
+ mCpuInfo,
+ NULL
+ );
+
+ return EFI_SUCCESS;
+}
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.h
new file mode 100644
index 0000000000..bc7ed92f4c
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.h
@@ -0,0 +1,385 @@
+/** @file
+ Private data structures and function prototypes.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _CPU_INIT_DXE_H
+#define _CPU_INIT_DXE_H
+
+#include <Protocol/Metronome.h>
+#include <Protocol/Cpu.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Guid/EventGroup.h>
+#include "MemoryAttribute.h"
+#include <CpuAccess.h>
+#include <Protocol/CpuInfo.h>
+#include <Library/CpuPlatformLib.h>
+#define INTERRUPT_VECTOR_NUMBER 256
+#define INTERRUPT_GATE_ATTRIBUTE 0x8e00
+
+extern UINT8 mSmmbaseSwSmiNumber;
+
+/**
+ 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
+ @retval 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[out] 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.
+ @retval 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.
+ @retval 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[out] TimerValue Pointer to the returned timer value.
+ @param[out] 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.
+ @retval 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
+ @retval 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
+ );
+
+/**
+ 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
+
+ @param[in] Buffer Not used.
+
+**/
+VOID
+EFIAPI
+MpMtrrSynchUp (
+ IN VOID *Buffer
+ );
+
+/**
+ Copy Global MTRR data to S3
+
+**/
+VOID
+SaveBspMtrrForS3 (
+ VOID
+ );
+
+/**
+ Returns the actual CPU core frequency in MHz.
+
+ @param[in] Metronome Metronome protocol
+ @param[out] 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
+ );
+
+/**
+ Initialize Cpu float point unit
+
+**/
+VOID
+CpuInitFloatPointUnit (
+ VOID
+ );
+
+
+/**
+ Initialize CPU info.
+
+ @retval EFI_SUCCESS successfully prepared.
+
+**/
+EFI_STATUS
+InitCpuInfo (
+ VOID
+ );
+
+/**
+ Set Interrupt Descriptor Table Handler Address.
+
+ @param[in] Index The Index of the interrupt descriptor table handle.
+
+**/
+VOID
+SetInterruptDescriptorTableHandlerAddress (
+ IN UINTN Index
+ );
+
+/**
+ Restore original Interrupt Descriptor Table Handler Address.
+
+ @param[in] Index The Index of the interrupt descriptor table handle.
+
+**/
+VOID
+RestoreInterruptDescriptorTableHandlerAddress (
+ IN UINTN Index
+ );
+
+#endif
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.inf b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.inf
new file mode 100644
index 0000000000..e486e05a9b
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/CpuInitDxe.inf
@@ -0,0 +1,90 @@
+## @file
+# MP Cpu module.
+#
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+ INF_VERSION = 0x00010017
+ BASE_NAME = CpuInitDxe
+ FILE_GUID = FC73690C-8D4A-4F8C-B7F5-BA241F316E28
+ VERSION_STRING = 1.0
+ MODULE_TYPE = DXE_DRIVER
+ ENTRY_POINT = InitializeCpu
+
+[LibraryClasses]
+ UefiDriverEntryPoint
+ ReportStatusCodeLib
+ DxeServicesTableLib
+ SynchronizationLib
+ HobLib
+ TimerLib
+ CpuCommonLib
+ ConfigBlockLib
+
+[Packages]
+ MdePkg/MdePkg.dec
+ BroxtonSiPkg/BroxtonSiPkg.dec
+ BroxtonSiPkg/BroxtonSiPrivate.dec
+
+[Sources]
+ CpuInitDxe.c
+ CpuInitDxe.h
+ Exception.h
+ MemoryAttribute.c
+ MemoryAttribute.h
+ MpCommon.c
+ MpCommon.h
+ MpService.c
+ MpService.h
+ MtrrSync.c
+
+[sources.ia32]
+ Ia32/CpuAsm.asm
+ Ia32/MpProc.asm
+ Ia32/MchkInitAsm.asm
+ Ia32/MPFuncs32.asm
+ Ia32/MpCommon32.asm
+ Ia32/MemoryOperation.c
+ Ia32/Exception.c
+ Ia32/MpCpu.c
+
+[Sources.X64]
+ x64/cpu.asm|MSFT
+ x64/MpFuncs.asm|MSFT
+ x64/cpu.S|GCC
+ x64/MpFuncs.S|GCC
+ x64/MemoryOperation.c
+ x64/Exception.c
+ x64/MpCpu.c
+ x64/ProcessorDef.h
+ x64/VirtualMemory.h
+
+[Protocols]
+ gEfiMetronomeArchProtocolGuid ## CONSUMES
+ gEfiCpuArchProtocolGuid ## PRODUCES
+ gEfiMpServiceProtocolGuid ## PRODUCES
+ gEfiSmmControl2ProtocolGuid ## CONSUMES
+ gCpuInfoProtocolGuid ## PRODUCES
+ gEfiLegacyBiosProtocolGuid ## CONSUMES
+ gCpuGlobalNvsAreaProtocolGuid ## PRODUCES
+
+[Guids]
+ gSmramCpuDataHeaderGuid ## UNDEFINED
+ gHtBistHobGuid ## UNDEFINED
+ gEfiEndOfDxeEventGroupGuid ## UNDEFINED
+ gCpuInitDataHobGuid ## UNDEFINED
+ gCpuStatusCodeDataTypeExceptionHandlerGuid ## UNDEFINED
+
+[Depex]
+ gEfiMetronomeArchProtocolGuid
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Exception.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Exception.h
new file mode 100644
index 0000000000..a087278cbc
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Exception.h
@@ -0,0 +1,93 @@
+/** @file
+ IA32 Exception Includes.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EXCEPTION_H
+#define _EXCEPTION_H
+
+//
+// Driver Consumed Protocol Prototypes
+//
+extern EFI_GUID gCpuStatusCodeDataTypeExceptionHandlerGuid;
+
+#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
+
+extern UINTN mOrigIdtEntryCount;
+
+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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.S b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.S
new file mode 100644
index 0000000000..50b965ecf4
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.S
@@ -0,0 +1,763 @@
+## @file
+# Assembly code for the IA-32 resources.
+#
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+#
+##
+
+.data
+
+ExternalVectorTablePtr: .space 4
+CommonInterruptEntry: .long CommonEntry
+Idtr: .space 2
+Idtr1: .space 4
+
+##include Htequ.inc
+
+.equ VacantFlag , 0x00
+.equ NotVacantFlag , 0xff
+.equ StartupApSignal , 0x6E755200
+.equ MonitorFilterSize, 0x40
+.equ ApCounterInit , 0
+.equ ApInHltLoop , 1
+.equ ApInMwaitLoop , 2
+.equ ApInRunLoop , 3
+.equ WakeUpApPerMwaitLoop32 , 4
+.equ WakeUpApPerRunLoop32 , 5
+.equ LockLocation , 0x1000 - 0x0400
+.equ StackStart , LockLocation + 0x4
+.equ StackSize , LockLocation + 0x8
+.equ RendezvousProc , LockLocation + 0x0C
+.equ GdtrProfile , LockLocation + 0x10
+.equ IdtrProfile , LockLocation + 0x16
+.equ BufferStart , LockLocation + 0x1C
+.equ Cr3Location , LockLocation + 0x20
+.equ InitFlag , LockLocation + 0x24
+.equ WakeUpApManner , LockLocation + 0x28
+.equ BistBuffer , LockLocation + 0x2C
+
+.macro PAUSE32
+ .byte 0xF3
+ .byte 0x90
+.endm
+
+.text
+
+#---------------------------------------#
+# _InitializeIdt #
+#----------------------------------------------------------------------------#
+#
+# Protocol prototype
+# InitializeIdt (
+# IN EFI_CPU_INTERRUPT_HANDLER TableStart,
+# IN UINTN *IdtTablePtr,
+# IN UINT16 IdtLimit
+# )
+#
+# Routine Description:
+#
+# Creates an IDT table starting at IdtTablPtr. It has IdtLimit/8 entries.
+# Table is initialized to intxx where xx is from 00 to number of entries or
+# 100h, whichever is smaller. After table has been initialized the LIDT
+# instruction is invoked.
+#
+# TableStart is the pointer to the callback table and is not used by
+# InitializedIdt but by commonEntry. CommonEntry handles all interrupts,
+# does the context save and calls the callback entry, if non-NULL.
+# It is the responsibility of the callback routine to do hardware EOIs.
+#
+# Arguments:
+#
+# TableStart - Pointer to interrupt callback table
+#
+# IdtTablePtr - Pointer to IDT table
+#
+# IdtLimit - IDT Table limit = number of interrupt entries * 8
+#
+# Returns:
+#
+# Nothing
+#
+#
+# Input: [ebp][0] = Original ebp
+# [ebp][4] = Return address
+# [ebp][8] = TableStart
+# [ebp][0c] = *IdtTablePtr
+# [ebp][10] = IdtLimit
+#
+# Output: Nothing
+#
+# Destroys: Nothing
+#-----------------------------------------------------------------------------#
+
+ASM_GLOBAL ASM_PFX(InitializeIdt)
+ASM_PFX(InitializeIdt):
+ pushl %ebp # C prolog
+ movl %esp,%ebp
+ pushl %edi
+
+ movl 8(%ebp),%eax # Get ExternalVectorTable Address
+ movl %eax, ExternalVectorTablePtr
+
+ movw 0x10(%ebp),%ax # Get IDT Table limit
+ decw %ax
+ movw %ax, Idtr # Store %ax to Idtr
+
+ movl 0xc(%ebp),%eax # Get Start of IDT
+ movl %eax, Idtr1
+
+ movl $Idtr, %edi # Addr of Idtr -> %edi
+ lidt %es:(%edi)
+
+ popl %edi
+ popl %ebp
+ ret
+
+#----------------------------------------------------------------------------#
+#
+# Protocol prototype
+# None
+#
+# Routine Description:
+#
+# These routines handle the individual interrupts. These routines always
+# gain control on any interrupt or exception. They save EAX and place
+# the interrupt number in EAX. CommonEntry is then jumped to.
+# instruction is invoked.
+#
+# CommonEntry handles all interrupts,does the context save and calls the
+# callback entry, if non-NULL. It is the responsibility of the callback
+# routine to do hardware EOIs. Callbacks are entered into the table
+# located at TableStart. Entries are modified by the InstallInterruptHandler
+# and UninstallInterruptHandler protocols.
+#
+# Arguments to CommonEntry:
+#
+# EAX - Interrupt or exception number
+#
+# TableStart - Pointer to interrupt callback table
+#
+# Returns:
+#
+# Nothing
+#
+#
+# Output: Nothing
+#
+# Destroys: Nothing
+#-----------------------------------------------------------------------------#
+
+TemplateStart:
+ pushl %eax
+
+ #mov eax, 0nnh (nn stands for vector number, which will be fixed at runtime
+ .byte 0xb8
+VectorNumber:
+ .long 0x0
+
+ jmp *CommonInterruptEntry
+TemplateEnd:
+
+CommonEntry:
+
+#---------------------------------------#
+# _CommonEntry #
+#----------------------------------------------------------------------------#
+# The follow algorithm is used for the common interrupt routine.
+# Entry from each interrupt with a push eax and eax=interrupt number
+
+#
+# +---------------------+
+# + EFlags +
+# +---------------------+
+# + CS +
+# +---------------------+
+# + EIP +
+# +---------------------+
+# + Error Code +
+# +---------------------+
+# + EAX / Vector Number +
+# +---------------------+
+# + EBP +
+# +---------------------+ <-- EBP
+#
+
+ cli
+ #
+ # All interrupt handlers are invoked through interrupt gates, so
+ # IF flag automatically cleared at the entry point
+ #
+ cmpl $32,%eax # Intel reserved vector for exceptions?
+ jae NoErrorCode
+ btl %eax, %cs:ASM_PFX(mErrorCodeFlag)
+ jc L1
+
+NoErrorCode:
+ #
+ # Push a dummy error code on the stack
+ # to maintain coherent stack map
+ #
+ pushl (%esp) # Push the value %esp pointing to
+ movl $0, 4(%esp)
+L1:
+ pushl %ebp
+ movl %esp,%ebp
+
+ #
+ # Align stack to make sure that EFI_FX_SAVE_STATE_IA32 of EFI_SYSTEM_CONTEXT_IA32
+ # is 16-byte aligned
+ #
+ andl $0xfffffff0,%esp
+ subl $12,%esp
+
+## UINT32 Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax#
+ pushl 0x4(%ebp)
+ pushl %ecx
+ pushl %edx
+ pushl %ebx
+ leal 24(%ebp),%ecx
+ pushl %ecx # ESP
+ pushl (%ebp)
+ pushl %esi
+ pushl %edi
+
+ movl %eax,4(%ebp) # save vector number
+
+## UINT32 Gs, Fs, Es, Ds, Cs, Ss#
+ movl %ss,%eax
+ pushl %eax
+ movzwl 16(%ebp), %eax
+ pushl %eax
+ movl %ds,%eax
+ pushl %eax
+ movl %es,%eax
+ pushl %eax
+ movl %fs,%eax
+ pushl %eax
+ movl %gs,%eax
+ pushl %eax
+
+## UINT32 Eip#
+ pushl 12(%ebp)
+
+## UINT32 Gdtr[2], Idtr[2]#
+ subl $8,%esp
+ sidt (%esp)
+ subl $8,%esp
+ sgdt (%esp)
+
+## UINT32 Ldtr, Tr#
+ xorl %eax,%eax
+ strl %eax
+ pushl %eax
+ sldtl %eax
+ pushl %eax
+
+## UINT32 EFlags#
+ pushl 20(%ebp)
+
+## UINT32 Cr0, Cr1, Cr2, Cr3, Cr4#
+ movl %cr4, %eax
+ orl $0x208,%eax
+ movl %eax, %cr4
+ pushl %eax
+ movl %cr3, %eax
+ pushl %eax
+ movl %cr2, %eax
+ pushl %eax
+ xorl %eax,%eax
+ pushl %eax
+ movl %cr0, %eax
+ pushl %eax
+
+## UINT32 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7#
+ movl %dr7, %eax
+ pushl %eax
+## clear Dr7 while executing debugger itself
+ xorl %eax, %eax
+ movl %eax, %dr7
+
+ movl %dr6, %eax
+ pushl %eax
+## insure all status bits in dr6 are clear...
+ xorl %eax, %eax
+ movl %eax, %dr6
+
+ movl %dr3, %eax
+ pushl %eax
+ movl %dr2, %eax
+ pushl %eax
+ movl %dr1, %eax
+ pushl %eax
+ movl %dr0, %eax
+ pushl %eax
+
+## FX_SAVE_STATE_IA32 FxSaveState#
+ subl $512, %esp
+ movl %esp, %edi
+ .byte 0x0f, 0xae, 0x07
+
+## UINT32 ExceptionData#
+ pushl 8(%ebp)
+
+## call into exception handler
+ movl 4(%ebp), %ebx
+ movl ExternalVectorTablePtr, %eax
+ movl (%eax,%ebx,4), %eax
+ orl %eax, %eax # NULL?
+ je nonNullValue #
+
+## Prepare parameter and call
+ movl %esp, %edx
+ pushl %edx
+ pushl %ebx
+ call *%eax
+ addl $8, %esp
+
+nonNullValue:
+ cli
+
+## UINT32 ExceptionData#
+ addl $4,%esp
+
+## FX_SAVE_STATE_IA32 FxSaveState#
+ movl %esp, %esi
+ .byte 0x0f, 0xae, 0x0e
+ addl $512, %esp
+
+## UINT32 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7#
+ popl %eax
+ movl %eax, %dr0
+ popl %eax
+ movl %eax, %dr1
+ popl %eax
+ movl %eax, %dr2
+ popl %eax
+ movl %eax, %dr3
+## skip restore of dr6. We cleared dr6 during the context save.
+ addl $4, %esp
+ popl %eax
+ movl %eax, %dr7
+
+## UINT32 Cr0, Cr1, Cr2, Cr3, Cr4#
+ popl %eax
+ movl %eax, %cr0
+ addl $4,%esp # not for Cr1
+ popl %eax
+ movl %eax, %cr2
+ popl %eax
+ movl %eax, %cr3
+ popl %eax
+ movl %eax, %cr4
+
+## UINT32 EFlags#
+ popl 20(%ebp)
+
+## UINT32 Ldtr, Tr#
+## UINT32 Gdtr[2], Idtr[2]#
+## Best not let anyone mess with these particular registers...
+ addl $24,%esp
+
+## UINT32 Eip#
+ pop 12(%ebp)
+
+## UINT32 Gs, Fs, Es, Ds, Cs, Ss#
+## NOTE - modified segment registers could hang the debugger... We
+## could attempt to insulate ourselves against this possibility,
+## but that poses risks as well.
+##
+ popl %gs
+ popl %fs
+ popl %es
+ popl %ds
+ popl 16(%ebp)
+ popl %ss
+
+## UINT32 Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax#
+ popl %edi
+ popl %esi
+ addl $4, %esp # not for ebp
+ addl $4, %esp # not for esp
+ popl %ebx
+ popl %edx
+ popl %ecx
+ popl %eax
+
+ movl %ebp, %esp
+ popl %ebp
+ addl $8,%esp
+ iretl
+
+
+#---------------------------------------#
+# _GetTemplateAddressMap #
+#----------------------------------------------------------------------------#
+#
+# Protocol prototype
+# GetTemplateAddressMap (
+# INTERRUPT_HANDLER_TEMPLATE_MAP *AddressMap
+# )#
+#
+# Routine Description:
+#
+# Return address map of interrupt handler template so that C code can generate
+# interrupt handlers, and dynamically do address fix.
+#
+# Arguments:
+#
+#
+# Returns:
+#
+# Nothing
+#
+#
+# Input: [ebp][0] = Original ebp
+# [ebp][4] = Return address
+#
+# Output: Nothing
+#
+# Destroys: Nothing
+#-----------------------------------------------------------------------------#
+ASM_GLOBAL ASM_PFX(GetTemplateAddressMap)
+ASM_PFX(GetTemplateAddressMap):
+ pushl %ebp # C prolog
+ movl %esp,%ebp
+ pushal
+
+ movl 8(%ebp), %ebx
+ movl $TemplateStart, (%ebx)
+ movl $(TemplateEnd - TemplateStart), 4(%ebx)
+
+ # Note: if code in Template is updated, the value fills into the 3rd parameter
+ # also needs update
+ movl $(VectorNumber - TemplateStart), 8(%ebx)
+
+ popal
+ popl %ebp
+ ret
+
+#---------------------------------------#
+# _InitializeSelectors #
+#----------------------------------------------------------------------------#
+#
+# Protocol prototype
+# InitializeSelectors (
+# )
+#
+# Routine Description:
+#
+# Creates an new GDT in RAM. The problem is that our former selectors
+# were ROM based and the EFI OS Loader does not manipulate the machine state
+# to change them (as it would for a 16-bit PC/AT startup code that had to
+# go from Real Mode to flat mode).
+#
+# Arguments:
+#
+#
+# Returns:
+#
+# Nothing
+#
+#
+# Input: [ebp][0] = Original ebp
+# [ebp][4] = Return address
+#
+# Output: Nothing
+#
+# Destroys: Nothing
+#-----------------------------------------------------------------------------#
+
+.equ CODE_SELECTOR, 0x10
+.equ DATA_SELECTOR, 0x18
+
+ASM_GLOBAL ASM_PFX(InitializeSelectors)
+ASM_PFX(InitializeSelectors):
+ pushl %ebp # C prolog
+ movl %esp,%ebp
+ pushal
+ movl $Gdtr, %edi
+
+ movw %cs,%ax # Get the selector data from our code image
+ .byte 0x66
+ movw %ax,%es
+ lgdt %es:(%edi)
+
+ .byte 0x67
+ .byte 0xea # Far Jump Offset:Selector to reload CS
+ .long SelectorRld
+ .word CODE_SELECTOR
+SelectorRld:
+ movw $DATA_SELECTOR, %ax # Update the Base for the new selectors, too
+ .byte 0x66
+ movw %ax,%ds
+ .byte 0x66
+ movw %ax,%es
+ .byte 0x66
+ movw %ax,%fs
+ .byte 0x66
+ movw %ax,%gs
+ .byte 0x66
+ movw %ax,%ss
+
+ popal
+ popl %ebp
+ ret
+
+#------------------------------------------------------------------------------
+# VOID
+# CpuEnableInterrupt (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(CpuEnableInterrupt)
+ASM_PFX(CpuEnableInterrupt):
+ sti
+ ret
+#CpuEnableInterrupt ENDP
+
+
+#------------------------------------------------------------------------------
+# VOID
+# CpuDisableInterrupt (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(CpuDisableInterrupt)
+ASM_PFX(CpuDisableInterrupt):
+ cli
+ ret
+#CpuDisableInterrupt ENDP
+
+#------------------------------------------------------------------------------
+# VOID
+# CpuInitFloatPointUnit (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(CpuInitFloatPointUnit)
+ASM_PFX(CpuInitFloatPointUnit):
+ finit
+ ret
+#CpuInitFloatPointUnit ENDP
+
+#------------------------------------------------------------------------------
+# UINT16
+# GetCodeSegment (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(GetCodeSegment)
+ASM_PFX(GetCodeSegment):
+ movw %cs, %ax
+ ret
+#GetCodeSegment ENDP
+
+
+#------------------------------------------------------------------------------
+# VOID
+# EfiWbinvd (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(EfiWbinvd)
+ASM_PFX(EfiWbinvd):
+ wbinvd
+ ret
+#EfiWbinvd ENDP
+
+#------------------------------------------------------------------------------
+# VOID
+# EfiInvd (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(EfiInvd)
+ASM_PFX(EfiInvd):
+ invd
+ ret
+#EfiInvd ENDP
+
+#------------------------------------------------------------------------------
+# VOID
+# GetIdt (
+# IDT_INFORMATION *IdtInfo
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(GetIdt)
+ASM_PFX(GetIdt):
+ push %ebp # C prolog
+
+ movl %esp, %ebp
+ movl 8(%ebp), %eax
+ sidt (%eax)
+
+ popl %ebp
+ ret
+#GetIdt ENDP
+
+#------------------------------------------------------------------------------
+# VOID
+# C1eExceptionHandler (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(C1eExceptionHandler)
+ASM_PFX(C1eExceptionHandler):
+ pushl %ebp # C prolog
+ movl %esp, %ebp
+ pushfl
+ cli
+ pushal
+
+ # Verify if GPE was caused by C1e write.
+ # If not, pass control to real exception handler.
+ cmp $0, ASM_PFX(mWroteMsr)
+ je notourexception
+
+ # Fix the return address on stack to skip offending
+ # code which caused the exception.
+ movl 8(%ebp), %eax
+ addl $2, %eax
+ movl %eax, 8(%ebp)
+ jmp exit
+
+notourexception:
+ popal
+ popfl
+ popl %ebp
+
+ jmp ASM_PFX(mOriginalInt13)
+
+exit:
+
+ popal
+ popfl
+ popl %ebp
+ addl $4, %esp
+ iretl
+#C1eExceptionHandler ENDP
+
+
+#------------------------------------------------------------------------------
+# UINT8
+# GetCoreNumber (
+# VOID
+# )
+#------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(GetCoreNumber)
+ASM_PFX(GetCoreNumber):
+
+ pushl %ebx
+
+ movl $4, %eax
+ movl $0, %ecx
+ cpuid
+
+ shrl $26, %eax
+ andl $0x3f, %eax
+ incb %al
+
+ popl %ebx
+
+ ret
+
+#GetCoreNumber ENDP
+
+#-----------------------------------------------------------------------------#
+# data
+#-----------------------------------------------------------------------------#
+
+ .p2align 4
+
+Gdtr: .word GDT_END - GDT_BASE - 1
+ .long GDT_BASE
+
+#-----------------------------------------------------------------------------#
+# global descriptor table (GDT)
+#-----------------------------------------------------------------------------#
+
+ .p2align 4
+
+GDT_BASE:
+# null descriptor
+# .equ NULL_SEL, $-GDT_BASE # Selector [0]
+ .word 0 # limit 15:0
+ .word 0 # base 15:0
+ .byte 0 # base 23:16
+ .byte 0 # type
+ .byte 0 # limit 19:16, flags
+ .byte 0 # base 31:24
+
+# linear data segment descriptor
+# .equ LINEAR_SEL, $-GDT_BASE # Selector [0x8]
+ .word 0xFFFF # limit 0xFFFFF
+ .word 0 # base 0
+ .byte 0
+ .byte 0x92 # present, ring 0, data, expand-up, writable
+ .byte 0xCF # page-granular, 32-bit
+ .byte 0
+
+# linear code segment descriptor
+# .equ LINEAR_CODE_SEL, $-GDT_BASE # Selector [0x10]
+ .word 0xFFFF # limit 0xFFFFF
+ .word 0 # base 0
+ .byte 0
+ .byte 0x9A # present, ring 0, data, expand-up, writable
+ .byte 0xCF # page-granular, 32-bit
+ .byte 0
+
+# system data segment descriptor
+# .equ SYS_DATA_SEL, $-GDT_BASE # Selector [0x18]
+ .word 0xFFFF # limit 0xFFFFF
+ .word 0 # base 0
+ .byte 0
+ .byte 0x92 # present, ring 0, data, expand-up, writable
+ .byte 0xCF # page-granular, 32-bit
+ .byte 0
+
+# system code segment descriptor
+# .equ SYS_CODE_SEL, $-GDT_BASE
+ .word 0xFFFF # limit 0xFFFFF
+ .word 0 # base 0
+ .byte 0
+ .byte 0x9A # present, ring 0, data, expand-up, writable
+ .byte 0xCF # page-granular, 32-bit
+ .byte 0
+
+# spare segment descriptor
+# .equ SPARE3_SEL, $-GDT_BASE
+ .word 0 # limit 0xFFFFF
+ .word 0 # base 0
+ .byte 0
+ .byte 0 # present, ring 0, data, expand-up, writable
+ .byte 0 # page-granular, 32-bit
+ .byte 0
+
+# spare segment descriptor
+# .equ SPARE4_SEL, $-GDT_BASE
+ .word 0 # limit 0xFFFFF
+ .word 0 # base 0
+ .byte 0
+ .byte 0 # present, ring 0, data, expand-up, writable
+ .byte 0 # page-granular, 32-bit
+ .byte 0
+
+# spare segment descriptor
+# .equ SPARE5_SEL, $-GDT_BASE
+ .word 0 # limit 0xFFFFF
+ .word 0 # base 0
+ .byte 0
+ .byte 0 # present, ring 0, data, expand-up, writable
+ .byte 0 # page-granular, 32-bit
+ .byte 0
+
+GDT_END:
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.asm b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.asm
new file mode 100644
index 0000000000..aa6b790064
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/CpuAsm.asm
@@ -0,0 +1,735 @@
+;; @file
+; Assembly code for the IA-32 resources.
+;
+; Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+ page ,132
+ title CPU ARCHITECTURAL DXE PROTOCOL ASSEMBLY HOOKS
+
+.686p
+.model flat
+
+.data
+ExternalVectorTablePtr DD ? ; Table of call backs
+CommonInterruptEntry DD CommonEntry ; Address of CommonEntry
+Idtr DW ? ; FWORD for IDT register
+Idtr1 DD ? ; MUST BE IMMEDIATELY AFTER Idtr
+
+EXTRN _mErrorCodeFlag:DWORD ; Error code flags for exceptions
+
+.stack
+.code
+.MMX
+.XMM
+
+include Htequ.inc
+
+UINT8 TYPEDEF BYTE
+UINT16 TYPEDEF WORD
+UINT32 TYPEDEF DWORD
+UINT64 TYPEDEF QWORD
+UINTN TYPEDEF UINT32
+
+;---------------------------------------;
+; _InitializeIdt ;
+;----------------------------------------------------------------------------;
+;
+; Protocol prototype
+; InitializeIdt (
+; IN EFI_CPU_INTERRUPT_HANDLER TableStart,
+; IN UINTN *IdtTablePtr,
+; IN UINT16 IdtLimit
+; )
+;
+; Routine Description:
+;
+; Creates an IDT table starting at IdtTablPtr. It has IdtLimit/8 entries.
+; Table is initialized to intxx where xx is from 00 to number of entries or
+; 100h, whichever is smaller. After table has been initialized the LIDT
+; instruction is invoked.
+;
+; TableStart is the pointer to the callback table and is not used by
+; InitializedIdt but by commonEntry. CommonEntry handles all interrupts,
+; does the context save and calls the callback entry, if non-NULL.
+; It is the responsibility of the callback routine to do hardware EOIs.
+;
+; Arguments:
+;
+; TableStart - Pointer to interrupt callback table
+;
+; IdtTablePtr - Pointer to IDT table
+;
+; IdtLimit - IDT Table limit = number of interrupt entries * 8
+;
+; Returns:
+;
+; Nothing
+;
+;
+; Input: [ebp][0] = Original ebp
+; [ebp][4] = Return address
+; [ebp][8] = TableStart
+; [ebp][0c] = *IdtTablePtr
+; [ebp][10] = IdtLimit
+;
+; Output: Nothing
+;
+; Destroys: Nothing
+;-----------------------------------------------------------------------------;
+
+_InitializeIdt proc near public
+ push ebp ; C prolog
+ mov ebp, esp
+ push edi
+
+ mov eax, [ebp+8] ; Get ExternalVectorTable Address
+ mov ExternalVectorTablePtr, eax
+
+ mov ax, [ebp+10h] ; Get IDT Table limit
+ dec ax
+ mov Idtr, ax
+
+ mov eax, [ebp+0ch] ; Get Start of IDT
+ mov Idtr1, eax
+
+ mov edi, OFFSET Idtr ; Load IDT register
+ lidt FWORD PTR es:[edi]
+
+ pop edi
+ pop ebp
+ ret
+_InitializeIdt endp
+
+;----------------------------------------------------------------------------;
+;
+; Protocol prototype
+; None
+;
+; Routine Description:
+;
+; These routines handle the individual interrupts. These routines always
+; gain control on any interrupt or exception. They save EAX and place
+; the interrupt number in EAX. CommonEntry is then jumped to.
+; instruction is invoked.
+;
+; CommonEntry handles all interrupts,does the context save and calls the
+; callback entry, if non-NULL. It is the responsibility of the callback
+; routine to do hardware EOIs. Callbacks are entered into the table
+; located at TableStart. Entries are modified by the InstallInterruptHandler
+; and UninstallInterruptHandler protocols.
+;
+; Arguments to CommonEntry:
+;
+; EAX - Interrupt or exception number
+;
+; TableStart - Pointer to interrupt callback table
+;
+; Returns:
+;
+; Nothing
+;
+;
+; Output: Nothing
+;
+; Destroys: Nothing
+;-----------------------------------------------------------------------------;
+
+TemplateStart:
+ push eax
+
+ ;mov eax, 0nnh (nn stands for vector number, which will be fixed at runtime
+ DB 0b8h
+VectorNumber:
+ DD 00h
+
+ jmp dword ptr [CommonInterruptEntry];
+TemplateEnd:
+
+CommonEntry:
+
+;---------------------------------------;
+; _CommonEntry ;
+;----------------------------------------------------------------------------;
+; The follow algorithm is used for the common interrupt routine.
+; Entry from each interrupt with a push eax and eax=interrupt number
+
+;
+; +---------------------+
+; + EFlags +
+; +---------------------+
+; + CS +
+; +---------------------+
+; + EIP +
+; +---------------------+
+; + Error Code +
+; +---------------------+
+; + EAX / Vector Number +
+; +---------------------+
+; + EBP +
+; +---------------------+ <-- EBP
+;
+
+ cli
+ ;
+ ; All interrupt handlers are invoked through interrupt gates, so
+ ; IF flag automatically cleared at the entry point
+ ;
+ cmp eax, 32 ; Intel reserved vector for exceptions?
+ jae NoErrorCode
+ bt cs:_mErrorCodeFlag, eax
+ jc @F
+
+NoErrorCode:
+ ;
+ ; Push a dummy error code on the stack
+ ; to maintain coherent stack map
+ ;
+ push [esp]
+ mov dword ptr [esp + 4], 0
+@@:
+ push ebp
+ mov ebp, esp
+
+ ;
+ ; Align stack to make sure that EFI_FX_SAVE_STATE_IA32 of EFI_SYSTEM_CONTEXT_IA32
+ ; is 16-byte aligned
+ ;
+ and esp, 0fffffff0h
+ sub esp, 12
+
+;; UINT32 Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+ push dword ptr [ebp + 4] ; EAX
+ push ecx
+ push edx
+ push ebx
+ lea ecx, [ebp + 24]
+ push ecx ; ESP
+ push dword ptr [ebp] ; EBP
+ push esi
+ push edi
+
+ mov [ebp + 4], eax ; save vector number
+
+;; UINT32 Gs, Fs, Es, Ds, Cs, Ss;
+ mov eax, ss
+ push eax
+ movzx eax, word ptr [ebp + 16]
+ push eax
+ mov eax, ds
+ push eax
+ mov eax, es
+ push eax
+ mov eax, fs
+ push eax
+ mov eax, gs
+ push eax
+
+;; UINT32 Eip;
+ push dword ptr [ebp + 12]
+
+;; UINT32 Gdtr[2], Idtr[2];
+ sub esp, 8
+ sidt fword ptr [esp]
+ sub esp, 8
+ sgdt fword ptr [esp]
+
+;; UINT32 Ldtr, Tr;
+ xor eax, eax
+ str ax
+ push eax
+ sldt ax
+ push eax
+
+;; UINT32 EFlags;
+ push dword ptr [ebp + 20]
+
+;; UINT32 Cr0, Cr1, Cr2, Cr3, Cr4;
+ mov eax, cr4
+ or eax, 208h
+ mov cr4, eax
+ push eax
+ mov eax, cr3
+ push eax
+ mov eax, cr2
+ push eax
+ xor eax, eax
+ push eax
+ mov eax, cr0
+ push eax
+
+;; UINT32 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+ mov eax, dr7
+ push eax
+;; clear Dr7 while executing debugger itself
+ xor eax, eax
+ mov dr7, eax
+
+ mov eax, dr6
+ push eax
+;; insure all status bits in dr6 are clear...
+ xor eax, eax
+ mov dr6, eax
+
+ mov eax, dr3
+ push eax
+ mov eax, dr2
+ push eax
+ mov eax, dr1
+ push eax
+ mov eax, dr0
+ push eax
+
+;; FX_SAVE_STATE_IA32 FxSaveState;
+ sub esp, 512
+ mov edi, esp
+ db 0fh, 0aeh, 00000111y ;fxsave [edi]
+
+;; UINT32 ExceptionData;
+ push dword ptr [ebp + 8]
+
+;; call into exception handler
+ mov ebx, [ebp + 4]
+ mov eax, ExternalVectorTablePtr
+ mov eax, [eax + ebx * 4]
+ or eax, eax ; NULL?
+ je nonNullValue;
+
+;; Prepare parameter and call
+ mov edx, esp
+ push edx
+ push ebx
+ call eax
+ add esp, 8
+
+nonNullValue:
+ cli
+;; UINT32 ExceptionData;
+ add esp, 4
+
+;; FX_SAVE_STATE_IA32 FxSaveState;
+ mov esi, esp
+ db 0fh, 0aeh, 00001110y ; fxrstor [esi]
+ add esp, 512
+
+;; UINT32 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+ pop eax
+ mov dr0, eax
+ pop eax
+ mov dr1, eax
+ pop eax
+ mov dr2, eax
+ pop eax
+ mov dr3, eax
+;; skip restore of dr6. We cleared dr6 during the context save.
+ add esp, 4
+ pop eax
+ mov dr7, eax
+
+;; UINT32 Cr0, Cr1, Cr2, Cr3, Cr4;
+ pop eax
+ mov cr0, eax
+ add esp, 4 ; not for Cr1
+ pop eax
+ mov cr2, eax
+ pop eax
+ mov cr3, eax
+ pop eax
+ mov cr4, eax
+
+;; UINT32 EFlags;
+ pop dword ptr [ebp + 20]
+
+;; UINT32 Ldtr, Tr;
+;; UINT32 Gdtr[2], Idtr[2];
+;; Best not let anyone mess with these particular registers...
+ add esp, 24
+
+;; UINT32 Eip;
+ pop dword ptr [ebp + 12]
+
+;; UINT32 Gs, Fs, Es, Ds, Cs, Ss;
+;; NOTE - modified segment registers could hang the debugger... We
+;; could attempt to insulate ourselves against this possibility,
+;; but that poses risks as well.
+;;
+ pop gs
+ pop fs
+ pop es
+ pop ds
+ pop dword ptr [ebp + 16]
+ pop ss
+
+;; UINT32 Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+ pop edi
+ pop esi
+ add esp, 4 ; not for ebp
+ add esp, 4 ; not for esp
+ pop ebx
+ pop edx
+ pop ecx
+ pop eax
+
+ mov esp, ebp
+ pop ebp
+ add esp, 8
+ iretd
+
+
+;---------------------------------------;
+; _GetTemplateAddressMap ;
+;----------------------------------------------------------------------------;
+;
+; Protocol prototype
+; GetTemplateAddressMap (
+; INTERRUPT_HANDLER_TEMPLATE_MAP *AddressMap
+; );
+;
+; Routine Description:
+;
+; Return address map of interrupt handler template so that C code can generate
+; interrupt handlers, and dynamically do address fix.
+;
+; Arguments:
+;
+;
+; Returns:
+;
+; Nothing
+;
+;
+; Input: [ebp][0] = Original ebp
+; [ebp][4] = Return address
+;
+; Output: Nothing
+;
+; Destroys: Nothing
+;-----------------------------------------------------------------------------;
+_GetTemplateAddressMap proc near public
+ push ebp ; C prolog
+ mov ebp, esp
+ pushad
+
+ mov ebx, dword ptr [ebp+08h]
+ mov dword ptr [ebx], TemplateStart
+ mov dword ptr [ebx+4h], TemplateEnd - TemplateStart
+
+ ; if code in Template is updated, the value fills into the 3rd parameter
+ ; also needs update
+ mov dword ptr [ebx+8h], VectorNumber - TemplateStart
+
+ popad
+ pop ebp
+ ret
+_GetTemplateAddressMap endp
+
+
+
+;---------------------------------------;
+; _InitializeSelectors ;
+;----------------------------------------------------------------------------;
+;
+; Protocol prototype
+; InitializeSelectors (
+; )
+;
+; Routine Description:
+;
+; Creates an new GDT in RAM. The problem is that our former selectors
+; were ROM based and the EFI OS Loader does not manipulate the machine state
+; to change them (as it would for a 16-bit PC/AT startup code that had to
+; go from Real Mode to flat mode).
+;
+; Arguments:
+;
+;
+; Returns:
+;
+; Nothing
+;
+;
+; Input: [ebp][0] = Original ebp
+; [ebp][4] = Return address
+;
+; Output: Nothing
+;
+; Destroys: Nothing
+;-----------------------------------------------------------------------------;
+
+CODE_SELECTOR EQU 10h
+DATA_SELECTOR EQU 18h
+
+_InitializeSelectors proc near public
+ push ebp ; C prolog
+ mov ebp, esp
+ pushad
+ mov edi, OFFSET Gdtr ; Load GDT register
+
+ mov ax,cs ; Get the selector data from our code image
+ mov es,ax
+ lgdt FWORD PTR es:[edi] ; and update the GDTR
+
+ db 067h
+ db 0eah ; Far Jump Offset:Selector to reload CS
+ dd OFFSET SelectorRld; Offset is ensuing instruction boundary
+ dw CODE_SELECTOR ; Selector is our code selector, 10h
+SelectorRld::
+ mov ax, DATA_SELECTOR ; Update the Base for the new selectors, too
+ mov ds, ax
+ mov es, ax
+ mov fs, ax
+ mov gs, ax
+ mov ss, ax
+
+ popad
+ pop ebp
+ ret
+_InitializeSelectors endp
+
+;------------------------------------------------------------------------------
+; VOID
+; CpuEnableInterrupt (
+; VOID
+; )
+;------------------------------------------------------------------------------
+CpuEnableInterrupt PROC C PUBLIC
+ sti
+ ret
+CpuEnableInterrupt ENDP
+
+
+;------------------------------------------------------------------------------
+; VOID
+; CpuDisableInterrupt (
+; VOID
+; )
+;------------------------------------------------------------------------------
+CpuDisableInterrupt PROC C PUBLIC
+ cli
+ ret
+CpuDisableInterrupt ENDP
+
+;------------------------------------------------------------------------------
+; VOID
+; CpuInitFloatPointUnit (
+; VOID
+; )
+;------------------------------------------------------------------------------
+CpuInitFloatPointUnit PROC C PUBLIC
+ finit
+ ret
+CpuInitFloatPointUnit ENDP
+
+;------------------------------------------------------------------------------
+; UINT16
+; GetCodeSegment (
+; VOID
+; )
+;------------------------------------------------------------------------------
+GetCodeSegment PROC C PUBLIC
+ mov ax,cs
+ ret
+GetCodeSegment ENDP
+
+;------------------------------------------------------------------------------
+; VOID
+; EfiWbinvd (
+; VOID
+; )
+;------------------------------------------------------------------------------
+EfiWbinvd PROC PUBLIC
+ wbinvd
+ ret
+EfiWbinvd ENDP
+
+;------------------------------------------------------------------------------
+; VOID
+; EfiInvd (
+; VOID
+; )
+;------------------------------------------------------------------------------
+EfiInvd PROC PUBLIC
+ invd
+ ret
+EfiInvd ENDP
+
+;------------------------------------------------------------------------------
+; VOID
+; GetIdt (
+; IDT_INFORMATION *IdtInfo
+; )
+;------------------------------------------------------------------------------
+_GetIdt proc near public
+ push ebp ; C prolog
+
+ mov ebp, esp
+ mov eax, [ebp+8]
+ sidt FWORD PTR [eax]
+
+ pop ebp
+ ret
+_GetIdt ENDP
+
+;------------------------------------------------------------------------------
+; VOID
+; C1eExceptionHandler (
+; VOID
+; )
+;------------------------------------------------------------------------------
+#if 0
+_C1eExceptionHandler proc near public
+ push ebp ; C prolog
+ mov ebp, esp
+ pushfd
+ cli
+ pushad
+
+ ; Verify if GPE was caused by C1e write.
+ ; If not, pass control to real exception handler.
+ cmp _mWroteMsr, 0
+ je notourexception
+
+ ; Fix the return address on stack to skip offending
+ ; code which caused the exception.
+ mov eax, DWORD PTR [ebp+8]
+ add eax, 2
+ mov DWORD PTR [ebp+8], eax
+ jmp exit
+
+notourexception:
+ popad
+ popfd
+ pop ebp
+
+ jmp DWORD PTR _mOriginalInt13
+
+exit:
+
+ popad
+ popfd
+ pop ebp
+ add esp, 4
+ iretd
+_C1eExceptionHandler ENDP
+#endif
+GetCoreNumber PROC C PUBLIC
+
+ push ebx
+
+ mov eax, 4
+ mov ecx, 0
+ cpuid
+
+ shr eax, 26
+ and eax, 3fh
+ inc al
+
+ pop ebx
+
+ ret
+
+GetCoreNumber ENDP
+
+
+;-----------------------------------------------------------------------------;
+; data
+;-----------------------------------------------------------------------------;
+
+ align 16
+
+gdtr dw GDT_END - GDT_BASE - 1 ; GDT limit
+ dd OFFSET GDT_BASE ; (GDT base gets set above)
+
+;-----------------------------------------------------------------------------;
+; global descriptor table (GDT)
+;-----------------------------------------------------------------------------;
+
+ align 16
+
+public GDT_BASE
+GDT_BASE:
+; null descriptor
+NULL_SEL equ $-GDT_BASE ; Selector [0]
+ dw 0 ; limit 15:0
+ dw 0 ; base 15:0
+ db 0 ; base 23:16
+ db 0 ; type
+ db 0 ; limit 19:16, flags
+ db 0 ; base 31:24
+
+; linear data segment descriptor
+LINEAR_SEL equ $-GDT_BASE ; Selector [0x8]
+ dw 0FFFFh ; limit 0xFFFFF
+ dw 0 ; base 0
+ db 0
+ db 092h ; present, ring 0, data, expand-up, writable
+ db 0CFh ; page-granular, 32-bit
+ db 0
+
+; linear code segment descriptor
+LINEAR_CODE_SEL equ $-GDT_BASE ; Selector [0x10]
+ dw 0FFFFh ; limit 0xFFFFF
+ dw 0 ; base 0
+ db 0
+ db 09Ah ; present, ring 0, data, expand-up, writable
+ db 0CFh ; page-granular, 32-bit
+ db 0
+
+; system data segment descriptor
+SYS_DATA_SEL equ $-GDT_BASE ; Selector [0x18]
+ dw 0FFFFh ; limit 0xFFFFF
+ dw 0 ; base 0
+ db 0
+ db 092h ; present, ring 0, data, expand-up, writable
+ db 0CFh ; page-granular, 32-bit
+ db 0
+
+; system code segment descriptor
+SYS_CODE_SEL equ $-GDT_BASE
+ dw 0FFFFh ; limit 0xFFFFF
+ dw 0 ; base 0
+ db 0
+ db 09Ah ; present, ring 0, data, expand-up, writable
+ db 0CFh ; page-granular, 32-bit
+ db 0
+
+; spare segment descriptor
+SPARE3_SEL equ $-GDT_BASE
+ dw 0 ; limit 0xFFFFF
+ dw 0 ; base 0
+ db 0
+ db 0 ; present, ring 0, data, expand-up, writable
+ db 0 ; page-granular, 32-bit
+ db 0
+
+; spare segment descriptor
+SPARE4_SEL equ $-GDT_BASE
+ dw 0 ; limit 0xFFFFF
+ dw 0 ; base 0
+ db 0
+ db 0 ; present, ring 0, data, expand-up, writable
+ db 0 ; page-granular, 32-bit
+ db 0
+
+; spare segment descriptor
+SPARE5_SEL equ $-GDT_BASE
+ dw 0 ; limit 0xFFFFF
+ dw 0 ; base 0
+ db 0
+ db 0 ; present, ring 0, data, expand-up, writable
+ db 0 ; page-granular, 32-bit
+ db 0
+
+GDT_END:
+
+
+ end \ No newline at end of file
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Exception.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Exception.c
new file mode 100644
index 0000000000..a97d7b8b3d
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Exception.c
@@ -0,0 +1,336 @@
+/** @file
+ IA-32 Exception Handler.
+
+ Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include "CpuInitDxe.h"
+#include "MpCommon.h"
+#include "Exception.h"
+#include <Library/BaseMemoryLib.h>
+
+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_IA32),
+ { 0 }
+ },
+ {
+ 0
+ }
+};
+
+/**
+ Reports StatusCode for Exception
+
+ This function reports status code for exception.
+
+ @param[in] InterruptType Interrupt type
+ @param[in] SystemContext EFI_SYSTEM_CONTEXT
+
+**/
+EFI_STATUS
+ReportData (
+ IN EFI_EXCEPTION_TYPE InterruptType,
+ IN EFI_SYSTEM_CONTEXT SystemContext
+ )
+{
+ UINT32 ErrorMessage;
+ UINT32 Index;
+
+ CopyMem (
+ &mStatusCodeData.SystemContext.SystemContextIa32,
+ SystemContext.SystemContextIa32,
+ sizeof (EFI_SYSTEM_CONTEXT_IA32)
+ );
+
+ 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 Pointer to EFI_SYSTEM_CONTEXT.
+
+**/
+VOID
+EFIAPI
+CommonExceptionHandler (
+ IN EFI_EXCEPTION_TYPE InterruptType,
+ IN EFI_SYSTEM_CONTEXT SystemContext
+ )
+{
+ DEBUG ((
+ DEBUG_ERROR,
+ "!!!! IA32 Exception Type - %08x !!!!\n",
+ InterruptType
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "EIP - %08x, CS - %08x, EFLAGS - %08x\n",
+ SystemContext.SystemContextIa32->Eip,
+ SystemContext.SystemContextIa32->Cs,
+ SystemContext.SystemContextIa32->Eflags
+ ));
+ if (mErrorCodeFlag & (1 << InterruptType)) {
+ DEBUG ((
+ DEBUG_ERROR,
+ "ExceptionData - %08x\n",
+ SystemContext.SystemContextIa32->ExceptionData
+ ));
+ }
+ DEBUG ((
+ DEBUG_ERROR,
+ "EAX - %08x, ECX - %08x, EDX - %08x, EBX - %08x\n",
+ SystemContext.SystemContextIa32->Eax,
+ SystemContext.SystemContextIa32->Ecx,
+ SystemContext.SystemContextIa32->Edx,
+ SystemContext.SystemContextIa32->Ebx
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "ESP - %08x, EBP - %08x, ESI - %08x, EDI - %08x\n",
+ SystemContext.SystemContextIa32->Esp,
+ SystemContext.SystemContextIa32->Ebp,
+ SystemContext.SystemContextIa32->Esi,
+ SystemContext.SystemContextIa32->Edi
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "DS - %08x, ES - %08x, FS - %08x, GS - %08x, SS - %08x\n",
+ SystemContext.SystemContextIa32->Ds,
+ SystemContext.SystemContextIa32->Es,
+ SystemContext.SystemContextIa32->Fs,
+ SystemContext.SystemContextIa32->Gs,
+ SystemContext.SystemContextIa32->Ss
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "GDTR - %08x %08x, IDTR - %08x %08x\n",
+ SystemContext.SystemContextIa32->Gdtr[0],
+ SystemContext.SystemContextIa32->Gdtr[1],
+ SystemContext.SystemContextIa32->Idtr[0],
+ SystemContext.SystemContextIa32->Idtr[1]
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "LDTR - %08x, TR - %08x\n",
+ SystemContext.SystemContextIa32->Ldtr,
+ SystemContext.SystemContextIa32->Tr
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "CR0 - %08x, CR2 - %08x, CR3 - %08x, CR4 - %08x\n",
+ SystemContext.SystemContextIa32->Cr0,
+ SystemContext.SystemContextIa32->Cr2,
+ SystemContext.SystemContextIa32->Cr3,
+ SystemContext.SystemContextIa32->Cr4
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "DR0 - %08x, DR1 - %08x, DR2 - %08x, DR3 - %08x\n",
+ SystemContext.SystemContextIa32->Dr0,
+ SystemContext.SystemContextIa32->Dr1,
+ SystemContext.SystemContextIa32->Dr2,
+ SystemContext.SystemContextIa32->Dr3
+ ));
+ DEBUG ((
+ DEBUG_ERROR,
+ "DR6 - %08x, DR7 - %08x\n",
+ SystemContext.SystemContextIa32->Dr6,
+ SystemContext.SystemContextIa32->Dr7
+ ));
+
+ //
+ // Report Status Code
+ //
+ ReportData (InterruptType, SystemContext);
+
+ //
+ // 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.
+ //
+ CpuDeadLoop ();
+
+ 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;
+
+ mStatusCodeData.Header.Type = gCpuStatusCodeDataTypeExceptionHandlerGuid;
+ Status = CpuProtocol->DisableInterrupt (CpuProtocol);
+
+ for (Index = 0; Index < mExceptionNumber; Index ++) {
+ Status = CpuProtocol->RegisterInterruptHandler (
+ CpuProtocol,
+ mExceptionTable[Index].Interrupt,
+ NULL
+ );
+
+ Status = CpuProtocol->RegisterInterruptHandler (
+ CpuProtocol,
+ mExceptionTable[Index].Interrupt,
+ CommonExceptionHandler
+ );
+ ASSERT_EFI_ERROR (Status);
+ }
+
+ return EFI_SUCCESS;
+}
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Htequ.inc b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Htequ.inc
new file mode 100644
index 0000000000..e4669587c8
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/Htequ.inc
@@ -0,0 +1,41 @@
+;; @file
+; This is the equates file for HT (Hyper-threading) support
+;
+; Copyright (c) 2000 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+VacantFlag EQU 00h
+NotVacantFlag EQU 0ffh
+StartupApSignal EQU 6E755200h
+MonitorFilterSize EQU 40h
+ApCounterInit EQU 0
+ApInHltLoop EQU 1
+ApInMwaitLoop EQU 2
+ApInRunLoop EQU 3
+WakeUpApPerMwaitLoop32 Equ 4
+WakeUpApPerRunLoop32 Equ 5
+
+LockLocation EQU 1000h - 0400h
+StackStart EQU LockLocation + 4h
+StackSize EQU LockLocation + 8h
+RendezvousProc EQU LockLocation + 0Ch
+GdtrProfile EQU LockLocation + 10h
+IdtrProfile EQU LockLocation + 16h
+BufferStart EQU LockLocation + 1Ch
+Cr3Location EQU LockLocation + 20h
+InitFlag EQU LockLocation + 24h
+WakeUpApManner EQU LockLocation + 28h
+BistBuffer EQU LockLocation + 2Ch
+PAUSE32 MACRO
+ DB 0F3h
+ DB 090h
+ ENDM
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.S b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.S
new file mode 100644
index 0000000000..a2dab52f57
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.S
@@ -0,0 +1,451 @@
+## @file
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+#
+##
+
+##include Htequ.inc
+
+.equ VacantFlag , 0x00
+.equ NotVacantFlag , 0xff
+.equ StartupApSignal , 0x6E755200
+.equ MonitorFilterSize, 0x40
+.equ ApCounterInit , 0
+.equ ApInHltLoop , 1
+.equ ApInMwaitLoop , 2
+.equ ApInRunLoop , 3
+
+.equ LockLocation , 0x1000 - 0x0400
+.equ StackStart , LockLocation + 0x4
+.equ StackSize , LockLocation + 0x8
+.equ RendezvousProc , LockLocation + 0x0C
+.equ GdtrProfile , LockLocation + 0x10
+.equ IdtrProfile , LockLocation + 0x16
+.equ BufferStart , LockLocation + 0x1C
+.equ Cr3Location , LockLocation + 0x20
+.equ InitFlag , LockLocation + 0x24
+.equ WakeUpApManner , LockLocation + 0x28
+.equ BistBuffer , LockLocation + 0x2C
+
+#-------------------------------------------------------------------------------------
+
+.macro PAUSE32
+ .byte 0xF3
+ .byte 0x90
+.endm
+
+.macro FJMP32 Selector, Offset
+ .byte 0x066
+ .byte 0x067
+ .byte 0x0EA # far jump
+ .long \Offset # 32-bit offset
+ .word \Selector # 16-bit selector
+.endm
+
+.macro FCALL32 Selector, Offset
+ .byte 0x09A
+ .long \Offset # 32-bit offset
+ .word \Selector # 16-bit selector
+.endm
+
+#-------------------------------------------------------------------------------------
+#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)
+
+ASM_GLOBAL ASM_PFX(RendezvousFunnelProc)
+ASM_PFX(RendezvousFunnelProc):
+RendezvousFunnelProcStart:
+
+# At this point CS = 0x(vv00) and ip= 0x0.
+
+ .byte 0x66,0x8b,0xe8 # mov ebp, eax
+
+ .byte 0x8c,0xc8 # mov ax, cs
+ .byte 0x8e,0xd8 # mov ds, ax
+ .byte 0x8e,0xc0 # mov es, ax
+ .byte 0x8e,0xd0 # mov ss, ax
+ .byte 0x33,0xc0 # xor ax, ax
+ .byte 0x8e,0xe0 # mov fs, ax
+ .byte 0x8e,0xe8 # mov gs, ax
+
+# Get APIC ID
+#
+ .byte 0x66, 0xB8
+ .long 0x00000001 # mov %eax, 1
+ .byte 0x0F, 0xA2 # cpuid
+ .byte 0x66, 0xC1, 0xEB, 0x18 # shr %ebx, 24
+ .byte 0x66, 0x81, 0xE3
+ .long 0x000000FF # and %ebx, 0ffh # EBX is keeping APIC ID
+
+# If it is the first time AP wakes up, just record APs BIST
+# Otherwise, switch to flat mode
+
+ .byte 0xBE, 0x24, 0x0C # mov si, InitFlag
+ .byte 0x66, 0x83, 0x3C, 0x01 # cmp dword ptr [si], 1
+ .byte 0x75, 0x18 # jnz flat32Start
+
+# Record BIST information
+#
+ .byte 0xB0, 0x08 # mov al, 8
+ .byte 0xF6, 0xE3 # mul bl
+
+ .byte 0xBE, 0x2C, 0x0C # mov si, BistBuffer
+ .byte 0x03, 0xF0 # add si, ax
+
+ .byte 0x66, 0xC7, 0x04
+ .long 0x00000001 # mov dword ptr [si], 1 # Set Valid Flag
+ .byte 0x66, 0x89, 0x6C, 0x04 # mov dword ptr [si + 4], ebp # Store BIST value
+
+ cli
+ hlt
+ jmp .-2
+
+# Switch to flat mode.
+
+flat32Start:
+
+ .byte 0xBE, 0x1C, 0x0C # mov si, BufferStart
+ .byte 0x66, 0x8B, 0x0C # mov ecx,dword ptr [si] # ECX is keeping the start address of wakeup buffer
+
+ .byte 0xFA # cli
+ .byte 0xBE, 0x10, 0x0C # mov si, GdtrProfile
+ .byte 0x66 # db 66h
+ .byte 0x2E, 0x0F, 0x01, 0x14 # lgdt fword ptr cs:[si]
+
+ .byte 0xBE, 0x16, 0x0C # mov si, IdtrProfile
+ .byte 0x66 # db 66h
+ .byte 0x2E, 0x0F, 0x01, 0x1C # lidt fword ptr cs:[si]
+
+
+ .byte 0x33, 0xC0 # xor ax, ax
+ .byte 0x8E, 0xD8 # mov ds, ax
+ .byte 0x0F, 0x20, 0xC0 # mov eax, cr0 # Get control register 0
+ .byte 0x66, 0x83, 0xC8, 0x01 # or eax, 000000001h # Set PE bit (bit #0)
+ .byte 0x0F, 0x22, 0xC0 # mov cr0, eax
+
+
+#step-4:
+
+FLAT32_JUMP:
+ FJMP32 0x10, 0x0 # Far jmp using code segment descriptor
+
+PMODE_ENTRY: # protected mode entry point
+
+ movw $0x8, %ax
+ movw %ax, %ds
+ movw %ax, %es
+ movw %ax, %fs
+ movw %ax, %gs
+ movw %ax, %ss # Flat mode setup.
+
+ movl %ecx,%esi
+
+ movl %esi,%edi
+ addl $InitFlag, %edi
+ cmpl $2, (%edi) # Check whether in S3 boot path
+ jz ProgramDynamicStack
+
+ProgramStaticStack:
+
+ xorl %ecx, %ecx
+ movl %esi, %edi
+ addl $BistBuffer, %edi
+ movl (%edi, %ebx, 8), %ecx # EBX = CpuNumber
+
+ movl %esi, %edi
+ addl $StackSize, %edi
+ movl (%edi), %eax
+ incl %ecx
+ mull %ecx # EAX = StackSize * (CpuNumber + 1)
+
+ movl %esi, %edi
+ addl $StackStart, %edi
+ movl (%edi), %edx
+ addl %edx, %eax # EAX = StackStart + StackSize * (CpuNumber + 1)
+
+ movl %eax, %esp
+ subl $MonitorFilterSize, %esp # Reserved Monitor data space
+ orl $StartupApSignal, %ebx # EBX = #Cpu run signature
+ jmp ProgramLocalApic
+
+ProgramDynamicStack:
+
+ movl %esi, %edi
+ addl $LockLocation, %edi
+ movb $NotVacantFlag, %al
+TestLock:
+ xchgb %al, (%edi)
+ cmpb $NotVacantFlag, %al
+ jz TestLock
+
+ movl %esi, %edi
+ addl $StackSize, %edi
+ movl (%edi), %eax
+ movl %esi, %edi
+ addl $StackStart, %edi
+ addl (%edi), %eax
+ movl %eax, %esp
+ movl %eax, (%edi)
+
+Releaselock:
+ movb $VacantFlag, %al
+ movl %esi, %edi
+ addl $LockLocation, %edi
+ xchgb %al, (%edi)
+
+ProgramLocalApic:
+
+ movl $0x0FEE000F0, %edi
+ movl (%edi), %eax
+ andl $0x0FFFFFD0F, %eax
+ orl $0x10F, %eax
+ movl %eax, (%edi)
+
+ movl $0x0FEE00350, %edi
+ movl (%edi), %eax
+ andl $0x0FFFE00FF, %eax
+ orl $0x700, %eax
+ movl %eax, (%edi)
+
+ movl $0x0FEE00360, %edi
+ movl (%edi), %eax
+ andl $0x0FFFE00FF, %eax
+ orl $0x10400, %eax
+ movl %eax, (%edi)
+
+EnableXmm:
+ movl $1, %eax
+ cpuid
+ btl $0x1A, %edx
+ jnc L1
+ #
+ # Enable XMM
+ #
+ movl %cr0, %eax
+ orl $2, %eax
+ movl %eax, %cr0
+ movl %cr4, %eax
+ orl $0x600, %eax
+ movl %eax, %cr4
+
+L1:
+ #
+ # Call C Function
+ #
+ movl %esi, %edi
+ addl $RendezvousProc, %edi
+ addl $WakeUpApManner, %esi # esi = WakeUpApManner Address Location
+
+WakeUpThisAp:
+
+ movl (%edi), %eax
+
+ testl %eax, %eax
+ jz CheckWakeUpCounterInit
+
+ push %ebx
+ push %ebx
+ push %esi
+ push %edi
+
+ subl $0x20, %esp
+ call *%eax # Call C function
+ addl $0x20, %esp
+
+ pop %edi
+ pop %esi
+ pop %ebx
+ pop %ebx
+
+CheckWakeUpCounterInit:
+ cmpl $ApCounterInit, (%esi)
+ jnz CheckWakeUpManner
+
+#
+# Initialize MONITOR_MWAIT_DATA data structure per thread
+#
+ xorl %ecx, %ecx
+ movl %ecx, 0(%esp) # BreakToRunApSignal
+ movl %ecx, 4(%esp) # HltLoopBreakCounter
+ movl %ecx, 8(%esp) # MwaitLoopBreakCounter
+ movl %ecx, 12(%esp) # RunLoopBreakCounter
+ movl %ecx, 16(%esp) # WakeUpApVectorChangeFlag
+ movl %ecx, 20(%esp) # MwaitTargetCstate
+
+CheckWakeUpManner:
+
+ cmpl $ApInHltLoop, (%esi)
+ jz HltApLoop
+
+ cmpl $ApInMwaitLoop, (%esi)
+ jnz CheckRunSignal
+
+ApMwaitLoop:
+
+ cli
+ movl %esp, %eax # Set Monitor Address
+ xorl %ecx, %ecx
+ xorl %edx, %edx
+ .byte 0x0f, 0x1, 0xc8 # MONITOR
+ movl 20(%esp), %eax # Mwait Target C-State per rax[7:4]
+ .byte 0x0f, 0x1, 0xc9 # MWAIT
+
+CheckRunSignal:
+
+ cmpl %ebx, (%esp) # Check if run sign%al correct?
+ jnz CheckWakeUpManner # Unknown break, go checking run manner
+
+ jmp WakeUpThisAp # Jmp to execute AP task
+
+HltApLoop:
+
+ cli
+ hlt
+ jmp HltApLoop
+
+#RendezvousFunnelProc ENDP
+RendezvousFunnelProcEnd:
+
+#-------------------------------------------------------------------------------------
+# AsmGetAddressMap (&AddressMap)
+#-------------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(AsmGetAddressMap)
+ASM_PFX(AsmGetAddressMap):
+ pushal
+ movl %esp, %ebp
+
+ movl 0x24(%ebp), %ebx
+ movl $RendezvousFunnelProcStart, (%ebx)
+ movl $(PMODE_ENTRY - RendezvousFunnelProcStart), 0x4(%ebx)
+ movl $(FLAT32_JUMP - RendezvousFunnelProcStart), 0x8(%ebx)
+ movl $(RendezvousFunnelProcEnd - RendezvousFunnelProcStart), 0xc(%ebx)
+
+ popal
+ ret
+#AsmGetAddressMap ENDP
+
+#-------------------------------------------------------------------------------------
+#AsmExchangeRole procedure follows. This procedure executed by current BSP, that is
+#about to become an AP. It switches its stack with the current AP.
+#AsmExchangeRole (IN CPU_EXCHANGE_INFO *MyInfo, IN CPU_EXCHANGE_INFO *OthersInfo)#
+#-------------------------------------------------------------------------------------
+.equ CPU_SWITCH_STATE_IDLE, 0
+.equ CPU_SWITCH_STATE_STORED, 1
+.equ CPU_SWITCH_STATE_LOADED, 2
+
+ASM_GLOBAL ASM_PFX(AsmExchangeRole)
+ASM_PFX(AsmExchangeRole):
+ # 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.
+ pushal
+ movl %esp, %ebp
+
+ # %esi contains MyInfo pointer
+ movl 0x24(%ebp), %esi
+
+ # %edi contains OthersInfo pointer
+ movl 0x28(%ebp), %edi
+
+ #Store EFLAGS, GDTR and IDTR regiter to stack
+ pushfl
+ sgdt 8(%esi)
+ sidt 14(%esi)
+
+ # Store the its StackPointer
+ movl %esp, 4(%esi)
+
+ # update its switch state to STORED
+ movb $NotVacantFlag, %al
+TryLock1:
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%esi), %al
+ cmpb $VacantFlag, %al
+ jz LockObtained1
+ PAUSE32
+ jmp TryLock1
+
+LockObtained1:
+ movb $CPU_SWITCH_STATE_STORED, 1(%esi)
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%esi), %al
+WaitForOtherStored:
+ # wait until the other CPU finish storing its state
+ movb $NotVacantFlag, %al
+TryLock2:
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%edi), %al
+ cmpb $VacantFlag, %al
+ jz LockObtained2
+ PAUSE32
+ jmp TryLock2
+
+LockObtained2:
+ movb 1(%edi), %bl
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%edi), %al
+ cmpb $CPU_SWITCH_STATE_STORED, %bl
+ jb WaitForOtherStored
+
+ # Since another CPU already stored its state, load them
+ # load GDTR value
+ lgdt 8(%edi)
+
+ # load IDTR value
+ lidt 14(%edi)
+
+ # load its future StackPointer
+ movl 4(%edi), %esp
+
+ # update its switch state to LOADED
+ movb $NotVacantFlag, %al
+TryLock3:
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%esi), %al
+ cmpb $VacantFlag, %al
+ jz LockObtained3
+ PAUSE32
+ jmp TryLock3
+
+LockObtained3:
+ movb $CPU_SWITCH_STATE_LOADED, 1(%esi)
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%esi), %al
+
+WaitForOtherLoaded:
+ # wait until the other CPU finish loading new state,
+ # otherwise the data in stack may corrupt
+ movb $NotVacantFlag, %al
+TryLock4:
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%edi), %al
+ cmpb $VacantFlag, %al
+ jz LockObtained4
+ PAUSE32
+ jmp TryLock4
+
+LockObtained4:
+ movb 1(%edi), %bl
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%edi), %al
+ cmpb $CPU_SWITCH_STATE_LOADED, %bl
+ jb WaitForOtherLoaded
+
+ # since the other CPU already get the data it want, leave this procedure
+ popfl
+
+ popal
+ ret
+#AsmExchangeRole ENDP
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.asm b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.asm
new file mode 100644
index 0000000000..a8ebb473e7
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MPFuncs32.asm
@@ -0,0 +1,443 @@
+;; @file
+; Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+.686p
+.model flat
+.code
+
+include Htequ.inc
+;-------------------------------------------------------------------------------------
+FJMP32 MACRO Selector, Offset
+ DB 066h
+ DB 067h
+ DB 0EAh ; far jump
+ DD Offset ; 32-bit offset
+ DW Selector ; 16-bit selector
+ ENDM
+FCALL32 MACRO Selector, Offset
+ DB 09Ah
+ DD Offset ; 32-bit offset
+ DW Selector ; 16-bit selector
+ ENDM
+;-------------------------------------------------------------------------------------
+;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);
+
+RendezvousFunnelProc PROC near C 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 keeping APIC ID
+
+; If it is the first time AP wakes up, just record AP's BIST
+; Otherwise, switch to flat mode
+
+ db 0BEh, 24h, 0Ch ; mov si, InitFlag
+ db 66h, 83h, 3Ch, 01h ; cmp dword ptr [si], 1
+ db 75h ; opcode jnz
+ db flat32Start - ($ + 1) ; jnz flat32Start
+
+; Record BIST information
+;
+ db 0B0h, 08h ; mov al, 8
+ db 0F6h, 0E3h ; mul bl
+
+ db 0BEh, 2Ch, 0Ch ; 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
+
+; cli
+; hlt
+; jmp $-2
+
+; Switch to flat mode.
+
+flat32Start::
+
+ db 0BEh, 1Ch, 0Ch ; mov si, BufferStart
+ db 66h, 8Bh, 0Ch ; mov ecx,dword ptr [si] ; ECX is keeping the start address of wakeup buffer
+
+ db 0FAh ; cli
+ db 0BEh, 10h, 0Ch ; mov si, GdtrProfile
+ db 66h ; db 66h
+ db 2Eh,0Fh, 01h, 14h ; lgdt fword ptr cs:[si]
+
+ db 0BEh, 16h, 0Ch ; mov si, IdtrProfile
+ db 66h ; db 66h
+ db 2Eh,0Fh, 01h, 1Ch ; lidt fword ptr cs:[si]
+
+
+ db 33h, 0C0h ; xor ax, ax
+ db 8Eh, 0D8h ; mov ds, ax
+ db 0Fh, 20h, 0C0h ; mov eax, cr0 ; Get control register 0
+ db 66h, 83h, 0C8h, 01h ; or eax, 000000001h ; Set PE bit (bit #0)
+ db 0Fh, 22h, 0C0h ; mov cr0, eax
+
+
+;step-4:
+
+FLAT32_JUMP::
+ FJMP32 010h,0h ; Far jmp using code segment descriptor
+
+PMODE_ENTRY:: ; protected mode entry point
+
+ mov ax, 8h
+ mov ds, ax
+ mov es, ax
+ mov fs, ax
+
+WaitFirstApTaskAssigned::
+;
+; First INIT-SIPI-SIPI will loop here until DetailedMpInitialization function assigned for each AP
+;
+
+ pause
+ cmp dword ptr [ecx+RendezvousProc], 0
+ jz WaitFirstApTaskAssigned
+
+ mov gs, ax
+ mov ss, ax ; Flat mode setup.
+
+ mov esi, ecx
+
+ mov edi, esi
+ add edi, InitFlag
+ cmp dword ptr [edi], 2 ; Check whether in S3 boot path
+ jz ProgramDynamicStack
+
+ProgramStaticStack::
+
+ xor ecx, ecx
+ mov edi, esi
+ add edi, BistBuffer
+ mov ecx, dword ptr [edi + 8 * ebx] ; EBX = CpuNumber
+
+ mov edi, esi
+ add edi, StackSize
+ mov eax, dword ptr [edi]
+ inc ecx
+ mul ecx ; EAX = StackSize * (CpuNumber + 1)
+
+ mov edi, esi
+ add edi, StackStart
+ mov edx, dword ptr [edi]
+ add eax, edx ; EAX = StackStart + StackSize * (CpuNumber + 1)
+
+ mov esp, eax
+ sub esp, MonitorFilterSize ; Reserved Monitor data space
+ or ebx, StartupApSignal ; ebx = #Cpu run signature
+ jmp ProgramLocalApic
+
+ProgramDynamicStack::
+
+ mov edi, esi
+ add edi, LockLocation
+ mov al, NotVacantFlag
+TestLock::
+ xchg byte ptr [edi], al
+ cmp al, NotVacantFlag
+ jz TestLock
+
+ mov edi, esi
+ add edi, StackSize
+ mov eax, dword ptr [edi]
+ mov edi, esi
+ add edi, StackStart
+ add eax, dword ptr [edi]
+ mov esp, eax
+ mov dword ptr [edi], eax
+
+Releaselock::
+ mov al, VacantFlag
+ mov edi, esi
+ add edi, LockLocation
+ xchg byte ptr [edi], al
+
+ProgramLocalApic::
+
+ mov edi, 0FEE000F0h
+ mov eax, dword ptr [edi]
+ and eax, 0FFFFFD0Fh
+ or eax, 10Fh
+ mov dword ptr [edi], eax
+
+ mov edi, 0FEE00350h
+ mov eax, dword ptr [edi]
+ and eax, 0FFFE00FFh
+ or eax, 700h
+ mov dword ptr [edi], eax
+
+ mov edi, 0FEE00360h
+ mov eax, dword ptr [edi]
+ and eax, 0FFFE00FFh
+ or eax, 10400h
+ mov dword ptr [edi], eax
+
+EnableXmm::
+ mov eax, 1
+ cpuid
+ bt edx,1Ah
+ jnc @F
+ ;
+ ; Enable XMM
+ ;
+ mov eax,cr0
+ or eax, 2
+ mov cr0, eax
+ mov eax, cr4
+ or eax, 600h
+ mov cr4, eax
+
+@@:
+ ;
+ ; Call C Function
+ ;
+ mov edi, esi
+ add edi, RendezvousProc
+ add esi, WakeUpApManner ; esi = WakeUpApManner Address Location
+
+WakeUpThisAp::
+
+ mov eax, dword ptr [edi]
+
+ test eax, eax
+ jz CheckWakeUpCounterInit
+
+ push ebx
+ push ebx
+ push esi
+ push edi
+
+ sub esp, 20h
+ call eax ; Call C function
+ add esp, 20h
+
+ pop edi
+ pop esi
+ pop ebx
+ pop ebx
+
+CheckWakeUpCounterInit::
+ cmp dword ptr [esi], ApCounterInit
+ jnz CheckWakeUpManner
+
+;
+; Initialize MONITOR_MWAIT_DATA data structure per thread
+;
+ xor ecx, ecx
+ mov dword ptr [esp + 0], ecx ; BreakToRunApSignal
+ mov dword ptr [esp + 4], ecx ; HltLoopBreakCounter
+ mov dword ptr [esp + 8], ecx ; MwaitLoopBreakCounter
+ mov dword ptr [esp + 12], ecx ; RunLoopBreakCounter
+ mov dword ptr [esp + 16], ecx ; MwaitLoopBreakCounter32
+ mov dword ptr [esp + 20], ecx ; RunLoopBreakCounter32
+ mov dword ptr [esp + 24], ecx ; WakeUpApVectorChangeFlag
+ mov dword ptr [esp + 28], ecx ; MwaitTargetCstate
+
+WaitWakeUpMannerAssigned::
+ pause
+ cmp dword ptr [esi], ApCounterInit
+ jz WaitWakeUpMannerAssigned
+
+CheckWakeUpManner::
+
+ cmp dword ptr [esi], ApInHltLoop
+ jz HltApLoop
+
+ cmp dword ptr [esi], ApInMwaitLoop
+ jnz CheckRunSignal
+
+ApMwaitLoop::
+
+ cli
+ mov eax, esp ; Set Monitor Address
+ xor ecx, ecx
+ xor edx, edx
+ DB 0fh, 1, 0c8h ; MONITOR
+ mov eax, dword ptr [esp + 28] ; Mwait Target C-State per rax[7:4]
+ DB 0fh, 1, 0c9h ; MWAIT
+
+CheckRunSignal::
+
+ cmp dword ptr [esp], ebx ; Check if run signal correct?
+ jnz CheckWakeUpManner ; Unknown break, go checking run manner
+
+ jmp WakeUpThisAp ; Jmp to execute AP task
+
+HltApLoop::
+
+ cli
+ hlt
+ jmp HltApLoop
+
+RendezvousFunnelProc ENDP
+RendezvousFunnelProcEnd::
+;-------------------------------------------------------------------------------------
+; AsmGetAddressMap (&AddressMap);
+;-------------------------------------------------------------------------------------
+AsmGetAddressMap PROC near C PUBLIC
+
+ pushad
+ mov ebp,esp
+
+ mov ebx, dword ptr [ebp+24h]
+ mov dword ptr [ebx], RendezvousFunnelProcStart
+ mov dword ptr [ebx+4h], PMODE_ENTRY - RendezvousFunnelProcStart
+ mov dword ptr [ebx+8h], FLAT32_JUMP - RendezvousFunnelProcStart
+ mov dword ptr [ebx+0ch], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
+
+ popad
+ ret
+AsmGetAddressMap 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 near C 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.
+ pushad
+ mov ebp,esp
+
+ ; esi contains MyInfo pointer
+ mov esi, dword ptr [ebp+24h]
+
+ ; edi contains OthersInfo pointer
+ mov edi, dword ptr [ebp+28h]
+
+ ;Store EFLAGS, GDTR and IDTR regiter to stack
+ pushfd
+ sgdt fword ptr [esi+8]
+ sidt fword ptr [esi+14]
+
+ ; Store the its StackPointer
+ mov dword ptr [esi+4],esp
+
+ ; update its switch state to STORED
+ mov al, NotVacantFlag
+TryLock1:
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [esi]
+ cmp al, VacantFlag
+ jz LockObtained1
+ PAUSE32
+ jmp TryLock1
+
+LockObtained1:
+ mov byte ptr [esi+1], CPU_SWITCH_STATE_STORED
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [esi]
+
+WaitForOtherStored::
+ ; wait until the other CPU finish storing its state
+ mov al, NotVacantFlag
+TryLock2:
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [edi]
+ cmp al, VacantFlag
+ jz LockObtained2
+ PAUSE32
+ jmp TryLock2
+
+LockObtained2:
+ mov bl, byte ptr [edi+1]
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [edi]
+ cmp bl, CPU_SWITCH_STATE_STORED
+ jb WaitForOtherStored
+
+ ; Since another CPU already stored its state, load them
+ ; load GDTR value
+ lgdt fword ptr [edi+8]
+
+ ; load IDTR value
+ lidt fword ptr [edi+14]
+
+ ; load its future StackPointer
+ mov esp, dword ptr [edi+4]
+
+ ; update its switch state to LOADED
+ mov al, NotVacantFlag
+TryLock3:
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [esi]
+ cmp al, VacantFlag
+ jz LockObtained3
+ PAUSE32
+ jmp TryLock3
+
+LockObtained3:
+ mov byte ptr [esi+1], CPU_SWITCH_STATE_LOADED
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [esi]
+
+WaitForOtherLoaded::
+ ; wait until the other CPU finish loading new state,
+ ; otherwise the data in stack may corrupt
+ mov al, NotVacantFlag
+TryLock4:
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [edi]
+ cmp al, VacantFlag
+ jz LockObtained4
+ PAUSE32
+ jmp TryLock4
+
+LockObtained4:
+ mov bl, byte ptr [edi+1]
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [edi]
+ cmp bl, CPU_SWITCH_STATE_LOADED
+ jb WaitForOtherLoaded
+
+ ; since the other CPU already get the data it want, leave this procedure
+ popfd
+
+ popad
+ ret
+AsmExchangeRole ENDP
+END
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.S b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.S
new file mode 100644
index 0000000000..0b524a3dac
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.S
@@ -0,0 +1,25 @@
+## @file
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+#
+##
+
+ASM_GLOBAL ASM_PFX(EnableMCE)
+ASM_PFX(EnableMCE):
+ push %ebp # C prolog
+ movl %esp, %ebp
+
+ movl %cr4, %eax
+ orl $0x40, %eax
+ movl %eax, %cr4
+
+ pop %ebp
+ ret
+#EnableMCE endp
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.asm b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.asm
new file mode 100644
index 0000000000..49da369faa
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MchkInitAsm.asm
@@ -0,0 +1,36 @@
+;; @file
+; Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+.686p
+.model flat
+.data
+.stack
+.code
+.MMX
+.XMM
+
+_EnableMCE proc near public
+ push ebp ; C prolog
+ mov ebp, esp
+
+ mov eax, cr4
+ or eax, 40h
+ mov cr4, eax
+
+ pop ebp
+ ret
+_EnableMCE endp
+
+
+ end
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MemoryOperation.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MemoryOperation.c
new file mode 100644
index 0000000000..2cbcfba987
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MemoryOperation.c
@@ -0,0 +1,519 @@
+/** @file
+ Memory Operation Functions for IA32 Architecture.
+
+ Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "CpuInitDxe.h"
+#include "MpCommon.h"
+#include "MpService.h"
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#if defined (__GNUC__)
+#define ALIGN_16BYTE_BOUNDRY __attribute__ ((aligned (16)))
+#else
+#define ALIGN_16BYTE_BOUNDRY __declspec (align (16))
+#endif
+
+typedef struct {
+ VOID *Start;
+ UINTN Size;
+ UINTN FixOffset;
+} INTERRUPT_HANDLER_TEMPLATE_MAP;
+
+/**
+ Get the address map of interrupt handler template
+
+ @param[out] AddressMap The address map of interrupt handle.
+
+**/
+VOID
+GetTemplateAddressMap (
+ OUT INTERRUPT_HANDLER_TEMPLATE_MAP *AddressMap
+ );
+
+
+/**
+ Get the code segment
+
+ @retval UINT16 The value of code segment
+
+**/
+UINT16
+GetCodeSegment (
+ VOID
+ );
+
+
+/**
+ Get GDTR and IDTR
+
+ @param[out] Gdt Pointer to GDT table.
+ @param[out] Idt Pointer to IDT table.
+
+**/
+VOID
+AsmGetGdtrIdtr (
+ OUT IA32_DESCRIPTOR **Gdt,
+ OUT IA32_DESCRIPTOR **Idt
+ );
+
+
+/**
+ Initialize Interrupt descriptor Tables
+
+ @param[in] TableStart Pointer to interrupt callback table.
+ @param[in] IdtTablePtr Pointer to IDT table.
+ @param[in] IdtTableLimit IDT Table limit.
+
+**/
+VOID
+InitializeIdt (
+ IN EFI_CPU_INTERRUPT_HANDLER *TableStart,
+ IN UINTN *IdtTablePtr,
+ IN UINT16 IdtTableLimit
+ );
+
+INTERRUPT_GATE_DESCRIPTOR gIdtTable[INTERRUPT_VECTOR_NUMBER] = { 0 };
+
+INTERRUPT_GATE_DESCRIPTOR gOrigIdtTable[INTERRUPT_VECTOR_NUMBER] = { { 0, 0, 0, 0 } };
+UINTN mOrigIdtEntryCount = 0;
+
+ALIGN_16BYTE_BOUNDRY IA32_DESCRIPTOR gLidtPseudoDescriptor = {
+ sizeof (gIdtTable) - 1,
+ (UINTN) gIdtTable
+};
+
+extern EFI_CPU_INTERRUPT_HANDLER mExternalVectorTable[];
+extern EFI_PHYSICAL_ADDRESS mBackupBuffer;
+
+UINT64 mValidMtrrAddressMask = 0xFFFFFF000;
+UINT64 mValidMtrrBitsMask = 0xFFFFFFFFF;
+
+/**
+ Initialize Interrupt descriptor Tables
+
+**/
+VOID
+InitializeInterruptTables (
+ VOID
+ )
+{
+ INTERRUPT_HANDLER_TEMPLATE_MAP TemplateMap;
+ UINT16 CodeSegment;
+ INTERRUPT_GATE_DESCRIPTOR *IdtEntry;
+ UINT8 *InterruptHandler;
+ UINT8 *CurrentHandler;
+ UINTN Index;
+ IA32_DESCRIPTOR IdtrForBsp;
+
+ IdtEntry = gIdtTable;
+
+ GetTemplateAddressMap (&TemplateMap);
+ InterruptHandler = AllocatePool (TemplateMap.Size * INTERRUPT_VECTOR_NUMBER);
+ CurrentHandler = InterruptHandler;
+
+ CodeSegment = GetCodeSegment ();
+
+ for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index++) {
+ CopyMem (CurrentHandler, TemplateMap.Start, TemplateMap.Size);
+ *(UINT32 *) (CurrentHandler + TemplateMap.FixOffset) = Index;
+
+ IdtEntry[Index].OffsetLow = (UINT16) (UINTN) CurrentHandler;
+ IdtEntry[Index].SegmentSelector = CodeSegment;
+ IdtEntry[Index].Attributes = INTERRUPT_GATE_ATTRIBUTE;
+ //
+ // 8e00;
+ //
+ IdtEntry[Index].OffsetHigh = (UINT16) ((UINTN) CurrentHandler >> 16);
+
+ CurrentHandler += TemplateMap.Size;
+ }
+
+ AsmReadIdtr (&IdtrForBsp);
+
+ //
+ // Size of the IDT currently programmed (number of entries)
+ //
+ mOrigIdtEntryCount = (IdtrForBsp.Limit + 1) / sizeof (INTERRUPT_GATE_DESCRIPTOR);
+
+ //
+ // Update debugger CS with DxeCpu CS.
+ //
+ IdtEntry = (INTERRUPT_GATE_DESCRIPTOR *) IdtrForBsp.Base;
+ for (Index = 0; Index < mOrigIdtEntryCount; Index++) {
+ IdtEntry[Index].SegmentSelector = CodeSegment;
+ }
+ //
+ // Save a copy of the original IDT
+ //
+ CopyMem (&gOrigIdtTable, (VOID *) IdtrForBsp.Base, IdtrForBsp.Limit + 1);
+
+ InitializeIdt (
+ &(mExternalVectorTable[0]),
+ (UINTN *) gIdtTable,
+ sizeof (INTERRUPT_GATE_DESCRIPTOR) * INTERRUPT_VECTOR_NUMBER
+ );
+
+ return;
+}
+
+
+/**
+ Set Interrupt Descriptor Table Handler Address.
+
+ @param[in] Index The Index of the interrupt descriptor table handle.
+
+**/
+VOID
+SetInterruptDescriptorTableHandlerAddress (
+ IN UINTN Index
+ )
+{
+ IA32_DESCRIPTOR IdtrForBsp;
+ INTERRUPT_GATE_DESCRIPTOR *IdtEntry;
+
+ AsmReadIdtr (&IdtrForBsp);
+ IdtEntry = (INTERRUPT_GATE_DESCRIPTOR *) IdtrForBsp.Base;
+
+ //
+ // Plug in CPU Driver version
+ //
+ CopyMem (&IdtEntry[Index], &gIdtTable[Index], sizeof (INTERRUPT_GATE_DESCRIPTOR));
+}
+
+
+/**
+ Restore original Interrupt Descriptor Table Handler Address.
+
+ @param[in] Index The Index of the interrupt descriptor table handle.
+
+**/
+VOID
+RestoreInterruptDescriptorTableHandlerAddress (
+ IN UINTN Index
+ )
+{
+ IA32_DESCRIPTOR IdtrForBsp;
+ INTERRUPT_GATE_DESCRIPTOR *IdtEntry;
+
+ if (Index >= mOrigIdtEntryCount) {
+ return;
+ }
+
+ AsmReadIdtr (&IdtrForBsp);
+ IdtEntry = (INTERRUPT_GATE_DESCRIPTOR *) IdtrForBsp.Base;
+ CopyMem (&IdtEntry[Index], &gOrigIdtTable[Index], sizeof (INTERRUPT_GATE_DESCRIPTOR));
+}
+
+
+/**
+ Prepare memory for essential system tables.
+
+ @retval EFI_SUCCESS Memory successfully prepared.
+
+**/
+EFI_STATUS
+PrepareMemory (
+ VOID
+ )
+{
+ ZeroMem (mExternalVectorTable, 0x100 * 4);
+
+ //
+ // Initialize the Interrupt Descriptor Table
+ //
+ InitializeInterruptTables ();
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Prepare Wakeup Buffer and stack for APs.
+
+ @param[out] WakeUpBuffer Pointer to the address of wakeup buffer for output.
+ @param[out] 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 64 aligned 4K aligned area for IA-32 to use broadcast APIs. But we need it only
+ // on a temporary basis.
+ //
+ Status = AllocateWakeUpBuffer (WakeUpBuffer);
+ if (EFI_ERROR (Status)) {
+ return Status;
+ }
+
+ //
+ // Claim memory for AP stack.
+ //
+ Status = gBS->AllocatePool (
+ EfiACPIMemoryNVS,
+ 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);
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Prepare exchange information for APs.
+
+ @param[out] 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
+ )
+{
+ gBS->SetMem ((VOID *) ExchangeInfo, EFI_PAGE_SIZE - MP_CPU_EXCHANGE_INFO_OFFSET, 0);
+
+ ExchangeInfo->Lock = VacantFlag;
+ ExchangeInfo->StackStart = StackAddressStart;
+ ExchangeInfo->StackSize = STACK_SIZE_PER_PROC;
+ ExchangeInfo->ApFunction = ApFunction;
+
+ CopyMem (&ExchangeInfo->GdtrProfile, (VOID *) (UINTN) mAcpiCpuData->GdtrProfile, sizeof (IA32_DESCRIPTOR));
+ CopyMem (&ExchangeInfo->IdtrProfile, (VOID *) (UINTN) mAcpiCpuData->IdtrProfile, sizeof (IA32_DESCRIPTOR));
+
+ ExchangeInfo->BufferStart = (UINT32) WakeUpBuffer;
+ ExchangeInfo->InitFlag = 1;
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Prepare Wakeup Buffer and stack for APs during S3.
+
+ @param[out] WakeUpBuffer Pointer to the address of wakeup buffer for output.
+ @param[out] 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
+ )
+{
+ MP_ASSEMBLY_ADDRESS_MAP AddressMap;
+
+ *WakeUpBuffer = mAcpiCpuData->WakeUpBuffer;
+ *StackAddressStart = (VOID *) (UINTN) mAcpiCpuData->StackAddress;
+
+ AsmGetAddressMap (&AddressMap);
+ CopyMem ((VOID *) (UINTN) *WakeUpBuffer, AddressMap.RendezvousFunnelAddress, AddressMap.Size);
+ *(UINT32 *) (UINTN) (*WakeUpBuffer + AddressMap.FlatJumpOffset + 3) = (UINT32) (*WakeUpBuffer + AddressMap.PModeEntryOffset);
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Prepare exchange information for APs during S3.
+
+ @param[out] 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
+ )
+{
+ ExchangeInfo->Lock = VacantFlag;
+ ExchangeInfo->StackStart = (VOID *) (UINTN) StackAddressStart;
+ ExchangeInfo->StackSize = STACK_SIZE_PER_PROC;
+ ExchangeInfo->ApFunction = ApFunction;
+
+ CopyMem (&ExchangeInfo->GdtrProfile, (VOID *) (UINTN) mAcpiCpuData->GdtrProfile, sizeof (IA32_DESCRIPTOR));
+ CopyMem (&ExchangeInfo->IdtrProfile, (VOID *) (UINTN) mAcpiCpuData->IdtrProfile, sizeof (IA32_DESCRIPTOR));
+
+ ExchangeInfo->BufferStart = (UINT32) WakeUpBuffer;
+ ExchangeInfo->InitFlag = 2;
+
+ //
+ // There is no need to initialize CpuNumber and BistBuffer fields in ExchangeInfo here.
+ //
+ 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) (mBackupBuffer + AddressMap.FlatJumpOffset + 3) = (UINT32) (mAcpiCpuData->WakeUpBuffer + AddressMap.PModeEntryOffset);
+
+ return;
+}
+
+
+/**
+ Set specified IDT entry with given function pointer.
+
+ @param[in] FunctionPointer Function pointer for IDT entry.
+ @param[out] IdtEntry The IDT entry to update.
+
+ @retval UINTN The original IDT entry value.
+
+**/
+UINTN
+SetIdtEntry (
+ IN UINTN FunctionPointer,
+ OUT INTERRUPT_GATE_DESCRIPTOR *IdtEntry
+)
+{
+ UINTN OriginalEntry;
+
+ OriginalEntry = ((UINT32) IdtEntry->OffsetHigh << 16) + IdtEntry->OffsetLow;
+
+ IdtEntry->OffsetLow = (UINT16) FunctionPointer;
+ IdtEntry->OffsetHigh = (UINT16) (FunctionPointer >> 16);
+
+ return OriginalEntry;
+}
+
+
+/**
+ Prepare GDTR and IDTR for AP
+
+ @param[out] Gdtr The GDTR profile
+ @param[out] 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 IA32_DESCRIPTOR *Gdtr,
+ OUT IA32_DESCRIPTOR *Idtr
+ )
+{
+ INTERRUPT_GATE_DESCRIPTOR *IdtForAP;
+ SEGMENT_DESCRIPTOR *GdtForAP;
+ IA32_DESCRIPTOR *IdtrForBSP;
+ IA32_DESCRIPTOR *GdtrForBSP;
+ UINT8 *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 (
+ EfiACPIMemoryNVS,
+ SIZE_OF_MCE_HANDLER,
+ (VOID **) &MceHandler
+ );
+ if (EFI_ERROR (Status)) {
+ return Status;
+ }
+ //
+ // McheHandler content: iret (opcode = 0xcf)
+ //
+ *MceHandler = 0xCF;
+
+ CopyMem (GdtForAP, (VOID *) GdtrForBSP->Base, GdtrForBSP->Limit + 1);
+ CopyMem (IdtForAP, (VOID *) IdtrForBSP->Base, IdtrForBSP->Limit + 1);
+
+ IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].OffsetLow = (UINT16) (UINTN) MceHandler;
+ IdtForAP[INTERRUPT_HANDLER_MACHINE_CHECK].OffsetHigh = (UINT16) ((UINTN) MceHandler >> 16);
+
+ //
+ // 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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.S b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.S
new file mode 100644
index 0000000000..b3bcd128b0
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.S
@@ -0,0 +1,113 @@
+## @file
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+#
+##
+
+##include Htequ.inc
+
+.equ VacantFlag , 0x00
+.equ NotVacantFlag , 0xff
+.equ StartupApSignal , 0x6E755200
+.equ MonitorFilterSize, 0x40
+.equ ApCounterInit , 0
+.equ ApInHltLoop , 1
+.equ ApInMwaitLoop , 2
+.equ ApInRunLoop , 3
+
+.equ LockLocation , 0x1000 - 0x0400
+.equ StackStart , LockLocation + 0x4
+.equ StackSize , LockLocation + 0x8
+.equ RendezvousProc , LockLocation + 0x0C
+.equ GdtrProfile , LockLocation + 0x10
+.equ IdtrProfile , LockLocation + 0x16
+.equ BufferStart , LockLocation + 0x1C
+.equ Cr3Location , LockLocation + 0x20
+.equ InitFlag , LockLocation + 0x24
+.equ WakeUpApManner , LockLocation + 0x28
+.equ BistBuffer , LockLocation + 0x2C
+
+.macro PAUSE32
+ .byte 0xF3
+ .byte 0x90
+.endm
+
+#-------------------------------------------------------------------------------
+# AsmAcquireMPLock (&Lock)
+#-------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(AsmAcquireMPLock)
+ASM_PFX(AsmAcquireMPLock):
+ pushal
+ movl %esp, %ebp
+
+ movb $NotVacantFlag, %al
+ movl 0x24(%ebp), %ebx
+TryGetLock:
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%ebx), %al
+ cmpb $VacantFlag, %al
+ jz LockObtained
+
+ PAUSE32
+
+ jmp TryGetLock
+
+LockObtained:
+ popal
+ ret
+#AsmAcquireMPLock ENDP
+
+#-------------------------------------------------------------------------------
+# AsmReleaseMPLock (&Lock)
+#-------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(AsmReleaseMPLock)
+ASM_PFX(AsmReleaseMPLock):
+ pushal
+ movl %esp, %ebp
+
+ movb $VacantFlag, %al
+ movl 0x24(%ebp), %ebx
+ .byte 0xF0 # opcode for lock instruction
+ xchgb (%ebx), %al
+
+ popal
+ ret
+#AsmReleaseMPLock ENDP
+
+#-------------------------------------------------------------------------------
+# AsmGetGdtrIdtr (&Gdt, &Idt)#
+#-------------------------------------------------------------------------------------
+ASM_GLOBAL ASM_PFX(AsmGetGdtrIdtr)
+ASM_PFX(AsmGetGdtrIdtr):
+ pushal
+ movl %esp, %ebp
+ sgdt GdtDesc
+ leal GdtDesc, %esi
+ movl 0x24(%ebp), %edi
+ movl %esi, (%edi)
+
+ sidt IdtDesc
+ leal IdtDesc, %esi
+ movl 0x28(%ebp), %edi
+ movl %esi, (%edi)
+
+ popal
+ ret
+#AsmGetGdtrIdtr ENDP
+
+GdtDesc: # GDT descriptor
+ .word 0x03f # GDT limit
+ .word 0x0 # GDT base and limit will be
+ .word 0x0 # filled using sgdt
+
+IdtDesc: # IDT descriptor
+ .word 0x0 # IDT limit
+ .word 0x0 # IDT base and limit will be
+ .word 0x0 # filled using sidt
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.asm b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.asm
new file mode 100644
index 0000000000..bd0514121d
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCommon32.asm
@@ -0,0 +1,102 @@
+;; @file
+; Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+.686p
+.model flat
+.data
+.stack
+.code
+.MMX
+.XMM
+
+ include Htequ.inc
+PAUSE32 MACRO
+ DB 0F3h
+ DB 090h
+ ENDM
+
+;-------------------------------------------------------------------------------
+; AsmAcquireMPLock (&Lock);
+;-------------------------------------------------------------------------------
+AsmAcquireMPLock PROC near C PUBLIC
+
+ pushad
+ mov ebp,esp
+
+ mov al, NotVacantFlag
+ mov ebx, dword ptr [ebp+24h]
+TryGetLock:
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [ebx]
+ cmp al, VacantFlag
+ jz LockObtained
+
+ PAUSE32
+ jmp TryGetLock
+
+LockObtained:
+ popad
+ ret
+AsmAcquireMPLock ENDP
+
+;-------------------------------------------------------------------------------
+; AsmReleaseMPLock (&Lock);
+;-------------------------------------------------------------------------------------
+AsmReleaseMPLock PROC near C PUBLIC
+
+ pushad
+ mov ebp,esp
+
+ mov al, VacantFlag
+ mov ebx, dword ptr [ebp+24h]
+ db 0f0h ; opcode for lock instruction
+ xchg al, byte ptr [ebx]
+
+ popad
+ ret
+AsmReleaseMPLock ENDP
+
+;-------------------------------------------------------------------------------
+; AsmGetGdtrIdtr (&Gdt, &Idt);
+;-------------------------------------------------------------------------------------
+AsmGetGdtrIdtr PROC near C PUBLIC
+
+ pushad
+ mov ebp,esp
+
+ sgdt fword ptr GdtDesc
+ lea esi, GdtDesc
+ mov edi, dword ptr [ebp+24h]
+ mov dword ptr [edi], esi
+
+ sidt fword ptr IdtDesc
+ lea esi, IdtDesc
+ mov edi, dword ptr [ebp+28h]
+ mov dword ptr [edi], esi
+
+ popad
+ ret
+AsmGetGdtrIdtr ENDP
+
+GdtDesc:: ; GDT descriptor
+ DW 03fh ; GDT limit
+ DW 0h ; GDT base and limit will be
+ DW 0h ; filled using sgdt
+
+IdtDesc:: ; IDT descriptor
+ DW 0h ; IDT limit
+ DW 0h ; IDT base and limit will be
+ DW 0h ; filled using sidt
+
+END
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCpu.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCpu.c
new file mode 100644
index 0000000000..af41e76ffa
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpCpu.c
@@ -0,0 +1,69 @@
+/** @file
+ MP Support functions.
+
+ Copyright (c) 2007 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <MpService.h>
+
+GLOBAL_REMOVE_IF_UNREFERENCED ACPI_CPU_DATA *mAcpiCpuData;
+GLOBAL_REMOVE_IF_UNREFERENCED 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;
+
+ MpCpuReservedData = NULL;
+
+ Status = AllocateReservedMemoryBelow4G (
+ sizeof (MP_CPU_RESERVED_DATA),
+ (VOID **) &MpCpuReservedData
+ );
+
+ if (EFI_ERROR (Status)) {
+ return Status;
+ }
+
+ if (MpCpuReservedData != NULL) {
+ ZeroMem (MpCpuReservedData, sizeof (MP_CPU_RESERVED_DATA));
+ } else {
+ ASSERT (MpCpuReservedData != NULL);
+ return EFI_OUT_OF_RESOURCES;
+ }
+
+ mMPSystemData = &(MpCpuReservedData->MPSystemData);
+ mAcpiCpuData = &(MpCpuReservedData->AcpiCpuData);
+ mAcpiCpuData->CpuPrivateData = (EFI_PHYSICAL_ADDRESS) (UINTN) (&(mMPSystemData->S3DataPointer));
+ mAcpiCpuData->S3BootPath = FALSE;
+ mAcpiCpuData->IdtrProfile = (EFI_PHYSICAL_ADDRESS) (UINTN) (&(MpCpuReservedData->IdtrProfile));
+
+ InitializeMpServices ();
+
+ return EFI_SUCCESS;
+}
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.S b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.S
new file mode 100644
index 0000000000..c74b640286
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.S
@@ -0,0 +1,67 @@
+## @file
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+#
+##
+
+ASM_GLOBAL ASM_PFX(MpMtrrSynchUpEntry)
+ASM_PFX(MpMtrrSynchUpEntry):
+ #
+ # Enter no fill cache mode, CD=1(Bit30), NW=0 (Bit29)
+ #
+ movl %cr0, %eax
+ andl $0x0DFFFFFFF, %eax
+ orl $0x040000000, %eax
+ movl %eax, %cr0
+ #
+ # Flush cache
+ #
+ wbinvd
+ #
+ # Clear PGE flag Bit 7
+ #
+ movl %cr4, %eax
+ movl %eax, %edx
+ andl $0x0FFFFFF7F, %eax
+ movl %eax, %cr4
+ #
+ # Flush all TLBs
+ #
+ movl %cr3, %eax
+ movl %eax, %cr3
+
+ movl %edx, %eax
+ ret
+#MpMtrrSynchUpEntry ENDP
+
+ASM_GLOBAL ASM_PFX(MpMtrrSynchUpExit)
+ASM_PFX(MpMtrrSynchUpExit):
+ push %ebp # C prolog
+ movl %esp, %ebp
+ #
+ # Flush all TLBs the second time
+ #
+ movl %cr3, %eax
+ movl %eax, %cr3
+ #
+ # Enable Normal Mode caching CD=NW=0, CD(Bit30), NW(Bit29)
+ #
+ movl %cr0, %eax
+ andl $0x09FFFFFFF, %eax
+ movl %eax, %cr0
+ #
+ # Set PGE Flag in CR4 if set
+ #
+ movl 8(%ebp), %eax
+ movl %eax, %cr4
+
+ pop %ebp
+ ret
+#MpMtrrSynchUpExit ENDP
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.asm b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.asm
new file mode 100644
index 0000000000..4195ee3a47
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/MpProc.asm
@@ -0,0 +1,84 @@
+;; @file
+; Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+ page ,132
+ title MP ASSEMBLY HOOKS
+
+.686p
+.model flat
+.data
+.stack
+.code
+.MMX
+.XMM
+#if 0
+_MpMtrrSynchUpEntry PROC NEAR PUBLIC
+ ;
+ ; Enter no fill cache mode, CD=1(Bit30), NW=0 (Bit29)
+ ;
+ mov eax, cr0
+ and eax, 0DFFFFFFFh
+ or eax, 040000000h
+ mov cr0, eax
+ ;
+ ; Flush cache
+ ;
+ wbinvd
+ ;
+ ; Clear PGE flag Bit 7
+ ;
+ mov eax, cr4
+ mov edx, eax
+ and eax, 0FFFFFF7Fh
+ mov cr4, eax
+ ;
+ ; Flush all TLBs
+ ;
+ mov eax, cr3
+ mov cr3, eax
+
+ mov eax, edx
+
+ ret
+
+_MpMtrrSynchUpEntry ENDP
+
+_MpMtrrSynchUpExit PROC NEAR PUBLIC
+
+ push ebp ; C prolog
+ mov ebp, esp
+ ;
+ ; Flush all TLBs the second time
+ ;
+ mov eax, cr3
+ mov cr3, eax
+ ;
+ ; Enable Normal Mode caching CD=NW=0, CD(Bit30), NW(Bit29)
+ ;
+ mov eax, cr0
+ and eax, 09FFFFFFFh
+ mov cr0, eax
+ ;
+ ; Set PGE Flag in CR4 if set
+ ;
+ mov eax, dword ptr [ebp + 8]
+ mov cr4, eax
+
+ pop ebp
+
+ ret
+
+_MpMtrrSynchUpExit ENDP
+#endif
+ END
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/ProcessorDef.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/ProcessorDef.h
new file mode 100644
index 0000000000..1002ecd7db
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/Ia32/ProcessorDef.h
@@ -0,0 +1,49 @@
+/** @file
+ Definition for IA32 Processor.
+
+ Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _PROCESSOR_DEF_H
+#define _PROCESSOR_DEF_H
+
+#pragma pack(1)
+
+typedef struct {
+ UINT16 OffsetLow;
+ UINT16 SegmentSelector;
+ UINT16 Attributes;
+ UINT16 OffsetHigh;
+} INTERRUPT_GATE_DESCRIPTOR;
+
+#pragma pack()
+
+typedef struct {
+ UINT8 *RendezvousFunnelAddress;
+ UINTN PModeEntryOffset;
+ UINTN FlatJumpOffset;
+ UINTN Size;
+} MP_ASSEMBLY_ADDRESS_MAP;
+
+/**
+ Get address map of RendezvousFunnelProc.
+
+ @param[out] AddressMap Output buffer for address map information.
+
+**/
+VOID
+EFIAPI
+AsmGetAddressMap (
+ OUT MP_ASSEMBLY_ADDRESS_MAP *AddressMap
+ );
+#endif
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.c
new file mode 100644
index 0000000000..f5876ae299
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.c
@@ -0,0 +1,865 @@
+/** @file
+ CPU MTRR programming driver.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include "MpService.h"
+#include "MemoryAttribute.h"
+#include "CpuInitDxe.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED FIXED_MTRR mFixedMtrrTable[V_FIXED_MTRR_NUMBER];
+
+GLOBAL_REMOVE_IF_UNREFERENCED 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 },
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED 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 },
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED VARIABLE_MTRR mVariableMtrr[V_MAXIMUM_VARIABLE_MTRR_NUMBER];
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32 mUsedMtrr;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32 mVariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+GLOBAL_REMOVE_IF_UNREFERENCED 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;
+
+ AsmDisableCache ();
+
+ //
+ // 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);
+
+ AsmEnableCache ();
+ 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
+
+ @retval 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;
+ UINT64 MtrrValue;
+
+ TempQword = 0;
+ OrMask = 0;
+ ClearMask = 0;
+
+ //
+ // Locate the Fixed MTRR MSR that contains the BaseAddress.
+ //
+ 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;
+ }
+
+ //
+ // Create memory attribute and/or masks for the found fixed MTRR.
+ //
+ 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;
+ }
+
+ if (ByteShift < 8 && (*Length != 0)) {
+ return EFI_UNSUPPORTED;
+ }
+
+ //
+ // Check the fixed MTRR changed or not.
+ //
+ MtrrValue = AsmReadMsr64 (mFixedMtrrTable[MsrNum].Msr);
+ TempQword = (MtrrValue & ~ClearMask) | OrMask;
+ if (MtrrValue != TempQword) {
+ mFixedMtrrChanged = TRUE;
+ 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;
+
+ mVariableMtrrLimit = (UINT32) (AsmReadMsr64 (IA32_MTRR_CAP) & B_IA32_MTRR_VARIABLE_SUPPORT);
+ DEBUG ((DEBUG_INFO, "Variable-range MTRR count = %d \n", mVariableMtrrLimit));
+
+ if (mVariableMtrrLimit > V_MAXIMUM_VARIABLE_MTRR_NUMBER) {
+ mVariableMtrrLimit = V_MAXIMUM_VARIABLE_MTRR_NUMBER;
+ DEBUG ((DEBUG_INFO, "Capping Variable-range MTRR count to %d \n", mVariableMtrrLimit));
+ }
+
+ //
+ // Get Default Mtrr Type
+ //
+ MsrValue = AsmReadMsr64 (CACHE_IA32_MTRR_DEF_TYPE);
+ mDefaultMemoryType = (UINT8) MsrValue;
+
+ //
+ // Get Variable Mtrr
+ //
+ ZeroMem (mVariableMtrr, (sizeof (VARIABLE_MTRR)) * mVariableMtrrLimit);
+ mUsedMtrr = 0;
+
+ for (MsrNum = CACHE_VARIABLE_MTRR_BASE, Index = 0;
+ ((MsrNum < (CACHE_VARIABLE_MTRR_BASE + mVariableMtrrLimit * 2 - 1)) && (Index < mVariableMtrrLimit));
+ 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 < mVariableMtrrLimit; 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 < mVariableMtrrLimit) {
+ 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 < mVariableMtrrLimit; 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 < mVariableMtrrLimit; 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 < mVariableMtrrLimit; 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:
+ 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[out] StartIndex Start index into the Gcd Memory Space Map
+ @param[out] 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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.h
new file mode 100644
index 0000000000..5d1e2d54f6
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MemoryAttribute.h
@@ -0,0 +1,285 @@
+/** @file
+ Header file for CPU MTRR programming driver.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _EFI_MEMORY_ATTRIB_H
+#define _EFI_MEMORY_ATTRIB_H
+
+extern UINT32 mUsedMtrr;
+
+///
+/// This structure represents fixed MTRR
+///
+typedef struct {
+ UINT32 Msr; ///< Fixed MTRR MSR, as described in EDS.
+ UINT32 BaseAddress; ///< Base address of the MTRR.
+ UINT32 Length; ///< Range of this MTRR.
+} FIXED_MTRR;
+
+///
+/// This structure provides the information on whether a Fixed MTRR needs to be changed
+///
+typedef struct {
+ UINT64 MsrValue; ///< Value read from the fixed MTRR MSR
+ BOOLEAN Changed; ///< Indicator of whether a range has been changed.
+} MTRR_VALUE;
+
+///
+/// This structure represents variable MTRR
+///
+typedef struct {
+ UINT64 BaseAddress; ///< Base address of the MTRR.
+ UINT64 Length; ///< Range of this MTRR.
+ UINT64 Type; ///< Cacheability type for this memory range.
+ UINT32 Msr; ///< Variable MTRR MSR
+ BOOLEAN Valid; ///< Indicator of whether the range is valid.
+} VARIABLE_MTRR;
+
+#if defined (__GNUC__)
+#define IA32API _EFIAPI
+#else
+#define IA32API __cdecl
+#endif
+
+#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
+
+ @retval 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[out] StartIndex Start index into the Gcd Memory Space Map
+ @param[out] 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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.c
new file mode 100644
index 0000000000..4f0e50392f
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.c
@@ -0,0 +1,769 @@
+/** @file
+ Code which support multi-processor.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/IoLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Protocol/SmmControl2.h>
+#include <Protocol/LegacyBios.h>
+#include <Protocol/CpuInfo.h>
+#include "MpCommon.h"
+#include "CpuInitDxe.h"
+#include "MpService.h"
+
+extern EFI_GUID gSmramCpuDataHeaderGuid;
+extern MP_SYSTEM_DATA *mMPSystemData;
+extern EFI_PHYSICAL_ADDRESS mOriginalBuffer;
+extern EFI_PHYSICAL_ADDRESS mBackupBuffer;
+extern EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+extern CPU_INIT_DATA_HOB *mCpuInitDataHob;
+GLOBAL_REMOVE_IF_UNREFERENCED volatile UINTN mSwitchToLegacyRegionCount = 0;
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PHYSICAL_ADDRESS mLegacyRegion;
+
+/**
+ 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 (
+ IN BOOLEAN XapicEnabled,
+ IN UINT32 MsrIndex,
+ IN 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 (
+ IN BOOLEAN XapicEnabled,
+ IN UINT32 MsrIndex,
+ IN UINT64 MemoryMappedIo,
+ IN 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;
+
+ /* 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)
+ );
+
+ MicroSecondDelay (10);
+
+ IcrLow = (UINT32) ReadApicMsrOrMemory (XapicEnabled, EXT_XAPIC_ICR, ApicBase + APIC_REGISTER_ICR_LOW_OFFSET);
+
+ if (IcrLow & BIT12) {
+ return EFI_NOT_READY;
+ }
+
+ MicroSecondDelay (100);
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Programs Local APIC registers.
+
+ @param[in] Bsp Is this BSP?
+
+**/
+VOID
+ProgramCpuXApic (
+ IN 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) {
+ DisableInterrupts ();
+ }
+
+ //
+ // 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;
+ }
+
+ 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[out] 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[out] 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;
+}
+
+
+/**
+ Allocate EfiReservedMemoryType below 4G memory address.
+
+ @param[in] Size Size of memory to allocate.
+ @param[out] 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 = (EFI_PHYSICAL_ADDRESS) (UINTN) AllocateReservedPages (Pages);
+ if (Address != 0) {
+ Status = EFI_SUCCESS;
+ } else {
+ Status = EFI_OUT_OF_RESOURCES;
+ }
+
+ *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.
+
+**/
+VOID
+EFIAPI
+InitializeSmramDataContent (
+ VOID
+ )
+{
+#ifndef SLE_FLAG
+ SMRAM_CPU_DATA SmramCpuDataTemplate;
+ UINTN LockBoxSize;
+ IA32_DESCRIPTOR *Idtr;
+ IA32_DESCRIPTOR *Gdtr;
+ EFI_STATUS Status;
+ EFI_SMM_CONTROL2_PROTOCOL *SmmControl;
+ UINT8 *SmramCpuData;
+ UINTN ArgBufferSize;
+ UINT8 ArgBuffer;
+ CPU_INFO_PROTOCOL *CpuInfo;
+
+ DEBUG ((DEBUG_INFO, "InitializeSmramDataContent\n"));
+ Status = gBS->LocateProtocol (&gEfiSmmControl2ProtocolGuid, 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 (IA32_DESCRIPTOR);
+ Gdtr = (IA32_DESCRIPTOR *) (UINTN) mAcpiCpuData->GdtrProfile;
+ SmramCpuDataTemplate.GdtSize = Gdtr->Limit + 1;
+ SmramCpuDataTemplate.IdtrProfileSize = sizeof (IA32_DESCRIPTOR);
+ Idtr = (IA32_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);
+
+ 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;
+
+ LockBoxSize = sizeof (SMRAM_CPU_DATA) +
+ SmramCpuDataTemplate.GdtrProfileSize +
+ SmramCpuDataTemplate.GdtSize +
+ SmramCpuDataTemplate.IdtrProfileSize +
+ SmramCpuDataTemplate.IdtSize +
+ SmramCpuDataTemplate.CpuPrivateDataSize +
+ SmramCpuDataTemplate.S3BootScriptTableSize +
+ SmramCpuDataTemplate.S3BspMtrrTableSize;
+
+ DEBUG ((DEBUG_INFO, "LockBoxSize - %x\n", LockBoxSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.GdtrProfileSize - %x\n", SmramCpuDataTemplate.GdtrProfileSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.GdtSize - %x\n", SmramCpuDataTemplate.GdtSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.IdtrProfileSize - %x\n", SmramCpuDataTemplate.IdtrProfileSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.IdtSize - %x\n", SmramCpuDataTemplate.IdtSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.CpuPrivateDataSize - %x\n", SmramCpuDataTemplate.CpuPrivateDataSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.S3BootScriptTableSize - %x\n", SmramCpuDataTemplate.S3BootScriptTableSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.S3BspMtrrTableSize - %x\n", SmramCpuDataTemplate.S3BspMtrrTableSize));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.GdtrProfileOffset - %x\n", SmramCpuDataTemplate.GdtrProfileOffset));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.GdtOffset - %x\n", SmramCpuDataTemplate.GdtOffset));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.IdtrProfileOffset - %x\n", SmramCpuDataTemplate.IdtrProfileOffset));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.IdtOffset - %x\n", SmramCpuDataTemplate.IdtOffset));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.CpuPrivateDataOffset - %x\n", SmramCpuDataTemplate.CpuPrivateDataOffset));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.S3BootScriptTableOffset - %x\n", SmramCpuDataTemplate.S3BootScriptTableOffset));
+ DEBUG ((DEBUG_INFO, "SmramCpuData.S3BspMtrrTableOffset - %x\n", SmramCpuDataTemplate.S3BspMtrrTableOffset));
+
+ //
+ // Allocate Normal Memory
+ //
+ SmramCpuData = AllocatePool (LockBoxSize);
+ ASSERT (SmramCpuData != NULL);
+
+ //
+ // Allocate SMRAM
+ //
+ //
+ // 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
+ );
+ //
+ // Save Mtrr Register for S3 resume
+ //
+ SaveBspMtrrForS3 ();
+
+ CopyMem (
+ SmramCpuData + SmramCpuDataTemplate.S3BspMtrrTableOffset,
+ (VOID *) (UINTN) mMPSystemData->S3DataPointer.S3BspMtrrTable,
+ SmramCpuDataTemplate.S3BspMtrrTableSize
+ );
+
+ //
+ // Locate CPU Info Protocol
+ //
+ Status = gBS->LocateProtocol (
+ &gCpuInfoProtocolGuid,
+ NULL,
+ (VOID **) &CpuInfo
+ );
+ if (EFI_ERROR (Status)) {
+ DEBUG ((DEBUG_ERROR, "Fail to locate CpuInfo protocol would cause S3 resume get error.\n"));
+ ASSERT_EFI_ERROR (Status);
+ return;
+ }
+
+ CpuInfo->SmramCpuInfo->LockBoxData = (EFI_PHYSICAL_ADDRESS) 0;
+ CpuInfo->SmramCpuInfo->SmramCpuData = (EFI_PHYSICAL_ADDRESS) (UINTN) SmramCpuData;
+ CpuInfo->SmramCpuInfo->LockBoxSize = (UINT64) LockBoxSize;
+ IoWrite8 (R_PCH_APM_STS, SMM_FROM_CPU_DRIVER_SAVE_INFO);
+
+ //
+ // Trigger SMI
+ //
+ ArgBufferSize = sizeof (ArgBuffer);
+ ArgBuffer = mSmmbaseSwSmiNumber;
+ Status = SmmControl->Trigger (SmmControl, (UINT8 *) &ArgBuffer, (UINT8 *) &ArgBufferSize, FALSE, 0);
+ Status = SmmControl->Clear (SmmControl, 0);
+#endif
+
+ 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));
+ mLegacyRegion = EbdaOld + (EbdaSize << 10);
+ mLegacyRegion = (mLegacyRegion - 0x1000) & 0xFFFFF000;
+ EbdaNew = mLegacyRegion - (EbdaSize << 10);
+ (*(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 ((DEBUG_INFO, "Legacy region freed before re-allocation: %X\n", mLegacyRegion));
+ Status = (gBS->FreePages) (mLegacyRegion, 1);
+ ASSERT_EFI_ERROR (Status);
+ DEBUG ((DEBUG_INFO, "Allocate and reserve the 4K buffer for Legacy Region\n"));
+ Status = (gBS->AllocatePages)(AllocateAddress, EfiReservedMemoryType, 1, &mLegacyRegion);
+ ASSERT_EFI_ERROR (Status);
+ DEBUG ((DEBUG_INFO, "mLegacyRegion CSM - %x\n", mLegacyRegion));
+}
+
+
+/**
+ This callback function will be executed when EndofDxe event is signaled.
+ This function will do the following:
+ Allocate memory (wakeup buffer) for AP
+ Set all threads to deepest C states
+ Initialize SMRAM contents
+
+ @param[in] Event The triggered event.
+ @param[in] Context Context for this event.
+
+**/
+VOID
+EFIAPI
+ReAllocateMemoryForAP (
+ IN EFI_EVENT Event,
+ IN VOID *Context
+ )
+{
+#ifdef SLE_FLAG
+ STATIC BOOLEAN InitDone = FALSE;
+ //
+ // Make sure it is invoked only once.
+ //
+ if (InitDone) {
+ return;
+ }
+
+ InitDone = TRUE;
+
+ while (ApRunning ()) { // SE06
+ CpuPause ();
+ }
+
+#else
+ EFI_LEGACY_BIOS_PROTOCOL *LegacyBios;
+ EFI_STATUS Status;
+ EFI_PHYSICAL_ADDRESS LegacyRegion;
+ MP_CPU_EXCHANGE_INFO *ExchangeInfo;
+ BOOLEAN HasCsm;
+ STATIC BOOLEAN InitDone = FALSE;
+ CPU_CONFIG *CpuConfig;
+
+ //
+ // 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 ();
+ }
+
+ if (HasCsm) {
+ LegacyRegion = mLegacyRegion;
+ DEBUG ((DEBUG_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 ((DEBUG_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);
+ CpuConfig = (CPU_CONFIG *) (UINTN) mCpuInitDataHob->CpuConfig;
+ if (CpuConfig->ApHandoffManner != WakeUpApPerHltLoop) {
+ } else {
+ ExchangeInfo->WakeUpApManner = WakeUpApPerHltLoop;
+ }
+
+ //
+ // Invoke the InitializeSmram directly, since it is in EndOfDxe event.
+ //
+ InitializeSmramDataContent ();
+#endif //SLE_FLAG
+}
+
+
+/**
+ 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
+EFIAPI
+ResetAps (
+ IN EFI_EVENT Event,
+ IN VOID *Context
+ )
+{
+ return;
+}
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.h
new file mode 100644
index 0000000000..d04ced5b01
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpCommon.h
@@ -0,0 +1,524 @@
+/** @file
+ Some definitions for MP and HT driver.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _MP_COMMON_
+#define _MP_COMMON_
+
+#include <Library/BaseLib.h>
+#include <Library/TimerLib.h>
+#include "CpuInitDxe.h"
+#include "Exception.h"
+#include "ProcessorDef.h"
+#include "ScAccess.h"
+#include <Private/CpuInitDataHob.h>
+
+#include <Protocol/MpService.h>
+
+#define VacantFlag 0x00
+#define NotVacantFlag 0xff
+#define MICROSECOND 10
+#define MAXIMUM_CPU_NUMBER 0x40
+#define STACK_SIZE_PER_PROC 0x8000
+
+///
+/// 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;
+ IA32_DESCRIPTOR GdtrProfile;
+ IA32_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
+//
+/**
+ 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[out] NumberOfProcessors Pointer to the total number of logical processors in the system,
+ including the BSP and disabled APs.
+ @param[out] NumberOfEnabledProcessors Pointer to the number of enabled logical processors that exist
+ in system, including the BSP.
+
+ @retval EFI_SUCCESS 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_MP_SERVICES_PROTOCOL *This,
+ OUT UINTN *NumberOfProcessors,
+ OUT UINTN *NumberOfEnabledProcessors
+ );
+
+/**
+ Implementation of GetProcessorInfo() 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[out] 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_MP_SERVICES_PROTOCOL *This,
+ IN UINTN ProcessorNumber,
+ OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer
+ );
+
+/**
+ 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[out] 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 VOID *ProcArguments OPTIONAL,
+ OUT BOOLEAN *Finished 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[out] 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,
+ IN 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 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 UINT32 *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[out] 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
+ );
+
+/**
+ Programs XAPIC registers.
+
+ @param[in] Bsp Is this BSP
+
+**/
+VOID
+ProgramCpuXApic (
+ 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[out] 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
+ );
+
+/**
+ Prepare GDTR and IDTR for AP
+
+ @param[out] GDTR The GDTR profile
+ @param[out] 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 IA32_DESCRIPTOR *GDTR,
+ OUT IA32_DESCRIPTOR *IDTR
+ );
+
+/**
+ Allocate Reserved Memory
+
+ @param[in] Size Memory Size
+ @param[in] Alignment Alignment size
+ @param[out] Pointer Return memory location
+
+ @retval EFI_SUCCESS Allocate a reserved memory successfully
+
+**/
+EFI_STATUS
+AllocateAlignedReservedMemory (
+ IN UINTN Size,
+ IN UINTN Alignment,
+ OUT VOID **Pointer
+ );
+
+/**
+ 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
+EFIAPI
+ResetAps (
+ IN EFI_EVENT Event,
+ IN VOID *Context
+ );
+
+/**
+ Prepare Wakeup Buffer and stack for APs.
+
+ @param[out] WakeUpBuffer Pointer to the address of wakeup buffer for output.
+ @param[out] 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[out] 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
+ );
+
+/**
+ 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[out] 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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.c
new file mode 100644
index 0000000000..425c128a1c
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.c
@@ -0,0 +1,2303 @@
+/** @file
+ Code which support multi-processor.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/UefiLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/HobLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Protocol/LegacyBios.h>
+#include <Library/CpuPlatformLib.h>
+#include <Private/Library/CpuCommonLib.h>
+
+#include "MpService.h"
+#include "CpuInitDxe.h"
+#include <Library/TimerLib.h>
+
+extern EFI_GUID gHtBistHobGuid;
+extern CPU_INIT_DATA_HOB *mCpuInitDataHob;
+extern EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+extern MP_SYSTEM_DATA *mMPSystemData;
+extern UINTN mCommonFeatures;
+extern volatile UINTN mSwitchToLegacyRegionCount;
+
+STATIC EFI_HANDLE mHandle = NULL;
+STATIC UINT32 mFinishedCount = 0;
+extern UINT32 mMcuLoadCount;
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_MP_SERVICES_PROTOCOL mMpService = {
+ GetNumberOfProcessors,
+ GetProcessorInfo,
+ StartupAllAPs,
+ StartupThisAP,
+ SwitchBSP,
+ EnableDisableAP,
+ WhoAmI
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PHYSICAL_ADDRESS mOriginalBuffer;
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PHYSICAL_ADDRESS mBackupBuffer;
+GLOBAL_REMOVE_IF_UNREFERENCED BOOLEAN mStopCheckApsStatus = FALSE;
+
+/**
+ Initialize MP services by MP Service Protocol.
+
+**/
+VOID
+EFIAPI
+InitializeMpServices (
+ VOID
+ )
+{
+ EFI_STATUS Status;
+ EFI_EVENT LegacyBootEvent;
+ EFI_EVENT ExitBootServicesEvent;
+ VOID *Registration;
+ EFI_EVENT EndOfDxeEvent;
+ 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;
+ }
+
+ //
+ // Since PI1.2.1, we need use EndOfDxe instead of ExitPmAuth
+ //
+ Status = gBS->CreateEventEx (
+ EVT_NOTIFY_SIGNAL,
+ TPL_CALLBACK,
+ ReAllocateMemoryForAP,
+ NULL,
+ &gEfiEndOfDxeEventGroupGuid,
+ &EndOfDxeEvent
+ );
+ ASSERT_EFI_ERROR (Status);
+
+ //
+ // 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 (
+ TPL_CALLBACK,
+ ResetAps,
+ mMPSystemData,
+ &LegacyBootEvent
+ );
+ if (EFI_ERROR (Status)) {
+ goto Done;
+ }
+
+ Status = gBS->CreateEvent (
+ EVT_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 (
+ EVT_TIMER | EVT_NOTIFY_SIGNAL,
+ TPL_CALLBACK,
+ CheckApsStatus,
+ NULL,
+ &mMPSystemData->CheckAPsEvent
+ );
+ ASSERT_EFI_ERROR (Status);
+
+ //
+ // Now install the Frameowrk & PI MP services protocol.
+ //
+ Status = gBS->InstallMultipleProtocolInterfaces (
+ &mHandle,
+ &gEfiMpServiceProtocolGuid,
+ &mMpService,
+ 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);
+ }
+}
+
+
+/**
+ 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[out] NumberOfProcessors Pointer to the total number of logical processors in the system,
+ including the BSP and disabled APs.
+ @param[out] NumberOfEnabledProcessors Pointer to the number of enabled logical processors that exist
+ in system, including the BSP.
+
+ @retval EFI_SUCCESS 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_MP_SERVICES_PROTOCOL *This,
+ OUT UINTN *NumberOfProcessors,
+ OUT UINTN *NumberOfEnabledProcessors
+ )
+{
+ UINTN CallerNumber;
+ UINTN Index;
+ CPU_DATA_BLOCK *CpuData;
+
+ //
+ // 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;
+ }
+
+ *NumberOfProcessors = mMPSystemData->NumberOfCpus;
+ *NumberOfEnabledProcessors = 0;
+ for (Index = 0; Index < mMPSystemData->NumberOfCpus; Index++) {
+ CpuData = &mMPSystemData->CpuData[Index];
+ if (mMPSystemData->EnableSecondaryCpu) {
+ if (CpuData->State != CPU_STATE_DISABLED) {
+ (*NumberOfEnabledProcessors)++;
+ }
+ } else if (CpuData->State != CPU_STATE_DISABLED && !mMPSystemData->CpuData[Index].SecondaryCpu) {
+ (*NumberOfEnabledProcessors)++;
+ }
+ }
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ Implementation of GetProcessorInfo() 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[out] 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_MP_SERVICES_PROTOCOL *This,
+ IN UINTN ProcessorNumber,
+ OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer
+ )
+{
+ UINTN CallerNumber;
+ CPU_DATA_BLOCK *CpuData;
+
+ //
+ // 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;
+ }
+
+ CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+ ProcessorInfoBuffer->ProcessorId = (UINT64) CpuData->ApicID;
+
+ //
+ // Get Status Flag of specified processor
+ //
+ ProcessorInfoBuffer->StatusFlag = 0;
+
+ ProcessorInfoBuffer->StatusFlag |= PROCESSOR_ENABLED_BIT;
+ if (CpuData->State == CPU_STATE_DISABLED) {
+ ProcessorInfoBuffer->StatusFlag &= ~PROCESSOR_ENABLED_BIT;
+ } else if (!mMPSystemData->EnableSecondaryCpu) {
+ if (CpuData->SecondaryCpu) {
+ ProcessorInfoBuffer->StatusFlag &= ~PROCESSOR_ENABLED_BIT;
+ }
+ }
+
+ if (ProcessorNumber == mMPSystemData->BSP) {
+ ProcessorInfoBuffer->StatusFlag |= PROCESSOR_AS_BSP_BIT;
+ }
+
+ if (CpuData->Health == 0) {
+ ProcessorInfoBuffer->StatusFlag |= PROCESSOR_HEALTH_STATUS_BIT;
+ }
+
+ ProcessorInfoBuffer->Location.Package = (UINT32) CpuData->PhysicalLocation.Package;
+ ProcessorInfoBuffer->Location.Core = (UINT32) CpuData->PhysicalLocation.Core;
+ ProcessorInfoBuffer->Location.Thread = (UINT32) CpuData->PhysicalLocation.Thread;
+
+ 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.
+ @param[out] Finished Pointer to the mode of AP running.
+
+ @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 VOID *ProcArguments OPTIONAL,
+ OUT BOOLEAN *Finished 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;
+ }
+
+ if (Finished != NULL) {
+ *Finished = TRUE;
+ }
+
+ 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;
+ }
+
+ MicroSecondDelay (CPU_CHECK_AP_INTERVAL);
+ }
+
+ CpuData->Finished = Finished;
+
+ 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[out] 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,
+ IN 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) {
+ return EFI_SUCCESS;
+ }
+
+ 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) {
+ return EFI_SUCCESS;
+ }
+
+#ifndef SLE_FLAG
+ 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;
+ }
+#endif // SLE_FLAG
+
+ MicroSecondDelay (CPU_CHECK_AP_INTERVAL);
+ }
+
+ 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
+ //
+ ProgramCpuXApic (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,
+ (EFI_AP_PROCEDURE) 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);
+ ProgramCpuXApic (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 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 UINT32 *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 (UINT32));
+ }
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ This procedure returns the calling CPU handle.
+
+ @param[in] This Pointer to MP Service Protocol.
+ @param[out] 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 = GetCpuApicId ();
+
+ NumOfCpus = mMPSystemData->NumberOfCpus;
+
+ for (Index = 0; Index < NumOfCpus; Index++) {
+ if (ApicID == mMPSystemData->CpuData[Index].ApicID) {
+ break;
+ }
+ }
+
+ *CpuNumber = Index;
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ 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;
+}
+
+
+/**
+ 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
+ )
+{
+#ifdef SLE_FLAG
+ return EFI_NOT_FOUND;
+#else
+ VOID *HobList;
+ VOID *DataInHob;
+ EFI_PEI_HOB_POINTERS Hob;
+ UINTN DataSize;
+
+ //
+ // Check for MP Data Hob.
+ //
+ HobList = GetFirstGuidHob (&gHtBistHobGuid);
+
+ if (HobList == NULL) {
+ DEBUG ((DEBUG_INFO, "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) {
+ MPSystemData->BistHobData = AllocateReservedPool (DataSize);
+ }
+
+ CopyMem (MPSystemData->BistHobData, DataInHob, DataSize);
+ MPSystemData->BistHobSize = DataSize;
+ }
+
+ return EFI_SUCCESS;
+#endif
+}
+
+
+/**
+ Allocate data pool for MP information and fill data in it.
+
+ @param[out] WakeUpBuffer The address of wakeup buffer.
+ @param[out] 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;
+ UINT64 MsrValue;
+ UINT16 ProcessorThreadCount;
+ UINT16 ProcessorCoreCount;
+ BOOLEAN HyperThreadingEnabled;
+ ACPI_CPU_DATA *AcpiCpuData;
+ CPU_CONFIG *CpuConfig;
+
+ //
+ // 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);
+ HyperThreadingEnabled = TRUE;
+ AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) mCpuInitDataHob->MpData;
+
+ /**
+ MSR_CORE_THREAD_COUNT(35h)
+ Bit Scope Description
+ 63:32 Reserved
+ 31:16 Package Core Count (RO). The Core Count reflects the enabled cores based
+ on the factory-configured core count and the value of the
+ RESOLVED_CORES_MASK register for Server processors or the PCH Soft
+ Reset Data register for Client processors at reset time.
+ 15:0 Package Thread Count (RO). The Thread Count reflects the enabled threads based
+ on the factory-configured thread count and the value of the
+ RESOLVED_CORES_MASK register for Server processors or the PCH Soft
+ Reset Data register for Client processors at reset time.
+ **/
+ MsrValue = AsmReadMsr64 (MSR_CORE_THREAD_COUNT);
+
+ //
+ // Get enabled thread count in the package
+ //
+ ProcessorThreadCount = MsrValue & B_THREAD_COUNT_MASK;
+
+ //
+ // Get enabled core count in the package
+ //
+ ProcessorCoreCount = (UINT16) RShiftU64 (MsrValue, N_CORE_COUNT_OFFSET);
+
+ //
+ // HyperThreading is disabled if Active Core Count is same as the Active Thread Count
+ //
+ if (ProcessorThreadCount == ProcessorCoreCount) {
+ HyperThreadingEnabled = FALSE;
+ }
+ mAcpiCpuData->CpuPrivateData = (EFI_PHYSICAL_ADDRESS) (UINTN) (&(mMPSystemData->S3DataPointer));
+ mAcpiCpuData->APState = HyperThreadingEnabled;
+ mAcpiCpuData->WakeUpBuffer = WakeUpBuffer;
+ mAcpiCpuData->StackAddress = (EFI_PHYSICAL_ADDRESS) (UINTN) StackAddressStart;
+
+ Status = PrepareGdtIdtForAP (
+ (IA32_DESCRIPTOR *) (UINTN) mAcpiCpuData->GdtrProfile,
+ (IA32_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 = HyperThreadingEnabled;
+
+ //
+ // Record these CPU configuration data (both for normal boot and for S3 use)
+ //
+ CpuConfig = (CPU_CONFIG *) (UINTN) mCpuInitDataHob->CpuConfig;
+
+
+ mMPSystemData->VmxEnable = (BOOLEAN) CpuConfig->VmxEnable;
+ mMPSystemData->TxtEnable = (BOOLEAN) CpuConfig->SmxEnable;
+ mMPSystemData->MonitorMwaitEnable = (BOOLEAN) CpuConfig->MonitorMwaitEnable;
+ mMPSystemData->MachineCheckEnable = (BOOLEAN) CpuConfig->MachineCheckEnable;
+ mMPSystemData->AesEnable = (BOOLEAN) CpuConfig->AesEnable;
+ mMPSystemData->DebugInterfaceEnable = (BOOLEAN) CpuConfig->DebugInterfaceEnable;
+ mMPSystemData->DebugInterfaceLockEnable = (BOOLEAN) CpuConfig->DebugInterfaceLockEnable;
+
+ mMPSystemData->S3DataPointer.S3BootScriptTable = (UINT32) (UINTN) mMPSystemData->S3BootScriptTable;
+ mMPSystemData->S3DataPointer.S3BspMtrrTable = (UINT32) (UINTN) mMPSystemData->S3BspMtrrTable;
+ mMPSystemData->ThreeStrikeCounterDisable = (BOOLEAN) CpuConfig->ThreeStrikeCounterDisable;
+
+ mMPSystemData->CpuArch = NULL;
+ gBS->LocateProtocol (&gEfiCpuArchProtocolGuid, NULL, (VOID **) &mMPSystemData->CpuArch);
+
+ mMPSystemData->MaximumCpusForThisSystem = MaximumCPUsForThisSystem;
+
+ mMPSystemData->BSP = 0;
+
+ //
+ // Locate HOB and copy contents to reserved memory
+ //
+ CopyMem (
+ (VOID *) mMPSystemData->S3BootScriptTable,
+ (VOID *) (UINTN) (((MP_CPU_S3_DATA_POINTER *) (UINTN) (AcpiCpuData->CpuPrivateData))->S3BootScriptTable),
+ sizeof (MP_CPU_S3_SCRIPT_DATA) * MAX_CPU_S3_TABLE_SIZE
+ );
+
+ //
+ // 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
+ );
+ MicroSecondDelay (10 * STALL_ONE_MILLI_SECOND);
+ SendInterrupt (
+ BROADCAST_MODE_ALL_EXCLUDING_SELF,
+ 0,
+ (UINT32) RShiftU64 (WakeUpBuffer, 12),
+ DELIVERY_MODE_SIPI,
+ TRIGGER_MODE_EDGE,
+ TRUE
+ );
+ MicroSecondDelay (200 * STALL_ONE_MICRO_SECOND);
+ SendInterrupt (
+ BROADCAST_MODE_ALL_EXCLUDING_SELF,
+ 0,
+ (UINT32) RShiftU64 (WakeUpBuffer, 12),
+ DELIVERY_MODE_SIPI,
+ TRIGGER_MODE_EDGE,
+ TRUE
+ );
+ MicroSecondDelay (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.
+ //
+ MicroSecondDelay (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;
+
+ ExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeUpBuffer + MP_CPU_EXCHANGE_INFO_OFFSET);
+
+ ExchangeInfo->ApFunction = (VOID *) (UINTN) DetailedMpInitialization;
+
+ CpuInitFloatPointUnit ();
+
+ //
+ // Wait until all APs finish
+ //
+ while (mFinishedCount < mAcpiCpuData->NumberOfCpus - 1) {
+ CpuPause ();
+ }
+ return EFI_SUCCESS;
+}
+
+
+/**
+ 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;
+ UINT16 MaxEnabledThreadsPerCore;
+ UINT16 MaxEnabledCoresPerDie;
+ UINT16 MaxDiesPerPackage;
+ UINT16 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;
+ EFI_HANDLE Handle;
+ Handle = NULL;
+
+ //
+ // Program Local APIC registers
+ //
+ ProgramCpuXApic (TRUE);
+
+ //
+ // Get information on enabled threads, cores, dies and package for the CPU(s) on this platform
+ //
+ GetEnabledCount (
+ &MaxEnabledThreadsPerCore,
+ &MaxEnabledCoresPerDie,
+ &MaxDiesPerPackage,
+ &MaxPackages
+ );
+ //
+ // Get the total CPU count
+ //
+ MaximumCPUsForThisSystem = MaxEnabledThreadsPerCore * MaxEnabledCoresPerDie * MaxDiesPerPackage * MaxPackages;
+
+ //
+ // Prepare Wakeup Buffer and Stack for APs
+ //
+ Status = PrepareMemoryForAPs (
+ &WakeUpBuffer,
+ &StackAddressStart,
+ MaximumCPUsForThisSystem
+ );
+ if (EFI_ERROR (Status)) {
+ return Status;
+ }
+
+ mOriginalBuffer = WakeUpBuffer;
+ mBackupBuffer = (EFI_PHYSICAL_ADDRESS) (UINTN) AllocatePages (1);
+
+ //
+ // 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);
+
+ //
+ // 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 = (WAKEUP_AP_MANNER) ((CPU_CONFIG *) (UINTN) (mCpuInitDataHob->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 != 0) {
+ DEBUG ((DEBUG_ERROR, "BIST for the following AP failed\n"));
+ DEBUG ((DEBUG_ERROR, "EAX=%x\n", CpuData->Health));
+ ReportStatusCode (
+ EFI_ERROR_MAJOR | EFI_ERROR_CODE,
+ EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_HP_EC_SELF_TEST
+ );
+ }
+ }
+
+ Status = gBS->CreateEvent (
+ EVT_TIMER | EVT_NOTIFY_SIGNAL,
+ 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 (
+ EVT_TIMER | EVT_NOTIFY_SIGNAL,
+ TPL_CALLBACK,
+ CheckThisApStatus,
+ (VOID *) CpuData,
+ CpuData->CheckThisAPEvent
+ );
+ }
+
+ 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;
+
+ 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
+ )
+{
+ CpuInitFloatPointUnit ();
+
+ //
+ // Save Mtrr Registers in global data areas
+ //
+ MpMtrrSynchUp (NULL);
+ ProgramCpuXApic (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
+EFIAPI
+FutureBspProc (
+ IN MP_SYSTEM_DATA *MPSystemData
+ )
+{
+ AsmExchangeRole (&MPSystemData->APInfo, &MPSystemData->BSPInfo);
+ return;
+}
+
+
+/**
+ Fill in the CPU location information
+
+ @param[out] Location CPU location information
+
+ @retval EFI_SUCCESS Always return success
+
+**/
+EFI_STATUS
+FillInCpuLocation (
+ OUT EFI_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->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);
+ if (HyperThreadingEnabled) {
+ Location->Core = (ApicId & Mask) >> 1;
+ Location->Thread = (ApicId & Mask) & 1;
+ } else {
+ Location->Core = (ApicId & Mask);
+ Location->Thread = 0;
+ }
+ }
+
+ return EFI_SUCCESS;
+}
+
+
+/**
+ 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 = GetCpuApicId ();
+ 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;
+ }
+ }
+
+ if (Health > 0) {
+ CpuData->State = CPU_STATE_DISABLED;
+ MPSystemData->DisableCause[CpuNumber] = CPU_CAUSE_SELFTEST_FAILURE;
+ } else {
+ MPSystemData->DisableCause[CpuNumber] = CPU_CAUSE_NOT_DISABLED;
+ }
+
+ FillInCpuLocation (&CpuData->PhysicalLocation);
+
+ 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;
+
+ CpuData = &mMPSystemData->CpuData[CpuNumber];
+
+ mMPSystemData->DisableCause[CpuNumber] = Cause;
+
+ if (!NewState) {
+ AsmAcquireMPLock (&CpuData->StateLock);
+ CpuData->State = CPU_STATE_DISABLED;
+ AsmReleaseMPLock (&CpuData->StateLock);
+
+ ReportStatusCode (
+ EFI_ERROR_MINOR | EFI_ERROR_CODE,
+ EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_EC_DISABLED
+ );
+ } 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 = GetCpuApicId ();
+
+ 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 = GetMaxSupportedCoreCount ();
+ } 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
+EFIAPI
+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
+EFIAPI
+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 = AsmReadTsc ();
+
+ 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 = AsmReadTsc ();
+ if (CurrentTsc >= ExpectedTsc) {
+ return TRUE;
+ }
+
+ return FALSE;
+}
+
+
+/**
+ Get the next blocked processor
+
+ @param[out] NextCpuNumber That will be updated for next blocked CPU number
+
+ @retval EFI_SUCCESS The next blocked CPU found
+ @retval EFI_NOT_FOUND Can not 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;
+}
+
+
+/**
+ 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
+ );
+#ifdef SLE_FLAG
+ if (Procedure) {
+ CpuData->State = CPU_STATE_BUSY;
+ }
+#endif // SLE_FLAG
+ 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;
+}
+
+
+/**
+ 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) {
+ *(mMPSystemData->FailedCpuList) = AllocatePool ((mMPSystemData->StartCount - mMPSystemData->FinishCount +1) * sizeof (UINTN));
+ if (*(mMPSystemData->FailedCpuList) == NULL) {
+ ASSERT (FALSE);
+ return EFI_OUT_OF_RESOURCES;
+ }
+ }
+
+ 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 (
+ IN 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;
+}
+
+
+/**
+ Searches for the next waiting AP.
+
+ Search for the next AP that is put in waiting state by single-threaded StartupAllAPs().
+
+ @param[out] 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;
+}
+
+
+/**
+ 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 (
+ IN UINTN ProcessorNumber
+ )
+{
+ CPU_DATA_BLOCK *CpuData;
+
+ CpuData = &mMPSystemData->CpuData[ProcessorNumber];
+
+ CpuSendIpi (
+ CpuData->ApicID,
+ 0,
+ DELIVERY_MODE_INIT
+ );
+
+ AsmAcquireMPLock (&CpuData->StateLock);
+ CpuData->State = CPU_STATE_IDLE;
+ AsmReleaseMPLock (&CpuData->StateLock);
+}
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.h
new file mode 100644
index 0000000000..52d21f6e9d
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MpService.h
@@ -0,0 +1,663 @@
+/** @file
+ Some definitions for MP services Protocol.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _MP_SERVICE_H_
+#define _MP_SERVICE_H_
+
+#include <Protocol/MpService.h>
+
+#include "MpCommon.h"
+
+//
+// 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
+ IA32_DESCRIPTOR Gdtr; ///< offset 8 / 16
+ IA32_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;
+ UINT32 Health;
+ BOOLEAN SecondaryCpu;
+ EFI_CPU_PHYSICAL_LOCATION PhysicalLocation;
+ CPU_STATE State;
+ //
+ // 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;
+
+///
+/// Define MP data block which consumes individual processor block.
+///
+typedef struct {
+ UINT8 ApSerializeLock;
+ BOOLEAN VmxEnable;
+ BOOLEAN TxtEnable;
+ BOOLEAN ThreeStrikeCounterDisable;
+ BOOLEAN MonitorMwaitEnable;
+ BOOLEAN MachineCheckEnable;
+ 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;
+ IA32_DESCRIPTOR GdtrProfile;
+ IA32_DESCRIPTOR IdtrProfile;
+} MP_CPU_RESERVED_DATA;
+
+extern MP_SYSTEM_DATA *mMPSystemData;
+extern ACPI_CPU_DATA *mAcpiCpuData;
+extern EFI_MP_SERVICES_PROTOCOL mMpService; // equals to EFI_MP_SERVICES_PROTOCOL
+
+///
+/// Prototypes.
+///
+
+/**
+ 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[out] NumberOfProcessors Pointer to the total number of logical processors in the system,
+ including the BSP and disabled APs.
+ @param[out] NumberOfEnabledProcessors Pointer to the number of enabled logical processors that exist
+ in system, including the BSP.
+
+ @retval EFI_SUCCESS 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_MP_SERVICES_PROTOCOL *This,
+ OUT UINTN *NumberOfProcessors,
+ OUT UINTN *NumberOfEnabledProcessors
+ );
+
+/**
+ Implementation of GetProcessorInfo() 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[out] 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_MP_SERVICES_PROTOCOL *This,
+ IN UINTN ProcessorNumber,
+ OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer
+ );
+
+/**
+ 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.
+ @param[out] Finished Pointer to the mode of AP running.
+
+ @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 VOID *ProcArguments OPTIONAL,
+ OUT BOOLEAN *Finished 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[out] 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,
+ IN 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
+ );
+
+/**
+ 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 UINT32 *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
+EFIAPI
+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
+ );
+
+/**
+ 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
+ );
+
+/**
+ 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
+EFIAPI
+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
+EFIAPI
+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[out] 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
+ );
+
+/**
+ 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
+ );
+
+/**
+ 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
+ );
+
+/**
+ 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
+ );
+
+/**
+ 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 (
+ IN UINTN ProcessorNumber
+ );
+
+/**
+ Searches for the next waiting AP.
+
+ Search for the next AP that is put in waiting state by single-threaded StartupAllAPs().
+
+ @param[out] 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
+ );
+
+/**
+ 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 (
+ IN UINTN ProcessorNumber
+ );
+#endif
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MtrrSync.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MtrrSync.c
new file mode 100644
index 0000000000..21425b5c2d
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/MtrrSync.c
@@ -0,0 +1,253 @@
+/** @file
+ Code to support MTRR synch operations.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/CpuPlatformLib.h>
+#include <Private/Library/CpuCommonLib.h>
+#include <MpService.h>
+
+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 = (LShiftU64 (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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/Exception.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/Exception.c
new file mode 100644
index 0000000000..3860587b0f
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/Exception.c
@@ -0,0 +1,382 @@
+/** @file
+ EM64T Exception Handler.
+
+ Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Private/Library/CpuCommonLib.h>
+#include "CpuInitDxe.h"
+#include "MpCommon.h"
+#include "Exception.h"
+
+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.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32 mErrorCodeFlag = 0x00027d00;
+
+//
+// Local Table
+//
+GLOBAL_REMOVE_IF_UNREFERENCED 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
+ }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN mExceptionNumber = sizeof (mExceptionTable) / sizeof (EFI_EXCEPTION_HANDLER);
+
+GLOBAL_REMOVE_IF_UNREFERENCED CPU_STATUS_CODE_TEMPLATE mStatusCodeData = {
+ {
+ sizeof (EFI_STATUS_CODE_DATA),
+ sizeof (EFI_SYSTEM_CONTEXT_X64),
+ { 0 }
+ },
+ {
+ { 0 }
+ }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED 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 (
+ (DEBUG_ERROR,
+ "!!!! X64 Exception Type - %016lx CPU Apic ID - %08x!!!!\n",
+ InterruptType,
+ GetCpuApicId ())
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "RIP - %016lx, CS - %016lx, RFLAGS - %016lx\n",
+ SystemContext.SystemContextX64->Rip,
+ SystemContext.SystemContextX64->Cs,
+ SystemContext.SystemContextX64->Rflags)
+ );
+ if (mErrorCodeFlag & (1 << InterruptType)) {
+ DEBUG (
+ (DEBUG_ERROR,
+ "ExceptionData - %016lx\n",
+ SystemContext.SystemContextX64->ExceptionData)
+ );
+ }
+
+ DEBUG (
+ (DEBUG_ERROR,
+ "RAX - %016lx, RCX - %016lx, RDX - %016lx\n",
+ SystemContext.SystemContextX64->Rax,
+ SystemContext.SystemContextX64->Rcx,
+ SystemContext.SystemContextX64->Rdx)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "RBX - %016lx, RSP - %016lx, RBP - %016lx\n",
+ SystemContext.SystemContextX64->Rbx,
+ SystemContext.SystemContextX64->Rsp,
+ SystemContext.SystemContextX64->Rbp)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "RSI - %016lx, RDI - %016lx\n",
+ SystemContext.SystemContextX64->Rsi,
+ SystemContext.SystemContextX64->Rdi)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "R8 - %016lx, R9 - %016lx, R10 - %016lx\n",
+ SystemContext.SystemContextX64->R8,
+ SystemContext.SystemContextX64->R9,
+ SystemContext.SystemContextX64->R10)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "R11 - %016lx, R12 - %016lx, R13 - %016lx\n",
+ SystemContext.SystemContextX64->R11,
+ SystemContext.SystemContextX64->R12,
+ SystemContext.SystemContextX64->R13)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "R14 - %016lx, R15 - %016lx\n",
+ SystemContext.SystemContextX64->R14,
+ SystemContext.SystemContextX64->R15)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "DS - %016lx, ES - %016lx, FS - %016lx\n",
+ SystemContext.SystemContextX64->Ds,
+ SystemContext.SystemContextX64->Es,
+ SystemContext.SystemContextX64->Fs)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "GS - %016lx, SS - %016lx\n",
+ SystemContext.SystemContextX64->Gs,
+ SystemContext.SystemContextX64->Ss)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "GDTR - %016lx %016lx, LDTR - %016lx\n",
+ SystemContext.SystemContextX64->Gdtr[0],
+ SystemContext.SystemContextX64->Gdtr[1],
+ SystemContext.SystemContextX64->Ldtr)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "IDTR - %016lx %016lx, TR - %016lx\n",
+ SystemContext.SystemContextX64->Idtr[0],
+ SystemContext.SystemContextX64->Idtr[1],
+ SystemContext.SystemContextX64->Tr)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "CR0 - %016lx, CR2 - %016lx, CR3 - %016lx\n",
+ SystemContext.SystemContextX64->Cr0,
+ SystemContext.SystemContextX64->Cr2,
+ SystemContext.SystemContextX64->Cr3)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "CR4 - %016lx, CR8 - %016lx\n",
+ SystemContext.SystemContextX64->Cr4,
+ SystemContext.SystemContextX64->Cr8)
+ );
+ DEBUG (
+ (DEBUG_ERROR,
+ "DR0 - %016lx, DR1 - %016lx, DR2 - %016lx\n",
+ SystemContext.SystemContextX64->Dr0,
+ SystemContext.SystemContextX64->Dr1,
+ SystemContext.SystemContextX64->Dr2)
+ );
+ DEBUG (
+ (DEBUG_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.
+ //
+ CpuDeadLoop ();
+
+ 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;
+
+ mStatusCodeData.Header.Type = gCpuStatusCodeDataTypeExceptionHandlerGuid;
+
+ 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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MemoryOperation.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MemoryOperation.c
new file mode 100644
index 0000000000..2e665ebf53
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MemoryOperation.c
@@ -0,0 +1,800 @@
+/** @file
+ Memory Operation Functions for IA32 Architecture.
+
+ Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include "CpuInitDxe.h"
+#include "MpCommon.h"
+#include "VirtualMemory.h"
+
+VOID
+InitializeExternalVectorTablePtr (
+ EFI_CPU_INTERRUPT_HANDLER *VectorTable
+ );
+
+extern EFI_CPU_INTERRUPT_HANDLER mExternalVectorTable[];
+extern EFI_PHYSICAL_ADDRESS mBackupBuffer;
+
+#if defined(__GNUC__)
+ extern UINT64 gCpuInitFloatPointUnit;
+#endif
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8 *mPageStore = NULL;
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN mPageStoreSize = 16;
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN mPageStoreIndex = 0;
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINT64 mValidMtrrAddressMask;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT64 mValidMtrrBitsMask;
+
+#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()
+
+GLOBAL_REMOVE_IF_UNREFERENCED 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
+ ///
+ }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED ALIGN_16BYTE_BOUNDRY PSEUDO_DESCRIPTOR_x64 gGdtPseudoDescriptor = {
+ sizeof (gGdt) - 1,
+ (UINTN) gGdt
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED INTERRUPT_GATE_DESCRIPTOR gIdtTable[INTERRUPT_VECTOR_NUMBER] = { { 0, 0, 0, 0, 0, 0 } };
+
+INTERRUPT_GATE_DESCRIPTOR gOrigIdtTable[INTERRUPT_VECTOR_NUMBER] = { { 0, 0, 0, 0, 0, 0 } };
+UINTN mOrigIdtEntryCount = 0;
+
+GLOBAL_REMOVE_IF_UNREFERENCED ALIGN_16BYTE_BOUNDRY PSEUDO_DESCRIPTOR_x64 gLidtPseudoDescriptor = {
+ sizeof (gIdtTable) - 1,
+ (UINTN) gIdtTable
+};
+
+/**
+ Init Global Descriptor table
+
+**/
+VOID
+InitializeSelectors (
+ VOID
+ )
+{
+ AsmWriteGdtr ((IA32_DESCRIPTOR *) &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;
+ IA32_DESCRIPTOR IdtrForBsp;
+
+ CodeSegment = AsmReadCs ();
+
+ 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;
+ }
+
+
+ AsmReadIdtr (&IdtrForBsp);
+
+ //
+ // Size of the IDT currently programmed (number of entries)
+ //
+ mOrigIdtEntryCount = (IdtrForBsp.Limit + 1) / sizeof (INTERRUPT_GATE_DESCRIPTOR);
+
+ //
+ // Update debugger CS with DxeCpu CS.
+ //
+ IdtEntry = (INTERRUPT_GATE_DESCRIPTOR *) IdtrForBsp.Base;
+ for (Index = 0; Index < mOrigIdtEntryCount; Index++) {
+ IdtEntry[Index].SegmentSelector = CodeSegment;
+ }
+ //
+ // Save a copy of the original IDT
+ //
+ CopyMem (&gOrigIdtTable, (VOID *) IdtrForBsp.Base, IdtrForBsp.Limit + 1);
+
+#ifdef SOURCE_DEBUG_ENABLE
+ //
+ // Use the original IDT table.
+ //
+ gLidtPseudoDescriptor.Limit = sizeof (gOrigIdtTable) - 1;
+ gLidtPseudoDescriptor.Base = (UINTN) gOrigIdtTable;
+#endif
+
+ AsmWriteIdtr ((IA32_DESCRIPTOR *) &gLidtPseudoDescriptor);
+
+ return;
+}
+
+
+/**
+ Set Interrupt Descriptor Table Handler Address.
+
+ @param[in] Index The Index of the interrupt descriptor table handle.
+
+**/
+VOID
+SetInterruptDescriptorTableHandlerAddress (
+ IN UINTN Index
+ )
+{
+ IA32_DESCRIPTOR IdtrForBsp;
+ INTERRUPT_GATE_DESCRIPTOR *IdtEntry;
+
+ AsmReadIdtr (&IdtrForBsp);
+ IdtEntry = (INTERRUPT_GATE_DESCRIPTOR *) IdtrForBsp.Base;
+
+ //
+ // Plug in CPU Driver version
+ //
+ CopyMem (&IdtEntry[Index], &gIdtTable[Index], sizeof (INTERRUPT_GATE_DESCRIPTOR));
+}
+
+
+/**
+ Restore original Interrupt Descriptor Table Handler Address.
+
+ @param[in] Index The Index of the interrupt descriptor table handle.
+
+**/
+VOID
+RestoreInterruptDescriptorTableHandlerAddress (
+ IN UINTN Index
+ )
+{
+ IA32_DESCRIPTOR IdtrForBsp;
+ INTERRUPT_GATE_DESCRIPTOR *IdtEntry;
+
+ if (Index >= mOrigIdtEntryCount) {
+ return;
+ }
+
+ AsmReadIdtr (&IdtrForBsp);
+ IdtEntry = (INTERRUPT_GATE_DESCRIPTOR *) IdtrForBsp.Base;
+ CopyMem (&IdtEntry[Index], &gOrigIdtTable[Index], sizeof (INTERRUPT_GATE_DESCRIPTOR));
+}
+
+
+/**
+ Initialize cache attributes based on MTRR
+
+**/
+VOID
+InitailizeCacheAttributes (
+ VOID
+ )
+{
+ 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
+ //
+ Page = (EFI_PHYSICAL_ADDRESS) (UINTN) AllocatePages (mPageStoreSize);
+ ASSERT (Page != 0);
+
+ 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, out] 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[out] PageTable page table that translated this base address
+ @param[out] 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[out] WakeUpBuffer Pointer to the address of wakeup buffer for output.
+ @param[out] 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)) {
+ FreePages (WakeUpBuffer, 1);
+ return Status;
+ }
+
+#if defined(__GNUC__)
+ gCpuInitFloatPointUnit = (UINT64) CpuInitFloatPointUnit;
+#endif
+
+ 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[out] 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 (IA32_DESCRIPTOR)
+ );
+ CopyMem (
+ (VOID *) (UINTN) &ExchangeInfo->IdtrProfile,
+ (VOID *) (UINTN) mAcpiCpuData->IdtrProfile,
+ sizeof (IA32_DESCRIPTOR)
+ );
+
+ ExchangeInfo->BufferStart = (UINT32) WakeUpBuffer;
+ ExchangeInfo->Cr3 = (UINT32) (AsmReadCr3 ());
+ ExchangeInfo->InitFlag = 1;
+
+ 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[out] Gdtr The GDTR profile
+ @param[out] 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 IA32_DESCRIPTOR *Gdtr,
+ OUT IA32_DESCRIPTOR *Idtr
+ )
+{
+ INTERRUPT_GATE_DESCRIPTOR *IdtForAp;
+ SEGMENT_DESCRIPTOR *GdtForAp;
+ IA32_DESCRIPTOR GdtrForBsp;
+ IA32_DESCRIPTOR IdtrForBsp;
+ UINT16 *MceHandler;
+ EFI_STATUS Status;
+
+ //
+ // Get Global Descriptor Table Register(GDTR) descriptor
+ //
+ AsmReadGdtr (&GdtrForBsp);
+
+ //
+ // Get Interrupt Descriptor Table Register(IDTR) descriptor
+ //
+ AsmReadIdtr (&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;
+ }
+
+ MceHandler = AllocateReservedPool (SIZE_OF_MCE_HANDLER);
+ if (MceHandler == NULL) {
+ return EFI_OUT_OF_RESOURCES;
+ }
+
+ //
+ // 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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpCpu.c b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpCpu.c
new file mode 100644
index 0000000000..fd09ba1b85
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpCpu.c
@@ -0,0 +1,80 @@
+/** @file
+ MP Support functions.
+
+ Copyright (c) 2007 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <MpService.h>
+
+GLOBAL_REMOVE_IF_UNREFERENCED ACPI_CPU_DATA *mAcpiCpuData;
+GLOBAL_REMOVE_IF_UNREFERENCED 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;
+
+ MpCpuReservedData = NULL;
+
+ //
+ // Allocate memory for MP CPU related data below 4G
+ //
+ Status = AllocateReservedMemoryBelow4G (
+ sizeof (MP_CPU_RESERVED_DATA),
+ (VOID **) &MpCpuReservedData
+ );
+
+ if (EFI_ERROR (Status)) {
+ return Status;
+ }
+
+ if (MpCpuReservedData != NULL) {
+ ZeroMem (MpCpuReservedData, sizeof (MP_CPU_RESERVED_DATA));
+ } else {
+ ASSERT (MpCpuReservedData != NULL);
+ return EFI_OUT_OF_RESOURCES;
+ }
+
+ mMPSystemData = &(MpCpuReservedData->MPSystemData);
+ mAcpiCpuData = &(MpCpuReservedData->AcpiCpuData);
+
+ //
+ // Initialize ACPI_CPU_DATA data
+ //
+ mAcpiCpuData->CpuPrivateData = (EFI_PHYSICAL_ADDRESS) (UINTN) (&(mMPSystemData->S3DataPointer));
+ mAcpiCpuData->S3BootPath = FALSE;
+ mAcpiCpuData->GdtrProfile = (EFI_PHYSICAL_ADDRESS) & (MpCpuReservedData->GdtrProfile);
+ mAcpiCpuData->IdtrProfile = (EFI_PHYSICAL_ADDRESS) & (MpCpuReservedData->IdtrProfile);
+
+ //
+ // Initialize MP services
+ //
+ InitializeMpServices ();
+
+ return EFI_SUCCESS;
+}
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.h
new file mode 100644
index 0000000000..98c3b925f1
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.h
@@ -0,0 +1,38 @@
+/** @file
+ This is the equates file for HT (Hyper-threading) support.
+
+ Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#define VacantFlag 0x00
+#define NotVacantFlag 0xff
+#define BreakToRunApSignal 0x6E755200
+#define MonitorFilterSize 0x40
+#define WakeUpApCounterInit 0
+#define WakeUpApPerHltLoop 1
+#define WakeUpApPerMwaitLoop 2
+#define WakeUpApPerRunLoop 3
+#define WakeUpApPerMwaitLoop32 4
+#define WakeUpApPerRunLoop32 5
+
+#define LockLocation (0x1000 - 0x0400)
+#define StackStartAddressLocation (LockLocation + 0x08)
+#define StackSizeLocation (LockLocation + 0x10)
+#define CProcedureLocation (LockLocation + 0x18)
+#define GdtrLocation (LockLocation + 0x20)
+#define IdtrLocation (LockLocation + 0x2A)
+#define BufferStartLocation (LockLocation + 0x34)
+#define Cr3OffsetLocation (LockLocation + 0x38)
+#define InitFlagLocation (LockLocation + 0x3C)
+#define WakeUpApManner (LockLocation + 0x40)
+#define BistBuffer (LockLocation + 0x44)
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.inc b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.inc
new file mode 100644
index 0000000000..af9a6bc45d
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpEqu.inc
@@ -0,0 +1,44 @@
+;; @file
+; Equates for MP initialization.
+;
+; Copyright (c) 2005 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+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/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.S b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.S
new file mode 100644
index 0000000000..6b1fa1f679
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.S
@@ -0,0 +1,610 @@
+## @file
+# This is the assembly code for EM64T MP support.
+#
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+#
+##
+
+#include "MpEqu.h"
+
+ .text
+ ASM_FUNCTION_REMOVE_IF_UNREFERENCED
+
+//-------------------------------------------------------------------------------------
+//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);
+
+.set L_RunLoopAndMwaitLoop32Offset, L_RunLoopAndMwaitLoop32 - L_LongModeStart
+.set L_RunLoopAndMwaitLoop32JumpOffset, L_RunLoopAndMwaitLoop32Jump - L_LongModeStart
+.set L_RunLoopAndMwaitLoop64Offset, L_RunLoopAndMwaitLoop64 - L_LongModeStart
+.set L_RunLoopAndMwaitLoop64JumpOffset, L_RunLoopAndMwaitLoop64Jump - L_LongModeStart
+.set L_LongModeStartJumpOffset, L_LongModeStartJump - L_RendezvousFunnelProcStart
+
+ASM_GLOBAL ASM_PFX(CpuInitFloatPointUnit)
+
+.globl ASM_PFX(RendezvousFunnelProc)
+ASM_PFX(RendezvousFunnelProc):
+L_RendezvousFunnelProcStart:
+
+// At this point CS = 0x(vv00) and ip= 0x0.
+ .byte 0x66, 0x8b, 0xe8 // mov ebp, eax
+
+ .byte 0x8c, 0xc8 // mov ax, cs
+ .byte 0x8e, 0xd8 // mov ds, ax
+ .byte 0x8e, 0xc0 // mov es, ax
+ .byte 0x8e, 0xd0 // mov ss, ax
+ .byte 0x33, 0xc0 // xor ax, ax
+ .byte 0x8e, 0xe0 // mov fs, ax
+ .byte 0x8e, 0xe8 // mov gs, ax
+
+// Get APIC ID
+//
+ .byte 0x66, 0xB8
+ .long 0x00000001 // mov eax, 1
+ .byte 0x0F, 0xA2 // cpuid
+ .byte 0x66, 0xC1, 0xEB, 0x18 // shr ebx, 24
+ .byte 0x66, 0x81, 0xE3
+ .long 0x000000FF // 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.
+
+ .byte 0xBE // opcode of mov si, imm16
+ .short InitFlagLocation // mov si, InitFlag
+ .byte 0x66, 0x83, 0x3C, 0x00 // cmp dword ptr [si], 0
+ .byte 0x74, 0x14 // jz flat32Start
+
+// Record BIST information
+//
+ .byte 0xB0, 0x08 // mov al, 8
+ .byte 0xF6, 0xE3 // mul bl
+
+ .byte 0xBE // mov si, imm16
+ .short BistBuffer // mov si, BistBuffer
+ .byte 0x03, 0xF0 // add si, ax
+
+ .byte 0x66, 0xC7, 0x04
+ .long 0x00000001 // mov dword ptr [si], 1 // Set Valid Flag
+ .byte 0x66, 0x89, 0x6C, 0x04 // mov dword ptr [si + 4], ebp // Store BIST value
+
+// Switch to flat mode.
+
+L_flat32Start:
+
+ .byte 0xBF // opcode of mov di, imm16
+ .short BufferStartLocation // mov di, BufferStartLocation
+ .byte 0x66, 0x8B, 0x35 // mov esi,dword ptr [di] // ESI is keeping the start address of wakeup buffer
+
+ .byte 0xBF // opcode of mov di, imm16
+ .short Cr3OffsetLocation // mov di, Cr3Location
+ .byte 0x66, 0x8B, 0x0D // mov ecx,dword ptr [di] // ECX is keeping the value of CR3
+
+ .byte 0xBF // opcode of mov di, imm16
+ .short GdtrLocation // mov di, GdtrProfile
+ .byte 0x66 // db 66h
+ .byte 0x2E, 0x0F, 0x01, 0x15 // lgdt fword ptr cs:[di]
+
+ .byte 0xBF // opcode of mov di, imm16
+ .short IdtrLocation // mov di, IdtrProfile
+ .byte 0x66 // db 66h
+ .byte 0x2E, 0x0F, 0x01, 0x1D // lidt fword ptr cs:[di]
+
+ .byte 0xBF // opcode of mov di, imm16
+ .short L_LongModeStartJumpOffset // Get offset of LongModeStartJump
+ .byte 0x66, 0x8B, 0x3D // mov edi,dword ptr [di] // EDI is keeping the LongModeStart Jump Address
+
+ .byte 0x31, 0xC0 // xor ax, ax
+ .byte 0x8E, 0xD8 // mov ds, ax
+
+ .byte 0x0F, 0x20, 0xC0 // mov eax, cr0 // Get control register 0
+ .byte 0x66, 0x83, 0xC8, 0x03 // or eax, 000000003h // Set PE bit (bit //0) and MP
+ .byte 0x0F, 0x22, 0xC0 // mov cr0, eax
+
+L_FLAT32_JUMP:
+
+ .byte 0x66, 0x67, 0xEA // far jump
+ .long 0x0 // 32-bit offset
+ .short 0x20 // 16-bit selector
+
+L_NemInit: // 32-bit protected mode entry point
+ .byte 0x66, 0xB8, 0x18, 0x00 // mov ax, 18h
+ .byte 0x66, 0x8E, 0xD8 // mov ds, ax
+ .byte 0x66, 0x8E, 0xC0 // mov es, ax
+ .byte 0x66, 0x8E, 0xE0 // mov fs, ax
+ .byte 0x66, 0x8E, 0xE8 // mov gs, ax
+ .byte 0x66, 0x8E, 0xD0 // mov ss, ax // Flat mode setup.
+
+L_PrepareToGoLongMode64:
+ .byte 0x0F, 0x20, 0xE0 // mov eax, cr4
+ .byte 0x66, 0x0D, 0x20, 0x06 // or ax, 0620h
+ .byte 0x0F, 0x22, 0xE0 // mov cr4, eax
+
+ .byte 0x0F, 0x22, 0xD9 // mov cr3, ecx
+
+ .byte 0xB9
+ .long 0xC0000080 // mov ecx, 0c0000080h // EFER MSR number.
+ .byte 0x0F, 0x32 // rdmsr // Read EFER.
+ .byte 0x0F, 0xBA, 0xE8, 0x08 // bts eax, 8 // Set LME=1.
+ .byte 0x0F, 0x30 // wrmsr // Write EFER.
+
+ .byte 0x0F, 0x20, 0xC0 // mov eax, cr0 // Read CR0.
+ .byte 0x0F, 0xBA, 0xE8, 0x1F // bts eax, 31 // Set PG=1.
+ .byte 0x0F, 0x22, 0xC0 // mov cr0, eax // Write CR0.
+
+L_LONG_JUMP:
+
+ .byte 0x67, 0xEA // far jump
+
+L_LongModeStartJump:
+
+ .long 0x0 // 32-bit offset
+ .short 0x38 // 16-bit selector
+
+L_LongModeStart:
+
+ mov $0x30, %ax
+ mov %ax, %ds
+ mov %ax, %es
+ mov %ax, %ss
+
+L_WaitFirstApTaskAssigned:
+//
+// First INIT-SIPI-SIPI will loop here until DetailedMpInitialization function assigned for each AP
+//
+ pause
+ cmpq $0, CProcedureLocation(%esi)
+ jz L_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 %edi, %eax
+ add $L_RunLoopAndMwaitLoop32Offset, %eax
+ mov %edi, %edx
+ add $L_RunLoopAndMwaitLoop32JumpOffset, %edx
+ mov %eax, (%rdx)
+
+ mov %rdx, %rbp // RBP = 32-bits compatibility mode FAR JUMP m16:32 operand pointer
+
+ mov %edi, %eax
+ add $L_RunLoopAndMwaitLoop64Offset, %eax
+ mov %edi, %edx
+ add $L_RunLoopAndMwaitLoop64JumpOffset, %edx
+ mov %eax, (%rdx)
+
+ //
+ // ProgramStack
+ //
+ xor %rcx, %rcx
+ mov %esi, %edi
+ add $BistBuffer, %edi
+ mov (%edi,%ebx,8), %ecx // RCX = CpuNumber
+
+ mov %esi, %edi
+ add $StackSizeLocation, %edi
+ mov (%edi), %rax
+ inc %rcx
+ mul %rcx // RAX = StackSize * (CpuNumber + 1)
+
+ mov %esi, %edi
+ add $StackStartAddressLocation, %edi
+ mov (%edi), %rdx
+ add %rdx, %rax // RAX = StackStart + StackSize * (CpuNumber + 1)
+
+ mov %rax, %rsp
+ sub $MonitorFilterSize, %rsp // Reserved Monitor data space
+ or $BreakToRunApSignal, %ebx // ebx = #Cpu run signature
+
+ //
+ // Call assembly function to initialize FPU.
+ //
+ mov ASM_PFX(gCpuInitFloatPointUnit)(%rip), %rax
+ sub $0x20, %rsp
+ callq *%rax
+ add $0x20, %rsp
+
+ //
+ // Load C Function pointer and wakeup manner location
+ //
+ mov %esi, %edi
+ add $CProcedureLocation, %edi
+ add $WakeUpApManner, %esi // esi = WakeUpApManner Address Location
+
+L_WakeUpThisAp64:
+
+ movq (%edi),%rax
+
+ test %rax, %rax
+ je L_CheckWakeUpCounterInit64
+
+ push %rbp
+ push %rbx
+ push %rsi
+ push %rdi
+
+ sub $0x20, %rsp
+ callq *%rax
+ add $0x20, %rsp
+
+ pop %rdi
+ pop %rsi
+ pop %rbx
+ pop %rbp
+
+L_CheckWakeUpCounterInit64:
+
+ cmpl $WakeUpApCounterInit, (%esi)
+ jnz L_CheckWakeUpManner64
+
+ //
+ // Initialize MONITOR_MWAIT_DATA data structure per thread
+ //
+ xor %rcx, %rcx
+ mov %rcx, (%rsp) // BreakToRunApSignal
+ mov %rcx, 0x8(%rsp) // HltLoopBreakCounter
+ mov %rcx, 0x10(%rsp) // MwaitLoopBreakCounter
+ mov %rcx, 0x18(%rsp) // RunLoopBreakCounter
+ mov %rcx, 0x20(%rsp) // MwaitLoopBreakCounter32
+ mov %rcx, 0x28(%rsp) // RunLoopBreakCounter32
+ mov %rcx, 0x30(%rsp) // WakeUpApVectorChangeFlag
+ mov %rcx, 0x38(%rsp) // MwaitTargetCstate
+
+L_WaitWakeUpMannerAssigned:
+
+ pause
+ cmpl $WakeUpApCounterInit, (%esi)
+ jz L_WaitWakeUpMannerAssigned
+
+L_CheckWakeUpManner64:
+
+ pause
+ mov (%esi), %edx
+ cmp $WakeUpApPerHltLoop, %edx
+ jz L_HltApLoop64
+
+ cmp $WakeUpApPerMwaitLoop, %edx
+ jz L_ApMwaitLoop64
+
+ cmp $WakeUpApPerRunLoop, %edx
+ jz L_CheckRunSignal64
+
+ jmp L_JumpToCompatibility32Mode
+
+L_ApMwaitLoop64:
+
+ cli
+ mov %rsp, %rax // Set Monitor Address
+ xor %rcx, %rcx
+ xor %rdx, %rdx
+ .byte 0x0F, 1, 0xC8 // MONITOR
+ mov 0x38(%rsp), %rax // Mwait Target C-State per rax[7:4]
+ .byte 0x0F, 1, 0xC9 // MWAIT
+
+L_CheckRunSignal64:
+
+ cmp %rbx, (%rsp) // Check if run signal correct?
+ jnz L_CheckWakeUpManner64 // Unknown break, go checking run manner
+
+ jmp L_WakeUpThisAp64 // Jmp to execute AP task
+
+L_HltApLoop64:
+ cli
+ hlt
+ jmp L_HltApLoop64 // Jump to halt loop
+
+L_JumpToCompatibility32Mode:
+
+ .byte 0xFF, 0x6D, 0x00 // jmp pword ptr [rbp+0] // Far jump to m16:32 for 32-bits compatibility mode
+
+L_RunLoopAndMwaitLoop32Jump:
+
+ .long 0x00 // m32 part of m16:32
+ .short 0x20 // m16 part of m16:32
+
+L_RunLoopAndMwaitLoop32:
+
+ .byte 0x66, 0xB8, 0x18, 0x00 // mov ax, 18h
+ .byte 0x66, 0x8E, 0xD8 // mov ds, ax
+ .byte 0x8e, 0xd0 // mov ss, ax
+
+ .byte 0xF, 0x20, 0xC0 // mov eax, cr0 // Read CR0.
+ .byte 0xF, 0xBA, 0xF0, 0x1F // btr eax, 31 // Reset PG=0.
+ .byte 0xF, 0x22, 0xC0 // mov cr0, eax // Write CR0.
+
+ .byte 0xB9
+ .long 0xC0000080 // mov ecx, 0c0000080h // EFER MSR number.
+ .byte 0xF, 0x32 // rdmsr // Read EFER.
+ .byte 0xF, 0xBA, 0xF0, 0x8 // btr eax, 8 // Reset LME=0.
+ .byte 0xF, 0x30 // wrmsr // Write EFER.
+
+ .byte 0xF, 0x20, 0xE0 // mov eax, cr4
+ .byte 0x24, 0xDF // and al, 0DFh // Reset PAE=0 in CR4 bit 5
+ .byte 0xF, 0x22, 0xE0 // mov cr4, eax
+
+L_CheckWakeUpManner32:
+
+ pause
+ cmpq $WakeUpApPerMwaitLoop32, (%rsi) // Use rsi for esi per compling in 64-bits mode
+ jnz L_CheckRunSignal32
+
+ cli
+ mov %esp, %eax // Set Monitor Address
+ xor %ecx, %ecx
+ xor %edx, %edx
+ .byte 0xf, 1, 0xc8 // MONITOR
+ mov 0x38(%rsp), %eax // Mwait Target C-State per eax[7:4]
+ .byte 0xf, 1, 0xc9 // MWAIT
+
+
+L_CheckRunSignal32:
+
+ cmp %ebx, (%rsp) // Check if run signal correct?
+ jnz L_CheckWakeUpManner32 // Unknown break, go checking run manner
+
+ .byte 0xF, 0x20, 0xE0 // mov eax, cr4
+ .byte 0xC, 0x20 // or al, 20h // Set PAE=1 in CR4 bit 5
+ .byte 0xF, 0x22, 0xE0 // mov cr4, eax
+
+ .byte 0xB9
+ .long 0xC0000080 // mov ecx, 0c0000080h // EFER MSR number.
+ .byte 0xF, 0x32 // rdmsr // Read EFER.
+ .byte 0xF, 0xBA, 0xE8, 0x8 // bts eax, 8 // Set LME=1.
+ .byte 0xF, 0x30 // wrmsr // Write EFER.
+
+ .byte 0xF, 0x20, 0xC0 // mov eax, cr0 // Read CR0.
+ .byte 0xF, 0xBA, 0xE8, 0x1F // bts eax, 31 // Set PG=1.
+ .byte 0xF, 0x22, 0xC0 // mov cr0, eax // Write CR0.
+
+ .byte 0x67, 0xEA // far jump back to 64-bits long mode
+
+L_RunLoopAndMwaitLoop64Jump:
+
+ .long 0x00 // 32-bit offset
+ .short 0x38 // 16-bit selector
+
+L_RunLoopAndMwaitLoop64:
+
+ mov $0x30, %ax
+ mov %ax, %ds
+ mov %ax, %ss
+
+ jmp L_WakeUpThisAp64
+
+.globl ASM_PFX(gCpuInitFloatPointUnit)
+ASM_PFX(gCpuInitFloatPointUnit):
+.long 0x0
+.long 0x0
+
+L_RendezvousFunnelProcEnd:
+
+//-------------------------------------------------------------------------------------
+// AsmGetAddressMap (&AddressMap);
+//-------------------------------------------------------------------------------------
+.set L_NemInitOffset, L_NemInit - L_RendezvousFunnelProcStart
+.set L_FLAT32_JUMPOffset, L_FLAT32_JUMP - L_RendezvousFunnelProcStart
+.set L_LongModeStartOffset, L_LongModeStart - L_RendezvousFunnelProcStart
+.set L_LONG_JUMPOffset, L_LONG_JUMP - L_RendezvousFunnelProcStart
+.set L_RendezvousFunnelProcEndOffset, L_RendezvousFunnelProcEnd - L_RendezvousFunnelProcStart
+
+.globl ASM_PFX(AsmGetAddressMap)
+ASM_PFX(AsmGetAddressMap):
+ leaq ASM_PFX(RendezvousFunnelProc)(%rip), %rax
+ mov %rax, (%rcx)
+ xor %rax, %rax
+ mov $L_NemInitOffset, %eax
+ mov %rax, 0x8(%rcx)
+ mov $L_FLAT32_JUMPOffset, %eax
+ mov %rax, 0x10(%rcx)
+ mov $L_LongModeStartOffset, %eax
+ mov %rax, 0x18(%rcx)
+ mov $L_LONG_JUMPOffset, %eax
+ mov %rax, 0x20(%rcx)
+ mov $L_RendezvousFunnelProcEndOffset, %eax
+ mov %rax, 0x28(%rcx)
+ retq
+
+#undef EFI_SUCCESS
+#undef EFI_NOT_READY
+#define EFI_SUCCESS 0
+#define EFI_NOT_READY 0x8000000000000006
+
+.globl ASM_PFX(AsmTryAcquireMPLock)
+ASM_PFX(AsmTryAcquireMPLock):
+
+ mov $NotVacantFlag, %al
+
+ xchg %al, (%rcx)
+ cmp $VacantFlag, %al
+ jz L_Vacant
+
+ mov $EFI_NOT_READY, %rax
+ jmp L_Done
+
+L_Vacant:
+ mov $EFI_SUCCESS, %rax
+L_Done:
+ retq
+
+.globl ASM_PFX(AsmAcquireMPLock)
+ASM_PFX(AsmAcquireMPLock):
+
+ mov $NotVacantFlag, %al
+L_TryGetLock:
+ xchg %al, (%rcx)
+ cmp $VacantFlag, %al
+ jz L_LockObtained
+
+ pause
+ jmp L_TryGetLock
+
+L_LockObtained:
+ retq
+
+.globl ASM_PFX(AsmReleaseMPLock)
+ASM_PFX(AsmReleaseMPLock):
+
+ mov $VacantFlag, %al
+ xchg %al, (%rcx)
+
+ retq
+
+//-------------------------------------------------------------------------------------
+//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);
+//-------------------------------------------------------------------------------------
+#define CPU_SWITCH_STATE_IDLE 0
+#define CPU_SWITCH_STATE_STORED 1
+#define CPU_SWITCH_STATE_LOADED 2
+
+.globl ASM_PFX(AsmExchangeRole)
+ASM_PFX(AsmExchangeRole):
+ // 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 %cr0, %rax
+ push %rax
+
+ mov %cr4, %rax
+ push %rax
+
+ // rsi contains MyInfo pointer
+ mov %rcx, %rsi
+
+ // rdi contains OthersInfo pointer
+ mov %rdx, %rdi
+
+ //Store EFLAGS, GDTR and IDTR regiter to stack
+ pushfq
+ sgdt 16(%rsi)
+ sidt 26(%rsi)
+
+ // Store the its StackPointer
+ mov %rsp, 8(%rsi)
+
+ // update its switch state to STORED
+ mov $NotVacantFlag, %al
+L_TryLock1:
+ xchg %al, (%rsi)
+ cmp $VacantFlag, %al
+ jz L_LockObtained1
+ pause
+ jmp L_TryLock1
+
+L_LockObtained1:
+ movb $CPU_SWITCH_STATE_STORED, 1(%rsi)
+ xchg %al, (%rsi)
+
+L_WaitForOtherStored:
+ // wait until the other CPU finish storing its state
+ mov $NotVacantFlag, %al
+L_TryLock2:
+ xchg %al, (%rdi)
+ cmp $VacantFlag, %al
+ jz L_LockObtained2
+ pause
+ jmp L_TryLock2
+
+L_LockObtained2:
+ mov 1(%rdi), %bl
+ xchg %al, (%rdi)
+ cmp $CPU_SWITCH_STATE_STORED, %bl
+ jb L_WaitForOtherStored
+
+ // Since another CPU already stored its state, load them
+ // load GDTR value
+ lgdt 16(%rdi)
+
+ // load IDTR value
+ lidt 26(%rdi)
+
+ // load its future StackPointer
+ mov 8(%rdi), %rsp
+
+ // update its switch state to LOADED
+ mov $NotVacantFlag, %al
+L_TryLock3:
+ xchg %al, (%rsi)
+ cmp $VacantFlag, %al
+ jz L_LockObtained3
+ pause
+ jmp L_TryLock3
+
+L_LockObtained3:
+ movb $CPU_SWITCH_STATE_LOADED, 1(%rsi)
+ xchg %al, (%rsi)
+
+L_WaitForOtherLoaded:
+ // wait until the other CPU finish loading new state,
+ // otherwise the data in stack may corrupt
+ mov $NotVacantFlag, %al
+L_TryLock4:
+ xchg %al, (%rdi)
+ cmp $VacantFlag, %al
+ jz L_LockObtained4
+ pause
+ jmp L_TryLock4
+
+L_LockObtained4:
+ mov 1(%rdi), %bl
+ xchg %al, (%rdi)
+ cmp $CPU_SWITCH_STATE_LOADED, %bl
+ jb L_WaitForOtherLoaded
+
+ // since the other CPU already get the data it want, leave this procedure
+ popfq
+
+ pop %rax
+ mov %rax, %cr4
+
+ pop %rax
+ mov %rax, %cr0
+
+ 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
+
+ retq
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.asm b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.asm
new file mode 100644
index 0000000000..ead13793f0
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/MpFuncs.asm
@@ -0,0 +1,591 @@
+;; @file
+; This is the assembly code for EM64T MP support.
+;
+; Copyright (c) 2005 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+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
+
+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:
+ xchg al, byte ptr [rsi]
+ cmp al, VacantFlag
+ jz LockObtained1
+ pause
+ jmp TryLock1
+
+LockObtained1:
+ mov byte ptr [rsi + 1], CPU_SWITCH_STATE_STORED
+ xchg al, byte ptr [rsi]
+
+WaitForOtherStored::
+ ; wait until the other CPU finish storing its state
+ mov al, NotVacantFlag
+TryLock2:
+ xchg al, byte ptr [rdi]
+ cmp al, VacantFlag
+ jz LockObtained2
+ PAUSE32
+ jmp TryLock2
+
+LockObtained2:
+ mov bl, byte ptr [rdi + 1]
+ 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:
+ xchg al, byte ptr [rsi]
+ cmp al, VacantFlag
+ jz LockObtained3
+ PAUSE32
+ jmp TryLock3
+
+LockObtained3:
+ mov byte ptr [rsi+1], CPU_SWITCH_STATE_LOADED
+ 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:
+ xchg al, byte ptr [rdi]
+ cmp al, VacantFlag
+ jz LockObtained4
+ PAUSE32
+ jmp TryLock4
+
+LockObtained4:
+ mov bl, byte ptr [rdi+1]
+ 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
+
+AsmWaitMem PROC PUBLIC
+AsmWaitMemS:
+ pause
+ cmp qword ptr [rcx], rdx
+ jz AsmWaitMemS
+ ret
+
+AsmWaitMem ENDP
+
+text ENDS
+
+END
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/ProcessorDef.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/ProcessorDef.h
new file mode 100644
index 0000000000..406e891c17
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/ProcessorDef.h
@@ -0,0 +1,53 @@
+/** @file
+ Definition for EM64T processor.
+
+ Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#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;
+
+/**
+ Get address map of RendezvousFunnelProc.
+
+ @param[out] AddressMap Output buffer for address map information
+
+**/
+VOID
+AsmGetAddressMap (
+ OUT MP_ASSEMBLY_ADDRESS_MAP *AddressMap
+ );
+
+#endif
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/VirtualMemory.h b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/VirtualMemory.h
new file mode 100644
index 0000000000..48a9159e38
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/VirtualMemory.h
@@ -0,0 +1,131 @@
+/** @file
+ x64 Long Mode Virtual Memory Management Definitions.
+
+ Copyright (c) 2004 - 2016, Intel Corporation. All rights reserved.<BR>
+
+ This program and the accompanying materials
+ are licensed and made available under the terms and conditions of the BSD License
+ which accompanies this distribution. The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php.
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#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 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 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;
+
+#pragma pack()
+
+#endif
+
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.S b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.S
new file mode 100644
index 0000000000..0812f8ea2b
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.S
@@ -0,0 +1,1658 @@
+## @file
+# Assembly code for the x64 resources.
+#
+# Copyright (c) 1999 - 2016, Intel Corporation. All rights reserved.<BR>
+#
+# This program and the accompanying materials
+# are licensed and made available under the terms and conditions of the BSD License
+# which accompanies this distribution. The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+#
+##
+
+//=============================================================================
+// Debug Macros
+//=============================================================================
+
+//ENABLE_DEADLOOP = Deadloop on exception handler entry, exit
+
+// #define ENABLE_DEADLOOP 1
+#ifdef ENABLE_DEADLOOP
+ .macro DEADLOOP
+ jmp .
+ .endm
+#else
+ .macro DEADLOOP
+ .endm
+#endif
+
+ .text
+ ASM_FUNCTION_REMOVE_IF_UNREFERENCED
+
+//.globl _CpuFlushTlb
+.globl __outp
+.globl ASM_PFX(mErrorCodeFlag)
+
+//
+// Float control word initial value:
+// all exceptions masked, double-extended-precision, round-to-nearest
+//
+mFpuControlWord:
+.word 0x37F
+
+//
+// Multimedia-extensions control word:
+// all exceptions masked, round-to-nearest, flush to zero for masked underflow
+//
+mMmxControlWord:
+.long 0x1F80
+
+//
+//
+//
+//------------------------------------------------------------------------------
+// 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
+//
+//------------------------------------------------------------------------------
+
+ .p2align 3
+
+.globl ASM_PFX(AsmIdtVector00)
+ASM_PFX(AsmIdtVector00):
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector00, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector00 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector01, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector01 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector02, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector02 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector03, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector03 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector04, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector04 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector05, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector05 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector06, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector06 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector07, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector07 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector08, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector08 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector09, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector09 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector0a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector0a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector0b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector0b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector0c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector0c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector0d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector0d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector0e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector0e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector0f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector0f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector10, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector10 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector11, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector11 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector12, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector12 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector13, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector13 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector14, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector14 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector15, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector15 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector16, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector16 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector17, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector17 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector18, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector18 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector19, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector19 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector1a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector1a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector1b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector1b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector1c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector1c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector1d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector1d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector1e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector1e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector1f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector1f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector20, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector20 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector21, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector21 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector22, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector22 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector23, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector23 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector24, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector24 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector25, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector25 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector26, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector26 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector27, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector27 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector28, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector28 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector29, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector29 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector2a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector2a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector2b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector2b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector2c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector2c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector2d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector2d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector2e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector2e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector2f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector2f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector30, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector30 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector31, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector31 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector32, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector32 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector33, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector33 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector34, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector34 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector35, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector35 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector36, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector36 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector37, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector37 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector38, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector38 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector39, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector39 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector3a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector3a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector3b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector3b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector3c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector3c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector3d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector3d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector3e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector3e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector3f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector3f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector40, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector40 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector41, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector41 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector42, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector42 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector43, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector43 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector44, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector44 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector45, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector45 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector46, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector46 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector47, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector47 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector48, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector48 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector49, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector49 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector4a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector4a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector4b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector4b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector4c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector4c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector4d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector4d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector4e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector4e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector4f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector4f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector50, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector50 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector51, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector51 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector52, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector52 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector53, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector53 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector54, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector54 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector55, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector55 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector56, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector56 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector57, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector57 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector58, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector58 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector59, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector59 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector5a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector5a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector5b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector5b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector5c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector5c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector5d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector5d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector5e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector5e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector5f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector5f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector60, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector60 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector61, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector61 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector62, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector62 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector63, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector63 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector64, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector64 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector65, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector65 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector66, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector66 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector67, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector67 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector68, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector68 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector69, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector69 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector6a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector6a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector6b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector6b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector6c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector6c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector6d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector6d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector6e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector6e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector6f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector6f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector70, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector70 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector71, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector71 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector72, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector72 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector73, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector73 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector74, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector74 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector75, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector75 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector76, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector76 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector77, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector77 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector78, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector78 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector79, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector79 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector7a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector7a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector7b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector7b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector7c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector7c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector7d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector7d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector7e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector7e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector7f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector7f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector80, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector80 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector81, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector81 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector82, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector82 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector83, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector83 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector84, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector84 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector85, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector85 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector86, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector86 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector87, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector87 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector88, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector88 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector89, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector89 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector8a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector8a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector8b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector8b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector8c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector8c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector8d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector8d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector8e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector8e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector8f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector8f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector90, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector90 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector91, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector91 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector92, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector92 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector93, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector93 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector94, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector94 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector95, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector95 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector96, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector96 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector97, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector97 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector98, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector98 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector99, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector99 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector9a, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector9a / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector9b, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector9b / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector9c, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector9c / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector9d, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector9d / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector9e, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector9e / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vector9f, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vector9f / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora0, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora0 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora1, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora1 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora2, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora2 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora3, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora3 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora4, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora4 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora5, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora5 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora6, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora6 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora7, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora7 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora8, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora8 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectora9, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectora9 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectoraa, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectoraa / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorab, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorab / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorac, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorac / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorad, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorad / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorae, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorae / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectoraf, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectoraf / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb0, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb0 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb1, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb1 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb2, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb2 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb3, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb3 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb4, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb4 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb5, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb5 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb6, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb6 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb7, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb7 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb8, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb8 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorb9, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorb9 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorba, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorba / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorbb, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorbb / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorbc, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorbc / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorbd, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorbd / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorbe, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorbe / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorbf, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorbf / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc0, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc0 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc1, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc1 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc2, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc2 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc3, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc3 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc4, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc4 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc5, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc5 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc6, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc6 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc7, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc7 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc8, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc8 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorc9, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorc9 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorca, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorca / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorcb, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorcb / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorcc, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorcc / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorcd, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorcd / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorce, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorce / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorcf, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorcf / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord0, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord0 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord1, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord1 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord2, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord2 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord3, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord3 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord4, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord4 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord5, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord5 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord6, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord6 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord7, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord7 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord8, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord8 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectord9, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectord9 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorda, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorda / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectordb, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectordb / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectordc, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectordc / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectordd, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectordd / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorde, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorde / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectordf, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectordf / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore0, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore0 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore1, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore1 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore2, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore2 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore3, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore3 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore4, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore4 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore5, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore5 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore6, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore6 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore7, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore7 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore8, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore8 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectore9, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectore9 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorea, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorea / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectoreb, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectoreb / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorec, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorec / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectored, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectored / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectoree, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectoree / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectoref, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectoref / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf0, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf0 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf1, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf1 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf2, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf2 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf3, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf3 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf4, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf4 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf5, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf5 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf6, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf6 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf7, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf7 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf8, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf8 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorf9, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorf9 / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorfa, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorfa / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorfb, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorfb / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorfc, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorfc / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorfd, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorfd / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorfe, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorfe / 8
+ nop
+
+ callq ASM_PFX(CommonInterruptEntry)
+ .set vectorff, . - ASM_PFX(AsmIdtVector00) - 5
+ .short vectorff / 8
+ nop
+
+//---------------------------------------//
+// CommonInterruptEntry //
+//---------------------------------------//
+// The follow algorithm is used for the common interrupt routine.
+
+// +---------------------+ <-- 16-byte aligned ensured by processor
+// 40 + Old SS +
+// +---------------------+
+// 32 + Old RSP +
+// +---------------------+
+// 24 + RFlags +
+// +---------------------+
+// 16 + CS +
+// +---------------------+
+// 8 + RIP +
+// +---------------------+
+// 0 + RBP + <-- RBP, 16-byte aligned
+// +---------------------+
+// -8 + RCX / Vector Number +
+// +---------------------+
+//-16 + Error Code +
+// +---------------------+
+//
+
+.globl ASM_PFX(CommonInterruptEntry)
+ASM_PFX(CommonInterruptEntry):
+
+ DEADLOOP
+
+ cli
+ //
+ // All interrupt handlers are invoked through interrupt gates, so
+ // IF flag automatically cleared at the entry point
+ //
+ //
+ // Calculate vector number
+ //
+ xchg (%rsp), %rcx // get the return address of call
+ movzwl (%rcx), %ecx // actually, it is the address of vector number.
+ cmp $32, %ecx // Intel reserved vector for exceptions?
+ jae L_NoErrorCode
+ bt %ecx, ASM_PFX(mErrorCodeFlag)(%rip)
+ jc L_ErrorCode
+
+L_NoErrorCode:
+ pushq (%rsp) // Copy RCX/Vector to next stack slot
+ mov %rbp, 8(%rsp) // Save RBP
+ push $0 // Push dummy error code
+ jmp L_SaveState
+
+L_ErrorCode:
+ xchg 8(%rsp), %rbp // Put RBP into Error Code slot
+ push %rbp // Push Error Code to stack
+
+ //
+ // 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//
+L_SaveState:
+ mov %rsp, %rbp
+ add $16, %rbp // Fixup RBP after RCX/Vector, Error Code swizzle
+ push %r15
+ push %r14
+ push %r13
+ push %r12
+ push %r11
+ push %r10
+ push %r9
+ push %r8
+ push %rax
+ push -8(%rbp) // RCX
+ push %rdx
+ push %rbx
+ pushq 32(%rbp) // RSP
+ pushq 0(%rbp) // RBP
+ push %rsi
+ push %rdi
+
+//// UINT64 Gs, Fs, Es, Ds, Cs, Ss// insure high 16 bits of each is zero
+ movzwq 40(%rbp), %rax
+ push %rax // for ss
+ movzwq 16(%rbp), %rax
+ push %rax // for cs
+ mov %ds, %eax
+ push %rax
+ mov %es, %eax
+ push %rax
+ mov %fs, %eax
+ push %rax
+ mov %gs, %eax
+ push %rax
+
+ mov %rcx, -8(%rbp) // save vector number
+
+//// UINT64 Rip
+ pushq 8(%rbp)
+
+//// UINT64 Gdtr[2], Idtr[2]//
+ sub $16, %rsp // IDT base / limit
+ sidt 6(%rsp)
+ movzwq 6(%rsp), %rax
+ mov %rax, (%rsp)
+
+ sub $16, %rsp // GDT base / limit
+ sgdt 6(%rsp)
+ movzwq 6(%rsp), %rax
+ mov %rax, (%rsp)
+
+//// UINT64 Ldtr, Tr//
+ xor %rax, %rax
+ str %ax
+ push %rax
+ sldt %ax
+ push %rax
+
+//// UINT64 RFlags//
+ pushq 24(%rbp)
+
+//// UINT64 Cr0, Cr1, Cr2, Cr3, Cr4, Cr8//
+ mov %cr8, %rax
+ push %rax
+ mov %cr4, %rax
+ or $0x208, %rax
+ mov %rax, %cr4
+ push %rax
+ mov %cr3, %rax
+ push %rax
+ mov %cr2, %rax
+ push %rax
+ xor %rax, %rax
+ push %rax
+ mov %cr0, %rax
+ push %rax
+
+//// UINT64 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7//
+ mov %db7, %rax
+ push %rax
+ mov %db6, %rax
+ push %rax
+ mov %db3, %rax
+ push %rax
+ mov %db2, %rax
+ push %rax
+ mov %db1, %rax
+ push %rax
+ mov %db0, %rax
+ push %rax
+
+//// FX_SAVE_STATE_X64 FxSaveState//
+
+ sub $512, %rsp
+ mov %rsp, %rdi
+ .byte 0x0f, 0xae, 0x07 //fxsave [rdi]
+
+//// UINT32 ExceptionData//
+ pushq -16(%rbp)
+
+//// call into exception handler
+ mov -8(%rbp), %rcx
+ mov ExternalVectorTablePtr(%rip), %rax // get the interrupt vectors base
+ mov (%rax,%rcx,8),%rax
+ or %rax, %rax // NULL?
+
+ je L_nonNullValue
+
+//// Prepare parameter and call
+ mov %rsp, %rdx
+
+ //
+ // Per X64 calling convention, allocate maximum parameter stack space
+ // and make sure RSP is 16-byte aligned
+ //
+ DEADLOOP
+
+ sub $0x28, %rsp
+ callq *%rax
+ add $0x28, %rsp
+
+ DEADLOOP
+
+L_nonNullValue:
+ cli
+//// UINT64 ExceptionData
+ pop %rax
+ mov %rax, -16(%rbp)
+
+//// FX_SAVE_STATE_X64 FxSaveState//
+
+ mov %rsp, %rsi
+ .byte 0x0f, 0xae, 0x0e // fxrstor [rsi]
+ add $512, %rsp
+
+//// UINT64 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7//
+ add $48, %rsp
+ xor %rax, %rax //// always clear dr6
+ mov %rax, %dr6
+
+//// UINT64 Cr0, Cr1, Cr2, Cr3, Cr4, Cr8//
+ pop %rax
+ mov %rax, %cr0
+ add $8, %rsp // not for Cr1
+ pop %rax
+ mov %rax, %cr2
+ pop %rax
+ mov %rax, %cr3
+ pop %rax
+ mov %rax, %cr4
+ pop %rax
+ mov %rax, %cr8
+
+//// UINT64 RFlags
+ popq 24(%rbp)
+
+//// UINT64 Ldtr, Tr//
+//// UINT64 Gdtr[2], Idtr[2]//
+//// Best not let anyone mess with these particular registers...
+ add $48, %rsp
+
+//// UINT64 Rip
+ popq 8(%rbp)
+
+//// 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 %eax, %es
+ pop %rax
+ mov %eax, %ds
+ popq 16(%rbp) // for cs
+ popq 40(%rbp) // 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 $8, %rsp // not for rbp
+ popq 32(%rbp) // 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 %rbp, %rsp
+ pop %rbp
+
+ DEADLOOP
+
+ iretq
+
+//
+//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).
+//
+.globl ASM_PFX(CpuInitFloatPointUnit)
+ASM_PFX(CpuInitFloatPointUnit):
+
+ //
+ // Initialize floating point units
+ //
+ // The following opcodes stand for instruction 'finit'
+ // to be supported by some 64-bit assemblers
+ //
+
+ //finit
+ .byte 0x9B, 0xDB, 0xE3
+ fldcw mFpuControlWord(%rip)
+
+ //
+ // Set OSFXSR bit 9 in CR4
+ //
+ mov %cr4,%rax
+ or $0x200,%rax
+ mov %rax,%cr4
+
+ ldmxcsr mMmxControlWord(%rip)
+ retq
+
+.globl ASM_PFX(InitializeExternalVectorTablePtr)
+ASM_PFX(InitializeExternalVectorTablePtr):
+ mov %rcx, ExternalVectorTablePtr(%rip)
+ retq
+
+ExternalVectorTablePtr:
+.quad 0 // point to the external interrupt vector table
+
+__outp:
+ mov %edx,%eax
+ movzwl %cx,%edx
+ out %al,(%dx)
+ retq
diff --git a/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.asm b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.asm
new file mode 100644
index 0000000000..03837c5670
--- /dev/null
+++ b/Silicon/BroxtonSoC/BroxtonSiPkg/Cpu/CpuInit/Dxe/x64/cpu.asm
@@ -0,0 +1,413 @@
+;; @file
+; Assembly code for the x64 resources.
+;
+; Copyright (c) 2005 - 2016, Intel Corporation. All rights reserved.<BR>
+;
+; This program and the accompanying materials
+; are licensed and made available under the terms and conditions of the BSD License
+; which accompanies this distribution. The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
+;
+;;
+
+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
+ ;
+ ; 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
+
+;
+; 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
+
+text ENDS
+END
+