diff options
24 files changed, 95 insertions, 60 deletions
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)
-
-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 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 |