diff --git a/Makefile b/Makefile
index 4fbb914..adfdba6 100644
--- a/Makefile
+++ b/Makefile
@@ -122,10 +122,6 @@
         FWU_FIP_DEPS += fwu_certificates
 endif
 
-# For AArch32, enable new version of image loading.
-ifeq (${ARCH},aarch32)
-        LOAD_IMAGE_V2	:=	1
-endif
 
 ################################################################################
 # Toolchain
@@ -147,16 +143,14 @@
 ASFLAGS_aarch32		=	-march=armv8-a
 TF_CFLAGS_aarch32	=	-march=armv8-a
 
-ASFLAGS			+= 	-nostdinc -ffreestanding -Wa,--fatal-warnings	\
-				-Werror -Wmissing-include-dirs			\
-				-D__ASSEMBLY__ $(ASFLAGS_$(ARCH))		\
-				${DEFINES} ${INCLUDES}
-TF_CFLAGS		+= 	-nostdinc -ffreestanding -Wall			\
-				-Werror -Wmissing-include-dirs			\
-				-std=c99 -c -Os					\
-				$(TF_CFLAGS_$(ARCH))				\
-				${DEFINES} ${INCLUDES}
-TF_CFLAGS		+=	-ffunction-sections -fdata-sections
+CPPFLAGS		=	${DEFINES} ${INCLUDES} -nostdinc		\
+				-Wmissing-include-dirs -Werror
+ASFLAGS			+=	$(CPPFLAGS) $(ASFLAGS_$(ARCH))			\
+				-D__ASSEMBLY__ -ffreestanding 			\
+				-Wa,--fatal-warnings
+TF_CFLAGS		+=	$(CPPFLAGS) $(TF_CFLAGS_$(ARCH))		\
+				-ffreestanding -Wall -std=c99 -Os		\
+				-ffunction-sections -fdata-sections
 
 LDFLAGS			+=	--fatal-warnings -O1
 LDFLAGS			+=	--gc-sections
@@ -294,19 +288,15 @@
         endif
 endif
 
-# TRUSTED_BOARD_BOOT is currently not supported when LOAD_IMAGE_V2 is enabled.
-ifeq (${LOAD_IMAGE_V2},1)
-        ifeq (${TRUSTED_BOARD_BOOT},1)
-                $(error "TRUSTED_BOARD_BOOT is currently not supported	\
-                for LOAD_IMAGE_V2=1")
-        endif
-endif
-
-# For AArch32, LOAD_IMAGE_V2 must be enabled.
 ifeq (${ARCH},aarch32)
+    # For AArch32, LOAD_IMAGE_V2 must be enabled.
     ifeq (${LOAD_IMAGE_V2}, 0)
         $(error "For AArch32, LOAD_IMAGE_V2 must be enabled.")
     endif
+    # TRUSTED_BOARD_BOOT is currently not supported for AArch32.
+    ifeq (${TRUSTED_BOARD_BOOT},1)
+        $(error "TRUSTED_BOARD_BOOT is currently not supported for AArch32")
+    endif
 endif
 
 
diff --git a/bl1/bl1_fwu.c b/bl1/bl1_fwu.c
index f333805..1cc7daf 100644
--- a/bl1/bl1_fwu.c
+++ b/bl1/bl1_fwu.c
@@ -41,6 +41,7 @@
 #include <platform_def.h>
 #include <smcc_helpers.h>
 #include <string.h>
+#include <utils.h>
 #include "bl1_private.h"
 
 /*
@@ -120,123 +121,130 @@
 			unsigned int image_size,
 			unsigned int flags)
 {
-	uintptr_t base_addr;
-	meminfo_t *mem_layout;
+	uintptr_t dest_addr;
+	unsigned int remaining;
 
 	/* Get the image descriptor. */
 	image_desc_t *image_desc = bl1_plat_get_image_desc(image_id);
-
-	/* Check if we are in correct state. */
-	if ((!image_desc) ||
-		((image_desc->state != IMAGE_STATE_RESET) &&
-		 (image_desc->state != IMAGE_STATE_COPYING))) {
-		WARN("BL1-FWU: Copy not allowed due to invalid state\n");
+	if (!image_desc) {
+		WARN("BL1-FWU: Invalid image ID %u\n", image_id);
 		return -EPERM;
 	}
 
-	/* Only Normal world is allowed to copy a Secure image. */
-	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");
+	/*
+	 * The request must originate from a non-secure caller and target a
+	 * secure image. Any other scenario is invalid.
+	 */
+	if (GET_SECURITY_STATE(flags) == SECURE) {
+		WARN("BL1-FWU: Copy not allowed from secure world.\n");
+		return -EPERM;
+	}
+	if (GET_SECURITY_STATE(image_desc->ep_info.h.attr) == NON_SECURE) {
+		WARN("BL1-FWU: Copy not allowed for non-secure images.\n");
 		return -EPERM;
 	}
 
-	if ((!image_src) || (!block_size)) {
+	/* Check whether the FWU state machine is in the correct state. */
+	if ((image_desc->state != IMAGE_STATE_RESET) &&
+	    (image_desc->state != IMAGE_STATE_COPYING)) {
+		WARN("BL1-FWU: Copy not allowed at this point of the FWU"
+			" process.\n");
+		return -EPERM;
+	}
+
+	if ((!image_src) || (!block_size) ||
+	    check_uptr_overflow(image_src, block_size - 1)) {
 		WARN("BL1-FWU: Copy not allowed due to invalid image source"
 			" or block size\n");
 		return -ENOMEM;
 	}
 
-	/* Get the image base address. */
-	base_addr = image_desc->image_info.image_base;
-
 	if (image_desc->state == IMAGE_STATE_COPYING) {
 		/*
-		 * If last block is more than expected then
-		 * clip the block to the required image size.
+		 * There must have been at least 1 copy operation for this image
+		 * previously.
 		 */
-		if (image_desc->copied_size + block_size >
-			 image_desc->image_info.image_size) {
-			block_size = image_desc->image_info.image_size -
-				image_desc->copied_size;
-			WARN("BL1-FWU: Copy argument block_size > remaining image size."
-				" Clipping block_size\n");
-		}
-
-		/* Make sure the image src/size is mapped. */
-		if (bl1_plat_mem_check(image_src, block_size, flags)) {
-			WARN("BL1-FWU: Copy arguments source/size not mapped\n");
-			return -ENOMEM;
-		}
+		assert(image_desc->copied_size != 0);
+		/*
+		 * The image size must have been recorded in the 1st copy
+		 * operation.
+		 */
+		image_size = image_desc->image_info.image_size;
+		assert(image_size != 0);
+		assert(image_desc->copied_size < image_size);
 
 		INFO("BL1-FWU: Continuing image copy in blocks\n");
-
-		/* Copy image for given 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->copied_size ==
-				image_desc->image_info.image_size) {
-			image_desc->state = IMAGE_STATE_COPIED;
-			INFO("BL1-FWU: Image copy in blocks completed\n");
-		}
-	} else {
-		/* This means image is in RESET state and ready to be copied. */
-		INFO("BL1-FWU: Fresh call to copy an image\n");
-
-		if (!image_size) {
-			WARN("BL1-FWU: Copy not allowed due to invalid image size\n");
-			return -ENOMEM;
-		}
+	} else { /* image_desc->state == IMAGE_STATE_RESET */
+		INFO("BL1-FWU: Initial call to copy an image\n");
 
 		/*
-		 * If block size is more than total size then
-		 * assume block size as the total image size.
+		 * image_size is relevant only for the 1st copy request, it is
+		 * then ignored for subsequent calls for this image.
 		 */
-		if (block_size > image_size) {
-			block_size = image_size;
-			WARN("BL1-FWU: Copy argument block_size > image size."
-				" Clipping block_size\n");
-		}
-
-		/* Make sure the image src/size is mapped. */
-		if (bl1_plat_mem_check(image_src, block_size, flags)) {
-			WARN("BL1-FWU: Copy arguments source/size not mapped\n");
+		if (!image_size) {
+			WARN("BL1-FWU: Copy not allowed due to invalid image"
+				" size\n");
 			return -ENOMEM;
 		}
 
-		/* Find out how much free trusted ram remains after BL1 load */
-		mem_layout = bl1_plat_sec_mem_layout();
-		if ((image_desc->image_info.image_base < mem_layout->free_base) ||
-			 (image_desc->image_info.image_base + image_size >
-			  mem_layout->free_base + mem_layout->free_size)) {
-			WARN("BL1-FWU: Memory not available to copy\n");
+#if LOAD_IMAGE_V2
+		/* Check that the image size to load is within limit */
+		if (image_size > image_desc->image_info.image_max_size) {
+			WARN("BL1-FWU: Image size out of bounds\n");
 			return -ENOMEM;
 		}
+#else
+		/*
+		 * Check the image will fit into the free trusted RAM after BL1
+		 * load.
+		 */
+		const meminfo_t *mem_layout = bl1_plat_sec_mem_layout();
+		if (!is_mem_free(mem_layout->free_base, mem_layout->free_size,
+					image_desc->image_info.image_base,
+					image_size)) {
+			WARN("BL1-FWU: Copy not allowed due to insufficient"
+			     " resources.\n");
+			return -ENOMEM;
+		}
+#endif
 
-		/* Update the image size. */
+		/* Save the given image size. */
 		image_desc->image_info.image_size = image_size;
 
-		/* Copy image for given size. */
-		memcpy((void *)base_addr, (const void *)image_src, block_size);
-		flush_dcache_range(base_addr, block_size);
-
-		/* Update the state. */
-		if (block_size == image_size) {
-			image_desc->state = IMAGE_STATE_COPIED;
-			INFO("BL1-FWU: Image is copied successfully\n");
-		} else {
-			image_desc->state = IMAGE_STATE_COPYING;
-			INFO("BL1-FWU: Started image copy in blocks\n");
-		}
-
-		image_desc->copied_size = block_size;
+		/*
+		 * copied_size must be explicitly initialized here because the
+		 * FWU code doesn't necessarily do it when it resets the state
+		 * machine.
+		 */
+		image_desc->copied_size = 0;
 	}
 
+	/*
+	 * If the given block size is more than the total image size
+	 * then clip the former to the latter.
+	 */
+	remaining = image_size - image_desc->copied_size;
+	if (block_size > remaining) {
+		WARN("BL1-FWU: Block size is too big, clipping it.\n");
+		block_size = remaining;
+	}
+
+	/* Make sure the source image is mapped in memory. */
+	if (bl1_plat_mem_check(image_src, block_size, flags)) {
+		WARN("BL1-FWU: Source image is not mapped.\n");
+		return -ENOMEM;
+	}
+
+	/* Everything looks sane. Go ahead and copy the block of data. */
+	dest_addr = image_desc->image_info.image_base + image_desc->copied_size;
+	memcpy((void *) dest_addr, (const void *) image_src, block_size);
+	flush_dcache_range(dest_addr, block_size);
+
+	image_desc->copied_size += block_size;
+	image_desc->state = (block_size == remaining) ?
+		IMAGE_STATE_COPIED : IMAGE_STATE_COPYING;
+
+	INFO("BL1-FWU: Copy operation successful.\n");
 	return 0;
 }
 
@@ -287,7 +295,8 @@
 		base_addr = image_desc->image_info.image_base;
 		total_size = image_desc->image_info.image_size;
 	} else {
-		if ((!image_src) || (!image_size)) {
+		if ((!image_src) || (!image_size) ||
+		    check_uptr_overflow(image_src, image_size - 1)) {
 			WARN("BL1-FWU: Auth not allowed due to invalid"
 				" image source/size\n");
 			return -ENOMEM;
diff --git a/bl1/tbbr/tbbr_img_desc.c b/bl1/tbbr/tbbr_img_desc.c
index 7651f1c..e3bd574 100644
--- a/bl1/tbbr/tbbr_img_desc.c
+++ b/bl1/tbbr/tbbr_img_desc.c
@@ -38,6 +38,9 @@
 	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
 		    VERSION_1, image_info_t, 0),
 	    .image_info.image_base = BL2_BASE,
+#if LOAD_IMAGE_V2
+	    .image_info.image_max_size = BL2_LIMIT - BL2_BASE,
+#endif
 	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
 		    VERSION_1, entry_point_info_t, SECURE),
     },
@@ -55,6 +58,9 @@
 	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
 		    VERSION_1, image_info_t, 0),
 	    .image_info.image_base = SCP_BL2U_BASE,
+#if LOAD_IMAGE_V2
+	    .image_info.image_max_size = SCP_BL2U_LIMIT - SCP_BL2U_BASE,
+#endif
 	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
 		    VERSION_1, entry_point_info_t, SECURE),
     },
@@ -65,6 +71,9 @@
 	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
 		    VERSION_1, image_info_t, 0),
 	    .image_info.image_base = BL2U_BASE,
+#if LOAD_IMAGE_V2
+	    .image_info.image_max_size = BL2U_LIMIT - BL2U_BASE,
+#endif
 	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
 		    VERSION_1, entry_point_info_t, SECURE | EXECUTABLE),
 	    .ep_info.pc = BL2U_BASE,
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 25385ca..4c296d4 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -180,6 +180,7 @@
 	b	tsp_sel1_intr_entry
 	b	tsp_system_off_entry
 	b	tsp_system_reset_entry
+	b	tsp_abort_std_smc_entry
 endfunc tsp_vector_table
 
 	/*---------------------------------------------
@@ -441,3 +442,30 @@
 	/* Should never reach here */
 	no_ret	plat_panic_handler
 endfunc tsp_std_smc_entry
+
+	/*---------------------------------------------------------------------
+	 * This entrypoint is used by the TSPD to abort a pre-empted Standard
+	 * SMC. It could be on behalf of non-secure world or because a CPU
+	 * suspend/CPU off request needs to abort the preempted SMC.
+	 * --------------------------------------------------------------------
+	 */
+func tsp_abort_std_smc_entry
+
+	/*
+	 * Exceptions masking is already done by the TSPD when entering this
+	 * hook so there is no need to do it here.
+	 */
+
+	/* Reset the stack used by the pre-empted SMC */
+	bl	plat_set_my_stack
+
+	/*
+	 * Allow some cleanup such as releasing locks.
+	 */
+	bl	tsp_abort_smc_handler
+
+	restore_args_call_smc
+
+	/* Should never reach here */
+	bl	plat_panic_handler
+endfunc tsp_abort_std_smc_entry
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index d03f7e2..2b36532 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.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:
@@ -416,3 +416,20 @@
 			    0, 0, 0, 0);
 }
 
