Merge pull request #561 from antonio-nino-diaz-arm/an/bootwrapper

Enable preloaded BL33 alternative boot flow
diff --git a/Makefile b/Makefile
index 3ab44af..6d17cfd 100644
--- a/Makefile
+++ b/Makefile
@@ -197,7 +197,19 @@
 				lib/aarch64/cache_helpers.S		\
 				lib/aarch64/misc_helpers.S		\
 				lib/aarch64/xlat_helpers.c		\
-				lib/stdlib/std.c			\
+				lib/stdlib/abort.c			\
+				lib/stdlib/assert.c			\
+				lib/stdlib/exit.c			\
+				lib/stdlib/mem.c			\
+				lib/stdlib/printf.c			\
+				lib/stdlib/putchar.c			\
+				lib/stdlib/puts.c			\
+				lib/stdlib/sscanf.c			\
+				lib/stdlib/strchr.c			\
+				lib/stdlib/strcmp.c			\
+				lib/stdlib/strlen.c			\
+				lib/stdlib/strncmp.c			\
+				lib/stdlib/subr_prf.c			\
 				plat/common/aarch64/platform_helpers.S
 
 INCLUDES		+=	-Iinclude/bl1			\
diff --git a/bl1/bl1_context_mgmt.c b/bl1/bl1_context_mgmt.c
index 6355190..bd40608 100644
--- a/bl1/bl1_context_mgmt.c
+++ b/bl1/bl1_context_mgmt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -74,7 +74,7 @@
 	next_bl_ep = &image_desc->ep_info;
 
 	/* Get the image security state. */
-	security_state = GET_SEC_STATE(next_bl_ep->h.attr);
+	security_state = GET_SECURITY_STATE(next_bl_ep->h.attr);
 
 	/* Setup the Secure/Non-Secure context if not done already. */
 	if (!cm_get_context(security_state))
diff --git a/bl1/bl1_fwu.c b/bl1/bl1_fwu.c
index 80ce831..f333805 100644
--- a/bl1/bl1_fwu.c
+++ b/bl1/bl1_fwu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -135,8 +135,8 @@
 	}
 
 	/* Only Normal world is allowed to copy a Secure image. */
