EmulatorPkg/MpService: Check Processor State when Set Procedure

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

diff --git a/EmulatorPkg/CpuRuntimeDxe/MpService.c b/EmulatorPkg/CpuRuntimeDxe/MpService.c
index 238ecc6113150d853e58e0cf1e018f61f7d48160..5ddd4e761ec6cae6aaac9caaf8ee3b0449d3826f 100644 (file)
--- a/EmulatorPkg/CpuRuntimeDxe/MpService.c
+++ b/EmulatorPkg/CpuRuntimeDxe/MpService.c
@@ -437,17 +437,17 @@ CpuMpServicesStartupAllAps (
     // state 1 by 1, until the previous 1 finished its task\r
     // 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
-      gMPSystem.StartCount++;\r
-\r
-      gThread->MutexLock (&ProcessorData->StateLock);\r
       ProcessorData->State = APInitialState;\r
-      gThread->MutexUnlock (&ProcessorData->StateLock);\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
     }\r
   }\r
@@ -465,7 +465,13 @@ CpuMpServicesStartupAllAps (
         continue;\r
       }\r
 \r
-      SetApProcedure (ProcessorData, Procedure, ProcedureArgument);\r
+      gThread->MutexLock (ProcessorData->StateLock);\r
+      ProcessorState = ProcessorData->State;\r
+      gThread->MutexUnlock (ProcessorData->StateLock);\r
+\r
+      if (ProcessorState == CPU_STATE_READY) {\r
+        SetApProcedure (ProcessorData, Procedure, ProcedureArgument);\r
+      }\r
     }\r
 \r
     //\r
