UefiCpuPkg/MpInitLib: Implementation of MpInitLibEnableDisableAP()

[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / DxeMpLib.c

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index 2d8bf25b0add3b53c622221629471f4a8c080a12..988920fc0701449892c6a24c85981cae83d650e0 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -52,6 +52,65 @@ SaveCpuMpData (
 }\r
 \r
 /**\r
+  Allocate reset vector buffer.\r
+\r
+  @param[in, out]  CpuMpData  The pointer to CPU MP Data structure.\r
+**/\r
+VOID\r
+AllocateResetVector (\r
+  IN OUT CPU_MP_DATA          *CpuMpData\r
+  )\r
+{\r
+  EFI_STATUS            Status;\r
+  UINTN                 ApResetVectorSize;\r
+  EFI_PHYSICAL_ADDRESS  StartAddress;\r
+\r
+  ApResetVectorSize = CpuMpData->AddressMap.RendezvousFunnelSize +\r
+                      sizeof (MP_CPU_EXCHANGE_INFO);\r
+\r
+  StartAddress = BASE_1MB;\r
+  Status = gBS->AllocatePages (\r
+                  AllocateMaxAddress,\r
+                  EfiACPIMemoryNVS,\r
+                  EFI_SIZE_TO_PAGES (ApResetVectorSize),\r
+                  &StartAddress\r
+                  );\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  CpuMpData->WakeupBuffer      = (UINTN) StartAddress;\r
+  CpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN)\r
+                  (CpuMpData->WakeupBuffer + CpuMpData->AddressMap.RendezvousFunnelSize);\r
+  //\r
+  // copy AP reset code in it\r
+  //\r
+  CopyMem (\r
+    (VOID *) CpuMpData->WakeupBuffer,\r
+    (VOID *) CpuMpData->AddressMap.RendezvousFunnelAddress,\r
+    CpuMpData->AddressMap.RendezvousFunnelSize\r
+    );\r
+}\r
+\r
+/**\r
+  Free AP reset vector buffer.\r
+\r
+  @param[in]  CpuMpData  The pointer to CPU MP Data structure.\r
+**/\r
+VOID\r
+FreeResetVector (\r
+  IN CPU_MP_DATA              *CpuMpData\r
+  )\r
+{\r
+  EFI_STATUS            Status;\r
+  UINTN                 ApResetVectorSize;\r
+  ApResetVectorSize = CpuMpData->AddressMap.RendezvousFunnelSize +\r
+                      sizeof (MP_CPU_EXCHANGE_INFO);\r
+  Status = gBS->FreePages(\r
+             (EFI_PHYSICAL_ADDRESS)CpuMpData->WakeupBuffer,\r
+             EFI_SIZE_TO_PAGES (ApResetVectorSize)\r
+             );\r
+  ASSERT_EFI_ERROR (Status);\r
+}\r
+\r
 /**\r
   Checks APs status and updates APs status if needed.\r
 \r
@@ -88,6 +147,7 @@ CheckApsStatus (
     CheckAndUpdateApsStatus ();\r
   }\r
 }\r
+\r
 /**\r
   Initialize global data for MP support.\r
 \r
@@ -329,7 +389,31 @@ MpInitLibSwitchBSP (
   IN BOOLEAN                   EnableOldBSP\r
   )\r
 {\r
-  return EFI_UNSUPPORTED;\r
+  EFI_STATUS            Status;\r
+  BOOLEAN               OldInterruptState;\r
+\r
+  //\r
+  // Before send both BSP and AP to a procedure to exchange their roles,\r
+  // interrupt must be disabled. This is because during the exchange role\r
+  // process, 2 CPU may use 1 stack. If interrupt happens, the stack will\r
+  // be corrupted, since interrupt return address will be pushed to stack\r
+  // by hardware.\r
+  //\r
+  OldInterruptState = SaveAndDisableInterrupts ();\r
+\r
+  //\r
+  // Mask LINT0 & LINT1 for the old BSP\r
+  //\r
+  DisableLvtInterrupts ();\r
+\r
+  Status = SwitchBSPWorker (ProcessorNumber, EnableOldBSP);\r
+\r
+  //\r
+  // Restore interrupt state.\r
+  //\r
+  SetInterruptState (OldInterruptState);\r
+\r
+  return Status;\r
 }\r
 \r
 /**\r
@@ -370,5 +454,23 @@ MpInitLibEnableDisableAP (
   IN  UINT32                    *HealthFlag OPTIONAL\r
   )\r
 {\r
-  return EFI_UNSUPPORTED;\r
+  EFI_STATUS     Status;\r
+  BOOLEAN        TempStopCheckState;\r
+\r
+  TempStopCheckState = FALSE;\r
+  //\r
+  // temporarily stop checkAllAPsStatus for initialize parameters.\r
+  //\r
+  if (!mStopCheckAllApsStatus) {\r
+    mStopCheckAllApsStatus = TRUE;\r
+    TempStopCheckState     = TRUE;\r
+  }\r
+\r
+  Status = EnableDisableApWorker (ProcessorNumber, EnableAP, HealthFlag);\r
+\r
+  if (TempStopCheckState) {\r
+    mStopCheckAllApsStatus = FALSE;\r
+  }\r
+\r
+  return Status;\r
 }\r