ArmPlatformPkg/ArmPlatformStackLib: Introduced helper library to initialize stacks

The new functions are:
- ArmPlatformStackSet(IN UINTN StackBase, IN UINTN MpId, IN UINTN PrimaryStackSize, IN UINTN SecondaryStackSize);
- ArmPlatformStackSetPrimary(IN UINTN StackBase, IN UINTN MpId, IN UINTN PrimaryStackSize, IN UINTN SecondaryStackSize);
- ArmPlatformStackSetSecondary(IN UINTN StackBase, IN UINTN MpId, IN UINTN PrimaryStackSize, IN UINTN SecondaryStackSize);

The stack topology can be changed by implementing a new ArmPlatformStackLib

Signed-off-by: Olivier Martin <olivier.martin@arm.com>




git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@13774 6f19259b-4bc3-4df7-8a09-765794883524

ArmPkg/Include/AsmMacroIoLib.h

@@ -144,6 +144,21 @@ _SetPrimaryStackInitGlobals:          ;             \
   b       _SetPrimaryStackInitGlobals ;             \
 _SetPrimaryStackEnd:
 
+// Initialize the Global Variable with '0'
+#define InitializePrimaryStack(GlobalSize, Tmp1)    \
+  and     Tmp1, GlobalSize, #7        ;             \
+  rsbne   Tmp1, Tmp1, #8              ;             \
+  add     GlobalSize, GlobalSize, Tmp1 ;            \
+                                      ;             \
+  mov     Tmp1, sp                    ;             \
+  sub     sp, GlobalSize              ;             \
+  mov     GlobalSize, #0x0            ;             \
+_InitializePrimaryStackLoop:          ;             \
+  cmp     Tmp1, sp                    ;             \
+  bls     _InitializePrimaryStackEnd  ;             \
+  str     GlobalSize, [Tmp1], #-4     ;             \
+  b       _InitializePrimaryStackLoop ;             \
+_InitializePrimaryStackEnd:
 
 #elif defined (__GNUC__)
 
@@ -213,6 +228,22 @@ _SetPrimaryStackInitGlobals:          ;             \
   b       _SetPrimaryStackInitGlobals ;             \
 _SetPrimaryStackEnd:
 
+// Initialize the Global Variable with '0'
+#define InitializePrimaryStack(GlobalSize, Tmp1)    \
+  and     Tmp1, GlobalSize, #7        ;             \
+  rsbne   Tmp1, Tmp1, #8              ;             \
+  add     GlobalSize, GlobalSize, Tmp1 ;            \
+                                      ;             \
+  mov     Tmp1, sp                    ;             \
+  sub     sp, GlobalSize              ;             \
+  mov     GlobalSize, #0x0            ;             \
+_InitializePrimaryStackLoop:          ;             \
+  cmp     Tmp1, sp                    ;             \
+  bls     _InitializePrimaryStackEnd  ;             \
+  str     GlobalSize, [Tmp1], #-4     ;             \
+  b       _InitializePrimaryStackLoop ;             \
+_InitializePrimaryStackEnd:
+
 #else
 
 //
@@ -278,6 +309,9 @@ _SetPrimaryStackEnd:
 
 #define SetPrimaryStack(StackTop,GlobalSize,Tmp) SetPrimaryStack StackTop, GlobalSize, Tmp
 
+// Initialize the Global Variable with '0'
+#define InitializePrimaryStack(GlobalSize, Tmp1) InitializePrimaryStack GlobalSize, Tmp1
+
 #endif
 
 #endif

ArmPkg/Include/AsmMacroIoLib.inc

@@ -104,8 +104,25 @@ _SetPrimaryStackInitGlobals
   beq     _SetPrimaryStackEnd
   str     $GlobalSize, [$Tmp], #4
   b       _SetPrimaryStackInitGlobals
-
 _SetPrimaryStackEnd
   MEND
 
