diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
index 3ff503758d..f3db9f6b7c 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
@@ -532,6 +532,12 @@ SwitchBSP (
   CPU_DATA_BLOCK        *CpuData;
   UINTN                 CallerNumber;
   UINTN                 BspNumber;
+  UINTN                 ApicBase;
+  UINT32                CurrentTimerValue;
+  UINT32                CurrentTimerRegister;
+  UINT32                CurrentTimerDivide;
+  UINT64                CurrentTscValue;
+  BOOLEAN               OldInterruptState;
 
   //
   // Check whether caller processor is BSP
@@ -572,6 +578,28 @@ SwitchBSP (
     return EFI_NOT_READY;
   }
 
+  //
+  // Save and disable interrupt.
+  //
+  OldInterruptState = SaveAndDisableInterrupts ();
+	
+  //
+  // Record the current local APIC timer setting of BSP
+  //
+  ApicBase = (UINTN)AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE, 12, 35) << 12;
+  CurrentTimerValue     = MmioRead32 (ApicBase + APIC_REGISTER_TIMER_COUNT);
+  CurrentTimerRegister  = MmioRead32 (ApicBase + APIC_REGISTER_LVT_TIMER);
+  CurrentTimerDivide    = MmioRead32 (ApicBase + APIC_REGISTER_TIMER_DIVIDE);
+  //
+  // Set mask bit (BIT 16) of LVT Timer Register to disable its interrupt
+  //
+  MmioBitFieldWrite32 (ApicBase + APIC_REGISTER_LVT_TIMER, 16, 16, 1);
+
+  //
+  // Record the current TSC value
+  //
+  CurrentTscValue = AsmReadTsc ();
+  
   Status = mFrameworkMpService->SwitchBSP (
                                   mFrameworkMpService,
                                   ProcessorNumber,
@@ -579,6 +607,23 @@ SwitchBSP (
                                   );
   ASSERT_EFI_ERROR (Status);
 
+  //
+  // Restore TSC value
+  //
+  AsmWriteMsr64 (MSR_IA32_TIME_STAMP_COUNTER, CurrentTscValue);
+
+  //
+  // Restore local APIC timer setting to new BSP
+  //
+  MmioWrite32 (ApicBase + APIC_REGISTER_TIMER_DIVIDE, CurrentTimerDivide);
+  MmioWrite32 (ApicBase + APIC_REGISTER_TIMER_INIT_COUNT, CurrentTimerValue);
+  MmioWrite32 (ApicBase + APIC_REGISTER_LVT_TIMER, CurrentTimerRegister);
+
+  //
+  // Restore interrupt state.
+  //
+  SetInterruptState (OldInterruptState);
+  
   ChangeCpuState (BspNumber, EnableOldBSP);
 
   return EFI_SUCCESS;
@@ -1091,7 +1136,7 @@ ProgramVirtualWireMode (
   UINTN                 ApicBase;
   UINT32                Value;
 
-  ApicBase = (UINTN)AsmMsrBitFieldRead64 (27, 12, 35) << 12;
+  ApicBase = (UINTN)AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE, 12, 35) << 12;
 
   //
   // Program the Spurious Vector entry
@@ -1157,8 +1202,16 @@ ApProcWrapper (
   UINTN                 ProcessorNumber;
   CPU_DATA_BLOCK        *CpuData;
 
+  //
+  // Program virtual wire mode for AP, since it will be lost after AP wake up
+  //
   ProgramVirtualWireMode (FALSE);
 
+  //
+  // Initialize Debug Agent to support source level debug on AP code.
+  //
+  InitializeDebugAgent (DEBUG_AGENT_INIT_DXE_AP, NULL, NULL);
+
   WhoAmI (&mMpService, &ProcessorNumber);
   CpuData = &mMPSystemData.CpuData[ProcessorNumber];
 
@@ -1235,7 +1288,7 @@ SendInitSipiSipi (
   DeliveryMode = DELIVERY_MODE_INIT;
   ICRLow      |= VectorNumber | (DeliveryMode << 8);
 
-  ApicBase = 0xfee00000;
+  ApicBase = (UINTN)AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE, 12, 35) << 12;;
 
   //
   // Write Interrupt Command Registers to send INIT IPI.
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h
index 414cc96cf1..8c98300a75 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.h
@@ -31,6 +31,7 @@ Module Name:
 #include <Library/DxeServicesTableLib.h>
 #include <Library/IoLib.h>
 #include <Library/TimerLib.h>
+#include <Library/DebugAgentLib.h>
 
 #define AP_STACK_SIZE                         0x8000
 #define MAX_CPU_NUMBER                        256
@@ -49,8 +50,18 @@ Module Name:
 #define APIC_REGISTER_SPURIOUS_VECTOR_OFFSET  0xF0
 #define APIC_REGISTER_ICR_LOW_OFFSET          0x300
 #define APIC_REGISTER_ICR_HIGH_OFFSET         0x310
+#define APIC_REGISTER_LVT_TIMER               0x320
+#define APIC_REGISTER_TIMER_INIT_COUNT        0x380
 #define APIC_REGISTER_LINT0_VECTOR_OFFSET     0x350
 #define APIC_REGISTER_LINT1_VECTOR_OFFSET     0x360
+#define APIC_REGISTER_TIMER_COUNT             0x390
+#define APIC_REGISTER_TIMER_DIVIDE            0x3E0
+
+//
+// Definition for MSR address
+//
+#define MSR_IA32_TIME_STAMP_COUNTER           0x10
+#define MSR_IA32_APIC_BASE                    0x1B
 
 typedef struct {
   UINTN             Lock;
diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.inf b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.inf
index faa6fe7f33..f583621c89 100644
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.inf
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.inf
@@ -58,6 +58,7 @@
   DebugLib
   BaseLib
   SynchronizationLib
+  DebugAgentLib
 
 [Protocols]
   gEfiMpServiceProtocolGuid                   ## PRODUCES
diff --git a/EdkCompatibilityPkg/EdkCompatibilityPkg.dsc b/EdkCompatibilityPkg/EdkCompatibilityPkg.dsc
index f068419820..54b55c44e7 100644
--- a/EdkCompatibilityPkg/EdkCompatibilityPkg.dsc
+++ b/EdkCompatibilityPkg/EdkCompatibilityPkg.dsc
@@ -67,6 +67,7 @@ define GCC_MACRO                 = -DEFI_SPECIFICATION_VERSION=0x00020000 -DPI_S
   PeCoffExtraActionLib|MdePkg/Library/BasePeCoffExtraActionLibNull/BasePeCoffExtraActionLibNull.inf
   CacheMaintenanceLib|MdePkg/Library/BaseCacheMaintenanceLib/BaseCacheMaintenanceLib.inf
   CpuLib|MdePkg/Library/BaseCpuLib/BaseCpuLib.inf
+  DebugAgentLib|MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.inf
 
 [LibraryClasses.common.PEIM]
   MemoryAllocationLib|MdePkg/Library/PeiMemoryAllocationLib/PeiMemoryAllocationLib.inf