EmulatorPkg/CpuRuntimeDxe/MpService.c

   1 /** @file
   2   Construct MP Services Protocol on top of the EMU Thread protocol.
   3   This code makes APs show up in the emulator. PcdEmuApCount is the
   4   number of APs the emulator should produce.
   5
   6   The MP Services Protocol provides a generalized way of performing following tasks:
   7     - Retrieving information of multi-processor environment and MP-related status of
   8       specific processors.
   9     - Dispatching user-provided function to APs.
  10     - Maintain MP-related processor status.
  11
  12   The MP Services Protocol must be produced on any system with more than one logical
  13   processor.
  14
  15   The Protocol is available only during boot time.
  16
  17   MP Services Protocol is hardware-independent. Most of the logic of this protocol
  18   is architecturally neutral. It abstracts the multi-processor environment and
  19   status of processors, and provides interfaces to retrieve information, maintain,
  20   and dispatch.
  21
  22   MP Services Protocol may be consumed by ACPI module. The ACPI module may use this
  23   protocol to retrieve data that are needed for an MP platform and report them to OS.
  24   MP Services Protocol may also be used to program and configure processors, such
  25   as MTRR synchronization for memory space attributes setting in DXE Services.
  26   MP Services Protocol may be used by non-CPU DXE drivers to speed up platform boot
  27   by taking advantage of the processing capabilities of the APs, for example, using
  28   APs to help test system memory in parallel with other device initialization.
  29   Diagnostics applications may also use this protocol for multi-processor.
  30
  31 Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR>
  32 Portitions Copyright (c) 2011, Apple Inc. All rights reserved.
  33 This program and the accompanying materials are licensed and made available under
  34 the terms and conditions of the BSD License that accompanies this distribution.
  35 The full text of the license may be found at
  36 http://opensource.org/licenses/bsd-license.php.
  37
  38 THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  39 WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  40
  41
  42 **/
  43
  44 #include "CpuDriver.h"
  45
  46
  47 MP_SYSTEM_DATA                gMPSystem;
  48 EMU_THREAD_THUNK_PROTOCOL     *gThread = NULL;
  49 EFI_EVENT                     gReadToBootEvent;
  50 BOOLEAN                       gReadToBoot = FALSE;
  51 UINTN                         gPollInterval;
  52
  53
  54 BOOLEAN
  55 IsBSP (
  56   VOID
  57   )
  58 {
  59   EFI_STATUS  Status;
  60   UINTN       ProcessorNumber;
  61
  62   Status = CpuMpServicesWhoAmI (&mMpServicesTemplate, &ProcessorNumber);
  63   if (EFI_ERROR (Status)) {
  64     return FALSE;
  65   }
  66
  67   return (gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0;
  68 }
  69
  70
  71 VOID
  72 SetApProcedure (
  73   IN   PROCESSOR_DATA_BLOCK  *Processor,
  74   IN   EFI_AP_PROCEDURE      Procedure,
  75   IN   VOID                  *ProcedureArgument
  76   )
  77 {
  78   gThread->MutexLock (Processor->ProcedureLock);
  79   Processor->Parameter  = ProcedureArgument;
  80   Processor->Procedure  = Procedure;
  81   gThread->MutexUnlock (Processor->ProcedureLock);
  82 }
  83
  84
  85 EFI_STATUS
  86 GetNextBlockedNumber (
  87   OUT UINTN                               *NextNumber
  88   )
  89 {
  90   UINTN                 Number;
  91   PROCESSOR_STATE       ProcessorState;
  92   PROCESSOR_DATA_BLOCK  *Data;
  93
  94   for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
  95     Data = &gMPSystem.ProcessorData[Number];
  96     if ((Data->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
  97       // Skip BSP
  98       continue;
  99     }
 100
 101     gThread->MutexLock (Data->StateLock);
 102     ProcessorState = Data->State;
 103     gThread->MutexUnlock (Data->StateLock);
 104
 105     if (ProcessorState == CPU_STATE_BLOCKED) {
 106       *NextNumber = Number;
 107       return EFI_SUCCESS;
 108     }
 109   }
 110
 111   return EFI_NOT_FOUND;
 112 }
 113
 114 /**
 115  * Calculated and stalled the interval time by BSP to check whether
 116  * the APs have finished.
 117  *
 118  * @param[in]  Timeout    The time limit in microseconds for
 119  *                        APs to return from Procedure.
 120  *
 121  * @retval     StallTime  Time of execution stall.
 122 **/
 123 UINTN
 124 CalculateAndStallInterval (
 125   IN UINTN                  Timeout
 126   )
 127 {
 128   UINTN                 StallTime;
 129
 130   if (Timeout < gPollInterval && Timeout != 0) {
 131     StallTime = Timeout;
 132   } else {
 133     StallTime = gPollInterval;
 134   }
 135   gBS->Stall (StallTime);
 136
 137   return StallTime;
 138 }
 139
 140 /**
 141   This service retrieves the number of logical processor in the platform
 142   and the number of those logical processors that are enabled on this boot.
 143   This service may only be called from the BSP.
 144
 145   This function is used to retrieve the following information:
 146     - The number of logical processors that are present in the system.
 147     - The number of enabled logical processors in the system at the instant
 148       this call is made.
 149
 150   Because MP Service Protocol provides services to enable and disable processors
 151   dynamically, the number of enabled logical processors may vary during the
 152   course of a boot session.
 153
 154   If this service is called from an AP, then EFI_DEVICE_ERROR is returned.
 155   If NumberOfProcessors or NumberOfEnabledProcessors is NULL, then
 156   EFI_INVALID_PARAMETER is returned. Otherwise, the total number of processors
 157   is returned in NumberOfProcessors, the number of currently enabled processor
 158   is returned in NumberOfEnabledProcessors, and EFI_SUCCESS is returned.
 159
 160   @param[in]  This                        A pointer to the EFI_MP_SERVICES_PROTOCOL
 161                                           instance.
 162   @param[out] NumberOfProcessors          Pointer to the total number of logical
 163                                           processors in the system, including the BSP
 164                                           and disabled APs.
 165   @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled logical
 166                                           processors that exist in system, including
 167                                           the BSP.
 168
 169   @retval EFI_SUCCESS             The number of logical processors and enabled
 170                                   logical processors was retrieved.
 171   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 172   @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL.
 173   @retval EFI_INVALID_PARAMETER   NumberOfEnabledProcessors is NULL.
 174
 175 **/
 176 EFI_STATUS
 177 EFIAPI
 178 CpuMpServicesGetNumberOfProcessors (
 179   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 180   OUT UINTN                     *NumberOfProcessors,
 181   OUT UINTN                     *NumberOfEnabledProcessors
 182   )
 183 {
 184   if ((NumberOfProcessors == NULL) || (NumberOfEnabledProcessors == NULL)) {
 185     return EFI_INVALID_PARAMETER;
 186   }
 187
 188   if (!IsBSP ()) {
 189     return EFI_DEVICE_ERROR;
 190   }
 191
 192   *NumberOfProcessors        = gMPSystem.NumberOfProcessors;
 193   *NumberOfEnabledProcessors = gMPSystem.NumberOfEnabledProcessors;
 194   return EFI_SUCCESS;
 195 }
 196
 197
 198
 199 /**
 200   Gets detailed MP-related information on the requested processor at the
 201   instant this call is made. This service may only be called from the BSP.
 202
 203   This service retrieves detailed MP-related information about any processor
 204   on the platform. Note the following:
 205     - The processor information may change during the course of a boot session.
 206     - The information presented here is entirely MP related.
 207
 208   Information regarding the number of caches and their sizes, frequency of operation,
 209   slot numbers is all considered platform-related information and is not provided
 210   by this service.
 211
 212   @param[in]  This                  A pointer to the EFI_MP_SERVICES_PROTOCOL
 213                                     instance.
 214   @param[in]  ProcessorNumber       The handle number of processor.
 215   @param[out] ProcessorInfoBuffer   A pointer to the buffer where information for
 216                                     the requested processor is deposited.
 217
 218   @retval EFI_SUCCESS             Processor information was returned.
 219   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 220   @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
 221   @retval EFI_NOT_FOUND           The processor with the handle specified by
 222                                   ProcessorNumber does not exist in the platform.
 223
 224 **/
 225 EFI_STATUS
 226 EFIAPI
 227 CpuMpServicesGetProcessorInfo (
 228   IN  EFI_MP_SERVICES_PROTOCOL   *This,
 229   IN  UINTN                      ProcessorNumber,
 230   OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
 231   )
 232 {
 233   if (ProcessorInfoBuffer == NULL) {
 234     return EFI_INVALID_PARAMETER;
 235   }
 236
 237   if (!IsBSP ()) {
 238     return EFI_DEVICE_ERROR;
 239   }
 240
 241   if (ProcessorNumber >= gMPSystem.NumberOfProcessors) {
 242     return EFI_NOT_FOUND;
 243   }
 244
 245   CopyMem (ProcessorInfoBuffer, &gMPSystem.ProcessorData[ProcessorNumber], sizeof (EFI_PROCESSOR_INFORMATION));
 246   return EFI_SUCCESS;
 247 }
 248
 249
 250 /**
 251   This service executes a caller provided function on all enabled APs. APs can
 252   run either simultaneously or one at a time in sequence. This service supports
 253   both blocking and non-blocking requests. The non-blocking requests use EFI
 254   events so the BSP can detect when the APs have finished. This service may only
 255   be called from the BSP.
 256
 257   This function is used to dispatch all the enabled APs to the function specified
 258   by Procedure.  If any enabled AP is busy, then EFI_NOT_READY is returned
 259   immediately and Procedure is not started on any AP.
 260
 261   If SingleThread is TRUE, all the enabled APs execute the function specified by
 262   Procedure one by one, in ascending order of processor handle number. Otherwise,
 263   all the enabled APs execute the function specified by Procedure simultaneously.
 264
 265   If WaitEvent is NULL, execution is in blocking mode. The BSP waits until all
 266   APs finish or TimeoutInMicroseconds expires. Otherwise, execution is in non-blocking
 267   mode, and the BSP returns from this service without waiting for APs. If a
 268   non-blocking mode is requested after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT
 269   is signaled, then EFI_UNSUPPORTED must be returned.
 270
 271   If the timeout specified by TimeoutInMicroseconds expires before all APs return
 272   from Procedure, then Procedure on the failed APs is terminated. All enabled APs
 273   are always available for further calls to EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
 274   and EFI_MP_SERVICES_PROTOCOL.StartupThisAP(). If FailedCpuList is not NULL, its
 275   content points to the list of processor handle numbers in which Procedure was
 276   terminated.
 277
 278   Note: It is the responsibility of the consumer of the EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
 279   to make sure that the nature of the code that is executed on the BSP and the
 280   dispatched APs is well controlled. The MP Services Protocol does not guarantee
 281   that the Procedure function is MP-safe. Hence, the tasks that can be run in
 282   parallel are limited to certain independent tasks and well-controlled exclusive
 283   code. EFI services and protocols may not be called by APs unless otherwise
 284   specified.
 285
 286   In blocking execution mode, BSP waits until all APs finish or
 287   TimeoutInMicroseconds expires.
 288
 289   In non-blocking execution mode, BSP is freed to return to the caller and then
 290   proceed to the next task without having to wait for APs. The following
 291   sequence needs to occur in a non-blocking execution mode:
 292
 293     -# The caller that intends to use this MP Services Protocol in non-blocking
 294        mode creates WaitEvent by calling the EFI CreateEvent() service.  The caller
 295        invokes EFI_MP_SERVICES_PROTOCOL.StartupAllAPs(). If the parameter WaitEvent
 296        is not NULL, then StartupAllAPs() executes in non-blocking mode. It requests
 297        the function specified by Procedure to be started on all the enabled APs,
 298        and releases the BSP to continue with other tasks.
 299     -# The caller can use the CheckEvent() and WaitForEvent() services to check
 300        the state of the WaitEvent created in step 1.
 301     -# When the APs complete their task or TimeoutInMicroSecondss expires, the MP
 302        Service signals WaitEvent by calling the EFI SignalEvent() function. If
 303        FailedCpuList is not NULL, its content is available when WaitEvent is
 304        signaled. If all APs returned from Procedure prior to the timeout, then
 305        FailedCpuList is set to NULL. If not all APs return from Procedure before
 306        the timeout, then FailedCpuList is filled in with the list of the failed
 307        APs. The buffer is allocated by MP Service Protocol using AllocatePool().
 308        It is the caller's responsibility to free the buffer with FreePool() service.
 309     -# This invocation of SignalEvent() function informs the caller that invoked
 310        EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() that either all the APs completed
 311        the specified task or a timeout occurred. The contents of FailedCpuList
 312        can be examined to determine which APs did not complete the specified task
 313        prior to the timeout.
 314
 315   @param[in]  This                    A pointer to the EFI_MP_SERVICES_PROTOCOL
 316                                       instance.
 317   @param[in]  Procedure               A pointer to the function to be run on
 318                                       enabled APs of the system. See type
 319                                       EFI_AP_PROCEDURE.
 320   @param[in]  SingleThread            If TRUE, then all the enabled APs execute
 321                                       the function specified by Procedure one by
 322                                       one, in ascending order of processor handle
 323                                       number.  If FALSE, then all the enabled APs
 324                                       execute the function specified by Procedure
 325                                       simultaneously.
 326   @param[in]  WaitEvent               The event created by the caller with CreateEvent()
 327                                       service.  If it is NULL, then execute in
 328                                       blocking mode. BSP waits until all APs finish
 329                                       or TimeoutInMicroseconds expires.  If it's
 330                                       not NULL, then execute in non-blocking mode.
 331                                       BSP requests the function specified by
 332                                       Procedure to be started on all the enabled
 333                                       APs, and go on executing immediately. If
 334                                       all return from Procedure, or TimeoutInMicroseconds
 335                                       expires, this event is signaled. The BSP
 336                                       can use the CheckEvent() or WaitForEvent()
 337                                       services to check the state of event.  Type
 338                                       EFI_EVENT is defined in CreateEvent() in
 339                                       the Unified Extensible Firmware Interface
 340                                       Specification.
 341   @param[in]  TimeoutInMicrosecsond   Indicates the time limit in microseconds for
 342                                       APs to return from Procedure, either for
 343                                       blocking or non-blocking mode. Zero means
 344                                       infinity.  If the timeout expires before
 345                                       all APs return from Procedure, then Procedure
 346                                       on the failed APs is terminated. All enabled
 347                                       APs are available for next function assigned
 348                                       by EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
 349                                       or EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 350                                       If the timeout expires in blocking mode,
 351                                       BSP returns EFI_TIMEOUT.  If the timeout
 352                                       expires in non-blocking mode, WaitEvent
 353                                       is signaled with SignalEvent().
 354   @param[in]  ProcedureArgument       The parameter passed into Procedure for
 355                                       all APs.
 356   @param[out] FailedCpuList           If NULL, this parameter is ignored. Otherwise,
 357                                       if all APs finish successfully, then its
 358                                       content is set to NULL. If not all APs
 359                                       finish before timeout expires, then its
 360                                       content is set to address of the buffer
 361                                       holding handle numbers of the failed APs.
 362                                       The buffer is allocated by MP Service Protocol,
 363                                       and it's the caller's responsibility to
 364                                       free the buffer with FreePool() service.
 365                                       In blocking mode, it is ready for consumption
 366                                       when the call returns. In non-blocking mode,
 367                                       it is ready when WaitEvent is signaled.  The
 368                                       list of failed CPU is terminated by
 369                                       END_OF_CPU_LIST.
 370
 371   @retval EFI_SUCCESS             In blocking mode, all APs have finished before
 372                                   the timeout expired.
 373   @retval EFI_SUCCESS             In non-blocking mode, function has been dispatched
 374                                   to all enabled APs.
 375   @retval EFI_UNSUPPORTED         A non-blocking mode request was made after the
 376                                   UEFI event EFI_EVENT_GROUP_READY_TO_BOOT was
 377                                   signaled.
 378   @retval EFI_DEVICE_ERROR        Caller processor is AP.
 379   @retval EFI_NOT_STARTED         No enabled APs exist in the system.
 380   @retval EFI_NOT_READY           Any enabled APs are busy.
 381   @retval EFI_TIMEOUT             In blocking mode, the timeout expired before
 382                                   all enabled APs have finished.
 383   @retval EFI_INVALID_PARAMETER   Procedure is NULL.
 384
 385 **/
 386 EFI_STATUS
 387 EFIAPI
 388 CpuMpServicesStartupAllAps (
 389   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 390   IN  EFI_AP_PROCEDURE          Procedure,
 391   IN  BOOLEAN                   SingleThread,
 392   IN  EFI_EVENT                 WaitEvent               OPTIONAL,
 393   IN  UINTN                     TimeoutInMicroseconds,
 394   IN  VOID                      *ProcedureArgument      OPTIONAL,
 395   OUT UINTN                     **FailedCpuList         OPTIONAL
 396   )
 397 {
 398   EFI_STATUS            Status;
 399   PROCESSOR_DATA_BLOCK  *ProcessorData;
 400   UINTN                 Number;
 401   UINTN                 NextNumber;
 402   PROCESSOR_STATE       APInitialState;
 403   PROCESSOR_STATE       ProcessorState;
 404   UINTN                 Timeout;
 405
 406
 407   if (!IsBSP ()) {
 408     return EFI_DEVICE_ERROR;
 409   }
 410
 411   if (gMPSystem.NumberOfProcessors == 1) {
 412     return EFI_NOT_STARTED;
 413   }
 414
 415   if (Procedure == NULL) {
 416     return EFI_INVALID_PARAMETER;
 417   }
 418
 419   if ((WaitEvent != NULL)  && gReadToBoot) {
 420     return EFI_UNSUPPORTED;
 421   }
 422
 423   for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
 424     ProcessorData = &gMPSystem.ProcessorData[Number];
 425     if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
 426       // Skip BSP
 427       continue;
 428     }
 429
 430     if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 431       // Skip Disabled processors
 432       continue;
 433     }
 434     gThread->MutexLock(ProcessorData->StateLock);
 435     if (ProcessorData->State != CPU_STATE_IDLE) {
 436       gThread->MutexUnlock (ProcessorData->StateLock);
 437       return EFI_NOT_READY;
 438     }
 439     gThread->MutexUnlock(ProcessorData->StateLock);
 440   }
 441
 442   if (FailedCpuList != NULL) {
 443     gMPSystem.FailedList = AllocatePool ((gMPSystem.NumberOfProcessors + 1) * sizeof (UINTN));
 444     if (gMPSystem.FailedList == NULL) {
 445       return EFI_OUT_OF_RESOURCES;
 446     }
 447     SetMemN (gMPSystem.FailedList, (gMPSystem.NumberOfProcessors + 1) * sizeof (UINTN), END_OF_CPU_LIST);
 448     gMPSystem.FailedListIndex = 0;
 449     *FailedCpuList = gMPSystem.FailedList;
 450   }
 451
 452   Timeout = TimeoutInMicroseconds;
 453
 454   ProcessorData               = NULL;
 455
 456   gMPSystem.FinishCount   = 0;
 457   gMPSystem.StartCount    = 0;
 458   gMPSystem.SingleThread  = SingleThread;
 459   APInitialState          = CPU_STATE_READY;
 460
 461   for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
 462     ProcessorData = &gMPSystem.ProcessorData[Number];
 463
 464     if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
 465       // Skip BSP
 466       continue;
 467     }
 468
 469     if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 470       // Skip Disabled processors
 471       gMPSystem.FailedList[gMPSystem.FailedListIndex++] = Number;
 472       continue;
 473     }
 474
 475     //
 476     // Get APs prepared, and put failing APs into FailedCpuList
 477     // if "SingleThread", only 1 AP will put to ready state, other AP will be put to ready
 478     // state 1 by 1, until the previous 1 finished its task
 479     // if not "SingleThread", all APs are put to ready state from the beginning
 480     //
 481     gThread->MutexLock(ProcessorData->StateLock);
 482     ASSERT (ProcessorData->State == CPU_STATE_IDLE);
 483     ProcessorData->State = APInitialState;
 484     gThread->MutexUnlock (ProcessorData->StateLock);
 485
 486     gMPSystem.StartCount++;
 487     if (SingleThread) {
 488       APInitialState = CPU_STATE_BLOCKED;
 489     }
 490   }
 491
 492   if (WaitEvent != NULL) {
 493     for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
 494       ProcessorData = &gMPSystem.ProcessorData[Number];
 495       if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
 496        // Skip BSP
 497         continue;
 498       }
 499
 500       if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 501         // Skip Disabled processors
 502         continue;
 503       }
 504
 505       gThread->MutexLock (ProcessorData->StateLock);
 506       ProcessorState = ProcessorData->State;
 507       gThread->MutexUnlock (ProcessorData->StateLock);
 508
 509       if (ProcessorState == CPU_STATE_READY) {
 510         SetApProcedure (ProcessorData, Procedure, ProcedureArgument);
 511       }
 512     }
 513
 514     //
 515     // Save data into private data structure, and create timer to poll AP state before exiting
 516     //
 517     gMPSystem.Procedure         = Procedure;
 518     gMPSystem.ProcedureArgument = ProcedureArgument;
 519     gMPSystem.WaitEvent         = WaitEvent;
 520     gMPSystem.Timeout           = TimeoutInMicroseconds;
 521     gMPSystem.TimeoutActive     = (BOOLEAN)(TimeoutInMicroseconds != 0);
 522     Status = gBS->SetTimer (
 523                     gMPSystem.CheckAllAPsEvent,
 524                     TimerPeriodic,
 525                     gPollInterval
 526                     );
 527     return Status;
 528
 529   }
 530
 531   while (TRUE) {
 532     for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
 533       ProcessorData = &gMPSystem.ProcessorData[Number];
 534       if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
 535        // Skip BSP
 536         continue;
 537       }
 538
 539       if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 540         // Skip Disabled processors
 541         continue;
 542       }
 543
 544       gThread->MutexLock (ProcessorData->StateLock);
 545       ProcessorState = ProcessorData->State;
 546       gThread->MutexUnlock (ProcessorData->StateLock);
 547
 548       switch (ProcessorState) {
 549       case CPU_STATE_READY:
 550         SetApProcedure (ProcessorData, Procedure, ProcedureArgument);
 551         break;
 552
 553       case CPU_STATE_FINISHED:
 554         gMPSystem.FinishCount++;
 555         if (SingleThread) {
 556           Status = GetNextBlockedNumber (&NextNumber);
 557           if (!EFI_ERROR (Status)) {
 558             gThread->MutexLock (gMPSystem.ProcessorData[NextNumber].StateLock);
 559             gMPSystem.ProcessorData[NextNumber].State = CPU_STATE_READY;
 560             gThread->MutexUnlock (gMPSystem.ProcessorData[NextNumber].StateLock);
 561           }
 562         }
 563
 564         gThread->MutexLock (ProcessorData->StateLock);
 565         ProcessorData->State = CPU_STATE_IDLE;
 566         gThread->MutexUnlock (ProcessorData->StateLock);
 567
 568         break;
 569
 570       default:
 571         break;
 572       }
 573     }
 574
 575     if (gMPSystem.FinishCount == gMPSystem.StartCount) {
 576       Status = EFI_SUCCESS;
 577       goto Done;
 578     }
 579
 580     if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
 581       Status = EFI_TIMEOUT;
 582       goto Done;
 583     }
 584
 585     Timeout -= CalculateAndStallInterval (Timeout);
 586   }
 587
 588 Done:
 589   if (FailedCpuList != NULL) {
 590     if (gMPSystem.FailedListIndex == 0) {
 591       FreePool (*FailedCpuList);
 592       *FailedCpuList = NULL;
 593     }
 594   }
 595
 596   return EFI_SUCCESS;
 597 }
 598
 599
 600 /**
 601   This service lets the caller get one enabled AP to execute a caller-provided
 602   function. The caller can request the BSP to either wait for the completion
 603   of the AP or just proceed with the next task by using the EFI event mechanism.
 604   See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blocking
 605   execution support.  This service may only be called from the BSP.
 606
 607   This function is used to dispatch one enabled AP to the function specified by
 608   Procedure passing in the argument specified by ProcedureArgument.  If WaitEvent
 609   is NULL, execution is in blocking mode. The BSP waits until the AP finishes or
 610   TimeoutInMicroSecondss expires. Otherwise, execution is in non-blocking mode.
 611   BSP proceeds to the next task without waiting for the AP. If a non-blocking mode
 612   is requested after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled,
 613   then EFI_UNSUPPORTED must be returned.
 614
 615   If the timeout specified by TimeoutInMicroseconds expires before the AP returns
 616   from Procedure, then execution of Procedure by the AP is terminated. The AP is
 617   available for subsequent calls to EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and
 618   EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 619
 620   @param[in]  This                    A pointer to the EFI_MP_SERVICES_PROTOCOL
 621                                       instance.
 622   @param[in]  Procedure               A pointer to the function to be run on
 623                                       enabled APs of the system. See type
 624                                       EFI_AP_PROCEDURE.
 625   @param[in]  ProcessorNumber         The handle number of the AP. The range is
 626                                       from 0 to the total number of logical
 627                                       processors minus 1. The total number of
 628                                       logical processors can be retrieved by
 629                                       EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 630   @param[in]  WaitEvent               The event created by the caller with CreateEvent()
 631                                       service.  If it is NULL, then execute in
 632                                       blocking mode. BSP waits until all APs finish
 633                                       or TimeoutInMicroseconds expires.  If it's
 634                                       not NULL, then execute in non-blocking mode.
 635                                       BSP requests the function specified by
 636                                       Procedure to be started on all the enabled
 637                                       APs, and go on executing immediately. If
 638                                       all return from Procedure or TimeoutInMicroseconds
 639                                       expires, this event is signaled. The BSP
 640                                       can use the CheckEvent() or WaitForEvent()
 641                                       services to check the state of event.  Type
 642                                       EFI_EVENT is defined in CreateEvent() in
 643                                       the Unified Extensible Firmware Interface
 644                                       Specification.
 645   @param[in]  TimeoutInMicrosecsond   Indicates the time limit in microseconds for
 646                                       APs to return from Procedure, either for
 647                                       blocking or non-blocking mode. Zero means
 648                                       infinity.  If the timeout expires before
 649                                       all APs return from Procedure, then Procedure
 650                                       on the failed APs is terminated. All enabled
 651                                       APs are available for next function assigned
 652                                       by EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
 653                                       or EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 654                                       If the timeout expires in blocking mode,
 655                                       BSP returns EFI_TIMEOUT.  If the timeout
 656                                       expires in non-blocking mode, WaitEvent
 657                                       is signaled with SignalEvent().
 658   @param[in]  ProcedureArgument       The parameter passed into Procedure for
 659                                       all APs.
 660   @param[out] Finished                If NULL, this parameter is ignored.  In
 661                                       blocking mode, this parameter is ignored.
 662                                       In non-blocking mode, if AP returns from
 663                                       Procedure before the timeout expires, its
 664                                       content is set to TRUE. Otherwise, the
 665                                       value is set to FALSE. The caller can
 666                                       determine if the AP returned from Procedure
 667                                       by evaluating this value.
 668
 669   @retval EFI_SUCCESS             In blocking mode, specified AP finished before
 670                                   the timeout expires.
 671   @retval EFI_SUCCESS             In non-blocking mode, the function has been
 672                                   dispatched to specified AP.
 673   @retval EFI_UNSUPPORTED         A non-blocking mode request was made after the
 674                                   UEFI event EFI_EVENT_GROUP_READY_TO_BOOT was
 675                                   signaled.
 676   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 677   @retval EFI_TIMEOUT             In blocking mode, the timeout expired before
 678                                   the specified AP has finished.
 679   @retval EFI_NOT_READY           The specified AP is busy.
 680   @retval EFI_NOT_FOUND           The processor with the handle specified by
 681                                   ProcessorNumber does not exist.
 682   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or disabled AP.
 683   @retval EFI_INVALID_PARAMETER   Procedure is NULL.
 684
 685 **/
 686 EFI_STATUS
 687 EFIAPI
 688 CpuMpServicesStartupThisAP (
 689   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 690   IN  EFI_AP_PROCEDURE          Procedure,
 691   IN  UINTN                     ProcessorNumber,
 692   IN  EFI_EVENT                 WaitEvent               OPTIONAL,
 693   IN  UINTN                     TimeoutInMicroseconds,
 694   IN  VOID                      *ProcedureArgument      OPTIONAL,
 695   OUT BOOLEAN                   *Finished               OPTIONAL
 696   )
 697 {
 698   UINTN            Timeout;
 699
 700   if (!IsBSP ()) {
 701     return EFI_DEVICE_ERROR;
 702   }
 703
 704   if (Procedure == NULL) {
 705     return EFI_INVALID_PARAMETER;
 706   }
 707
 708   if (ProcessorNumber >= gMPSystem.NumberOfProcessors) {
 709     return EFI_NOT_FOUND;
 710   }
 711
 712   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 713     return EFI_INVALID_PARAMETER;
 714   }
 715
 716   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 717     return EFI_INVALID_PARAMETER;
 718   }
 719
 720   gThread->MutexLock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 721   if (gMPSystem.ProcessorData[ProcessorNumber].State != CPU_STATE_IDLE) {
 722     gThread->MutexUnlock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 723     return EFI_NOT_READY;
 724   }
 725   gThread->MutexUnlock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 726
 727   if ((WaitEvent != NULL)  && gReadToBoot) {
 728     return EFI_UNSUPPORTED;
 729   }
 730
 731   Timeout = TimeoutInMicroseconds;
 732
 733   gMPSystem.StartCount   = 1;
 734   gMPSystem.FinishCount  = 0;
 735
 736   SetApProcedure (&gMPSystem.ProcessorData[ProcessorNumber], Procedure, ProcedureArgument);
 737
 738   if (WaitEvent != NULL) {
 739     // Non Blocking
 740     gMPSystem.WaitEvent = WaitEvent;
 741     gBS->SetTimer (
 742            gMPSystem.ProcessorData[ProcessorNumber].CheckThisAPEvent,
 743            TimerPeriodic,
 744            gPollInterval
 745            );
 746     return EFI_SUCCESS;
 747   }
 748
 749   // Blocking
 750   while (TRUE) {
 751     gThread->MutexLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 752     if (gMPSystem.ProcessorData[ProcessorNumber].State == CPU_STATE_FINISHED) {
 753       gMPSystem.ProcessorData[ProcessorNumber].State = CPU_STATE_IDLE;
 754       gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 755       break;
 756     }
 757
 758     gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 759
 760     if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
 761       return EFI_TIMEOUT;
 762     }
 763
 764     Timeout -= CalculateAndStallInterval (Timeout);
 765   }
 766
 767   return EFI_SUCCESS;
 768
 769 }
 770
 771
 772 /**
 773   This service switches the requested AP to be the BSP from that point onward.
 774   This service changes the BSP for all purposes.   This call can only be performed
 775   by the current BSP.
 776
 777   This service switches the requested AP to be the BSP from that point onward.
 778   This service changes the BSP for all purposes. The new BSP can take over the
 779   execution of the old BSP and continue seamlessly from where the old one left
 780   off. This service may not be supported after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT
 781   is signaled.
 782
 783   If the BSP cannot be switched prior to the return from this service, then
 784   EFI_UNSUPPORTED must be returned.
 785
 786   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 787   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 788                                BSP. The range is from 0 to the total number of
 789                                logical processors minus 1. The total number of
 790                                logical processors can be retrieved by
 791                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 792   @param[in] EnableOldBSP      If TRUE, then the old BSP will be listed as an
 793                                enabled AP. Otherwise, it will be disabled.
 794
 795   @retval EFI_SUCCESS             BSP successfully switched.
 796   @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed prior to
 797                                   this service returning.
 798   @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
 799   @retval EFI_SUCCESS             The calling processor is an AP.
 800   @retval EFI_NOT_FOUND           The processor with the handle specified by
 801                                   ProcessorNumber does not exist.
 802   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BSP or
 803                                   a disabled AP.
 804   @retval EFI_NOT_READY           The specified AP is busy.
 805
 806 **/
 807 EFI_STATUS
 808 EFIAPI
 809 CpuMpServicesSwitchBSP (
 810   IN EFI_MP_SERVICES_PROTOCOL  *This,
 811   IN  UINTN                    ProcessorNumber,
 812   IN  BOOLEAN                  EnableOldBSP
 813   )
 814 {
 815   UINTN   Index;
 816
 817   if (!IsBSP ()) {
 818     return EFI_DEVICE_ERROR;
 819   }
 820
 821   if (ProcessorNumber >= gMPSystem.NumberOfProcessors) {
 822     return EFI_NOT_FOUND;
 823   }
 824
 825   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 826     return EFI_INVALID_PARAMETER;
 827   }
 828
 829   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 830     return EFI_INVALID_PARAMETER;
 831   }
 832
 833   for (Index = 0; Index < gMPSystem.NumberOfProcessors; Index++) {
 834     if ((gMPSystem.ProcessorData[Index].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 835       break;
 836     }
 837   }
 838   ASSERT (Index != gMPSystem.NumberOfProcessors);
 839
 840   gThread->MutexLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 841   if (gMPSystem.ProcessorData[ProcessorNumber].State != CPU_STATE_IDLE) {
 842     gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 843     return EFI_NOT_READY;
 844   }
 845   gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 846
 847   // Skip for now as we need switch a bunch of stack stuff around and it's complex
 848   // May not be worth it?
 849   return EFI_NOT_READY;
 850 }
 851
 852
 853 /**
 854   This service lets the caller enable or disable an AP from this point onward.
 855   This service may only be called from the BSP.
 856
 857   This service allows the caller enable or disable an AP from this point onward.
 858   The caller can optionally specify the health status of the AP by Health. If
 859   an AP is being disabled, then the state of the disabled AP is implementation
 860   dependent. If an AP is enabled, then the implementation must guarantee that a
 861   complete initialization sequence is performed on the AP, so the AP is in a state
 862   that is compatible with an MP operating system. This service may not be supported
 863   after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled.
 864
 865   If the enable or disable AP operation cannot be completed prior to the return
 866   from this service, then EFI_UNSUPPORTED must be returned.
 867
 868   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 869   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 870                                BSP. The range is from 0 to the total number of
 871                                logical processors minus 1. The total number of
 872                                logical processors can be retrieved by
 873                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 874   @param[in] EnableAP          Specifies the new state for the processor for
 875                                enabled, FALSE for disabled.
 876   @param[in] HealthFlag        If not NULL, a pointer to a value that specifies
 877                                the new health status of the AP. This flag
 878                                corresponds to StatusFlag defined in
 879                                EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo(). Only
 880                                the PROCESSOR_HEALTH_STATUS_BIT is used. All other
 881                                bits are ignored.  If it is NULL, this parameter
 882                                is ignored.
 883
 884   @retval EFI_SUCCESS             The specified AP was enabled or disabled successfully.
 885   @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be completed
 886                                   prior to this service returning.
 887   @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not supported.
 888   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 889   @retval EFI_NOT_FOUND           Processor with the handle specified by ProcessorNumber
 890                                   does not exist.
 891   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.
 892
 893 **/
 894 EFI_STATUS
 895 EFIAPI
 896 CpuMpServicesEnableDisableAP (
 897   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 898   IN  UINTN                     ProcessorNumber,
 899   IN  BOOLEAN                   EnableAP,
 900   IN  UINT32                    *HealthFlag OPTIONAL
 901   )
 902 {
 903   if (!IsBSP ()) {
 904     return EFI_DEVICE_ERROR;
 905   }
 906
 907   if (ProcessorNumber >= gMPSystem.NumberOfProcessors) {
 908     return EFI_NOT_FOUND;
 909   }
 910
 911   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 912     return EFI_INVALID_PARAMETER;
 913   }
 914
 915   gThread->MutexLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 916   if (gMPSystem.ProcessorData[ProcessorNumber].State != CPU_STATE_IDLE) {
 917     gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 918     return EFI_UNSUPPORTED;
 919   }
 920   gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 921
 922   if (EnableAP) {
 923     if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0 ) {
 924       gMPSystem.NumberOfEnabledProcessors++;
 925     }
 926     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag |= PROCESSOR_ENABLED_BIT;
 927   } else {
 928     if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == PROCESSOR_ENABLED_BIT ) {
 929       gMPSystem.NumberOfEnabledProcessors--;
 930     }
 931     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag &= ~PROCESSOR_ENABLED_BIT;
 932   }
 933
 934   if (HealthFlag != NULL) {
 935     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag &= ~PROCESSOR_HEALTH_STATUS_BIT;
 936     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag |= (*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT);
 937   }
 938
 939   return EFI_SUCCESS;
 940 }
 941
 942
 943 /**
 944   This return the handle number for the calling processor.  This service may be
 945   called from the BSP and APs.
 946
 947   This service returns the processor handle number for the calling processor.
 948   The returned value is in the range from 0 to the total number of logical
 949   processors minus 1. The total number of logical processors can be retrieved
 950   with EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors(). This service may be
 951   called from the BSP and APs. If ProcessorNumber is NULL, then EFI_INVALID_PARAMETER
 952   is returned. Otherwise, the current processors handle number is returned in
 953   ProcessorNumber, and EFI_SUCCESS is returned.
 954
 955   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 956   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 957                                BSP. The range is from 0 to the total number of
 958                                logical processors minus 1. The total number of
 959                                logical processors can be retrieved by
 960                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 961
 962   @retval EFI_SUCCESS             The current processor handle number was returned
 963                                   in ProcessorNumber.
 964   @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.
 965
 966 **/
 967 EFI_STATUS
 968 EFIAPI
 969 CpuMpServicesWhoAmI (
 970   IN EFI_MP_SERVICES_PROTOCOL  *This,
 971   OUT UINTN                    *ProcessorNumber
 972   )
 973 {
 974   UINTN   Index;
 975   UINT64  ProcessorId;
 976
 977   if (ProcessorNumber == NULL) {
 978     return EFI_INVALID_PARAMETER;
 979   }
 980
 981   ProcessorId = gThread->Self ();
 982   for (Index = 0; Index < gMPSystem.NumberOfProcessors; Index++) {
 983     if (gMPSystem.ProcessorData[Index].Info.ProcessorId == ProcessorId) {
 984       break;
 985     }
 986   }
 987
 988   *ProcessorNumber = Index;
 989   return EFI_SUCCESS;
 990 }
 991
 992
 993
 994 EFI_MP_SERVICES_PROTOCOL  mMpServicesTemplate = {
 995   CpuMpServicesGetNumberOfProcessors,
 996   CpuMpServicesGetProcessorInfo,
 997   CpuMpServicesStartupAllAps,
 998   CpuMpServicesStartupThisAP,
 999   CpuMpServicesSwitchBSP,
1000   CpuMpServicesEnableDisableAP,
1001   CpuMpServicesWhoAmI
1002 };
1003
1004
1005
1006 /*++
1007   If timeout occurs in StartupAllAps(), a timer is set, which invokes this
1008   procedure periodically to check whether all APs have finished.
1009
1010
1011 --*/
1012 VOID
1013 EFIAPI
1014 CpuCheckAllAPsStatus (
1015   IN  EFI_EVENT   Event,
1016   IN  VOID        *Context
1017   )
1018 {
1019   UINTN                 ProcessorNumber;
1020   UINTN                 NextNumber;
1021   PROCESSOR_DATA_BLOCK  *ProcessorData;
1022   PROCESSOR_DATA_BLOCK  *NextData;
1023   EFI_STATUS            Status;
1024   PROCESSOR_STATE       ProcessorState;
1025   UINTN                 Cpu;
1026   BOOLEAN               Found;
1027
1028   if (gMPSystem.TimeoutActive) {
1029     gMPSystem.Timeout -= CalculateAndStallInterval (gMPSystem.Timeout);
1030   }
1031
1032   for (ProcessorNumber = 0; ProcessorNumber < gMPSystem.NumberOfProcessors; ProcessorNumber++) {
1033     ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];
1034     if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
1035      // Skip BSP
1036       continue;
1037     }
1038
1039     if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
1040       // Skip Disabled processors
1041       continue;
1042     }
1043
1044     // This is an Interrupt Service routine.
1045     // This can grab a lock that is held in a non-interrupt
1046     // context. Meaning deadlock. Which is a bad thing.
1047     // So, try lock it. If we can get it, cool, do our thing.
1048     // otherwise, just dump out & try again on the next iteration.
1049     Status = gThread->MutexTryLock (ProcessorData->StateLock);
1050     if (EFI_ERROR(Status)) {
1051       return;
1052     }
1053     ProcessorState = ProcessorData->State;
1054     gThread->MutexUnlock (ProcessorData->StateLock);
1055
1056     switch (ProcessorState) {
1057     case CPU_STATE_FINISHED:
1058       if (gMPSystem.SingleThread) {
1059         Status = GetNextBlockedNumber (&NextNumber);
1060         if (!EFI_ERROR (Status)) {
1061           NextData = &gMPSystem.ProcessorData[NextNumber];
1062
1063           gThread->MutexLock (NextData->StateLock);
1064           NextData->State = CPU_STATE_READY;
1065           gThread->MutexUnlock (NextData->StateLock);
1066
1067           SetApProcedure (NextData, gMPSystem.Procedure, gMPSystem.ProcedureArgument);
1068         }
1069       }
1070
1071       gThread->MutexLock (ProcessorData->StateLock);
1072       ProcessorData->State = CPU_STATE_IDLE;
1073       gThread->MutexUnlock (ProcessorData->StateLock);
1074       gMPSystem.FinishCount++;
1075       break;
1076
1077     default:
1078       break;
1079     }
1080   }
1081
1082   if (gMPSystem.TimeoutActive && gMPSystem.Timeout == 0) {
1083     //
1084     // Timeout
1085     //
1086     if (gMPSystem.FailedList != NULL) {
1087       for (ProcessorNumber = 0; ProcessorNumber < gMPSystem.NumberOfProcessors; ProcessorNumber++) {
1088         ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];
1089         if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
1090          // Skip BSP
1091           continue;
1092         }
1093
1094         if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
1095           // Skip Disabled processors
1096           continue;
1097         }
1098
1099         // Mark the
1100         Status = gThread->MutexTryLock (ProcessorData->StateLock);
1101         if (EFI_ERROR(Status)) {
1102           return;
1103         }
1104         ProcessorState = ProcessorData->State;
1105         gThread->MutexUnlock (ProcessorData->StateLock);
1106
1107         if (ProcessorState != CPU_STATE_IDLE) {
1108           // If we are retrying make sure we don't double count
1109           for (Cpu = 0, Found = FALSE; Cpu < gMPSystem.NumberOfProcessors; Cpu++) {
1110             if (gMPSystem.FailedList[Cpu] == END_OF_CPU_LIST) {
1111               break;
1112             }
1113             if (gMPSystem.FailedList[ProcessorNumber] == Cpu) {
1114               Found = TRUE;
1115               break;
1116             }
1117           }
1118           if (!Found) {
1119             gMPSystem.FailedList[gMPSystem.FailedListIndex++] = Cpu;
1120           }
1121         }
1122       }
1123     }
1124     // Force terminal exit
1125     gMPSystem.FinishCount = gMPSystem.StartCount;
1126   }
1127
1128   if (gMPSystem.FinishCount != gMPSystem.StartCount) {
1129     return;
1130   }
1131
1132   gBS->SetTimer (
1133          gMPSystem.CheckAllAPsEvent,
1134          TimerCancel,
1135          0
1136          );
1137
1138   if (gMPSystem.FailedListIndex == 0) {
1139     if (gMPSystem.FailedList != NULL) {
1140       FreePool (gMPSystem.FailedList);
1141       gMPSystem.FailedList = NULL;
1142     }
1143   }
1144
1145   Status = gBS->SignalEvent (gMPSystem.WaitEvent);
1146
1147   return ;
1148 }
1149
1150 VOID
1151 EFIAPI
1152 CpuCheckThisAPStatus (
1153   IN  EFI_EVENT                           Event,
1154   IN  VOID                                *Context
1155   )
1156 {
1157   EFI_STATUS            Status;
1158   PROCESSOR_DATA_BLOCK  *ProcessorData;
1159   PROCESSOR_STATE       ProcessorState;
1160
1161   ProcessorData = (PROCESSOR_DATA_BLOCK *) Context;
1162
1163   //
1164   // This is an Interrupt Service routine.
1165   // that can grab a lock that is held in a non-interrupt
1166   // context. Meaning deadlock. Which is a badddd thing.
1167   // So, try lock it. If we can get it, cool, do our thing.
1168   // otherwise, just dump out & try again on the next iteration.
1169   //
1170   Status = gThread->MutexTryLock (ProcessorData->StateLock);
1171   if (EFI_ERROR(Status)) {
1172     return;
1173   }
1174   ProcessorState = ProcessorData->State;
1175   gThread->MutexUnlock (ProcessorData->StateLock);
1176
1177   if (ProcessorState == CPU_STATE_FINISHED) {
1178     Status = gBS->SetTimer (ProcessorData->CheckThisAPEvent, TimerCancel, 0);
1179     ASSERT_EFI_ERROR (Status);
1180
1181     Status = gBS->SignalEvent (gMPSystem.WaitEvent);
1182     ASSERT_EFI_ERROR (Status);
1183
1184     gThread->MutexLock (ProcessorData->StateLock);
1185     ProcessorData->State = CPU_STATE_IDLE;
1186     gThread->MutexUnlock (ProcessorData->StateLock);
1187   }
1188
1189   return ;
1190 }
1191
1192
1193 /*++
1194   This function is called by all processors (both BSP and AP) once and collects MP related data
1195
1196   MPSystemData  - Pointer to the data structure containing MP related data
1197   BSP           - TRUE if the CPU is BSP
1198
1199   EFI_SUCCESS   - Data for the processor collected and filled in
1200
1201 --*/
1202 EFI_STATUS
1203 FillInProcessorInformation (
1204   IN     BOOLEAN              BSP,
1205   IN     UINTN                ProcessorNumber
1206   )
1207 {
1208   gMPSystem.ProcessorData[ProcessorNumber].Info.ProcessorId  = gThread->Self ();
1209   gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag   = PROCESSOR_ENABLED_BIT | PROCESSOR_HEALTH_STATUS_BIT;
1210   if (BSP) {
1211     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag |= PROCESSOR_AS_BSP_BIT;
1212   }
1213
1214   gMPSystem.ProcessorData[ProcessorNumber].Info.Location.Package = (UINT32) ProcessorNumber;
1215   gMPSystem.ProcessorData[ProcessorNumber].Info.Location.Core    = 0;
1216   gMPSystem.ProcessorData[ProcessorNumber].Info.Location.Thread  = 0;
1217   gMPSystem.ProcessorData[ProcessorNumber].State = BSP ? CPU_STATE_BUSY : CPU_STATE_IDLE;
1218
1219   gMPSystem.ProcessorData[ProcessorNumber].Procedure        = NULL;
1220   gMPSystem.ProcessorData[ProcessorNumber].Parameter        = NULL;
1221   gMPSystem.ProcessorData[ProcessorNumber].StateLock        = gThread->MutexInit ();
1222   gMPSystem.ProcessorData[ProcessorNumber].ProcedureLock    = gThread->MutexInit ();
1223
1224   return EFI_SUCCESS;
1225 }
1226
1227 VOID *
1228 EFIAPI
1229 CpuDriverApIdolLoop (
1230   VOID  *Context
1231   )
1232 {
1233   EFI_AP_PROCEDURE      Procedure;
1234   VOID                  *Parameter;
1235   UINTN                 ProcessorNumber;
1236   PROCESSOR_DATA_BLOCK  *ProcessorData;
1237
1238   ProcessorNumber = (UINTN)Context;
1239   ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];
1240
1241   ProcessorData->Info.ProcessorId = gThread->Self ();
1242
1243   while (TRUE) {
1244     //
1245     // Make a local copy on the stack to be extra safe
1246     //
1247     gThread->MutexLock (ProcessorData->ProcedureLock);
1248     Procedure = ProcessorData->Procedure;
1249     Parameter = ProcessorData->Parameter;
1250     gThread->MutexUnlock (ProcessorData->ProcedureLock);
1251
1252     if (Procedure != NULL) {
1253       gThread->MutexLock (ProcessorData->StateLock);
1254       ProcessorData->State = CPU_STATE_BUSY;
1255       gThread->MutexUnlock (ProcessorData->StateLock);
1256
1257       Procedure (Parameter);
1258
1259       gThread->MutexLock (ProcessorData->ProcedureLock);
1260       ProcessorData->Procedure = NULL;
1261       gThread->MutexUnlock (ProcessorData->ProcedureLock);
1262
1263       gThread->MutexLock (ProcessorData->StateLock);
1264       ProcessorData->State = CPU_STATE_FINISHED;
1265       gThread->MutexUnlock (ProcessorData->StateLock);
1266     }
1267
1268     // Poll 5 times a seconds, 200ms
1269     // Don't want to burn too many system resources doing nothing.
1270     gEmuThunk->Sleep (200 * 1000);
1271   }
1272
1273   return 0;
1274 }
1275
1276
1277 EFI_STATUS
1278 InitializeMpSystemData (
1279   IN   UINTN     NumberOfProcessors
1280   )
1281 {
1282   EFI_STATUS              Status;
1283   UINTN                   Index;
1284
1285
1286   //
1287   // Clear the data structure area first.
1288   //
1289   ZeroMem (&gMPSystem, sizeof (MP_SYSTEM_DATA));
1290
1291   //
1292   // First BSP fills and inits all known values, including it's own records.
1293   //
1294   gMPSystem.NumberOfProcessors         = NumberOfProcessors;
1295   gMPSystem.NumberOfEnabledProcessors  = NumberOfProcessors;
1296
1297   gMPSystem.ProcessorData = AllocateZeroPool (gMPSystem.NumberOfProcessors * sizeof (PROCESSOR_DATA_BLOCK));
1298   ASSERT (gMPSystem.ProcessorData != NULL);
1299
1300   FillInProcessorInformation (TRUE, 0);
1301
1302   Status = gBS->CreateEvent (
1303                   EVT_TIMER | EVT_NOTIFY_SIGNAL,
1304                   TPL_CALLBACK,
1305                   CpuCheckAllAPsStatus,
1306                   NULL,
1307                   &gMPSystem.CheckAllAPsEvent
1308                   );
1309   ASSERT_EFI_ERROR (Status);
1310
1311
1312   for (Index = 0; Index < gMPSystem.NumberOfProcessors; Index++) {
1313     if ((gMPSystem.ProcessorData[Index].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
1314      // Skip BSP
1315       continue;
1316     }
1317
1318     FillInProcessorInformation (FALSE, Index);
1319
1320     Status = gThread->CreateThread (
1321                         (VOID *)&gMPSystem.ProcessorData[Index].Info.ProcessorId,
1322                         NULL,
1323                         CpuDriverApIdolLoop,
1324                         (VOID *)Index
1325                         );
1326
1327
1328     Status = gBS->CreateEvent (
1329                          EVT_TIMER | EVT_NOTIFY_SIGNAL,
1330                          TPL_CALLBACK,
1331                          CpuCheckThisAPStatus,
1332                          (VOID *)  &gMPSystem.ProcessorData[Index],
1333                          &gMPSystem.ProcessorData[Index].CheckThisAPEvent
1334                          );
1335   }
1336
1337   return EFI_SUCCESS;
1338 }
1339
1340
1341
1342 /**
1343   Invoke a notification event
1344
1345   @param  Event                 Event whose notification function is being invoked.
1346   @param  Context               The pointer to the notification function's context,
1347                                 which is implementation-dependent.
1348
1349 **/
1350 VOID
1351 EFIAPI
1352 CpuReadToBootFunction (
1353   IN  EFI_EVENT                Event,
1354   IN  VOID                     *Context
1355   )
1356 {
1357   gReadToBoot = TRUE;
1358 }
1359
1360
1361
1362 EFI_STATUS
1363 CpuMpServicesInit (
1364   OUT UINTN *MaxCpus
1365   )
1366 {
1367   EFI_STATUS              Status;
1368   EFI_HANDLE              Handle;
1369   EMU_IO_THUNK_PROTOCOL   *IoThunk;
1370
1371   *MaxCpus = 1; // BSP
1372   IoThunk = GetIoThunkInstance (&gEmuThreadThunkProtocolGuid, 0);
1373   if (IoThunk != NULL) {
1374     Status = IoThunk->Open (IoThunk);
1375     if (!EFI_ERROR (Status)) {
1376       if (IoThunk->ConfigString != NULL) {
1377         *MaxCpus += StrDecimalToUintn (IoThunk->ConfigString);
1378         gThread = IoThunk->Interface;
1379       }
1380     }
1381   }
1382
1383   if (*MaxCpus == 1) {
1384     // We are not MP so nothing to do
1385     return EFI_SUCCESS;
1386   }
1387
1388   gPollInterval = (UINTN) PcdGet64 (PcdEmuMpServicesPollingInterval);
1389
1390   Status  = InitializeMpSystemData (*MaxCpus);
1391   if (EFI_ERROR (Status)) {
1392     return Status;
1393   }
1394
1395   Status = EfiCreateEventReadyToBootEx (TPL_CALLBACK, CpuReadToBootFunction, NULL, &gReadToBootEvent);
1396   ASSERT_EFI_ERROR (Status);
1397
1398   //
1399   // Now install the MP services protocol.
1400   //
1401   Handle = NULL;
1402   Status = gBS->InstallMultipleProtocolInterfaces (
1403                   &Handle,
1404                   &gEfiMpServiceProtocolGuid,   &mMpServicesTemplate,
1405                   NULL
1406                   );
1407   return Status;
1408 }
1409
1410