UINTN Index2;\r
UINTN Index3;\r
UINT32 ApicId;\r
- EFI_HEALTH_FLAGS Health;\r
+ PEI_CPU_DATA CpuData;\r
UINT32 ApCount;\r
\r
ApCount = PeiCpuMpData->CpuCount - 1;\r
}\r
}\r
if (Index3 != Index1) {\r
- PeiCpuMpData->CpuData[Index3].ApicId = PeiCpuMpData->CpuData[Index1].ApicId;\r
- PeiCpuMpData->CpuData[Index1].ApicId = ApicId;\r
- Health = PeiCpuMpData->CpuData[Index3].Health;\r
- PeiCpuMpData->CpuData[Index3].Health = PeiCpuMpData->CpuData[Index1].Health;\r
- PeiCpuMpData->CpuData[Index1].Health = Health;\r
+ CopyMem (&CpuData, &PeiCpuMpData->CpuData[Index3], sizeof (PEI_CPU_DATA));\r
+ CopyMem (\r
+ &PeiCpuMpData->CpuData[Index3],\r
+ &PeiCpuMpData->CpuData[Index1],\r
+ sizeof (PEI_CPU_DATA)\r
+ );\r
+ CopyMem (&PeiCpuMpData->CpuData[Index1], &CpuData, sizeof (PEI_CPU_DATA));\r
}\r
}\r
\r
return CpuMpData;\r
}\r
\r
+/**\r
+ Save the volatile registers required to be restored following INIT IPI.\r
+ \r
+ @param VolatileRegisters Returns buffer saved the volatile resisters\r
+**/\r
+VOID\r
+SaveVolatileRegisters (\r
+ OUT CPU_VOLATILE_REGISTERS *VolatileRegisters\r
+ )\r
+{\r
+ UINT32 RegEdx;\r
+\r
+ VolatileRegisters->Cr0 = AsmReadCr0 ();\r
+ VolatileRegisters->Cr3 = AsmReadCr3 ();\r
+ VolatileRegisters->Cr4 = AsmReadCr4 ();\r
+\r
+ AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);\r
+ if ((RegEdx & BIT2) != 0) {\r
+ //\r
+ // If processor supports Debugging Extensions feature\r
+ // by CPUID.[EAX=01H]:EDX.BIT2\r
+ //\r
+ VolatileRegisters->Dr0 = AsmReadDr0 ();\r
+ VolatileRegisters->Dr1 = AsmReadDr1 ();\r
+ VolatileRegisters->Dr2 = AsmReadDr2 ();\r
+ VolatileRegisters->Dr3 = AsmReadDr3 ();\r
+ VolatileRegisters->Dr6 = AsmReadDr6 ();\r
+ VolatileRegisters->Dr7 = AsmReadDr7 ();\r
+ }\r
+}\r
+\r
+/**\r
+ Restore the volatile registers following INIT IPI.\r
+ \r
+ @param VolatileRegisters Pointer to volatile resisters\r
+ @param IsRestoreDr TRUE: Restore DRx if supported\r
+ FALSE: Do not restore DRx\r
+**/\r
+VOID\r
+RestoreVolatileRegisters (\r
+ IN CPU_VOLATILE_REGISTERS *VolatileRegisters,\r
+ IN BOOLEAN IsRestoreDr\r
+ )\r
+{\r
+ UINT32 RegEdx;\r
+\r
+ AsmWriteCr0 (VolatileRegisters->Cr0);\r
+ AsmWriteCr3 (VolatileRegisters->Cr3);\r
+ AsmWriteCr4 (VolatileRegisters->Cr4);\r
+\r
+ if (IsRestoreDr) {\r
+ AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);\r
+ if ((RegEdx & BIT2) != 0) {\r
+ //\r
+ // If processor supports Debugging Extensions feature\r
+ // by CPUID.[EAX=01H]:EDX.BIT2\r
+ //\r
+ AsmWriteDr0 (VolatileRegisters->Dr0);\r
+ AsmWriteDr1 (VolatileRegisters->Dr1);\r
+ AsmWriteDr2 (VolatileRegisters->Dr2);\r
+ AsmWriteDr3 (VolatileRegisters->Dr3);\r
+ AsmWriteDr6 (VolatileRegisters->Dr6);\r
+ AsmWriteDr7 (VolatileRegisters->Dr7);\r
+ }\r
+ }\r
+}\r
+\r
/**\r
This function will be called from AP reset code if BSP uses WakeUpAP.\r
\r
@param ExchangeInfo Pointer to the MP exchange info buffer\r
- @param NumApsExecuting Number of curret executing AP\r
+ @param NumApsExecuting Number of current executing AP\r
**/\r
VOID\r
EFIAPI\r
\r
PeiCpuMpData = ExchangeInfo->PeiCpuMpData;\r
if (PeiCpuMpData->InitFlag) {\r
+ ProcessorNumber = NumApsExecuting;\r
+ //\r
+ // Sync BSP's Control registers to APs\r
+ //\r
+ RestoreVolatileRegisters (&PeiCpuMpData->CpuData[0].VolatileRegisters, FALSE);\r
//\r
// This is first time AP wakeup, get BIST information from AP stack\r
//\r
- BistData = *(UINTN *) (PeiCpuMpData->Buffer + NumApsExecuting * PeiCpuMpData->CpuApStackSize - sizeof (UINTN));\r
- PeiCpuMpData->CpuData[NumApsExecuting].Health.Uint32 = (UINT32) BistData;\r
- PeiCpuMpData->CpuData[NumApsExecuting].ApicId = GetInitialApicId ();\r
- if (PeiCpuMpData->CpuData[NumApsExecuting].ApicId >= 0xFF) {\r
+ BistData = *(UINTN *) (PeiCpuMpData->Buffer + ProcessorNumber * PeiCpuMpData->CpuApStackSize - sizeof (UINTN));\r
+ PeiCpuMpData->CpuData[ProcessorNumber].Health.Uint32 = (UINT32) BistData;\r
+ PeiCpuMpData->CpuData[ProcessorNumber].ApicId = GetInitialApicId ();\r
+ if (PeiCpuMpData->CpuData[ProcessorNumber].ApicId >= 0xFF) {\r
//\r
// Set x2APIC mode if there are any logical processor reporting\r
// an APIC ID of 255 or greater.\r
//\r
MtrrSetAllMtrrs (&PeiCpuMpData->MtrrTable);\r
MicrocodeDetect ();\r
+ PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;\r
} else {\r
//\r
// Execute AP function if AP is not disabled\r
//\r
GetProcessorNumber (PeiCpuMpData, &ProcessorNumber);\r
+ //\r
+ // Restore AP's volatile registers saved\r
+ //\r
+ RestoreVolatileRegisters (&PeiCpuMpData->CpuData[ProcessorNumber].VolatileRegisters, TRUE);\r
+\r
if ((PeiCpuMpData->CpuData[ProcessorNumber].State != CpuStateDisabled) &&\r
(PeiCpuMpData->ApFunction != 0)) {\r
PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateBusy;\r
Procedure = (EFI_AP_PROCEDURE)(UINTN)PeiCpuMpData->ApFunction;\r
+ //\r
+ // Invoke AP function here\r
+ //\r
Procedure ((VOID *)(UINTN)PeiCpuMpData->ApFunctionArgument);\r
PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;\r
}\r
//\r
InterlockedIncrement ((UINT32 *)&PeiCpuMpData->FinishedCount);\r
\r
+ //\r
+ // Save AP volatile registers\r
+ //\r
+ SaveVolatileRegisters (&PeiCpuMpData->CpuData[ProcessorNumber].VolatileRegisters);\r
+\r
AsmCliHltLoop ();\r
}\r
\r
PeiCpuMpData->CpuData[0].Health.Uint32 = 0;\r
PeiCpuMpData->EndOfPeiFlag = FALSE;\r
InitializeSpinLock(&PeiCpuMpData->MpLock);\r
+ SaveVolatileRegisters (&PeiCpuMpData->CpuData[0].VolatileRegisters);\r
CopyMem (&PeiCpuMpData->AddressMap, &AddressMap, sizeof (MP_ASSEMBLY_ADDRESS_MAP));\r
\r
//\r
EFI_PEI_HOB_POINTERS Hob;\r
EFI_HOB_MEMORY_ALLOCATION *MemoryHob;\r
\r
- DEBUG ((EFI_D_INFO, "CpuMpPei: CpuMpEndOfPeiCallback () invokded\n"));\r
+ DEBUG ((EFI_D_INFO, "CpuMpPei: CpuMpEndOfPeiCallback () invoked\n"));\r
\r
Status = PeiServicesGetBootMode (&BootMode);\r
ASSERT_EFI_ERROR (Status);\r