[Patch v5 30/48] UefiCpuPkg/MpInitLib: Implementation of MpInitLibSwitchBSP()

[Jeff Fan <jeff.fan@intel.com>]

v4:
  1. Simply the internal function SwitchBSPWorker()'s comment header
     due to it is duplicated with MpInitLibSwitchBSP().

v3:
  1. Rename MpInitLibSwitchBsp to MpInitLibSwitchBSP.

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>
---
 UefiCpuPkg/Library/MpInitLib/DxeMpLib.c |  26 +++++-
 UefiCpuPkg/Library/MpInitLib/MpLib.c    | 144 ++++++++++++++++++++++++++++++--
 UefiCpuPkg/Library/MpInitLib/MpLib.h    |  53 ++++++++++++
 UefiCpuPkg/Library/MpInitLib/PeiMpLib.c |   2 +-
 4 files changed, 218 insertions(+), 7 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index 762d76d..785653b 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -389,7 +389,31 @@ MpInitLibSwitchBSP (
   IN BOOLEAN                   EnableOldBSP
   )
 {
-  return EFI_UNSUPPORTED;
+  EFI_STATUS            Status;
+  BOOLEAN               OldInterruptState;
+
+  //
+  // Before send both BSP and AP to a procedure to exchange their roles,
+  // interrupt must be disabled. This is because during the exchange role
+  // process, 2 CPU may use 1 stack. If interrupt happens, the stack will
+  // be corrupted, since interrupt return address will be pushed to stack
+  // by hardware.
+  //
+  OldInterruptState = SaveAndDisableInterrupts ();
+
+  //
+  // Mask LINT0 & LINT1 for the old BSP
+  //
+  DisableLvtInterrupts ();
+
+  Status = SwitchBSPWorker (ProcessorNumber, EnableOldBSP);
+
+  //
+  // Restore interrupt state.
+  //
+  SetInterruptState (OldInterruptState);
+
+  return Status;
 }
 
 /**
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 5fbcb26..8ae08f4 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -184,6 +184,26 @@ ExtractProcessorLocation (
 }
 
 /**
+  Worker function for SwitchBSP().
+
+  Worker function for SwitchBSP(), assigned to the AP which is intended
+  to become BSP.
+
+  @param[in] Buffer   Pointer to CPU MP Data
+**/
+VOID
+EFIAPI
+FutureBSPProc (
+  IN  VOID            *Buffer
+  )
+{
+  CPU_MP_DATA         *DataInHob;
+
+  DataInHob = (CPU_MP_DATA *) Buffer;
+  AsmExchangeRole (&DataInHob->APInfo, &DataInHob->BSPInfo);
+}
+
+/**
   Get the Application Processors state.
 
   @param[in]  CpuData    The pointer to CPU_AP_DATA of specified AP
@@ -646,11 +666,20 @@ ApWakeupFunction (
           // Invoke AP function here
           //
           Procedure (Parameter);
-          //
-          // Re-get the CPU APICID and Initial APICID
-          //
-          CpuMpData->CpuData[ProcessorNumber].ApicId        = GetApicId ();
-          CpuMpData->CpuData[ProcessorNumber].InitialApicId = GetInitialApicId ();
+          if (CpuMpData->SwitchBspFlag) {
+            //
+            // Re-get the processor number due to BSP/AP maybe exchange in AP function
+            //
+            GetProcessorNumber (CpuMpData, &ProcessorNumber);
+            CpuMpData->CpuData[ProcessorNumber].ApFunction = 0;
+            CpuMpData->CpuData[ProcessorNumber].ApFunctionArgument = 0;
+          } else {
+            //
+            // Re-get the CPU APICID and Initial APICID
+            //
+            CpuMpData->CpuData[ProcessorNumber].ApicId        = GetApicId ();
+            CpuMpData->CpuData[ProcessorNumber].InitialApicId = GetInitialApicId ();
+          }
         }
         SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateFinished);
       }
@@ -941,6 +970,7 @@ MpInitLibInitialize (
   CpuMpData->CpuCount         = 1;
   CpuMpData->BspNumber        = 0;
   CpuMpData->WaitEvent        = NULL;
+  CpuMpData->SwitchBspFlag    = FALSE;
   CpuMpData->CpuData          = (CPU_AP_DATA *) (CpuMpData + 1);
   CpuMpData->CpuInfoInHob     = (UINT64) (UINTN) (CpuMpData->CpuData + MaxLogicalProcessorNumber);
   InitializeSpinLock(&CpuMpData->MpLock);
@@ -1103,6 +1133,110 @@ MpInitLibGetProcessorInfo (
   return EFI_SUCCESS;
 }
 
+/**
+  Worker function to switch the requested AP to be the BSP from that point onward.
+
+  @param[in] ProcessorNumber   The handle number of AP that is to become the new BSP.
+  @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 others               Failed to switch BSP. 
+
+**/
+EFI_STATUS
+SwitchBSPWorker (
+  IN UINTN                     ProcessorNumber,
+  IN BOOLEAN                   EnableOldBSP
+  )
+{
+  CPU_MP_DATA                  *CpuMpData;
+  UINTN                        CallerNumber;
+  CPU_STATE                    State;
+  MSR_IA32_APIC_BASE_REGISTER  ApicBaseMsr;
+
+  CpuMpData = GetCpuMpData ();
+
+  //
+  // Check whether caller processor is BSP
+  //
+  MpInitLibWhoAmI (&CallerNumber);
+  if (CallerNumber != CpuMpData->BspNumber) {
+    return EFI_SUCCESS;
+  }
+
+  if (ProcessorNumber >= CpuMpData->CpuCount) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Check whether specified AP is disabled
+  //
+  State = GetApState (&CpuMpData->CpuData[ProcessorNumber]);
+  if (State == CpuStateDisabled) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether ProcessorNumber specifies the current BSP
+  //
+  if (ProcessorNumber == CpuMpData->BspNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether specified AP is busy
+  //
+  if (State == CpuStateBusy) {
+    return EFI_NOT_READY;
+  }
+
+  CpuMpData->BSPInfo.State = CPU_SWITCH_STATE_IDLE;
+  CpuMpData->APInfo.State  = CPU_SWITCH_STATE_IDLE;
+  CpuMpData->SwitchBspFlag = TRUE;
+
+  //
+  // Clear the BSP bit of MSR_IA32_APIC_BASE
+  //
+  ApicBaseMsr.Uint64 = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  ApicBaseMsr.Bits.BSP = 0;
+  AsmWriteMsr64 (MSR_IA32_APIC_BASE, ApicBaseMsr.Uint64);
+
+  //
+  // Need to wakeUp AP (future BSP).
+  //
+  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, FutureBSPProc, CpuMpData);
+
+  AsmExchangeRole (&CpuMpData->BSPInfo, &CpuMpData->APInfo);
+
+  //
+  // Set the BSP bit of MSR_IA32_APIC_BASE on new BSP
+  //
+  ApicBaseMsr.Uint64 = AsmReadMsr64 (MSR_IA32_APIC_BASE);
+  ApicBaseMsr.Bits.BSP = 1;
+  AsmWriteMsr64 (MSR_IA32_APIC_BASE, ApicBaseMsr.Uint64);
+
+  //
+  // Wait for old BSP finished AP task
+  //
+  while (GetApState (&CpuMpData->CpuData[CallerNumber]) != CpuStateFinished) {
+    CpuPause ();
+  }
+
+  CpuMpData->SwitchBspFlag = FALSE;
+  //
+  // Set old BSP enable state
+  //
+  if (!EnableOldBSP) {
+    SetApState (&CpuMpData->CpuData[CallerNumber], CpuStateDisabled);
+  }
+  //
+  // Save new BSP number
+  //
+  CpuMpData->BspNumber = (UINT32) ProcessorNumber;
+
+  return EFI_SUCCESS;
+}
 
 /**
   This return the handle number for the calling processor.  This service may be
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index 84e0970..bfc7fb7 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -43,6 +43,23 @@
   }
 
 //
+//  The MP data for switch BSP
+//
+#define CPU_SWITCH_STATE_IDLE   0
+#define CPU_SWITCH_STATE_STORED 1
+#define CPU_SWITCH_STATE_LOADED 2
+
+//
+// CPU exchange information for switch BSP
+//
+typedef struct {
+  UINT8             State;        // offset 0
+  UINTN             StackPointer; // offset 4 / 8
+  IA32_DESCRIPTOR   Gdtr;         // offset 8 / 16
+  IA32_DESCRIPTOR   Idtr;         // offset 14 / 26
+} CPU_EXCHANGE_ROLE_INFO;
+
+//
 // AP loop state when APs are in idle state
 // It's value is the same with PcdCpuApLoopMode
 //
@@ -198,6 +215,9 @@ struct _CPU_MP_DATA {
 
   AP_INIT_STATE                  InitFlag;
   BOOLEAN                        X2ApicEnable;
+  BOOLEAN                        SwitchBspFlag;
+  CPU_EXCHANGE_ROLE_INFO         BSPInfo;
+  CPU_EXCHANGE_ROLE_INFO         APInfo;
   MTRR_SETTINGS                  MtrrTable;
   UINT8                          ApLoopMode;
   UINT8                          ApTargetCState;
@@ -243,6 +263,22 @@ AsmGetAddressMap (
   );
 
 /**
+  This function is called by both the BSP and the AP which is to become the BSP to
+  Exchange execution context including stack between them. After return from this
+  function, the BSP becomes AP and the AP becomes the BSP.
+
+  @param[in] MyInfo      Pointer to buffer holding the exchanging information for the executing processor.
+  @param[in] OthersInfo  Pointer to buffer holding the exchanging information for the peer.
+
+**/
+VOID
+EFIAPI
+AsmExchangeRole (
+  IN CPU_EXCHANGE_ROLE_INFO    *MyInfo,
+  IN CPU_EXCHANGE_ROLE_INFO    *OthersInfo
+  );
+
+/**
   Get the pointer to CPU MP Data structure.
 
   @return  The pointer to CPU MP Data structure.
@@ -312,6 +348,23 @@ InitMpGlobalData (
   );
 
 /**
+  Worker function to switch the requested AP to be the BSP from that point onward.
+
+  @param[in] ProcessorNumber   The handle number of AP that is to become the new BSP.
+  @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 others               Failed to switch BSP. 
+
+**/
+EFI_STATUS
+SwitchBSPWorker (
+  IN UINTN                     ProcessorNumber,
+  IN BOOLEAN                   EnableOldBSP
+  );
+
+/**
   Get pointer to CPU MP Data structure from GUIDed HOB.
 
   @return  The pointer to CPU MP Data structure.
diff --git a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
index a7e1cde..cdec010 100644
--- a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
@@ -563,7 +563,7 @@ MpInitLibSwitchBSP (
   IN  BOOLEAN                  EnableOldBSP
   )
 {
-  return EFI_UNSUPPORTED;
+  return SwitchBSPWorker (ProcessorNumber, EnableOldBSP);
 }
 
 /**
-- 
2.7.4.windows.1