ArmPlatformPkg: Introduce Primary core macros
authoroliviermartin <oliviermartin@6f19259b-4bc3-4df7-8a09-765794883524>
Thu, 22 Sep 2011 23:01:13 +0000 (23:01 +0000)
committeroliviermartin <oliviermartin@6f19259b-4bc3-4df7-8a09-765794883524>
Thu, 22 Sep 2011 23:01:13 +0000 (23:01 +0000)
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

24 files changed:
ArmPkg/ArmPkg.dec
ArmPkg/Include/Library/ArmLib.h
ArmPlatformPkg/ArmRealViewEbPkg/Library/ArmRealViewEbLibRTSM/ArmRealViewEbSec.c
ArmPlatformPkg/Include/Library/ArmPlatformLib.h
ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.c
ArmPlatformPkg/Library/DebugSecExtraActionLib/DebugSecExtraActionLib.inf
ArmPlatformPkg/PrePeiCore/MainMPCore.c
ArmPlatformPkg/PrePeiCore/MainUniCore.c
ArmPlatformPkg/PrePeiCore/PrePeiCore.c
ArmPlatformPkg/PrePeiCore/PrePeiCore.h
ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.S
ArmPlatformPkg/PrePeiCore/PrePeiCoreEntryPoint.asm
ArmPlatformPkg/PrePeiCore/PrePeiCoreMPCore.inf
ArmPlatformPkg/PrePi/MainMPCore.c
ArmPlatformPkg/PrePi/MainUniCore.c
ArmPlatformPkg/PrePi/ModuleEntryPoint.S
ArmPlatformPkg/PrePi/ModuleEntryPoint.asm
ArmPlatformPkg/PrePi/PeiUniCore.inf
ArmPlatformPkg/PrePi/PrePi.c
ArmPlatformPkg/PrePi/PrePi.h
ArmPlatformPkg/Sec/Sec.c
ArmPlatformPkg/Sec/Sec.inf
ArmPlatformPkg/Sec/SecEntryPoint.S
ArmPlatformPkg/Sec/SecEntryPoint.asm

index 7801b9892835f4b804029f5c632ab0c9c595eb1e..65a97574c045fd0b4dfb570a5cc8639dd968a50e 100644 (file)
   gArmTokenSpaceGuid.PcdSystemMemoryBase|0|UINT32|0x00000029\r
   gArmTokenSpaceGuid.PcdSystemMemorySize|0|UINT32|0x0000002A\r
 \r
+  # Use ClusterId + CoreId to identify the PrimaryCore\r
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask|0xF03|UINT32|0x00000031\r
+  # The Primary Core is ClusterId[0] & CoreId[0] \r
+  gArmTokenSpaceGuid.PcdArmPrimaryCore|0|UINT32|0x00000037\r
+\r
   #\r
   # ARM MPCore MailBox PCDs\r
   #\r
