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

2. Copy LVT Timer setting from old BSP to new BSP when SwitchBsp() to make sure debug timer worked after bsp switched.
3. Save and restore old BSP's TSC value to new one.



git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@10698 6f19259b-4bc3-4df7-8a09-765794883524

4 files changed, 68 insertions, 2 deletions

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