+/*******************************************************************************
+ * TSP smc abort handler. This function is called when aborting a preemtped
+ * standard SMC request. It should cleanup all resources owned by the SMC
+ * handler such as locks or dynamically allocated memory so following SMC
+ * request are executed in a clean environment.
+ ******************************************************************************/
+tsp_args_t *tsp_abort_smc_handler(uint64_t func,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7)
+{
+	return set_smc_args(TSP_ABORT_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
diff --git a/common/aarch32/debug.S b/common/aarch32/debug.S
index cfce7ed..ecf9faf 100644
--- a/common/aarch32/debug.S
+++ b/common/aarch32/debug.S
@@ -31,14 +31,48 @@
 #include <arch.h>
 #include <asm_macros.S>
 
+	.globl	asm_assert
 	.globl	do_panic
 	.globl	report_exception
 
+/* Since the max decimal input number is 65536 */
+#define MAX_DEC_DIVISOR		10000
+
+/* The offset to add to get ascii for numerals '0 - 9' */
+#define ASCII_OFFSET_NUM	'0'
+
+	.section .rodata.panic_str, "aS"
+panic_msg:
+	.asciz "PANIC at PC : 0x"
+panic_end:
+	.asciz "\r\n"
+
 	/***********************************************************
 	 * The common implementation of do_panic for all BL stages
 	 ***********************************************************/
 func do_panic
-	no_ret	plat_panic_handler
+	/* Have LR copy point to PC at the time of panic */
+	sub	r6, lr, #4
+
+	/* Initialize crash console and verify success */
+	bl	plat_crash_console_init
+	cmp	r0, #0
+	beq	1f
+
+	/* Print panic message */
+	ldr	r4, =panic_msg
+	bl	asm_print_str
+
+	/* Print LR in hex */
+	mov	r4, r6
+	bl	asm_print_hex
+
+	/* Print new line */
+	ldr	r4, =panic_end
+	bl	asm_print_str
+1:
+	mov	lr, r6
+	b	plat_panic_handler
 endfunc do_panic
 
 	/***********************************************************
@@ -52,3 +86,103 @@
 	bl	plat_report_exception
 	no_ret	plat_panic_handler
 endfunc report_exception
+
+#if ASM_ASSERTION
+.section .rodata.assert_str, "aS"
+assert_msg1:
+	.asciz "ASSERT: File "
+assert_msg2:
+	.asciz " Line "
+
+/* ---------------------------------------------------------------------------
+ * Assertion support in assembly.
+ * The below function helps to support assertions in assembly where we do not
+ * have a C runtime stack. Arguments to the function are :
+ * r0 - File name
+ * r1 - Line no
+ * Clobber list : lr, r0 - r6
+ * ---------------------------------------------------------------------------
+ */
+func asm_assert
+	/* Stash the parameters already in r0 and r1 */
+	mov	r5, r0
+	mov	r6, r1
+
+	/* Initialize crash console and verify success */
+	bl	plat_crash_console_init
+	cmp	r0, #0
+	beq	1f
+
+	/* Print file name */
+	ldr	r4, =assert_msg1
+	bl	asm_print_str
+	mov	r4, r5
+	bl	asm_print_str
+
+	/* Print line number string */
+	ldr	r4, =assert_msg2
+	bl	asm_print_str
+
+	/* Test for maximum supported line number */
+	ldr	r4, =~0xffff
+	tst	r6, r4
+	bne	1f
+	mov	r4, r6
+
+	/* Print line number in decimal */
+	mov	r6, #10			/* Divide by 10 after every loop iteration */
+	ldr	r5, =MAX_DEC_DIVISOR
+dec_print_loop:
+	udiv	r0, r4, r5			/* Quotient */
+	mls	r4, r0, r5, r4			/* Remainder */
+	add	r0, r0, #ASCII_OFFSET_NUM	/* Convert to ASCII */
+	bl	plat_crash_console_putc
+	udiv	r5, r5, r6			/* Reduce divisor */
+	cmp	r5, #0
+	bne	dec_print_loop
+1:
+	no_ret	plat_panic_handler
+endfunc asm_assert
+#endif
+
+/*
+ * This function prints a string from address in r4
+ * Clobber: lr, r0 - r4
+ */
+func asm_print_str
+	mov	r3, lr
+1:
+	ldrb	r0, [r4], #0x1
+	cmp	r0, #0
+	beq	2f
+	bl	plat_crash_console_putc
+	b	1b
+2:
+	bx	r3
+endfunc asm_print_str
+
+/*
+ * This function prints a hexadecimal number in r4.
+ * In: r4 = the hexadecimal to print.
+ * Clobber: lr, r0 - r3, r5
+ */
+func asm_print_hex
+	mov	r3, lr
+	mov	r5, #32  /* No of bits to convert to ascii */
+1:
+	sub	r5, r5, #4
+	lsr	r0, r4, r5
+	and	r0, r0, #0xf
+	cmp	r0, #0xa
+	blo	2f
+	/* Add by 0x27 in addition to ASCII_OFFSET_NUM
+	 * to get ascii for characters 'a - f'.
+	 */
+	add	r0, r0, #0x27
+2:
+	add	r0, r0, #ASCII_OFFSET_NUM
+	bl	plat_crash_console_putc
+	cmp	r5, #0
+	bne	1b
+	bx	r3
+endfunc asm_print_hex
diff --git a/common/bl_common.c b/common/bl_common.c
index 15d5bde..47bdad5 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -53,14 +53,13 @@
 	return value;
 }
 
-#if !LOAD_IMAGE_V2
 /******************************************************************************
  * Determine whether the memory region delimited by 'addr' and 'size' is free,
  * given the extents of free memory.
  * Return 1 if it is free, 0 if it is not free or if the input values are
  * invalid.
  *****************************************************************************/
-static int is_mem_free(uintptr_t free_base, size_t free_size,
+int is_mem_free(uintptr_t free_base, size_t free_size,
 		uintptr_t addr, size_t size)
 {
 	uintptr_t free_end, requested_end;
@@ -97,6 +96,7 @@
 	return (addr >= free_base) && (requested_end <= free_end);
 }
 
+#if !LOAD_IMAGE_V2
 /******************************************************************************
  * Inside a given memory region, determine whether a sub-region of memory is
  * closer from the top or the bottom of the encompassing region. Return the
diff --git a/docs/firmware-design.md b/docs/firmware-design.md
index c37f9c5..0acb1fa 100644
--- a/docs/firmware-design.md
+++ b/docs/firmware-design.md
@@ -1127,7 +1127,8 @@
 The CPU specific operations framework depends on the `cpu_ops` structure which
 needs to be exported for each type of CPU in the platform. It is defined in
 `include/lib/cpus/aarch64/cpu_macros.S` and has the following fields : `midr`,
-`reset_func()`, `core_pwr_dwn()`, `cluster_pwr_dwn()` and `cpu_reg_dump()`.
+`reset_func()`, `cpu_pwr_down_ops` (array of power down functions) and
+`cpu_reg_dump()`.
 
 The CPU specific files in `lib/cpus` export a `cpu_ops` data structure with
 suitable handlers for that CPU.  For example, `lib/cpus/aarch64/cortex_a53.S`
@@ -1161,15 +1162,15 @@
 entry is stored in per-CPU data by `init_cpu_ops()` so that it can be quickly
 retrieved during power down sequences.
 
-The PSCI service, upon receiving a power down request, determines the highest
-power level at which to execute power down sequence for a particular CPU and
-invokes the corresponding 'prepare' power down handler in the CPU specific
-operations framework. For example, when a CPU executes a power down for power
-level 0, the `prepare_core_pwr_dwn()` retrieves the `cpu_ops` pointer from the
-per-CPU data and the corresponding `core_pwr_dwn()` is invoked. Similarly when
-a CPU executes power down at power level 1, the `prepare_cluster_pwr_dwn()`
-retrieves the `cpu_ops` pointer and the corresponding `cluster_pwr_dwn()` is
-invoked.
+Various CPU drivers register handlers to perform power down at certain power
+levels for that specific CPU. The PSCI service, upon receiving a power down
+request, determines the highest power level at which to execute power down
+sequence for a particular CPU. It uses the `prepare_cpu_pwr_dwn()` function to
+pick the right power down handler for the requested level. The function
+retrieves `cpu_ops` pointer member of per-CPU data, and from that, further
+retrieves `cpu_pwr_down_ops` array, and indexes into the required level. If the
+requested power level is higher than what a CPU driver supports, the handler
+registered for highest level is invoked.
 
 At runtime the platform hooks for power down are invoked by the PSCI service to
 perform platform specific operations during a power down sequence, for example
diff --git a/docs/firmware-update.md b/docs/firmware-update.md
index 97df8cf..21872fd 100644
--- a/docs/firmware-update.md
+++ b/docs/firmware-update.md
@@ -206,21 +206,31 @@
         if (image_id is non-secure image) return -EPERM
         if (image_id state is not (RESET or COPYING)) return -EPERM
         if (secure world caller) return -EPERM
+        if (image_addr + block_size overflows) return -ENOMEM
+        if (image destination address + image_size overflows) return -ENOMEM
         if (source block is in secure memory) return -ENOMEM
         if (source block is not mapped into BL1) return -ENOMEM
         if (image_size > free secure memory) return -ENOMEM
 
-This SMC copies the secure image indicated by `image_id` into secure memory. The
-image may be copied in a single block or multiple blocks. In either case, the
-total size of the image must be provided in `image_size` when invoking this SMC
-the first time for each image. The `image_addr` and `block_size` specify the
-source memory block to copy from. If `block_size` >= the size of the remaining
-image to copy, then BL1 completes the copy operation and sets the image state
-to COPIED. If there is still more to copy, BL1 sets the image state to COPYING.
+This SMC copies the secure image indicated by `image_id` from non-secure memory
+to secure memory for later authentication. The image may be copied in a single
+block or multiple blocks. In either case, the total size of the image must be
+provided in `image_size` when invoking this SMC for the first time for each
+image; it is ignored in subsequent calls (if any) for the same image.
+
+The `image_addr` and `block_size` specify the source memory block to copy from.
+The destination address is provided by the platform code.
+
+If `block_size` is greater than the amount of remaining bytes to copy for this
+image then the former is truncated to the latter. The copy operation is then
+considered as complete and the FWU state machine transitions to the "COPIED"
+state. If there is still more to copy, the FWU state machine stays in or
+transitions to the COPYING state (depending on the previous state).
+
 When using multiple blocks, the source blocks do not necessarily need to be in
 contiguous memory.
 
-BL1 returns from exception to the normal world caller.
+Once the SMC is handled, BL1 returns from exception to the normal world caller.
 
 
 ### FWU_SMC_IMAGE_AUTH
@@ -347,7 +357,7 @@
 
 - - - - - - - - - - - - - - - - - - - - - - - - - -
 
-_Copyright (c) 2015, ARM Limited and Contributors. All rights reserved._
+_Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved._
 
 
 [Porting Guide]:        ./porting-guide.md
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index 74a0a85..e8486f1 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -443,7 +443,19 @@
 *   **#define : ADDR_SPACE_SIZE**
 
     Defines the total size of the address space in bytes. For example, for a 32
-    bit address space, this value should be `(1ull << 32)`.
+    bit address space, this value should be `(1ull << 32)`. This definition is
+    now deprecated, platforms should use `PLAT_PHY_ADDR_SPACE_SIZE` and
+    `PLAT_VIRT_ADDR_SPACE_SIZE` instead.
+
+*   **#define : PLAT_VIRT_ADDR_SPACE_SIZE**
+
+    Defines the total size of the virtual address space in bytes. For example,
+    for a 32 bit virtual address space, this value should be `(1ull << 32)`.
+
+*   **#define : PLAT_PHY_ADDR_SPACE_SIZE**
+
+    Defines the total size of the physical address space in bytes. For example,
+    for a 32 bit physical address space, this value should be `(1ull << 32)`.
 
 If the platform port uses the IO storage framework, the following constants
 must also be defined:
@@ -690,12 +702,32 @@
 
 This function is mandatory when Trusted Board Boot is enabled. It sets a new
 counter value in the platform. The cookie in the first argument may be used to
-select the counter (as explained in plat_get_nv_ctr()).
+select the counter (as explained in plat_get_nv_ctr()). The second argument is
+the updated counter value to be written to the NV counter.
 
 The function returns 0 on success. Any other value means the counter value could
 not be updated.
 
 
+### Function: plat_set_nv_ctr2()
+
+    Argument : void *, const auth_img_desc_t *, unsigned int
+    Return   : int
+
+This function is optional when Trusted Board Boot is enabled. If this
+interface is defined, then `plat_set_nv_ctr()` need not be defined. The
+first argument passed is a cookie and is typically used to
+differentiate between a Non Trusted NV Counter and a Trusted NV
+Counter. The second argument is a pointer to an authentication image
+descriptor and may be used to decide if the counter is allowed to be
+updated or not. The third argument is the updated counter value to
+be written to the NV counter.
+
+The function returns 0 on success. Any other value means the counter value
+either could not be updated or the authentication image descriptor indicates
+that it is not allowed to be updated.
+
+
 2.3 Common mandatory function modifications
 ---------------------------------
 
@@ -1089,10 +1121,15 @@
                unsigned int flags
     Return   : int
 
-BL1 calls this function while handling FWU copy and authenticate SMCs. The
-platform must ensure that the provided `mem_base` and `mem_size` are mapped into
-BL1, and that this memory corresponds to either a secure or non-secure memory
-region as indicated by the security state of the `flags` argument.
+BL1 calls this function while handling FWU related SMCs, more specifically when
+copying or authenticating an image. Its responsibility is to ensure that the
+region of memory identified by `mem_base` and `mem_size` is mapped in BL1, and
+that this memory corresponds to either a secure or non-secure memory region as
+indicated by the security state of the `flags` argument.
+
+This function can safely assume that the value resulting from the addition of
+`mem_base` and `mem_size` fits into a `uintptr_t` type variable and does not
+overflow.
 
 This function must return 0 on success, a non-null error code otherwise.
 
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index ac43372..5abaa1c 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -40,6 +40,13 @@
 static const gicv3_driver_data_t *driver_data;
 static unsigned int gicv2_compat;
 
+/*
+ * Redistributor power operations are weakly bound so that they can be
+ * overridden
+ */
+#pragma weak gicv3_rdistif_off
+#pragma weak gicv3_rdistif_on
+
 /*******************************************************************************
  * This function initialises the ARM GICv3 driver in EL3 with provided platform
  * inputs.
@@ -188,6 +195,9 @@
 
 	assert(IS_IN_EL3());
 
+	/* Power on redistributor */
+	gicv3_rdistif_on(proc_num);
+
 	gicr_base = driver_data->rdistif_base_addrs[proc_num];
 
 	/* Set the default attribute of all SGIs and PPIs */
@@ -211,6 +221,19 @@
 }
 
 /*******************************************************************************
+ * Functions to perform power operations on GIC Redistributor
+ ******************************************************************************/
+void gicv3_rdistif_off(unsigned int proc_num)
+{
+	return;
+}
+
+void gicv3_rdistif_on(unsigned int proc_num)
+{
+	return;
+}
+
+/*******************************************************************************
  * This function enables the GIC CPU interface of the calling CPU using only
  * system register accesses.
  ******************************************************************************/
diff --git a/drivers/auth/auth_mod.c b/drivers/auth/auth_mod.c
index 88ef0b0..2c8643f 100644
--- a/drivers/auth/auth_mod.c
+++ b/drivers/auth/auth_mod.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:
@@ -50,6 +50,8 @@
 		} \
 	} while (0)
 
+#pragma weak plat_set_nv_ctr2
+
 /* Pointer to CoT */
 extern const auth_img_desc_t *const cot_desc_ptr;
 extern unsigned int auth_img_flags[];
@@ -297,21 +299,20 @@
 		/* Invalid NV-counter */
 		return 1;
 	} else if (cert_nv_ctr > plat_nv_ctr) {
-		if (img_desc->parent == NULL) {
-			/* This certificate has been signed with the ROT key.
-			 * Update the platform counter value */
-			rc = plat_set_nv_ctr(param->plat_nv_ctr->cookie,
-					     cert_nv_ctr);
-			return_if_error(rc);
-		} else {
-			/* Secondary certificates cannot modify the counter */
-			return 1;
-		}
+		rc = plat_set_nv_ctr2(param->plat_nv_ctr->cookie,
+			img_desc, cert_nv_ctr);
+		return_if_error(rc);
 	}
 
 	return 0;
 }
 
+int plat_set_nv_ctr2(void *cookie, const auth_img_desc_t *img_desc __unused,
+		unsigned int nv_ctr)
+{
+	return plat_set_nv_ctr(cookie, nv_ctr);
+}
+
 /*
  * Return the parent id in the output parameter '*parent_id'
  *
diff --git a/drivers/auth/mbedtls/mbedtls_crypto.mk b/drivers/auth/mbedtls/mbedtls_crypto.mk
index 275ed55..b788097 100644
--- a/drivers/auth/mbedtls/mbedtls_crypto.mk
+++ b/drivers/auth/mbedtls/mbedtls_crypto.mk
@@ -31,7 +31,7 @@
 include drivers/auth/mbedtls/mbedtls_common.mk
 
 # The platform may define the variable 'MBEDTLS_KEY_ALG' to select the key
-# algorithm to use. Default algorithm is ECDSA.
+# algorithm to use. Default algorithm is RSA.
 ifeq (${MBEDTLS_KEY_ALG},)
     MBEDTLS_KEY_ALG		:=	rsa
 endif
diff --git a/include/bl32/tsp/tsp.h b/include/bl32/tsp/tsp.h
index fd43fd3..1e35788 100644
--- a/include/bl32/tsp/tsp.h
+++ b/include/bl32/tsp/tsp.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:
@@ -41,6 +41,7 @@
 #define TSP_SUSPEND_DONE	0xf2000003
 #define TSP_RESUME_DONE		0xf2000004
 #define TSP_PREEMPTED		0xf2000005
+#define TSP_ABORT_DONE		0xf2000007
 #define TSP_SYSTEM_OFF_DONE	0xf2000008
 #define TSP_SYSTEM_RESET_DONE	0xf2000009
 
@@ -66,26 +67,32 @@
 #define TSP_HANDLE_SEL1_INTR_AND_RETURN	0x2004
 
 /*
- * Generate function IDs for TSP services to be used in SMC calls, by
- * appropriately setting bit 31 to differentiate standard and fast SMC calls
- */
-#define TSP_STD_FID(fid)	((fid) | 0x72000000 | (0 << 31))
-#define TSP_FAST_FID(fid)	((fid) | 0x72000000 | (1 << 31))
-
-/* SMC function ID to request a previously preempted std smc */
-#define TSP_FID_RESUME		TSP_STD_FID(0x3000)
-
-/*
  * Identify a TSP service from function ID filtering the last 16 bits from the
  * SMC function ID
  */
 #define TSP_BARE_FID(fid)	((fid) & 0xffff)
 
 /*
+ * Generate function IDs for TSP services to be used in SMC calls, by
+ * appropriately setting bit 31 to differentiate standard and fast SMC calls
+ */
+#define TSP_STD_FID(fid)	((TSP_BARE_FID(fid) | 0x72000000))
+#define TSP_FAST_FID(fid)	((TSP_BARE_FID(fid) | 0x72000000) | (1u << 31))
+
+/* SMC function ID to request a previously preempted std smc */
+#define TSP_FID_RESUME		TSP_STD_FID(0x3000)
+/*
+ * SMC function ID to request abortion of a previously preempted std smc. A
+ * fast SMC is used so that the TSP abort handler does not have to be
+ * reentrant.
+ */
+#define TSP_FID_ABORT		TSP_FAST_FID(0x3001)
+
+/*
  * Total number of function IDs implemented for services offered to NS clients.
  * The function IDs are defined above
  */
-#define TSP_NUM_FID		0x4
+#define TSP_NUM_FID		0x5
 
 /* TSP implementation version numbers */
 #define TSP_VERSION_MAJOR	0x0 /* Major version */
@@ -118,6 +125,7 @@
 	tsp_vector_isn_t sel1_intr_entry;
 	tsp_vector_isn_t system_off_entry;
 	tsp_vector_isn_t system_reset_entry;
+	tsp_vector_isn_t abort_std_smc_entry;
 } tsp_vectors_t;
 
 
diff --git a/include/common/aarch32/assert_macros.S b/include/common/aarch32/assert_macros.S
index f35fc6a..f32ef7a 100644
--- a/include/common/aarch32/assert_macros.S
+++ b/include/common/aarch32/assert_macros.S
@@ -43,8 +43,8 @@
 .endif ;\
 	b##_cc	300f ;\
 	ldr	r0, =.L_assert_filename ;\
-	mov	r1, #__LINE__ ;\
-	b	. ;\
+	ldr	r1, =__LINE__ ;\
+	b	asm_assert;\
 300:
 
 #endif /* __ASSERT_MACROS_S__ */
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 12d5036..5076dfd 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -361,6 +361,9 @@
  ******************************************************************************/
 size_t image_size(unsigned int image_id);
 
+int is_mem_free(uintptr_t free_base, size_t free_size,
+		uintptr_t addr, size_t size);
+
 #if LOAD_IMAGE_V2
 
 int load_image(unsigned int image_id, image_info_t *image_data);
diff --git a/include/drivers/arm/gicv3.h b/include/drivers/arm/gicv3.h
index b7ad778..0f6034c 100644
--- a/include/drivers/arm/gicv3.h
+++ b/include/drivers/arm/gicv3.h
@@ -259,6 +259,8 @@
 void gicv3_driver_init(const gicv3_driver_data_t *plat_driver_data);
 void gicv3_distif_init(void);
 void gicv3_rdistif_init(unsigned int proc_num);
+void gicv3_rdistif_on(unsigned int proc_num);
+void gicv3_rdistif_off(unsigned int proc_num);
 void gicv3_cpuif_enable(unsigned int proc_num);
 void gicv3_cpuif_disable(unsigned int proc_num);
 unsigned int gicv3_get_pending_interrupt_type(void);
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index a034ae2..989667a 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -134,6 +134,16 @@
 #define ID_AA64PFR0_GIC_WIDTH	4
 #define ID_AA64PFR0_GIC_MASK	((1 << ID_AA64PFR0_GIC_WIDTH) - 1)
 
+/* ID_AA64MMFR0_EL1 definitions */
+#define ID_AA64MMFR0_EL1_PARANGE_MASK	0xf
+
+#define PARANGE_0000	32
+#define PARANGE_0001	36
+#define PARANGE_0010	40
+#define PARANGE_0011	42
+#define PARANGE_0100	44
+#define PARANGE_0101	48
+
 /* ID_PFR1_EL1 definitions */
 #define ID_PFR1_VIRTEXT_SHIFT	12
 #define ID_PFR1_VIRTEXT_MASK	0xf
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index a013809..aa26203 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -196,6 +196,7 @@
  ******************************************************************************/
 DEFINE_SYSREG_READ_FUNC(midr_el1)
 DEFINE_SYSREG_READ_FUNC(mpidr_el1)
+DEFINE_SYSREG_READ_FUNC(id_aa64mmfr0_el1)
 
 DEFINE_SYSREG_RW_FUNCS(scr_el3)
 DEFINE_SYSREG_RW_FUNCS(hcr_el2)
diff --git a/include/lib/cpus/aarch32/cpu_macros.S b/include/lib/cpus/aarch32/cpu_macros.S
index 2b9947e..17dd258 100644
--- a/include/lib/cpus/aarch32/cpu_macros.S
+++ b/include/lib/cpus/aarch32/cpu_macros.S
@@ -35,6 +35,15 @@
 #define CPU_IMPL_PN_MASK	(MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) | \
 				(MIDR_PN_MASK << MIDR_PN_SHIFT)
 
+/* The number of CPU operations allowed */
+#define CPU_MAX_PWR_DWN_OPS		2
+
+/* Special constant to specify that CPU has no reset function */
+#define CPU_NO_RESET_FUNC		0
+
+/* Word size for 32-bit CPUs */
+#define CPU_WORD_SIZE			4
+
 	/*
 	 * Define the offsets to the fields in cpu_ops structure.
 	 */
