Add ArmPlatformPkg from ARM Ltd. patch.

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

8 files changed, 897 insertions, 0 deletions

diff --git a/ArmPlatformPkg/Sec/Exception.S b/ArmPlatformPkg/Sec/Exception.S
new file mode 100644
index 0000000000..81e2659c1b
--- /dev/null
+++ b/ArmPlatformPkg/Sec/Exception.S
@@ -0,0 +1,106 @@
+//
+//  Copyright (c) 2011, 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         
+#  which accompanies this distribution.  The full text of the license may be found at        
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+#
+#
+
+#include <AsmMacroIoLib.h>
+#include <Base.h>
+#include <AutoGen.h>
+
+#start of the code section
+.text
+.align 5

+

+# IMPORT
+GCC_ASM_IMPORT(SecCommonExceptionEntry)
+
+# EXPORT
+GCC_ASM_EXPORT(SecVectorTable)
+

+//============================================================
+//Default Exception Handlers
+//============================================================
+  
+//FIXME: One of the EDK2 tool is broken. It does not look to respect the alignment. Even, if we specify 32-byte alignment for this file.
+Dummy1: .word      0

+Dummy2: .word      0

+  
+ASM_PFX(SecVectorTable):
+  b _DefaultResetHandler
+  b _DefaultUndefined
+  b _DefaultSWI
+  b _DefaultPrefetchAbort
+  b _DefaultDataAbort
+  b _DefaultReserved
+  b _DefaultIrq
+  b _DefaultFiq
+
+//
+// Default Exception handlers: There is no plan to return from any of these exceptions.
+// No context saving at all.
+//
+_DefaultResetHandler:
+  mov  r1, lr
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #0
+  blx  ASM_PFX(SecCommonExceptionEntry)
+
+_DefaultUndefined:
+  sub  r1, LR, #4
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #1
+  blx  ASM_PFX(SecCommonExceptionEntry)
+
+_DefaultSWI:
+  sub  r1, LR, #4
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #2
+  blx  ASM_PFX(SecCommonExceptionEntry)
+
+_DefaultPrefetchAbort:
+  sub  r1, LR, #4
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #3
+  blx  ASM_PFX(SecCommonExceptionEntry)
+
+_DefaultDataAbort:
+  sub  r1, LR, #8
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #4
+  blx  ASM_PFX(SecCommonExceptionEntry)
+
+_DefaultReserved:
+  mov  r1, lr
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #5
+  blx  SecCommonExceptionEntry
+
+_DefaultIrq:
+  sub  r1, LR, #4
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #6
+  blx  SecCommonExceptionEntry
+
+_DefaultFiq:
+  sub  r1, LR, #4
+  # Switch to SVC for common stack
+  cps  #0x13
+  mov  r0, #7
+  blx  SecCommonExceptionEntry
+

+.end

diff --git a/ArmPlatformPkg/Sec/Exception.asm b/ArmPlatformPkg/Sec/Exception.asm
new file mode 100644
index 0000000000..dcf2cf93b4
--- /dev/null
+++ b/ArmPlatformPkg/Sec/Exception.asm
@@ -0,0 +1,94 @@
+//

+//  Copyright (c) 2011, 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         

+//  which accompanies this distribution.  The full text of the license may be found at        

+//  http://opensource.org/licenses/bsd-license.php                                            

+//

+//  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     

+//  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             

+//

+//

+

+#include <AsmMacroIoLib.h>

+#include <Base.h>

+#include <AutoGen.h>

+

+  IMPORT  SecCommonExceptionEntry

+  EXPORT  SecVectorTable

+  

+  PRESERVE8

+  AREA    SecException, CODE, READONLY, CODEALIGN, ALIGN=5

+

+//============================================================
+//Default Exception Handlers
+//============================================================
+  
+//FIXME: One of the EDK2 tool is broken. It does not look to respect the alignment. Even, if we specify 32-byte alignment for this file.
+Dummy1        DCD      0

+Dummy2        DCD      0

+  
+SecVectorTable
+  b _DefaultResetHandler
+  b _DefaultUndefined
+  b _DefaultSWI
+  b _DefaultPrefetchAbort
+  b _DefaultDataAbort
+  b _DefaultReserved
+  b _DefaultIrq
+  b _DefaultFiq
+