-	if ((GET_SEC_STATE(flags) == SECURE) ||
-		(GET_SEC_STATE(image_desc->ep_info.h.attr) == NON_SECURE)) {
+	if ((GET_SECURITY_STATE(flags) == SECURE) ||
+	    (GET_SECURITY_STATE(image_desc->ep_info.h.attr) == NON_SECURE)) {
 		WARN("BL1-FWU: Copy not allowed for Non-Secure "
 			 "image from Secure-world\n");
 		return -EPERM;
@@ -156,10 +156,10 @@
 		 * If last block is more than expected then
 		 * clip the block to the required image size.
 		 */
-		if (image_desc->image_info.copied_size + block_size >
+		if (image_desc->copied_size + block_size >
 			 image_desc->image_info.image_size) {
 			block_size = image_desc->image_info.image_size -
-				image_desc->image_info.copied_size;
+				image_desc->copied_size;
 			WARN("BL1-FWU: Copy argument block_size > remaining image size."
 				" Clipping block_size\n");
 		}
@@ -173,13 +173,13 @@
 		INFO("BL1-FWU: Continuing image copy in blocks\n");
 
 		/* Copy image for given block size. */
-		base_addr += image_desc->image_info.copied_size;
-		image_desc->image_info.copied_size += block_size;
+		base_addr += image_desc->copied_size;
+		image_desc->copied_size += block_size;
 		memcpy((void *)base_addr, (const void *)image_src, block_size);
 		flush_dcache_range(base_addr, block_size);
 
 		/* Update the state if last block. */
-		if (image_desc->image_info.copied_size ==
+		if (image_desc->copied_size ==
 				image_desc->image_info.image_size) {
 			image_desc->state = IMAGE_STATE_COPIED;
 			INFO("BL1-FWU: Image copy in blocks completed\n");
@@ -234,7 +234,7 @@
 			INFO("BL1-FWU: Started image copy in blocks\n");
 		}
 
-		image_desc->image_info.copied_size = block_size;
+		image_desc->copied_size = block_size;
 	}
 
 	return 0;
@@ -257,14 +257,14 @@
 	if (!image_desc)
 		return -EPERM;
 
-	if (GET_SEC_STATE(flags) == SECURE) {
+	if (GET_SECURITY_STATE(flags) == SECURE) {
 		if (image_desc->state != IMAGE_STATE_RESET) {
 			WARN("BL1-FWU: Authentication from secure world "
 				"while in invalid state\n");
 			return -EPERM;
 		}
 	} else {
-		if (GET_SEC_STATE(image_desc->ep_info.h.attr) == SECURE) {
+		if (GET_SECURITY_STATE(image_desc->ep_info.h.attr) == SECURE) {
 			if (image_desc->state != IMAGE_STATE_COPIED) {
 				WARN("BL1-FWU: Authentication of secure image "
 					"from non-secure world while not in copied state\n");
@@ -369,10 +369,10 @@
 	 * Image is NOT in AUTHENTICATED state.
 	 */
 	if ((!image_desc) ||
-		(GET_SEC_STATE(flags) == SECURE) ||
-		(GET_SEC_STATE(image_desc->ep_info.h.attr) == NON_SECURE) ||
-		(GET_EXEC_STATE(image_desc->image_info.h.attr) == NON_EXECUTABLE) ||
-		(image_desc->state != IMAGE_STATE_AUTHENTICATED)) {
+	    (GET_SECURITY_STATE(flags) == SECURE) ||
+	    (GET_SECURITY_STATE(image_desc->ep_info.h.attr) == NON_SECURE) ||
+	    (EP_GET_EXE(image_desc->ep_info.h.attr) == NON_EXECUTABLE) ||
+	    (image_desc->state != IMAGE_STATE_AUTHENTICATED)) {
 		WARN("BL1-FWU: Execution not allowed due to invalid state/args\n");
 		return -EPERM;
 	}
@@ -402,7 +402,7 @@
 {
 	image_desc_t *image_desc;
 	unsigned int resume_sec_state;
-	unsigned int caller_sec_state = GET_SEC_STATE(flags);
+	unsigned int caller_sec_state = GET_SECURITY_STATE(flags);
 
 	/* Get the image descriptor for last executed secure image id. */
 	image_desc = bl1_plat_get_image_desc(sec_exec_image_id);
@@ -417,8 +417,8 @@
 		assert(image_desc);
 	}
 
-	assert(GET_SEC_STATE(image_desc->ep_info.h.attr) == SECURE);
-	assert(GET_EXEC_STATE(image_desc->image_info.h.attr) == EXECUTABLE);
+	assert(GET_SECURITY_STATE(image_desc->ep_info.h.attr) == SECURE);
+	assert(EP_GET_EXE(image_desc->ep_info.h.attr) == EXECUTABLE);
 
 	if (caller_sec_state == SECURE) {
 		assert(image_desc->state == IMAGE_STATE_EXECUTED);
@@ -458,7 +458,7 @@
 	image_desc_t *image_desc;
 
 	/* Make sure caller is from the secure world */
-	if (GET_SEC_STATE(flags) == NON_SECURE) {
+	if (GET_SECURITY_STATE(flags) == NON_SECURE) {
 		WARN("BL1-FWU: Image done not allowed from normal world\n");
 		return -EPERM;
 	}
@@ -468,8 +468,8 @@
 
 	/* image_desc must correspond to a valid secure executing image */
 	assert(image_desc);
-	assert(GET_SEC_STATE(image_desc->ep_info.h.attr) == SECURE);
-	assert(GET_EXEC_STATE(image_desc->image_info.h.attr) == EXECUTABLE);
+	assert(GET_SECURITY_STATE(image_desc->ep_info.h.attr) == SECURE);
+	assert(EP_GET_EXE(image_desc->ep_info.h.attr) == EXECUTABLE);
 	assert(image_desc->state == IMAGE_STATE_EXECUTED);
 
 	/* Update the flags. */
diff --git a/bl1/tbbr/tbbr_img_desc.c b/bl1/tbbr/tbbr_img_desc.c
index 42de851..7651f1c 100644
--- a/bl1/tbbr/tbbr_img_desc.c
+++ b/bl1/tbbr/tbbr_img_desc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -35,42 +35,46 @@
 image_desc_t bl1_tbbr_image_descs[] = {
     {
 	    .image_id = FWU_CERT_ID,
-	    .image_info.h.attr = SET_EXEC_STATE(NON_EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, image_info_t, 0),
 	    .image_info.image_base = BL2_BASE,
-	    .ep_info.h.attr = SET_SEC_STATE(SECURE),
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, entry_point_info_t, SECURE),
     },
 #if NS_BL1U_BASE
     {
 	    .image_id = NS_BL1U_IMAGE_ID,
-	    .image_info.h.attr = SET_EXEC_STATE(EXECUTABLE),
-	    .image_info.image_base = NS_BL1U_BASE,
-	    .ep_info.h.attr = SET_SEC_STATE(NON_SECURE),
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		    VERSION_1, entry_point_info_t, NON_SECURE | EXECUTABLE),
 	    .ep_info.pc = NS_BL1U_BASE,
     },
 #endif
 #if SCP_BL2U_BASE
     {
 	    .image_id = SCP_BL2U_IMAGE_ID,
-	    .image_info.h.attr = SET_EXEC_STATE(NON_EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, image_info_t, 0),
 	    .image_info.image_base = SCP_BL2U_BASE,
-	    .ep_info.h.attr = SET_SEC_STATE(SECURE),
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, entry_point_info_t, SECURE),
     },
 #endif
 #if BL2U_BASE
     {
 	    .image_id = BL2U_IMAGE_ID,
-	    .image_info.h.attr = SET_EXEC_STATE(EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+		    VERSION_1, image_info_t, 0),
 	    .image_info.image_base = BL2U_BASE,
-	    .ep_info.h.attr = SET_SEC_STATE(SECURE),
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		    VERSION_1, entry_point_info_t, SECURE | EXECUTABLE),
 	    .ep_info.pc = BL2U_BASE,
     },
 #endif
 #if NS_BL2U_BASE
     {
 	    .image_id = NS_BL2U_IMAGE_ID,
-	    .image_info.h.attr = SET_EXEC_STATE(NON_EXECUTABLE),
-	    .image_info.image_base = NS_BL2U_BASE,
-	    .ep_info.h.attr = SET_SEC_STATE(NON_SECURE),
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		    VERSION_1, entry_point_info_t, NON_SECURE),
     },
 #endif
 	    BL2_IMAGE_DESC,
diff --git a/bl2/aarch64/bl2_entrypoint.S b/bl2/aarch64/bl2_entrypoint.S
index 75eb02a..476efe8 100644
--- a/bl2/aarch64/bl2_entrypoint.S
+++ b/bl2/aarch64/bl2_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -43,8 +43,8 @@
 	 * available to BL2 for future use.
 	 * x0 is not currently used.
 	 * ---------------------------------------------
- 	 */
- 	mov	x20, x1
+	 */
+	mov	x20, x1
 
 	/* ---------------------------------------------
 	 * Set the exception vector to something sane.
@@ -126,6 +126,11 @@
 	 * ---------------------------------------------
 	 */
 	bl	bl2_main
-_panic:
-	b	_panic
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	bl	plat_panic_handler
+
 endfunc bl2_entrypoint
diff --git a/bl2u/aarch64/bl2u_entrypoint.S b/bl2u/aarch64/bl2u_entrypoint.S
index c9aad81..ef3db92 100644
--- a/bl2u/aarch64/bl2u_entrypoint.S
+++ b/bl2u/aarch64/bl2u_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -122,6 +122,10 @@
 	 */
 	bl	bl2u_main
 
-_panic:
-	b	_panic
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	bl	plat_panic_handler
+
 endfunc bl2u_entrypoint
diff --git a/bl31/aarch64/crash_reporting.S b/bl31/aarch64/crash_reporting.S
index 1d1e60d..ff91572 100644
--- a/bl31/aarch64/crash_reporting.S
+++ b/bl31/aarch64/crash_reporting.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -351,17 +351,17 @@
 	plat_print_interconnect_regs
 
 	/* Done reporting */
-	b	crash_panic
+	bl	plat_panic_handler
 endfunc do_crash_reporting
 
 #else	/* CRASH_REPORTING */
 func report_unhandled_exception
 report_unhandled_interrupt:
-	b	crash_panic
+	bl	plat_panic_handler
 endfunc report_unhandled_exception
 #endif	/* CRASH_REPORTING */
 
 
 func crash_panic
-	b	crash_panic
-endfunc crash_panic
\ No newline at end of file
+	bl	plat_panic_handler
+endfunc crash_panic
diff --git a/bl31/runtime_svc.c b/bl31/runtime_svc.c
index 5b7a21c..f011f11 100644
--- a/bl31/runtime_svc.c
+++ b/bl31/runtime_svc.c
@@ -103,8 +103,8 @@
 		 */
 		rc = validate_rt_svc_desc(&rt_svc_descs[index]);
 		if (rc) {
-			ERROR("Invalid runtime service descriptor 0x%lx (%s)\n",
-					(uintptr_t) &rt_svc_descs[index],
+			ERROR("Invalid runtime service descriptor %p (%s)\n",
+					(void *) &rt_svc_descs[index],
 					rt_svc_descs[index].name);
 			goto error;
 		}
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 531ab9b..453d2c1 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -391,7 +391,7 @@
 
 	/* Should never reach here */
 tsp_sel1_int_entry_panic:
-	b	tsp_sel1_int_entry_panic
+	bl	plat_panic_handler
 endfunc tsp_sel1_intr_entry
 
 	/*---------------------------------------------
@@ -407,8 +407,9 @@
 func tsp_cpu_resume_entry
 	bl	tsp_cpu_resume_main
 	restore_args_call_smc
-tsp_cpu_resume_panic:
-	b	tsp_cpu_resume_panic
+
+	/* Should never reach here */
+	bl	plat_panic_handler
 endfunc tsp_cpu_resume_entry
 
 	/*---------------------------------------------
@@ -419,8 +420,9 @@
 func tsp_fast_smc_entry
 	bl	tsp_smc_handler
 	restore_args_call_smc
-tsp_fast_smc_entry_panic:
-	b	tsp_fast_smc_entry_panic
+
+	/* Should never reach here */
+	bl	plat_panic_handler
 endfunc tsp_fast_smc_entry
 
 	/*---------------------------------------------
@@ -435,6 +437,7 @@
 	bl	tsp_smc_handler
 	msr	daifset, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
 	restore_args_call_smc
-tsp_std_smc_entry_panic:
-	b	tsp_std_smc_entry_panic
+
+	/* Should never reach here */
+	bl	plat_panic_handler
 endfunc tsp_std_smc_entry
diff --git a/bl32/tsp/aarch64/tsp_exceptions.S b/bl32/tsp/aarch64/tsp_exceptions.S
index d5e089f..edcfb71 100644
--- a/bl32/tsp/aarch64/tsp_exceptions.S
+++ b/bl32/tsp/aarch64/tsp_exceptions.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -108,24 +108,23 @@
 	 * -----------------------------------------------------
 	 */
 sync_exception_sp_el0:
-	wfi
-	b	sync_exception_sp_el0
+	bl	plat_panic_handler
 	check_vector_size sync_exception_sp_el0
 
 	.align	7
 
 irq_sp_el0:
-	b	irq_sp_el0
+	bl	plat_panic_handler
 	check_vector_size irq_sp_el0
 
 	.align	7
 fiq_sp_el0:
-	b	fiq_sp_el0
+	bl	plat_panic_handler
 	check_vector_size fiq_sp_el0
 
 	.align	7
 serror_sp_el0:
-	b	serror_sp_el0
+	bl	plat_panic_handler
 	check_vector_size serror_sp_el0
 
 
@@ -136,8 +135,7 @@
 	 */
 	.align	7
 sync_exception_sp_elx:
-	wfi
-	b	sync_exception_sp_elx
+	bl	plat_panic_handler
 	check_vector_size sync_exception_sp_elx
 
 	.align	7
@@ -152,7 +150,7 @@
 
 	.align	7
 serror_sp_elx:
-	b	serror_sp_elx
+	bl	plat_panic_handler
 	check_vector_size serror_sp_elx
 
 
@@ -163,23 +161,22 @@
 	 */
 	.align	7
 sync_exception_aarch64:
-	wfi
-	b	sync_exception_aarch64
+	bl	plat_panic_handler
 	check_vector_size sync_exception_aarch64
 
 	.align	7
 irq_aarch64:
-	b	irq_aarch64
+	bl	plat_panic_handler
 	check_vector_size irq_aarch64
 
 	.align	7
 fiq_aarch64:
-	b	fiq_aarch64
+	bl	plat_panic_handler
 	check_vector_size fiq_aarch64
 
 	.align	7
 serror_aarch64:
-	b	serror_aarch64
+	bl	plat_panic_handler
 	check_vector_size serror_aarch64
 
 
@@ -190,22 +187,21 @@
 	 */
 	.align	7
 sync_exception_aarch32:
-	wfi
-	b	sync_exception_aarch32
+	bl	plat_panic_handler
 	check_vector_size sync_exception_aarch32
 
 	.align	7
 irq_aarch32:
-	b	irq_aarch32
+	bl	plat_panic_handler
 	check_vector_size irq_aarch32
 
 	.align	7
 fiq_aarch32:
-	b	fiq_aarch32
+	bl	plat_panic_handler
 	check_vector_size fiq_aarch32
 
 	.align	7
 serror_aarch32:
-	b	serror_aarch32
+	bl	plat_panic_handler
 	check_vector_size serror_aarch32
 	.align	7
diff --git a/common/aarch64/debug.S b/common/aarch64/debug.S
index b3caafb..d353879 100644
--- a/common/aarch64/debug.S
+++ b/common/aarch64/debug.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -120,7 +120,7 @@
 /*
  * This function prints a hexadecimal number in x4.
  * In: x4 = the hexadecimal to print.
- * Clobber: x30, x0, x5, x1, x2, x3
+ * Clobber: x30, x0 - x3, x5
  */
 func asm_print_hex
 	mov	x3, x30
@@ -178,7 +178,7 @@
 	mov	x6, x30
 	bl	plat_crash_console_init
 	/* Check if the console is initialized */
-	cbz	x0, _panic_loop
+	cbz	x0, _panic_handler
 	/* The console is initialized */
 	adr	x4, panic_msg
 	bl	asm_print_str
@@ -186,7 +186,10 @@
 	/* The panic location is lr -4 */
 	sub	x4, x4, #4
 	bl	asm_print_hex
-_panic_loop:
-	b	_panic_loop
-endfunc do_panic
 
+_panic_handler:
+	/* Pass to plat_panic_handler the address from where el3_panic was
+	 * called, not the address of the call from el3_panic. */
+	mov	x30,x6
+	b	plat_panic_handler
+endfunc do_panic
diff --git a/common/bl_common.c b/common/bl_common.c
index 0eeef83..d5b095a 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -229,7 +229,8 @@
 		return io_result;
 	}
 
-	INFO("Loading image id=%u at address 0x%lx\n", image_id, image_base);
+	INFO("Loading image id=%u at address %p\n", image_id,
+		(void *) image_base);
 
 	/* Find the size of the image */
 	io_result = io_size(image_handle, &image_size);
@@ -242,8 +243,8 @@
 	/* Check that the memory where the image will be loaded is free */
 	if (!is_mem_free(mem_layout->free_base, mem_layout->free_size,
 			 image_base, image_size)) {
-		WARN("Failed to reserve memory: 0x%lx - 0x%lx\n",
-			image_base, image_base + image_size);
+		WARN("Failed to reserve memory: %p - %p\n", (void *) image_base,
+		     (void *) (image_base + image_size));
 		dump_load_info(image_base, image_size, mem_layout);
 		io_result = -ENOMEM;
 		goto exit;
@@ -268,8 +269,8 @@
 		reserve_mem(&mem_layout->free_base, &mem_layout->free_size,
 				image_base, image_size);
 	} else {
-		INFO("Skip reserving memory: 0x%lx - 0x%lx\n",
-				image_base, image_base + image_size);
+		INFO("Skip reserving memory: %p - %p\n", (void *) image_base,
+		     (void *) (image_base + image_size));
 	}
 
 	image_data->image_base = image_base;
@@ -284,8 +285,8 @@
 	 */
 	flush_dcache_range(image_base, image_size);
 
-	INFO("Image id=%u loaded: 0x%lx - 0x%lx\n", image_id, image_base,
-	     image_base + image_size);
+	INFO("Image id=%u loaded: %p - %p\n", image_id, (void *) image_base,
+	     (void *) (image_base + image_size));
 
 exit:
 	io_close(image_handle);
diff --git a/common/tf_printf.c b/common/tf_printf.c
index c68b990..c1d4188 100644
--- a/common/tf_printf.c
+++ b/common/tf_printf.c
@@ -68,6 +68,7 @@
  * %u - unsigned 32 bit decimal format
  * %ld and %lld - signed 64 bit decimal format
  * %lu and %llu - unsigned 64 bit decimal format
+ * %p - pointer format
  * Exits on all other formats.
  *******************************************************************/
 
@@ -107,6 +108,14 @@
 				str = va_arg(args, char *);
 				string_print(str);
 				break;
+			case 'p':
+				unum = (uint64_t)va_arg(args, void *);
+
+				if (unum)
+					string_print("0x");
+
+				unsigned_num_print(unum, 16);
+				break;
 			case 'x':
 				if (bit64)
 					unum = va_arg(args, uint64_t);
diff --git a/docs/cpu-specific-build-macros.md b/docs/cpu-specific-build-macros.md
index e7185aa..c57dc7e 100644
--- a/docs/cpu-specific-build-macros.md
+++ b/docs/cpu-specific-build-macros.md
@@ -85,8 +85,12 @@
 
 *    `A57_DISABLE_NON_TEMPORAL_HINT`: This flag has the same behaviour as
      `A53_DISABLE_NON_TEMPORAL_HINT` but for Cortex-A57. This needs to be
-     enabled only for revisions <= r1p2 of the CPU and is enabled by default.
+     enabled only for revisions <= r1p2 of the CPU and is enabled by default,
+     as recommended in section "4.7 Non-Temporal Loads/Stores" of the
+     [Cortex-A57 Software Optimization Guide][A57 SW Optimization Guide].
 
 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
 _Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved._
+
+[A57 SW Optimization Guide]: http://infocenter.arm.com/help/topic/com.arm.doc.uan0015b/Cortex_A57_Software_Optimization_Guide_external.pdf
diff --git a/docs/diagrams/default_reset_code.png b/docs/diagrams/default_reset_code.png
index e7e0d85..d8675e4 100644
--- a/docs/diagrams/default_reset_code.png
+++ b/docs/diagrams/default_reset_code.png
Binary files differ
diff --git a/docs/diagrams/reset_code_flow.dia b/docs/diagrams/reset_code_flow.dia
index 5de00da..133c9cf 100644
--- a/docs/diagrams/reset_code_flow.dia
+++ b/docs/diagrams/reset_code_flow.dia
Binary files differ
diff --git a/docs/diagrams/reset_code_no_boot_type_check.png b/docs/diagrams/reset_code_no_boot_type_check.png
index 8ce7e97..23e865f 100644
--- a/docs/diagrams/reset_code_no_boot_type_check.png
+++ b/docs/diagrams/reset_code_no_boot_type_check.png
Binary files differ
diff --git a/docs/diagrams/reset_code_no_checks.png b/docs/diagrams/reset_code_no_checks.png
index 8a02f0f..26a179b 100644
--- a/docs/diagrams/reset_code_no_checks.png
+++ b/docs/diagrams/reset_code_no_checks.png
Binary files differ
diff --git a/docs/diagrams/reset_code_no_cpu_check.png b/docs/diagrams/reset_code_no_cpu_check.png
index 8b05ea4..4150dbe 100644
--- a/docs/diagrams/reset_code_no_cpu_check.png
+++ b/docs/diagrams/reset_code_no_cpu_check.png
Binary files differ
diff --git a/docs/firmware-design.md b/docs/firmware-design.md
index d0cb399..54c5068 100644
--- a/docs/firmware-design.md
+++ b/docs/firmware-design.md
@@ -152,11 +152,15 @@
     CLCD window of the FVP.
 
     BL1 does not expect to receive any exceptions other than the SMC exception.
-    For the latter, BL1 installs a simple stub. The stub expects to receive
-    only a single type of SMC (determined by its function ID in the general
-    purpose register `X0`). This SMC is raised by BL2 to make BL1 pass control
-    to BL31 (loaded by BL2) at EL3. Any other SMC leads to an assertion
-    failure.
+    For the latter, BL1 installs a simple stub. The stub expects to receive a
+    limited set of SMC types (determined by their function IDs in the general
+    purpose register `X0`):
+    -   `BL1_SMC_RUN_IMAGE`: This SMC is raised by BL2 to make BL1 pass control
+        to BL31 (loaded by BL2) at EL3.
+    -   All SMCs listed in section "BL1 SMC Interface" in the [Firmware Update]
+        Design Guide.
+
+    Any other SMC leads to an assertion failure.
 
 *   CPU initialization
 
@@ -164,12 +168,6 @@
     specific reset handler function (see the section: "CPU specific operations
     framework").
 
-*   MMU setup
-
-    BL1 sets up EL3 memory translation by creating page tables to cover the
-    first 4GB of physical address space. This covers all the memories and
-    peripherals needed by BL1.
-
 *   Control register setup
     -   `SCTLR_EL3`. Instruction cache is enabled by setting the `SCTLR_EL3.I`
         bit. Alignment and stack alignment checking is enabled by setting the
@@ -187,12 +185,19 @@
         and Advanced SIMD execution are configured to not trap to EL3 by
         clearing the `CPTR_EL3.TFP` bit.
 
+    -   `DAIF`. The SError interrupt is enabled by clearing the SError interrupt
+        mask bit.
+
 #### Platform initialization
 
-BL1 enables issuing of snoop and DVM (Distributed Virtual Memory) requests to
-the CCI slave interface corresponding to the cluster that includes the
-primary CPU. BL1 also initializes a UART (PL011 console), which enables access
-to the `printf` family of functions in BL1.
+On ARM platforms, BL1 performs the following platform initializations:
+
+*   Enable the Trusted Watchdog.
+*   Initialize the console.
+*   Configure the Interconnect to enable hardware coherency.
+*   Enable the MMU and map the memory it needs to access.
+*   Configure any required platform storage to load the next bootloader image
+    (BL2).
 
 #### Firmware Update detection and execution
 
@@ -210,7 +215,12 @@
 
 In the normal boot flow, BL1 execution continues as follows:
 
-1.  BL1 determines the amount of free trusted SRAM memory available by
+1.  BL1 prints the following string from the primary CPU to indicate successful
+    execution of the BL1 stage:
+
+        "Booting Trusted Firmware"
+
+2.  BL1 determines the amount of free trusted SRAM memory available by
     calculating the extent of its own data section, which also resides in
     trusted SRAM. BL1 loads a BL2 raw binary image from platform storage, at a
     platform-specific base address. If the BL2 image file is not present or if
@@ -225,11 +235,6 @@
     provided as a base address in the platform header. Further description of
     the memory layout can be found later in this document.
 
-2.  BL1 prints the following string from the primary CPU to indicate successful
-    execution of the BL1 stage:
-
-        "Booting Trusted Firmware"
-
 3.  BL1 passes control to the BL2 image at Secure EL1, starting from its load
     address.
 
@@ -247,23 +252,23 @@
 #### Architectural initialization
 
 BL2 performs minimal architectural initialization required for subsequent
-stages of the ARM Trusted Firmware and normal world software. It sets up
-Secure EL1 memory translation by creating page tables to address the first 4GB
-of the physical address space in a similar way to BL1. EL1 and EL0 are given
-access to Floating Point & Advanced SIMD registers by clearing the `CPACR.FPEN`
-bits.
+stages of the ARM Trusted Firmware and normal world software. EL1 and EL0 are
+given access to Floating Point & Advanced SIMD registers by clearing the
+`CPACR.FPEN` bits.
 
 #### Platform initialization
 
+On ARM platforms, BL2 performs the following platform initializations:
+
-BL2 copies the information regarding the trusted SRAM populated by BL1 using a
-platform-specific mechanism. It calculates the limits of DRAM (main memory)
-to determine whether there is enough space to load the BL33 image. A platform
-defined base address is used to specify the load address for the BL31 image.
-It also defines the extents of memory available for use by the BL32 image.
-BL2 also initializes a UART (PL011 console), which enables  access to the
-`printf` family of functions in BL2. Platform security is initialized to allow
-access to controlled components. The storage abstraction layer is initialized
-which is used to load further bootloader images.
+*   Initialize the console.
+*   Configure any required platform storage to allow loading further bootloader
+    images.
+*   Enable the MMU and map the memory it needs to access.
+*   Perform platform security setup to allow access to controlled components.
+*   Reserve some memory for passing information to the next bootloader image
+    (BL31) and populate it.
+*   Define the extents of memory available for loading each subsequent
+    bootloader image.
 
 #### SCP_BL2 (System Control Processor Firmware) image load
 
@@ -334,89 +339,75 @@
 Currently, BL31 performs a similar architectural initialization to BL1 as
 far as system register settings are concerned. Since BL1 code resides in ROM,
 architectural initialization in BL31 allows override of any previous
-initialization done by BL1. BL31 creates page tables to address the first
-4GB of physical address space and initializes the MMU accordingly. It initializes
-a buffer of frequently used pointers, called per-CPU pointer cache, in memory for
-faster access. Currently the per-CPU pointer cache contains only the pointer
-to crash stack. It then replaces the exception vectors populated by BL1 with its
-own. BL31 exception vectors implement more elaborate support for
-handling SMCs since this is the only mechanism to access the runtime services
-implemented by BL31 (PSCI for example). BL31 checks each SMC for validity as
-specified by the [SMC calling convention PDD][SMCCC] before passing control to
-the required SMC handler routine. BL31 programs the `CNTFRQ_EL0` register with
-the clock frequency of the system counter, which is provided by the platform.
+initialization done by BL1.
 
-#### Platform initialization
+BL31 initializes the per-CPU data framework, which provides a cache of
+frequently accessed per-CPU data optimised for fast, concurrent manipulation
+on different CPUs. This buffer includes pointers to per-CPU contexts, crash
+buffer, CPU reset and power down operations, PSCI data, platform data and so on.
 
-BL31 performs detailed platform initialization, which enables normal world
-software to function correctly. It also retrieves entrypoint information for
-the BL33 image loaded by BL2 from the platform defined memory address populated
-by BL2. It enables issuing of snoop and DVM (Distributed Virtual Memory)
-requests to the CCI slave interface corresponding to the cluster that includes
-the primary CPU. BL31 also initializes a UART (PL011 console), which enables
-access to the `printf` family of functions in BL31.  It enables the system
-level implementation of the generic timer through the memory mapped interface.
+It then replaces the exception vectors populated by BL1 with its own. BL31
+exception vectors implement more elaborate support for handling SMCs since this
+is the only mechanism to access the runtime services implemented by BL31 (PSCI
+for example). BL31 checks each SMC for validity as specified by the
+[SMC calling convention PDD][SMCCC] before passing control to the required SMC
+handler routine.
 
-* GICv2 initialization:
+BL31 programs the `CNTFRQ_EL0` register with the clock frequency of the system
+counter, which is provided by the platform.
 
-    -   Enable group0 interrupts in the GIC CPU interface.
-    -   Configure group0 interrupts to be asserted as FIQs.
-    -   Disable the legacy interrupt bypass mechanism.
-    -   Configure the priority mask register to allow interrupts of all
-        priorities to be signaled to the CPU interface.
-    -   Mark SGIs 8-15 and the other secure interrupts on the platform
-        as group0 (secure).
-    -   Target all secure SPIs to CPU0.
-    -   Enable these group0 interrupts in the GIC distributor.
-    -   Configure all other interrupts as group1 (non-secure).
-    -   Enable signaling of group0 interrupts in the GIC distributor.
+#### Platform initialization
 
-*   GICv3 initialization:
+BL31 performs detailed platform initialization, which enables normal world
+software to function correctly.
 
-    If a GICv3 implementation is available in the platform, BL31 initializes
-    the GICv3 in GICv2 emulation mode with settings as described for GICv2
-    above.
+On ARM platforms, this consists of the following:
 
-*   Power management initialization:
+*   Initialize the console.
+*   Configure the Interconnect to enable hardware coherency.
+*   Enable the MMU and map the memory it needs to access.
+*   Initialize the generic interrupt controller.
+*   Initialize the power controller device.
+*   Detect the system topology.
 
-    BL31 implements a state machine to track CPU and cluster state. The state
-    can be one of `OFF`, `ON_PENDING`, `SUSPEND` or `ON`. All secondary CPUs are
-    initially in the `OFF` state. The cluster that the primary CPU belongs to is
-    `ON`; any other cluster is `OFF`. BL31 initializes the data structures that
-    implement the state machine, including the locks that protect them. BL31
-    accesses the state of a CPU or cluster immediately after reset and before
-    the data cache is enabled in the warm boot path. It is not currently
-    possible to use 'exclusive' based spinlocks, therefore BL31 uses locks
-    based on Lamport's Bakery algorithm instead. BL31 allocates these locks in
-    device memory by default.
+#### Runtime services initialization
 
-*   Runtime services initialization:
+BL31 is responsible for initializing the runtime services. One of them is PSCI.
 
-    The runtime service framework and its initialization is described in the
-    "EL3 runtime services framework" section below.
+As part of the PSCI initializations, BL31 detects the system topology. It also
+initializes the data structures that implement the state machine used to track
+the state of power domain nodes. The state can be one of `OFF`, `RUN` or
+`RETENTION`. All secondary CPUs are initially in the `OFF` state. The cluster
+that the primary CPU belongs to is `ON`; any other cluster is `OFF`. It also
+initializes the locks that protect them. BL31 accesses the state of a CPU or
+cluster immediately after reset and before the data cache is enabled in the
+warm boot path. It is not currently possible to use 'exclusive' based spinlocks,
+therefore BL31 uses locks based on Lamport's Bakery algorithm instead.
 
-    Details about the PSCI service are provided in the "Power State Coordination
-    Interface" section below.
+The runtime service framework and its initialization is described in more
+detail in the "EL3 runtime services framework" section below.
 
-*   BL32 (Secure-EL1 Payload) image initialization
+Details about the status of the PSCI implementation are provided in the
+"Power State Coordination Interface" section below.
 
-    If a BL32 image is present then there must be a matching Secure-EL1 Payload
-    Dispatcher (SPD) service (see later for details). During initialization
-    that service  must register a function to carry out initialization of BL32
-    once the runtime services are fully initialized. BL31 invokes such a
-    registered function to initialize BL32 before running BL33.
+#### BL32 (Secure-EL1 Payload) image initialization
 
-    Details on BL32 initialization and the SPD's role are described in the
-    "Secure-EL1 Payloads and Dispatchers" section below.
+If a BL32 image is present then there must be a matching Secure-EL1 Payload
+Dispatcher (SPD) service (see later for details). During initialization
+that service must register a function to carry out initialization of BL32
+once the runtime services are fully initialized. BL31 invokes such a
+registered function to initialize BL32 before running BL33.
 
-*   BL33 (Non-trusted Firmware) execution
+Details on BL32 initialization and the SPD's role are described in the
+"Secure-EL1 Payloads and Dispatchers" section below.
 
-    BL31 initializes the EL2 or EL1 processor context for normal-world cold
-    boot, ensuring that no secure state information finds its way into the
-    non-secure execution state. BL31 uses the entrypoint information provided
-    by BL2 to jump to the Non-trusted firmware image (BL33) at the highest
-    available Exception Level (EL2 if available, otherwise EL1).
+#### BL33 (Non-trusted Firmware) execution
 
+BL31 initializes the EL2 or EL1 processor context for normal-world cold
+boot, ensuring that no secure state information finds its way into the
+non-secure execution state. BL31 uses the entrypoint information provided
+by BL2 to jump to the Non-trusted firmware image (BL33) at the highest
+available Exception Level (EL2 if available, otherwise EL1).
 
 ### Using alternative Trusted Boot Firmware in place of BL1 and BL2
 
@@ -558,9 +549,6 @@
     Coordination Interface ([PSCI]) is the first set of standard service calls
     defined by ARM (see PSCI section later).
 
-    NOTE: Currently this service is called PSCI since there are no other
-    defined standard service calls.
-
 2.  Secure-EL1 Payload Dispatcher service
 
     If a system runs a Trusted OS or other Secure-EL1 Payload (SP) then
@@ -1194,6 +1182,72 @@
 on FVP, BL31 and TSP need to know the limit address that their PROGBITS
 sections must not overstep. The platform code must provide those.
 
+Trusted Firmware provides a mechanism to verify at boot time that the memory
+to load a new image is free to prevent overwriting a previously loaded image.
+For this mechanism to work, the platform must specify the memory available in
+the system as regions, where each region consists of base address, total size
+and the free area within it (as defined in the `meminfo_t` structure). Trusted
+Firmware retrieves these memory regions by calling the corresponding platform
+API:
+
+*   `meminfo_t *bl1_plat_sec_mem_layout(void)`
+*   `meminfo_t *bl2_plat_sec_mem_layout(void)`
+*   `void bl2_plat_get_scp_bl2_meminfo(meminfo_t *scp_bl2_meminfo)`
+*   `void bl2_plat_get_bl32_meminfo(meminfo_t *bl32_meminfo)`
+*   `void bl2_plat_get_bl33_meminfo(meminfo_t *bl33_meminfo)`
+
+For example, in the case of BL1 loading BL2, `bl1_plat_sec_mem_layout()` will
+return the region defined by the platform where BL1 intends to load BL2. The
+`load_image()` function will check that the memory where BL2 will be loaded is
+within the specified region and marked as free.
+
+The actual number of regions and their base addresses and sizes is platform
+specific. The platform may return the same region or define a different one for
+each API. However, the overlap verification mechanism applies only to a single
+region. Hence, it is the platform responsibility to guarantee that different
+regions do not overlap, or that if they do, the overlapping images are not
+accessed at the same time. This could be used, for example, to load temporary
+images (e.g. certificates) or firmware images prior to being transfered to its
+corresponding processor (e.g. the SCP BL2 image).
+
+To reduce fragmentation and simplify the tracking of free memory, all the free
+memory within a region is always located in one single buffer defined by its
+base address and size. Trusted Firmware implements a top/bottom load approach:
+after a new image is loaded, it checks how much memory remains free above and
+below the image. The smallest area is marked as unavailable, while the larger
+area becomes the new free memory buffer. Platforms should take this behaviour
+into account when defining the base address for each of the images. For example,
+if an image is loaded near the middle of the region, small changes in image size
+could cause a flip between a top load and a bottom load, which may result in an
+unexpected memory layout.
+
+The following diagram is an example of an image loaded in the bottom part of
+the memory region. The region is initially free (nothing has been loaded yet):
+
+               Memory region
+               +----------+
+               |          |
+               |          |  <<<<<<<<<<<<<  Free
+               |          |
+               |----------|                 +------------+
+               |  image   |  <<<<<<<<<<<<<  |   image    |
+               |----------|                 +------------+
+               | xxxxxxxx |  <<<<<<<<<<<<<  Marked as unavailable
+               +----------+
+
+And the following diagram is an example of an image loaded in the top part:
+
+               Memory region
+               +----------+
+               | xxxxxxxx |  <<<<<<<<<<<<<  Marked as unavailable
+               |----------|                 +------------+
+               |  image   |  <<<<<<<<<<<<<  |   image    |
+               |----------|                 +------------+
+               |          |
+               |          |  <<<<<<<<<<<<<  Free
+               |          |
+               +----------+
+
 
 ####  Memory layout on ARM development platforms
 
@@ -1229,9 +1283,47 @@
     *   Secure region of DRAM (top 16MB of DRAM configured by the TrustZone
         controller)
 
+    When BL32 is loaded into Trusted SRAM, its NOBITS sections are allowed to
+    overlay BL2. This memory layout is designed to give the BL32 image as much
+    memory as possible when it is loaded into Trusted SRAM.
+
-When BL32 is loaded into Trusted SRAM, its NOBITS sections are allowed to
-overlay BL2. This memory layout is designed to give the BL32 image as much
-memory as possible when it is loaded into Trusted SRAM.
+The memory regions for the overlap detection mechanism at boot time are
+defined as follows (shown per API):
+
+*   `meminfo_t *bl1_plat_sec_mem_layout(void)`
+
+    This region corresponds to the whole Trusted SRAM except for the shared
+    memory at the base. This region is initially free. At boot time, BL1 will
+    mark the BL1(rw) section within this region as occupied. The BL1(rw) section
+    is placed at the top of Trusted SRAM.
+
+*   `meminfo_t *bl2_plat_sec_mem_layout(void)`
+
+    This region corresponds to the whole Trusted SRAM as defined by
+    `bl1_plat_sec_mem_layout()`, but with the BL1(rw) section marked as
+    occupied. This memory region is used to check that BL2 and BL31 do not
+    overlap with each other. BL2_BASE and BL1_RW_BASE are carefully chosen so
+    that the memory for BL31 is top loaded above BL2.
+
+*   `void bl2_plat_get_scp_bl2_meminfo(meminfo_t *scp_bl2_meminfo)`
+
+    This region is an exact copy of the region defined by
+    `bl2_plat_sec_mem_layout()`. Being a disconnected copy means that all the
+    changes made to this region by the Trusted Firmware will not be propagated.
+    This approach is valid because the SCP BL2 image is loaded temporarily
+    while it is being transferred to the SCP, so this memory is reused
+    afterwards.
+
+*   `void bl2_plat_get_bl32_meminfo(meminfo_t *bl32_meminfo)`
+
+    This region depends on the location of the BL32 image. Currently, ARM
+    platforms support three different locations (detailed below): Trusted SRAM,
+    Trusted DRAM and the TZC-Secured DRAM.
+
+*   `void bl2_plat_get_bl33_meminfo(meminfo_t *bl33_meminfo)`
+
+    This region corresponds to the Non-Secure DDR-DRAM, excluding the
+    TZC-Secured area.
 
 The location of the BL32 image will result in different memory maps. This is
 illustrated for both FVP and Juno in the following diagrams, using the TSP as
@@ -1729,7 +1821,7 @@
 
 - - - - - - - - - - - - - - - - - - - - - - - - - -
 
-_Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved._
+_Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved._
 
 [ARM ARM]:          http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0487a.e/index.html "ARMv8-A Reference Manual (ARM DDI0487A.E)"
 [PSCI]:             http://infocenter.arm.com/help/topic/com.arm.doc.den0022c/DEN0022C_Power_State_Coordination_Interface.pdf "Power State Coordination Interface PDD (ARM DEN 0022C)"
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index 7c9f404..f0adf57 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -471,6 +471,18 @@
 
     Defines the maximum address that the TSP's progbits sections can occupy.
 
+If the platform port uses the PL061 GPIO driver, the following constant may
+optionally be defined:
+
+*   **PLAT_PL061_MAX_GPIOS**
+    Maximum number of GPIOs required by the platform. This allows control how
+    much memory is allocated for PL061 GPIO controllers. The default value is
+    32.
+    [For example, define the build flag in platform.mk]:
+    PLAT_PL061_MAX_GPIOS    :=      160
+    $(eval $(call add_define,PLAT_PL061_MAX_GPIOS))
+
+
 ### File : plat_macros.S [mandatory]
 
 Each platform must ensure a file of this name is in the system include path with
@@ -637,7 +649,8 @@
 per-CPU stacks). This function will be invoked very early in the
 initialization sequence which mandates that this function should be
 implemented in assembly and should not rely on the avalability of a C
-runtime environment.
+runtime environment. This function can clobber x0 - x8 and must preserve
+x9 - x29.
 
 This function plays a crucial role in the power domain topology framework in
 PSCI and details of this can be found in [Power Domain Topology Design].
@@ -765,6 +778,20 @@
 
 The default implementation simply spins.
 
+### Function : plat_panic_handler()
+
+    Argument : void
+    Return   : void
+
+This API is called when the generic code encounters an unexpected error
+situation from which it cannot recover. This function must not return,
+and must be implemented in assembly because it may be called before the C
+environment is initialized.
+
+Note: The address from where it was called is stored in x30 (Link Register).
+
+The default implementation simply spins.
+
 
 3.  Modifications specific to a Boot Loader stage
 -------------------------------------------------
@@ -822,8 +849,16 @@
 This function executes with the MMU and data caches disabled. It is only called
 by the primary CPU.
 
-In ARM standard platforms, this function initializes the console and enables
-snoop requests into the primary CPU's cluster.
+On ARM standard platforms, this function:
+
+*   Enables a secure instance of SP805 to act as the Trusted Watchdog.
+
+*   Initializes a UART (PL011 console), which enables access to the `printf`
+    family of functions in BL1.
+
+*   Enables issuing of snoop and DVM (Distributed Virtual Memory) requests to
+    the CCI slave interface corresponding to the cluster that includes the
+    primary CPU.
 
 ### Function : bl1_plat_arch_setup() [mandatory]
 
@@ -1029,16 +1064,20 @@
 by the primary CPU. The arguments to this function is the address of the
 `meminfo` structure populated by BL1.
 
-The platform must copy the contents of the `meminfo` structure into a private
+The platform may copy the contents of the `meminfo` structure into a private
 variable as the original memory may be subsequently overwritten by BL2. The
 copied structure is made available to all BL2 code through the
 `bl2_plat_sec_mem_layout()` function.
 
+On ARM standard platforms, this function also:
+
-In ARM standard platforms, this function also initializes the storage
-abstraction layer used to load further bootloader images. It is necessary to do
-this early on platforms with a SCP_BL2 image, since the later
-`bl2_platform_setup` must be done after SCP_BL2 is loaded.
+*   Initializes a UART (PL011 console), which enables access to the `printf`
+    family of functions in BL2.
 
+*   Initializes the storage abstraction layer used to load further bootloader
+    images. It is necessary to do this early on platforms with a SCP_BL2 image,
+    since the later `bl2_platform_setup` must be done after SCP_BL2 is loaded.
+
 
 ### Function : bl2_plat_arch_setup() [mandatory]
 
@@ -1049,9 +1088,9 @@
 by the primary CPU.
 
 The purpose of this function is to perform any architectural initialization
-that varies across platforms, for example enabling the MMU (since the memory
-map differs across platforms).
+that varies across platforms.
 
+On ARM standard platforms, this function enables the MMU.
 
 ### Function : bl2_platform_setup() [mandatory]
 
@@ -1262,7 +1301,7 @@
 called by the primary CPU. The arguments to this function is the address
 of the `meminfo` structure and platform specific info provided by BL1.
 
-The platform must copy the contents of the `mem_info` and `plat_info` into
+The platform may copy the contents of the `mem_info` and `plat_info` into
 private storage as the original memory may be subsequently overwritten by BL2U.
 
 On ARM CSS platforms `plat_info` is interpreted as an `image_info_t` structure,
@@ -1364,8 +1403,15 @@
 
 In ARM standard platforms, BL2 passes a pointer to a `bl31_params` structure
 in BL2 memory. BL31 copies the information in this pointer to internal data
-structures.
+structures. It also performs the following:
+
+*   Initialize a UART (PL011 console), which enables access to the `printf`
+    family of functions in BL31.
 
+*   Enable issuing of snoop and DVM (Distributed Virtual Memory) requests to the
+    CCI slave interface corresponding to the cluster that includes the primary
+    CPU.
+
 
 ### Function : bl31_plat_arch_setup() [mandatory]
 
@@ -1376,9 +1422,10 @@
 by the primary CPU.
 
 The purpose of this function is to perform any architectural initialization
-that varies across platforms, for example enabling the MMU (since the memory
-map differs across platforms).
+that varies across platforms.
 
+On ARM standard platforms, this function enables the MMU.
+
 
 ### Function : bl31_platform_setup() [mandatory]
 
@@ -1392,13 +1439,33 @@
 The purpose of this function is to complete platform initialization so that both
 BL31 runtime services and normal world software can function correctly.
 
-In ARM standard platforms, this function does the following:
-*   Initializes the generic interrupt controller.
-*   Enables system-level implementation of the generic timer counter.
-*   Grants access to the system counter timer module
-*   Initializes the power controller device
-*   Detects the system topology.
+On ARM standard platforms, this function does the following:
 
+*   Initialize the generic interrupt controller.
+
+    Depending on the GIC driver selected by the platform, the appropriate GICv2
+    or GICv3 initialization will be done, which mainly consists of:
+
+    - Enable secure interrupts in the GIC CPU interface.
+    - Disable the legacy interrupt bypass mechanism.
+    - Configure the priority mask register to allow interrupts of all priorities
+      to be signaled to the CPU interface.
+    - Mark SGIs 8-15 and the other secure interrupts on the platform as secure.
+    - Target all secure SPIs to CPU0.
+    - Enable these secure interrupts in the GIC distributor.
+    - Configure all other interrupts as non-secure.
+    - Enable signaling of secure interrupts in the GIC distributor.
+
+*   Enable system-level implementation of the generic timer counter through the
+    memory mapped interface.
+
+*   Grant access to the system counter timer module
+
+*   Initialize the power controller device.
+
+    In particular, initialise the locks that prevent concurrent accesses to the
+    power controller device.
+
 
 ### Function : bl31_plat_runtime_setup() [optional]
 
@@ -1999,7 +2066,7 @@
 
 - - - - - - - - - - - - - - - - - - - - - - - - - -
 
-_Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved._
+_Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved._
 
 
 [ARM GIC Architecture Specification 2.0]: http://infocenter.arm.com/help/topic/com.arm.doc.ihi0048b/index.html
diff --git a/docs/user-guide.md b/docs/user-guide.md
index d6c6c93..ea10a81 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -420,6 +420,25 @@
     any register that is not part of the SBSA generic UART specification.
     Default value is 0 (a full PL011 compliant UART is present).
 
+*   `CTX_INCLUDE_FPREGS`: Boolean option that, when set to 1, will cause the FP
+    registers to be included when saving and restoring the CPU context. Default
+    is 0.
+
+*   `DISABLE_PEDANTIC`: When set to 1 it will disable the -pedantic option in
+    the GCC command line. Default is 0.
+
+*   `BUILD_STRING`: Input string for VERSION_STRING, which allows the TF build
+    to be uniquely identified. Defaults to the current git commit id.
+
+*   `VERSION_STRING`: String used in the log output for each TF image. Defaults
+    to a string formed by concatenating the version number, build type and build
+    string.
+
+*   `BUILD_MESSAGE_TIMESTAMP`: String used to identify the time and date of the
+    compilation of each build. It must be set to a C string (including quotes
+    where applicable). Defaults to a string that contains the time and date of
+    the compilation.
+
 #### ARM development platform specific build options
 
 *   `ARM_TSP_RAM_LOCATION`: location of the TSP binary. Options:
@@ -469,6 +488,13 @@
     match the frame used by the Non-Secure image (normally the Linux kernel).
     Default is true (access to the frame is allowed).
 
+*   `ARM_BOARD_OPTIMISE_MMAP`: Boolean option to enable or disable optimisation
+    of page table and MMU related macros `PLAT_ARM_MMAP_ENTRIES` and
+    `MAX_XLAT_TABLES`. By default this flag is 0, which means it uses the
+    default unoptimised values for these macros. ARM development platforms
+    that wish to optimise memory usage for page tables need to set this flag to 1
+    and must override the related macros.
+
 #### ARM CSS platform specific build options
 
 *   `CSS_DETECT_PRE_1_7_0_SCP`: Boolean flag to detect SCP version
@@ -478,6 +504,10 @@
     set to 1 then Trusted Firmware will detect if an earlier version is in use.
     Default is 1.
 
+*   `CSS_LOAD_SCP_IMAGES`: Boolean flag, which when set, adds SCP_BL2 and
+    SCP_BL2U to the FIP and FWU_FIP respectively, and enables them to be loaded
+    during boot. Default is 1.
+
 #### ARM FVP platform specific build options
 
 *   `FVP_USE_GIC_DRIVER`   : Selects the GIC driver to be built. Options:
@@ -489,6 +519,11 @@
     Trusted Firmware must be compiled with GICv2 only driver using
     `FVP_USE_GIC_DRIVER=FVP_GICV2` build option.
 
+*   `FVP_CLUSTER_COUNT`    : Configures the cluster count to be used to
+     build the topology tree within Trusted Firmware. By default the
+     Trusted Firmware is configured for dual cluster topology and this option
+     can be used to override the default value.
+
 ### Creating a Firmware Image Package
 
 FIPs are automatically created as part of the build instructions described in
@@ -1052,14 +1087,15 @@
 This version of the ARM Trusted Firmware has been tested on the following ARM
 FVPs (64-bit versions only).
 
-*   `Foundation_Platform` (Version 9.4, Build 9.4.59)
-*   `FVP_Base_AEMv8A-AEMv8A` (Version 7.0, Build 0.8.7004)
-*   `FVP_Base_Cortex-A57x4-A53x4` (Version 7.0, Build 0.8.7004)
-*   `FVP_Base_Cortex-A57x1-A53x1` (Version 7.0, Build 0.8.7004)
-*   `FVP_Base_Cortex-A57x2-A53x4` (Version 7.0, Build 0.8.7004)
+*   `Foundation_Platform` (Version 9.5, Build 9.5.40)
+*   `FVP_Base_AEMv8A-AEMv8A` (Version 7.2, Build 0.8.7202)
+*   `FVP_Base_Cortex-A57x4-A53x4` (Version 7.2, Build 0.8.7202)
+*   `FVP_Base_Cortex-A57x1-A53x1` (Version 7.2, Build 0.8.7202)
+*   `FVP_Base_Cortex-A57x2-A53x4` (Version 7.2, Build 0.8.7202)
 
 NOTE: The build numbers quoted above are those reported by launching the FVP
-with the `--version` parameter.
+with the `--version` parameter. `Foundation_Platform` tarball for `--version`
+9.5.40 is labeled as version 9.5.41.
 
 NOTE: The software will not work on Version 1.0 of the Foundation FVP.
 The commands below would report an `unhandled argument` error in this case.
diff --git a/drivers/arm/gic/common/gic_common.c b/drivers/arm/gic/common/gic_common.c
index 17be61d..cfa0d23 100644
--- a/drivers/arm/gic/common/gic_common.c
+++ b/drivers/arm/gic/common/gic_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -31,6 +31,7 @@
 #include <assert.h>
 #include <gic_common.h>
 #include <mmio.h>
+#include "gic_common_private.h"
 
 /*******************************************************************************
  * GIC Distributor interface accessors for reading entire registers
@@ -239,7 +240,10 @@
 }
 
 /*******************************************************************************
- * GIC Distributor interface accessors for individual interrupt manipulation
+ * GIC Distributor functions for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
  ******************************************************************************/
 unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id)
 {
@@ -306,3 +310,8 @@
 
 	gicd_write_icactiver(base, id, (1 << bit_num));
 }
+
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	mmio_write_8(base + GICD_IPRIORITYR + id, pri & GIC_PRI_MASK);
+}
diff --git a/drivers/arm/gic/common/gic_common_private.h b/drivers/arm/gic/common/gic_common_private.h
new file mode 100644
index 0000000..2919b7f
--- /dev/null
+++ b/drivers/arm/gic/common/gic_common_private.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2016, 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 GIC_COMMON_PRIVATE_H_
+#define GIC_COMMON_PRIVATE_H_
+
+#include <gic_common.h>
+#include <mmio.h>
+#include <stdint.h>
+
+/*******************************************************************************
+ * GIC Distributor interface register accessors that are common to GICv3 & GICv2
+ ******************************************************************************/
+static inline unsigned int gicd_read_ctlr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_CTLR);
+}
+
+static inline unsigned int gicd_read_typer(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_TYPER);
+}
+
+static inline unsigned int gicd_read_iidr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_IIDR);
+}
+
+static inline void gicd_write_ctlr(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICD_CTLR, val);
+}
+
+/*******************************************************************************
+ * GIC Distributor function prototypes for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
+ ******************************************************************************/
+unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id);
+unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id);
+unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id);
+void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val);
+
+/*******************************************************************************
+ * GIC Distributor function prototypes for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id);
+void gicd_set_igroupr(uintptr_t base, unsigned int id);
+void gicd_clr_igroupr(uintptr_t base, unsigned int id);
+void gicd_set_isenabler(uintptr_t base, unsigned int id);
+void gicd_set_icenabler(uintptr_t base, unsigned int id);
+void gicd_set_ispendr(uintptr_t base, unsigned int id);
+void gicd_set_icpendr(uintptr_t base, unsigned int id);
+void gicd_set_isactiver(uintptr_t base, unsigned int id);
+void gicd_set_icactiver(uintptr_t base, unsigned int id);
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri);
+
+#endif /* GIC_COMMON_PRIVATE_H_ */
diff --git a/drivers/arm/gic/gic_v2.c b/drivers/arm/gic/gic_v2.c
index dc5dc08..05399c3 100644
--- a/drivers/arm/gic/gic_v2.c
+++ b/drivers/arm/gic/gic_v2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -261,10 +261,6 @@
  */
 void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
 {
-	unsigned int reg = base + GICD_IPRIORITYR + (id & ~3);
-	unsigned int shift = (id & 3) << 3;
-	unsigned int reg_val = mmio_read_32(reg);
-
 	/*
 	 * Enforce ARM recommendation to manage priority values such
 	 * that group1 interrupts always have a lower priority than
@@ -278,17 +274,12 @@
 		pri >= GIC_HIGHEST_SEC_PRIORITY &&
 			pri <= GIC_LOWEST_SEC_PRIORITY);
 
-	reg_val &= ~(GIC_PRI_MASK << shift);
-	reg_val |= (pri & GIC_PRI_MASK) << shift;
-	mmio_write_32(reg, reg_val);
+	mmio_write_8(base + GICD_IPRIORITYR + id, pri & GIC_PRI_MASK);
 }
 
 void gicd_set_itargetsr(uintptr_t base, unsigned int id, unsigned int target)
 {
-	unsigned byte_off = id & ((1 << ITARGETSR_SHIFT) - 1);
-	unsigned int reg_val = gicd_read_itargetsr(base, id);
-
-	gicd_write_itargetsr(base, id, reg_val | (target << (byte_off << 3)));
+	mmio_write_8(base + GICD_ITARGETSR + id, target & GIC_TARGET_CPU_MASK);
 }
 
 /*******************************************************************************
diff --git a/drivers/arm/gic/gic_v3.c b/drivers/arm/gic/gic_v3.c
index f429662..11185b2 100644
--- a/drivers/arm/gic/gic_v3.c
+++ b/drivers/arm/gic/gic_v3.c
@@ -60,8 +60,8 @@
 			/* Disable this print for now as it appears every time
 			 * when using PSCI CPU_SUSPEND.
 			 * TODO: Print this only the first time for each CPU.
-			 * INFO("GICv3 - Found RDIST for MPIDR(0x%lx) at 0x%lx\n",
-			 *	mpidr, addr);
+			 * INFO("GICv3 - Found RDIST for MPIDR(0x%lx) at %p\n",
+			 *	mpidr, (void *) addr);
 			 */
 			return addr;
 		}
