diff --git a/Makefile b/Makefile
index 8ec8472a2ff49e196f61a04b7558a00a91d20bf3..1069cd5ae0495cc5b99c1b1cef7e66198455a375 100644
--- a/Makefile
+++ b/Makefile
@@ -47,6 +47,8 @@ SPD := none
BASE_COMMIT := origin/master
# NS timer register save and restore
NS_TIMER_SWITCH := 0
+# By default, Bl1 acts as the reset handler, not BL31
+RESET_TO_BL31 := 0
# Checkpatch ignores
@@ -178,6 +180,10 @@ endif
$(eval $(call assert_boolean,NS_TIMER_SWITCH))
$(eval $(call add_define,NS_TIMER_SWITCH))
+# Process RESET_TO_BL31 flag
+$(eval $(call assert_boolean,RESET_TO_BL31))
+$(eval $(call add_define,RESET_TO_BL31))
+
ASFLAGS += -nostdinc -ffreestanding -Wa,--fatal-warnings \
-mgeneral-regs-only -D__ASSEMBLY__ \
${DEFINES} ${INCLUDES}
diff --git a/bl1/aarch64/bl1_entrypoint.S b/bl1/aarch64/bl1_entrypoint.S
index 7259601d15c501a0a1c6775d99a2139955cadd26..1c4f8e1e0b335dd7824c3983d17f121fa7926184 100644
--- a/bl1/aarch64/bl1_entrypoint.S
+++ b/bl1/aarch64/bl1_entrypoint.S
@@ -98,31 +98,10 @@ func bl1_entrypoint
msr sctlr_el3, x0
isb
-_wait_for_entrypoint:
- /* ---------------------------------------------
- * Find the type of reset and jump to handler
- * if present. If the handler is null then it is
- * a cold boot. The primary cpu will set up the
- * platform while the secondaries wait for
- * their turn to be woken up
- * ---------------------------------------------
- */
- mrs x0, mpidr_el1
- bl platform_get_entrypoint
- cbnz x0, _do_warm_boot
- mrs x0, mpidr_el1
- bl platform_is_primary_cpu
- cbnz x0, _do_cold_boot
+ wait_for_entrypoint
- /* ---------------------------------------------
- * Perform any platform specific secondary cpu
- * actions
- * ---------------------------------------------
- */
- bl plat_secondary_cold_boot_setup
- b _wait_for_entrypoint
+ bl platform_mem_init
-_do_cold_boot:
/* ---------------------------------------------
* Init C runtime environment.
* - Zero-initialise the NOBITS sections.
@@ -148,19 +127,37 @@ _do_cold_boot:
bl memcpy16
/* ---------------------------------------------
- * Initialize platform and jump to our c-entry
- * point for this type of reset
+ * Give ourselves a small coherent stack to
+ * ease the pain of initializing the MMU and
+ * CCI in assembler
* ---------------------------------------------
*/
- adr x0, bl1_main
- bl platform_cold_boot_init
- b _panic
+ mrs x0, mpidr_el1
+ bl platform_set_coherent_stack
-_do_warm_boot:
/* ---------------------------------------------
- * Jump to BL31 for all warm boot init.
+ * Architectural init. can be generic e.g.
+ * enabling stack alignment and platform spec-
+ * ific e.g. MMU & page table setup as per the
+ * platform memory map. Perform the latter here
+ * and the former in bl1_main.
* ---------------------------------------------
*/
- blr x0
-_panic:
- b _panic
+ bl bl1_early_platform_setup
+ bl bl1_plat_arch_setup
+
+ /* ---------------------------------------------
+ * Give ourselves a stack allocated in Normal
+ * -IS-WBWA memory
+ * ---------------------------------------------
+ */
+ mrs x0, mpidr_el1
+ bl platform_set_stack
+ /* -----------------------------------------------
+ * Initialize platform and jump to our c-entry point for this type
+ * of reset. Panic if it returns
+ * -----------------------------------------------
+ */
+ bl bl1_main
+panic:
+ b panic
diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index 68e37b149929a42790e5bea9d32c48222b11df2a..93166d7c27b500ce5d9d379ab27e4d9a388c36ab 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -49,8 +49,17 @@ func bl31_entrypoint
* structure & x1 with a pointer to platform specific structure
* ---------------------------------------------------------------
*/
+#if !RESET_TO_BL31
mov x20, x0
mov x21, x1
+#endif
+ /* -----------------------------------------------------
+ * Perform any processor specific actions upon reset
+ * e.g. cache, tlb invalidations etc. Override the
+ * Boot ROM(BL0) programming sequence
+ * -----------------------------------------------------
+ */
+ bl cpu_reset_handler
/* ---------------------------------------------
* Set the exception vector to something sane.
@@ -90,7 +99,10 @@ func bl31_entrypoint
orr x1, x1, #SCTLR_I_BIT
msr sctlr_el3, x1
isb
-
+#if RESET_TO_BL31
+ wait_for_entrypoint
+ bl platform_mem_init
+#else
/* ---------------------------------------------
* This is BL31 which is expected to be executed
* only by the primary cpu (at least for now).
@@ -100,6 +112,7 @@ func bl31_entrypoint
mrs x0, mpidr_el1
bl platform_is_primary_cpu
cbz x0, _panic
+#endif
/* ---------------------------------------------
* Zero out NOBITS sections. There are 2 of them:
@@ -133,8 +146,10 @@ func bl31_entrypoint
* Perform platform specific early arch. setup
* ---------------------------------------------
*/
+#if !RESET_TO_BL31
mov x0, x20
mov x1, x21
+#endif
bl bl31_early_platform_setup
bl bl31_plat_arch_setup
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index c0dc2fd152660c002e5ec0c4f7880e2603ada6f3..6c9650f0341ef5b1ea81acd4274f36945b63649a 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -37,6 +37,7 @@ BL31_SOURCES += bl31/bl31_main.c \
bl31/aarch64/runtime_exceptions.S \
bl31/aarch64/crash_reporting.S \
common/aarch64/early_exceptions.S \
+ lib/aarch64/cpu_helpers.S \
lib/locks/bakery/bakery_lock.c \
lib/locks/exclusive/spinlock.S \
services/std_svc/std_svc_setup.c \
diff --git a/include/common/asm_macros.S b/include/common/asm_macros.S
index 3dbd9f2a41ea65487da08a74d0f1c78b5099c28c..f8cca3ac3229a0244a6f151f8e8addc506b78b6a 100644
--- a/include/common/asm_macros.S
+++ b/include/common/asm_macros.S
@@ -88,6 +88,41 @@
\_name:
.endm
+ /* ---------------------------------------------
+ * Find the type of reset and jump to handler
+ * if present. If the handler is null then it is
+ * a cold boot. The primary cpu will set up the
+ * platform while the secondaries wait for
+ * their turn to be woken up
+ * ---------------------------------------------
+ */
+ .macro wait_for_entrypoint
+wait_for_entrypoint:
+ mrs x0, mpidr_el1
+ bl platform_get_entrypoint
+ cbnz x0, do_warm_boot
+ mrs x0, mpidr_el1
+ bl platform_is_primary_cpu
+ cbnz x0, do_cold_boot
+
+ /* ---------------------------------------------
+ * Perform any platform specific secondary cpu
+ * actions
+ * ---------------------------------------------
+ */
+ bl plat_secondary_cold_boot_setup
+ b wait_for_entrypoint
+
+ do_warm_boot:
+ /* ---------------------------------------------
+ * Jump to BL31 for all warm boot init.
+ * ---------------------------------------------
+ */
+ blr x0
+
+ do_cold_boot:
+ .endm
+
/*
* This macro declares an array of 1 or more stacks, properly
* aligned and in the requested section
diff --git a/plat/fvp/aarch64/bl1_plat_helpers.S b/plat/fvp/aarch64/bl1_plat_helpers.S
deleted file mode 100644
index b4d44586d2e51b648e785715a03f8b46d1a1ed7a..0000000000000000000000000000000000000000
--- a/plat/fvp/aarch64/bl1_plat_helpers.S
+++ /dev/null
@@ -1,213 +0,0 @@
-/*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. 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 <asm_macros.S>
-#include <gic_v2.h>
-#include <platform.h>
-#include "../drivers/pwrc/fvp_pwrc.h"
-
- .globl platform_get_entrypoint
- .globl platform_cold_boot_init
- .globl plat_secondary_cold_boot_setup
-
-
- .macro platform_choose_gicmmap param1, param2, x_tmp, w_tmp, res
- ldr \x_tmp, =VE_SYSREGS_BASE + V2M_SYS_ID
- ldr \w_tmp, [\x_tmp]
- ubfx \w_tmp, \w_tmp, #SYS_ID_BLD_SHIFT, #SYS_ID_BLD_LENGTH
- cmp \w_tmp, #BLD_GIC_VE_MMAP
- csel \res, \param1, \param2, eq
- .endm
-
- /* -----------------------------------------------------
- * void plat_secondary_cold_boot_setup (void);
- *
- * This function performs any platform specific actions
- * needed for a secondary cpu after a cold reset e.g
- * mark the cpu's presence, mechanism to place it in a
- * holding pen etc.
- * TODO: Should we read the PSYS register to make sure
- * that the request has gone through.
- * -----------------------------------------------------
- */
-func plat_secondary_cold_boot_setup
- /* ---------------------------------------------
- * Power down this cpu.
- * TODO: Do we need to worry about powering the
- * cluster down as well here. That will need
- * locks which we won't have unless an elf-
- * loader zeroes out the zi section.
- * ---------------------------------------------
- */
- mrs x0, mpidr_el1
- ldr x1, =PWRC_BASE
- str w0, [x1, #PPOFFR_OFF]
-
- /* ---------------------------------------------
- * Deactivate the gic cpu interface as well
- * ---------------------------------------------
- */
- ldr x0, =VE_GICC_BASE
- ldr x1, =BASE_GICC_BASE
- platform_choose_gicmmap x0, x1, x2, w2, x1
- mov w0, #(IRQ_BYP_DIS_GRP1 | FIQ_BYP_DIS_GRP1)
- orr w0, w0, #(IRQ_BYP_DIS_GRP0 | FIQ_BYP_DIS_GRP0)
- str w0, [x1, #GICC_CTLR]
-
- /* ---------------------------------------------
- * There is no sane reason to come out of this
- * wfi so panic if we do. This cpu will be pow-
- * ered on and reset by the cpu_on pm api
- * ---------------------------------------------
- */
- dsb sy
- wfi
-cb_panic:
- b cb_panic
-
-
- /* -----------------------------------------------------
- * void platform_get_entrypoint (unsigned int mpid);
- *
- * Main job of this routine is to distinguish between
- * a cold and warm boot.
- * On a cold boot the secondaries first wait for the
- * platform to be initialized after which they are
- * hotplugged in. The primary proceeds to perform the
- * platform initialization.
- * On a warm boot, each cpu jumps to the address in its
- * mailbox.
- *
- * TODO: Not a good idea to save lr in a temp reg
- * TODO: PSYSR is a common register and should be
- * accessed using locks. Since its not possible
- * to use locks immediately after a cold reset
- * we are relying on the fact that after a cold
- * reset all cpus will read the same WK field
- * -----------------------------------------------------
- */
-func platform_get_entrypoint
- mov x9, x30 // lr
- mov x2, x0
- ldr x1, =PWRC_BASE
- str w2, [x1, #PSYSR_OFF]
- ldr w2, [x1, #PSYSR_OFF]
- ubfx w2, w2, #PSYSR_WK_SHIFT, #PSYSR_WK_MASK
- cbnz w2, warm_reset
- mov x0, x2
- b exit
-warm_reset:
- /* ---------------------------------------------
- * A per-cpu mailbox is maintained in the tru-
- * sted DRAM. Its flushed out of the caches
- * after every update using normal memory so
- * its safe to read it here with SO attributes
- * ---------------------------------------------
- */
- ldr x10, =TZDRAM_BASE + MBOX_OFF
- bl platform_get_core_pos
- lsl x0, x0, #CACHE_WRITEBACK_SHIFT
- ldr x0, [x10, x0]
- cbz x0, _panic
-exit:
- ret x9
-_panic: b _panic
-
-
- /* -----------------------------------------------------
- * void platform_mem_init (void);
- *
- * Zero out the mailbox registers in the TZDRAM. The
- * mmu is turned off right now and only the primary can
- * ever execute this code. Secondaries will read the
- * mailboxes using SO accesses. In short, BL31 will
- * update the mailboxes after mapping the tzdram as
- * normal memory. It will flush its copy after update.
- * BL1 will always read the mailboxes with the MMU off
- * -----------------------------------------------------
- */
-func platform_mem_init
- ldr x0, =TZDRAM_BASE + MBOX_OFF
- stp xzr, xzr, [x0, #0]
- stp xzr, xzr, [x0, #0x10]
- stp xzr, xzr, [x0, #0x20]
- stp xzr, xzr, [x0, #0x30]
- ret
-
-
- /* -----------------------------------------------------
- * void platform_cold_boot_init (bl1_main function);
- *
- * Routine called only by the primary cpu after a cold
- * boot to perform early platform initialization
- * -----------------------------------------------------
- */
-func platform_cold_boot_init
- mov x20, x0
- bl platform_mem_init
-
- /* ---------------------------------------------
- * Give ourselves a small coherent stack to
- * ease the pain of initializing the MMU and
- * CCI in assembler
- * ---------------------------------------------
- */
- mrs x0, mpidr_el1
- bl platform_set_coherent_stack
-
- /* ---------------------------------------------
- * Architectural init. can be generic e.g.
- * enabling stack alignment and platform spec-
- * ific e.g. MMU & page table setup as per the
- * platform memory map. Perform the latter here
- * and the former in bl1_main.
- * ---------------------------------------------
- */
- bl bl1_early_platform_setup
- bl bl1_plat_arch_setup
-
- /* ---------------------------------------------
- * Give ourselves a stack allocated in Normal
- * -IS-WBWA memory
- * ---------------------------------------------
- */
- mrs x0, mpidr_el1
- bl platform_set_stack
-
- /* ---------------------------------------------
- * Jump to the main function. Returning from it
- * is a terminal error.
- * ---------------------------------------------
- */
- blr x20
-
-cb_init_panic:
- b cb_init_panic
diff --git a/plat/fvp/aarch64/plat_common.c b/plat/fvp/aarch64/plat_common.c
index b17093fd958dcb68aa7703fb4fc9e56f30d441b7..217905a023445bc1996c0da001b4e801d6b27aa7 100644
--- a/plat/fvp/aarch64/plat_common.c
+++ b/plat/fvp/aarch64/plat_common.c
@@ -32,6 +32,7 @@
#include <arch_helpers.h>
#include <assert.h>
#include <bl_common.h>
+#include <cci400.h>
#include <debug.h>
#include <mmio.h>
#include <platform.h>
@@ -252,3 +253,57 @@ uint64_t plat_get_syscnt_freq(void)
return counter_base_frequency;
}
+
+void plat_cci_setup(void)
+{
+ unsigned long cci_setup;
+
+ /*
+ * Enable CCI-400 for this cluster. No need
+ * for locks as no other cpu is active at the
+ * moment
+ */
+ cci_setup = platform_get_cfgvar(CONFIG_HAS_CCI);
+ if (cci_setup)
+ cci_enable_coherency(read_mpidr());
+}
+
+
+/*******************************************************************************
+ * Set SPSR and secure state for BL32 image
+ ******************************************************************************/
+void fvp_set_bl32_entrypoint(el_change_info_t *bl32_ep)
+{
+ SET_SECURITY_STATE(bl32_ep->h.attr, SECURE);
+ /*
+ * The Secure Payload Dispatcher service is responsible for
+ * setting the SPSR prior to entry into the BL32 image.
+ */
+ bl32_ep->spsr = 0;
+}
+
+/*******************************************************************************
+ * Set SPSR and secure state for BL33 image
+ ******************************************************************************/
+void fvp_set_bl33_entrypoint(el_change_info_t *bl33_ep)
+{
+ unsigned long el_status;
+ unsigned int mode;
+
+ /* Figure out what mode we enter the non-secure world in */
+ el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
+ el_status &= ID_AA64PFR0_ELX_MASK;
+
+ if (el_status)
+ mode = MODE_EL2;
+ else
+ mode = MODE_EL1;
+
+ /*
+ * TODO: Consider the possibility of specifying the SPSR in
+ * the FIP ToC and allowing the platform to have a say as
+ * well.
+ */
+ bl33_ep->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+ SET_SECURITY_STATE(bl33_ep->h.attr, NON_SECURE);
+}
diff --git a/plat/fvp/aarch64/plat_helpers.S b/plat/fvp/aarch64/plat_helpers.S
index 00579035733457a767a88a6da3c76d4dc631904b..9b6b13aaea38a897e8350d6dc53d6a1b0d4f6789 100644
--- a/plat/fvp/aarch64/plat_helpers.S
+++ b/plat/fvp/aarch64/plat_helpers.S
@@ -31,10 +31,139 @@
#include <arch.h>
#include <asm_macros.S>
#include <bl_common.h>
+#include <gic_v2.h>
#include <platform.h>
+#include "../drivers/pwrc/fvp_pwrc.h"
+ .globl platform_get_entrypoint
+ .globl plat_secondary_cold_boot_setup
+ .globl platform_mem_init
.globl plat_report_exception
+ .macro platform_choose_gicmmap param1, param2, x_tmp, w_tmp, res
+ ldr \x_tmp, =VE_SYSREGS_BASE + V2M_SYS_ID
+ ldr \w_tmp, [\x_tmp]
+ ubfx \w_tmp, \w_tmp, #SYS_ID_BLD_SHIFT, #SYS_ID_BLD_LENGTH
+ cmp \w_tmp, #BLD_GIC_VE_MMAP
+ csel \res, \param1, \param2, eq
+ .endm
+
+ /* -----------------------------------------------------
+ * void plat_secondary_cold_boot_setup (void);
+ *
+ * This function performs any platform specific actions
+ * needed for a secondary cpu after a cold reset e.g
+ * mark the cpu's presence, mechanism to place it in a
+ * holding pen etc.
+ * TODO: Should we read the PSYS register to make sure
+ * that the request has gone through.
+ * -----------------------------------------------------
+ */
+func plat_secondary_cold_boot_setup
+ /* ---------------------------------------------
+ * Power down this cpu.
+ * TODO: Do we need to worry about powering the
+ * cluster down as well here. That will need
+ * locks which we won't have unless an elf-
+ * loader zeroes out the zi section.
+ * ---------------------------------------------
+ */
+ mrs x0, mpidr_el1
+ ldr x1, =PWRC_BASE
+ str w0, [x1, #PPOFFR_OFF]
+
+ /* ---------------------------------------------
+ * Deactivate the gic cpu interface as well
+ * ---------------------------------------------
+ */
+ ldr x0, =VE_GICC_BASE
+ ldr x1, =BASE_GICC_BASE
+ platform_choose_gicmmap x0, x1, x2, w2, x1
+ mov w0, #(IRQ_BYP_DIS_GRP1 | FIQ_BYP_DIS_GRP1)
+ orr w0, w0, #(IRQ_BYP_DIS_GRP0 | FIQ_BYP_DIS_GRP0)
+ str w0, [x1, #GICC_CTLR]
+
+ /* ---------------------------------------------
+ * There is no sane reason to come out of this
+ * wfi so panic if we do. This cpu will be pow-
+ * ered on and reset by the cpu_on pm api
+ * ---------------------------------------------
+ */
+ dsb sy
+ wfi
+cb_panic:
+ b cb_panic
+
+
+ /* -----------------------------------------------------
+ * void platform_get_entrypoint (unsigned int mpid);
+ *
+ * Main job of this routine is to distinguish between
+ * a cold and warm boot.
+ * On a cold boot the secondaries first wait for the
+ * platform to be initialized after which they are
+ * hotplugged in. The primary proceeds to perform the
+ * platform initialization.
+ * On a warm boot, each cpu jumps to the address in its
+ * mailbox.
+ *
+ * TODO: Not a good idea to save lr in a temp reg
+ * TODO: PSYSR is a common register and should be
+ * accessed using locks. Since its not possible
+ * to use locks immediately after a cold reset
+ * we are relying on the fact that after a cold
+ * reset all cpus will read the same WK field
+ * -----------------------------------------------------
+ */
+func platform_get_entrypoint
+ mov x9, x30 // lr
+ mov x2, x0
+ ldr x1, =PWRC_BASE
+ str w2, [x1, #PSYSR_OFF]
+ ldr w2, [x1, #PSYSR_OFF]
+ ubfx w2, w2, #PSYSR_WK_SHIFT, #PSYSR_WK_MASK
+ cbnz w2, warm_reset
+ mov x0, x2
+ b exit
+warm_reset:
+ /* ---------------------------------------------
+ * A per-cpu mailbox is maintained in the tru-
+ * sted DRAM. Its flushed out of the caches
+ * after every update using normal memory so
+ * its safe to read it here with SO attributes
+ * ---------------------------------------------
+ */
+ ldr x10, =TZDRAM_BASE + MBOX_OFF
+ bl platform_get_core_pos
+ lsl x0, x0, #CACHE_WRITEBACK_SHIFT
+ ldr x0, [x10, x0]
+ cbz x0, _panic
+exit:
+ ret x9
+_panic: b _panic
+
+
+ /* -----------------------------------------------------
+ * void platform_mem_init (void);
+ *
+ * Zero out the mailbox registers in the TZDRAM. The
+ * mmu is turned off right now and only the primary can
+ * ever execute this code. Secondaries will read the
+ * mailboxes using SO accesses. In short, BL31 will
+ * update the mailboxes after mapping the tzdram as
+ * normal memory. It will flush its copy after update.
+ * BL1 will always read the mailboxes with the MMU off
+ * -----------------------------------------------------
+ */
+func platform_mem_init
+ ldr x0, =TZDRAM_BASE + MBOX_OFF
+ mov w1, #PLATFORM_CORE_COUNT
+loop:
+ str xzr, [x0], #CACHE_WRITEBACK_GRANULE
+ subs w1, w1, #1
+ b.gt loop
+ ret
+
/* ---------------------------------------------
* void plat_report_exception(unsigned int type)
* Function to report an unhandled exception
diff --git a/plat/fvp/bl1_plat_setup.c b/plat/fvp/bl1_plat_setup.c
index 6d890a02ac8826a92b21ca16d0c9c6efa2c2c218..b69b2ab54be6e954356713f972d61346eb97733a 100644
--- a/plat/fvp/bl1_plat_setup.c
+++ b/plat/fvp/bl1_plat_setup.c
@@ -33,7 +33,6 @@
#include <bl_common.h>
#include <bl1.h>
#include <console.h>
-#include <cci400.h>
#include <mmio.h>
#include <platform.h>
@@ -126,17 +125,8 @@ void bl1_platform_setup(void)
******************************************************************************/
void bl1_plat_arch_setup(void)
{
- unsigned long cci_setup;
+ plat_cci_setup();
- /*
- * Enable CCI-400 for this cluster. No need
- * for locks as no other cpu is active at the
- * moment
- */
- cci_setup = platform_get_cfgvar(CONFIG_HAS_CCI);
- if (cci_setup) {
- cci_enable_coherency(read_mpidr());
- }
configure_mmu_el3(bl1_tzram_layout.total_base,
bl1_tzram_layout.total_size,
diff --git a/plat/fvp/bl2_plat_setup.c b/plat/fvp/bl2_plat_setup.c
index 96282aaa840a21b3778540bb396ac073eca56606..38525eac5d7c834a1e1b34b60662e6ad616cac72 100644
--- a/plat/fvp/bl2_plat_setup.c
+++ b/plat/fvp/bl2_plat_setup.c
@@ -238,12 +238,7 @@ void bl2_plat_bl31_post_load_actions(image_info_t *bl31_image,
void bl2_plat_bl32_post_load_actions(image_info_t *bl32_image,
el_change_info_t *bl32_ep)
{
- SET_SECURITY_STATE(bl32_ep->h.attr, SECURE);
- /*
- * The Secure Payload Dispatcher service is responsible for
- * setting the SPSR prior to entry into the BL32 image.
- */
- bl32_ep->spsr = 0;
+ fvp_set_bl32_entrypoint(bl32_ep);
}
/*******************************************************************************
@@ -255,25 +250,7 @@ void bl2_plat_bl32_post_load_actions(image_info_t *bl32_image,
void bl2_plat_bl33_post_load_actions(image_info_t *image,
el_change_info_t *bl33_ep)
{
- unsigned long el_status;
- unsigned int mode;
-
- /* Figure out what mode we enter the non-secure world in */
- el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
- el_status &= ID_AA64PFR0_ELX_MASK;
-
- if (el_status)
- mode = MODE_EL2;
- else
- mode = MODE_EL1;
-
- /*
- * TODO: Consider the possibility of specifying the SPSR in
- * the FIP ToC and allowing the platform to have a say as
- * well.
- */
- bl33_ep->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
- SET_SECURITY_STATE(bl33_ep->h.attr, NON_SECURE);
+ fvp_set_bl33_entrypoint(bl33_ep);
}
diff --git a/plat/fvp/bl31_plat_setup.c b/plat/fvp/bl31_plat_setup.c
index f4e74a5dce643877d7f38c9e9abe9bb5d60fff00..729ca5b28ea00a5141ce12aa6f445884a00cfce8 100644
--- a/plat/fvp/bl31_plat_setup.c
+++ b/plat/fvp/bl31_plat_setup.c
@@ -29,6 +29,7 @@
*/
#include <arch.h>
+#include <arch_helpers.h>
#include <assert.h>
#include <bl_common.h>
#include <bl31.h>
@@ -67,25 +68,43 @@ extern unsigned long __COHERENT_RAM_END__;
#define BL31_COHERENT_RAM_BASE (unsigned long)(&__COHERENT_RAM_START__)
#define BL31_COHERENT_RAM_LIMIT (unsigned long)(&__COHERENT_RAM_END__)
+
+#if RESET_TO_BL31
+static el_change_info_t bl32_entrypoint_info;
+static el_change_info_t bl33_entrypoint_info;
+#else
/*******************************************************************************
* Reference to structure which holds the arguments that have been passed to
* BL31 from BL2.
******************************************************************************/
static bl31_params_t *bl2_to_bl31_params;
+#endif
-/*******************************************************************************
+/*****************************************************************************
* Return a pointer to the 'el_change_info' structure of the next image for the
* security state specified. BL33 corresponds to the non-secure image type
* while BL32 corresponds to the secure image type. A NULL pointer is returned
* if the image does not exist.
- ******************************************************************************/
+ *****************************************************************************/
el_change_info_t *bl31_get_next_image_info(uint32_t type)
{
el_change_info_t *next_image_info;
+#if RESET_TO_BL31
+
+ if (type == NON_SECURE)
+ plat_get_entry_point_info(NON_SECURE, &bl33_entrypoint_info);
+ else
+ plat_get_entry_point_info(SECURE, &bl32_entrypoint_info);
+
+ next_image_info = (type == NON_SECURE) ?
+ &bl33_entrypoint_info :
+ &bl32_entrypoint_info;
+#else
next_image_info = (type == NON_SECURE) ?
bl2_to_bl31_params->bl33_ep :
bl2_to_bl31_params->bl32_ep;
+#endif
/* None of the images on this platform can have 0x0 as the entrypoint */
if (next_image_info->pc)
@@ -108,16 +127,26 @@ el_change_info_t *bl31_get_next_image_info(uint32_t type)
void bl31_early_platform_setup(bl31_params_t *from_bl2,
void *plat_params_from_bl2)
{
+ /* Initialize the platform config for future decision making */
+ platform_config_setup();
+
+ console_init(PL011_UART0_BASE);
+
+#if RESET_TO_BL31
+
+ /*
+ * Do initial security configuration to allow DRAM/device access. On
+ * Base FVP only DRAM security is programmable (via TrustZone), but
+ * other platforms might have more programmable security devices
+ * present.
+ */
+ plat_security_setup();
+#else
assert(from_bl2->h.type == PARAM_BL31);
assert(from_bl2->h.version >= VERSION_1);
bl2_to_bl31_params = from_bl2;
-
- /* Initialize the console to provide early debug support */
- console_init(PL011_UART0_BASE);
-
- /* Initialize the platform config for future decision making */
- platform_config_setup();
+#endif
}
/*******************************************************************************
@@ -166,6 +195,10 @@ void bl31_platform_setup()
******************************************************************************/
void bl31_plat_arch_setup()
{
+#if RESET_TO_BL31
+ plat_cci_setup();
+
+#endif
configure_mmu_el3(TZRAM_BASE,
TZRAM_SIZE,
BL31_RO_BASE,
@@ -173,3 +206,38 @@ void bl31_plat_arch_setup()
BL31_COHERENT_RAM_BASE,
BL31_COHERENT_RAM_LIMIT);
}
+
+#if RESET_TO_BL31
+/*******************************************************************************
+ * Generate the entry point info for Non Secure and Secure images
+ * for transferring control from BL31
+ ******************************************************************************/
+void plat_get_entry_point_info(unsigned long target_security,
+ el_change_info_t *target_entry_info)
+{
+ if (target_security == NON_SECURE) {
+ SET_PARAM_HEAD(target_entry_info,
+ PARAM_EP,
+ VERSION_1,
+ 0);
+ /*
+ * Tell BL31 where the non-trusted software image
+ * is located and the entry state information
+ */
+ target_entry_info->pc = plat_get_ns_image_entrypoint();
+
+ fvp_set_bl33_entrypoint(target_entry_info);
+
+ } else {
+ SET_PARAM_HEAD(target_entry_info,
+ PARAM_EP,
+ VERSION_1,
+ 0);
+ if (BL32_BASE != 0) {
+ /* Hard coding entry point to the base of the BL32 */
+ target_entry_info->pc = BL32_BASE;
+ fvp_set_bl32_entrypoint(target_entry_info);
+ }
+ }
+}
+#endif
diff --git a/plat/fvp/plat_pm.c b/plat/fvp/plat_pm.c
index f80e2d7b0a1c03ad81ee92b515028ca4c5bcc0e3..459d6221b4920e152f47ce2495e8f2efa42ed7c7 100644
--- a/plat/fvp/plat_pm.c
+++ b/plat/fvp/plat_pm.c
@@ -285,7 +285,7 @@ int fvp_affinst_on_finish(unsigned long mpidr,
unsigned int state)
{
int rc = PSCI_E_SUCCESS;
- unsigned long linear_id, cpu_setup, cci_setup;
+ unsigned long linear_id, cpu_setup;
mailbox_t *fvp_mboxes;
unsigned int gicd_base, gicc_base, reg_val, ectlr;
@@ -308,10 +308,7 @@ int fvp_affinst_on_finish(unsigned long mpidr,
*/
fvp_pwrc_write_pponr(mpidr);
- cci_setup = platform_get_cfgvar(CONFIG_HAS_CCI);
- if (cci_setup) {
- cci_enable_coherency(mpidr);
- }
+ plat_cci_setup();
}
break;
diff --git a/plat/fvp/platform.h b/plat/fvp/platform.h
index 69f2fb4067e16cc073052ad782c278e101c4579b..4c8d6064a1b6be5a2731a5e52e7de8ceb84b484a 100644
--- a/plat/fvp/platform.h
+++ b/plat/fvp/platform.h
@@ -32,6 +32,7 @@
#define __PLATFORM_H__
#include <arch.h>
+#include <bl_common.h>
/*******************************************************************************
@@ -414,6 +415,11 @@ extern void plat_report_exception(unsigned long);
extern unsigned long plat_get_ns_image_entrypoint(void);
extern unsigned long platform_get_stack(unsigned long mpidr);
extern uint64_t plat_get_syscnt_freq(void);
+#if RESET_TO_BL31
+extern void plat_get_entry_point_info(unsigned long target_security,
+ el_change_info_t *target_entry_info);
+#endif
+extern void plat_cci_setup(void);
/* Declarations for fvp_gic.c */
extern void gic_cpuif_deactivate(unsigned int);
@@ -476,6 +482,12 @@ extern void bl2_plat_get_bl32_meminfo(meminfo_t *mem_info);
/* Gets the memory layout for BL33 */
extern void bl2_plat_get_bl33_meminfo(meminfo_t *mem_info);
+/* Sets the entrypoint for BL32 */
+extern void fvp_set_bl32_entrypoint(el_change_info_t *bl32_ep);
+
+/* Sets the entrypoint for BL33 */
+extern void fvp_set_bl33_entrypoint(el_change_info_t *bl33_ep);
+
#endif /*__ASSEMBLY__*/
diff --git a/plat/fvp/platform.mk b/plat/fvp/platform.mk
index 511a25c2afc3804fe8f084a25ac4f86c6bf57e59..4de001bcb5f3f86eb6f86baefe7aeb083945c0db 100644
--- a/plat/fvp/platform.mk
+++ b/plat/fvp/platform.mk
@@ -45,7 +45,6 @@ PLAT_BL_COMMON_SOURCES := drivers/arm/pl011/pl011.c \
BL1_SOURCES += drivers/arm/cci400/cci400.c \
plat/common/aarch64/platform_up_stack.S \
plat/fvp/bl1_plat_setup.c \
- plat/fvp/aarch64/bl1_plat_helpers.S \
plat/fvp/aarch64/plat_common.c \
plat/fvp/aarch64/plat_helpers.S
@@ -67,3 +66,8 @@ BL31_SOURCES += drivers/arm/gic/gic_v2.c \
plat/fvp/aarch64/plat_helpers.S \
plat/fvp/aarch64/plat_common.c \
plat/fvp/drivers/pwrc/fvp_pwrc.c
+
+ifeq (${RESET_TO_BL31}, 1)
+ BL31_SOURCES += drivers/arm/tzc400/tzc400.c \
+ plat/fvp/plat_security.c
+endif