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