UefiCpuPkg/CpuDxe: introduce EFI_MP_SERVICES_PROTOCOL

[mirror_edk2.git] / UefiCpuPkg / CpuDxe / CpuMp.c

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