UefiCpuPkg/CpuMpPei: Add missing point at function header

[mirror_edk2.git] / UefiCpuPkg / CpuMpPei / CpuMpPei.c

diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.c b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
index ae071ac1f81abaab4d8d8c9342821412df4aeb9d..2e6e7611a2618ea1492d6debceb2f756bc2113bb 100644 (file)
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.c
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
@@ -141,11 +141,78 @@ GetMpHobData (
   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
@@ -161,13 +228,18 @@ ApCFunction (
 \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
@@ -181,15 +253,24 @@ ApCFunction (
     //\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
@@ -200,6 +281,11 @@ ApCFunction (
   //\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
@@ -491,6 +577,7 @@ PrepareAPStartupVector (
   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
@@ -528,7 +615,7 @@ CpuMpEndOfPeiCallback (
   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