diff --git a/Makefile b/Makefile
index fef89c2..520a0d7 100644
--- a/Makefile
+++ b/Makefile
@@ -80,8 +80,12 @@
 
 ifneq (${DEBUG}, 0)
 	BUILD_TYPE	:=	debug
+	# Use LOG_LEVEL_INFO by default for debug builds
+	LOG_LEVEL	:=	40
 else
 	BUILD_TYPE	:=	release
+	# Use LOG_LEVEL_NOTICE by default for release builds
+	LOG_LEVEL	:=	20
 endif
 
 # Default build string (git branch and commit)
@@ -98,7 +102,6 @@
 				lib/aarch64/misc_helpers.S		\
 				lib/aarch64/xlat_helpers.c		\
 				lib/stdlib/std.c			\
-				lib/io_storage.c			\
 				plat/common/aarch64/platform_helpers.S
 
 BUILD_BASE		:=	./build
@@ -171,11 +174,10 @@
 
 INCLUDES		+=	-Iinclude/bl31			\
 				-Iinclude/bl31/services		\
-				-Iinclude/bl32			\
-				-Iinclude/bl32/payloads		\
 				-Iinclude/common		\
 				-Iinclude/drivers		\
 				-Iinclude/drivers/arm		\
+				-Iinclude/drivers/io		\
 				-Iinclude/lib			\
 				-Iinclude/lib/aarch64		\
 				-Iinclude/plat/common		\
@@ -213,6 +215,9 @@
 $(eval $(call assert_boolean,ASM_ASSERTION))
 $(eval $(call add_define,ASM_ASSERTION))
 
+# Process LOG_LEVEL flag
+$(eval $(call add_define,LOG_LEVEL))
+
 ASFLAGS			+= 	-nostdinc -ffreestanding -Wa,--fatal-warnings	\
 				-Werror -Wmissing-include-dirs			\
 				-mgeneral-regs-only -D__ASSEMBLY__		\
@@ -300,7 +305,7 @@
 
 $(OBJ) : $(2)
 	@echo "  CC      $$<"
-	$$(Q)$$(CC) $$(CFLAGS) -c $$< -o $$@
+	$$(Q)$$(CC) $$(CFLAGS) -DIMAGE_BL$(3) -c $$< -o $$@
 
 
 $(PREREQUISITES) : $(2)
@@ -322,7 +327,7 @@
 
 $(OBJ) : $(2)
 	@echo "  AS      $$<"
-	$$(Q)$$(AS) $$(ASFLAGS) -c $$< -o $$@
+	$$(Q)$$(AS) $$(ASFLAGS) -DIMAGE_BL$(3) -c $$< -o $$@
 
 $(PREREQUISITES) : $(2)
 	@echo "  DEPS    $$@"
@@ -359,11 +364,11 @@
 define MAKE_OBJS
 	$(eval C_OBJS := $(filter %.c,$(2)))
 	$(eval REMAIN := $(filter-out %.c,$(2)))
-	$(eval $(foreach obj,$(C_OBJS),$(call MAKE_C,$(1),$(obj))))
+	$(eval $(foreach obj,$(C_OBJS),$(call MAKE_C,$(1),$(obj),$(3))))
 
 	$(eval S_OBJS := $(filter %.S,$(REMAIN)))
 	$(eval REMAIN := $(filter-out %.S,$(REMAIN)))
-	$(eval $(foreach obj,$(S_OBJS),$(call MAKE_S,$(1),$(obj))))
+	$(eval $(foreach obj,$(S_OBJS),$(call MAKE_S,$(1),$(obj),$(3))))
 
 	$(and $(REMAIN),$(error Unexpected source files present: $(REMAIN)))
 endef
@@ -387,7 +392,7 @@
 	$(eval DUMP       := $(BUILD_DIR)/bl$(1).dump)
 	$(eval BIN        := $(BUILD_PLAT)/bl$(1).bin)
 
-	$(eval $(call MAKE_OBJS,$(BUILD_DIR),$(SOURCES)))
+	$(eval $(call MAKE_OBJS,$(BUILD_DIR),$(SOURCES),$(1)))
 	$(eval $(call MAKE_LD,$(LINKERFILE),$(BL$(1)_LINKERFILE)))
 
 $(BUILD_DIR) :
diff --git a/bl1/bl1.ld.S b/bl1/bl1.ld.S
index 967ba32..0ca4a63 100644
--- a/bl1/bl1.ld.S
+++ b/bl1/bl1.ld.S
@@ -35,8 +35,8 @@
 ENTRY(bl1_entrypoint)
 
 MEMORY {
-    ROM (rx): ORIGIN = TZROM_BASE, LENGTH = TZROM_SIZE
-    RAM (rwx): ORIGIN = TZRAM_BASE, LENGTH = TZRAM_SIZE
+    ROM (rx): ORIGIN = BL1_RO_BASE, LENGTH = BL1_RO_LIMIT
+    RAM (rwx): ORIGIN = BL1_RW_BASE, LENGTH = BL1_RW_LIMIT
 }
 
 SECTIONS
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
index c6f2caa..a5bd648 100644
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_main.c
@@ -106,6 +106,13 @@
   ******************************************************************************/
 void bl1_main(void)
 {
+	/* Announce our arrival */
+	NOTICE(FIRMWARE_WELCOME_STR);
+	NOTICE("BL1: %s\n", version_string);
+	NOTICE("BL1: %s\n", build_message);
+
+	INFO("BL1: RAM 0x%lx - 0x%lx\n", BL1_RAM_BASE, BL1_RAM_LIMIT);
+
 #if DEBUG
 	unsigned long sctlr_el3 = read_sctlr_el3();
 #endif
@@ -128,11 +135,6 @@
 	/* Perform platform setup in BL1. */
 	bl1_platform_setup();
 
-	/* Announce our arrival */
-	tf_printf(FIRMWARE_WELCOME_STR);
-	tf_printf("%s\n", version_string);
-	tf_printf("%s\n", build_message);
-
 	SET_PARAM_HEAD(&bl2_image_info, PARAM_IMAGE_BINARY, VERSION_1, 0);
 	SET_PARAM_HEAD(&bl2_ep, PARAM_EP, VERSION_1, 0);
 
@@ -150,7 +152,7 @@
 		 * TODO: print failure to load BL2 but also add a tzwdog timer
 		 * which will reset the system eventually.
 		 */
-		tf_printf("Failed to load boot loader stage 2 (BL2) firmware.\n");
+		ERROR("Failed to load BL2 firmware.\n");
 		panic();
 	}
 	/*
@@ -165,14 +167,13 @@
 
 	bl1_plat_set_bl2_ep_info(&bl2_image_info, &bl2_ep);
 	bl2_ep.args.arg1 = (unsigned long)bl2_tzram_layout;
-	tf_printf("Booting trusted firmware boot loader stage 2\n");
-#if DEBUG
-	tf_printf("BL2 address = 0x%llx\n",
+	NOTICE("BL1: Booting BL2\n");
+	INFO("BL1: BL2 address = 0x%llx\n",
 		(unsigned long long) bl2_ep.pc);
-	tf_printf("BL2 cpsr = 0x%x\n", bl2_ep.spsr);
-	tf_printf("BL2 memory layout address = 0x%llx\n",
-	       (unsigned long long) bl2_tzram_layout);
-#endif
+	INFO("BL1: BL2 spsr = 0x%x\n", bl2_ep.spsr);
+	VERBOSE("BL1: BL2 memory layout address = 0x%llx\n",
+		(unsigned long long) bl2_tzram_layout);
+
 	bl1_run_bl2(&bl2_ep);
 
 	return;
@@ -184,14 +185,13 @@
  ******************************************************************************/
 void display_boot_progress(entry_point_info_t *bl31_ep_info)
 {
-	tf_printf("Booting trusted firmware boot loader stage 3\n\r");
-#if DEBUG
-	tf_printf("BL31 address = 0x%llx\n", (unsigned long long)bl31_ep_info->pc);
-	tf_printf("BL31 cpsr = 0x%llx\n", (unsigned long long)bl31_ep_info->spsr);
-	tf_printf("BL31 params address = 0x%llx\n",
-			(unsigned long long)bl31_ep_info->args.arg0);
-	tf_printf("BL31 plat params address = 0x%llx\n",
-			(unsigned long long)bl31_ep_info->args.arg1);
-#endif
-	return;
+	NOTICE("BL1: Booting BL3-1\n");
+	INFO("BL1: BL3-1 address = 0x%llx\n",
+		(unsigned long long)bl31_ep_info->pc);
+	INFO("BL1: BL3-1 spsr = 0x%llx\n",
+		(unsigned long long)bl31_ep_info->spsr);
+	INFO("BL1: BL3-1 params address = 0x%llx\n",
+		(unsigned long long)bl31_ep_info->args.arg0);
+	INFO("BL1: BL3-1 plat params address = 0x%llx\n",
+		(unsigned long long)bl31_ep_info->args.arg1);
 }
diff --git a/bl2/bl2.ld.S b/bl2/bl2.ld.S
index e348d4f..1665f5d 100644
--- a/bl2/bl2.ld.S
+++ b/bl2/bl2.ld.S
@@ -35,7 +35,7 @@
 ENTRY(bl2_entrypoint)
 
 MEMORY {
-    RAM (rwx): ORIGIN = TZRAM_BASE, LENGTH = TZRAM_SIZE
+    RAM (rwx): ORIGIN = BL2_BASE, LENGTH = BL2_LIMIT
 }
 
 
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
index ca83842..51c55e0 100644
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -59,6 +59,7 @@
 	 * The entry point information is not relevant in this case as the AP
 	 * won't execute the BL3-0 image.
 	 */
