[PATCH 2/2] MdeModulePkg: Add new Application/MpServicesTest application

["Rebecca Cran" <quic_rcran@quicinc.com>]

The MpServicesTest application exercises the EFI_MP_SERVICES_PROTOCOL.

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

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

Signed-off-by: Rebecca Cran <rebecca@quicinc.com>
---
 MdeModulePkg/MdeModulePkg.dsc                              |   9 +
 MdeModulePkg/Application/MpServicesTest/MpServicesTest.inf |  41 ++
 MdeModulePkg/Application/MpServicesTest/Options.h          |  39 ++
 MdeModulePkg/Application/MpServicesTest/MpServicesTest.c   | 558 ++++++++++++++++++++
 MdeModulePkg/Application/MpServicesTest/Options.c          | 215 ++++++++
 5 files changed, 862 insertions(+)

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