ArmPlatformPkg: Introduce Primary core macros

On MpCore system, the primary core can now be any core of the system.

To identify the primary core, you can use 'gArmTokenSpaceGuid.PcdArmPrimaryCoreMask'
and 'gArmTokenSpaceGuid.PcdArmPrimaryCore'.
These PCDs by default use the ClusterId and CoreId to identify the core. And the
primary core is defined as the ClusetrId=0 and CoreId=0.

The helper macros are: IS_PRIMARY_CORE(MpId), GET_CORE_ID(MpId), GET_CLUSTER_ID(MpId),
GET_CORE_POS(MpId), PRIMARY_CORE_ID.



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

22 files changed, 82 insertions, 60 deletions

diff --git a/ArmPlatformPkg/ArmRealViewEbPkg/Library/ArmRealViewEbLibRTSM/ArmRealViewEbSec.c b/ArmPlatformPkg/ArmRealViewEbPkg/Library/ArmRealViewEbLibRTSM/ArmRealViewEbSec.c
index 35a873867..ee4a404da 100755
--- a/ArmPlatformPkg/ArmRealViewEbPkg/Library/ArmRealViewEbLibRTSM/ArmRealViewEbSec.c
+++ b/ArmPlatformPkg/ArmRealViewEbPkg/Library/ArmRealViewEbLibRTSM/ArmRealViewEbSec.c
@@ -61,7 +61,7 @@ ArmPlatformSecInitialize (
 **/
 VOID
 ArmPlatformSecExtraAction (
-  IN  UINTN         CoreId,
+  IN  UINTN         MpId,
   OUT UINTN*        JumpAddress
   )
 {
diff --git a/ArmPlatformPkg/Include/Library/ArmPlatformLib.h b/ArmPlatformPkg/Include/Library/ArmPlatformLib.h
index c49812560..4ea640f0d 100644
--- a/ArmPlatformPkg/Include/Library/ArmPlatformLib.h
+++ b/ArmPlatformPkg/Include/Library/ArmPlatformLib.h
@@ -103,7 +103,7 @@ ArmPlatformSecInitialize (
 **/
 VOID
 ArmPlatformSecExtraAction (
-  IN  UINTN         CoreId,
+  IN  UINTN         MpId,
   OUT UINTN*        JumpAddress
   );
 
diff --git a/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.c b/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.c
index b9812142e..57390793f 100755
--- a/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.c
+++ b/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.c
@@ -14,14 +14,14 @@
 

 #include <PiPei.h>

 

+#include <Library/ArmLib.h>

 #include <Library/ArmGicLib.h>

 #include <Library/DebugLib.h>

 #include <Library/PcdLib.h>

 #include <Library/PrintLib.h>

 #include <Library/SerialPortLib.h>

-#include <Chipset/ArmV7.h>
 

-#define ARM_PRIMARY_CORE    0

+#include <Chipset/ArmV7.h>
 

 // When the firmware is built as not Standalone, the secondary cores need to wait the firmware

 // entirely written into DRAM. It is the firmware from DRAM which will wake up the secondary cores.

@@ -38,7 +38,7 @@ NonSecureWaitForFirmware (
   ArmCallWFI();

 

   // Acknowledge the interrupt and send End of Interrupt signal.

-  ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), ARM_PRIMARY_CORE);

+  ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), PRIMARY_CORE_ID);

 

   // Jump to secondary core entry point.

   secondary_start ();