diff --git a/drivers/arm/gic/v2/gicv2_helpers.c b/drivers/arm/gic/v2/gicv2_helpers.c
index 1f904c5..b60a5cd 100644
--- a/drivers/arm/gic/v2/gicv2_helpers.c
+++ b/drivers/arm/gic/v2/gicv2_helpers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <debug.h>
 #include <gic_common.h>
+#include "../common/gic_common_private.h"
 #include "gicv2_private.h"
 
 /*
@@ -101,10 +102,7 @@
  */
 void gicd_set_itargetsr(uintptr_t base, unsigned int id, unsigned int target)
 {
-	unsigned byte_off = id & ((1 << ITARGETSR_SHIFT) - 1);
-	unsigned int reg_val = gicd_read_itargetsr(base, id);
-
-	gicd_write_itargetsr(base, id, reg_val | (target << (byte_off << 3)));
+	mmio_write_8(base + GICD_ITARGETSR + id, target & GIC_TARGET_CPU_MASK);
 }
 
 /*******************************************************************************
@@ -166,7 +164,7 @@
 			gicd_clr_igroupr(gicd_base, irq_num);
 
 			/* Set the priority of this interrupt */
-			gicd_write_ipriorityr(gicd_base,
+			gicd_set_ipriorityr(gicd_base,
 					      irq_num,
 					      GIC_HIGHEST_SEC_PRIORITY);
 
@@ -213,7 +211,7 @@
 			sec_ppi_sgi_mask |= 1U << irq_num;
 
 			/* Set the priority of this interrupt */
-			gicd_write_ipriorityr(gicd_base,
+			gicd_set_ipriorityr(gicd_base,
 					    irq_num,
 					    GIC_HIGHEST_SEC_PRIORITY);
 		}
diff --git a/drivers/arm/gic/v2/gicv2_main.c b/drivers/arm/gic/v2/gicv2_main.c
index cf93926..305a8b0 100644
--- a/drivers/arm/gic/v2/gicv2_main.c
+++ b/drivers/arm/gic/v2/gicv2_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -34,6 +34,7 @@
 #include <debug.h>
 #include <gic_common.h>
 #include <gicv2.h>
+#include "../common/gic_common_private.h"
 #include "gicv2_private.h"
 
 static const gicv2_driver_data_t *driver_data;
diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index 2fb98cb..07ae54c 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <debug.h>
 #include <gic_common.h>
+#include "../common/gic_common_private.h"
 #include "gicv3_private.h"
 
 /*
@@ -194,6 +195,15 @@
 	gicr_write_isenabler0(base, (1 << bit_num));
 }
 
+/*
+ * Accessor to set the byte corresponding to interrupt ID
+ * in GIC Re-distributor IPRIORITYR.
+ */
+void gicr_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	mmio_write_8(base + GICR_IPRIORITYR + id, pri & GIC_PRI_MASK);
+}
+
 /******************************************************************************
  * This function marks the core as awake in the re-distributor and
  * ensures that the interface is active.
@@ -330,7 +340,7 @@
 				gicd_clr_igrpmodr(gicd_base, irq_num);
 
 			/* Set the priority of this interrupt */
-			gicd_write_ipriorityr(gicd_base,
+			gicd_set_ipriorityr(gicd_base,
 					      irq_num,
 					      GIC_HIGHEST_SEC_PRIORITY);
 
@@ -404,7 +414,7 @@
 				gicr_clr_igrpmodr0(gicr_base, irq_num);
 
 			/* Set the priority of this interrupt */
-			gicr_write_ipriorityr(gicr_base,
+			gicr_set_ipriorityr(gicr_base,
 					    irq_num,
 					    GIC_HIGHEST_SEC_PRIORITY);
 
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index d5cd0ed..6c6c7af 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -34,6 +34,7 @@
 #include <debug.h>
 #include <gic_common.h>
 #include <gicv3.h>
+#include "../common/gic_common_private.h"
 #include "gicv3_private.h"
 
 static const gicv3_driver_data_t *driver_data;
diff --git a/drivers/arm/gic/v3/gicv3_private.h b/drivers/arm/gic/v3/gicv3_private.h
index c8b311a..5e2409f 100644
--- a/drivers/arm/gic/v3/gicv3_private.h
+++ b/drivers/arm/gic/v3/gicv3_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -84,16 +84,24 @@
 	 ((typer_val >> 32) & 0xffffff))
 
 /*******************************************************************************
- * Private function prototypes
+ * Private GICv3 function prototypes for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
  ******************************************************************************/
 unsigned int gicd_read_igrpmodr(uintptr_t base, unsigned int id);
 unsigned int gicr_read_ipriorityr(uintptr_t base, unsigned int id);
+void gicd_write_igrpmodr(uintptr_t base, unsigned int id, unsigned int val);
+void gicr_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
+
+/*******************************************************************************
+ * Private GICv3 function prototypes for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
 unsigned int gicd_get_igrpmodr(uintptr_t base, unsigned int id);
 unsigned int gicr_get_igrpmodr0(uintptr_t base, unsigned int id);
 unsigned int gicr_get_igroupr0(uintptr_t base, unsigned int id);
-unsigned int gicv3_get_pending_grp1_interrupt_id(unsigned int pending_grp);
-void gicd_write_igrpmodr(uintptr_t base, unsigned int id, unsigned int val);
-void gicr_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
 void gicd_set_igrpmodr(uintptr_t base, unsigned int id);
 void gicr_set_igrpmodr0(uintptr_t base, unsigned int id);
 void gicr_set_isenabler0(uintptr_t base, unsigned int id);
@@ -101,6 +109,11 @@
 void gicd_clr_igrpmodr(uintptr_t base, unsigned int id);
 void gicr_clr_igrpmodr0(uintptr_t base, unsigned int id);
 void gicr_clr_igroupr0(uintptr_t base, unsigned int id);
+void gicr_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri);
+
+/*******************************************************************************
+ * Private GICv3 helper function prototypes
+ ******************************************************************************/
 void gicv3_spis_configure_defaults(uintptr_t gicd_base);
 void gicv3_ppi_sgi_configure_defaults(uintptr_t gicr_base);
 void gicv3_secure_spis_configure(uintptr_t gicd_base,
@@ -179,6 +192,11 @@
 	mmio_write_32(base + GICR_WAKER, val);
 }
 
+/*******************************************************************************
+ * GIC Re-distributor functions for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
+ ******************************************************************************/
 static inline unsigned int gicr_read_icenabler0(uintptr_t base)
 {
 	return mmio_read_32(base + GICR_ICENABLER0);
diff --git a/drivers/arm/pl011/pl011_console.S b/drivers/arm/pl011/pl011_console.S
index f29f895..5e97e91 100644
--- a/drivers/arm/pl011/pl011_console.S
+++ b/drivers/arm/pl011/pl011_console.S
@@ -69,15 +69,6 @@
 	mov	w4, #PL011_UARTCR_UARTEN
 	bic	w3, w3, w4
 	str	w3, [x0, #UARTCR]
-	/* Flush the transmit FIFO */
-	ldr	w3, [x0, #UARTLCR_H]
-	mov	w4, #PL011_UARTLCR_H_FEN
-	bic	w3, w3, w4
-	str	w3, [x0, #UARTLCR_H]
-	/* Wait for the end of Tx of current character */
-busy_loop:
-	ldr	w3, [x0, #UARTFR]
-	tbnz	w3, #PL011_UARTFR_BUSY_BIT, busy_loop
 	/* Program the baudrate */
 	/* Divisor =  (Uart clock * 4) / baudrate */
 	lsl	w1, w1, #2
diff --git a/drivers/arm/pl061/pl061_gpio.c b/drivers/arm/pl061/pl061_gpio.c
new file mode 100644
index 0000000..fca0056
--- /dev/null
+++ b/drivers/arm/pl061/pl061_gpio.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2016, 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.
+ *
+ * ARM PL061 GPIO Driver.
+ * Reference to ARM DDI 0190B document.
+ *
+ */
+
+#include <assert.h>
+#include <cassert.h>
+#include <debug.h>
+#include <errno.h>
+#include <gpio.h>
+#include <mmio.h>
+#include <pl061_gpio.h>
+
+#if !PLAT_PL061_MAX_GPIOS
+# define PLAT_PL061_MAX_GPIOS	32
+#endif	/* PLAT_PL061_MAX_GPIOS */
+
+CASSERT(PLAT_PL061_MAX_GPIOS > 0, assert_plat_pl061_max_gpios);
+
+#define MAX_GPIO_DEVICES	((PLAT_PL061_MAX_GPIOS +		\
+				 (GPIOS_PER_PL061 - 1)) / GPIOS_PER_PL061)
+
+#define PL061_GPIO_DIR		0x400
+
+#define GPIOS_PER_PL061		8
+#define BIT(nr)			(1UL << (nr))
+
+static int pl061_get_direction(int gpio);
+static void pl061_set_direction(int gpio, int direction);
+static int pl061_get_value(int gpio);
+static void pl061_set_value(int gpio, int value);
+
+static uintptr_t pl061_reg_base[MAX_GPIO_DEVICES];
+
+static const gpio_ops_t pl061_gpio_ops = {
+	.get_direction	= pl061_get_direction,
+	.set_direction	= pl061_set_direction,
+	.get_value	= pl061_get_value,
+	.set_value	= pl061_set_value,
+};
+
+static int pl061_get_direction(int gpio)
+{
+	uintptr_t base_addr;
+	unsigned int data, offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	data = mmio_read_8(base_addr + PL061_GPIO_DIR);
+	if (data & BIT(offset))
+		return GPIO_DIR_OUT;
+	return GPIO_DIR_IN;
+}
+
+static void pl061_set_direction(int gpio, int direction)
+{
+	uintptr_t base_addr;
+	unsigned int data, offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	if (direction == GPIO_DIR_OUT) {
+		data = mmio_read_8(base_addr + PL061_GPIO_DIR) | BIT(offset);
+		mmio_write_8(base_addr + PL061_GPIO_DIR, data);
+	} else {
+		data = mmio_read_8(base_addr + PL061_GPIO_DIR) & ~BIT(offset);
+		mmio_write_8(base_addr + PL061_GPIO_DIR, data);
+	}
+}
+
+/*
+ * The offset of GPIODATA register is 0.
+ * The values read from GPIODATA are determined for each bit, by the mask bit
+ * derived from the address used to access the data register, PADDR[9:2].
+ * Bits that are 1 in the address mask cause the corresponding bits in GPIODATA
+ * to be read, and bits that are 0 in the address mask cause the corresponding
+ * bits in GPIODATA to be read as 0, regardless of their value.
+ */
+static int pl061_get_value(int gpio)
+{
+	uintptr_t base_addr;
+	unsigned int offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	if (mmio_read_8(base_addr + BIT(offset + 2)))
+		return GPIO_LEVEL_HIGH;
+	return GPIO_LEVEL_LOW;
+}
+
+/*
+ * In order to write GPIODATA, the corresponding bits in the mask, resulting
+ * from the address bus, PADDR[9:2], must be HIGH. Otherwise the bit values
+ * remain unchanged by the write.
+ */
+static void pl061_set_value(int gpio, int value)
+{
+	uintptr_t base_addr;
+	int offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	if (value == GPIO_LEVEL_HIGH)
+		mmio_write_8(base_addr + BIT(offset + 2), BIT(offset));
+	else
+		mmio_write_8(base_addr + BIT(offset + 2), 0);
+}
+
+
+/*
+ * Register the PL061 GPIO controller with a base address and the offset
+ * of start pin in this GPIO controller.
+ * This function is called after pl061_gpio_ops_init().
+ */
+void pl061_gpio_register(uintptr_t base_addr, int gpio_dev)
+{
+	assert((gpio_dev >= 0) && (gpio_dev < MAX_GPIO_DEVICES));
+
+	pl061_reg_base[gpio_dev] = base_addr;
+}
+
+/*
+ * Initialize PL061 GPIO controller with the total GPIO numbers in SoC.
+ */
+void pl061_gpio_init(void)
+{
+	gpio_init(&pl061_gpio_ops);
+}
diff --git a/drivers/gpio/gpio.c b/drivers/gpio/gpio.c
new file mode 100644
index 0000000..c06172f
--- /dev/null
+++ b/drivers/gpio/gpio.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2016, 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.
+ *
+ * GPIO -- General Purpose Input/Output
+ *
+ * Defines a simple and generic interface to access GPIO device.
+ *
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <gpio.h>
+
+/*
+ * The gpio implementation
+ */
+static const gpio_ops_t *ops;
+
+int gpio_get_direction(int gpio)
+{
+	assert(ops);
+	assert(ops->get_direction != 0);
+	assert(gpio >= 0);
+
+	return ops->get_direction(gpio);
+}
+
+void gpio_set_direction(int gpio, int direction)
+{
+	assert(ops);
+	assert(ops->set_direction != 0);
+	assert((direction == GPIO_DIR_OUT) || (direction == GPIO_DIR_IN));
+	assert(gpio >= 0);
+
+	ops->set_direction(gpio, direction);
+}
+
+int gpio_get_value(int gpio)
+{
+	assert(ops);
+	assert(ops->get_value != 0);
+	assert(gpio >= 0);
+
+	return ops->get_value(gpio);
+}
+
+void gpio_set_value(int gpio, int value)
+{
+	assert(ops);
+	assert(ops->set_value != 0);
+	assert((value == GPIO_LEVEL_LOW) || (value == GPIO_LEVEL_HIGH));
+	assert(gpio >= 0);
+
+	ops->set_value(gpio, value);
+}
+
+/*
+ * Initialize the gpio. The fields in the provided gpio
+ * ops pointer must be valid.
+ */
+void gpio_init(const gpio_ops_t *ops_ptr)
+{
+	assert(ops_ptr != 0  &&
+	       (ops_ptr->get_direction != 0) &&
+	       (ops_ptr->set_direction != 0) &&
+	       (ops_ptr->get_value != 0) &&
+	       (ops_ptr->set_value != 0));
+
+	ops = ops_ptr;
+}
diff --git a/drivers/io/io_memmap.c b/drivers/io/io_memmap.c
index ff4efa8..fe39652 100644
--- a/drivers/io/io_memmap.c
+++ b/drivers/io/io_memmap.c
@@ -45,6 +45,7 @@
 	int		in_use;
 	uintptr_t	base;
 	size_t		file_pos;
+	size_t		size;
 } file_state_t;
 
 static file_state_t current_file = {0};
@@ -61,6 +62,7 @@
 			     io_entity_t *entity);
 static int memmap_block_seek(io_entity_t *entity, int mode,
 			     ssize_t offset);
+static int memmap_block_len(io_entity_t *entity, size_t *length);
 static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
 			     size_t length, size_t *length_read);
 static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
@@ -78,7 +80,7 @@
 	.type = device_type_memmap,
 	.open = memmap_block_open,
 	.seek = memmap_block_seek,
-	.size = NULL,
+	.size = memmap_block_len,
 	.read = memmap_block_read,
 	.write = memmap_block_write,
 	.close = memmap_block_close,
@@ -135,6 +137,7 @@
 		current_file.base = block_spec->offset;
 		/* File cursor offset for seek and incremental reads etc. */
 		current_file.file_pos = 0;
+		current_file.size = block_spec->length;
 		entity->info = (uintptr_t)&current_file;
 		result = 0;
 	} else {
@@ -163,6 +166,18 @@
 }
 
 
+/* Return the size of a file on the memmap device */
+static int memmap_block_len(io_entity_t *entity, size_t *length)
+{
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	*length = ((file_state_t *)entity->info)->size;
+
+	return 0;
+}
+
+
 /* Read data from a file on the memmap device */
 static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
 			     size_t length, size_t *length_read)