+//
+// Default Exception handlers: There is no plan to return from any of these exceptions.

+// No context saving at all.

+//
+_DefaultResetHandler
+   mov  r1, lr

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #0
+   blx   SecCommonExceptionEntry
+
+_DefaultUndefined
+   sub  r1, LR

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #1
+   blx   SecCommonExceptionEntry
+
+_DefaultSWI
+   sub  r1, LR, #4

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #2
+   blx   SecCommonExceptionEntry
+
+_DefaultPrefetchAbort
+   sub  r1, LR, #4

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #3
+   blx   SecCommonExceptionEntry
+
+_DefaultDataAbort
+   sub  r1, LR, #8

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #4
+   blx   SecCommonExceptionEntry
+
+_DefaultReserved
+   mov  r1, lr

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #5
+   blx   SecCommonExceptionEntry
+   
+_DefaultIrq
+   sub  r1, LR, #4

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #6
+   blx   SecCommonExceptionEntry
+
+_DefaultFiq
+   sub  r1, LR, #4

+   cps       #0x13                     ; Switch to SVC for common stack
+   mov  r0, #7
+   blx   SecCommonExceptionEntry
+

+  END

diff --git a/ArmPlatformPkg/Sec/Helper.S b/ArmPlatformPkg/Sec/Helper.S
new file mode 100644
index 0000000000..94bd68f8bc
--- /dev/null
+++ b/ArmPlatformPkg/Sec/Helper.S
@@ -0,0 +1,74 @@
+#========================================================================================

+#  Copyright (c) 2011, 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         

+#  which accompanies this distribution.  The full text of the license may be found at        

+#  http:#opensource.org/licenses/bsd-license.php                                            

+#

+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     

+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             

+#

+#=======================================================================================

+

+#start of the code section

+.text 

+.align 3

+

+GCC_ASM_EXPORT(monitor_vector_table)

+GCC_ASM_EXPORT(return_from_exception)

+GCC_ASM_EXPORT(enter_monitor_mode)

+GCC_ASM_EXPORT(copy_cpsr_into_spsr)

+

+ASM_PFX(monitor_vector_table):

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+

+# arg0: 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

+    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

+    

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

+    bx      lr

+

+# 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

+# 'pc'; we will not change the CPSR flag and it will crash.

+# The way to fix this limitation is to do the movs into the ARM assmbler code and then do a 'bx'.

+ASM_PFX(return_from_exception):

+    ldr     lr, returned_exception

+

+    #The following instruction breaks the code.

+    #movs    pc, lr

+    mrs     r2, cpsr

+    bic     r2, r2, #0x1f

+    orr     r2, r2, #0x13

+    msr     cpsr_c, r2

+

+returned_exception:                           @ We are now in non-secure state

+    bx      r0

+

+# Save the current Program Status Register (PSR) into the Saved PSR

+ASM_PFX(copy_cpsr_into_spsr):

+    mrs     r0, cpsr

+    msr     spsr_cxsf, r0

+    bx      lr

+

+dead:

+    B       dead

+    

+.end

diff --git a/ArmPlatformPkg/Sec/Helper.asm b/ArmPlatformPkg/Sec/Helper.asm
new file mode 100644
index 0000000000..43a0749138
--- /dev/null
+++ b/ArmPlatformPkg/Sec/Helper.asm
@@ -0,0 +1,66 @@
+//

+//  Copyright (c) 2011, 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         

+//  which accompanies this distribution.  The full text of the license may be found at        

+//  http://opensource.org/licenses/bsd-license.php                                            

+//

+//  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     

+//  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             

+//

+//

+

+    EXPORT  monitor_vector_table

+    EXPORT  return_from_exception

+    EXPORT  enter_monitor_mode

+    EXPORT  copy_cpsr_into_spsr

+    

+    AREA   Helper, CODE, READONLY

+

+    ALIGN 32

+monitor_vector_table

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+    ldr pc, dead

+

+// arg0: 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

+    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

+    

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

+    bx      lr

+

+// 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

+// 'pc'; we will not change the CPSR flag and it will crash.

+// The way to fix this limitation is to do the movs into the ARM assmbler code and then do a 'bx'.

+return_from_exception

+    adr     lr, returned_exception