+  MACRO
+  InitializePrimaryStack $GlobalSize, $Tmp1
+  and     $Tmp1, $GlobalSize, #7
+  rsbne   $Tmp1, $Tmp1, #8
+  add     $GlobalSize, $GlobalSize, $Tmp1
+
+  mov     $Tmp1, sp
+  sub     sp, $GlobalSize
+  ; Set all the global variables to 0
+  mov     $GlobalSize, #0x0
+_InitializePrimaryStackLoop
+  cmp     $Tmp1, sp
+  bls     _InitializePrimaryStackEnd
+  str     $GlobalSize, [$Tmp1], #-4
+  b       _InitializePrimaryStackLoop
+_InitializePrimaryStackEnd
+  MEND
+
   END

ArmPlatformPkg/ArmVExpressPkg/ArmVExpress.dsc.inc

@@ -61,6 +61,7 @@
   ArmDisassemblerLib|ArmPkg/Library/ArmDisassemblerLib/ArmDisassemblerLib.inf
   DmaLib|ArmPkg/Library/ArmDmaLib/ArmDmaLib.inf
   ArmGicLib|ArmPkg/Drivers/PL390Gic/PL390GicLib.inf
+  ArmPlatformStackLib|ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf
 
   # Versatile Express Specific Libraries
   ArmPlatformSysConfigLib|ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressSysConfigLib/ArmVExpressSysConfigLib.inf

ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.S

@@ -0,0 +1,106 @@
+//
+//  Copyright (c) 2012, ARM Limited. All rights reserved.
+//
+//  This program and the accompanying materials
+//  are licensed and made available under the terms and conditions of the BSD License
+//  which accompanies this distribution.  The full text of the license may be found at
+//  http://opensource.org/licenses/bsd-license.php
+//
+//  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+//
+//
+
+#include <AsmMacroIoLib.h>
+#include <Base.h>
+#include <AutoGen.h>
+
+.text
+.align 3
+
+GCC_ASM_EXPORT(ArmPlatformStackSet)
+GCC_ASM_EXPORT(ArmPlatformStackSetPrimary)
+GCC_ASM_EXPORT(ArmPlatformStackSetSecondary)
+
+GCC_ASM_IMPORT(ArmPlatformGetCorePosition)
+
+GCC_ASM_IMPORT(gPcd_FixedAtBuild_PcdCoreCount)
+GCC_ASM_IMPORT(_gPcd_FixedAtBuild_PcdArmPrimaryCore)
+
+//VOID
+//ArmPlatformStackSet (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ASM_PFX(ArmPlatformStackSet):
+  // Identify Stack
+  // Mask for ClusterId|CoreId
+  LoadConstantToReg (0xFFFF, r4)
+  and	r1, r1, r4
+  // Is it the Primary Core ?
+  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r4)
+  ldr	r4, [r4]
+  cmp   r1, r4
+  beq	ASM_PFX(ArmPlatformStackSetPrimary)
+  bne   ASM_PFX(ArmPlatformStackSetSecondary)
+
+//VOID
+//ArmPlatformStackSetPrimary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ASM_PFX(ArmPlatformStackSetPrimary):
+  mov	r4, lr
+
+  // Add stack of primary stack to StackBase
+  add	r0, r0, r2
+
+  // Compute SecondaryCoresCount * SecondaryCoreStackSize
+  LoadConstantToReg (_gPcd_FixedAtBuild_PcdCoreCount, r1)
+  ldr	r1, [r1]
+  sub	r1, #1
+  mul	r3, r3, r1
+
+  // Set Primary Stack ((StackBase + PrimaryStackSize) + (SecondaryCoresCount * SecondaryCoreStackSize))
+  add	sp, r0, r3
+
+  bx    r4
+
+//VOID
+//ArmPlatformStackSetSecondary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ASM_PFX(ArmPlatformStackSetSecondary):
+  mov	r4, lr
+  mov   sp, r0
+
+  // Get Core Position
+  mov	r0, r1
+  bl    ASM_PFX(ArmPlatformGetCorePosition)
+  mov	r5, r0
+
+  // Get Primary Core Position
+  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r0)
+  ldr	r0, [r0]
+  bl    ASM_PFX(ArmPlatformGetCorePosition)
+
+  // Get Secondary Core Position. We should get consecutive secondary stack number from 1...(CoreCount-1)
+  cmp	r5, r0
+  subhi r5, r5, #1
+  add	r5, r5, #1
+
+  // Compute top of the secondary stack
+  mul	r3, r3, r5
+
+  // Set stack
+  add	sp, sp, r3
+
+  bx r4
+

ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.asm

