1 files changed, 42 insertions, 17 deletions
diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm b/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm
index e4eb612610..1964f18ebf 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm
@@ -34,25 +34,47 @@ _ModuleEntryPoint
   LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)
   and   r0, r0, r1
 
-_SetupStack
-  // Setup Stack for the 4 CPU cores
-  LoadConstantToReg (FixedPcdGet32(PcdCPUCoresNonSecStackBase), r1)
-  LoadConstantToReg (FixedPcdGet32(PcdCPUCoresNonSecStackSize), r2)
-  
-  mov   r3, r0              // r3 = core_id
-  mul   r3, r3, r2          // r3 = core_id * stack_size = offset from the stack base
-  add   r3, r3, r1          // r3 = stack_base + offset
-  add   r3, r3, r2, LSR #1  // r3 = stack_offset + (stack_size/2) <-- the top half is for the heap
-  mov   sp, r3
+  // Calculate the top of the primary stack
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoresStackBase), r1)
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
+  add   r2, r2, r1
+
+  // Is it the Primary Core ?
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r1)
+  cmp   r0, r1
+  beq   _SetupPrimaryCoreStack
+
+_SetupSecondaryCoreStack
+  // r2 = Top of the primary stack = Base of the Secondary Stacks
+
+  // Get the position of the cores (ClusterId * 4) + CoreId
+  GetCorePositionInStack(r3, r0, r1)
+  // The stack starts at the top of the stack region. Add '1' to the Core Position to get the top of the stack
+  add   r3, r3, #1
 
-  // Only allocate memory in top of the primary core stack
-  cmp   r0, #0
-  bne   _PrepareArguments
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecondaryStackSize), r1)
+  // StackOffset = CorePos * StackSize
+  mul   r3, r3, r1
+  // SP = StackBase + StackOffset
+  add   sp, r2, r3
 
-_AllocateGlobalPeiVariables
-  // Reserve top of the stack for Global PEI Variables (eg: PeiServicesTablePointer)
-  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r1)
-  sub   sp, sp, r1
+  b     _PrepareArguments
+
+_SetupPrimaryCoreStack
+  // r2 = Top of the primary stack
+  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r3)
+
+  // The reserved space for global variable must be 8-bytes aligned for pushing
+  // 64-bit variable on the stack
+  SetPrimaryStack (r2, r3, r1)
+
+  // Set all the PEI global variables to 0
+  mov   r3, sp
+  mov   r1, #0x0
+_InitGlobals
+  str   r1, [r3], #4
+  cmp   r3, r2
+  blt   _InitGlobals
 
 _PrepareArguments
   // The PEI Core Entry Point has been computed by GenFV and stored in the second entry of the Reset Vector
@@ -69,4 +91,7 @@ _PrepareArguments
   //    r1 = pei_core_address
   blx   r2
 
+_NeverReturn
+  b _NeverReturn
+
   END