+    movs    pc, lr

+returned_exception                           // We are now in non-secure state

+    bx      r0

+

+// Save the current Program Status Register (PSR) into the Saved PSR

+copy_cpsr_into_spsr

+    mrs     r0, cpsr

+    msr     spsr_cxsf, r0

+    bx      lr

+

+dead

+    B       dead

+    

+    END

diff --git a/ArmPlatformPkg/Sec/Sec.c b/ArmPlatformPkg/Sec/Sec.c
new file mode 100644
index 0000000000..2ae01d8e54
--- /dev/null
+++ b/ArmPlatformPkg/Sec/Sec.c
@@ -0,0 +1,275 @@
+/** @file
+*  Main file supporting the SEC Phase for Versatile Express
+*
+*  Copyright (c) 2011, 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         
+*  which accompanies this distribution.  The full text of the license may be found at        
+*  http://opensource.org/licenses/bsd-license.php                                            
+*
+*  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+*  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+*
+**/
+
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/ArmLib.h>
+#include <Chipset/ArmV7.h>
+#include <Drivers/PL390Gic.h>
+#include <Library/L2X0CacheLib.h>
+#include <Library/SerialPortLib.h>
+#include <Library/ArmPlatformLib.h>
+
+extern VOID *monitor_vector_table;
+
+VOID ArmSetupGicNonSecure (
+        IN  INTN          GicDistributorBase,
+        IN  INTN          GicInterruptInterfaceBase
+);
+
+// Vector Table for Sec Phase
+VOID  SecVectorTable (VOID);
+
+VOID NonSecureWaitForFirmware (
+  VOID
+  );
+
+VOID
+enter_monitor_mode(
+  IN VOID* Stack
+  );
+
+VOID
+return_from_exception (
+  IN UINTN NonSecureBase
+  );
+
+VOID
+copy_cpsr_into_spsr (
+  VOID
+  );
+
+VOID
+CEntryPoint (
+  IN  UINTN                     CoreId
+  )
+{
+  // Primary CPU clears out the SCU tag RAMs, secondaries wait
+  if (CoreId == 0) {
+    if (FixedPcdGet32(PcdMPCoreSupport)) {
+      ArmInvalidScu();
+    }
+
+    // SEC phase needs to run library constructors by hand. This assumes we are linked against the SerialLib
+    // In non SEC modules the init call is in autogenerated code.
+    SerialPortInitialize ();
+    // Start talking
+    DEBUG ((EFI_D_ERROR, "UART Enabled\n"));
+
+    // Now we've got UART, make the check:
+    // - The Vector table must be 32-byte aligned
+    ASSERT(((UINT32)SecVectorTable & ((1 << 5)-1)) == 0);
+  }
+
+  // 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);
+
+  // Enable SWP instructions
+  ArmEnableSWPInstruction();
+
+  // Enable program flow prediction, if supported.
+  ArmEnableBranchPrediction();
+
+  if (FixedPcdGet32(PcdVFPEnabled)) {
+    ArmEnableVFP();
+  }
+
+  if (CoreId == 0) {
+    // Initialize L2X0 but not enabled
+    L2x0CacheInit(PcdGet32(PcdL2x0ControllerBase), FALSE);
+
+    // If we skip the PEI Core we could want to initialize the DRAM in the SEC phase.
+    // If we are in standalone, we need the initialization to copy the UEFI firmware into DRAM
+    if (FeaturePcdGet(PcdSkipPeiCore) || !FeaturePcdGet(PcdStandalone)) {
+      // Initialize system memory (DRAM)
+      ArmPlatformInitializeSystemMemory();
+    }
+
+    // Turn Off NOR flash remapping to 0. We can will now see DRAM in low memory
+    ArmPlatformBootRemapping();
+  }
+
+  // Test if Trustzone is supported on this platform
+  if (ArmPlatformTrustzoneSupported()) {
+    if (FixedPcdGet32(PcdMPCoreSupport)) {
+      // Setup SMP in Non Secure world
+      ArmSetupSmpNonSecure(CoreId);
+    }
+
+    // Enter Monitor Mode
+    enter_monitor_mode((VOID*)(PcdGet32(PcdCPUCoresSecMonStackBase) + (PcdGet32(PcdCPUCoreSecMonStackSize) * CoreId)));
+
+    //Write the monitor mode vector table address
+    ArmWriteVMBar((UINT32) &monitor_vector_table);
+
+    //-------------------- Monitor Mode ---------------------
+    // setup the Trustzone Chipsets
+    if (CoreId == 0) {
+      ArmPlatformTrustzoneInit();
+
+      // Wake up the secondary cores by sending a interrupt to everyone else
+      // NOTE 1: The Software Generated Interrupts are always enabled on Cortex-A9
+      //         MPcore test chip on Versatile Express board, So the Software doesn't have to
+      //         enable SGI's explicitly.
+      //      2: As no other Interrupts are enabled,  doesn't have to worry about the priority.
+      //      3: As all the cores are in secure state, use secure SGI's
+      //
+
+      PL390GicEnableDistributor (PcdGet32(PcdGicDistributorBase));
+      PL390GicEnableInterruptInterface(PcdGet32(PcdGicInterruptInterfaceBase));
+
+      // Send SGI to all Secondary core to wake them up from WFI state.
+      PL390GicSendSgiTo (PcdGet32(PcdGicDistributorBase), GIC_ICDSGIR_FILTER_EVERYONEELSE, 0x0E);
+    } else {
+      // The secondary cores need to wait until the Trustzone chipsets configuration is done
+      // before swtching to Non Secure World
+
+      // Enabled GIC CPU Interface
+      PL390GicEnableInterruptInterface (PcdGet32(PcdGicInterruptInterfaceBase));
+
+      // Waiting for the SGI from the primary core
+      ArmCallWFI();
+
+      //Acknowledge the interrupt and send End of Interrupt signal.
+      PL390GicAcknowledgeSgiFrom(PcdGet32(PcdGicInterruptInterfaceBase),0/*CoreId*/);
+    }
+
+    // Transfer the interrupt to Non-secure World
+    PL390GicSetupNonSecure(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));
+
+    // 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);
+  } else {
+    if(0 == CoreId){
+      DEBUG ((EFI_D_ERROR, "Trust Zone Configuration is disabled\n"));
+    }
+
+    //Trustzone is not enabled, just enable the Distributor and CPU interface
+    PL390GicEnableInterruptInterface(PcdGet32(PcdGicInterruptInterfaceBase));
+
+    // 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();
+  }
+
+  // If ArmVe has not been built as Standalone then we need to patch the DRAM to add an infinite loop at the start address
+  if (FeaturePcdGet(PcdStandalone) == FALSE) {
+    if (CoreId == 0) {
+      UINTN*   StartAddress = (UINTN*)PcdGet32(PcdEmbeddedFdBaseAddress);
+
+      DEBUG ((EFI_D_ERROR, "Waiting for firmware at 0x%08X ...\n",StartAddress));
+
+      // Patch the DRAM to make an infinite loop at the start address
+      *StartAddress = 0xEAFFFFFE; // opcode for while(1)
+
+      // To enter into Non Secure state, we need to make a return from exception
+      return_from_exception(PcdGet32(PcdEmbeddedFdBaseAddress));
+    } else {
+      // When the primary core is stopped by the hardware debugger to copy the firmware
+      // into DRAM. The secondary cores are still running. As soon as the first bytes of
+      // the firmware are written into DRAM, the secondary cores will start to execute the
+      // code even if the firmware is not entirely written into the memory.
+      // That's why the secondary cores need to be parked in WFI and wake up once the
+      // firmware is ready.
+
+      // Enter Secondary Cores into non Secure State. To enter into Non Secure state, we need to make a return from exception
+      return_from_exception((UINTN)NonSecureWaitForFirmware);
+    }
+  } else {
+    if (CoreId == 0) {
+      DEBUG ((EFI_D_ERROR, "Standalone Firmware\n"));
+    }
+
+    // To enter into Non Secure state, we need to make a return from exception
+    return_from_exception(PcdGet32(PcdEmbeddedFdBaseAddress));
+  }
+  //-------------------- Non Secure Mode ---------------------
+
+  // PEI Core should always load and never return
+  ASSERT (FALSE);
+}
+
+// When the firmware is built as not Standalone, the secondary cores need to wait the firmware
+// entirely written into DRAM. It is the firmware from DRAM which will wake up the secondary cores.
+VOID NonSecureWaitForFirmware() {
+  VOID (*secondary_start)(VOID);
+
+  // The secondary cores will execute the fimrware once wake from WFI.
+  secondary_start = (VOID (*)())PcdGet32(PcdEmbeddedFdBaseAddress);
+
+  ArmCallWFI();
+
+  //Acknowledge the interrupt and send End of Interrupt signal.
+  PL390GicAcknowledgeSgiFrom(PcdGet32(PcdGicInterruptInterfaceBase),0/*CoreId*/);
+
+  //Jump to secondary core entry point.
+  secondary_start();
+
+  // PEI Core should always load and never return
+  ASSERT (FALSE);
+}
+
+VOID SecCommonExceptionEntry(UINT32 Entry, UINT32 LR) {
+  switch (Entry) {
+  case 0:
+    DEBUG((EFI_D_ERROR,"Reset Exception at 0x%X\n",LR));
+    break;
+  case 1:
+    DEBUG((EFI_D_ERROR,"Undefined Exception at 0x%X\n",LR));
+    break;
+  case 2:
+    DEBUG((EFI_D_ERROR,"SWI Exception at 0x%X\n",LR));
+    break;
+  case 3:
+    DEBUG((EFI_D_ERROR,"PrefetchAbort Exception at 0x%X\n",LR));
+    break;
+  case 4:
+    DEBUG((EFI_D_ERROR,"DataAbort Exception at 0x%X\n",LR));
+    break;
+  case 5:
+    DEBUG((EFI_D_ERROR,"Reserved Exception at 0x%X\n",LR));
+    break;
+  case 6:
+    DEBUG((EFI_D_ERROR,"IRQ Exception at 0x%X\n",LR));
+    break;
+  case 7:
+    DEBUG((EFI_D_ERROR,"FIQ Exception at 0x%X\n",LR));
+    break;
+  default:
+    DEBUG((EFI_D_ERROR,"Unknown Exception at 0x%X\n",LR));
+    break;
+  }
+  while(1);
+}
diff --git a/ArmPlatformPkg/Sec/Sec.inf b/ArmPlatformPkg/Sec/Sec.inf
new file mode 100644
index 0000000000..7180e1a5ad
--- /dev/null
+++ b/ArmPlatformPkg/Sec/Sec.inf
@@ -0,0 +1,66 @@
+#/** @file
+#  SEC - Reset vector code that jumps to C and loads DXE core
+#  
+#  Copyright (c) 2011, 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         
+#  which accompanies this distribution.  The full text of the license may be found at        
+#  http://opensource.org/licenses/bsd-license.php                                            
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+#  
+#**/
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ArmPlatformSec
+  FILE_GUID                      = c536bbfe-c813-4e48-9f90-01fe1ecf9d54
+  MODULE_TYPE                    = SEC
+  VERSION_STRING                 = 1.0
+
+[Sources.ARM]
+  Helper.asm           | RVCT
+  Helper.S             | GCC
+  Sec.c
+  SecEntryPoint.S      | GCC
+  SecEntryPoint.asm    | RVCT
+  Exception.asm        | RVCT
+  Exception.S          | GCC
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  EmbeddedPkg/EmbeddedPkg.dec
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  IoLib
+  ArmLib
+  ArmPlatformLib
+  L2X0CacheLib
+  PL390GicSecLib
+  
+[FeaturePcd]
+  gArmPlatformTokenSpaceGuid.PcdStandalone
+  gArmTokenSpaceGuid.PcdSkipPeiCore
+  
+[FixedPcd]
+  gArmTokenSpaceGuid.PcdVFPEnabled
+  gArmPlatformTokenSpaceGuid.PcdMPCoreSupport
+  
+  gEmbeddedTokenSpaceGuid.PcdEmbeddedFdBaseAddress
+  
+  gArmPlatformTokenSpaceGuid.PcdCPUCoresSecStackBase
+  gArmPlatformTokenSpaceGuid.PcdCPUCoreSecStackSize
+  gArmPlatformTokenSpaceGuid.PcdCPUCoresSecMonStackBase
+  gArmPlatformTokenSpaceGuid.PcdCPUCoreSecMonStackSize
+  
+  gArmTokenSpaceGuid.PcdL2x0ControllerBase
+  
+  gArmTokenSpaceGuid.PcdGicDistributorBase
+  gArmTokenSpaceGuid.PcdGicInterruptInterfaceBase
diff --git a/ArmPlatformPkg/Sec/SecEntryPoint.S b/ArmPlatformPkg/Sec/SecEntryPoint.S
new file mode 100644
index 0000000000..e7d7160b87
--- /dev/null
+++ b/ArmPlatformPkg/Sec/SecEntryPoint.S
@@ -0,0 +1,112 @@
+#------------------------------------------------------------------------------ 
+#
+# ARM VE Entry point. Reset vector in FV header will brach to
+# _ModuleEntryPoint. 
+#
+#  Copyright (c) 2011, 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         
+#  which accompanies this distribution.  The full text of the license may be found at        
+#  http://opensource.org/licenses/bsd-license.php                                            
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+#
+#------------------------------------------------------------------------------
+
+#include <AsmMacroIoLib.h>
+#include <Base.h>
+#include <Library/PcdLib.h>
+#include <Library/ArmPlatformLib.h>
+#include <AutoGen.h>
+
+#Start of Code section
+.text
+.align 3
+
+#make _ModuleEntryPoint as global
+GCC_ASM_EXPORT(_ModuleEntryPoint)
+
+#global functions referenced by this module
+GCC_ASM_IMPORT(CEntryPoint)
+GCC_ASM_IMPORT(ArmPlatformIsMemoryInitialized)
+GCC_ASM_IMPORT(ArmPlatformInitializeBootMemory)
+GCC_ASM_IMPORT(ArmDisableInterrupts)
+GCC_ASM_IMPORT(ArmDisableCachesAndMmu)
+GCC_ASM_IMPORT(ArmWriteVBar)
+GCC_ASM_IMPORT(SecVectorTable)
+
+#if (FixedPcdGet32(PcdMPCoreSupport))
+GCC_ASM_IMPORT(ArmIsScuEnable)
+#endif
+
+StartupAddr: .word    		CEntryPoint
+SecVectorTableAddr: .word		SecVectorTable
+
+ASM_PFX(_ModuleEntryPoint):
+ #Set VBAR to the start of the exception vectors in Secure Mode
+  ldr   r0, SecVectorTableAddr
+  bl   ASM_PFX(ArmWriteVBar)
+
+  # First ensure all interrupts are disabled
+  bl   ASM_PFX(ArmDisableInterrupts)
+
+  # Ensure that the MMU and caches are off
+  bl  ASM_PFX(ArmDisableCachesAndMmu)
+
+_IdentifyCpu: 
+  # Identify CPU ID
+  bl    ASM_PFX(ArmReadMpidr)
+  and   r5, r0, #0xf
+  
+  #get ID of this CPU in Multicore system
+  cmp   r5, #0
+  # Only the primary core initialize the memory (SMC)
+  beq   _InitMem
+  
+#if (FixedPcdGet32(PcdMPCoreSupport))
+  # ... The secondary cores wait for SCU to be enabled
+_WaitForEnabledScu:
+  bl    ASM_PFX(ArmIsScuEnable)
+  tst   r1, #1
+  beq   _WaitForEnabledScu
+  b     _SetupStack
+#endif
+  
+_InitMem:
+  bl    ASM_PFX(ArmPlatformIsMemoryInitialized)
+  bne   _SetupStack
+  
+  # Initialize Init Memory
+  bl    ASM_PFX(ArmPlatformInitializeBootMemory)
+
+  # Only Primary CPU could run this line (the secondary cores have jumped from _IdentifyCpu to _SetupStack)
+  mov   r5, #0
+ 
+_SetupStack:
+  # Setup Stack for the 4 CPU cores
+  #Read Stack Base address from PCD
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoresSecStackBase) ,r1)
+
+  #read Stack size from PCD
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecStackSize) ,r2)
+
+  #calcuate Stack Pointer reg value using Stack size and CPU ID.
+  mov     r3,r5         @ r3 = core_id
+  mul     r3,r3,r2      @ r3 = core_id * stack_size = offset from the stack base
+  add     r3,r3,r1      @ r3 ldr= stack_base + offset
+  mov     sp, r3
+  
+  # move sec startup address into a data register
+  # ensure we're jumping to FV version of the code (not boot remapped alias)
+  ldr   r3, StartupAddr
+  
+  # Move the CoreId in r0 to be the first argument of the SEC Entry Point
+  mov   r0, r5
+
+  # jump to SEC C code
+  #    r0 = core_id
+  blx  r3
+
+.end
diff --git a/ArmPlatformPkg/Sec/SecEntryPoint.asm b/ArmPlatformPkg/Sec/SecEntryPoint.asm
new file mode 100644
index 0000000000..794a8c02d1
--- /dev/null
+++ b/ArmPlatformPkg/Sec/SecEntryPoint.asm
@@ -0,0 +1,104 @@
+//
+//  Copyright (c) 2011, 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         
+//  which accompanies this distribution.  The full text of the license may be found at        
+//  http://opensource.org/licenses/bsd-license.php                                            
+//
+//  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,                     
+//  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.             
+//
+//
+
+#include <AutoGen.h>
+#include <AsmMacroIoLib.h>
+#include <Base.h>
+#include <Library/PcdLib.h>
+#include <Library/ArmPlatformLib.h>
+
+  INCLUDE AsmMacroIoLib.inc
+  
+  IMPORT  CEntryPoint
+  IMPORT  ArmPlatformIsMemoryInitialized
+  IMPORT  ArmPlatformInitializeBootMemory
+  IMPORT  ArmDisableInterrupts
+  IMPORT  ArmDisableCachesAndMmu
+  IMPORT  ArmWriteVBar
+  IMPORT  ArmReadMpidr
+  IMPORT  SecVectorTable
+  EXPORT  _ModuleEntryPoint
+
+#if (FixedPcdGet32(PcdMPCoreSupport))
+  IMPORT  ArmIsScuEnable
+#endif
+  
+  PRESERVE8
+  AREA    SecEntryPoint, CODE, READONLY
+  
+StartupAddr        DCD      CEntryPoint
+
+_ModuleEntryPoint
+  //Set VBAR to the start of the exception vectors in Secure Mode
+  ldr   r0, =SecVectorTable
+  blx   ArmWriteVBar
+
+  // First ensure all interrupts are disabled
+  blx   ArmDisableInterrupts
+
+  // Ensure that the MMU and caches are off
+  blx   ArmDisableCachesAndMmu
+
+_IdentifyCpu 
+  // Identify CPU ID
+  bl    ArmReadMpidr
+  and   r5, r0, #0xf
+  
+  //get ID of this CPU in Multicore system
+  cmp   r5, #0
+  // Only the primary core initialize the memory (SMC)
+  beq   _InitMem
+  
+#if (FixedPcdGet32(PcdMPCoreSupport))
+  // ... The secondary cores wait for SCU to be enabled
+_WaitForEnabledScu
+  bl    ArmIsScuEnable
+  tst   r1, #1
+  beq   _WaitForEnabledScu
+  b     _SetupStack
+#endif
+  
+_InitMem
+  bl    ArmPlatformIsMemoryInitialized
+  bne   _SetupStack
+  
+  // Initialize Init Memory
+  bl    ArmPlatformInitializeBootMemory
+
+  // Only Primary CPU could run this line (the secondary cores have jumped from _IdentifyCpu to _SetupStack)
+  mov   r5, #0
+
+_SetupStack
+  // Setup Stack for the 4 CPU cores
+  //Read Stack Base address from PCD
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoresSecStackBase), r1)
+
+  // Read Stack size from PCD
+  LoadConstantToReg (FixedPcdGet32(PcdCPUCoreSecStackSize), r2)
+
+  // Calcuate Stack Pointer reg value using Stack size and CPU ID.
+  mov     r3,r5         // r3 = core_id
+  mul     r3,r3,r2      // r3 = core_id * stack_size = offset from the stack base
+  add     r3,r3,r1      // r3 = stack_base + offset
+  mov     sp, r3
+  
+  // Move sec startup address into a data register
+  // ensure we're jumping to FV version of the code (not boot remapped alias)
+  ldr   r3, StartupAddr
+  
+  // Jump to SEC C code
+  //    r0 = core_id
+  mov   r0, r5
+  blx   r3
+  
+  END