diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec
index 7801b98928..65a97574c0 100644
--- a/ArmPkg/ArmPkg.dec
+++ b/ArmPkg/ArmPkg.dec
@@ -97,6 +97,11 @@
   gArmTokenSpaceGuid.PcdSystemMemoryBase|0|UINT32|0x00000029
   gArmTokenSpaceGuid.PcdSystemMemorySize|0|UINT32|0x0000002A
 
+  # Use ClusterId + CoreId to identify the PrimaryCore
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask|0xF03|UINT32|0x00000031
+  # The Primary Core is ClusterId[0] & CoreId[0] 
+  gArmTokenSpaceGuid.PcdArmPrimaryCore|0|UINT32|0x00000037
+
   #
   # ARM MPCore MailBox PCDs
   #
diff --git a/ArmPkg/Include/Library/ArmLib.h b/ArmPkg/Include/Library/ArmLib.h
index 468e663989..89d915a8df 100644
--- a/ArmPkg/Include/Library/ArmLib.h
+++ b/ArmPkg/Include/Library/ArmLib.h
@@ -73,6 +73,14 @@ typedef enum {
   ARM_PROCESSOR_MODE_MASK       = 0x1F
 } ARM_PROCESSOR_MODE;
 
+#define IS_PRIMARY_CORE(MpId) (((MpId) & PcdGet32(PcdArmPrimaryCoreMask)) == PcdGet32(PcdArmPrimaryCore))
+#define GET_CORE_ID(MpId)     ((MpId) & 0x3)
+#define GET_CLUSTER_ID(MpId)  (((MpId) >> 6) & 0x3C)
+// Get the position of the core for the Stack Offset (4 Core per Cluster)
+//   Position = (ClusterId * 4) + CoreId
+#define GET_CORE_POS(MpId)    ((((MpId) >> 6) & 0x3C) + ((MpId) & 0x3))
+#define PRIMARY_CORE_ID       (PcdGet32(PcdArmPrimaryCore) & 0x3)
+
 ARM_CACHE_TYPE
 EFIAPI
 ArmCacheType (
diff --git a/ArmPlatformPkg/ArmRealViewEbPkg/Library/ArmRealViewEbLibRTSM/ArmRealViewEbSec.c b/ArmPlatformPkg/ArmRealViewEbPkg/Library/ArmRealViewEbLibRTSM/ArmRealViewEbSec.c
index 35a8738673..ee4a404da3 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 c49812560e..4ea640f0d2 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 b9812142ed..57390793f7 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 2a09063a54..93249cfe34 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 a369c6e49c..036b192064 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 4d8331219a..179e1ea344 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 0b7f973d53..c557efebce 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 7a369b4f6f..05198b8290 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 49174cc942..01723a2eac 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)
+GCC_ASM_IMPORT(ArmReadMpidr)
+GCC_ASM_EXPORT(_ModuleEntryPoint)
 
 StartupAddr: .word    CEntryPoint
 
-#make _ModuleEntryPoint as global
-GCC_ASM_EXPORT(_ModuleEntryPoint)
-
-
 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 e214ed3744..e4eb612610 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 c2506ab542..25dae0fb9e 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 82d31905fe..2f1f86a4b7 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 c92d9a55b3..f1dff8af37 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 a8c779fba9..4d64aa7e86 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 00abcb304a..881871d34e 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 2439452d61..5882afc13a 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 c4060d9b63..a0198d89b8 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 f62e263571..9cf953a796 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 5610168df1..6f4738fd4d 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 bffa9b6795..7b116c7fd9 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 8e5ea31f66..909f7fafbc 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 794a8c02d1..b291e5062f 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