@@ -56,7 +56,7 @@ NonSecureWaitForFirmware (
 **/

 VOID

 ArmPlatformSecExtraAction (

-  IN  UINTN         CoreId,

+  IN  UINTN         MpId,

   OUT UINTN*        JumpAddress

   )

 {

@@ -64,7 +64,7 @@ ArmPlatformSecExtraAction (
   UINTN           CharCount;

 

   if (FeaturePcdGet (PcdStandalone) == FALSE) {

-    if (CoreId == ARM_PRIMARY_CORE) {

+    if (IS_PRIMARY_CORE(MpId)) {

       UINTN*   StartAddress = (UINTN*)PcdGet32(PcdNormalFvBaseAddress);

 

       // Patch the DRAM to make an infinite loop at the start address

@@ -85,7 +85,7 @@ ArmPlatformSecExtraAction (
       *JumpAddress = (UINTN)NonSecureWaitForFirmware;

     }

   } else if (FeaturePcdGet (PcdSystemMemoryInitializeInSec)) {

-    if (CoreId == ARM_PRIMARY_CORE) {

+    if (IS_PRIMARY_CORE(MpId)) {

       // Signal the secondary cores they can jump to PEI phase

       ArmGicSendSgiTo (PcdGet32(PcdGicDistributorBase), ARM_GIC_ICDSGIR_FILTER_EVERYONEELSE, 0x0E);

 

diff --git a/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.inf b/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.inf
index 2a09063a5..93249cfe3 100755
--- a/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.inf
+++ b/ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.inf
@@ -47,5 +47,8 @@
 [FixedPcd]
   gArmTokenSpaceGuid.PcdNormalFvBaseAddress
   
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
+  gArmTokenSpaceGuid.PcdArmPrimaryCore
+
   gArmTokenSpaceGuid.PcdGicDistributorBase
   gArmTokenSpaceGuid.PcdGicInterruptInterfaceBase
diff --git a/ArmPlatformPkg/PrePeiCore/MainMPCore.c b/ArmPlatformPkg/PrePeiCore/MainMPCore.c
index a369c6e49..036b19206 100644
--- a/ArmPlatformPkg/PrePeiCore/MainMPCore.c
+++ b/ArmPlatformPkg/PrePeiCore/MainMPCore.c
@@ -32,7 +32,7 @@ extern EFI_PEI_PPI_DESCRIPTOR *gSecPpiTable;
 VOID

 EFIAPI

 SecondaryMain (

-  IN UINTN CoreId

+  IN UINTN MpId

   )

 {

   // Function pointer to Secondary Core entry point

@@ -45,7 +45,7 @@ SecondaryMain (
   while (secondary_entry_addr = ArmGetMPCoreMailbox(), secondary_entry_addr == 0) {

     ArmCallWFI();

     // Acknowledge the interrupt and send End of Interrupt signal.

-    ArmGicAcknowledgeSgiFrom(PcdGet32(PcdGicInterruptInterfaceBase),0/*CoreId*/);

+    ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), PRIMARY_CORE_ID);

   }

 

   secondary_start = (VOID (*)())secondary_entry_addr;

diff --git a/ArmPlatformPkg/PrePeiCore/MainUniCore.c b/ArmPlatformPkg/PrePeiCore/MainUniCore.c
index 4d8331219..179e1ea34 100644
--- a/ArmPlatformPkg/PrePeiCore/MainUniCore.c
+++ b/ArmPlatformPkg/PrePeiCore/MainUniCore.c
@@ -21,7 +21,7 @@ extern EFI_PEI_PPI_DESCRIPTOR *gSecPpiTable;
 VOID

 EFIAPI

 SecondaryMain (

-  IN UINTN CoreId

+  IN UINTN MpId

   )

 {

   ASSERT(FALSE);

diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCore.c b/ArmPlatformPkg/PrePeiCore/PrePeiCore.c
index 0b7f973d5..c557efebc 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCore.c
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCore.c
@@ -36,7 +36,7 @@ EFI_PEI_PPI_DESCRIPTOR      gSecPpiTable[] = {
 

 VOID

 CEntryPoint (

-  IN  UINTN                     CoreId,

+  IN  UINTN                     MpId,

   IN  EFI_PEI_CORE_ENTRY_POINT  PeiCoreEntryPoint

   )

 {

@@ -62,7 +62,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(0 == CoreId) {

+  if (IS_PRIMARY_CORE(MpId)) {

     // Initialize the Debug Agent for Source Level Debugging

     InitializeDebugAgent (DEBUG_AGENT_INIT_POSTMEM_SEC, NULL, NULL);

     SaveAndSetDebugTimerInterrupt (TRUE);

@@ -70,7 +70,7 @@ CEntryPoint (
     // Goto primary Main.

     PrimaryMain (PeiCoreEntryPoint);

   } else {

-    SecondaryMain (CoreId);

+    SecondaryMain (MpId);

   }

 

   // PEI Core should always load and never return

diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCore.h b/ArmPlatformPkg/PrePeiCore/PrePeiCore.h
index 7a369b4f6..05198b829 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCore.h
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCore.h
@@ -57,7 +57,7 @@ PrimaryMain (
 VOID

 EFIAPI

 SecondaryMain (

-  IN UINTN CoreId

+  IN UINTN MpId

   );

 

 #endif

diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.S b/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.S
index 49174cc94..01723a2ea 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.S
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.S
@@ -19,19 +19,18 @@
 .text

 .align 3

 

-#global symbols referenced by this module

 GCC_ASM_IMPORT(CEntryPoint)

-

-StartupAddr: .word    CEntryPoint

-

-#make _ModuleEntryPoint as global

+GCC_ASM_IMPORT(ArmReadMpidr)

 GCC_ASM_EXPORT(_ModuleEntryPoint)

 

+StartupAddr: .word    CEntryPoint

 

 ASM_PFX(_ModuleEntryPoint):

   # Identify CPU ID

-  mrc   p15, 0, r0, c0, c0, 5

-  and   r0, #0xf

+  bl    ASM_PFX(ArmReadMpidr)

+  // Get ID of this CPU in Multicore system

+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)

+  and   r0, r0, r1

 

 _SetupStack:

   # Setup Stack for the 4 CPU cores

diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm b/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm
index e214ed374..e4eb61261 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm
@@ -19,6 +19,7 @@
   INCLUDE AsmMacroIoLib.inc

   

   IMPORT  CEntryPoint

+  IMPORT  ArmReadMpidr

   EXPORT  _ModuleEntryPoint

   

   PRESERVE8

@@ -28,8 +29,10 @@ StartupAddr        DCD      CEntryPoint
 

 _ModuleEntryPoint

   // Identify CPU ID

-  mrc   p15, 0, r0, c0, c0, 5

-  and   r0, #0xf

+  bl    ArmReadMpidr

+  // Get ID of this CPU in Multicore system

+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)

+  and   r0, r0, r1

 

 _SetupStack

   // Setup Stack for the 4 CPU cores

@@ -62,7 +65,7 @@ _PrepareArguments
   ldr   r2, StartupAddr

 

   // jump to PrePeiCore C code

-  //    r0 = core_id

+  //    r0 = mp_id

   //    r1 = pei_core_address

   blx   r2

 

diff --git a/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf b/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf
index c2506ab54..25dae0fb9 100644
--- a/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf
+++ b/ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf
@@ -58,6 +58,9 @@
   gArmTokenSpaceGuid.PcdNormalFvBaseAddress

   gArmTokenSpaceGuid.PcdNormalFvSize

 

+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask

+  gArmTokenSpaceGuid.PcdArmPrimaryCore

+

   gArmPlatformTokenSpaceGuid.PcdCPUCoresNonSecStackBase

   gArmPlatformTokenSpaceGuid.PcdCPUCoresNonSecStackSize

   

diff --git a/ArmPlatformPkg/PrePi/MainMPCore.c b/ArmPlatformPkg/PrePi/MainMPCore.c
index 82d31905f..2f1f86a4b 100644
--- a/ArmPlatformPkg/PrePi/MainMPCore.c
+++ b/ArmPlatformPkg/PrePi/MainMPCore.c
@@ -41,7 +41,7 @@ PrimaryMain (
 

 VOID

 SecondaryMain (

-  IN  UINTN                     CoreId

+  IN  UINTN                     MpId

   )

 {

   // Function pointer to Secondary Core entry point

@@ -54,7 +54,7 @@ SecondaryMain (
   while (secondary_entry_addr = ArmGetMPCoreMailbox(), secondary_entry_addr == 0) {

     ArmCallWFI();

     // Acknowledge the interrupt and send End of Interrupt signal.

-    ArmGicAcknowledgeSgiFrom(PcdGet32(PcdGicInterruptInterfaceBase),0/*CoreId*/);

+    ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), PRIMARY_CORE_ID);

   }

 

   secondary_start = (VOID (*)())secondary_entry_addr;

diff --git a/ArmPlatformPkg/PrePi/MainUniCore.c b/ArmPlatformPkg/PrePi/MainUniCore.c
index c92d9a55b..f1dff8af3 100644
--- a/ArmPlatformPkg/PrePi/MainUniCore.c
+++ b/ArmPlatformPkg/PrePi/MainUniCore.c
@@ -28,7 +28,7 @@ PrimaryMain (
 

 VOID

 SecondaryMain (

-  IN  UINTN                     CoreId

+  IN  UINTN                     MpId

   )

 {

   // We must never get into this function on UniCore system

diff --git a/ArmPlatformPkg/PrePi/ModuleEntryPoint.S b/ArmPlatformPkg/PrePi/ModuleEntryPoint.S
index a8c779fba..4d64aa7e8 100755
--- a/ArmPlatformPkg/PrePi/ModuleEntryPoint.S
+++ b/ArmPlatformPkg/PrePi/ModuleEntryPoint.S
@@ -21,15 +21,17 @@
 

 # Global symbols referenced by this module

 GCC_ASM_IMPORT(CEntryPoint)

+GCC_ASM_IMPORT(ArmReadMpidr)

 GCC_ASM_EXPORT(_ModuleEntryPoint)

 

 StartupAddr: .word    CEntryPoint

 

 

 ASM_PFX(_ModuleEntryPoint):

-  // Identify CPU ID

-  mrc   p15, 0, r0, c0, c0, 5

-  and   r0, #0xf

+  // Get ID of this CPU in Multicore system

+  bl    ASM_PFX(ArmReadMpidr)

+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)

+  and   r0, r0, r1

 

 _SetSVCMode:

   // Enter SVC mode

@@ -94,7 +96,7 @@ _PrepareArguments:
   ldr   r2, StartupAddr

 

   // Jump to PrePiCore C code

-  //    r0 = core_id

+  //    r0 = MpId

   //    r1 = UefiMemoryBase

   blx   r2

 

diff --git a/ArmPlatformPkg/PrePi/ModuleEntryPoint.asm b/ArmPlatformPkg/PrePi/ModuleEntryPoint.asm
index 00abcb304..881871d34 100644
--- a/ArmPlatformPkg/PrePi/ModuleEntryPoint.asm
+++ b/ArmPlatformPkg/PrePi/ModuleEntryPoint.asm
@@ -19,6 +19,7 @@
   INCLUDE AsmMacroIoLib.inc

   

   IMPORT  CEntryPoint

+  IMPORT  ArmReadMpidr

   EXPORT  _ModuleEntryPoint

 

   PRESERVE8

@@ -27,9 +28,10 @@
 StartupAddr        DCD      CEntryPoint

 

 _ModuleEntryPoint

-  // Identify CPU ID

-  mrc   p15, 0, r0, c0, c0, 5

-  and   r0, #0xf

+  // Get ID of this CPU in Multicore system

+  bl    ArmReadMpidr

+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)

+  and   r5, r0, r1

 

 _SetSVCMode

   // Enter SVC mode

@@ -94,7 +96,7 @@ _PrepareArguments
   ldr   r2, StartupAddr

 

   // Jump to PrePiCore C code

-  //    r0 = core_id

+  //    r0 = MpId

   //    r1 = UefiMemoryBase

   blx   r2

 

diff --git a/ArmPlatformPkg/PrePi/PeiUniCore.inf b/ArmPlatformPkg/PrePi/PeiUniCore.inf
index 2439452d6..5882afc13 100755
--- a/ArmPlatformPkg/PrePi/PeiUniCore.inf
+++ b/ArmPlatformPkg/PrePi/PeiUniCore.inf
@@ -78,7 +78,10 @@
   gArmTokenSpaceGuid.PcdSystemMemorySize

   gArmPlatformTokenSpaceGuid.PcdSystemMemoryUefiRegionSize

   

-  gArmPlatformTokenSpaceGuid.PcdMPCoreMaxCores

+  gArmPlatformTokenSpaceGuid.PcdClusterCount

+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask

+  gArmTokenSpaceGuid.PcdArmPrimaryCore

+

   gEmbeddedTokenSpaceGuid.PcdPrePiCpuMemorySize

   gEmbeddedTokenSpaceGuid.PcdPrePiCpuIoSize

 

diff --git a/ArmPlatformPkg/PrePi/PrePi.c b/ArmPlatformPkg/PrePi/PrePi.c
index c4060d9b6..a0198d89b 100755
--- a/ArmPlatformPkg/PrePi/PrePi.c
+++ b/ArmPlatformPkg/PrePi/PrePi.c
@@ -136,13 +136,13 @@ PrePiMain (
 

 VOID

 CEntryPoint (

-  IN  UINTN                     CoreId,

+  IN  UINTN                     MpId,

   IN  UINTN                     UefiMemoryBase

   )

 {

   UINT64   StartTimeStamp;

  

-  if ((CoreId == ARM_PRIMARY_CORE) && PerformanceMeasurementEnabled ()) {

+  if (IS_PRIMARY_CORE(MpId) && PerformanceMeasurementEnabled ()) {

     // Initialize the Timer Library to setup the Timer HW controller

     TimerConstructor ();

     // We cannot call yet the PerformanceLib because the HOB List has not been initialized

@@ -168,11 +168,11 @@ CEntryPoint (
   ArmWriteVBar ((UINT32)PrePiVectorTable);

 

   // If not primary Jump to Secondary Main

-  if (CoreId == ARM_PRIMARY_CORE) {

+  if (IS_PRIMARY_CORE(MpId)) {

     // Goto primary Main.

     PrimaryMain (UefiMemoryBase, StartTimeStamp);

   } else {

-    SecondaryMain (CoreId);

+    SecondaryMain (MpId);

   }

 

   // DXE Core should always load and never return

diff --git a/ArmPlatformPkg/PrePi/PrePi.h b/ArmPlatformPkg/PrePi/PrePi.h
index f62e26357..9cf953a79 100644
--- a/ArmPlatformPkg/PrePi/PrePi.h
+++ b/ArmPlatformPkg/PrePi/PrePi.h
@@ -27,7 +27,6 @@
 

 #include <Chipset/ArmV7.h>

 

-#define ARM_PRIMARY_CORE  0

 #define SerialPrint(txt)  SerialPortWrite (txt, AsciiStrLen(txt)+1);

 

 // Vector Table for PrePi Phase

@@ -69,7 +68,7 @@ PrimaryMain (
 

 VOID

 SecondaryMain (

-  IN  UINTN                     CoreId

+  IN  UINTN                     MpId

   );

 

 // Either implemented by PrePiLib or by MemoryInitPei

diff --git a/ArmPlatformPkg/Sec/Sec.c b/ArmPlatformPkg/Sec/Sec.c
index 5610168df..6f4738fd4 100644
--- a/ArmPlatformPkg/Sec/Sec.c
+++ b/ArmPlatformPkg/Sec/Sec.c
@@ -26,8 +26,6 @@
 #include <Chipset/ArmV7.h>
 #include <Library/ArmGicLib.h>
 
-#define ARM_PRIMARY_CORE  0
-
 #define SerialPrint(txt)  SerialPortWrite ((UINT8*)txt, AsciiStrLen(txt)+1);
 
 extern VOID *monitor_vector_table;
@@ -66,7 +64,7 @@ copy_cpsr_into_spsr (
 
 VOID
 CEntryPoint (
-  IN  UINTN                     CoreId
+  IN  UINTN                     MpId
   )
 {
   CHAR8           Buffer[100];
@@ -74,7 +72,7 @@ CEntryPoint (
   UINTN           JumpAddress;
 
   // Primary CPU clears out the SCU tag RAMs, secondaries wait
-  if (CoreId == ARM_PRIMARY_CORE) {
+  if (IS_PRIMARY_CORE(MpId)) {
     if (FixedPcdGet32(PcdMPCoreSupport)) {
       ArmInvalidScu();
     }
@@ -118,7 +116,7 @@ CEntryPoint (
     ArmEnableVFP();
   }
 
-  if (CoreId == ARM_PRIMARY_CORE) {
+  if (IS_PRIMARY_CORE(MpId)) {
     // Initialize peripherals that must be done at the early stage
     // Example: Some L2x0 controllers must be initialized in Secure World
     ArmPlatformSecInitialize ();
@@ -138,18 +136,18 @@ CEntryPoint (
   if (ArmPlatformTrustzoneSupported()) {
     if (FixedPcdGet32(PcdMPCoreSupport)) {
       // Setup SMP in Non Secure world
-      ArmSetupSmpNonSecure (CoreId);
+      ArmSetupSmpNonSecure (GET_CORE_ID(MpId));
     }
 
     // Enter Monitor Mode
-    enter_monitor_mode((VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * CoreId)));
+    enter_monitor_mode ((VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * GET_CORE_POS(MpId))));
 
     //Write the monitor mode vector table address
     ArmWriteVMBar((UINT32) &monitor_vector_table);
 
     //-------------------- Monitor Mode ---------------------
     // Setup the Trustzone Chipsets
-    if (CoreId == ARM_PRIMARY_CORE) {
+    if (IS_PRIMARY_CORE(MpId)) {
       ArmPlatformTrustzoneInit();
 
       // Wake up the secondary cores by sending a interrupt to everyone else
@@ -193,12 +191,12 @@ CEntryPoint (
     // security state (SCR_AW), CPSR.F modified in any security state (SCR_FW)
     ArmWriteScr(SCR_NS | SCR_FW | SCR_AW);
   } else {
-    if (CoreId == ARM_PRIMARY_CORE) {
+    if (IS_PRIMARY_CORE(MpId)) {
       SerialPrint ("Trust Zone Configuration is disabled\n\r");
     }
 
     // Trustzone is not enabled, just enable the Distributor and CPU interface
-    if (CoreId == ARM_PRIMARY_CORE) {
+    if (IS_PRIMARY_CORE(MpId)) {
       ArmGicEnableDistributor (PcdGet32(PcdGicDistributorBase));
     }
     ArmGicEnableInterruptInterface (PcdGet32(PcdGicInterruptInterfaceBase));
@@ -210,7 +208,7 @@ CEntryPoint (
   }
 
   JumpAddress = PcdGet32 (PcdNormalFvBaseAddress);
-  ArmPlatformSecExtraAction (CoreId, &JumpAddress);
+  ArmPlatformSecExtraAction (MpId, &JumpAddress);
 
   return_from_exception (JumpAddress);
   //-------------------- Non Secure Mode ---------------------
diff --git a/ArmPlatformPkg/Sec/Sec.inf b/ArmPlatformPkg/Sec/Sec.inf
index bffa9b679..7b116c7fd 100644
--- a/ArmPlatformPkg/Sec/Sec.inf
+++ b/ArmPlatformPkg/Sec/Sec.inf
@@ -53,6 +53,9 @@
   gArmTokenSpaceGuid.PcdVFPEnabled
   gArmPlatformTokenSpaceGuid.PcdMPCoreSupport
   
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
+  gArmTokenSpaceGuid.PcdArmPrimaryCore
+  
   gArmTokenSpaceGuid.PcdNormalFvBaseAddress
   
   gArmPlatformTokenSpaceGuid.PcdCPUCoresSecStackBase
diff --git a/ArmPlatformPkg/Sec/SecEntryPoint.S b/ArmPlatformPkg/Sec/SecEntryPoint.S
index 8e5ea31f6..909f7fafb 100644
--- a/ArmPlatformPkg/Sec/SecEntryPoint.S
+++ b/ArmPlatformPkg/Sec/SecEntryPoint.S
@@ -35,6 +35,7 @@ GCC_ASM_IMPORT(ArmPlatformInitializeBootMemory)
 GCC_ASM_IMPORT(ArmDisableInterrupts)

 GCC_ASM_IMPORT(ArmDisableCachesAndMmu)

 GCC_ASM_IMPORT(ArmWriteVBar)

+GCC_ASM_IMPORT(ArmReadMpidr)

 GCC_ASM_IMPORT(SecVectorTable)

 

 #if (FixedPcdGet32(PcdMPCoreSupport))

@@ -58,10 +59,13 @@ ASM_PFX(_ModuleEntryPoint):
 _IdentifyCpu: 

   # Identify CPU ID

   bl    ASM_PFX(ArmReadMpidr)

-  and   r5, r0, #0xf

+  // Get ID of this CPU in Multicore system

+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)

+  and   r5, r0, r1

   

   #get ID of this CPU in Multicore system

-  cmp   r5, #0

+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r1)

+  cmp   r5, r1

   # Only the primary core initialize the memory (SMC)

   beq   _InitMem

   

diff --git a/ArmPlatformPkg/Sec/SecEntryPoint.asm b/ArmPlatformPkg/Sec/SecEntryPoint.asm
index 794a8c02d..b291e5062 100644
--- a/ArmPlatformPkg/Sec/SecEntryPoint.asm
+++ b/ArmPlatformPkg/Sec/SecEntryPoint.asm
@@ -52,10 +52,13 @@ _ModuleEntryPoint
 _IdentifyCpu 
   // Identify CPU ID
   bl    ArmReadMpidr
-  and   r5, r0, #0xf
+  // Get ID of this CPU in Multicore system
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)
+  and   r5, r0, r1
   
-  //get ID of this CPU in Multicore system
-  cmp   r5, #0
+  // Is it the Primary Core ?
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r1)
+  cmp   r5, r1
   // Only the primary core initialize the memory (SMC)
   beq   _InitMem
   
@@ -97,7 +100,7 @@ _SetupStack
   ldr   r3, StartupAddr
   
   // Jump to SEC C code
-  //    r0 = core_id
+  //    r0 = mp_id
   mov   r0, r5
   blx   r3