IntelFsp2WrapperPkg: Update gFspWrapperTokenSpaceGuid to gIntelFsp2WrapperTokenSpaceGuid.

[mirror_edk2.git] / EmulatorPkg / CpuRuntimeDxe / MpService.c

diff --git a/EmulatorPkg/CpuRuntimeDxe/MpService.c b/EmulatorPkg/CpuRuntimeDxe/MpService.c
index d6dd9842aa6e619b403fee223e9cd3146bc8a416..36fa68f3e83bac65dedde4028bb4fd967f3296c7 100644 (file)
--- a/EmulatorPkg/CpuRuntimeDxe/MpService.c
+++ b/EmulatorPkg/CpuRuntimeDxe/MpService.c
@@ -111,8 +111,31 @@ GetNextBlockedNumber (
   return EFI_NOT_FOUND;\r
 }\r
 \r
+/**\r
+ * Calculated and stalled the interval time by BSP to check whether\r
+ * the APs have finished.\r
+ *\r
+ * @param[in]  Timeout    The time limit in microseconds for\r
+ *                        APs to return from Procedure.\r
+ *\r
+ * @retval     StallTime  Time of execution stall.\r
+**/\r
+UINTN\r
+CalculateAndStallInterval (\r
+  IN UINTN                  Timeout\r
+  )\r
+{\r
+  UINTN                 StallTime;\r
 \r
+  if (Timeout < gPollInterval && Timeout != 0) {\r
+    StallTime = Timeout;\r
+  } else {\r
+    StallTime = gPollInterval;\r
+  }\r
+  gBS->Stall (StallTime);\r
 \r
+  return StallTime;\r
+}\r
 \r
 /**\r
   This service retrieves the number of logical processor in the platform\r
@@ -378,7 +401,7 @@ CpuMpServicesStartupAllAps (
   UINTN                 NextNumber;\r
   PROCESSOR_STATE       APInitialState;\r
   PROCESSOR_STATE       ProcessorState;\r
-  INTN                  Timeout;\r
+  UINTN                 Timeout;\r
 \r
 \r
   if (!IsBSP ()) {\r
@@ -397,6 +420,24 @@ CpuMpServicesStartupAllAps (
     return EFI_UNSUPPORTED;\r
   }\r
 \r
+  for (Number = 0; Number < gMPSystem.NumberOfProcessors; Number++) {\r
+    ProcessorData = &gMPSystem.ProcessorData[Number];\r
+    if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {\r
+      // Skip BSP\r
+      continue;\r
+    }\r
+\r
+    if ((ProcessorData->Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {\r
+      // Skip Disabled processors\r
+      continue;\r
+    }\r
+    gThread->MutexLock(ProcessorData->StateLock);\r
+    if (ProcessorData->State != CPU_STATE_IDLE) {\r
+      gThread->MutexUnlock (ProcessorData->StateLock);\r
+      return EFI_NOT_READY;\r
+    }\r
+    gThread->MutexUnlock(ProcessorData->StateLock);\r
+  }\r
 \r
   if (FailedCpuList != NULL) {\r
     gMPSystem.FailedList = AllocatePool ((gMPSystem.NumberOfProcessors + 1) * sizeof (UINTN));\r
@@ -438,17 +479,13 @@ CpuMpServicesStartupAllAps (
     // if not "SingleThread", all APs are put to ready state from the beginning\r
     //\r
     gThread->MutexLock(ProcessorData->StateLock);\r
-    if (ProcessorData->State == CPU_STATE_IDLE) {\r
-      ProcessorData->State = APInitialState;\r
-      gThread->MutexUnlock (ProcessorData->StateLock);\r
+    ASSERT (ProcessorData->State == CPU_STATE_IDLE);\r
+    ProcessorData->State = APInitialState;\r
+    gThread->MutexUnlock (ProcessorData->StateLock);\r
 \r
-      gMPSystem.StartCount++;\r
-      if (SingleThread) {\r
-        APInitialState = CPU_STATE_BLOCKED;\r
-      }\r
-    } else {\r
-      gThread->MutexUnlock (ProcessorData->StateLock);\r
-      return EFI_NOT_READY;\r
+    gMPSystem.StartCount++;\r
+    if (SingleThread) {\r
+      APInitialState = CPU_STATE_BLOCKED;\r
     }\r
   }\r
 \r
@@ -540,13 +577,12 @@ CpuMpServicesStartupAllAps (
       goto Done;\r
     }\r
 \r
-    if ((TimeoutInMicroseconds != 0) && (Timeout < 0)) {\r
+    if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {\r
       Status = EFI_TIMEOUT;\r
       goto Done;\r
     }\r
 \r
-    gBS->Stall (gPollInterval);\r
-    Timeout -= gPollInterval;\r
+    Timeout -= CalculateAndStallInterval (Timeout);\r
   }\r
 \r
 Done:\r
@@ -659,7 +695,7 @@ CpuMpServicesStartupThisAP (
   OUT BOOLEAN                   *Finished               OPTIONAL\r
   )\r
 {\r
-  INTN            Timeout;\r
+  UINTN            Timeout;\r
 \r
   if (!IsBSP ()) {\r
     return EFI_DEVICE_ERROR;\r
@@ -677,6 +713,10 @@ CpuMpServicesStartupThisAP (
     return EFI_INVALID_PARAMETER;\r
   }\r
 \r
+  if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
   gThread->MutexLock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
   if (gMPSystem.ProcessorData[ProcessorNumber].State != CPU_STATE_IDLE) {\r
     gThread->MutexUnlock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
@@ -717,12 +757,11 @@ CpuMpServicesStartupThisAP (
 \r
     gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
 \r
-    if ((TimeoutInMicroseconds != 0) && (Timeout < 0)) {\r
+    if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {\r
       return EFI_TIMEOUT;\r
     }\r
 \r
-    gBS->Stall (gPollInterval);\r
-    Timeout -= gPollInterval;\r
+    Timeout -= CalculateAndStallInterval (Timeout);\r
   }\r
 \r
   return EFI_SUCCESS;\r
@@ -987,7 +1026,7 @@ CpuCheckAllAPsStatus (
   BOOLEAN               Found;\r
 \r
   if (gMPSystem.TimeoutActive) {\r
-    gMPSystem.Timeout -= gPollInterval;\r
+    gMPSystem.Timeout -= CalculateAndStallInterval (gMPSystem.Timeout);\r
   }\r
 \r
   for (ProcessorNumber = 0; ProcessorNumber < gMPSystem.NumberOfProcessors; ProcessorNumber++) {\r
@@ -1040,7 +1079,7 @@ CpuCheckAllAPsStatus (
     }\r
   }\r
 \r
-  if (gMPSystem.TimeoutActive && gMPSystem.Timeout < 0) {\r
+  if (gMPSystem.TimeoutActive && gMPSystem.Timeout == 0) {\r
     //\r
     // Timeout\r
     //\r