From e37109bcc75b3242fccb223979f954c162ee103f Mon Sep 17 00:00:00 2001
From: Jeff Fan <jeff.fan@intel.com>
Date: Thu, 21 Jul 2016 21:23:05 +0800
Subject: [PATCH] UefiCpuPkg/MpInitLib: Implementation of
 MpInitLibEnableDisableAP()

v4:
  1. Simply the internal function MpInitLibEnableDisableAP()'s function
     header due to it is duplicated with MpInitLibEnableDisableAP().
v3:
  1. Use CamelCase for mCheckAllAPsEvent, mStopCheckAllApsStatus.

Cc: Michael Kinney <michael.d.kinney@intel.com>
Cc: Feng Tian <feng.tian@intel.com>
Cc: Giri P Mudusuru <giri.p.mudusuru@intel.com>
Cc: Laszlo Ersek <lersek@redhat.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
Reviewed-by: Michael Kinney <michael.d.kinney@intel.com>
Tested-by: Laszlo Ersek <lersek@redhat.com>
Tested-by: Michael Kinney <michael.d.kinney@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/DxeMpLib.c | 20 ++++++++-
 UefiCpuPkg/Library/MpInitLib/MpLib.c    | 56 +++++++++++++++++++++++++
 UefiCpuPkg/Library/MpInitLib/MpLib.h    | 21 ++++++++++
 UefiCpuPkg/Library/MpInitLib/PeiMpLib.c |  2 +-
 4 files changed, 97 insertions(+), 2 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index 785653be8f..988920fc07 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -454,5 +454,23 @@ MpInitLibEnableDisableAP (
   IN  UINT32                    *HealthFlag OPTIONAL
   )
 {
-  return EFI_UNSUPPORTED;
+  EFI_STATUS     Status;
+  BOOLEAN        TempStopCheckState;
+
+  TempStopCheckState = FALSE;
+  //
+  // temporarily stop checkAllAPsStatus for initialize parameters.
+  //
+  if (!mStopCheckAllApsStatus) {
+    mStopCheckAllApsStatus = TRUE;
+    TempStopCheckState     = TRUE;
+  }
+
+  Status = EnableDisableApWorker (ProcessorNumber, EnableAP, HealthFlag);
+
+  if (TempStopCheckState) {
+    mStopCheckAllApsStatus = FALSE;
+  }
+
+  return Status;
 }
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 8ae08f4d5d..3a266e9607 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -1238,6 +1238,62 @@ SwitchBSPWorker (
   return EFI_SUCCESS;
 }
 
+/**
+  Worker function to let the caller enable or disable an AP from this point onward.
+  This service may only be called from the BSP.
+
+  @param[in] ProcessorNumber   The handle number of AP.
+  @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.
+
+  @retval EFI_SUCCESS          The specified AP was enabled or disabled successfully.
+  @retval others               Failed to Enable/Disable AP.
+
+**/
+EFI_STATUS
+EnableDisableApWorker (
+  IN  UINTN                     ProcessorNumber,
+  IN  BOOLEAN                   EnableAP,
+  IN  UINT32                    *HealthFlag OPTIONAL
+  )
+{
+  CPU_MP_DATA               *CpuMpData;
+  UINTN                     CallerNumber;
+
+  CpuMpData = GetCpuMpData ();
+
+  //
+  // Check whether caller processor is BSP
+  //
+  MpInitLibWhoAmI (&CallerNumber);
+  if (CallerNumber != CpuMpData->BspNumber) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (ProcessorNumber == CpuMpData->BspNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (ProcessorNumber >= CpuMpData->CpuCount) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (!EnableAP) {
+    SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateDisabled);
+  } else {
+    SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateIdle);
+  }
+
+  if (HealthFlag != NULL) {
+    CpuMpData->CpuData[ProcessorNumber].CpuHealthy =
+          (BOOLEAN) ((*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT) != 0);
+  }
+
+  return EFI_SUCCESS;
+}
+
 /**
   This return the handle number for the calling processor.  This service may be
   called from the BSP and APs.
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index bfc7fb72e0..f8b172f414 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -364,6 +364,27 @@ SwitchBSPWorker (
   IN BOOLEAN                   EnableOldBSP
   );
 
+/**
+  Worker function to let the caller enable or disable an AP from this point onward.
+  This service may only be called from the BSP.
+
+  @param[in] ProcessorNumber   The handle number of AP.
+  @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.
+
+  @retval EFI_SUCCESS          The specified AP was enabled or disabled successfully.
+  @retval others               Failed to Enable/Disable AP.
+
+**/
+EFI_STATUS
+EnableDisableApWorker (
+  IN  UINTN                     ProcessorNumber,
+  IN  BOOLEAN                   EnableAP,
+  IN  UINT32                    *HealthFlag OPTIONAL
+  );
+
 /**
   Get pointer to CPU MP Data structure from GUIDed HOB.
 
diff --git a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
index cdec0108c5..fe585bdce8 100644
--- a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
@@ -604,7 +604,7 @@ MpInitLibEnableDisableAP (
   IN  UINT32                    *HealthFlag OPTIONAL
   )
 {
-  return EFI_UNSUPPORTED;
+  return EnableDisableApWorker (ProcessorNumber, EnableAP, HealthFlag);
 }
 
 
-- 
2.39.2