diff --git a/fdts/fvp-base-gicv2-psci.dtb b/fdts/fvp-base-gicv2-psci.dtb
index 89318c1..245a6c3 100644
--- a/fdts/fvp-base-gicv2-psci.dtb
+++ b/fdts/fvp-base-gicv2-psci.dtb
Binary files differ
diff --git a/fdts/fvp-base-gicv2-psci.dts b/fdts/fvp-base-gicv2-psci.dts
index 1d26541..fc343ba 100644
--- a/fdts/fvp-base-gicv2-psci.dts
+++ b/fdts/fvp-base-gicv2-psci.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -125,6 +125,7 @@
 			reg = <0x0 0x0>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU1:cpu@1 {
@@ -133,6 +134,7 @@
 			reg = <0x0 0x1>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU2:cpu@2 {
@@ -141,6 +143,7 @@
 			reg = <0x0 0x2>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU3:cpu@3 {
@@ -149,6 +152,7 @@
 			reg = <0x0 0x3>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU4:cpu@100 {
@@ -157,6 +161,7 @@
 			reg = <0x0 0x100>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU5:cpu@101 {
@@ -165,6 +170,7 @@
 			reg = <0x0 0x101>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU6:cpu@102 {
@@ -173,6 +179,7 @@
 			reg = <0x0 0x102>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU7:cpu@103 {
@@ -181,6 +188,11 @@
 			reg = <0x0 0x103>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
+		};
+
+		L2_0: l2-cache0 {
+			compatible = "cache";
 		};
 	};
 
diff --git a/fdts/fvp-base-gicv2legacy-psci.dtb b/fdts/fvp-base-gicv2legacy-psci.dtb
index 0d17620..4c6f37a 100644
--- a/fdts/fvp-base-gicv2legacy-psci.dtb
+++ b/fdts/fvp-base-gicv2legacy-psci.dtb
Binary files differ
diff --git a/fdts/fvp-base-gicv2legacy-psci.dts b/fdts/fvp-base-gicv2legacy-psci.dts
index 58d3159..5a7ce2f 100644
--- a/fdts/fvp-base-gicv2legacy-psci.dts
+++ b/fdts/fvp-base-gicv2legacy-psci.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -125,6 +125,7 @@
 			reg = <0x0 0x0>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU1:cpu@1 {
@@ -133,6 +134,7 @@
 			reg = <0x0 0x1>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU2:cpu@2 {
@@ -141,6 +143,7 @@
 			reg = <0x0 0x2>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU3:cpu@3 {
@@ -149,6 +152,7 @@
 			reg = <0x0 0x3>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU4:cpu@100 {
@@ -157,6 +161,7 @@
 			reg = <0x0 0x100>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU5:cpu@101 {
@@ -165,6 +170,7 @@
 			reg = <0x0 0x101>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU6:cpu@102 {
@@ -173,6 +179,7 @@
 			reg = <0x0 0x102>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU7:cpu@103 {
@@ -181,6 +188,11 @@
 			reg = <0x0 0x103>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
+		};
+
+		L2_0: l2-cache0 {
+			compatible = "cache";
 		};
 	};
 
diff --git a/fdts/fvp-base-gicv3-psci.dtb b/fdts/fvp-base-gicv3-psci.dtb
index f1dc75e..0acbe17 100644
--- a/fdts/fvp-base-gicv3-psci.dtb
+++ b/fdts/fvp-base-gicv3-psci.dtb
Binary files differ
diff --git a/fdts/fvp-base-gicv3-psci.dts b/fdts/fvp-base-gicv3-psci.dts
index da090f5..5d54dbf 100644
--- a/fdts/fvp-base-gicv3-psci.dts
+++ b/fdts/fvp-base-gicv3-psci.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -123,6 +123,7 @@
 			reg = <0x0 0x0>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU1:cpu@1 {
@@ -131,6 +132,7 @@
 			reg = <0x0 0x1>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU2:cpu@2 {
@@ -139,6 +141,7 @@
 			reg = <0x0 0x2>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU3:cpu@3 {
@@ -147,6 +150,7 @@
 			reg = <0x0 0x3>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU4:cpu@100 {
@@ -155,6 +159,7 @@
 			reg = <0x0 0x100>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU5:cpu@101 {
@@ -163,6 +168,7 @@
 			reg = <0x0 0x101>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU6:cpu@102 {
@@ -171,6 +177,7 @@
 			reg = <0x0 0x102>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU7:cpu@103 {
@@ -179,6 +186,11 @@
 			reg = <0x0 0x103>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
+		};
+
+		L2_0: l2-cache0 {
+			compatible = "cache";
 		};
 	};
 
diff --git a/fdts/fvp-foundation-gicv2-psci.dtb b/fdts/fvp-foundation-gicv2-psci.dtb
index e457918..5acb139 100644
--- a/fdts/fvp-foundation-gicv2-psci.dtb
+++ b/fdts/fvp-foundation-gicv2-psci.dtb
Binary files differ
diff --git a/fdts/fvp-foundation-gicv2-psci.dts b/fdts/fvp-foundation-gicv2-psci.dts
index 5f93daa..d5c2dfb 100644
--- a/fdts/fvp-foundation-gicv2-psci.dts
+++ b/fdts/fvp-foundation-gicv2-psci.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -110,6 +110,7 @@
 			reg = <0x0 0x0>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU1:cpu@1 {
@@ -118,6 +119,7 @@
 			reg = <0x0 0x1>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU2:cpu@2 {
@@ -126,6 +128,7 @@
 			reg = <0x0 0x2>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU3:cpu@3 {
@@ -134,6 +137,11 @@
 			reg = <0x0 0x3>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
+		};
+
+		L2_0: l2-cache0 {
+			compatible = "cache";
 		};
 	};
 
diff --git a/fdts/fvp-foundation-gicv2legacy-psci.dtb b/fdts/fvp-foundation-gicv2legacy-psci.dtb
index ac7555d..efded44 100644
--- a/fdts/fvp-foundation-gicv2legacy-psci.dtb
+++ b/fdts/fvp-foundation-gicv2legacy-psci.dtb
Binary files differ
diff --git a/fdts/fvp-foundation-gicv2legacy-psci.dts b/fdts/fvp-foundation-gicv2legacy-psci.dts
index 4238b33..e6e4012 100644
--- a/fdts/fvp-foundation-gicv2legacy-psci.dts
+++ b/fdts/fvp-foundation-gicv2legacy-psci.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -110,6 +110,7 @@
 			reg = <0x0 0x0>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU1:cpu@1 {
@@ -118,6 +119,7 @@
 			reg = <0x0 0x1>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU2:cpu@2 {
@@ -126,6 +128,7 @@
 			reg = <0x0 0x2>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU3:cpu@3 {
@@ -134,6 +137,11 @@
 			reg = <0x0 0x3>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
+		};
+
+		L2_0: l2-cache0 {
+			compatible = "cache";
 		};
 	};
 
diff --git a/fdts/fvp-foundation-gicv3-psci.dtb b/fdts/fvp-foundation-gicv3-psci.dtb
index f3e0c5d..4402436 100644
--- a/fdts/fvp-foundation-gicv3-psci.dtb
+++ b/fdts/fvp-foundation-gicv3-psci.dtb
Binary files differ
diff --git a/fdts/fvp-foundation-gicv3-psci.dts b/fdts/fvp-foundation-gicv3-psci.dts
index daad1fb..45c699a 100644
--- a/fdts/fvp-foundation-gicv3-psci.dts
+++ b/fdts/fvp-foundation-gicv3-psci.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -108,6 +108,7 @@
 			reg = <0x0 0x0>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU1:cpu@1 {
@@ -116,6 +117,7 @@
 			reg = <0x0 0x1>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU2:cpu@2 {
@@ -124,6 +126,7 @@
 			reg = <0x0 0x2>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
 		};
 
 		CPU3:cpu@3 {
@@ -132,6 +135,11 @@
 			reg = <0x0 0x3>;
 			enable-method = "psci";
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+			next-level-cache = <&L2_0>;
+		};
+
+		L2_0: l2-cache0 {
+			compatible = "cache";
 		};
 	};
 
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index e5e6717..f13dc31 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -53,28 +53,12 @@
 #define ENTRY_POINT_INFO_ARGS_OFFSET	0x18
 
 /* The following are used to set/get image attributes. */
-#define EXECUTABLE			(0x1)
-#define NON_EXECUTABLE			(0x0)
-#define PARAM_EP_EXECUTE_MASK		(0x1)
-#define PARAM_EP_EXECUTE_SHIFT		(0x1)
 #define PARAM_EP_SECURITY_MASK		(0x1)
-#define PARAM_EP_SECURITY_SHIFT		(0x0)
 
 #define GET_SECURITY_STATE(x) (x & PARAM_EP_SECURITY_MASK)
 #define SET_SECURITY_STATE(x, security) \
 			((x) = ((x) & ~PARAM_EP_SECURITY_MASK) | (security))
 
-#define GET_EXEC_STATE(x)    \
-    (((x) >> PARAM_EP_EXECUTE_SHIFT) & PARAM_EP_EXECUTE_MASK)
-
-#define SET_EXEC_STATE(x)    \
-    (((x) & PARAM_EP_EXECUTE_MASK) << PARAM_EP_EXECUTE_SHIFT)
-
-#define GET_SEC_STATE(x)    \
-    (((x) >> PARAM_EP_SECURITY_SHIFT) & PARAM_EP_SECURITY_MASK)
-
-#define SET_SEC_STATE(x)    \
-    (((x) & PARAM_EP_SECURITY_MASK) << PARAM_EP_SECURITY_SHIFT)
 
 /*
  * The following are used for image state attributes.
@@ -99,11 +83,17 @@
 #define EP_GET_ST(x) (x & EP_ST_MASK)
 #define EP_SET_ST(x, ee) ((x) = ((x) & ~EP_ST_MASK) | (ee))
 
+#define EP_EXE_MASK	0x8
+#define NON_EXECUTABLE	0x0
+#define EXECUTABLE	0x8
+#define EP_GET_EXE(x) (x & EP_EXE_MASK)
+#define EP_SET_EXE(x, ee) ((x) = ((x) & ~EP_EXE_MASK) | (ee))
+
-#define PARAM_EP     0x01
-#define PARAM_IMAGE_BINARY  0x02
-#define PARAM_BL31       0x03
+#define PARAM_EP		0x01
+#define PARAM_IMAGE_BINARY	0x02
+#define PARAM_BL31		0x03
 
-#define VERSION_1		0x01
+#define VERSION_1	0x01
 
 #define INVALID_IMAGE_ID		(0xFFFFFFFF)
 
@@ -114,6 +104,14 @@
 	(_p)->h.attr = (uint32_t)(_attr) ; \
 	} while (0)
 
+/* Following is used for populating structure members statically. */
+#define SET_STATIC_PARAM_HEAD(_p, _type, _ver, _p_type, _attr)	\
+	._p.h.type = (uint8_t)(_type), \
+	._p.h.version = (uint8_t)(_ver), \
+	._p.h.size = (uint16_t)sizeof(_p_type), \
+	._p.h.attr = (uint32_t)(_attr)
+
+
 /*******************************************************************************
  * Constants to indicate type of exception to the common exception handler.
  ******************************************************************************/
@@ -224,7 +222,6 @@
 	param_header_t h;
 	uintptr_t image_base;   /* physical address of base of image */
 	uint32_t image_size;    /* bytes read from image file */
-	uint32_t copied_size;	/* image size copied in blocks */
 } image_info_t;
 
 /*****************************************************************************
@@ -238,6 +235,7 @@
 	 * Refer IMAGE_STATE_XXX defined above.
 	 */
 	unsigned int state;
+	uint32_t copied_size;	/* image size copied in blocks */
 	image_info_t image_info;
 	entry_point_info_t ep_info;
 } image_desc_t;
diff --git a/include/common/debug.h b/include/common/debug.h
index d198c32..41c8df0 100644
--- a/include/common/debug.h
+++ b/include/common/debug.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -31,8 +31,6 @@
 #ifndef __DEBUG_H__
 #define __DEBUG_H__
 
-#include <stdio.h>
-
 /* 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
@@ -49,6 +47,8 @@
 #define LOG_LEVEL_INFO			40
 #define LOG_LEVEL_VERBOSE		50
 
+#ifndef __ASSEMBLY__
+#include <stdio.h>
 
 #if LOG_LEVEL >= LOG_LEVEL_NOTICE
 # define NOTICE(...)	tf_printf("NOTICE:  " __VA_ARGS__)
@@ -86,4 +86,5 @@
 
 void tf_printf(const char *fmt, ...) __printflike(1, 2);
 
+#endif /* __ASSEMBLY__ */
 #endif /* __DEBUG_H__ */
diff --git a/include/common/el3_common_macros.S b/include/common/el3_common_macros.S
index 6f7136f..0cd85c3 100644
--- a/include/common/el3_common_macros.S
+++ b/include/common/el3_common_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -171,9 +171,19 @@
 	do_cold_boot:
 	.endif /* _warm_boot_mailbox */
 
+	/* ---------------------------------------------------------------------
+	 * It is a cold boot.
+	 * Perform any processor specific actions upon reset e.g. cache, TLB
+	 * invalidations etc.
+	 * ---------------------------------------------------------------------
+	 */
+	bl	reset_handler
+
+	el3_arch_init_common \_exception_vectors
+
 	.if \_secondary_cold_boot
 		/* -------------------------------------------------------------
-		 * It is a cold boot.
+		 * Check if this is a primary or secondary CPU cold boot.
 		 * The primary CPU will set up the platform while the
 		 * secondaries are placed in a platform-specific state until the
 		 * primary CPU performs the necessary actions to bring them out
@@ -186,20 +196,16 @@
 		/* This is a cold boot on a secondary CPU */
 		bl	plat_secondary_cold_boot_setup
 		/* plat_secondary_cold_boot_setup() is not supposed to return */
-	secondary_panic:
-		b	secondary_panic
+		bl	el3_panic
 
 	do_primary_cold_boot:
 	.endif /* _secondary_cold_boot */
 
 	/* ---------------------------------------------------------------------
-	 * Perform any processor specific actions upon reset e.g. cache, TLB
-	 * invalidations etc.
+	 * Initialize memory now. Secondary CPU initialization won't get to this
+	 * point.
 	 * ---------------------------------------------------------------------
 	 */
-	bl	reset_handler
-
-	el3_arch_init_common \_exception_vectors
 
 	.if \_init_memory
 		bl	platform_mem_init
diff --git a/include/drivers/arm/gic_common.h b/include/drivers/arm/gic_common.h
index 6a322a2..dd8efdc 100644
--- a/include/drivers/arm/gic_common.h
+++ b/include/drivers/arm/gic_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -107,67 +107,4 @@
 	(GIC_HIGHEST_NS_PRIORITY << 16)	|	\
 	(GIC_HIGHEST_NS_PRIORITY << 24))
 
-#ifndef __ASSEMBLY__
-
-#include <mmio.h>
-#include <stdint.h>
-
-/*******************************************************************************
- * GIC Distributor interface register accessors that are common to GICv3 & GICv2
- ******************************************************************************/
-static inline unsigned int gicd_read_ctlr(uintptr_t base)
-{
-	return mmio_read_32(base + GICD_CTLR);
-}
-
-static inline unsigned int gicd_read_typer(uintptr_t base)
-{
-	return mmio_read_32(base + GICD_TYPER);
-}
-
-static inline unsigned int gicd_read_iidr(uintptr_t base)
-{
-	return mmio_read_32(base + GICD_IIDR);
-}
-
-static inline void gicd_write_ctlr(uintptr_t base, unsigned int val)
-{
-	mmio_write_32(base + GICD_CTLR, val);
-}
-
-/*******************************************************************************
- * GIC Distributor function prototypes
- ******************************************************************************/
-unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id);
-unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id);
-unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id);
-void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val);
-unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id);
-void gicd_set_igroupr(uintptr_t base, unsigned int id);
-void gicd_clr_igroupr(uintptr_t base, unsigned int id);
-void gicd_set_isenabler(uintptr_t base, unsigned int id);
-void gicd_set_icenabler(uintptr_t base, unsigned int id);
-void gicd_set_ispendr(uintptr_t base, unsigned int id);
-void gicd_set_icpendr(uintptr_t base, unsigned int id);
-void gicd_set_isactiver(uintptr_t base, unsigned int id);
-void gicd_set_icactiver(uintptr_t base, unsigned int id);
-
-
-#endif /* __ASSEMBLY__ */
 #endif /* __GIC_COMMON_H__ */
diff --git a/lib/stdlib/std.c b/include/drivers/arm/pl061_gpio.h
similarity index 78%
rename from lib/stdlib/std.c
rename to include/drivers/arm/pl061_gpio.h
index 5f6ef75..ca79630 100644
--- a/lib/stdlib/std.c
+++ b/include/drivers/arm/pl061_gpio.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016, 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,18 +28,12 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#ifndef __PL061_GPIO_H__
+#define __PL061_GPIO_H__
+
+#include <gpio.h>
+
+void pl061_gpio_register(uintptr_t base_addr, int gpio_dev);
+void pl061_gpio_init(void);
 
-/* Include the various implemented functions */
-#include "abort.c"
-#include "assert.c"
-#include "exit.c"
-#include "mem.c"
-#include "printf.c"
-#include "putchar.c"
-#include "puts.c"
-#include "sscanf.c"
-#include "strchr.c"
-#include "strcmp.c"
-#include "strlen.c"
-#include "strncmp.c"
-#include "subr_prf.c"
+#endif	/* __PL061_GPIO_H__ */
diff --git a/lib/stdlib/std.c b/include/drivers/gpio.h
similarity index 69%
copy from lib/stdlib/std.c
copy to include/drivers/gpio.h
index 5f6ef75..a5cb5c7 100644
--- a/lib/stdlib/std.c
+++ b/include/drivers/gpio.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016, 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,18 +28,26 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#ifndef __GPIO_H__
+#define __GPIO_H__
+
+#define GPIO_DIR_OUT		0
+#define GPIO_DIR_IN		1
+
+#define GPIO_LEVEL_LOW		0
+#define GPIO_LEVEL_HIGH		1
+
+typedef struct gpio_ops {
+	int (*get_direction)(int gpio);
+	void (*set_direction)(int gpio, int direction);
+	int (*get_value)(int gpio);
+	void (*set_value)(int gpio, int value);
+} gpio_ops_t;
+
+int gpio_get_direction(int gpio);
+void gpio_set_direction(int gpio, int direction);
+int gpio_get_value(int gpio);
+void gpio_set_value(int gpio, int value);
+void gpio_init(const gpio_ops_t *ops);
 
-/* Include the various implemented functions */
-#include "abort.c"
-#include "assert.c"
-#include "exit.c"
-#include "mem.c"
-#include "printf.c"
-#include "putchar.c"
-#include "puts.c"
-#include "sscanf.c"
-#include "strchr.c"
-#include "strcmp.c"
-#include "strlen.c"
-#include "strncmp.c"
-#include "subr_prf.c"
+#endif	/* __GPIO_H__ */
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index 49efafc..a9b2dbb 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -419,11 +419,11 @@
 #define AP_RW			(0x0 << 5)
 
 #define NS				(0x1 << 3)
-#define ATTR_SO_INDEX			0x2
+#define ATTR_NON_CACHEABLE_INDEX	0x2
 #define ATTR_DEVICE_INDEX		0x1
 #define ATTR_IWBWA_OWBWA_NTR_INDEX	0x0
 #define LOWER_ATTRS(x)			(((x) & 0xfff) << 2)
-#define ATTR_SO				(0x0)
+#define ATTR_NON_CACHEABLE		(0x44)
 #define ATTR_DEVICE			(0x4)
 #define ATTR_IWBWA_OWBWA_NTR		(0xff)
 #define MAIR_ATTR_SET(attr, index)	(attr << (index << 3))
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 067b830..d1ad31d 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -194,6 +194,8 @@
 DEFINE_SYSOP_FUNC(sev)
 DEFINE_SYSOP_TYPE_FUNC(dsb, sy)
 DEFINE_SYSOP_TYPE_FUNC(dmb, sy)
+DEFINE_SYSOP_TYPE_FUNC(dmb, st)
+DEFINE_SYSOP_TYPE_FUNC(dmb, ld)
 DEFINE_SYSOP_TYPE_FUNC(dsb, ish)
 DEFINE_SYSOP_TYPE_FUNC(dmb, ish)
 DEFINE_SYSOP_FUNC(isb)
diff --git a/include/lib/aarch64/xlat_tables.h b/include/lib/aarch64/xlat_tables.h
index 0b5dbdf..d21100e 100644
--- a/include/lib/aarch64/xlat_tables.h
+++ b/include/lib/aarch64/xlat_tables.h
@@ -52,21 +52,41 @@
 #define MAP_REGION(pa, va, sz, attr) {(pa), (va), (sz), (attr)}
 
 /*
- * Flags for building up memory mapping attributes.
- * These are organised so that a clear bit gives a more restrictive  mapping
- * that a set bit, that way a bitwise-and two sets of attributes will never give
- * an attribute which has greater access rights that any of the original
- * attributes.
+ * Shifts and masks to access fields of an mmap_attr_t
+ */
+#define MT_TYPE_MASK	0x7
+#define MT_TYPE(_attr)	((_attr) & MT_TYPE_MASK)
+/* Access permissions (RO/RW) */
+#define MT_PERM_SHIFT	3
+/* Security state (SECURE/NS) */
+#define MT_SEC_SHIFT	4
+
+/*
+ * Memory mapping attributes
  */
 typedef enum  {
-	MT_DEVICE	= 0 << 0,
-	MT_MEMORY	= 1 << 0,
+	/*
+	 * Memory types supported.
+	 * These are organised so that, going down the list, the memory types
+	 * are getting weaker; conversely going up the list the memory types are
+	 * getting stronger.
+	 */
+	MT_DEVICE,
+	MT_NON_CACHEABLE,
+	MT_MEMORY,
+	/* Values up to 7 are reserved to add new memory types in the future */
 
-	MT_RO		= 0 << 1,
-	MT_RW		= 1 << 1,
+	/*
+	 * The following values are organised so that a clear bit gives a more
+	 * restrictive mapping than a set bit, that way a bitwise-and of two
+	 * sets of attributes will never give an attribute which has greater
+	 * access rights than any of the original attributes.
+	 */
+	MT_RO		= 0 << MT_PERM_SHIFT,
+	MT_RW		= 1 << MT_PERM_SHIFT,
 
-	MT_SECURE	= 0 << 2,
-	MT_NS		= 1 << 2
+	MT_SECURE	= 0 << MT_SEC_SHIFT,
+	MT_NS		= 1 << MT_SEC_SHIFT,
 } mmap_attr_t;
 
 /*
diff --git a/include/plat/arm/board/common/board_arm_def.h b/include/plat/arm/board/common/board_arm_def.h
index db2a8df..b065537 100644
--- a/include/plat/arm/board/common/board_arm_def.h
+++ b/include/plat/arm/board/common/board_arm_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -60,80 +60,28 @@
 #endif
 
 /*
- * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
- * plat_arm_mmap array defined for each BL stage.
+ * The constants below are not optimised for memory usage. Platforms that wish
+ * to optimise these constants should set `ARM_BOARD_OPTIMISE_MMAP` to 1 and
+ * provide there own values.
  */
-#if IMAGE_BL1
-# if PLAT_fvp
-#  if TRUSTED_BOARD_BOOT
-#   define PLAT_ARM_MMAP_ENTRIES	8
-#  else
-#   define PLAT_ARM_MMAP_ENTRIES	7
-#  endif	/* TRUSTED_BOARD_BOOT */
-# else
-#  if TRUSTED_BOARD_BOOT
-#   define PLAT_ARM_MMAP_ENTRIES	7
-#  else
-#   define PLAT_ARM_MMAP_ENTRIES	6
-#  endif	/* TRUSTED_BOARD_BOOT */
-# endif		/* PLAT_ */
-#endif
-#if IMAGE_BL2
-# if PLAT_fvp
-#  define PLAT_ARM_MMAP_ENTRIES		9
-# else
-#  define PLAT_ARM_MMAP_ENTRIES		8
-# endif
-#endif
-#if IMAGE_BL2U
-# if PLAT_fvp
-#  define PLAT_ARM_MMAP_ENTRIES		3
-# else
-#  define PLAT_ARM_MMAP_ENTRIES		4
-#endif
-#endif
-#if IMAGE_BL31
-#define PLAT_ARM_MMAP_ENTRIES		5
-#endif
-#if IMAGE_BL32
-#define PLAT_ARM_MMAP_ENTRIES		4
-#endif
-
+#if !ARM_BOARD_OPTIMISE_MMAP
 /*
- * Platform specific page table and MMU setup constants
+ * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
+ * plat_arm_mmap array defined for each BL stage.
+ *
+ * Provide relatively optimised values for the runtime images (BL31 and BL32).
+ * Optimisation is less important for the other, transient boot images so a
+ * common, maximum value is used across these images.
  */
-#if IMAGE_BL1
-# if TRUSTED_BOARD_BOOT
-#  define MAX_XLAT_TABLES		4
-# else
-#  if PLAT_juno
-#   define MAX_XLAT_TABLES		2
-#  else
-#   define MAX_XLAT_TABLES		3
-#  endif /* PLAT_ */
-# endif	/* TRUSTED_BOARD_BOOT */
-#elif IMAGE_BL2
-# if PLAT_juno
-#  define MAX_XLAT_TABLES		3
-# else
-#  define MAX_XLAT_TABLES		4
-# endif /* PLAT_ */
-#elif IMAGE_BL2U
-# if PLAT_juno
-#  define MAX_XLAT_TABLES		3
-# else
-#  define MAX_XLAT_TABLES		4
-# endif /* PLAT_ */
-#elif IMAGE_BL31
-# define MAX_XLAT_TABLES		2
-#elif IMAGE_BL32
-# if ARM_TSP_RAM_LOCATION_ID == ARM_DRAM_ID
-#  define MAX_XLAT_TABLES		3
-# else
-#  define MAX_XLAT_TABLES		2
-# endif
+#if IMAGE_BL31 || IMAGE_BL32
+# define PLAT_ARM_MMAP_ENTRIES		6
+# define MAX_XLAT_TABLES		3
+#else
+# define PLAT_ARM_MMAP_ENTRIES		9
+# define MAX_XLAT_TABLES		4
 #endif
 
+#endif /* ARM_BOARD_OPTIMISE_MMAP */
 
 #define MAX_IO_DEVICES			3
 #define MAX_IO_HANDLES			4
@@ -143,5 +91,8 @@
 #define PLAT_ARM_FIP_BASE		V2M_FLASH0_BASE
 #define PLAT_ARM_FIP_MAX_SIZE		V2M_FLASH0_SIZE
 
+#define PLAT_ARM_NVM_BASE		V2M_FLASH0_BASE
+#define PLAT_ARM_NVM_SIZE		V2M_FLASH0_SIZE
+
 
 #endif /* __BOARD_ARM_DEF_H__ */
diff --git a/include/plat/arm/common/aarch64/arm_macros.S b/include/plat/arm/common/aarch64/arm_macros.S
index 384bb51..3b19a7d 100644
--- a/include/plat/arm/common/aarch64/arm_macros.S
+++ b/include/plat/arm/common/aarch64/arm_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -30,7 +30,6 @@
 #ifndef __ARM_MACROS_S__
 #define __ARM_MACROS_S__
 
-#include <cci.h>
 #include <gic_common.h>
 #include <gicv2.h>
 #include <gicv3.h>
@@ -117,31 +116,4 @@
 exit_print_gic_regs:
 	.endm
 
-
-.section .rodata.cci_reg_name, "aS"
-cci_iface_regs:
-	.asciz "cci_snoop_ctrl_cluster0", "cci_snoop_ctrl_cluster1" , ""
-
-	/* ------------------------------------------------
-	 * The below required platform porting macro prints
-	 * out relevant interconnect registers whenever an
-	 * unhandled exception is taken in BL31.
-	 * Clobbers: x0 - x9, sp
-	 * ------------------------------------------------
-	 */
-	.macro plat_print_interconnect_regs
-	adr	x6, cci_iface_regs
-	/* Store in x7 the base address of the first interface */
-	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX))
-	ldr	w8, [x7, #SNOOP_CTRL_REG]
-	/* Store in x7 the base address of the second interface */
-	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX))
-	ldr	w9, [x7, #SNOOP_CTRL_REG]
-	/* Store to the crash buf and print to console */
-	bl	str_in_crash_buf_print
-	.endm
-
-
 #endif /* __ARM_MACROS_S__ */
diff --git a/include/plat/arm/common/aarch64/cci_macros.S b/include/plat/arm/common/aarch64/cci_macros.S
new file mode 100644
index 0000000..40f9d7e
--- /dev/null
+++ b/include/plat/arm/common/aarch64/cci_macros.S
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2016, 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 __CCI_MACROS_S__
+#define __CCI_MACROS_S__
+
+#include <cci.h>
+#include <platform_def.h>
+
+.section .rodata.cci_reg_name, "aS"
+cci_iface_regs:
+	.asciz "cci_snoop_ctrl_cluster0", "cci_snoop_ctrl_cluster1" , ""
+
+	/* ------------------------------------------------
+	 * The below required platform porting macro prints
+	 * out relevant interconnect registers whenever an
+	 * unhandled exception is taken in BL31.
+	 * Clobbers: x0 - x9, sp
+	 * ------------------------------------------------
+	 */
+	.macro plat_print_interconnect_regs
+	adr	x6, cci_iface_regs
+	/* Store in x7 the base address of the first interface */
+	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
+			PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX))
+	ldr	w8, [x7, #SNOOP_CTRL_REG]
+	/* Store in x7 the base address of the second interface */
+	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
+			PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX))
+	ldr	w9, [x7, #SNOOP_CTRL_REG]
+	/* Store to the crash buf and print to console */
+	bl	str_in_crash_buf_print
+	.endm
+
+#endif /* __CCI_MACROS_S__ */
diff --git a/include/plat/arm/common/arm_config.h b/include/plat/arm/common/arm_config.h
index 24c1f0a..0380849 100644
--- a/include/plat/arm/common/arm_config.h
+++ b/include/plat/arm/common/arm_config.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -35,8 +35,8 @@
 enum arm_config_flags {
 	/* Whether Base memory map is in use */
 	ARM_CONFIG_BASE_MMAP		= 0x1,
-	/* Whether CCI should be enabled */
-	ARM_CONFIG_HAS_CCI		= 0x2,
+	/* Whether interconnect should be enabled */
+	ARM_CONFIG_HAS_INTERCONNECT	= 0x2,
 	/* Whether TZC should be configured */
 	ARM_CONFIG_HAS_TZC		= 0x4
 };
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index b2db616..d04f9d6 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -44,7 +44,6 @@
 /* Special value used to verify platform parameters from BL2 to BL31 */
 #define ARM_BL31_PLAT_PARAM_VAL		0x0f1e2d3c4b5a6978ULL
 
-#define ARM_CLUSTER_COUNT		2
 #define ARM_SYSTEM_COUNT		1
 
 #define ARM_CACHE_WRITEBACK_SHIFT	6
@@ -151,14 +150,10 @@
 #define ARM_G0_IRQS			ARM_IRQ_SEC_SGI_0,		\
 					ARM_IRQ_SEC_SGI_6
 
-#define ARM_SHARED_RAM_ATTR		((PLAT_ARM_SHARED_RAM_CACHED ?	\
-						MT_MEMORY : MT_DEVICE)	\
-						| MT_RW | MT_SECURE)
-
 #define ARM_MAP_SHARED_RAM		MAP_REGION_FLAT(		\
 						ARM_SHARED_RAM_BASE,	\
 						ARM_SHARED_RAM_SIZE,	\
-						ARM_SHARED_RAM_ATTR)
+						MT_DEVICE | MT_RW | MT_SECURE)
 
 #define ARM_MAP_NS_DRAM1		MAP_REGION_FLAT(		\
 						ARM_NS_DRAM1_BASE,	\
@@ -218,10 +213,6 @@
  */
 #define PLAT_MAX_OFF_STATE		ARM_LOCAL_STATE_OFF
 
-
-#define PLATFORM_CORE_COUNT		(PLAT_ARM_CLUSTER0_CORE_COUNT + \
-					 PLAT_ARM_CLUSTER1_CORE_COUNT)
-
 /*
  * Some data must be aligned on the biggest cache line size in the platform.
  * This is known only to the platform as it might have a combination of
@@ -239,44 +230,31 @@
 #define BL1_RO_LIMIT			(PLAT_ARM_TRUSTED_ROM_BASE	\
 					 + PLAT_ARM_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.
  */
