From 9908a829d0c1c92ca9bf462c0d9210f63784c131 Mon Sep 17 00:00:00 2001
From: Chen Fan <chen.fan.fnst@cn.fujitsu.com>
Date: Thu, 13 Nov 2014 18:28:10 +0000
Subject: [PATCH] UefiCpuPkg/CpuDxe: Ap do loop routine to execute procedure

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Chen Fan <chen.fan.fnst@cn.fujitsu.com>
Reviewed-by: Jeff Fan <jeff.fan@intel.com>

git-svn-id: https://svn.code.sf.net/p/edk2/code/trunk/edk2@16361 6f19259b-4bc3-4df7-8a09-765794883524
---
 UefiCpuPkg/CpuDxe/CpuMp.c | 33 ++++++++++++++++++++++++++++++++-
 1 file changed, 32 insertions(+), 1 deletion(-)

diff --git a/UefiCpuPkg/CpuDxe/CpuMp.c b/UefiCpuPkg/CpuDxe/CpuMp.c
index 878cdf25db..1b449a7a8d 100644
--- a/UefiCpuPkg/CpuDxe/CpuMp.c
+++ b/UefiCpuPkg/CpuDxe/CpuMp.c
@@ -1108,10 +1108,41 @@ ProcessorToIdleState (
   IN      VOID                      *Context2   OPTIONAL
   )
 {
-  DEBUG ((DEBUG_INFO, "Ap apicid is %d\n", GetApicId ()));
+  UINTN                 ProcessorNumber;
+  CPU_DATA_BLOCK        *CpuData;
+  EFI_AP_PROCEDURE      Procedure;
+  VOID                  *ProcedureArgument;
+
+  WhoAmI (&mMpServicesTemplate, &ProcessorNumber);
+  CpuData = &mMpSystemData.CpuDatas[ProcessorNumber];
 
   AsmApDoneWithCommonStack ();
 
+  while (TRUE) {
+    while (!AcquireSpinLockOrFail (&CpuData->CpuDataLock)) {
+      CpuPause ();
+    }
+
+    ProcedureArgument = CpuData->Parameter;
+    Procedure = CpuData->Procedure;
+    ReleaseSpinLock (&CpuData->CpuDataLock);
+
+    if (Procedure != NULL) {
+      Procedure (ProcedureArgument);
+
+      while (!AcquireSpinLockOrFail (&CpuData->CpuDataLock)) {
+        CpuPause ();
+      }
+
+      CpuData->Procedure = NULL;
+      ReleaseSpinLock (&CpuData->CpuDataLock);
+
+      SetApState (CpuData, CpuStateFinished);
+    }
+
+    CpuPause ();
+  }
+
   CpuSleep ();
   CpuDeadLoop ();
 }