ReleaseSpinLock (&CpuData->ApLock);\r
}\r
\r
+/**\r
+ Save BSP's local APIC timer setting.\r
+\r
+ @param[in] CpuMpData Pointer to CPU MP Data\r
+**/\r
+VOID\r
+SaveLocalApicTimerSetting (\r
+ IN CPU_MP_DATA *CpuMpData\r
+ )\r
+{\r
+ //\r
+ // Record the current local APIC timer setting of BSP\r
+ //\r
+ GetApicTimerState (\r
+ &CpuMpData->DivideValue,\r
+ &CpuMpData->PeriodicMode,\r
+ &CpuMpData->Vector\r
+ );\r
+ CpuMpData->CurrentTimerCount = GetApicTimerCurrentCount ();\r
+ CpuMpData->TimerInterruptState = GetApicTimerInterruptState ();\r
+}\r
+\r
+/**\r
+ Sync local APIC timer setting from BSP to AP.\r
+\r
+ @param[in] CpuMpData Pointer to CPU MP Data\r
+**/\r
+VOID\r
+SyncLocalApicTimerSetting (\r
+ IN CPU_MP_DATA *CpuMpData\r
+ )\r
+{\r
+ //\r
+ // Sync local APIC timer setting from BSP to AP\r
+ //\r
+ InitializeApicTimer (\r
+ CpuMpData->DivideValue,\r
+ CpuMpData->CurrentTimerCount,\r
+ CpuMpData->PeriodicMode,\r
+ CpuMpData->Vector\r
+ );\r
+ //\r
+ // Disable AP's local APIC timer interrupt\r
+ //\r
+ DisableApicTimerInterrupt ();\r
+}\r
+\r
/**\r
Save the volatile registers required to be restored following INIT IPI.\r
\r
/**\r
Find the current Processor number by APIC ID.\r
\r
- @param[in] CpuMpData Pointer to PEI CPU MP Data\r
- @param[in] ProcessorNumber Return the pocessor number found\r
+ @param[in] CpuMpData Pointer to PEI CPU MP Data\r
+ @param[out] ProcessorNumber Return the pocessor number found\r
\r
@retval EFI_SUCCESS ProcessorNumber is found and returned.\r
@retval EFI_NOT_FOUND ProcessorNumber is not found.\r
return CpuMpData->CpuCount;\r
}\r
\r
-/*\r
+/**\r
Initialize CPU AP Data when AP is wakeup at the first time.\r
\r
@param[in, out] CpuMpData Pointer to PEI CPU MP Data\r
@param[in] ProcessorNumber The handle number of processor\r
@param[in] BistData Processor BIST data\r
+ @param[in] ApTopOfStack Top of AP stack\r
\r
**/\r
VOID\r
InitializeApData (\r
IN OUT CPU_MP_DATA *CpuMpData,\r
IN UINTN ProcessorNumber,\r
- IN UINT32 BistData\r
+ IN UINT32 BistData,\r
+ IN UINT64 ApTopOfStack\r
)\r
{\r
CPU_INFO_IN_HOB *CpuInfoInHob;\r
CpuInfoInHob[ProcessorNumber].InitialApicId = GetInitialApicId ();\r
CpuInfoInHob[ProcessorNumber].ApicId = GetApicId ();\r
CpuInfoInHob[ProcessorNumber].Health = BistData;\r
+ CpuInfoInHob[ProcessorNumber].ApTopOfStack = ApTopOfStack;\r
\r
CpuMpData->CpuData[ProcessorNumber].Waiting = FALSE;\r
CpuMpData->CpuData[ProcessorNumber].CpuHealthy = (BistData == 0) ? TRUE : FALSE;\r
UINT32 BistData;\r
volatile UINT32 *ApStartupSignalBuffer;\r
CPU_INFO_IN_HOB *CpuInfoInHob;\r
+ UINT64 ApTopOfStack;\r
\r
//\r
// AP finished assembly code and begin to execute C code\r
//\r
CpuMpData = ExchangeInfo->CpuMpData;\r
\r
- ProgramVirtualWireMode (); \r
+ //\r
+ // AP's local APIC settings will be lost after received INIT IPI\r
+ // We need to re-initialize them at here\r
+ //\r
+ ProgramVirtualWireMode ();\r
+ SyncLocalApicTimerSetting (CpuMpData);\r
\r
while (TRUE) {\r
if (CpuMpData->InitFlag == ApInitConfig) {\r
//\r
// This is first time AP wakeup, get BIST information from AP stack\r
//\r
- BistData = *(UINT32 *) (CpuMpData->Buffer + ProcessorNumber * CpuMpData->CpuApStackSize - sizeof (UINTN));\r
+ ApTopOfStack = CpuMpData->Buffer + (ProcessorNumber + 1) * CpuMpData->CpuApStackSize;\r
+ BistData = *(UINT32 *) ((UINTN) ApTopOfStack - sizeof (UINTN));\r
//\r
// Do some AP initialize sync\r
//\r
// Sync BSP's Control registers to APs\r
//\r
RestoreVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters, FALSE);\r
- InitializeApData (CpuMpData, ProcessorNumber, BistData);\r
+ InitializeApData (CpuMpData, ProcessorNumber, BistData, ApTopOfStack);\r
ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;\r
} else {\r
//\r
if (Procedure != NULL) {\r
SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateBusy);\r
//\r
+ // Enable source debugging on AP function\r
+ // \r
+ EnableDebugAgent ();\r
+ //\r
// Invoke AP function here\r
//\r
Procedure (Parameter);\r
GetProcessorNumber (CpuMpData, &ProcessorNumber);\r
CpuMpData->CpuData[ProcessorNumber].ApFunction = 0;\r
CpuMpData->CpuData[ProcessorNumber].ApFunctionArgument = 0;\r
+ ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;\r
+ CpuInfoInHob[ProcessorNumber].ApTopOfStack = CpuInfoInHob[CpuMpData->NewBspNumber].ApTopOfStack;\r
} else {\r
//\r
// Re-get the CPU APICID and Initial APICID\r
\r
ExchangeInfo->CFunction = (UINTN) ApWakeupFunction;\r
ExchangeInfo->NumApsExecuting = 0;\r
+ ExchangeInfo->InitFlag = (UINTN) CpuMpData->InitFlag;\r
+ ExchangeInfo->CpuInfo = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
ExchangeInfo->CpuMpData = CpuMpData;\r
\r
ExchangeInfo->EnableExecuteDisable = IsBspExecuteDisableEnabled ();\r
AsmReadIdtr ((IA32_DESCRIPTOR *) &ExchangeInfo->IdtrProfile);\r
}\r
\r
+/**\r
+ Helper function that waits until the finished AP count reaches the specified\r
+ limit, or the specified timeout elapses (whichever comes first).\r
+\r
+ @param[in] CpuMpData Pointer to CPU MP Data.\r
+ @param[in] FinishedApLimit The number of finished APs to wait for.\r
+ @param[in] TimeLimit The number of microseconds to wait for.\r
+**/\r
+VOID\r
+TimedWaitForApFinish (\r
+ IN CPU_MP_DATA *CpuMpData,\r
+ IN UINT32 FinishedApLimit,\r
+ IN UINT32 TimeLimit\r
+ );\r
+\r
/**\r
This function will be called by BSP to wakeup AP.\r
\r
ResetVectorRequired = TRUE;\r
AllocateResetVector (CpuMpData);\r
FillExchangeInfoData (CpuMpData);\r
+ SaveLocalApicTimerSetting (CpuMpData);\r
} else if (CpuMpData->ApLoopMode == ApInMwaitLoop) {\r
//\r
// Get AP target C-state each time when waking up AP,\r
//\r
// Wait for all potential APs waken up in one specified period\r
//\r
- MicroSecondDelay (PcdGet32(PcdCpuApInitTimeOutInMicroSeconds));\r
+ TimedWaitForApFinish (\r
+ CpuMpData,\r
+ PcdGet32 (PcdCpuMaxLogicalProcessorNumber) - 1,\r
+ PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds)\r
+ );\r
} else {\r
//\r
// Wait all APs waken up if this is not the 1st broadcast of SIPI\r
return FALSE;\r
}\r
\r
+/**\r
+ Helper function that waits until the finished AP count reaches the specified\r
+ limit, or the specified timeout elapses (whichever comes first).\r
+\r
+ @param[in] CpuMpData Pointer to CPU MP Data.\r
+ @param[in] FinishedApLimit The number of finished APs to wait for.\r
+ @param[in] TimeLimit The number of microseconds to wait for.\r
+**/\r
+VOID\r
+TimedWaitForApFinish (\r
+ IN CPU_MP_DATA *CpuMpData,\r
+ IN UINT32 FinishedApLimit,\r
+ IN UINT32 TimeLimit\r
+ )\r
+{\r
+ //\r
+ // CalculateTimeout() and CheckTimeout() consider a TimeLimit of 0\r
+ // "infinity", so check for (TimeLimit == 0) explicitly.\r
+ //\r
+ if (TimeLimit == 0) {\r
+ return;\r
+ }\r
+\r
+ CpuMpData->TotalTime = 0;\r
+ CpuMpData->ExpectedTime = CalculateTimeout (\r
+ TimeLimit,\r
+ &CpuMpData->CurrentTime\r
+ );\r
+ while (CpuMpData->FinishedCount < FinishedApLimit &&\r
+ !CheckTimeout (\r
+ &CpuMpData->CurrentTime,\r
+ &CpuMpData->TotalTime,\r
+ CpuMpData->ExpectedTime\r
+ )) {\r
+ CpuPause ();\r
+ }\r
+\r
+ if (CpuMpData->FinishedCount >= FinishedApLimit) {\r
+ DEBUG ((\r
+ DEBUG_VERBOSE,\r
+ "%a: reached FinishedApLimit=%u in %Lu microseconds\n",\r
+ __FUNCTION__,\r
+ FinishedApLimit,\r
+ DivU64x64Remainder (\r
+ MultU64x32 (CpuMpData->TotalTime, 1000000),\r
+ GetPerformanceCounterProperties (NULL, NULL),\r
+ NULL\r
+ )\r
+ ));\r
+ }\r
+}\r
+\r
/**\r
Reset an AP to Idle state.\r
\r
//\r
// Set BSP basic information\r
//\r
- InitializeApData (CpuMpData, 0, 0);\r
+ InitializeApData (CpuMpData, 0, 0, CpuMpData->Buffer);\r
//\r
// Save assembly code information\r
//\r
UINTN CallerNumber;\r
CPU_STATE State;\r
MSR_IA32_APIC_BASE_REGISTER ApicBaseMsr;\r
+ BOOLEAN OldInterruptState;\r
+ BOOLEAN OldTimerInterruptState;\r
+\r
+ //\r
+ // Save and Disable Local APIC timer interrupt\r
+ //\r
+ OldTimerInterruptState = GetApicTimerInterruptState ();\r
+ DisableApicTimerInterrupt ();\r
+ //\r
+ // Before send both BSP and AP to a procedure to exchange their roles,\r
+ // interrupt must be disabled. This is because during the exchange role\r
+ // process, 2 CPU may use 1 stack. If interrupt happens, the stack will\r
+ // be corrupted, since interrupt return address will be pushed to stack\r
+ // by hardware.\r
+ //\r
+ OldInterruptState = SaveAndDisableInterrupts ();\r
+\r
+ //\r
+ // Mask LINT0 & LINT1 for the old BSP\r
+ //\r
+ DisableLvtInterrupts ();\r
\r
CpuMpData = GetCpuMpData ();\r
\r
CpuMpData->BSPInfo.State = CPU_SWITCH_STATE_IDLE;\r
CpuMpData->APInfo.State = CPU_SWITCH_STATE_IDLE;\r
CpuMpData->SwitchBspFlag = TRUE;\r
+ CpuMpData->NewBspNumber = ProcessorNumber;\r
\r
//\r
// Clear the BSP bit of MSR_IA32_APIC_BASE\r
//\r
if (!EnableOldBSP) {\r
SetApState (&CpuMpData->CpuData[CallerNumber], CpuStateDisabled);\r
+ } else {\r
+ SetApState (&CpuMpData->CpuData[CallerNumber], CpuStateIdle);\r
}\r
//\r
// Save new BSP number\r
//\r
CpuMpData->BspNumber = (UINT32) ProcessorNumber;\r
\r
+ //\r
+ // Restore interrupt state.\r
+ //\r
+ SetInterruptState (OldInterruptState);\r
+\r
+ if (OldTimerInterruptState) {\r
+ EnableApicTimerInterrupt ();\r
+ }\r
+\r
return EFI_SUCCESS;\r
}\r
\r
simultaneously.\r
@param[in] WaitEvent The event created by the caller with CreateEvent()\r
service.\r
- @param[in] TimeoutInMicrosecsond Indicates the time limit in microseconds for\r
+ @param[in] TimeoutInMicroseconds Indicates the time limit in microseconds for\r
APs to return from Procedure, either for\r
blocking or non-blocking mode.\r
@param[in] ProcedureArgument The parameter passed into Procedure for\r
@param[in] ProcessorNumber The handle number of the AP.\r
@param[in] WaitEvent The event created by the caller with CreateEvent()\r
service.\r
- @param[in] TimeoutInMicrosecsond Indicates the time limit in microseconds for\r
+ @param[in] TimeoutInMicroseconds Indicates the time limit in microseconds for\r
APs to return from Procedure, either for\r
blocking or non-blocking mode.\r
@param[in] ProcedureArgument The parameter passed into Procedure for\r