-#if TRUSTED_BOARD_BOOT
 #define BL1_RW_BASE			(ARM_BL_RAM_BASE +		\
 						ARM_BL_RAM_SIZE -	\
-						0x9000)
-#else
-#define BL1_RW_BASE			(ARM_BL_RAM_BASE +		\
-						ARM_BL_RAM_SIZE -	\
-						0x6000)
-#endif
+						PLAT_ARM_MAX_BL1_RW_SIZE)
 #define BL1_RW_LIMIT			(ARM_BL_RAM_BASE + ARM_BL_RAM_SIZE)
 
 /*******************************************************************************
  * BL2 specific defines.
  ******************************************************************************/
 /*
- * Put BL2 just below BL31. BL2_BASE is calculated using the current BL2 debug
- * size plus a little space for growth.
+ * Put BL2 just below BL31.
  */
-#if TRUSTED_BOARD_BOOT
-#define BL2_BASE			(BL31_BASE - 0x1D000)
-#else
-#define BL2_BASE			(BL31_BASE - 0xC000)
-#endif
+#define BL2_BASE			(BL31_BASE - PLAT_ARM_MAX_BL2_SIZE)
 #define BL2_LIMIT			BL31_BASE
 
 /*******************************************************************************
  * BL31 specific defines.
  ******************************************************************************/
 /*
- * Put BL31 at the top of the Trusted SRAM. BL31_BASE is calculated using the
- * current BL31 debug size plus a little space for growth.
+ * Put BL31 at the top of the Trusted SRAM.
  */
 #define BL31_BASE			(ARM_BL_RAM_BASE +		\
 						ARM_BL_RAM_SIZE -	\
-						0x1D000)
+						PLAT_ARM_MAX_BL31_SIZE)
 #define BL31_PROGBITS_LIMIT		BL1_RW_BASE
 #define BL31_LIMIT			(ARM_BL_RAM_BASE + ARM_BL_RAM_SIZE)
 
@@ -316,7 +294,7 @@
 #define BL2U_BASE			BL2_BASE
 #define BL2U_LIMIT			BL31_BASE
 #define NS_BL2U_BASE			ARM_NS_DRAM1_BASE
-#define NS_BL1U_BASE			(V2M_FLASH0_BASE + 0x03EB8000)
+#define NS_BL1U_BASE			(PLAT_ARM_NVM_BASE + 0x03EB8000)
 
 /*
  * ID of the secure physical generic timer interrupt used by the TSP.
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 3b6a04b..e9eebaa 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -37,11 +37,6 @@
 #include <stdint.h>
 #include <xlat_tables.h>
 
-/*
- * Extern declarations common to ARM standard platforms
- */
-extern const mmap_region_t plat_arm_mmap[];
-
 #define ARM_CASSERT_MMAP						\
 	CASSERT((ARRAY_SIZE(plat_arm_mmap) + ARM_BL_REGIONS)		\
 		<= MAX_MMAP_REGIONS,					\
@@ -131,9 +126,6 @@
 #endif /* __ARM_RECOM_STATE_ID_ENC__ */
 
 
-/* CCI utility functions */
-void arm_cci_init(void);
-
 /* IO storage utility functions */
 void arm_io_setup(void);
 
@@ -187,6 +179,7 @@
 /*
  * Mandatory functions required in ARM standard platforms
  */
+unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr);
 void plat_arm_gic_driver_init(void);
 void plat_arm_gic_init(void);
 void plat_arm_gic_cpuif_enable(void);
@@ -194,6 +187,9 @@
 void plat_arm_gic_pcpu_init(void);
 void plat_arm_security_setup(void);
 void plat_arm_pwrc_setup(void);
+void plat_arm_interconnect_init(void);
+void plat_arm_interconnect_enter_coherency(void);
+void plat_arm_interconnect_exit_coherency(void);
 
 /*
  * Optional functions required in ARM standard platforms
@@ -204,6 +200,6 @@
 	uintptr_t *dev_handle,
 	uintptr_t *image_spec);
 unsigned int plat_arm_calc_core_pos(u_register_t mpidr);
-
+const mmap_region_t *plat_arm_get_mmap(void);
 
 #endif /* __PLAT_ARM_H__ */
diff --git a/include/plat/arm/css/common/css_def.h b/include/plat/arm/css/common/css_def.h
index c900278..f92126b 100644
--- a/include/plat/arm/css/common/css_def.h
+++ b/include/plat/arm/css/common/css_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -37,15 +37,12 @@
 /*************************************************************************
  * Definitions common to all ARM Compute SubSystems (CSS)
  *************************************************************************/
-#define MHU_PAYLOAD_CACHED		0
-
 #define NSROM_BASE			0x1f000000
 #define NSROM_SIZE			0x00001000
 
 /* Following covers CSS Peripherals excluding NSROM and NSRAM  */
 #define CSS_DEVICE_BASE			0x20000000
 #define CSS_DEVICE_SIZE			0x0e000000
-#define MHU_BASE			0x2b1f0000
 
 #define NSRAM_BASE			0x2e000000
 #define NSRAM_SIZE			0x00008000
@@ -78,31 +75,47 @@
  * SCP <=> AP boot configuration
  *
  * The SCP/AP boot configuration is a 32-bit word located at a known offset from
- * the start of the Trusted SRAM. Part of this configuration is which CPU is the
- * primary, according to the shift and mask definitions below.
+ * the start of the Trusted SRAM.
  *
  * Note that the value stored at this address is only valid at boot time, before
  * the SCP_BL2 image is transferred to SCP.
  */
-#define SCP_BOOT_CFG_ADDR		(ARM_TRUSTED_SRAM_BASE + 0x80)
-#define PRIMARY_CPU_SHIFT		8
-#define PRIMARY_CPU_BIT_WIDTH		4
-
-/*
- * Base address of the first memory region used for communication between AP
- * and SCP. Used by the BOM and SCPI protocols.
- *
- * Note that this is located at the same address as SCP_BOOT_CFG_ADDR, which
- * means the SCP/AP configuration data gets overwritten when the AP initiates
- * communication with the SCP.
- */
-#define SCP_COM_SHARED_MEM_BASE		(ARM_TRUSTED_SRAM_BASE + 0x80)
+#define SCP_BOOT_CFG_ADDR		PLAT_CSS_SCP_COM_SHARED_MEM_BASE
 
 #define CSS_MAP_DEVICE			MAP_REGION_FLAT(		\
 						CSS_DEVICE_BASE,	\
 						CSS_DEVICE_SIZE,	\
 						MT_DEVICE | MT_RW | MT_SECURE)
 
+/* Platform ID address */
+#define SSC_VERSION_OFFSET			0x040
+
+#define SSC_VERSION_CONFIG_SHIFT		28
+#define SSC_VERSION_MAJOR_REV_SHIFT		24
+#define SSC_VERSION_MINOR_REV_SHIFT		20
+#define SSC_VERSION_DESIGNER_ID_SHIFT		12
+#define SSC_VERSION_PART_NUM_SHIFT		0x0
+#define SSC_VERSION_CONFIG_MASK			0xf
+#define SSC_VERSION_MAJOR_REV_MASK		0xf
+#define SSC_VERSION_MINOR_REV_MASK		0xf
+#define SSC_VERSION_DESIGNER_ID_MASK		0xff
+#define SSC_VERSION_PART_NUM_MASK		0xfff
+
+#ifndef __ASSEMBLY__
+
+/* SSC_VERSION related accessors */
+
+/* Returns the part number of the platform */
+#define GET_SSC_VERSION_PART_NUM(val)				\
+		(((val) >> SSC_VERSION_PART_NUM_SHIFT) &	\
+		SSC_VERSION_PART_NUM_MASK)
+
+/* Returns the configuration number of the platform */
+#define GET_SSC_VERSION_CONFIG(val)				\
+		(((val) >> SSC_VERSION_CONFIG_SHIFT) &		\
+		SSC_VERSION_CONFIG_MASK)
+
+#endif /* __ASSEMBLY__ */
 
 /*************************************************************************
  * Required platform porting definitions common to all
@@ -110,6 +123,13 @@
  ************************************************************************/
 
 /*
+ * The loading of SCP images(SCP_BL2 or SCP_BL2U) is done if there
+ * respective base addresses are defined (i.e SCP_BL2_BASE, SCP_BL2U_BASE).
+ * Hence, `CSS_LOAD_SCP_IMAGES` needs to be set to 1 if BL2 needs to load
+ * an SCP_BL2/SCP_BL2U image.
+ */
+#if CSS_LOAD_SCP_IMAGES
+/*
  * Load address of SCP_BL2 in CSS platform ports
  * SCP_BL2 is loaded to the same place as BL31.  Once SCP_BL2 is transferred to the
  * SCP, it is discarded and BL31 is loaded over the top.
@@ -117,18 +137,13 @@
 #define SCP_BL2_BASE			BL31_BASE
 
 #define SCP_BL2U_BASE			BL31_BASE
-
-#define PLAT_ARM_SHARED_RAM_CACHED	MHU_PAYLOAD_CACHED
+#endif /* CSS_LOAD_SCP_IMAGES */
 
 /* Load address of Non-Secure Image for CSS platform ports */
 #define PLAT_ARM_NS_IMAGE_OFFSET	0xE0000000
 
 /* TZC related constants */
 #define PLAT_ARM_TZC_FILTERS		REG_ATTR_FILTER_BIT_ALL
-#define PLAT_ARM_TZC_BASE		0x2a4a0000
-
-/* System timer related constants */
-#define PLAT_ARM_NSTIMER_FRAME_ID	1
 
 /* Trusted mailbox base address common to all CSS */
 #define PLAT_ARM_TRUSTED_MAILBOX_BASE	ARM_TRUSTED_SRAM_BASE
diff --git a/include/plat/common/common_def.h b/include/plat/common/common_def.h
index 916720c..9fac9fa 100644
--- a/include/plat/common/common_def.h
+++ b/include/plat/common/common_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -72,11 +72,12 @@
 
 #define BL2_IMAGE_DESC {				\
 	.image_id = BL2_IMAGE_ID,			\
-	.image_info.h.version = VERSION_1,		\
-	.image_info.h.attr = SET_EXEC_STATE(EXECUTABLE),\
+	SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,	\
+		VERSION_1, image_info_t, 0),		\
 	.image_info.image_base = BL2_BASE,		\
-	.ep_info.h.attr = SET_SEC_STATE(SECURE),	\
-	.ep_info.pc = BL2_BASE				\
+	SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,	\
+		VERSION_1, entry_point_info_t, SECURE | EXECUTABLE),\
+	.ep_info.pc = BL2_BASE,				\
 }
 
 #endif /* __COMMON_DEF_H__ */
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index f37a80f..5c61f38 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -83,6 +83,7 @@
 int plat_crash_console_init(void);
 int plat_crash_console_putc(int c);
 void plat_error_handler(int err) __dead2;
+void plat_panic_handler(void) __dead2;
 
 /*******************************************************************************
  * Mandatory BL1 functions
diff --git a/lib/aarch64/xlat_tables.c b/lib/aarch64/xlat_tables.c
index 2f2ca81..269743f 100644
--- a/lib/aarch64/xlat_tables.c
+++ b/lib/aarch64/xlat_tables.c
@@ -52,7 +52,17 @@
 #define debug_print(...) ((void)0)
 #endif
 
-CASSERT(ADDR_SPACE_SIZE > 0, assert_valid_addr_space_size);
+#define IS_POWER_OF_TWO(x)	(((x) & ((x) - 1)) == 0)
+
+/*
+ * The virtual address space size must be a power of two (as set in TCR.T0SZ).
+ * As we start the initial lookup at level 1, it must also be between 2 GB and
+ * 512 GB (with the virtual address size therefore 31 to 39 bits). See section
+ * D4.2.5 in the ARMv8-A Architecture Reference Manual (DDI 0487A.i) for more
+ * information.
+ */
+CASSERT(ADDR_SPACE_SIZE >= (1ull << 31) && ADDR_SPACE_SIZE <= (1ull << 39) &&
+	IS_POWER_OF_TWO(ADDR_SPACE_SIZE), assert_valid_addr_space_size);
 
 #define UNSET_DESC	~0ul
 
@@ -138,6 +148,7 @@
 					unsigned level)
 {
 	unsigned long desc = addr_pa;
+	int mem_type;
 
 	desc |= level == 3 ? TABLE_DESC : BLOCK_DESC;
 
@@ -147,16 +158,23 @@
 
 	desc |= LOWER_ATTRS(ACCESS_FLAG);
 
-	if (attr & MT_MEMORY) {
+	mem_type = MT_TYPE(attr);
+	if (mem_type == MT_MEMORY) {
 		desc |= LOWER_ATTRS(ATTR_IWBWA_OWBWA_NTR_INDEX | ISH);
 		if (attr & MT_RW)
 			desc |= UPPER_ATTRS(XN);
+	} else if (mem_type == MT_NON_CACHEABLE) {
+		desc |= LOWER_ATTRS(ATTR_NON_CACHEABLE_INDEX | OSH);
+		if (attr & MT_RW)
+			desc |= UPPER_ATTRS(XN);
 	} else {
+		assert(mem_type == MT_DEVICE);
 		desc |= LOWER_ATTRS(ATTR_DEVICE_INDEX | OSH);
 		desc |= UPPER_ATTRS(XN);
 	}
 
-	debug_print(attr & MT_MEMORY ? "MEM" : "DEV");
+	debug_print((mem_type == MT_MEMORY) ? "MEM" :
+		((mem_type == MT_NON_CACHEABLE) ? "NC" : "DEV"));
 	debug_print(attr & MT_RW ? "-RW" : "-RO");
 	debug_print(attr & MT_NS ? "-NS" : "-S");
 
@@ -167,6 +185,7 @@
 					unsigned long size)
 {
 	int attr = mm->attr;
+	int old_mem_type, new_mem_type;
 
 	for (;;) {
 		++mm;
@@ -183,7 +202,20 @@
 		if ((mm->attr & attr) == attr)
 			continue; /* Region doesn't override attribs so skip */
 
+		/*
+		 * Update memory mapping attributes in 2 steps:
+		 * 1) Update access permissions and security state flags
+		 * 2) Update memory type.
+		 *
+		 * See xlat_tables.h for details about the attributes priority
+		 * system and the rules dictating whether attributes should be
+		 * updated.
+		 */
+		old_mem_type = MT_TYPE(attr);
+		new_mem_type = MT_TYPE(mm->attr);
 		attr &= mm->attr;
+		if (new_mem_type < old_mem_type)
+			attr = (attr & ~MT_TYPE_MASK) | new_mem_type;
 
 		if (mm->base_va > base_va ||
 			mm->base_va + mm->size < base_va + size)
@@ -207,7 +239,10 @@
 	do  {
 		unsigned long desc = UNSET_DESC;
 
-		if (mm->base_va + mm->size <= base_va) {
+		if (!mm->size) {
+			/* Done mapping regions; finish zeroing the table */
+			desc = INVALID_DESC;
+		} else if (mm->base_va + mm->size <= base_va) {
 			/* Area now after the region so skip it */
 			++mm;
 			continue;
@@ -245,7 +280,7 @@
 
 		*table++ = desc;
 		base_va += level_size;
-	} while (mm->size && (base_va & level_index_mask));
+	} while ((base_va & level_index_mask) && (base_va < ADDR_SPACE_SIZE));
 
 	return mm;
 }
@@ -309,6 +344,8 @@
 		mair = MAIR_ATTR_SET(ATTR_DEVICE, ATTR_DEVICE_INDEX);	\
 		mair |= MAIR_ATTR_SET(ATTR_IWBWA_OWBWA_NTR,		\
 				ATTR_IWBWA_OWBWA_NTR_INDEX);		\
+		mair |= MAIR_ATTR_SET(ATTR_NON_CACHEABLE,		\
+				ATTR_NON_CACHEABLE_INDEX);		\
 		write_mair_el##_el(mair);				\
 									\
 		/* Invalidate TLBs at the current exception level */	\
diff --git a/lib/cpus/aarch64/cortex_a53.S b/lib/cpus/aarch64/cortex_a53.S
index c33ba57..bb56516 100644
--- a/lib/cpus/aarch64/cortex_a53.S
+++ b/lib/cpus/aarch64/cortex_a53.S
@@ -32,6 +32,7 @@
 #include <bl_common.h>
 #include <cortex_a53.h>
 #include <cpu_macros.S>
+#include <debug.h>
 #include <plat_macros.S>
 
 	/* ---------------------------------------------
@@ -73,7 +74,7 @@
 	 */
 	cmp	x0, #2
 	b.ls	apply_826319
-#if DEBUG
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	b	print_revision_warning
 #else
 	ret
@@ -109,7 +110,7 @@
 	 */
 	cmp	x0, #3
 	b.ls	disable_hint
-#if DEBUG
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	b	print_revision_warning
 #else
 	ret
diff --git a/lib/cpus/aarch64/cortex_a57.S b/lib/cpus/aarch64/cortex_a57.S
index 99db25b..4c0b8ce 100644
--- a/lib/cpus/aarch64/cortex_a57.S
+++ b/lib/cpus/aarch64/cortex_a57.S
@@ -33,6 +33,7 @@
 #include <bl_common.h>
 #include <cortex_a57.h>
 #include <cpu_macros.S>
+#include <debug.h>
 #include <plat_macros.S>
 
 	/* ---------------------------------------------
@@ -99,7 +100,7 @@
 	 * Compare x0 against revision r0p0
 	 */
 	cbz	x0, apply_806969
-#if DEBUG
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	b	print_revision_warning
 #else
 	ret
@@ -125,7 +126,7 @@
 	 * Compare x0 against revision r0p0
 	 */
 	cbz	x0, apply_813420
-#if DEBUG
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	b	print_revision_warning
 #else
 	ret
@@ -154,7 +155,7 @@
 	 */
 	cmp	x0, #0x12
 	b.ls	disable_hint
-#if DEBUG
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	b	print_revision_warning
 #else
 	ret
diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S
index e41d95b..dab933c 100644
--- a/lib/cpus/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -35,6 +35,7 @@
 #if IMAGE_BL31
 #include <cpu_data.h>
 #endif
+#include <debug.h>
 
  /* Reset fn is needed in BL at reset vector */
 #if IMAGE_BL1 || IMAGE_BL31
@@ -204,10 +205,10 @@
 	ret
 endfunc get_cpu_ops_ptr
 
-#if DEBUG
-.section .rodata.rev_warn_str, "aS"
-rev_warn_str:
-	.asciz "Warning: Skipping CPU specific reset operation for non-matching CPU revision number.\n"
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+.section .rodata.rev_verbose_str, "aS"
+rev_verbose_str:
+	.asciz "VERBOSE: Skipping CPU specific reset operation for non-matching CPU revision number.\n"
 
 	/*
 	 * This function prints the above warning message to the crash console.
@@ -224,7 +225,7 @@
 	/* Check if the console is initialized */
 	cbz	x0, 1f
 	/* The console is initialized */
-	adr	x4, rev_warn_str
+	adr	x4, rev_verbose_str
 	bl	asm_print_str
 1:
 	ret	x5
diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index d6a4e3a..b22eaf9 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@@ -290,6 +290,10 @@
         $(if $(3),$(eval $(call FIP_ADD_PAYLOAD,$(2),--$(3),bl$(1))))
 endef
 
+# Allow overriding the timestamp, for example for reproducible builds, or to
+# synchronize timestamps across multiple projects.
+# This must be set to a C string (including quotes where applicable).
+BUILD_MESSAGE_TIMESTAMP ?= __TIME__", "__DATE__
 
 # MAKE_BL macro defines the targets and options to build each BL image.
 # Arguments:
@@ -315,7 +319,7 @@
 
 $(ELF): $(OBJS) $(LINKERFILE)
 	@echo "  LD      $$@"
-	@echo 'const char build_message[] = "Built : "__TIME__", "__DATE__; \
+	@echo 'const char build_message[] = "Built : "$(BUILD_MESSAGE_TIMESTAMP); \
 	       const char version_string[] = "${VERSION_STRING}";' | \
 		$$(CC) $$(CFLAGS) -xc - -o $(BUILD_DIR)/build_message.o
 	$$(Q)$$(LD) -o $$@ $$(LDFLAGS) -Map=$(MAPFILE) --script $(LINKERFILE) \
diff --git a/plat/arm/board/common/board_common.mk b/plat/arm/board/common/board_common.mk
index 9e0c848..da6beec 100644
--- a/plat/arm/board/common/board_common.mk
+++ b/plat/arm/board/common/board_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2016, 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:
@@ -54,3 +54,10 @@
     BL1_SOURCES		+=	plat/arm/board/common/board_arm_trusted_boot.c
     BL2_SOURCES		+=	plat/arm/board/common/board_arm_trusted_boot.c
 endif
+
+# This flag controls whether memory usage needs to be optimised
+ARM_BOARD_OPTIMISE_MMAP	?=	0
+
+# Process flags
+$(eval $(call assert_boolean,ARM_BOARD_OPTIMISE_MMAP))
+$(eval $(call add_define,ARM_BOARD_OPTIMISE_MMAP))
diff --git a/plat/arm/board/fvp/aarch64/fvp_common.c b/plat/arm/board/fvp/aarch64/fvp_common.c
index 305505d..f684d97 100644
--- a/plat/arm/board/fvp/aarch64/fvp_common.c
+++ b/plat/arm/board/fvp/aarch64/fvp_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -30,7 +30,6 @@
 
 #include <arm_config.h>
 #include <arm_def.h>
-#include <cci.h>
 #include <debug.h>
 #include <gicv2.h>
 #include <mmio.h>
@@ -50,9 +49,9 @@
 /*******************************************************************************
  * arm_config holds the characteristics of the differences between the three FVP
  * platforms (Base, A53_A57 & Foundation). It will be populated during cold boot
- * at each boot stage by the primary before enabling the MMU (to allow cci
- * configuration) & used thereafter. Each BL will have its own copy to allow
- * independent operation.
+ * at each boot stage by the primary before enabling the MMU (to allow
+ * interconnect configuration) & used thereafter. Each BL will have its own copy
+ * to allow independent operation.
  ******************************************************************************/
 arm_config_t arm_config;
 
@@ -209,7 +208,7 @@
 		break;
 	case HBI_BASE_FVP:
 		arm_config.flags |= ARM_CONFIG_BASE_MMAP |
-			ARM_CONFIG_HAS_CCI | ARM_CONFIG_HAS_TZC;
+			ARM_CONFIG_HAS_INTERCONNECT | ARM_CONFIG_HAS_TZC;
 
 		/*
 		 * Check for supported revisions
@@ -230,23 +229,20 @@
 }
 
 
-void fvp_cci_init(void)
+void fvp_interconnect_init(void)
 {
-	/*
-	 * Initialize CCI-400 driver
-	 */
-	if (arm_config.flags & ARM_CONFIG_HAS_CCI)
-		arm_cci_init();
+	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT)
+		plat_arm_interconnect_init();
 }
 
-void fvp_cci_enable(void)
+void fvp_interconnect_enable(void)
 {
-	if (arm_config.flags & ARM_CONFIG_HAS_CCI)
-		cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT)
+		plat_arm_interconnect_enter_coherency();
 }
 
-void fvp_cci_disable(void)
+void fvp_interconnect_disable(void)
 {
-	if (arm_config.flags & ARM_CONFIG_HAS_CCI)
-		cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT)
+		plat_arm_interconnect_exit_coherency();
 }
diff --git a/plat/arm/board/fvp/aarch64/fvp_helpers.S b/plat/arm/board/fvp/aarch64/fvp_helpers.S
index fe7358f..884fee8 100644
--- a/plat/arm/board/fvp/aarch64/fvp_helpers.S
+++ b/plat/arm/board/fvp/aarch64/fvp_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -111,8 +111,7 @@
 	 */
 	dsb	sy
 	wfi
-cb_panic:
-	b	cb_panic
+	bl	plat_panic_handler
 #else
 	mov_imm	x0, PLAT_ARM_TRUSTED_MAILBOX_BASE
 
@@ -174,7 +173,7 @@
 	 */
 	mov_imm	x0, PLAT_ARM_TRUSTED_MAILBOX_BASE
 	ldr	x0, [x0]
-	cbz	x0, _panic
+	cbz	x0, _panic_handler
 	ret
 
 	/* ---------------------------------------------------------------------
@@ -182,8 +181,8 @@
 	 * is empty. This should never happen!
 	 * ---------------------------------------------------------------------
 	 */
-_panic:
-	b	_panic
+_panic_handler:
+	bl	plat_panic_handler
 endfunc plat_get_my_entrypoint
 
 	/* -----------------------------------------------------
diff --git a/plat/arm/board/fvp/fvp_bl1_setup.c b/plat/arm/board/fvp/fvp_bl1_setup.c
index 91bc9c4..cc7feae 100644
--- a/plat/arm/board/fvp/fvp_bl1_setup.c
+++ b/plat/arm/board/fvp/fvp_bl1_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -44,14 +44,14 @@
 	fvp_config_setup();
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
+	 * Initialize Interconnect for this cluster during cold boot.
 	 * No need for locks as no other CPU is active.
 	 */
-	fvp_cci_init();
+	fvp_interconnect_init();
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable coherency in Interconnect for the primary CPU's cluster.
 	 */
-	fvp_cci_enable();
+	fvp_interconnect_enable();
 }
 
 /*******************************************************************************
diff --git a/plat/arm/board/fvp/fvp_bl31_setup.c b/plat/arm/board/fvp/fvp_bl31_setup.c
index f29af64..2ee3ba5 100644
--- a/plat/arm/board/fvp/fvp_bl31_setup.c
+++ b/plat/arm/board/fvp/fvp_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -41,17 +41,17 @@
 	fvp_config_setup();
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
-	 * No need for locks as no other CPU is active.
+	 * Initialize the correct interconnect for this cluster during cold
+	 * boot. No need for locks as no other CPU is active.
 	 */
