X-Git-Url: https://git.proxmox.com/?a=blobdiff_plain;f=UefiCpuPkg%2FCpuMpPei%2FCpuMpPei.c;h=950d61cc4853797dd3b6f67879481d9114d5a351;hb=a09647f3fe74ebbff2c49f47b983478f75bdf045;hp=8e35f288fe3c64fd235a84658a851671f6317029;hpb=944f45ae2f7ecbff2c66622d15d52ffbc3455bfb;p=mirror_edk2.git

diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.c b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
index 8e35f288fe..950d61cc48 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.c
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
@@ -61,7 +61,7 @@ SortApicId (
   UINTN             Index2;
   UINTN             Index3;
   UINT32            ApicId;
-  EFI_HEALTH_FLAGS  Health;
+  PEI_CPU_DATA      CpuData;
   UINT32            ApCount;
 
   ApCount = PeiCpuMpData->CpuCount - 1;
@@ -80,11 +80,13 @@ SortApicId (
         }
       }
       if (Index3 != Index1) {
-        PeiCpuMpData->CpuData[Index3].ApicId = PeiCpuMpData->CpuData[Index1].ApicId;
-        PeiCpuMpData->CpuData[Index1].ApicId = ApicId;
-        Health = PeiCpuMpData->CpuData[Index3].Health;
-        PeiCpuMpData->CpuData[Index3].Health = PeiCpuMpData->CpuData[Index1].Health;
-        PeiCpuMpData->CpuData[Index1].Health = Health;
+        CopyMem (&CpuData, &PeiCpuMpData->CpuData[Index3], sizeof (PEI_CPU_DATA));
+        CopyMem (
+          &PeiCpuMpData->CpuData[Index3],
+          &PeiCpuMpData->CpuData[Index1],
+          sizeof (PEI_CPU_DATA)
+          );
+        CopyMem (&PeiCpuMpData->CpuData[Index1], &CpuData, sizeof (PEI_CPU_DATA));
       }
     }
 
@@ -101,6 +103,68 @@ SortApicId (
   }
 }
 