@@ -47,33 +56,86 @@
 	.space  4
 #endif
 #if IMAGE_BL32 /* The power down core and cluster is needed only in BL32 */
-CPU_PWR_DWN_CORE: /* cpu_ops core_pwr_dwn */
-	.space  4
-CPU_PWR_DWN_CLUSTER: /* cpu_ops cluster_pwr_dwn */
-	.space  4
+CPU_PWR_DWN_OPS: /* cpu_ops power down functions */
+	.space  (4 * CPU_MAX_PWR_DWN_OPS)
 #endif
 CPU_OPS_SIZE = .
 
 	/*
-	 * Convenience macro to declare cpu_ops structure.
-	 * Make sure the structure fields are as per the offsets
-	 * defined above.
+	 * Write given expressions as words
+	 *
+	 * _count:
+	 *	Write at least _count words. If the given number of expressions
+	 *	is less than _count, repeat the last expression to fill _count
+	 *	words in total
+	 * _rest:
+	 *	Optional list of expressions. _this is for parameter extraction
+	 *	only, and has no significance to the caller
+	 *
+	 * Invoked as:
+	 *	fill_constants 2, foo, bar, blah, ...
 	 */
-	.macro declare_cpu_ops _name:req, _midr:req, _noresetfunc = 0
+	.macro fill_constants _count:req, _this, _rest:vararg
+	  .ifgt \_count
+	    /* Write the current expression */
+	    .ifb \_this
+	      .error "Nothing to fill"
+	    .endif
+	    .word \_this
+
+	    /* Invoke recursively for remaining expressions */
+	    .ifnb \_rest
+	      fill_constants \_count-1, \_rest
+	    .else
+	      fill_constants \_count-1, \_this
+	    .endif
+	  .endif
+	.endm
+
+	/*
+	 * Declare CPU operations
+	 *
+	 * _name:
+	 *	Name of the CPU for which operations are being specified
+	 * _midr:
+	 *	Numeric value expected to read from CPU's MIDR
+	 * _resetfunc:
+	 *	Reset function for the CPU. If there's no CPU reset function,
+	 *	specify CPU_NO_RESET_FUNC
+	 * _power_down_ops:
+	 *	Comma-separated list of functions to perform power-down
+	 *	operatios on the CPU. At least one, and up to
+	 *	CPU_MAX_PWR_DWN_OPS number of functions may be specified.
+	 *	Starting at power level 0, these functions shall handle power
+	 *	down at subsequent power levels. If there aren't exactly
+	 *	CPU_MAX_PWR_DWN_OPS functions, the last specified one will be
+	 *	used to handle power down at subsequent levels
+	 */
+	.macro declare_cpu_ops _name:req, _midr:req, _resetfunc:req, \
+		_power_down_ops:vararg
 	.section cpu_ops, "a"
 	.align 2
 	.type cpu_ops_\_name, %object
 	.word \_midr
 #if IMAGE_BL1 || IMAGE_BL32
-	.if \_noresetfunc
-	.word 0
-	.else
-	.word \_name\()_reset_func
-	.endif
+	.word \_resetfunc
 #endif
 #if IMAGE_BL32
-	.word \_name\()_core_pwr_dwn
-	.word \_name\()_cluster_pwr_dwn
+1:
+	/* Insert list of functions */
+	fill_constants CPU_MAX_PWR_DWN_OPS, \_power_down_ops
+2:
+	/*
+	 * Error if no or more than CPU_MAX_PWR_DWN_OPS were specified in the
+	 * list
+	 */
+	.ifeq 2b - 1b
+	  .error "At least one power down function must be specified"
+	.else
+	  .iflt 2b - 1b - (CPU_MAX_PWR_DWN_OPS * CPU_WORD_SIZE)
+	    .error "More than CPU_MAX_PWR_DWN_OPS functions specified"
+	  .endif
+	.endif
 #endif
 	.endm
 
diff --git a/include/lib/cpus/aarch64/cpu_macros.S b/include/lib/cpus/aarch64/cpu_macros.S
index f34f078..570ef88 100644
--- a/include/lib/cpus/aarch64/cpu_macros.S
+++ b/include/lib/cpus/aarch64/cpu_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:
@@ -35,6 +35,15 @@
 #define CPU_IMPL_PN_MASK	(MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) | \
 				(MIDR_PN_MASK << MIDR_PN_SHIFT)
 
+/* The number of CPU operations allowed */
+#define CPU_MAX_PWR_DWN_OPS		2
+
+/* Special constant to specify that CPU has no reset function */
+#define CPU_NO_RESET_FUNC		0
+
+/* Word size for 64-bit CPUs */
+#define CPU_WORD_SIZE			8
+
 	/*
 	 * Define the offsets to the fields in cpu_ops structure.
 	 */
@@ -47,10 +56,8 @@
 	.space  8
 #endif
 #if IMAGE_BL31 /* The power down core and cluster is needed only in BL31 */
-CPU_PWR_DWN_CORE: /* cpu_ops core_pwr_dwn */
-	.space  8
-CPU_PWR_DWN_CLUSTER: /* cpu_ops cluster_pwr_dwn */
-	.space  8
+CPU_PWR_DWN_OPS: /* cpu_ops power down functions */
+	.space  (8 * CPU_MAX_PWR_DWN_OPS)
 #endif
 #if (IMAGE_BL31 && CRASH_REPORTING)
 CPU_REG_DUMP: /* cpu specific register dump for crash reporting */
@@ -59,24 +66,80 @@
 CPU_OPS_SIZE = .
 
 	/*
-	 * Convenience macro to declare cpu_ops structure.
-	 * Make sure the structure fields are as per the offsets
-	 * defined above.
+	 * Write given expressions as quad words
+	 *
+	 * _count:
+	 *	Write at least _count quad words. If the given number of
+	 *	expressions is less than _count, repeat the last expression to
+	 *	fill _count quad words in total
+	 * _rest:
+	 *	Optional list of expressions. _this is for parameter extraction
+	 *	only, and has no significance to the caller
+	 *
+	 * Invoked as:
+	 *	fill_constants 2, foo, bar, blah, ...
 	 */
-	.macro declare_cpu_ops _name:req, _midr:req, _noresetfunc = 0
-	.section cpu_ops, "a"; .align 3
+	.macro fill_constants _count:req, _this, _rest:vararg
+	  .ifgt \_count
+	    /* Write the current expression */
+	    .ifb \_this
+	      .error "Nothing to fill"
+	    .endif
+	    .quad \_this
+
+	    /* Invoke recursively for remaining expressions */
+	    .ifnb \_rest
+	      fill_constants \_count-1, \_rest
+	    .else
+	      fill_constants \_count-1, \_this
+	    .endif
+	  .endif
+	.endm
+
+	/*
+	 * Declare CPU operations
+	 *
+	 * _name:
+	 *	Name of the CPU for which operations are being specified
+	 * _midr:
+	 *	Numeric value expected to read from CPU's MIDR
+	 * _resetfunc:
+	 *	Reset function for the CPU. If there's no CPU reset function,
+	 *	specify CPU_NO_RESET_FUNC
+	 * _power_down_ops:
+	 *	Comma-separated list of functions to perform power-down
+	 *	operatios on the CPU. At least one, and up to
+	 *	CPU_MAX_PWR_DWN_OPS number of functions may be specified.
+	 *	Starting at power level 0, these functions shall handle power
+	 *	down at subsequent power levels. If there aren't exactly
+	 *	CPU_MAX_PWR_DWN_OPS functions, the last specified one will be
+	 *	used to handle power down at subsequent levels
+	 */
+	.macro declare_cpu_ops _name:req, _midr:req, _resetfunc:req, \
+		_power_down_ops:vararg
+	.section cpu_ops, "a"
+	.align 3
 	.type cpu_ops_\_name, %object
 	.quad \_midr
 #if IMAGE_BL1 || IMAGE_BL31
-	.if \_noresetfunc
-	.quad 0
-	.else
-	.quad \_name\()_reset_func
-	.endif
+	.quad \_resetfunc
 #endif
 #if IMAGE_BL31
-	.quad \_name\()_core_pwr_dwn
-	.quad \_name\()_cluster_pwr_dwn
+1:
+	/* Insert list of functions */
+	fill_constants CPU_MAX_PWR_DWN_OPS, \_power_down_ops
+2:
+	/*
+	 * Error if no or more than CPU_MAX_PWR_DWN_OPS were specified in the
+	 * list
+	 */
+	.ifeq 2b - 1b
+	  .error "At least one power down function must be specified"
+	.else
+	  .iflt 2b - 1b - (CPU_MAX_PWR_DWN_OPS * CPU_WORD_SIZE)
+	    .error "More than CPU_MAX_PWR_DWN_OPS functions specified"
+	  .endif
+	.endif
 #endif
 #if (IMAGE_BL31 && CRASH_REPORTING)
 	.quad \_name\()_cpu_reg_dump
diff --git a/include/lib/runtime_instr.h b/include/lib/runtime_instr.h
index d409002..4d05ba4 100644
--- a/include/lib/runtime_instr.h
+++ b/include/lib/runtime_instr.h
@@ -31,11 +31,13 @@
 #ifndef __RUNTIME_INSTR_H__
 #define __RUNTIME_INSTR_H__
 
-#define RT_INSTR_TOTAL_IDS		4
 #define RT_INSTR_ENTER_PSCI		0
 #define RT_INSTR_EXIT_PSCI		1
 #define RT_INSTR_ENTER_HW_LOW_PWR	2
 #define RT_INSTR_EXIT_HW_LOW_PWR	3
+#define RT_INSTR_ENTER_CFLUSH		4
+#define RT_INSTR_EXIT_CFLUSH		5
+#define RT_INSTR_TOTAL_IDS		6
 
 #ifndef __ASSEMBLY__
 PMF_DECLARE_CAPTURE_TIMESTAMP(rt_instr_svc)
diff --git a/include/lib/stdlib/sys/uuid.h b/include/lib/stdlib/sys/uuid.h
index d43b641..6d935bd 100644
--- a/include/lib/stdlib/sys/uuid.h
+++ b/include/lib/stdlib/sys/uuid.h
@@ -37,6 +37,9 @@
 /* Length of a node address (an IEEE 802 address). */
 #define	_UUID_NODE_LEN		6
 
+/* Length of UUID string including dashes. */
+#define _UUID_STR_LEN		36
+
 /*
  * See also:
  *      http://www.opengroup.org/dce/info/draft-leach-uuids-guids-01.txt
diff --git a/include/lib/xlat_tables.h b/include/lib/xlat_tables.h
index 0e9800a..f447618 100644
--- a/include/lib/xlat_tables.h
+++ b/include/lib/xlat_tables.h
@@ -93,6 +93,11 @@
 #define L2_XLAT_ADDRESS_SHIFT	(L3_XLAT_ADDRESS_SHIFT + XLAT_TABLE_ENTRIES_SHIFT)
 #define L1_XLAT_ADDRESS_SHIFT	(L2_XLAT_ADDRESS_SHIFT + XLAT_TABLE_ENTRIES_SHIFT)
 #define L0_XLAT_ADDRESS_SHIFT	(L1_XLAT_ADDRESS_SHIFT + XLAT_TABLE_ENTRIES_SHIFT)
+#define XLAT_ADDR_SHIFT(level)	(PAGE_SIZE_SHIFT + \
+		  ((XLAT_TABLE_LEVEL_MAX - (level)) * XLAT_TABLE_ENTRIES_SHIFT))
+
+#define XLAT_BLOCK_SIZE(level)	((u_register_t)1 << XLAT_ADDR_SHIFT(level))
+#define XLAT_BLOCK_MASK(level)	(XLAT_BLOCK_SIZE(level) - 1)
 
 /*
  * AP[1] bit is ignored by hardware and is
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 4a4dfd4..6d7bcd1 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -205,7 +205,8 @@
  * Required platform porting definitions common to all ARM standard platforms
  *****************************************************************************/
 
-#define ADDR_SPACE_SIZE			(1ull << 32)
+#define PLAT_PHY_ADDR_SPACE_SIZE			(1ull << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE			(1ull << 32)
 
 /*
  * This macro defines the deepest retention state possible. A higher state
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index bc32e40..c167aa2 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -194,6 +194,8 @@
 void plat_arm_gic_init(void);
 void plat_arm_gic_cpuif_enable(void);
 void plat_arm_gic_cpuif_disable(void);
+void plat_arm_gic_redistif_on(void);
+void plat_arm_gic_redistif_off(void);
 void plat_arm_gic_pcpu_init(void);
 void plat_arm_security_setup(void);
 void plat_arm_pwrc_setup(void);
diff --git a/include/plat/arm/css/common/css_def.h b/include/plat/arm/css/common/css_def.h
index 173de1b..a2fe0d5 100644
--- a/include/plat/arm/css/common/css_def.h
+++ b/include/plat/arm/css/common/css_def.h
@@ -135,8 +135,10 @@
  * SCP, it is discarded and BL31 is loaded over the top.
  */
 #define SCP_BL2_BASE			BL31_BASE
+#define SCP_BL2_LIMIT			(SCP_BL2_BASE + PLAT_CSS_MAX_SCP_BL2_SIZE)
 
 #define SCP_BL2U_BASE			BL31_BASE
+#define SCP_BL2U_LIMIT			(SCP_BL2U_BASE + PLAT_CSS_MAX_SCP_BL2U_SIZE)
 #endif /* CSS_LOAD_SCP_IMAGES */
 
 /* Load address of Non-Secure Image for CSS platform ports */
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 5b4d11d..f904292 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -39,6 +39,7 @@
 /*******************************************************************************
  * Forward declarations
  ******************************************************************************/
+struct auth_img_desc_s;
 struct meminfo;
 struct image_info;
 struct entry_point_info;
@@ -274,6 +275,8 @@
 			unsigned int *flags);
 int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr);
 int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr);
+int plat_set_nv_ctr2(void *cookie, const struct auth_img_desc_s *img_desc,
+		unsigned int nv_ctr);
 
 #if LOAD_IMAGE_V2
 /*******************************************************************************
diff --git a/lib/cpus/aarch32/aem_generic.S b/lib/cpus/aarch32/aem_generic.S
index 10ea4e4..3d6064c 100644
--- a/lib/cpus/aarch32/aem_generic.S
+++ b/lib/cpus/aarch32/aem_generic.S
@@ -65,4 +65,6 @@
 endfunc aem_generic_cluster_pwr_dwn
 
 /* cpu_ops for Base AEM FVP */
-declare_cpu_ops aem_generic, BASE_AEM_MIDR, 1
+declare_cpu_ops aem_generic, BASE_AEM_MIDR, CPU_NO_RESET_FUNC, \
+	aem_generic_core_pwr_dwn, \
+	aem_generic_cluster_pwr_dwn
diff --git a/lib/cpus/aarch32/cortex_a32.S b/lib/cpus/aarch32/cortex_a32.S
index f2b85a3..f631c4c 100644
--- a/lib/cpus/aarch32/cortex_a32.S
+++ b/lib/cpus/aarch32/cortex_a32.S
@@ -141,4 +141,7 @@
 	b	cortex_a32_disable_smp
 endfunc cortex_a32_cluster_pwr_dwn
 
-declare_cpu_ops cortex_a32, CORTEX_A32_MIDR
+declare_cpu_ops cortex_a32, CORTEX_A32_MIDR, \
+	cortex_a32_reset_func, \
+	cortex_a32_core_pwr_dwn, \
+	cortex_a32_cluster_pwr_dwn
diff --git a/lib/cpus/aarch32/cpu_helpers.S b/lib/cpus/aarch32/cpu_helpers.S
index a4dfe5f..900d158 100644
--- a/lib/cpus/aarch32/cpu_helpers.S
+++ b/lib/cpus/aarch32/cpu_helpers.S
@@ -70,50 +70,39 @@
 
 #if IMAGE_BL32 /* The power down core and cluster is needed only in  BL32 */
 	/*
-	 * The prepare core power down function for all platforms.  After
-	 * the cpu_ops pointer is retrieved from cpu_data, the corresponding
-	 * pwr_dwn_core in the cpu_ops is invoked. Follows AAPCS.
+	 * void prepare_cpu_pwr_dwn(unsigned int power_level)
+	 *
+	 * Prepare CPU power down function for all platforms. The function takes
+	 * a domain level to be powered down as its parameter. After the cpu_ops
+	 * pointer is retrieved from cpu_data, the handler for requested power
+	 * level is called.
 	 */
