UefiCpuPkg/CpuMpPei/CpuMpPei.c

   1 /** @file
   2   CPU PEI Module installs CPU Multiple Processor PPI.
   3
   4   Copyright (c) 2015, Intel Corporation. All rights reserved.<BR>
   5   This program and the accompanying materials
   6   are licensed and made available under the terms and conditions of the BSD License
   7   which accompanies this distribution.  The full text of the license may be found at
   8   http://opensource.org/licenses/bsd-license.php
   9
  10   THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  11   WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  12
  13 **/
  14
  15 #include "CpuMpPei.h"
  16
  17 //
  18 // Global Descriptor Table (GDT)
  19 //
  20 GLOBAL_REMOVE_IF_UNREFERENCED IA32_GDT mGdtEntries[] = {
  21 /* selector { Global Segment Descriptor                              } */
  22 /* 0x00 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //null descriptor
  23 /* 0x08 */  {{0xffff, 0,  0,  0x2,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear data segment descriptor
  24 /* 0x10 */  {{0xffff, 0,  0,  0xf,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear code segment descriptor
  25 /* 0x18 */  {{0xffff, 0,  0,  0x3,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor
  26 /* 0x20 */  {{0xffff, 0,  0,  0xa,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system code segment descriptor
  27 /* 0x28 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor
  28 /* 0x30 */  {{0xffff, 0,  0,  0x2,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor
  29 /* 0x38 */  {{0xffff, 0,  0,  0xa,  1,  0,  1,  0xf,  0,  1, 0,  1,  0}}, //system code segment descriptor
  30 /* 0x40 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor
  31 };
  32
  33 //
  34 // IA32 Gdt register
  35 //
  36 GLOBAL_REMOVE_IF_UNREFERENCED IA32_DESCRIPTOR mGdt = {
  37   sizeof (mGdtEntries) - 1,
  38   (UINTN) mGdtEntries
  39   };
  40
  41 GLOBAL_REMOVE_IF_UNREFERENCED EFI_PEI_NOTIFY_DESCRIPTOR mNotifyList = {
  42   (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
  43   &gEfiEndOfPeiSignalPpiGuid,
  44   CpuMpEndOfPeiCallback
  45 };
  46
  47 /**
  48   Sort the APIC ID of all processors.
  49
  50   This function sorts the APIC ID of all processors so that processor number is
  51   assigned in the ascending order of APIC ID which eases MP debugging.
  52
  53   @param PeiCpuMpData        Pointer to PEI CPU MP Data
  54 **/
  55 VOID
  56 SortApicId (
  57   IN PEI_CPU_MP_DATA   *PeiCpuMpData
  58   )
  59 {
  60   UINTN             Index1;
  61   UINTN             Index2;
  62   UINTN             Index3;
  63   UINT32            ApicId;
  64   PEI_CPU_DATA      CpuData;
  65   UINT32            ApCount;
  66
  67   ApCount = PeiCpuMpData->CpuCount - 1;
  68
  69   if (ApCount != 0) {
  70     for (Index1 = 0; Index1 < ApCount; Index1++) {
  71       Index3 = Index1;
  72       //
  73       // Sort key is the hardware default APIC ID
  74       //
  75       ApicId = PeiCpuMpData->CpuData[Index1].ApicId;
  76       for (Index2 = Index1 + 1; Index2 <= ApCount; Index2++) {
  77         if (ApicId > PeiCpuMpData->CpuData[Index2].ApicId) {
  78           Index3 = Index2;
  79           ApicId = PeiCpuMpData->CpuData[Index2].ApicId;
  80         }
  81       }
  82       if (Index3 != Index1) {
  83         CopyMem (&CpuData, &PeiCpuMpData->CpuData[Index3], sizeof (PEI_CPU_DATA));
  84         CopyMem (
  85           &PeiCpuMpData->CpuData[Index3],
  86           &PeiCpuMpData->CpuData[Index1],
  87           sizeof (PEI_CPU_DATA)
  88           );
  89         CopyMem (&PeiCpuMpData->CpuData[Index1], &CpuData, sizeof (PEI_CPU_DATA));
  90       }
  91     }
  92
  93     //
  94     // Get the processor number for the BSP
  95     //
  96     ApicId = GetInitialApicId ();
  97     for (Index1 = 0; Index1 < PeiCpuMpData->CpuCount; Index1++) {
  98       if (PeiCpuMpData->CpuData[Index1].ApicId == ApicId) {
  99         PeiCpuMpData->BspNumber = (UINT32) Index1;
 100         break;
 101       }
 102     }
 103   }
 104 }
 105
 106 /**
 107   Enable x2APIC mode on APs.
 108
 109   @param Buffer  Pointer to private data buffer.
 110 **/
 111 VOID
 112 EFIAPI
 113 ApFuncEnableX2Apic (
 114   IN OUT VOID  *Buffer
 115   )
 116 {
 117   SetApicMode (LOCAL_APIC_MODE_X2APIC);
 118 }
 119
 120 /**
 121   Get AP loop mode.
 122
 123   @param MonitorFilterSize  Returns the largest monitor-line size in bytes.
 124
 125   @return The AP loop mode.
 126 **/
 127 UINT8
 128 GetApLoopMode (
 129   OUT UINT16     *MonitorFilterSize
 130   )
 131 {
 132   UINT8          ApLoopMode;
 133   UINT32         RegEbx;
 134   UINT32         RegEcx;
 135   UINT32         RegEdx;
 136
 137   ASSERT (MonitorFilterSize != NULL);
 138
 139   ApLoopMode = PcdGet8 (PcdCpuApLoopMode);
 140   ASSERT (ApLoopMode >= ApInHltLoop && ApLoopMode <= ApInRunLoop);
 141   if (ApLoopMode == ApInMwaitLoop) {
 142     AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &RegEcx, NULL);
 143     if ((RegEcx & BIT3) == 0) {
 144       //
 145       // If processor does not support MONITOR/MWAIT feature
 146       // by CPUID.[EAX=01H]:ECX.BIT3, force AP in Hlt-loop mode
 147       //
 148       ApLoopMode = ApInHltLoop;
 149     }
 150   }
 151
 152   if (ApLoopMode == ApInHltLoop) {
 153     *MonitorFilterSize = 0;
 154   } else if (ApLoopMode == ApInRunLoop) {
 155     *MonitorFilterSize = sizeof (UINT32);
 156   } else if (ApLoopMode == ApInMwaitLoop) {
 157     //
 158     // CPUID.[EAX=05H]:EBX.BIT0-15: Largest monitor-line size in bytes
 159     // CPUID.[EAX=05H].EDX: C-states supported using MWAIT
 160     //
 161     AsmCpuid (CPUID_MONITOR_MWAIT, NULL, &RegEbx, NULL, &RegEdx);
 162     *MonitorFilterSize = RegEbx & 0xFFFF;
 163   }
 164
 165   return ApLoopMode;
 166 }
 167
 168 /**
 169   Get CPU MP Data pointer from the Guided HOB.
 170
 171   @return  Pointer to Pointer to PEI CPU MP Data
 172 **/
 173 PEI_CPU_MP_DATA *
 174 GetMpHobData (
 175   VOID
 176   )
 177 {
 178   EFI_HOB_GUID_TYPE       *GuidHob;
 179   VOID                    *DataInHob;
 180   PEI_CPU_MP_DATA         *CpuMpData;
 181
 182   CpuMpData = NULL;
 183   GuidHob = GetFirstGuidHob (&gEfiCallerIdGuid);
 184   if (GuidHob != NULL) {
 185     DataInHob = GET_GUID_HOB_DATA (GuidHob);
 186     CpuMpData = (PEI_CPU_MP_DATA *)(*(UINTN *)DataInHob);
 187   }
 188   ASSERT (CpuMpData != NULL);
 189   return CpuMpData;
 190 }
 191
 192 /**
 193   Save the volatile registers required to be restored following INIT IPI.
 194
 195   @param  VolatileRegisters    Returns buffer saved the volatile resisters
 196 **/
 197 VOID
 198 SaveVolatileRegisters (
 199   OUT CPU_VOLATILE_REGISTERS    *VolatileRegisters
 200   )
 201 {
 202   UINT32                        RegEdx;
 203
 204   VolatileRegisters->Cr0 = AsmReadCr0 ();
 205   VolatileRegisters->Cr3 = AsmReadCr3 ();
 206   VolatileRegisters->Cr4 = AsmReadCr4 ();
 207
 208   AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);
 209   if ((RegEdx & BIT2) != 0) {
 210     //
 211     // If processor supports Debugging Extensions feature
 212     // by CPUID.[EAX=01H]:EDX.BIT2
 213     //
 214     VolatileRegisters->Dr0 = AsmReadDr0 ();
 215     VolatileRegisters->Dr1 = AsmReadDr1 ();
 216     VolatileRegisters->Dr2 = AsmReadDr2 ();
 217     VolatileRegisters->Dr3 = AsmReadDr3 ();
 218     VolatileRegisters->Dr6 = AsmReadDr6 ();
 219     VolatileRegisters->Dr7 = AsmReadDr7 ();
 220   }
 221 }
 222
 223 /**
 224   Restore the volatile registers following INIT IPI.
 225
 226   @param  VolatileRegisters   Pointer to volatile resisters
 227   @param  IsRestoreDr         TRUE:  Restore DRx if supported
 228                               FALSE: Do not restore DRx
 229 **/
 230 VOID
 231 RestoreVolatileRegisters (
 232   IN CPU_VOLATILE_REGISTERS    *VolatileRegisters,
 233   IN BOOLEAN                   IsRestoreDr
 234   )
 235 {
 236   UINT32                        RegEdx;
 237
 238   AsmWriteCr0 (VolatileRegisters->Cr0);
 239   AsmWriteCr3 (VolatileRegisters->Cr3);
 240   AsmWriteCr4 (VolatileRegisters->Cr4);
 241
 242   if (IsRestoreDr) {
 243     AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);
 244     if ((RegEdx & BIT2) != 0) {
 245       //
 246       // If processor supports Debugging Extensions feature
 247       // by CPUID.[EAX=01H]:EDX.BIT2
 248       //
 249       AsmWriteDr0 (VolatileRegisters->Dr0);
 250       AsmWriteDr1 (VolatileRegisters->Dr1);
 251       AsmWriteDr2 (VolatileRegisters->Dr2);
 252       AsmWriteDr3 (VolatileRegisters->Dr3);
 253       AsmWriteDr6 (VolatileRegisters->Dr6);
 254       AsmWriteDr7 (VolatileRegisters->Dr7);
 255     }
 256   }
 257 }
 258
 259 /**
 260   This function will be called from AP reset code if BSP uses WakeUpAP.
 261
 262   @param ExchangeInfo     Pointer to the MP exchange info buffer
 263   @param NumApsExecuting  Number of current executing AP
 264 **/
 265 VOID
 266 EFIAPI
 267 ApCFunction (
 268   IN MP_CPU_EXCHANGE_INFO      *ExchangeInfo,
 269   IN UINTN                     NumApsExecuting
 270   )
 271 {
 272   PEI_CPU_MP_DATA            *PeiCpuMpData;
 273   UINTN                      ProcessorNumber;
 274   EFI_AP_PROCEDURE           Procedure;
 275   UINTN                      BistData;
 276
 277   PeiCpuMpData = ExchangeInfo->PeiCpuMpData;
 278   if (PeiCpuMpData->InitFlag) {
 279     ProcessorNumber = NumApsExecuting;
 280     //
 281     // Sync BSP's Control registers to APs
 282     //
 283     RestoreVolatileRegisters (&PeiCpuMpData->CpuData[0].VolatileRegisters, FALSE);
 284     //
 285     // This is first time AP wakeup, get BIST information from AP stack
 286     //
 287     BistData = *(UINTN *) (PeiCpuMpData->Buffer + ProcessorNumber * PeiCpuMpData->CpuApStackSize - sizeof (UINTN));
 288     PeiCpuMpData->CpuData[ProcessorNumber].Health.Uint32 = (UINT32) BistData;
 289     PeiCpuMpData->CpuData[ProcessorNumber].ApicId = GetInitialApicId ();
 290     if (PeiCpuMpData->CpuData[ProcessorNumber].ApicId >= 0xFF) {
 291       //
 292       // Set x2APIC mode if there are any logical processor reporting
 293       // an APIC ID of 255 or greater.
 294       //
 295       AcquireSpinLock(&PeiCpuMpData->MpLock);
 296       PeiCpuMpData->X2ApicEnable = TRUE;
 297       ReleaseSpinLock(&PeiCpuMpData->MpLock);
 298     }
 299     //
 300     // Sync BSP's Mtrr table to all wakeup APs and load microcode on APs.
 301     //
 302     MtrrSetAllMtrrs (&PeiCpuMpData->MtrrTable);
 303     MicrocodeDetect ();
 304     PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;
 305   } else {
 306     //
 307     // Execute AP function if AP is not disabled
 308     //
 309     GetProcessorNumber (PeiCpuMpData, &ProcessorNumber);
 310     //
 311     // Restore AP's volatile registers saved
 312     //
 313     RestoreVolatileRegisters (&PeiCpuMpData->CpuData[ProcessorNumber].VolatileRegisters, TRUE);
 314
 315     if ((PeiCpuMpData->CpuData[ProcessorNumber].State != CpuStateDisabled) &&
 316         (PeiCpuMpData->ApFunction != 0)) {
 317       PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateBusy;
 318       Procedure = (EFI_AP_PROCEDURE)(UINTN)PeiCpuMpData->ApFunction;
 319       //
 320       // Invoke AP function here
 321       //
 322       Procedure ((VOID *)(UINTN)PeiCpuMpData->ApFunctionArgument);
 323       PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;
 324     }
 325   }
 326
 327   //
 328   // AP finished executing C code
 329   //
 330   InterlockedIncrement ((UINT32 *)&PeiCpuMpData->FinishedCount);
 331
 332   //
 333   // Save AP volatile registers
 334   //
 335   SaveVolatileRegisters (&PeiCpuMpData->CpuData[ProcessorNumber].VolatileRegisters);
 336
 337   AsmCliHltLoop ();
 338 }
 339
 340 /**
 341   This function will be called by BSP to wakeup AP.
 342
 343   @param PeiCpuMpData       Pointer to PEI CPU MP Data
 344   @param Broadcast          TRUE:  Send broadcast IPI to all APs
 345                             FALSE: Send IPI to AP by ApicId
 346   @param ApicId             Apic ID for the processor to be waked
 347   @param Procedure          The function to be invoked by AP
 348   @param ProcedureArgument  The argument to be passed into AP function
 349 **/
 350 VOID
 351 WakeUpAP (
 352   IN PEI_CPU_MP_DATA           *PeiCpuMpData,
 353   IN BOOLEAN                   Broadcast,
 354   IN UINT32                    ApicId,
 355   IN EFI_AP_PROCEDURE          Procedure,              OPTIONAL
 356   IN VOID                      *ProcedureArgument      OPTIONAL
 357   )
 358 {
 359   volatile MP_CPU_EXCHANGE_INFO    *ExchangeInfo;
 360
 361   PeiCpuMpData->ApFunction         = (UINTN) Procedure;
 362   PeiCpuMpData->ApFunctionArgument = (UINTN) ProcedureArgument;
 363   PeiCpuMpData->FinishedCount      = 0;
 364
 365   ExchangeInfo                     = PeiCpuMpData->MpCpuExchangeInfo;
 366   ExchangeInfo->Lock               = 0;
 367   ExchangeInfo->StackStart         = PeiCpuMpData->Buffer;
 368   ExchangeInfo->StackSize          = PeiCpuMpData->CpuApStackSize;
 369   ExchangeInfo->BufferStart        = PeiCpuMpData->WakeupBuffer;
 370   ExchangeInfo->PmodeOffset        = PeiCpuMpData->AddressMap.PModeEntryOffset;
 371   ExchangeInfo->LmodeOffset        = PeiCpuMpData->AddressMap.LModeEntryOffset;
 372   ExchangeInfo->Cr3                = AsmReadCr3 ();
 373   ExchangeInfo->CFunction          = (UINTN) ApCFunction;
 374   ExchangeInfo->NumApsExecuting    = 0;
 375   ExchangeInfo->PeiCpuMpData       = PeiCpuMpData;
 376
 377   //
 378   // Get the BSP's data of GDT and IDT
 379   //
 380   CopyMem ((VOID *)&ExchangeInfo->GdtrProfile, &mGdt, sizeof(mGdt));
 381   AsmReadIdtr ((IA32_DESCRIPTOR *) &ExchangeInfo->IdtrProfile);
 382
 383   if (Broadcast) {
 384     SendInitSipiSipiAllExcludingSelf ((UINT32) ExchangeInfo->BufferStart);
 385   } else {
 386     SendInitSipiSipi (ApicId, (UINT32) ExchangeInfo->BufferStart);
 387   }
 388
 389   return ;
 390 }
 391
 392 /**
 393   Get available system memory below 1MB by specified size.
 394
 395   @param  WakeupBufferSize   Wakeup buffer size required
 396
 397   @retval other   Return wakeup buffer address below 1MB.
 398   @retval -1      Cannot find free memory below 1MB.
 399 **/
 400 UINTN
 401 GetWakeupBuffer (
 402   IN UINTN                WakeupBufferSize
 403   )
 404 {
 405   EFI_PEI_HOB_POINTERS    Hob;
 406   UINTN                   WakeupBufferStart;
 407   UINTN                   WakeupBufferEnd;
 408
 409   //
 410   // Get the HOB list for processing
 411   //
 412   Hob.Raw = GetHobList ();
 413
 414   //
 415   // Collect memory ranges
 416   //
 417   while (!END_OF_HOB_LIST (Hob)) {
 418     if (Hob.Header->HobType == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
 419       if ((Hob.ResourceDescriptor->PhysicalStart < BASE_1MB) &&
 420           (Hob.ResourceDescriptor->ResourceType == EFI_RESOURCE_SYSTEM_MEMORY) &&
 421           ((Hob.ResourceDescriptor->ResourceAttribute &
 422             (EFI_RESOURCE_ATTRIBUTE_READ_PROTECTED |
 423              EFI_RESOURCE_ATTRIBUTE_WRITE_PROTECTED |
 424              EFI_RESOURCE_ATTRIBUTE_EXECUTION_PROTECTED
 425              )) == 0)
 426            ) {
 427         //
 428         // Need memory under 1MB to be collected here
 429         //
 430         WakeupBufferEnd = (UINTN) (Hob.ResourceDescriptor->PhysicalStart + Hob.ResourceDescriptor->ResourceLength);
 431         if (WakeupBufferEnd > BASE_1MB) {
 432           //
 433           // Wakeup buffer should be under 1MB
 434           //
 435           WakeupBufferEnd = BASE_1MB;
 436         }
 437         //
 438         // Wakeup buffer should be aligned on 4KB
 439         //
 440         WakeupBufferStart = (WakeupBufferEnd - WakeupBufferSize) & ~(SIZE_4KB - 1);
 441         if (WakeupBufferStart < Hob.ResourceDescriptor->PhysicalStart) {
 442           continue;
 443         }
 444         //
 445         // Create a memory allocation HOB.
 446         //
 447         BuildMemoryAllocationHob (
 448           WakeupBufferStart,
 449           WakeupBufferSize,
 450           EfiBootServicesData
 451           );
 452         return WakeupBufferStart;
 453       }
 454     }
 455     //
 456     // Find the next HOB
 457     //
 458     Hob.Raw = GET_NEXT_HOB (Hob);
 459   }
 460
 461   return (UINTN) -1;
 462 }
 463
 464 /**
 465   Get available system memory below 1MB by specified size.
 466
 467   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 468 **/
 469 VOID
 470 BackupAndPrepareWakeupBuffer(
 471   IN PEI_CPU_MP_DATA         *PeiCpuMpData
 472   )
 473 {
 474   CopyMem (
 475     (VOID *) PeiCpuMpData->BackupBuffer,
 476     (VOID *) PeiCpuMpData->WakeupBuffer,
 477     PeiCpuMpData->BackupBufferSize
 478     );
 479   CopyMem (
 480     (VOID *) PeiCpuMpData->WakeupBuffer,
 481     (VOID *) PeiCpuMpData->AddressMap.RendezvousFunnelAddress,
 482     PeiCpuMpData->AddressMap.RendezvousFunnelSize
 483     );
 484 }
 485
 486 /**
 487   Restore wakeup buffer data.
 488
 489   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 490 **/
 491 VOID
 492 RestoreWakeupBuffer(
 493   IN PEI_CPU_MP_DATA         *PeiCpuMpData
 494   )
 495 {
 496   CopyMem ((VOID *) PeiCpuMpData->WakeupBuffer, (VOID *) PeiCpuMpData->BackupBuffer, PeiCpuMpData->BackupBufferSize);
 497 }
 498
 499 /**
 500   This function will get CPU count in the system.
 501
 502   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 503
 504   @return  AP processor count
 505 **/
 506 UINT32
 507 CountProcessorNumber (
 508   IN PEI_CPU_MP_DATA            *PeiCpuMpData
 509   )
 510 {
 511   //
 512   // Load Microcode on BSP
 513   //
 514   MicrocodeDetect ();
 515   //
 516   // Store BSP's MTRR setting
 517   //
 518   MtrrGetAllMtrrs (&PeiCpuMpData->MtrrTable);
 519
 520   //
 521   // Only perform AP detection if PcdCpuMaxLogicalProcessorNumber is greater than 1
 522   //
 523   if (PcdGet32 (PcdCpuMaxLogicalProcessorNumber) > 1) {
 524     //
 525     // Send 1st broadcast IPI to APs to wakeup APs
 526     //
 527     PeiCpuMpData->InitFlag     = TRUE;
 528     PeiCpuMpData->X2ApicEnable = FALSE;
 529     WakeUpAP (PeiCpuMpData, TRUE, 0, NULL, NULL);
 530     //
 531     // Wait for AP task to complete and then exit.
 532     //
 533     MicroSecondDelay (PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds));
 534     PeiCpuMpData->InitFlag  = FALSE;
 535     PeiCpuMpData->CpuCount += (UINT32)PeiCpuMpData->MpCpuExchangeInfo->NumApsExecuting;
 536     ASSERT (PeiCpuMpData->CpuCount <= PcdGet32 (PcdCpuMaxLogicalProcessorNumber));
 537     //
 538     // Wait for all APs finished the initialization
 539     //
 540     while (PeiCpuMpData->FinishedCount < (PeiCpuMpData->CpuCount - 1)) {
 541       CpuPause ();
 542     }
 543
 544     if (PeiCpuMpData->X2ApicEnable) {
 545       DEBUG ((EFI_D_INFO, "Force x2APIC mode!\n"));
 546       //
 547       // Send 2nd broadcast IPI to all APs to enable x2APIC mode
 548       //
 549       WakeUpAP (PeiCpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL);
 550       //
 551       // Wait for all known APs finished
 552       //
 553       while (PeiCpuMpData->FinishedCount < (PeiCpuMpData->CpuCount - 1)) {
 554         CpuPause ();
 555       }
 556       //
 557       // Enable x2APIC on BSP
 558       //
 559       SetApicMode (LOCAL_APIC_MODE_X2APIC);
 560     }
 561     DEBUG ((EFI_D_INFO, "APIC MODE is %d\n", GetApicMode ()));
 562     //
 563     // Sort BSP/Aps by CPU APIC ID in ascending order
 564     //
 565     SortApicId (PeiCpuMpData);
 566   }
 567
 568   DEBUG ((EFI_D_INFO, "CpuMpPei: Find %d processors in system.\n", PeiCpuMpData->CpuCount));
 569   return PeiCpuMpData->CpuCount;
 570 }
 571
 572 /**
 573   Prepare for AP wakeup buffer and copy AP reset code into it.
 574
 575   Get wakeup buffer below 1MB. Allocate memory for CPU MP Data and APs Stack.
 576
 577   @return   Pointer to PEI CPU MP Data
 578 **/
 579 PEI_CPU_MP_DATA *
 580 PrepareAPStartupVector (
 581   VOID
 582   )
 583 {
 584   EFI_STATUS                    Status;
 585   UINT32                        MaxCpuCount;
 586   PEI_CPU_MP_DATA               *PeiCpuMpData;
 587   EFI_PHYSICAL_ADDRESS          Buffer;
 588   UINTN                         BufferSize;
 589   UINTN                         WakeupBuffer;
 590   UINTN                         WakeupBufferSize;
 591   MP_ASSEMBLY_ADDRESS_MAP       AddressMap;
 592   UINT8                         ApLoopMode;
 593   UINT16                        MonitorFilterSize;
 594   UINT8                         *MonitorBuffer;
 595   UINTN                         Index;
 596
 597   AsmGetAddressMap (&AddressMap);
 598   WakeupBufferSize = AddressMap.RendezvousFunnelSize + sizeof (MP_CPU_EXCHANGE_INFO);
 599   WakeupBuffer     = GetWakeupBuffer ((WakeupBufferSize + SIZE_4KB - 1) & ~(SIZE_4KB - 1));
 600   ASSERT (WakeupBuffer != (UINTN) -1);
 601   DEBUG ((EFI_D_INFO, "CpuMpPei: WakeupBuffer = 0x%x\n", WakeupBuffer));
 602
 603   //
 604   // Allocate Pages for APs stack, CPU MP Data, backup buffer for wakeup buffer,
 605   // and monitor buffer if required.
 606   //
 607   MaxCpuCount = PcdGet32(PcdCpuMaxLogicalProcessorNumber);
 608   BufferSize  = PcdGet32 (PcdCpuApStackSize) * MaxCpuCount + sizeof (PEI_CPU_MP_DATA)
 609                   + WakeupBufferSize + sizeof (PEI_CPU_DATA) * MaxCpuCount;
 610   ApLoopMode = GetApLoopMode (&MonitorFilterSize);
 611   BufferSize += MonitorFilterSize * MaxCpuCount;
 612   Status = PeiServicesAllocatePages (
 613              EfiBootServicesData,
 614              EFI_SIZE_TO_PAGES (BufferSize),
 615              &Buffer
 616              );
 617   ASSERT_EFI_ERROR (Status);
 618
 619   PeiCpuMpData = (PEI_CPU_MP_DATA *) (UINTN) (Buffer + PcdGet32 (PcdCpuApStackSize) * MaxCpuCount);
 620   PeiCpuMpData->Buffer            = (UINTN) Buffer;
 621   PeiCpuMpData->CpuApStackSize    = PcdGet32 (PcdCpuApStackSize);
 622   PeiCpuMpData->WakeupBuffer      = WakeupBuffer;
 623   PeiCpuMpData->BackupBuffer      = (UINTN)PeiCpuMpData + sizeof (PEI_CPU_MP_DATA);
 624   PeiCpuMpData->BackupBufferSize  = WakeupBufferSize;
 625   PeiCpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeupBuffer + AddressMap.RendezvousFunnelSize);
 626
 627   PeiCpuMpData->CpuCount                 = 1;
 628   PeiCpuMpData->BspNumber                = 0;
 629   PeiCpuMpData->CpuData                  = (PEI_CPU_DATA *) (PeiCpuMpData->BackupBuffer +
 630                                                              PeiCpuMpData->BackupBufferSize);
 631   PeiCpuMpData->CpuData[0].ApicId        = GetInitialApicId ();
 632   PeiCpuMpData->CpuData[0].Health.Uint32 = 0;
 633   PeiCpuMpData->EndOfPeiFlag             = FALSE;
 634   InitializeSpinLock(&PeiCpuMpData->MpLock);
 635   SaveVolatileRegisters (&PeiCpuMpData->CpuData[0].VolatileRegisters);
 636   CopyMem (&PeiCpuMpData->AddressMap, &AddressMap, sizeof (MP_ASSEMBLY_ADDRESS_MAP));
 637   //
 638   // Initialize AP loop mode
 639   //
 640   PeiCpuMpData->ApLoopMode = ApLoopMode;
 641   DEBUG ((EFI_D_INFO, "AP Loop Mode is %d\n", PeiCpuMpData->ApLoopMode));
 642   MonitorBuffer = (UINT8 *)(PeiCpuMpData->CpuData + MaxCpuCount);
 643   if (PeiCpuMpData->ApLoopMode != ApInHltLoop) {
 644     //
 645     // Set up APs wakeup signal buffer
 646     //
 647     for (Index = 0; Index < MaxCpuCount; Index++) {
 648       PeiCpuMpData->CpuData[Index].StartupApSignal =
 649         (UINT32 *)(MonitorBuffer + MonitorFilterSize * Index);
 650     }
 651   }
 652   //
 653   // Backup original data and copy AP reset code in it
 654   //
 655   BackupAndPrepareWakeupBuffer(PeiCpuMpData);
 656
 657   return PeiCpuMpData;
 658 }
 659
 660 /**
 661   Notify function on End Of Pei PPI.
 662
 663   On S3 boot, this function will restore wakeup buffer data.
 664   On normal boot, this function will flag wakeup buffer to be un-used type.
 665
 666   @param  PeiServices        The pointer to the PEI Services Table.
 667   @param  NotifyDescriptor   Address of the notification descriptor data structure.
 668   @param  Ppi                Address of the PPI that was installed.
 669
 670   @retval EFI_SUCCESS        When everything is OK.
 671
 672 **/
 673 EFI_STATUS
 674 EFIAPI
 675 CpuMpEndOfPeiCallback (
 676   IN      EFI_PEI_SERVICES        **PeiServices,
 677   IN EFI_PEI_NOTIFY_DESCRIPTOR    *NotifyDescriptor,
 678   IN VOID                         *Ppi
 679   )
 680 {
 681   EFI_STATUS                Status;
 682   EFI_BOOT_MODE             BootMode;
 683   PEI_CPU_MP_DATA           *PeiCpuMpData;
 684   EFI_PEI_HOB_POINTERS      Hob;
 685   EFI_HOB_MEMORY_ALLOCATION *MemoryHob;
 686
 687   DEBUG ((EFI_D_INFO, "CpuMpPei: CpuMpEndOfPeiCallback () invoked\n"));
 688
 689   Status = PeiServicesGetBootMode (&BootMode);
 690   ASSERT_EFI_ERROR (Status);
 691
 692   PeiCpuMpData = GetMpHobData ();
 693   ASSERT (PeiCpuMpData != NULL);
 694
 695   if (BootMode != BOOT_ON_S3_RESUME) {
 696     //
 697     // Get the HOB list for processing
 698     //
 699     Hob.Raw = GetHobList ();
 700     //
 701     // Collect memory ranges
 702     //
 703     while (!END_OF_HOB_LIST (Hob)) {
 704       if (Hob.Header->HobType == EFI_HOB_TYPE_MEMORY_ALLOCATION) {
 705         MemoryHob = Hob.MemoryAllocation;
 706         if(MemoryHob->AllocDescriptor.MemoryBaseAddress == PeiCpuMpData->WakeupBuffer) {
 707           //
 708           // Flag this HOB type to un-used
 709           //
 710           GET_HOB_TYPE (Hob) = EFI_HOB_TYPE_UNUSED;
 711           break;
 712         }
 713       }
 714       Hob.Raw = GET_NEXT_HOB (Hob);
 715     }
 716   } else {
 717     RestoreWakeupBuffer (PeiCpuMpData);
 718     PeiCpuMpData->EndOfPeiFlag = TRUE;
 719   }
 720   return EFI_SUCCESS;
 721 }
 722
 723 /**
 724   The Entry point of the MP CPU PEIM.
 725
 726   This function will wakeup APs and collect CPU AP count and install the
 727   Mp Service Ppi.
 728
 729   @param  FileHandle    Handle of the file being invoked.
 730   @param  PeiServices   Describes the list of possible PEI Services.
 731
 732   @retval EFI_SUCCESS   MpServicePpi is installed successfully.
 733
 734 **/
 735 EFI_STATUS
 736 EFIAPI
 737 CpuMpPeimInit (
 738   IN       EFI_PEI_FILE_HANDLE  FileHandle,
 739   IN CONST EFI_PEI_SERVICES     **PeiServices
 740   )
 741 {
 742   EFI_STATUS           Status;
 743   PEI_CPU_MP_DATA      *PeiCpuMpData;
 744   UINT32               ProcessorCount;
 745
 746   //
 747   // Load new GDT table on BSP
 748   //
 749   AsmInitializeGdt (&mGdt);
 750   //
 751   // Get wakeup buffer and copy AP reset code in it
 752   //
 753   PeiCpuMpData = PrepareAPStartupVector ();
 754   //
 755   // Count processor number and collect processor information
 756   //
 757   ProcessorCount = CountProcessorNumber (PeiCpuMpData);
 758   //
 759   // Build location of PEI CPU MP DATA buffer in HOB
 760   //
 761   BuildGuidDataHob (
 762     &gEfiCallerIdGuid,
 763     (VOID *)&PeiCpuMpData,
 764     sizeof(UINT64)
 765     );
 766   //
 767   // Update and publish CPU BIST information
 768   //
 769   CollectBistDataFromPpi (PeiServices, PeiCpuMpData);
 770   //
 771   // register an event for EndOfPei
 772   //
 773   Status  = PeiServicesNotifyPpi (&mNotifyList);
 774   ASSERT_EFI_ERROR (Status);
 775   //
 776   // Install CPU MP PPI
 777   //
 778   Status = PeiServicesInstallPpi(&mPeiCpuMpPpiDesc);
 779   ASSERT_EFI_ERROR (Status);
 780
 781   return Status;
 782 }