diff --git a/Makefile b/Makefile
index 8ec8472..1069cd5 100644
--- a/Makefile
+++ b/Makefile
@@ -47,6 +47,8 @@
 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 @@
 $(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 7259601..cbbaead 100644
--- a/bl1/aarch64/bl1_entrypoint.S
+++ b/bl1/aarch64/bl1_entrypoint.S
@@ -59,6 +59,15 @@
 	 */
 	bl	cpu_reset_handler
 
+	/* -------------------------------
+	 * Enable the instruction cache.
+	 * -------------------------------
+	 */
+	mrs	x0, sctlr_el3
+	orr	x0, x0, #SCTLR_I_BIT
+	msr	sctlr_el3, x0
+	isb
+
 	/* ---------------------------------------------
 	 * Set the exception vector to something sane.
 	 * ---------------------------------------------
@@ -90,16 +99,6 @@
 	msr	cptr_el3, x0
 
 	/* ---------------------------------------------
-	 * Enable the instruction cache.
-	 * ---------------------------------------------
-	 */
-	mrs	x0, sctlr_el3
-	orr	x0, x0, #SCTLR_I_BIT
-	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
@@ -107,22 +106,10 @@
 	 * 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 +135,38 @@
 	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 c67d029..f582e76 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -50,8 +50,28 @@
 	 * specific structure
 	 * ---------------------------------------------------------------
 	 */
+#if !RESET_TO_BL31
 	mov	x20, x0
 	mov	x21, x1
+#else
+
+	/* -----------------------------------------------------
+	 * Perform any processor specific actions upon reset
+	 * e.g. cache, tlb invalidations etc. Override the
+	 * Boot ROM(BL0) programming sequence
+	 * -----------------------------------------------------
+	 */
+	bl	cpu_reset_handler
+#endif
+
+	/* ---------------------------------------------
+	 * Enable the instruction cache.
+	 * ---------------------------------------------
+	 */
+	mrs	x1, sctlr_el3
+	orr	x1, x1, #SCTLR_I_BIT
+	msr	sctlr_el3, x1
+	isb
 
 	/* ---------------------------------------------
 	 * Set the exception vector to something sane.
@@ -83,15 +103,10 @@
 	bic	w1, w1, #TFP_BIT
 	msr	cptr_el3, x1
 
-	/* ---------------------------------------------
-	 * Enable the instruction cache.
-	 * ---------------------------------------------
-	 */
-	mrs	x1, sctlr_el3
-	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).
@@ -101,6 +116,7 @@
 	mrs	x0, mpidr_el1
 	bl	platform_is_primary_cpu
 	cbz	x0, _panic
+#endif
 
 	/* ---------------------------------------------
 	 * Zero out NOBITS sections. There are 2 of them:
@@ -134,8 +150,14 @@
 	 * Perform platform specific early arch. setup
 	 * ---------------------------------------------
 	 */
+#if RESET_TO_BL31
+	mov	x0, 0
+	mov	x1, 0
+#else
 	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 c0dc2fd..6c9650f 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -37,6 +37,7 @@
 				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 3dbd9f2..2bccf58 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 b4d4458..0000000
--- 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 b17093f..29bf602 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 @@
 
 	return counter_base_frequency;
 }
+
+void fvp_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_ep_info(entry_point_info_t *bl32_ep_info)
+{
+	SET_SECURITY_STATE(bl32_ep_info->h.attr, SECURE);
+	/*
+	 * The Secure Payload Dispatcher service is responsible for
+	 * setting the SPSR prior to entry into the BL32 image.
+	 */
+	bl32_ep_info->spsr = 0;
+}
+
+/*******************************************************************************
+ * Set SPSR and secure state for BL33 image
+ ******************************************************************************/
+void fvp_set_bl33_ep_info(entry_point_info_t *bl33_ep_info)
+{
+	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_info->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	SET_SECURITY_STATE(bl33_ep_info->h.attr, NON_SECURE);
+}
diff --git a/plat/fvp/aarch64/plat_helpers.S b/plat/fvp/aarch64/plat_helpers.S
index 0057903..f1c2c09 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 76d6963..45eb754 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,7 @@
  ******************************************************************************/
 void bl1_plat_arch_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());
-	}
+	fvp_cci_setup();
 
 	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 3d02c5c..ea9d0a4 100644
