From de243ee44476ccdb4979e5719cb7ac8a8c290565 Mon Sep 17 00:00:00 2001
From: vanjeff <vanjeff@6f19259b-4bc3-4df7-8a09-765794883524>
Date: Fri, 11 Jun 2010 08:24:01 +0000
Subject: [PATCH] 1.Restore BSP IDT table to AP when AP wakeup. 2.Restore
 Virtual wire mode on AP when AP wakeup.

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@10575 6f19259b-4bc3-4df7-8a09-765794883524
---
 .../IA32/AsmInclude.inc                       |   5 +-
 .../IA32/MpFuncs.S                            |   7 +-
 .../IA32/MpFuncs.asm                          |   7 +-
 .../MpServicesOnFrameworkMpServicesThunk.c    | 119 ++++++++++++++----
 .../MpServicesOnFrameworkMpServicesThunk.h    |  18 +--
 .../X64/AsmInclude.inc                        |   7 +-
 .../X64/MpFuncs.S                             |   7 +-
 .../X64/MpFuncs.asm                           |  35 +++---
 8 files changed, 153 insertions(+), 52 deletions(-)

diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/AsmInclude.inc b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/AsmInclude.inc
index 6ebd98c98f..d396726bfe 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/AsmInclude.inc
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/AsmInclude.inc
@@ -1,7 +1,7 @@
 ;------------------------------------------------------------------------------
 ; Include file for IA32 MpFuncs.asm
 ;
-; Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2009 - 2010, 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
@@ -20,6 +20,7 @@ StackStart          equ        LockLocation + 4h
 StackSize           equ        LockLocation + 8h
 RendezvousProc      equ        LockLocation + 0Ch
 GdtrProfile         equ        LockLocation + 10h
-BufferStart         equ        LockLocation + 18h
+IdtrProfile         equ        LockLocation + 16h
+BufferStart         equ        LockLocation + 1Ch
 
 ;-------------------------------------------------------------------------------
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S
index 91313b1aca..4da7fd5011 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S
@@ -1,7 +1,7 @@
 #------------------------------------------------------------------------------
 # IA32 assembly file for AP startup vector.
 #
-# Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2009 - 2010, 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
@@ -58,6 +58,11 @@ RendezvousFunnelProcStart:
         .byte 0x66                    # db         66h
         .byte 0x2E,0xF,0x1,0x14       # lgdt       fword ptr cs:[si]
 
+        .byte 0xBE
+        .word IdtrProfile
+        .byte 0x66                    # db         66h
+        .byte 0x2E,0xF,0x1,0x1C       # lidt       fword ptr cs:[si]
+
         .byte 0x33,0xC0               # xor        ax,  ax
         .byte 0x8E,0xD8               # mov        ds,  ax
         .byte 0xF,0x20,0xC0           # mov        eax, cr0                    ; Get control register 0
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.asm b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.asm
index e822cdbc60..e5f38a6bbe 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.asm
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.asm
@@ -1,7 +1,7 @@
 ;------------------------------------------------------------------------------
 ; IA32 assembly file for AP startup vector.
 ;
-; Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2009 - 2010, 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
@@ -60,6 +60,11 @@ RendezvousFunnelProcStart::
         dw GdtrProfile                ; mov        si, GdtrProfile
         db 66h                        ; db         66h
         db 2Eh,0Fh, 01h, 14h          ; lgdt       fword ptr cs:[si]