-	.globl	prepare_core_pwr_dwn
-func prepare_core_pwr_dwn
-	/* r12 is pushed to meet the 8 byte stack alignment requirement */
-	push	{r12, lr}
-	bl	_cpu_data
-	pop	{r12, lr}
-
-	ldr	r1, [r0, #CPU_DATA_CPU_OPS_PTR]
-#if ASM_ASSERTION
-	cmp	r1, #0
-	ASM_ASSERT(ne)
-#endif
-
-	/* Get the cpu_ops core_pwr_dwn handler */
-	ldr	r0, [r1, #CPU_PWR_DWN_CORE]
-	bx	r0
-endfunc prepare_core_pwr_dwn
-
+	.globl	prepare_cpu_pwr_dwn
+func prepare_cpu_pwr_dwn
 	/*
-	 * The prepare cluster power down function for all platforms.  After
-	 * the cpu_ops pointer is retrieved from cpu_data, the corresponding
-	 * pwr_dwn_cluster in the cpu_ops is invoked. Follows AAPCS.
+	 * If the given power level exceeds CPU_MAX_PWR_DWN_OPS, we call the
+	 * power down handler for the last power level
 	 */
-	.globl	prepare_cluster_pwr_dwn
-func prepare_cluster_pwr_dwn
-	/* r12 is pushed to meet the 8 byte stack alignment requirement */
-	push	{r12, lr}
-	bl	_cpu_data
-	pop	{r12, lr}
+	mov	r2, #(CPU_MAX_PWR_DWN_OPS - 1)
+	cmp	r0, r2
+	movhi	r0, r2
 
-	ldr	r1, [r0, #CPU_DATA_CPU_OPS_PTR]
+	push	{r0, lr}
+	bl	_cpu_data
+	pop	{r2, lr}
+
+	ldr	r0, [r0, #CPU_DATA_CPU_OPS_PTR]
 #if ASM_ASSERTION
-	cmp	r1, #0
+	cmp	r0, #0
 	ASM_ASSERT(ne)
 #endif
 
-	/* Get the cpu_ops cluster_pwr_dwn handler */
-	ldr	r0, [r1, #CPU_PWR_DWN_CLUSTER]
-	bx	r0
-endfunc prepare_cluster_pwr_dwn
+	/* Get the appropriate power down handler */
+	mov	r1, #CPU_PWR_DWN_OPS
+	add	r1, r1, r2, lsl #2
+	ldr	r1, [r0, r1]
+	bx	r1
+endfunc prepare_cpu_pwr_dwn
 
 	/*
 	 * Initializes the cpu_ops_ptr if not already initialized
diff --git a/lib/cpus/aarch64/aem_generic.S b/lib/cpus/aarch64/aem_generic.S
index 0ab5253..0cedd85 100644
--- a/lib/cpus/aarch64/aem_generic.S
+++ b/lib/cpus/aarch64/aem_generic.S
@@ -90,7 +90,11 @@
 
 
 /* cpu_ops for Base AEM FVP */
-declare_cpu_ops aem_generic, BASE_AEM_MIDR, 1
+declare_cpu_ops aem_generic, BASE_AEM_MIDR, CPU_NO_RESET_FUNC, \
+	aem_generic_core_pwr_dwn, \
+	aem_generic_cluster_pwr_dwn
 
 /* cpu_ops for Foundation FVP */
-declare_cpu_ops aem_generic, FOUNDATION_AEM_MIDR, 1
+declare_cpu_ops aem_generic, FOUNDATION_AEM_MIDR, CPU_NO_RESET_FUNC, \
+	aem_generic_core_pwr_dwn, \
+	aem_generic_cluster_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_a35.S b/lib/cpus/aarch64/cortex_a35.S
index ba29d6d..c17c8f1 100644
--- a/lib/cpus/aarch64/cortex_a35.S
+++ b/lib/cpus/aarch64/cortex_a35.S
@@ -157,4 +157,7 @@
 	ret
 endfunc cortex_a35_cpu_reg_dump
 
-declare_cpu_ops cortex_a35, CORTEX_A35_MIDR
+declare_cpu_ops cortex_a35, CORTEX_A35_MIDR, \
+	cortex_a35_reset_func, \
+	cortex_a35_core_pwr_dwn, \
+	cortex_a35_cluster_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_a53.S b/lib/cpus/aarch64/cortex_a53.S
index ed546e7..06be9ce 100644
--- a/lib/cpus/aarch64/cortex_a53.S
+++ b/lib/cpus/aarch64/cortex_a53.S
@@ -244,4 +244,7 @@
 	ret
 endfunc cortex_a53_cpu_reg_dump
 
-declare_cpu_ops cortex_a53, CORTEX_A53_MIDR
+declare_cpu_ops cortex_a53, CORTEX_A53_MIDR, \
+	cortex_a53_reset_func, \
+	cortex_a53_core_pwr_dwn, \
+	cortex_a53_cluster_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_a57.S b/lib/cpus/aarch64/cortex_a57.S
index d6b181d..e531b1e 100644
--- a/lib/cpus/aarch64/cortex_a57.S
+++ b/lib/cpus/aarch64/cortex_a57.S
@@ -488,4 +488,7 @@
 endfunc cortex_a57_cpu_reg_dump
 
 
-declare_cpu_ops cortex_a57, CORTEX_A57_MIDR
+declare_cpu_ops cortex_a57, CORTEX_A57_MIDR, \
+	cortex_a57_reset_func, \
+	cortex_a57_core_pwr_dwn, \
+	cortex_a57_cluster_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_a72.S b/lib/cpus/aarch64/cortex_a72.S
index 9f04fb7..fffc99f 100644
--- a/lib/cpus/aarch64/cortex_a72.S
+++ b/lib/cpus/aarch64/cortex_a72.S
@@ -242,4 +242,7 @@
 endfunc cortex_a72_cpu_reg_dump
 
 
-declare_cpu_ops cortex_a72, CORTEX_A72_MIDR
+declare_cpu_ops cortex_a72, CORTEX_A72_MIDR, \
+	cortex_a72_reset_func, \
+	cortex_a72_core_pwr_dwn, \
+	cortex_a72_cluster_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_a73.S b/lib/cpus/aarch64/cortex_a73.S
index e1615db..49d5449 100644
--- a/lib/cpus/aarch64/cortex_a73.S
+++ b/lib/cpus/aarch64/cortex_a73.S
@@ -153,4 +153,7 @@
 	ret
 endfunc cortex_a73_cpu_reg_dump
 
-declare_cpu_ops cortex_a73, CORTEX_A73_MIDR
+declare_cpu_ops cortex_a73, CORTEX_A73_MIDR, \
+	cortex_a73_reset_func, \
+	cortex_a73_core_pwr_dwn, \
+	cortex_a73_cluster_pwr_dwn
diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S
index dab933c..ec7f1dd 100644
--- a/lib/cpus/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -74,31 +74,23 @@
 
 #if IMAGE_BL31 /* The power down core and cluster is needed only in  BL31 */
 	/*
-	 * The prepare core power down function for all platforms.  After
-	 * the cpu_ops pointer is retrieved from cpu_data, the corresponding
-	 * pwr_dwn_core in the cpu_ops is invoked.
+	 * void prepare_cpu_pwr_dwn(unsigned int power_level)
+	 *
+	 * Prepare CPU power down function for all platforms. The function takes
+	 * a domain level to be powered down as its parameter. After the cpu_ops
+	 * pointer is retrieved from cpu_data, the handler for requested power
+	 * level is called.
 	 */
-	.globl	prepare_core_pwr_dwn
-func prepare_core_pwr_dwn
-	mrs	x1, tpidr_el3
-	ldr	x0, [x1, #CPU_DATA_CPU_OPS_PTR]
-#if ASM_ASSERTION
-	cmp	x0, #0
-	ASM_ASSERT(ne)
-#endif
-
-	/* Get the cpu_ops core_pwr_dwn handler */
-	ldr	x1, [x0, #CPU_PWR_DWN_CORE]
-	br	x1
-endfunc prepare_core_pwr_dwn
-
+	.globl	prepare_cpu_pwr_dwn
+func prepare_cpu_pwr_dwn
 	/*
-	 * The prepare cluster power down function for all platforms.  After
-	 * the cpu_ops pointer is retrieved from cpu_data, the corresponding
-	 * pwr_dwn_cluster in the cpu_ops is invoked.
+	 * If the given power level exceeds CPU_MAX_PWR_DWN_OPS, we call the
+	 * power down handler for the last power level
 	 */
-	.globl	prepare_cluster_pwr_dwn
-func prepare_cluster_pwr_dwn
+	mov_imm	x2, (CPU_MAX_PWR_DWN_OPS - 1)
+	cmp	x0, x2
+	csel	x2, x2, x0, hi
+
 	mrs	x1, tpidr_el3
 	ldr	x0, [x1, #CPU_DATA_CPU_OPS_PTR]
 #if ASM_ASSERTION
@@ -106,10 +98,12 @@
 	ASM_ASSERT(ne)
 #endif
 
-	/* Get the cpu_ops cluster_pwr_dwn handler */
-	ldr	x1, [x0, #CPU_PWR_DWN_CLUSTER]
+	/* Get the appropriate power down handler */
+	mov	x1, #CPU_PWR_DWN_OPS
+	add	x1, x1, x2, lsl #3
+	ldr	x1, [x0, x1]
 	br	x1
-endfunc prepare_cluster_pwr_dwn
+endfunc prepare_cpu_pwr_dwn
 
 
 	/*
diff --git a/lib/cpus/aarch64/denver.S b/lib/cpus/aarch64/denver.S
index bce0573..0b61440 100644
--- a/lib/cpus/aarch64/denver.S
+++ b/lib/cpus/aarch64/denver.S
@@ -163,4 +163,7 @@
 	ret
 endfunc denver_cpu_reg_dump
 
-declare_cpu_ops denver, DENVER_1_0_MIDR
+declare_cpu_ops denver, DENVER_1_0_MIDR, \
+	denver_reset_func, \
+	denver_core_pwr_dwn, \
+	denver_cluster_pwr_dwn
diff --git a/lib/psci/aarch32/psci_helpers.S b/lib/psci/aarch32/psci_helpers.S
index 5a41ff3..9f991df 100644
--- a/lib/psci/aarch32/psci_helpers.S
+++ b/lib/psci/aarch32/psci_helpers.S
@@ -65,22 +65,13 @@
 	bl	do_stack_maintenance
 
 	/* ---------------------------------------------
-	 * Determine how many levels of cache will be
-	 * subject to cache maintenance. Power level
-	 * 0 implies that only the cpu is being powered
-	 * down. Only the L1 data cache needs to be
-	 * flushed to the PoU in this case. For a higher
-	 * power level we are assuming that a flush
-	 * of L1 data and L2 unified cache is enough.
-	 * This information should be provided by the
-	 * platform.
+	 * Invoke CPU-specifc power down operations for
+	 * the appropriate level
 	 * ---------------------------------------------
 	 */
-	cmp	r4, #PSCI_CPU_PWR_LVL
-	pop	{r4,lr}
-
-	beq	prepare_core_pwr_dwn
-	b	prepare_cluster_pwr_dwn
+	mov	r0, r4
+	pop	{r4, lr}
+	b	prepare_cpu_pwr_dwn
 endfunc psci_do_pwrdown_cache_maintenance
 
 
diff --git a/lib/psci/aarch64/psci_helpers.S b/lib/psci/aarch64/psci_helpers.S
index eaa17c7..108f068 100644
--- a/lib/psci/aarch64/psci_helpers.S
+++ b/lib/psci/aarch64/psci_helpers.S
@@ -59,24 +59,11 @@
 	stp     x19, x20, [sp,#-16]!
 
 	/* ---------------------------------------------
-	 * Determine to how many levels of cache will be
-	 * subject to cache maintenance. Power level
-	 * 0 implies that only the cpu is being powered
-	 * down. Only the L1 data cache needs to be
-	 * flushed to the PoU in this case. For a higher
-	 * power level we are assuming that a flush
-	 * of L1 data and L2 unified cache is enough.
-	 * This information should be provided by the
-	 * platform.
+	 * Invoke CPU-specific power down operations for
+	 * the appropriate level
 	 * ---------------------------------------------
 	 */
-	cmp	w0, #PSCI_CPU_PWR_LVL
-	b.eq	do_core_pwr_dwn
-	bl	prepare_cluster_pwr_dwn
-	b	do_stack_maintenance
-
-do_core_pwr_dwn:
-	bl	prepare_core_pwr_dwn
+	bl	prepare_cpu_pwr_dwn
 
 	/* ---------------------------------------------
 	 * Do stack maintenance by flushing the used
@@ -84,7 +71,6 @@
 	 * remainder.
 	 * ---------------------------------------------
 	 */
-do_stack_maintenance:
 	bl	plat_get_my_stack
 
 	/* ---------------------------------------------
diff --git a/lib/psci/psci_off.c b/lib/psci/psci_off.c
index 1cc6ede..897bf31 100644
--- a/lib/psci/psci_off.c
+++ b/lib/psci/psci_off.c
@@ -107,12 +107,29 @@
 	psci_stats_update_pwr_down(end_pwrlvl, &state_info);
 #endif
 
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+	/*
+	 * Flush cache line so that even if CPU power down happens
+	 * the timestamp update is reflected in memory.
+	 */
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_ENTER_CFLUSH,
+		PMF_CACHE_MAINT);
+#endif
+
 	/*
 	 * Arch. management. Perform the necessary steps to flush all
 	 * cpu caches.
 	 */
 	psci_do_pwrdown_cache_maintenance(psci_find_max_off_lvl(&state_info));
 
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_EXIT_CFLUSH,
+		PMF_NO_CACHE_MAINT);
+#endif
+
 	/*
 	 * Plat. management: Perform platform specific actions to turn this
 	 * cpu off e.g. exit cpu coherency, program the power controller etc.
diff --git a/lib/psci/psci_suspend.c b/lib/psci/psci_suspend.c
index 10d2481..dc2ab77 100644
--- a/lib/psci/psci_suspend.c
+++ b/lib/psci/psci_suspend.c
@@ -109,6 +109,17 @@
 	 */
 	cm_init_my_context(ep);
 
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+	/*
+	 * Flush cache line so that even if CPU power down happens
+	 * the timestamp update is reflected in memory.
+	 */
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_ENTER_CFLUSH,
+		PMF_CACHE_MAINT);
+#endif
+
 	/*
 	 * Arch. management. Perform the necessary steps to flush all
 	 * cpu caches. Currently we assume that the power level correspond
@@ -117,6 +128,12 @@
 	 * and the cpu-ops power down to perform from the platform.
 	 */
 	psci_do_pwrdown_cache_maintenance(max_off_lvl);
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
+		RT_INSTR_EXIT_CFLUSH,
+		PMF_NO_CACHE_MAINT);
+#endif
 }
 
 /*******************************************************************************
diff --git a/lib/xlat_tables/aarch32/xlat_tables.c b/lib/xlat_tables/aarch32/xlat_tables.c
index d70a6ef..e8408da 100644
--- a/lib/xlat_tables/aarch32/xlat_tables.c
+++ b/lib/xlat_tables/aarch32/xlat_tables.c
@@ -39,49 +39,60 @@
 
 /*
  * Each platform can define the size of the virtual address space, which is
- * defined in ADDR_SPACE_SIZE. TTBCR.TxSZ is calculated as 32 minus the width
- * of said address space. The value of TTBCR.TxSZ must be in the range 0 to
- * 7 [1], which means that the virtual address space width must be in the range
- * 32 to 25 bits.
+ * defined in PLAT_VIRT_ADDR_SPACE_SIZE. TTBCR.TxSZ is calculated as 32 minus
+ * the width of said address space. The value of TTBCR.TxSZ must be in the
+ * range 0 to 7 [1], which means that the virtual address space width must be
+ * in the range 32 to 25 bits.
  *
- * Here we calculate the initial lookup level from the value of ADDR_SPACE_SIZE.
- * For a 4 KB page size, level 1 supports virtual address spaces of widths 32
- * to 31 bits, and level 2 from 30 to 25. Wider or narrower address spaces are
- * not supported. As a result, level 3 cannot be used as initial lookup level
- * with 4 KB granularity [1].
+ * Here we calculate the initial lookup level from the value of
+ * PLAT_VIRT_ADDR_SPACE_SIZE. For a 4 KB page size, level 1 supports virtual
+ * address spaces of widths 32 to 31 bits, and level 2 from 30 to 25. Wider or
+ * narrower address spaces are not supported. As a result, level 3 cannot be
+ * used as initial lookup level with 4 KB granularity [1].
  *
- * For example, for a 31-bit address space (i.e. ADDR_SPACE_SIZE == 1 << 31),
- * TTBCR.TxSZ will be programmed to (32 - 31) = 1. According to Table G4-5 in
- * the ARM ARM, the initial lookup level for such an address space is 1.
+ * For example, for a 31-bit address space (i.e. PLAT_VIRT_ADDR_SPACE_SIZE ==
+ * 1 << 31), TTBCR.TxSZ will be programmed to (32 - 31) = 1. According to Table
+ * G4-5 in the ARM ARM, the initial lookup level for an address space like that
+ * is 1.
  *
  * See the ARMv8-A Architecture Reference Manual (DDI 0487A.j) for more
  * information:
  * [1] Section G4.6.5
  */
 
-#if ADDR_SPACE_SIZE > (1ULL << (32 - TTBCR_TxSZ_MIN))
+#if PLAT_VIRT_ADDR_SPACE_SIZE > (1ULL << (32 - TTBCR_TxSZ_MIN))
 
-# error "ADDR_SPACE_SIZE is too big."
+# error "PLAT_VIRT_ADDR_SPACE_SIZE is too big."
 
-#elif ADDR_SPACE_SIZE > (1 << L1_XLAT_ADDRESS_SHIFT)
+#elif PLAT_VIRT_ADDR_SPACE_SIZE > (1 << L1_XLAT_ADDRESS_SHIFT)
 
 # define XLAT_TABLE_LEVEL_BASE	1
-# define NUM_BASE_LEVEL_ENTRIES	(ADDR_SPACE_SIZE >> L1_XLAT_ADDRESS_SHIFT)
+# define NUM_BASE_LEVEL_ENTRIES	\
+		(PLAT_VIRT_ADDR_SPACE_SIZE >> L1_XLAT_ADDRESS_SHIFT)
 
-#elif ADDR_SPACE_SIZE >= (1 << (32 - TTBCR_TxSZ_MAX))
+#elif PLAT_VIRT_ADDR_SPACE_SIZE >= (1 << (32 - TTBCR_TxSZ_MAX))
 
 # define XLAT_TABLE_LEVEL_BASE	2
-# define NUM_BASE_LEVEL_ENTRIES	(ADDR_SPACE_SIZE >> L2_XLAT_ADDRESS_SHIFT)
+# define NUM_BASE_LEVEL_ENTRIES	\
+		(PLAT_VIRT_ADDR_SPACE_SIZE >> L2_XLAT_ADDRESS_SHIFT)
 
 #else
 
-# error "ADDR_SPACE_SIZE is too small."
+# error "PLAT_VIRT_ADDR_SPACE_SIZE is too small."
 
 #endif
 
 static uint64_t base_xlation_table[NUM_BASE_LEVEL_ENTRIES]
 		__aligned(NUM_BASE_LEVEL_ENTRIES * sizeof(uint64_t));
 
+#if DEBUG
+static unsigned long long get_max_supported_pa(void)
+{
+	/* Physical address space size for long descriptor format. */
+	return (1ULL << 40) - 1ULL;
+}
+#endif
+
 void init_xlat_tables(void)
 {
 	unsigned long long max_pa;
@@ -89,7 +100,10 @@
 	print_mmap();
 	init_xlation_table(0, base_xlation_table, XLAT_TABLE_LEVEL_BASE,
 						&max_va, &max_pa);
-	assert(max_va < ADDR_SPACE_SIZE);
+
+	assert(max_va <= PLAT_VIRT_ADDR_SPACE_SIZE - 1);
+	assert(max_pa <= PLAT_PHY_ADDR_SPACE_SIZE - 1);
+	assert((PLAT_PHY_ADDR_SPACE_SIZE - 1) <= get_max_supported_pa());
 }
 
 /*******************************************************************************
@@ -122,7 +136,7 @@
 	ttbcr = TTBCR_EAE_BIT |
 		TTBCR_SH0_INNER_SHAREABLE | TTBCR_RGN0_OUTER_WBA |
 		TTBCR_RGN0_INNER_WBA |
-		(32 - __builtin_ctzl((uintptr_t)ADDR_SPACE_SIZE));
+		(32 - __builtin_ctzl((uintptr_t)PLAT_VIRT_ADDR_SPACE_SIZE));
 	ttbcr |= TTBCR_EPD1_BIT;
 	write_ttbcr(ttbcr);
 
diff --git a/lib/xlat_tables/aarch64/xlat_tables.c b/lib/xlat_tables/aarch64/xlat_tables.c
index 5b639b7..a168636 100644
--- a/lib/xlat_tables/aarch64/xlat_tables.c
+++ b/lib/xlat_tables/aarch64/xlat_tables.c
@@ -31,28 +31,33 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <assert.h>
+#include <bl_common.h>
 #include <cassert.h>
+#include <common_def.h>
 #include <platform_def.h>
+#include <sys/types.h>
 #include <utils.h>
 #include <xlat_tables.h>
 #include "../xlat_tables_private.h"
 
 /*
  * Each platform can define the size of the virtual address space, which is
- * defined in ADDR_SPACE_SIZE. TCR.TxSZ is calculated as 64 minus the width of
- * said address space. The value of TCR.TxSZ must be in the range 16 to 39 [1],
- * which means that the virtual address space width must be in the range 48 to
- * 25 bits.
+ * defined in PLAT_VIRT_ADDR_SPACE_SIZE. TCR.TxSZ is calculated as 64 minus the
+ * width of said address space. The value of TCR.TxSZ must be in the range 16
+ * to 39 [1], which means that the virtual address space width must be in the
+ * range 48 to 25 bits.
  *
- * Here we calculate the initial lookup level from the value of ADDR_SPACE_SIZE.
- * For a 4 KB page size, level 0 supports virtual address spaces of widths 48 to
- * 40 bits, level 1 from 39 to 31, and level 2 from 30 to 25. Wider or narrower
- * address spaces are not supported. As a result, level 3 cannot be used as
- * initial lookup level with 4 KB granularity. [2]
+ * Here we calculate the initial lookup level from the value of
+ * PLAT_VIRT_ADDR_SPACE_SIZE. For a 4 KB page size, level 0 supports virtual
+ * address spaces of widths 48 to 40 bits, level 1 from 39 to 31, and level 2
+ * from 30 to 25. Wider or narrower address spaces are not supported. As a
+ * result, level 3 cannot be used as initial lookup level with 4 KB
+ * granularity. [2]
  *
- * For example, for a 35-bit address space (i.e. ADDR_SPACE_SIZE == 1 << 35),
- * TCR.TxSZ will be programmed to (64 - 35) = 29. According to Table D4-11 in
- * the ARM ARM, the initial lookup level for such an address space is 1.
+ * For example, for a 35-bit address space (i.e. PLAT_VIRT_ADDR_SPACE_SIZE ==
+ * 1 << 35), TCR.TxSZ will be programmed to (64 - 35) = 29. According to Table
+ * D4-11 in the ARM ARM, the initial lookup level for an address space like
+ * that is 1.
  *
  * See the ARMv8-A Architecture Reference Manual (DDI 0487A.j) for more
  * information:
@@ -60,28 +65,31 @@
  * [2] Section D4.2.5
  */
 
-#if ADDR_SPACE_SIZE > (1ULL << (64 - TCR_TxSZ_MIN))
+#if PLAT_VIRT_ADDR_SPACE_SIZE > (1ULL << (64 - TCR_TxSZ_MIN))
 
-# error "ADDR_SPACE_SIZE is too big."
+# error "PLAT_VIRT_ADDR_SPACE_SIZE is too big."
 
-#elif ADDR_SPACE_SIZE > (1ULL << L0_XLAT_ADDRESS_SHIFT)
+#elif PLAT_VIRT_ADDR_SPACE_SIZE > (1ULL << L0_XLAT_ADDRESS_SHIFT)
 
 # define XLAT_TABLE_LEVEL_BASE	0
-# define NUM_BASE_LEVEL_ENTRIES	(ADDR_SPACE_SIZE >> L0_XLAT_ADDRESS_SHIFT)
+# define NUM_BASE_LEVEL_ENTRIES	\
+		(PLAT_VIRT_ADDR_SPACE_SIZE >> L0_XLAT_ADDRESS_SHIFT)
 
-#elif ADDR_SPACE_SIZE > (1 << L1_XLAT_ADDRESS_SHIFT)
+#elif PLAT_VIRT_ADDR_SPACE_SIZE > (1 << L1_XLAT_ADDRESS_SHIFT)
 
 # define XLAT_TABLE_LEVEL_BASE	1
-# define NUM_BASE_LEVEL_ENTRIES	(ADDR_SPACE_SIZE >> L1_XLAT_ADDRESS_SHIFT)
+# define NUM_BASE_LEVEL_ENTRIES	\
+		(PLAT_VIRT_ADDR_SPACE_SIZE >> L1_XLAT_ADDRESS_SHIFT)
 
-#elif ADDR_SPACE_SIZE >= (1 << (64 - TCR_TxSZ_MAX))
+#elif PLAT_VIRT_ADDR_SPACE_SIZE >= (1 << (64 - TCR_TxSZ_MAX))
 
 # define XLAT_TABLE_LEVEL_BASE	2
-# define NUM_BASE_LEVEL_ENTRIES	(ADDR_SPACE_SIZE >> L2_XLAT_ADDRESS_SHIFT)
+# define NUM_BASE_LEVEL_ENTRIES	\
+		(PLAT_VIRT_ADDR_SPACE_SIZE >> L2_XLAT_ADDRESS_SHIFT)
 
 #else
 
-# error "ADDR_SPACE_SIZE is too small."
+# error "PLAT_VIRT_ADDR_SPACE_SIZE is too small."
 
 #endif
 
@@ -119,6 +127,25 @@
 	return TCR_PS_BITS_4GB;
 }
 
+#if DEBUG
+/* Physical Address ranges supported in the AArch64 Memory Model */
+static const unsigned int pa_range_bits_arr[] = {
+	PARANGE_0000, PARANGE_0001, PARANGE_0010, PARANGE_0011, PARANGE_0100,
+	PARANGE_0101
+};
+
+static unsigned long long get_max_supported_pa(void)
+{
+	u_register_t pa_range = read_id_aa64mmfr0_el1() &
+						ID_AA64MMFR0_EL1_PARANGE_MASK;
+
+	/* All other values are reserved */
+	assert(pa_range < ARRAY_SIZE(pa_range_bits_arr));
+
+	return (1ULL << pa_range_bits_arr[pa_range]) - 1ULL;
+}
+#endif
+
 void init_xlat_tables(void)
 {
 	unsigned long long max_pa;
@@ -126,8 +153,12 @@
 	print_mmap();
 	init_xlation_table(0, base_xlation_table, XLAT_TABLE_LEVEL_BASE,
 			   &max_va, &max_pa);
+
+	assert(max_va <= PLAT_VIRT_ADDR_SPACE_SIZE - 1);
+	assert(max_pa <= PLAT_PHY_ADDR_SPACE_SIZE - 1);
+	assert((PLAT_PHY_ADDR_SPACE_SIZE - 1) <= get_max_supported_pa());
+
 	tcr_ps_bits = calc_physical_addr_size_bits(max_pa);
-	assert(max_va < ADDR_SPACE_SIZE);
 }
 
 /*******************************************************************************
@@ -165,7 +196,7 @@
 		/* Set T0SZ to (64 - width of virtual address space) */	\
 		tcr = TCR_SH_INNER_SHAREABLE | TCR_RGN_OUTER_WBA |	\
 			TCR_RGN_INNER_WBA |				\
-			(64 - __builtin_ctzl(ADDR_SPACE_SIZE));		\
+			(64 - __builtin_ctzl(PLAT_VIRT_ADDR_SPACE_SIZE));\
 		tcr |= _tcr_extra;					\
 		write_tcr_el##_el(tcr);					\
 									\
diff --git a/lib/xlat_tables/xlat_tables_common.c b/lib/xlat_tables/xlat_tables_common.c
index ebbc916..81c4dc6 100644
--- a/lib/xlat_tables/xlat_tables_common.c
+++ b/lib/xlat_tables/xlat_tables_common.c
@@ -32,12 +32,14 @@
 #include <arch_helpers.h>
 #include <assert.h>
 #include <cassert.h>
+#include <common_def.h>
 #include <debug.h>
 #include <platform_def.h>
 #include <string.h>
 #include <types.h>
 #include <utils.h>
 #include <xlat_tables.h>
+#include "xlat_tables_private.h"
 
 #if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 #define LVL0_SPACER ""
@@ -102,6 +104,11 @@
 	assert(base_pa < end_pa); /* Check for overflows */
 	assert(base_va < end_va);
 
+	assert((base_va + (uintptr_t)size - (uintptr_t)1) <=
+					(PLAT_VIRT_ADDR_SPACE_SIZE - 1));
+	assert((base_pa + (unsigned long long)size - 1ULL) <=
+					(PLAT_PHY_ADDR_SPACE_SIZE - 1));
+
 #if DEBUG
 
 	/* Check for PAs and VAs overlaps with all other regions */
@@ -198,6 +205,9 @@
 	uint64_t desc;
 	int mem_type;
 
+	/* Make sure that the granularity is fine enough to map this address. */
+	assert((addr_pa & XLAT_BLOCK_MASK(level)) == 0);
+
 	desc = addr_pa;
 	/*
 	 * There are different translation table descriptors for level 3 and the
@@ -343,7 +353,8 @@
 		if (mm->base_va > base_va + level_size - 1) {
 			/* Next region is after this area. Nothing to map yet */
 			desc = INVALID_DESC;
-		} else {
+		/* Make sure that the current level allows block descriptors */
+		} else if (level >= XLAT_BLOCK_LEVEL_MIN) {
 			/*
 			 * Try to get attributes of this area. It will fail if
 			 * there are partially overlapping regions. On success,
@@ -372,7 +383,8 @@
 
 		*table++ = desc;
 		base_va += level_size;
-	} while ((base_va & level_index_mask) && (base_va - 1 < ADDR_SPACE_SIZE - 1));
+	} while ((base_va & level_index_mask) &&
+		 (base_va - 1 < PLAT_VIRT_ADDR_SPACE_SIZE - 1));
 
 	return mm;
 }
diff --git a/lib/xlat_tables/xlat_tables_private.h b/lib/xlat_tables/xlat_tables_private.h
index 159d071..f0f656b 100644
--- a/lib/xlat_tables/xlat_tables_private.h
+++ b/lib/xlat_tables/xlat_tables_private.h
@@ -32,10 +32,61 @@
 #define __XLAT_TABLES_PRIVATE_H__
 
 #include <cassert.h>
+#include <platform_def.h>
 #include <utils.h>
 
-/* The virtual address space size must be a power of two. */
-CASSERT(IS_POWER_OF_TWO(ADDR_SPACE_SIZE), assert_valid_addr_space_size);
+/*
+ * If the platform hasn't defined a physical and a virtual address space size
+ * default to ADDR_SPACE_SIZE.
+ */
+#if ERROR_DEPRECATED
+# ifdef ADDR_SPACE_SIZE
+#  error "ADDR_SPACE_SIZE is deprecated. Use PLAT_xxx_ADDR_SPACE_SIZE instead."
+# endif
+#elif defined(ADDR_SPACE_SIZE)
+# ifndef PLAT_PHY_ADDR_SPACE_SIZE
+#  define PLAT_PHY_ADDR_SPACE_SIZE	ADDR_SPACE_SIZE
+# endif
+# ifndef PLAT_VIRT_ADDR_SPACE_SIZE
+#  define PLAT_VIRT_ADDR_SPACE_SIZE	ADDR_SPACE_SIZE
+# endif
+#endif
+
+/* The virtual and physical address space sizes must be powers of two. */
+CASSERT(IS_POWER_OF_TWO(PLAT_VIRT_ADDR_SPACE_SIZE),
+	assert_valid_virt_addr_space_size);
+CASSERT(IS_POWER_OF_TWO(PLAT_PHY_ADDR_SPACE_SIZE),
+	assert_valid_phy_addr_space_size);
+
+/*
+ * In AArch32 state, the MMU only supports 4KB page granularity, which means
+ * that the first translation table level is either 1 or 2. Both of them are
+ * allowed to have block and table descriptors. See section G4.5.6 of the
+ * ARMv8-A Architecture Reference Manual (DDI 0487A.k) for more information.
+ *
+ * In AArch64 state, the MMU may support 4 KB, 16 KB and 64 KB page
+ * granularity. For 4KB granularity, a level 0 table descriptor doesn't support
+ * block translation. For 16KB, the same thing happens to levels 0 and 1. For
+ * 64KB, same for level 1. See section D4.3.1 of the ARMv8-A Architecture
+ * Reference Manual (DDI 0487A.k) for more information.
+ *
+ * The define below specifies the first table level that allows block
+ * descriptors.
+ */
+
+#ifdef AARCH32
+
+# define XLAT_BLOCK_LEVEL_MIN 1
+
+#else /* if AArch64 */
+
+# if PAGE_SIZE == (4*1024) /* 4KB */
+#  define XLAT_BLOCK_LEVEL_MIN 1
+# else /* 16KB or 64KB */
+#  define XLAT_BLOCK_LEVEL_MIN 2
+# endif
+
+#endif /* AARCH32 */
 
 void print_mmap(void);
 void init_xlation_table(uintptr_t base_va, uint64_t *table,
diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index 16bdd78..bf9dc79 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@@ -184,24 +184,7 @@
 # Auxiliary macros to build TF images from sources
 ################################################################################
 
-# If no goal is specified in the command line, .DEFAULT_GOAL is used.
-# .DEFAULT_GOAL is defined in the main Makefile before including this file.
-ifeq ($(MAKECMDGOALS),)
-MAKECMDGOALS := $(.DEFAULT_GOAL)
-endif
-
-define match_goals
-$(strip $(foreach goal,$(1),$(filter $(goal),$(MAKECMDGOALS))))
-endef
-
-# List of rules that involve building things
-BUILD_TARGETS := all bl1 bl2 bl2u bl31 bl32 certificates fip
-
-# Does the list of goals specified on the command line include a build target?
-ifneq ($(call match_goals,${BUILD_TARGETS}),)
-IS_ANYTHING_TO_BUILD := 1
-endif
-
+MAKE_DEP = -Wp,-MD,$(DEP) -MT $$@ -MP
 
 # MAKE_C builds a C source file and generates the dependency file
 #   $(1) = output directory
@@ -210,20 +193,14 @@
 define MAKE_C
 
 $(eval OBJ := $(1)/$(patsubst %.c,%.o,$(notdir $(2))))
-$(eval PREREQUISITES := $(patsubst %.o,%.d,$(OBJ)))
+$(eval DEP := $(patsubst %.o,%.d,$(OBJ)))
 $(eval IMAGE := IMAGE_BL$(call uppercase,$(3)))
 
-$(OBJ): $(2)
+$(OBJ): $(2) | bl$(3)_dirs
 	@echo "  CC      $$<"
-	$$(Q)$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -D$(IMAGE) -c $$< -o $$@
+	$$(Q)$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -D$(IMAGE) $(MAKE_DEP) -c $$< -o $$@
 
-$(PREREQUISITES): $(2) | bl$(3)_dirs
-	@echo "  DEPS    $$@"
-	$$(Q)$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -M -MT $(OBJ) -MF $$@ $$<
-
-ifdef IS_ANYTHING_TO_BUILD
--include $(PREREQUISITES)
-endif
+-include $(DEP)
 
 endef
 
@@ -235,20 +212,14 @@
 define MAKE_S
 
 $(eval OBJ := $(1)/$(patsubst %.S,%.o,$(notdir $(2))))
-$(eval PREREQUISITES := $(patsubst %.o,%.d,$(OBJ)))
+$(eval DEP := $(patsubst %.o,%.d,$(OBJ)))
 $(eval IMAGE := IMAGE_BL$(call uppercase,$(3)))
 
-$(OBJ): $(2)
+$(OBJ): $(2) | bl$(3)_dirs
 	@echo "  AS      $$<"
-	$$(Q)$$(AS) $$(ASFLAGS) -D$(IMAGE) -c $$< -o $$@
+	$$(Q)$$(AS) $$(ASFLAGS) -D$(IMAGE) $(MAKE_DEP) -c $$< -o $$@
 
-$(PREREQUISITES): $(2) | bl$(3)_dirs
-	@echo "  DEPS    $$@"
-	$$(Q)$$(AS) $$(ASFLAGS) -M -MT $(OBJ) -MF $$@ $$<
-
-ifdef IS_ANYTHING_TO_BUILD
--include $(PREREQUISITES)
-endif
+-include $(DEP)
 
 endef
 
@@ -258,19 +229,13 @@
 #   $(2) = input template
 define MAKE_LD
 
-$(eval PREREQUISITES := $(1).d)
+$(eval DEP := $(1).d)
 
-$(1): $(2)
+$(1): $(2) | $(dir ${1})
 	@echo "  PP      $$<"
-	$$(Q)$$(AS) $$(ASFLAGS) -P -E -D__LINKER__ -o $$@ $$<
+	$$(Q)$$(CPP) $$(CPPFLAGS) -P -D__ASSEMBLY__ -D__LINKER__ $(MAKE_DEP) -o $$@ $$<
 
-$(PREREQUISITES): $(2) | $(dir ${1})
-	@echo "  DEPS    $$@"
-	$$(Q)$$(AS) $$(ASFLAGS) -M -MT $(1) -MF $$@ $$<
-
-ifdef IS_ANYTHING_TO_BUILD
--include $(PREREQUISITES)
-endif
+-include $(DEP)
 
 endef
 
@@ -358,7 +323,7 @@
 else
 	@echo 'const char build_message[] = "Built : "$(BUILD_MESSAGE_TIMESTAMP); \
 	       const char version_string[] = "${VERSION_STRING}";' | \
-		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -xc - -o $(BUILD_DIR)/build_message.o
+		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -xc -c - -o $(BUILD_DIR)/build_message.o
 endif
 	$$(Q)$$(LD) -o $$@ $$(LDFLAGS) -Map=$(MAPFILE) --script $(LINKERFILE) \
 					$(BUILD_DIR)/build_message.o $(OBJS)
diff --git a/make_helpers/windows.mk b/make_helpers/windows.mk
index fe5e8c1..c4317d5 100644
--- a/make_helpers/windows.mk
+++ b/make_helpers/windows.mk
@@ -104,6 +104,6 @@
 VERSION_STRING_MESSAGE = const char version_string[] = "${VERSION_STRING}";
 define MAKE_BUILD_STRINGS
 	@echo $$(BUILT_TIME_DATE_STRING) $$(VERSION_STRING_MESSAGE) | \
-		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -x c - -o $1
+		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -x c -c - -o $1
 endef
 
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index 139f713..fde476a 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -66,19 +66,6 @@
 
 /*******************************************************************************
  * Function which implements the common FVP specific operations to power down a
- * cpu in response to a CPU_OFF or CPU_SUSPEND request.
- ******************************************************************************/
-static void fvp_cpu_pwrdwn_common(void)
-{
-	/* Prevent interrupts from spuriously waking up this cpu */
-	plat_arm_gic_cpuif_disable();
-
-	/* Program the power controller to power off this cpu. */
-	fvp_pwrc_write_ppoffr(read_mpidr_el1());
-}
-
-/*******************************************************************************
- * Function which implements the common FVP specific operations to power down a
  * cluster in response to a CPU_OFF or CPU_SUSPEND request.
  ******************************************************************************/
 static void fvp_cluster_pwrdwn_common(void)
@@ -180,7 +167,15 @@
 	 * suspended. Perform at least the cpu specific actions followed
 	 * by the cluster specific operations if applicable.
 	 */
-	fvp_cpu_pwrdwn_common();
+
+	/* Prevent interrupts from spuriously waking up this cpu */
+	plat_arm_gic_cpuif_disable();
+
+	/* Turn redistributor off */
+	plat_arm_gic_redistif_off();
+
+	/* Program the power controller to power off this cpu. */
+	fvp_pwrc_write_ppoffr(read_mpidr_el1());
 
 	if (target_state->pwr_domain_state[ARM_PWR_LVL1] ==
 					ARM_LOCAL_STATE_OFF)
@@ -213,8 +208,17 @@
 	/* Program the power controller to enable wakeup interrupts. */
 	fvp_pwrc_set_wen(mpidr);
 
-	/* Perform the common cpu specific operations */
-	fvp_cpu_pwrdwn_common();
+	/* Prevent interrupts from spuriously waking up this cpu */
+	plat_arm_gic_cpuif_disable();
+
+	/*
+	 * The Redistributor is not powered off as it can potentially prevent
+	 * wake up events reaching the CPUIF and/or might lead to losing
+	 * register context.
+	 */
+
+	/* Program the power controller to power off this cpu. */
+	fvp_pwrc_write_ppoffr(read_mpidr_el1());
 
 	/* Perform the common cluster specific operations */
 	if (target_state->pwr_domain_state[ARM_PWR_LVL1] ==
diff --git a/plat/arm/board/juno/include/platform_def.h b/plat/arm/board/juno/include/platform_def.h
index 691e2f7..adc4704 100644
--- a/plat/arm/board/juno/include/platform_def.h
+++ b/plat/arm/board/juno/include/platform_def.h
@@ -191,6 +191,12 @@
 #define PLAT_CSS_MAX_SCP_BL2_SIZE	0x1D000
 
 /*
+ * PLAT_CSS_MAX_SCP_BL2U_SIZE is calculated using the current
+ * SCP_BL2U size plus a little space for growth.
+ */
+#define PLAT_CSS_MAX_SCP_BL2U_SIZE	0x1D000
+
+/*
  * 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
  * as Group 0 interrupts.
diff --git a/plat/arm/common/aarch32/arm_helpers.S b/plat/arm/common/aarch32/arm_helpers.S
index 0839913..5d238ec 100644
--- a/plat/arm/common/aarch32/arm_helpers.S
+++ b/plat/arm/common/aarch32/arm_helpers.S
@@ -31,6 +31,8 @@
 #include <platform_def.h>
 
 	.weak	plat_arm_calc_core_pos
+	.weak	plat_crash_console_init
+	.weak	plat_crash_console_putc
 	.weak	plat_my_core_pos
 
 	/* -----------------------------------------------------
@@ -57,3 +59,29 @@
 	add	r0, r1, r0, LSR #6
 	bx	lr
 endfunc plat_arm_calc_core_pos
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_init(void)
+	 * Function to initialize the crash console
+	 * without a C Runtime to print crash report.
+	 * Clobber list : r0 - r3
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_init
+	ldr	r0, =PLAT_ARM_CRASH_UART_BASE
+	ldr	r1, =PLAT_ARM_CRASH_UART_CLK_IN_HZ
+	ldr	r2, =ARM_CONSOLE_BAUDRATE
+	b	console_core_init
+endfunc plat_crash_console_init
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_putc(int c)
+	 * Function to print a character on the crash
+	 * console without a C Runtime.
+	 * Clobber list : r1 - r2
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_putc
+	ldr	r1, =PLAT_ARM_CRASH_UART_BASE
+	b	console_core_putc
+endfunc plat_crash_console_putc
diff --git a/plat/arm/common/arm_bl1_fwu.c b/plat/arm/common/arm_bl1_fwu.c
index 2a18d34..da4107b 100644
--- a/plat/arm/common/arm_bl1_fwu.c
+++ b/plat/arm/common/arm_bl1_fwu.c
@@ -35,7 +35,7 @@
 #include <plat_arm.h>
 #include <platform_def.h>
 #include <tbbr_img_desc.h>
-
+#include <utils.h>
 
 /* Struct to keep track of usable memory */
 typedef struct bl1_mem_info {
@@ -76,6 +76,12 @@
 
 	assert(mem_base);
 	assert(mem_size);
+	/*
+	 * The caller of this function is responsible for checking upfront that
+	 * the end address doesn't overflow. We double-check this in debug
+	 * builds.
+	 */
+	assert(!check_uptr_overflow(mem_base, mem_size - 1));
 
 	/*
 	 * Check the given image source and size.
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 626b443..c2f28f9 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -97,6 +97,8 @@
 # mapping the former as executable and the latter as execute-never.
 SEPARATE_CODE_AND_RODATA	:=	1
 
+# Enable new version of image loading on ARM platforms
+LOAD_IMAGE_V2			:=	1
 
 PLAT_INCLUDES		+=	-Iinclude/common/tbbr				\
 				-Iinclude/plat/arm/common
@@ -165,9 +167,11 @@
 
     BL1_SOURCES		+=	${AUTH_SOURCES}					\
 				bl1/tbbr/tbbr_img_desc.c			\
-				plat/arm/common/arm_bl1_fwu.c
+				plat/arm/common/arm_bl1_fwu.c			\
+				plat/common/tbbr/plat_tbbr.c
 
-    BL2_SOURCES		+=	${AUTH_SOURCES}
+    BL2_SOURCES		+=	${AUTH_SOURCES}					\
+				plat/common/tbbr/plat_tbbr.c
 
     $(eval $(call FWU_FIP_ADD_IMG,NS_BL2U,--fwu))
 
diff --git a/plat/arm/common/arm_gicv2.c b/plat/arm/common/arm_gicv2.c
index 2636d1c..1017c9e 100644
--- a/plat/arm/common/arm_gicv2.c
+++ b/plat/arm/common/arm_gicv2.c
@@ -97,3 +97,17 @@
 {
 	gicv2_pcpu_distif_init();
 }
+
+/******************************************************************************
+ * Stubs for Redistributor power management. Although GICv2 doesn't have
+ * Redistributor interface, these are provided for the sake of uniform GIC API
+ *****************************************************************************/
+void plat_arm_gic_redistif_on(void)
+{
+	return;
+}
+
+void plat_arm_gic_redistif_off(void)
+{
+	return;
+}
diff --git a/plat/arm/common/arm_gicv3.c b/plat/arm/common/arm_gicv3.c
index ac309f2..6d68bfb 100644
--- a/plat/arm/common/arm_gicv3.c
+++ b/plat/arm/common/arm_gicv3.c
@@ -43,6 +43,8 @@
 #pragma weak plat_arm_gic_cpuif_enable
 #pragma weak plat_arm_gic_cpuif_disable
 #pragma weak plat_arm_gic_pcpu_init
+#pragma weak plat_arm_gic_redistif_on
+#pragma weak plat_arm_gic_redistif_off
 
 /* The GICv3 driver only needs to be initialized in EL3 */
 static uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
@@ -115,3 +117,16 @@
 {
 	gicv3_rdistif_init(plat_my_core_pos());
 }
+
+/******************************************************************************
+ * ARM common helpers to power GIC redistributor interface
+ *****************************************************************************/
+void plat_arm_gic_redistif_on(void)
+{
+	gicv3_rdistif_on(plat_my_core_pos());
+}
+
+void plat_arm_gic_redistif_off(void)
+{
+	gicv3_rdistif_off(plat_my_core_pos());
+}
diff --git a/plat/arm/common/arm_gicv3_legacy.c b/plat/arm/common/arm_gicv3_legacy.c
index 8396b60..cbf11fb 100644
--- a/plat/arm/common/arm_gicv3_legacy.c
+++ b/plat/arm/common/arm_gicv3_legacy.c
@@ -94,3 +94,17 @@
 {
 	arm_gic_pcpu_distif_setup();
 }
+
+/******************************************************************************
+ * Stubs for Redistributor power management. Although legacy configuration isn't
+ * supported, these are provided for the sake of uniform GIC API
+ *****************************************************************************/
+void plat_arm_gic_redistif_on(void)
+{
+	return;
+}
+
+void plat_arm_gic_redistif_off(void)
+{
+	return;
+}
diff --git a/plat/common/tbbr/plat_tbbr.c b/plat/common/tbbr/plat_tbbr.c
new file mode 100644
index 0000000..475564a
--- /dev/null
+++ b/plat/common/tbbr/plat_tbbr.c
@@ -0,0 +1,71 @@
+/*
+ * 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 <assert.h>
+#include <auth/auth_mod.h>
+#include <platform.h>
+#include <platform_oid.h>
+#include <string.h>
+
+/*
+ * Store a new non-volatile counter value. This implementation
+ * only allows updating of the platform's Trusted NV counter when a
+ * certificate protected by the Trusted NV counter is signed with
+ * the ROT key. This avoids a compromised secondary certificate from
+ * updating the platform's Trusted NV counter, which could lead to the
+ * platform becoming unusable. The function is suitable for all TBBR
+ * compliant platforms.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_set_nv_ctr2(void *cookie, const auth_img_desc_t *img_desc,
+		unsigned int nv_ctr)
+{
+	int trusted_nv_ctr;
+
+	assert(cookie != NULL);
+	assert(img_desc != NULL);
+
+	trusted_nv_ctr = strcmp(cookie, TRUSTED_FW_NVCOUNTER_OID) == 0;
+
+	/*
+	 * Only update the Trusted NV Counter if the certificate
+	 * has been signed with the ROT key. Non Trusted NV counter
+	 * updates are unconditional.
+	 */
+	if (!trusted_nv_ctr || (trusted_nv_ctr && img_desc->parent == NULL))
+		return plat_set_nv_ctr(cookie, nv_ctr);
+
+	/*
+	 * Trusted certificates not signed with the ROT key are not
+	 * allowed to update the Trusted NV Counter.
+	 */
+	return 1;
+}
diff --git a/plat/rockchip/rk3399/drivers/m0/Makefile b/plat/rockchip/rk3399/drivers/m0/Makefile
index b8e3cd4..c9454fe 100644
--- a/plat/rockchip/rk3399/drivers/m0/Makefile
+++ b/plat/rockchip/rk3399/drivers/m0/Makefile
@@ -59,7 +59,7 @@
 ASFLAGS			+= -mcpu=$(ARCH) -mthumb -Wall -ffunction-sections -O3
 CFLAGS			+= -mcpu=$(ARCH) -mthumb -Wall -ffunction-sections -O3
 
-LDFLAGS			:= -mcpu=$(ARCH) -mthumb -g -nostartfiles -O3
+LDFLAGS			:= -mcpu=$(ARCH) -mthumb -g -nostartfiles -nostdlib -O3
 LDFLAGS			+= -Wl,--gc-sections -Wl,--build-id=none
 
 # Cross tool
diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index 047aeaa..8b73aae 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h
@@ -98,7 +98,8 @@
 /*******************************************************************************
  * Platform specific page table and MMU setup constants
  ******************************************************************************/
-#define ADDR_SPACE_SIZE			(1ull << 32)
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ull << 32)
 #define MAX_MMAP_REGIONS		7
 #define MAX_XLAT_TABLES			5
 
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index 588ba61..d00b694 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -27,12 +27,12 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-ENABLE_PLAT_COMPAT := 0
-PROGRAMMABLE_RESET_ADDRESS := 1
+override ENABLE_PLAT_COMPAT := 0
+override PROGRAMMABLE_RESET_ADDRESS := 1
 PSCI_EXTENDED_STATE_ID := 1
 A53_DISABLE_NON_TEMPORAL_HINT := 0
 SEPARATE_CODE_AND_RODATA := 1
-RESET_TO_BL31 := 1
+override RESET_TO_BL31 := 1
 
 ifdef ZYNQMP_ATF_MEM_BASE
     $(eval $(call add_define,ZYNQMP_ATF_MEM_BASE))
@@ -97,7 +97,3 @@
 				plat/xilinx/zynqmp/pm_service/pm_api_sys.c	\
 				plat/xilinx/zynqmp/pm_service/pm_ipi.c		\
 				plat/xilinx/zynqmp/pm_service/pm_client.c
-
-ifneq (${RESET_TO_BL31},1)
-  $(error "Using BL31 as the reset vector is only one option supported on ZynqMP. Please set RESET_TO_BL31 to 1.")
-endif
diff --git a/services/spd/tspd/tspd_common.c b/services/spd/tspd/tspd_common.c
index 322413c..3dcefea 100644
--- a/services/spd/tspd/tspd_common.c
+++ b/services/spd/tspd/tspd_common.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:
@@ -32,7 +32,9 @@
 #include <assert.h>
 #include <bl_common.h>
 #include <context_mgmt.h>
+#include <debug.h>
 #include <string.h>
+#include <tsp.h>
 #include "tspd_private.h"
 
 /*******************************************************************************
@@ -129,3 +131,31 @@
 	/* Should never reach here */
 	assert(0);
 }
+
+/*******************************************************************************
+ * This function takes an SP context pointer and abort any preempted SMC
+ * request.
+ * Return 1 if there was a preempted SMC request, 0 otherwise.
+ ******************************************************************************/
+int tspd_abort_preempted_smc(tsp_context_t *tsp_ctx)
+{
+	if (!get_std_smc_active_flag(tsp_ctx->state))
+		return 0;
+
+	/* Abort any preempted SMC request */
+	clr_std_smc_active_flag(tsp_ctx->state);
+
+	/*
+	 * Arrange for an entry into the test secure payload. It will
+	 * be returned via TSP_ABORT_DONE case in tspd_smc_handler.
+	 */
+	cm_set_elr_el3(SECURE,
+		       (uint64_t) &tsp_vectors->abort_std_smc_entry);
+	uint64_t rc = tspd_synchronous_sp_entry(tsp_ctx);
+
+	if (rc != 0)
+		panic();
+
+	return 1;
+}
+
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index 1a06459..2850e70 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.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:
@@ -459,6 +459,11 @@
 		 */
 		tspd_synchronous_sp_exit(tsp_ctx, x1);
 #endif
+	/*
+	 * This function ID is used only by the SP to indicate it has finished
+	 * aborting a preempted Standard SMC request.
+	 */
+	case TSP_ABORT_DONE:
 
 	/*
 	 * These function IDs are used only by the SP to indicate it has
@@ -596,6 +601,26 @@
 		}
 
 		break;
+	/*
+	 * Request from the non-secure world to abort a preempted Standard SMC
+	 * call.
+	 */
+	case TSP_FID_ABORT:
+		/* ABORT should only be invoked by normal world */
+		if (!ns) {
+			assert(0);
+			break;
+		}
+
+		/* Abort the preempted SMC request */
+		if (!tspd_abort_preempted_smc(tsp_ctx))
+			/*
+			 * If there was no preempted SMC to abort, return
+			 * SMC_UNK.
+			 */
+			SMC_RET1(handle, SMC_UNK);
+
+		break;
 
 		/*
 		 * Request from non secure world to resume the preempted
diff --git a/services/spd/tspd/tspd_pm.c b/services/spd/tspd/tspd_pm.c
index 55562ba..bc5435a 100644
--- a/services/spd/tspd/tspd_pm.c
+++ b/services/spd/tspd/tspd_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:
@@ -58,6 +58,12 @@
 	assert(tsp_vectors);
 	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
 
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
 	/* Program the entry point and enter the TSP */
 	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_off_entry);
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
@@ -75,7 +81,7 @@
 	 */
 	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF);
 
-	 return 0;
+	return 0;
 }
 
 /*******************************************************************************
@@ -91,6 +97,12 @@
 	assert(tsp_vectors);
 	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
 
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
 	/* Program the entry point and enter the TSP */
 	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_suspend_entry);
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
@@ -99,7 +111,7 @@
 	 * Read the response from the TSP. A non-zero return means that
 	 * something went wrong while communicating with the TSP.
 	 */
