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
 424   if (FailedCpuList != NULL) {
 425     gMPSystem.FailedList = AllocatePool ((gMPSystem.NumberOfProcessors + 1) * sizeof (UINTN));
 426     if (gMPSystem.FailedList == NULL) {
 427       return EFI_OUT_OF_RESOURCES;
 428     }
 429     SetMemN (gMPSystem.FailedList, (gMPSystem.NumberOfProcessors + 1) * sizeof (UINTN), END_OF_CPU_LIST);
 430     gMPSystem.FailedListIndex = 0;
 431     *FailedCpuList = gMPSystem.FailedList;
 432   }
 433
 434   Timeout = TimeoutInMicroseconds;
 435
 436   ProcessorData               = NULL;
 437
 438   gMPSystem.FinishCount   = 0;
 439   gMPSystem.StartCount    = 0;
 440   gMPSystem.SingleThread  = SingleThread;
 441   APInitialState          = CPU_STATE_READY;
 442
 443   for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
 444     ProcessorData = &gMPSystem.ProcessorData[Number];
 445
 446     if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
 447       // Skip BSP
 448       continue;
 449     }
 450
 451     if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 452       // Skip Disabled processors
 453       gMPSystem.FailedList[gMPSystem.FailedListIndex++] = Number;
 454       continue;
 455     }
 456
 457     //
 458     // Get APs prepared, and put failing APs into FailedCpuList
 459     // if "SingleThread", only 1 AP will put to ready state, other AP will be put to ready
 460     // state 1 by 1, until the previous 1 finished its task
 461     // if not "SingleThread", all APs are put to ready state from the beginning
 462     //
 463     gThread->MutexLock(ProcessorData->StateLock);
 464     if (ProcessorData->State == CPU_STATE_IDLE) {
 465       ProcessorData->State = APInitialState;
 466       gThread->MutexUnlock (ProcessorData->StateLock);
 467
 468       gMPSystem.StartCount++;
 469       if (SingleThread) {
 470         APInitialState = CPU_STATE_BLOCKED;
 471       }
 472     } else {
 473       gThread->MutexUnlock (ProcessorData->StateLock);
 474       return EFI_NOT_READY;
 475     }
 476   }
 477
 478   if (WaitEvent != NULL) {
 479     for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
 480       ProcessorData = &gMPSystem.ProcessorData[Number];
 481       if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
 482        // Skip BSP
 483         continue;
 484       }
 485
 486       if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 487         // Skip Disabled processors
 488         continue;
 489       }
 490
 491       gThread->MutexLock (ProcessorData->StateLock);
 492       ProcessorState = ProcessorData->State;
 493       gThread->MutexUnlock (ProcessorData->StateLock);
 494
 495       if (ProcessorState == CPU_STATE_READY) {
 496         SetApProcedure (ProcessorData, Procedure, ProcedureArgument);
 497       }
 498     }
 499
 500     //
 501     // Save data into private data structure, and create timer to poll AP state before exiting
 502     //
 503     gMPSystem.Procedure         = Procedure;
 504     gMPSystem.ProcedureArgument = ProcedureArgument;
 505     gMPSystem.WaitEvent         = WaitEvent;
 506     gMPSystem.Timeout           = TimeoutInMicroseconds;
 507     gMPSystem.TimeoutActive     = (BOOLEAN)(TimeoutInMicroseconds != 0);
 508     Status = gBS->SetTimer (
 509                     gMPSystem.CheckAllAPsEvent,
 510                     TimerPeriodic,
 511                     gPollInterval
 512                     );
 513     return Status;
 514
 515   }
 516
 517   while (TRUE) {
 518     for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {
 519       ProcessorData = &gMPSystem.ProcessorData[Number];
 520       if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
 521        // Skip BSP
 522         continue;
 523       }
 524
 525       if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 526         // Skip Disabled processors
 527         continue;
 528       }
 529
 530       gThread->MutexLock (ProcessorData->StateLock);
 531       ProcessorState = ProcessorData->State;
 532       gThread->MutexUnlock (ProcessorData->StateLock);
 533
 534       switch (ProcessorState) {
 535       case CPU_STATE_READY:
 536         SetApProcedure (ProcessorData, Procedure, ProcedureArgument);
 537         break;
 538
 539       case CPU_STATE_FINISHED:
 540         gMPSystem.FinishCount++;
 541         if (SingleThread) {
 542           Status = GetNextBlockedNumber (&NextNumber);
 543           if (!EFI_ERROR (Status)) {
 544             gThread->MutexLock (gMPSystem.ProcessorData[NextNumber].StateLock);
 545             gMPSystem.ProcessorData[NextNumber].State = CPU_STATE_READY;
 546             gThread->MutexUnlock (gMPSystem.ProcessorData[NextNumber].StateLock);
 547           }
 548         }
 549
 550         gThread->MutexLock (ProcessorData->StateLock);
 551         ProcessorData->State = CPU_STATE_IDLE;
 552         gThread->MutexUnlock (ProcessorData->StateLock);
 553
 554         break;
 555
 556       default:
 557         break;
 558       }
 559     }
 560
 561     if (gMPSystem.FinishCount == gMPSystem.StartCount) {
 562       Status = EFI_SUCCESS;
 563       goto Done;
 564     }
 565
 566     if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
 567       Status = EFI_TIMEOUT;
 568       goto Done;
 569     }
 570
 571     Timeout -= CalculateAndStallInterval (Timeout);
 572   }
 573
 574 Done:
 575   if (FailedCpuList != NULL) {
 576     if (gMPSystem.FailedListIndex == 0) {
 577       FreePool (*FailedCpuList);
 578       *FailedCpuList = NULL;
 579     }
 580   }
 581
 582   return EFI_SUCCESS;
 583 }
 584
 585
 586 /**
 587   This service lets the caller get one enabled AP to execute a caller-provided
 588   function. The caller can request the BSP to either wait for the completion
 589   of the AP or just proceed with the next task by using the EFI event mechanism.
 590   See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blocking
 591   execution support.  This service may only be called from the BSP.
 592
 593   This function is used to dispatch one enabled AP to the function specified by
 594   Procedure passing in the argument specified by ProcedureArgument.  If WaitEvent
 595   is NULL, execution is in blocking mode. The BSP waits until the AP finishes or
 596   TimeoutInMicroSecondss expires. Otherwise, execution is in non-blocking mode.
 597   BSP proceeds to the next task without waiting for the AP. If a non-blocking mode
 598   is requested after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled,
 599   then EFI_UNSUPPORTED must be returned.
 600
 601   If the timeout specified by TimeoutInMicroseconds expires before the AP returns
 602   from Procedure, then execution of Procedure by the AP is terminated. The AP is
 603   available for subsequent calls to EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and
 604   EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 605
 606   @param[in]  This                    A pointer to the EFI_MP_SERVICES_PROTOCOL
 607                                       instance.
 608   @param[in]  Procedure               A pointer to the function to be run on
 609                                       enabled APs of the system. See type
 610                                       EFI_AP_PROCEDURE.
 611   @param[in]  ProcessorNumber         The handle number of the AP. The range is
 612                                       from 0 to the total number of logical
 613                                       processors minus 1. The total number of
 614                                       logical processors can be retrieved by
 615                                       EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 616   @param[in]  WaitEvent               The event created by the caller with CreateEvent()
 617                                       service.  If it is NULL, then execute in
 618                                       blocking mode. BSP waits until all APs finish
 619                                       or TimeoutInMicroseconds expires.  If it's
 620                                       not NULL, then execute in non-blocking mode.
 621                                       BSP requests the function specified by
 622                                       Procedure to be started on all the enabled
 623                                       APs, and go on executing immediately. If
 624                                       all return from Procedure or TimeoutInMicroseconds
 625                                       expires, this event is signaled. The BSP
 626                                       can use the CheckEvent() or WaitForEvent()
 627                                       services to check the state of event.  Type
 628                                       EFI_EVENT is defined in CreateEvent() in
 629                                       the Unified Extensible Firmware Interface
 630                                       Specification.
 631   @param[in]  TimeoutInMicrosecsond   Indicates the time limit in microseconds for
 632                                       APs to return from Procedure, either for
 633                                       blocking or non-blocking mode. Zero means
 634                                       infinity.  If the timeout expires before
 635                                       all APs return from Procedure, then Procedure
 636                                       on the failed APs is terminated. All enabled
 637                                       APs are available for next function assigned
 638                                       by EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
 639                                       or EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 640                                       If the timeout expires in blocking mode,
 641                                       BSP returns EFI_TIMEOUT.  If the timeout
 642                                       expires in non-blocking mode, WaitEvent
 643                                       is signaled with SignalEvent().
 644   @param[in]  ProcedureArgument       The parameter passed into Procedure for
 645                                       all APs.
 646   @param[out] Finished                If NULL, this parameter is ignored.  In
 647                                       blocking mode, this parameter is ignored.
 648                                       In non-blocking mode, if AP returns from
 649                                       Procedure before the timeout expires, its
 650                                       content is set to TRUE. Otherwise, the
 651                                       value is set to FALSE. The caller can
 652                                       determine if the AP returned from Procedure
 653                                       by evaluating this value.
 654
 655   @retval EFI_SUCCESS             In blocking mode, specified AP finished before
 656                                   the timeout expires.
 657   @retval EFI_SUCCESS             In non-blocking mode, the function has been
 658                                   dispatched to specified AP.
 659   @retval EFI_UNSUPPORTED         A non-blocking mode request was made after the
 660                                   UEFI event EFI_EVENT_GROUP_READY_TO_BOOT was
 661                                   signaled.
 662   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 663   @retval EFI_TIMEOUT             In blocking mode, the timeout expired before
 664                                   the specified AP has finished.
 665   @retval EFI_NOT_READY           The specified AP is busy.
 666   @retval EFI_NOT_FOUND           The processor with the handle specified by
 667                                   ProcessorNumber does not exist.
 668   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or disabled AP.
 669   @retval EFI_INVALID_PARAMETER   Procedure is NULL.
 670
 671 **/
 672 EFI_STATUS
 673 EFIAPI
 674 CpuMpServicesStartupThisAP (
 675   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 676   IN  EFI_AP_PROCEDURE          Procedure,
 677   IN  UINTN                     ProcessorNumber,
 678   IN  EFI_EVENT                 WaitEvent               OPTIONAL,
 679   IN  UINTN                     TimeoutInMicroseconds,
 680   IN  VOID                      *ProcedureArgument      OPTIONAL,
 681   OUT BOOLEAN                   *Finished               OPTIONAL
 682   )
 683 {
 684   UINTN            Timeout;
 685
 686   if (!IsBSP ()) {
 687     return EFI_DEVICE_ERROR;
 688   }
 689
 690   if (Procedure == NULL) {
 691     return EFI_INVALID_PARAMETER;
 692   }
 693
 694   if (ProcessorNumber >= gMPSystem.NumberOfProcessors) {
 695     return EFI_NOT_FOUND;
 696   }
 697
 698   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 699     return EFI_INVALID_PARAMETER;
 700   }
 701
 702   gThread->MutexLock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 703   if (gMPSystem.ProcessorData[ProcessorNumber].State != CPU_STATE_IDLE) {
 704     gThread->MutexUnlock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 705     return EFI_NOT_READY;
 706   }
 707   gThread->MutexUnlock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 708
 709   if ((WaitEvent != NULL)  && gReadToBoot) {
 710     return EFI_UNSUPPORTED;
 711   }
 712
 713   Timeout = TimeoutInMicroseconds;
 714
 715   gMPSystem.StartCount   = 1;
 716   gMPSystem.FinishCount  = 0;
 717
 718   SetApProcedure (&gMPSystem.ProcessorData[ProcessorNumber], Procedure, ProcedureArgument);
 719
 720   if (WaitEvent != NULL) {
 721     // Non Blocking
 722     gMPSystem.WaitEvent = WaitEvent;
 723     gBS->SetTimer (
 724            gMPSystem.ProcessorData[ProcessorNumber].CheckThisAPEvent,
 725            TimerPeriodic,
 726            gPollInterval
 727            );
 728     return EFI_SUCCESS;
 729   }
 730
 731   // Blocking
 732   while (TRUE) {
 733     gThread->MutexLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 734     if (gMPSystem.ProcessorData[ProcessorNumber].State == CPU_STATE_FINISHED) {
 735       gMPSystem.ProcessorData[ProcessorNumber].State = CPU_STATE_IDLE;
 736       gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 737       break;
 738     }
 739
 740     gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 741
 742     if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
 743       return EFI_TIMEOUT;
 744     }
 745
 746     Timeout -= CalculateAndStallInterval (Timeout);
 747   }
 748
 749   return EFI_SUCCESS;
 750
 751 }
 752
 753
 754 /**
 755   This service switches the requested AP to be the BSP from that point onward.
 756   This service changes the BSP for all purposes.   This call can only be performed
 757   by the current BSP.
 758
 759   This service switches the requested AP to be the BSP from that point onward.
 760   This service changes the BSP for all purposes. The new BSP can take over the
 761   execution of the old BSP and continue seamlessly from where the old one left
 762   off. This service may not be supported after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT
 763   is signaled.
 764
 765   If the BSP cannot be switched prior to the return from this service, then
 766   EFI_UNSUPPORTED must be returned.
 767
 768   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 769   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 770                                BSP. The range is from 0 to the total number of
 771                                logical processors minus 1. The total number of
 772                                logical processors can be retrieved by
 773                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 774   @param[in] EnableOldBSP      If TRUE, then the old BSP will be listed as an
 775                                enabled AP. Otherwise, it will be disabled.
 776
 777   @retval EFI_SUCCESS             BSP successfully switched.
 778   @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed prior to
 779                                   this service returning.
 780   @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
 781   @retval EFI_SUCCESS             The calling processor is an AP.
 782   @retval EFI_NOT_FOUND           The processor with the handle specified by
 783                                   ProcessorNumber does not exist.
 784   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BSP or
 785                                   a disabled AP.
 786   @retval EFI_NOT_READY           The specified AP is busy.
 787
 788 **/
 789 EFI_STATUS
 790 EFIAPI
 791 CpuMpServicesSwitchBSP (
 792   IN EFI_MP_SERVICES_PROTOCOL  *This,
 793   IN  UINTN                    ProcessorNumber,
 794   IN  BOOLEAN                  EnableOldBSP
 795   )
 796 {
 797   UINTN   Index;
 798
 799   if (!IsBSP ()) {
 800     return EFI_DEVICE_ERROR;
 801   }
 802
 803   if (ProcessorNumber >= gMPSystem.NumberOfProcessors) {
 804     return EFI_NOT_FOUND;
 805   }
 806
 807   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
 808     return EFI_INVALID_PARAMETER;
 809   }
 810
 811   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 812     return EFI_INVALID_PARAMETER;
 813   }
 814
 815   for (Index = 0; Index < gMPSystem.NumberOfProcessors; Index++) {
 816     if ((gMPSystem.ProcessorData[Index].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 817       break;
 818     }
 819   }
 820   ASSERT (Index != gMPSystem.NumberOfProcessors);
 821
 822   gThread->MutexLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 823   if (gMPSystem.ProcessorData[ProcessorNumber].State != CPU_STATE_IDLE) {
 824     gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 825     return EFI_NOT_READY;
 826   }
 827   gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 828
 829   // Skip for now as we need switch a bunch of stack stuff around and it's complex
 830   // May not be worth it?
 831   return EFI_NOT_READY;
 832 }
 833
 834
 835 /**
 836   This service lets the caller enable or disable an AP from this point onward.
 837   This service may only be called from the BSP.
 838
 839   This service allows the caller enable or disable an AP from this point onward.
 840   The caller can optionally specify the health status of the AP by Health. If
 841   an AP is being disabled, then the state of the disabled AP is implementation
 842   dependent. If an AP is enabled, then the implementation must guarantee that a
 843   complete initialization sequence is performed on the AP, so the AP is in a state
 844   that is compatible with an MP operating system. This service may not be supported
 845   after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled.
 846
 847   If the enable or disable AP operation cannot be completed prior to the return
 848   from this service, then EFI_UNSUPPORTED must be returned.
 849
 850   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 851   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 852                                BSP. The range is from 0 to the total number of
 853                                logical processors minus 1. The total number of
 854                                logical processors can be retrieved by
 855                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 856   @param[in] EnableAP          Specifies the new state for the processor for
 857                                enabled, FALSE for disabled.
 858   @param[in] HealthFlag        If not NULL, a pointer to a value that specifies
 859                                the new health status of the AP. This flag
 860                                corresponds to StatusFlag defined in
 861                                EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo(). Only
 862                                the PROCESSOR_HEALTH_STATUS_BIT is used. All other
 863                                bits are ignored.  If it is NULL, this parameter
 864                                is ignored.
 865
 866   @retval EFI_SUCCESS             The specified AP was enabled or disabled successfully.
 867   @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be completed
 868                                   prior to this service returning.
 869   @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not supported.
 870   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 871   @retval EFI_NOT_FOUND           Processor with the handle specified by ProcessorNumber
 872                                   does not exist.
 873   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.
 874
 875 **/
 876 EFI_STATUS
 877 EFIAPI
 878 CpuMpServicesEnableDisableAP (
 879   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 880   IN  UINTN                     ProcessorNumber,
 881   IN  BOOLEAN                   EnableAP,
 882   IN  UINT32                    *HealthFlag OPTIONAL
 883   )
 884 {
 885   if (!IsBSP ()) {
 886     return EFI_DEVICE_ERROR;
 887   }
 888
 889   if (ProcessorNumber >= gMPSystem.NumberOfProcessors) {
 890     return EFI_NOT_FOUND;
 891   }
 892
 893   if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) != 0) {
 894     return EFI_INVALID_PARAMETER;
 895   }
 896
 897   gThread->MutexLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 898   if (gMPSystem.ProcessorData[ProcessorNumber].State != CPU_STATE_IDLE) {
 899     gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 900     return EFI_UNSUPPORTED;
 901   }
 902   gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);
 903
 904   if (EnableAP) {
 905     if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0 ) {
 906       gMPSystem.NumberOfEnabledProcessors++;
 907     }
 908     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag |= PROCESSOR_ENABLED_BIT;
 909   } else {
 910     if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == PROCESSOR_ENABLED_BIT ) {
 911       gMPSystem.NumberOfEnabledProcessors--;
 912     }
 913     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag &= ~PROCESSOR_ENABLED_BIT;
 914   }
 915
 916   if (HealthFlag != NULL) {
 917     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag &= ~PROCESSOR_HEALTH_STATUS_BIT;
 918     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag |= (*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT);
 919   }
 920
 921   return EFI_SUCCESS;
 922 }
 923
 924
 925 /**
 926   This return the handle number for the calling processor.  This service may be
 927   called from the BSP and APs.
 928
 929   This service returns the processor handle number for the calling processor.
 930   The returned value is in the range from 0 to the total number of logical
 931   processors minus 1. The total number of logical processors can be retrieved
 932   with EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors(). This service may be
 933   called from the BSP and APs. If ProcessorNumber is NULL, then EFI_INVALID_PARAMETER
 934   is returned. Otherwise, the current processors handle number is returned in
 935   ProcessorNumber, and EFI_SUCCESS is returned.
 936
 937   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 938   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 939                                BSP. The range is from 0 to the total number of
 940                                logical processors minus 1. The total number of
 941                                logical processors can be retrieved by
 942                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 943
 944   @retval EFI_SUCCESS             The current processor handle number was returned
 945                                   in ProcessorNumber.
 946   @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.
 947
 948 **/
 949 EFI_STATUS
 950 EFIAPI
 951 CpuMpServicesWhoAmI (
 952   IN EFI_MP_SERVICES_PROTOCOL  *This,
 953   OUT UINTN                    *ProcessorNumber
 954   )
 955 {
 956   UINTN   Index;
 957   UINT64  ProcessorId;
 958
 959   if (ProcessorNumber == NULL) {
 960     return EFI_INVALID_PARAMETER;
 961   }
 962
 963   ProcessorId = gThread->Self ();
 964   for (Index = 0; Index < gMPSystem.NumberOfProcessors; Index++) {
 965     if (gMPSystem.ProcessorData[Index].Info.ProcessorId == ProcessorId) {
 966       break;
 967     }
 968   }
 969
 970   *ProcessorNumber = Index;
 971   return EFI_SUCCESS;
 972 }
 973
 974
 975
 976 EFI_MP_SERVICES_PROTOCOL  mMpServicesTemplate = {
 977   CpuMpServicesGetNumberOfProcessors,
 978   CpuMpServicesGetProcessorInfo,
 979   CpuMpServicesStartupAllAps,
 980   CpuMpServicesStartupThisAP,
 981   CpuMpServicesSwitchBSP,
 982   CpuMpServicesEnableDisableAP,
 983   CpuMpServicesWhoAmI
 984 };
 985
 986
 987
 988 /*++
 989   If timeout occurs in StartupAllAps(), a timer is set, which invokes this
 990   procedure periodically to check whether all APs have finished.
 991
 992
 993 --*/
 994 VOID
 995 EFIAPI
 996 CpuCheckAllAPsStatus (
 997   IN  EFI_EVENT   Event,
 998   IN  VOID        *Context
 999   )
