1 files changed, 42 insertions, 15 deletions
diff --git a/ArmPlatformPkg/Sec/SecEntryPoint.asm b/ArmPlatformPkg/Sec/SecEntryPoint.asm
index e85d6ce609..c39b80a85f 100644
--- a/ArmPlatformPkg/Sec/SecEntryPoint.asm
+++ b/ArmPlatformPkg/Sec/SecEntryPoint.asm
@@ -71,32 +71,57 @@ _WaitForEnabledScu
   bl    ArmIsScuEnable
   tst   r1, #1
   beq   _WaitForEnabledScu
-  b     _SetupStack
+  b     _SetupSecondaryCoreStack
 #endif
   
 _InitMem
   // Initialize Init Boot Memory
   bl    ArmPlatformInitializeBootMemory
-
+  
   // Only Primary CPU could run this line (the secondary cores have jumped from _IdentifyCpu to _SetupStack)
-  mov   r5, #0
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r5)
+
+_SetupPrimaryCoreStack
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoresSecStackBase), r2)
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecPrimaryStackSize), r3)
+  // Calculate the Top of the Stack
+  add   r2, r2, r3
+  LoadConstantToReg (FixedPcdGet32(PcdSecGlobalVariableSize), 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 SEC global variables to 0
+  mov     r3, sp
+  mov     r1, #0x0
+_InitGlobals
+  str     r1, [r3], #4
+  cmp     r3, r2
+  blt     _InitGlobals
 
-_SetupStack
-  // Setup Stack for the 4 CPU cores
-  //Read Stack Base address from PCD
+  b     _PrepareArguments
+
+_SetupSecondaryCoreStack
+  // Get the Core Position (ClusterId * 4) + CoreId
+  GetCorePositionInStack(r0, r5, 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   r0, r0, #1
+
+  // Get the base of the stack for the secondary cores
   LoadConstantToReg (FixedPcdGet32(PcdCPUCoresSecStackBase), r1)
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecSecondaryStackSize), r2)
+  add   r1, r1, r2
 
-  // Read Stack size from PCD
-  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecStackSize), r2)
+  // StackOffset = CorePos * StackSize
+  mul   r0, r0, r2
+  // SP = StackBase + StackOffset
+  add   sp, r1, r0
 
-  // Calcuate Stack Pointer reg value using Stack size and CPU ID.
-  mov     r3,r5         // 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
-  mov     sp, r3
-  
+
+_PrepareArguments
   // Move sec startup address into a data register
-  // ensure we're jumping to FV version of the code (not boot remapped alias)
+  // Ensure we're jumping to FV version of the code (not boot remapped alias)
   ldr   r3, StartupAddr
   
   // Jump to SEC C code
@@ -104,4 +129,6 @@ _SetupStack
   mov   r0, r5
   blx   r3
   
+_NeverReturn
+  b _NeverReturn
   END