5 files changed, 40 insertions, 26 deletions
diff --git a/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S b/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S
index f902a251c..af4350b0c 100644
--- a/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S
+++ b/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.S
@@ -1,5 +1,5 @@
 //
-//  Copyright (c) 2011, ARM Limited. All rights reserved.
+//  Copyright (c) 2011-2013, 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         
@@ -20,33 +20,42 @@
 .align 3
 
 GCC_ASM_IMPORT(CEntryPoint)
+GCC_ASM_IMPORT(ArmPlatformGetCorePosition)
+GCC_ASM_IMPORT(ArmPlatformIsPrimaryCore)
 GCC_ASM_IMPORT(ArmReadMpidr)
+GCC_ASM_IMPORT(ArmPlatformPeiBootAction)
 GCC_ASM_EXPORT(_ModuleEntryPoint)
 
 StartupAddr: .word    CEntryPoint
 
 ASM_PFX(_ModuleEntryPoint):
+  // Do early platform specific actions
+  bl    ASM_PFX(ArmPlatformPeiBootAction)
+
   // Identify CPU ID
   bl    ASM_PFX(ArmReadMpidr)
-  // Get ID of this CPU in Multicore system
-  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)
-  and   r5, r0, r1
-  
+  // Keep a copy of the MpId register value
+  mov   r5, r0
+
+  // Is it the Primary Core ?
+  bl    ASM_PFX(ArmPlatformIsPrimaryCore)
+
   // Get the top of the primary stacks (and the base of the secondary stacks)
   LoadConstantToReg (FixedPcdGet32(PcdCPUCoresStackBase), r1)
   LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
   add   r1, r1, r2
 
-  // Is it the Primary Core ?
-  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r3)
-  cmp   r5, r3
+  // r0 is equal to 1 if I am the primary core
+  cmp   r0, #1
   beq   _SetupPrimaryCoreStack
 
 _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
 
@@ -54,7 +63,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 a3dd1fb2c..be1a76769 100644
--- a/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.asm
+++ b/ArmPlatformPkg/PrePeiCore/Arm/PrePeiCoreEntryPoint.asm
@@ -1,5 +1,5 @@
 //
-//  Copyright (c) 2011, ARM Limited. All rights reserved.
+//  Copyright (c) 2011-2013, 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         
@@ -19,7 +19,10 @@
   INCLUDE AsmMacroIoLib.inc
   
   IMPORT  CEntryPoint
+  IMPORT  ArmPlatformGetCorePosition
+  IMPORT  ArmPlatformIsPrimaryCore
   IMPORT  ArmReadMpidr
+  IMPORT  ArmPlatformPeiBootAction
   EXPORT  _ModuleEntryPoint
   
   PRESERVE8
@@ -28,27 +31,33 @@
 StartupAddr        DCD      CEntryPoint
 
 _ModuleEntryPoint
+  // Do early platform specific actions
+  bl    ArmPlatformPeiBootAction
+
   // Identify CPU ID
   bl    ArmReadMpidr
-  // Get ID of this CPU in Multicore system
-  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)
-  and   r5, r0, r1
+  // Keep a copy of the MpId register value
+  mov   r5, r0
   
+  // Is it the Primary Core ?
+  bl	ArmPlatformIsPrimaryCore
+
   // Get the top of the primary stacks (and the base of the secondary stacks)
   LoadConstantToReg (FixedPcdGet32(PcdCPUCoresStackBase), r1)
   LoadConstantToReg (FixedPcdGet32(PcdCPUCorePrimaryStackSize), r2)
   add   r1, r1, r2
 
-  // Is it the Primary Core ?
-  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r3)
-  cmp   r5, r3
+  // r0 is equal to 1 if I am the primary core
+  cmp   r0, #1
   beq   _SetupPrimaryCoreStack
 
 _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
 
@@ -56,7 +65,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/PrePeiCore.c b/ArmPlatformPkg/PrePeiCore/PrePeiCore.c
index fdd8697ed..ececd4c76 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCore.c
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCore.c
@@ -93,7 +93,7 @@ CEntryPoint (
   //Note: The MMU will be enabled by MemoryPeim. Only the primary core will have the MMU on.
 
   // If not primary Jump to Secondary Main
-  if (IS_PRIMARY_CORE(MpId)) {
+  if (ArmPlatformIsPrimaryCore (MpId)) {
     // Initialize the Debug Agent for Source Level Debugging
     InitializeDebugAgent (DEBUG_AGENT_INIT_POSTMEM_SEC, NULL, NULL);
     SaveAndSetDebugTimerInterrupt (TRUE);
diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf b/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf
index 91f9b3a78..465848633 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf
@@ -62,7 +62,6 @@
   gArmTokenSpaceGuid.PcdFvBaseAddress
   gArmTokenSpaceGuid.PcdFvSize
 
-  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
   gArmTokenSpaceGuid.PcdArmPrimaryCore
   gArmTokenSpaceGuid.PcdGicPrimaryCoreId
 
diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCoreUniCore.inf b/ArmPlatformPkg/PrePeiCore/PrePeiCoreUniCore.inf
index bf590dbac..13f86902b 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCoreUniCore.inf
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCoreUniCore.inf
@@ -60,9 +60,6 @@
   gArmTokenSpaceGuid.PcdFvBaseAddress
   gArmTokenSpaceGuid.PcdFvSize
 
-  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
-  gArmTokenSpaceGuid.PcdArmPrimaryCore
-
   gArmPlatformTokenSpaceGuid.PcdCPUCoresStackBase
   gArmPlatformTokenSpaceGuid.PcdCPUCorePrimaryStackSize
   gArmPlatformTokenSpaceGuid.PcdCPUCoreSecondaryStackSize