+	INFO("BL2: Loading BL3-0\n");
 	bl2_plat_get_bl30_meminfo(&bl30_mem_info);
 	e = load_image(&bl30_mem_info,
 		       BL30_IMAGE_NAME,
@@ -87,6 +88,7 @@
 	meminfo_t *bl2_tzram_layout;
 	int e;
 
+	INFO("BL2: Loading BL3-1\n");
 	assert(bl2_to_bl31_params != NULL);
 	assert(bl31_ep_info != NULL);
 
@@ -125,6 +127,7 @@
 #ifdef BL32_BASE
 	meminfo_t bl32_mem_info;
 
+	INFO("BL2: Loading BL3-2\n");
 	assert(bl2_to_bl31_params != NULL);
 
 	/*
@@ -160,6 +163,7 @@
 	meminfo_t bl33_mem_info;
 	int e;
 
+	INFO("BL2: Loading BL3-3\n");
 	assert(bl2_to_bl31_params != NULL);
 
 	bl2_plat_get_bl33_meminfo(&bl33_mem_info);
@@ -189,15 +193,15 @@
 	entry_point_info_t *bl31_ep_info;
 	int e;
 
+	NOTICE("BL2: %s\n", version_string);
+	NOTICE("BL2: %s\n", build_message);
+
 	/* Perform remaining generic architectural setup in S-EL1 */
 	bl2_arch_setup();
 
 	/* Perform platform setup in BL2 */
 	bl2_platform_setup();
 
-	tf_printf("BL2 %s\n", version_string);
-	tf_printf("BL2 %s\n", build_message);
-
 	/*
 	 * Load the subsequent bootloader images
 	 */
diff --git a/bl31/bl31.ld.S b/bl31/bl31.ld.S
index 34a349f..83ef7e7 100644
--- a/bl31/bl31.ld.S
+++ b/bl31/bl31.ld.S
@@ -36,7 +36,7 @@
 
 
 MEMORY {
-    RAM (rwx): ORIGIN = TZRAM_BASE, LENGTH = TZRAM_SIZE
+    RAM (rwx): ORIGIN = BL31_BASE, LENGTH = BL31_LIMIT
 }
 
 
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index 68bdd36..19f3774 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -71,19 +71,20 @@
  ******************************************************************************/
 void bl31_main(void)
 {
+	NOTICE("BL3-1: %s\n", version_string);
+	NOTICE("BL3-1: %s\n", build_message);
+
 	/* Perform remaining generic architectural setup from EL3 */
 	bl31_arch_setup();
 
 	/* Perform platform setup in BL1 */
 	bl31_platform_setup();
 
-	tf_printf("BL31 %s\n", version_string);
-	tf_printf("BL31 %s\n", build_message);
-
 	/* Initialise helper libraries */
 	bl31_lib_init();
 
 	/* Initialize the runtime services e.g. psci */
+	INFO("BL3-1: Initializing runtime services\n");
 	runtime_svc_init();
 
 	/* Clean caches before re-entering normal world */
@@ -102,9 +103,10 @@
 	/*
 	 * If SPD had registerd an init hook, invoke it.
 	 */
-	if (bl32_init)
+	if (bl32_init) {
+		INFO("BL3-1: Initializing BL3-2\n");
 		(*bl32_init)();
-
+	}
 	/*
 	 * We are ready to enter the next EL. Prepare entry into the image
 	 * corresponding to the desired security state after the next ERET.
@@ -148,6 +150,11 @@
 	assert(next_image_info);
 	assert(image_type == GET_SECURITY_STATE(next_image_info->h.attr));
 
+	INFO("BL3-1: Preparing for EL3 exit to %s world\n",
+		(image_type == SECURE) ? "secure" : "normal");
+	INFO("BL3-1: Next image address = 0x%llx\n",
+		(unsigned long long) next_image_info->pc);
+	INFO("BL3-1: Next image spsr = 0x%x\n", next_image_info->spsr);
 	cm_init_context(read_mpidr_el1(), next_image_info);
 	cm_prepare_el3_exit(image_type);
 }
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 002c41b..8fae1b2 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -32,6 +32,7 @@
 #include <asm_macros.S>
 #include <tsp.h>
 #include <xlat_tables.h>
+#include "../tsp_private.h"
 
 
 	.globl	tsp_entrypoint
@@ -127,8 +128,8 @@
 	 * specific early arch. setup e.g. mmu setup
 	 * ---------------------------------------------
 	 */
-	bl	bl32_early_platform_setup
-	bl	bl32_plat_arch_setup
+	bl	tsp_early_platform_setup
+	bl	tsp_plat_arch_setup
 
 	/* ---------------------------------------------
 	 * Jump to main function.
diff --git a/bl32/tsp/tsp.ld.S b/bl32/tsp/tsp.ld.S
index 5807141..5d7ffa1 100644
--- a/bl32/tsp/tsp.ld.S
+++ b/bl32/tsp/tsp.ld.S
@@ -68,8 +68,8 @@
         __DATA_END__ = .;
     } >RAM
 
-#ifdef BL32_PROGBITS_LIMIT
-    ASSERT(. <= BL32_PROGBITS_LIMIT, "BL3-2 progbits has exceeded its limit.")
+#ifdef TSP_PROGBITS_LIMIT
+    ASSERT(. <= TSP_PROGBITS_LIMIT, "TSP progbits has exceeded its limit.")
 #endif
 
     stacks (NOLOAD) : {
diff --git a/bl32/tsp/tsp.mk b/bl32/tsp/tsp.mk
index 02cc13d..f17ef1e 100644
--- a/bl32/tsp/tsp.mk
+++ b/bl32/tsp/tsp.mk
@@ -28,6 +28,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
+INCLUDES		+=	-Iinclude/bl32/tsp
+
 BL32_SOURCES		+=	bl32/tsp/tsp_main.c			\
 				bl32/tsp/aarch64/tsp_entrypoint.S	\
 				bl32/tsp/aarch64/tsp_exceptions.S	\
@@ -50,7 +52,7 @@
 # Include the platform-specific TSP Makefile
 # If no platform-specific TSP Makefile exists, it means TSP is not supported
 # on this platform.
-TSP_PLAT_MAKEFILE := bl32/tsp/tsp-${PLAT}.mk
+TSP_PLAT_MAKEFILE := plat/${PLAT}/tsp/tsp-${PLAT}.mk
 ifeq (,$(wildcard ${TSP_PLAT_MAKEFILE}))
   $(error TSP is not supported on platform ${PLAT})
 else
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
index c1c9aad..7163bad 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -32,9 +32,10 @@
 #include <assert.h>
 #include <debug.h>
 #include <gic_v2.h>
-#include <tsp.h>
 #include <platform.h>
 #include <platform_def.h>
+#include <tsp.h>
+#include "tsp_private.h"
 
 /*******************************************************************************
  * This function updates the TSP statistics for FIQs handled synchronously i.e
@@ -55,14 +56,16 @@
 	if (type == TSP_HANDLE_FIQ_AND_RETURN)
 		tsp_stats[linear_id].sync_fiq_ret_count++;
 
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	spin_lock(&console_lock);
-	tf_printf("TSP: cpu 0x%x sync fiq request from 0x%llx \n\r",
-	       mpidr, elr_el3);
-	INFO("cpu 0x%x: %d sync fiq requests, %d sync fiq returns\n",
-	     mpidr,
-	     tsp_stats[linear_id].sync_fiq_count,
-	     tsp_stats[linear_id].sync_fiq_ret_count);
+	VERBOSE("TSP: cpu 0x%x sync fiq request from 0x%llx\n",
+		mpidr, elr_el3);
+	VERBOSE("TSP: cpu 0x%x: %d sync fiq requests, %d sync fiq returns\n",
+		mpidr,
+		tsp_stats[linear_id].sync_fiq_count,
+		tsp_stats[linear_id].sync_fiq_ret_count);
 	spin_unlock(&console_lock);
+#endif
 }
 
 /*******************************************************************************
@@ -85,7 +88,7 @@
 	id = plat_ic_get_pending_interrupt_id();
 
 	/* TSP can only handle the secure physical timer interrupt */
-	if (id != IRQ_SEC_PHY_TIMER)
+	if (id != TSP_IRQ_SEC_PHY_TIMER)
 		return TSP_EL3_FIQ;
 
 	/*
@@ -93,19 +96,20 @@
 	 * another secure interrupt through an assertion.
 	 */
 	id = plat_ic_acknowledge_interrupt();
-	assert(id == IRQ_SEC_PHY_TIMER);
+	assert(id == TSP_IRQ_SEC_PHY_TIMER);
 	tsp_generic_timer_handler();
 	plat_ic_end_of_interrupt(id);
 
 	/* Update the statistics and print some messages */
 	tsp_stats[linear_id].fiq_count++;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	spin_lock(&console_lock);
-	tf_printf("TSP: cpu 0x%x handled fiq %d \n\r",
+	VERBOSE("TSP: cpu 0x%x handled fiq %d\n",
 	       mpidr, id);
-	INFO("cpu 0x%x: %d fiq requests \n",
+	VERBOSE("TSP: cpu 0x%x: %d fiq requests\n",
 	     mpidr, tsp_stats[linear_id].fiq_count);
 	spin_unlock(&console_lock);
-
+#endif
 	return 0;
 }
 
@@ -115,11 +119,12 @@
 	uint32_t linear_id = platform_get_core_pos(mpidr);
 
 	tsp_stats[linear_id].irq_count++;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	spin_lock(&console_lock);
-	tf_printf("TSP: cpu 0x%x received irq\n\r", mpidr);
-	INFO("cpu 0x%x: %d irq requests \n",
-	     mpidr, tsp_stats[linear_id].irq_count);
+	VERBOSE("TSP: cpu 0x%x received irq\n", mpidr);
+	VERBOSE("TSP: cpu 0x%x: %d irq requests\n",
+		mpidr, tsp_stats[linear_id].irq_count);
 	spin_unlock(&console_lock);
-
+#endif
 	return TSP_PREEMPTED;
 }
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index b2850e9..08d89c3 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -33,8 +33,10 @@
 #include <debug.h>
 #include <platform.h>
 #include <platform_def.h>
+#include <platform_tsp.h>
 #include <spinlock.h>
 #include <tsp.h>
+#include "tsp_private.h"
 
 /*******************************************************************************
  * Declarations of linker defined symbols which will help us find the layout
@@ -105,11 +107,17 @@
  ******************************************************************************/
 uint64_t tsp_main(void)
 {
+	NOTICE("TSP: %s\n", version_string);
+	NOTICE("TSP: %s\n", build_message);
+	INFO("TSP: Total memory base : 0x%x\n", (unsigned long)BL32_TOTAL_BASE);
+	INFO("TSP: Total memory size : 0x%x bytes\n",
+			 (unsigned long)(BL32_TOTAL_LIMIT - BL32_TOTAL_BASE));
+
 	uint64_t mpidr = read_mpidr();
 	uint32_t linear_id = platform_get_core_pos(mpidr);
 
 	/* Initialize the platform */
-	bl32_platform_setup();
+	tsp_platform_setup();
 
 	/* Initialize secure/applications state here */
 	tsp_generic_timer_start();
@@ -119,18 +127,14 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_on_count++;
 
+#if LOG_LEVEL >= LOG_LEVEL_INFO
 	spin_lock(&console_lock);
-	tf_printf("TSP %s\n", version_string);
-	tf_printf("TSP %s\n", build_message);
-	INFO("Total memory base : 0x%x\n", (unsigned long)BL32_TOTAL_BASE);
-	INFO("Total memory size : 0x%x bytes\n",
-			 (unsigned long)(BL32_TOTAL_LIMIT - BL32_TOTAL_BASE));
-	INFO("cpu 0x%x: %d smcs, %d erets %d cpu on requests\n", mpidr,
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets %d cpu on requests\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
 	     tsp_stats[linear_id].eret_count,
 	     tsp_stats[linear_id].cpu_on_count);
 	spin_unlock(&console_lock);
-
+#endif
 	return (uint64_t) &tsp_vector_table;
 }
 
@@ -152,14 +156,15 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_on_count++;
 
+#if LOG_LEVEL >= LOG_LEVEL_INFO
 	spin_lock(&console_lock);
-	tf_printf("SP: cpu 0x%x turned on\n\r", mpidr);
-	INFO("cpu 0x%x: %d smcs, %d erets %d cpu on requests\n", mpidr,
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count,
-	     tsp_stats[linear_id].cpu_on_count);
+	INFO("TSP: cpu 0x%x turned on\n", mpidr);
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets %d cpu on requests\n", mpidr,
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_on_count);
 	spin_unlock(&console_lock);
-
+#endif
 	/* Indicate to the SPD that we have completed turned ourselves on */
 	return set_smc_args(TSP_ON_DONE, 0, 0, 0, 0, 0, 0, 0);
 }
@@ -192,14 +197,15 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_off_count++;
 
+#if LOG_LEVEL >= LOG_LEVEL_INFO
 	spin_lock(&console_lock);
-	tf_printf("SP: cpu 0x%x off request\n\r", mpidr);
-	INFO("cpu 0x%x: %d smcs, %d erets %d cpu off requests\n", mpidr,
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count,
-	     tsp_stats[linear_id].cpu_off_count);
+	INFO("TSP: cpu 0x%x off request\n", mpidr);
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets %d cpu off requests\n", mpidr,
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_off_count);
 	spin_unlock(&console_lock);
-
+#endif
 
 	/* Indicate to the SPD that we have completed this request */
 	return set_smc_args(TSP_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
@@ -234,14 +240,17 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_suspend_count++;
 
+#if LOG_LEVEL >= LOG_LEVEL_INFO
 	spin_lock(&console_lock);
-	tf_printf("SP: cpu 0x%x suspend request. power state: 0x%x\n\r",
-	       mpidr, power_state);
-	INFO("cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n", mpidr,
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count,
-	     tsp_stats[linear_id].cpu_suspend_count);
+	INFO("TSP: cpu 0x%x suspend request. power state: 0x%x\n",
+		mpidr, power_state);
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n",
+		mpidr,
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_suspend_count);
 	spin_unlock(&console_lock);
+#endif
 
 	/* Indicate to the SPD that we have completed this request */
 	return set_smc_args(TSP_SUSPEND_DONE, 0, 0, 0, 0, 0, 0, 0);
@@ -272,15 +281,17 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_resume_count++;
 
+#if LOG_LEVEL >= LOG_LEVEL_INFO
 	spin_lock(&console_lock);
-	tf_printf("SP: cpu 0x%x resumed. suspend level %d \n\r",
-	       mpidr, suspend_level);
-	INFO("cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n", mpidr,
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count,
-	     tsp_stats[linear_id].cpu_suspend_count);
+	INFO("TSP: cpu 0x%x resumed. suspend level %d\n",
+		mpidr, suspend_level);
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n",
+		mpidr,
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_suspend_count);
 	spin_unlock(&console_lock);
-
+#endif
 	/* Indicate to the SPD that we have completed this request */
 	return set_smc_args(TSP_RESUME_DONE, 0, 0, 0, 0, 0, 0, 0);
 }
@@ -304,18 +315,17 @@
 	uint64_t service_args[2];
 	uint64_t mpidr = read_mpidr();
 	uint32_t linear_id = platform_get_core_pos(mpidr);
-	const char *smc_type;
 
 	/* Update this cpu's statistics */
 	tsp_stats[linear_id].smc_count++;
 	tsp_stats[linear_id].eret_count++;
 
-	smc_type = ((func >> 31) & 1) == 1 ? "fast" : "standard";
-
-	tf_printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
-	INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count);
+	INFO("TSP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(),
+		((func >> 31) & 1) == 1 ? "fast" : "standard",
+		func);
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets\n", mpidr,
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count);
 
 	/* Render secure services and obtain results here */
 	results[0] = arg1;
diff --git a/include/bl32/payloads/tsp.h b/bl32/tsp/tsp_private.h
similarity index 61%
rename from include/bl32/payloads/tsp.h
rename to bl32/tsp/tsp_private.h
index 2db3b34..39fb5f6 100644
--- a/include/bl32/payloads/tsp.h
+++ b/bl32/tsp/tsp_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 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:
@@ -28,75 +28,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef __TSP_H__
-#define __TSP_H__
-
-/*
- * SMC function IDs that TSP uses to signal various forms of completions
- * to the secure payload dispatcher.
- */
-#define TSP_ENTRY_DONE		0xf2000000
-#define TSP_ON_DONE		0xf2000001
-#define TSP_OFF_DONE		0xf2000002
-#define TSP_SUSPEND_DONE	0xf2000003
-#define TSP_RESUME_DONE		0xf2000004
-#define TSP_PREEMPTED		0xf2000005
-
-/*
- * Function identifiers to handle FIQs through the synchronous handling model.
- * If the TSP was previously interrupted then control has to be returned to
- * the TSPD after handling the interrupt else execution can remain in the TSP.
- */
-#define TSP_HANDLED_S_EL1_FIQ		0xf2000006
-#define TSP_EL3_FIQ			0xf2000007
-
-/* SMC function ID that TSP uses to request service from secure monitor */
-#define TSP_GET_ARGS		0xf2001000
-
-/*
- * Identifiers for various TSP services. Corresponding function IDs (whether
- * fast or standard) are generated by macros defined below
- */
-#define TSP_ADD		0x2000
-#define TSP_SUB		0x2001
-#define TSP_MUL		0x2002
-#define TSP_DIV		0x2003
-#define TSP_HANDLE_FIQ_AND_RETURN	0x2004
-
-/*
- * Generate function IDs for TSP services to be used in SMC calls, by
- * appropriately setting bit 31 to differentiate standard and fast SMC calls
- */
-#define TSP_STD_FID(fid)	((fid) | 0x72000000 | (0 << 31))
-#define TSP_FAST_FID(fid)	((fid) | 0x72000000 | (1 << 31))
-
-/* SMC function ID to request a previously preempted std smc */
-#define TSP_FID_RESUME		TSP_STD_FID(0x3000)
-
-/*
- * Identify a TSP service from function ID filtering the last 16 bits from the
- * SMC function ID
- */
-#define TSP_BARE_FID(fid)	((fid) & 0xffff)
-
-/*
- * Total number of function IDs implemented for services offered to NS clients.
- * The function IDs are defined above
- */
-#define TSP_NUM_FID		0x4
-
-/* TSP implementation version numbers */
-#define TSP_VERSION_MAJOR	0x0 /* Major version */
-#define TSP_VERSION_MINOR	0x1 /* Minor version */
-
-/*
- * Standard Trusted OS Function IDs that fall under Trusted OS call range
- * according to SMC calling convention
- */
-#define TOS_CALL_COUNT		0xbf00ff00 /* Number of calls implemented */
-#define TOS_UID			0xbf00ff01 /* Implementation UID */
-/*				0xbf00ff02 is reserved */
-#define TOS_CALL_VERSION	0xbf00ff03 /* Trusted OS Call Version */
+#ifndef __TSP_PRIVATE_H__
+#define __TSP_PRIVATE_H__
 
 /* Definitions to help the assembler access the SMC/ERET args structure */
 #define TSP_ARGS_SIZE		0x40
@@ -110,24 +43,15 @@
 #define TSP_ARG7		0x38
 #define TSP_ARGS_END		0x40
 
+
 #ifndef __ASSEMBLY__
 
 #include <cassert.h>
 #include <platform_def.h> /* For CACHE_WRITEBACK_GRANULE */
 #include <spinlock.h>
 #include <stdint.h>
+#include <tsp.h>
 
-typedef uint32_t tsp_vector_isn_t;
-
-typedef struct tsp_vectors {
-	tsp_vector_isn_t std_smc_entry;
-	tsp_vector_isn_t fast_smc_entry;
-	tsp_vector_isn_t cpu_on_entry;
-	tsp_vector_isn_t cpu_off_entry;
-	tsp_vector_isn_t cpu_resume_entry;
-	tsp_vector_isn_t cpu_suspend_entry;
-	tsp_vector_isn_t fiq_entry;
-} tsp_vectors_t;
 
 typedef struct work_statistics {
 	uint32_t fiq_count;		/* Number of FIQs on this cpu */
@@ -150,7 +74,6 @@
 #define read_sp_arg(args, offset)	((args)->_regs[offset >> 3])
 #define write_sp_arg(args, offset, val) (((args)->_regs[offset >> 3])	\
 					 = val)