-	fvp_cci_init();
+	fvp_interconnect_init();
 
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable coherency in interconnect for the primary CPU's cluster.
 	 * Earlier bootloader stages might already do this (e.g. Trusted
 	 * Firmware's BL1 does it) but we can't assume so. There is no harm in
 	 * executing this code twice anyway.
 	 * FVP PSCI code will enable coherency for other clusters.
 	 */
-	fvp_cci_enable();
+	fvp_interconnect_enable();
 }
diff --git a/plat/arm/board/fvp/fvp_def.h b/plat/arm/board/fvp/fvp_def.h
index 41b872a..dbca280 100644
--- a/plat/arm/board/fvp/fvp_def.h
+++ b/plat/arm/board/fvp/fvp_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -33,7 +33,9 @@
 
 #include <arm_def.h>
 
-
+#ifndef FVP_CLUSTER_COUNT
+#define FVP_CLUSTER_COUNT		2
+#endif
 #define FVP_MAX_CPUS_PER_CLUSTER	4
 
 #define FVP_PRIMARY_CPU			0x0
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index f959fab..3976ef2 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -86,7 +86,7 @@
 	uint64_t mpidr = read_mpidr_el1();
 
 	/* Disable coherency if this cluster is to be turned off */
-	fvp_cci_disable();
+	fvp_interconnect_disable();
 
 	/* Program the power controller to turn the cluster off */
 	fvp_pwrc_write_pcoffr(mpidr);
@@ -117,7 +117,7 @@
 		fvp_pwrc_write_pponr(mpidr);
 
 		/* Enable coherency if this cluster was off */
-		fvp_cci_enable();
+		fvp_interconnect_enable();
 	}
 
 	/*
diff --git a/plat/arm/board/fvp/fvp_private.h b/plat/arm/board/fvp/fvp_private.h
index e88a45e..bb115e1 100644
--- a/plat/arm/board/fvp/fvp_private.h
+++ b/plat/arm/board/fvp/fvp_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -39,9 +39,9 @@
 
 void fvp_config_setup(void);
 
-void fvp_cci_init(void);
-void fvp_cci_enable(void);
-void fvp_cci_disable(void);
+void fvp_interconnect_init(void);
+void fvp_interconnect_enable(void);
+void fvp_interconnect_disable(void);
 
 
 #endif /* __FVP_PRIVATE_H__ */
diff --git a/plat/arm/board/fvp/fvp_topology.c b/plat/arm/board/fvp/fvp_topology.c
index a212eda..741aad6 100644
--- a/plat/arm/board/fvp/fvp_topology.c
+++ b/plat/arm/board/fvp/fvp_topology.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -29,27 +29,47 @@
  */
 
 #include <arch.h>
+#include <cassert.h>
 #include <plat_arm.h>
 #include <platform_def.h>
 #include "drivers/pwrc/fvp_pwrc.h"
 
-/*
- * The FVP power domain tree does not have a single system level power domain
- * i.e. a single root node. The first entry in the power domain descriptor
- * specifies the number of power domains at the highest power level. For the FVP
- * this is 2 i.e. the number of cluster power domains.
- */
-#define FVP_PWR_DOMAINS_AT_MAX_PWR_LVL	ARM_CLUSTER_COUNT
-
 /* The FVP power domain tree descriptor */
-const unsigned char arm_power_domain_tree_desc[] = {
-	/* No of root nodes */
-	FVP_PWR_DOMAINS_AT_MAX_PWR_LVL,
-	/* No of children for the first node */
-	PLAT_ARM_CLUSTER0_CORE_COUNT,
-	/* No of children for the second node */
-	PLAT_ARM_CLUSTER1_CORE_COUNT
-};
+unsigned char fvp_power_domain_tree_desc[FVP_CLUSTER_COUNT + 1];
+
+
+CASSERT(FVP_CLUSTER_COUNT && FVP_CLUSTER_COUNT <= 256, assert_invalid_fvp_cluster_count);
+
+/*******************************************************************************
+ * This function dynamically constructs the topology according to
+ * FVP_CLUSTER_COUNT and returns it.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	int i;
+
+	/*
+	 * The FVP power domain tree does not have a single system level power domain
+	 * i.e. a single root node. The first entry in the power domain descriptor
+	 * specifies the number of power domains at the highest power level. For the FVP
+	 * this is the number of cluster power domains.
+	 */
+	fvp_power_domain_tree_desc[0] = FVP_CLUSTER_COUNT;
+
+	for (i = 0; i < FVP_CLUSTER_COUNT; i++)
+		fvp_power_domain_tree_desc[i + 1] = FVP_MAX_CPUS_PER_CLUSTER;
+
+	return fvp_power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function returns the core count within the cluster corresponding to
+ * `mpidr`.
+ ******************************************************************************/
+unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr)
+{
+	return FVP_MAX_CPUS_PER_CLUSTER;
+}
 
 /*******************************************************************************
  * This function implements a part of the critical interface between the psci
diff --git a/plat/arm/board/fvp/include/plat_macros.S b/plat/arm/board/fvp/include/plat_macros.S
index 2117843..df66a52 100644
--- a/plat/arm/board/fvp/include/plat_macros.S
+++ b/plat/arm/board/fvp/include/plat_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -31,6 +31,7 @@
 #define __PLAT_MACROS_S__
 
 #include <arm_macros.S>
+#include <cci_macros.S>
 #include <v2m_def.h>
 #include "../fvp_def.h"
 
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 0d671dc..a8267de 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -39,9 +39,10 @@
 #include "../fvp_def.h"
 
 /* Required platform porting definitions */
-#define PLAT_NUM_PWR_DOMAINS		(ARM_CLUSTER_COUNT + \
+#define PLAT_NUM_PWR_DOMAINS		(FVP_CLUSTER_COUNT + \
 					PLATFORM_CORE_COUNT)
 #define PLAT_MAX_PWR_LVL		ARM_PWR_LVL1
+#define PLATFORM_CORE_COUNT		(FVP_CLUSTER_COUNT * FVP_MAX_CPUS_PER_CLUSTER)
 
 /*
  * Other platform porting definitions are provided by included headers
@@ -50,8 +51,7 @@
 /*
  * Required ARM standard platform porting definitions
  */
-#define PLAT_ARM_CLUSTER0_CORE_COUNT	4
-#define PLAT_ARM_CLUSTER1_CORE_COUNT	4
+#define PLAT_ARM_CLUSTER_COUNT		FVP_CLUSTER_COUNT
 
 #define PLAT_ARM_TRUSTED_ROM_BASE	0x00000000
 #define PLAT_ARM_TRUSTED_ROM_SIZE	0x04000000	/* 64 MB */
@@ -64,8 +64,6 @@
 
 #define PLAT_ARM_DRAM2_SIZE		MAKE_ULL(0x780000000)
 
-#define PLAT_ARM_SHARED_RAM_CACHED	1
-
 /*
  * Load address of BL33 for this platform port
  */
@@ -144,4 +142,30 @@
 
 #define PLAT_ARM_G0_IRQS		ARM_G0_IRQS
 
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x9000
+#else
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x6000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
+#else
+# define PLAT_ARM_MAX_BL2_SIZE		0xC000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL31_SIZE is calculated using the current BL31 debug size plus a
+ * little space for growth.
+ */
+#define PLAT_ARM_MAX_BL31_SIZE		0x1D000
+
 #endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index ed4c898..aad2e2e 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -34,6 +34,11 @@
 # The FVP platform depends on this macro to build with correct GIC driver.
 $(eval $(call add_define,FVP_USE_GIC_DRIVER))
 
+# If FVP_CLUSTER_COUNT has been defined, pass it into the build system.
+ifdef FVP_CLUSTER_COUNT
+$(eval $(call add_define,FVP_CLUSTER_COUNT))
+endif
+
 # Choose the GIC sources depending upon the how the FVP will be invoked
 ifeq (${FVP_USE_GIC_DRIVER}, FVP_GICV3)
 FVP_GIC_SOURCES		:=	drivers/arm/gic/common/gic_common.c	\
@@ -57,6 +62,14 @@
 $(error "Incorrect GIC driver chosen on FVP port")
 endif
 
+FVP_INTERCONNECT_SOURCES	:= 	drivers/arm/cci/cci.c		\
+					plat/arm/common/arm_cci.c
+
+FVP_SECURITY_SOURCES	:=	drivers/arm/tzc400/tzc400.c		\
+				plat/arm/board/fvp/fvp_security.c	\
+				plat/arm/common/arm_tzc400.c
+
+
 PLAT_INCLUDES		:=	-Iplat/arm/board/fvp/include
 
 
@@ -69,13 +82,15 @@
 				lib/cpus/aarch64/cortex_a72.S
 
 BL1_SOURCES		+=	drivers/io/io_semihosting.c			\
-				${FVP_CPU_LIBS}					\
 				lib/semihosting/semihosting.c			\
 				lib/semihosting/aarch64/semihosting_call.S	\
 				plat/arm/board/fvp/aarch64/fvp_helpers.S	\
 				plat/arm/board/fvp/fvp_bl1_setup.c		\
 				plat/arm/board/fvp/fvp_err.c			\
-				plat/arm/board/fvp/fvp_io_storage.c
+				plat/arm/board/fvp/fvp_io_storage.c		\
+				${FVP_CPU_LIBS}					\
+				${FVP_INTERCONNECT_SOURCES}
+
 
 BL2_SOURCES		+=	drivers/arm/sp804/sp804_delay_timer.c		\
 				drivers/io/io_semihosting.c			\
@@ -85,19 +100,20 @@
 				plat/arm/board/fvp/fvp_bl2_setup.c		\
 				plat/arm/board/fvp/fvp_err.c			\
 				plat/arm/board/fvp/fvp_io_storage.c		\
-				plat/arm/board/fvp/fvp_security.c
+				${FVP_SECURITY_SOURCES}
 
 BL2U_SOURCES		+=	plat/arm/board/fvp/fvp_bl2u_setup.c		\
-				plat/arm/board/fvp/fvp_security.c
+				${FVP_SECURITY_SOURCES}
 
-BL31_SOURCES		+=	${FVP_CPU_LIBS}					\
-				plat/arm/board/fvp/fvp_bl31_setup.c		\
+BL31_SOURCES		+=	plat/arm/board/fvp/fvp_bl31_setup.c		\
 				plat/arm/board/fvp/fvp_pm.c			\
-				plat/arm/board/fvp/fvp_security.c		\
 				plat/arm/board/fvp/fvp_topology.c		\
 				plat/arm/board/fvp/aarch64/fvp_helpers.S	\
 				plat/arm/board/fvp/drivers/pwrc/fvp_pwrc.c	\
-				${FVP_GIC_SOURCES}
+				${FVP_CPU_LIBS}					\
+				${FVP_GIC_SOURCES}				\
+				${FVP_INTERCONNECT_SOURCES}			\
+				${FVP_SECURITY_SOURCES}
 
 # Disable the PSCI platform compatibility layer
 ENABLE_PLAT_COMPAT	:= 	0
diff --git a/plat/arm/board/juno/aarch64/juno_helpers.S b/plat/arm/board/juno/aarch64/juno_helpers.S
index 1931535..377b0cb 100644
--- a/plat/arm/board/juno/aarch64/juno_helpers.S
+++ b/plat/arm/board/juno/aarch64/juno_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -201,8 +201,7 @@
 	JUMP_TO_HANDLER_IF_JUNO_R(2)
 
 	/* Board revision is not supported */
-not_supported:
-	b	not_supported
+	bl	plat_panic_handler
 
 endfunc plat_reset_handler
 
diff --git a/plat/arm/board/juno/include/plat_macros.S b/plat/arm/board/juno/include/plat_macros.S
index db0c1d2..d2a88ed 100644
--- a/plat/arm/board/juno/include/plat_macros.S
+++ b/plat/arm/board/juno/include/plat_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -30,6 +30,7 @@
 #ifndef __PLAT_MACROS_S__
 #define __PLAT_MACROS_S__
 
+#include <cci_macros.S>
 #include <css_macros.S>
 
 /*
diff --git a/plat/arm/board/juno/include/platform_def.h b/plat/arm/board/juno/include/platform_def.h
index 924eb0a..a2cf036 100644
--- a/plat/arm/board/juno/include/platform_def.h
+++ b/plat/arm/board/juno/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -41,11 +41,15 @@
 #include <v2m_def.h>
 #include "../juno_def.h"
 
+/* Required platform porting definitions */
 /* Juno supports system power domain */
 #define PLAT_MAX_PWR_LVL		ARM_PWR_LVL2
 #define PLAT_NUM_PWR_DOMAINS		(ARM_SYSTEM_COUNT + \
-					ARM_CLUSTER_COUNT + \
+					JUNO_CLUSTER_COUNT + \
 					PLATFORM_CORE_COUNT)
+#define PLATFORM_CORE_COUNT		(JUNO_CLUSTER0_CORE_COUNT + \
+					JUNO_CLUSTER1_CORE_COUNT)
+
 /*
  * Other platform porting definitions are provided by included headers
  */
@@ -53,8 +57,7 @@
 /*
  * Required ARM standard platform porting definitions
  */
-#define PLAT_ARM_CLUSTER0_CORE_COUNT	2
-#define PLAT_ARM_CLUSTER1_CORE_COUNT	4
+#define PLAT_ARM_CLUSTER_COUNT		JUNO_CLUSTER_COUNT
 
 /* Use the bypass address */
 #define PLAT_ARM_TRUSTED_ROM_BASE	V2M_FLASH0_BASE + BL1_ROM_BYPASS_OFFSET
@@ -70,13 +73,52 @@
 #define PLAT_ARM_TRUSTED_ROM_SIZE	0x00010000
 #endif /* TRUSTED_BOARD_BOOT */
 
+/*
+ * If ARM_BOARD_OPTIMISE_MMAP=0 then Juno uses the default, unoptimised values
+ * defined for ARM development platforms.
+ */
+#if ARM_BOARD_OPTIMISE_MMAP
+/*
+ * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
+ * plat_arm_mmap array defined for each BL stage.
+ */
+#if IMAGE_BL1
+# define PLAT_ARM_MMAP_ENTRIES		7
+# define MAX_XLAT_TABLES		4
+#endif
+
+#if IMAGE_BL2
+# define PLAT_ARM_MMAP_ENTRIES		8
+# define MAX_XLAT_TABLES		3
+#endif
+
+#if IMAGE_BL2U
+# define PLAT_ARM_MMAP_ENTRIES		4
+# define MAX_XLAT_TABLES		3
+#endif
+
+#if IMAGE_BL31
+# define PLAT_ARM_MMAP_ENTRIES		5
+# define MAX_XLAT_TABLES		2
+#endif
+
+#if IMAGE_BL32
+# define PLAT_ARM_MMAP_ENTRIES		4
+# define MAX_XLAT_TABLES		3
+#endif
+
+#endif /* ARM_BOARD_OPTIMISE_MMAP */
 
 /* CCI related constants */
 #define PLAT_ARM_CCI_BASE		0x2c090000
 #define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	4
 #define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	3
 
+/* System timer related constants */
+#define PLAT_ARM_NSTIMER_FRAME_ID		1
+
 /* TZC related constants */
+#define PLAT_ARM_TZC_BASE		0x2a4a0000
 #define PLAT_ARM_TZC_NS_DEV_ACCESS	(				\
 		TZC_REGION_ACCESS_RDWR(TZC400_NSAID_CCI400)	|	\
 		TZC_REGION_ACCESS_RDWR(TZC400_NSAID_PCIE)	|	\
@@ -99,6 +141,23 @@
 #define PLAT_ARM_GICH_BASE		0x2c04f000
 #define PLAT_ARM_GICV_BASE		0x2c06f000
 
+/* MHU related constants */
+#define PLAT_CSS_MHU_BASE		0x2b1f0000
+
+/*
+ * Base address of the first memory region used for communication between AP
+ * and SCP. Used by the BOM and SCPI protocols.
+ *
+ * Note that this is located at the same address as SCP_BOOT_CFG_ADDR, which
+ * means the SCP/AP configuration data gets overwritten when the AP initiates
+ * communication with the SCP. The configuration data is expected to be a
+ * 32-bit word on all CSS platforms. On Juno, part of this configuration is
+ * which CPU is the primary, according to the shift and mask definitions below.
+ */
+#define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	(ARM_TRUSTED_SRAM_BASE + 0x80)
+#define PLAT_CSS_PRIMARY_CPU_SHIFT		8
+#define PLAT_CSS_PRIMARY_CPU_BIT_WIDTH		4
+
 /*
  * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
  * terminology. On a GICv2 system or mode, the lists will be merged and treated
@@ -124,5 +183,30 @@
 /* CSS SoC NIC-400 Global Programmers View (GPV) */
 #define PLAT_SOC_CSS_NIC400_BASE	0x2a000000
 
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x9000
+#else
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x6000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
+#else
+# define PLAT_ARM_MAX_BL2_SIZE		0xC000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL31_SIZE is calculated using the current BL31 debug size plus a
+ * little space for growth.
+ */
+#define PLAT_ARM_MAX_BL31_SIZE		0x1D000
 
 #endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/arm/board/juno/juno_def.h b/plat/arm/board/juno/juno_def.h
index 1f367f2..f27bbb2 100644
--- a/plat/arm/board/juno/juno_def.h
+++ b/plat/arm/board/juno/juno_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -50,6 +50,14 @@
 #define PSRAM_BASE			0x14000000
 #define PSRAM_SIZE			0x02000000
 
+#define JUNO_SSC_VER_PART_NUM		0x030
+
+/*******************************************************************************
+ * Juno topology related constants
+ ******************************************************************************/
+#define JUNO_CLUSTER_COUNT		2
+#define JUNO_CLUSTER0_CORE_COUNT	2
+#define JUNO_CLUSTER1_CORE_COUNT	4
 
 /*******************************************************************************
  * TZC-400 related constants
diff --git a/plat/arm/board/juno/juno_topology.c b/plat/arm/board/juno/juno_topology.c
new file mode 100644
index 0000000..ee4ec44
--- /dev/null
+++ b/plat/arm/board/juno/juno_topology.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arm_def.h>
+#include <plat_arm.h>
+#include "juno_def.h"
+
+/*
+ * On Juno, the system power level is the highest power level.
+ * The first entry in the power domain descriptor specifies the
+ * number of system power domains i.e. 1.
+ */
+#define JUNO_PWR_DOMAINS_AT_MAX_PWR_LVL	 ARM_SYSTEM_COUNT
+
+/*
+ * The Juno power domain tree descriptor. The cluster power domains
+ * are arranged so that when the PSCI generic code creates the power
+ * domain tree, the indices of the CPU power domain nodes it allocates
+ * match the linear indices returned by plat_core_pos_by_mpidr()
+ * i.e. CLUSTER1 CPUs are allocated indices from 0 to 3 and the higher
+ * indices for CLUSTER0 CPUs.
+ */
+const unsigned char juno_power_domain_tree_desc[] = {
+	/* No of root nodes */
+	JUNO_PWR_DOMAINS_AT_MAX_PWR_LVL,
+	/* No of children for the root node */
+	JUNO_CLUSTER_COUNT,
+	/* No of children for the first cluster node */
+	JUNO_CLUSTER1_CORE_COUNT,
+	/* No of children for the second cluster node */
+	JUNO_CLUSTER0_CORE_COUNT
+};
+
+/*******************************************************************************
+ * This function returns the Juno topology tree information.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return juno_power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function returns the core count within the cluster corresponding to
+ * `mpidr`.
+ ******************************************************************************/
+unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr)
+{
+	return (((mpidr) & 0x100) ? JUNO_CLUSTER1_CORE_COUNT :\
+				JUNO_CLUSTER0_CORE_COUNT);
+}
diff --git a/plat/arm/board/juno/platform.mk b/plat/arm/board/juno/platform.mk
index fae30e7..0a2244d 100644
--- a/plat/arm/board/juno/platform.mk
+++ b/plat/arm/board/juno/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2016, 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:
@@ -34,6 +34,14 @@
 				plat/common/plat_gicv2.c		\
 				plat/arm/common/arm_gicv2.c
 
+JUNO_INTERCONNECT_SOURCES	:=	drivers/arm/cci/cci.c		\
+					plat/arm/common/arm_cci.c
+
+JUNO_SECURITY_SOURCES	:=	drivers/arm/tzc400/tzc400.c		\
+				plat/arm/board/juno/juno_security.c	\
+				plat/arm/common/arm_tzc400.c
+
+
 PLAT_INCLUDES		:=	-Iplat/arm/board/juno/include
 
 PLAT_BL_COMMON_SOURCES	:=	plat/arm/board/juno/aarch64/juno_helpers.S
@@ -42,30 +50,36 @@
 				lib/cpus/aarch64/cortex_a57.S		\
 				lib/cpus/aarch64/cortex_a72.S		\
 				plat/arm/board/juno/juno_bl1_setup.c	\
-				plat/arm/board/juno/juno_err.c
+				plat/arm/board/juno/juno_err.c		\
+				${JUNO_INTERCONNECT_SOURCES}
 
-BL2_SOURCES		+=	plat/arm/board/juno/juno_security.c	\
-				plat/arm/board/juno/juno_err.c
+BL2_SOURCES		+=	plat/arm/board/juno/juno_err.c		\
+				${JUNO_SECURITY_SOURCES}
 
-BL2U_SOURCES		+=	plat/arm/board/juno/juno_security.c
+BL2U_SOURCES		+=	${JUNO_SECURITY_SOURCES}
 
 BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a53.S		\
 				lib/cpus/aarch64/cortex_a57.S		\
 				lib/cpus/aarch64/cortex_a72.S		\
 				plat/arm/board/juno/juno_pm.c		\
-				plat/arm/board/juno/juno_security.c	\
-				${JUNO_GIC_SOURCES}
+				plat/arm/board/juno/juno_topology.c	\
+				${JUNO_GIC_SOURCES}			\
+				${JUNO_INTERCONNECT_SOURCES}		\
+				${JUNO_SECURITY_SOURCES}
 
 # Enable workarounds for selected Cortex-A57 erratas.
-ERRATA_A57_806969	:=	0
-ERRATA_A57_813420	:=	1
+ERRATA_A57_806969		:=	0
+ERRATA_A57_813420		:=	1
 
 # Enable option to skip L1 data cache flush during the Cortex-A57 cluster
 # power down sequence
 SKIP_A57_L1_FLUSH_PWR_DWN	:=	 1
 
 # Disable the PSCI platform compatibility layer
-ENABLE_PLAT_COMPAT	:= 	0
+ENABLE_PLAT_COMPAT		:= 	0
+
+# Enable memory map related constants optimisation
+ARM_BOARD_OPTIMISE_MMAP		:=	1
 
 include plat/arm/board/common/board_css.mk
 include plat/arm/common/arm_common.mk
diff --git a/plat/arm/board/juno/tsp/tsp-juno.mk b/plat/arm/board/juno/tsp/tsp-juno.mk
index 2ef964e..4e80607 100644
--- a/plat/arm/board/juno/tsp/tsp-juno.mk
+++ b/plat/arm/board/juno/tsp/tsp-juno.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2014-2016, 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,7 +28,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
-BL32_SOURCES		+=	plat/arm/css/common/css_topology.c	\
+BL32_SOURCES		+=	plat/arm/board/juno/juno_topology.c	\
+				plat/arm/css/common/css_topology.c	\
 				${JUNO_GIC_SOURCES}
 
 include plat/arm/common/tsp/arm_tsp.mk
diff --git a/plat/arm/common/aarch64/arm_common.c b/plat/arm/common/aarch64/arm_common.c
index a211f16..c84a65b 100644
--- a/plat/arm/common/aarch64/arm_common.c
+++ b/plat/arm/common/aarch64/arm_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -29,20 +29,16 @@
  */
 #include <arch.h>
 #include <arch_helpers.h>
-#include <cci.h>
 #include <mmio.h>
 #include <plat_arm.h>
 #include <platform_def.h>
 #include <xlat_tables.h>
 
-
-static const int cci_map[] = {
-	PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX,
-	PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX
-};
+extern const mmap_region_t plat_arm_mmap[];
 
 /* Weak definitions may be overridden in specific ARM standard platform */
 #pragma weak plat_get_ns_image_entrypoint
+#pragma weak plat_arm_get_mmap
 
 
 /*******************************************************************************
@@ -67,7 +63,7 @@
 		mmap_add_region(coh_start, coh_start,			\
 				coh_limit - coh_start,			\
 				MT_DEVICE | MT_RW | MT_SECURE);		\
-		mmap_add(plat_arm_mmap);				\
+		mmap_add(plat_arm_get_mmap());				\
 		init_xlat_tables();					\
 									\
 		enable_mmu_el##_el(0);					\
@@ -85,7 +81,7 @@
 		mmap_add_region(ro_start, ro_start,			\
 				ro_limit - ro_start,			\
 				MT_MEMORY | MT_RO | MT_SECURE);		\
-		mmap_add(plat_arm_mmap);				\
+		mmap_add(plat_arm_get_mmap());				\
 		init_xlat_tables();					\
 									\
 		enable_mmu_el##_el(0);					\
@@ -138,12 +134,6 @@
 	return spsr;
 }
 
-
-void arm_cci_init(void)
-{
-	cci_init(PLAT_ARM_CCI_BASE, cci_map, ARRAY_SIZE(cci_map));
-}
-
 /*******************************************************************************
  * Configures access to the system counter timer module.
  ******************************************************************************/
@@ -161,3 +151,11 @@
 	reg_val = (1 << CNTNSAR_NS_SHIFT(PLAT_ARM_NSTIMER_FRAME_ID));
 	mmio_write_32(ARM_SYS_TIMCTL_BASE + CNTNSAR, reg_val);
 }
+
+/*******************************************************************************
+ * Returns ARM platform specific memory map regions.
+ ******************************************************************************/
+const mmap_region_t *plat_arm_get_mmap(void)
+{
+	return plat_arm_mmap;
+}
diff --git a/plat/arm/common/arm_bl1_fwu.c b/plat/arm/common/arm_bl1_fwu.c
index 9a0d93a..2a18d34 100644
--- a/plat/arm/common/arm_bl1_fwu.c
+++ b/plat/arm/common/arm_bl1_fwu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -33,11 +33,12 @@
 #include <debug.h>
 #include <errno.h>
 #include <plat_arm.h>
+#include <platform_def.h>
 #include <tbbr_img_desc.h>
 
 
 /* Struct to keep track of usable memory */