index 468e663989234fb42a6e0750c60846492a1cbcd0..89d915a8df761fb64a1b0e0ddbc39993196768e2 100644 (file)
@@ -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 (
index 35a87386732d759f76be81f246156255cba6212a..ee4a404da38147e3a94d2abb93732cfd74ca35b1 100755 (executable)
@@ -61,7 +61,7 @@ ArmPlatformSecInitialize (
 **/
 VOID
 ArmPlatformSecExtraAction (
-  IN  UINTN         CoreId,
+  IN  UINTN         MpId,
   OUT UINTN*        JumpAddress
   )
 {
index c49812560e65b20bda279b4e77048ac1db9fc8ee..4ea640f0d269317455a4fbb7d53888443225a78e 100644 (file)
@@ -103,7 +103,7 @@ ArmPlatformSecInitialize (
 **/
 VOID
 ArmPlatformSecExtraAction (
-  IN  UINTN         CoreId,
+  IN  UINTN         MpId,
   OUT UINTN*        JumpAddress
   );
 
index b9812142ed6013b23a56545fafbd53783ff81255..57390793f75d722cd053de4312ede53c05de6310 100755 (executable)
 \r
 #include <PiPei.h>\r
 \r
+#include <Library/ArmLib.h>\r
 #include <Library/ArmGicLib.h>\r
 #include <Library/DebugLib.h>\r
 #include <Library/PcdLib.h>\r
 #include <Library/PrintLib.h>\r
 #include <Library/SerialPortLib.h>\r
-#include <Chipset/ArmV7.h>
 \r
-#define ARM_PRIMARY_CORE    0\r
+#include <Chipset/ArmV7.h>
 \r
 // When the firmware is built as not Standalone, the secondary cores need to wait the firmware\r
 // entirely written into DRAM. It is the firmware from DRAM which will wake up the secondary cores.\r
@@ -38,7 +38,7 @@ NonSecureWaitForFirmware (
   ArmCallWFI();\r
 \r
   // Acknowledge the interrupt and send End of Interrupt signal.\r
-  ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), ARM_PRIMARY_CORE);\r
+  ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), PRIMARY_CORE_ID);\r
 \r
   // Jump to secondary core entry point.\r
   secondary_start ();\r
@@ -56,7 +56,7 @@ NonSecureWaitForFirmware (
 **/\r
 VOID\r
 ArmPlatformSecExtraAction (\r
-  IN  UINTN         CoreId,\r
+  IN  UINTN         MpId,\r
   OUT UINTN*        JumpAddress\r
   )\r
 {\r
@@ -64,7 +64,7 @@ ArmPlatformSecExtraAction (
   UINTN           CharCount;\r
 \r
   if (FeaturePcdGet (PcdStandalone) == FALSE) {\r
-    if (CoreId == ARM_PRIMARY_CORE) {\r
+    if (IS_PRIMARY_CORE(MpId)) {\r
       UINTN*   StartAddress = (UINTN*)PcdGet32(PcdNormalFvBaseAddress);\r
 \r
       // Patch the DRAM to make an infinite loop at the start address\r
@@ -85,7 +85,7 @@ ArmPlatformSecExtraAction (
       *JumpAddress = (UINTN)NonSecureWaitForFirmware;\r
     }\r
   } else if (FeaturePcdGet (PcdSystemMemoryInitializeInSec)) {\r
-    if (CoreId == ARM_PRIMARY_CORE) {\r
+    if (IS_PRIMARY_CORE(MpId)) {\r
       // Signal the secondary cores they can jump to PEI phase\r
       ArmGicSendSgiTo (PcdGet32(PcdGicDistributorBase), ARM_GIC_ICDSGIR_FILTER_EVERYONEELSE, 0x0E);\r
 \r
index 2a09063a54f97501e38ff97d681ea064efd663d6..93249cfe341741f39733ed4a67d405d1f7b9ded2 100755 (executable)
@@ -47,5 +47,8 @@
 [FixedPcd]
   gArmTokenSpaceGuid.PcdNormalFvBaseAddress
   
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
+  gArmTokenSpaceGuid.PcdArmPrimaryCore
+
   gArmTokenSpaceGuid.PcdGicDistributorBase
   gArmTokenSpaceGuid.PcdGicInterruptInterfaceBase
index a369c6e49cb1299c6e7d5ecb1302f44ca31a2313..036b1920648d1e9c4b9a21cb75a629b55faf5a61 100644 (file)
@@ -32,7 +32,7 @@ extern EFI_PEI_PPI_DESCRIPTOR *gSecPpiTable;
 VOID\r
 EFIAPI\r
 SecondaryMain (\r
-  IN UINTN CoreId\r
+  IN UINTN MpId\r
   )\r
 {\r
   // Function pointer to Secondary Core entry point\r
@@ -45,7 +45,7 @@ SecondaryMain (
   while (secondary_entry_addr = ArmGetMPCoreMailbox(), secondary_entry_addr == 0) {\r
     ArmCallWFI();\r
     // Acknowledge the interrupt and send End of Interrupt signal.\r
-    ArmGicAcknowledgeSgiFrom(PcdGet32(PcdGicInterruptInterfaceBase),0/*CoreId*/);\r
+    ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), PRIMARY_CORE_ID);\r
   }\r
 \r
   secondary_start = (VOID (*)())secondary_entry_addr;\r
index 4d8331219abd11a9ce45505110acc76deb5d4a02..179e1ea344d405a74722830e1c75162b67c5151c 100644 (file)
@@ -21,7 +21,7 @@ extern EFI_PEI_PPI_DESCRIPTOR *gSecPpiTable;
 VOID\r
 EFIAPI\r
 SecondaryMain (\r
-  IN UINTN CoreId\r
+  IN UINTN MpId\r
   )\r
 {\r
   ASSERT(FALSE);\r
index 0b7f973d53c94ef98f2c6c5b94b377c8f54ad94d..c557efebcea072e13e73df943b947f43c8c7c989 100644 (file)
@@ -36,7 +36,7 @@ EFI_PEI_PPI_DESCRIPTOR      gSecPpiTable[] = {
 \r
 VOID\r
 CEntryPoint (\r
-  IN  UINTN                     CoreId,\r
+  IN  UINTN                     MpId,\r
   IN  EFI_PEI_CORE_ENTRY_POINT  PeiCoreEntryPoint\r
   )\r
 {\r
@@ -62,7 +62,7 @@ CEntryPoint (
   //Note: The MMU will be enabled by MemoryPeim. Only the primary core will have the MMU on.\r
 \r
   //If not primary Jump to Secondary Main\r
-  if(0 == CoreId) {\r
+  if (IS_PRIMARY_CORE(MpId)) {\r
     // Initialize the Debug Agent for Source Level Debugging\r
     InitializeDebugAgent (DEBUG_AGENT_INIT_POSTMEM_SEC, NULL, NULL);\r
     SaveAndSetDebugTimerInterrupt (TRUE);\r
@@ -70,7 +70,7 @@ CEntryPoint (
     // Goto primary Main.\r
     PrimaryMain (PeiCoreEntryPoint);\r
   } else {\r
-    SecondaryMain (CoreId);\r
+    SecondaryMain (MpId);\r
   }\r
 \r
   // PEI Core should always load and never return\r
index 7a369b4f6f01268e1be3cf740c330d6a873f6f56..05198b8290e1ed508962cf386a8d220083351311 100644 (file)
@@ -57,7 +57,7 @@ PrimaryMain (
 VOID\r
 EFIAPI\r
 SecondaryMain (\r
-  IN UINTN CoreId\r
+  IN UINTN MpId\r
   );\r
 \r
 #endif\r
index 49174cc94207f2069b56f112a419b341265372bb..01723a2eac53ecfbd35ce91864270694188d8754 100644 (file)
 .text\r
 .align 3\r
 \r
-#global symbols referenced by this module\r
 GCC_ASM_IMPORT(CEntryPoint)\r
-\r
-StartupAddr: .word    CEntryPoint\r
-\r
-#make _ModuleEntryPoint as global\r
+GCC_ASM_IMPORT(ArmReadMpidr)\r
 GCC_ASM_EXPORT(_ModuleEntryPoint)\r
 \r
+StartupAddr: .word    CEntryPoint\r
 \r
 ASM_PFX(_ModuleEntryPoint):\r
   # Identify CPU ID\r
-  mrc   p15, 0, r0, c0, c0, 5\r
-  and   r0, #0xf\r
+  bl    ASM_PFX(ArmReadMpidr)\r
+  // Get ID of this CPU in Multicore system\r
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)\r
+  and   r0, r0, r1\r
 \r
 _SetupStack:\r
   # Setup Stack for the 4 CPU cores\r
index e214ed3744f755372abecc39d83b823d6430840e..e4eb612610d4052e5e21d2632c19b028d16af228 100644 (file)
@@ -19,6 +19,7 @@
   INCLUDE AsmMacroIoLib.inc\r
   \r
   IMPORT  CEntryPoint\r
+  IMPORT  ArmReadMpidr\r
   EXPORT  _ModuleEntryPoint\r
   \r
   PRESERVE8\r
@@ -28,8 +29,10 @@ StartupAddr        DCD      CEntryPoint
 \r
 _ModuleEntryPoint\r
   // Identify CPU ID\r
-  mrc   p15, 0, r0, c0, c0, 5\r
-  and   r0, #0xf\r
+  bl    ArmReadMpidr\r
+  // Get ID of this CPU in Multicore system\r
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)\r
+  and   r0, r0, r1\r
 \r
 _SetupStack\r
   // Setup Stack for the 4 CPU cores\r
@@ -62,7 +65,7 @@ _PrepareArguments
   ldr   r2, StartupAddr\r
 \r
   // jump to PrePeiCore C code\r
-  //    r0 = core_id\r
+  //    r0 = mp_id\r
   //    r1 = pei_core_address\r
   blx   r2\r
 \r
index c2506ab5426306d22f5f3532f38757ec545df0b3..25dae0fb9ede1c5d9e839dcbd69b7ddb862bcf0b 100644 (file)
@@ -58,6 +58,9 @@
   gArmTokenSpaceGuid.PcdNormalFvBaseAddress\r
   gArmTokenSpaceGuid.PcdNormalFvSize\r
 \r
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask\r
+  gArmTokenSpaceGuid.PcdArmPrimaryCore\r
+\r
   gArmPlatformTokenSpaceGuid.PcdCPUCoresNonSecStackBase\r
   gArmPlatformTokenSpaceGuid.PcdCPUCoresNonSecStackSize\r
   \r
index 82d31905feea94041ea5a7bfcdd553f83d8ea38e..2f1f86a4b72783dce1f51c8fc2c3445453a72773 100644 (file)
@@ -41,7 +41,7 @@ PrimaryMain (
 \r
 VOID\r
 SecondaryMain (\r
-  IN  UINTN                     CoreId\r
+  IN  UINTN                     MpId\r
   )\r
 {\r
   // Function pointer to Secondary Core entry point\r
@@ -54,7 +54,7 @@ SecondaryMain (
   while (secondary_entry_addr = ArmGetMPCoreMailbox(), secondary_entry_addr == 0) {\r
     ArmCallWFI();\r
     // Acknowledge the interrupt and send End of Interrupt signal.\r
-    ArmGicAcknowledgeSgiFrom(PcdGet32(PcdGicInterruptInterfaceBase),0/*CoreId*/);\r
+    ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), PRIMARY_CORE_ID);\r
   }\r
 \r
   secondary_start = (VOID (*)())secondary_entry_addr;\r
index c92d9a55b309d0a506e02cbbeefc8af14e56ceeb..f1dff8af37916bb731789856f2533025a2cff098 100644 (file)
@@ -28,7 +28,7 @@ PrimaryMain (
 \r
 VOID\r
 SecondaryMain (\r
-  IN  UINTN                     CoreId\r
+  IN  UINTN                     MpId\r
   )\r
 {\r
   // We must never get into this function on UniCore system\r
index a8c779fba914249191339e5748eec46acd86cb51..4d64aa7e86cb88edaff609fc72891399c957727a 100755 (executable)
 \r
 # Global symbols referenced by this module\r
 GCC_ASM_IMPORT(CEntryPoint)\r
+GCC_ASM_IMPORT(ArmReadMpidr)\r
 GCC_ASM_EXPORT(_ModuleEntryPoint)\r
 \r
 StartupAddr: .word    CEntryPoint\r
 \r
 \r
 ASM_PFX(_ModuleEntryPoint):\r
-  // Identify CPU ID\r
-  mrc   p15, 0, r0, c0, c0, 5\r
-  and   r0, #0xf\r
+  // Get ID of this CPU in Multicore system\r
+  bl    ASM_PFX(ArmReadMpidr)\r
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)\r
+  and   r0, r0, r1\r
 \r
 _SetSVCMode:\r
   // Enter SVC mode\r
@@ -94,7 +96,7 @@ _PrepareArguments:
   ldr   r2, StartupAddr\r
 \r
   // Jump to PrePiCore C code\r
-  //    r0 = core_id\r
+  //    r0 = MpId\r
   //    r1 = UefiMemoryBase\r
   blx   r2\r
 \r
index 00abcb304af1861ffa67fa3afe977dc536dc46b3..881871d34e67ac88eefdf30244fb04feaa8a5680 100644 (file)
@@ -19,6 +19,7 @@
   INCLUDE AsmMacroIoLib.inc\r
   \r
   IMPORT  CEntryPoint\r
+  IMPORT  ArmReadMpidr\r
   EXPORT  _ModuleEntryPoint\r
 \r
   PRESERVE8\r
 StartupAddr        DCD      CEntryPoint\r
 \r
 _ModuleEntryPoint\r
-  // Identify CPU ID\r
-  mrc   p15, 0, r0, c0, c0, 5\r
-  and   r0, #0xf\r
+  // Get ID of this CPU in Multicore system\r
+  bl    ArmReadMpidr\r
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)\r
+  and   r5, r0, r1\r
 \r
 _SetSVCMode\r
   // Enter SVC mode\r
@@ -94,7 +96,7 @@ _PrepareArguments
   ldr   r2, StartupAddr\r
 \r
   // Jump to PrePiCore C code\r
-  //    r0 = core_id\r
+  //    r0 = MpId\r
   //    r1 = UefiMemoryBase\r
   blx   r2\r
 \r
index 2439452d614520978195add6d3551cfcc28a75c3..5882afc13a9d8ba699e43509ae2e593b5cc2cc3e 100755 (executable)
   gArmTokenSpaceGuid.PcdSystemMemorySize\r
   gArmPlatformTokenSpaceGuid.PcdSystemMemoryUefiRegionSize\r
   \r
-  gArmPlatformTokenSpaceGuid.PcdMPCoreMaxCores\r
+  gArmPlatformTokenSpaceGuid.PcdClusterCount\r
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask\r
+  gArmTokenSpaceGuid.PcdArmPrimaryCore\r
+\r
   gEmbeddedTokenSpaceGuid.PcdPrePiCpuMemorySize\r
   gEmbeddedTokenSpaceGuid.PcdPrePiCpuIoSize\r
 \r
index c4060d9b6369543482edd7c7f54cd94f38aebb66..a0198d89b80c2acf648407af1ac29028db28109f 100755 (executable)
@@ -136,13 +136,13 @@ PrePiMain (
 \r
 VOID\r
 CEntryPoint (\r
-  IN  UINTN                     CoreId,\r
+  IN  UINTN                     MpId,\r
   IN  UINTN                     UefiMemoryBase\r
   )\r
 {\r
   UINT64   StartTimeStamp;\r
  \r
-  if ((CoreId == ARM_PRIMARY_CORE) && PerformanceMeasurementEnabled ()) {\r
+  if (IS_PRIMARY_CORE(MpId) && PerformanceMeasurementEnabled ()) {\r
     // Initialize the Timer Library to setup the Timer HW controller\r
     TimerConstructor ();\r
     // We cannot call yet the PerformanceLib because the HOB List has not been initialized\r
@@ -168,11 +168,11 @@ CEntryPoint (
   ArmWriteVBar ((UINT32)PrePiVectorTable);\r
 \r
   // If not primary Jump to Secondary Main\r
-  if (CoreId == ARM_PRIMARY_CORE) {\r
+  if (IS_PRIMARY_CORE(MpId)) {\r
     // Goto primary Main.\r
     PrimaryMain (UefiMemoryBase, StartTimeStamp);\r
   } else {\r
-    SecondaryMain (CoreId);\r
+    SecondaryMain (MpId);\r
   }\r
 \r
   // DXE Core should always load and never return\r
index f62e263571cb2fe179e1292272737568f5ff4495..9cf953a7966e096eefcbf50dc49f45ed11ba18bb 100644 (file)
@@ -27,7 +27,6 @@
 \r
 #include <Chipset/ArmV7.h>\r
 \r
-#define ARM_PRIMARY_CORE  0\r
 #define SerialPrint(txt)  SerialPortWrite (txt, AsciiStrLen(txt)+1);\r
 \r
 // Vector Table for PrePi Phase\r
@@ -69,7 +68,7 @@ PrimaryMain (
 \r
 VOID\r
 SecondaryMain (\r
-  IN  UINTN                     CoreId\r
+  IN  UINTN                     MpId\r
   );\r
 \r
 // Either implemented by PrePiLib or by MemoryInitPei\r
index 5610168df1ceaf1a5c96ceeb47048607a3ca485f..6f4738fd4dae57ca2ab0e9bbba023c4ec68fd3d1 100644 (file)
@@ -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 ---------------------
index bffa9b6795e440f09e18994059b898f5bbe01f1e..7b116c7fd97a5e72109c1c569dac9231f06c5c8e 100644 (file)
@@ -53,6 +53,9 @@
   gArmTokenSpaceGuid.PcdVFPEnabled
   gArmPlatformTokenSpaceGuid.PcdMPCoreSupport
   
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
+  gArmTokenSpaceGuid.PcdArmPrimaryCore
+  
   gArmTokenSpaceGuid.PcdNormalFvBaseAddress
   
   gArmPlatformTokenSpaceGuid.PcdCPUCoresSecStackBase
index 8e5ea31f66da875a730b65c5312a7ff23b5c3b1b..909f7fafbc3d3a5dd55c514751c105df06b3bc0e 100644 (file)
@@ -35,6 +35,7 @@ GCC_ASM_IMPORT(ArmPlatformInitializeBootMemory)
 GCC_ASM_IMPORT(ArmDisableInterrupts)\r
 GCC_ASM_IMPORT(ArmDisableCachesAndMmu)\r
 GCC_ASM_IMPORT(ArmWriteVBar)\r
+GCC_ASM_IMPORT(ArmReadMpidr)\r
 GCC_ASM_IMPORT(SecVectorTable)\r
 \r
 #if (FixedPcdGet32(PcdMPCoreSupport))\r
@@ -58,10 +59,13 @@ ASM_PFX(_ModuleEntryPoint):
 _IdentifyCpu: \r
   # Identify CPU ID\r
   bl    ASM_PFX(ArmReadMpidr)\r
-  and   r5, r0, #0xf\r
+  // Get ID of this CPU in Multicore system\r
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCoreMask), r1)\r
+  and   r5, r0, r1\r
   \r
   #get ID of this CPU in Multicore system\r
-  cmp   r5, #0\r
+  LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r1)\r
+  cmp   r5, r1\r
   # Only the primary core initialize the memory (SMC)\r
   beq   _InitMem\r
   \r
index 794a8c02d165f0673f0a70d10863ee978e0d49d5..b291e5062f3e725d527c7628a3e067b1477df1f6 100644 (file)
@@ -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