[RFC] [PATCH 2/2] ArmPkg: Add Library/MpInitLib to support EFI_MP_SERVICES_PROTOCOL


Rebecca Cran
 

Add support for EFI_MP_SERVICES_PROTOCOL during the DXE phase under
AArch64.

Call PSCI_CPU_ON to power on the core, call the supplied procedure and
then call PSCI_CPU_OFF to power off the core.

Signed-off-by: Rebecca Cran <rebecca@nuviainc.com>
---
ArmPkg/ArmPkg.dec | 4 +
ArmPkg/ArmPkg.dsc | 4 +
ArmPkg/Drivers/CpuDxe/AArch64/Arch.c | 22 +
ArmPkg/Drivers/CpuDxe/Arm/Arch.c | 22 +
ArmPkg/Drivers/CpuDxe/CpuDxe.c | 2 +
ArmPkg/Drivers/CpuDxe/CpuDxe.h | 10 +
ArmPkg/Drivers/CpuDxe/CpuDxe.inf | 6 +
ArmPkg/Drivers/CpuDxe/CpuMpInit.c | 604 ++++++++
ArmPkg/Include/Library/MpInitLib.h | 366 +++++
ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S | 61 +
ArmPkg/Library/MpInitLib/DxeMpInitLib.inf | 53 +
ArmPkg/Library/MpInitLib/DxeMpLib.c | 1498 ++++++++++++++++++++
ArmPkg/Library/MpInitLib/InternalMpInitLib.h | 358 +++++
ArmVirtPkg/ArmVirt.dsc.inc | 3 +
14 files changed, 3013 insertions(+)

diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec
index 6ed51edd0340..cd4458de6ec1 100644
--- a/ArmPkg/ArmPkg.dec
+++ b/ArmPkg/ArmPkg.dec
@@ -74,6 +74,10 @@
#
DefaultExceptionHandlerLib|Include/Library/DefaultExceptionHandlerLib.h

+ ## @libraryclass Provides a MP Services interface.
+ #
+ MpInitLib|Include/Library/MpInitLib.h
+
## @libraryclass Provides an interface to query miscellaneous OEM
# information.
#
diff --git a/ArmPkg/ArmPkg.dsc b/ArmPkg/ArmPkg.dsc
index 8abe3713c829..26fb8bb94c07 100644
--- a/ArmPkg/ArmPkg.dsc
+++ b/ArmPkg/ArmPkg.dsc
@@ -99,6 +99,9 @@
PeiServicesLib|MdePkg/Library/PeiServicesLib/PeiServicesLib.inf
PeiServicesTablePointerLib|MdePkg/Library/PeiServicesTablePointerLib/PeiServicesTablePointerLib.inf

+[LibraryClasses.AARCH64]
+ MpInitLib|ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
+
[LibraryClasses.ARM, LibraryClasses.AARCH64]
NULL|ArmPkg/Library/CompilerIntrinsicsLib/CompilerIntrinsicsLib.inf

@@ -163,4 +166,5 @@
ArmPkg/Library/ArmMmuLib/ArmMmuPeiLib.inf

[Components.AARCH64, Components.ARM]
+ ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
ArmPkg/Library/StandaloneMmMmuLib/ArmMmuStandaloneMmLib.inf
diff --git a/ArmPkg/Drivers/CpuDxe/AArch64/Arch.c b/ArmPkg/Drivers/CpuDxe/AArch64/Arch.c
new file mode 100644
index 000000000000..a4b9e9037100
--- /dev/null
+++ b/ArmPkg/Drivers/CpuDxe/AArch64/Arch.c
@@ -0,0 +1,22 @@
+/** @file
+ Architecture specific functions.
+
+ Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+
+ SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+
+#include <CpuDxe.h>
+
+/** Initializes multi-processor support.
+ *
+**/
+VOID
+ArchInitializeMpSupport (
+ VOID
+ )
+{
+ InitializeMpSupport ();
+}
diff --git a/ArmPkg/Drivers/CpuDxe/Arm/Arch.c b/ArmPkg/Drivers/CpuDxe/Arm/Arch.c
new file mode 100644
index 000000000000..0ded264cd06a
--- /dev/null
+++ b/ArmPkg/Drivers/CpuDxe/Arm/Arch.c
@@ -0,0 +1,22 @@
+/** @file
+ Architecture specific functions.
+
+ Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+
+ SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+
+#include <CpuDxe.h>
+
+/** Initializes multi-processor support.
+ *
+**/
+VOID
+ArchInitializeMpSupport (
+ VOID
+ )
+{
+ /* Nothing to do - ARM doesn't support EFI_MP_SERVICES_PROTOCOL */
+}
diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.c b/ArmPkg/Drivers/CpuDxe/CpuDxe.c
index 082ef30fb6c4..db8752f5b54f 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.c
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.c
@@ -279,5 +279,7 @@ CpuDxeInitialize (
);
ASSERT_EFI_ERROR (Status);