-typedef struct bl1_mem_info{
+typedef struct bl1_mem_info {
 	uintptr_t mem_base;
 	unsigned int mem_size;
 } bl1_mem_info_t;
@@ -58,8 +59,8 @@
 		.mem_size = ARM_NS_DRAM1_SIZE
 	},
 	{
-		.mem_base = V2M_FLASH0_BASE,
-		.mem_size = V2M_FLASH0_SIZE
+		.mem_base = PLAT_ARM_NVM_BASE,
+		.mem_size = PLAT_ARM_NVM_SIZE
 	},
 	{
 		.mem_size = 0
@@ -79,7 +80,7 @@
 	/*
 	 * Check the given image source and size.
 	 */
-	if (GET_SEC_STATE(flags) == SECURE)
+	if (GET_SECURITY_STATE(flags) == SECURE)
 		mmap = fwu_addr_map_secure;
 	else
 		mmap = fwu_addr_map_non_secure;
diff --git a/plat/arm/common/arm_bl1_setup.c b/plat/arm/common/arm_bl1_setup.c
index d0a4c0b..951f48a 100644
--- a/plat/arm/common/arm_bl1_setup.c
+++ b/plat/arm/common/arm_bl1_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -31,7 +31,6 @@
 #include <arch.h>
 #include <arm_def.h>
 #include <bl_common.h>
-#include <cci.h>
 #include <console.h>
 #include <platform_def.h>
 #include <plat_arm.h>
@@ -101,14 +100,14 @@
 	arm_bl1_early_platform_setup();
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
+	 * Initialize Interconnect for this cluster during cold boot.
 	 * No need for locks as no other CPU is active.
 	 */
-	arm_cci_init();
+	plat_arm_interconnect_init();
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable Interconnect coherency for the primary CPU's cluster.
 	 */
-	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	plat_arm_interconnect_enter_coherency();
 }
 
 /******************************************************************************
diff --git a/plat/arm/common/arm_bl31_setup.c b/plat/arm/common/arm_bl31_setup.c
index 6c58ff1..5cc8bfb 100644
--- a/plat/arm/common/arm_bl31_setup.c
+++ b/plat/arm/common/arm_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -33,7 +33,6 @@
 #include <arm_def.h>
 #include <assert.h>
 #include <bl_common.h>
-#include <cci.h>
 #include <console.h>
 #include <debug.h>
 #include <mmio.h>
@@ -178,20 +177,20 @@
 	arm_bl31_early_platform_setup(from_bl2, plat_params_from_bl2);
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
+	 * Initialize Interconnect for this cluster during cold boot.
 	 * No need for locks as no other CPU is active.
 	 */
-	arm_cci_init();
+	plat_arm_interconnect_init();
 
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable Interconnect coherency for the primary CPU's cluster.
 	 * Earlier bootloader stages might already do this (e.g. Trusted
 	 * Firmware's BL1 does it) but we can't assume so. There is no harm in
 	 * executing this code twice anyway.
 	 * Platform specific PSCI code will enable coherency for other
 	 * clusters.
 	 */
-	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	plat_arm_interconnect_enter_coherency();
 }
 
 /*******************************************************************************
diff --git a/plat/arm/common/arm_cci.c b/plat/arm/common/arm_cci.c
new file mode 100644
index 0000000..41054c2
--- /dev/null
+++ b/plat/arm/common/arm_cci.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <cci.h>
+#include <plat_arm.h>
+#include <platform_def.h>
+
+static const int cci_map[] = {
+	PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX,
+	PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX
+};
+
+/******************************************************************************
+ * The following functions are defined as weak to allow a platform to override
+ * the way ARM CCI driver is initialised and used.
+ *****************************************************************************/
+#pragma weak plat_arm_interconnect_init
+#pragma weak plat_arm_interconnect_enter_coherency
+#pragma weak plat_arm_interconnect_exit_coherency
+
+
+/******************************************************************************
+ * Helper function to initialize ARM CCI driver.
+ *****************************************************************************/
+void plat_arm_interconnect_init(void)
+{
+	cci_init(PLAT_ARM_CCI_BASE, cci_map, ARRAY_SIZE(cci_map));
+}
+
+/******************************************************************************
+ * Helper function to place current master into coherency
+ *****************************************************************************/
+void plat_arm_interconnect_enter_coherency(void)
+{
+	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
+
+/******************************************************************************
+ * Helper function to remove current master from coherency
+ *****************************************************************************/
+void plat_arm_interconnect_exit_coherency(void)
+{
+	cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
diff --git a/plat/arm/common/arm_ccn.c b/plat/arm/common/arm_ccn.c
new file mode 100644
index 0000000..5cb443a
--- /dev/null
+++ b/plat/arm/common/arm_ccn.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <ccn.h>
+#include <plat_arm.h>
+#include <platform_def.h>
+
+static const unsigned char master_to_rn_id_map[] = {
+	PLAT_ARM_CLUSTER_TO_CCN_ID_MAP
+};
+
+static const ccn_desc_t arm_ccn_desc = {
+	.periphbase = PLAT_ARM_CCN_BASE,
+	.num_masters = ARRAY_SIZE(master_to_rn_id_map),
+	.master_to_rn_id_map = master_to_rn_id_map
+};
+
+/******************************************************************************
+ * The following functions are defined as weak to allow a platform to override
+ * the way ARM CCN driver is initialised and used.
+ *****************************************************************************/
+#pragma weak plat_arm_interconnect_init
+#pragma weak plat_arm_interconnect_enter_coherency
+#pragma weak plat_arm_interconnect_exit_coherency
+
+
+/******************************************************************************
+ * Helper function to initialize ARM CCN driver.
+ *****************************************************************************/
+void plat_arm_interconnect_init(void)
+{
+	ccn_init(&arm_ccn_desc);
+}
+
+/******************************************************************************
+ * Helper function to place current master into coherency
+ *****************************************************************************/
+void plat_arm_interconnect_enter_coherency(void)
+{
+	ccn_enter_snoop_dvm_domain(1 << MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
+
+/******************************************************************************
+ * Helper function to remove current master from coherency
+ *****************************************************************************/
+void plat_arm_interconnect_exit_coherency(void)
+{
+	ccn_exit_snoop_dvm_domain(1 << MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 3202735..425e0d3 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2016, 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:
@@ -87,9 +87,7 @@
 				plat/arm/common/aarch64/arm_helpers.S		\
 				plat/common/aarch64/plat_common.c
 
-BL1_SOURCES		+=	drivers/arm/cci/cci.c				\
-				drivers/arm/ccn/ccn.c				\
-				drivers/arm/sp805/sp805.c			\
+BL1_SOURCES		+=	drivers/arm/sp805/sp805.c			\
 				drivers/io/io_fip.c				\
 				drivers/io/io_memmap.c				\
 				drivers/io/io_storage.c				\
@@ -102,26 +100,18 @@
 BL1_SOURCES		+=	plat/arm/common/arm_pm.c
 endif
 
-BL2_SOURCES		+=	drivers/arm/tzc400/tzc400.c			\
-				drivers/io/io_fip.c				\
+BL2_SOURCES		+=	drivers/io/io_fip.c				\
 				drivers/io/io_memmap.c				\
 				drivers/io/io_storage.c				\
 				plat/arm/common/arm_bl2_setup.c			\
 				plat/arm/common/arm_io_storage.c		\
-				plat/arm/common/arm_security.c			\
 				plat/common/aarch64/platform_up_stack.S
 
-BL2U_SOURCES		+=	drivers/arm/tzc400/tzc400.c			\
-				plat/arm/common/arm_bl2u_setup.c		\
-				plat/arm/common/arm_security.c			\
+BL2U_SOURCES		+=	plat/arm/common/arm_bl2u_setup.c		\
 				plat/common/aarch64/platform_up_stack.S
 
-BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
-				drivers/arm/ccn/ccn.c				\
-				drivers/arm/tzc400/tzc400.c			\
-				plat/arm/common/arm_bl31_setup.c		\
+BL31_SOURCES		+=	plat/arm/common/arm_bl31_setup.c		\
 				plat/arm/common/arm_pm.c			\
-				plat/arm/common/arm_security.c			\
 				plat/arm/common/arm_topology.c			\
 				plat/common/aarch64/platform_mp_stack.S		\
 				plat/common/aarch64/plat_psci_common.c
@@ -139,8 +129,8 @@
 
     PLAT_INCLUDES	+=	-Iinclude/bl1/tbbr
 
-    BL1_SOURCES		+=	${AUTH_SOURCES}			\
-				bl1/tbbr/tbbr_img_desc.c	\
+    BL1_SOURCES		+=	${AUTH_SOURCES}					\
+				bl1/tbbr/tbbr_img_desc.c			\
 				plat/arm/common/arm_bl1_fwu.c
 
     BL2_SOURCES		+=	${AUTH_SOURCES}
diff --git a/plat/arm/common/arm_pm.c b/plat/arm/common/arm_pm.c
index 2ddc583..1e756a9 100644
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_pm.c
@@ -192,11 +192,6 @@
 	assert((PLAT_ARM_TRUSTED_MAILBOX_BASE >= ARM_SHARED_RAM_BASE) &&
 		((PLAT_ARM_TRUSTED_MAILBOX_BASE + sizeof(*mailbox)) <= \
 				(ARM_SHARED_RAM_BASE + ARM_SHARED_RAM_SIZE)));
-
-	/* Flush data cache if the mail box shared RAM is cached */
-#if PLAT_ARM_SHARED_RAM_CACHED
-	flush_dcache_range((uintptr_t) mailbox, sizeof(*mailbox));
-#endif
 }
 
 /*******************************************************************************
diff --git a/plat/arm/common/arm_topology.c b/plat/arm/common/arm_topology.c
index cb0bb9c..4430b13 100644
--- a/plat/arm/common/arm_topology.c
+++ b/plat/arm/common/arm_topology.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -29,26 +29,9 @@
  */
 
 #include <arch.h>
-#include <psci.h>
 #include <plat_arm.h>
 #include <platform_def.h>
 
-#define get_arm_cluster_core_count(mpidr)\
-		(((mpidr) & 0x100) ? PLAT_ARM_CLUSTER1_CORE_COUNT :\
-				PLAT_ARM_CLUSTER0_CORE_COUNT)
-
-/* The power domain tree descriptor which need to be exported by ARM platforms */
-extern const unsigned char arm_power_domain_tree_desc[];
-
-
-/*******************************************************************************
- * This function returns the ARM default topology tree information.
- ******************************************************************************/
-const unsigned char *plat_get_power_domain_tree_desc(void)
-{
-	return arm_power_domain_tree_desc;
-}
-
 /*******************************************************************************
  * This function validates an MPIDR by checking whether it falls within the
  * acceptable bounds. An error code (-1) is returned if an incorrect mpidr
@@ -66,12 +49,12 @@
 	cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
 	cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
 
-	if (cluster_id >= ARM_CLUSTER_COUNT)
+	if (cluster_id >= PLAT_ARM_CLUSTER_COUNT)
 		return -1;
 
 	/* Validate cpu_id by checking whether it represents a CPU in
 	   one of the two clusters present on the platform. */
-	if (cpu_id >= get_arm_cluster_core_count(mpidr))
+	if (cpu_id >= plat_arm_get_cluster_core_count(mpidr))
 		return -1;
 
 	return 0;
diff --git a/plat/arm/common/arm_security.c b/plat/arm/common/arm_tzc400.c
similarity index 100%
rename from plat/arm/common/arm_security.c
rename to plat/arm/common/arm_tzc400.c
diff --git a/plat/arm/css/common/aarch64/css_helpers.S b/plat/arm/css/common/aarch64/css_helpers.S
index 2747618..0763a3e 100644
--- a/plat/arm/css/common/aarch64/css_helpers.S
+++ b/plat/arm/css/common/aarch64/css_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -117,7 +117,8 @@
 	bl	plat_my_core_pos
 	ldr	x1, =SCP_BOOT_CFG_ADDR
 	ldr	x1, [x1]
-	ubfx	x1, x1, #PRIMARY_CPU_SHIFT, #PRIMARY_CPU_BIT_WIDTH
+	ubfx	x1, x1, #PLAT_CSS_PRIMARY_CPU_SHIFT, \
+			#PLAT_CSS_PRIMARY_CPU_BIT_WIDTH
 	cmp	x0, x1
 	cset	w0, eq
 	ret	x9
diff --git a/plat/arm/css/common/css_common.mk b/plat/arm/css/common/css_common.mk
index 6a8773d..65e125e 100644
--- a/plat/arm/css/common/css_common.mk
+++ b/plat/arm/css/common/css_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2016, 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,6 +28,10 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
+
+# By default, SCP images are needed by CSS platforms.
+CSS_LOAD_SCP_IMAGES	?=	1
+
 PLAT_INCLUDES		+=	-Iinclude/plat/arm/css/common			\
 				-Iinclude/plat/arm/css/common/aarch64
 
@@ -38,12 +42,10 @@
 
 BL2_SOURCES		+=	plat/arm/css/common/css_bl2_setup.c		\
 				plat/arm/css/common/css_mhu.c			\
-				plat/arm/css/common/css_scp_bootloader.c	\
 				plat/arm/css/common/css_scpi.c
 
 BL2U_SOURCES		+=	plat/arm/css/common/css_bl2u_setup.c		\
 				plat/arm/css/common/css_mhu.c			\
-				plat/arm/css/common/css_scp_bootloader.c	\
 				plat/arm/css/common/css_scpi.c
 
 BL31_SOURCES		+=	plat/arm/css/common/css_mhu.c			\
@@ -51,17 +53,25 @@
 				plat/arm/css/common/css_scpi.c			\
 				plat/arm/css/common/css_topology.c
 
-ifneq (${TRUSTED_BOARD_BOOT},0)
-$(eval $(call FWU_FIP_ADD_IMG,SCP_BL2U,--scp-fwu-cfg))
-endif
 
 ifneq (${RESET_TO_BL31},0)
   $(error "Using BL31 as the reset vector is not supported on CSS platforms. \
   Please set RESET_TO_BL31 to 0.")
 endif
 
-# Subsystems require a SCP_BL2 image
-$(eval $(call FIP_ADD_IMG,SCP_BL2,--scp-fw))
+# Process CSS_LOAD_SCP_IMAGES flag
+$(eval $(call assert_boolean,CSS_LOAD_SCP_IMAGES))
+$(eval $(call add_define,CSS_LOAD_SCP_IMAGES))
+
+ifeq (${CSS_LOAD_SCP_IMAGES},1)
+  $(eval $(call FIP_ADD_IMG,SCP_BL2,--scp-fw))
+  ifneq (${TRUSTED_BOARD_BOOT},0)
+    $(eval $(call FWU_FIP_ADD_IMG,SCP_BL2U,--scp-fwu-cfg))
+  endif
+
+  BL2U_SOURCES		+=	plat/arm/css/common/css_scp_bootloader.c
+  BL2_SOURCES		+=	plat/arm/css/common/css_scp_bootloader.c
+endif
 
 # Enable option to detect whether the SCP ROM firmware in use predates version
 # 1.7.0 and therefore, is incompatible.
diff --git a/plat/arm/css/common/css_mhu.c b/plat/arm/css/common/css_mhu.c
index b1714e2..265d6c2 100644
--- a/plat/arm/css/common/css_mhu.c
+++ b/plat/arm/css/common/css_mhu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -33,6 +33,7 @@
 #include <bakery_lock.h>
 #include <css_def.h>
 #include <mmio.h>
+#include <platform_def.h>
 #include <plat_arm.h>
 #include "css_mhu.h"
 
@@ -66,24 +67,26 @@
 	arm_lock_get();
 
 	/* Make sure any previous command has finished */
-	while (mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) & (1 << slot_id))
+	while (mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) &
+							(1 << slot_id))
 		;
 }
 
 void mhu_secure_message_send(unsigned int slot_id)
 {
 	assert(slot_id <= MHU_MAX_SLOT_ID);
-	assert(!(mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) & (1 << slot_id)));
+	assert(!(mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) &
+							(1 << slot_id)));
 
 	/* Send command to SCP */
-	mmio_write_32(MHU_BASE + CPU_INTR_S_SET, 1 << slot_id);
+	mmio_write_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_SET, 1 << slot_id);
 }
 
 uint32_t mhu_secure_message_wait(void)
 {
 	/* Wait for response from SCP */
 	uint32_t response;
-	while (!(response = mmio_read_32(MHU_BASE + SCP_INTR_S_STAT)))
+	while (!(response = mmio_read_32(PLAT_CSS_MHU_BASE + SCP_INTR_S_STAT)))
 		;
 
 	return response;
@@ -97,7 +100,7 @@
 	 * Clear any response we got by writing one in the relevant slot bit to
 	 * the CLEAR register
 	 */
-	mmio_write_32(MHU_BASE + SCP_INTR_S_CLEAR, 1 << slot_id);
+	mmio_write_32(PLAT_CSS_MHU_BASE + SCP_INTR_S_CLEAR, 1 << slot_id);
 
 	arm_lock_release();
 }
@@ -111,7 +114,7 @@
 	 * as a stale or garbage value would make us think it's a message we've
 	 * already sent.
 	 */
-	assert(mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) == 0);
+	assert(mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) == 0);
 }
 
 void plat_arm_pwrc_setup(void)
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index 6d6646e..b6f94ac 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -31,7 +31,6 @@
 #include <arch_helpers.h>
 #include <assert.h>
 #include <cassert.h>
-#include <cci.h>
 #include <css_pm.h>
 #include <debug.h>
 #include <errno.h>
@@ -108,7 +107,7 @@
 	 * if this cluster was off.
 	 */
 	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
-		cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+		plat_arm_interconnect_enter_coherency();
 }
 
 /*******************************************************************************
@@ -153,7 +152,7 @@
 
 	/* Cluster is to be turned off, so disable coherency */
 	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF) {
-		cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+		plat_arm_interconnect_exit_coherency();
 		cluster_state = scpi_power_off;
 	}
 
diff --git a/plat/arm/css/common/css_scp_bootloader.c b/plat/arm/css/common/css_scp_bootloader.c
index c01f42f..d3f671e 100644
--- a/plat/arm/css/common/css_scp_bootloader.c
+++ b/plat/arm/css/common/css_scp_bootloader.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -60,7 +60,7 @@
  * Unlike the SCPI protocol, the boot protocol uses the same memory region
  * for both AP -> SCP and SCP -> AP transfers; define the address of this...
  */
-#define BOM_SHARED_MEM		SCP_COM_SHARED_MEM_BASE
+#define BOM_SHARED_MEM		PLAT_CSS_SCP_COM_SHARED_MEM_BASE
 #define BOM_CMD_HEADER		((bom_cmd_t *) BOM_SHARED_MEM)
 #define BOM_CMD_PAYLOAD		((void *) (BOM_SHARED_MEM + sizeof(bom_cmd_t)))
 
@@ -77,10 +77,10 @@
 
 static void scp_boot_message_send(size_t payload_size)
 {
-	/* Make sure payload can be seen by SCP */
-	if (MHU_PAYLOAD_CACHED)
-		flush_dcache_range(BOM_SHARED_MEM,
-				   sizeof(bom_cmd_t) + payload_size);
+	/* Ensure that any write to the BOM payload area is seen by SCP before
+	 * we write to the MHU register. If these 2 writes were reordered by
+	 * the CPU then SCP would read stale payload data */
+	dmbst();
 
 	/* Send command to SCP */
 	mhu_secure_message_send(BOM_MHU_SLOT_ID);
@@ -99,9 +99,10 @@
 		panic();
 	}
 
-	/* Make sure we see the reply from the SCP and not any stale data */
-	if (MHU_PAYLOAD_CACHED)
-		inv_dcache_range(BOM_SHARED_MEM, size);
+	/* Ensure that any read to the BOM payload area is done after reading
+	 * the MHU register. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data */
+	dmbld();
 
 	return *(uint32_t *) BOM_SHARED_MEM;
 }
diff --git a/plat/arm/css/common/css_scpi.c b/plat/arm/css/common/css_scpi.c
index 0a4eafe..02d573c 100644
--- a/plat/arm/css/common/css_scpi.c
+++ b/plat/arm/css/common/css_scpi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, 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:
@@ -37,8 +37,9 @@
 #include "css_mhu.h"
 #include "css_scpi.h"
 
-#define SCPI_SHARED_MEM_SCP_TO_AP	SCP_COM_SHARED_MEM_BASE
-#define SCPI_SHARED_MEM_AP_TO_SCP	(SCP_COM_SHARED_MEM_BASE + 0x100)
+#define SCPI_SHARED_MEM_SCP_TO_AP	PLAT_CSS_SCP_COM_SHARED_MEM_BASE
+#define SCPI_SHARED_MEM_AP_TO_SCP	(PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
+								 + 0x100)
 
 #define SCPI_CMD_HEADER_AP_TO_SCP		\
 	((scpi_cmd_t *) SCPI_SHARED_MEM_AP_TO_SCP)
@@ -55,10 +56,10 @@
 
 static void scpi_secure_message_send(size_t payload_size)
 {
-	/* Make sure payload can be seen by SCP */
-	if (MHU_PAYLOAD_CACHED)
-		flush_dcache_range(SCPI_SHARED_MEM_AP_TO_SCP,
-				   sizeof(scpi_cmd_t) + payload_size);
+	/* Ensure that any write to the SCPI payload area is seen by SCP before
+	 * we write to the MHU register. If these 2 writes were reordered by
+	 * the CPU then SCP would read stale payload data */
+	dmbst();
 
 	mhu_secure_message_send(SCPI_MHU_SLOT_ID);
 }
@@ -78,9 +79,10 @@
 		panic();
 	}
 