-	if (rc != 0)
+	if (rc)
 		panic();
 
 	/* Update its context to reflect the state the TSP is in */
@@ -108,7 +120,7 @@
 
 /*******************************************************************************
  * This cpu has been turned on. Enter the TSP to initialise S-EL1 and other bits
- * before passing control back to the Secure Monitor. Entry in S-El1 is done
+ * before passing control back to the Secure Monitor. Entry in S-EL1 is done
  * after initialising minimal architectural state that guarantees safe
  * execution.
  ******************************************************************************/
@@ -205,6 +217,12 @@
 	assert(tsp_vectors);
 	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
 
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
 	/* Program the entry point */
 	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->system_off_entry);
 
@@ -225,11 +243,19 @@
 	assert(tsp_vectors);
 	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
 
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
 	/* Program the entry point */
 	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->system_reset_entry);
 
-	/* Enter the TSP. We do not care about the return value because we
-	 * must continue the reset anyway */
+	/*
+	 * Enter the TSP. We do not care about the return value because we
+	 * must continue the reset anyway
+	 */
 	tspd_synchronous_sp_entry(tsp_ctx);
 }
 
diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index cadc6aa..82039a4 100644
--- a/services/spd/tspd/tspd_private.h
+++ b/services/spd/tspd/tspd_private.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:
@@ -242,6 +242,7 @@
 				uint32_t rw,
 				uint64_t pc,
 				tsp_context_t *tsp_ctx);
