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

4 files changed, 94 insertions, 50 deletions

diff --git a/ArmPlatformPkg/Sec/Helper.S b/ArmPlatformPkg/Sec/Helper.S
index 50cd733f97..9e3b13b38f 100644
--- a/ArmPlatformPkg/Sec/Helper.S
+++ b/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     r1, cpsr                    @ Save current mode (SVC) in r1

-    bic     r3, r1, #0x1f               @ Clear all mode bits

+    mrs     r4, cpsr                    @ Save current mode (SVC) in r1

+    bic     r3, r4, #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

-    mov     lr, r2                      @ Use the same lr as before

+    cmp     r2, #0                      @ If a Secure Monitor stack base has been passed, used it

+    movne   sp, r2                      @ Use the passed sp

+

+    mov     lr, r0                      @ Use the pass entrypoint as lr

     

-    msr     spsr_cxsf, r1               @ Use saved mode for the MOVS jump to the kernel

-    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

diff --git a/ArmPlatformPkg/Sec/Helper.asm b/ArmPlatformPkg/Sec/Helper.asm
index 1ba7b434e7..15b8f47522 100644
--- a/ArmPlatformPkg/Sec/Helper.asm
+++ b/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     r1, cpsr                    // Save current mode (SVC) in r1

-    bic     r3, r1, #0x1f               // Clear all mode bits

+    mrs     r4, cpsr                    // Save current mode (SVC) in r1

+    bic     r3, r4, #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

-    mov     lr, r2                      // Use the same lr as before

+    cmp     r2, #0                      // If a Secure Monitor stack base has been passed, used it

+    movne   sp, r2                      // Use the passed sp

+

+    mov     lr, r0                      // Use the pass entrypoint as lr

     

-    msr     spsr_cxsf, r1               // Use saved mode for the MOVS jump to the kernel

-    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

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);
diff --git a/ArmPlatformPkg/Sec/SecInternal.h b/ArmPlatformPkg/Sec/SecInternal.h
index cf9c1707d5..badd004b34 100644
--- a/ArmPlatformPkg/Sec/SecInternal.h
+++ b/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

@@ -27,9 +27,19 @@
 #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,

-  IN  INTN          GicInterruptInterfaceBase

+  IN  INTN                  GicDistributorBase,

+  IN  INTN                  GicInterruptInterfaceBase

 );

 

 // Vector Table for Sec Phase

@@ -45,12 +55,14 @@ NonSecureWaitForFirmware (
 

 VOID

 enter_monitor_mode (

-  IN VOID* Stack

+  IN UINTN                  MonitorEntryPoint,

+  IN UINTN                  MpId,

+  IN VOID*                  Stack

   );

 

 VOID

 return_from_exception (

-  IN UINTN NonSecureBase

+  IN UINTN                  NonSecureBase

   );

 

 VOID

@@ -60,7 +72,7 @@ copy_cpsr_into_spsr (
 

 VOID

 set_non_secure_mode (

-  IN ARM_PROCESSOR_MODE              Mode

+  IN ARM_PROCESSOR_MODE     Mode

   );

 

 VOID