-	/* Make sure we don't read stale data */
-	if (MHU_PAYLOAD_CACHED)
-		inv_dcache_range(SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
+	/* Ensure that any read to the SCPI payload area is done after reading
+	 * the MHU register. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data */
+	dmbld();
 
 	memcpy(cmd, (void *) SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
 }
diff --git a/plat/arm/css/common/css_scpi.h b/plat/arm/css/common/css_scpi.h
index 379a821..4a601f3 100644
--- a/plat/arm/css/common/css_scpi.h
+++ b/plat/arm/css/common/css_scpi.h
@@ -45,7 +45,7 @@
 	uint32_t set		: 1;
 	/* Sender ID to match a reply. The value is sender specific. */
 	uint32_t sender		: 8;
-	/* Size of the payload in bytes (0 – 511) */
+	/* Size of the payload in bytes (0 - 511) */
 	uint32_t size		: 9;
 	uint32_t reserved	: 7;
 	/*
diff --git a/plat/arm/css/common/css_topology.c b/plat/arm/css/common/css_topology.c
index 03f81e6..d5f0275 100644
--- a/plat/arm/css/common/css_topology.c
+++ b/plat/arm/css/common/css_topology.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, 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:
@@ -30,33 +30,6 @@
 
 #include <plat_arm.h>
 
-/*
- * On ARM CSS platforms, by default, the system power level is treated as the
- * highest. The first entry in the power domain descriptor specifies the
- * number of system power domains i.e. 1.
- */
-#define CSS_PWR_DOMAINS_AT_MAX_PWR_LVL	ARM_SYSTEM_COUNT
-
-/*
- * The CSS power domain tree descriptor for dual cluster CSS platforms.
- * The cluster power domains are arranged so that when the PSCI generic
- * code creates the power domain tree, the indices of the CPU power
- * domain nodes it allocates match the linear indices returned by
- * plat_core_pos_by_mpidr() i.e. CLUSTER1 CPUs are allocated indices
- * from 0 to 3 and the higher indices for CLUSTER0 CPUs.
- */
-const unsigned char arm_power_domain_tree_desc[] = {
-	/* No of root nodes */
-	CSS_PWR_DOMAINS_AT_MAX_PWR_LVL,
-	/* No of children for the root node */
-	ARM_CLUSTER_COUNT,
-	/* No of children for the first cluster node */
-	PLAT_ARM_CLUSTER1_CORE_COUNT,
-	/* No of children for the second cluster node */
-	PLAT_ARM_CLUSTER0_CORE_COUNT
-};
-
-
 /******************************************************************************
  * This function implements a part of the critical interface between the psci
  * generic layer and the platform that allows the former to query the platform
diff --git a/plat/common/aarch64/platform_helpers.S b/plat/common/aarch64/platform_helpers.S
index 29f01ce..0863830 100644
--- a/plat/common/aarch64/platform_helpers.S
+++ b/plat/common/aarch64/platform_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, 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:
@@ -39,6 +39,7 @@
 	.weak	plat_disable_acp
 	.weak	bl1_plat_prepare_exit
 	.weak	plat_error_handler
+	.weak	plat_panic_handler
 
 #if !ENABLE_PLAT_COMPAT
 	.globl	platform_get_core_pos
@@ -131,3 +132,12 @@
 func plat_error_handler
 	b	plat_error_handler
 endfunc plat_error_handler
+
+	/* -----------------------------------------------------
+	 * void plat_panic_handler(void) __dead2;
+	 * Endless loop by default.
+	 * -----------------------------------------------------
+	 */
+func plat_panic_handler
+	b	plat_panic_handler
+endfunc plat_panic_handler
diff --git a/plat/mediatek/mt8173/aarch64/platform_common.c b/plat/mediatek/mt8173/aarch64/platform_common.c
index b537f7b..23116f5 100644
--- a/plat/mediatek/mt8173/aarch64/platform_common.c
+++ b/plat/mediatek/mt8173/aarch64/platform_common.c
@@ -44,7 +44,7 @@
 /* Table of regions to map using the MMU.  */
 const mmap_region_t plat_mmap[] = {
 	/* for TF text, RO, RW */
-	MAP_REGION_FLAT(TZRAM_BASE, TZRAM_SIZE + TZRAM2_SIZE,
+	MAP_REGION_FLAT(TZRAM_BASE, TZRAM_SIZE,
 			MT_MEMORY | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(MTK_DEV_RNG0_BASE, MTK_DEV_RNG0_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
diff --git a/plat/mediatek/mt8173/bl31_plat_setup.c b/plat/mediatek/mt8173/bl31_plat_setup.c
index ec95143..8783775 100644
--- a/plat/mediatek/mt8173/bl31_plat_setup.c
+++ b/plat/mediatek/mt8173/bl31_plat_setup.c
@@ -146,8 +146,6 @@
 	assert(from_bl2->h.type == PARAM_BL31);
 	assert(from_bl2->h.version >= VERSION_1);
 
-	assert(((unsigned long)plat_params_from_bl2) == MT_BL31_PLAT_PARAM_VAL);
-
 	bl32_ep_info = *from_bl2->bl32_ep_info;
 	bl33_ep_info = *from_bl2->bl33_ep_info;
 }
diff --git a/plat/mediatek/mt8173/drivers/gpio/gpio.c b/plat/mediatek/mt8173/drivers/gpio/gpio.c
deleted file mode 100644
index 20473b9..0000000
--- a/plat/mediatek/mt8173/drivers/gpio/gpio.c
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this
- * list of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * Neither the name of ARM nor the names of its contributors may be used
- * to endorse or promote products derived from this software without specific
- * prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-#include <debug.h>
-#include <gpio.h>
-#include <mmio.h>
-#include <mt8173_def.h>
-#include <pmic_wrap_init.h>
-
-enum {
-	MAX_GPIO_REG_BITS = 16,
-};
-
-struct mt_gpio_obj {
-	struct gpio_regs *reg;
-};
-
-static struct mt_gpio_obj gpio_dat = {
-	.reg = (struct gpio_regs *)(GPIO_BASE),
-};
-
-static struct mt_gpio_obj *gpio_obj = &gpio_dat;
-
-struct mt_gpioext_obj {
-	struct gpioext_regs *reg;
-};
-
-static struct mt_gpioext_obj gpioext_dat = {
-	.reg = (struct gpioext_regs *)(GPIOEXT_BASE),
-};
-
-static struct mt_gpioext_obj *gpioext_obj = &gpioext_dat;
-
-static inline struct mt_gpio_obj *mt_get_gpio_obj(void)
-{
-	return gpio_obj;
-}
-
-static inline struct mt_gpioext_obj *mt_get_gpioext_obj(void)
-{
-	return gpioext_obj;
-}
-
-enum {
-	GPIO_PRO_DIR = 0,
-	GPIO_PRO_DOUT,
-	GPIO_PRO_DIN,
-	GPIO_PRO_PULLEN,
-	GPIO_PRO_PULLSEL,
-	GPIO_PRO_MODE,
-	GPIO_PRO_MAX,
-};
-
-static inline int32_t gpioext_write(uint16_t *addr, int64_t data)
-{
-	return pwrap_write((uint32_t)(uintptr_t)addr, data);
-}
-
-static inline int32_t gpioext_set_bits(uint32_t bit, uint16_t *reg)
-{
-	return gpioext_write(reg, bit);
-}
-
-static int32_t mt_set_gpio_chip(uint32_t pin, uint32_t property, uint32_t val)
-{
-	uint32_t pos = 0;
-	uint32_t bit = 0;
-	struct mt_gpio_obj *obj = mt_get_gpio_obj();
-	uint16_t *reg;
-	uint32_t data = 0;
-
-	if (!obj)
-		return -ERACCESS;
-
-	if (pin >= GPIO_EXTEND_START)
-		return -ERINVAL;
-
-	if (property >= GPIO_PRO_MAX)
-		return -ERINVAL;
-
-	pos = pin / MAX_GPIO_REG_BITS;
-	bit = pin % MAX_GPIO_REG_BITS;
-	data = 1L << bit;
-
-	switch (property) {
-	case GPIO_PRO_DIR:
-		if (val == GPIO_DIR_IN)
-			reg = &obj->reg->dir[pos].rst;
-		else
-			reg = &obj->reg->dir[pos].set;
-		break;
-	case GPIO_PRO_DOUT:
-		if (val == GPIO_OUT_ZERO)
-			reg = &obj->reg->dout[pos].rst;
-		else
-			reg = &obj->reg->dout[pos].set;
-		break;
-	default:
-		return -ERINVAL;
-	}
-
-	mmio_write_16((uintptr_t)reg, data);
-
-	return RSUCCESS;
-}
-
-static int32_t mt_set_gpio_ext(uint32_t pin, uint32_t property, uint32_t val)
-{
-	uint32_t pos = 0;
-	uint32_t bit = 0;
-	struct mt_gpioext_obj *obj = mt_get_gpioext_obj();
-	uint16_t *reg;
-	uint32_t data = 0;
-	int ret = 0;
-
-	if (!obj)
-		return -ERACCESS;
-
-	if (pin >= MAX_GPIO_PIN)
-		return -ERINVAL;
-
-	if (property >= GPIO_PRO_MAX)
-		return -ERINVAL;
-
-	pin -= GPIO_EXTEND_START;
-	pos = pin / MAX_GPIO_REG_BITS;
-	bit = pin % MAX_GPIO_REG_BITS;
-
-	switch (property) {
-	case GPIO_PRO_DIR:
-		if (val == GPIO_DIR_IN)
-			reg = &obj->reg->dir[pos].rst;
-		else
-			reg = &obj->reg->dir[pos].set;
-		break;
-	case GPIO_PRO_DOUT:
-		if (val == GPIO_OUT_ZERO)
-			reg = &obj->reg->dout[pos].rst;
-		else
-			reg = &obj->reg->dout[pos].set;
-		break;
-	default:
-		return -ERINVAL;
-	}
-	data = (1L << bit);
-	ret = gpioext_set_bits(data, reg);
-
-	return ret ? -ERWRAPPER : RSUCCESS;
-}
-
-static void mt_gpio_pin_decrypt(uint32_t *cipher)
-{
-	if ((*cipher & (0x80000000)) == 0)
-		INFO("Pin %u decrypt warning!\n", *cipher);
-	*cipher &= ~(0x80000000);
-}
-
-int32_t mt_set_gpio_out(uint32_t pin, uint32_t output)
-{
-	uint32_t gp = GPIO_PRO_DOUT;
-
-	mt_gpio_pin_decrypt(&pin);
-
-	return (pin >= GPIO_EXTEND_START) ?
-		mt_set_gpio_ext(pin, gp, output) :
-		mt_set_gpio_chip(pin, gp, output);
-}
-
-void gpio_set(uint32_t gpio, int32_t value)
-{
-	mt_set_gpio_out(gpio, value);
-}
diff --git a/plat/mediatek/mt8173/drivers/gpio/gpio.h b/plat/mediatek/mt8173/drivers/gpio/gpio.h
deleted file mode 100644
index ccc99e1..0000000
--- a/plat/mediatek/mt8173/drivers/gpio/gpio.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- * Copyright (c) 2015, 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 __PLAT_DRIVER_GPIO_H__
-#define __PLAT_DRIVER_GPIO_H__
-
-#include <stdint.h>
-
-enum {
-	GPIOEXT_BASE = 0xC000,
-};
-
-/* Error Code No. */
-enum {
-	RSUCCESS = 0,
-	ERACCESS,
-	ERINVAL,
-	ERWRAPPER,
-};
-
-enum {
-	GPIO_UNSUPPORTED = -1,
-
-	GPIO0,   GPIO1,   GPIO2,   GPIO3,   GPIO4,   GPIO5,   GPIO6,   GPIO7,
-	GPIO8,   GPIO9,   GPIO10,  GPIO11,  GPIO12,  GPIO13,  GPIO14,  GPIO15,
-	GPIO16,  GPIO17,  GPIO18,  GPIO19,  GPIO20,  GPIO21,  GPIO22,  GPIO23,
-	GPIO24,  GPIO25,  GPIO26,  GPIO27,  GPIO28,  GPIO29,  GPIO30,  GPIO31,
-	GPIO32,  GPIO33,  GPIO34,  GPIO35,  GPIO36,  GPIO37,  GPIO38,  GPIO39,
-	GPIO40,  GPIO41,  GPIO42,  GPIO43,  GPIO44,  GPIO45,  GPIO46,  GPIO47,
-	GPIO48,  GPIO49,  GPIO50,  GPIO51,  GPIO52,  GPIO53,  GPIO54,  GPIO55,
-	GPIO56,  GPIO57,  GPIO58,  GPIO59,  GPIO60,  GPIO61,  GPIO62,  GPIO63,
-	GPIO64,  GPIO65,  GPIO66,  GPIO67,  GPIO68,  GPIO69,  GPIO70,  GPIO71,
-	GPIO72,  GPIO73,  GPIO74,  GPIO75,  GPIO76,  GPIO77,  GPIO78,  GPIO79,
-	GPIO80,  GPIO81,  GPIO82,  GPIO83,  GPIO84,  GPIO85,  GPIO86,  GPIO87,
-	GPIO88,  GPIO89,  GPIO90,  GPIO91,  GPIO92,  GPIO93,  GPIO94,  GPIO95,
-	GPIO96,  GPIO97,  GPIO98,  GPIO99,  GPIO100, GPIO101, GPIO102, GPIO103,
-	GPIO104, GPIO105, GPIO106, GPIO107, GPIO108, GPIO109, GPIO110, GPIO111,
-	GPIO112, GPIO113, GPIO114, GPIO115, GPIO116, GPIO117, GPIO118, GPIO119,
-	GPIO120, GPIO121, GPIO122, GPIO123, GPIO124, GPIO125, GPIO126, GPIO127,
-	GPIO128, GPIO129, GPIO130, GPIO131, GPIO132, GPIO133, GPIO134,
-
-	GPIOEXT0,  GPIOEXT1,  GPIOEXT2,  GPIOEXT3,  GPIOEXT4,  GPIOEXT5,
-	GPIOEXT6,  GPIOEXT7,  GPIOEXT8,  GPIOEXT9,  GPIOEXT10, GPIOEXT11,
-	GPIOEXT12, GPIOEXT13, GPIOEXT14, GPIOEXT15, GPIOEXT16, GPIOEXT17,
-	GPIOEXT18, GPIOEXT19, GPIOEXT20, GPIOEXT21, GPIOEXT22, GPIOEXT23,
-	GPIOEXT24, GPIOEXT25, GPIOEXT26, GPIOEXT27, GPIOEXT28, GPIOEXT29,
-	GPIOEXT30, GPIOEXT31, GPIOEXT32, GPIOEXT33, GPIOEXT34, GPIOEXT35,
-	GPIOEXT36, GPIOEXT37, GPIOEXT38, GPIOEXT39, GPIOEXT40,
-
-	GPIO_MAX
-};
-
-#define MAX_GPIO_PIN		GPIO_MAX
-
-#define GPIO_EXTEND_START	GPIOEXT0
-
-/* GPIO DIRECTION */
-enum {
-	GPIO_DIR_UNSUPPORTED = -1,
-	GPIO_DIR_IN = 0,
-	GPIO_DIR_OUT = 1,
-	GPIO_DIR_MAX,
-	GPIO_DIR_DEFAULT = GPIO_DIR_IN,
-};
-
-/* GPIO OUTPUT */
-enum {
-	GPIO_OUT_UNSUPPORTED = -1,
-	GPIO_OUT_ZERO = 0,
-	GPIO_OUT_ONE = 1,
-	GPIO_OUT_MAX,
-	GPIO_OUT_DEFAULT = GPIO_OUT_ZERO,
-	GPIO_DATA_OUT_DEFAULT = GPIO_OUT_ZERO,	/* compatible with DCT */
-};
-
-struct val_regs {
-	uint16_t val;
-	uint16_t _align1;
-	uint16_t set;
-	uint16_t _align2;
-	uint16_t rst;
-	uint16_t _align3[3];
-};
-
-struct gpio_regs {
-	struct val_regs dir[9];		/* 0x0000 ~ 0x008F: 144 bytes */
-	uint8_t rsv00[112];		/* 0x0090 ~ 0x00FF: 112 bytes */
-	struct val_regs pullen[9];	/* 0x0100 ~ 0x018F: 144 bytes */
-	uint8_t rsv01[112];		/* 0x0190 ~ 0x01FF: 112 bytes */
-	struct val_regs pullsel[9];	/* 0x0200 ~ 0x028F: 144 bytes */
-	uint8_t rsv02[112];		/* 0x0290 ~ 0x02FF: 112 bytes */
-	uint8_t rsv03[256];		/* 0x0300 ~ 0x03FF: 256 bytes */
-	struct val_regs dout[9];	/* 0x0400 ~ 0x048F: 144 bytes */
-	uint8_t rsv04[112];		/* 0x0490 ~ 0x04FF: 112 bytes */
-	struct val_regs din[9];		/* 0x0500 ~ 0x058F: 114 bytes */
-	uint8_t rsv05[112];		/* 0x0590 ~ 0x05FF: 112 bytes */
-	struct val_regs mode[27];	/* 0x0600 ~ 0x07AF: 432 bytes */
-	uint8_t rsv06[336];		/* 0x07B0 ~ 0x08FF: 336 bytes */
-	struct val_regs ies[3];		/* 0x0900 ~ 0x092F:  48 bytes */
-	struct val_regs smt[3];		/* 0x0930 ~ 0x095F:  48 bytes */
-	uint8_t rsv07[160];		/* 0x0960 ~ 0x09FF: 160 bytes */
-	struct val_regs tdsel[8];	/* 0x0A00 ~ 0x0A7F: 128 bytes */
-	struct val_regs rdsel[6];	/* 0x0A80 ~ 0x0ADF:  96 bytes */
-	uint8_t rsv08[32];		/* 0x0AE0 ~ 0x0AFF:  32 bytes */
-	struct val_regs drv_mode[10];	/* 0x0B00 ~ 0x0B9F: 160 bytes */
-	uint8_t rsv09[96];		/* 0x0BA0 ~ 0x0BFF:  96 bytes */
-	struct val_regs msdc0_ctrl0;	/* 0x0C00 ~ 0x0C0F:  16 bytes */
-	struct val_regs msdc0_ctrl1;	/* 0x0C10 ~ 0x0C1F:  16 bytes */
-	struct val_regs msdc0_ctrl2;	/* 0x0C20 ~ 0x0C2F:  16 bytes */
-	struct val_regs msdc0_ctrl5;	/* 0x0C30 ~ 0x0C3F:  16 bytes */
-	struct val_regs msdc1_ctrl0;	/* 0x0C40 ~ 0x0C4F:  16 bytes */
-	struct val_regs msdc1_ctrl1;	/* 0x0C50 ~ 0x0C5F:  16 bytes */
-	struct val_regs msdc1_ctrl2;	/* 0x0C60 ~ 0x0C6F:  16 bytes */
-	struct val_regs msdc1_ctrl5;	/* 0x0C70 ~ 0x0C7F:  16 bytes */
-	struct val_regs msdc2_ctrl0;	/* 0x0C80 ~ 0x0C8F:  16 bytes */
-	struct val_regs msdc2_ctrl1;	/* 0x0C90 ~ 0x0C9F:  16 bytes */
-	struct val_regs msdc2_ctrl2;	/* 0x0CA0 ~ 0x0CAF:  16 bytes */
-	struct val_regs msdc2_ctrl5;	/* 0x0CB0 ~ 0x0CBF:  16 bytes */
-	struct val_regs msdc3_ctrl0;	/* 0x0CC0 ~ 0x0CCF:  16 bytes */
-	struct val_regs msdc3_ctrl1;	/* 0x0CD0 ~ 0x0CDF:  16 bytes */
-	struct val_regs msdc3_ctrl2;	/* 0x0CE0 ~ 0x0CEF:  16 bytes */
-	struct val_regs msdc3_ctrl5;	/* 0x0CF0 ~ 0x0CFF:  16 bytes */
-	struct val_regs msdc0_ctrl3;	/* 0x0D00 ~ 0x0D0F:  16 bytes */
-	struct val_regs msdc0_ctrl4;	/* 0x0D10 ~ 0x0D1F:  16 bytes */
-	struct val_regs msdc1_ctrl3;	/* 0x0D20 ~ 0x0D2F:  16 bytes */
-	struct val_regs msdc1_ctrl4;	/* 0x0D30 ~ 0x0D3F:  16 bytes */
-	struct val_regs msdc2_ctrl3;	/* 0x0D40 ~ 0x0D4F:  16 bytes */
-	struct val_regs msdc2_ctrl4;	/* 0x0D50 ~ 0x0D5F:  16 bytes */
-	struct val_regs msdc3_ctrl3;	/* 0x0D60 ~ 0x0D6F:  16 bytes */
-	struct val_regs msdc3_ctrl4;	/* 0x0D70 ~ 0x0D7F:  16 bytes */
-	uint8_t rsv10[64];		/* 0x0D80 ~ 0x0DBF:  64 bytes */
-	struct val_regs exmd_ctrl[1];	/* 0x0DC0 ~ 0x0DCF:  16 bytes */
-	uint8_t rsv11[48];		/* 0x0DD0 ~ 0x0DFF:  48 bytes */
-	struct val_regs kpad_ctrl[2];	/* 0x0E00 ~ 0x0E1F:  32 bytes */
-	struct val_regs hsic_ctrl[4];	/* 0x0E20 ~ 0x0E5F:  64 bytes */
-};
-
-struct ext_val_regs {
-	uint16_t val;
-	uint16_t set;
-	uint16_t rst;
-	uint16_t _align;
-};
-
-struct gpioext_regs {
-	struct ext_val_regs dir[4];	/* 0x0000 ~ 0x001F: 32 bytes */
-	struct ext_val_regs pullen[4];	/* 0x0020 ~ 0x003F: 32 bytes */
-	struct ext_val_regs pullsel[4];	/* 0x0040 ~ 0x005F: 32 bytes */
-	struct ext_val_regs dinv[4];	/* 0x0060 ~ 0x007F: 32 bytes */
-	struct ext_val_regs dout[4];	/* 0x0080 ~ 0x009F: 32 bytes */
-	struct ext_val_regs din[4];	/* 0x00A0 ~ 0x00BF: 32 bytes */
-	struct ext_val_regs mode[10];	/* 0x00C0 ~ 0x010F: 80 bytes */
-};
-
-/* GPIO Driver interface */
-void gpio_set(uint32_t gpio, int32_t value);
-
-#endif /* __PLAT_DRIVER_GPIO_H__ */
diff --git a/plat/mediatek/mt8173/drivers/rtc/rtc.c b/plat/mediatek/mt8173/drivers/rtc/rtc.c
index e171863..daaac42 100644
--- a/plat/mediatek/mt8173/drivers/rtc/rtc.c
+++ b/plat/mediatek/mt8173/drivers/rtc/rtc.c
@@ -95,8 +95,8 @@
 	if (Writeif_unlock()) {
 		RTC_Write(RTC_BBPU, bbpu);
 		if (!Write_trigger())
-			assert(1);
+			assert(0);
 	} else {
-		assert(1);
+		assert(0);
 	}
 }
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_suspend.c b/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
index 6bf37f3..843b5f9 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
@@ -29,6 +29,9 @@
  */
 #include <bakery_lock.h>
 #include <debug.h>
+#include <delay_timer.h>
+#include <mmio.h>
+#include <mt8173_def.h>
 #include <spm.h>
 #include <spm_suspend.h>
 
@@ -38,8 +41,6 @@
  * This driver controls the system power in system suspend flow.
  */
 
-#define WAIT_UART_ACK_TIMES     80	/* 80 * 10us */
-
 #define WAKE_SRC_FOR_SUSPEND					\
 	(WAKE_SRC_KP | WAKE_SRC_EINT | WAKE_SRC_MD32 |		\
 	WAKE_SRC_USB_CD | WAKE_SRC_USB_PDN | WAKE_SRC_THERM |	\
@@ -50,6 +51,13 @@
 #define spm_is_wakesrc_invalid(wakesrc)	\
 	(!!((unsigned int)(wakesrc) & 0xc0003803))
 
+#define ARMCA15PLL_CON0		(APMIXED_BASE + 0x200)
+#define ARMCA15PLL_CON1		(APMIXED_BASE + 0x204)
+#define ARMCA15PLL_PWR_CON0	(APMIXED_BASE + 0x20c)
+#define ARMCA15PLL_PWR_ON	(1U << 0)
+#define ARMCA15PLL_ISO_EN	(1U << 1)
+#define ARMCA15PLL_EN		(1U << 0)
+
 const unsigned int spm_flags =
 	SPM_DUALVCORE_PDN_DIS | SPM_PASR_DIS | SPM_DPD_DIS |
 	SPM_CPU_DVS_DIS | SPM_OPT | SPM_INFRA_PDN_DIS;
@@ -293,8 +301,23 @@
 	return last_wr;
 }
 
+static void bigcore_pll_on(void)
+{
+	mmio_setbits_32(ARMCA15PLL_PWR_CON0, ARMCA15PLL_PWR_ON);
+	mmio_clrbits_32(ARMCA15PLL_PWR_CON0, ARMCA15PLL_ISO_EN);
+	mmio_setbits_32(ARMCA15PLL_CON0, ARMCA15PLL_EN);
+}
+
+static void bigcore_pll_off(void)
+{
+	mmio_clrbits_32(ARMCA15PLL_CON0, ARMCA15PLL_EN);
+	mmio_setbits_32(ARMCA15PLL_PWR_CON0, ARMCA15PLL_ISO_EN);
+	mmio_clrbits_32(ARMCA15PLL_PWR_CON0, ARMCA15PLL_PWR_ON);
+}
+
 void spm_system_suspend(void)
 {
+	bigcore_pll_off();
 	spm_lock_get();
 	go_to_sleep_before_wfi(spm_flags);
 	set_suspend_ready();
@@ -308,4 +331,7 @@
 	INFO("spm_wake_reason=%d\n", spm_wake_reason);
 	clear_all_ready();
 	spm_lock_release();
+	bigcore_pll_on();
+	/* Add 20us delay for turning on PLL*/
+	udelay(20);
 }
diff --git a/plat/mediatek/mt8173/mt8173_def.h b/plat/mediatek/mt8173/include/mt8173_def.h
similarity index 93%
rename from plat/mediatek/mt8173/mt8173_def.h
rename to plat/mediatek/mt8173/include/mt8173_def.h
index ecf4cc6..39bab14 100644
--- a/plat/mediatek/mt8173/mt8173_def.h
+++ b/plat/mediatek/mt8173/include/mt8173_def.h
@@ -28,22 +28,26 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef __PLAT_DEF_H__
-#define __PLAT_DEF_H__
+#ifndef __MT8173_DEF_H__
+#define __MT8173_DEF_H__
 
-#define MT8173_PRIMARY_CPU	0x0
+#if RESET_TO_BL31
+#error "MT8173 is incompatible with RESET_TO_BL31!"
+#endif
 
-/* Special value used to verify platform parameters from BL2 to BL3-1 */
-#define MT_BL31_PLAT_PARAM_VAL	0x0f1e2d3c4b5a6978ULL
+#define MT8173_PRIMARY_CPU	0x0
 
+/* Register base address */
 #define IO_PHYS			(0x10000000)
 #define INFRACFG_AO_BASE	(IO_PHYS + 0x1000)
+#define PERI_CON_BASE		(IO_PHYS + 0x3000)
 #define GPIO_BASE		(IO_PHYS + 0x5000)
 #define SPM_BASE		(IO_PHYS + 0x6000)
 #define RGU_BASE		(IO_PHYS + 0x7000)
 #define PMIC_WRAP_BASE		(IO_PHYS + 0xD000)
 #define MCUCFG_BASE		(IO_PHYS + 0x200000)
-#define TRNG_base		(IO_PHYS + 0x20F000)
+#define APMIXED_BASE		(IO_PHYS + 0x209000)
+#define TRNG_BASE		(IO_PHYS + 0x20F000)
 #define MT_GIC_BASE		(IO_PHYS + 0x220000)
 #define PLAT_MT_CCI_BASE	(IO_PHYS + 0x390000)
 
@@ -112,4 +116,4 @@
 #define MT_IRQ_SEC_SGI_6	14
 #define MT_IRQ_SEC_SGI_7	15
 
-#endif /* __PLAT_DEF_H__ */
+#endif /* __MT8173_DEF_H__ */
diff --git a/plat/mediatek/mt8173/plat_private.h b/plat/mediatek/mt8173/include/plat_private.h
similarity index 100%
rename from plat/mediatek/mt8173/plat_private.h
rename to plat/mediatek/mt8173/include/plat_private.h
diff --git a/plat/mediatek/mt8173/include/platform_def.h b/plat/mediatek/mt8173/include/platform_def.h
index 0573bc5..dc5b000 100644
--- a/plat/mediatek/mt8173/include/platform_def.h
+++ b/plat/mediatek/mt8173/include/platform_def.h
@@ -70,7 +70,21 @@
 /*******************************************************************************
  * Platform memory map related constants
  ******************************************************************************/
-/* TF txet, ro, rw, internal SRAM, Size: release: 80KB, debug: 92KB */
+/*
+ * MT8173 SRAM memory layout
+ * 0x100000 +-------------------+
+ *          | shared mem (4KB)  |
+ * 0x101000 +-------------------+
+ *          |                   |
+ *          |   BL3-1 (124KB)   |
+ *          |                   |
+ * 0x120000 +-------------------+
+ *          |  reserved (64KB)  |
+ * 0x130000 +-------------------+
+ */
+/* TF txet, ro, rw, xlat table, coherent memory ... etc.
+ * Size: release: 128KB, debug: 128KB
+ */
 #define TZRAM_BASE		(0x100000)
 #if DEBUG
 #define TZRAM_SIZE		(0x20000)
@@ -78,7 +92,7 @@
 #define TZRAM_SIZE		(0x20000)
 #endif
 
-/* xlat_table , coherence ram, 64KB */
+/* Reserved: 64KB */
 #define TZRAM2_BASE		(TZRAM_BASE + TZRAM_SIZE)
 #define TZRAM2_SIZE		(0x10000)
 
diff --git a/plat/mediatek/mt8173/plat_pm.c b/plat/mediatek/mt8173/plat_pm.c
index f28f8ed..6bb8a9b 100644
--- a/plat/mediatek/mt8173/plat_pm.c
+++ b/plat/mediatek/mt8173/plat_pm.c
@@ -36,7 +36,6 @@
 #include <console.h>
 #include <debug.h>
 #include <errno.h>
-#include <gpio.h>
 #include <mcucfg.h>
 #include <mmio.h>
 #include <mt8173_def.h>
@@ -478,7 +477,6 @@
 {
 	INFO("MTK System Off\n");
 
-	gpio_set(GPIO120, GPIO_OUT_ZERO);
 	rtc_bbpu_power_down();
 
 	wfi();
diff --git a/plat/mediatek/mt8173/platform.mk b/plat/mediatek/mt8173/platform.mk
index 30df32f..4169823 100644
--- a/plat/mediatek/mt8173/platform.mk
+++ b/plat/mediatek/mt8173/platform.mk
@@ -32,8 +32,6 @@
 MTK_PLAT_SOC		:=	${MTK_PLAT}/${PLAT}
 
 PLAT_INCLUDES		:=	-I${MTK_PLAT}/common/				\
-				-I${MTK_PLAT_SOC}/				\
-				-I${MTK_PLAT_SOC}/drivers/gpio/			\
 				-I${MTK_PLAT_SOC}/drivers/mtcmos/		\
 				-I${MTK_PLAT_SOC}/drivers/pmic/			\
 				-I${MTK_PLAT_SOC}/drivers/rtc/			\
@@ -61,7 +59,6 @@
 				${MTK_PLAT_SOC}/aarch64/plat_helpers.S		\
 				${MTK_PLAT_SOC}/aarch64/platform_common.c	\
 				${MTK_PLAT_SOC}/bl31_plat_setup.c		\
-				${MTK_PLAT_SOC}/drivers/gpio/gpio.c		\
 				${MTK_PLAT_SOC}/drivers/mtcmos/mtcmos.c		\
 				${MTK_PLAT_SOC}/drivers/pmic/pmic_wrap_init.c	\
 				${MTK_PLAT_SOC}/drivers/rtc/rtc.c		\