-
 /*
  * Ensure that the assembler's view of the size of the tsp_args is the
  * same as the compilers
@@ -195,6 +118,7 @@
 /* FIQ management functions */
 void tsp_update_sync_fiq_stats(uint32_t type, uint64_t elr_el3);
 
+
 /* Data structure to keep track of TSP statistics */
 extern spinlock_t console_lock;
 extern work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
@@ -202,6 +126,8 @@
 /* Vector table of jumps */
 extern tsp_vectors_t tsp_vector_table;
 
+
 #endif /* __ASSEMBLY__ */
 
-#endif /* __BL2_H__ */
+#endif /* __TSP_PRIVATE_H__ */
+
diff --git a/bl32/tsp/tsp_timer.c b/bl32/tsp/tsp_timer.c
index a7fdfda..d9460b6 100644
--- a/bl32/tsp/tsp_timer.c
+++ b/bl32/tsp/tsp_timer.c
@@ -30,7 +30,7 @@
 #include <arch_helpers.h>
 #include <assert.h>
 #include <platform.h>
-#include <tsp.h>
+#include "tsp_private.h"
 
 /*******************************************************************************
  * Data structure to keep track of per-cpu secure generic timer context across
diff --git a/common/bl_common.c b/common/bl_common.c
index d2c60ef..60f8b2f 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -137,7 +137,7 @@
 	if (pos == BOTTOM)
 		*free_base = addr + size;
 
-	INFO("Reserved %u bytes (discarded %u bytes %s)\n",
+	VERBOSE("Reserved %u bytes (discarded %u bytes %s)\n",
 	     reserved_size, discard_size,
 	     pos == TOP ? "above" : "below");
 }
@@ -146,15 +146,13 @@
 			   unsigned long image_size,
 			   const meminfo_t *mem_layout)
 {
-#if DEBUG
-	tf_printf("Trying to load image at address 0x%lx, size = 0x%lx\r\n",
+	INFO("Trying to load image at address 0x%lx, size = 0x%lx\n",
 		image_load_addr, image_size);
-	tf_printf("Current memory layout:\r\n");
-	tf_printf("  total region = [0x%lx, 0x%lx]\r\n", mem_layout->total_base,
+	INFO("Current memory layout:\n");
+	INFO("  total region = [0x%lx, 0x%lx]\n", mem_layout->total_base,
 			mem_layout->total_base + mem_layout->total_size);
-	tf_printf("  free region = [0x%lx, 0x%lx]\r\n", mem_layout->free_base,
+	INFO("  free region = [0x%lx, 0x%lx]\n", mem_layout->free_base,
 			mem_layout->free_base + mem_layout->free_size);
-#endif
 }
 
 /* Generic function to return the size of an image */
diff --git a/docs/firmware-design.md b/docs/firmware-design.md
index 3203a52..e98c4fa 100644
--- a/docs/firmware-design.md
+++ b/docs/firmware-design.md
@@ -955,22 +955,95 @@
 bytes at the location of this NOBITS section, making the image unnecessarily
 bigger. Smaller images allow faster loading from the FIP to the main memory.
 
-On FVP platforms, we use the Trusted ROM and Trusted SRAM to store the trusted
-firmware binaries.
+On FVP platforms, we use the Trusted ROM, Trusted SRAM and, optionally, Trusted
+DRAM to store the trusted firmware binaries and shared data.
+
+ *    A 4KB page of shared memory is used to store the entrypoint mailboxes
+      and the parameters passed between bootloaders. The shared memory can be
+      allocated either at the top of Trusted SRAM or at the base of Trusted
+      DRAM at build time. When allocated in Trusted SRAM, the amount of Trusted
+      SRAM available to load the bootloader images will be reduced by the size
+      of the shared memory.
 
  *    BL1 is originally sitting in the Trusted ROM at address `0x0`. Its
       read-write data are relocated at the top of the Trusted SRAM at runtime.
+      If the shared memory is allocated in Trusted SRAM, the BL1 read-write data
+      is relocated just below the shared memory.
 
  *    BL3-1 is loaded at the top of the Trusted SRAM, such that its NOBITS
       sections will overwrite BL1 R/W data.
 
  *    BL2 is loaded below BL3-1.
 
- *    The TSP is loaded as the BL3-2 image at the base of the Trusted SRAM. Its
-      NOBITS sections are allowed to overlay BL2.
+ *    The TSP is loaded as the BL3-2 image at the base of either the Trusted
+      SRAM or Trusted DRAM. When loaded into Trusted SRAM, its NOBITS sections
+      are allowed to overlay BL2. When loaded into Trusted DRAM, an offset
+      corresponding to the size of the shared memory is applied to avoid
+      overlap.
 
 This memory layout is designed to give the BL3-2 image as much memory as
-possible. It is illustrated by the following diagram.
+possible when it is loaded into Trusted SRAM. Depending on the location of the
+shared memory page and the TSP, it will result in different memory maps,
+illustrated by the following diagrams.
+
+** Shared data & TSP in Trusted SRAM (default option): **
+
+               Trusted SRAM
+    0x04040000 +----------+
+               |  Shared  |
+    0x0403F000 +----------+  loaded by BL2  ------------------
+               | BL1 (rw) |  <<<<<<<<<<<<<  |  BL3-1 NOBITS  |
+               |----------|  <<<<<<<<<<<<<  |----------------|
+               |          |  <<<<<<<<<<<<<  | BL3-1 PROGBITS |
+               |----------|                 ------------------
+               |   BL2    |  <<<<<<<<<<<<<  |  BL3-2 NOBITS  |
+               |----------|  <<<<<<<<<<<<<  |----------------|
+               |          |  <<<<<<<<<<<<<  | BL3-2 PROGBITS |
+    0x04000000 +----------+                 ------------------
+
+               Trusted ROM
+    0x04000000 +----------+
+               | BL1 (ro) |
+    0x00000000 +----------+
+
+
+** Shared data & TSP in Trusted DRAM: **
+
+               Trusted DRAM
+    0x08000000 +----------+
+               |          |
+               |  BL3-2   |
+               |          |
+    0x06001000 |----------|
+               |  Shared  |
+    0x06000000 +----------+
+
+               Trusted SRAM
+    0x04040000 +----------+  loaded by BL2  ------------------
+               | BL1 (rw) |  <<<<<<<<<<<<<  |  BL3-1 NOBITS  |
+               |----------|  <<<<<<<<<<<<<  |----------------|
+               |          |  <<<<<<<<<<<<<  | BL3-1 PROGBITS |
+               |----------|                 ------------------
+               |   BL2    |
+               |----------|
+               |          |
+    0x04000000 +----------+
+
+               Trusted ROM
+    0x04000000 +----------+
+               | BL1 (ro) |
+    0x00000000 +----------+
+
+** Shared data in Trusted DRAM, TSP in Trusted SRAM: **
+
+               Trusted DRAM
+    0x08000000 +----------+
+               |          |
+               |          |
+               |          |
+    0x06001000 |----------|
+               |  Shared  |
+    0x06000000 +----------+
 
                Trusted SRAM
     0x04040000 +----------+  loaded by BL2  ------------------
@@ -988,8 +1061,8 @@
                | BL1 (ro) |
     0x00000000 +----------+
 
-The TSP image may be loaded in Trusted DRAM instead. This doesn't change the
-memory layout of the other boot loader images in Trusted SRAM.
+Loading the TSP image in Trusted DRAM doesn't change the memory layout of the
+other boot loader images in Trusted SRAM.
 
 Each bootloader stage image layout is described by its own linker script. The
 linker scripts export some symbols into the program symbol table. Their values
@@ -1226,6 +1299,11 @@
     lib          Yes             Yes             Yes
     services     No              No              Yes
 
+The build system provides a non configurable build option IMAGE_BLx for each
+boot loader stage (where x = BL stage). e.g. for BL1 , IMAGE_BL1 will be
+defined by the build system. This enables the Trusted Firmware to compile
+certain code only for specific boot loader stages
+
 All assembler files have the `.S` extension. The linker source files for each
 boot stage have the extension `.ld.S`. These are processed by GCC to create the
 linker scripts which have the extension `.ld`.
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index db2bad8..62ea6a0 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -150,31 +150,6 @@
     Defines the total number of nodes in the affinity heirarchy at all affinity
     levels used by the platform.
 
-*   **#define : TZROM_BASE**
-
-    Defines the base address of secure ROM on the platform, where the BL1 binary
-    is loaded. This constant is used by the linker scripts to ensure that the
-    BL1 image fits into the available memory.
-
-*   **#define : TZROM_SIZE**
-
-    Defines the size of secure ROM on the platform. This constant is used by the
-    linker scripts to ensure that the BL1 image fits into the available memory.
-
-*   **#define : TZRAM_BASE**
-
-    Defines the base address of the secure RAM on platform, where the data
-    section of the BL1 binary is loaded. The BL2 and BL3-1 images are also
-    loaded in this secure RAM region. This constant is used by the linker
-    scripts to ensure that the BL1 data section and BL2/BL3-1 binary images fit
-    into the available memory.
-
-*   **#define : TZRAM_SIZE**
-
-    Defines the size of the secure RAM on the platform. This constant is used by
-    the linker scripts to ensure that the BL1 data section and BL2/BL3-1 binary
-    images fit into the available memory.
-
 *   **#define : BL1_RO_BASE**
 
     Defines the base address in secure ROM where BL1 originally lives. Must be
@@ -218,30 +193,57 @@
     Defines the base address in non-secure DRAM where BL2 loads the BL3-3 binary
     image. Must be aligned on a page-size boundary.
 
-If the BL3-2 image is supported by the platform, the following constants must
-be defined as well:
+If a BL3-2 image is supported by the platform, the following constants must
+also be defined:
 
-*   **#define : TSP_SEC_MEM_BASE**
+*   **#define : BL32_IMAGE_NAME**
 
-    Defines the base address of the secure memory used by the BL3-2 image on the
-    platform.
-
-*   **#define : TSP_SEC_MEM_SIZE**
-
-    Defines the size of the secure memory used by the BL3-2 image on the
-    platform.
+    Name of the BL3-2 binary image on the host file-system. This name is used by
+    BL2 to load BL3-2 into secure memory from platform storage.
 
 *   **#define : BL32_BASE**
 
     Defines the base address in secure memory where BL2 loads the BL3-2 binary
-    image. Must be inside the secure memory identified by `TSP_SEC_MEM_BASE` and
-    `TSP_SEC_MEM_SIZE` constants. Must also be aligned on a page-size boundary.
+    image. Must be aligned on a page-size boundary.
 
 *   **#define : BL32_LIMIT**
 
-    Defines the maximum address that the BL3-2 image can occupy. Must be inside
-    the secure memory identified by `TSP_SEC_MEM_BASE` and `TSP_SEC_MEM_SIZE`
-    constants.
+    Defines the maximum address that the BL3-2 image can occupy.
+
+If the Test Secure-EL1 Payload (TSP) instantiation of BL3-2 is supported by the
+platform, the following constants must also be defined:
+
+*   **#define : TSP_SEC_MEM_BASE**
+
+    Defines the base address of the secure memory used by the TSP image on the
+    platform. This must be at the same address or below `BL32_BASE`.
+
+*   **#define : TSP_SEC_MEM_SIZE**
+
+    Defines the size of the secure memory used by the BL3-2 image on the
+    platform. `TSP_SEC_MEM_BASE` and `TSP_SEC_MEM_SIZE` must fully accomodate
+    the memory required by the BL3-2 image, defined by `BL32_BASE` and
+    `BL32_LIMIT`.
+
+*   **#define : TSP_IRQ_SEC_PHY_TIMER**
+
+    Defines the ID of the secure physical generic timer interrupt used by the
+    TSP's interrupt handling code.
+
+If the platform port uses the IO storage framework, the following constants
+must also be defined:
+
+*   **#define : MAX_IO_DEVICES**
+
+    Defines the maximum number of registered IO devices. Attempting to register
+    more devices than this value using `io_register_device()` will fail with
+    IO_RESOURCES_EXHAUSTED.
+
+*   **#define : MAX_IO_HANDLES**
+
+    Defines the maximum number of open IO handles. Attempting to open more IO
+    entities than this value using `io_open()` will fail with
+    IO_RESOURCES_EXHAUSTED.
 
 The following constants are optional. They should be defined when the platform
 memory layout implies some image overlaying like on FVP.
@@ -251,7 +253,7 @@
     Defines the maximum address in secure RAM that the BL3-1's progbits sections
     can occupy.
 
-*   **#define : BL32_PROGBITS_LIMIT**
+*   **#define : TSP_PROGBITS_LIMIT**
 
     Defines the maximum address that the TSP's progbits sections can occupy.
 
diff --git a/docs/user-guide.md b/docs/user-guide.md
index ef5de71..532236a 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -51,10 +51,10 @@
 
 *   Baremetal GNU GCC tools. Verified packages can be downloaded from [Linaro]
     [Linaro Toolchain]. The rest of this document assumes that the
-    `gcc-linaro-aarch64-none-elf-4.8-2013.11_linux.tar.xz` tools are used.
+    `gcc-linaro-aarch64-none-elf-4.9-2014.07_linux.tar.xz` tools are used.
 
-        wget http://releases.linaro.org/13.11/components/toolchain/binaries/gcc-linaro-aarch64-none-elf-4.8-2013.11_linux.tar.xz
-        tar -xf gcc-linaro-aarch64-none-elf-4.8-2013.11_linux.tar.xz
+        wget http://releases.linaro.org/14.07/components/toolchain/binaries/gcc-linaro-aarch64-none-elf-4.9-2014.07_linux.tar.xz
+        tar -xf gcc-linaro-aarch64-none-elf-4.9-2014.07_linux.tar.xz
 
 *   The Device Tree Compiler (DTC) included with Linux kernel 3.15-rc6 is used
     to build the Flattened Device Tree (FDT) source files (`.dts` files)
@@ -133,6 +133,8 @@
 of the build options are changed from a previous build, a clean build must be
 performed.
 
+#### Common build options
+
 *   `BL30`: Path to BL3-0 image in the host file system. This image is optional.
     If a BL3-0 image is present then this option must be passed for the `fip`
     target
@@ -146,6 +148,19 @@
 *   `DEBUG`: Chooses between a debug and release build. It can take either 0
     (release) or 1 (debug) as values. 0 is the default
 