@@ -0,0 +1,112 @@
+//
+//  Copyright (c) 2012, ARM Limited. All rights reserved.
+//
+//  This program and the accompanying materials
+//  are licensed and made available under the terms and conditions of the BSD License
+//  which accompanies this distribution.  The full text of the license may be found at
+//  http://opensource.org/licenses/bsd-license.php
+//
+//  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+//
+//
+
+#include <AsmMacroIoLib.h>
+#include <Base.h>
+#include <AutoGen.h>
+
+  INCLUDE AsmMacroIoLib.inc
+
+  EXPORT  ArmPlatformStackSet
+  EXPORT  ArmPlatformStackSetPrimary
+  EXPORT  ArmPlatformStackSetSecondary
+
+  IMPORT  ArmPlatformGetCorePosition
+
+  IMPORT  _gPcd_FixedAtBuild_PcdCoreCount
+  IMPORT  _gPcd_FixedAtBuild_PcdArmPrimaryCore
+
+  PRESERVE8
+  AREA    ArmPlatformStackLib, CODE, READONLY
+
+//VOID
+//ArmPlatformStackSet (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSet FUNCTION
+  // Identify Stack
+  // Mask for ClusterId|CoreId
+  LoadConstantToReg (0xFFFF, r4)
+  and	r1, r1, r4
+  // Is it the Primary Core ?
+  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r4)
+  ldr	r4, [r4]
+  cmp   r1, r4
+  beq	ArmPlatformStackSetPrimary
+  bne   ArmPlatformStackSetSecondary
+  ENDFUNC
+
+//VOID
+//ArmPlatformStackSetPrimary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSetPrimary FUNCTION
+  mov	r4, lr
+
+  // Add stack of primary stack to StackBase
+  add	r0, r0, r2
+
+  // Compute SecondaryCoresCount * SecondaryCoreStackSize
+  LoadConstantToReg (_gPcd_FixedAtBuild_PcdCoreCount, r1)
+  ldr	r1, [r1]
+  sub	r1, #1
+  mul	r3, r3, r1
+
+  // Set Primary Stack ((StackBase + PrimaryStackSize) + (SecondaryCoresCount * SecondaryCoreStackSize))
+  add	sp, r0, r3
+
+  bx    r4
+  ENDFUNC
+
+//VOID
+//ArmPlatformStackSetSecondary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSetSecondary FUNCTION
+  mov	r4, lr
+  mov   sp, r0
+
+  // Get Core Position
+  mov	r0, r1
+  bl ArmPlatformGetCorePosition
+  mov	r5, r0
+
+  // Get Primary Core Position
+  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r0)
+  ldr	r0, [r0]
+  bl ArmPlatformGetCorePosition
+
+  // Get Secondary Core Position. We should get consecutive secondary stack number from 1...(CoreCount-1)
+  cmp	r5, r0
+  subhi r5, r5, #1
+  add	r5, r5, #1
+
+  // Compute top of the secondary stack
+  mul	r3, r3, r5
+
+  // Set stack
+  add	sp, sp, r3
+
+  bx r4
+  ENDFUNC
+
+  END

ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf

@@ -0,0 +1,39 @@
+#/* @file
+#
+#  Copyright (c) 2012, ARM Limited. All rights reserved.
+#  
+#  This program and the accompanying materials                          
+#  are licensed and made available under the terms and conditions of the BSD License         
+#  which accompanies this distribution.  The full text of the license may be found at        
+#  http://opensource.org/licenses/bsd-license.php                                            
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+#
+#*/
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ArmPlatformStackLib
+  FILE_GUID                      = 5e2e44af-53c1-44c2-a801-9c149f3d6ba0
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ArmPlatformStackLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+
+[Sources.common]
+  ArmPlatformStackLib.asm     | RVCT
+  ArmPlatformStackLib.S       | GCC
+
+[FixedPcd]
+  gArmPlatformTokenSpaceGuid.PcdCoreCount
+
+  gArmPlatformTokenSpaceGuid.PcdCoreCount
+
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
+  gArmTokenSpaceGuid.PcdArmPrimaryCore

ArmPlatformPkg/PrePi/ModuleEntryPoint.S

@@ -21,7 +21,7 @@
 
 GCC_ASM_IMPORT(CEntryPoint)
 GCC_ASM_IMPORT(ArmReadMpidr)
-GCC_ASM_IMPORT(ArmIsMpCore)
+GCC_ASM_IMPORT(ArmPlatformStackSet)
 GCC_ASM_EXPORT(_ModuleEntryPoint)
 
 StartupAddr: .word    CEntryPoint
@@ -31,10 +31,11 @@ ASM_PFX(_ModuleEntryPoint):
   // Get ID of this CPU in Multicore system
   bl    ASM_PFX(ArmReadMpidr)
   LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)
-  and   r5, r0, r1
+  and   r6, r0, r1
 
 _SetSVCMode:
   // Enter SVC mode, Disable FIQ and IRQ
+  //TODO: remove hardcoded values
   mov     r1, #0x13|0x80|0x40
   msr     CPSR_c, r1
 
@@ -76,92 +77,59 @@ _SetupStack:
   // Because the 'push' instruction is equivalent to 'stmdb' (decrement before), we need to increment
   // one to the top of the stack. We check if incrementing one does not overflow (case of DRAM at the
   // top of the memory space)
-  adds  r6, r1, #1
+  adds  r7, r1, #1
   bcs   _SetupOverflowStack
 
 _SetupAlignedStack:
-  mov   r1, r6
+  mov   r1, r7
   b     _GetBaseUefiMemory
 
 _SetupOverflowStack:
   // Case memory at the top of the address space. Ensure the top of the stack is EFI_PAGE_SIZE
   // aligned (4KB)
-  LoadConstantToReg (EFI_PAGE_MASK, r6)
-  and   r6, r6, r1
-  sub   r1, r1, r6
+  LoadConstantToReg (EFI_PAGE_MASK, r7)
+  and   r7, r7, r1
+  sub   r1, r1, r7
 
 _GetBaseUefiMemory:
   // Calculate the Base of the UEFI Memory
-  sub   r6, r1, r4
+  sub   r7, r1, r4
 
 _GetStackBase:
-  // Compute Base of Normal stacks for CPU Cores
-  // Is it MpCore system
-  bl    ASM_PFX(ArmIsMpCore)
-  cmp   r0, #0
-  // Case it is not an MP Core system. Just setup the primary core
-  beq   _SetupUnicoreStack
-
-_GetStackBaseMpCore:
   // r1 = The top of the Mpcore Stacks
   // Stack for the primary core = PrimaryCoreStack
   LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
-  sub   r7, r1, r2
+  sub   r8, r1, r2
 
-  // Stack for the secondary core = Number of Clusters * (4 Cores per cluster) * SecondaryStackSize
-  LoadConstantToReg (FixedPcdGet32(PcdClusterCount), r2)
-  lsl   r2, r2, #2
-  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecondaryStackSize), r3)
-  mul   r2, r2, r3
-  sub   r7, r7, r2
+  // Stack for the secondary core = Number of Cores - 1
+  LoadConstantToReg (FixedPcdGet32(PcdCoreCount), r0)
+  sub	r0, r0, #1
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecondaryStackSize), r1)
+  mul   r1, r1, r0
+  sub   r8, r8, r1
 
-  // The base of the secondary Stacks = Top of Primary stack
+  // r8 = The base of the MpCore Stacks (primary stack & secondary stacks)
+  mov	r0, r8
+  mov	r1, r6
+  //ArmPlatformStackSet(StackBase, MpId, PrimaryStackSize, SecondaryStackSize)
   LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