+
+        db 0BEh
+        dw IdtrProfile                ; 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
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
index 4097b9e87a..3ff503758d 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
@@ -117,7 +117,7 @@ GetNumberOfProcessors (
   @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. 
+  @retval EFI_NOT_FOUND         Processor with the handle specified by ProcessorNumber does not exist.
 
 **/
 EFI_STATUS
@@ -165,7 +165,7 @@ GetProcessorInfo (
   ASSERT_EFI_ERROR (Status);
 
   ProcessorInfoBuffer->ProcessorId = (UINT64) ProcessorContextBuffer.ApicID;
-  
+
   //
   // Get Status Flag of specified processor
   //
@@ -208,7 +208,7 @@ GetProcessorInfo (
   @param  FailedCpuList         The list of processor numbers that fail to finish the function before
                                 TimeoutInMicrosecsond expires.
 
-  @retval EFI_SUCCESS           In blocking mode, all APs have finished before the timeout expired. 
+  @retval EFI_SUCCESS           In blocking mode, all APs have finished before the timeout expired.
   @retval EFI_SUCCESS           In non-blocking mode, function has been dispatched to all enabled APs.
   @retval EFI_DEVICE_ERROR      Caller processor is AP.
   @retval EFI_NOT_STARTED       No enabled AP exists in the system.
@@ -275,7 +275,7 @@ StartupAllAPs (
           mStopCheckAPsStatus = FALSE;
           return EFI_NOT_READY;
         } else {
-          //     
+          //
           // Mark this processor as responsible for current calling.
           //
           mMPSystemData.CpuList[ProcessorNumber] = TRUE;
@@ -318,7 +318,7 @@ StartupAllAPs (
       }
     }
   }
-  
+
   //
   // If no enabled AP exists, return EFI_NOT_STARTED.
   //
@@ -599,7 +599,7 @@ SwitchBSP (
   @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
@@ -661,7 +661,7 @@ EnableDisableAP (
   ASSERT_EFI_ERROR (Status);
 
   ChangeCpuState (ProcessorNumber, EnableAP);
-  
+
   return EFI_SUCCESS;
 }
 
@@ -721,7 +721,7 @@ CheckAPsStatus (
   UINTN           ProcessorNumber;
   CPU_DATA_BLOCK  *CpuData;
   EFI_STATUS      Status;
-  
+
   //
   // If CheckAPsStatus() is stopped, then return immediately.
   //
@@ -853,7 +853,7 @@ CheckAllAPs (
       ASSERT (*mMPSystemData.FailedCpuList != NULL);
     }
     ListIndex = 0;
- 
+
     for (ProcessorNumber = 0; ProcessorNumber < mNumberOfProcessors; ProcessorNumber++) {
       //
       // Check whether this processor is responsible for StartupAllAPs().
@@ -920,7 +920,7 @@ CheckThisAP (
     AcquireSpinLock (&CpuData->CpuDataLock);
     CpuData->State = CpuStateIdle;
     ReleaseSpinLock (&CpuData->CpuDataLock);
-    
+
     if (CpuData->Finished != NULL) {
       *(CpuData->Finished) = TRUE;
     }
@@ -1075,6 +1075,71 @@ GetNextWaitingProcessorNumber (
   return EFI_NOT_FOUND;
 }
 
+/**
+  Programs Local APIC registers for virtual wire mode.
+
+  This function programs Local APIC registers for virtual wire mode.
+
+  @param  Bsp  Indicates whether the programmed processor is going to be BSP
+
+**/
+VOID
+ProgramVirtualWireMode (
+  BOOLEAN                       Bsp
+  )
+{
+  UINTN                 ApicBase;
+  UINT32                Value;
+
+  ApicBase = (UINTN)AsmMsrBitFieldRead64 (27, 12, 35) << 12;
+
+  //
+  // Program the Spurious Vector entry
+  // Set bit 8 (APIC Software Enable/Disable) to enable local APIC,
+  // and set Spurious Vector as 0x0F.
+  //
+  MmioBitFieldWrite32 (ApicBase + APIC_REGISTER_SPURIOUS_VECTOR_OFFSET, 0, 9, 0x10F);
+
+  //
+  // Program the LINT0 vector entry as ExtInt
+  // Set bits 8..10 to 7 as ExtInt Delivery Mode,
+  // and clear bits for Delivery Status, Interrupt Input Pin Polarity, Remote IRR,
+  // Trigger Mode, and Mask
+  //
+  if (!Bsp) {
+    DisableInterrupts ();
+  }
+  Value = MmioRead32 (ApicBase + APIC_REGISTER_LINT0_VECTOR_OFFSET);
+  Value = BitFieldWrite32 (Value, 8, 10, 7);
+  Value = BitFieldWrite32 (Value, 12, 16, 0);
+  if (!Bsp) {
+    //
+    // For APs, LINT0 is masked
+    //
+    Value = BitFieldWrite32 (Value, 16, 16, 1);
+  }
+  MmioWrite32 (ApicBase + APIC_REGISTER_LINT0_VECTOR_OFFSET, Value);
+
+  //
+  // Program the LINT1 vector entry as NMI
+  // Set bits 8..10 to 4 as NMI Delivery Mode,
+  // and clear bits for Delivery Status, Interrupt Input Pin Polarity, Remote IRR,
+  // Trigger Mode.
+  // For BSP clear Mask bit, and for AP set mask bit.
+  //
+  Value = MmioRead32 (ApicBase + APIC_REGISTER_LINT1_VECTOR_OFFSET);
+  Value = BitFieldWrite32 (Value, 8, 10, 4);
+  Value = BitFieldWrite32 (Value, 12, 16, 0);
+  if (!Bsp) {
+    //
+    // For APs, LINT1 is masked
+    //
+    Value = BitFieldWrite32 (Value, 16, 16, 1);
+  }
+  MmioWrite32 (ApicBase + APIC_REGISTER_LINT1_VECTOR_OFFSET, Value);
+}
+
+
 /**
   Wrapper function for all procedures assigned to AP.
 
@@ -1092,6 +1157,8 @@ ApProcWrapper (
   UINTN                 ProcessorNumber;
   CPU_DATA_BLOCK        *CpuData;
 
+  ProgramVirtualWireMode (FALSE);
+
   WhoAmI (&mMpService, &ProcessorNumber);
   CpuData = &mMPSystemData.CpuData[ProcessorNumber];
 
@@ -1149,7 +1216,7 @@ SendInitSipiSipi (
   UINTN                 ApicBase;
   UINT32                ICRLow;
   UINT32                ICRHigh;
-  
+
   UINT32                VectorNumber;
   UINT32                DeliveryMode;
 
@@ -1203,7 +1270,7 @@ SendInitSipiSipi (
 
 /**
   Function to wake up a specified AP and assign procedure to it.
-  
+
   @param  ProcessorNumber  Handle number of the specified processor.
   @param  Procedure        Procedure to assign.
   @param  ProcArguments    Argument for Procedure.
@@ -1246,7 +1313,7 @@ WakeUpAp (
 
 /**
   Terminate AP's task and set it to idle state.
-  
+
   This function terminates AP's task due to timeout by sending INIT-SIPI,
   and sends it to idle state.
 
@@ -1444,7 +1511,9 @@ PrepareAPStartupVector (
 {
   MP_ASSEMBLY_ADDRESS_MAP   AddressMap;
   IA32_DESCRIPTOR           GdtrForBSP;
+  IA32_DESCRIPTOR           IdtrForBSP;
   EFI_PHYSICAL_ADDRESS      GdtForAP;
+  EFI_PHYSICAL_ADDRESS      IdtForAP;
   EFI_STATUS                Status;
 
   //
@@ -1483,6 +1552,7 @@ PrepareAPStartupVector (
   mExchangeInfo->StackSize  = AP_STACK_SIZE;
 
   AsmReadGdtr (&GdtrForBSP);
+  AsmReadIdtr (&IdtrForBSP);
 
   //
   // Allocate memory under 4G to hold GDT for APs
@@ -1491,15 +1561,20 @@ PrepareAPStartupVector (
   Status   = gBS->AllocatePages (
                     AllocateMaxAddress,
                     EfiBootServicesData,
-                    EFI_SIZE_TO_PAGES (GdtrForBSP.Limit + 1),
+                    EFI_SIZE_TO_PAGES ((GdtrForBSP.Limit + 1) + (IdtrForBSP.Limit + 1)),
                     &GdtForAP
                     );
   ASSERT_EFI_ERROR (Status);
 
+  IdtForAP = (UINTN) GdtForAP + GdtrForBSP.Limit + 1;
+
   CopyMem ((VOID *) (UINTN) GdtForAP, (VOID *) GdtrForBSP.Base, GdtrForBSP.Limit + 1);
+  CopyMem ((VOID *) (UINTN) IdtForAP, (VOID *) IdtrForBSP.Base, IdtrForBSP.Limit + 1);
 
   mExchangeInfo->GdtrProfile.Base  = (UINTN) GdtForAP;
   mExchangeInfo->GdtrProfile.Limit = GdtrForBSP.Limit;
+  mExchangeInfo->IdtrProfile.Base  = (UINTN) IdtForAP;
+  mExchangeInfo->IdtrProfile.Limit = IdtrForBSP.Limit;
 
   mExchangeInfo->BufferStart = (UINT32) mStartupVector;
   mExchangeInfo->Cr3         = (UINT32) (AsmReadCr3 ());
@@ -1507,7 +1582,7 @@ PrepareAPStartupVector (
 
 /**
   Prepares memory region for processor configuration.
-  
+
   This function prepares memory region for processor configuration.
 
 **/
@@ -1525,13 +1600,13 @@ PrepareMemoryForConfiguration (
   for (Index = 0; Index < MAX_CPU_NUMBER; Index++) {
     InitializeSpinLock (&mMPSystemData.CpuData[Index].CpuDataLock);
   }
-  
+
   PrepareAPStartupVector ();
 }
 
 /**
   Gets the processor number of BSP.
-  
+
   @return  The processor number of BSP.
 
 **/
@@ -1555,7 +1630,7 @@ GetBspNumber (
                                     &ProcessorContextBuffer
                                     );
     ASSERT_EFI_ERROR (Status);
-    
+
     if (ProcessorContextBuffer.Designation == EfiCpuBSP) {
       break;
     }
@@ -1568,9 +1643,9 @@ GetBspNumber (
 /**
   Entrypoint of MP Services Protocol thunk driver.
 
-  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
   @param[in] SystemTable    A pointer to the EFI System Table.
-  
+
   @retval EFI_SUCCESS       The entry point is executed successfully.
 
 **/
@@ -1589,8 +1664,8 @@ InitializeMpServicesProtocol (
   // Locates Framework version MP Services Protocol
   //
   Status = gBS->LocateProtocol (
-                  &gFrameworkEfiMpServiceProtocolGuid, 
-                  NULL, 
+                  &gFrameworkEfiMpServiceProtocolGuid,
+                  NULL,
                   (VOID **) &mFrameworkMpService
                   );
   ASSERT_EFI_ERROR (Status);
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h
index c4e4b2b127..414cc96cf1 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h
@@ -1,7 +1,7 @@
 /** @file
 Include file for PI MP Services Protocol Thunk.
 
-Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2009 - 2010, 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
@@ -46,8 +46,11 @@ Module Name:
 //
 // Local APIC register definition for IPI.
 //
+#define APIC_REGISTER_SPURIOUS_VECTOR_OFFSET  0xF0
 #define APIC_REGISTER_ICR_LOW_OFFSET          0x300
 #define APIC_REGISTER_ICR_HIGH_OFFSET         0x310
+#define APIC_REGISTER_LINT0_VECTOR_OFFSET     0x350
+#define APIC_REGISTER_LINT1_VECTOR_OFFSET     0x360
 
 typedef struct {
   UINTN             Lock;
@@ -55,6 +58,7 @@ typedef struct {
   UINTN             StackSize;
   VOID              *ApFunction;
   IA32_DESCRIPTOR   GdtrProfile;
+  IA32_DESCRIPTOR   IdtrProfile;
   UINT32            BufferStart;
   UINT32            Cr3;
 } MP_CPU_EXCHANGE_INFO;
@@ -159,7 +163,7 @@ GetNumberOfProcessors (
   @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. 
+  @retval EFI_NOT_FOUND         Processor with the handle specified by ProcessorNumber does not exist.
 
 **/
 EFI_STATUS
@@ -188,7 +192,7 @@ GetProcessorInfo (
   @param  FailedCpuList         The list of processor numbers that fail to finish the function before
                                 TimeoutInMicrosecsond expires.
 
-  @retval EFI_SUCCESS           In blocking mode, all APs have finished before the timeout expired. 
+  @retval EFI_SUCCESS           In blocking mode, all APs have finished before the timeout expired.
   @retval EFI_SUCCESS           In non-blocking mode, function has been dispatched to all enabled APs.
   @retval EFI_DEVICE_ERROR      Caller processor is AP.
   @retval EFI_NOT_STARTED       No enabled AP exists in the system.
@@ -289,7 +293,7 @@ SwitchBSP (
   @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
@@ -445,7 +449,7 @@ ApProcWrapper (
 
 /**
   Function to wake up a specified AP and assign procedure to it.
-  
+
   @param  ProcessorNumber  Handle number of the specified processor.
   @param  Procedure        Procedure to assign.
   @param  ProcArguments    Argument for Procedure.
@@ -460,7 +464,7 @@ WakeUpAp (
 
 /**
   Terminate AP's task and set it to idle state.
-  
+
   This function terminates AP's task due to timeout by sending INIT-SIPI,
   and sends it to idle state.
 
@@ -491,7 +495,7 @@ ChangeCpuState (
 
 /**
   Gets the processor number of BSP.
-  
+
   @return  The processor number of BSP.
 
 **/
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/AsmInclude.inc b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/AsmInclude.inc
index 073ce1c613..d0269760a6 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/AsmInclude.inc
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/AsmInclude.inc
@@ -1,7 +1,7 @@
 ;------------------------------------------------------------------------------
 ; Include file for X64 MpFuncs.asm
 ;
-; Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2009 - 2010, 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
@@ -20,7 +20,8 @@ StackStartAddressLocation     equ        LockLocation + 08h
 StackSizeLocation             equ        LockLocation + 10h
 CProcedureLocation            equ        LockLocation + 18h
 GdtrLocation                  equ        LockLocation + 20h
-BufferStartLocation           equ        LockLocation + 2Ch
-Cr3OffsetLocation             equ        LockLocation + 30h
+IdtrLocation                  equ        LockLocation + 2Ah
+BufferStartLocation           equ        LockLocation + 34h
+Cr3OffsetLocation             equ        LockLocation + 38h
 
 ;-------------------------------------------------------------------------------
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S
index dc21c144a8..2c9feb07b0 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S
@@ -1,7 +1,7 @@
 #------------------------------------------------------------------------------
 # X64 assembly file for AP startup vector.
 #
-# Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2009 - 2010, 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
@@ -66,6 +66,11 @@ RendezvousFunnelProcStart:
         .byte 0x66                    # db         66h
         .byte 0x2E,0xF,0x1,0x14       # lgdt       fword ptr cs:[si]
 
+        .byte 0xBE
+        .word IdtrLocation
+        .byte 0x66                    # db         66h
+        .byte 0x2E,0xF,0x1,0x1C       # lidt       fword ptr cs:[si]
+
         .byte 0x33,0xC0               # xor        ax,  ax
         .byte 0x8E,0xD8               # mov        ds,  ax
 
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.asm b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.asm
index ad74c2dace..91bdb22451 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.asm
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.asm
@@ -1,7 +1,7 @@
 ;------------------------------------------------------------------------------
 ; X64 assembly file for AP startup vector.
 ;
-; Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2009 - 2010, 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
@@ -14,7 +14,7 @@
 
 .code
 
-include  AsmInclude.inc 
+include  AsmInclude.inc
 ;-------------------------------------------------------------------------------------
 
 ;-------------------------------------------------------------------------------------
@@ -34,7 +34,7 @@ RendezvousFunnelProcStart::
         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 8eh,  0d0h                 ; mov        ss,  ax
         db 33h,  0c0h                 ; xor        ax,  ax
         db 8eh,  0e0h                 ; mov        fs,  ax
         db 8eh,  0e8h                 ; mov        gs,  ax
@@ -48,15 +48,20 @@ RendezvousFunnelProcStart::
         db 0BEh
         dw Cr3OffsetLocation          ; mov        si, Cr3Location
         db 66h,  8Bh, 0Ch             ; mov        ecx,dword ptr [si]          ; ECX is keeping the value of CR3
-        
+
         db 0BEh
         dw GdtrLocation               ; mov        si, GdtrProfile
         db 66h                        ; db         66h
         db 2Eh,  0Fh, 01h, 14h        ; lgdt       fword ptr cs:[si]
 
+        db 0BEh
+        dw IdtrLocation               ; 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
@@ -81,25 +86,25 @@ ProtectedModeStart::
         db 0Fh,  22h,  0E0h           ; mov        cr4, eax
 
         db 0Fh,  22h,  0D9h           ; mov        cr3, ecx
-        
+
         db 8Bh,  0F2h                 ; mov        esi, edx                    ; Save wakeup buffer address
-        
+
         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
         dd 0h                         ; 32-bit offset
         dw 38h                        ; 16-bit selector
-        
+
 LongModeStart::
 
         mov         ax,  30h
@@ -146,13 +151,13 @@ Releaselock::
         sub         rsp, 20h
         call        rax
         add         rsp, 20h
-        
+
 GoToSleep::
 
         cli
         hlt
         jmp         $-2
-        
+
 RendezvousFunnelProc   ENDP
 RendezvousFunnelProcEnd::
 
@@ -160,7 +165,7 @@ RendezvousFunnelProcEnd::
 ;-------------------------------------------------------------------------------------
 ;  AsmGetAddressMap (&AddressMap);
 ;-------------------------------------------------------------------------------------
-AsmGetAddressMap   PROC  
+AsmGetAddressMap   PROC
 
         mov         rax, offset RendezvousFunnelProcStart
         mov         qword ptr [rcx], rax
@@ -169,9 +174,9 @@ AsmGetAddressMap   PROC
         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
 
 END
-- 
2.39.2