ARMv8 Trusted Firmware release v0.2
diff --git a/bl31/aarch64/bl31_arch_setup.c b/bl31/aarch64/bl31_arch_setup.c
new file mode 100644
index 0000000..f6fa088
--- /dev/null
+++ b/bl31/aarch64/bl31_arch_setup.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch_helpers.h>
+#include <platform.h>
+#include <assert.h>
+
+/*******************************************************************************
+ * This duplicates what the primary cpu did after a cold boot in BL1. The same
+ * needs to be done when a cpu is hotplugged in. This function could also over-
+ * ride any EL3 setup done by BL1 as this code resides in rw memory.
+ ******************************************************************************/
+void bl31_arch_setup(void)
+{
+ unsigned long tmp_reg = 0;
+ unsigned int counter_base_frequency;
+
+ /* Enable alignment checks and set the exception endianness to LE */
+ tmp_reg = read_sctlr();
+ tmp_reg |= (SCTLR_A_BIT | SCTLR_SA_BIT);
+ tmp_reg &= ~SCTLR_EE_BIT;
+ write_sctlr(tmp_reg);
+
+ /*
+ * Enable HVCs, allow NS to mask CPSR.A, route FIQs to EL3, set the
+ * next EL to be aarch64
+ */
+ tmp_reg = SCR_RES1_BITS | SCR_RW_BIT | SCR_HCE_BIT | SCR_FIQ_BIT;
+ write_scr(tmp_reg);
+
+ /* Do not trap coprocessor accesses from lower ELs to EL3 */
+ write_cptr_el3(0);
+
+ /* Read the frequency from Frequency modes table */
+ counter_base_frequency = mmio_read_32(SYS_CNTCTL_BASE + CNTFID_OFF);
+ /* The first entry of the frequency modes table must not be 0 */
+ assert(counter_base_frequency != 0);
+
+ /* Program the counter frequency */
+ write_cntfrq_el0(counter_base_frequency);
+ return;
+}
+
+/*******************************************************************************
+ * Detect what is the next Non-Secure EL and setup the required architectural
+ * state
+ ******************************************************************************/
+void bl31_arch_next_el_setup(void) {
+ unsigned long id_aa64pfr0 = read_id_aa64pfr0_el1();
+ unsigned long current_sctlr, next_sctlr;
+ unsigned long el_status;
+ unsigned long scr = read_scr();
+
+ /* Use the same endianness than the current BL */
+ current_sctlr = read_sctlr();
+ next_sctlr = (current_sctlr & SCTLR_EE_BIT);
+
+ /* Find out which EL we are going to */
+ el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) & ID_AA64PFR0_ELX_MASK;
+
+ /* Check what if EL2 is supported */
+ if (el_status && (scr & SCR_HCE_BIT)) {
+ /* Set SCTLR EL2 */
+ next_sctlr |= SCTLR_EL2_RES1;
+
+ write_sctlr_el2(next_sctlr);
+ } else {
+ /* Set SCTLR Non-Secure EL1 */
+ next_sctlr |= SCTLR_EL1_RES1;
+
+ write_sctlr_el1(next_sctlr);
+ }
+}
diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
new file mode 100644
index 0000000..3a850e6
--- /dev/null
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <bl1.h>
+#include <bl_common.h>
+#include <platform.h>
+
+
+ .globl bl31_entrypoint
+
+
+ .section entry_code, "ax"; .align 3
+
+ /* -----------------------------------------------------
+ * bl31_entrypoint() is the cold boot entrypoint,
+ * executed only by the primary cpu.
+ * -----------------------------------------------------
+ */
+
+bl31_entrypoint:; .type bl31_entrypoint, %function
+ /* ---------------------------------------------
+ * BL2 has populated x0,x3,x4 with the opcode
+ * indicating BL31 should be run, memory layout
+ * of the trusted SRAM available to BL31 and
+ * information about running the non-trusted
+ * software already loaded by BL2. Check the
+ * opcode out of paranoia.
+ * ---------------------------------------------
+ */
+ mov x19, #RUN_IMAGE
+ cmp x0, x19
+ b.ne _panic
+ mov x20, x3
+ mov x21, x4
+
+ /* ---------------------------------------------
+ * This is BL31 which is expected to be executed
+ * only by the primary cpu (at least for now).
+ * So, make sure no secondary has lost its way.
+ * ---------------------------------------------
+ */
+ bl read_mpidr
+ mov x19, x0
+ bl platform_is_primary_cpu
+ cbz x0, _panic
+
+ /* --------------------------------------------
+ * Give ourselves a small coherent stack to
+ * ease the pain of initializing the MMU
+ * --------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_coherent_stack
+
+ /* ---------------------------------------------
+ * Perform platform specific early arch. setup
+ * ---------------------------------------------
+ */
+ mov x0, x20
+ mov x1, x21
+ mov x2, x19
+ bl bl31_early_platform_setup
+ bl bl31_plat_arch_setup
+
+ /* ---------------------------------------------
+ * Give ourselves a stack allocated in Normal
+ * -IS-WBWA memory
+ * ---------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_stack
+
+ /* ---------------------------------------------
+ * Use SP_EL0 to initialize BL31. It allows us
+ * to jump to the next image without having to
+ * come back here to ensure all of the stack's
+ * been popped out. run_image() is not nice
+ * enough to reset the stack pointer before
+ * handing control to the next stage.
+ * ---------------------------------------------
+ */
+ mov x0, sp
+ msr sp_el0, x0
+ msr spsel, #0
+ isb
+
+ /* ---------------------------------------------
+ * Jump to main function.
+ * ---------------------------------------------
+ */
+ bl bl31_main
+
+_panic:
+ b _panic
diff --git a/bl31/aarch64/exception_handlers.c b/bl31/aarch64/exception_handlers.c
new file mode 100644
index 0000000..860d8eb
--- /dev/null
+++ b/bl31/aarch64/exception_handlers.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch_helpers.h>
+#include <platform.h>
+#include <bl_common.h>
+#include <bl31.h>
+#include <psci.h>
+#include <assert.h>
+#include <runtime_svc.h>
+
+/*******************************************************************************
+ * This function checks whether this is a valid smc e.g.
+ * the function id is correct, top word of args are zeroed
+ * when aarch64 makes an aarch32 call etc.
+ ******************************************************************************/
+int validate_smc(gp_regs *regs)
+{
+ unsigned int rw = GET_RW(regs->spsr);
+ unsigned int cc = GET_SMC_CC(regs->x0);
+
+ /* Check if there is a difference in the caller RW and SMC CC */
+ if (rw == cc) {
+
+ /* Check whether the caller has chosen the right func. id */
+ if (cc == SMC_64) {
+ regs->x0 = SMC_UNK;
+ return SMC_UNK;
+ }
+
+ /*
+ * Paranoid check to zero the top word of passed args
+ * irrespective of caller's register width.
+ *
+ * TODO: Check if this needed if the caller is aarch32
+ */
+ regs->x0 &= (unsigned int) 0xFFFFFFFF;
+ regs->x1 &= (unsigned int) 0xFFFFFFFF;
+ regs->x2 &= (unsigned int) 0xFFFFFFFF;
+ regs->x3 &= (unsigned int) 0xFFFFFFFF;
+ regs->x4 &= (unsigned int) 0xFFFFFFFF;
+ regs->x5 &= (unsigned int) 0xFFFFFFFF;
+ regs->x6 &= (unsigned int) 0xFFFFFFFF;
+ }
+
+ return 0;
+}
+
+/* TODO: Break down the SMC handler into fast and standard SMC handlers. */
+void smc_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ /* Check if the SMC has been correctly called */
+ if (validate_smc(regs) != 0)
+ return;
+
+ switch (regs->x0) {
+ case PSCI_VERSION:
+ regs->x0 = psci_version();
+ break;
+
+ case PSCI_CPU_OFF:
+ regs->x0 = __psci_cpu_off();
+ break;
+
+ case PSCI_CPU_SUSPEND_AARCH64:
+ case PSCI_CPU_SUSPEND_AARCH32:
+ regs->x0 = __psci_cpu_suspend(regs->x1, regs->x2, regs->x3);
+ break;
+
+ case PSCI_CPU_ON_AARCH64:
+ case PSCI_CPU_ON_AARCH32:
+ regs->x0 = psci_cpu_on(regs->x1, regs->x2, regs->x3);
+ break;
+
+ case PSCI_AFFINITY_INFO_AARCH32:
+ case PSCI_AFFINITY_INFO_AARCH64:
+ regs->x0 = psci_affinity_info(regs->x1, regs->x2);
+ break;
+
+ default:
+ regs->x0 = SMC_UNK;
+ }
+
+ return;
+}
+
+void irq_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ plat_report_exception(type);
+ assert(0);
+}
+
+void fiq_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ plat_report_exception(type);
+ assert(0);
+}
+
+void serror_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ plat_report_exception(type);
+ assert(0);
+}
+
+void sync_exception_handler(unsigned type, gp_regs *regs)
+{
+ unsigned long esr = read_esr();
+ unsigned int ec = EC_BITS(esr);
+
+ switch (ec) {
+
+ case EC_AARCH32_SMC:
+ case EC_AARCH64_SMC:
+ smc_handler(type, esr, regs);
+ break;
+
+ default:
+ plat_report_exception(type);
+ assert(0);
+ }
+ return;
+}
+
+void async_exception_handler(unsigned type, gp_regs *regs)
+{
+ unsigned long esr = read_esr();
+
+ switch (type) {
+
+ case IRQ_SP_EL0:
+ case IRQ_SP_ELX:
+ case IRQ_AARCH64:
+ case IRQ_AARCH32:
+ irq_handler(type, esr, regs);
+ break;
+
+ case FIQ_SP_EL0:
+ case FIQ_SP_ELX:
+ case FIQ_AARCH64:
+ case FIQ_AARCH32:
+ fiq_handler(type, esr, regs);
+ break;
+
+ case SERROR_SP_EL0:
+ case SERROR_SP_ELX:
+ case SERROR_AARCH64:
+ case SERROR_AARCH32:
+ serror_handler(type, esr, regs);
+ break;
+
+ default:
+ plat_report_exception(type);
+ assert(0);
+ }
+
+ return;
+}
diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
new file mode 100644
index 0000000..21976ad
--- /dev/null
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <runtime_svc.h>
+
+ .globl runtime_exceptions
+
+
+#include <asm_macros.S>
+
+
+ .section aarch64_code, "ax"; .align 11
+
+ .align 7
+runtime_exceptions:
+ /* -----------------------------------------------------
+ * Current EL with _sp_el0 : 0x0 - 0x180
+ * -----------------------------------------------------
+ */
+sync_exception_sp_el0:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_SP_EL0
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_sp_el0:
+ exception_entry save_regs
+ mov x0, #IRQ_SP_EL0
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_sp_el0:
+ exception_entry save_regs
+ mov x0, #FIQ_SP_EL0
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_sp_el0:
+ exception_entry save_regs
+ mov x0, #SERROR_SP_EL0
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ /* -----------------------------------------------------
+ * Current EL with SPx: 0x200 - 0x380
+ * -----------------------------------------------------
+ */
+ .align 7
+sync_exception_sp_elx:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_SP_ELX
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_sp_elx:
+ exception_entry save_regs
+ mov x0, #IRQ_SP_ELX
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_sp_elx:
+ exception_entry save_regs
+ mov x0, #FIQ_SP_ELX
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_sp_elx:
+ exception_entry save_regs
+ mov x0, #SERROR_SP_ELX
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ /* -----------------------------------------------------
+ * Lower EL using AArch64 : 0x400 - 0x580
+ * -----------------------------------------------------
+ */
+ .align 7
+sync_exception_aarch64:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_AARCH64
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_aarch64:
+ exception_entry save_regs
+ mov x0, #IRQ_AARCH64
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_aarch64:
+ exception_entry save_regs
+ mov x0, #FIQ_AARCH64
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_aarch64:
+ exception_entry save_regs
+ mov x0, #IRQ_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ /* -----------------------------------------------------
+ * Lower EL using AArch32 : 0x600 - 0x780
+ * -----------------------------------------------------
+ */
+ .align 7
+sync_exception_aarch32:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_AARCH32
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_aarch32:
+ exception_entry save_regs
+ mov x0, #IRQ_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_aarch32:
+ exception_entry save_regs
+ mov x0, #FIQ_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_aarch32:
+ exception_entry save_regs
+ mov x0, #SERROR_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+
+save_regs:; .type save_regs, %function
+ sub sp, sp, #0x100
+ stp x0, x1, [sp, #0x0]
+ stp x2, x3, [sp, #0x10]
+ stp x4, x5, [sp, #0x20]
+ stp x6, x7, [sp, #0x30]
+ stp x8, x9, [sp, #0x40]
+ stp x10, x11, [sp, #0x50]
+ stp x12, x13, [sp, #0x60]
+ stp x14, x15, [sp, #0x70]
+ stp x16, x17, [sp, #0x80]
+ stp x18, x19, [sp, #0x90]
+ stp x20, x21, [sp, #0xa0]
+ stp x22, x23, [sp, #0xb0]
+ stp x24, x25, [sp, #0xc0]
+ stp x26, x27, [sp, #0xd0]
+ mrs x0, sp_el0
+ stp x28, x0, [sp, #0xe0]
+ mrs x0, spsr_el3
+ str x0, [sp, #0xf0]
+ ret
+
+
+restore_regs:; .type restore_regs, %function
+ ldr x9, [sp, #0xf0]
+ msr spsr_el3, x9
+ ldp x28, x9, [sp, #0xe0]
+ msr sp_el0, x9
+ ldp x26, x27, [sp, #0xd0]
+ ldp x24, x25, [sp, #0xc0]
+ ldp x22, x23, [sp, #0xb0]
+ ldp x20, x21, [sp, #0xa0]
+ ldp x18, x19, [sp, #0x90]
+ ldp x16, x17, [sp, #0x80]
+ ldp x14, x15, [sp, #0x70]
+ ldp x12, x13, [sp, #0x60]
+ ldp x10, x11, [sp, #0x50]
+ ldp x8, x9, [sp, #0x40]
+ ldp x6, x7, [sp, #0x30]
+ ldp x4, x5, [sp, #0x20]
+ ldp x2, x3, [sp, #0x10]
+ ldp x0, x1, [sp, #0x0]
+ add sp, sp, #0x100
+ ret
diff --git a/bl31/bl31.ld.S b/bl31/bl31.ld.S
new file mode 100644
index 0000000..5ad8648
--- /dev/null
+++ b/bl31/bl31.ld.S
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <platform.h>
+
+OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
+OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
+
+
+MEMORY {
+ /* RAM is read/write and Initialised */
+ RAM (rwx): ORIGIN = TZRAM_BASE, LENGTH = TZRAM_SIZE
+}
+
+
+SECTIONS
+{
+ . = BL31_BASE;
+
+ BL31_RO ALIGN (4096): {
+ *(entry_code)
+ *(.text)
+ *(.rodata)
+ } >RAM
+
+ BL31_STACKS ALIGN (4096): {
+ . += 0x1000;
+ *(tzfw_normal_stacks)
+ } >RAM
+
+ BL31_COHERENT_RAM ALIGN (4096): {
+ *(tzfw_coherent_mem)
+ /* . += 0x1000;*/
+ /* Do we need to ensure at least 4k here? */
+ . = ALIGN(4096);
+ } >RAM
+
+ __BL31_DATA_START__ = .;
+ .bss ALIGN (4096): {
+ *(.bss)
+ *(COMMON)
+ } >RAM
+
+ .data : {
+ *(.data)
+ } >RAM
+ __BL31_DATA_STOP__ = .;
+
+
+ __BL31_RO_BASE__ = LOADADDR(BL31_RO);
+ __BL31_RO_SIZE__ = SIZEOF(BL31_RO);
+
+ __BL31_STACKS_BASE__ = LOADADDR(BL31_STACKS);
+ __BL31_STACKS_SIZE__ = SIZEOF(BL31_STACKS);
+
+ __BL31_COHERENT_RAM_BASE__ = LOADADDR(BL31_COHERENT_RAM);
+ __BL31_COHERENT_RAM_SIZE__ = SIZEOF(BL31_COHERENT_RAM);
+
+ __BL31_RW_BASE__ = __BL31_DATA_START__;
+ __BL31_RW_SIZE__ = __BL31_DATA_STOP__ - __BL31_DATA_START__;
+}
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
new file mode 100644
index 0000000..dcf78bc
--- /dev/null
+++ b/bl31/bl31.mk
@@ -0,0 +1,55 @@
+#
+# Copyright (c) 2013, ARM Limited. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# Neither the name of ARM nor the names of its contributors may be used
+# to endorse or promote products derived from this software without specific
+# prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+vpath %.c drivers/arm/interconnect/cci-400/ common/ lib/ \
+ drivers/arm/peripherals/pl011 plat/fvp common/psci \
+ lib/semihosting arch/aarch64/ lib/non-semihosting \
+ lib/sync/locks/bakery/ drivers/power/ arch/system/gic/ \
+ plat/fvp/aarch64/
+
+vpath %.S lib/arch/aarch64 common/psci \
+ lib/semihosting/aarch64 include/ plat/fvp/${ARCH} \
+ lib/sync/locks/exclusive plat/common/aarch64/ \
+ arch/system/gic/${ARCH}
+
+BL31_ASM_OBJS := bl31_entrypoint.o runtime_exceptions.o psci_entry.o \
+ spinlock.o gic_v3_sysregs.o fvp_helpers.o
+BL31_C_OBJS := bl31_main.o bl31_plat_setup.o bl31_arch_setup.o \
+ exception_handlers.o bakery_lock.o cci400.o \
+ fvp_common.o fvp_pm.o fvp_pwrc.o fvp_topology.o \
+ runtime_svc.o gic_v3.o gic_v2.o psci_setup.o \
+ psci_common.o psci_afflvl_on.o psci_main.o \
+ psci_afflvl_off.o psci_afflvl_suspend.o
+
+BL31_ENTRY_POINT := bl31_entrypoint
+BL31_MAPFILE := bl31.map
+BL31_LINKERFILE := bl31.ld
+
+BL31_OBJS := $(BL31_C_OBJS) $(BL31_ASM_OBJS)
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
new file mode 100644
index 0000000..e8fa2f8
--- /dev/null
+++ b/bl31/bl31_main.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <semihosting.h>
+#include <bl_common.h>
+#include <bl31.h>
+#include <runtime_svc.h>
+
+void bl31_arch_next_el_setup(void);
+
+/*******************************************************************************
+ * BL31 is responsible for setting up the runtime services for the primary cpu
+ * before passing control to the bootloader (UEFI) or Linux.
+ ******************************************************************************/
+void bl31_main(void)
+{
+ el_change_info *image_info;
+ unsigned long mpidr = read_mpidr();
+
+ /* Perform remaining generic architectural setup from EL3 */
+ bl31_arch_setup();
+
+ /* Perform platform setup in BL1 */
+ bl31_platform_setup();
+
+#if defined (__GNUC__)
+ printf("BL31 Built : %s, %s\n\r", __TIME__, __DATE__);
+#endif
+
+
+ /* Initialize the runtime services e.g. psci */
+ runtime_svc_init(mpidr);
+
+ /* Clean caches before re-entering normal world */
+ dcsw_op_all(DCCSW);
+
+ image_info = bl31_get_next_image_info(mpidr);
+ bl31_arch_next_el_setup();
+ change_el(image_info);
+
+ /* There is no valid reason for change_el() to return */
+ assert(0);
+}