@@ -512,9 +518,9 @@ CpuMpServicesStartupAllAps (
         if (SingleThread) {\r
           Status = GetNextBlockedNumber (&NextNumber);\r
           if (!EFI_ERROR (Status)) {\r
-            gThread->MutexLock (&gMPSystem.ProcessorData[NextNumber].StateLock);\r
+            gThread->MutexLock (gMPSystem.ProcessorData[NextNumber].StateLock);\r
             gMPSystem.ProcessorData[NextNumber].State = CPU_STATE_READY;\r
-            gThread->MutexUnlock (&gMPSystem.ProcessorData[NextNumber].StateLock);\r
+            gThread->MutexUnlock (gMPSystem.ProcessorData[NextNumber].StateLock);\r
           }\r
         }\r
 \r
@@ -671,9 +677,12 @@ CpuMpServicesStartupThisAP (
     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
     return EFI_NOT_READY;\r
   }\r
+  gThread->MutexUnlock(gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
 \r
   if ((WaitEvent != NULL)  && gReadToBoot) {\r
     return EFI_UNSUPPORTED;\r
@@ -699,14 +708,14 @@ CpuMpServicesStartupThisAP (
 \r
   // Blocking\r
   while (TRUE) {\r
-    gThread->MutexLock (&gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+    gThread->MutexLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
     if (gMPSystem.ProcessorData[ProcessorNumber].State == CPU_STATE_FINISHED) {\r
       gMPSystem.ProcessorData[ProcessorNumber].State = CPU_STATE_IDLE;\r
-      gThread->MutexUnlock (&gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+      gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
       break;\r
     }\r
 \r
-    gThread->MutexUnlock (&gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+    gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
 \r
     if ((TimeoutInMicroseconds != 0) && (Timeout < 0)) {\r
       return EFI_TIMEOUT;\r
@@ -789,9 +798,12 @@ CpuMpServicesSwitchBSP (
   }\r
   ASSERT (Index != gMPSystem.NumberOfProcessors);\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
     return EFI_NOT_READY;\r
   }\r
+  gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
 \r
   // Skip for now as we need switch a bunch of stack stuff around and it's complex\r
   // May not be worth it?\r
@@ -861,11 +873,12 @@ CpuMpServicesEnableDisableAP (
     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
     return EFI_UNSUPPORTED;\r
   }\r
-\r
-  gThread->MutexLock (&gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+  gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
 \r
   if (EnableAP) {\r
     if ((gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_ENABLED_BIT) == 0 ) {\r
@@ -884,8 +897,6 @@ CpuMpServicesEnableDisableAP (
     gMPSystem.ProcessorData[ProcessorNumber].Info.StatusFlag |= (*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT);\r
   }\r
 \r
-  gThread->MutexUnlock (&gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
-\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -979,10 +990,9 @@ CpuCheckAllAPsStatus (
     gMPSystem.Timeout -= gPollInterval;\r
   }\r
 \r
-  ProcessorData = (PROCESSOR_DATA_BLOCK *) Context;\r
-\r
   for (ProcessorNumber = 0; ProcessorNumber < gMPSystem.NumberOfProcessors; ProcessorNumber++) {\r
-    if ((ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {\r
+    ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];\r
+    if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {\r
      // Skip BSP\r
       continue;\r
     }\r
@@ -997,12 +1007,12 @@ CpuCheckAllAPsStatus (
     // context. Meaning deadlock. Which is a bad thing.\r
     // So, try lock it. If we can get it, cool, do our thing.\r
     // otherwise, just dump out & try again on the next iteration.\r
-    Status = gThread->MutexTryLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+    Status = gThread->MutexTryLock (ProcessorData->StateLock);\r
     if (EFI_ERROR(Status)) {\r
       return;\r
     }\r
-    ProcessorState = gMPSystem.ProcessorData[ProcessorNumber].State;\r
-    gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+    ProcessorState = ProcessorData->State;\r
+    gThread->MutexUnlock (ProcessorData->StateLock);\r
 \r
     switch (ProcessorState) {\r
     case CPU_STATE_READY:\r
@@ -1015,15 +1025,17 @@ CpuCheckAllAPsStatus (
         if (!EFI_ERROR (Status)) {\r
           NextData = &gMPSystem.ProcessorData[NextNumber];\r
 \r
-          gThread->MutexLock (&NextData->StateLock);\r
+          gThread->MutexLock (NextData->StateLock);\r
           NextData->State = CPU_STATE_READY;\r
-          gThread->MutexUnlock (&NextData->StateLock);\r
+          gThread->MutexUnlock (NextData->StateLock);\r
 \r
           SetApProcedure (NextData, gMPSystem.Procedure, gMPSystem.ProcedureArgument);\r
         }\r
       }\r
 \r
-      gMPSystem.ProcessorData[ProcessorNumber].State = CPU_STATE_IDLE;\r
+      gThread->MutexLock (ProcessorData->StateLock);\r
+      ProcessorData->State = CPU_STATE_IDLE;\r
+      gThread->MutexUnlock (ProcessorData->StateLock);\r
       gMPSystem.FinishCount++;\r
       break;\r
 \r
@@ -1038,7 +1050,8 @@ CpuCheckAllAPsStatus (
     //\r
     if (gMPSystem.FailedList != NULL) {\r
       for (ProcessorNumber = 0; ProcessorNumber < gMPSystem.NumberOfProcessors; ProcessorNumber++) {\r
-        if ((ProcessorData[ProcessorNumber].Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {\r
+        ProcessorData = &gMPSystem.ProcessorData[ProcessorNumber];\r
+        if ((ProcessorData->Info.StatusFlag & PROCESSOR_AS_BSP_BIT) == PROCESSOR_AS_BSP_BIT) {\r
          // Skip BSP\r
           continue;\r
         }\r
@@ -1049,12 +1062,12 @@ CpuCheckAllAPsStatus (
         }\r
 \r
         // Mark the\r
-        Status = gThread->MutexTryLock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+        Status = gThread->MutexTryLock (ProcessorData->StateLock);\r
         if (EFI_ERROR(Status)) {\r
           return;\r
         }\r
-        ProcessorState = gMPSystem.ProcessorData[ProcessorNumber].State;\r
-        gThread->MutexUnlock (gMPSystem.ProcessorData[ProcessorNumber].StateLock);\r
+        ProcessorState = ProcessorData->State;\r
+        gThread->MutexUnlock (ProcessorData->StateLock);\r
 \r
         if (ProcessorState != CPU_STATE_IDLE) {\r
           // If we are retrying make sure we don't double count\r