ArmPlatformPkg/Sec: Fix transition to Trusted Monitor World with ARMGCC

The enter_monitor_world() function was trashing r0/r1/r2 registers and then
was returning back to 'C'. The compiler might have used these registers in the C code.
These new design prevents register corruptions.



git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@13060 6f19259b-4bc3-4df7-8a09-765794883524

ArmPlatformPkg/Sec/Helper.S

@@ -20,20 +20,26 @@ GCC_ASM_EXPORT(enter_monitor_mode)
 GCC_ASM_EXPORT(copy_cpsr_into_spsr)
 GCC_ASM_EXPORT(set_non_secure_mode)
 
-# arg0: Secure Monitor mode stack
+# r0: Monitor World EntryPoint
+# r1: MpId
+# r2: Secure Monitor mode stack
 ASM_PFX(enter_monitor_mode):
-    mov     r2, lr                      @ Save current lr
+    mrs     r4, cpsr                    @ Save current mode (SVC) in r1
-
+    bic     r3, r4, #0x1f               @ Clear all mode bits
-    mrs     r1, cpsr                    @ Save current mode (SVC) in r1
-    bic     r3, r1, #0x1f               @ Clear all mode bits
     orr     r3, r3, #0x16               @ Set bits for Monitor mode
     msr     cpsr_cxsf, r3               @ We are now in Monitor Mode
 
-    mov     sp, r0                      @ Use the passed sp
+    cmp     r2, #0                      @ If a Secure Monitor stack base has been passed, used it
-    mov     lr, r2                      @ Use the same lr as before
+    movne   sp, r2                      @ Use the passed sp
 
-    msr     spsr_cxsf, r1               @ Use saved mode for the MOVS jump to the kernel
+    mov     lr, r0                      @ Use the pass entrypoint as lr
-    bx      lr
+    
+    msr     spsr_cxsf, r4               @ Use saved mode for the MOVS jump to the kernel
+
+    mov     r4, r0                      @ Swap EntryPoint and MpId registers
+    mov     r0, r1
+
+    bx      r4
 
 # We cannot use the instruction 'movs pc, lr' because the caller can be written either in ARM or Thumb2 assembler.
 # When we will jump into this function, we will set the CPSR flag to ARM assembler. By copying directly 'lr' into

ArmPlatformPkg/Sec/Helper.asm

@@ -18,20 +18,26 @@
     
     AREA   Helper, CODE, READONLY
 
-// arg0: Secure Monitor mode stack
+// r0: Monitor World EntryPoint
+// r1: MpId
+// r2: Secure Monitor mode stack
 enter_monitor_mode
-    mov     r2, lr                      // Save current lr
+    mrs     r4, cpsr                    // Save current mode (SVC) in r1
-
+    bic     r3, r4, #0x1f               // Clear all mode bits
-    mrs     r1, cpsr                    // Save current mode (SVC) in r1
-    bic     r3, r1, #0x1f               // Clear all mode bits
     orr     r3, r3, #0x16               // Set bits for Monitor mode
     msr     cpsr_cxsf, r3               // We are now in Monitor Mode
 
-    mov     sp, r0                      // Use the passed sp
+    cmp     r2, #0                      // If a Secure Monitor stack base has been passed, used it
-    mov     lr, r2                      // Use the same lr as before
+    movne   sp, r2                      // Use the passed sp
 
-    msr     spsr_cxsf, r1               // Use saved mode for the MOVS jump to the kernel
+    mov     lr, r0                      // Use the pass entrypoint as lr
-    bx      lr
+    
+    msr     spsr_cxsf, r4               // Use saved mode for the MOVS jump to the kernel
+
+    mov     r4, r0                      // Swap EntryPoint and MpId registers
+    mov     r0, r1
+
+    bx      r4
 
 // We cannot use the instruction 'movs pc, lr' because the caller can be written either in ARM or Thumb2 assembler.
 // When we will jump into this function, we will set the CPSR flag to ARM assembler. By copying directly 'lr' into

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,8 +110,27 @@ 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)
@@ -122,8 +140,10 @@ CEntryPoint (
   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
@@ -140,16 +160,16 @@ CEntryPoint (
 
   // CP15 Secure Configuration Register
   ArmWriteScr (PcdGet32 (PcdArmScr));
-  } else {
+
-    if (IS_PRIMARY_CORE(MpId)) {
+  NonTrustedWorldTransition (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().
+VOID
-    // If we want to keep this function call we need to ensure the SVC's SPSR point to the same Program
+NonTrustedWorldTransition (
-    // Status Register as the the current one (CPSR).
+  IN  UINTN                     MpId
-    copy_cpsr_into_spsr ();
+  )
-  }
+{
+  UINTN           JumpAddress;
 
   JumpAddress = PcdGet32 (PcdFvBaseAddress);
   ArmPlatformSecExtraAction (MpId, &JumpAddress);

ArmPlatformPkg/Sec/SecInternal.h

@@ -1,7 +1,7 @@
 /** @file
 *  Main file supporting the SEC Phase on ARM PLatforms
 *
-*  Copyright (c) 2011, ARM Limited. All rights reserved.
+*  Copyright (c) 2011-2012, ARM Limited. All rights reserved.
 *
 *  This program and the accompanying materials
 *  are licensed and made available under the terms and conditions of the BSD License
@@ -26,6 +26,16 @@
 
 #define IS_ALIGNED(Address, Align) (((UINTN)Address & (Align-1)) == 0)
 
+VOID
+TrustedWorldInitialization (
+  IN  UINTN                 MpId
+  );
+
+VOID
+NonTrustedWorldTransition (
+  IN  UINTN                 MpId
+  );
+
 VOID
 ArmSetupGicNonSecure (
   IN  INTN                  GicDistributorBase,
@@ -45,6 +55,8 @@ NonSecureWaitForFirmware (
 
 VOID
 enter_monitor_mode (
+  IN UINTN                  MonitorEntryPoint,
+  IN UINTN                  MpId,
   IN VOID*                  Stack
   );