1 files changed, 46 insertions, 26 deletions

diff --git a/ArmPlatformPkg/Sec/Sec.c b/ArmPlatformPkg/Sec/Sec.c
index 59012a1266..b79537f5f6 100644
--- a/ArmPlatformPkg/Sec/Sec.c
+++ b/ArmPlatformPkg/Sec/Sec.c
@@ -32,7 +32,6 @@ CEntryPoint (
 {
   CHAR8           Buffer[100];
   UINTN           CharCount;
-  UINTN           JumpAddress;
 
   // Invalidate the data cache. Doesn't have to do the Data cache clean.
   ArmInvalidateDataCache();
@@ -111,45 +110,66 @@ CEntryPoint (
     }
 
     // Enter Monitor Mode
-    enter_monitor_mode ((VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * (GET_CORE_POS(MpId) + 1))));
+    enter_monitor_mode ((UINTN)TrustedWorldInitialization, MpId, (VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * (GET_CORE_POS(MpId) + 1))));
+  } else {
+    if (IS_PRIMARY_CORE(MpId)) {
+      SerialPrint ("Trust Zone Configuration is disabled\n\r");
+    }
 
+    // With Trustzone support the transition from Sec to Normal world is done by return_from_exception().
+    // If we want to keep this function call we need to ensure the SVC's SPSR point to the same Program
+    // Status Register as the the current one (CPSR).
+    copy_cpsr_into_spsr ();
+
+    NonTrustedWorldTransition (MpId);
+  }
+  ASSERT (0); // We must never return from the above function
+}
+
+VOID
+TrustedWorldInitialization (
+  IN  UINTN                     MpId
+  )
+{
   //-------------------- Monitor Mode ---------------------
 
   // Set up Monitor World (Vector Table, etc)
   ArmSecureMonitorWorldInitialize ();
 
-    // Setup the Trustzone Chipsets
-    if (IS_PRIMARY_CORE(MpId)) {
-      ArmPlatformTrustzoneInit ();
+  // Setup the Trustzone Chipsets
+  if (IS_PRIMARY_CORE(MpId)) {
+    ArmPlatformTrustzoneInit ();
 
+    if (ArmIsMpCore()) {
       // Waiting for the Primary Core to have finished to initialize the Secure World
       ArmCpuSynchronizeSignal (ARM_CPU_EVENT_SECURE_INIT);
-    } else {
-      // The secondary cores need to wait until the Trustzone chipsets configuration is done
-      // before switching to Non Secure World
-
-      // Waiting for the Primary Core to have finished to initialize the Secure World
-      ArmCpuSynchronizeWait (ARM_CPU_EVENT_SECURE_INIT);
     }
+  } else {
+    // The secondary cores need to wait until the Trustzone chipsets configuration is done
+    // before switching to Non Secure World
 
-    // Transfer the interrupt to Non-secure World
-    ArmGicSetupNonSecure (PcdGet32(PcdGicDistributorBase), PcdGet32(PcdGicInterruptInterfaceBase));
+    // Waiting for the Primary Core to have finished to initialize the Secure World
+    ArmCpuSynchronizeWait (ARM_CPU_EVENT_SECURE_INIT);
+  }
 
-    // Write to CP15 Non-secure Access Control Register
-    ArmWriteNsacr (PcdGet32 (PcdArmNsacr));
+  // Transfer the interrupt to Non-secure World
+  ArmGicSetupNonSecure (PcdGet32(PcdGicDistributorBase), PcdGet32(PcdGicInterruptInterfaceBase));
 
-    // CP15 Secure Configuration Register
-    ArmWriteScr (PcdGet32 (PcdArmScr));
-  } else {
-    if (IS_PRIMARY_CORE(MpId)) {
-      SerialPrint ("Trust Zone Configuration is disabled\n\r");
-    }
+  // Write to CP15 Non-secure Access Control Register
+  ArmWriteNsacr (PcdGet32 (PcdArmNsacr));
 
-    // With Trustzone support the transition from Sec to Normal world is done by return_from_exception().
-    // If we want to keep this function call we need to ensure the SVC's SPSR point to the same Program
-    // Status Register as the the current one (CPSR).
-    copy_cpsr_into_spsr ();
-  }
+  // CP15 Secure Configuration Register
+  ArmWriteScr (PcdGet32 (PcdArmScr));
+
+  NonTrustedWorldTransition (MpId);
+}
+
+VOID
+NonTrustedWorldTransition (
+  IN  UINTN                     MpId
+  )
+{
+  UINTN           JumpAddress;
 
   JumpAddress = PcdGet32 (PcdFvBaseAddress);
   ArmPlatformSecExtraAction (MpId, &JumpAddress);