+/**
+  Enable x2APIC mode on APs.
+
+  @param Buffer  Pointer to private data buffer.
+**/
+VOID
+EFIAPI
+ApFuncEnableX2Apic (
+  IN OUT VOID  *Buffer
+  )
+{
+  SetApicMode (LOCAL_APIC_MODE_X2APIC);
+}
+
+/**
+  Get AP loop mode.
+
+  @param MonitorFilterSize  Returns the largest monitor-line size in bytes.
+
+  @return The AP loop mode.
+**/
+UINT8
+GetApLoopMode (
+  OUT UINT16     *MonitorFilterSize
+  )
+{
+  UINT8          ApLoopMode;
+  UINT32         RegEbx;
+  UINT32         RegEcx;
+  UINT32         RegEdx;
+
+  ASSERT (MonitorFilterSize != NULL);
+
+  ApLoopMode = PcdGet8 (PcdCpuApLoopMode);
+  ASSERT (ApLoopMode >= ApInHltLoop && ApLoopMode <= ApInRunLoop);
+  if (ApLoopMode == ApInMwaitLoop) {
+    AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &RegEcx, NULL);
+    if ((RegEcx & BIT3) == 0) {
+      //
+      // If processor does not support MONITOR/MWAIT feature
+      // by CPUID.[EAX=01H]:ECX.BIT3, force AP in Hlt-loop mode
+      //
+      ApLoopMode = ApInHltLoop;
+    }
+  }
+
+  if (ApLoopMode == ApInHltLoop) {
+    *MonitorFilterSize = 0;
+  } else if (ApLoopMode == ApInRunLoop) {
+    *MonitorFilterSize = sizeof (UINT32);
+  } else if (ApLoopMode == ApInMwaitLoop) {
+    //
+    // CPUID.[EAX=05H]:EBX.BIT0-15: Largest monitor-line size in bytes
+    // CPUID.[EAX=05H].EDX: C-states supported using MWAIT
+    //
+    AsmCpuid (CPUID_MONITOR_MWAIT, NULL, &RegEbx, NULL, &RegEdx);
+    *MonitorFilterSize = RegEbx & 0xFFFF;
+  }
+
+  return ApLoopMode;
+}
+
 /**
   Get CPU MP Data pointer from the Guided HOB.
 
@@ -125,11 +189,78 @@ GetMpHobData (
   return CpuMpData;
 }
 
+/**
+  Save the volatile registers required to be restored following INIT IPI.
+  
+  @param  VolatileRegisters    Returns buffer saved the volatile resisters
+**/
+VOID
+SaveVolatileRegisters (
+  OUT CPU_VOLATILE_REGISTERS    *VolatileRegisters
+  )
+{
+  UINT32                        RegEdx;
+
+  VolatileRegisters->Cr0 = AsmReadCr0 ();
+  VolatileRegisters->Cr3 = AsmReadCr3 ();
+  VolatileRegisters->Cr4 = AsmReadCr4 ();
+
+  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);
+  if ((RegEdx & BIT2) != 0) {
+    //
+    // If processor supports Debugging Extensions feature
+    // by CPUID.[EAX=01H]:EDX.BIT2
+    //
+    VolatileRegisters->Dr0 = AsmReadDr0 ();
+    VolatileRegisters->Dr1 = AsmReadDr1 ();
+    VolatileRegisters->Dr2 = AsmReadDr2 ();
+    VolatileRegisters->Dr3 = AsmReadDr3 ();
+    VolatileRegisters->Dr6 = AsmReadDr6 ();
+    VolatileRegisters->Dr7 = AsmReadDr7 ();
+  }
+}
+
+/**
+  Restore the volatile registers following INIT IPI.
+  
+  @param  VolatileRegisters   Pointer to volatile resisters
+  @param  IsRestoreDr         TRUE:  Restore DRx if supported
+                              FALSE: Do not restore DRx
+**/
+VOID
+RestoreVolatileRegisters (
+  IN CPU_VOLATILE_REGISTERS    *VolatileRegisters,
+  IN BOOLEAN                   IsRestoreDr
+  )
+{
+  UINT32                        RegEdx;
+
+  AsmWriteCr0 (VolatileRegisters->Cr0);
+  AsmWriteCr3 (VolatileRegisters->Cr3);
+  AsmWriteCr4 (VolatileRegisters->Cr4);
+
+  if (IsRestoreDr) {
+    AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);
+    if ((RegEdx & BIT2) != 0) {
+      //
+      // If processor supports Debugging Extensions feature
+      // by CPUID.[EAX=01H]:EDX.BIT2
+      //
+      AsmWriteDr0 (VolatileRegisters->Dr0);
+      AsmWriteDr1 (VolatileRegisters->Dr1);
+      AsmWriteDr2 (VolatileRegisters->Dr2);
+      AsmWriteDr3 (VolatileRegisters->Dr3);
+      AsmWriteDr6 (VolatileRegisters->Dr6);
+      AsmWriteDr7 (VolatileRegisters->Dr7);
+    }
+  }
+}
+
 /**
   This function will be called from AP reset code if BSP uses WakeUpAP.
 
   @param ExchangeInfo     Pointer to the MP exchange info buffer
-  @param NumApsExecuting  Number of curret executing AP
+  @param NumApsExecuting  Number of current executing AP
 **/
 VOID
 EFIAPI
