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