diff --git a/Makefile b/Makefile
index ac688ba..81991b8 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/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/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/porting-guide.md b/docs/porting-guide.md
index 004f70d..bd1b448 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -654,7 +654,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].
@@ -782,6 +783,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
 -------------------------------------------------
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/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/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 PARAM_EP     0x01
-#define PARAM_IMAGE_BINARY  0x02
-#define PARAM_BL31       0x03
+#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 VERSION_1		0x01
+#define PARAM_EP		0x01
+#define PARAM_IMAGE_BINARY	0x02
+#define PARAM_BL31		0x03
+
+#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/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/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/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 aaf419e..b065537 100644
--- a/include/plat/arm/board/common/board_arm_def.h
+++ b/include/plat/arm/board/common/board_arm_def.h
@@ -91,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/arm_def.h b/include/plat/arm/common/arm_def.h
index dab7545..d04f9d6 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -294,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/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/stdlib/std.c b/lib/stdlib/std.c
deleted file mode 100644
index 5f6ef75..0000000
--- a/lib/stdlib/std.c
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this
- * list of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * Neither the name of ARM nor the names of its contributors may be used
- * to endorse or promote products derived from this software without specific
- * prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-
-/* Include 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"
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/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/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_common.mk b/plat/arm/common/arm_common.mk
index 2647f04..425e0d3 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -129,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/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 f19c931..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 <mmio.h>
-#include <mt8173_def.h>
-#include <pmic_wrap_init.h>
-#include "gpio.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__
+
+#if RESET_TO_BL31
+#error "MT8173 is incompatible with RESET_TO_BL31!"
+#endif
 
 #define MT8173_PRIMARY_CPU	0x0
 
-/* Special value used to verify platform parameters from BL2 to BL3-1 */
-#define MT_BL31_PLAT_PARAM_VAL	0x0f1e2d3c4b5a6978ULL
-
+/* 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 be33c91..6bb8a9b 100644
--- a/plat/mediatek/mt8173/plat_pm.c
+++ b/plat/mediatek/mt8173/plat_pm.c
@@ -48,7 +48,6 @@
 #include <spm_hotplug.h>
 #include <spm_mcdi.h>
 #include <spm_suspend.h>
-#include "drivers/gpio/gpio.h"
 
 struct core_context {
 	unsigned long timer_data[8];
@@ -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		\