2 files changed, 12 insertions, 6 deletions
diff --git a/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S b/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S
index 1fb8570eb3..3b463eabee 100644
--- a/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S
+++ b/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S
@@ -20,6 +20,7 @@
 .align 3
 
 GCC_ASM_IMPORT(CEntryPoint)
+GCC_ASM_IMPORT(ArmPlatformGetCorePosition)
 GCC_ASM_IMPORT(ArmPlatformIsPrimaryCore)
 GCC_ASM_IMPORT(ArmReadMpidr)
 GCC_ASM_EXPORT(_ModuleEntryPoint)
@@ -47,8 +48,10 @@ ASM_PFX(_ModuleEntryPoint):
 _SetupSecondaryCoreStack:
   // r1 contains the base of the secondary stacks
 
-  // Get the Core Position (ClusterId * 4) + CoreId
-  GetCorePositionFromMpId(r0, r5, r2)
+  // Get the Core Position
+  mov   r6, r1      // Save base of the secondary stacks
+  mov   r0, r5
+  bl    ASM_PFX(ArmPlatformGetCorePosition)
   // 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
 
@@ -56,7 +59,7 @@ _SetupSecondaryCoreStack:
   LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecondaryStackSize), r2)
   mul   r0, r0, r2
   // SP = StackBase + StackOffset
-  add   sp, r1, r0
+  add   sp, r6, r0
 
 _PrepareArguments:
   // The PEI Core Entry Point has been computed by GenFV and stored in the second entry of the Reset Vector
diff --git a/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.asm b/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.asm
index 3b756b492a..1510fb84d0 100644
--- a/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.asm
+++ b/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.asm
@@ -19,6 +19,7 @@
   INCLUDE AsmMacroIoLib.inc
   
   IMPORT  CEntryPoint
+  IMPORT  ArmPlatformGetCorePosition
   IMPORT  ArmPlatformIsPrimaryCore
   IMPORT  ArmReadMpidr
   EXPORT  _ModuleEntryPoint
@@ -49,8 +50,10 @@ _ModuleEntryPoint
 _SetupSecondaryCoreStack
   // r1 contains the base of the secondary stacks
 
-  // Get the Core Position (ClusterId * 4) + CoreId
-  GetCorePositionFromMpId(r0, r5, r2)
+  // Get the Core Position
+  mov   r6, r1      // Save base of the secondary stacks
+  mov   r0, r5
+  bl    ArmPlatformGetCorePosition
   // 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
 
@@ -58,7 +61,7 @@ _SetupSecondaryCoreStack
   LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecondaryStackSize), r2)
   mul   r0, r0, r2
   // SP = StackBase + StackOffset
-  add   sp, r1, r0
+  add   sp, r6, r0
 
 _PrepareArguments
   // The PEI Core Entry Point has been computed by GenFV and stored in the second entry of the Reset Vector