1000 {
1001   UINTN                 ProcessorNumber;
1002   UINTN                 NextNumber;
1003   PROCESSOR_DATA_BLOCK  *ProcessorData;
1004   PROCESSOR_DATA_BLOCK  *NextData;
1005   EFI_STATUS            Status;
1006   PROCESSOR_STATE       ProcessorState;
1007   UINTN                 Cpu;
1008   BOOLEAN               Found;
1009
1010   if (gMPSystem.TimeoutActive) {
1011     gMPSystem.Timeout -= CalculateAndStallInterval (gMPSystem.Timeout);
1012   }
1013
1014   for (ProcessorNumber = 0; ProcessorNumber < gMPSystem.NumberOfProcessors; ProcessorNumber++) {
1015     ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];
1016     if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
1017      // Skip BSP
1018       continue;
1019     }
1020
1021     if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
1022       // Skip Disabled processors
1023       continue;
1024     }
1025
1026     // This is an Interrupt Service routine.
1027     // This can grab a lock that is held in a non-interrupt
1028     // context. Meaning deadlock. Which is a bad thing.
1029     // So, try lock it. If we can get it, cool, do our thing.
1030     // otherwise, just dump out & try again on the next iteration.
1031     Status = gThread->MutexTryLock (ProcessorData->StateLock);
1032     if (EFI_ERROR(Status)) {
1033       return;
1034     }
1035     ProcessorState = ProcessorData->State;
1036     gThread->MutexUnlock (ProcessorData->StateLock);
1037
1038     switch (ProcessorState) {
1039     case CPU_STATE_FINISHED:
1040       if (gMPSystem.SingleThread) {
1041         Status = GetNextBlockedNumber (&NextNumber);
1042         if (!EFI_ERROR (Status)) {
1043           NextData = &gMPSystem.ProcessorData[NextNumber];
1044
1045           gThread->MutexLock (NextData->StateLock);
1046           NextData->State = CPU_STATE_READY;
1047           gThread->MutexUnlock (NextData->StateLock);
1048
1049           SetApProcedure (NextData, gMPSystem.Procedure, gMPSystem.ProcedureArgument);
1050         }
1051       }
1052
1053       gThread->MutexLock (ProcessorData->StateLock);
1054       ProcessorData->State = CPU_STATE_IDLE;
1055       gThread->MutexUnlock (ProcessorData->StateLock);
1056       gMPSystem.FinishCount++;
1057       break;
1058
1059     default:
1060       break;
1061     }
1062   }
1063
1064   if (gMPSystem.TimeoutActive && gMPSystem.Timeout == 0) {
1065     //
1066     // Timeout
1067     //
1068     if (gMPSystem.FailedList != NULL) {
1069       for (ProcessorNumber = 0; ProcessorNumber < gMPSystem.NumberOfProcessors; ProcessorNumber++) {
1070         ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];
1071         if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
1072          // Skip BSP
1073           continue;
1074         }
1075
1076         if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {
1077           // Skip Disabled processors
1078           continue;
1079         }
1080
1081         // Mark the
1082         Status = gThread->MutexTryLock (ProcessorData->StateLock);
1083         if (EFI_ERROR(Status)) {
1084           return;
1085         }
1086         ProcessorState = ProcessorData->State;
1087         gThread->MutexUnlock (ProcessorData->StateLock);
1088
1089         if (ProcessorState != CPU_STATE_IDLE) {
1090           // If we are retrying make sure we don't double count
1091           for (Cpu = 0, Found = FALSE; Cpu < gMPSystem.NumberOfProcessors; Cpu++) {
1092             if (gMPSystem.FailedList[Cpu] == END_OF_CPU_LIST) {
1093               break;
1094             }
1095             if (gMPSystem.FailedList[ProcessorNumber] == Cpu) {
1096               Found = TRUE;
1097               break;
1098             }
1099           }
1100           if (!Found) {
1101             gMPSystem.FailedList[gMPSystem.FailedListIndex++] = Cpu;
1102           }
1103         }
1104       }
1105     }
1106     // Force terminal exit
1107     gMPSystem.FinishCount = gMPSystem.StartCount;
1108   }
1109
1110   if (gMPSystem.FinishCount != gMPSystem.StartCount) {
1111     return;
1112   }
1113
1114   gBS->SetTimer (
1115          gMPSystem.CheckAllAPsEvent,
1116          TimerCancel,
1117          0
1118          );
1119
1120   if (gMPSystem.FailedListIndex == 0) {
1121     if (gMPSystem.FailedList != NULL) {
1122       FreePool (gMPSystem.FailedList);
1123       gMPSystem.FailedList = NULL;
1124     }
1125   }
1126
1127   Status = gBS->SignalEvent (gMPSystem.WaitEvent);
1128
1129   return ;
1130 }
1131
1132 VOID
1133 EFIAPI
1134 CpuCheckThisAPStatus (
1135   IN  EFI_EVENT                           Event,
1136   IN  VOID                                *Context
1137   )
1138 {
1139   EFI_STATUS            Status;
1140   PROCESSOR_DATA_BLOCK  *ProcessorData;
1141   PROCESSOR_STATE       ProcessorState;
1142
1143   ProcessorData = (PROCESSOR_DATA_BLOCK *) Context;
1144
1145   //
1146   // This is an Interrupt Service routine.
1147   // that can grab a lock that is held in a non-interrupt
1148   // context. Meaning deadlock. Which is a badddd thing.
1149   // So, try lock it. If we can get it, cool, do our thing.
1150   // otherwise, just dump out & try again on the next iteration.
1151   //
1152   Status = gThread->MutexTryLock (ProcessorData->StateLock);
1153   if (EFI_ERROR(Status)) {
1154     return;
1155   }
1156   ProcessorState = ProcessorData->State;
1157   gThread->MutexUnlock (ProcessorData->StateLock);
1158
1159   if (ProcessorState == CPU_STATE_FINISHED) {
1160     Status = gBS->SetTimer (ProcessorData->CheckThisAPEvent, TimerCancel, 0);
1161     ASSERT_EFI_ERROR (Status);
1162
1163     Status = gBS->SignalEvent (gMPSystem.WaitEvent);
1164     ASSERT_EFI_ERROR (Status);
1165
1166     gThread->MutexLock (ProcessorData->StateLock);
1167     ProcessorData->State = CPU_STATE_IDLE;
1168     gThread->MutexUnlock (ProcessorData->StateLock);
1169   }
1170
1171   return ;
1172 }
1173
1174
1175 /*++
1176   This function is called by all processors (both BSP and AP) once and collects MP related data
1177
1178   MPSystemData  - Pointer to the data structure containing MP related data
1179   BSP           - TRUE if the CPU is BSP
1180
1181   EFI_SUCCESS   - Data for the processor collected and filled in
1182
1183 --*/
1184 EFI_STATUS
1185 FillInProcessorInformation (
1186   IN     BOOLEAN              BSP,
1187   IN     UINTN                ProcessorNumber
1188   )
1189 {
1190   gMPSystem.ProcessorData[ProcessorNumber].Info.ProcessorId  = gThread->Self ();
1191   gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag   = PROCESSOR_ENABLED_BIT | PROCESSOR_HEALTH_STATUS_BIT;
1192   if (BSP) {
1193     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag |= PROCESSOR_AS_BSP_BIT;
1194   }
1195
1196   gMPSystem.ProcessorData[ProcessorNumber].Info.Location.Package = (UINT32) ProcessorNumber;
1197   gMPSystem.ProcessorData[ProcessorNumber].Info.Location.Core    = 0;
1198   gMPSystem.ProcessorData[ProcessorNumber].Info.Location.Thread  = 0;
1199   gMPSystem.ProcessorData[ProcessorNumber].State = BSP ? CPU_STATE_BUSY : CPU_STATE_IDLE;
1200
1201   gMPSystem.ProcessorData[ProcessorNumber].Procedure        = NULL;
1202   gMPSystem.ProcessorData[ProcessorNumber].Parameter        = NULL;
1203   gMPSystem.ProcessorData[ProcessorNumber].StateLock        = gThread->MutexInit ();
1204   gMPSystem.ProcessorData[ProcessorNumber].ProcedureLock    = gThread->MutexInit ();
1205
1206   return EFI_SUCCESS;
1207 }
1208
1209 VOID *
1210 EFIAPI
1211 CpuDriverApIdolLoop (
1212   VOID  *Context
1213   )
1214 {
1215   EFI_AP_PROCEDURE      Procedure;
1216   VOID                  *Parameter;
1217   UINTN                 ProcessorNumber;
1218   PROCESSOR_DATA_BLOCK  *ProcessorData;
1219
1220   ProcessorNumber = (UINTN)Context;
1221   ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];
1222
1223   ProcessorData->Info.ProcessorId = gThread->Self ();
1224
1225   while (TRUE) {
1226     //
1227     // Make a local copy on the stack to be extra safe
1228     //
1229     gThread->MutexLock (ProcessorData->ProcedureLock);
1230     Procedure = ProcessorData->Procedure;
1231     Parameter = ProcessorData->Parameter;
1232     gThread->MutexUnlock (ProcessorData->ProcedureLock);
1233
1234     if (Procedure != NULL) {
1235       gThread->MutexLock (ProcessorData->StateLock);
1236       ProcessorData->State = CPU_STATE_BUSY;
1237       gThread->MutexUnlock (ProcessorData->StateLock);
1238
1239       Procedure (Parameter);
1240
1241       gThread->MutexLock (ProcessorData->ProcedureLock);
1242       ProcessorData->Procedure = NULL;
1243       gThread->MutexUnlock (ProcessorData->ProcedureLock);
1244
1245       gThread->MutexLock (ProcessorData->StateLock);
1246       ProcessorData->State = CPU_STATE_FINISHED;
1247       gThread->MutexUnlock (ProcessorData->StateLock);
1248     }
1249
1250     // Poll 5 times a seconds, 200ms
1251     // Don't want to burn too many system resources doing nothing.
1252     gEmuThunk->Sleep (200 * 1000);
1253   }
1254
1255   return 0;
1256 }
1257
1258
1259 EFI_STATUS
1260 InitializeMpSystemData (
1261   IN   UINTN     NumberOfProcessors
1262   )
1263 {
1264   EFI_STATUS              Status;
1265   UINTN                   Index;
1266
1267
1268   //
1269   // Clear the data structure area first.
1270   //
1271   ZeroMem (&gMPSystem, sizeof (MP_SYSTEM_DATA));
1272
1273   //
1274   // First BSP fills and inits all known values, including it's own records.
1275   //
1276   gMPSystem.NumberOfProcessors         = NumberOfProcessors;
1277   gMPSystem.NumberOfEnabledProcessors  = NumberOfProcessors;
1278
1279   gMPSystem.ProcessorData = AllocateZeroPool (gMPSystem.NumberOfProcessors * sizeof (PROCESSOR_DATA_BLOCK));
1280   ASSERT (gMPSystem.ProcessorData != NULL);
1281
1282   FillInProcessorInformation (TRUE, 0);
1283
1284   Status = gBS->CreateEvent (
1285                   EVT_TIMER | EVT_NOTIFY_SIGNAL,
1286                   TPL_CALLBACK,
1287                   CpuCheckAllAPsStatus,
1288                   NULL,
1289                   &gMPSystem.CheckAllAPsEvent
1290                   );
1291   ASSERT_EFI_ERROR (Status);
1292
1293
1294   for (Index = 0; Index < gMPSystem.NumberOfProcessors; Index++) {
1295     if ((gMPSystem.ProcessorData[Index].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {
1296      // Skip BSP
1297       continue;
1298     }
1299
1300     FillInProcessorInformation (FALSE, Index);
1301
1302     Status = gThread->CreateThread (
1303                         (VOID *)&gMPSystem.ProcessorData[Index].Info.ProcessorId,
1304                         NULL,
1305                         CpuDriverApIdolLoop,
1306                         (VOID *)Index
1307                         );
1308
1309
1310     Status = gBS->CreateEvent (
1311                          EVT_TIMER | EVT_NOTIFY_SIGNAL,
1312                          TPL_CALLBACK,
1313                          CpuCheckThisAPStatus,
1314                          (VOID *)  &gMPSystem.ProcessorData[Index],
1315                          &gMPSystem.ProcessorData[Index].CheckThisAPEvent
1316                          );
1317   }
1318
1319   return EFI_SUCCESS;
1320 }
1321
1322
1323
1324 /**
1325   Invoke a notification event
1326
1327   @param  Event                 Event whose notification function is being invoked.
1328   @param  Context               The pointer to the notification function's context,
1329                                 which is implementation-dependent.
1330
1331 **/
1332 VOID
1333 EFIAPI
1334 CpuReadToBootFunction (
1335   IN  EFI_EVENT                Event,
1336   IN  VOID                     *Context
1337   )
1338 {
1339   gReadToBoot = TRUE;
1340 }
1341
1342
1343
1344 EFI_STATUS
1345 CpuMpServicesInit (
1346   OUT UINTN *MaxCpus
1347   )
1348 {
1349   EFI_STATUS              Status;
1350   EFI_HANDLE              Handle;
1351   EMU_IO_THUNK_PROTOCOL   *IoThunk;
1352
1353   *MaxCpus = 1; // BSP
1354   IoThunk = GetIoThunkInstance (&gEmuThreadThunkProtocolGuid, 0);
1355   if (IoThunk != NULL) {
1356     Status = IoThunk->Open (IoThunk);
1357     if (!EFI_ERROR (Status)) {
1358       if (IoThunk->ConfigString != NULL) {
1359         *MaxCpus += StrDecimalToUintn (IoThunk->ConfigString);
1360         gThread = IoThunk->Interface;
1361       }
1362     }
1363   }
1364
1365   if (*MaxCpus == 1) {
1366     // We are not MP so nothing to do
1367     return EFI_SUCCESS;
1368   }
1369
1370   gPollInterval = (UINTN) PcdGet64 (PcdEmuMpServicesPollingInterval);
1371
1372   Status  = InitializeMpSystemData (*MaxCpus);
1373   if (EFI_ERROR (Status)) {
1374     return Status;
1375   }
1376
1377   Status = EfiCreateEventReadyToBootEx (TPL_CALLBACK, CpuReadToBootFunction, NULL, &gReadToBootEvent);
1378   ASSERT_EFI_ERROR (Status);
1379
1380   //
1381   // Now install the MP services protocol.
1382   //
1383   Handle = NULL;
1384   Status = gBS->InstallMultipleProtocolInterfaces (
1385                   &Handle,
1386                   &gEfiMpServiceProtocolGuid,   &mMpServicesTemplate,
1387                   NULL
1388                   );
1389   return Status;
1390 }
1391
1392