public inbox for devel@edk2.groups.io
 help / color / mirror / Atom feed
* [PATCH v3 0/3] ArmPkg,MdeModulePkg: Implement EFI_MP_SERVICES_PROTOCOL for AArch64 and add MpServicesTest application
@ 2023-01-04 15:30 Rebecca Cran
  2023-01-04 15:30 ` [PATCH v3 1/3] ArmPkg: Add GET_MPIDR_AFFINITY_BITS and MPIDR_MT_BIT to ArmLib.h Rebecca Cran
                   ` (2 more replies)
  0 siblings, 3 replies; 4+ messages in thread
From: Rebecca Cran @ 2023-01-04 15:30 UTC (permalink / raw)
  To: devel, Sami Mujawar, Ard Biesheuvel, Leif Lindholm, Jian J Wang,
	Liming Gao
  Cc: Rebecca Cran

Implement EFI_MP_SERVICES_PROTOCOL based on PSCI calls for AArch64, and
add an MpServicesTest application to exercise it.

Changes from v2:

- Moved definition of MT bit and macro to get affinity bits into
  ArmLib.h.
- Fixed issues noted during review.
- Fixed various issues in non-blocking mode.
- Fixed bugs when a timeout occurs.

Tested on NVIDIA Jetson AGX Orin Developer Kit,
Arm Juno r2.
The RDN2 (Neoverse N2) FVP has problems with caches, but I believe this
is a bug in the model, not the MP Services code.

Personal (test) PR: https://github.com/tianocore/edk2/pull/3849

Rebecca Cran (3):
  ArmPkg: Add GET_MPIDR_AFFINITY_BITS and MPIDR_MT_BIT to ArmLib.h
  ArmPkg: implement EFI_MP_SERVICES_PROTOCOL based on PSCI calls
  MdeModulePkg: Add new Application/MpServicesTest application

 ArmPkg/ArmPkg.dsc                                            |    1 +
 MdeModulePkg/MdeModulePkg.dsc                                |    2 +
 ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf |   56 +
 MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf   |   40 +
 ArmPkg/Drivers/ArmPsciMpServicesDxe/MpServicesInternal.h     |  344 ++++
 ArmPkg/Include/Library/ArmLib.h                              |   16 +-
 MdeModulePkg/Application/MpServicesTest/Options.h            |   39 +
 ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c   | 1847 ++++++++++++++++++++
 MdeModulePkg/Application/MpServicesTest/MpServicesTest.c     |  560 ++++++
 MdeModulePkg/Application/MpServicesTest/Options.c            |  164 ++
 ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S                |   57 +
 11 files changed, 3119 insertions(+), 7 deletions(-)
 create mode 100644 ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf
 create mode 100644 MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf
 create mode 100644 ArmPkg/Drivers/ArmPsciMpServicesDxe/MpServicesInternal.h
 create mode 100644 MdeModulePkg/Application/MpServicesTest/Options.h
 create mode 100644 ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c
 create mode 100644 MdeModulePkg/Application/MpServicesTest/MpServicesTest.c
 create mode 100644 MdeModulePkg/Application/MpServicesTest/Options.c
 create mode 100644 ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S

-- 
2.30.2


^ permalink raw reply	[flat|nested] 4+ messages in thread

* [PATCH v3 1/3] ArmPkg: Add GET_MPIDR_AFFINITY_BITS and MPIDR_MT_BIT to ArmLib.h
  2023-01-04 15:30 [PATCH v3 0/3] ArmPkg,MdeModulePkg: Implement EFI_MP_SERVICES_PROTOCOL for AArch64 and add MpServicesTest application Rebecca Cran
@ 2023-01-04 15:30 ` Rebecca Cran
  2023-01-04 15:30 ` [PATCH v3 2/3] ArmPkg: implement EFI_MP_SERVICES_PROTOCOL based on PSCI calls Rebecca Cran
  2023-01-04 15:30 ` [PATCH v3 3/3] MdeModulePkg: Add new Application/MpServicesTest application Rebecca Cran
  2 siblings, 0 replies; 4+ messages in thread
From: Rebecca Cran @ 2023-01-04 15:30 UTC (permalink / raw)
  To: devel, Sami Mujawar, Ard Biesheuvel, Leif Lindholm, Jian J Wang,
	Liming Gao
  Cc: Rebecca Cran

Signed-off-by: Rebecca Cran <rebecca@quicinc.com>
---
 ArmPkg/Include/Library/ArmLib.h | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/ArmPkg/Include/Library/ArmLib.h b/ArmPkg/Include/Library/ArmLib.h
index 6566deebdde2..fa605f128bfd 100644
--- a/ArmPkg/Include/Library/ArmLib.h
+++ b/ArmPkg/Include/Library/ArmLib.h
@@ -108,14 +108,16 @@ typedef enum {
 
 #define ARM_CORE_MASK     ARM_CORE_AFF0
 #define ARM_CLUSTER_MASK  ARM_CORE_AFF1
-#define GET_CORE_ID(MpId)            ((MpId) & ARM_CORE_MASK)
-#define GET_CLUSTER_ID(MpId)         (((MpId) & ARM_CLUSTER_MASK) >> 8)
-#define GET_MPID(ClusterId, CoreId)  (((ClusterId) << 8) | (CoreId))
-#define GET_MPIDR_AFF0(MpId)         ((MpId) & ARM_CORE_AFF0)
-#define GET_MPIDR_AFF1(MpId)         (((MpId) & ARM_CORE_AFF1) >> 8)
-#define GET_MPIDR_AFF2(MpId)         (((MpId) & ARM_CORE_AFF2) >> 16)
-#define GET_MPIDR_AFF3(MpId)         (((MpId) & ARM_CORE_AFF3) >> 32)
+#define GET_CORE_ID(MpId)              ((MpId) & ARM_CORE_MASK)
+#define GET_CLUSTER_ID(MpId)           (((MpId) & ARM_CLUSTER_MASK) >> 8)
+#define GET_MPID(ClusterId, CoreId)    (((ClusterId) << 8) | (CoreId))
+#define GET_MPIDR_AFF0(MpId)           ((MpId) & ARM_CORE_AFF0)
+#define GET_MPIDR_AFF1(MpId)           (((MpId) & ARM_CORE_AFF1) >> 8)
+#define GET_MPIDR_AFF2(MpId)           (((MpId) & ARM_CORE_AFF2) >> 16)
+#define GET_MPIDR_AFF3(MpId)           (((MpId) & ARM_CORE_AFF3) >> 32)
+#define GET_MPIDR_AFFINITY_BITS(MpId)  ((MpId) & 0xFF00FFFFFF)
 #define PRIMARY_CORE_ID  (PcdGet32(PcdArmPrimaryCore) & ARM_CORE_MASK)
+#define MPIDR_MT_BIT     BIT24
 
 /** Reads the CCSIDR register for the specified cache.
 
-- 
2.30.2


^ permalink raw reply related	[flat|nested] 4+ messages in thread

* [PATCH v3 2/3] ArmPkg: implement EFI_MP_SERVICES_PROTOCOL based on PSCI calls
  2023-01-04 15:30 [PATCH v3 0/3] ArmPkg,MdeModulePkg: Implement EFI_MP_SERVICES_PROTOCOL for AArch64 and add MpServicesTest application Rebecca Cran
  2023-01-04 15:30 ` [PATCH v3 1/3] ArmPkg: Add GET_MPIDR_AFFINITY_BITS and MPIDR_MT_BIT to ArmLib.h Rebecca Cran
@ 2023-01-04 15:30 ` Rebecca Cran
  2023-01-04 15:30 ` [PATCH v3 3/3] MdeModulePkg: Add new Application/MpServicesTest application Rebecca Cran
  2 siblings, 0 replies; 4+ messages in thread
From: Rebecca Cran @ 2023-01-04 15:30 UTC (permalink / raw)
  To: devel, Sami Mujawar, Ard Biesheuvel, Leif Lindholm, Jian J Wang,
	Liming Gao
  Cc: Rebecca Cran, Ard Biesheuvel

Add support for EFI_MP_SERVICES_PROTOCOL during the DXE phase under
AArch64.

PSCI_CPU_ON is called to power on the core, the supplied procedure is
executed and PSCI_CPU_OFF is called to power off the core.

Fixes contributed by Ard Biesheuvel.

Signed-off-by: Rebecca Cran <rebecca@quicinc.com>
Reviewed-by: Ard Biesheuvel <ardb@kernel.org>
---
 ArmPkg/ArmPkg.dsc                                            |    1 +
 ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf |   56 +
 ArmPkg/Drivers/ArmPsciMpServicesDxe/MpServicesInternal.h     |  344 ++++
 ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c   | 1847 ++++++++++++++++++++
 ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S                |   57 +
 5 files changed, 2305 insertions(+)

diff --git a/ArmPkg/ArmPkg.dsc b/ArmPkg/ArmPkg.dsc
index ac24ebce4892..1e873b90c56d 100644
--- a/ArmPkg/ArmPkg.dsc
+++ b/ArmPkg/ArmPkg.dsc
@@ -164,6 +164,7 @@ [Components.common]
   ArmPkg/Universal/Smbios/OemMiscLibNull/OemMiscLibNull.inf
 
 [Components.AARCH64]
+  ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf
   ArmPkg/Drivers/MmCommunicationDxe/MmCommunication.inf
   ArmPkg/Library/ArmMmuLib/ArmMmuPeiLib.inf
 
