Align the top of the stack correctly when the PEI Core calls SwitchStack()

[mirror_edk2.git] / EdkModulePkg / Core / Pei / Dispatcher / Dispatcher.c

diff --git a/EdkModulePkg/Core/Pei/Dispatcher/Dispatcher.c b/EdkModulePkg/Core/Pei/Dispatcher/Dispatcher.c
index 1eb5a4a0204486ce76c6def7dae81ecbed425855..ccd840d3761e42ed8b039bec7f50db74cc7e19b0 100644 (file)
--- a/EdkModulePkg/Core/Pei/Dispatcher/Dispatcher.c
+++ b/EdkModulePkg/Core/Pei/Dispatcher/Dispatcher.c
@@ -63,6 +63,7 @@ Returns:
   BOOLEAN                           NextFvFound;\r
   EFI_FIRMWARE_VOLUME_HEADER        *NextFvAddress;\r
   EFI_FIRMWARE_VOLUME_HEADER        *DefaultFvAddress;\r
+  VOID                              *TopOfStack;\r
   //\r
   // Debug data for uninstalled Peim list\r
   //\r
@@ -105,7 +106,7 @@ Returns:
       //\r
       if (Status == EFI_SUCCESS) {\r
 \r
-        DEBUG_CODE (\r
+        DEBUG_CODE_BEGIN ();\r
 \r
           //\r
           // Fill list of found Peims for later list of those not installed\r
@@ -116,7 +117,7 @@ Returns:
             sizeof (EFI_GUID)\r
             );\r
 \r
-        );\r
+        DEBUG_CODE_END ();\r
 \r
         if (!Dispatched (\r
                DispatchData->CurrentPeim,\r
@@ -204,16 +205,18 @@ Returns:
                 PrivateDataInMem = (UINTN) TransferOldDataToNewDataRange (PrivateData);\r
                 ASSERT (PrivateDataInMem != 0);\r
                 //\r
-                //Subtract 0x10 from the 4th parameter indicating the new stack base,\r
-                //in order to provide buffer protection against possible illegal stack\r
-                //access that might corrupt the stack.\r
+                // Adjust the top of stack to be aligned at CPU_STACK_ALIGNMENT\r
                 //\r
-                SwitchStack (\r
+                TopOfStack = (VOID *)((UINTN)PrivateData->StackBase + (UINTN)PrivateData->StackSize - CPU_STACK_ALIGNMENT);\r
+                TopOfStack = ALIGN_POINTER (TopOfStack, CPU_STACK_ALIGNMENT);\r
+\r
+                PeiSwitchStacks (\r
                   (SWITCH_STACK_ENTRY_POINT)(UINTN)TempPtr.Raw,\r
                   PeiStartupDescriptor,\r
                   (VOID*)PrivateDataInMem,\r
-                  (VOID*)((UINTN)PrivateData->StackBase + (UINTN)PrivateData->StackSize)\r
-                );\r
+                  TopOfStack,\r
+                  (VOID*)(UINTN)PrivateData->StackBase\r
+                  );\r
               }\r
             }\r
           }\r
@@ -320,7 +323,7 @@ Returns:
     DispatchData->CurrentFvAddress = DefaultFvAddress;\r
   }\r
 \r
-  DEBUG_CODE (\r
+  DEBUG_CODE_BEGIN ();\r
     //\r
     // Debug data for uninstalled Peim list\r
     //\r
@@ -346,9 +349,9 @@ Returns:
       DebugNotDispatchedBitmap >>= 1;\r
     }\r
 \r
-  );\r
+  DEBUG_CODE_END ();\r
 \r
-   return EFI_NOT_FOUND;\r
+  return EFI_NOT_FOUND;\r
 }\r
 \r
 VOID\r