1. Consume Debug Agent Library to support debugging AP code based on PI MP protocol.

[mirror_edk2.git] / EdkCompatibilityPkg / Compatibility / MpServicesOnFrameworkMpServicesThunk / MpServicesOnFrameworkMpServicesThunk.c

diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
index 3ff503758d84916819d7a347ea6ed4dfd1778e59..f3db9f6b7c01b11037047a2e24d27507199bbed4 100644 (file)
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/MpServicesOnFrameworkMpServicesThunk.c
@@ -532,6 +532,12 @@ SwitchBSP (
   CPU_DATA_BLOCK        *CpuData;\r
   UINTN                 CallerNumber;\r
   UINTN                 BspNumber;\r
+  UINTN                 ApicBase;\r
+  UINT32                CurrentTimerValue;\r
+  UINT32                CurrentTimerRegister;\r
+  UINT32                CurrentTimerDivide;\r
+  UINT64                CurrentTscValue;\r
+  BOOLEAN               OldInterruptState;\r
 \r
   //\r
   // Check whether caller processor is BSP\r
@@ -572,6 +578,28 @@ SwitchBSP (
     return EFI_NOT_READY;\r
   }\r
 \r
+  //\r
+  // Save and disable interrupt.\r
+  //\r
+  OldInterruptState = SaveAndDisableInterrupts ();\r
+       \r
+  //\r
+  // Record the current local APIC timer setting of BSP\r
+  //\r
+  ApicBase = (UINTN)AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE, 12, 35) << 12;\r
+  CurrentTimerValue     = MmioRead32 (ApicBase + APIC_REGISTER_TIMER_COUNT);\r
+  CurrentTimerRegister  = MmioRead32 (ApicBase + APIC_REGISTER_LVT_TIMER);\r
+  CurrentTimerDivide    = MmioRead32 (ApicBase + APIC_REGISTER_TIMER_DIVIDE);\r
+  //\r
+  // Set mask bit (BIT 16) of LVT Timer Register to disable its interrupt\r
+  //\r
+  MmioBitFieldWrite32 (ApicBase + APIC_REGISTER_LVT_TIMER, 16, 16, 1);\r
+\r
+  //\r
+  // Record the current TSC value\r
+  //\r
+  CurrentTscValue = AsmReadTsc ();\r
+  \r
   Status = mFrameworkMpService->SwitchBSP (\r
                                   mFrameworkMpService,\r
                                   ProcessorNumber,\r
@@ -579,6 +607,23 @@ SwitchBSP (
                                   );\r
   ASSERT_EFI_ERROR (Status);\r
 \r
+  //\r
+  // Restore TSC value\r
+  //\r
+  AsmWriteMsr64 (MSR_IA32_TIME_STAMP_COUNTER, CurrentTscValue);\r
+\r
+  //\r
+  // Restore local APIC timer setting to new BSP\r
+  //\r
+  MmioWrite32 (ApicBase + APIC_REGISTER_TIMER_DIVIDE, CurrentTimerDivide);\r
+  MmioWrite32 (ApicBase + APIC_REGISTER_TIMER_INIT_COUNT, CurrentTimerValue);\r
+  MmioWrite32 (ApicBase + APIC_REGISTER_LVT_TIMER, CurrentTimerRegister);\r
+\r
+  //\r
+  // Restore interrupt state.\r
+  //\r
+  SetInterruptState (OldInterruptState);\r
+  \r
   ChangeCpuState (BspNumber, EnableOldBSP);\r
 \r
   return EFI_SUCCESS;\r
@@ -1091,7 +1136,7 @@ ProgramVirtualWireMode (
   UINTN                 ApicBase;\r
   UINT32                Value;\r
 \r
-  ApicBase = (UINTN)AsmMsrBitFieldRead64 (27, 12, 35) << 12;\r
+  ApicBase = (UINTN)AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE, 12, 35) << 12;\r
 \r
   //\r
   // Program the Spurious Vector entry\r
@@ -1157,8 +1202,16 @@ ApProcWrapper (
   UINTN                 ProcessorNumber;\r
   CPU_DATA_BLOCK        *CpuData;\r
 \r
+  //\r
+  // Program virtual wire mode for AP, since it will be lost after AP wake up\r
+  //\r
   ProgramVirtualWireMode (FALSE);\r
 \r
+  //\r
+  // Initialize Debug Agent to support source level debug on AP code.\r
+  //\r
+  InitializeDebugAgent (DEBUG_AGENT_INIT_DXE_AP, NULL, NULL);\r
+\r
   WhoAmI (&mMpService, &ProcessorNumber);\r
   CpuData = &mMPSystemData.CpuData[ProcessorNumber];\r
 \r
@@ -1235,7 +1288,7 @@ SendInitSipiSipi (
   DeliveryMode = DELIVERY_MODE_INIT;\r
   ICRLow      |= VectorNumber | (DeliveryMode << 8);\r
 \r
-  ApicBase = 0xfee00000;\r
+  ApicBase = (UINTN)AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE, 12, 35) << 12;;\r
 \r
   //\r
   // Write Interrupt Command Registers to send INIT IPI.\r