--- a/plat/fvp/bl2_plat_setup.c
+++ b/plat/fvp/bl2_plat_setup.c
@@ -242,12 +242,7 @@
 void bl2_plat_set_bl32_ep_info(image_info_t *bl32_image_info,
 					entry_point_info_t *bl32_ep_info)
 {
-	SET_SECURITY_STATE(bl32_ep_info->h.attr, SECURE);
-	/*
-	 * The Secure Payload Dispatcher service is responsible for
-	 * setting the SPSR prior to entry into the BL32 image.
-	*/
-	bl32_ep_info->spsr = 0;
+	fvp_set_bl32_ep_info(bl32_ep_info);
 }
 
 /*******************************************************************************
@@ -259,26 +254,7 @@
 void bl2_plat_set_bl33_ep_info(image_info_t *image,
 					entry_point_info_t *bl33_ep_info)
 {
-	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_info->spsr = SPSR_64(mode, MODE_SP_ELX,
-					DISABLE_ALL_EXCEPTIONS);
-	SET_SECURITY_STATE(bl33_ep_info->h.attr, NON_SECURE);
+	fvp_set_bl33_ep_info(bl33_ep_info);
 }
 
 
diff --git a/plat/fvp/bl31_plat_setup.c b/plat/fvp/bl31_plat_setup.c
index 949b156..033a8fa 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,11 +68,17 @@
 #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 entry_point_info_t  bl32_entrypoint_info;
+static entry_point_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 'entry_point_info' structure of the next image for the
@@ -83,9 +90,22 @@
 {
 	entry_point_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_info :
 		bl2_to_bl31_params->bl32_ep_info;
+#endif
+
 
 	/* None of the images on this platform can have 0x0 as the entrypoint */
 	if (next_image_info->pc)
@@ -108,16 +128,36 @@
 void bl31_early_platform_setup(bl31_params_t *from_bl2,
 				void *plat_params_from_bl2)
 {
-	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();
+
+#if RESET_TO_BL31
+	/* There are no parameters from BL2 if BL31 is a reset vector */
+	assert(from_bl2 == NULL);
+	assert(plat_params_from_bl2 == NULL);
+
+
+	/*
+	 * 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
+	/* Check params passed from BL2 should not be NULL,
+	 * We are not checking plat_params_from_bl2 as NULL as we are not
+	 * using it on FVP
+	 */
+	assert(from_bl2 != NULL);
+	assert(from_bl2->h.type == PARAM_BL31);
+	assert(from_bl2->h.version >= VERSION_1);
+
+	bl2_to_bl31_params = from_bl2;
+#endif
 }
 
 /*******************************************************************************
@@ -166,6 +206,10 @@
  ******************************************************************************/
 void bl31_plat_arch_setup()
 {
+#if RESET_TO_BL31
+	fvp_cci_setup();
+#endif
+
 	configure_mmu_el3(TZRAM_BASE,
 			  TZRAM_SIZE,
 			  BL31_RO_BASE,
@@ -173,3 +217,38 @@
 			  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,
+					entry_point_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_ep_info(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_ep_info(target_entry_info);
+		}
+	}
+}
+#endif
diff --git a/plat/fvp/plat_pm.c b/plat/fvp/plat_pm.c
index f80e2d7..c47cdb0 100644
--- a/plat/fvp/plat_pm.c
+++ b/plat/fvp/plat_pm.c
@@ -285,7 +285,7 @@
 			  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 @@
 			 */
 			fvp_pwrc_write_pponr(mpidr);
 
-			cci_setup = platform_get_cfgvar(CONFIG_HAS_CCI);
-			if (cci_setup) {
-				cci_enable_coherency(mpidr);
-			}
+			fvp_cci_setup();
 		}
 		break;
 
diff --git a/plat/fvp/platform.h b/plat/fvp/platform.h
index 50f1124..bd76d67 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>
 
 
 /*******************************************************************************
@@ -417,6 +418,12 @@
 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,
+				struct entry_point_info *target_entry_info);
+#endif
+
+extern void fvp_cci_setup(void);
 
 /* Declarations for fvp_gic.c */
 extern void gic_cpuif_deactivate(unsigned int);
@@ -480,6 +487,12 @@
 /* Gets the memory layout for BL33 */
 extern void bl2_plat_get_bl33_meminfo(struct meminfo *mem_info);
 
+/* Sets the entrypoint for BL32 */
+extern void fvp_set_bl32_ep_info(struct entry_point_info *bl32_ep_info);
+
+/* Sets the entrypoint for BL33 */
+extern void fvp_set_bl33_ep_info(struct entry_point_info *bl33_ep_info);
+
 
 #endif /*__ASSEMBLY__*/
 
diff --git a/plat/fvp/platform.mk b/plat/fvp/platform.mk
index 511a25c..4de001b 100644
--- a/plat/fvp/platform.mk
+++ b/plat/fvp/platform.mk
@@ -45,7 +45,6 @@
 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 @@
 				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