+*   `LOG_LEVEL`: Chooses the log level, which controls the amount of console log
+    output compiled into the build. This should be one of the following:
+
+        0  (LOG_LEVEL_NONE)
+        10 (LOG_LEVEL_NOTICE)
+        20 (LOG_LEVEL_ERROR)
+        30 (LOG_LEVEL_WARNING)
+        40 (LOG_LEVEL_INFO)
+        50 (LOG_LEVEL_VERBOSE)
+
+    All log output up to and including the log level is compiled into the build.
+    The default value is 40 in debug builds and 20 in release builds.
+
 *   `NS_TIMER_SWITCH`: Enable save and restore for non-secure timer register
     contents upon world switch. It can take either 0 (don't save and restore) or
     1 (do save and restore). 0 is the default. An SPD could set this to 1 if it
@@ -192,6 +207,19 @@
     synchronous method) or 1 (BL3-2 is initialized using asynchronous method).
     Default is 0.
 
+#### FVP specific build options
+
+*   `FVP_SHARED_DATA_LOCATION`: location of the shared memory page. Available
+    options:
+      - 'tsram' (default) : top of Trusted SRAM
+      - 'tdram' : base of Trusted DRAM
+
+*   `FVP_TSP_RAM_LOCATION`: location of the TSP binary. Options:
+      - 'tsram' (default) : base of Trusted SRAM
+      - 'tdram' : Trusted DRAM (above shared data)
+
+For a better understanding of FVP options, the FVP memory map is detailed in
+[Firmware Design].
 
 ### Creating a Firmware Image Package
 
@@ -314,11 +342,11 @@
 
 On FVP, the TSP binary runs from Trusted SRAM by default. It is also possible
 to run it from Trusted DRAM. This is controlled by the build configuration
-`TSP_RAM_LOCATION`:
+`FVP_TSP_RAM_LOCATION`:
 
     CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf- \
     BL33=<path-to>/<bl33_image>                               \
-    make PLAT=fvp SPD=tspd TSP_RAM_LOCATION=tdram all fip
+    make PLAT=fvp SPD=tspd FVP_TSP_RAM_LOCATION=tdram all fip
 
 
 ### Checking source code style
@@ -719,16 +747,16 @@
     -C cluster1.NUM_CORES=4                                      \
     -C cache_state_modelled=1                                    \
     -C bp.pl011_uart0.untimed_fifos=1                            \
-    -C cluster0.cpu0.RVBAR=0x04006000                            \
-    -C cluster0.cpu1.RVBAR=0x04006000                            \
-    -C cluster0.cpu2.RVBAR=0x04006000                            \
-    -C cluster0.cpu3.RVBAR=0x04006000                            \
-    -C cluster1.cpu0.RVBAR=0x04006000                            \
-    -C cluster1.cpu1.RVBAR=0x04006000                            \
-    -C cluster1.cpu2.RVBAR=0x04006000                            \
-    -C cluster1.cpu3.RVBAR=0x04006000                            \
-    --data cluster0.cpu0="<path-to>/<bl31-binary>"@0x04006000    \
-    --data cluster0.cpu0="<path-to>/<bl32-binary>"@0x04024000    \
+    -C cluster0.cpu0.RVBAR=0x04023000                            \
+    -C cluster0.cpu1.RVBAR=0x04023000                            \
+    -C cluster0.cpu2.RVBAR=0x04023000                            \
+    -C cluster0.cpu3.RVBAR=0x04023000                            \
+    -C cluster1.cpu0.RVBAR=0x04023000                            \
+    -C cluster1.cpu1.RVBAR=0x04023000                            \
+    -C cluster1.cpu2.RVBAR=0x04023000                            \
+    -C cluster1.cpu3.RVBAR=0x04023000                            \
+    --data cluster0.cpu0="<path-to>/<bl31-binary>"@0x04023000    \
+    --data cluster0.cpu0="<path-to>/<bl32-binary>"@0x04000000    \
     --data cluster0.cpu0="<path-to>/<bl33-binary>"@0x88000000    \
     -C bp.virtioblockdevice.image_path="<path-to>/<file-system-image>"
 
@@ -750,16 +778,16 @@
     -C bp.tzc_400.diagnostics=1                                  \
     -C cache_state_modelled=1                                    \
     -C bp.pl011_uart0.untimed_fifos=1                            \
-    -C cluster0.cpu0.RVBARADDR=0x04006000                        \
-    -C cluster0.cpu1.RVBARADDR=0x04006000                        \
-    -C cluster0.cpu2.RVBARADDR=0x04006000                        \
-    -C cluster0.cpu3.RVBARADDR=0x04006000                        \
-    -C cluster1.cpu0.RVBARADDR=0x04006000                        \
-    -C cluster1.cpu1.RVBARADDR=0x04006000                        \
-    -C cluster1.cpu2.RVBARADDR=0x04006000                        \
-    -C cluster1.cpu3.RVBARADDR=0x04006000                        \
-    --data cluster0.cpu0="<path-to>/<bl31-binary>"@0x04006000    \
-    --data cluster0.cpu0="<path-to>/<bl32-binary>"@0x04024000    \
+    -C cluster0.cpu0.RVBARADDR=0x04023000                        \
+    -C cluster0.cpu1.RVBARADDR=0x04023000                        \
+    -C cluster0.cpu2.RVBARADDR=0x04023000                        \
+    -C cluster0.cpu3.RVBARADDR=0x04023000                        \
+    -C cluster1.cpu0.RVBARADDR=0x04023000                        \
+    -C cluster1.cpu1.RVBARADDR=0x04023000                        \
+    -C cluster1.cpu2.RVBARADDR=0x04023000                        \
+    -C cluster1.cpu3.RVBARADDR=0x04023000                        \
+    --data cluster0.cpu0="<path-to>/<bl31-binary>"@0x04023000    \
+    --data cluster0.cpu0="<path-to>/<bl32-binary>"@0x04000000    \
     --data cluster0.cpu0="<path-to>/<bl33-binary>"@0x88000000    \
     -C bp.virtioblockdevice.image_path="<path-to>/<file-system-image>"
 
@@ -852,6 +880,6 @@
 [Firmware Design]:  ./firmware-design.md
 
 [ARM FVP website]:  http://www.arm.com/fvp
-[Linaro Toolchain]: http://releases.linaro.org/13.11/components/toolchain/binaries/
+[Linaro Toolchain]: http://releases.linaro.org/14.07/components/toolchain/binaries/
 [EDK2]:             http://github.com/tianocore/edk2
 [DS-5]:             http://www.arm.com/products/tools/software-tools/ds-5/index.php
diff --git a/drivers/arm/cci400/cci400.c b/drivers/arm/cci400/cci400.c
index af10f21..6a8737a 100644
--- a/drivers/arm/cci400/cci400.c
+++ b/drivers/arm/cci400/cci400.c
@@ -28,34 +28,80 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <arch.h>
+#include <assert.h>
 #include <cci400.h>
 #include <mmio.h>
-#include <platform_def.h>
+
+#define MAX_CLUSTERS		2
+
+static unsigned long cci_base_addr;
+static unsigned int cci_cluster_ix_to_iface[MAX_CLUSTERS];
+
+
+void cci_init(unsigned long cci_base,
+		int slave_iface3_cluster_ix,
+		int slave_iface4_cluster_ix)
+{
+	/*
+	 * Check the passed arguments are valid. The cluster indices must be
+	 * less than MAX_CLUSTERS, not the same as each other and at least one
+	 * of them must be refer to a valid cluster index.
+	 */
+	assert(cci_base);
+	assert(slave_iface3_cluster_ix < MAX_CLUSTERS);
+	assert(slave_iface4_cluster_ix < MAX_CLUSTERS);
+	assert(slave_iface3_cluster_ix != slave_iface4_cluster_ix);
+	assert((slave_iface3_cluster_ix >= 0) ||
+		(slave_iface3_cluster_ix >= 0));
+
+	cci_base_addr = cci_base;
+	if (slave_iface3_cluster_ix >= 0)
+		cci_cluster_ix_to_iface[slave_iface3_cluster_ix] =
+			SLAVE_IFACE3_OFFSET;
+	if (slave_iface4_cluster_ix >= 0)
+		cci_cluster_ix_to_iface[slave_iface4_cluster_ix] =
+			SLAVE_IFACE4_OFFSET;
+}
 
 static inline unsigned long get_slave_iface_base(unsigned long mpidr)
 {
-	return CCI400_BASE + SLAVE_IFACE_OFFSET(CCI400_SL_IFACE_INDEX(mpidr));
+	/*
+	 * We assume the TF topology code allocates affinity instances
+	 * consecutively from zero.
+	 * It is a programming error if this is called without initializing
+	 * the slave interface to use for this cluster.
+	 */
+	unsigned int cluster_id =
+		(mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+
+	assert(cluster_id < MAX_CLUSTERS);
+	assert(cci_cluster_ix_to_iface[cluster_id] != 0);
+
+	return cci_base_addr + cci_cluster_ix_to_iface[cluster_id];
 }
 
-void cci_enable_coherency(unsigned long mpidr)
+void cci_enable_cluster_coherency(unsigned long mpidr)
 {
+	assert(cci_base_addr);
 	/* Enable Snoops and DVM messages */
 	mmio_write_32(get_slave_iface_base(mpidr) + SNOOP_CTRL_REG,
 		      DVM_EN_BIT | SNOOP_EN_BIT);
 
 	/* Wait for the dust to settle down */
-	while (mmio_read_32(CCI400_BASE + STATUS_REG) & CHANGE_PENDING_BIT)
+	while (mmio_read_32(cci_base_addr + STATUS_REG) & CHANGE_PENDING_BIT)
 		;
 }
 
-void cci_disable_coherency(unsigned long mpidr)
+void cci_disable_cluster_coherency(unsigned long mpidr)
 {
+	assert(cci_base_addr);
 	/* Disable Snoops and DVM messages */
 	mmio_write_32(get_slave_iface_base(mpidr) + SNOOP_CTRL_REG,
 		      ~(DVM_EN_BIT | SNOOP_EN_BIT));
 
 	/* Wait for the dust to settle down */
-	while (mmio_read_32(CCI400_BASE + STATUS_REG) & CHANGE_PENDING_BIT)
+	while (mmio_read_32(cci_base_addr + STATUS_REG) & CHANGE_PENDING_BIT)
 		;
 }
 
diff --git a/drivers/arm/tzc400/tzc400.c b/drivers/arm/tzc400/tzc400.c
index 715ea6c..3ab1f31 100644
--- a/drivers/arm/tzc400/tzc400.c
+++ b/drivers/arm/tzc400/tzc400.c
@@ -34,54 +34,88 @@
 #include <stddef.h>
 #include <tzc400.h>
 
-static uint32_t tzc_read_build_config(uint64_t base)
+/*
+ * Implementation defined values used to validate inputs later.
+ * Filters : max of 4 ; 0 to 3
+ * Regions : max of 9 ; 0 to 8
+ * Address width : Values between 32 to 64
+ */
+typedef struct tzc_instance {
+	uint64_t base;
+	uint8_t addr_width;
+	uint8_t num_filters;
+	uint8_t num_regions;
+} tzc_instance_t;
+
+tzc_instance_t tzc;
+
+
+static inline uint32_t tzc_read_build_config(uint64_t base)
 {
 	return mmio_read_32(base + BUILD_CONFIG_OFF);
 }
 
-static uint32_t tzc_read_gate_keeper(uint64_t base)
+static inline uint32_t tzc_read_gate_keeper(uint64_t base)
 {
 	return mmio_read_32(base + GATE_KEEPER_OFF);
 }
 
-static void tzc_write_gate_keeper(uint64_t base, uint32_t val)
+static inline void tzc_write_gate_keeper(uint64_t base, uint32_t val)
 {
 	mmio_write_32(base + GATE_KEEPER_OFF, val);
 }
 
-static void tzc_write_action(uint64_t base, tzc_action_t action)
+static inline void tzc_write_action(uint64_t base, tzc_action_t action)
 {
 	mmio_write_32(base + ACTION_OFF, action);
 }
 
-static void tzc_write_region_base_low(uint64_t base, uint32_t region, uint32_t val)
+static inline void tzc_write_region_base_low(uint64_t base,
+					uint32_t region,
+					uint32_t val)
 {
-	mmio_write_32(base + REGION_BASE_LOW_OFF + REGION_NUM_OFF(region), val);
+	mmio_write_32(base + REGION_BASE_LOW_OFF +
+		REGION_NUM_OFF(region), val);
 }
 
-static void tzc_write_region_base_high(uint64_t base, uint32_t region, uint32_t val)
+static inline void tzc_write_region_base_high(uint64_t base,
+					uint32_t region,
+					uint32_t val)
 {
-	mmio_write_32(base + REGION_BASE_HIGH_OFF + REGION_NUM_OFF(region), val);
+	mmio_write_32(base + REGION_BASE_HIGH_OFF +
+		REGION_NUM_OFF(region), val);
 }
 
-static void tzc_write_region_top_low(uint64_t base, uint32_t region, uint32_t val)
+static inline void tzc_write_region_top_low(uint64_t base,
+					uint32_t region,
+					uint32_t val)
 {
-	mmio_write_32(base + REGION_TOP_LOW_OFF + REGION_NUM_OFF(region), val);
+	mmio_write_32(base + REGION_TOP_LOW_OFF +
+		REGION_NUM_OFF(region), val);
 }
 
-static void tzc_write_region_top_high(uint64_t base, uint32_t region, uint32_t val)
+static inline void tzc_write_region_top_high(uint64_t base,
+					uint32_t region,
+					uint32_t val)
 {
-	mmio_write_32(base + REGION_TOP_HIGH_OFF + REGION_NUM_OFF(region), val);
+	mmio_write_32(base + REGION_TOP_HIGH_OFF +
+		REGION_NUM_OFF(region), val);
 }
 
-static void tzc_write_region_attributes(uint64_t base, uint32_t region, uint32_t val)
+static inline void tzc_write_region_attributes(uint64_t base,
+					uint32_t region,
+					uint32_t val)
 {
-	mmio_write_32(base + REGION_ATTRIBUTES_OFF + REGION_NUM_OFF(region), val);
+	mmio_write_32(base + REGION_ATTRIBUTES_OFF +
+		REGION_NUM_OFF(region), val);
 }
 
-static void tzc_write_region_id_access(uint64_t base, uint32_t region, uint32_t val)
+static inline void tzc_write_region_id_access(uint64_t base,
+					uint32_t region,
+					uint32_t val)
 {
-	mmio_write_32(base + REGION_ID_ACCESS_OFF + REGION_NUM_OFF(region), val);
+	mmio_write_32(base + REGION_ID_ACCESS_OFF +
+		REGION_NUM_OFF(region), val);
 }
 
 static uint32_t tzc_read_component_id(uint64_t base)
@@ -130,29 +164,30 @@
 }
 
 
