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