diff options
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
|