-void tzc_init(tzc_instance_t *controller)
+void tzc_init(uint64_t base)
 {
 	uint32_t tzc_id, tzc_build;
 
-	assert(controller != NULL);
+	assert(base);
+	tzc.base = base;
 
 	/*
 	 * We expect to see a tzc400. Check component ID. The TZC-400 TRM shows
 	 * component ID is expected to be "0xB105F00D".
 	 */
-	tzc_id = tzc_read_component_id(controller->base);
+	tzc_id = tzc_read_component_id(tzc.base);
 	if (tzc_id != TZC400_COMPONENT_ID) {
 		ERROR("TZC : Wrong device ID (0x%x).\n", tzc_id);
 		panic();
 	}
 
 	/* Save values we will use later. */
-	tzc_build = tzc_read_build_config(controller->base);
-	controller->num_filters = ((tzc_build >> BUILD_CONFIG_NF_SHIFT) &
+	tzc_build = tzc_read_build_config(tzc.base);
+	tzc.num_filters = ((tzc_build >> BUILD_CONFIG_NF_SHIFT) &
 			   BUILD_CONFIG_NF_MASK) + 1;
-	controller->addr_width  = ((tzc_build >> BUILD_CONFIG_AW_SHIFT) &
+	tzc.addr_width  = ((tzc_build >> BUILD_CONFIG_AW_SHIFT) &
 			   BUILD_CONFIG_AW_MASK) + 1;
-	controller->num_regions = ((tzc_build >> BUILD_CONFIG_NR_SHIFT) &
+	tzc.num_regions = ((tzc_build >> BUILD_CONFIG_NR_SHIFT) &
 			   BUILD_CONFIG_NR_MASK) + 1;
 }
 
@@ -166,29 +201,25 @@
  * this cannot be changed. It is, however, possible to change some region 0
  * permissions.
  */
-void tzc_configure_region(const tzc_instance_t *controller,
-			  uint32_t filters,
+void tzc_configure_region(uint32_t filters,
 			  uint8_t  region,
 			  uint64_t region_base,
 			  uint64_t region_top,
 			  tzc_region_attributes_t sec_attr,
 			  uint32_t ns_device_access)
 {
-	uint64_t max_addr;
-
-	assert(controller != NULL);
+	assert(tzc.base);
 
 	/* Do range checks on filters and regions. */
-	assert(((filters >> controller->num_filters) == 0) &&
-	       (region < controller->num_regions));
+	assert(((filters >> tzc.num_filters) == 0) &&
+	       (region < tzc.num_regions));
 
 	/*
 	 * Do address range check based on TZC configuration. A 64bit address is
 	 * the max and expected case.
 	 */
-	max_addr = UINT64_MAX >> (64 - controller->addr_width);
-	if ((region_top > max_addr) || (region_base >= region_top))
-		assert(0);
+	assert(((region_top <= (UINT64_MAX >> (64 - tzc.addr_width))) &&
+		(region_base < region_top)));
 
 	/* region_base and (region_top + 1) must be 4KB aligned */
 	assert(((region_base | (region_top + 1)) & (4096 - 1)) == 0);
@@ -200,46 +231,50 @@
 	 * All the address registers are 32 bits wide and have a LOW and HIGH
 	 * component used to construct a up to a 64bit address.
 	 */
-	tzc_write_region_base_low(controller->base, region, (uint32_t)(region_base));
-	tzc_write_region_base_high(controller->base, region, (uint32_t)(region_base >> 32));
+	tzc_write_region_base_low(tzc.base, region,
+				(uint32_t)(region_base));
+	tzc_write_region_base_high(tzc.base, region,
+				(uint32_t)(region_base >> 32));
 
-	tzc_write_region_top_low(controller->base, region, (uint32_t)(region_top));
-	tzc_write_region_top_high(controller->base, region, (uint32_t)(region_top >> 32));
+	tzc_write_region_top_low(tzc.base, region,
+				(uint32_t)(region_top));
+	tzc_write_region_top_high(tzc.base, region,
+				(uint32_t)(region_top >> 32));
 
 	/* Assign the region to a filter and set secure attributes */
-	tzc_write_region_attributes(controller->base, region,
+	tzc_write_region_attributes(tzc.base, region,
 		(sec_attr << REGION_ATTRIBUTES_SEC_SHIFT) | filters);
 
 	/*
 	 * Specify which non-secure devices have permission to access this
 	 * region.
 	 */
-	tzc_write_region_id_access(controller->base, region, ns_device_access);
+	tzc_write_region_id_access(tzc.base, region, ns_device_access);
 }
 
 
-void tzc_set_action(const tzc_instance_t *controller, tzc_action_t action)
+void tzc_set_action(tzc_action_t action)
 {
-	assert(controller != NULL);
+	assert(tzc.base);
 
 	/*
 	 * - Currently no handler is provided to trap an error via interrupt
 	 *   or exception.
 	 * - The interrupt action has not been tested.
 	 */
-	tzc_write_action(controller->base, action);
+	tzc_write_action(tzc.base, action);
 }
 
 
-void tzc_enable_filters(const tzc_instance_t *controller)
+void tzc_enable_filters(void)
 {
 	uint32_t state;
 	uint32_t filter;
 
-	assert(controller != NULL);
+	assert(tzc.base);
 
-	for (filter = 0; filter < controller->num_filters; filter++) {
-		state = tzc_get_gate_keeper(controller->base, filter);
+	for (filter = 0; filter < tzc.num_filters; filter++) {
+		state = tzc_get_gate_keeper(tzc.base, filter);
 		if (state) {
 			/* The TZC filter is already configured. Changing the
 			 * programmer's view in an active system can cause
@@ -252,21 +287,21 @@
 				filter);
 			panic();
 		}
-		tzc_set_gate_keeper(controller->base, filter, 1);
+		tzc_set_gate_keeper(tzc.base, filter, 1);
 	}
 }
 
 
-void tzc_disable_filters(const tzc_instance_t *controller)
+void tzc_disable_filters(void)
 {
 	uint32_t filter;
 
-	assert(controller != NULL);
+	assert(tzc.base);
 
 	/*
 	 * We don't do the same state check as above as the Gatekeepers are
 	 * disabled after reset.
 	 */
-	for (filter = 0; filter < controller->num_filters; filter++)
-		tzc_set_gate_keeper(controller->base, filter, 0);
+	for (filter = 0; filter < tzc.num_filters; filter++)
+		tzc_set_gate_keeper(tzc.base, filter, 0);
 }
diff --git a/drivers/io/io_fip.c b/drivers/io/io_fip.c
index 7df229d..4262a9d 100644
--- a/drivers/io/io_fip.c
+++ b/drivers/io/io_fip.c
@@ -215,7 +215,7 @@
 			WARN("Firmware Image Package header check failed.\n");
 			result = IO_FAIL;
 		} else {
-			INFO("FIP header looks OK.\n");
+			VERBOSE("FIP header looks OK.\n");
 		}
 	}
 
diff --git a/lib/io_storage.c b/drivers/io/io_storage.c
similarity index 94%
rename from lib/io_storage.c
rename to drivers/io/io_storage.c
index 204310a..a3a8186 100644
--- a/lib/io_storage.c
+++ b/drivers/io/io_storage.c
@@ -31,13 +31,10 @@
 #include <assert.h>
 #include <io_driver.h>
 #include <io_storage.h>
+#include <platform_def.h>
 #include <stddef.h>
 
 
-#define MAX_DEVICES(plat_data)						\
-	(sizeof((plat_data)->devices)/sizeof((plat_data)->devices[0]))
-
-
 /* Storage for a fixed maximum number of IO entities, definable by platform */
 static io_entity_t entity_pool[MAX_IO_HANDLES];
 
@@ -48,9 +45,11 @@
 /* Track number of allocated entities */
 static unsigned int entity_count;
 
+/* Array of fixed maximum of registered devices, definable by platform */
+static const io_dev_info_t *devices[MAX_IO_DEVICES];
 
-/* Used to keep a reference to platform-specific data */
-static io_plat_data_t *platform_data;
+/* Number of currently registered devices */
+static unsigned int dev_count;
 
 
 #if DEBUG	/* Extra validation functions only used in debug builds */
@@ -167,27 +166,15 @@
 
 /* Exported API */
 
-
-/* Initialise the IO layer */
-void io_init(io_plat_data_t *data)
-{
-	assert(data != NULL);
-	platform_data = data;
-}
-
-
 /* Register a device driver */
 int io_register_device(const io_dev_info_t *dev_info)
 {
 	int result = IO_FAIL;
 	assert(dev_info != NULL);
-	assert(platform_data != NULL);
 
-	unsigned int dev_count = platform_data->dev_count;
-
-	if (dev_count < MAX_DEVICES(platform_data)) {
-		platform_data->devices[dev_count] = dev_info;
-		platform_data->dev_count++;
+	if (dev_count < MAX_IO_DEVICES) {
+		devices[dev_count] = dev_info;
+		dev_count++;
 		result = IO_SUCCESS;
 	} else {
 		result = IO_RESOURCES_EXHAUSTED;
diff --git a/include/bl32/tsp/platform_tsp.h b/include/bl32/tsp/platform_tsp.h
new file mode 100644
index 0000000..f6f7391
--- /dev/null
+++ b/include/bl32/tsp/platform_tsp.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 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.
+ */
+
+#ifndef __PLATFORM_TSP_H__
+
+
+/*******************************************************************************
+ * Mandatory TSP functions (only if platform contains a TSP)
+ ******************************************************************************/
+void tsp_early_platform_setup(void);
+void tsp_plat_arch_setup(void);
+void tsp_platform_setup(void);
+
+
+#define __PLATFORM_H__
+
+#endif
diff --git a/include/bl32/tsp/tsp.h b/include/bl32/tsp/tsp.h
new file mode 100644
index 0000000..c0b191f
--- /dev/null
+++ b/include/bl32/tsp/tsp.h
@@ -0,0 +1,122 @@
+/*
+ * 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.
+ */
+
+#ifndef __TSP_H__
+#define __TSP_H__
+
+/*
+ * SMC function IDs that TSP uses to signal various forms of completions
+ * to the secure payload dispatcher.
+ */
+#define TSP_ENTRY_DONE		0xf2000000
+#define TSP_ON_DONE		0xf2000001
+#define TSP_OFF_DONE		0xf2000002
+#define TSP_SUSPEND_DONE	0xf2000003
+#define TSP_RESUME_DONE		0xf2000004
+#define TSP_PREEMPTED		0xf2000005
+
+/*
+ * Function identifiers to handle FIQs through the synchronous handling model.
+ * If the TSP was previously interrupted then control has to be returned to
+ * the TSPD after handling the interrupt else execution can remain in the TSP.
+ */
+#define TSP_HANDLED_S_EL1_FIQ		0xf2000006
+#define TSP_EL3_FIQ			0xf2000007
+
+/* SMC function ID that TSP uses to request service from secure monitor */
+#define TSP_GET_ARGS		0xf2001000
+
+/*
+ * Identifiers for various TSP services. Corresponding function IDs (whether
+ * fast or standard) are generated by macros defined below
+ */
+#define TSP_ADD		0x2000
+#define TSP_SUB		0x2001
+#define TSP_MUL		0x2002
+#define TSP_DIV		0x2003
+#define TSP_HANDLE_FIQ_AND_RETURN	0x2004
+
+/*
+ * Generate function IDs for TSP services to be used in SMC calls, by
+ * appropriately setting bit 31 to differentiate standard and fast SMC calls
+ */
+#define TSP_STD_FID(fid)	((fid) | 0x72000000 | (0 << 31))
+#define TSP_FAST_FID(fid)	((fid) | 0x72000000 | (1 << 31))
+
+/* SMC function ID to request a previously preempted std smc */
+#define TSP_FID_RESUME		TSP_STD_FID(0x3000)
+
+/*
+ * Identify a TSP service from function ID filtering the last 16 bits from the
+ * SMC function ID
+ */
+#define TSP_BARE_FID(fid)	((fid) & 0xffff)
+
+/*
+ * Total number of function IDs implemented for services offered to NS clients.
+ * The function IDs are defined above
+ */
+#define TSP_NUM_FID		0x4
+
+/* TSP implementation version numbers */
+#define TSP_VERSION_MAJOR	0x0 /* Major version */
+#define TSP_VERSION_MINOR	0x1 /* Minor version */
+
+/*
+ * Standard Trusted OS Function IDs that fall under Trusted OS call range
+ * according to SMC calling convention
+ */
+#define TOS_CALL_COUNT		0xbf00ff00 /* Number of calls implemented */
+#define TOS_UID			0xbf00ff01 /* Implementation UID */
+/*				0xbf00ff02 is reserved */
+#define TOS_CALL_VERSION	0xbf00ff03 /* Trusted OS Call Version */
+
+
+#ifndef __ASSEMBLY__
+
+#include <stdint.h>
+
+
+typedef uint32_t tsp_vector_isn_t;
+
+typedef struct tsp_vectors {
+	tsp_vector_isn_t std_smc_entry;
+	tsp_vector_isn_t fast_smc_entry;
+	tsp_vector_isn_t cpu_on_entry;
+	tsp_vector_isn_t cpu_off_entry;
+	tsp_vector_isn_t cpu_resume_entry;
+	tsp_vector_isn_t cpu_suspend_entry;
+	tsp_vector_isn_t fiq_entry;
+} tsp_vectors_t;
+
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* __TSP_H__ */
diff --git a/include/common/debug.h b/include/common/debug.h
index 3f5655b..a8dcb8d 100644
--- a/include/common/debug.h
+++ b/include/common/debug.h
@@ -33,24 +33,53 @@
 
 #include <stdio.h>
 
-/* If building the project with DEBUG disabled the INFO and WARN macros
- * won't produce any output. The ERROR macro is always enabled.
- * The format expected is the same as for printf().
- * INFO("Info %s.\n", "message")    -> INFO: Info message.
- * WARN("Warning %s.\n", "message") -> WARN: Warning message.
- * ERROR("Error %s.\n", "message")  -> ERROR: Error message.
- *
- * TODO : add debug levels.
+/* The log output macros print output to the console. These macros produce
+ * compiled log output only if the LOG_LEVEL defined in the makefile (or the
+ * make command line) is greater or equal than the level required for that
+ * type of log output.
+ * The format expected is the same as for printf(). For example:
+ * INFO("Info %s.\n", "message")    -> INFO:    Info message.
+ * WARN("Warning %s.\n", "message") -> WARNING: Warning message.
  */
-#if DEBUG
- #define INFO(...)	tf_printf("INFO: " __VA_ARGS__)
- #define WARN(...)	tf_printf("WARN: " __VA_ARGS__)
+
+#define LOG_LEVEL_NONE			0
+#define LOG_LEVEL_ERROR			10
+#define LOG_LEVEL_NOTICE		20
+#define LOG_LEVEL_WARNING		30
+#define LOG_LEVEL_INFO			40
+#define LOG_LEVEL_VERBOSE		50
+
+
+#if LOG_LEVEL >= LOG_LEVEL_NOTICE
+# define NOTICE(...)	tf_printf("NOTICE:  " __VA_ARGS__)
 #else
- #define INFO(...)
- #define WARN(...)
+# define NOTICE(...)
 #endif
 
-#define ERROR(...)	tf_printf("ERROR: " __VA_ARGS__)
+#if LOG_LEVEL >= LOG_LEVEL_ERROR
+# define ERROR(...)	tf_printf("ERROR:   " __VA_ARGS__)
+#else
+# define ERROR(...)
+#endif
+
+#if LOG_LEVEL >= LOG_LEVEL_WARNING
+# define WARN(...)	tf_printf("WARNING: " __VA_ARGS__)
+#else
+# define WARN(...)
+#endif
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+# define INFO(...)	tf_printf("INFO:    " __VA_ARGS__)
+#else
+# define INFO(...)
+#endif
+
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+# define VERBOSE(...)	tf_printf("VERBOSE: " __VA_ARGS__)
+#else
+# define VERBOSE(...)
+#endif
+
 
 void __dead2 do_panic(void);
 #define panic()	do_panic()
diff --git a/include/drivers/arm/cci400.h b/include/drivers/arm/cci400.h
index 6246e48..7756bdf 100644
--- a/include/drivers/arm/cci400.h
+++ b/include/drivers/arm/cci400.h
@@ -37,7 +37,8 @@
 #define SLAVE_IFACE2_OFFSET		0x3000
 #define SLAVE_IFACE1_OFFSET		0x2000
 #define SLAVE_IFACE0_OFFSET		0x1000
-#define SLAVE_IFACE_OFFSET(index)	SLAVE_IFACE0_OFFSET + (0x1000 * index)
+#define SLAVE_IFACE_OFFSET(index)	SLAVE_IFACE0_OFFSET +	\
+					(0x1000 * (index))
 
 /* Control and ID register offsets */
 #define CTRL_OVERRIDE_REG		0x0
@@ -68,8 +69,22 @@
 #ifndef __ASSEMBLY__
 
 /* Function declarations */
-void cci_enable_coherency(unsigned long mpidr);
-void cci_disable_coherency(unsigned long mpidr);
+
+/*
+ * The CCI-400 driver must be initialized with the base address of the
+ * CCI-400 device in the platform memory map, and the cluster indices for
+ * the CCI-400 slave interfaces 3 and 4 respectively. These are the fully
+ * coherent ACE slave interfaces of CCI-400.
+ * The cluster indices must either be 0 or 1, corresponding to the level 1
+ * affinity instance of the mpidr representing the cluster. A negative cluster
+ * index indicates that no cluster is present on that slave interface.
+ */
+void cci_init(unsigned long cci_base,
+		int slave_iface3_cluster_ix,
+		int slave_iface4_cluster_ix);
+
+void cci_enable_cluster_coherency(unsigned long mpidr);
+void cci_disable_cluster_coherency(unsigned long mpidr);
 
 #endif /* __ASSEMBLY__ */
 #endif /* __CCI_400_H__ */
diff --git a/include/drivers/arm/tzc400.h b/include/drivers/arm/tzc400.h
index 03fce54..ff8b49a 100644
--- a/include/drivers/arm/tzc400.h
+++ b/include/drivers/arm/tzc400.h
@@ -182,27 +182,17 @@
 	TZC_REGION_S_RDWR = (TZC_REGION_S_RD | TZC_REGION_S_WR)
 } tzc_region_attributes_t;
 
-/*
- * Implementation defined values used to validate inputs later.
- * Filters : max of 4 ; 0 to 3
- * Regions : max of 9 ; 0 to 8
- * Address width : Values between 32 to 64
- */
-typedef struct tzc_instance {
-	uint64_t base;
-	uint32_t aid_width;
-	uint8_t addr_width;
-	uint8_t num_filters;
-	uint8_t num_regions;
-} tzc_instance_t ;
 
-void tzc_init(tzc_instance_t *controller);
-void tzc_configure_region(const tzc_instance_t *controller, uint32_t filters,
-	uint8_t region, uint64_t region_base, uint64_t region_top,
-	tzc_region_attributes_t sec_attr, uint32_t ns_device_access);
-void tzc_enable_filters(const tzc_instance_t *controller);
-void tzc_disable_filters(const tzc_instance_t *controller);
-void tzc_set_action(const tzc_instance_t *controller, tzc_action_t action);
+void tzc_init(uint64_t base);
+void tzc_configure_region(uint32_t filters,
+			uint8_t region,
+			uint64_t region_base,
+			uint64_t region_top,
+			tzc_region_attributes_t sec_attr,
+			uint32_t ns_device_access);
+void tzc_enable_filters(void);
+void tzc_disable_filters(void);
+void tzc_set_action(tzc_action_t action);
 
 
 #endif /* __TZC400__ */
diff --git a/include/drivers/io_driver.h b/include/drivers/io/io_driver.h
similarity index 89%
rename from include/drivers/io_driver.h
rename to include/drivers/io/io_driver.h
index 867abbf..adb38b0 100644
--- a/include/drivers/io_driver.h
+++ b/include/drivers/io/io_driver.h
@@ -32,7 +32,6 @@
 #define __IO_DRIVER_H__
 
 #include <io_storage.h>
-#include <platform_def.h> /* For MAX_IO_DEVICES */
 #include <stdint.h>
 
 
@@ -76,20 +75,9 @@
 } io_dev_funcs_t;
 
 
-/* IO platform data - used to track devices registered for a specific
- * platform */
-typedef struct io_plat_data {
-	const io_dev_info_t *devices[MAX_IO_DEVICES];
-	unsigned int dev_count;
-} io_plat_data_t;
-
-
 /* Operations intended to be performed during platform initialisation */
 
-/* Initialise the IO layer */
-void io_init(io_plat_data_t *data);
-
-/* Register a device driver */
+/* Register an IO device */
 int io_register_device(const io_dev_info_t *dev_info);
 
 #endif  /* __IO_DRIVER_H__ */
diff --git a/include/drivers/io_fip.h b/include/drivers/io/io_fip.h
similarity index 100%
rename from include/drivers/io_fip.h
rename to include/drivers/io/io_fip.h
diff --git a/include/drivers/io_memmap.h b/include/drivers/io/io_memmap.h
similarity index 100%
rename from include/drivers/io_memmap.h
rename to include/drivers/io/io_memmap.h
diff --git a/include/drivers/io_semihosting.h b/include/drivers/io/io_semihosting.h
similarity index 100%
rename from include/drivers/io_semihosting.h
rename to include/drivers/io/io_semihosting.h
diff --git a/include/lib/io_storage.h b/include/drivers/io/io_storage.h
similarity index 100%
rename from include/lib/io_storage.h
rename to include/drivers/io/io_storage.h
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index ab93123..8e038aa 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -78,6 +78,7 @@
 /*******************************************************************************
  * Mandatory BL1 functions
  ******************************************************************************/
+void bl1_early_platform_setup(void);
 void bl1_plat_arch_setup(void);
 void bl1_platform_setup(void);
 struct meminfo *bl1_plat_sec_mem_layout(void);
@@ -98,6 +99,7 @@
 /*******************************************************************************
  * Mandatory BL2 functions
  ******************************************************************************/
+void bl2_early_platform_setup(struct meminfo *mem_layout);
 void bl2_plat_arch_setup(void);
 void bl2_platform_setup(void);
 struct meminfo *bl2_plat_sec_mem_layout(void);
@@ -185,11 +187,6 @@
 void bl31_plat_enable_mmu(uint32_t flags);
 
 /*******************************************************************************
- * Mandatory BL3-2 functions (only if platform contains a BL3-2)
- ******************************************************************************/
-void bl32_platform_setup(void);
-
-/*******************************************************************************
  * Optional BL3-2 functions (may be overridden)
  ******************************************************************************/
 void bl32_plat_enable_mmu(uint32_t flags);
diff --git a/lib/stdlib/abort.c b/lib/stdlib/abort.c
index 2ef3bee..862bf9c 100644
--- a/lib/stdlib/abort.c
+++ b/lib/stdlib/abort.c
@@ -28,13 +28,13 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <stdio.h>
+#include <debug.h>
 
 /*
  * This is a basic implementation. This could be improved.
  */
 void abort (void)
 {
-	printf("ABORT\n\r");
-	while(1);
+	ERROR("ABORT\n");
+	panic();
 }
diff --git a/lib/stdlib/assert.c b/lib/stdlib/assert.c
index c614854..90a1afe 100644
--- a/lib/stdlib/assert.c
+++ b/lib/stdlib/assert.c
@@ -36,6 +36,6 @@
 void __assert (const char *function, const char *file, unsigned int line,
 		const char *assertion)
 {
-	tf_printf("ASSERT: %s <%d> : %s\n\r", function, line, assertion);
+	tf_printf("ASSERT: %s <%d> : %s\n", function, line, assertion);
 	while(1);
 }
diff --git a/plat/fvp/aarch64/fvp_common.c b/plat/fvp/aarch64/fvp_common.c
index d22fd55..89fd8b3 100644
--- a/plat/fvp/aarch64/fvp_common.c
+++ b/plat/fvp/aarch64/fvp_common.c
@@ -31,7 +31,6 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <arm_gic.h>
-#include <assert.h>
 #include <bl_common.h>
 #include <cci400.h>
 #include <debug.h>
@@ -56,9 +55,9 @@
  * configure_mmu_elx() will give the available subset of that,
  */
 const mmap_region_t fvp_mmap[] = {
-	{ TZROM_BASE,	TZROM_BASE,	TZROM_SIZE,
-						MT_MEMORY | MT_RO | MT_SECURE },
-	{ TZDRAM_BASE,	TZDRAM_BASE,	TZDRAM_SIZE,
+	{ FVP_SHARED_RAM_BASE,	FVP_SHARED_RAM_BASE,	FVP_SHARED_RAM_SIZE,
+						MT_MEMORY | MT_RW | MT_SECURE },
+	{ FVP_TRUSTED_DRAM_BASE, FVP_TRUSTED_DRAM_BASE,	FVP_TRUSTED_DRAM_SIZE,
 						MT_MEMORY | MT_RW | MT_SECURE },
 	{ FLASH0_BASE,	FLASH0_BASE,	FLASH0_SIZE,
 						MT_MEMORY | MT_RO | MT_SECURE },
@@ -243,15 +242,26 @@
 	return counter_base_frequency;
 }
 
-void fvp_cci_setup(void)
+void fvp_cci_init(void)
 {
 	/*
-	 * Enable CCI-400 for this cluster. No need
+	 * Initialize CCI-400 driver
+	 */
+	if (plat_config.flags & CONFIG_HAS_CCI)
+		cci_init(CCI400_BASE,
+			CCI400_SL_IFACE3_CLUSTER_IX,
+			CCI400_SL_IFACE4_CLUSTER_IX);
+}
+
+void fvp_cci_enable(void)
+{
+	/*
+	 * Enable CCI-400 coherency for this cluster. No need
 	 * for locks as no other cpu is active at the
 	 * moment
 	 */
 	if (plat_config.flags & CONFIG_HAS_CCI)
-		cci_enable_coherency(read_mpidr());
+		cci_enable_cluster_coherency(read_mpidr());
 }
 
 void fvp_gic_init(void)
diff --git a/plat/fvp/aarch64/fvp_helpers.S b/plat/fvp/aarch64/fvp_helpers.S
index 4011306..94f15c0 100644
--- a/plat/fvp/aarch64/fvp_helpers.S
+++ b/plat/fvp/aarch64/fvp_helpers.S
@@ -32,9 +32,9 @@
 #include <asm_macros.S>
 #include <bl_common.h>
 #include <gic_v2.h>
+#include <platform_def.h>
 #include <pl011.h>
 #include "../drivers/pwrc/fvp_pwrc.h"
-#include "../fvp_def.h"
 
 	.globl	platform_get_entrypoint
 	.globl	plat_secondary_cold_boot_setup
@@ -140,7 +140,7 @@
 	 * its safe to read it here with SO attributes
 	 * ---------------------------------------------
 	 */
-	ldr	x10, =TZDRAM_BASE + MBOX_OFF
+	ldr	x10, =MBOX_BASE
 	bl	platform_get_core_pos
 	lsl	x0, x0, #CACHE_WRITEBACK_SHIFT
 	ldr	x0, [x10, x0]
@@ -153,8 +153,8 @@
 	/* -----------------------------------------------------
 	 * 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
+	 * Zero out the mailbox registers in the shared memory.
+	 * 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
@@ -163,7 +163,7 @@
 	 * -----------------------------------------------------
 	 */
 func platform_mem_init
-	ldr	x0, =TZDRAM_BASE + MBOX_OFF
+	ldr	x0, =MBOX_BASE
 	mov	w1, #PLATFORM_CORE_COUNT
 loop:
 	str	xzr, [x0], #CACHE_WRITEBACK_GRANULE
diff --git a/plat/fvp/bl1_fvp_setup.c b/plat/fvp/bl1_fvp_setup.c
index b146fdb..b1205d4 100644
--- a/plat/fvp/bl1_fvp_setup.c
+++ b/plat/fvp/bl1_fvp_setup.c
@@ -76,20 +76,17 @@
 	console_init(PL011_UART0_BASE, PL011_UART0_CLK_IN_HZ, PL011_BAUDRATE);
 
 	/* Allow BL1 to see the whole Trusted RAM */
-	bl1_tzram_layout.total_base = TZRAM_BASE;
-	bl1_tzram_layout.total_size = TZRAM_SIZE;
+	bl1_tzram_layout.total_base = FVP_TRUSTED_SRAM_BASE;
+	bl1_tzram_layout.total_size = FVP_TRUSTED_SRAM_SIZE;
 
 	/* Calculate how much RAM BL1 is using and how much remains free */
-	bl1_tzram_layout.free_base = TZRAM_BASE;
-	bl1_tzram_layout.free_size = TZRAM_SIZE;
+	bl1_tzram_layout.free_base = FVP_TRUSTED_SRAM_BASE;
+	bl1_tzram_layout.free_size = FVP_TRUSTED_SRAM_SIZE;
 	reserve_mem(&bl1_tzram_layout.free_base,
 		    &bl1_tzram_layout.free_size,
 		    BL1_RAM_BASE,
 		    bl1_size);
 
-	INFO("BL1: 0x%lx - 0x%lx [size = %u]\n", BL1_RAM_BASE, BL1_RAM_LIMIT,
-	     bl1_size);
-
 	/* Initialize the platform config for future decision making */
 	fvp_config_setup();
 }
@@ -113,12 +110,13 @@
  ******************************************************************************/
 void bl1_plat_arch_setup(void)
 {
-	fvp_cci_setup();
+	fvp_cci_init();
+	fvp_cci_enable();
 
 	fvp_configure_mmu_el3(bl1_tzram_layout.total_base,
 			      bl1_tzram_layout.total_size,
-			      TZROM_BASE,
-			      TZROM_BASE + TZROM_SIZE,
+			      BL1_RO_BASE,
+			      BL1_RO_LIMIT,
 			      BL1_COHERENT_RAM_BASE,
 			      BL1_COHERENT_RAM_LIMIT);
 }
diff --git a/plat/fvp/bl2_fvp_setup.c b/plat/fvp/bl2_fvp_setup.c
index c0ad340..2c26d97 100644
--- a/plat/fvp/bl2_fvp_setup.c
+++ b/plat/fvp/bl2_fvp_setup.c
@@ -72,6 +72,11 @@
 __attribute__ ((aligned(PLATFORM_CACHE_LINE_SIZE),
 		section("tzfw_coherent_mem")));
 
+/* Assert that BL3-1 parameters fit in shared memory */
+CASSERT((PARAMS_BASE + sizeof(bl2_to_bl31_params_mem_t)) <
+	(FVP_SHARED_RAM_BASE + FVP_SHARED_RAM_SIZE),
+	assert_bl31_params_do_not_fit_in_shared_memory);
+
 /*******************************************************************************
  * Reference to structures which holds the arguments which need to be passed
  * to BL31
@@ -97,14 +102,6 @@
 {
 	bl2_to_bl31_params_mem_t *bl31_params_mem;
 
-#if TSP_RAM_LOCATION_ID == TSP_IN_TZDRAM
-	/*
-	 * Ensure that the secure DRAM memory used for passing BL31 arguments
-	 * does not overlap with the BL32_BASE.
-	 */
-	assert(BL32_BASE > PARAMS_BASE + sizeof(bl2_to_bl31_params_mem_t));
-#endif
-
 	/*
 	 * Allocate the memory for all the arguments that needs to
 	 * be passed to BL31
diff --git a/plat/fvp/bl31_fvp_setup.c b/plat/fvp/bl31_fvp_setup.c
index 0693a12..69efc9c 100644
--- a/plat/fvp/bl31_fvp_setup.c
+++ b/plat/fvp/bl31_fvp_setup.c
@@ -230,9 +230,9 @@
  ******************************************************************************/
 void bl31_plat_arch_setup(void)
 {
+	fvp_cci_init();
 #if RESET_TO_BL31
-	fvp_cci_setup();
-
+	fvp_cci_enable();
 #endif
 	fvp_configure_mmu_el3(BL31_RO_BASE,
 			      (BL31_COHERENT_RAM_LIMIT - BL31_RO_BASE),
diff --git a/plat/fvp/fvp_def.h b/plat/fvp/fvp_def.h
index a757b4d..b2c26fc 100644
--- a/plat/fvp/fvp_def.h
+++ b/plat/fvp/fvp_def.h
@@ -1,4 +1,4 @@
-#/*
+/*
  * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -31,19 +31,29 @@
 #ifndef __FVP_DEF_H__
 #define __FVP_DEF_H__
 
-#include <platform_def.h> /* for TZROM_SIZE */
-
-
 /* Firmware Image Package */
 #define FIP_IMAGE_NAME			"fip.bin"
 #define FVP_PRIMARY_CPU			0x0
 
+/* Memory location options for Shared data and TSP in FVP */
+#define FVP_IN_TRUSTED_SRAM		0
+#define FVP_IN_TRUSTED_DRAM		1
+
 /*******************************************************************************
  * FVP memory map related constants
  ******************************************************************************/
 
+#define FVP_TRUSTED_ROM_BASE	0x00000000
+#define FVP_TRUSTED_ROM_SIZE	0x04000000	/* 64 MB */
+
+#define FVP_TRUSTED_SRAM_BASE	0x04000000
+#define FVP_TRUSTED_SRAM_SIZE	0x00040000	/* 256 KB */
+
+#define FVP_TRUSTED_DRAM_BASE	0x06000000
+#define FVP_TRUSTED_DRAM_SIZE	0x02000000	/* 32 MB */
+
 #define FLASH0_BASE		0x08000000
-#define FLASH0_SIZE		TZROM_SIZE
+#define FLASH0_SIZE		0x04000000
 
 #define FLASH1_BASE		0x0c000000
 #define FLASH1_SIZE		0x04000000
@@ -64,10 +74,27 @@
 #define NSRAM_BASE		0x2e000000
 #define NSRAM_SIZE		0x10000
 
-#define MBOX_OFF		0x1000
+/* 4KB shared memory */
+#define FVP_SHARED_RAM_SIZE	0x1000
 
-/* Base address where parameters to BL31 are stored */
-#define PARAMS_BASE		TZDRAM_BASE
+/* Location of shared memory */
+#if (FVP_SHARED_DATA_LOCATION_ID == FVP_IN_TRUSTED_DRAM)
+/* Shared memory at the base of Trusted DRAM */
+# define FVP_SHARED_RAM_BASE		FVP_TRUSTED_DRAM_BASE
+# define FVP_TRUSTED_SRAM_LIMIT		(FVP_TRUSTED_SRAM_BASE \
+					+ FVP_TRUSTED_SRAM_SIZE)
+#elif (FVP_SHARED_DATA_LOCATION_ID == FVP_IN_TRUSTED_SRAM)
+# if (FVP_TSP_RAM_LOCATION_ID == FVP_IN_TRUSTED_DRAM)
+#  error "Shared data in Trusted SRAM and TSP in Trusted DRAM is not supported"
+# endif
+/* Shared memory at the top of the Trusted SRAM */
+# define FVP_SHARED_RAM_BASE		(FVP_TRUSTED_SRAM_BASE \
+					+ FVP_TRUSTED_SRAM_SIZE \
+					- FVP_SHARED_RAM_SIZE)
+# define FVP_TRUSTED_SRAM_LIMIT		FVP_SHARED_RAM_BASE
+#else
+# error "Unsupported FVP_SHARED_DATA_LOCATION_ID value"
+#endif
 
 #define DRAM1_BASE		0x80000000ull
 #define DRAM1_SIZE		0x80000000ull
@@ -157,11 +184,8 @@
  * CCI-400 related constants
  ******************************************************************************/
 #define CCI400_BASE			0x2c090000
-#define CCI400_SL_IFACE_CLUSTER0	3
-#define CCI400_SL_IFACE_CLUSTER1	4
-#define CCI400_SL_IFACE_INDEX(mpidr)	(mpidr & MPIDR_CLUSTER_MASK ? \
-					 CCI400_SL_IFACE_CLUSTER1 :   \
-					 CCI400_SL_IFACE_CLUSTER0)
+#define CCI400_SL_IFACE3_CLUSTER_IX	0
+#define CCI400_SL_IFACE4_CLUSTER_IX	1
 
 /*******************************************************************************
  * GIC-400 & interrupt handling related constants
@@ -215,9 +239,6 @@
  * The NSAIDs for this platform as used to program the TZC400.
  */
 
-/* The FVP has 4 bits of NSAIDs. Used with TZC FAIL_ID (ACE Lite ID width) */
-#define FVP_AID_WIDTH			4
-
 /* NSAIDs used by devices in TZC filter 0 on FVP */
 #define FVP_NSAID_DEFAULT		0
 #define FVP_NSAID_PCI			1
@@ -229,5 +250,15 @@
 #define FVP_NSAID_HDLCD0		2
 #define FVP_NSAID_CLCD			7
 
+/*******************************************************************************
+ *  Shared Data
+ ******************************************************************************/
+
+/* Entrypoint mailboxes */
+#define MBOX_BASE		FVP_SHARED_RAM_BASE
+#define MBOX_SIZE		0x200
+
+/* Base address where parameters to BL31 are stored */
+#define PARAMS_BASE		(MBOX_BASE + MBOX_SIZE)
 
 #endif /* __FVP_DEF_H__ */
diff --git a/plat/fvp/fvp_io_storage.c b/plat/fvp/fvp_io_storage.c
index c32cca9..b4a04f1 100644
--- a/plat/fvp/fvp_io_storage.c
+++ b/plat/fvp/fvp_io_storage.c
@@ -35,12 +35,11 @@
 #include <io_memmap.h>
 #include <io_storage.h>
 #include <io_semihosting.h>
+#include <platform_def.h>
 #include <semihosting.h>	/* For FOPEN_MODE_... */
 #include <string.h>
-#include "fvp_def.h"
 
 /* IO devices */
-static io_plat_data_t io_data;
 static const io_dev_connector_t *sh_dev_con;
 static uintptr_t sh_dev_spec;
 static uintptr_t sh_init_params;
@@ -127,7 +126,7 @@
 	/* See if a Firmware Image Package is available */
 	result = io_dev_init(fip_dev_handle, (uintptr_t)FIP_IMAGE_NAME);
 	if (result == IO_SUCCESS) {
-		INFO("Using FIP\n");
+		VERBOSE("Using FIP\n");
 		/*TODO: Check image defined in spec is present in FIP. */
 	}
 	return result;
@@ -143,7 +142,7 @@
 	if (result == IO_SUCCESS) {
 		result = io_open(memmap_dev_handle, spec, &local_image_handle);
 		if (result == IO_SUCCESS) {
-			/* INFO("Using Memmap IO\n"); */
+			VERBOSE("Using Memmap IO\n");
 			io_close(local_image_handle);
 		}
 	}
@@ -161,7 +160,7 @@
 	if (result == IO_SUCCESS) {
 		result = io_open(sh_dev_handle, spec, &local_image_handle);
 		if (result == IO_SUCCESS) {
-			INFO("Using Semi-hosting IO\n");
+			VERBOSE("Using Semi-hosting IO\n");
 			io_close(local_image_handle);
 		}
 	}
@@ -172,9 +171,6 @@
 {
 	int io_result = IO_FAIL;
 
-	/* Initialise the IO layer */
-	io_init(&io_data);
-
 	/* Register the IO devices on this platform */
 	io_result = register_io_dev_sh(&sh_dev_con);
 	assert(io_result == IO_SUCCESS);
diff --git a/plat/fvp/fvp_pm.c b/plat/fvp/fvp_pm.c
index 22e53e1..b7e49a2 100644
--- a/plat/fvp/fvp_pm.c
+++ b/plat/fvp/fvp_pm.c
@@ -103,7 +103,7 @@
 	} while (psysr & PSYSR_AFF_L0);
 
 	linear_id = platform_get_core_pos(mpidr);
-	fvp_mboxes = (mailbox_t *) (TZDRAM_BASE + MBOX_OFF);
+	fvp_mboxes = (mailbox_t *)MBOX_BASE;
 	fvp_mboxes[linear_id].value = sec_entrypoint;
 	flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
 			   sizeof(unsigned long));
@@ -140,7 +140,7 @@
 			 * turned off
 			 */
 			if (get_plat_config()->flags & CONFIG_HAS_CCI)
-				cci_disable_coherency(mpidr);
+				cci_disable_cluster_coherency(mpidr);
 
 			/*
 			 * Program the power controller to turn the
@@ -215,7 +215,7 @@
 			 * turned off
 			 */
 			if (get_plat_config()->flags & CONFIG_HAS_CCI)
-				cci_disable_coherency(mpidr);
+				cci_disable_cluster_coherency(mpidr);
 
 			/*
 			 * Program the power controller to turn the
@@ -240,7 +240,7 @@
 
 			/* Program the jump address for the target cpu */
 			linear_id = platform_get_core_pos(mpidr);
-			fvp_mboxes = (mailbox_t *) (TZDRAM_BASE + MBOX_OFF);
+			fvp_mboxes = (mailbox_t *)MBOX_BASE;
 			fvp_mboxes[linear_id].value = sec_entrypoint;
 			flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
 					   sizeof(unsigned long));
@@ -302,7 +302,7 @@
 			 */
 			fvp_pwrc_write_pponr(mpidr);
 
-			fvp_cci_setup();
+			fvp_cci_enable();
 		}
 		break;
 
@@ -329,7 +329,7 @@
 		fvp_pwrc_clr_wen(mpidr);
 
 		/* Zero the jump address in the mailbox for this cpu */
-		fvp_mboxes = (mailbox_t *) (TZDRAM_BASE + MBOX_OFF);
+		fvp_mboxes = (mailbox_t *)MBOX_BASE;
 		linear_id = platform_get_core_pos(mpidr);
 		fvp_mboxes[linear_id].value = 0;
 		flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
diff --git a/plat/fvp/fvp_private.h b/plat/fvp/fvp_private.h
index 054baa8..2dcb327 100644
--- a/plat/fvp/fvp_private.h
+++ b/plat/fvp/fvp_private.h
@@ -77,7 +77,8 @@
 			   unsigned long);
 int fvp_config_setup(void);
 
-void fvp_cci_setup(void);
+void fvp_cci_init(void);
+void fvp_cci_enable(void);
 
 void fvp_gic_init(void);
 
diff --git a/plat/fvp/fvp_security.c b/plat/fvp/fvp_security.c
index 0adbbc5..06ab575 100644
--- a/plat/fvp/fvp_security.c
+++ b/plat/fvp/fvp_security.c
@@ -46,8 +46,6 @@
  */
 void fvp_security_setup(void)
 {
-	tzc_instance_t controller;
-
 	/*
 	 * The Base FVP has a TrustZone address space controller, the Foundation
 	 * FVP does not. Trying to program the device on the foundation FVP will
@@ -71,9 +69,7 @@
 	 * - Provide base address of device on platform.
 	 * - Provide width of ACE-Lite IDs on platform.
 	 */
-	controller.base = TZC400_BASE;
-	controller.aid_width = FVP_AID_WIDTH;
-	tzc_init(&controller);
+	tzc_init(TZC400_BASE);
 
 	/*
 	 * Currently only filters 0 and 2 are connected on Base FVP.
@@ -87,7 +83,7 @@
 	 */
 
 	/* Disable all filters before programming. */
-	tzc_disable_filters(&controller);
+	tzc_disable_filters();
 
 	/*
 	 * Allow only non-secure access to all DRAM to supported devices.
@@ -101,7 +97,7 @@
 	 */
 
 	/* Set to cover the first block of DRAM */
-	tzc_configure_region(&controller, FILTER_SHIFT(0), 1,
+	tzc_configure_region(FILTER_SHIFT(0), 1,
 			DRAM1_BASE, DRAM1_END - DRAM1_SEC_SIZE,
 			TZC_REGION_S_NONE,
 			TZC_REGION_ACCESS_RDWR(FVP_NSAID_DEFAULT) |
@@ -111,13 +107,13 @@
 			TZC_REGION_ACCESS_RDWR(FVP_NSAID_VIRTIO_OLD));
 
 	/* Set to cover the secure reserved region */
-	tzc_configure_region(&controller, FILTER_SHIFT(0), 3,
+	tzc_configure_region(FILTER_SHIFT(0), 3,
 			(DRAM1_END - DRAM1_SEC_SIZE) + 1 , DRAM1_END,
 			TZC_REGION_S_RDWR,
 			0x0);
 
 	/* Set to cover the second block of DRAM */
-	tzc_configure_region(&controller, FILTER_SHIFT(0), 2,
+	tzc_configure_region(FILTER_SHIFT(0), 2,
 			DRAM2_BASE, DRAM2_END, TZC_REGION_S_NONE,
 			TZC_REGION_ACCESS_RDWR(FVP_NSAID_DEFAULT) |
 			TZC_REGION_ACCESS_RDWR(FVP_NSAID_PCI) |
@@ -130,8 +126,8 @@
 	 * options we have are for access errors to occur quietly or to
 	 * cause an exception. We choose to cause an exception.
 	 */
-	tzc_set_action(&controller, TZC_ACTION_ERR);
+	tzc_set_action(TZC_ACTION_ERR);
 
 	/* Enable filters. */
-	tzc_enable_filters(&controller);
+	tzc_enable_filters();
 }
diff --git a/plat/fvp/include/plat_macros.S b/plat/fvp/include/plat_macros.S
index 727b958..5d11d36 100644
--- a/plat/fvp/include/plat_macros.S
+++ b/plat/fvp/include/plat_macros.S
@@ -30,7 +30,7 @@
 #include <cci400.h>
 #include <gic_v2.h>
 #include <plat_config.h>
-#include "platform_def.h"
+#include "../fvp_def.h"
 
 .section .rodata.gic_reg_name, "aS"
 gic_regs:
diff --git a/plat/fvp/include/platform_def.h b/plat/fvp/include/platform_def.h
index 70f84bb..32f070f 100644
--- a/plat/fvp/include/platform_def.h
+++ b/plat/fvp/include/platform_def.h
@@ -32,6 +32,7 @@
 #define __PLATFORM_DEF_H__
 
 #include <arch.h>
+#include "../fvp_def.h"
 
 
 /*******************************************************************************
@@ -45,9 +46,19 @@
  ******************************************************************************/
 
 /* Size of cacheable stacks */
-#define PLATFORM_STACK_SIZE	0x800
+#if DEBUG_XLAT_TABLE
+#define PLATFORM_STACK_SIZE 0x800
+#elif IMAGE_BL1
+#define PLATFORM_STACK_SIZE 0x440
+#elif IMAGE_BL2
+#define PLATFORM_STACK_SIZE 0x400
+#elif IMAGE_BL31
+#define PLATFORM_STACK_SIZE 0x400
+#elif IMAGE_BL32
+#define PLATFORM_STACK_SIZE 0x440
+#endif
 
-#define FIRMWARE_WELCOME_STR		"Booting trusted firmware boot loader stage 1\n\r"
+#define FIRMWARE_WELCOME_STR		"Booting Trusted Firmware\n"
 
 /* Trusted Boot Firmware BL2 */
 #define BL2_IMAGE_NAME			"bl2.bin"
@@ -74,31 +85,20 @@
 #define MAX_IO_HANDLES			4
 
 /*******************************************************************************
- * Platform memory map related constants
- ******************************************************************************/
-#define TZROM_BASE		0x00000000
-#define TZROM_SIZE		0x04000000
-
-#define TZRAM_BASE		0x04000000
-#define TZRAM_SIZE		0x40000
-
-/* Location of trusted dram on the base fvp */
-#define TZDRAM_BASE		0x06000000
-#define TZDRAM_SIZE		0x02000000
-
-/*******************************************************************************
  * BL1 specific defines.
  * BL1 RW data is relocated from ROM to RAM at runtime so we need 2 sets of
  * addresses.
  ******************************************************************************/
-#define BL1_RO_BASE			TZROM_BASE
-#define BL1_RO_LIMIT			(TZROM_BASE + TZROM_SIZE)
+#define BL1_RO_BASE			FVP_TRUSTED_ROM_BASE
+#define BL1_RO_LIMIT			(FVP_TRUSTED_ROM_BASE \
+					+ FVP_TRUSTED_ROM_SIZE)
 /*
- * Put BL1 RW at the top of the Trusted SRAM. BL1_RW_BASE is calculated using
- * the current BL1 RW debug size plus a little space for growth.
+ * Put BL1 RW at the top of the Trusted SRAM (just below the shared memory, if
+ * present). BL1_RW_BASE is calculated using the current BL1 RW debug size plus
+ * a little space for growth.
  */
-#define BL1_RW_BASE			(TZRAM_BASE + TZRAM_SIZE - 0x6000)
-#define BL1_RW_LIMIT			(TZRAM_BASE + TZRAM_SIZE)
+#define BL1_RW_BASE			(FVP_TRUSTED_SRAM_LIMIT - 0x6000)
+#define BL1_RW_LIMIT			FVP_TRUSTED_SRAM_LIMIT
 
 /*******************************************************************************
  * BL2 specific defines.
@@ -114,12 +114,13 @@
  * BL31 specific defines.
  ******************************************************************************/
 /*
- * Put BL3-1 at the top of the Trusted SRAM. BL31_BASE is calculated using the
- * current BL3-1 debug size plus a little space for growth.
+ * Put BL3-1 at the top of the Trusted SRAM (just below the shared memory, if
+ * present). BL31_BASE is calculated using the current BL3-1 debug size plus a
+ * little space for growth.
  */
-#define BL31_BASE			(TZRAM_BASE + TZRAM_SIZE - 0x1D000)
+#define BL31_BASE			(FVP_TRUSTED_SRAM_LIMIT - 0x1D000)
 #define BL31_PROGBITS_LIMIT		BL1_RW_BASE
-#define BL31_LIMIT			(TZRAM_BASE + TZRAM_SIZE)
+#define BL31_LIMIT			FVP_TRUSTED_SRAM_LIMIT
 
 /*******************************************************************************
  * BL32 specific defines.
@@ -127,24 +128,27 @@
 /*
  * On FVP, the TSP can execute either from Trusted SRAM or Trusted DRAM.
  */
-#define TSP_IN_TZRAM			0
-#define TSP_IN_TZDRAM			1
-
-#if TSP_RAM_LOCATION_ID == TSP_IN_TZRAM
-# define TSP_SEC_MEM_BASE		TZRAM_BASE
-# define TSP_SEC_MEM_SIZE		TZRAM_SIZE
-# define BL32_BASE			TZRAM_BASE
-# define BL32_PROGBITS_LIMIT		BL2_BASE
+#if FVP_TSP_RAM_LOCATION_ID == FVP_IN_TRUSTED_SRAM
+# define TSP_SEC_MEM_BASE		FVP_TRUSTED_SRAM_BASE
+# define TSP_SEC_MEM_SIZE		FVP_TRUSTED_SRAM_SIZE
+# define TSP_PROGBITS_LIMIT		BL2_BASE
+# define BL32_BASE			FVP_TRUSTED_SRAM_BASE
 # define BL32_LIMIT			BL31_BASE
-#elif TSP_RAM_LOCATION_ID == TSP_IN_TZDRAM
-# define TSP_SEC_MEM_BASE		TZDRAM_BASE
-# define TSP_SEC_MEM_SIZE		TZDRAM_SIZE
-# define BL32_BASE			(TZDRAM_BASE + 0x2000)
-# define BL32_LIMIT			(TZDRAM_BASE + (1 << 21))
+#elif FVP_TSP_RAM_LOCATION_ID == FVP_IN_TRUSTED_DRAM
+# define TSP_SEC_MEM_BASE		FVP_TRUSTED_DRAM_BASE
+# define TSP_SEC_MEM_SIZE		FVP_TRUSTED_DRAM_SIZE
+# define BL32_BASE			(FVP_TRUSTED_DRAM_BASE \
+					+ FVP_SHARED_RAM_SIZE)
+# define BL32_LIMIT			(FVP_TRUSTED_DRAM_BASE + (1 << 21))
 #else
-# error "Unsupported TSP_RAM_LOCATION_ID value"
+# error "Unsupported FVP_TSP_RAM_LOCATION_ID value"
 #endif
 
+/*
+ * ID of the secure physical generic timer interrupt used by the TSP.
+ */
+#define TSP_IRQ_SEC_PHY_TIMER		IRQ_SEC_PHY_TIMER
+
 /*******************************************************************************
  * Platform specific page table and MMU setup constants
  ******************************************************************************/
@@ -153,22 +157,6 @@
 #define MAX_MMAP_REGIONS		16
 
 /*******************************************************************************
- * ID of the secure physical generic timer interrupt.
- ******************************************************************************/
-#define IRQ_SEC_PHY_TIMER		29
-
-/*******************************************************************************
- * CCI-400 related constants
- ******************************************************************************/
-#define CCI400_BASE			0x2c090000
-#define CCI400_SL_IFACE_CLUSTER0	3
-#define CCI400_SL_IFACE_CLUSTER1	4
-#define CCI400_SL_IFACE_INDEX(mpidr)	(mpidr & MPIDR_CLUSTER_MASK ? \
-					 CCI400_SL_IFACE_CLUSTER1 :   \
-					 CCI400_SL_IFACE_CLUSTER0)
-
-
-/*******************************************************************************
  * Declarations and constants to access the mailboxes safely. Each mailbox is
  * aligned on the biggest cache line size in the platform. This is known only
  * to the platform as it might have a combination of integrated and external
diff --git a/plat/fvp/platform.mk b/plat/fvp/platform.mk
index f6275b7..ffed7e4 100644
--- a/plat/fvp/platform.mk
+++ b/plat/fvp/platform.mk
@@ -28,20 +28,37 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
-# On FVP, the TSP can execute either from Trusted SRAM or Trusted DRAM.
-# Trusted SRAM is the default.
-TSP_RAM_LOCATION	:=	tsram
-
-ifeq (${TSP_RAM_LOCATION}, tsram)
-  TSP_RAM_LOCATION_ID := TSP_IN_TZRAM
-else ifeq (${TSP_RAM_LOCATION}, tdram)
-  TSP_RAM_LOCATION_ID := TSP_IN_TZDRAM
+# Shared memory may be allocated at the top of Trusted SRAM (tsram) or at the
+# base of Trusted SRAM (tdram)
+FVP_SHARED_DATA_LOCATION	:=	tsram
+ifeq (${FVP_SHARED_DATA_LOCATION}, tsram)
+  FVP_SHARED_DATA_LOCATION_ID := FVP_IN_TRUSTED_SRAM
+else ifeq (${FVP_SHARED_DATA_LOCATION}, tdram)
+  FVP_SHARED_DATA_LOCATION_ID := FVP_IN_TRUSTED_DRAM
 else
-  $(error "Unsupported TSP_RAM_LOCATION value")
+  $(error "Unsupported FVP_SHARED_DATA_LOCATION value")
 endif
 
-# Process TSP_RAM_LOCATION_ID flag
-$(eval $(call add_define,TSP_RAM_LOCATION_ID))
+# On FVP, the TSP can execute either from Trusted SRAM or Trusted DRAM.
+# Trusted SRAM is the default.
+FVP_TSP_RAM_LOCATION	:=	tsram
+ifeq (${FVP_TSP_RAM_LOCATION}, tsram)
+  FVP_TSP_RAM_LOCATION_ID := FVP_IN_TRUSTED_SRAM
+else ifeq (${FVP_TSP_RAM_LOCATION}, tdram)
+  FVP_TSP_RAM_LOCATION_ID := FVP_IN_TRUSTED_DRAM
+else
+  $(error "Unsupported FVP_TSP_RAM_LOCATION value")
+endif
+
+ifeq (${FVP_SHARED_DATA_LOCATION}, tsram)
+  ifeq (${FVP_TSP_RAM_LOCATION}, tdram)
+    $(error Shared data in Trusted SRAM and TSP in Trusted DRAM is not supported)
+  endif
+endif
+
+# Process flags
+$(eval $(call add_define,FVP_SHARED_DATA_LOCATION_ID))
+$(eval $(call add_define,FVP_TSP_RAM_LOCATION_ID))
 
 PLAT_INCLUDES		:=	-Iplat/fvp/include/
 
@@ -49,6 +66,7 @@
 				drivers/io/io_fip.c				\
 				drivers/io/io_memmap.c				\
 				drivers/io/io_semihosting.c			\
+				drivers/io/io_storage.c				\
 				lib/aarch64/xlat_tables.c			\
 				lib/semihosting/semihosting.c			\
 				lib/semihosting/aarch64/semihosting_call.S	\
diff --git a/bl32/tsp/tsp-fvp.mk b/plat/fvp/tsp/tsp-fvp.mk
similarity index 97%
rename from bl32/tsp/tsp-fvp.mk
rename to plat/fvp/tsp/tsp-fvp.mk
index 3220c08..d2e112a 100644
--- a/bl32/tsp/tsp-fvp.mk
+++ b/plat/fvp/tsp/tsp-fvp.mk
@@ -35,4 +35,4 @@
 				plat/common/plat_gic.c				\
 				plat/fvp/aarch64/fvp_common.c			\
 				plat/fvp/aarch64/fvp_helpers.S			\
-				plat/fvp/bl32_fvp_setup.c
+				plat/fvp/tsp/tsp_fvp_setup.c
diff --git a/plat/fvp/bl32_fvp_setup.c b/plat/fvp/tsp/tsp_fvp_setup.c
similarity index 95%
rename from plat/fvp/bl32_fvp_setup.c
rename to plat/fvp/tsp/tsp_fvp_setup.c
index aa49ff3..ae63a7d 100644
--- a/plat/fvp/bl32_fvp_setup.c
+++ b/plat/fvp/tsp/tsp_fvp_setup.c
@@ -30,9 +30,9 @@
 
 #include <bl_common.h>
 #include <console.h>
-#include <platform.h>
-#include "fvp_def.h"
-#include "fvp_private.h"
+#include <platform_tsp.h>
+#include "../fvp_def.h"
+#include "../fvp_private.h"
 
 /*******************************************************************************
  * Declarations of linker defined symbols which will help us find the layout
@@ -66,7 +66,7 @@
 /*******************************************************************************
  * Initialize the UART
  ******************************************************************************/
-void bl32_early_platform_setup(void)
+void tsp_early_platform_setup(void)
 {
 	/*
 	 * Initialize a different console than already in use to display
@@ -81,7 +81,7 @@
 /*******************************************************************************
  * Perform platform specific setup placeholder
  ******************************************************************************/
-void bl32_platform_setup(void)
+void tsp_platform_setup(void)
 {
 	fvp_gic_init();
 }
@@ -90,7 +90,7 @@
  * Perform the very early platform specific architectural setup here. At the
  * moment this is only intializes the MMU
  ******************************************************************************/
-void bl32_plat_arch_setup(void)
+void tsp_plat_arch_setup(void)
 {
 	fvp_configure_mmu_el1(BL32_RO_BASE,
 			      (BL32_COHERENT_RAM_LIMIT - BL32_RO_BASE),
diff --git a/services/spd/tspd/tspd.mk b/services/spd/tspd/tspd.mk
index a32f4c4..cd4b45a 100644
--- a/services/spd/tspd/tspd.mk
+++ b/services/spd/tspd/tspd.mk
@@ -29,7 +29,7 @@
 #
 
 TSPD_DIR		:=	services/spd/tspd
-SPD_INCLUDES		:=	-Iinclude/bl32/payloads
+SPD_INCLUDES		:=	-Iinclude/bl32/tsp
 
 SPD_SOURCES		:=	services/spd/tspd/tspd_common.c		\
 				services/spd/tspd/tspd_helpers.S	\