+int tspd_abort_preempted_smc(tsp_context_t *tsp_ctx);
 
 extern tsp_context_t tspd_sp_context[TSPD_CORE_COUNT];
 extern struct tsp_vectors *tsp_vectors;
diff --git a/tools/fiptool/fiptool.c b/tools/fiptool/fiptool.c
index 7145815..fc0c8d6 100644
--- a/tools/fiptool/fiptool.c
+++ b/tools/fiptool/fiptool.c
@@ -51,6 +51,8 @@
 #define OPT_TOC_ENTRY 0
 #define OPT_PLAT_TOC_FLAGS 1
 
+static image_desc_t *lookup_image_desc_from_uuid(const uuid_t *uuid);
+static image_t *lookup_image_from_uuid(const uuid_t *uuid);
 static int info_cmd(int argc, char *argv[]);
 static void info_usage(void);
 static int create_cmd(int argc, char *argv[]);
@@ -77,7 +79,9 @@
 	{ .name = "help",    .handler = help_cmd,    .usage = NULL          },
 };
 
-static image_t *images[MAX_IMAGES];
+static image_desc_t *image_desc_head;
+static size_t nr_image_descs;
+static image_t *image_head;
 static size_t nr_images;
 static uuid_t uuid_null = { 0 };
 static int verbose;
@@ -137,7 +141,7 @@
 
 	d = strdup(s);
 	if (d == NULL)
-		log_errx("strdup: ", msg);
+		log_errx("strdup: %s", msg);
 	return d;
 }
 
@@ -147,15 +151,92 @@
 
 	d = malloc(size);
 	if (d == NULL)
-		log_errx("malloc: ", msg);
+		log_errx("malloc: %s", msg);
 	return d;
 }
 
+static image_desc_t *new_image_desc(const uuid_t *uuid,
+    const char *name, const char *cmdline_name)
+{
+	image_desc_t *desc;
+
+	desc = xmalloc(sizeof(*desc),
+	    "failed to allocate memory for image descriptor");
+	memcpy(&desc->uuid, uuid, sizeof(uuid_t));
+	desc->name = xstrdup(name,
+	    "failed to allocate memory for image name");
+	desc->cmdline_name = xstrdup(cmdline_name,
+	    "failed to allocate memory for image command line name");
+	desc->action = DO_UNSPEC;
+	desc->action_arg = NULL;
+	return desc;
+}
+
+static void set_image_desc_action(image_desc_t *desc, int action,
+    const char *arg)
+{
+	assert(desc != NULL);
+
+	if (desc->action_arg != DO_UNSPEC)
+		free(desc->action_arg);
+	desc->action = action;
+	desc->action_arg = NULL;
+	if (arg != NULL)
+		desc->action_arg = xstrdup(arg,
+		    "failed to allocate memory for argument");
+}
+
+static void free_image_desc(image_desc_t *desc)
+{
+	free(desc->name);
+	free(desc->cmdline_name);
+	free(desc->action_arg);
+	free(desc);
+}
+
+static void add_image_desc(image_desc_t *desc)
+{
+	assert(lookup_image_desc_from_uuid(&desc->uuid) == NULL);
+	desc->next = image_desc_head;
+	image_desc_head = desc;
+	nr_image_descs++;
+}
+
+static void free_image_descs(void)
+{
+	image_desc_t *desc = image_desc_head, *tmp;
+
+	while (desc != NULL) {
+		tmp = desc->next;
+		free_image_desc(desc);
+		desc = tmp;
+		nr_image_descs--;
+	}
+	assert(nr_image_descs == 0);
+}
+
+static void fill_image_descs(void)
+{
+	toc_entry_t *toc_entry;
+
+	for (toc_entry = toc_entries;
+	     toc_entry->cmdline_name != NULL;
+	     toc_entry++) {
+		image_desc_t *desc;
+
+		desc = new_image_desc(&toc_entry->uuid,
+		    toc_entry->name,
+		    toc_entry->cmdline_name);
+		add_image_desc(desc);
+	}
+}
+
 static void add_image(image_t *image)
 {
-	if (nr_images + 1 > MAX_IMAGES)
-		log_errx("Too many images");
-	images[nr_images++] = image;
+	assert(lookup_image_from_uuid(&image->uuid) == NULL);
+	image->next = image_head;
+	image_head = image;
+	nr_images++;
 }
 
 static void free_image(image_t *image)