diff --git a/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf b/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf
new file mode 100644
index 000000000000..2c9ab99038f2
--- /dev/null
+++ b/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf
@@ -0,0 +1,56 @@
+## @file
+#  ARM MP services protocol driver
+#
+#  Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 1.27
+  BASE_NAME                      = ArmPsciMpServicesDxe
+  FILE_GUID                      = 007ab472-dc4a-4df8-a5c2-abb4a327278c
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = ArmPsciMpServicesDxeInitialize
+
+[Sources.Common]
+  ArmPsciMpServicesDxe.c
+  MpFuncs.S
+  MpServicesInternal.h
+
+[Packages]
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  ArmLib
+  ArmMmuLib
+  ArmSmcLib
+  BaseMemoryLib
+  CacheMaintenanceLib
+  CpuExceptionHandlerLib
+  DebugLib
+  HobLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Protocols]
+  gEfiMpServiceProtocolGuid            ## PRODUCES
+  gEfiLoadedImageProtocolGuid          ## CONSUMES
+
+[Guids]
+  gArmMpCoreInfoGuid
+
+[Depex]
+  TRUE
+
+[BuildOptions]
+  GCC:*_*_*_CC_FLAGS = -mstrict-align
diff --git a/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpServicesInternal.h b/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpServicesInternal.h
new file mode 100644
index 000000000000..5ba63700dd18
--- /dev/null
+++ b/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpServicesInternal.h
@@ -0,0 +1,344 @@
+/** @file
+
+Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>
+Portions copyright (c) 2011, Apple Inc. All rights reserved.
+
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef MP_SERVICES_INTERNAL_H_
+#define MP_SERVICES_INTERNAL_H_
+
+#include <Protocol/Cpu.h>
+#include <Protocol/MpService.h>
+
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+
+#define AP_STACK_SIZE  0x1000
+
+//
+// Internal Data Structures
+//
+
+//
+// AP state
+//
+// The state transitions for an AP when it processes a procedure are:
+//  Idle ----> Ready ----> Busy ----> Finished ----> Idle
+//       [BSP]       [BSP]      [AP]           [BSP]
+//
+typedef enum {
+  CpuStateIdle,
+  CpuStateReady,
+  CpuStateBlocked,
+  CpuStateBusy,
+  CpuStateFinished,
+  CpuStateDisabled
+} CPU_STATE;
+
+//
+// Define Individual Processor Data block.
+//
+typedef struct {
+  EFI_PROCESSOR_INFORMATION    Info;
+  EFI_AP_PROCEDURE             Procedure;
+  VOID                         *Parameter;
+  CPU_STATE                    State;
+  EFI_EVENT                    CheckThisAPEvent;
+  VOID                         *Ttbr0;
+  UINTN                        Tcr;
+  UINTN                        Mair;
+} CPU_AP_DATA;
+
+//
+// Define MP data block which consumes individual processor block.
+//
+typedef struct {
+  UINTN               NumberOfProcessors;
+  UINTN               NumberOfEnabledProcessors;
+  EFI_EVENT           CheckAllAPsEvent;
+  EFI_EVENT           WaitEvent;
+  UINTN               FinishCount;
+  UINTN               StartCount;
+  EFI_AP_PROCEDURE    Procedure;
+  VOID                *ProcedureArgument;
+  BOOLEAN             SingleThread;
+  UINTN               StartedNumber;
+  CPU_AP_DATA         *CpuData;
+  UINTN               Timeout;
+  UINTN               TimeTaken;
+  UINTN               *FailedList;
+  UINTN               FailedListIndex;
+  BOOLEAN             TimeoutActive;
+  BOOLEAN             *SingleApFinished;
+} CPU_MP_DATA;
+
+/** Secondary core entry point.
+
+**/
+VOID
+ApEntryPoint (
+  VOID
+  );
+
+/** C entry-point for the AP.
+    This function gets called from the assembly function ApEntryPoint.
+**/
+VOID
+ApProcedure (
+  VOID
+  );
+
+/** Turns on the specified core using PSCI and executes the user-supplied
+    function that's been configured via a previous call to SetApProcedure.
+
+   @param ProcessorIndex The index of the core to turn on.
+
+   @retval EFI_SUCCESS       The processor was successfully turned on.
+   @retval EFI_DEVICE_ERROR  An error occurred turning the processor on.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+DispatchCpu (
+  IN UINTN  ProcessorIndex
+  );
+
+/** Returns whether the specified processor is the BSP.
+
+   @param[in] ProcessorIndex The index the processor to check.
+
+   @return TRUE if the processor is the BSP, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorBSP (
+  UINTN  ProcessorIndex
+  );
+
+/** Returns whether the processor executing this function is the BSP.
+
+   @return Whether the current processor is the BSP.
+**/
+STATIC
+BOOLEAN
+IsCurrentProcessorBSP (
+  VOID
+  );
+
+/** Returns whether the specified processor is enabled.
+
+   @param[in] ProcessorIndex The index of the processor to check.
+
+   @return TRUE if the processor is enabled, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorEnabled (
+  UINTN  ProcessorIndex
+  );
+
+/** Configures the processor context with the user-supplied procedure and
+    argument.
+
+   @param CpuData           The processor context.
+   @param Procedure         The user-supplied procedure.
+   @param ProcedureArgument The user-supplied procedure argument.
+
+**/
+STATIC
+VOID
+SetApProcedure (
+  IN   CPU_AP_DATA       *CpuData,
+  IN   EFI_AP_PROCEDURE  Procedure,
+  IN   VOID              *ProcedureArgument
+  );
+
+/**
+  Get the Application Processors state.
+
+  @param[in]  CpuData    The pointer to CPU_AP_DATA of specified AP
+
+  @return  The AP status
+**/
+CPU_STATE
+GetApState (
+  IN  CPU_AP_DATA  *CpuData
+  );
+
+/** Returns the index of the next processor that is blocked.
+
+   @param[out] NextNumber The index of the next blocked processor.
+
+   @retval EFI_SUCCESS   Successfully found the next blocked processor.
+   @retval EFI_NOT_FOUND There are no blocked processors.
+
+**/
+STATIC
+EFI_STATUS
+GetNextBlockedNumber (
+  OUT UINTN  *NextNumber
+  );
+
+/** Stalls the BSP for the minimum of gPollInterval and Timeout.
+
+   @param[in]  Timeout    The time limit in microseconds remaining for
+                          APs to return from Procedure.
+
+   @retval     StallTime  Time of execution stall.
+**/
+STATIC
+UINTN
+CalculateAndStallInterval (
+  IN UINTN  Timeout
+  );
+
+/** Sets up the state for the StartupAllAPs function.
+
+   @param SingleThread Whether the APs will execute sequentially.
+
+**/
+STATIC
+VOID
+StartupAllAPsPrepareState (
+  IN BOOLEAN  SingleThread
+  );
+
+/** Handles execution of StartupAllAPs when a WaitEvent has been specified.
+
+  @param Procedure         The user-supplied procedure.
+  @param ProcedureArgument The user-supplied procedure argument.
+  @param WaitEvent         The wait event to be signaled when the work is
+                           complete or a timeout has occurred.
+  @param TimeoutInMicroseconds The timeout for the work to be completed. Zero
+                               indicates an infinite timeout.
+  @param SingleThread          Whether the APs will execute sequentially.
+  @param FailedCpuList         User-supplied pointer for list of failed CPUs.
+
+   @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsWithWaitEvent (
+  IN EFI_AP_PROCEDURE  Procedure,
+  IN VOID              *ProcedureArgument,
+  IN EFI_EVENT         WaitEvent,
+  IN UINTN             TimeoutInMicroseconds,
+  IN BOOLEAN           SingleThread,
+  IN UINTN             **FailedCpuList
+  );
+
+/** Handles execution of StartupAllAPs when no wait event has been specified.
+
+   @param Procedure             The user-supplied procedure.
+   @param ProcedureArgument     The user-supplied procedure argument.
+   @param TimeoutInMicroseconds The timeout for the work to be completed. Zero
+                                indicates an infinite timeout.
+   @param SingleThread          Whether the APs will execute sequentially.
+   @param FailedCpuList         User-supplied pointer for list of failed CPUs.
+
+   @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsNoWaitEvent (
+  IN EFI_AP_PROCEDURE  Procedure,
+  IN VOID              *ProcedureArgument,
+  IN UINTN             TimeoutInMicroseconds,
+  IN BOOLEAN           SingleThread,
+  IN UINTN             **FailedCpuList
+  );
+
+/** Adds the specified processor the list of failed processors.
+
+   @param ProcessorIndex The processor index to add.
+   @param ApState         Processor state.
+
+**/
+STATIC
+VOID
+AddProcessorToFailedList (
+  UINTN      ProcessorIndex,
+  CPU_STATE  ApState
+  );
+
+/** Handles the StartupAllAPs case where the timeout has occurred.
+
+**/
+STATIC
+VOID
+ProcessStartupAllAPsTimeout (
+  VOID
+  );
+
+/**
+  If a timeout is specified in StartupAllAps(), a timer is set, which invokes
+  this procedure periodically to check whether all APs have finished.
+
+  @param[in] Event   The WaitEvent the user supplied.
+  @param[in] Context The event context.
+**/
+STATIC
+VOID
+EFIAPI
+CheckAllAPsStatus (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  );
+
+/** Invoked periodically via a timer to check the state of the processor.
+
+   @param Event   The event supplied by the timer expiration.
+   @param Context The processor context.
+
+**/
+STATIC
+VOID
+EFIAPI
+CheckThisAPStatus (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  );
+
+/**
+  This function is called by all processors (both BSP and AP) once and collects
+  MP related data.
+
+  @param BSP            TRUE if the processor is the BSP.
+  @param Mpidr          The MPIDR for the specified processor. This should be
+                        the full MPIDR and not only the affinity bits.
+  @param ProcessorIndex The index of the processor.
+
+  @return EFI_SUCCESS if the data for the processor collected and filled in.
+
+**/
+STATIC
+EFI_STATUS
+FillInProcessorInformation (
+  IN BOOLEAN  BSP,
+  IN UINTN    Mpidr,
+  IN UINTN    ProcessorIndex
+  );
+
+/**
+  Event notification function called when the EFI_EVENT_GROUP_READY_TO_BOOT is
+  signaled. After this point, non-blocking mode is no longer allowed.
+
+  @param  Event     Event whose notification function is being invoked.
+  @param  Context   The pointer to the notification function's context,
+                    which is implementation-dependent.
+
+**/
+STATIC
+VOID
+EFIAPI
+ReadyToBootSignaled (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  );
+
+#endif /* MP_SERVICES_INTERNAL_H_ */
diff --git a/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c b/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c
new file mode 100644
index 000000000000..ab439bffd722
--- /dev/null
+++ b/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c
@@ -0,0 +1,1847 @@
+/** @file
+  Construct MP Services Protocol.
+
+  The MP Services Protocol provides a generalized way of performing following tasks:
+    - Retrieving information of multi-processor environment and MP-related status of
+      specific processors.
+    - Dispatching user-provided function to APs.
+    - Maintain MP-related processor status.
+
+  The MP Services Protocol must be produced on any system with more than one logical
+  processor.
+
+  The Protocol is available only during boot time.
+
+  MP Services Protocol is hardware-independent. Most of the logic of this protocol
+  is architecturally neutral. It abstracts the multi-processor environment and
+  status of processors, and provides interfaces to retrieve information, maintain,
+  and dispatch.
+
+  MP Services Protocol may be consumed by ACPI module. The ACPI module may use this
+  protocol to retrieve data that are needed for an MP platform and report them to OS.
+  MP Services Protocol may also be used to program and configure processors, such
+  as MTRR synchronization for memory space attributes setting in DXE Services.
+  MP Services Protocol may be used by non-CPU DXE drivers to speed up platform boot
+  by taking advantage of the processing capabilities of the APs, for example, using
+  APs to help test system memory in parallel with other device initialization.
+  Diagnostics applications may also use this protocol for multi-processor.
+
+  Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <PiDxe.h>
+
+#include <Library/ArmLib.h>
+#include <Library/ArmMmuLib.h>
+#include <Library/ArmPlatformLib.h>
+#include <Library/ArmSmcLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/CpuExceptionHandlerLib.h>
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <IndustryStandard/ArmStdSmc.h>
+#include <Ppi/ArmMpCoreInfo.h>
+#include <Protocol/LoadedImage.h>
+
+#include "MpServicesInternal.h"
+
+#define POLL_INTERVAL_US  50000
+
+STATIC CPU_MP_DATA  mCpuMpData;
+STATIC BOOLEAN      mNonBlockingModeAllowed;
+UINT64              *gApStacksBase;
+UINT64              *gProcessorIDs;
+CONST UINT64        gApStackSize = AP_STACK_SIZE;
+
+STATIC
+BOOLEAN
+IsCurrentProcessorBSP (
+  VOID
+  );
+
+/** Turns on the specified core using PSCI and executes the user-supplied
+    function that's been configured via a previous call to SetApProcedure.
+
+    @param ProcessorIndex The index of the core to turn on.
+
+    @retval EFI_SUCCESS      Success.
+    @retval EFI_DEVICE_ERROR The processor could not be turned on.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+DispatchCpu (
+  IN UINTN  ProcessorIndex
+  )
+{
+  ARM_SMC_ARGS  Args;
+  EFI_STATUS    Status;
+
+  Status = EFI_SUCCESS;
+
+  mCpuMpData.CpuData[ProcessorIndex].State = CpuStateBusy;
+
+  /* Turn the AP on */
+  if (sizeof (Args.Arg0) == sizeof (UINT32)) {
+    Args.Arg0 = ARM_SMC_ID_PSCI_CPU_ON_AARCH32;
+  } else {
+    Args.Arg0 = ARM_SMC_ID_PSCI_CPU_ON_AARCH64;
+  }
+
+  Args.Arg1 = gProcessorIDs[ProcessorIndex];
+  Args.Arg2 = (UINTN)ApEntryPoint;
+
+  mCpuMpData.CpuData[ProcessorIndex].Tcr   = ArmGetTCR ();
+  mCpuMpData.CpuData[ProcessorIndex].Mair  = ArmGetMAIR ();
+  mCpuMpData.CpuData[ProcessorIndex].Ttbr0 = ArmGetTTBR0BaseAddress ();
+  WriteBackDataCacheRange (&mCpuMpData.CpuData[ProcessorIndex], sizeof (CPU_AP_DATA));
+
+  ArmCallSmc (&Args);
+
+  if (Args.Arg0 != ARM_SMC_PSCI_RET_SUCCESS) {
+    DEBUG ((DEBUG_ERROR, "PSCI_CPU_ON call failed: %d\n", Args.Arg0));
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/** Returns whether the specified processor is the BSP.
+
+  @param[in] ProcessorIndex The index the processor to check.
+
+  @return TRUE if the processor is the BSP, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorBSP (
+  UINTN  ProcessorIndex
+  )
+{
+  EFI_PROCESSOR_INFORMATION  *CpuInfo;
+
+  CpuInfo = &mCpuMpData.CpuData[ProcessorIndex].Info;
+
+  return (CpuInfo->StatusFlag & PROCESSOR_AS_BSP_BIT) != 0;
+}
+
+/** Get the Application Processors state.
+
+  @param[in]  CpuData    The pointer to CPU_AP_DATA of specified AP.
+
+  @return The AP status.
+**/
+CPU_STATE
+GetApState (
+  IN  CPU_AP_DATA  *CpuData
+  )
+{
+  return CpuData->State;
+}
+
+/** Configures the processor context with the user-supplied procedure and
+    argument.
+
+   @param CpuData           The processor context.
+   @param Procedure         The user-supplied procedure.
+   @param ProcedureArgument The user-supplied procedure argument.
+
+**/
+STATIC
+VOID
+SetApProcedure (
+  IN   CPU_AP_DATA       *CpuData,
+  IN   EFI_AP_PROCEDURE  Procedure,
+  IN   VOID              *ProcedureArgument
+  )
+{
+  ASSERT (CpuData != NULL);
+  ASSERT (Procedure != NULL);
+
+  CpuData->Parameter = ProcedureArgument;
+  CpuData->Procedure = Procedure;
+}
+
+/** Returns the index of the next processor that is blocked.
+
+   @param[out] NextNumber The index of the next blocked processor.
+
+   @retval EFI_SUCCESS   Successfully found the next blocked processor.
+   @retval EFI_NOT_FOUND There are no blocked processors.
+
+**/
+STATIC
+EFI_STATUS
+GetNextBlockedNumber (
+  OUT UINTN  *NextNumber
+  )
+{
+  UINTN        Index;
+  CPU_STATE    State;
+  CPU_AP_DATA  *CpuData;
+
+  for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData = &mCpuMpData.CpuData[Index];
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    State = CpuData->State;
+
+    if (State == CpuStateBlocked) {
+      *NextNumber = Index;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/** Stalls the BSP for the minimum of POLL_INTERVAL_US and Timeout.
+
+   @param[in]  Timeout    The time limit in microseconds remaining for
+                          APs to return from Procedure.
+
+   @retval     StallTime  Time of execution stall.
+**/
+STATIC
+UINTN
+CalculateAndStallInterval (
+  IN UINTN  Timeout
+  )
+{
+  UINTN  StallTime;
+
+  if ((Timeout < POLL_INTERVAL_US) && (Timeout != 0)) {
+    StallTime = Timeout;
+  } else {
+    StallTime = POLL_INTERVAL_US;
+  }
+
+  gBS->Stall (StallTime);
+
+  return StallTime;
+}
+
+/**
+  This service retrieves the number of logical processor in the platform
+  and the number of those logical processors that are enabled on this boot.
+  This service may only be called from the BSP.
+
+  This function is used to retrieve the following information:
+    - The number of logical processors that are present in the system.
+    - The number of enabled logical processors in the system at the instant
+      this call is made.
+
+  Because MP Service Protocol provides services to enable and disable processors
+  dynamically, the number of enabled logical processors may vary during the
+  course of a boot session.
+
+  If this service is called from an AP, then EFI_DEVICE_ERROR is returned.
+  If NumberOfProcessors or NumberOfEnabledProcessors is NULL, then
+  EFI_INVALID_PARAMETER is returned. Otherwise, the total number of processors
+  is returned in NumberOfProcessors, the number of currently enabled processor
+  is returned in NumberOfEnabledProcessors, and EFI_SUCCESS is returned.
+
+  @param[in]  This                        A pointer to the
+                                          EFI_MP_SERVICES_PROTOCOL instance.
+  @param[out] NumberOfProcessors          Pointer to the total number of logical
+                                          processors in the system, including
+                                          the BSP and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled
+                                          logical processors that exist in the
+                                          system, including the BSP.
+
+  @retval EFI_SUCCESS             The number of logical processors and enabled
+                                  logical processors was retrieved.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL.
+  @retval EFI_INVALID_PARAMETER   NumberOfEnabledProcessors is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+GetNumberOfProcessors (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                     *NumberOfProcessors,
+  OUT UINTN                     *NumberOfEnabledProcessors
+  )
+{
+  if ((NumberOfProcessors == NULL) || (NumberOfEnabledProcessors == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  *NumberOfProcessors        = mCpuMpData.NumberOfProcessors;
+  *NumberOfEnabledProcessors = mCpuMpData.NumberOfEnabledProcessors;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made. This service may only be called from the BSP.
+
+  This service retrieves detailed MP-related information about any processor
+  on the platform. Note the following:
+    - The processor information may change during the course of a boot session.
+    - The information presented here is entirely MP related.
+
+  Information regarding the number of caches and their sizes, frequency of
+  operation, slot numbers is all considered platform-related information and is
+  not provided by this service.
+
+  @param[in]  This                  A pointer to the EFI_MP_SERVICES_PROTOCOL
+                                    instance.
+  @param[in]  ProcessorIndex        The index of the processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where information
+                                    for the requested processor is deposited.
+
+  @retval EFI_SUCCESS             Processor information was returned.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist in the platform.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+GetProcessorInfo (
+  IN  EFI_MP_SERVICES_PROTOCOL   *This,
+  IN  UINTN                      ProcessorIndex,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
+  )
+{
+  if (ProcessorInfoBuffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ProcessorIndex &= ~CPU_V2_EXTENDED_TOPOLOGY;
+
+  if (ProcessorIndex >= mCpuMpData.NumberOfProcessors) {
+    return EFI_NOT_FOUND;
+  }
+
+  CopyMem (
+    ProcessorInfoBuffer,
+    &mCpuMpData.CpuData[ProcessorIndex],
+    sizeof (EFI_PROCESSOR_INFORMATION)
+    );
+  return EFI_SUCCESS;
+}
+
+/**
+  This service executes a caller provided function on all enabled APs. APs can
+  run either simultaneously or one at a time in sequence. This service supports
+  both blocking and non-blocking requests. The non-blocking requests use EFI
+  events so the BSP can detect when the APs have finished. This service may only
+  be called from the BSP.
+
+  This function is used to dispatch all the enabled APs to the function
+  specified by Procedure.  If any enabled AP is busy, then EFI_NOT_READY is
+  returned immediately and Procedure is not started on any AP.
+
+  If SingleThread is TRUE, all the enabled APs execute the function specified by
+  Procedure one by one, in ascending order of processor handle number.
+  Otherwise, all the enabled APs execute the function specified by Procedure
+  simultaneously.
+
+  If WaitEvent is NULL, execution is in blocking mode. The BSP waits until all
+  APs finish or TimeoutInMicroseconds expires. Otherwise, execution is in
+  non-blocking mode, and the BSP returns from this service without waiting for
+  APs. If a non-blocking mode is requested after the UEFI Event
+  EFI_EVENT_GROUP_READY_TO_BOOT is signaled, then EFI_UNSUPPORTED must be
+  returned.
+
+  If the timeout specified by TimeoutInMicroseconds expires before all APs
+  return from Procedure, then Procedure on the failed APs is terminated.
+  All enabled APs are always available for further calls to
+  EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and
+  EFI_MP_SERVICES_PROTOCOL.StartupThisAP(). If FailedCpuList is not NULL, its
+  content points to the list of processor handle numbers in which Procedure was
+  terminated.
+
+  Note: It is the responsibility of the consumer of the
+  EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() to make sure that the nature of the
+  code that is executed on the BSP and the dispatched APs is well controlled.
+  The MP Services Protocol does not guarantee that the Procedure function is
+  MP-safe. Hence, the tasks that can be run in parallel are limited to certain
+  independent tasks and well-controlled exclusive code. EFI services and
+  protocols may not be called by APs unless otherwise specified.
+
+  In blocking execution mode, BSP waits until all APs finish or
+  TimeoutInMicroseconds expires.
+
+  In non-blocking execution mode, BSP is freed to return to the caller and then
+  proceed to the next task without having to wait for APs. The following
+  sequence needs to occur in a non-blocking execution mode:
+
+    -# The caller that intends to use this MP Services Protocol in non-blocking
+       mode creates WaitEvent by calling the EFI CreateEvent() service.  The
+       caller invokes EFI_MP_SERVICES_PROTOCOL.StartupAllAPs(). If the parameter
+       WaitEvent is not NULL, then StartupAllAPs() executes in non-blocking
+       mode. It requests the function specified by Procedure to be started on
+       all the enabled APs, and releases the BSP to continue with other tasks.
+    -# The caller can use the CheckEvent() and WaitForEvent() services to check
+       the state of the WaitEvent created in step 1.
+    -# When the APs complete their task or TimeoutInMicroSecondss expires, the
+       MP Service signals WaitEvent by calling the EFI SignalEvent() function.
+       If FailedCpuList is not NULL, its content is available when WaitEvent is
+       signaled. If all APs returned from Procedure prior to the timeout, then
+       FailedCpuList is set to NULL. If not all APs return from Procedure before
+       the timeout, then FailedCpuList is filled in with the list of the failed
+       APs. The buffer is allocated by MP Service Protocol using AllocatePool().
+       It is the caller's responsibility to free the buffer with FreePool()
+       service.
+    -# This invocation of SignalEvent() function informs the caller that invoked
+       EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() that either all the APs
+       completed the specified task or a timeout occurred. The contents of
+       FailedCpuList can be examined to determine which APs did not complete the
+       specified task prior to the timeout.
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PROTOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  SingleThread            If TRUE, then all the enabled APs execute
+                                      the function specified by Procedure one by
+                                      one, in ascending order of processor
+                                      handle number.  If FALSE, then all the
+                                      enabled APs execute the function specified
+                                      by Procedure simultaneously.
+  @param[in]  WaitEvent               The event created by the caller with
+                                      CreateEvent() service.  If it is NULL,
+                                      then execute in blocking mode. BSP waits
+                                      until all APs finish or
+                                      TimeoutInMicroseconds expires.  If it's
+                                      not NULL, then execute in non-blocking
+                                      mode. BSP requests the function specified
+                                      by Procedure to be started on all the
+                                      enabled APs, and go on executing
+                                      immediately. If all return from Procedure,
+                                      or TimeoutInMicroseconds expires, this
+                                      event is signaled. The BSP can use the
+                                      CheckEvent() or WaitForEvent()
+                                      services to check the state of event. Type
+                                      EFI_EVENT is defined in CreateEvent() in
+                                      the Unified Extensible Firmware Interface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microseconds
+                                      for APs to return from Procedure, either
+                                      for blocking or non-blocking mode. Zero
+                                      means infinity.  If the timeout expires
+                                      before all APs return from Procedure, then
+                                      Procedure on the failed APs is terminated.
+                                      All enabled APs are available for next
+                                      function assigned by
+                                      EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
+                                      If the timeout expires in blocking mode,
+                                      BSP returns EFI_TIMEOUT.  If the timeout
+                                      expires in non-blocking mode, WaitEvent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure for
+                                      all APs.
+  @param[out] FailedCpuList           If NULL, this parameter is ignored.
+                                      Otherwise, if all APs finish successfully,
+                                      then its content is set to NULL. If not
+                                      all APs finish before timeout expires,
+                                      then its content is set to address of the
+                                      buffer holding handle numbers of the
+                                      failed APs.
+                                      The buffer is allocated by MP Service
+                                      Protocol, and it's the caller's
+                                      responsibility to free the buffer with
+                                      FreePool() service.
+                                      In blocking mode, it is ready for
+                                      consumption when the call returns. In
+                                      non-blocking mode, it is ready when
+                                      WaitEvent is signaled. The list of failed
+                                      CPU is terminated by  END_OF_CPU_LIST.
+
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished before
+                                  the timeout expired.
+  @retval EFI_SUCCESS             In non-blocking mode, function has been
+                                  dispatched to all enabled APs.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made after the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        Caller processor is AP.
+  @retval EFI_NOT_STARTED         No enabled APs exist in the system.
+  @retval EFI_NOT_READY           Any enabled APs are busy.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired before
+                                  all enabled APs have finished.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+StartupAllAPs (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  BOOLEAN                   SingleThread,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT UINTN                     **FailedCpuList         OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (mCpuMpData.NumberOfProcessors == 1) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (Procedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((WaitEvent != NULL) && !mNonBlockingModeAllowed) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (FailedCpuList != NULL) {
+    mCpuMpData.FailedList = AllocateZeroPool (
+                              (mCpuMpData.NumberOfProcessors + 1) *
+                              sizeof (UINTN)
+                              );
+    if (mCpuMpData.FailedList == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    SetMemN (
+      mCpuMpData.FailedList,
+      (mCpuMpData.NumberOfProcessors + 1) *
+      sizeof (UINTN),
+      END_OF_CPU_LIST
+      );
+    mCpuMpData.FailedListIndex = 0;
+    *FailedCpuList             = mCpuMpData.FailedList;
+  }
+
+  StartupAllAPsPrepareState (SingleThread);
+
+  if (WaitEvent != NULL) {
+    Status = StartupAllAPsWithWaitEvent (
+               Procedure,
+               ProcedureArgument,
+               WaitEvent,
+               TimeoutInMicroseconds,
+               SingleThread,
+               FailedCpuList
+               );
+
+    if (EFI_ERROR (Status) && (FailedCpuList != NULL)) {
+      if (mCpuMpData.FailedListIndex == 0) {
+        FreePool (*FailedCpuList);
+        *FailedCpuList = NULL;
+      }
+    }
+  } else {
+    Status = StartupAllAPsNoWaitEvent (
+               Procedure,
+               ProcedureArgument,
+               TimeoutInMicroseconds,
+               SingleThread,
+               FailedCpuList
+               );
+
+    if (FailedCpuList != NULL) {
+      if (mCpuMpData.FailedListIndex == 0) {
+        FreePool (*FailedCpuList);
+        *FailedCpuList = NULL;
+      }
+    }
+  }
+
+  return Status;
+}
+
+/**
+  This service lets the caller get one enabled AP to execute a caller-provided
+  function. The caller can request the BSP to either wait for the completion
+  of the AP or just proceed with the next task by using the EFI event mechanism.
+  See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blocking
+  execution support.  This service may only be called from the BSP.
+
+  This function is used to dispatch one enabled AP to the function specified by
+  Procedure passing in the argument specified by ProcedureArgument.  If WaitEvent
+  is NULL, execution is in blocking mode. The BSP waits until the AP finishes or
+  TimeoutInMicroSecondss expires. Otherwise, execution is in non-blocking mode.
+  BSP proceeds to the next task without waiting for the AP. If a non-blocking mode
+  is requested after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled,
+  then EFI_UNSUPPORTED must be returned.
+
+  If the timeout specified by TimeoutInMicroseconds expires before the AP returns
+  from Procedure, then execution of Procedure by the AP is terminated. The AP is
+  available for subsequent calls to EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and
+  EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PROTOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  ProcessorNumber         The handle number of the AP. The range is
+                                      from 0 to the total number of logical
+                                      processors minus 1. The total number of
+                                      logical processors can be retrieved by
+                                      EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
+  @param[in]  WaitEvent               The event created by the caller with CreateEvent()
+                                      service.  If it is NULL, then execute in
+                                      blocking mode. BSP waits until all APs finish
+                                      or TimeoutInMicroseconds expires.  If it's
+                                      not NULL, then execute in non-blocking mode.
+                                      BSP requests the function specified by
+                                      Procedure to be started on all the enabled
+                                      APs, and go on executing immediately. If
+                                      all return from Procedure or TimeoutInMicroseconds
+                                      expires, this event is signaled. The BSP
+                                      can use the CheckEvent() or WaitForEvent()
+                                      services to check the state of event.  Type
+                                      EFI_EVENT is defined in CreateEvent() in
+                                      the Unified Extensible Firmware Interface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microseconds for
+                                      APs to return from Procedure, either for
+                                      blocking or non-blocking mode. Zero means
+                                      infinity.  If the timeout expires before
+                                      all APs return from Procedure, then Procedure
+                                      on the failed APs is terminated. All enabled
+                                      APs are available for next function assigned
+                                      by EFI_MP_SERVICES_PROTOCOL.StartupAllAPs()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
+                                      If the timeout expires in blocking mode,
+                                      BSP returns EFI_TIMEOUT.  If the timeout
+                                      expires in non-blocking mode, WaitEvent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure for
+                                      all APs.
+  @param[out] Finished                If NULL, this parameter is ignored.  In
+                                      blocking mode, this parameter is ignored.
+                                      In non-blocking mode, if AP returns from
+                                      Procedure before the timeout expires, its
+                                      content is set to TRUE. Otherwise, the
+                                      value is set to FALSE. The caller can
+                                      determine if the AP returned from Procedure
+                                      by evaluating this value.
+
+  @retval EFI_SUCCESS             In blocking mode, specified AP finished before
+                                  the timeout expires.
+  @retval EFI_SUCCESS             In non-blocking mode, the function has been
+                                  dispatched to specified AP.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made after the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired before
+                                  the specified AP has finished.
+  @retval EFI_NOT_READY           The specified AP is busy.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or disabled AP.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+StartupThisAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  UINTN                     ProcessorNumber,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT BOOLEAN                   *Finished               OPTIONAL
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        Timeout;
+  CPU_AP_DATA  *CpuData;
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Procedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (ProcessorNumber >= mCpuMpData.NumberOfProcessors) {
+    return EFI_NOT_FOUND;
+  }
+
+  CpuData = &mCpuMpData.CpuData[ProcessorNumber];
+
+  if (IsProcessorBSP (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsProcessorEnabled (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (GetApState (CpuData) != CpuStateIdle) {
+    return EFI_NOT_READY;
+  }
+
+  if ((WaitEvent != NULL) && !mNonBlockingModeAllowed) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Timeout = TimeoutInMicroseconds;
+
+  mCpuMpData.Timeout       = TimeoutInMicroseconds;
+  mCpuMpData.TimeTaken     = 0;
+  mCpuMpData.TimeoutActive = (BOOLEAN)(TimeoutInMicroseconds != 0);
+
+  mCpuMpData.StartCount  = 1;
+  mCpuMpData.FinishCount = 0;
+
+  SetApProcedure (
+    CpuData,
+    Procedure,
+    ProcedureArgument
+    );
+
+  Status = DispatchCpu (ProcessorNumber);
+  if (EFI_ERROR (Status)) {
+    CpuData->State = CpuStateIdle;
+    return EFI_NOT_READY;
+  }
+
+  if (WaitEvent != NULL) {
+    // Non Blocking
+    if (Finished != NULL) {
+      mCpuMpData.SingleApFinished = Finished;
+      *Finished                   = FALSE;
+    }
+
+    mCpuMpData.WaitEvent = WaitEvent;
+    Status               = gBS->SetTimer (
+                                  CpuData->CheckThisAPEvent,
+                                  TimerPeriodic,
+                                  POLL_INTERVAL_US
+                                  );
+
+    return EFI_SUCCESS;
+  }
+
+  // Blocking
+  while (TRUE) {
+    if (GetApState (CpuData) == CpuStateFinished) {
+      CpuData->State = CpuStateIdle;
+      break;
+    }
+
+    if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
+      return EFI_TIMEOUT;
+    }
+
+    Timeout -= CalculateAndStallInterval (Timeout);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This service switches the requested AP to be the BSP from that point onward.
+  This service changes the BSP for all purposes.   This call can only be
+  performed by the current BSP.
+
+  This service switches the requested AP to be the BSP from that point onward.
+  This service changes the BSP for all purposes. The new BSP can take over the
+  execution of the old BSP and continue seamlessly from where the old one left
+  off. This service may not be supported after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT
+  is signaled.
+
+  If the BSP cannot be switched prior to the return from this service, then
+  EFI_UNSUPPORTED must be returned.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become the new
+                               BSP. The range is from 0 to the total number of
+                               logical processors minus 1. The total number of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
+  @param[in] EnableOldBSP      If TRUE, then the old BSP will be listed as an
+                               enabled AP. Otherwise, it will be disabled.
+
+  @retval EFI_SUCCESS             BSP successfully switched.
+  @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed prior to
+                                  this service returning.
+  @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
+  @retval EFI_SUCCESS             The calling processor is an AP.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BSP or
+                                  a disabled AP.
+  @retval EFI_NOT_READY           The specified AP is busy.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+SwitchBSP (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                    ProcessorNumber,
+  IN  BOOLEAN                  EnableOldBSP
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  This service lets the caller enable or disable an AP from this point onward.
+  This service may only be called from the BSP.
+
+  This service allows the caller enable or disable an AP from this point onward.
+  The caller can optionally specify the health status of the AP by Health. If
+  an AP is being disabled, then the state of the disabled AP is implementation
+  dependent. If an AP is enabled, then the implementation must guarantee that a
+  complete initialization sequence is performed on the AP, so the AP is in a state
+  that is compatible with an MP operating system. This service may not be supported
+  after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled.
+
+  If the enable or disable AP operation cannot be completed prior to the return
+  from this service, then EFI_UNSUPPORTED must be returned.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become the new
+                               BSP. The range is from 0 to the total number of
+                               logical processors minus 1. The total number of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
+  @param[in] EnableAP          Specifies the new state for the processor for
+                               enabled, FALSE for disabled.
+  @param[in] HealthFlag        If not NULL, a pointer to a value that specifies
+                               the new health status of the AP. This flag
+                               corresponds to StatusFlag defined in
+                               EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo(). Only
+                               the PROCESSOR_HEALTH_STATUS_BIT is used. All other
+                               bits are ignored.  If it is NULL, this parameter
+                               is ignored.
+
+  @retval EFI_SUCCESS             The specified AP was enabled or disabled successfully.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be completed
+                                  prior to this service returning.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not supported.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_NOT_FOUND           Processor with the handle specified by ProcessorNumber
+                                  does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+EnableDisableAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                     ProcessorNumber,
+  IN  BOOLEAN                   EnableAP,
+  IN  UINT32                    *HealthFlag OPTIONAL
+  )
+{
+  UINTN        StatusFlag;
+  CPU_AP_DATA  *CpuData;
+
+  StatusFlag = mCpuMpData.CpuData[ProcessorNumber].Info.StatusFlag;
+  CpuData    = &mCpuMpData.CpuData[ProcessorNumber];
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (ProcessorNumber >= mCpuMpData.NumberOfProcessors) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (IsProcessorBSP (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (GetApState (CpuData) != CpuStateIdle) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (EnableAP) {
+    if (!IsProcessorEnabled (ProcessorNumber)) {
+      mCpuMpData.NumberOfEnabledProcessors++;
+    }
+
+    StatusFlag |= PROCESSOR_ENABLED_BIT;
+  } else {
+    if (IsProcessorEnabled (ProcessorNumber)) {
+      mCpuMpData.NumberOfEnabledProcessors--;
+    }
+
+    StatusFlag &= ~PROCESSOR_ENABLED_BIT;
+  }
+
+  if (HealthFlag != NULL) {
+    StatusFlag &= ~PROCESSOR_HEALTH_STATUS_BIT;
+    StatusFlag |= (*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT);
+  }
+
+  mCpuMpData.CpuData[ProcessorNumber].Info.StatusFlag = StatusFlag;
+  return EFI_SUCCESS;
+}
+
+/**
+  This return the handle number for the calling processor.  This service may be
+  called from the BSP and APs.
+
+  This service returns the processor handle number for the calling processor.
+  The returned value is in the range from 0 to the total number of logical
+  processors minus 1. The total number of logical processors can be retrieved
+  with EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors(). This service may be
+  called from the BSP and APs. If ProcessorNumber is NULL, then EFI_INVALID_PARAMETER
+  is returned. Otherwise, the current processors handle number is returned in
+  ProcessorNumber, and EFI_SUCCESS is returned.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL instance.
+  @param[out] ProcessorNumber  The handle number of AP that is to become the new
+                               BSP. The range is from 0 to the total number of
+                               logical processors minus 1. The total number of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors().
+
+  @retval EFI_SUCCESS             The current processor handle number was returned
+                                  in ProcessorNumber.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+WhoAmI (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                    *ProcessorNumber
+  )
+{
+  UINTN   Index;
+  UINT64  ProcessorId;
+
+  if (ProcessorNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ProcessorId = GET_MPIDR_AFFINITY_BITS (ArmReadMpidr ());
+  for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    if (ProcessorId == gProcessorIDs[Index]) {
+      *ProcessorNumber = Index;
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+STATIC EFI_MP_SERVICES_PROTOCOL  mMpServicesProtocol = {
+  GetNumberOfProcessors,
+  GetProcessorInfo,
+  StartupAllAPs,
+  StartupThisAP,
+  SwitchBSP,
+  EnableDisableAP,
+  WhoAmI
+};
+
+/** Adds the specified processor the list of failed processors.
+
+   @param ProcessorIndex The processor index to add.
+   @param ApState        Processor state.
+
+**/
+STATIC
+VOID
+AddProcessorToFailedList (
+  UINTN      ProcessorIndex,
+  CPU_STATE  ApState
+  )
+{
+  UINTN    Index;
+  BOOLEAN  Found;
+
+  Found = FALSE;
+
+  if ((mCpuMpData.FailedList == NULL) ||
+      (ApState == CpuStateIdle) ||
+      (ApState == CpuStateFinished) ||
+      IsProcessorBSP (ProcessorIndex))
+  {
+    return;
+  }
+
+  // If we are retrying make sure we don't double count
+  for (Index = 0; Index < mCpuMpData.FailedListIndex; Index++) {
+    if (mCpuMpData.FailedList[Index] == ProcessorIndex) {
+      Found = TRUE;
+      break;
+    }
+  }
+
+  /* If the CPU isn't already in the FailedList, add it */
+  if (!Found) {
+    mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] = ProcessorIndex;
+  }
+}
+
+/** Handles the StartupAllAPs case where the timeout has occurred.
+
+**/
+STATIC
+VOID
+ProcessStartupAllAPsTimeout (
+  VOID
+  )
+{
+  CPU_AP_DATA  *CpuData;
+  UINTN        Index;
+
+  if (mCpuMpData.FailedList == NULL) {
+    return;
+  }
+
+  for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData = &mCpuMpData.CpuData[Index];
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    if (!IsProcessorEnabled (Index)) {
+      // Skip Disabled processors
+      continue;
+    }
+
+    CpuData = &mCpuMpData.CpuData[Index];
+    AddProcessorToFailedList (Index, GetApState (CpuData));
+  }
+}
+
+/** Updates the status of the APs.
+
+   @param[in] ProcessorIndex The index of the AP to update.
+**/
+STATIC
+VOID
+UpdateApStatus (
+  IN UINTN  ProcessorIndex
+  )
+{
+  EFI_STATUS   Status;
+  CPU_AP_DATA  *CpuData;
+  CPU_AP_DATA  *NextCpuData;
+  CPU_STATE    State;
+  UINTN        NextNumber;
+
+  CpuData = &mCpuMpData.CpuData[ProcessorIndex];
+
+  if (IsProcessorBSP (ProcessorIndex)) {
+    // Skip BSP
+    return;
+  }
+
+  if (!IsProcessorEnabled (ProcessorIndex)) {
+    // Skip Disabled processors
+    return;
+  }
+
+  State = GetApState (CpuData);
+
+  switch (State) {
+    case CpuStateFinished:
+      if (mCpuMpData.SingleThread) {
+        Status = GetNextBlockedNumber (&NextNumber);
+        if (!EFI_ERROR (Status)) {
+          NextCpuData = &mCpuMpData.CpuData[NextNumber];
+
+          NextCpuData->State = CpuStateReady;
+
+          SetApProcedure (
+            NextCpuData,
+            mCpuMpData.Procedure,
+            mCpuMpData.ProcedureArgument
+            );
+
+          Status = DispatchCpu (NextNumber);
+          if (!EFI_ERROR (Status)) {
+            mCpuMpData.StartCount++;
+          } else {
+            AddProcessorToFailedList (NextNumber, NextCpuData->State);
+          }
+        }
+      }
+
+      CpuData->State = CpuStateIdle;
+      mCpuMpData.FinishCount++;
+      break;
+
+    default:
+      break;
+  }
+}
+
+/**
+  If a timeout is specified in StartupAllAps(), a timer is set, which invokes
+  this procedure periodically to check whether all APs have finished.
+
+  @param[in] Event   The WaitEvent the user supplied.
+  @param[in] Context The event context.
+**/
+STATIC
+VOID
+EFIAPI
+CheckAllAPsStatus (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  UINTN  Index;
+
+  mCpuMpData.TimeTaken += POLL_INTERVAL_US;
+
+  for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    UpdateApStatus (Index);
+  }
+
+  if (mCpuMpData.TimeoutActive && (mCpuMpData.TimeTaken > mCpuMpData.Timeout)) {
+    ProcessStartupAllAPsTimeout ();
+
+    // Force terminal exit
+    mCpuMpData.FinishCount = mCpuMpData.StartCount;
+  }
+
+  if (mCpuMpData.FinishCount != mCpuMpData.StartCount) {
+    return;
+  }
+
+  gBS->SetTimer (
+         mCpuMpData.CheckAllAPsEvent,
+         TimerCancel,
+         0
+         );
+
+  if (mCpuMpData.FailedListIndex == 0) {
+    if (mCpuMpData.FailedList != NULL) {
+      // Since we don't have the original `FailedCpuList`
+      // pointer here to set to NULL, don't free the
+      // memory.
+    }
+  }
+
+  gBS->SignalEvent (mCpuMpData.WaitEvent);
+}
+
+/** Invoked periodically via a timer to check the state of the processor.
+
+   @param Event   The event supplied by the timer expiration.
+   @param Context The processor context.
+
+**/
+STATIC
+VOID
+EFIAPI
+CheckThisAPStatus (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  EFI_STATUS   Status;
+  CPU_AP_DATA  *CpuData;
+  CPU_STATE    State;
+
+  CpuData = Context;
+
+  mCpuMpData.TimeTaken += POLL_INTERVAL_US;
+
+  State = GetApState (CpuData);
+
+  if (State == CpuStateFinished) {
+    Status = gBS->SetTimer (CpuData->CheckThisAPEvent, TimerCancel, 0);
+    ASSERT_EFI_ERROR (Status);
+
+    if (mCpuMpData.SingleApFinished != NULL) {
+      *mCpuMpData.SingleApFinished = TRUE;
+    }
+
+    if (mCpuMpData.WaitEvent != NULL) {
+      Status = gBS->SignalEvent (mCpuMpData.WaitEvent);
+      ASSERT_EFI_ERROR (Status);
+    }
+
+    CpuData->State = CpuStateIdle;
+  }
+
+  if (mCpuMpData.TimeoutActive && (mCpuMpData.TimeTaken > mCpuMpData.Timeout)) {
+    Status = gBS->SetTimer (CpuData->CheckThisAPEvent, TimerCancel, 0);
+    if (mCpuMpData.WaitEvent != NULL) {
+      Status = gBS->SignalEvent (mCpuMpData.WaitEvent);
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+}
+
+/**
+  This function is called by all processors (both BSP and AP) once and collects
+  MP related data.
+
+  @param BSP            TRUE if the processor is the BSP.
+  @param Mpidr          The MPIDR for the specified processor. This should be
+                        the full MPIDR and not only the affinity bits.
+  @param ProcessorIndex The index of the processor.
+
+  @return EFI_SUCCESS if the data for the processor collected and filled in.
+
+**/
+STATIC
+EFI_STATUS
+FillInProcessorInformation (
+  IN BOOLEAN  BSP,
+  IN UINTN    Mpidr,
+  IN UINTN    ProcessorIndex
+  )
+{
+  EFI_PROCESSOR_INFORMATION  *CpuInfo;
+
+  CpuInfo = &mCpuMpData.CpuData[ProcessorIndex].Info;
+
+  CpuInfo->ProcessorId = GET_MPIDR_AFFINITY_BITS (Mpidr);
+  CpuInfo->StatusFlag  = PROCESSOR_ENABLED_BIT | PROCESSOR_HEALTH_STATUS_BIT;
+
+  if (BSP) {
+    CpuInfo->StatusFlag |= PROCESSOR_AS_BSP_BIT;
+  }
+
+  if ((Mpidr & MPIDR_MT_BIT) > 0) {
+    CpuInfo->Location.Package = GET_MPIDR_AFF2 (Mpidr);
+    CpuInfo->Location.Core    = GET_MPIDR_AFF1 (Mpidr);
+    CpuInfo->Location.Thread  = GET_MPIDR_AFF0 (Mpidr);
+
+    CpuInfo->ExtendedInformation.Location2.Package = GET_MPIDR_AFF3 (Mpidr);
+    CpuInfo->ExtendedInformation.Location2.Die     = GET_MPIDR_AFF2 (Mpidr);
+    CpuInfo->ExtendedInformation.Location2.Core    = GET_MPIDR_AFF1 (Mpidr);
+    CpuInfo->ExtendedInformation.Location2.Thread  = GET_MPIDR_AFF0 (Mpidr);
+  } else {
+    CpuInfo->Location.Package = GET_MPIDR_AFF1 (Mpidr);
+    CpuInfo->Location.Core    = GET_MPIDR_AFF0 (Mpidr);
+    CpuInfo->Location.Thread  = 0;
+
+    CpuInfo->ExtendedInformation.Location2.Package = GET_MPIDR_AFF2 (Mpidr);
+    CpuInfo->ExtendedInformation.Location2.Die     = GET_MPIDR_AFF1 (Mpidr);
+    CpuInfo->ExtendedInformation.Location2.Core    = GET_MPIDR_AFF0 (Mpidr);
+    CpuInfo->ExtendedInformation.Location2.Thread  = 0;
+  }
+
+  mCpuMpData.CpuData[ProcessorIndex].State = BSP ? CpuStateBusy : CpuStateIdle;
+
+  mCpuMpData.CpuData[ProcessorIndex].Procedure = NULL;
+  mCpuMpData.CpuData[ProcessorIndex].Parameter = NULL;
+
+  return EFI_SUCCESS;
+}
+
+/** Initializes the MP Services system data
+
+   @param NumberOfProcessors The number of processors, both BSP and AP.
+   @param CoreInfo           CPU information gathered earlier during boot.
+
+**/
+STATIC
+EFI_STATUS
+MpServicesInitialize (
+  IN   UINTN                NumberOfProcessors,
+  IN   CONST ARM_CORE_INFO  *CoreInfo
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  EFI_EVENT   ReadyToBootEvent;
+  BOOLEAN     IsBsp;
+
+  //
+  // Clear the data structure area first.
+  //
+  ZeroMem (&mCpuMpData, sizeof (CPU_MP_DATA));
+  //
+  // First BSP fills and inits all known values, including its own records.
+  //
+  mCpuMpData.NumberOfProcessors        = NumberOfProcessors;
+  mCpuMpData.NumberOfEnabledProcessors = NumberOfProcessors;
+
+  mCpuMpData.CpuData = AllocateZeroPool (
+                         mCpuMpData.NumberOfProcessors * sizeof (CPU_AP_DATA)
+                         );
+
+  if (mCpuMpData.CpuData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  /* Allocate one extra for the sentinel entry at the end */
+  gProcessorIDs = AllocateZeroPool ((mCpuMpData.NumberOfProcessors + 1) * sizeof (UINT64));
+  ASSERT (gProcessorIDs != NULL);
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  CheckAllAPsStatus,
+                  NULL,
+                  &mCpuMpData.CheckAllAPsEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  gApStacksBase = AllocatePages (
+                    EFI_SIZE_TO_PAGES (
+                      mCpuMpData.NumberOfProcessors *
+                      gApStackSize
+                      )
+                    );
+  ASSERT (gApStacksBase != NULL);
+
+  for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    if (GET_MPIDR_AFFINITY_BITS (ArmReadMpidr ()) == CoreInfo[Index].Mpidr) {
+      IsBsp = TRUE;
+    } else {
+      IsBsp = FALSE;
+    }
+
+    FillInProcessorInformation (IsBsp, CoreInfo[Index].Mpidr, Index);
+
+    gProcessorIDs[Index] = mCpuMpData.CpuData[Index].Info.ProcessorId;
+
+    Status = gBS->CreateEvent (
+                    EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                    TPL_CALLBACK,
+                    CheckThisAPStatus,
+                    (VOID *)&mCpuMpData.CpuData[Index],
+                    &mCpuMpData.CpuData[Index].CheckThisAPEvent
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  gProcessorIDs[Index] = MAX_UINT64;
+
+  //
+  // The global pointer variables as well as the gProcessorIDs array contents
+  // are accessed by the other cores so we must clean them to the PoC
+  //
+  WriteBackDataCacheRange (&gProcessorIDs, sizeof (UINT64 *));
+  WriteBackDataCacheRange (&gApStacksBase, sizeof (UINT64 *));
+
+  WriteBackDataCacheRange (
+    gProcessorIDs,
+    (mCpuMpData.NumberOfProcessors + 1) * sizeof (UINT64)
+    );
+
+  mNonBlockingModeAllowed = TRUE;
+
+  Status = EfiCreateEventReadyToBootEx (
+             TPL_CALLBACK,
+             ReadyToBootSignaled,
+             NULL,
+             &ReadyToBootEvent
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Event notification function called when the EFI_EVENT_GROUP_READY_TO_BOOT is
+  signaled. After this point, non-blocking mode is no longer allowed.
+
+  @param  Event     Event whose notification function is being invoked.
+  @param  Context   The pointer to the notification function's context,
+                    which is implementation-dependent.
+
+**/
+STATIC
+VOID
+EFIAPI
+ReadyToBootSignaled (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  mNonBlockingModeAllowed = FALSE;
+}
+
+/** Initialize multi-processor support.
+
+  @param ImageHandle  Image handle.
+  @param SystemTable  System table.
+
+  @return EFI_SUCCESS on success, or an error code.
+
+**/
+EFI_STATUS
+EFIAPI
+ArmPsciMpServicesDxeInitialize (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_HANDLE                 Handle;
+  UINTN                      MaxCpus;
+  EFI_LOADED_IMAGE_PROTOCOL  *Image;
+  EFI_HOB_GENERIC_HEADER     *Hob;
+  VOID                       *HobData;
+  UINTN                      HobDataSize;
+  CONST ARM_CORE_INFO        *CoreInfo;
+
+  MaxCpus = 1;
+
+  Status = gBS->HandleProtocol (
+                  ImageHandle,
+                  &gEfiLoadedImageProtocolGuid,
+                  (VOID **)&Image
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Parts of the code in this driver may be executed by other cores running
+  // with the MMU off so we need to ensure that everything is clean to the
+  // point of coherency (PoC)
+  //
+  WriteBackDataCacheRange (Image->ImageBase, Image->ImageSize);
+
+  Hob = GetFirstGuidHob (&gArmMpCoreInfoGuid);
+  if (Hob != NULL) {
+    HobData     = GET_GUID_HOB_DATA (Hob);
+    HobDataSize = GET_GUID_HOB_DATA_SIZE (Hob);
+    CoreInfo    = (ARM_CORE_INFO *)HobData;
+    MaxCpus     = HobDataSize / sizeof (ARM_CORE_INFO);
+  }
+
+  if (MaxCpus == 1) {
+    DEBUG ((DEBUG_WARN, "Trying to use EFI_MP_SERVICES_PROTOCOL on a UP system"));
+    // We are not MP so nothing to do
+    return EFI_NOT_FOUND;
+  }
+
+  Status = MpServicesInitialize (MaxCpus, CoreInfo);
+  if (Status != EFI_SUCCESS) {
+    ASSERT_EFI_ERROR (Status);
+    return Status;
+  }
+
+  //
+  // Now install the MP services protocol.
+  //
+  Handle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gEfiMpServiceProtocolGuid,
+                  &mMpServicesProtocol,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/** AP exception handler.
+
+  @param InterruptType The AArch64 CPU exception type.
+  @param SystemContext System context.
+
+**/
+STATIC
+VOID
+EFIAPI
+ApExceptionHandler (
+  IN CONST EFI_EXCEPTION_TYPE  InterruptType,
+  IN CONST EFI_SYSTEM_CONTEXT  SystemContext
+  )
+{
+  ARM_SMC_ARGS  Args;
+  UINT64        Mpidr;
+  UINTN         Index;
+  UINTN         ProcessorIndex;
+
+  Mpidr = GET_MPIDR_AFFINITY_BITS (ArmReadMpidr ());
+
+  Index          = 0;
+  ProcessorIndex = MAX_UINT64;
+
+  do {
+    if (gProcessorIDs[Index] == Mpidr) {
+      ProcessorIndex = Index;
+      break;
+    }
+
+    Index++;
+  } while (gProcessorIDs[Index] != MAX_UINT64);
+
+  if (ProcessorIndex != MAX_UINT64) {
+    mCpuMpData.CpuData[ProcessorIndex].State = CpuStateFinished;
+    ArmDataMemoryBarrier ();
+  }
+
+  Args.Arg0 = ARM_SMC_ID_PSCI_CPU_OFF;
+  ArmCallSmc (&Args);
+
+  /* Should never be reached */
+  ASSERT (FALSE);
+  CpuDeadLoop ();
+}
+
+/** C entry-point for the AP.
+    This function gets called from the assembly function ApEntryPoint.
+
+**/
+VOID
+ApProcedure (
+  VOID
+  )
+{
+  ARM_SMC_ARGS      Args;
+  EFI_AP_PROCEDURE  UserApProcedure;
+  VOID              *UserApParameter;
+  UINTN             ProcessorIndex;
+
+  ProcessorIndex = 0;
+
+  WhoAmI (&mMpServicesProtocol, &ProcessorIndex);
+
+  /* Fetch the user-supplied procedure and parameter to execute */
+  UserApProcedure = mCpuMpData.CpuData[ProcessorIndex].Procedure;
+  UserApParameter = mCpuMpData.CpuData[ProcessorIndex].Parameter;
+
+  // Configure the MMU and caches
+  ArmSetTCR (mCpuMpData.CpuData[ProcessorIndex].Tcr);
+  ArmSetTTBR0 (mCpuMpData.CpuData[ProcessorIndex].Ttbr0);
+  ArmSetMAIR (mCpuMpData.CpuData[ProcessorIndex].Mair);
+  ArmDisableAlignmentCheck ();
+  ArmEnableStackAlignmentCheck ();
+  ArmEnableInstructionCache ();
+  ArmEnableDataCache ();
+  ArmEnableMmu ();
+
+  InitializeCpuExceptionHandlers (NULL);
+  RegisterCpuInterruptHandler (EXCEPT_AARCH64_SYNCHRONOUS_EXCEPTIONS, ApExceptionHandler);
+  RegisterCpuInterruptHandler (EXCEPT_AARCH64_IRQ, ApExceptionHandler);
+  RegisterCpuInterruptHandler (EXCEPT_AARCH64_FIQ, ApExceptionHandler);
+  RegisterCpuInterruptHandler (EXCEPT_AARCH64_SERROR, ApExceptionHandler);
+
+  UserApProcedure (UserApParameter);
+
+  mCpuMpData.CpuData[ProcessorIndex].State = CpuStateFinished;
+
+  ArmDataMemoryBarrier ();
+
+  /* Since we're finished with this AP, turn it off */
+  Args.Arg0 = ARM_SMC_ID_PSCI_CPU_OFF;
+  ArmCallSmc (&Args);
+
+  /* Should never be reached */
+  ASSERT (FALSE);
+  CpuDeadLoop ();
+}
+
+/** Returns whether the processor executing this function is the BSP.
+
+    @return Whether the current processor is the BSP.
+**/
+STATIC
+BOOLEAN
+IsCurrentProcessorBSP (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       ProcessorIndex;
+
+  Status = WhoAmI (&mMpServicesProtocol, &ProcessorIndex);
+  if (EFI_ERROR (Status)) {
+    ASSERT_EFI_ERROR (Status);
+    return FALSE;
+  }
+
+  return IsProcessorBSP (ProcessorIndex);
+}
+
+/** Returns whether the specified processor is enabled.
+
+   @param[in] ProcessorIndex The index of the processor to check.
+
+   @return TRUE if the processor is enabled, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorEnabled (
+  UINTN  ProcessorIndex
+  )
+{
+  EFI_PROCESSOR_INFORMATION  *CpuInfo;
+
+  CpuInfo = &mCpuMpData.CpuData[ProcessorIndex].Info;
+
+  return (CpuInfo->StatusFlag & PROCESSOR_ENABLED_BIT) != 0;
+}
+
+/** Sets up the state for the StartupAllAPs function.
+
+   @param SingleThread Whether the APs will execute sequentially.
+
+**/
+STATIC
+VOID
+StartupAllAPsPrepareState (
+  IN BOOLEAN  SingleThread
+  )
+{
+  UINTN        Index;
+  CPU_STATE    APInitialState;
+  CPU_AP_DATA  *CpuData;
+
+  mCpuMpData.FinishCount  = 0;
+  mCpuMpData.StartCount   = 0;
+  mCpuMpData.SingleThread = SingleThread;
+
+  APInitialState = CpuStateReady;
+
+  for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData = &mCpuMpData.CpuData[Index];
+
+    //
+    // Get APs prepared, and put failing APs into FailedCpuList.
+    // If "SingleThread", only 1 AP will put into ready state, other AP will be
+    // put into ready state 1 by 1, until the previous 1 finished its task.
+    // If not "SingleThread", all APs are put into ready state from the
+    // beginning
+    //
+
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    if (!IsProcessorEnabled (Index)) {
+      // Skip Disabled processors
+      if (mCpuMpData.FailedList != NULL) {
+        mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] = Index;
+      }
+
+      continue;
+    }
+
+    CpuData->State = APInitialState;
+
+    mCpuMpData.StartCount++;
+    if (SingleThread) {
+      APInitialState = CpuStateBlocked;
+    }
+  }
+}
+
+/** Handles execution of StartupAllAPs when a WaitEvent has been specified.
+
+  @param Procedure         The user-supplied procedure.
+  @param ProcedureArgument The user-supplied procedure argument.
+  @param WaitEvent         The wait event to be signaled when the work is
+                           complete or a timeout has occurred.
+  @param TimeoutInMicroseconds The timeout for the work to be completed. Zero
+                               indicates an infinite timeout.
+  @param SingleThread          Whether the APs will execute sequentially.
+  @param FailedCpuList         User-supplied pointer for list of failed CPUs.
+
+   @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsWithWaitEvent (
+  IN EFI_AP_PROCEDURE  Procedure,
+  IN VOID              *ProcedureArgument,
+  IN EFI_EVENT         WaitEvent,
+  IN UINTN             TimeoutInMicroseconds,
+  IN BOOLEAN           SingleThread,
+  IN UINTN             **FailedCpuList
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        Index;
+  CPU_AP_DATA  *CpuData;
+
+  for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData = &mCpuMpData.CpuData[Index];
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    if (!IsProcessorEnabled (Index)) {
+      // Skip Disabled processors
+      continue;
+    }
+
+    if (GetApState (CpuData) == CpuStateReady) {
+      SetApProcedure (CpuData, Procedure, ProcedureArgument);
+      if ((mCpuMpData.StartCount == 0) || !SingleThread) {
+        Status = DispatchCpu (Index);
+        if (EFI_ERROR (Status)) {
+          AddProcessorToFailedList (Index, CpuData->State);
+          break;
+        }
+      }
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Save data into private data structure, and create timer to poll AP state
+  // before exiting
+  //
+  mCpuMpData.Procedure         = Procedure;
+  mCpuMpData.ProcedureArgument = ProcedureArgument;
+  mCpuMpData.WaitEvent         = WaitEvent;
+  mCpuMpData.Timeout           = TimeoutInMicroseconds;
+  mCpuMpData.TimeTaken         = 0;
+  mCpuMpData.TimeoutActive     = (BOOLEAN)(TimeoutInMicroseconds != 0);
+  Status                       = gBS->SetTimer (
+                                        mCpuMpData.CheckAllAPsEvent,
+                                        TimerPeriodic,
+                                        POLL_INTERVAL_US
+                                        );
+
+  return Status;
+}
+
+/** Handles execution of StartupAllAPs when no wait event has been specified.
+
+  @param Procedure             The user-supplied procedure.
+  @param ProcedureArgument     The user-supplied procedure argument.
+  @param TimeoutInMicroseconds The timeout for the work to be completed. Zero
+                                indicates an infinite timeout.
+  @param SingleThread          Whether the APs will execute sequentially.
+  @param FailedCpuList         User-supplied pointer for list of failed CPUs.
+
+  @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsNoWaitEvent (
+  IN EFI_AP_PROCEDURE  Procedure,
+  IN VOID              *ProcedureArgument,
+  IN UINTN             TimeoutInMicroseconds,
+  IN BOOLEAN           SingleThread,
+  IN UINTN             **FailedCpuList
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        Index;
+  UINTN        NextIndex;
+  UINTN        Timeout;
+  CPU_AP_DATA  *CpuData;
+  BOOLEAN      DispatchError;
+
+  Timeout       = TimeoutInMicroseconds;
+  DispatchError = FALSE;
+
+  while (TRUE) {
+    for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+      CpuData = &mCpuMpData.CpuData[Index];
+      if (IsProcessorBSP (Index)) {
+        // Skip BSP
+        continue;
+      }
+
+      if (!IsProcessorEnabled (Index)) {
+        // Skip Disabled processors
+        continue;
+      }
+
+      switch (GetApState (CpuData)) {
+        case CpuStateReady:
+          SetApProcedure (CpuData, Procedure, ProcedureArgument);
+          Status = DispatchCpu (Index);
+          if (EFI_ERROR (Status)) {
+            AddProcessorToFailedList (Index, CpuData->State);
+            CpuData->State = CpuStateIdle;
+            mCpuMpData.StartCount--;
+            DispatchError = TRUE;
+
+            if (SingleThread) {
+              // Dispatch the next available AP
+              Status = GetNextBlockedNumber (&NextIndex);
+              if (!EFI_ERROR (Status)) {
+                mCpuMpData.CpuData[NextIndex].State = CpuStateReady;
+              }
+            }
+          }
+
+          break;
+
+        case CpuStateFinished:
+          mCpuMpData.FinishCount++;
+          if (SingleThread) {
+            Status = GetNextBlockedNumber (&NextIndex);
+            if (!EFI_ERROR (Status)) {
+              mCpuMpData.CpuData[NextIndex].State = CpuStateReady;
+            }
+          }
+
+          CpuData->State = CpuStateIdle;
+          break;
+
+        default:
+          break;
+      }
+    }
+
+    if (mCpuMpData.FinishCount == mCpuMpData.StartCount) {
+      Status = EFI_SUCCESS;
+      break;
+    }
+
+    if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
+      Status = EFI_TIMEOUT;
+      break;
+    }
+
+    Timeout -= CalculateAndStallInterval (Timeout);
+  }
+
+  if (Status == EFI_TIMEOUT) {
+    // Add any remaining CPUs to the FailedCpuList
+    if (FailedCpuList != NULL) {
+      for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+        AddProcessorToFailedList (Index, mCpuMpData.CpuData[Index].State);
+      }
+    }
+  }
+
+  if (DispatchError) {
+    Status = EFI_NOT_READY;
+  }
+
+  return Status;
+}
diff --git a/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S b/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S
new file mode 100644
index 000000000000..a90fa8a0075f
--- /dev/null
+++ b/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S
@@ -0,0 +1,57 @@
+#===============================================================================
+#  Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#===============================================================================
+
+.text
+.align 3
+
+#include <AsmMacroIoLibV8.h>
+#include <IndustryStandard/ArmStdSmc.h>
+
+#include "MpServicesInternal.h"
+
+GCC_ASM_IMPORT (gApStacksBase)
+GCC_ASM_IMPORT (gProcessorIDs)
+GCC_ASM_IMPORT (ApProcedure)
+GCC_ASM_IMPORT (gApStackSize)
+
+GCC_ASM_EXPORT (ApEntryPoint)
+
+// Entry-point for the AP
+// VOID
+// ApEntryPoint (
+//   VOID
+//   );
+ASM_PFX(ApEntryPoint):
+  mrs x0, mpidr_el1
+  // Mask the non-affinity bits
+  bic x0, x0, 0x00ff000000
+  and x0, x0, 0xffffffffff
+  ldr x1, gProcessorIDs
+  mov x2, 0                   // x2 = processor index
+
+// Find index in gProcessorIDs for current processor
+1:
+  ldr x3, [x1, x2, lsl #3]    // x4 = gProcessorIDs + x2 * 8
+  cmp x3, #-1                 // check if we've reached the end of gProcessorIDs
+  beq ProcessorNotFound
+  add x2, x2, 1               // x2++
+  cmp x0, x3                  // if mpidr_el1 != gProcessorIDs[x] then loop
+  bne 1b
+
+// Calculate stack address
+  // x2 contains the index for the current processor plus 1
+  ldr x0, gApStacksBase
+  ldr x1, gApStackSize
+  mul x3, x2, x1              // x3 = (ProcessorIndex + 1) * gApStackSize
+  add sp, x0, x3              // sp = gApStacksBase + x3
+  mov x29, xzr
+  bl ApProcedure              // doesn't return
+
+ProcessorNotFound:
+// Turn off the processor
+  MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF)
+  smc #0
+  b .
-- 
2.30.2


^ permalink raw reply related	[flat|nested] 4+ messages in thread

* [PATCH v3 3/3] MdeModulePkg: Add new Application/MpServicesTest application
  2023-01-04 15:30 [PATCH v3 0/3] ArmPkg,MdeModulePkg: Implement EFI_MP_SERVICES_PROTOCOL for AArch64 and add MpServicesTest application Rebecca Cran
  2023-01-04 15:30 ` [PATCH v3 1/3] ArmPkg: Add GET_MPIDR_AFFINITY_BITS and MPIDR_MT_BIT to ArmLib.h Rebecca Cran
  2023-01-04 15:30 ` [PATCH v3 2/3] ArmPkg: implement EFI_MP_SERVICES_PROTOCOL based on PSCI calls Rebecca Cran
@ 2023-01-04 15:30 ` Rebecca Cran
  2 siblings, 0 replies; 4+ messages in thread
From: Rebecca Cran @ 2023-01-04 15:30 UTC (permalink / raw)
  To: devel, Sami Mujawar, Ard Biesheuvel, Leif Lindholm, Jian J Wang,
	Liming Gao
  Cc: Rebecca Cran, Ard Biesheuvel

The MpServicesTest application exercises the EFI_MP_SERVICES_PROTOCOL.

usage:
  MpServicesTest -A [-O]
  MpServicesTest -T <Timeout>
  MpServicesTest -S <Processor #>
  MpServicesTest -P
  MpServicesTest -U
  MpServicesTest -W
  MpServicesTest -E <Processor #>
  MpServicesTest -D <Processor #>
  MpServicesTest -h

Parameter:
  -A:  Run all APs.
  -O:  Run APs sequentially (use with -A).
  -T:  Specify timeout in milliseconds. Default is to wait forever.
  -S:  Specify the single AP to run.
  -P:  Print processor information.
  -U:  Set the specified AP to the Unhealthy status (use with -E/-D).
  -W:  Run WhoAmI and print index of BSP.
  -E:  Enable the specified AP.
  -D:  Disable the specified AP.
  -h:  Print this help page.

Signed-off-by: Rebecca Cran <rebecca@quicinc.com>
Reviewed-by: Ard Biesheuvel <ardb@kernel.org>
---
 MdeModulePkg/MdeModulePkg.dsc                              |   2 +
 MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf |  40 ++
 MdeModulePkg/Application/MpServicesTest/Options.h          |  39 ++
 MdeModulePkg/Application/MpServicesTest/MpServicesTest.c   | 560 ++++++++++++++++++++
 MdeModulePkg/Application/MpServicesTest/Options.c          | 164 ++++++
 5 files changed, 805 insertions(+)

diff --git a/MdeModulePkg/MdeModulePkg.dsc b/MdeModulePkg/MdeModulePkg.dsc
index 659482ab737f..6992b3ae8db6 100644
--- a/MdeModulePkg/MdeModulePkg.dsc
+++ b/MdeModulePkg/MdeModulePkg.dsc
@@ -166,6 +166,7 @@ [LibraryClasses.common.UEFI_APPLICATION]
   MemoryAllocationLib|MdePkg/Library/UefiMemoryAllocationLib/UefiMemoryAllocationLib.inf
   DebugLib|MdePkg/Library/UefiDebugLibStdErr/UefiDebugLibStdErr.inf
   FileHandleLib|MdePkg/Library/UefiFileHandleLib/UefiFileHandleLib.inf
+  ShellLib|ShellPkg/Library/UefiShellLib/UefiShellLib.inf
 
 [LibraryClasses.common.MM_STANDALONE]
   HobLib|MdeModulePkg/Library/BaseHobLibNull/BaseHobLibNull.inf
@@ -445,6 +446,7 @@ [Components]
   MdeModulePkg/Library/BaseVariableFlashInfoLib/BaseVariableFlashInfoLib.inf
 
 [Components.IA32, Components.X64, Components.AARCH64]
+  MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf
   MdeModulePkg/Universal/EbcDxe/EbcDxe.inf
   MdeModulePkg/Universal/EbcDxe/EbcDebugger.inf
   MdeModulePkg/Universal/EbcDxe/EbcDebuggerConfig.inf
diff --git a/MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf b/MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf
new file mode 100644
index 000000000000..07ee4afec845
--- /dev/null
+++ b/MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf
@@ -0,0 +1,40 @@
+## @file
+#  UEFI Application to exercise EFI_MP_SERVICES_PROTOCOL.
+#
+#  Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 1.29
+  BASE_NAME                      = MpServicesTest
+  FILE_GUID                      = 43e9defa-7209-4b0d-b136-cc4ca02cb469
+  MODULE_TYPE                    = UEFI_APPLICATION
+  VERSION_STRING                 = 0.1
+  ENTRY_POINT                    = UefiMain
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 AARCH64
+#
+
+[Sources]
+  MpServicesTest.c
+  Options.c
+  Options.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  ShellLib
+  UefiApplicationEntryPoint
+  UefiLib
+
+[Protocols]
+  gEfiMpServiceProtocolGuid    ## CONSUMES
+
diff --git a/MdeModulePkg/Application/MpServicesTest/Options.h b/MdeModulePkg/Application/MpServicesTest/Options.h
new file mode 100644
index 000000000000..cb28230ab095
--- /dev/null
+++ b/MdeModulePkg/Application/MpServicesTest/Options.h
@@ -0,0 +1,39 @@
+/** @file
+  Options handling code.
+
+  Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+
+#ifndef MPSERVICESTEST_OPTIONS_H_
+#define MPSERVICESTEST_OPTIONS_H_
+
+#define INFINITE_TIMEOUT  0
+
+typedef struct {
+  UINTN      Timeout;
+  UINTN      ProcessorIndex;
+  BOOLEAN    RunAllAPs;
+  BOOLEAN    RunSingleAP;
+  BOOLEAN    DisableProcessor;
+  BOOLEAN    EnableProcessor;
+  BOOLEAN    SetProcessorHealthy;
+  BOOLEAN    SetProcessorUnhealthy;
+  BOOLEAN    PrintProcessorInformation;
+  BOOLEAN    PrintBspProcessorIndex;
+  BOOLEAN    RunAPsSequentially;
+} MP_SERVICES_TEST_OPTIONS;
+
+/**
+  Parses any arguments provided on the command line.
+
+  @param Options  The arguments structure.
+
+  @return EFI_SUCCESS on success, or an error code.
+**/
+EFI_STATUS
+ParseArguments (
+  MP_SERVICES_TEST_OPTIONS  *Options
+  );
+
+#endif /* MPSERVICESTEST_OPTIONS_H_ */
diff --git a/MdeModulePkg/Application/MpServicesTest/MpServicesTest.c b/MdeModulePkg/Application/MpServicesTest/MpServicesTest.c
new file mode 100644
index 000000000000..3f3d9752d500
--- /dev/null
+++ b/MdeModulePkg/Application/MpServicesTest/MpServicesTest.c
@@ -0,0 +1,560 @@
+/** @file
+  UEFI Application to exercise EFI_MP_SERVICES_PROTOCOL.
+
+  Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+
+#include <Uefi.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Pi/PiMultiPhase.h>
+#include <Protocol/MpService.h>
+
+#include "Options.h"
+
+#define APFUNC_BUFFER_LEN  256
+
+typedef struct {
+  EFI_MP_SERVICES_PROTOCOL    *Mp;
+  CHAR16                      **Buffer;
+} APFUNC_ARG;
+
+/** The procedure to run with the MP Services interface.
+
+  @param Arg The procedure argument.
+
+**/
+STATIC
+VOID
+EFIAPI
+ApFunction (
+  IN OUT VOID  *Arg
+  )
+{
+  APFUNC_ARG  *Param;
+  UINTN       ProcessorId;
+
+  if (Arg != NULL) {
+    Param = Arg;
+
+    Param->Mp->WhoAmI (Param->Mp, &ProcessorId);
+    UnicodeSPrint (Param->Buffer[ProcessorId], APFUNC_BUFFER_LEN, L"Hello from CPU %ld\n", ProcessorId);
+  }
+}
+
+/**
+  Fetches the number of processors and which processor is the BSP.
+
+  @param Mp  MP Services Protocol.
+  @param NumProcessors Number of processors.
+  @param BspIndex      The index of the BSP.
+**/
+STATIC
+EFI_STATUS
+GetProcessorInformation (
+  IN  EFI_MP_SERVICES_PROTOCOL  *Mp,
+  OUT UINTN                     *NumProcessors,
+  OUT UINTN                     *BspIndex
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       NumEnabledProcessors;
+
+  Status = Mp->GetNumberOfProcessors (Mp, NumProcessors, &NumEnabledProcessors);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = Mp->WhoAmI (Mp, BspIndex);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/** Displays information returned from MP Services Protocol.
+
+  @param Mp       The MP Services Protocol
+  @param BspIndex On return, contains the index of the BSP.
+
+  @return The number of CPUs in the system.
+
+**/
+STATIC
+UINTN
+PrintProcessorInformation (
+  IN   EFI_MP_SERVICES_PROTOCOL  *Mp,
+  OUT  UINTN                     *BspIndex
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_PROCESSOR_INFORMATION  CpuInfo;
+  UINTN                      Index;
+  UINTN                      NumCpu;
+  UINTN                      NumEnabledCpu;
+
+  Status = Mp->GetNumberOfProcessors (Mp, &NumCpu, &NumEnabledCpu);
+  if (EFI_ERROR (Status)) {
+    Print (L"GetNumberOfProcessors failed: %r\n", Status);
+  } else {
+    Print (L"Number of CPUs: %ld, Enabled: %d\n", NumCpu, NumEnabledCpu);
+  }
+
+  for (Index = 0; Index < NumCpu; Index++) {
+    Status = Mp->GetProcessorInfo (Mp, CPU_V2_EXTENDED_TOPOLOGY | Index, &CpuInfo);
+    if (EFI_ERROR (Status)) {
+      Print (L"GetProcessorInfo for Processor %d failed: %r\n", Index, Status);
+    } else {
+      Print (
+        L"Processor %d:\n"
+        L"\tID: %016lx\n"
+        L"\tStatus: %s | ",
+        Index,
+        CpuInfo.ProcessorId,
+        (CpuInfo.StatusFlag & PROCESSOR_AS_BSP_BIT) ? L"BSP" : L"AP"
+        );
+
+      if ((CpuInfo.StatusFlag & PROCESSOR_AS_BSP_BIT) && (BspIndex != NULL)) {
+        *BspIndex = Index;
+      }
+
+      Print (L"%s | ", (CpuInfo.StatusFlag & PROCESSOR_ENABLED_BIT) ? L"Enabled" : L"Disabled");
+      Print (L"%s\n", (CpuInfo.StatusFlag & PROCESSOR_HEALTH_STATUS_BIT) ? L"Healthy" : L"Faulted");
+
+      Print (
+        L"\tLocation: Package %d, Core %d, Thread %d\n"
+        L"\tExtended Information: Package %d, Module %d, Tile %d, Die %d, Core %d, Thread %d\n\n",
+        CpuInfo.Location.Package,
+        CpuInfo.Location.Core,
+        CpuInfo.Location.Thread,
+        CpuInfo.ExtendedInformation.Location2.Package,
+        CpuInfo.ExtendedInformation.Location2.Module,
+        CpuInfo.ExtendedInformation.Location2.Tile,
+        CpuInfo.ExtendedInformation.Location2.Die,
+        CpuInfo.ExtendedInformation.Location2.Core,
+        CpuInfo.ExtendedInformation.Location2.Thread
+        );
+    }
+  }
+
+  return NumCpu;
+}
+
+/** Allocates memory in ApArg for the single AP specified.
+
+  @param ApArg          Pointer to the AP argument structure.
+  @param Mp             The MP Services Protocol.
+  @param ProcessorIndex The index of the AP.
+
+  @retval EFI_SUCCESS          Memory was successfully allocated.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+
+**/
+STATIC
+EFI_STATUS
+AllocateApFuncBufferSingleAP (
+  IN APFUNC_ARG                *ApArg,
+  IN EFI_MP_SERVICES_PROTOCOL  *Mp,
+  IN UINTN                     ProcessorIndex
+  )
+{
+  ApArg->Mp = Mp;
+
+  ApArg->Buffer = AllocateZeroPool ((ProcessorIndex + 1) * sizeof (VOID *));
+  if (ApArg->Buffer == NULL) {
+    Print (L"Failed to allocate buffer for AP buffer\n");
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ApArg->Buffer[ProcessorIndex] = AllocateZeroPool (APFUNC_BUFFER_LEN);
+  if (ApArg->Buffer[ProcessorIndex] == NULL) {
+    Print (L"Failed to allocate buffer for AP buffer\n");
+    FreePool (ApArg->Buffer);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/** Allocates memory in ApArg for all APs.
+
+  @param ApArg   Pointer to the AP argument structure.
+  @param Mp      The MP Services Protocol.
+  @param NumCpus The number of CPUs.
+
+  @retval EFI_SUCCESS          Memory was successfully allocated.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+
+**/
+STATIC
+EFI_STATUS
+AllocateApFuncBufferAllAPs (
+  IN APFUNC_ARG                *ApArg,
+  IN EFI_MP_SERVICES_PROTOCOL  *Mp,
+  IN UINTN                     NumCpus
+  )
+{
+  UINT32  Index;
+
+  ApArg->Mp = Mp;
+
+  ApArg->Buffer = AllocateZeroPool (NumCpus * sizeof (VOID *));
+  if (ApArg->Buffer == NULL) {
+    Print (L"Failed to allocate buffer for AP message\n");
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  for (Index = 0; Index < NumCpus; Index++) {
+    ApArg->Buffer[Index] = AllocateZeroPool (APFUNC_BUFFER_LEN);
+    if (ApArg->Buffer[Index] == NULL) {
+      Print (L"Failed to allocate buffer for AP message\n");
+      for (--Index; Index >= 0; Index++) {
+        FreePool (ApArg->Buffer[Index]);
+      }
+
+      FreePool (ApArg->Buffer);
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/** Frees memory in ApArg for all APs.
+
+  @param ApArg   Pointer to the AP argument structure.
+  @param NumCpus The number of CPUs.
+
+**/
+STATIC
+VOID
+FreeApFuncBuffer (
+  APFUNC_ARG  *ApArg,
+  UINTN       NumCpus
+  )
+{
+  UINTN  Index;
+
+  for (Index = 0; Index < NumCpus; Index++) {
+    if (ApArg->Buffer[Index] != NULL) {
+      FreePool (ApArg->Buffer[Index]);
+    }
+  }
+
+  FreePool (ApArg->Buffer);
+}
+
+/** Runs a specified AP.
+
+  @param Mp             The MP Services Protocol.
+  @param ProcessorIndex The processor index.
+  @param Timeout        Timeout in milliseconds.
+
+  @return EFI_SUCCESS on success, or an error code.
+
+**/
+STATIC
+EFI_STATUS
+StartupThisAP (
+  IN EFI_MP_SERVICES_PROTOCOL  *Mp,
+  IN UINTN                     ProcessorIndex,
+  IN UINTN                     Timeout
+  )
+{
+  EFI_STATUS  Status;
+  APFUNC_ARG  ApArg;
+
+  Status = AllocateApFuncBufferSingleAP (&ApArg, Mp, ProcessorIndex);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = AllocateApFuncBufferSingleAP (&ApArg, Mp, ProcessorIndex);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Print (
+    L"StartupThisAP on Processor %d with %d%s timeout...",
+    ProcessorIndex,
+    Timeout,
+    (Timeout == INFINITE_TIMEOUT) ? L" (infinite)" : L"ms"
+    );
+  Status = Mp->StartupThisAP (
+                 Mp,
+                 ApFunction,
+                 ProcessorIndex,
+                 NULL,
+                 Timeout * 1000,
+                 &ApArg,
+                 NULL
+                 );
+  if (EFI_ERROR (Status)) {
+    Print (L"failed: %r\n", Status);
+    return Status;
+  } else {
+    Print (L"done.\n");
+    Print (ApArg.Buffer[ProcessorIndex]);
+  }
+
+  FreeApFuncBuffer (&ApArg, ProcessorIndex + 1);
+
+  return EFI_SUCCESS;
+}
+
+/** Runs all APs.
+
+  @param Mp                 The MP Services Protocol.
+  @param NumCpus            The number of CPUs in the system.
+  @param Timeout            Timeout in milliseconds.
+  @param RunAPsSequentially Run APs sequentially (FALSE: run simultaneously)
+
+  @return EFI_SUCCESS on success, or an error code.
+
+**/
+STATIC
+EFI_STATUS
+StartupAllAPs (
+  IN EFI_MP_SERVICES_PROTOCOL  *Mp,
+  IN UINTN                     NumCpus,
+  IN UINTN                     Timeout,
+  IN BOOLEAN                   RunAPsSequentially
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  APFUNC_ARG  ApArg;
+
+  Status = AllocateApFuncBufferAllAPs (&ApArg, Mp, NumCpus);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Print (
+    L"Running with SingleThread %s, %u%s timeout...",
+    (RunAPsSequentially) ? L"TRUE" : L"FALSE",
+    Timeout,
+    (Timeout == INFINITE_TIMEOUT) ? L" (infinite)" : L"ms"
+    );
+
+  Status = Mp->StartupAllAPs (
+                 Mp,
+                 ApFunction,
+                 RunAPsSequentially,
+                 NULL,
+                 Timeout * 1000,
+                 &ApArg,
+                 NULL
+                 );
+
+  if (EFI_ERROR (Status)) {
+    Print (L"failed: %r\n", Status);
+
+    return Status;
+  } else {
+    Print (L"done.\n");
+
+    for (Index = 0; Index < NumCpus; Index++) {
+      Print (ApArg.Buffer[Index]);
+    }
+  }
+
+  FreeApFuncBuffer (&ApArg, NumCpus);
+  return EFI_SUCCESS;
+}
+
+/**
+  Enables the specified AP.
+
+  @param Mp               The MP Services Protocol.
+  @param ProcessorIndex   The processor to enable.
+  @param ProcessorHealthy The health status of the processor.
+
+  @return EFI_SUCCESS on success, or an error code.
+**/
+STATIC
+EFI_STATUS
+EnableAP (
+  IN EFI_MP_SERVICES_PROTOCOL  *Mp,
+  UINTN                        ProcessorIndex,
+  BOOLEAN                      ProcessorHealthy
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      HealthFlag;
+
+  if (ProcessorHealthy) {
+    Print (L"Enabling Processor %d with HealthFlag healthy...", ProcessorIndex);
+    HealthFlag = PROCESSOR_HEALTH_STATUS_BIT;
+  } else {
+    Print (L"Enabling Processor %d with HealthFlag faulted...", ProcessorIndex);
+    HealthFlag = 0;
+  }
+
+  Status = Mp->EnableDisableAP (Mp, ProcessorIndex, TRUE, &HealthFlag);
+  if (EFI_ERROR (Status)) {
+    Print (L"failed: %r\n", Status);
+    return Status;
+  } else {
+    Print (L"done.\n");
+  }
+
+  return Status;
+}
+
+/**
+  Disables the specified AP.
+
+  @param Mp               The MP Services Protocol.
+  @param ProcessorIndex   The processor to disable.
+  @param ProcessorHealthy The health status of the processor.
+
+  @return EFI_SUCCESS on success, or an error code.
+**/
+STATIC
+EFI_STATUS
+DisableAP (
+  IN EFI_MP_SERVICES_PROTOCOL  *Mp,
+  UINTN                        ProcessorIndex,
+  BOOLEAN                      ProcessorHealthy
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      HealthFlag;
+
+  if (ProcessorHealthy) {
+    Print (L"Disabling Processor %d with HealthFlag healthy...", ProcessorIndex);
+    HealthFlag = PROCESSOR_HEALTH_STATUS_BIT;
+  } else {
+    Print (L"Disabling Processor %d with HealthFlag faulted...", ProcessorIndex);
+    HealthFlag = 0;
+  }
+
+  Status = Mp->EnableDisableAP (Mp, ProcessorIndex, FALSE, &HealthFlag);
+  if (EFI_ERROR (Status)) {
+    Print (L"failed: %r\n", Status);
+    return Status;
+  } else {
+    Print (L"done.\n");
+  }
+
+  return Status;
+}
+
+/**
+  The user Entry Point for Application. The user code starts with this function
+  as the real entry point for the application.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+UefiMain (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+  EFI_MP_SERVICES_PROTOCOL  *Mp;
+  UINTN                     BspIndex;
+  UINTN                     CpuIndex;
+  UINTN                     NumCpus;
+  BOOLEAN                   ProcessorHealthy;
+  MP_SERVICES_TEST_OPTIONS  Options;
+
+  BspIndex = 0;
+
+  Status = gBS->LocateProtocol (
+                  &gEfiMpServiceProtocolGuid,
+                  NULL,
+                  (VOID **)&Mp
+                  );
+  if (EFI_ERROR (Status)) {
+    Print (L"Failed to locate EFI_MP_SERVICES_PROTOCOL (%r). Not installed on platform?\n", Status);
+    return EFI_NOT_FOUND;
+  }
+
+  Status = ParseArguments (&Options);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = GetProcessorInformation (Mp, &NumCpus, &BspIndex);
+  if (EFI_ERROR (Status)) {
+    Print (L"Error: Failed to fetch processor information.\n");
+    return Status;
+  }
+
+  if (Options.PrintBspProcessorIndex) {
+    Status = Mp->WhoAmI (Mp, &CpuIndex);
+    if (EFI_ERROR (Status)) {
+      Print (L"WhoAmI failed: %r\n", Status);
+      return Status;
+    } else {
+      Print (L"BSP: %016lx\n", CpuIndex);
+    }
+  }
+
+  if (Options.PrintProcessorInformation) {
+    NumCpus = PrintProcessorInformation (Mp, &BspIndex);
+    if (NumCpus < 2) {
+      Print (L"Error: Uniprocessor system found.\n");
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  if (Options.RunSingleAP) {
+    Status = StartupThisAP (
+               Mp,
+               Options.ProcessorIndex,
+               Options.Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if (Options.RunAllAPs) {
+    Status = StartupAllAPs (Mp, NumCpus, Options.Timeout, Options.RunAPsSequentially);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if (Options.EnableProcessor) {
+    ProcessorHealthy = TRUE;
+    if (Options.SetProcessorUnhealthy) {
+      ProcessorHealthy = FALSE;
+    }
+
+    Status = EnableAP (Mp, Options.ProcessorIndex, ProcessorHealthy);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if (Options.DisableProcessor) {
+    ProcessorHealthy = TRUE;
+    if (Options.SetProcessorUnhealthy) {
+      ProcessorHealthy = FALSE;
+    }
+
+    Status = DisableAP (Mp, Options.ProcessorIndex, ProcessorHealthy);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/MdeModulePkg/Application/MpServicesTest/Options.c b/MdeModulePkg/Application/MpServicesTest/Options.c
new file mode 100644
index 000000000000..e820c061e1ec
--- /dev/null
+++ b/MdeModulePkg/Application/MpServicesTest/Options.c
@@ -0,0 +1,164 @@
+/** @file
+  Options handling code.
+
+  Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved.<BR>
+  Copyright (c) 2010-2019  Finnbarr P. Murphy.   All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+
+#include <Uefi.h>
+#include <Library/BaseMemoryLib.h>
+#include <Protocol/ShellParameters.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#include "Options.h"
+
+STATIC UINTN   Argc;
+STATIC CHAR16  **Argv;
+
+/**
+
+  This function provides argc and argv.
+
+  @return Status
+**/
+EFI_STATUS
+GetArg (
+  VOID
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_SHELL_PARAMETERS_PROTOCOL  *ShellParameters;
+
+  Status = gBS->HandleProtocol (
+                  gImageHandle,
+                  &gEfiShellParametersProtocolGuid,
+                  (VOID **)&ShellParameters
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Argc = ShellParameters->Argc;
+  Argv = ShellParameters->Argv;
+  return EFI_SUCCESS;
+}
+
+/**
+  Print app usage.
+**/
+STATIC
+VOID
+PrintUsage (
+  VOID
+  )
+{
+  Print (L"MpServicesTest:  usage\n");
+  Print (L"  MpServicesTest -A [-O]\n");
+  Print (L"  MpServicesTest -T <Timeout>\n");
+  Print (L"  MpServicesTest -S <Processor #>\n");
+  Print (L"  MpServicesTest -P\n");
+  Print (L"  MpServicesTest -U\n");
+  Print (L"  MpServicesTest -W\n");
+  Print (L"  MpServicesTest -E <Processor #>\n");
+  Print (L"  MpServicesTest -D <Processor #>\n");
+  Print (L"  MpServicesTest -h\n");
+  Print (L"Parameter:\n");
+  Print (L"  -A:  Run all APs.\n");
+  Print (L"  -O:  Run APs sequentially (use with -A).\n");
+  Print (L"  -T:  Specify timeout in milliseconds. Default is to wait forever.\n");
+  Print (L"  -S:  Specify the single AP to run.\n");
+  Print (L"  -P:  Print processor information.\n");
+  Print (L"  -U:  Set the specified AP to the Unhealthy status (use with -E/-D).\n");
+  Print (L"  -W:  Run WhoAmI and print index of BSP.\n");
+  Print (L"  -E:  Enable the specified AP.\n");
+  Print (L"  -D:  Disable the specified AP.\n");
+  Print (L"  -h:  Print this help page.\n");
+}
+
+/**
+  Parses any arguments provided on the command line.
+
+  @param Options  The arguments structure.
+
+  @return EFI_SUCCESS on success, or an error code.
+**/
+EFI_STATUS
+ParseArguments (
+  MP_SERVICES_TEST_OPTIONS  *Options
+  )
+{
+  EFI_STATUS    Status;
+  UINT32        ArgIndex;
+  CONST CHAR16  *Argument;
+  BOOLEAN       NeedsValue;
+  UINTN         *Value;
+
+  Status = GetArg ();
+  if (EFI_ERROR (Status)) {
+    Print (L"Please use the UEFI Shell to run this application!\n", Status);
+    return Status;
+  }
+
+  if (Argc == 1) {
+    PrintUsage ();
+  }
+
+  ZeroMem (Options, sizeof (MP_SERVICES_TEST_OPTIONS));
+
+  for (ArgIndex = 1; ArgIndex < Argc; ArgIndex++) {
+    Argument   = Argv[ArgIndex];
+    NeedsValue = FALSE;
+
+    if (StrCmp (Argument, L"-A") == 0) {
+      Options->RunAllAPs = TRUE;
+    } else if (StrCmp (Argument, L"-O") == 0) {
+      Options->RunAPsSequentially = TRUE;
+    } else if (StrCmp (Argument, L"-T") == 0) {
+      NeedsValue = TRUE;
+      Value      = &Options->Timeout;
+    } else if (StrCmp (Argument, L"-S") == 0) {
+      Options->RunSingleAP = TRUE;
+      NeedsValue           = TRUE;
+      Value                = &Options->ProcessorIndex;
+    } else if (StrCmp (Argument, L"-P") == 0) {
+      Options->PrintProcessorInformation = TRUE;
+    } else if (StrCmp (Argument, L"-U") == 0) {
+      Options->SetProcessorUnhealthy = TRUE;
+    } else if (StrCmp (Argument, L"-W") == 0) {
+      Options->PrintBspProcessorIndex = TRUE;
+    } else if (StrCmp (Argument, L"-E") == 0) {
+      Options->EnableProcessor = TRUE;
+      NeedsValue               = TRUE;
+      Value                    = &Options->ProcessorIndex;
+    } else if (StrCmp (Argument, L"-D") == 0) {
+      Options->DisableProcessor = TRUE;
+      NeedsValue                = TRUE;
+      Value                     = &Options->ProcessorIndex;
+    } else {
+      PrintUsage ();
+      ZeroMem (Options, sizeof (MP_SERVICES_TEST_OPTIONS));
+      return EFI_SUCCESS;
+    }
+
+    if (NeedsValue) {
+      if ((ArgIndex + 1) < Argc) {
+        Status = StrDecimalToUintnS (Argv[ArgIndex + 1], NULL, Value);
+        if (EFI_ERROR (Status)) {
+          Print (L"Error: option value must be a positive integer.\n");
+          PrintUsage ();
+          return EFI_INVALID_PARAMETER;
+        }
+
+        ArgIndex++;
+      } else {
+        PrintUsage ();
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
-- 
2.30.2


^ permalink raw reply related	[flat|nested] 4+ messages in thread

end of thread, other threads:[~2023-01-04 15:31 UTC | newest]

Thread overview: 4+ messages (download: mbox.gz follow: Atom feed
-- links below jump to the message on this page --
2023-01-04 15:30 [PATCH v3 0/3] ArmPkg,MdeModulePkg: Implement EFI_MP_SERVICES_PROTOCOL for AArch64 and add MpServicesTest application Rebecca Cran
2023-01-04 15:30 ` [PATCH v3 1/3] ArmPkg: Add GET_MPIDR_AFFINITY_BITS and MPIDR_MT_BIT to ArmLib.h Rebecca Cran
2023-01-04 15:30 ` [PATCH v3 2/3] ArmPkg: implement EFI_MP_SERVICES_PROTOCOL based on PSCI calls Rebecca Cran
2023-01-04 15:30 ` [PATCH v3 3/3] MdeModulePkg: Add new Application/MpServicesTest application Rebecca Cran

This is a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox