ArmPlatformPkg/Sec: Fixed Monitor stack setup

[mirror_edk2.git] / ArmPlatformPkg / Sec / Sec.c

diff --git a/ArmPlatformPkg/Sec/Sec.c b/ArmPlatformPkg/Sec/Sec.c
index 241ad7cb16ff23451657a825318bab9e2d6ed8a6..a32cd3857e3975b56771cdb21c65f21f95455cb5 100644 (file)
--- a/ArmPlatformPkg/Sec/Sec.c
+++ b/ArmPlatformPkg/Sec/Sec.c
@@ -17,7 +17,6 @@
 #include <Library/PrintLib.h>
 #include <Library/BaseMemoryLib.h>
 #include <Library/SerialPortLib.h>
-#include <Library/ArmPlatformLib.h>
 #include <Library/ArmGicLib.h>
 #include <Library/ArmCpuLib.h>
 
@@ -36,10 +35,25 @@ CEntryPoint (
   UINTN           CharCount;
   UINTN           JumpAddress;
 
+  // Invalidate the data cache. Doesn't have to do the Data cache clean.
+  ArmInvalidateDataCache();
+
+  // Invalidate Instruction Cache
+  ArmInvalidateInstructionCache();
+
+  // Invalidate I & D TLBs
+  ArmInvalidateInstructionAndDataTlb();
+
+  // CPU specific settings
+  ArmCpuSetup (MpId);
+
+  // Enable Floating Point Coprocessor if supported by the platform
+  if (FixedPcdGet32 (PcdVFPEnabled)) {
+    ArmEnableVFP();
+  }
+       
   // Primary CPU clears out the SCU tag RAMs, secondaries wait
   if (IS_PRIMARY_CORE(MpId)) {
-    ArmCpuSetup (MpId);
-
     if (ArmIsMpCore()) {
       ArmCpuSynchronizeSignal (ARM_CPU_EVENT_BOOT_MEM_INIT);
     }
@@ -70,22 +84,9 @@ CEntryPoint (
     ArmGicEnableInterruptInterface (PcdGet32(PcdGicInterruptInterfaceBase));
   }
 
-  // Invalidate the data cache. Doesn't have to do the Data cache clean.
-  ArmInvalidateDataCache();
-
-  // Invalidate Instruction Cache
-  ArmInvalidateInstructionCache();
-
-  // Invalidate I & D TLBs
-  ArmInvalidateInstructionAndDataTlb();
-
   // Enable Full Access to CoProcessors
   ArmWriteCPACR (CPACR_CP_FULL_ACCESS);
 
-  if (FixedPcdGet32 (PcdVFPEnabled)) {
-    ArmEnableVFP();
-  }
-
   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
@@ -97,9 +98,6 @@ CEntryPoint (
       // Initialize system memory (DRAM)
       ArmPlatformInitializeSystemMemory ();
     }
-
-    // Some platform can change their physical memory mapping
-    ArmPlatformBootRemapping ();
   }
 
   // Test if Trustzone is supported on this platform
@@ -114,7 +112,7 @@ CEntryPoint (
     }
 
     // Enter Monitor Mode
-    enter_monitor_mode ((VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * GET_CORE_POS(MpId))));
+    enter_monitor_mode ((VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * (GET_CORE_POS(MpId) + 1))));
 
     //Write the monitor mode vector table address
     ArmWriteVMBar((UINT32) &monitor_vector_table);
@@ -137,16 +135,11 @@ CEntryPoint (
     // Transfer the interrupt to Non-secure World
     ArmGicSetupNonSecure (PcdGet32(PcdGicDistributorBase), PcdGet32(PcdGicInterruptInterfaceBase));
 
-    // Write to CP15 Non-secure Access Control Register :
-    //   - Enable CP10 and CP11 accesses in NS World
-    //   - Enable Access to Preload Engine in NS World
-    //   - Enable lockable TLB entries allocation in NS world
-    //   - Enable R/W access to SMP bit of Auxiliary Control Register in NS world
-    ArmWriteNsacr (NSACR_NS_SMP | NSACR_TL | NSACR_PLE | NSACR_CP(10) | NSACR_CP(11));
+    // Write to CP15 Non-secure Access Control Register
+    ArmWriteNsacr (PcdGet32 (PcdArmNsacr));
 
-    // CP15 Secure Configuration Register with Non Secure bit (SCR_NS), CPSR.A modified in any
-    // security state (SCR_AW), CPSR.F modified in any security state (SCR_FW)
-    ArmWriteScr (SCR_NS | SCR_FW | SCR_AW);
+    // CP15 Secure Configuration Register
+    ArmWriteScr (PcdGet32 (PcdArmScr));
   } else {
     if (IS_PRIMARY_CORE(MpId)) {
       SerialPrint ("Trust Zone Configuration is disabled\n\r");
@@ -161,6 +154,12 @@ CEntryPoint (
   JumpAddress = PcdGet32 (PcdFvBaseAddress);
   ArmPlatformSecExtraAction (MpId, &JumpAddress);
 
+  // If PcdArmNonSecModeTransition is defined then set this specific mode to CPSR before the transition
+  // By not set, the mode for Non Secure World is SVC
+  if (PcdGet32 (PcdArmNonSecModeTransition) != 0) {
+    set_non_secure_mode ((ARM_PROCESSOR_MODE)PcdGet32 (PcdArmNonSecModeTransition));
+  }
+
   return_from_exception (JumpAddress);
   //-------------------- Non Secure Mode ---------------------