@@ -164,72 +245,104 @@
 	free(image);
 }
 
-static void replace_image(image_t *image_dst, image_t *image_src)
-{
-	int i;
-
-	for (i = 0; i < nr_images; i++) {
-		if (images[i] == image_dst) {
-			free_image(images[i]);
-			images[i] = image_src;
-			break;
-		}
-	}
-	assert(i != nr_images);
-}
-
 static void remove_image(image_t *image)
 {
-	int i;
+	image_t *tmp = image_head, *prev;
 
-	for (i = 0; i < nr_images; i++) {
-		if (images[i] == image) {
-			free_image(images[i]);
-			images[i] = NULL;
-			break;
+	if (tmp == image) {
+		image_head = tmp->next;
+		free_image(tmp);
+	} else {
+		while (tmp != NULL && tmp != image) {
+			prev = tmp;
+			tmp = tmp->next;
 		}
+		assert(tmp != NULL);
+		prev->next = tmp->next;
+		free_image(tmp);
 	}
-	assert(i != nr_images);
-
-	/* Compact array. */
-	memmove(&images[i], &images[i + 1],
-	    (nr_images - i - 1) * sizeof(*images));
 	nr_images--;
 }
 
 static void free_images(void)
 {
-	int i;
+	image_t *image = image_head, *tmp;
 
-	for (i = 0; i < nr_images; i++) {
-		free_image(images[i]);
-		images[i] = NULL;
+	while (image != NULL) {
+		tmp = image->next;
+		free_image(image);
+		image = tmp;
+		nr_images--;
 	}
+	assert(nr_images == 0);
 }
 
-static toc_entry_t *lookup_entry_from_uuid(uuid_t *uuid)
+static image_desc_t *lookup_image_desc_from_uuid(const uuid_t *uuid)
 {
-	toc_entry_t *toc_entry = toc_entries;
+	image_desc_t *desc;
 
-	for (; toc_entry->cmdline_name != NULL; toc_entry++)
-		if (memcmp(&toc_entry->uuid, uuid, sizeof(uuid_t)) == 0)
-			return toc_entry;
+	for (desc = image_desc_head; desc != NULL; desc = desc->next)
+		if (memcmp(&desc->uuid, uuid, sizeof(uuid_t)) == 0)
+			return desc;
 	return NULL;
 }
 
-static image_t *lookup_image_from_uuid(uuid_t *uuid)
+static image_desc_t *lookup_image_desc_from_opt(const char *opt)
+{
+	image_desc_t *desc;
+
+	for (desc = image_desc_head; desc != NULL; desc = desc->next)
+		if (strcmp(desc->cmdline_name, opt) == 0)
+			return desc;
+	return NULL;
+}
+
+static image_t *lookup_image_from_uuid(const uuid_t *uuid)
 {
 	image_t *image;
-	int i;
 
-	for (i = 0; i < nr_images; i++) {
-		image = images[i];
+	for (image = image_head; image != NULL; image = image->next)
 		if (memcmp(&image->uuid, uuid, sizeof(uuid_t)) == 0)
 			return image;
-	}
 	return NULL;
 }
 
+static void uuid_to_str(char *s, size_t len, const uuid_t *u)
+{
+	assert(len >= (_UUID_STR_LEN + 1));
+
+	snprintf(s, len, "%08X-%04X-%04X-%04X-%04X%04X%04X",
+	    u->time_low,
+	    u->time_mid,
+	    u->time_hi_and_version,
+	    ((uint16_t)u->clock_seq_hi_and_reserved << 8) | u->clock_seq_low,
+	    ((uint16_t)u->node[0] << 8) | u->node[1],
+	    ((uint16_t)u->node[2] << 8) | u->node[3],
+	    ((uint16_t)u->node[4] << 8) | u->node[5]);
+}
+
+static void uuid_from_str(uuid_t *u, const char *s)
+{
+	int n;
+
+	if (s == NULL)
+		log_errx("UUID cannot be NULL");
+	if (strlen(s) != _UUID_STR_LEN)
+		log_errx("Invalid UUID: %s", s);
+
+	n = sscanf(s,
+	    "%8x-%4hx-%4hx-%2hhx%2hhx-%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx",
+	    &u->time_low, &u->time_mid, &u->time_hi_and_version,
+	    &u->clock_seq_hi_and_reserved, &u->clock_seq_low, &u->node[0],
+	    &u->node[1], &u->node[2], &u->node[3], &u->node[4], &u->node[5]);
+	/*
+	 * Given the format specifier above, we expect 11 items to be scanned
+	 * for a properly formatted UUID.
+	 */
+	if (n != 11)
+		log_errx("Invalid UUID: %s", s);
+}
+
 static int parse_fip(const char *filename, fip_toc_header_t *toc_header_out)
 {
 	struct stat st;
@@ -237,7 +350,6 @@
 	char *buf, *bufend;
 	fip_toc_header_t *toc_header;
 	fip_toc_entry_t *toc_entry;
-	image_t *image;
 	int terminated = 0;
 
 	fp = fopen(filename, "r");
@@ -268,6 +380,9 @@
 
 	/* Walk through each ToC entry in the file. */
 	while ((char *)toc_entry + sizeof(*toc_entry) - 1 < bufend) {
+		image_t *image;
+		image_desc_t *desc;
+
 		/* Found the ToC terminator, we are done. */
 		if (memcmp(&toc_entry->uuid, &uuid_null, sizeof(uuid_t)) == 0) {
 			terminated = 1;
@@ -293,6 +408,21 @@
 		    toc_entry->size);
 		image->size = toc_entry->size;
 
+		/* If this is an unknown image, create a descriptor for it. */
+		desc = lookup_image_desc_from_uuid(&image->uuid);
+		if (desc == NULL) {
+			char name[_UUID_STR_LEN + 1], filename[PATH_MAX];
+
+			uuid_to_str(name, sizeof(name), &image->uuid);
+			snprintf(filename, sizeof(filename), "%s%s",
+			    name, ".bin");
+			desc = new_image_desc(&image->uuid, name, "blob");
+			desc->action = DO_UNPACK;
+			desc->action_arg = xstrdup(filename,
+			    "failed to allocate memory for blob filename");
+			add_image_desc(desc);
+		}
+
 		add_image(image);
 
 		toc_entry++;
@@ -344,29 +474,32 @@
 	return 0;
 }
 
-static int fill_common_opts(struct option *opts, int has_arg)
+static struct option *add_opt(struct option *opts, size_t *nr_opts,
+    const char *name, int has_arg, int val)
 {
-	int i;
-
-	for (i = 0; toc_entries[i].cmdline_name != NULL; i++) {
-		opts[i].name = toc_entries[i].cmdline_name;
-		opts[i].has_arg = has_arg;
-		opts[i].flag = NULL;
-		opts[i].val = 0;
-	}
-	return i;
+	opts = realloc(opts, (*nr_opts + 1) * sizeof(*opts));
+	if (opts == NULL)
+		log_err("realloc");
+	opts[*nr_opts].name = name;
+	opts[*nr_opts].has_arg = has_arg;
+	opts[*nr_opts].flag = NULL;
+	opts[*nr_opts].val = val;
+	++*nr_opts;
+	return opts;
 }
 
-static void add_opt(struct option *opts, int idx, char *name,
-    int has_arg, int val)
+static struct option *fill_common_opts(struct option *opts, size_t *nr_opts,
+    int has_arg)
 {
-	opts[idx].name = name;
-	opts[idx].has_arg = has_arg;
-	opts[idx].flag = NULL;
-	opts[idx].val = val;
+	image_desc_t *desc;
+
+	for (desc = image_desc_head; desc != NULL; desc = desc->next)
+		opts = add_opt(opts, nr_opts, desc->cmdline_name, has_arg,
+		    OPT_TOC_ENTRY);
+	return opts;
 }
 
-static void md_print(unsigned char *md, size_t len)
+static void md_print(const unsigned char *md, size_t len)
 {
 	size_t i;
 
@@ -378,9 +511,8 @@
 {
 	image_t *image;
 	uint64_t image_offset;
-	uint64_t image_size = 0;
+	uint64_t image_size;
 	fip_toc_header_t toc_header;
-	int i;
 
 	if (argc != 2)
 		info_usage();
@@ -400,22 +532,19 @@
 	image_offset = sizeof(fip_toc_header_t) +
 	    (sizeof(fip_toc_entry_t) * (nr_images + 1));
 
-	for (i = 0; i < nr_images; i++) {
-		toc_entry_t *toc_entry;
+	for (image = image_head; image != NULL; image = image->next) {
+		image_desc_t *desc;
 
-		image = images[i];
-		toc_entry = lookup_entry_from_uuid(&image->uuid);
-		if (toc_entry != NULL)
-			printf("%s: ", toc_entry->name);
-		else
-			printf("Unknown entry: ");
+		desc = lookup_image_desc_from_uuid(&image->uuid);
+		assert(desc != NULL);
+		printf("%s: ", desc->name);
 		image_size = image->size;
 		printf("offset=0x%llX, size=0x%llX",
 		    (unsigned long long)image_offset,
 		    (unsigned long long)image_size);
-		if (toc_entry != NULL)
+		if (desc != NULL)
 			printf(", cmdline=\"--%s\"",
-			    toc_entry->cmdline_name);
+			    desc->cmdline_name);
 		if (verbose) {
 			unsigned char md[SHA256_DIGEST_LENGTH];
 
@@ -437,7 +566,7 @@
 	exit(1);
 }
 
-static int pack_images(char *filename, uint64_t toc_flags)
+static int pack_images(const char *filename, uint64_t toc_flags)
 {
 	FILE *fp;
 	image_t *image;
@@ -445,12 +574,11 @@
 	fip_toc_entry_t *toc_entry;
 	char *buf;
 	uint64_t entry_offset, buf_size, payload_size;
-	int i;
 
 	/* Calculate total payload size and allocate scratch buffer. */
 	payload_size = 0;
-	for (i = 0; i < nr_images; i++)
-		payload_size += images[i]->size;
+	for (image = image_head; image != NULL; image = image->next)
+		payload_size += image->size;
 
 	buf_size = sizeof(fip_toc_header_t) +
 	    sizeof(fip_toc_entry_t) * (nr_images + 1);
@@ -467,8 +595,7 @@
 	toc_entry = (fip_toc_entry_t *)(toc_header + 1);
 
 	entry_offset = buf_size;
-	for (i = 0; i < nr_images; i++) {
-		image = images[i];
+	for (image = image_head; image != NULL; image = image->next) {
 		memcpy(&toc_entry->uuid, &image->uuid, sizeof(uuid_t));
 		toc_entry->offset_address = entry_offset;
 		toc_entry->size = image->size;
@@ -498,11 +625,9 @@
 	if (verbose)
 		log_dbgx("Payload size: %zu bytes", payload_size);
 
-	for (i = 0; i < nr_images; i++) {
-		image = images[i];
+	for (image = image_head; image != NULL; image = image->next)
 		if (fwrite(image->buffer, 1, image->size, fp) != image->size)
 			log_errx("Failed to write image to %s", filename);
-	}
 
 	fclose(fp);
 	return 0;
@@ -517,38 +642,36 @@
  */
 static void update_fip(void)
 {
-	toc_entry_t *toc_entry;
-	image_t *new_image, *old_image;
+	image_desc_t *desc;
 
 	/* Add or replace images in the FIP file. */
-	for (toc_entry = toc_entries;
-	     toc_entry->cmdline_name != NULL;
-	     toc_entry++) {
-		if (toc_entry->action != DO_PACK)
+	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
+		image_t *new_image, *old_image;
+
+		if (desc->action != DO_PACK)
 			continue;
 
-		new_image = read_image_from_file(&toc_entry->uuid,
-		    toc_entry->action_arg);
-		old_image = lookup_image_from_uuid(&toc_entry->uuid);
+		new_image = read_image_from_file(&desc->uuid,
+		    desc->action_arg);
+		old_image = lookup_image_from_uuid(&desc->uuid);
 		if (old_image != NULL) {
-			if (verbose)
-				log_dbgx("Replacing image %s.bin with %s",
-				    toc_entry->cmdline_name,
-				    toc_entry->action_arg);
-			replace_image(old_image, new_image);
+			if (verbose) {
+				log_dbgx("Replacing %s with %s",
+				    desc->cmdline_name,
+				    desc->action_arg);
+			}
+			remove_image(old_image);
+			add_image(new_image);
 		} else {
 			if (verbose)
 				log_dbgx("Adding image %s",
-				    toc_entry->action_arg);
+				    desc->action_arg);
 			add_image(new_image);
 		}
-
-		free(toc_entry->action_arg);
-		toc_entry->action_arg = NULL;
 	}
 }
 
-static void parse_plat_toc_flags(char *arg, unsigned long long *toc_flags)
+static void parse_plat_toc_flags(const char *arg, unsigned long long *toc_flags)
 {
 	unsigned long long flags;
 	char *endptr;
@@ -561,46 +684,83 @@
 	*toc_flags |= flags << 32;
 }
 
+static void parse_blob_opt(char *arg, uuid_t *uuid, char *filename, size_t len)
+{
+	char *p;
+
+	for (p = strtok(arg, ","); p != NULL; p = strtok(NULL, ",")) {
+		if (strncmp(p, "uuid=", strlen("uuid=")) == 0) {
+			p += strlen("uuid=");
+			uuid_from_str(uuid, p);
+		} else if (strncmp(p, "file=", strlen("file=")) == 0) {
+			p += strlen("file=");
+			snprintf(filename, len, "%s", p);
+		}
+	}
+}
+
 static int create_cmd(int argc, char *argv[])
 {
-	struct option opts[toc_entries_len + 1];
+	struct option *opts = NULL;
+	size_t nr_opts = 0;
 	unsigned long long toc_flags = 0;
-	int i;
 
 	if (argc < 2)
 		create_usage();
 
-	i = fill_common_opts(opts, required_argument);
-	add_opt(opts, i, "plat-toc-flags", required_argument,
+	opts = fill_common_opts(opts, &nr_opts, required_argument);
+	opts = add_opt(opts, &nr_opts, "plat-toc-flags", required_argument,
 	    OPT_PLAT_TOC_FLAGS);
-	add_opt(opts, ++i, NULL, 0, 0);
+	opts = add_opt(opts, &nr_opts, "blob", required_argument, 'b');
+	opts = add_opt(opts, &nr_opts, NULL, 0, 0);
 
 	while (1) {
 		int c, opt_index = 0;
 
-		c = getopt_long(argc, argv, "", opts, &opt_index);
+		c = getopt_long(argc, argv, "b:", opts, &opt_index);
 		if (c == -1)
 			break;
 
 		switch (c) {
 		case OPT_TOC_ENTRY: {
-			toc_entry_t *toc_entry;
+			image_desc_t *desc;
 
-			toc_entry = &toc_entries[opt_index];
-			toc_entry->action = DO_PACK;
-			toc_entry->action_arg = xstrdup(optarg,
-			    "failed to allocate memory for argument");
+			desc = lookup_image_desc_from_opt(opts[opt_index].name);
+			set_image_desc_action(desc, DO_PACK, optarg);
 			break;
 		}
 		case OPT_PLAT_TOC_FLAGS:
 			parse_plat_toc_flags(optarg, &toc_flags);
 			break;
+		case 'b': {
+			char name[_UUID_STR_LEN + 1];
+			char filename[PATH_MAX] = { 0 };
+			uuid_t uuid = { 0 };
+			image_desc_t *desc;
+
+			parse_blob_opt(optarg, &uuid,
+			    filename, sizeof(filename));
+
+			if (memcmp(&uuid, &uuid_null, sizeof(uuid_t)) == 0 ||
+			    filename[0] == '\0')
+				create_usage();
+
+			desc = lookup_image_desc_from_uuid(&uuid);
+			if (desc == NULL) {
+				uuid_to_str(name, sizeof(name), &uuid);
+				desc = new_image_desc(&uuid, name, "blob");
+				add_image_desc(desc);
+			}
+			set_image_desc_action(desc, DO_PACK, filename);
+			break;
+		}
 		default:
 			create_usage();
 		}
 	}
 	argc -= optind;
 	argv += optind;
+	free(opts);
 
 	if (argc == 0)
 		create_usage();
@@ -616,7 +776,10 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool create [--plat-toc-flags <value>] [opts] FIP_FILENAME\n");
+	printf("fiptool create [--blob uuid=...,file=...] "
+	    "[--plat-toc-flags <value>] [opts] FIP_FILENAME\n");
+	printf("  --blob uuid=...,file=...\tAdd an image with the given UUID "
+	    "pointed to by file.\n");
 	printf("  --plat-toc-flags <value>\t16-bit platform specific flag field "
 	    "occupying bits 32-47 in 64-bit ToC header.\n");
 	fputc('\n', stderr);
@@ -629,43 +792,63 @@
 
 static int update_cmd(int argc, char *argv[])
 {
-	struct option opts[toc_entries_len + 2];
-	char outfile[FILENAME_MAX] = { 0 };
+	struct option *opts = NULL;
+	size_t nr_opts = 0;
+	char outfile[PATH_MAX] = { 0 };
 	fip_toc_header_t toc_header = { 0 };
 	unsigned long long toc_flags = 0;
 	int pflag = 0;
-	int i;
 
 	if (argc < 2)
 		update_usage();
 
-	i = fill_common_opts(opts, required_argument);
-	add_opt(opts, i, "out", required_argument, 'o');
-	add_opt(opts, ++i, "plat-toc-flags", required_argument,
+	opts = fill_common_opts(opts, &nr_opts, required_argument);
+	opts = add_opt(opts, &nr_opts, "blob", required_argument, 'b');
+	opts = add_opt(opts, &nr_opts, "out", required_argument, 'o');
+	opts = add_opt(opts, &nr_opts, "plat-toc-flags", required_argument,
 	    OPT_PLAT_TOC_FLAGS);
-	add_opt(opts, ++i, NULL, 0, 0);
+	opts = add_opt(opts, &nr_opts, NULL, 0, 0);
 
 	while (1) {
 		int c, opt_index = 0;
 
-		c = getopt_long(argc, argv, "o:", opts, &opt_index);
+		c = getopt_long(argc, argv, "b:o:", opts, &opt_index);
 		if (c == -1)
 			break;
 
 		switch (c) {
 		case OPT_TOC_ENTRY: {
-			toc_entry_t *toc_entry;
+			image_desc_t *desc;
 
-			toc_entry = &toc_entries[opt_index];
-			toc_entry->action = DO_PACK;
-			toc_entry->action_arg = xstrdup(optarg,
-			    "failed to allocate memory for argument");
+			desc = lookup_image_desc_from_opt(opts[opt_index].name);
+			set_image_desc_action(desc, DO_PACK, optarg);
 			break;
 		}
-		case OPT_PLAT_TOC_FLAGS: {
+		case OPT_PLAT_TOC_FLAGS:
 			parse_plat_toc_flags(optarg, &toc_flags);
 			pflag = 1;
 			break;
+		case 'b': {
+			char name[_UUID_STR_LEN + 1];
+			char filename[PATH_MAX] = { 0 };
+			uuid_t uuid = { 0 };
+			image_desc_t *desc;
+
+			parse_blob_opt(optarg, &uuid,
+			    filename, sizeof(filename));
+
+			if (memcmp(&uuid, &uuid_null, sizeof(uuid_t)) == 0 ||
+			    filename[0] == '\0')
+				update_usage();
+
+			desc = lookup_image_desc_from_uuid(&uuid);
+			if (desc == NULL) {
+				uuid_to_str(name, sizeof(name), &uuid);
+				desc = new_image_desc(&uuid, name, "blob");
+				add_image_desc(desc);
+			}
+			set_image_desc_action(desc, DO_PACK, filename);
+			break;
 		}
 		case 'o':
 			snprintf(outfile, sizeof(outfile), "%s", optarg);
@@ -676,6 +859,7 @@
 	}
 	argc -= optind;
 	argv += optind;
+	free(opts);
 
 	if (argc == 0)
 		update_usage();
@@ -701,8 +885,10 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool update [--out FIP_FILENAME] "
+	printf("fiptool update [--blob uuid=...,file=...] [--out FIP_FILENAME] "
 	    "[--plat-toc-flags <value>] [opts] FIP_FILENAME\n");
+	printf("  --blob uuid=...,file=...\tAdd or update an image "
+	    "with the given UUID pointed to by file.\n");
 	printf("  --out FIP_FILENAME\t\tSet an alternative output FIP file.\n");
 	printf("  --plat-toc-flags <value>\t16-bit platform specific flag field "
 	    "occupying bits 32-47 in 64-bit ToC header.\n");
@@ -716,36 +902,61 @@
 
 static int unpack_cmd(int argc, char *argv[])
 {
-	struct option opts[toc_entries_len + 3];
-	char file[FILENAME_MAX], outdir[PATH_MAX] = { 0 };
-	toc_entry_t *toc_entry;
+	struct option *opts = NULL;
+	size_t nr_opts = 0;
+	char outdir[PATH_MAX] = { 0 };
+	image_desc_t *desc;
 	int fflag = 0;
 	int unpack_all = 1;
-	int i;
 
 	if (argc < 2)
 		unpack_usage();
 
-	i = fill_common_opts(opts, required_argument);
-	add_opt(opts, i, "force", no_argument, 'f');
-	add_opt(opts, ++i, "out", required_argument, 'o');
-	add_opt(opts, ++i, NULL, 0, 0);
+	opts = fill_common_opts(opts, &nr_opts, required_argument);
+	opts = add_opt(opts, &nr_opts, "blob", required_argument, 'b');
+	opts = add_opt(opts, &nr_opts, "force", no_argument, 'f');
+	opts = add_opt(opts, &nr_opts, "out", required_argument, 'o');
+	opts = add_opt(opts, &nr_opts, NULL, 0, 0);
 
 	while (1) {
 		int c, opt_index = 0;
 
-		c = getopt_long(argc, argv, "fo:", opts, &opt_index);
+		c = getopt_long(argc, argv, "b:fo:", opts, &opt_index);
 		if (c == -1)
 			break;
 
 		switch (c) {
-		case OPT_TOC_ENTRY:
+		case OPT_TOC_ENTRY: {
+			image_desc_t *desc;
+
+			desc = lookup_image_desc_from_opt(opts[opt_index].name);
+			set_image_desc_action(desc, DO_UNPACK, optarg);
 			unpack_all = 0;
-			toc_entry = &toc_entries[opt_index];
-			toc_entry->action = DO_UNPACK;
-			toc_entry->action_arg = xstrdup(optarg,
-			    "failed to allocate memory for argument");
 			break;
+		}
+		case 'b': {
+			char name[_UUID_STR_LEN + 1];
+			char filename[PATH_MAX] = { 0 };
+			uuid_t uuid = { 0 };
+			image_desc_t *desc;
+
+			parse_blob_opt(optarg, &uuid,
+			    filename, sizeof(filename));
+
+			if (memcmp(&uuid, &uuid_null, sizeof(uuid_t)) == 0 ||
+			    filename[0] == '\0')
+				unpack_usage();
+
+			desc = lookup_image_desc_from_uuid(&uuid);
+			if (desc == NULL) {
+				uuid_to_str(name, sizeof(name), &uuid);
+				desc = new_image_desc(&uuid, name, "blob");
+				add_image_desc(desc);
+			}
+			set_image_desc_action(desc, DO_UNPACK, filename);
+			unpack_all = 0;
+			break;
+		}
 		case 'f':
 			fflag = 1;
 			break;
@@ -758,6 +969,7 @@
 	}
 	argc -= optind;
 	argv += optind;
+	free(opts);
 
 	if (argc == 0)
 		unpack_usage();
@@ -769,29 +981,26 @@
 			log_err("chdir %s", outdir);
 
 	/* Unpack all specified images. */
-	for (toc_entry = toc_entries;
-	     toc_entry->cmdline_name != NULL;
-	     toc_entry++) {
+	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
+		char file[PATH_MAX];
 		image_t *image;
 
-		if (!unpack_all && toc_entry->action != DO_UNPACK)
+		if (!unpack_all && desc->action != DO_UNPACK)
 			continue;
 
 		/* Build filename. */
-		if (toc_entry->action_arg == NULL)
+		if (desc->action_arg == NULL)
 			snprintf(file, sizeof(file), "%s.bin",
-			    toc_entry->cmdline_name);
+			    desc->cmdline_name);
 		else
 			snprintf(file, sizeof(file), "%s",
-			    toc_entry->action_arg);
+			    desc->action_arg);
 
-		image = lookup_image_from_uuid(&toc_entry->uuid);
+		image = lookup_image_from_uuid(&desc->uuid);
 		if (image == NULL) {
 			if (!unpack_all)
-				log_warnx("Requested image %s is not in %s",
+				log_warnx("%s does not exist in %s",
 				    file, argv[0]);
-			free(toc_entry->action_arg);
-			toc_entry->action_arg = NULL;
 			continue;
 		}
 
@@ -803,9 +1012,6 @@
 			log_warnx("File %s already exists, use --force to overwrite it",
 			    file);
 		}
-
-		free(toc_entry->action_arg);
-		toc_entry->action_arg = NULL;
 	}
 
 	free_images();
@@ -816,10 +1022,13 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool unpack [--force] [--out <path>] [opts] FIP_FILENAME\n");
-	printf("  --force\tIf the output file already exists, use --force to "
+	printf("fiptool unpack [--blob uuid=...,file=...] [--force] "
+	    "[--out <path>] [opts] FIP_FILENAME\n");
+	printf("  --blob uuid=...,file=...\tUnpack an image with the given UUID "
+	    "to file.\n");
+	printf("  --force\t\t\tIf the output file already exists, use --force to "
 	    "overwrite it.\n");
-	printf("  --out path\tSet the output directory path.\n");
+	printf("  --out path\t\t\tSet the output directory path.\n");
 	fputc('\n', stderr);
 	printf("Specific images are unpacked with the following options:\n");
 	for (; toc_entry->cmdline_name != NULL; toc_entry++)
@@ -832,33 +1041,57 @@
 
 static int remove_cmd(int argc, char *argv[])
 {
-	struct option opts[toc_entries_len + 2];
-	char outfile[FILENAME_MAX] = { 0 };
+	struct option *opts = NULL;
+	size_t nr_opts = 0;
+	char outfile[PATH_MAX] = { 0 };
 	fip_toc_header_t toc_header;
-	toc_entry_t *toc_entry;
+	image_desc_t *desc;
 	int fflag = 0;
-	int i;
 
 	if (argc < 2)
 		remove_usage();
 
-	i = fill_common_opts(opts, no_argument);
-	add_opt(opts, i, "force", no_argument, 'f');
-	add_opt(opts, ++i, "out", required_argument, 'o');
-	add_opt(opts, ++i, NULL, 0, 0);
+	opts = fill_common_opts(opts, &nr_opts, no_argument);
+	opts = add_opt(opts, &nr_opts, "blob", required_argument, 'b');
+	opts = add_opt(opts, &nr_opts, "force", no_argument, 'f');
+	opts = add_opt(opts, &nr_opts, "out", required_argument, 'o');
+	opts = add_opt(opts, &nr_opts, NULL, 0, 0);
 
 	while (1) {
 		int c, opt_index = 0;
 
-		c = getopt_long(argc, argv, "fo:", opts, &opt_index);
+		c = getopt_long(argc, argv, "b:fo:", opts, &opt_index);
 		if (c == -1)
 			break;
 
 		switch (c) {
-		case OPT_TOC_ENTRY:
-			toc_entry = &toc_entries[opt_index];
-			toc_entry->action = DO_REMOVE;
+		case OPT_TOC_ENTRY: {
+			image_desc_t *desc;
+
+			desc = lookup_image_desc_from_opt(opts[opt_index].name);
+			set_image_desc_action(desc, DO_REMOVE, NULL);
 			break;
+		}
+		case 'b': {
+			char name[_UUID_STR_LEN + 1], filename[PATH_MAX];
+			uuid_t uuid = { 0 };
+			image_desc_t *desc;
+
+			parse_blob_opt(optarg, &uuid,
+			    filename, sizeof(filename));
+
+			if (memcmp(&uuid, &uuid_null, sizeof(uuid_t)) == 0)
+				remove_usage();
+
+			desc = lookup_image_desc_from_uuid(&uuid);
+			if (desc == NULL) {
+				uuid_to_str(name, sizeof(name), &uuid);
+				desc = new_image_desc(&uuid, name, "blob");
+				add_image_desc(desc);
+			}
+			set_image_desc_action(desc, DO_REMOVE, NULL);
+			break;
+		}
 		case 'f':
 			fflag = 1;
 			break;
@@ -871,6 +1104,7 @@
 	}
 	argc -= optind;
 	argv += optind;
+	free(opts);
 
 	if (argc == 0)
 		remove_usage();
@@ -884,22 +1118,21 @@
 
 	parse_fip(argv[0], &toc_header);
 
-	for (toc_entry = toc_entries;
-	     toc_entry->cmdline_name != NULL;
-	     toc_entry++) {
+	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
 		image_t *image;
 
-		if (toc_entry->action != DO_REMOVE)
+		if (desc->action != DO_REMOVE)
 			continue;
-		image = lookup_image_from_uuid(&toc_entry->uuid);
+
+		image = lookup_image_from_uuid(&desc->uuid);
 		if (image != NULL) {
 			if (verbose)
-				log_dbgx("Removing %s.bin",
-				    toc_entry->cmdline_name);
+				log_dbgx("Removing %s",
+				    desc->cmdline_name);
 			remove_image(image);
 		} else {
-			log_warnx("Requested image %s.bin is not in %s",
-			    toc_entry->cmdline_name, argv[0]);
+			log_warnx("%s does not exist in %s",
+			    desc->cmdline_name, argv[0]);
 		}
 	}
 
@@ -912,7 +1145,9 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool remove [--force] [--out FIP_FILENAME] [opts] FIP_FILENAME\n");
+	printf("fiptool remove [--blob uuid=...] [--force] "
+	    "[--out FIP_FILENAME] [opts] FIP_FILENAME\n");
+	printf("  --blob uuid=...\tRemove an image with the given UUID.\n");
 	printf("  --force\t\tIf the output FIP file already exists, use --force to "
 	    "overwrite it.\n");
 	printf("  --out FIP_FILENAME\tSet an alternative output FIP file.\n");
@@ -1011,6 +1246,7 @@
 	if (argc == 0)
 		usage();
 
+	fill_image_descs();
 	for (i = 0; i < NELEM(cmds); i++) {
 		if (strcmp(cmds[i].name, argv[0]) == 0) {
 			ret = cmds[i].handler(argc, argv);
@@ -1019,5 +1255,6 @@
 	}
 	if (i == NELEM(cmds))
 		usage();
+	free_image_descs();
 	return ret;
 }
diff --git a/tools/fiptool/fiptool.h b/tools/fiptool/fiptool.h
index 7705107..6ace400 100644
--- a/tools/fiptool/fiptool.h
+++ b/tools/fiptool/fiptool.h
@@ -38,10 +38,8 @@
 
 #define NELEM(x) (sizeof (x) / sizeof *(x))
 
-/* TODO: Do not hardcode, use realloc() */
-#define MAX_IMAGES 32
-
 enum {
+	DO_UNSPEC = 0,
 	DO_PACK   = 1,
 	DO_UNPACK = 2,
 	DO_REMOVE = 3
@@ -53,16 +51,26 @@
 	LOG_ERR
 };
 
+typedef struct image_desc {
+	uuid_t             uuid;
+	char              *name;
+	char              *cmdline_name;
+	int                action;
+	char              *action_arg;
+	struct image_desc *next;
+} image_desc_t;
+
 typedef struct image {
-	uuid_t            uuid;
-	size_t            size;
-	void             *buffer;
+	uuid_t             uuid;
+	size_t             size;
+	void              *buffer;
+	struct image      *next;
 } image_t;
 
 typedef struct cmd {
-	char             *name;
-	int             (*handler)(int, char **);
-	void            (*usage)(void);
+	char              *name;
+	int              (*handler)(int, char **);
+	void             (*usage)(void);
 } cmd_t;
 
 #endif /* __FIPTOOL_H__ */
diff --git a/tools/fiptool/tbbr_config.c b/tools/fiptool/tbbr_config.c
index b92e8da..7e5e0a3 100644
--- a/tools/fiptool/tbbr_config.c
+++ b/tools/fiptool/tbbr_config.c
@@ -38,152 +38,110 @@
 	{
 		.name = "SCP Firmware Updater Configuration FWU SCP_BL2U",
 		.uuid = UUID_TRUSTED_UPDATE_FIRMWARE_SCP_BL2U,
-		.cmdline_name = "scp-fwu-cfg",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "scp-fwu-cfg"
 	},
 	{
 		.name = "AP Firmware Updater Configuration BL2U",
 		.uuid = UUID_TRUSTED_UPDATE_FIRMWARE_BL2U,
-		.cmdline_name = "ap-fwu-cfg",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "ap-fwu-cfg"
 	},
 	{
 		.name = "Firmware Updater NS_BL2U",
 		.uuid = UUID_TRUSTED_UPDATE_FIRMWARE_NS_BL2U,
-		.cmdline_name = "fwu",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "fwu"
 	},
 	{
 		.name = "Non-Trusted Firmware Updater certificate",
 		.uuid = UUID_TRUSTED_FWU_CERT,
-		.cmdline_name = "fwu-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "fwu-cert"
 	},
 	{
 		.name = "Trusted Boot Firmware BL2",
 		.uuid = UUID_TRUSTED_BOOT_FIRMWARE_BL2,
-		.cmdline_name = "tb-fw",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "tb-fw"
 	},
 	{
 		.name = "SCP Firmware SCP_BL2",
 		.uuid = UUID_SCP_FIRMWARE_SCP_BL2,
-		.cmdline_name = "scp-fw",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "scp-fw"
 	},
 	{
 		.name = "EL3 Runtime Firmware BL31",
 		.uuid = UUID_EL3_RUNTIME_FIRMWARE_BL31,
-		.cmdline_name = "soc-fw",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "soc-fw"
 	},
 	{
 		.name = "Secure Payload BL32 (Trusted OS)",
 		.uuid = UUID_SECURE_PAYLOAD_BL32,
-		.cmdline_name = "tos-fw",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "tos-fw"
 	},
 	{
 		.name = "Non-Trusted Firmware BL33",
 		.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33,
-		.cmdline_name = "nt-fw",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "nt-fw"
 	},
+
 	/* Key Certificates */
 	{
 		.name = "Root Of Trust key certificate",
 		.uuid = UUID_ROT_KEY_CERT,
-		.cmdline_name = "rot-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "rot-cert"
 	},
 	{
 		.name = "Trusted key certificate",
 		.uuid = UUID_TRUSTED_KEY_CERT,
-		.cmdline_name = "trusted-key-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "trusted-key-cert"
 	},
 	{
 		.name = "SCP Firmware key certificate",
 		.uuid = UUID_SCP_FW_KEY_CERT,
-		.cmdline_name = "scp-fw-key-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "scp-fw-key-cert"
 	},
 	{
 		.name = "SoC Firmware key certificate",
 		.uuid = UUID_SOC_FW_KEY_CERT,
-		.cmdline_name = "soc-fw-key-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "soc-fw-key-cert"
 	},
 	{
 		.name = "Trusted OS Firmware key certificate",
 		.uuid = UUID_TRUSTED_OS_FW_KEY_CERT,
-		.cmdline_name = "tos-fw-key-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "tos-fw-key-cert"
 	},
 	{
 		.name = "Non-Trusted Firmware key certificate",
 		.uuid = UUID_NON_TRUSTED_FW_KEY_CERT,
-		.cmdline_name = "nt-fw-key-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "nt-fw-key-cert"
 	},
+
 	/* Content certificates */
 	{
 		.name = "Trusted Boot Firmware BL2 certificate",
 		.uuid = UUID_TRUSTED_BOOT_FW_CERT,
-		.cmdline_name = "tb-fw-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "tb-fw-cert"
 	},
 	{
 		.name = "SCP Firmware content certificate",
 		.uuid = UUID_SCP_FW_CONTENT_CERT,
-		.cmdline_name = "scp-fw-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "scp-fw-cert"
 	},
 	{
 		.name = "SoC Firmware content certificate",
 		.uuid = UUID_SOC_FW_CONTENT_CERT,
-		.cmdline_name = "soc-fw-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "soc-fw-cert"
 	},
 	{
 		.name = "Trusted OS Firmware content certificate",
 		.uuid = UUID_TRUSTED_OS_FW_CONTENT_CERT,
-		.cmdline_name = "tos-fw-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "tos-fw-cert"
 	},
 	{
 		.name = "Non-Trusted Firmware content certificate",
 		.uuid = UUID_NON_TRUSTED_FW_CONTENT_CERT,
-		.cmdline_name = "nt-fw-cert",
-		.action = 0,
-		.action_arg = NULL
+		.cmdline_name = "nt-fw-cert"
 	},
 	{
 		.name = NULL,
 		.uuid = { 0 },
 		.cmdline_name = NULL,
-		.action = 0,
-		.action_arg = NULL
 	}
 };
-
-size_t toc_entries_len = sizeof(toc_entries) / sizeof(toc_entries[0]);
diff --git a/tools/fiptool/tbbr_config.h b/tools/fiptool/tbbr_config.h
index c459335..e1246c4 100644
--- a/tools/fiptool/tbbr_config.h
+++ b/tools/fiptool/tbbr_config.h
@@ -39,14 +39,11 @@
 #define TOC_HEADER_SERIAL_NUMBER 0x12345678
 
 typedef struct toc_entry {
-	const char   *name;
+	char         *name;
 	uuid_t        uuid;
-	const char   *cmdline_name;
-	int           action;
-	char         *action_arg;
+	char         *cmdline_name;
 } toc_entry_t;
 
 extern toc_entry_t toc_entries[];
-extern size_t toc_entries_len;
 
 #endif /* __TBBR_CONFIG_H__ */