X-Git-Url: https://git.proxmox.com/?p=mirror_edk2.git;a=blobdiff_plain;f=UefiCpuPkg%2FCpuDxe%2FCpuMp.c;h=e3734c27908bd31199c2e22ba5df309440441f6a;hp=aa564c1f93cdaebf00c78bd39510855f78c49dd6;hb=003973d98cf1ef84ab810cb4f3870acd3a7f40a6;hpb=1535c888c6f06bb35881e83cd7ee49fb8554942b

diff --git a/UefiCpuPkg/CpuDxe/CpuMp.c b/UefiCpuPkg/CpuDxe/CpuMp.c
index aa564c1f93..e3734c2790 100644
--- a/UefiCpuPkg/CpuDxe/CpuMp.c
+++ b/UefiCpuPkg/CpuDxe/CpuMp.c
@@ -15,6 +15,47 @@
 #include "CpuDxe.h"
 #include "CpuMp.h"
 
+UINTN gMaxLogicalProcessorNumber;
+UINTN gApStackSize;
+
+VOID *mCommonStack = 0;
+VOID *mTopOfApCommonStack = 0;
+VOID *mApStackStart = 0;
+
+volatile UINTN mNumberOfProcessors;
+
+EFI_MP_SERVICES_PROTOCOL  mMpServicesTemplate = {
+  NULL, // GetNumberOfProcessors,
+  NULL, // GetProcessorInfo,
+  NULL, // StartupAllAPs,
+  NULL, // StartupThisAP,
+  NULL, // SwitchBSP,
+  NULL, // EnableDisableAP,
+  NULL  // WhoAmI
+};
+
+/**
+  Application Processors do loop routine
+  after switch to its own stack.
+
+  @param  Context1    A pointer to the context to pass into the function.
+  @param  Context2    A pointer to the context to pass into the function.
+
+**/
+VOID
+ProcessorToIdleState (
+  IN      VOID                      *Context1,  OPTIONAL
+  IN      VOID                      *Context2   OPTIONAL
+  )
+{
+  DEBUG ((DEBUG_INFO, "Ap apicid is %d\n", GetApicId ()));
+
+  AsmApDoneWithCommonStack ();
+
+  CpuSleep ();
+  CpuDeadLoop ();
+}
+
 /**
   Application Processor C code entry point.
 
@@ -25,6 +66,14 @@ ApEntryPointInC (
   VOID
   )
 {
+  mNumberOfProcessors++;
+  mApStackStart = (UINT8*)mApStackStart + gApStackSize;
+
+  SwitchStack (
+    (SWITCH_STACK_ENTRY_POINT)(UINTN)ProcessorToIdleState,
+    NULL,
+    NULL,
+    mApStackStart);
 }
 
 
@@ -37,5 +86,39 @@ InitializeMpSupport (
   VOID
   )
 {
-}
+  gMaxLogicalProcessorNumber = (UINTN) PcdGet32 (PcdCpuMaxLogicalProcessorNumber);
+  if (gMaxLogicalProcessorNumber < 1) {
+    DEBUG ((DEBUG_ERROR, "Setting PcdCpuMaxLogicalProcessorNumber should be more than zero.\n"));
+    return;
+  }
+
+  if (gMaxLogicalProcessorNumber == 1) {
+    return;
+  }
 
+  gApStackSize = (UINTN) PcdGet32 (PcdCpuApStackSize);
+  ASSERT ((gApStackSize & (SIZE_4KB - 1)) == 0);
+
+  mApStackStart = AllocatePages (EFI_SIZE_TO_PAGES (gMaxLogicalProcessorNumber * gApStackSize));
+  ASSERT (mApStackStart != NULL);
+
+  //
+  // the first buffer of stack size used for common stack, when the amount of AP
+  // more than 1, we should never free the common stack which maybe used for AP reset.
+  //
+  mCommonStack = mApStackStart;
+  mTopOfApCommonStack = (UINT8*) mApStackStart + gApStackSize;
+  mApStackStart = mTopOfApCommonStack;
+
+  mNumberOfProcessors = 1;
+
+  if (mNumberOfProcessors == 1) {
+    FreePages (mCommonStack, EFI_SIZE_TO_PAGES (gMaxLogicalProcessorNumber * gApStackSize));
+    return;
+  }
+
+  if (mNumberOfProcessors < gMaxLogicalProcessorNumber) {
+    FreePages (mApStackStart, EFI_SIZE_TO_PAGES ((gMaxLogicalProcessorNumber - mNumberOfProcessors) *
+                                                 gApStackSize));
+  }
+}