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,35 +110,7 @@ CEntryPoint ( } // Enter Monitor Mode - enter_monitor_mode ((VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * (GET_CORE_POS(MpId) + 1)))); - - //-------------------- Monitor Mode --------------------- - - // Set up Monitor World (Vector Table, etc) - ArmSecureMonitorWorldInitialize (); - - // Setup the Trustzone Chipsets - if (IS_PRIMARY_CORE(MpId)) { - ArmPlatformTrustzoneInit (); - - // 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); - } - - // Transfer the interrupt to Non-secure World - ArmGicSetupNonSecure (PcdGet32(PcdGicDistributorBase), PcdGet32(PcdGicInterruptInterfaceBase)); - - // Write to CP15 Non-secure Access Control Register - ArmWriteNsacr (PcdGet32 (PcdArmNsacr)); - - // CP15 Secure Configuration Register - ArmWriteScr (PcdGet32 (PcdArmScr)); + 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"); @@ -149,7 +120,56 @@ CEntryPoint ( // 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 (); + + 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); + } + + // Transfer the interrupt to Non-secure World + ArmGicSetupNonSecure (PcdGet32(PcdGicDistributorBase), PcdGet32(PcdGicInterruptInterfaceBase)); + + // Write to CP15 Non-secure Access Control Register + ArmWriteNsacr (PcdGet32 (PcdArmNsacr)); + + // 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 @@ -26,10 +26,20 @@ #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