-  add   r1, r7, r2
-
-  // r7 = The base of the MpCore Stacks (primary stack + cluster_count * 4 * secondary stacks)
-  // r1 = The base of the secondary Stacks = Top of the Primary stack
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecondaryStackSize), r3)
+  bl	ASM_PFX(ArmPlatformStackSet)
 
   // Is it the Primary Core ?
   LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r4)
-  cmp   r5, r4
-  beq   _SetupPrimaryCoreStack
-
-_SetupSecondaryCoreStack:
-  // r1 = The base of the secondary Stacks
-
-  // Get the position of the cores (ClusterId * 4) + CoreId
-  GetCorePositionFromMpId(r0, r5, r4)
-  // 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 offset for the Secondary Stack
-  mul   r0, r0, r3
-  add   sp, r1, r0
-
+  cmp   r6, r4
   bne   _PrepareArguments
 
-_SetupPrimaryCoreStack:
-  // r1 = Top of the primary stack
-  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r2)
-  b     _PreparePrimaryStack
-
-_SetupUnicoreStack:
-  // The top of the Unicore Stack is in r1
-  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r2)
-  LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r3)
-
-  // Calculate the bottom of the primary stack (StackBase)
-  sub   r7, r1, r3
-
-_PreparePrimaryStack:
-  // The reserved space for global variable must be 8-bytes aligned for pushing
-  // 64-bit variable on the stack
-  SetPrimaryStack (r1, r2, r3)
+_ReserveGlobalVariable:
+  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r0)
+  // InitializePrimaryStack($GlobalVariableSize, $Tmp1)
+  InitializePrimaryStack(r0, r1)
 
 _PrepareArguments:
-  mov   r0, r5
-  mov   r1, r6
-  mov   r2, r7
+  mov   r0, r6
+  mov   r1, r7
+  mov   r2, r8
   mov   r3, sp
 
   // Move sec startup address into a data register

ArmPlatformPkg/PrePi/ModuleEntryPoint.asm

@@ -33,10 +33,11 @@ _ModuleEntryPoint
   // Get ID of this CPU in Multicore system
   bl    ArmReadMpidr
   LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)
-  and   r5, r0, r1
+  and   r6, r0, r1
 
 _SetSVCMode
   // Enter SVC mode, Disable FIQ and IRQ
+  //TODO: remove hardcoded values
   mov     r1, #0x13 :OR: 0x80 :OR: 0x40
   msr     CPSR_c, r1
 
@@ -78,33 +79,25 @@ _SetupStack
   // Because the 'push' instruction is equivalent to 'stmdb' (decrement before), we need to increment
   // one to the top of the stack. We check if incrementing one does not overflow (case of DRAM at the
   // top of the memory space)
-  adds  r6, r1, #1
+  adds  r7, r1, #1
   bcs   _SetupOverflowStack
 
 _SetupAlignedStack
-  mov   r1, r6
+  mov   r1, r7
   b     _GetBaseUefiMemory
 
 _SetupOverflowStack
   // Case memory at the top of the address space. Ensure the top of the stack is EFI_PAGE_SIZE
   // aligned (4KB)
-  LoadConstantToReg (EFI_PAGE_MASK, r6)
-  and   r6, r6, r1
-  sub   r1, r1, r6
+  LoadConstantToReg (EFI_PAGE_MASK, r7)
+  and   r7, r7, r1
+  sub   r1, r1, r7
 
 _GetBaseUefiMemory
   // Calculate the Base of the UEFI Memory
-  sub   r6, r1, r4
+  sub   r7, r1, r4
 
 _GetStackBase
-  // Compute Base of Normal stacks for CPU Cores
-  // Is it MpCore system
-  bl    ArmIsMpCore
-  cmp   r0, #0
-  // Case it is not an MP Core system. Just setup the primary core
-  beq   _SetupUnicoreStack
-
-_GetStackBaseMpCore
   // r1 = The top of the Mpcore Stacks
   // Stack for the primary core = PrimaryCoreStack
   LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
