PI Enable:

[mirror_edk2.git] / MdeModulePkg / Core / Pei / Dispatcher / Stack.c

diff --git a/MdeModulePkg/Core/Pei/Dispatcher/Stack.c b/MdeModulePkg/Core/Pei/Dispatcher/Stack.c
index 9e74eee5872983b94f9d0de453ccefc9fb00c6a6..551e23826c8a3287cc37a06eb7cc9b550983f63e 100644 (file)
--- a/MdeModulePkg/Core/Pei/Dispatcher/Stack.c
+++ b/MdeModulePkg/Core/Pei/Dispatcher/Stack.c
@@ -47,9 +47,33 @@ PeiSwitchStacks (
   IN      SWITCH_STACK_ENTRY_POINT  EntryPoint,\r
   IN      VOID                      *Context1,  OPTIONAL\r
   IN      VOID                      *Context2,  OPTIONAL\r
+  IN      VOID                      *Context3,  OPTIONAL\r
   IN      VOID                      *NewStack,\r
   IN      VOID                      *NewBsp\r
   )\r
 {\r
-  SwitchStack (EntryPoint, Context1, Context2, NewStack);\r
+  BASE_LIBRARY_JUMP_BUFFER  JumpBuffer;\r
+  \r
+  ASSERT (EntryPoint != NULL);\r
+  ASSERT (NewStack != NULL);\r
+\r
+  //\r
+  // Stack should be aligned with CPU_STACK_ALIGNMENT\r
+  //\r
+  ASSERT (((UINTN)NewStack & (CPU_STACK_ALIGNMENT - 1)) == 0);\r
+\r
+  JumpBuffer.Eip = (UINTN)EntryPoint;\r
+  JumpBuffer.Esp = (UINTN)NewStack - sizeof (VOID*);\r
+  JumpBuffer.Esp -= sizeof (Context1) + sizeof (Context2) + sizeof(Context3);\r
+  ((VOID**)JumpBuffer.Esp)[1] = Context1;\r
+  ((VOID**)JumpBuffer.Esp)[2] = Context2;\r
+  ((VOID**)JumpBuffer.Esp)[3] = Context3;\r
+\r
+  LongJump (&JumpBuffer, (UINTN)-1);\r
+  \r
+\r
+  //\r
+  // InternalSwitchStack () will never return\r
+  //\r
+  ASSERT (FALSE);  \r
 }\r