+ ArchInitializeMpSupport ();
+
return Status;
}
diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.h b/ArmPkg/Drivers/CpuDxe/CpuDxe.h
index 4cf3ab258c24..6e7ceafa4436 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.h
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.h
@@ -143,4 +143,14 @@ SetGcdMemorySpaceAttributes (
IN UINT64 Attributes
);

+VOID
+InitializeMpSupport (
+ VOID
+ );
+
+VOID
+ArchInitializeMpSupport (
+ VOID
+ );
+
#endif // CPU_DXE_H_
diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf b/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
index e5549fc71df7..f4cdb8ab5613 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
@@ -26,10 +26,13 @@
Exception.c

[Sources.ARM]
+ Arm/Arch.c
Arm/Mmu.c

[Sources.AARCH64]
+ AArch64/Arch.c
AArch64/Mmu.c
+ CpuMpInit.c

[Packages]
ArmPkg/ArmPkg.dec
@@ -37,6 +40,9 @@
MdePkg/MdePkg.dec
MdeModulePkg/MdeModulePkg.dec

+[LibraryClasses.AARCH64]
+ MpInitLib
+
[LibraryClasses]
ArmLib
ArmMmuLib
diff --git a/ArmPkg/Drivers/CpuDxe/CpuMpInit.c b/ArmPkg/Drivers/CpuDxe/CpuMpInit.c
new file mode 100644
index 000000000000..5bf4ed12b711
--- /dev/null
+++ b/ArmPkg/Drivers/CpuDxe/CpuMpInit.c
@@ -0,0 +1,604 @@
+/** @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) 2021, NUVIA Inc. All rights reserved.<BR>
+ SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/MpInitLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+/**
+ 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
+ )
+{
+ return MpInitLibGetNumberOfProcessors (
+ This,
+ NumberOfProcessors,
+ NumberOfEnabledProcessors
+ );
+}
+
+/**
+ 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] ProcessorNumber 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 ProcessorNumber,
+ OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer
+ )
+{
+ return MpInitLibGetProcessorInfo (
+ This,
+ ProcessorNumber,
+ ProcessorInfoBuffer
+ );
+}
+
+/**
+ 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
+ )
+{
+ return MpInitLibStartupAllAPs (
+ This,
+ Procedure,
+ SingleThread,
+ WaitEvent,
+ TimeoutInMicroseconds,
+ ProcedureArgument,
+ FailedCpuList
+ );
+}
+
+/**
+ 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
+ )
+{
+ return MpInitLibStartupThisAP (
+ This,
+ Procedure,
+ ProcessorNumber,
+ WaitEvent,
+ TimeoutInMicroseconds,
+ ProcedureArgument,
+ Finished
+ );
+}
+
+/**
+ 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 MpInitLibSwitchBSP (This, ProcessorNumber, EnableOldBSP);
+}
+
+/**
+ 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
+ )
+{
+ return MpInitLibEnableDisableAP (This, ProcessorNumber, EnableAP, HealthFlag);
+}
+
+/**
+ 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
+ )
+{
+ return MpInitLibWhoAmI (This, ProcessorNumber);
+}
+
+EFI_MP_SERVICES_PROTOCOL mMpServicesTemplate = {
+ GetNumberOfProcessors,
+ GetProcessorInfo,
+ StartupAllAPs,
+ StartupThisAP,
+ SwitchBSP,
+ EnableDisableAP,
+ WhoAmI
+};
+
+/** Initialize multi-processor support.
+
+**/
+VOID
+InitializeMpSupport (
+ VOID
+ )
+{
+ EFI_STATUS Status;
+ EFI_HANDLE Handle;
+ UINTN MaxCpus;
+ EFI_HOB_GENERIC_HEADER *Hob;
+ VOID *HobData;
+ UINTN HobDataSize;
+ ARM_PROCESSOR_TABLE CpuInfo;
+
+ DEBUG ((DEBUG_INFO, "Starting MP services"));
+
+ Hob = GetFirstGuidHob (&gArmMpCoreInfoGuid);
+ if (Hob != NULL) {
+ HobData = GET_GUID_HOB_DATA (Hob);
+ HobDataSize = GET_GUID_HOB_DATA_SIZE (Hob);
+ CpuInfo.ArmCpus = (ARM_CORE_INFO *)HobData;
+ CpuInfo.NumberOfEntries = HobDataSize / sizeof (ARM_CORE_INFO);
+ MaxCpus = CpuInfo.NumberOfEntries;
+ }
+
+ 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;
+ }
+
+ MpInitLibInitialize (MaxCpus, &CpuInfo);
+
+ //
+ // Now install the MP services protocol.
+ //
+ Handle = NULL;
+ Status = gBS->InstallMultipleProtocolInterfaces (
+ &Handle,
+ &gEfiMpServiceProtocolGuid,
+ &mMpServicesTemplate,
+ NULL
+ );
+ ASSERT_EFI_ERROR (Status);
+}
diff --git a/ArmPkg/Include/Library/MpInitLib.h b/ArmPkg/Include/Library/MpInitLib.h
new file mode 100644
index 000000000000..a4b80c18a9e8
--- /dev/null
+++ b/ArmPkg/Include/Library/MpInitLib.h
@@ -0,0 +1,366 @@
+/** @file
+
+Copyright (c) 2021, NUVIA 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_INITLIB_H_
+#define MP_INITLIB_H_
+
+#include <Protocol/Cpu.h>
+#include <Protocol/MpService.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Guid/ArmMpCoreInfo.h>
+
+
+/**
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetNumberOfProcessors (
+ IN EFI_MP_SERVICES_PROTOCOL *This,
+ OUT UINTN *NumberOfProcessors,
+ OUT UINTN *NumberOfEnabledProcessors
+ );
+
+/**
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetProcessorInfo (
+ IN EFI_MP_SERVICES_PROTOCOL *This,
+ IN UINTN ProcessorIndex,
+ OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer
+ );
+
+
+/**
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupAllAPs (
+ 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
+ );
+
+/**
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupThisAP (
+ 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
+ );
+
+/**
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibSwitchBSP (
+ IN EFI_MP_SERVICES_PROTOCOL *This,
+ IN UINTN ProcessorNumber,
+ IN BOOLEAN EnableOldBSP
+ );
+
+/**
+ This service lets the caller enable or disable an AP from this point onward.
+ This service may only be called from the BSP.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibEnableDisableAP (
+ IN EFI_MP_SERVICES_PROTOCOL *This,
+ IN UINTN ProcessorNumber,
+ IN BOOLEAN EnableAP,
+ IN UINT32 *HealthFlag OPTIONAL
+ );
+
+/**
+ This return the handle number for the calling processor. This service may be
+ called from the BSP and APs.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibWhoAmI (
+ IN EFI_MP_SERVICES_PROTOCOL *This,
+ OUT UINTN *ProcessorNumber
+ );
+
+/** Initializes the MP Services system data
+
+ @param NumberOfProcessors The number of processors, both BSP and AP.
+ @param CpuInfo CPU information gathered earlier during boot.
+
+**/
+VOID
+MpInitLibInitialize (
+ IN UINTN NumberOfProcessors,
+ IN ARM_PROCESSOR_TABLE *CpuInfo
+ );
+
+
+
+#endif /* MP_INITLIB_H_ */
diff --git a/ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S b/ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S
new file mode 100644
index 000000000000..287db060e594
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S
@@ -0,0 +1,61 @@
+#===============================================================================
+# Copyright (c) 2021 NUVIA Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-2-Clause-Patent
+#===============================================================================
+
+.text
+.align 3
+
+#include <AsmMacroIoLibV8.h>
+#include <IndustryStandard/ArmStdSmc.h>
+
+#include "InternalMpInitLib.h"
+
+GCC_ASM_IMPORT (gApStacksBase)
+GCC_ASM_IMPORT (gProcessorIDs)
+GCC_ASM_IMPORT (ApProcedure)
+GCC_ASM_IMPORT (gApStackSize)
+
+GCC_ASM_EXPORT (ApEntryPoint)
+
+StartupAddr: .8byte ASM_PFX(ApProcedure)
+
+// Entry-point for the AP
+// VOID
+// ApEntryPoint (
+// VOID
+// );
+ASM_PFX(ApEntryPoint):
+ mrs x0, mpidr_el1
+ ldr x1, gProcessorIDs
+ mov x2, 0 // x2 = processor index
+ mov x3, 0 // x3 = address offset
+
+// Find index in gProcessorIDs for current processor
+1:
+ ldr x4, [x1, x3] // x4 = gProcessorIDs + x3
+ cmp x4, 0 // check if we've reached the end of gProcessorIDs
+ beq ProcessorNotFound
+ add x3, x3, 8 // x3 += sizeof (*gProcessorIDs)
+ add x2, x2, 1 // x2++
+ cmp x0, x4 // if mpidr_el1 != *(gProcessorIDs + x3) then loop
+ bne 1b
+ sub x2, x2, 1
+
+// Calculate stack address
+ // x2 contains the index for the current processor
+ ldr x0, gApStacksBase
+ ldr x1, gApStackSize
+ mul x3, x2, x1 // x3 = ProcessorIndex * gApStackSize
+ add x4, x0, x3 // x4 = gApStacksBase + x3
+ add sp, x4, x1 // sp = x4 + gApStackSize
+
+ ldr x0, StartupAddr // ASM_PFX(ApProcedure)
+ blr x0 // doesn't return
+
+ProcessorNotFound:
+// Turn off the processor
+ MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF)
+ smc #0
+ b .
diff --git a/ArmPkg/Library/MpInitLib/DxeMpInitLib.inf b/ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
new file mode 100644
index 000000000000..2275b6cca33a
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
@@ -0,0 +1,53 @@
+#/** @file
+#
+# Component description file for the DxeMpInitLib module.
+#
+# Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+#
+# SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+#**/
+
+[Defines]
+ INF_VERSION = 1.29
+ BASE_NAME = DxeMpInitLib
+ FILE_GUID = c9ca773c-8ae4-4b74-82fd-f7345503294e
+ MODULE_TYPE = DXE_DRIVER
+ VERSION_STRING = 1.0
+ LIBRARY_CLASS = MpInitLib|DXE_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+# VALID_ARCHITECTURES = AARCH64
+#
+
+[Sources.AARCH64]
+ AArch64/MpFuncs.S
+
+[Sources]
+ DxeMpLib.c
+ InternalMpInitLib.h
+
+[Packages]
+ ArmPkg/ArmPkg.dec
+ MdeModulePkg/MdeModulePkg.dec
+ MdePkg/MdePkg.dec
+
+[LibraryClasses]
+ ArmLib
+ ArmSmcLib
+ BaseLib
+ BaseMemoryLib
+ DebugLib
+ HobLib
+ MemoryAllocationLib
+ UefiBootServicesTableLib
+ UefiDriverEntryPoint
+ UefiLib
+
+[Protocols]
+ gEfiMpServiceProtocolGuid
+
+[Guids]
+ gArmMpCoreInfoGuid
diff --git a/ArmPkg/Library/MpInitLib/DxeMpLib.c b/ArmPkg/Library/MpInitLib/DxeMpLib.c
new file mode 100644
index 000000000000..6053db740624
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/DxeMpLib.c
@@ -0,0 +1,1498 @@
+/** @file
+ Construct MP Services Protocol.
+
+ Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+ Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR>
+ Portions Copyright (c) 2011, Apple Inc. All rights reserved.
+
+ SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <Ppi/ArmMpCoreInfo.h>
+#include <Library/ArmLib.h>
+#include <Library/ArmSmcLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/MpInitLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <IndustryStandard/ArmStdSmc.h>
+
+#include "InternalMpInitLib.h"
+
+#define POLL_INTERVAL_US 50000
+
+STATIC CPU_MP_DATA mCpuMpData;
+STATIC BOOLEAN mNonBlockingModeAllowed;
+UINT64 *gApStacksBase;
+UINT64 *gProcessorIDs;
+CONST UINT64 gApStackSize = AP_STACK_SIZE;
+
+/** 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 ApProcedure;
+ VOID *ApParameter;
+ UINTN ProcessorIndex;
+
+ MpInitLibWhoAmI (&mMpServicesTemplate, &ProcessorIndex);
+
+ /* Fetch the user-supplied procedure and parameter to execute */
+ ApProcedure = mCpuMpData.CpuData[ProcessorIndex].Procedure;
+ ApParameter = mCpuMpData.CpuData[ProcessorIndex].Parameter;
+
+ ApProcedure (ApParameter);
+
+ mCpuMpData.CpuData[ProcessorIndex].State = CpuStateFinished;
+
+ /* 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 ();
+}
+
+/** 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;
+
+ ArmCallSmc (&Args);
+
+ if (Args.Arg0 != ARM_SMC_PSCI_RET_SUCCESS) {
+ DEBUG ((DEBUG_ERROR, "PSCI_CPU_ON call failed: %d", 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;
+}
+
+/** 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 = MpInitLibWhoAmI (&mMpServicesTemplate, &ProcessorIndex);
+ ASSERT_EFI_ERROR (Status);
+ if (EFI_ERROR (Status)) {
+ return FALSE;
+ }
+
+ return IsProcessorBSP (ProcessorIndex);
+}
+
+/** 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetNumberOfProcessors (
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetProcessorInfo (
+ 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;
+}
+
+/** 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;
+}
+
+/** Returns whether all processors are in the idle state.
+
+ @return Whether all the processors are idle.
+
+**/
+STATIC
+BOOLEAN
+CheckAllCpusReady (
+ VOID
+ )
+{
+ 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) != CpuStateIdle) {
+ return FALSE;
+ }
+ }
+
+ return TRUE;
+}
+
+/** 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;
+ }
+
+ ASSERT (GetApState (CpuData) == CpuStateIdle);
+ 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.
+
+ @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsWithWaitEvent (
+ IN EFI_AP_PROCEDURE Procedure,
+ IN VOID *ProcedureArgument,
+ IN EFI_EVENT WaitEvent,
+ IN UINTN TimeoutInMicroseconds
+ )
+{
+ 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);
+ }
+ }
+
+ //
+ // 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.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;
+
+ Timeout = TimeoutInMicroseconds;
+
+ 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)) {
+ CpuData->State = CpuStateIdle;
+ Status = EFI_NOT_READY;
+ goto Done;
+ }
+
+ 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;
+ goto Done;
+ }
+
+ if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
+ Status = EFI_TIMEOUT;
+ goto Done;
+ }
+
+ Timeout -= CalculateAndStallInterval (Timeout);
+ }
+
+Done:
+ if (FailedCpuList != NULL) {
+ if (mCpuMpData.FailedListIndex == 0) {
+ FreePool (*FailedCpuList);
+ *FailedCpuList = NULL;
+ }
+ }
+
+ return Status;
+}
+
+/**
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupAllAPs (
+ 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 (!CheckAllCpusReady ()) {
+ return EFI_NOT_READY;
+ }
+
+ if (FailedCpuList != NULL) {
+ mCpuMpData.FailedList = AllocatePool (
+ (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
+ );
+ } else {
+ Status = StartupAllAPsNoWaitEvent (
+ Procedure,
+ ProcedureArgument,
+ TimeoutInMicroseconds,
+ SingleThread,
+ FailedCpuList
+ );
+ }
+
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupThisAP (
+ 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.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
+ mCpuMpData.WaitEvent = WaitEvent;
+ 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.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibSwitchBSP (
+ IN EFI_MP_SERVICES_PROTOCOL *This,
+ IN UINTN ProcessorNumber,
+ IN BOOLEAN EnableOldBSP
+ )
+{
+ UINTN Index;
+ CPU_AP_DATA *CpuData;
+
+ CpuData = &mCpuMpData.CpuData[ProcessorNumber];
+
+ if (!IsCurrentProcessorBSP ()) {
+ return EFI_DEVICE_ERROR;
+ }
+
+ if (ProcessorNumber >= mCpuMpData.NumberOfProcessors) {
+ return EFI_NOT_FOUND;
+ }
+
+ if (!IsProcessorEnabled (ProcessorNumber)) {
+ return EFI_INVALID_PARAMETER;
+ }
+
+ if (IsProcessorBSP (ProcessorNumber)) {
+ return EFI_INVALID_PARAMETER;
+ }
+
+ for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+ if (IsProcessorBSP (Index)) {
+ break;
+ }
+ }
+
+ ASSERT (Index != mCpuMpData.NumberOfProcessors);
+
+ if (GetApState (CpuData) != CpuStateIdle) {
+ return EFI_NOT_READY;
+ }
+
+ // Skip for now as we need switch a bunch of stack stuff around and it's
+ // complex. May not be worth it?
+ return EFI_NOT_READY;
+}
+
+/**
+ This service lets the caller enable or disable an AP from this point onward.
+ This service may only be called from the BSP.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibEnableDisableAP (
+ 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);
+ }
+
+ return EFI_SUCCESS;
+}
+
+/**
+ This return the handle number for the calling processor. This service may be
+ called from the BSP and APs.
+
+ @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.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibWhoAmI (
+ IN EFI_MP_SERVICES_PROTOCOL *This,
+ OUT UINTN *ProcessorNumber
+ )
+{
+ UINTN Index;
+ UINT64 ProcessorId;
+ CPU_AP_DATA *CpuData;
+
+ if (ProcessorNumber == NULL) {
+ return EFI_INVALID_PARAMETER;
+ }
+
+ ProcessorId = ArmReadMpidr ();
+ for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+ CpuData = &mCpuMpData.CpuData[Index];
+ if (CpuData->Info.ProcessorId == ProcessorId) {
+ break;
+ }
+ }
+
+ *ProcessorNumber = Index;
+ return EFI_SUCCESS;
+}
+
+/** 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 (ApState == CpuStateIdle) {
+ return;
+ }
+
+ // If we are retrying make sure we don't double count
+ for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+ if (mCpuMpData.FailedList[Index] == END_OF_CPU_LIST) {
+ break;
+ }
+
+ if (mCpuMpData.FailedList[ProcessorIndex] == Index) {
+ Found = TRUE;
+ break;
+ }
+ }
+
+ /* If the CPU isn't already in the FailedList, add it */
+ if (!Found) {
+ mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] = Index;
+ }
+}
+
+/** 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;
+ }
+
+ 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
+ );
+ }
+ }
+
+ 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;
+
+ if (mCpuMpData.TimeoutActive) {
+ mCpuMpData.Timeout -= CalculateAndStallInterval (mCpuMpData.Timeout);
+ }
+
+ for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+ UpdateApStatus (Index);
+ }
+
+ if (mCpuMpData.TimeoutActive && mCpuMpData.Timeout == 0) {
+ 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) {
+ FreePool (mCpuMpData.FailedList);
+ mCpuMpData.FailedList = NULL;
+ }
+ }
+
+ 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;
+ CpuData->TimeTaken += POLL_INTERVAL_US;
+
+ State = GetApState (CpuData);
+
+ if (State == CpuStateFinished) {
+ Status = gBS->SetTimer (CpuData->CheckThisAPEvent, TimerCancel, 0);
+ ASSERT_EFI_ERROR (Status);
+
+ if (mCpuMpData.WaitEvent != NULL) {
+ Status = gBS->SignalEvent (mCpuMpData.WaitEvent);
+ ASSERT_EFI_ERROR (Status);
+ }
+
+ CpuData->State = CpuStateIdle;
+ }
+
+ if (CpuData->TimeTaken > CpuData->Timeout) {
+ 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.
+ @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;
+
+ /* The MPIDR passed in may be a pseudo-MPIDR that's missing the bit 31 RES1.
+ * Fix it so it's a proper MPIDR.
+ */
+ CpuInfo->ProcessorId = BIT31 | Mpidr;
+ CpuInfo->StatusFlag = PROCESSOR_ENABLED_BIT | PROCESSOR_HEALTH_STATUS_BIT;
+
+ if (BSP) {
+ CpuInfo->StatusFlag |= PROCESSOR_AS_BSP_BIT;
+ }
+
+ CpuInfo->Location.Package = GET_CLUSTER_ID (Mpidr);
+ CpuInfo->Location.Core = GET_CORE_ID (Mpidr);
+ CpuInfo->Location.Thread = 0;
+
+ CpuInfo->ExtendedInformation.Location2.Package = 0;
+ CpuInfo->ExtendedInformation.Location2.Module = 0;
+ CpuInfo->ExtendedInformation.Location2.Tile = 0;
+ CpuInfo->ExtendedInformation.Location2.Die = GET_CLUSTER_ID (Mpidr);
+ CpuInfo->ExtendedInformation.Location2.Core = GET_CORE_ID (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 CpuInfo CPU information gathered earlier during boot.
+
+**/
+VOID
+MpInitLibInitialize (
+ IN UINTN NumberOfProcessors,
+ IN ARM_PROCESSOR_TABLE *CpuInfo
+ )
+{
+ EFI_STATUS Status;
+ UINTN Index;
+ EFI_EVENT ReadyToBootEvent;
+
+ //
+ // 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)
+ );
+ ASSERT (mCpuMpData.CpuData != NULL);
+
+ /* Allocate one extra for the NULL entry at the end */
+ gProcessorIDs = AllocatePool ((mCpuMpData.NumberOfProcessors + 1) * sizeof (UINT64));
+ ASSERT (gProcessorIDs != NULL);
+
+ FillInProcessorInformation (TRUE, CpuInfo->ArmCpus[0].Mpidr, 0);
+ gProcessorIDs[0] = mCpuMpData.CpuData[0].Info.ProcessorId;
+
+ Status = gBS->CreateEvent (
+ EVT_TIMER | EVT_NOTIFY_SIGNAL,
+ TPL_CALLBACK,
+ CheckAllAPsStatus,
+ NULL,
+ &mCpuMpData.CheckAllAPsEvent
+ );
+ ASSERT_EFI_ERROR (Status);
+
+ gApStacksBase = AllocatePool (
+ mCpuMpData.NumberOfProcessors *
+ gApStackSize
+ );
+ ASSERT (gApStacksBase != NULL);
+
+ for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+ if (IsProcessorBSP (Index)) {
+ /* Skip BSP */
+ continue;
+ }
+
+ FillInProcessorInformation (FALSE, CpuInfo->ArmCpus[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 (Status == EFI_SUCCESS);
+ }
+
+ Status = EfiCreateEventReadyToBootEx (
+ TPL_CALLBACK,
+ ReadyToBootSignaled,
+ NULL,
+ &ReadyToBootEvent
+ );
+ ASSERT_EFI_ERROR (Status);
+
+ gProcessorIDs[Index] = 0;
+}
+
+/**
+ 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;
+}
+
diff --git a/ArmPkg/Library/MpInitLib/InternalMpInitLib.h b/ArmPkg/Library/MpInitLib/InternalMpInitLib.h
new file mode 100644
index 000000000000..d08bb76246d7
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/InternalMpInitLib.h
@@ -0,0 +1,358 @@
+/** @file
+
+Copyright (c) 2021, NUVIA 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_INTERNAL_INIT_LIB_H_
+#define MP_INTERNAL_INIT_LIB_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 process a procedure are:
+// Idle ----> Ready ----> Busy ----> Idle
+// [BSP] [AP] [AP]
+//
+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;
+ UINTN Timeout;
+ UINTN TimeTaken;
+} 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 *FailedList;
+ UINTN FailedListIndex;
+ BOOLEAN TimeoutActive;
+} CPU_MP_DATA;
+
+EFI_STATUS
+EFIAPI
+CpuMpServicesInit (
+ IN EFI_HANDLE ImageHandle,
+ IN EFI_SYSTEM_TABLE *SystemTable
+ );
+
+extern EFI_MP_SERVICES_PROTOCOL mMpServicesTemplate;
+
+/** 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
+ );
+
+/** Returns whether all processors are in the idle state.
+
+ @return Whether all the processors are idle.
+
+**/
+STATIC
+BOOLEAN
+CheckAllCpusReady (
+ VOID
+ );
+
+/** 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.
+
+ @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsWithWaitEvent (
+ IN EFI_AP_PROCEDURE Procedure,
+ IN VOID *ProcedureArgument,
+ IN EFI_EVENT WaitEvent,
+ IN UINTN TimeoutInMicroseconds
+ );
+
+/** 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.
+ @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_INTERNAL_INIT_LIB_H_ */
diff --git a/ArmVirtPkg/ArmVirt.dsc.inc b/ArmVirtPkg/ArmVirt.dsc.inc
index 5a1598d90ca7..af6e48fd6cfc 100644
--- a/ArmVirtPkg/ArmVirt.dsc.inc
+++ b/ArmVirtPkg/ArmVirt.dsc.inc
@@ -261,6 +261,9 @@
[LibraryClasses.ARM]
ArmSoftFloatLib|ArmPkg/Library/ArmSoftFloatLib/ArmSoftFloatLib.inf

+[LibraryClasses.AARCH64]
+ MpInitLib|ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
+
[BuildOptions]
RVCT:RELEASE_*_*_CC_FLAGS = -DMDEPKG_NDEBUG

--
2.31.1