@@ -123,56 +116,22 @@ _GetStackBaseMpCore
   //ArmPlatformStackSet(StackBase, MpId, PrimaryStackSize, SecondaryStackSize)
   LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
   LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecondaryStackSize), r3)
-  mul   r2, r2, r3
-  sub   r7, r7, r2
-
-  // The base of the secondary Stacks = Top of Primary stack
-  LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
-  add   r1, r7, r2
-
-  // r7 = The base of the MpCore Stacks (primary stack + cluster_count * 4 * secondary stacks)
-  // r1 = The base of the secondary Stacks = Top of the Primary stack
+  bl	ArmPlatformStackSet
 
   // Is it the Primary Core ?
   LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r4)
-  cmp   r5, r4
-  beq   _SetupPrimaryCoreStack
-
-_SetupSecondaryCoreStack
-  // r1 = The base of the secondary Stacks
-
-  // Get the position of the cores (ClusterId * 4) + CoreId
-  GetCorePositionFromMpId(r0, r5, r4)
-  // 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 offset for the Secondary Stack
-  mul   r0, r0, r3
-  add   sp, r1, r0
-
+  cmp   r6, r4
   bne   _PrepareArguments
 
-_SetupPrimaryCoreStack
-  // r1 = Top of the primary stack
-  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r2)
-  b     _PreparePrimaryStack
-
-_SetupUnicoreStack
-  // The top of the Unicore Stack is in r1
-  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r2)
-  LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r3)
-
-  // Calculate the bottom of the primary stack (StackBase)
-  sub   r7, r1, r3
-
-_PreparePrimaryStack
-  // The reserved space for global variable must be 8-bytes aligned for pushing
-  // 64-bit variable on the stack
-  SetPrimaryStack (r1, r2, r3)
+_ReserveGlobalVariable
+  LoadConstantToReg (FixedPcdGet32(PcdPeiGlobalVariableSize), r0)
+  // InitializePrimaryStack($GlobalVariableSize, $Tmp1)
+  InitializePrimaryStack r0, r1
 
 _PrepareArguments
-  mov   r0, r5
-  mov   r1, r6
-  mov   r2, r7
+  mov   r0, r6
+  mov   r1, r7
+  mov   r2, r8
   mov   r3, sp
 
   // Move sec startup address into a data register

ArmPlatformPkg/PrePi/PeiMPCore.inf

@@ -48,6 +48,7 @@
   DebugAgentLib
   PrePiLib
   ArmPlatformLib
+  ArmPlatformStackLib
   MemoryAllocationLib
   HobLib
   PrePiHobListPointerLib
@@ -77,7 +78,6 @@
   gArmTokenSpaceGuid.PcdFvBaseAddress
   gArmTokenSpaceGuid.PcdFvSize
 
-  gArmPlatformTokenSpaceGuid.PcdCPUCoresStackBase
   gArmPlatformTokenSpaceGuid.PcdCPUCorePrimaryStackSize
   gArmPlatformTokenSpaceGuid.PcdCPUCoreSecondaryStackSize
 

ArmPlatformPkg/PrePi/PeiUniCore.inf

@@ -47,6 +47,7 @@
   DebugAgentLib
   PrePiLib
   ArmPlatformLib
+  ArmPlatformStackLib
   MemoryAllocationLib
   HobLib
   PrePiHobListPointerLib

BeagleBoardPkg/BeagleBoardPkg.dsc

@@ -34,6 +34,7 @@
   ArmLib|ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.inf
   ArmPlatformLib|BeagleBoardPkg/Library/BeagleBoardLib/BeagleBoardLib.inf
   ArmCpuLib|ArmPkg/Drivers/ArmCpuLib/ArmCortexA8Lib/ArmCortexA8Lib.inf
+  ArmPlatformStackLib|ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf
   
   HiiLib|MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.inf
   UefiHiiServicesLib|MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.inf