@@ -142,40 +273,124 @@ ApCFunction (
   UINTN                      ProcessorNumber;
   EFI_AP_PROCEDURE           Procedure;
   UINTN                      BistData;
+  volatile UINT32            *ApStartupSignalBuffer;
 
   PeiCpuMpData = ExchangeInfo->PeiCpuMpData;
-  if (PeiCpuMpData->InitFlag) {
+  while (TRUE) {
+    if (PeiCpuMpData->InitFlag) {
+      ProcessorNumber = NumApsExecuting;
+      //
+      // Sync BSP's Control registers to APs
+      //
+      RestoreVolatileRegisters (&PeiCpuMpData->CpuData[0].VolatileRegisters, FALSE);
+      //
+      // This is first time AP wakeup, get BIST information from AP stack
+      //
+      BistData = *(UINTN *) (PeiCpuMpData->Buffer + ProcessorNumber * PeiCpuMpData->CpuApStackSize - sizeof (UINTN));
+      PeiCpuMpData->CpuData[ProcessorNumber].Health.Uint32 = (UINT32) BistData;
+      PeiCpuMpData->CpuData[ProcessorNumber].ApicId = GetInitialApicId ();
+      if (PeiCpuMpData->CpuData[ProcessorNumber].ApicId >= 0xFF) {
+        //
+        // Set x2APIC mode if there are any logical processor reporting
+        // an APIC ID of 255 or greater.
+        //
+        AcquireSpinLock(&PeiCpuMpData->MpLock);
+        PeiCpuMpData->X2ApicEnable = TRUE;
+        ReleaseSpinLock(&PeiCpuMpData->MpLock);
+      }
+      //
+      // Sync BSP's Mtrr table to all wakeup APs and load microcode on APs.
+      //
+      MtrrSetAllMtrrs (&PeiCpuMpData->MtrrTable);
+      MicrocodeDetect ();
+      PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;
+    } else {
+      //
+      // Execute AP function if AP is not disabled
+      //
+      GetProcessorNumber (PeiCpuMpData, &ProcessorNumber);
+      if (PeiCpuMpData->ApLoopMode == ApInHltLoop) {
+        //
+        // Restore AP's volatile registers saved
+        //
+        RestoreVolatileRegisters (&PeiCpuMpData->CpuData[ProcessorNumber].VolatileRegisters, TRUE);
+      }
+
+      if ((PeiCpuMpData->CpuData[ProcessorNumber].State != CpuStateDisabled) &&
+          (PeiCpuMpData->ApFunction != 0)) {
+        PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateBusy;
+        Procedure = (EFI_AP_PROCEDURE)(UINTN)PeiCpuMpData->ApFunction;
+        //
+        // Invoke AP function here
+        //
+        Procedure ((VOID *)(UINTN)PeiCpuMpData->ApFunctionArgument);
+        //
+        // Re-get the processor number due to BSP/AP maybe exchange in AP function
+        //
+        GetProcessorNumber (PeiCpuMpData, &ProcessorNumber);
+        PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;
+      }
+    }
+
     //
-    // This is first time AP wakeup, get BIST inforamtion from AP stack
+    // AP finished executing C code
     //
-    BistData = *(UINTN *) (PeiCpuMpData->Buffer + NumApsExecuting * PeiCpuMpData->CpuApStackSize - sizeof (UINTN));
-    PeiCpuMpData->CpuData[NumApsExecuting].ApicId        = GetInitialApicId ();
-    PeiCpuMpData->CpuData[NumApsExecuting].Health.Uint32 = (UINT32) BistData;
+    InterlockedIncrement ((UINT32 *)&PeiCpuMpData->FinishedCount);
+
     //
-    // Sync BSP's Mtrr table to all wakeup APs and load microcode on APs.
+    // Place AP is specified loop mode
     //
-    MtrrSetAllMtrrs (&PeiCpuMpData->MtrrTable);
-    MicrocodeDetect ();
-  } else {
+    if (PeiCpuMpData->ApLoopMode == ApInHltLoop) {
+      //
+      // Save AP volatile registers
+      //
+      SaveVolatileRegisters (&PeiCpuMpData->CpuData[ProcessorNumber].VolatileRegisters);
+      //
+      // Place AP in Hlt-loop
+      //
+      while (TRUE) {
+        DisableInterrupts ();
+        CpuSleep ();
+        CpuPause ();
+      }
+    }
+    ApStartupSignalBuffer = PeiCpuMpData->CpuData[ProcessorNumber].StartupApSignal;
     //
-    // Execute AP function if AP is not disabled
+    // Clear AP start-up signal
     //
-    GetProcessorNumber (PeiCpuMpData, &ProcessorNumber);
-    if ((PeiCpuMpData->CpuData[ProcessorNumber].State != CpuStateDisabled) &&
-        (PeiCpuMpData->ApFunction != 0)) {
-      PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateBusy;
-      Procedure = (EFI_AP_PROCEDURE)(UINTN)PeiCpuMpData->ApFunction;
-      Procedure ((VOID *)(UINTN)PeiCpuMpData->ApFunctionArgument);
-      PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;
+    *ApStartupSignalBuffer = 0;
+    while (TRUE) {
+      DisableInterrupts ();
+      if (PeiCpuMpData->ApLoopMode == ApInMwaitLoop) {
+        //
+        // Place AP in Mwait-loop
+        //
+        AsmMonitor ((UINTN)ApStartupSignalBuffer, 0, 0);
+        if (*ApStartupSignalBuffer != WAKEUP_AP_SIGNAL) {
+          //
+          // If AP start-up signal is not set, place AP into
+          // the maximum C-state
+          //
+          AsmMwait (PeiCpuMpData->ApTargetCState << 4, 0);
+        }
+      } else if (PeiCpuMpData->ApLoopMode == ApInRunLoop) {
+        //
+        // Place AP in Run-loop
+        //
+        CpuPause ();
+      } else {
+        ASSERT (FALSE);
+      }
+
+      //
+      // If AP start-up signal is written, AP is waken up
+      // otherwise place AP in loop again
+      //
+      if (*ApStartupSignalBuffer == WAKEUP_AP_SIGNAL) {
+        break;
+      }
     }
   }
-
-  //
-  // AP finished executing C code
-  //
-  InterlockedIncrement ((UINT32 *)&PeiCpuMpData->FinishedCount);
-
-  AsmCliHltLoop ();
 }
 
 /**
@@ -184,7 +399,7 @@ ApCFunction (
   @param PeiCpuMpData       Pointer to PEI CPU MP Data
   @param Broadcast          TRUE:  Send broadcast IPI to all APs
                             FALSE: Send IPI to AP by ApicId
-  @param ApicId             Apic ID for the processor to be waked
+  @param ProcessorNumber    The handle number of specified processor
   @param Procedure          The function to be invoked by AP
   @param ProcedureArgument  The argument to be passed into AP function
 **/
@@ -192,12 +407,13 @@ VOID
 WakeUpAP (
   IN PEI_CPU_MP_DATA           *PeiCpuMpData,
   IN BOOLEAN                   Broadcast,
-  IN UINT32                    ApicId,
+  IN UINTN                     ProcessorNumber,
   IN EFI_AP_PROCEDURE          Procedure,              OPTIONAL
   IN VOID                      *ProcedureArgument      OPTIONAL
   )
 {
   volatile MP_CPU_EXCHANGE_INFO    *ExchangeInfo;
+  UINTN                            Index;
 
   PeiCpuMpData->ApFunction         = (UINTN) Procedure;
   PeiCpuMpData->ApFunctionArgument = (UINTN) ProcedureArgument;
@@ -221,12 +437,40 @@ WakeUpAP (
   CopyMem ((VOID *)&ExchangeInfo->GdtrProfile, &mGdt, sizeof(mGdt));
   AsmReadIdtr ((IA32_DESCRIPTOR *) &ExchangeInfo->IdtrProfile);
 
-  if (Broadcast) {
-    SendInitSipiSipiAllExcludingSelf ((UINT32) ExchangeInfo->BufferStart);
-  } else {
-    SendInitSipiSipi (ApicId, (UINT32) ExchangeInfo->BufferStart);
+  if (PeiCpuMpData->ApLoopMode == ApInMwaitLoop) {
+    //
+    // Get AP target C-state each time when waking up AP,
+    // for it maybe updated by platform again
+    //
+    PeiCpuMpData->ApTargetCState = PcdGet8 (PcdCpuApTargetCstate);
   }
 
+  //
+  // Wakeup APs per AP loop state
+  //
+  if (PeiCpuMpData->ApLoopMode == ApInHltLoop || PeiCpuMpData->InitFlag) {
+    if (Broadcast) {
+      SendInitSipiSipiAllExcludingSelf ((UINT32) ExchangeInfo->BufferStart);
+    } else {
+      SendInitSipiSipi (
+        PeiCpuMpData->CpuData[ProcessorNumber].ApicId,
+        (UINT32) ExchangeInfo->BufferStart
+        );
+    }
+  } else if ((PeiCpuMpData->ApLoopMode == ApInMwaitLoop) ||
+             (PeiCpuMpData->ApLoopMode == ApInRunLoop)) {
+    if (Broadcast) {
+      for (Index = 0; Index < PeiCpuMpData->CpuCount; Index++) {
+        if (Index != PeiCpuMpData->BspNumber) {
+          *(PeiCpuMpData->CpuData[Index].StartupApSignal) = WAKEUP_AP_SIGNAL;
+        }
+      }
+    } else {
+      *(PeiCpuMpData->CpuData[ProcessorNumber].StartupApSignal) = WAKEUP_AP_SIGNAL;
+    }
+  } else {
+    ASSERT (FALSE);
+  }
   return ;
 }
 
@@ -363,18 +607,44 @@ CountProcessorNumber (
   //
   if (PcdGet32 (PcdCpuMaxLogicalProcessorNumber) > 1) {
     //
-    // Send broadcast IPI to APs to wakeup APs
+    // Send 1st broadcast IPI to APs to wakeup APs
     //
-    PeiCpuMpData->InitFlag = 1;
+    PeiCpuMpData->InitFlag     = TRUE;
+    PeiCpuMpData->X2ApicEnable = FALSE;
     WakeUpAP (PeiCpuMpData, TRUE, 0, NULL, NULL);
     //
     // Wait for AP task to complete and then exit.
     //
     MicroSecondDelay (PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds));
-    PeiCpuMpData->InitFlag = 0;
+    PeiCpuMpData->InitFlag  = FALSE;
     PeiCpuMpData->CpuCount += (UINT32)PeiCpuMpData->MpCpuExchangeInfo->NumApsExecuting;
     ASSERT (PeiCpuMpData->CpuCount <= PcdGet32 (PcdCpuMaxLogicalProcessorNumber));
     //
+    // Wait for all APs finished the initialization
+    //
+    while (PeiCpuMpData->FinishedCount < (PeiCpuMpData->CpuCount - 1)) {
+      CpuPause ();
+    }
+
+    if (PeiCpuMpData->X2ApicEnable) {
+      DEBUG ((EFI_D_INFO, "Force x2APIC mode!\n"));
+      //
+      // Wakeup all APs to enable x2APIC mode
+      //
+      WakeUpAP (PeiCpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL);
+      //
+      // Wait for all known APs finished
+      //
+      while (PeiCpuMpData->FinishedCount < (PeiCpuMpData->CpuCount - 1)) {
+        CpuPause ();
+      }
+      //
+      // Enable x2APIC on BSP
+      //
+      SetApicMode (LOCAL_APIC_MODE_X2APIC);
+    }
+    DEBUG ((EFI_D_INFO, "APIC MODE is %d\n", GetApicMode ()));
+    //
     // Sort BSP/Aps by CPU APIC ID in ascending order
     //
     SortApicId (PeiCpuMpData);
@@ -404,6 +674,10 @@ PrepareAPStartupVector (
   UINTN                         WakeupBuffer;
   UINTN                         WakeupBufferSize;
   MP_ASSEMBLY_ADDRESS_MAP       AddressMap;
+  UINT8                         ApLoopMode;
+  UINT16                        MonitorFilterSize;
+  UINT8                         *MonitorBuffer;
+  UINTN                         Index;
 
   AsmGetAddressMap (&AddressMap);
   WakeupBufferSize = AddressMap.RendezvousFunnelSize + sizeof (MP_CPU_EXCHANGE_INFO);
@@ -412,11 +686,14 @@ PrepareAPStartupVector (
   DEBUG ((EFI_D_INFO, "CpuMpPei: WakeupBuffer = 0x%x\n", WakeupBuffer));
 
   //
-  // Allocate Pages for APs stack, CPU MP Data and backup buffer for wakeup buffer
+  // Allocate Pages for APs stack, CPU MP Data, backup buffer for wakeup buffer,
+  // and monitor buffer if required.
   //
   MaxCpuCount = PcdGet32(PcdCpuMaxLogicalProcessorNumber);
   BufferSize  = PcdGet32 (PcdCpuApStackSize) * MaxCpuCount + sizeof (PEI_CPU_MP_DATA)
                   + WakeupBufferSize + sizeof (PEI_CPU_DATA) * MaxCpuCount;
+  ApLoopMode = GetApLoopMode (&MonitorFilterSize);
+  BufferSize += MonitorFilterSize * MaxCpuCount;
   Status = PeiServicesAllocatePages (
              EfiBootServicesData,
              EFI_SIZE_TO_PAGES (BufferSize),
@@ -439,8 +716,24 @@ PrepareAPStartupVector (
   PeiCpuMpData->CpuData[0].ApicId        = GetInitialApicId ();
   PeiCpuMpData->CpuData[0].Health.Uint32 = 0;
   PeiCpuMpData->EndOfPeiFlag             = FALSE;
+  InitializeSpinLock(&PeiCpuMpData->MpLock);
+  SaveVolatileRegisters (&PeiCpuMpData->CpuData[0].VolatileRegisters);
   CopyMem (&PeiCpuMpData->AddressMap, &AddressMap, sizeof (MP_ASSEMBLY_ADDRESS_MAP));
-
+  //
+  // Initialize AP loop mode
+  //
+  PeiCpuMpData->ApLoopMode = ApLoopMode;
+  DEBUG ((EFI_D_INFO, "AP Loop Mode is %d\n", PeiCpuMpData->ApLoopMode));
+  MonitorBuffer = (UINT8 *)(PeiCpuMpData->CpuData + MaxCpuCount);
+  if (PeiCpuMpData->ApLoopMode != ApInHltLoop) {
+    //
+    // Set up APs wakeup signal buffer
+    //
+    for (Index = 0; Index < MaxCpuCount; Index++) {
+      PeiCpuMpData->CpuData[Index].StartupApSignal = 
+        (UINT32 *)(MonitorBuffer + MonitorFilterSize * Index);
+    }
+  }
   //
   // Backup original data and copy AP reset code in it
   //
@@ -476,7 +769,7 @@ CpuMpEndOfPeiCallback (
   EFI_PEI_HOB_POINTERS      Hob;
   EFI_HOB_MEMORY_ALLOCATION *MemoryHob;
 
-  DEBUG ((EFI_D_INFO, "CpuMpPei: CpuMpEndOfPeiCallback () invokded\n"));
+  DEBUG ((EFI_D_INFO, "CpuMpPei: CpuMpEndOfPeiCallback () invoked\n"));
 
   Status = PeiServicesGetBootMode (&BootMode);
   ASSERT_EFI_ERROR (Status);