ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c

   1 /** @file
   2   Construct MP Services Protocol.
   3
   4   The MP Services Protocol provides a generalized way of performing following tasks:
   5     - Retrieving information of multi-processor environment and MP-related status of
   6       specific processors.
   7     - Dispatching user-provided function to APs.
   8     - Maintain MP-related processor status.
   9
  10   The MP Services Protocol must be produced on any system with more than one logical
  11   processor.
  12
  13   The Protocol is available only during boot time.
  14
  15   MP Services Protocol is hardware-independent. Most of the logic of this protocol
  16   is architecturally neutral. It abstracts the multi-processor environment and
  17   status of processors, and provides interfaces to retrieve information, maintain,
  18   and dispatch.
  19
  20   MP Services Protocol may be consumed by ACPI module. The ACPI module may use this
  21   protocol to retrieve data that are needed for an MP platform and report them to OS.
  22   MP Services Protocol may also be used to program and configure processors, such
  23   as MTRR synchronization for memory space attributes setting in DXE Services.
  24   MP Services Protocol may be used by non-CPU DXE drivers to speed up platform boot
  25   by taking advantage of the processing capabilities of the APs, for example, using
  26   APs to help test system memory in parallel with other device initialization.
  27   Diagnostics applications may also use this protocol for multi-processor.
  28
  29   Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
  30   SPDX-License-Identifier: BSD-2-Clause-Patent
  31
  32 **/
  33
  34 #include <PiDxe.h>
  35
  36 #include <Library/ArmLib.h>
  37 #include <Library/ArmMmuLib.h>
  38 #include <Library/ArmPlatformLib.h>
  39 #include <Library/ArmSmcLib.h>
  40 #include <Library/BaseMemoryLib.h>
  41 #include <Library/CacheMaintenanceLib.h>
  42 #include <Library/CpuExceptionHandlerLib.h>
  43 #include <Library/DebugLib.h>
  44 #include <Library/HobLib.h>
  45 #include <Library/MemoryAllocationLib.h>
  46 #include <Library/UefiBootServicesTableLib.h>
  47 #include <Library/UefiLib.h>
  48 #include <IndustryStandard/ArmStdSmc.h>
  49 #include <Ppi/ArmMpCoreInfo.h>
  50 #include <Protocol/LoadedImage.h>
  51
  52 #include "MpServicesInternal.h"
  53
  54 #define POLL_INTERVAL_US  50000
  55
  56 STATIC CPU_MP_DATA  mCpuMpData;
  57 STATIC BOOLEAN      mNonBlockingModeAllowed;
  58 UINT64              *gApStacksBase;
  59 UINT64              *gProcessorIDs;
  60 CONST UINT64        gApStackSize = AP_STACK_SIZE;
  61 VOID                *gTtbr0;
  62 UINTN               gTcr;
  63 UINTN               gMair;
  64
  65 STATIC
  66 BOOLEAN
  67 IsCurrentProcessorBSP (
  68   VOID
  69   );
  70
  71 /** Turns on the specified core using PSCI and executes the user-supplied
  72     function that's been configured via a previous call to SetApProcedure.
  73
  74     @param ProcessorIndex The index of the core to turn on.
  75
  76     @retval EFI_SUCCESS      Success.
  77     @retval EFI_DEVICE_ERROR The processor could not be turned on.
  78
  79 **/
  80 STATIC
  81 EFI_STATUS
  82 EFIAPI
  83 DispatchCpu (
  84   IN UINTN  ProcessorIndex
  85   )
  86 {
  87   ARM_SMC_ARGS  Args;
  88   EFI_STATUS    Status;
  89
  90   Status = EFI_SUCCESS;
  91
  92   mCpuMpData.CpuData[ProcessorIndex].State = CpuStateBusy;
  93
  94   /* Turn the AP on */
  95   if (sizeof (Args.Arg0) == sizeof (UINT32)) {
  96     Args.Arg0 = ARM_SMC_ID_PSCI_CPU_ON_AARCH32;
  97   } else {
  98     Args.Arg0 = ARM_SMC_ID_PSCI_CPU_ON_AARCH64;
  99   }
 100
 101   Args.Arg1 = gProcessorIDs[ProcessorIndex];
 102   Args.Arg2 = (UINTN)ApEntryPoint;
 103
 104   ArmCallSmc (&Args);
 105
 106   if (Args.Arg0 != ARM_SMC_PSCI_RET_SUCCESS) {
 107     DEBUG ((DEBUG_ERROR, "PSCI_CPU_ON call failed: %d\n", Args.Arg0));
 108     Status = EFI_DEVICE_ERROR;
 109   }
 110
 111   return Status;
 112 }
 113
 114 /** Returns whether the specified processor is the BSP.
 115
 116   @param[in] ProcessorIndex The index the processor to check.
 117
 118   @return TRUE if the processor is the BSP, FALSE otherwise.
 119 **/
 120 STATIC
 121 BOOLEAN
 122 IsProcessorBSP (
 123   UINTN  ProcessorIndex
 124   )
 125 {
 126   EFI_PROCESSOR_INFORMATION  *CpuInfo;
 127
 128   CpuInfo = &mCpuMpData.CpuData[ProcessorIndex].Info;
 129
 130   return (CpuInfo->StatusFlag & PROCESSOR_AS_BSP_BIT) != 0;
 131 }
 132
 133 /** Get the Application Processors state.
 134
 135   @param[in]  CpuData    The pointer to CPU_AP_DATA of specified AP.
 136
 137   @return The AP status.
 138 **/
 139 CPU_STATE
 140 GetApState (
 141   IN  CPU_AP_DATA  *CpuData
 142   )
 143 {
 144   return CpuData->State;
 145 }
 146
 147 /** Configures the processor context with the user-supplied procedure and
 148     argument.
 149
 150    @param CpuData           The processor context.
 151    @param Procedure         The user-supplied procedure.
 152    @param ProcedureArgument The user-supplied procedure argument.
 153
 154 **/
 155 STATIC
 156 VOID
 157 SetApProcedure (
 158   IN   CPU_AP_DATA       *CpuData,
 159   IN   EFI_AP_PROCEDURE  Procedure,
 160   IN   VOID              *ProcedureArgument
 161   )
 162 {
 163   ASSERT (CpuData != NULL);
 164   ASSERT (Procedure != NULL);
 165
 166   CpuData->Parameter = ProcedureArgument;
 167   CpuData->Procedure = Procedure;
 168 }
 169
 170 /** Returns the index of the next processor that is blocked.
 171
 172    @param[out] NextNumber The index of the next blocked processor.
 173
 174    @retval EFI_SUCCESS   Successfully found the next blocked processor.
 175    @retval EFI_NOT_FOUND There are no blocked processors.
 176
 177 **/
 178 STATIC
 179 EFI_STATUS
 180 GetNextBlockedNumber (
 181   OUT UINTN  *NextNumber
 182   )
 183 {
 184   UINTN        Index;
 185   CPU_STATE    State;
 186   CPU_AP_DATA  *CpuData;
 187
 188   for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
 189     CpuData = &mCpuMpData.CpuData[Index];
 190     if (IsProcessorBSP (Index)) {
 191       // Skip BSP
 192       continue;
 193     }
 194
 195     State = CpuData->State;
 196
 197     if (State == CpuStateBlocked) {
 198       *NextNumber = Index;
 199       return EFI_SUCCESS;
 200     }
 201   }
 202
 203   return EFI_NOT_FOUND;
 204 }
 205
 206 /** Stalls the BSP for the minimum of POLL_INTERVAL_US and Timeout.
 207
 208    @param[in]  Timeout    The time limit in microseconds remaining for
 209                           APs to return from Procedure.
 210
 211    @retval     StallTime  Time of execution stall.
 212 **/
 213 STATIC
 214 UINTN
 215 CalculateAndStallInterval (
 216   IN UINTN  Timeout
 217   )
 218 {
 219   UINTN  StallTime;
 220
 221   if ((Timeout < POLL_INTERVAL_US) && (Timeout != 0)) {
 222     StallTime = Timeout;
 223   } else {
 224     StallTime = POLL_INTERVAL_US;
 225   }
 226
 227   gBS->Stall (StallTime);
 228
 229   return StallTime;
 230 }
 231
 232 /**
 233   This service retrieves the number of logical processor in the platform
 234   and the number of those logical processors that are enabled on this boot.
 235   This service may only be called from the BSP.
 236
 237   This function is used to retrieve the following information:
 238     - The number of logical processors that are present in the system.
 239     - The number of enabled logical processors in the system at the instant
 240       this call is made.
 241
 242   Because MP Service Protocol provides services to enable and disable processors
 243   dynamically, the number of enabled logical processors may vary during the
 244   course of a boot session.
 245
 246   If this service is called from an AP, then EFI_DEVICE_ERROR is returned.
 247   If NumberOfProcessors or NumberOfEnabledProcessors is NULL, then
 248   EFI_INVALID_PARAMETER is returned. Otherwise, the total number of processors
 249   is returned in NumberOfProcessors, the number of currently enabled processor
 250   is returned in NumberOfEnabledProcessors, and EFI_SUCCESS is returned.
 251
 252   @param[in]  This                        A pointer to the
 253                                           EFI_MP_SERVICES_PROTOCOL instance.
 254   @param[out] NumberOfProcessors          Pointer to the total number of logical
 255                                           processors in the system, including
 256                                           the BSP and disabled APs.
 257   @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled
 258                                           logical processors that exist in the
 259                                           system, including the BSP.
 260
 261   @retval EFI_SUCCESS             The number of logical processors and enabled
 262                                   logical processors was retrieved.
 263   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 264   @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL.
 265   @retval EFI_INVALID_PARAMETER   NumberOfEnabledProcessors is NULL.
 266
 267 **/
 268 STATIC
 269 EFI_STATUS
 270 EFIAPI
 271 GetNumberOfProcessors (
 272   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 273   OUT UINTN                     *NumberOfProcessors,
 274   OUT UINTN                     *NumberOfEnabledProcessors
 275   )
 276 {
 277   if ((NumberOfProcessors == NULL) || (NumberOfEnabledProcessors == NULL)) {
 278     return EFI_INVALID_PARAMETER;
 279   }
 280
 281   if (!IsCurrentProcessorBSP ()) {
 282     return EFI_DEVICE_ERROR;
 283   }
 284
 285   *NumberOfProcessors        = mCpuMpData.NumberOfProcessors;
 286   *NumberOfEnabledProcessors = mCpuMpData.NumberOfEnabledProcessors;
 287   return EFI_SUCCESS;
 288 }
 289
 290 /**
 291   Gets detailed MP-related information on the requested processor at the
 292   instant this call is made. This service may only be called from the BSP.
 293
 294   This service retrieves detailed MP-related information about any processor
 295   on the platform. Note the following:
 296     - The processor information may change during the course of a boot session.
 297     - The information presented here is entirely MP related.
 298
 299   Information regarding the number of caches and their sizes, frequency of
 300   operation, slot numbers is all considered platform-related information and is
 301   not provided by this service.
 302
 303   @param[in]  This                  A pointer to the EFI_MP_SERVICES_PROTOCOL
 304                                     instance.
 305   @param[in]  ProcessorIndex        The index of the processor.
 306   @param[out] ProcessorInfoBuffer   A pointer to the buffer where information
 307                                     for the requested processor is deposited.
 308
 309   @retval EFI_SUCCESS             Processor information was returned.
 310   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 311   @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
 312   @retval EFI_NOT_FOUND           The processor with the handle specified by
 313                                   ProcessorNumber does not exist in the platform.
 314
 315 **/
 316 STATIC
 317 EFI_STATUS
 318 EFIAPI
 319 GetProcessorInfo (
 320   IN  EFI_MP_SERVICES_PROTOCOL   *This,
 321   IN  UINTN                      ProcessorIndex,
 322   OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
 323   )
 324 {
 325   if (ProcessorInfoBuffer == NULL) {
 326     return EFI_INVALID_PARAMETER;
 327   }
 328
 329   if (!IsCurrentProcessorBSP ()) {
 330     return EFI_DEVICE_ERROR;
 331   }
 332
 333   ProcessorIndex &= ~CPU_V2_EXTENDED_TOPOLOGY;
 334
 335   if (ProcessorIndex >= mCpuMpData.NumberOfProcessors) {
 336     return EFI_NOT_FOUND;
 337   }
 338
 339   CopyMem (
 340     ProcessorInfoBuffer,
 341     &mCpuMpData.CpuData[ProcessorIndex],
 342     sizeof (EFI_PROCESSOR_INFORMATION)
 343     );
 344   return EFI_SUCCESS;
 345 }
 346
 347 /**
 348   This service executes a caller provided function on all enabled APs. APs can
 349   run either simultaneously or one at a time in sequence. This service supports
 350   both blocking and non-blocking requests. The non-blocking requests use EFI
 351   events so the BSP can detect when the APs have finished. This service may only
 352   be called from the BSP.
 353
 354   This function is used to dispatch all the enabled APs to the function
 355   specified by Procedure.  If any enabled AP is busy, then EFI_NOT_READY is
 356   returned immediately and Procedure is not started on any AP.
 357
 358   If SingleThread is TRUE, all the enabled APs execute the function specified by
 359   Procedure one by one, in ascending order of processor handle number.
 360   Otherwise, all the enabled APs execute the function specified by Procedure
 361   simultaneously.
 362
 363   If WaitEvent is NULL, execution is in blocking mode. The BSP waits until all
 364   APs finish or TimeoutInMicroseconds expires. Otherwise, execution is in
 365   non-blocking mode, and the BSP returns from this service without waiting for
 366   APs. If a non-blocking mode is requested after the UEFI Event
 367   EFI_EVENT_GROUP_READY_TO_BOOT is signaled, then EFI_UNSUPPORTED must be
 368   returned.
 369
 370   If the timeout specified by TimeoutInMicroseconds expires before all APs
 371   return from Procedure, then Procedure on the failed APs is terminated.
 372   All enabled APs are always available for further calls to
 373   EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and
 374   EFI_MP_SERVICES_PROTOCOL.StartupThisAP(). If FailedCpuList is not NULL, its
 375   content points to the list of processor handle numbers in which Procedure was
 376   terminated.
 377
 378   Note: It is the responsibility of the consumer of the
 379   EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() to make sure that the nature of the
 380   code that is executed on the BSP and the dispatched APs is well controlled.
 381   The MP Services Protocol does not guarantee that the Procedure function is
 382   MP-safe. Hence, the tasks that can be run in parallel are limited to certain
 383   independent tasks and well-controlled exclusive code. EFI services and
 384   protocols may not be called by APs unless otherwise specified.
 385
 386   In blocking execution mode, BSP waits until all APs finish or
 387   TimeoutInMicroseconds expires.
 388
 389   In non-blocking execution mode, BSP is freed to return to the caller and then
 390   proceed to the next task without having to wait for APs. The following
 391   sequence needs to occur in a non-blocking execution mode:
 392
 393     -# The caller that intends to use this MP Services Protocol in non-blocking
 394        mode creates WaitEvent by calling the EFI CreateEvent() service.  The
 395        caller invokes EFI_MP_SERVICES_PROTOCOL.StartupAllAPs(). If the parameter
 396        WaitEvent is not NULL, then StartupAllAPs() executes in non-blocking
 397        mode. It requests the function specified by Procedure to be started on
 398        all the enabled APs, and releases the BSP to continue with other tasks.
 399     -# The caller can use the CheckEvent() and WaitForEvent() services to check
 400        the state of the WaitEvent created in step 1.
 401     -# When the APs complete their task or TimeoutInMicroSecondss expires, the
 402        MP Service signals WaitEvent by calling the EFI SignalEvent() function.
 403        If FailedCpuList is not NULL, its content is available when WaitEvent is
 404        signaled. If all APs returned from Procedure prior to the timeout, then
 405        FailedCpuList is set to NULL. If not all APs return from Procedure before
 406        the timeout, then FailedCpuList is filled in with the list of the failed
 407        APs. The buffer is allocated by MP Service Protocol using AllocatePool().
 408        It is the caller's responsibility to free the buffer with FreePool()
 409        service.
 410     -# This invocation of SignalEvent() function informs the caller that invoked
 411        EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() that either all the APs
 412        completed the specified task or a timeout occurred. The contents of
 413        FailedCpuList can be examined to determine which APs did not complete the
 414        specified task prior to the timeout.
 415
 416   @param[in]  This                    A pointer to the EFI_MP_SERVICES_PROTOCOL
 417                                       instance.
 418   @param[in]  Procedure               A pointer to the function to be run on
 419                                       enabled APs of the system. See type
 420                                       EFI_AP_PROCEDURE.
 421   @param[in]  SingleThread            If TRUE, then all the enabled APs execute
 422                                       the function specified by Procedure one by
 423                                       one, in ascending order of processor
 424                                       handle number.  If FALSE, then all the
 425                                       enabled APs execute the function specified
 426                                       by Procedure simultaneously.
 427   @param[in]  WaitEvent               The event created by the caller with
 428                                       CreateEvent() service.  If it is NULL,
 429                                       then execute in blocking mode. BSP waits
 430                                       until all APs finish or
 431                                       TimeoutInMicroseconds expires.  If it's
 432                                       not NULL, then execute in non-blocking
 433                                       mode. BSP requests the function specified
 434                                       by Procedure to be started on all the
 435                                       enabled APs, and go on executing
 436                                       immediately. If all return from Procedure,
 437                                       or TimeoutInMicroseconds expires, this
 438                                       event is signaled. The BSP can use the
 439                                       CheckEvent() or WaitForEvent()
 440                                       services to check the state of event. Type
 441                                       EFI_EVENT is defined in CreateEvent() in
 442                                       the Unified Extensible Firmware Interface
 443                                       Specification.
 444   @param[in]  TimeoutInMicroseconds   Indicates the time limit in microseconds
 445                                       for APs to return from Procedure, either
 446                                       for blocking or non-blocking mode. Zero
 447                                       means infinity.  If the timeout expires
 448                                       before all APs return from Procedure, then
 449                                       Procedure on the failed APs is terminated.
 450                                       All enabled APs are available for next
 451                                       function assigned by
 452                                       EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
 453                                       or EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 454                                       If the timeout expires in blocking mode,
 455                                       BSP returns EFI_TIMEOUT.  If the timeout
 456                                       expires in non-blocking mode, WaitEvent
 457                                       is signaled with SignalEvent().
 458   @param[in]  ProcedureArgument       The parameter passed into Procedure for
 459                                       all APs.
 460   @param[out] FailedCpuList           If NULL, this parameter is ignored.
 461                                       Otherwise, if all APs finish successfully,
 462                                       then its content is set to NULL. If not
 463                                       all APs finish before timeout expires,
 464                                       then its content is set to address of the
 465                                       buffer holding handle numbers of the
 466                                       failed APs.
 467                                       The buffer is allocated by MP Service
 468                                       Protocol, and it's the caller's
 469                                       responsibility to free the buffer with
 470                                       FreePool() service.
 471                                       In blocking mode, it is ready for
 472                                       consumption when the call returns. In
 473                                       non-blocking mode, it is ready when
 474                                       WaitEvent is signaled. The list of failed
 475                                       CPU is terminated by  END_OF_CPU_LIST.
 476
 477   @retval EFI_SUCCESS             In blocking mode, all APs have finished before
 478                                   the timeout expired.
 479   @retval EFI_SUCCESS             In non-blocking mode, function has been
 480                                   dispatched to all enabled APs.
 481   @retval EFI_UNSUPPORTED         A non-blocking mode request was made after the
 482                                   UEFI event EFI_EVENT_GROUP_READY_TO_BOOT was
 483                                   signaled.
 484   @retval EFI_DEVICE_ERROR        Caller processor is AP.
 485   @retval EFI_NOT_STARTED         No enabled APs exist in the system.
 486   @retval EFI_NOT_READY           Any enabled APs are busy.
 487   @retval EFI_TIMEOUT             In blocking mode, the timeout expired before
 488                                   all enabled APs have finished.
 489   @retval EFI_INVALID_PARAMETER   Procedure is NULL.
 490
 491 **/
 492 STATIC
 493 EFI_STATUS
 494 EFIAPI
 495 StartupAllAPs (
 496   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 497   IN  EFI_AP_PROCEDURE          Procedure,
 498   IN  BOOLEAN                   SingleThread,
 499   IN  EFI_EVENT                 WaitEvent               OPTIONAL,
 500   IN  UINTN                     TimeoutInMicroseconds,
 501   IN  VOID                      *ProcedureArgument      OPTIONAL,
 502   OUT UINTN                     **FailedCpuList         OPTIONAL
 503   )
 504 {
 505   EFI_STATUS  Status;
 506
 507   if (!IsCurrentProcessorBSP ()) {
 508     return EFI_DEVICE_ERROR;
 509   }
 510
 511   if ((mCpuMpData.NumberOfProcessors == 1) || (mCpuMpData.NumberOfEnabledProcessors == 1)) {
 512     return EFI_NOT_STARTED;
 513   }
 514
 515   if (Procedure == NULL) {
 516     return EFI_INVALID_PARAMETER;
 517   }
 518
 519   if ((WaitEvent != NULL) && !mNonBlockingModeAllowed) {
 520     return EFI_UNSUPPORTED;
 521   }
 522
 523   if (FailedCpuList != NULL) {
 524     mCpuMpData.FailedList = AllocateZeroPool (
 525                               (mCpuMpData.NumberOfProcessors + 1) *
 526                               sizeof (UINTN)
 527                               );
 528     if (mCpuMpData.FailedList == NULL) {
 529       return EFI_OUT_OF_RESOURCES;
 530     }
 531
 532     SetMemN (
 533       mCpuMpData.FailedList,
 534       (mCpuMpData.NumberOfProcessors + 1) *
 535       sizeof (UINTN),
 536       END_OF_CPU_LIST
 537       );
 538     mCpuMpData.FailedListIndex = 0;
 539     *FailedCpuList             = mCpuMpData.FailedList;
 540   }
 541
 542   StartupAllAPsPrepareState (SingleThread);
 543
 544   // If any enabled APs are busy (ignoring the BSP), return EFI_NOT_READY
 545   if (mCpuMpData.StartCount != (mCpuMpData.NumberOfEnabledProcessors - 1)) {
 546     return EFI_NOT_READY;
 547   }
 548
 549   if (WaitEvent != NULL) {
 550     Status = StartupAllAPsWithWaitEvent (
 551                Procedure,
 552                ProcedureArgument,
 553                WaitEvent,
 554                TimeoutInMicroseconds,
 555                SingleThread,
 556                FailedCpuList
 557                );
 558
 559     if (EFI_ERROR (Status) && (FailedCpuList != NULL)) {
 560       if (mCpuMpData.FailedListIndex == 0) {
 561         FreePool (*FailedCpuList);
 562         *FailedCpuList = NULL;
 563       }
 564     }
 565   } else {
 566     Status = StartupAllAPsNoWaitEvent (
 567                Procedure,
 568                ProcedureArgument,
 569                TimeoutInMicroseconds,
 570                SingleThread,
 571                FailedCpuList
 572                );
 573
 574     if (FailedCpuList != NULL) {
 575       if (mCpuMpData.FailedListIndex == 0) {
 576         FreePool (*FailedCpuList);
 577         *FailedCpuList = NULL;
 578       }
 579     }
 580   }
 581
 582   return Status;
 583 }
 584
 585 /**
 586   This service lets the caller get one enabled AP to execute a caller-provided
 587   function. The caller can request the BSP to either wait for the completion
 588   of the AP or just proceed with the next task by using the EFI event mechanism.
 589   See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blocking
 590   execution support.  This service may only be called from the BSP.
 591
 592   This function is used to dispatch one enabled AP to the function specified by
 593   Procedure passing in the argument specified by ProcedureArgument.  If WaitEvent
 594   is NULL, execution is in blocking mode. The BSP waits until the AP finishes or
 595   TimeoutInMicroSecondss expires. Otherwise, execution is in non-blocking mode.
 596   BSP proceeds to the next task without waiting for the AP. If a non-blocking mode
 597   is requested after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled,
 598   then EFI_UNSUPPORTED must be returned.
 599
 600   If the timeout specified by TimeoutInMicroseconds expires before the AP returns
 601   from Procedure, then execution of Procedure by the AP is terminated. The AP is
 602   available for subsequent calls to EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and
 603   EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 604
 605   @param[in]  This                    A pointer to the EFI_MP_SERVICES_PROTOCOL
 606                                       instance.
 607   @param[in]  Procedure               A pointer to the function to be run on
 608                                       enabled APs of the system. See type
 609                                       EFI_AP_PROCEDURE.
 610   @param[in]  ProcessorNumber         The handle number of the AP. The range is
 611                                       from 0 to the total number of logical
 612                                       processors minus 1. The total number of
 613                                       logical processors can be retrieved by
 614                                       EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 615   @param[in]  WaitEvent               The event created by the caller with CreateEvent()
 616                                       service.  If it is NULL, then execute in
 617                                       blocking mode. BSP waits until all APs finish
 618                                       or TimeoutInMicroseconds expires.  If it's
 619                                       not NULL, then execute in non-blocking mode.
 620                                       BSP requests the function specified by
 621                                       Procedure to be started on all the enabled
 622                                       APs, and go on executing immediately. If
 623                                       all return from Procedure or TimeoutInMicroseconds
 624                                       expires, this event is signaled. The BSP
 625                                       can use the CheckEvent() or WaitForEvent()
 626                                       services to check the state of event.  Type
 627                                       EFI_EVENT is defined in CreateEvent() in
 628                                       the Unified Extensible Firmware Interface
 629                                       Specification.
 630   @param[in]  TimeoutInMicroseconds   Indicates the time limit in microseconds for
 631                                       APs to return from Procedure, either for
 632                                       blocking or non-blocking mode. Zero means
 633                                       infinity.  If the timeout expires before
 634                                       all APs return from Procedure, then Procedure
 635                                       on the failed APs is terminated. All enabled
 636                                       APs are available for next function assigned
 637                                       by EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
 638                                       or EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
 639                                       If the timeout expires in blocking mode,
 640                                       BSP returns EFI_TIMEOUT.  If the timeout
 641                                       expires in non-blocking mode, WaitEvent
 642                                       is signaled with SignalEvent().
 643   @param[in]  ProcedureArgument       The parameter passed into Procedure for
 644                                       all APs.
 645   @param[out] Finished                If NULL, this parameter is ignored.  In
 646                                       blocking mode, this parameter is ignored.
 647                                       In non-blocking mode, if AP returns from
 648                                       Procedure before the timeout expires, its
 649                                       content is set to TRUE. Otherwise, the
 650                                       value is set to FALSE. The caller can
 651                                       determine if the AP returned from Procedure
 652                                       by evaluating this value.
 653
 654   @retval EFI_SUCCESS             In blocking mode, specified AP finished before
 655                                   the timeout expires.
 656   @retval EFI_SUCCESS             In non-blocking mode, the function has been
 657                                   dispatched to specified AP.
 658   @retval EFI_UNSUPPORTED         A non-blocking mode request was made after the
 659                                   UEFI event EFI_EVENT_GROUP_READY_TO_BOOT was
 660                                   signaled.
 661   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 662   @retval EFI_TIMEOUT             In blocking mode, the timeout expired before
 663                                   the specified AP has finished.
 664   @retval EFI_NOT_READY           The specified AP is busy.
 665   @retval EFI_NOT_FOUND           The processor with the handle specified by
 666                                   ProcessorNumber does not exist.
 667   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or disabled AP.
 668   @retval EFI_INVALID_PARAMETER   Procedure is NULL.
 669
 670 **/
 671 STATIC
 672 EFI_STATUS
 673 EFIAPI
 674 StartupThisAP (
 675   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 676   IN  EFI_AP_PROCEDURE          Procedure,
 677   IN  UINTN                     ProcessorNumber,
 678   IN  EFI_EVENT                 WaitEvent               OPTIONAL,
 679   IN  UINTN                     TimeoutInMicroseconds,
 680   IN  VOID                      *ProcedureArgument      OPTIONAL,
 681   OUT BOOLEAN                   *Finished               OPTIONAL
 682   )
 683 {
 684   EFI_STATUS   Status;
 685   UINTN        Timeout;
 686   CPU_AP_DATA  *CpuData;
 687
 688   if (!IsCurrentProcessorBSP ()) {
 689     return EFI_DEVICE_ERROR;
 690   }
 691
 692   if (Procedure == NULL) {
 693     return EFI_INVALID_PARAMETER;
 694   }
 695
 696   if (ProcessorNumber >= mCpuMpData.NumberOfProcessors) {
 697     return EFI_NOT_FOUND;
 698   }
 699
 700   CpuData = &mCpuMpData.CpuData[ProcessorNumber];
 701
 702   if (IsProcessorBSP (ProcessorNumber)) {
 703     return EFI_INVALID_PARAMETER;
 704   }
 705
 706   if (!IsProcessorEnabled (ProcessorNumber)) {
 707     return EFI_INVALID_PARAMETER;
 708   }
 709
 710   if ((GetApState (CpuData) != CpuStateIdle) &&
 711       (GetApState (CpuData) != CpuStateFinished))
 712   {
 713     return EFI_NOT_READY;
 714   }
 715
 716   if ((WaitEvent != NULL) && !mNonBlockingModeAllowed) {
 717     return EFI_UNSUPPORTED;
 718   }
 719
 720   Timeout = TimeoutInMicroseconds;
 721
 722   CpuData->Timeout       = TimeoutInMicroseconds;
 723   CpuData->TimeTaken     = 0;
 724   CpuData->TimeoutActive = (BOOLEAN)(TimeoutInMicroseconds != 0);
 725
 726   SetApProcedure (
 727     CpuData,
 728     Procedure,
 729     ProcedureArgument
 730     );
 731
 732   Status = DispatchCpu (ProcessorNumber);
 733   if (EFI_ERROR (Status)) {
 734     CpuData->State = CpuStateIdle;
 735     return EFI_NOT_READY;
 736   }
 737
 738   if (WaitEvent != NULL) {
 739     // Non Blocking
 740     if (Finished != NULL) {
 741       CpuData->SingleApFinished = Finished;
 742       *Finished                 = FALSE;
 743     }
 744
 745     CpuData->WaitEvent = WaitEvent;
 746     Status             = gBS->SetTimer (
 747                                 CpuData->CheckThisAPEvent,
 748                                 TimerPeriodic,
 749                                 POLL_INTERVAL_US
 750                                 );
 751
 752     return EFI_SUCCESS;
 753   }
 754
 755   // Blocking
 756   while (TRUE) {
 757     if (GetApState (CpuData) == CpuStateFinished) {
 758       CpuData->State = CpuStateIdle;
 759       break;
 760     }
 761
 762     if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
 763       return EFI_TIMEOUT;
 764     }
 765
 766     Timeout -= CalculateAndStallInterval (Timeout);
 767   }
 768
 769   return EFI_SUCCESS;
 770 }
 771
 772 /**
 773   This service switches the requested AP to be the BSP from that point onward.
 774   This service changes the BSP for all purposes.   This call can only be
 775   performed by the current BSP.
 776
 777   This service switches the requested AP to be the BSP from that point onward.
 778   This service changes the BSP for all purposes. The new BSP can take over the
 779   execution of the old BSP and continue seamlessly from where the old one left
 780   off. This service may not be supported after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT
 781   is signaled.
 782
 783   If the BSP cannot be switched prior to the return from this service, then
 784   EFI_UNSUPPORTED must be returned.
 785
 786   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 787   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 788                                BSP. The range is from 0 to the total number of
 789                                logical processors minus 1. The total number of
 790                                logical processors can be retrieved by
 791                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 792   @param[in] EnableOldBSP      If TRUE, then the old BSP will be listed as an
 793                                enabled AP. Otherwise, it will be disabled.
 794
 795   @retval EFI_SUCCESS             BSP successfully switched.
 796   @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed prior to
 797                                   this service returning.
 798   @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
 799   @retval EFI_SUCCESS             The calling processor is an AP.
 800   @retval EFI_NOT_FOUND           The processor with the handle specified by
 801                                   ProcessorNumber does not exist.
 802   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BSP or
 803                                   a disabled AP.
 804   @retval EFI_NOT_READY           The specified AP is busy.
 805
 806 **/
 807 STATIC
 808 EFI_STATUS
 809 EFIAPI
 810 SwitchBSP (
 811   IN EFI_MP_SERVICES_PROTOCOL  *This,
 812   IN  UINTN                    ProcessorNumber,
 813   IN  BOOLEAN                  EnableOldBSP
 814   )
 815 {
 816   return EFI_UNSUPPORTED;
 817 }
 818
 819 /**
 820   This service lets the caller enable or disable an AP from this point onward.
 821   This service may only be called from the BSP.
 822
 823   This service allows the caller enable or disable an AP from this point onward.
 824   The caller can optionally specify the health status of the AP by Health. If
 825   an AP is being disabled, then the state of the disabled AP is implementation
 826   dependent. If an AP is enabled, then the implementation must guarantee that a
 827   complete initialization sequence is performed on the AP, so the AP is in a state
 828   that is compatible with an MP operating system. This service may not be supported
 829   after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled.
 830
 831   If the enable or disable AP operation cannot be completed prior to the return
 832   from this service, then EFI_UNSUPPORTED must be returned.
 833
 834   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 835   @param[in] ProcessorNumber   The handle number of AP that is to become the new
 836                                BSP. The range is from 0 to the total number of
 837                                logical processors minus 1. The total number of
 838                                logical processors can be retrieved by
 839                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 840   @param[in] EnableAP          Specifies the new state for the processor for
 841                                enabled, FALSE for disabled.
 842   @param[in] HealthFlag        If not NULL, a pointer to a value that specifies
 843                                the new health status of the AP. This flag
 844                                corresponds to StatusFlag defined in
 845                                EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo(). Only
 846                                the PROCESSOR_HEALTH_STATUS_BIT is used. All other
 847                                bits are ignored.  If it is NULL, this parameter
 848                                is ignored.
 849
 850   @retval EFI_SUCCESS             The specified AP was enabled or disabled successfully.
 851   @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be completed
 852                                   prior to this service returning.
 853   @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not supported.
 854   @retval EFI_DEVICE_ERROR        The calling processor is an AP.
 855   @retval EFI_NOT_FOUND           Processor with the handle specified by ProcessorNumber
 856                                   does not exist.
 857   @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.
 858
 859 **/
 860 STATIC
 861 EFI_STATUS
 862 EFIAPI
 863 EnableDisableAP (
 864   IN  EFI_MP_SERVICES_PROTOCOL  *This,
 865   IN  UINTN                     ProcessorNumber,
 866   IN  BOOLEAN                   EnableAP,
 867   IN  UINT32                    *HealthFlag OPTIONAL
 868   )
 869 {
 870   UINTN        StatusFlag;
 871   CPU_AP_DATA  *CpuData;
 872
 873   StatusFlag = mCpuMpData.CpuData[ProcessorNumber].Info.StatusFlag;
 874   CpuData    = &mCpuMpData.CpuData[ProcessorNumber];
 875
 876   if (!IsCurrentProcessorBSP ()) {
 877     return EFI_DEVICE_ERROR;
 878   }
 879
 880   if (ProcessorNumber >= mCpuMpData.NumberOfProcessors) {
 881     return EFI_NOT_FOUND;
 882   }
 883
 884   if (IsProcessorBSP (ProcessorNumber)) {
 885     return EFI_INVALID_PARAMETER;
 886   }
 887
 888   if (GetApState (CpuData) != CpuStateIdle) {
 889     return EFI_UNSUPPORTED;
 890   }
 891
 892   if (EnableAP) {
 893     if (!IsProcessorEnabled (ProcessorNumber)) {
 894       mCpuMpData.NumberOfEnabledProcessors++;
 895     }
 896
 897     StatusFlag |= PROCESSOR_ENABLED_BIT;
 898   } else {
 899     if (IsProcessorEnabled (ProcessorNumber) && !IsProcessorBSP (ProcessorNumber)) {
 900       mCpuMpData.NumberOfEnabledProcessors--;
 901     }
 902
 903     StatusFlag &= ~PROCESSOR_ENABLED_BIT;
 904   }
 905
 906   if ((HealthFlag != NULL) && !IsProcessorBSP (ProcessorNumber)) {
 907     StatusFlag &= ~PROCESSOR_HEALTH_STATUS_BIT;
 908     StatusFlag |= (*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT);
 909   }
 910
 911   mCpuMpData.CpuData[ProcessorNumber].Info.StatusFlag = StatusFlag;
 912   return EFI_SUCCESS;
 913 }
 914
 915 /**
 916   This return the handle number for the calling processor.  This service may be
 917   called from the BSP and APs.
 918
 919   This service returns the processor handle number for the calling processor.
 920   The returned value is in the range from 0 to the total number of logical
 921   processors minus 1. The total number of logical processors can be retrieved
 922   with EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors(). This service may be
 923   called from the BSP and APs. If ProcessorNumber is NULL, then EFI_INVALID_PARAMETER
 924   is returned. Otherwise, the current processors handle number is returned in
 925   ProcessorNumber, and EFI_SUCCESS is returned.
 926
 927   @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
 928   @param[out] ProcessorNumber  The handle number of AP that is to become the new
 929                                BSP. The range is from 0 to the total number of
 930                                logical processors minus 1. The total number of
 931                                logical processors can be retrieved by
 932                                EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
 933
 934   @retval EFI_SUCCESS             The current processor handle number was returned
 935                                   in ProcessorNumber.
 936   @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.
 937
 938 **/
 939 STATIC
 940 EFI_STATUS
 941 EFIAPI
 942 WhoAmI (
 943   IN EFI_MP_SERVICES_PROTOCOL  *This,
 944   OUT UINTN                    *ProcessorNumber
 945   )
 946 {
 947   UINTN   Index;
 948   UINT64  ProcessorId;
 949
 950   if (ProcessorNumber == NULL) {
 951     return EFI_INVALID_PARAMETER;
 952   }
 953
 954   ProcessorId = GET_MPIDR_AFFINITY_BITS (ArmReadMpidr ());
 955   for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
 956     if (ProcessorId == gProcessorIDs[Index]) {
 957       *ProcessorNumber = Index;
 958       break;
 959     }
 960   }
 961
 962   return EFI_SUCCESS;
 963 }
 964
 965 STATIC EFI_MP_SERVICES_PROTOCOL  mMpServicesProtocol = {
 966   GetNumberOfProcessors,
 967   GetProcessorInfo,
 968   StartupAllAPs,
 969   StartupThisAP,
 970   SwitchBSP,
 971   EnableDisableAP,
 972   WhoAmI
 973 };
 974
 975 /** Adds the specified processor the list of failed processors.
 976
 977    @param ProcessorIndex The processor index to add.
 978    @param ApState        Processor state.
 979
 980 **/
 981 STATIC
 982 VOID
 983 AddProcessorToFailedList (
 984   UINTN      ProcessorIndex,
 985   CPU_STATE  ApState
 986   )
 987 {
 988   UINTN    Index;
 989   BOOLEAN  Found;
 990
 991   Found = FALSE;
 992
 993   if ((mCpuMpData.FailedList == NULL) ||
 994       (ApState == CpuStateIdle) ||
 995       (ApState == CpuStateFinished) ||
 996       IsProcessorBSP (ProcessorIndex))
 997   {
 998     return;
 999   }
1000
1001   // If we are retrying make sure we don't double count
1002   for (Index = 0; Index < mCpuMpData.FailedListIndex; Index++) {
1003     if (mCpuMpData.FailedList[Index] == ProcessorIndex) {
1004       Found = TRUE;
1005       break;
1006     }
1007   }
1008
1009   /* If the CPU isn't already in the FailedList, add it */
1010   if (!Found) {
1011     mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] = ProcessorIndex;
1012   }
1013 }
1014
1015 /** Handles the StartupAllAPs case where the timeout has occurred.
1016
1017 **/
1018 STATIC
1019 VOID
1020 ProcessStartupAllAPsTimeout (
1021   VOID
1022   )
1023 {
1024   CPU_AP_DATA  *CpuData;
1025   UINTN        Index;
1026
1027   if (mCpuMpData.FailedList == NULL) {
1028     return;
1029   }
1030
1031   for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
1032     CpuData = &mCpuMpData.CpuData[Index];
1033     if (IsProcessorBSP (Index)) {
1034       // Skip BSP
1035       continue;
1036     }
1037
1038     if (!IsProcessorEnabled (Index)) {
1039       // Skip Disabled processors
1040       continue;
1041     }
1042
1043     CpuData = &mCpuMpData.CpuData[Index];
1044     AddProcessorToFailedList (Index, GetApState (CpuData));
1045   }
1046 }
1047
1048 /** Updates the status of the APs.
1049
1050    @param[in] ProcessorIndex The index of the AP to update.
1051 **/
1052 STATIC
1053 VOID
1054 UpdateApStatus (
1055   IN UINTN  ProcessorIndex
1056   )
1057 {
1058   EFI_STATUS   Status;
1059   CPU_AP_DATA  *CpuData;
1060   CPU_AP_DATA  *NextCpuData;
1061   CPU_STATE    State;
1062   UINTN        NextNumber;
1063
1064   CpuData = &mCpuMpData.CpuData[ProcessorIndex];
1065
1066   if (IsProcessorBSP (ProcessorIndex)) {
1067     // Skip BSP
1068     return;
1069   }
1070
1071   if (!IsProcessorEnabled (ProcessorIndex)) {
1072     // Skip Disabled processors
1073     return;
1074   }
1075
1076   State = GetApState (CpuData);
1077
1078   switch (State) {
1079     case CpuStateFinished:
1080       if (mCpuMpData.SingleThread) {
1081         Status = GetNextBlockedNumber (&NextNumber);
1082         if (!EFI_ERROR (Status)) {
1083           NextCpuData = &mCpuMpData.CpuData[NextNumber];
1084
1085           NextCpuData->State = CpuStateReady;
1086
1087           SetApProcedure (
1088             NextCpuData,
1089             mCpuMpData.Procedure,
1090             mCpuMpData.ProcedureArgument
1091             );
1092
1093           Status = DispatchCpu (NextNumber);
1094           if (!EFI_ERROR (Status)) {
1095             mCpuMpData.StartCount++;
1096           } else {
1097             AddProcessorToFailedList (NextNumber, NextCpuData->State);
1098           }
1099         }
1100       }
1101
1102       CpuData->State = CpuStateIdle;
1103       mCpuMpData.FinishCount++;
1104       break;
1105
1106     default:
1107       break;
1108   }
1109 }
1110
1111 /**
1112   If a timeout is specified in StartupAllAps(), a timer is set, which invokes
1113   this procedure periodically to check whether all APs have finished.
1114
1115   @param[in] Event   The WaitEvent the user supplied.
1116   @param[in] Context The event context.
1117 **/
1118 STATIC
1119 VOID
1120 EFIAPI
1121 CheckAllAPsStatus (
1122   IN  EFI_EVENT  Event,
1123   IN  VOID       *Context
1124   )
1125 {
1126   EFI_STATUS  Status;
1127   UINTN       Index;
1128
1129   mCpuMpData.AllTimeTaken += POLL_INTERVAL_US;
1130
1131   for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
1132     UpdateApStatus (Index);
1133   }
1134
1135   if (mCpuMpData.AllTimeoutActive && (mCpuMpData.AllTimeTaken > mCpuMpData.AllTimeout)) {
1136     ProcessStartupAllAPsTimeout ();
1137
1138     // Force terminal exit
1139     mCpuMpData.FinishCount = mCpuMpData.StartCount;
1140   }
1141
1142   if (mCpuMpData.FinishCount != mCpuMpData.StartCount) {
1143     return;
1144   }
1145
1146   gBS->SetTimer (
1147          mCpuMpData.CheckAllAPsEvent,
1148          TimerCancel,
1149          0
1150          );
1151
1152   if (mCpuMpData.FailedListIndex == 0) {
1153     if (mCpuMpData.FailedList != NULL) {
1154       // Since we don't have the original `FailedCpuList`
1155       // pointer here to set to NULL, don't free the
1156       // memory.
1157     }
1158   }
1159
1160   Status = gBS->SignalEvent (mCpuMpData.AllWaitEvent);
1161   ASSERT_EFI_ERROR (Status);
1162   mCpuMpData.AllWaitEvent = NULL;
1163 }
1164
1165 /** Invoked periodically via a timer to check the state of the processor.
1166
1167    @param Event   The event supplied by the timer expiration.
1168    @param Context The processor context.
1169
1170 **/
1171 STATIC
1172 VOID
1173 EFIAPI
1174 CheckThisAPStatus (
1175   IN  EFI_EVENT  Event,
1176   IN  VOID       *Context
1177   )
1178 {
1179   EFI_STATUS   Status;
1180   CPU_AP_DATA  *CpuData;
1181   CPU_STATE    State;
1182
1183   CpuData = Context;
1184
1185   CpuData->TimeTaken += POLL_INTERVAL_US;
1186
1187   State = GetApState (CpuData);
1188
1189   if (State == CpuStateFinished) {
1190     Status = gBS->SetTimer (CpuData->CheckThisAPEvent, TimerCancel, 0);
1191     ASSERT_EFI_ERROR (Status);
1192
1193     if (CpuData->SingleApFinished != NULL) {
1194       *(CpuData->SingleApFinished) = TRUE;
1195     }
1196
1197     if (CpuData->WaitEvent != NULL) {
1198       Status = gBS->SignalEvent (CpuData->WaitEvent);
1199       ASSERT_EFI_ERROR (Status);
1200     }
1201
1202     CpuData->State = CpuStateIdle;
1203   }
1204
1205   if (CpuData->TimeoutActive && (CpuData->TimeTaken > CpuData->Timeout)) {
1206     Status = gBS->SetTimer (CpuData->CheckThisAPEvent, TimerCancel, 0);
1207     if (CpuData->WaitEvent != NULL) {
1208       Status = gBS->SignalEvent (CpuData->WaitEvent);
1209       ASSERT_EFI_ERROR (Status);
1210       CpuData->WaitEvent = NULL;
1211     }
1212   }
1213 }
1214
1215 /**
1216   This function is called by all processors (both BSP and AP) once and collects
1217   MP related data.
1218
1219   @param BSP            TRUE if the processor is the BSP.
1220   @param Mpidr          The MPIDR for the specified processor. This should be
1221                         the full MPIDR and not only the affinity bits.
1222   @param ProcessorIndex The index of the processor.
1223
1224   @return EFI_SUCCESS if the data for the processor collected and filled in.
1225
1226 **/
1227 STATIC
1228 EFI_STATUS
1229 FillInProcessorInformation (
1230   IN BOOLEAN  BSP,
1231   IN UINTN    Mpidr,
1232   IN UINTN    ProcessorIndex
1233   )
1234 {
1235   EFI_PROCESSOR_INFORMATION  *CpuInfo;
1236
1237   CpuInfo = &mCpuMpData.CpuData[ProcessorIndex].Info;
1238
1239   CpuInfo->ProcessorId = GET_MPIDR_AFFINITY_BITS (Mpidr);
1240   CpuInfo->StatusFlag  = PROCESSOR_ENABLED_BIT | PROCESSOR_HEALTH_STATUS_BIT;
1241
1242   if (BSP) {
1243     CpuInfo->StatusFlag |= PROCESSOR_AS_BSP_BIT;
1244   }
1245
1246   if ((Mpidr & MPIDR_MT_BIT) > 0) {
1247     CpuInfo->Location.Package = GET_MPIDR_AFF2 (Mpidr);
1248     CpuInfo->Location.Core    = GET_MPIDR_AFF1 (Mpidr);
1249     CpuInfo->Location.Thread  = GET_MPIDR_AFF0 (Mpidr);
1250
1251     CpuInfo->ExtendedInformation.Location2.Package = GET_MPIDR_AFF3 (Mpidr);
1252     CpuInfo->ExtendedInformation.Location2.Die     = GET_MPIDR_AFF2 (Mpidr);
1253     CpuInfo->ExtendedInformation.Location2.Core    = GET_MPIDR_AFF1 (Mpidr);
1254     CpuInfo->ExtendedInformation.Location2.Thread  = GET_MPIDR_AFF0 (Mpidr);
1255   } else {
1256     CpuInfo->Location.Package = GET_MPIDR_AFF1 (Mpidr);
1257     CpuInfo->Location.Core    = GET_MPIDR_AFF0 (Mpidr);
1258     CpuInfo->Location.Thread  = 0;
1259
1260     CpuInfo->ExtendedInformation.Location2.Package = GET_MPIDR_AFF2 (Mpidr);
1261     CpuInfo->ExtendedInformation.Location2.Die     = GET_MPIDR_AFF1 (Mpidr);
1262     CpuInfo->ExtendedInformation.Location2.Core    = GET_MPIDR_AFF0 (Mpidr);
1263     CpuInfo->ExtendedInformation.Location2.Thread  = 0;
1264   }
1265
1266   mCpuMpData.CpuData[ProcessorIndex].State = BSP ? CpuStateBusy : CpuStateIdle;
1267
1268   mCpuMpData.CpuData[ProcessorIndex].Procedure = NULL;
1269   mCpuMpData.CpuData[ProcessorIndex].Parameter = NULL;
1270
1271   return EFI_SUCCESS;
1272 }
1273
1274 /** Initializes the MP Services system data
1275
1276    @param NumberOfProcessors The number of processors, both BSP and AP.
1277    @param CoreInfo           CPU information gathered earlier during boot.
1278
1279 **/
1280 STATIC
1281 EFI_STATUS
1282 MpServicesInitialize (
1283   IN   UINTN                NumberOfProcessors,
1284   IN   CONST ARM_CORE_INFO  *CoreInfo
1285   )
1286 {
1287   EFI_STATUS  Status;
1288   UINTN       Index;
1289   EFI_EVENT   ReadyToBootEvent;
1290   BOOLEAN     IsBsp;
1291
1292   //
1293   // Clear the data structure area first.
1294   //
1295   ZeroMem (&mCpuMpData, sizeof (CPU_MP_DATA));
1296   //
1297   // First BSP fills and inits all known values, including its own records.
1298   //
1299   mCpuMpData.NumberOfProcessors        = NumberOfProcessors;
1300   mCpuMpData.NumberOfEnabledProcessors = NumberOfProcessors;
1301
1302   mCpuMpData.CpuData = AllocateZeroPool (
1303                          mCpuMpData.NumberOfProcessors * sizeof (CPU_AP_DATA)
1304                          );
1305
1306   if (mCpuMpData.CpuData == NULL) {
1307     return EFI_OUT_OF_RESOURCES;
1308   }
1309
1310   /* Allocate one extra for the sentinel entry at the end */
1311   gProcessorIDs = AllocateZeroPool ((mCpuMpData.NumberOfProcessors + 1) * sizeof (UINT64));
1312   ASSERT (gProcessorIDs != NULL);
1313
1314   Status = gBS->CreateEvent (
1315                   EVT_TIMER | EVT_NOTIFY_SIGNAL,
1316                   TPL_CALLBACK,
1317                   CheckAllAPsStatus,
1318                   NULL,
1319                   &mCpuMpData.CheckAllAPsEvent
1320                   );
1321   ASSERT_EFI_ERROR (Status);
1322
1323   gApStacksBase = AllocatePages (
1324                     EFI_SIZE_TO_PAGES (
1325                       mCpuMpData.NumberOfProcessors *
1326                       gApStackSize
1327                       )
1328                     );
1329   ASSERT (gApStacksBase != NULL);
1330
1331   for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
1332     if (GET_MPIDR_AFFINITY_BITS (ArmReadMpidr ()) == CoreInfo[Index].Mpidr) {
1333       IsBsp = TRUE;
1334     } else {
1335       IsBsp = FALSE;
1336     }
1337
1338     FillInProcessorInformation (IsBsp, CoreInfo[Index].Mpidr, Index);
1339
1340     gProcessorIDs[Index] = mCpuMpData.CpuData[Index].Info.ProcessorId;
1341
1342     Status = gBS->CreateEvent (
1343                     EVT_TIMER | EVT_NOTIFY_SIGNAL,
1344                     TPL_CALLBACK,
1345                     CheckThisAPStatus,
1346                     (VOID *)&mCpuMpData.CpuData[Index],
1347                     &mCpuMpData.CpuData[Index].CheckThisAPEvent
1348                     );
1349     ASSERT_EFI_ERROR (Status);
1350   }
1351
1352   gProcessorIDs[Index] = MAX_UINT64;
1353
1354   gTcr   = ArmGetTCR ();
1355   gMair  = ArmGetMAIR ();
1356   gTtbr0 = ArmGetTTBR0BaseAddress ();
1357
1358   //
1359   // The global pointer variables as well as the gProcessorIDs array contents
1360   // are accessed by the other cores so we must clean them to the PoC
1361   //
1362   WriteBackDataCacheRange (&gProcessorIDs, sizeof (UINT64 *));
1363   WriteBackDataCacheRange (&gApStacksBase, sizeof (UINT64 *));
1364
1365   WriteBackDataCacheRange (
1366     gProcessorIDs,
1367     (mCpuMpData.NumberOfProcessors + 1) * sizeof (UINT64)
1368     );
1369
1370   mNonBlockingModeAllowed = TRUE;
1371
1372   Status = EfiCreateEventReadyToBootEx (
1373              TPL_CALLBACK,
1374              ReadyToBootSignaled,
1375              NULL,
1376              &ReadyToBootEvent
1377              );
1378   ASSERT_EFI_ERROR (Status);
1379
1380   return EFI_SUCCESS;
1381 }
1382
1383 /**
1384   Event notification function called when the EFI_EVENT_GROUP_READY_TO_BOOT is
1385   signaled. After this point, non-blocking mode is no longer allowed.
1386
1387   @param  Event     Event whose notification function is being invoked.
1388   @param  Context   The pointer to the notification function's context,
1389                     which is implementation-dependent.
1390
1391 **/
1392 STATIC
1393 VOID
1394 EFIAPI
1395 ReadyToBootSignaled (
1396   IN  EFI_EVENT  Event,
1397   IN  VOID       *Context
1398   )
1399 {
1400   mNonBlockingModeAllowed = FALSE;
1401 }
1402
1403 /** Initialize multi-processor support.
1404
1405   @param ImageHandle  Image handle.
1406   @param SystemTable  System table.
1407
1408   @return EFI_SUCCESS on success, or an error code.
1409
1410 **/
1411 EFI_STATUS
1412 EFIAPI
1413 ArmPsciMpServicesDxeInitialize (
1414   IN EFI_HANDLE        ImageHandle,
1415   IN EFI_SYSTEM_TABLE  *SystemTable
1416   )
1417 {
1418   EFI_STATUS                 Status;
1419   EFI_HANDLE                 Handle;
1420   UINTN                      MaxCpus;
1421   EFI_LOADED_IMAGE_PROTOCOL  *Image;
1422   EFI_HOB_GENERIC_HEADER     *Hob;
1423   VOID                       *HobData;
1424   UINTN                      HobDataSize;
1425   CONST ARM_CORE_INFO        *CoreInfo;
1426
1427   MaxCpus = 1;
1428
1429   Status = gBS->HandleProtocol (
1430                   ImageHandle,
1431                   &gEfiLoadedImageProtocolGuid,
1432                   (VOID **)&Image
1433                   );
1434   ASSERT_EFI_ERROR (Status);
1435
1436   //
1437   // Parts of the code in this driver may be executed by other cores running
1438   // with the MMU off so we need to ensure that everything is clean to the
1439   // point of coherency (PoC)
1440   //
1441   WriteBackDataCacheRange (Image->ImageBase, Image->ImageSize);
1442
1443   Hob = GetFirstGuidHob (&gArmMpCoreInfoGuid);
1444   if (Hob != NULL) {
1445     HobData     = GET_GUID_HOB_DATA (Hob);
1446     HobDataSize = GET_GUID_HOB_DATA_SIZE (Hob);
1447     CoreInfo    = (ARM_CORE_INFO *)HobData;
1448     MaxCpus     = HobDataSize / sizeof (ARM_CORE_INFO);
1449   }
1450
1451   if (MaxCpus == 1) {
1452     DEBUG ((DEBUG_WARN, "Trying to use EFI_MP_SERVICES_PROTOCOL on a UP system"));
1453     // We are not MP so nothing to do
1454     return EFI_NOT_FOUND;
1455   }
1456
1457   Status = MpServicesInitialize (MaxCpus, CoreInfo);
1458   if (Status != EFI_SUCCESS) {
1459     ASSERT_EFI_ERROR (Status);
1460     return Status;
1461   }
1462
1463   //
1464   // Now install the MP services protocol.
1465   //
1466   Handle = NULL;
1467   Status = gBS->InstallMultipleProtocolInterfaces (
1468                   &Handle,
1469                   &gEfiMpServiceProtocolGuid,
1470                   &mMpServicesProtocol,
1471                   NULL
1472                   );
1473   ASSERT_EFI_ERROR (Status);
1474
1475   return Status;
1476 }
1477
1478 /** AP exception handler.
1479
1480   @param InterruptType The AArch64 CPU exception type.
1481   @param SystemContext System context.
1482
1483 **/
1484 STATIC
1485 VOID
1486 EFIAPI
1487 ApExceptionHandler (
1488   IN CONST EFI_EXCEPTION_TYPE  InterruptType,
1489   IN CONST EFI_SYSTEM_CONTEXT  SystemContext
1490   )
1491 {
1492   ARM_SMC_ARGS  Args;
1493   UINT64        Mpidr;
1494   UINTN         Index;
1495   UINTN         ProcessorIndex;
1496
1497   Mpidr = GET_MPIDR_AFFINITY_BITS (ArmReadMpidr ());
1498
1499   Index          = 0;
1500   ProcessorIndex = MAX_UINT64;
1501
1502   do {
1503     if (gProcessorIDs[Index] == Mpidr) {
1504       ProcessorIndex = Index;
1505       break;
1506     }
1507
1508     Index++;
1509   } while (gProcessorIDs[Index] != MAX_UINT64);
1510
1511   if (ProcessorIndex != MAX_UINT64) {
1512     mCpuMpData.CpuData[ProcessorIndex].State = CpuStateFinished;
1513     ArmDataMemoryBarrier ();
1514   }
1515
1516   Args.Arg0 = ARM_SMC_ID_PSCI_CPU_OFF;
1517   ArmCallSmc (&Args);
1518
1519   /* Should never be reached */
1520   ASSERT (FALSE);
1521   CpuDeadLoop ();
1522 }
1523
1524 /** C entry-point for the AP.
1525     This function gets called from the assembly function ApEntryPoint.
1526
1527 **/
1528 VOID
1529 ApProcedure (
1530   VOID
1531   )
1532 {
1533   ARM_SMC_ARGS      Args;
1534   EFI_AP_PROCEDURE  UserApProcedure;
1535   VOID              *UserApParameter;
1536   UINTN             ProcessorIndex;
1537
1538   ProcessorIndex = 0;
1539
1540   WhoAmI (&mMpServicesProtocol, &ProcessorIndex);
1541
1542   /* Fetch the user-supplied procedure and parameter to execute */
1543   UserApProcedure = mCpuMpData.CpuData[ProcessorIndex].Procedure;
1544   UserApParameter = mCpuMpData.CpuData[ProcessorIndex].Parameter;
1545
1546   InitializeCpuExceptionHandlers (NULL);
1547   RegisterCpuInterruptHandler (EXCEPT_AARCH64_SYNCHRONOUS_EXCEPTIONS, ApExceptionHandler);
1548   RegisterCpuInterruptHandler (EXCEPT_AARCH64_IRQ, ApExceptionHandler);
1549   RegisterCpuInterruptHandler (EXCEPT_AARCH64_FIQ, ApExceptionHandler);
1550   RegisterCpuInterruptHandler (EXCEPT_AARCH64_SERROR, ApExceptionHandler);
1551
1552   UserApProcedure (UserApParameter);
1553
1554   mCpuMpData.CpuData[ProcessorIndex].State = CpuStateFinished;
1555
1556   ArmDataMemoryBarrier ();
1557
1558   /* Since we're finished with this AP, turn it off */
1559   Args.Arg0 = ARM_SMC_ID_PSCI_CPU_OFF;
1560   ArmCallSmc (&Args);
1561
1562   /* Should never be reached */
1563   ASSERT (FALSE);
1564   CpuDeadLoop ();
1565 }
1566
1567 /** Returns whether the processor executing this function is the BSP.
1568
1569     @return Whether the current processor is the BSP.
1570 **/
1571 STATIC
1572 BOOLEAN
1573 IsCurrentProcessorBSP (
1574   VOID
1575   )
1576 {
1577   EFI_STATUS  Status;
1578   UINTN       ProcessorIndex;
1579
1580   Status = WhoAmI (&mMpServicesProtocol, &ProcessorIndex);
1581   if (EFI_ERROR (Status)) {
1582     ASSERT_EFI_ERROR (Status);
1583     return FALSE;
1584   }
1585
1586   return IsProcessorBSP (ProcessorIndex);
1587 }
1588
1589 /** Returns whether the specified processor is enabled.
1590
1591    @param[in] ProcessorIndex The index of the processor to check.
1592
1593    @return TRUE if the processor is enabled, FALSE otherwise.
1594 **/
1595 STATIC
1596 BOOLEAN
1597 IsProcessorEnabled (
1598   UINTN  ProcessorIndex
1599   )
1600 {
1601   EFI_PROCESSOR_INFORMATION  *CpuInfo;
1602
1603   CpuInfo = &mCpuMpData.CpuData[ProcessorIndex].Info;
1604
1605   return (CpuInfo->StatusFlag & PROCESSOR_ENABLED_BIT) != 0;
1606 }
1607
1608 /** Sets up the state for the StartupAllAPs function.
1609
1610    @param SingleThread Whether the APs will execute sequentially.
1611
1612 **/
1613 STATIC
1614 VOID
1615 StartupAllAPsPrepareState (
1616   IN BOOLEAN  SingleThread
1617   )
1618 {
1619   UINTN        Index;
1620   CPU_STATE    APInitialState;
1621   CPU_AP_DATA  *CpuData;
1622
1623   mCpuMpData.FinishCount  = 0;
1624   mCpuMpData.StartCount   = 0;
1625   mCpuMpData.SingleThread = SingleThread;
1626
1627   APInitialState = CpuStateReady;
1628
1629   for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
1630     CpuData = &mCpuMpData.CpuData[Index];
1631
1632     //
1633     // Get APs prepared, and put failing APs into FailedCpuList.
1634     // If "SingleThread", only 1 AP will put into ready state, other AP will be
1635     // put into ready state 1 by 1, until the previous 1 finished its task.
1636     // If not "SingleThread", all APs are put into ready state from the
1637     // beginning
1638     //
1639
1640     if (IsProcessorBSP (Index)) {
1641       // Skip BSP
1642       continue;
1643     }
1644
1645     if (!IsProcessorEnabled (Index)) {
1646       // Skip Disabled processors
1647       if (mCpuMpData.FailedList != NULL) {
1648         mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] = Index;
1649       }
1650
1651       continue;
1652     }
1653
1654     // If any APs finished after timing out, reset state to Idle
1655     if (GetApState (CpuData) == CpuStateFinished) {
1656       CpuData->State = CpuStateIdle;
1657     }
1658
1659     if (GetApState (CpuData) != CpuStateIdle) {
1660       // Skip busy processors
1661       if (mCpuMpData.FailedList != NULL) {
1662         mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] = Index;
1663       }
1664     }
1665
1666     CpuData->State = APInitialState;
1667
1668     mCpuMpData.StartCount++;
1669     if (SingleThread) {
1670       APInitialState = CpuStateBlocked;
1671     }
1672   }
1673 }
1674
1675 /** Handles execution of StartupAllAPs when a WaitEvent has been specified.
1676
1677   @param Procedure         The user-supplied procedure.
1678   @param ProcedureArgument The user-supplied procedure argument.
1679   @param WaitEvent         The wait event to be signaled when the work is
1680                            complete or a timeout has occurred.
1681   @param TimeoutInMicroseconds The timeout for the work to be completed. Zero
1682                                indicates an infinite timeout.
1683   @param SingleThread          Whether the APs will execute sequentially.
1684   @param FailedCpuList         User-supplied pointer for list of failed CPUs.
1685
1686    @return EFI_SUCCESS on success.
1687 **/
1688 STATIC
1689 EFI_STATUS
1690 StartupAllAPsWithWaitEvent (
1691   IN EFI_AP_PROCEDURE  Procedure,
1692   IN VOID              *ProcedureArgument,
1693   IN EFI_EVENT         WaitEvent,
1694   IN UINTN             TimeoutInMicroseconds,
1695   IN BOOLEAN           SingleThread,
1696   IN UINTN             **FailedCpuList
1697   )
1698 {
1699   EFI_STATUS   Status;
1700   UINTN        Index;
1701   CPU_AP_DATA  *CpuData;
1702
1703   for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
1704     CpuData = &mCpuMpData.CpuData[Index];
1705     if (IsProcessorBSP (Index)) {
1706       // Skip BSP
1707       continue;
1708     }
1709
1710     if (!IsProcessorEnabled (Index)) {
1711       // Skip Disabled processors
1712       continue;
1713     }
1714
1715     if (GetApState (CpuData) == CpuStateReady) {
1716       SetApProcedure (CpuData, Procedure, ProcedureArgument);
1717       if ((mCpuMpData.StartCount == 0) || !SingleThread) {
1718         Status = DispatchCpu (Index);
1719         if (EFI_ERROR (Status)) {
1720           AddProcessorToFailedList (Index, CpuData->State);
1721           break;
1722         }
1723       }
1724     }
1725   }
1726
1727   if (EFI_ERROR (Status)) {
1728     return EFI_NOT_READY;
1729   }
1730
1731   //
1732   // Save data into private data structure, and create timer to poll AP state
1733   // before exiting
1734   //
1735   mCpuMpData.Procedure         = Procedure;
1736   mCpuMpData.ProcedureArgument = ProcedureArgument;
1737   mCpuMpData.AllWaitEvent      = WaitEvent;
1738   mCpuMpData.AllTimeout        = TimeoutInMicroseconds;
1739   mCpuMpData.AllTimeTaken      = 0;
1740   mCpuMpData.AllTimeoutActive  = (BOOLEAN)(TimeoutInMicroseconds != 0);
1741   Status                       = gBS->SetTimer (
1742                                         mCpuMpData.CheckAllAPsEvent,
1743                                         TimerPeriodic,
1744                                         POLL_INTERVAL_US
1745                                         );
1746
1747   return Status;
1748 }
1749
1750 /** Handles execution of StartupAllAPs when no wait event has been specified.
1751
1752   @param Procedure             The user-supplied procedure.
1753   @param ProcedureArgument     The user-supplied procedure argument.
1754   @param TimeoutInMicroseconds The timeout for the work to be completed. Zero
1755                                 indicates an infinite timeout.
1756   @param SingleThread          Whether the APs will execute sequentially.
1757   @param FailedCpuList         User-supplied pointer for list of failed CPUs.
1758
1759   @return EFI_SUCCESS on success.
1760 **/
1761 STATIC
1762 EFI_STATUS
1763 StartupAllAPsNoWaitEvent (
1764   IN EFI_AP_PROCEDURE  Procedure,
1765   IN VOID              *ProcedureArgument,
1766   IN UINTN             TimeoutInMicroseconds,
1767   IN BOOLEAN           SingleThread,
1768   IN UINTN             **FailedCpuList
1769   )
1770 {
1771   EFI_STATUS   Status;
1772   UINTN        Index;
1773   UINTN        NextIndex;
1774   UINTN        Timeout;
1775   CPU_AP_DATA  *CpuData;
1776   BOOLEAN      DispatchError;
1777
1778   Timeout       = TimeoutInMicroseconds;
1779   DispatchError = FALSE;
1780
1781   while (TRUE) {
1782     for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
1783       CpuData = &mCpuMpData.CpuData[Index];
1784       if (IsProcessorBSP (Index)) {
1785         // Skip BSP
1786         continue;
1787       }
1788
1789       if (!IsProcessorEnabled (Index)) {
1790         // Skip Disabled processors
1791         continue;
1792       }
1793
1794       switch (GetApState (CpuData)) {
1795         case CpuStateReady:
1796           SetApProcedure (CpuData, Procedure, ProcedureArgument);
1797           Status = DispatchCpu (Index);
1798           if (EFI_ERROR (Status)) {
1799             AddProcessorToFailedList (Index, CpuData->State);
1800             CpuData->State = CpuStateIdle;
1801             mCpuMpData.StartCount--;
1802             DispatchError = TRUE;
1803
1804             if (SingleThread) {
1805               // Dispatch the next available AP
1806               Status = GetNextBlockedNumber (&NextIndex);
1807               if (!EFI_ERROR (Status)) {
1808                 mCpuMpData.CpuData[NextIndex].State = CpuStateReady;
1809               }
1810             }
1811           }
1812
1813           break;
1814
1815         case CpuStateFinished:
1816           mCpuMpData.FinishCount++;
1817           if (SingleThread) {
1818             Status = GetNextBlockedNumber (&NextIndex);
1819             if (!EFI_ERROR (Status)) {
1820               mCpuMpData.CpuData[NextIndex].State = CpuStateReady;
1821             }
1822           }
1823
1824           CpuData->State = CpuStateIdle;
1825           break;
1826
1827         default:
1828           break;
1829       }
1830     }
1831
1832     if (mCpuMpData.FinishCount == mCpuMpData.StartCount) {
1833       Status = EFI_SUCCESS;
1834       break;
1835     }
1836
1837     if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
1838       Status = EFI_TIMEOUT;
1839       break;
1840     }
1841
1842     Timeout -= CalculateAndStallInterval (Timeout);
1843   }
1844
1845   if (Status == EFI_TIMEOUT) {
1846     // Add any remaining CPUs to the FailedCpuList
1847     if (FailedCpuList != NULL) {
1848       for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
1849         AddProcessorToFailedList (Index, mCpuMpData.CpuData[Index].State);
1850       }
1851     }
1852   }
1853
1854   if (DispatchError) {
1855     Status = EFI_NOT_READY;
1856   }
1857
1858   return Status;
1859 }