diff --git a/.checkpatch.conf b/.checkpatch.conf
new file mode 100644
index 0000000..c8a6084
--- /dev/null
+++ b/.checkpatch.conf
@@ -0,0 +1,87 @@
+#
+# 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.
+#
+
+#
+# Configure how the Linux checkpatch script should be invoked in the context of
+# the Trusted Firmware source tree.
+#
+
+# This is not Linux so don't expect a Linux tree!
+--no-tree
+
+# 'Signed-off-by' lines in commit messages are not mandated for TF.
+--no-signoff
+
+# This clarifes the lines indications in the report.
+#
+# E.g.:
+# Without this option, we have the following output:
+#      #333: FILE: drivers/arm/gic/arm_gic.c:160:
+# So we have 2 lines indications (333 and 160), which is confusing.
+# We only care about the position in the source file.
+#
+# With this option, it becomes:
+#      drivers/arm/gic/arm_gic.c:160:
+--showfile
+
+#
+# Ignore the following message types, as they don't necessarily make sense in
+# the context of the Trusted Firmware.
+#
+
+# COMPLEX_MACRO generates false positives.
+--ignore COMPLEX_MACRO
+
+# Commit messages might contain a Gerrit Change-Id.
+--ignore GERRIT_CHANGE_ID
+
+# Do not check the format of commit messages, as Github's merge commits do not
+# observe it.
+--ignore GIT_COMMIT_ID
+
+# FILE_PATH_CHANGES reports this kind of message:
+# "added, moved or deleted file(s), does MAINTAINERS need updating?"
+# We do not use this MAINTAINERS file process in TF.
+--ignore FILE_PATH_CHANGES
+
+# AVOID_EXTERNS reports this kind of messages:
+# "externs should be avoided in .c files"
+# We don't follow this convention in TF.
+--ignore AVOID_EXTERNS
+
+# NEW_TYPEDEFS reports this kind of messages:
+# "do not add new typedefs"
+# We allow adding new typedefs in TF.
+--ignore NEW_TYPEDEFS
+
+# VOLATILE reports this kind of messages:
+# "Use of volatile is usually wrong: see Documentation/volatile-considered-harmful.txt"
+# We allow the usage of the volatile keyword in TF.
+--ignore VOLATILE
diff --git a/Makefile b/Makefile
index c5ec6d1..5f4a93c 100644
--- a/Makefile
+++ b/Makefile
@@ -62,6 +62,9 @@
 RESET_TO_BL31			:= 0
 # Include FP registers in cpu context
 CTX_INCLUDE_FPREGS		:= 0
+# Build flag to include AArch32 registers in cpu context save and restore
+# during world switch. This flag must be set to 0 for AArch64-only platforms.
+CTX_INCLUDE_AARCH32_REGS	:= 1
 # Determine the version of ARM GIC architecture to use for interrupt management
 # in EL3. The platform port can change this value if needed.
 ARM_GIC_ARCH			:= 2
@@ -101,22 +104,38 @@
 SPIN_ON_BL1_EXIT		:= 0
 # Build PL011 UART driver in minimal generic UART mode
 PL011_GENERIC_UART		:= 0
-
+# Flag to enable Performance Measurement Framework
+ENABLE_PMF			:= 0
+# Flag to enable PSCI STATs functionality
+ENABLE_PSCI_STAT	:= 0
 
 ################################################################################
 # Checkpatch script options
 ################################################################################
 
-CHECK_IGNORE		:=	--ignore COMPLEX_MACRO \
-				--ignore GERRIT_CHANGE_ID \
-				--ignore GIT_COMMIT_ID
-CHECKPATCH_ARGS		:=	--no-tree --no-signoff ${CHECK_IGNORE}
-CHECKCODE_ARGS		:=	--no-patch --no-tree --no-signoff ${CHECK_IGNORE}
-# Do not check the coding style on C library files or documentation files
-INCLUDE_DIRS_TO_CHECK	:=	$(sort $(filter-out include/stdlib, $(wildcard include/*)))
-LIB_DIRS_TO_CHECK	:=	$(sort $(filter-out lib/stdlib, $(wildcard lib/*)))
-ROOT_DIRS_TO_CHECK	:=	$(sort $(filter-out lib include docs %.md, $(wildcard *)))
-CHECK_PATHS		:=	${ROOT_DIRS_TO_CHECK} ${INCLUDE_DIRS_TO_CHECK} ${LIB_DIRS_TO_CHECK}
+CHECKCODE_ARGS		:=	--no-patch
+# Do not check the coding style on imported library files or documentation files
+INC_LIB_DIRS_TO_CHECK	:=	$(sort $(filter-out			\
+					include/lib/libfdt		\
+					include/lib/stdlib,		\
+					$(wildcard include/lib/*)))
+INC_DIRS_TO_CHECK	:=	$(sort $(filter-out			\
+					include/lib,			\
+					$(wildcard include/*)))
+LIB_DIRS_TO_CHECK	:=	$(sort $(filter-out			\
+					lib/libfdt			\
+					lib/stdlib,			\
+					$(wildcard lib/*)))
+ROOT_DIRS_TO_CHECK	:=	$(sort $(filter-out			\
+					lib				\
+					include				\
+					docs				\
+					%.md,				\
+					$(wildcard *)))
+CHECK_PATHS		:=	${ROOT_DIRS_TO_CHECK}			\
+				${INC_DIRS_TO_CHECK}			\
+				${INC_LIB_DIRS_TO_CHECK}		\
+				${LIB_DIRS_TO_CHECK}
 
 
 ################################################################################
@@ -136,7 +155,7 @@
 $(eval $(call add_define,DEBUG))
 ifneq (${DEBUG}, 0)
         BUILD_TYPE	:=	debug
-        CFLAGS		+= 	-g
+        TF_CFLAGS	+= 	-g
         ASFLAGS		+= 	-g -Wa,--gdwarf-2
         # Use LOG_LEVEL_INFO by default for debug builds
         LOG_LEVEL	:=	40
@@ -179,12 +198,12 @@
 				-Werror -Wmissing-include-dirs			\
 				-mgeneral-regs-only -D__ASSEMBLY__		\
 				${DEFINES} ${INCLUDES}
-CFLAGS			+= 	-nostdinc -ffreestanding -Wall			\
+TF_CFLAGS		+= 	-nostdinc -ffreestanding -Wall			\
 				-Werror -Wmissing-include-dirs			\
 				-mgeneral-regs-only -mstrict-align		\
 				-std=c99 -c -Os					\
 				${DEFINES} ${INCLUDES}
-CFLAGS			+=	-ffunction-sections -fdata-sections
+TF_CFLAGS		+=	-ffunction-sections -fdata-sections
 
 LDFLAGS			+=	--fatal-warnings -O1
 LDFLAGS			+=	--gc-sections
@@ -193,26 +212,15 @@
 ################################################################################
 # Common sources and include directories
 ################################################################################
+include lib/stdlib/stdlib.mk
 
 BL_COMMON_SOURCES	+=	common/bl_common.c			\
 				common/tf_printf.c			\
 				common/aarch64/debug.S			\
 				lib/aarch64/cache_helpers.S		\
 				lib/aarch64/misc_helpers.S		\
-				lib/stdlib/abort.c			\
-				lib/stdlib/assert.c			\
-				lib/stdlib/exit.c			\
-				lib/stdlib/mem.c			\
-				lib/stdlib/printf.c			\
-				lib/stdlib/putchar.c			\
-				lib/stdlib/puts.c			\
-				lib/stdlib/sscanf.c			\
-				lib/stdlib/strchr.c			\
-				lib/stdlib/strcmp.c			\
-				lib/stdlib/strlen.c			\
-				lib/stdlib/strncmp.c			\
-				lib/stdlib/subr_prf.c			\
-				plat/common/aarch64/platform_helpers.S
+				plat/common/aarch64/platform_helpers.S	\
+				${STDLIB_SRCS}
 
 INCLUDES		+=	-Iinclude/bl1			\
 				-Iinclude/bl31			\
@@ -227,8 +235,6 @@
 				-Iinclude/lib/aarch64		\
 				-Iinclude/lib/cpus/aarch64	\
 				-Iinclude/plat/common		\
-				-Iinclude/stdlib		\
-				-Iinclude/stdlib/sys		\
 				${PLAT_INCLUDES}		\
 				${SPD_INCLUDES}
 
@@ -331,7 +337,7 @@
 
 # Check if -pedantic option should be used
 ifeq (${DISABLE_PEDANTIC},0)
-        CFLAGS		+= 	-pedantic
+        TF_CFLAGS	+= 	-pedantic
 endif
 
 # Using the ARM Trusted Firmware BL2 implies that a BL33 image also needs to be
@@ -370,6 +376,10 @@
         endif
 endif
 
+# Make sure PMF is enabled if PSCI STAT is enabled.
+ifeq (${ENABLE_PSCI_STAT},1)
+ENABLE_PMF			:= 1
+endif
 
 ################################################################################
 # Auxiliary tools (fip_create, cert_create, etc)
@@ -392,6 +402,7 @@
 $(eval $(call assert_boolean,NS_TIMER_SWITCH))
 $(eval $(call assert_boolean,RESET_TO_BL31))
 $(eval $(call assert_boolean,CTX_INCLUDE_FPREGS))
+$(eval $(call assert_boolean,CTX_INCLUDE_AARCH32_REGS))
 $(eval $(call assert_boolean,ASM_ASSERTION))
 $(eval $(call assert_boolean,USE_COHERENT_MEM))
 $(eval $(call assert_boolean,DISABLE_PEDANTIC))
@@ -406,6 +417,8 @@
 $(eval $(call assert_boolean,ENABLE_PLAT_COMPAT))
 $(eval $(call assert_boolean,SPIN_ON_BL1_EXIT))
 $(eval $(call assert_boolean,PL011_GENERIC_UART))
+$(eval $(call assert_boolean,ENABLE_PMF))
+$(eval $(call assert_boolean,ENABLE_PSCI_STAT))
 
 
 ################################################################################
@@ -419,6 +432,7 @@
 $(eval $(call add_define,NS_TIMER_SWITCH))
 $(eval $(call add_define,RESET_TO_BL31))
 $(eval $(call add_define,CTX_INCLUDE_FPREGS))
+$(eval $(call add_define,CTX_INCLUDE_AARCH32_REGS))
 $(eval $(call add_define,ARM_GIC_ARCH))
 $(eval $(call add_define,ARM_CCI_PRODUCT_ID))
 $(eval $(call add_define,ASM_ASSERTION))
@@ -432,6 +446,8 @@
 $(eval $(call add_define,ENABLE_PLAT_COMPAT))
 $(eval $(call add_define,SPIN_ON_BL1_EXIT))
 $(eval $(call add_define,PL011_GENERIC_UART))
+$(eval $(call add_define,ENABLE_PMF))
+$(eval $(call add_define,ENABLE_PSCI_STAT))
 # Define the EL3_PAYLOAD_BASE flag only if it is provided.
 ifdef EL3_PAYLOAD_BASE
         $(eval $(call add_define,EL3_PAYLOAD_BASE))
@@ -443,7 +459,6 @@
         endif
 endif
 
-
 ################################################################################
 # Include BL specific makefiles
 ################################################################################
@@ -487,7 +502,7 @@
 
 # Check if deprecated declarations should be treated as error or not.
 ifeq (${ERROR_DEPRECATED},0)
-    CFLAGS		+= 	-Wno-error=deprecated-declarations
+    TF_CFLAGS		+= 	-Wno-error=deprecated-declarations
 endif
 
 # Expand build macros for the different images
@@ -550,15 +565,24 @@
 
 checkcodebase:		locate-checkpatch
 	@echo "  CHECKING STYLE"
-	@if test -d .git ; then	\
-		git ls-files | grep -v stdlib | while read GIT_FILE ; do ${CHECKPATCH} ${CHECKCODE_ARGS} -f $$GIT_FILE ; done ;	\
-	 else			\
-		 find . -type f -not -iwholename "*.git*" -not -iwholename "*build*" -not -iwholename "*stdlib*" -exec ${CHECKPATCH} ${CHECKCODE_ARGS} -f {} \; ;	\
-	 fi
+	@if test -d .git ; then						\
+		git ls-files | grep -E -v libfdt\|stdlib\|docs\|\.md |	\
+		while read GIT_FILE ;					\
+		do ${CHECKPATCH} ${CHECKCODE_ARGS} -f $$GIT_FILE ;	\
+		done ;							\
+	else								\
+		 find . -type f -not -iwholename "*.git*"		\
+		 -not -iwholename "*build*"				\
+		 -not -iwholename "*libfdt*"				\
+		 -not -iwholename "*stdlib*"				\
+		 -not -iwholename "*docs*"				\
+		 -not -iwholename "*.md"				\
+		 -exec ${CHECKPATCH} ${CHECKCODE_ARGS} -f {} \; ;	\
+	fi
 
 checkpatch:		locate-checkpatch
 	@echo "  CHECKING STYLE"
-	${Q}git log -p ${BASE_COMMIT}..HEAD -- ${CHECK_PATHS} | ${CHECKPATCH} ${CHECKPATCH_ARGS} - || true
+	${Q}git log -p ${BASE_COMMIT}..HEAD -- ${CHECK_PATHS} | ${CHECKPATCH} - || true
 
 certtool: ${CRTTOOL}
 
diff --git a/bl1/aarch64/bl1_exceptions.S b/bl1/aarch64/bl1_exceptions.S
index 9ff6a57..68f9b7a 100644
--- a/bl1/aarch64/bl1_exceptions.S
+++ b/bl1/aarch64/bl1_exceptions.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -34,42 +34,37 @@
 #include <bl1.h>
 #include <context.h>
 
+/* -----------------------------------------------------------------------------
+ * Very simple stackless exception handlers used by BL1.
+ * -----------------------------------------------------------------------------
+ */
 	.globl	bl1_exceptions
 
-	.section	.vectors, "ax"; .align 11
+vector_base bl1_exceptions
 
 	/* -----------------------------------------------------
-	 * Very simple stackless exception handlers used by BL1.
-	 * -----------------------------------------------------
-	 */
-	.align	7
-bl1_exceptions:
-	/* -----------------------------------------------------
 	 * Current EL with SP0 : 0x0 - 0x200
 	 * -----------------------------------------------------
 	 */
-SynchronousExceptionSP0:
+vector_entry SynchronousExceptionSP0
 	mov	x0, #SYNC_EXCEPTION_SP_EL0
 	bl	plat_report_exception
 	b	SynchronousExceptionSP0
 	check_vector_size SynchronousExceptionSP0
 
-	.align	7
-IrqSP0:
+vector_entry IrqSP0
 	mov	x0, #IRQ_SP_EL0
 	bl	plat_report_exception
 	b	IrqSP0
 	check_vector_size IrqSP0
 
-	.align	7
-FiqSP0:
+vector_entry FiqSP0
 	mov	x0, #FIQ_SP_EL0
 	bl	plat_report_exception
 	b	FiqSP0
 	check_vector_size FiqSP0
 
-	.align	7
-SErrorSP0:
+vector_entry SErrorSP0
 	mov	x0, #SERROR_SP_EL0
 	bl	plat_report_exception
 	b	SErrorSP0
@@ -79,29 +74,25 @@
 	 * Current EL with SPx: 0x200 - 0x400
 	 * -----------------------------------------------------
 	 */
-	.align	7
-SynchronousExceptionSPx:
+vector_entry SynchronousExceptionSPx
 	mov	x0, #SYNC_EXCEPTION_SP_ELX
 	bl	plat_report_exception
 	b	SynchronousExceptionSPx
 	check_vector_size SynchronousExceptionSPx
 
-	.align	7
-IrqSPx:
+vector_entry IrqSPx
 	mov	x0, #IRQ_SP_ELX
 	bl	plat_report_exception
 	b	IrqSPx
 	check_vector_size IrqSPx
 
-	.align	7
-FiqSPx:
+vector_entry FiqSPx
 	mov	x0, #FIQ_SP_ELX
 	bl	plat_report_exception
 	b	FiqSPx
 	check_vector_size FiqSPx
 
-	.align	7
-SErrorSPx:
+vector_entry SErrorSPx
 	mov	x0, #SERROR_SP_ELX
 	bl	plat_report_exception
 	b	SErrorSPx
@@ -111,8 +102,7 @@
 	 * Lower EL using AArch64 : 0x400 - 0x600
 	 * -----------------------------------------------------
 	 */
-	.align	7
-SynchronousExceptionA64:
+vector_entry SynchronousExceptionA64
 	/* Enable the SError interrupt */
 	msr	daifclr, #DAIF_ABT_BIT
 
@@ -127,22 +117,19 @@
 	b	smc_handler64
 	check_vector_size SynchronousExceptionA64
 
-	.align	7
-IrqA64:
+vector_entry IrqA64
 	mov	x0, #IRQ_AARCH64
 	bl	plat_report_exception
 	b	IrqA64
 	check_vector_size IrqA64
 
-	.align	7
-FiqA64:
+vector_entry FiqA64
 	mov	x0, #FIQ_AARCH64
 	bl	plat_report_exception
 	b	FiqA64
 	check_vector_size FiqA64
 
-	.align	7
-SErrorA64:
+vector_entry SErrorA64
 	mov	x0, #SERROR_AARCH64
 	bl	plat_report_exception
 	b   	SErrorA64
@@ -152,29 +139,25 @@
 	 * Lower EL using AArch32 : 0x600 - 0x800
 	 * -----------------------------------------------------
 	 */
-	.align	7
-SynchronousExceptionA32:
+vector_entry SynchronousExceptionA32
 	mov	x0, #SYNC_EXCEPTION_AARCH32
 	bl	plat_report_exception
 	b	SynchronousExceptionA32
 	check_vector_size SynchronousExceptionA32
 
-	.align	7
-IrqA32:
+vector_entry IrqA32
 	mov	x0, #IRQ_AARCH32
 	bl	plat_report_exception
 	b	IrqA32
 	check_vector_size IrqA32
 
-	.align	7
-FiqA32:
+vector_entry FiqA32
 	mov	x0, #FIQ_AARCH32
 	bl	plat_report_exception
 	b	FiqA32
 	check_vector_size FiqA32
 
-	.align	7
-SErrorA32:
+vector_entry SErrorA32
 	mov	x0, #SERROR_AARCH32
 	bl	plat_report_exception
 	b	SErrorA32
diff --git a/bl1/bl1_context_mgmt.c b/bl1/bl1_context_mgmt.c
index bd40608..972c7f6 100644
--- a/bl1/bl1_context_mgmt.c
+++ b/bl1/bl1_context_mgmt.c
@@ -32,6 +32,7 @@
 #include <assert.h>
 #include <context.h>
 #include <context_mgmt.h>
+#include <debug.h>
 #include <platform.h>
 
 /*
@@ -66,6 +67,19 @@
 	image_desc_t *image_desc;
 	entry_point_info_t *next_bl_ep;
 
+#if CTX_INCLUDE_AARCH32_REGS
+	/*
+	 * Ensure that the build flag to save AArch32 system registers in CPU
+	 * context is not set for AArch64-only platforms.
+	 */
+	if (((read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL1_SHIFT)
+			& ID_AA64PFR0_ELX_MASK) == 0x1) {
+		ERROR("EL1 supports AArch64-only. Please set build flag "
+				"CTX_INCLUDE_AARCH32_REGS = 0");
+		panic();
+	}
+#endif
+
 	/* Get the image descriptor. */
 	image_desc = bl1_plat_get_image_desc(image_id);
 	assert(image_desc);
diff --git a/bl31/aarch64/bl31_arch_setup.c b/bl31/aarch64/bl31_arch_setup.c
index 0871b41..3deacba 100644
--- a/bl31/aarch64/bl31_arch_setup.c
+++ b/bl31/aarch64/bl31_arch_setup.c
@@ -44,7 +44,7 @@
 void bl31_arch_setup(void)
 {
 	/* Program the counter frequency */
-	write_cntfrq_el0(plat_get_syscnt_freq());
+	write_cntfrq_el0(plat_get_syscnt_freq2());
 
 	/* Initialize the cpu_ops pointer. */
 	init_cpu_ops();
diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
index dc11e0a..799062e 100644
--- a/bl31/aarch64/runtime_exceptions.S
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -161,14 +161,14 @@
 	str	x18, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_SP_EL0]
 	.endm
 
-	.section	.vectors, "ax"; .align 11
-	.align	7
-runtime_exceptions:
+
+vector_base runtime_exceptions
+
 	/* -----------------------------------------------------
 	 * Current EL with _sp_el0 : 0x0 - 0x200
 	 * -----------------------------------------------------
 	 */
-sync_exception_sp_el0:
+vector_entry sync_exception_sp_el0
 	/* -----------------------------------------------------
 	 * We don't expect any synchronous exceptions from EL3
 	 * -----------------------------------------------------
@@ -176,23 +176,22 @@
 	bl	report_unhandled_exception
 	check_vector_size sync_exception_sp_el0
 
-	.align	7
 	/* -----------------------------------------------------
 	 * EL3 code is non-reentrant. Any asynchronous exception
 	 * is a serious error. Loop infinitely.
 	 * -----------------------------------------------------
 	 */
-irq_sp_el0:
+vector_entry irq_sp_el0
 	bl	report_unhandled_interrupt
 	check_vector_size irq_sp_el0
 
-	.align	7
-fiq_sp_el0:
+
+vector_entry fiq_sp_el0
 	bl	report_unhandled_interrupt
 	check_vector_size fiq_sp_el0
 
-	.align	7
-serror_sp_el0:
+
+vector_entry serror_sp_el0
 	bl	report_unhandled_exception
 	check_vector_size serror_sp_el0
 
@@ -200,8 +199,8 @@
 	 * Current EL with SPx: 0x200 - 0x400
 	 * -----------------------------------------------------
 	 */
-	.align	7
-sync_exception_sp_elx:
+
+vector_entry sync_exception_sp_elx
 	/* -----------------------------------------------------
 	 * This exception will trigger if anything went wrong
 	 * during a previous exception entry or exit or while
@@ -212,18 +211,15 @@
 	bl	report_unhandled_exception
 	check_vector_size sync_exception_sp_elx
 
-	.align	7
-irq_sp_elx:
+vector_entry irq_sp_elx
 	bl	report_unhandled_interrupt
 	check_vector_size irq_sp_elx
 
-	.align	7
-fiq_sp_elx:
+vector_entry fiq_sp_elx
 	bl	report_unhandled_interrupt
 	check_vector_size fiq_sp_elx
 
-	.align	7
-serror_sp_elx:
+vector_entry serror_sp_elx
 	bl	report_unhandled_exception
 	check_vector_size serror_sp_elx
 
@@ -231,8 +227,7 @@
 	 * Lower EL using AArch64 : 0x400 - 0x600
 	 * -----------------------------------------------------
 	 */
-	.align	7
-sync_exception_aarch64:
+vector_entry sync_exception_aarch64
 	/* -----------------------------------------------------
 	 * This exception vector will be the entry point for
 	 * SMCs and traps that are unhandled at lower ELs most
@@ -244,23 +239,20 @@
 	handle_sync_exception
 	check_vector_size sync_exception_aarch64
 
-	.align	7
 	/* -----------------------------------------------------
 	 * Asynchronous exceptions from lower ELs are not
 	 * currently supported. Report their occurrence.
 	 * -----------------------------------------------------
 	 */
-irq_aarch64:
+vector_entry irq_aarch64
 	handle_interrupt_exception irq_aarch64
 	check_vector_size irq_aarch64
 
-	.align	7
-fiq_aarch64:
+vector_entry fiq_aarch64
 	handle_interrupt_exception fiq_aarch64
 	check_vector_size fiq_aarch64
 
-	.align	7
-serror_aarch64:
+vector_entry serror_aarch64
 	bl	report_unhandled_exception
 	check_vector_size serror_aarch64
 
@@ -268,8 +260,7 @@
 	 * Lower EL using AArch32 : 0x600 - 0x800
 	 * -----------------------------------------------------
 	 */
-	.align	7
-sync_exception_aarch32:
+vector_entry sync_exception_aarch32
 	/* -----------------------------------------------------
 	 * This exception vector will be the entry point for
 	 * SMCs and traps that are unhandled at lower ELs most
@@ -281,27 +272,23 @@
 	handle_sync_exception
 	check_vector_size sync_exception_aarch32
 
-	.align	7
 	/* -----------------------------------------------------
 	 * Asynchronous exceptions from lower ELs are not
 	 * currently supported. Report their occurrence.
 	 * -----------------------------------------------------
 	 */
-irq_aarch32:
+vector_entry irq_aarch32
 	handle_interrupt_exception irq_aarch32
 	check_vector_size irq_aarch32
 
-	.align	7
-fiq_aarch32:
+vector_entry fiq_aarch32
 	handle_interrupt_exception fiq_aarch32
 	check_vector_size fiq_aarch32
 
-	.align	7
-serror_aarch32:
+vector_entry serror_aarch32
 	bl	report_unhandled_exception
 	check_vector_size serror_aarch32
 
-	.align	7
 
 	/* -----------------------------------------------------
 	 * The following code handles secure monitor calls.
diff --git a/bl31/bl31.ld.S b/bl31/bl31.ld.S
index dc6e7a4..33cbe4b 100644
--- a/bl31/bl31.ld.S
+++ b/bl31/bl31.ld.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -58,6 +58,14 @@
         KEEP(*(rt_svc_descs))
         __RT_SVC_DESCS_END__ = .;
 
+#if ENABLE_PMF
+        /* Ensure 8-byte alignment for descriptors and ensure inclusion */
+        . = ALIGN(8);
+        __PMF_SVC_DESCS_START__ = .;
+        KEEP(*(pmf_svc_descs))
+        __PMF_SVC_DESCS_END__ = .;
+#endif /* ENABLE_PMF */
+
         /*
          * Ensure 8-byte alignment for cpu_ops so that its fields are also
          * aligned. Also ensure cpu_ops inclusion.
@@ -132,6 +140,24 @@
         "PLAT_PERCPU_BAKERY_LOCK_SIZE does not match bakery lock requirements");
 #endif
 #endif
+
+#if ENABLE_PMF
+        /*
+         * Time-stamps are stored in normal .bss memory
+         *
+         * The compiler will allocate enough memory for one CPU's time-stamps,
+         * the remaining memory for other CPU's is allocated by the
+         * linker script
+         */
+        . = ALIGN(CACHE_WRITEBACK_GRANULE);
+        __PMF_TIMESTAMP_START__ = .;
+        KEEP(*(pmf_timestamp_array))
+        . = ALIGN(CACHE_WRITEBACK_GRANULE);
+        __PMF_PERCPU_TIMESTAMP_END__ = .;
+        __PERCPU_TIMESTAMP_SIZE__ = ABSOLUTE(. - __PMF_TIMESTAMP_START__);
+        . = . + (__PERCPU_TIMESTAMP_SIZE__ * (PLATFORM_CORE_COUNT - 1));
+        __PMF_TIMESTAMP_END__ = .;
+#endif /* ENABLE_PMF */
         __BSS_END__ = .;
     } >RAM
 
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 0f14bfa..8a7fccb 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -59,6 +59,14 @@
 BL31_SOURCES		+=	lib/locks/bakery/bakery_lock_normal.c
 endif
 
+ifeq (${ENABLE_PMF}, 1)
+BL31_SOURCES		+=	lib/pmf/pmf_main.c
+endif
+
+ifeq (${ENABLE_PSCI_STAT}, 1)
+BL31_SOURCES		+=	services/std_svc/psci/psci_stat.c
+endif
+
 BL31_LINKERFILE		:=	bl31/bl31.ld.S
 
 # Flag used to indicate if Crash reporting via console should be included
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index 835d41e..7f04d21 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -145,6 +145,19 @@
 	entry_point_info_t *next_image_info;
 	uint32_t image_type;
 
+#if CTX_INCLUDE_AARCH32_REGS
+	/*
+	 * Ensure that the build flag to save AArch32 system registers in CPU
+	 * context is not set for AArch64-only platforms.
+	 */
+	if (((read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL1_SHIFT)
+			& ID_AA64PFR0_ELX_MASK) == 0x1) {
+		ERROR("EL1 supports AArch64-only. Please set build flag "
+				"CTX_INCLUDE_AARCH32_REGS = 0");
+		panic();
+	}
+#endif
+
 	/* Determine which image to execute next */
 	image_type = bl31_get_next_image_type();
 
diff --git a/bl32/tsp/aarch64/tsp_exceptions.S b/bl32/tsp/aarch64/tsp_exceptions.S
index edcfb71..20e40df 100644
--- a/bl32/tsp/aarch64/tsp_exceptions.S
+++ b/bl32/tsp/aarch64/tsp_exceptions.S
@@ -28,10 +28,10 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <bl_common.h>
 #include <arch.h>
-#include <tsp.h>
 #include <asm_macros.S>
+#include <bl_common.h>
+#include <tsp.h>
 
 
 	/* ----------------------------------------------------
@@ -98,110 +98,90 @@
 	 * TSP exception handlers.
 	 * -----------------------------------------------------
 	 */
-	.section	.vectors, "ax"; .align 11
-
-	.align	7
-tsp_exceptions:
+vector_base tsp_exceptions
 	/* -----------------------------------------------------
-	 * Current EL with _sp_el0 : 0x0 - 0x180. No exceptions
+	 * Current EL with _sp_el0 : 0x0 - 0x200. No exceptions
 	 * are expected and treated as irrecoverable errors.
 	 * -----------------------------------------------------
 	 */
-sync_exception_sp_el0:
+vector_entry sync_exception_sp_el0
 	bl	plat_panic_handler
 	check_vector_size sync_exception_sp_el0
 
-	.align	7
-
-irq_sp_el0:
+vector_entry irq_sp_el0
 	bl	plat_panic_handler
 	check_vector_size irq_sp_el0
 
-	.align	7
-fiq_sp_el0:
+vector_entry fiq_sp_el0
 	bl	plat_panic_handler
 	check_vector_size fiq_sp_el0
 
-	.align	7
-serror_sp_el0:
+vector_entry serror_sp_el0
 	bl	plat_panic_handler
 	check_vector_size serror_sp_el0
 
 
 	/* -----------------------------------------------------
-	 * Current EL with SPx: 0x200 - 0x380. Only IRQs/FIQs
+	 * Current EL with SPx: 0x200 - 0x400. Only IRQs/FIQs
 	 * are expected and handled
 	 * -----------------------------------------------------
 	 */
-	.align	7
-sync_exception_sp_elx:
+vector_entry sync_exception_sp_elx
 	bl	plat_panic_handler
 	check_vector_size sync_exception_sp_elx
 
-	.align	7
-irq_sp_elx:
+vector_entry irq_sp_elx
 	handle_tsp_interrupt irq_sp_elx
 	check_vector_size irq_sp_elx
 
-	.align	7
-fiq_sp_elx:
+vector_entry fiq_sp_elx
 	handle_tsp_interrupt fiq_sp_elx
 	check_vector_size fiq_sp_elx
 
-	.align	7
-serror_sp_elx:
+vector_entry serror_sp_elx
 	bl	plat_panic_handler
 	check_vector_size serror_sp_elx
 
 
 	/* -----------------------------------------------------
-	 * Lower EL using AArch64 : 0x400 - 0x580. No exceptions
+	 * Lower EL using AArch64 : 0x400 - 0x600. No exceptions
 	 * are handled since TSP does not implement a lower EL
 	 * -----------------------------------------------------
 	 */
-	.align	7
-sync_exception_aarch64:
+vector_entry sync_exception_aarch64
 	bl	plat_panic_handler
 	check_vector_size sync_exception_aarch64
 
-	.align	7
-irq_aarch64:
+vector_entry irq_aarch64
 	bl	plat_panic_handler
 	check_vector_size irq_aarch64
 
-	.align	7
-fiq_aarch64:
+vector_entry fiq_aarch64
 	bl	plat_panic_handler
 	check_vector_size fiq_aarch64
 
-	.align	7
-serror_aarch64:
+vector_entry serror_aarch64
 	bl	plat_panic_handler
 	check_vector_size serror_aarch64
 
 
 	/* -----------------------------------------------------
-	 * Lower EL using AArch32 : 0x600 - 0x780. No exceptions
+	 * Lower EL using AArch32 : 0x600 - 0x800. No exceptions
 	 * handled since the TSP does not implement a lower EL.
 	 * -----------------------------------------------------
 	 */
-	.align	7
-sync_exception_aarch32:
+vector_entry sync_exception_aarch32
 	bl	plat_panic_handler
 	check_vector_size sync_exception_aarch32
 
-	.align	7
-irq_aarch32:
+vector_entry irq_aarch32
 	bl	plat_panic_handler
 	check_vector_size irq_aarch32
 
-	.align	7
-fiq_aarch32:
+vector_entry fiq_aarch32
 	bl	plat_panic_handler
 	check_vector_size fiq_aarch32
 
-	.align	7
-serror_aarch32:
+vector_entry serror_aarch32
 	bl	plat_panic_handler
 	check_vector_size serror_aarch32
-	.align	7
diff --git a/common/aarch64/context.S b/common/aarch64/context.S
index 0baa9b2..d51daa7 100644
--- a/common/aarch64/context.S
+++ b/common/aarch64/context.S
@@ -57,14 +57,6 @@
 	mrs	x10, elr_el1
 	stp	x9, x10, [x0, #CTX_SPSR_EL1]
 
-	mrs	x11, spsr_abt
-	mrs	x12, spsr_und
-	stp	x11, x12, [x0, #CTX_SPSR_ABT]
-
-	mrs	x13, spsr_irq
-	mrs	x14, spsr_fiq
-	stp	x13, x14, [x0, #CTX_SPSR_IRQ]
-
 	mrs	x15, sctlr_el1
 	mrs	x16, actlr_el1
 	stp	x15, x16, [x0, #CTX_SCTLR_EL1]
@@ -93,10 +85,6 @@
 	mrs	x10, tpidrro_el0
 	stp	x9, x10, [x0, #CTX_TPIDR_EL0]
 
-	mrs	x11, dacr32_el2
-	mrs	x12, ifsr32_el2
-	stp	x11, x12, [x0, #CTX_DACR32_EL2]
-
 	mrs	x13, par_el1
 	mrs	x14, far_el1
 	stp	x13, x14, [x0, #CTX_PAR_EL1]
@@ -109,6 +97,24 @@
 	mrs	x9, vbar_el1
 	stp	x17, x9, [x0, #CTX_CONTEXTIDR_EL1]
 
+	/* Save AArch32 system registers if the build has instructed so */
+#if CTX_INCLUDE_AARCH32_REGS
+	mrs	x11, spsr_abt
+	mrs	x12, spsr_und
+	stp	x11, x12, [x0, #CTX_SPSR_ABT]
+
+	mrs	x13, spsr_irq
+	mrs	x14, spsr_fiq
+	stp	x13, x14, [x0, #CTX_SPSR_IRQ]
+
+	mrs	x15, dacr32_el2
+	mrs	x16, ifsr32_el2
+	stp	x15, x16, [x0, #CTX_DACR32_EL2]
+
+	mrs	x17, fpexc32_el2
+	str	x17, [x0, #CTX_FP_FPEXC32_EL2]
+#endif
+
 	/* Save NS timer registers if the build has instructed so */
 #if NS_TIMER_SWITCH
 	mrs	x10, cntp_ctl_el0
@@ -123,9 +129,6 @@
 	str	x14, [x0, #CTX_CNTKCTL_EL1]
 #endif
 
-	mrs	x15, fpexc32_el2
-	str	x15, [x0, #CTX_FP_FPEXC32_EL2]
-
 	ret
 endfunc el1_sysregs_context_save
 
@@ -143,14 +146,6 @@
 	msr	spsr_el1, x9
 	msr	elr_el1, x10
 
-	ldp	x11, x12, [x0, #CTX_SPSR_ABT]
-	msr	spsr_abt, x11
-	msr	spsr_und, x12
-
-	ldp	x13, x14, [x0, #CTX_SPSR_IRQ]
-	msr	spsr_irq, x13
-	msr	spsr_fiq, x14
-
 	ldp	x15, x16, [x0, #CTX_SCTLR_EL1]
 	msr	sctlr_el1, x15
 	msr	actlr_el1, x16
@@ -179,10 +174,6 @@
 	msr	tpidr_el0, x9
 	msr	tpidrro_el0, x10
 
-	ldp	x11, x12, [x0, #CTX_DACR32_EL2]
-	msr	dacr32_el2, x11
-	msr	ifsr32_el2, x12
-
 	ldp	x13, x14, [x0, #CTX_PAR_EL1]
 	msr	par_el1, x13
 	msr	far_el1, x14
@@ -195,6 +186,23 @@
 	msr	contextidr_el1, x17
 	msr	vbar_el1, x9
 
+	/* Restore AArch32 system registers if the build has instructed so */
+#if CTX_INCLUDE_AARCH32_REGS
+	ldp	x11, x12, [x0, #CTX_SPSR_ABT]
+	msr	spsr_abt, x11
+	msr	spsr_und, x12
+
+	ldp	x13, x14, [x0, #CTX_SPSR_IRQ]
+	msr	spsr_irq, x13
+	msr	spsr_fiq, x14
+
+	ldp	x15, x16, [x0, #CTX_DACR32_EL2]
+	msr	dacr32_el2, x15
+	msr	ifsr32_el2, x16
+
+	ldr	x17, [x0, #CTX_FP_FPEXC32_EL2]
+	msr	fpexc32_el2, x17
+#endif
 	/* Restore NS timer registers if the build has instructed so */
 #if NS_TIMER_SWITCH
 	ldp	x10, x11, [x0, #CTX_CNTP_CTL_EL0]
@@ -209,11 +217,7 @@
 	msr	cntkctl_el1, x14
 #endif
 
-	ldr	x15, [x0, #CTX_FP_FPEXC32_EL2]
-	msr	fpexc32_el2, x15
-
 	/* No explict ISB required here as ERET covers it */
-
 	ret
 endfunc el1_sysregs_context_restore
 
diff --git a/common/aarch64/early_exceptions.S b/common/aarch64/early_exceptions.S
index 64bfcd0..0ef5950 100644
--- a/common/aarch64/early_exceptions.S
+++ b/common/aarch64/early_exceptions.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -31,140 +31,122 @@
 #include <asm_macros.S>
 #include <bl_common.h>
 
+/* -----------------------------------------------------------------------------
+ * Very simple stackless exception handlers used by BL2 and BL31 stages.
+ * BL31 uses them before stacks are setup. BL2 uses them throughout.
+ * -----------------------------------------------------------------------------
+ */
 	.globl	early_exceptions
 
-	.section	.vectors, "ax"; .align 11
+vector_base early_exceptions
 
 	/* -----------------------------------------------------
-	 * Very simple stackless exception handlers used by BL2
-	 * and BL31 bootloader stages. BL31 uses them before
-	 * stacks are setup. BL2 uses them throughout.
-	 * -----------------------------------------------------
-	 */
-	.align	7
-early_exceptions:
-	/* -----------------------------------------------------
-	 * Current EL with SP0 : 0x0 - 0x180
+	 * Current EL with SP0 : 0x0 - 0x200
 	 * -----------------------------------------------------
 	 */
-SynchronousExceptionSP0:
+vector_entry SynchronousExceptionSP0
 	mov	x0, #SYNC_EXCEPTION_SP_EL0
 	bl	plat_report_exception
 	b	SynchronousExceptionSP0
 	check_vector_size SynchronousExceptionSP0
 
-	.align	7
-IrqSP0:
+vector_entry IrqSP0
 	mov	x0, #IRQ_SP_EL0
 	bl	plat_report_exception
 	b	IrqSP0
 	check_vector_size IrqSP0
 
-	.align	7
-FiqSP0:
+vector_entry FiqSP0
 	mov	x0, #FIQ_SP_EL0
 	bl	plat_report_exception
 	b	FiqSP0
 	check_vector_size FiqSP0
 
-	.align	7
-SErrorSP0:
+vector_entry SErrorSP0
 	mov	x0, #SERROR_SP_EL0
 	bl	plat_report_exception
 	b	SErrorSP0
 	check_vector_size SErrorSP0
 
 	/* -----------------------------------------------------
-	 * Current EL with SPx: 0x200 - 0x380
+	 * Current EL with SPx: 0x200 - 0x400
 	 * -----------------------------------------------------
 	 */
-	.align	7
-SynchronousExceptionSPx:
+vector_entry SynchronousExceptionSPx
 	mov	x0, #SYNC_EXCEPTION_SP_ELX
 	bl	plat_report_exception
 	b	SynchronousExceptionSPx
 	check_vector_size SynchronousExceptionSPx
 
-	.align	7
-IrqSPx:
+vector_entry IrqSPx
 	mov	x0, #IRQ_SP_ELX
 	bl	plat_report_exception
 	b	IrqSPx
 	check_vector_size IrqSPx
 
-	.align	7
-FiqSPx:
+vector_entry FiqSPx
 	mov	x0, #FIQ_SP_ELX
 	bl	plat_report_exception
 	b	FiqSPx
 	check_vector_size FiqSPx
 
-	.align	7
-SErrorSPx:
+vector_entry SErrorSPx
 	mov	x0, #SERROR_SP_ELX
 	bl	plat_report_exception
 	b	SErrorSPx
 	check_vector_size SErrorSPx
 
 	/* -----------------------------------------------------
-	 * Lower EL using AArch64 : 0x400 - 0x580
+	 * Lower EL using AArch64 : 0x400 - 0x600
 	 * -----------------------------------------------------
 	 */
-	.align	7
-SynchronousExceptionA64:
+vector_entry SynchronousExceptionA64
 	mov	x0, #SYNC_EXCEPTION_AARCH64
 	bl	plat_report_exception
 	b	SynchronousExceptionA64
 	check_vector_size SynchronousExceptionA64
 
-	.align	7
-IrqA64:
+vector_entry IrqA64
 	mov	x0, #IRQ_AARCH64
 	bl	plat_report_exception
 	b	IrqA64
 	check_vector_size IrqA64
 
-	.align	7
-FiqA64:
+vector_entry FiqA64
 	mov	x0, #FIQ_AARCH64
 	bl	plat_report_exception
 	b	FiqA64
 	check_vector_size FiqA64
 
-	.align	7
-SErrorA64:
+vector_entry SErrorA64
 	mov	x0, #SERROR_AARCH64
 	bl	plat_report_exception
 	b   	SErrorA64
 	check_vector_size SErrorA64
 
 	/* -----------------------------------------------------
-	 * Lower EL using AArch32 : 0x0 - 0x180
+	 * Lower EL using AArch32 : 0x600 - 0x800
 	 * -----------------------------------------------------
 	 */
-	.align	7
-SynchronousExceptionA32:
+vector_entry SynchronousExceptionA32
 	mov	x0, #SYNC_EXCEPTION_AARCH32
 	bl	plat_report_exception
 	b	SynchronousExceptionA32
 	check_vector_size SynchronousExceptionA32
 
-	.align	7
-IrqA32:
+vector_entry IrqA32
 	mov	x0, #IRQ_AARCH32
 	bl	plat_report_exception
 	b	IrqA32
 	check_vector_size IrqA32
 
-	.align	7
-FiqA32:
+vector_entry FiqA32
 	mov	x0, #FIQ_AARCH32
 	bl	plat_report_exception
 	b	FiqA32
 	check_vector_size FiqA32
 
-	.align	7
-SErrorA32:
+vector_entry SErrorA32
 	mov	x0, #SERROR_AARCH32
 	bl	plat_report_exception
 	b	SErrorA32
diff --git a/common/bl_common.c b/common/bl_common.c
index 2e23fbf..7cafe63 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -189,12 +189,20 @@
 }
 
 /*******************************************************************************
- * Generic function to load an image at a specific address given a name and
- * extents of free memory. It updates the memory layout if the load is
- * successful, as well as the image information and the entry point information.
- * The caller might pass a NULL pointer for the entry point if it is not
- * interested in this information, e.g. because the image just needs to be
- * loaded in memory but won't ever be executed.
+ * Generic function to load an image at a specific address given an image ID and
+ * extents of free memory.
+ *
+ * If the load is successful then the image information is updated.
+ *
+ * If the entry_point_info argument is not NULL then this function also updates:
+ * - the memory layout to mark the memory as reserved;
+ * - the entry point information.
+ *
+ * The caller might pass a NULL pointer for the entry point if they are not
+ * interested in this information. This is typically the case for non-executable
+ * images (e.g. certificates) and executable images that won't ever be executed
+ * on the application processor (e.g. additional microcontroller firmware).
+ *
  * Returns 0 on success, a negative error code otherwise.
  ******************************************************************************/
 int load_image(meminfo_t *mem_layout,
@@ -259,6 +267,9 @@
 		goto exit;
 	}
 
+	image_data->image_base = image_base;
+	image_data->image_size = image_size;
+
 	/*
 	 * Update the memory usage info.
 	 * This is done after the actual loading so that it is not updated when
@@ -269,20 +280,16 @@
 	if (entry_point_info != NULL) {
 		reserve_mem(&mem_layout->free_base, &mem_layout->free_size,
 				image_base, image_size);
+		entry_point_info->pc = image_base;
 	} else {
 		INFO("Skip reserving memory: %p - %p\n", (void *) image_base,
 		     (void *) (image_base + image_size));
 	}
 
-	image_data->image_base = image_base;
-	image_data->image_size = image_size;
-
-	if (entry_point_info != NULL)
-		entry_point_info->pc = image_base;
-
 	/*
 	 * File has been successfully loaded.
-	 * Flush the image in TZRAM so that the next EL can see it.
+	 * Flush the image in Trusted SRAM so that the next exception level can
+	 * see it.
 	 */
 	flush_dcache_range(image_base, image_size);
 
diff --git a/docs/cpu-specific-build-macros.md b/docs/cpu-specific-build-macros.md
index c57dc7e..df2fbd8 100644
--- a/docs/cpu-specific-build-macros.md
+++ b/docs/cpu-specific-build-macros.md
@@ -22,14 +22,19 @@
 ARM Trusted Firmware exports a series of build flags which control the
 errata workarounds that are applied to each CPU by the reset handler. The
 errata details can be found in the CPU specific errata documents published
-by ARM. The errata workarounds are implemented for a particular revision
-or a set of processor revisions. This is checked by reset handler at runtime.
-Each errata workaround is identified by its `ID` as specified in the processor's
+by ARM:
+
+*   [Cortex-A53 MPCore Software Developers Errata Notice][A53 Errata Notice]
+*   [Cortex-A57 MPCore Software Developers Errata Notice][A57 Errata Notice]
+
+The errata workarounds are implemented for a particular revision or a set of
+processor revisions. This is checked by the reset handler at runtime. Each
+errata workaround is identified by its `ID` as specified in the processor's
 errata notice document. The format of the define used to enable/disable the
 errata workaround is `ERRATA_<Processor name>_<ID>`, where the `Processor name`
 is for example `A57` for the `Cortex_A57` CPU.
 
-All workarounds are disabled by default. The platform is reponsible for
+All workarounds are disabled by default. The platform is responsible for
 enabling these workarounds according to its requirement by defining the
 errata workaround build flags in the platform specific makefile. In case
 these workarounds are enabled for the wrong CPU revision then the errata
@@ -60,6 +65,21 @@
 *   `ERRATA_A57_813420`: This applies errata 813420 workaround to Cortex-A57
      CPU. This needs to be enabled only for revision r0p0 of the CPU.
 
+*   `ERRATA_A57_826974`: This applies errata 826974 workaround to Cortex-A57
+     CPU. This needs to be enabled only for revision <= r1p1 of the CPU.
+
+*   `ERRATA_A57_826977`: This applies errata 826977 workaround to Cortex-A57
+     CPU. This needs to be enabled only for revision <= r1p1 of the CPU.
+
+*   `ERRATA_A57_828024`: This applies errata 828024 workaround to Cortex-A57
+     CPU. This needs to be enabled only for revision <= r1p1 of the CPU.
+
+*   `ERRATA_A57_829520`: This applies errata 829520 workaround to Cortex-A57
+     CPU. This needs to be enabled only for revision <= r1p2 of the CPU.
+
+*   `ERRATA_A57_833471`: This applies errata 833471 workaround to Cortex-A57
+     CPU. This needs to be enabled only for revision <= r1p2 of the CPU.
+
 3.  CPU Specific optimizations
 ------------------------------
 
@@ -94,3 +114,5 @@
 _Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved._
 
 [A57 SW Optimization Guide]: http://infocenter.arm.com/help/topic/com.arm.doc.uan0015b/Cortex_A57_Software_Optimization_Guide_external.pdf
+[A53 Errata Notice]:         http://infocenter.arm.com/help/topic/com.arm.doc.epm048406/index.html
+[A57 Errata Notice]:         http://infocenter.arm.com/help/topic/com.arm.doc.epm049219/cortex_a57_mpcore_software_developers_errata_notice.pdf
diff --git a/docs/firmware-design.md b/docs/firmware-design.md
index 87bdb88..294349d 100644
--- a/docs/firmware-design.md
+++ b/docs/firmware-design.md
@@ -718,8 +718,8 @@
 |`CPU_HW_STATE`         | No      |                                           |
 |`SYSTEM_SUSPEND`       | Yes*    |                                           |
 |`PSCI_SET_SUSPEND_MODE`| No      |                                           |
-|`PSCI_STAT_RESIDENCY`  | No      |                                           |
-|`PSCI_STAT_COUNT`      | No      |                                           |
+|`PSCI_STAT_RESIDENCY`  | Yes*    |                                           |
+|`PSCI_STAT_COUNT`      | Yes*    |                                           |
 
 *Note : These PSCI APIs require platform power management hooks to be
 registered with the generic PSCI code to be supported.
diff --git a/docs/plat/qemu.md b/docs/plat/qemu.md
new file mode 100644
index 0000000..5e6bcbc
--- /dev/null
+++ b/docs/plat/qemu.md
@@ -0,0 +1,44 @@
+ARM Trusted Firmware for QEMU virt ARMv8-A
+==========================================
+
+ARM Trusted Firmware implements the EL3 firmware layer for QEMU virt
+ARMv8-A.  BL1 is used as the BootROM, supplied with the -bios argument.
+When QEMU starts all CPUs are released simultaneously, BL1 selects a
+primary CPU to handle the boot and the secondaries are placed in a polling
+loop to be released by normal world via PSCI.
+
+BL2 edits the Flattened Device Tree, FDT, generated by QEMU at run-time to
+add a node describing PSCI and also enable methods for the CPUs.
+
+An ARM64 defonfig v4.5 Linux kernel is known to boot, FTD doesn't need to be
+provided as it's generated by QEMU.
+
+Current limitations:
+* Only cold boot is supported
+* No build instructions for QEMU_EFI.fd and rootfs-arm64.cpio.gz
+* No instructions for how to load a BL32 (Secure Payload)
+
+`QEMU_EFI.fd` can be dowloaded from
+http://snapshots.linaro.org/components/kernel/leg-virt-tianocore-edk2-upstream/latest/QEMU-KERNEL-AARCH64/RELEASE_GCC49/QEMU_EFI.fd
+
+Boot binaries, except BL1, are primarily loaded via semi-hosting so all
+binaries has to reside in the same directory as QEMU is started from.  This
+is conveniently achieved with symlinks the local names as:
+* `bl2.bin` -> BL2
+* `bl31.bin` -> BL31
+* `bl33.bin` -> BL33 (`QEMU_EFI.fd`)
+* `Image` -> linux/Image
+
+To build:
+```
+make CROSS_COMPILE=aarch64-none-elf- PLAT=qemu 
+```
+
+To start (QEMU v2.6.0):
+```
+qemu-system-aarch64 -nographic -machine virt,secure=on -cpu cortex-a57	\
+	-kernel Image							\
+	-append console=ttyAMA0,38400 keep_bootcon root=/dev/vda2 	\
+	-initrd rootfs-arm64.cpio.gz -smp 2 -m 1024 -bios bl1.bin	\
+	-d unimp -semihosting-config enable,target=native
+```
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index 0d713c4..23033d5 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -76,8 +76,8 @@
 A platform port must enable the Memory Management Unit (MMU) as well as the
 instruction and data caches for each BL stage. Setting up the translation
 tables is the responsibility of the platform port because memory maps differ
-across platforms. A memory translation library (see `lib/aarch64/xlat_tables.c`)
-is provided to help in this setup. Note that although this library supports
+across platforms. A memory translation library (see `lib/xlat_tables/`) is
+provided to help in this setup. Note that although this library supports
 non-identity mappings, this is intended only for re-mapping peripheral physical
 addresses and allows platforms with high I/O addresses to reduce their virtual
 address space. All other addresses corresponding to code and data must currently
@@ -189,9 +189,20 @@
     Defines the local power state corresponding to the deepest retention state
     possible at every power domain level in the platform. This macro should be
     a value less than PLAT_MAX_OFF_STATE and greater than 0. It is used by the
-    PSCI implementation to distuiguish between retention and power down local
+    PSCI implementation to distinguish between retention and power down local
     power states within PSCI_CPU_SUSPEND call.
 
+*   **#define : PLAT_MAX_PWR_LVL_STATES**
+
+    Defines the maximum number of local power states per power domain level
+    that the platform supports. The default value of this macro is 2 since
+    most platforms just support a maximum of two local power states at each
+    power domain level (power-down and retention). If the platform needs to
+    account for more local power states, then it must redefine this macro.
+
+    Currently, this macro is used by the Generic PSCI implementation to size
+    the array used for PSCI_STAT_COUNT/RESIDENCY accounting.
+
 *   **#define : BL1_RO_BASE**
 
     Defines the base address in secure ROM where BL1 originally lives. Must be
@@ -448,6 +459,14 @@
     Defines the maximum number of open IO handles. Attempting to open more IO
     entities than this value using `io_open()` will fail with -ENOMEM.
 
+*   **#define : MAX_IO_BLOCK_DEVICES**
+
+    Defines the maximum number of registered IO block devices. Attempting to
+    register more devices this value using `io_dev_open()` will fail
+    with -ENOMEM. MAX_IO_BLOCK_DEVICES should be less than MAX_IO_DEVICES.
+    With this macro, multiple block devices could be supported at the same
+    time.
+
 If the platform needs to allocate data within the per-cpu data framework in
 BL31, it should define the following macro. Currently this is only required if
 the platform decides not to use the coherent memory section by undefining the
@@ -623,10 +642,19 @@
         digest            OCTET STRING
     }
 
-The function returns 0 on success. Any other value means the ROTPK could not be
-retrieved from the platform. The function also reports extra information related
-to the ROTPK in the flags parameter.
+The function returns 0 on success. Any other value is treated as error by the
+Trusted Board Boot. The function also reports extra information related
+to the ROTPK in the flags parameter:
 
+    ROTPK_IS_HASH      : Indicates that the ROTPK returned by the platform is a
+                         hash.
+    ROTPK_NOT_DEPLOYED : This allows the platform to skip certificate ROTPK
+                         verification while the platform ROTPK is not deployed.
+                         When this flag is set, the function does not need to
+                         return a platform ROTPK, and the authentication
+                         framework uses the ROTPK in the certificate without
+                         verifying it against the platform value. This flag
+                         must not be used in a deployed production environment.
 
 ### Function: plat_get_nv_ctr()
 
@@ -1521,10 +1549,10 @@
 (that was copied during `bl31_early_platform_setup()`) if the image exists. It
 should return NULL otherwise.
 
-### Function : plat_get_syscnt_freq() [mandatory]
+### Function : plat_get_syscnt_freq2() [mandatory]
 
     Argument : void
-    Return   : uint64_t
+    Return   : unsigned int
 
 This function is used by the architecture setup code to retrieve the counter
 frequency for the CPU's generic timer.  This value will be programmed into the
@@ -1707,6 +1735,22 @@
 resume execution by restoring this state when its powered on (see
 `pwr_domain_suspend_finish()`).
 
+#### plat_psci_ops.pwr_domain_pwr_down_wfi()
+
+This is an optional function and, if implemented, is expected to perform
+platform specific actions including the `wfi` invocation which allows the
+CPU to powerdown. Since this function is invoked outside the PSCI locks,
+the actions performed in this hook must be local to the CPU or the platform
+must ensure that races between multiple CPUs cannot occur.
+
+The `target_state` has a similar meaning as described in the `pwr_domain_off()`
+operation and it encodes the platform coordinated target local power states for
+the CPU power domain and its parent power domain levels. This function must
+not return back to the caller.
+
+If this function is not implemented by the platform, PSCI generic
+implementation invokes `psci_power_down_wfi()` for power down.
+
 #### plat_psci_ops.pwr_domain_on_finish()
 
 This function is called by the PSCI implementation after the calling CPU is
@@ -1759,6 +1803,34 @@
 `pwr_domain_suspend()` will be invoked with the coordinated target state to
 enter system suspend.
 
+#### plat_psci_ops.get_pwr_lvl_state_idx()
+
+This is an optional function and, if implemented, is invoked by the PSCI
+implementation to convert the `local_state` (first argument) at a specified
+`pwr_lvl` (second argument) to an index between 0 and
+`PLAT_MAX_PWR_LVL_STATES` - 1. This function is only needed if the platform
+supports more than two local power states at each power domain level, that is
+`PLAT_MAX_PWR_LVL_STATES` is greater than 2, and needs to account for these
+local power states.
+
+#### plat_psci_ops.translate_power_state_by_mpidr()
+
+This is an optional function and, if implemented, verifies the `power_state`
+(second argument) parameter of the PSCI API corresponding to a target power
+domain. The target power domain is identified by using both `MPIDR` (first
+argument) and the power domain level encoded in `power_state`. The power domain
+level specific local states are to be extracted from `power_state` and be
+populated in the `output_state` (third argument) array.  The functionality
+is similar to the `validate_power_state` function described above and is
+envisaged to be used in case the validity of `power_state` depend on the
+targeted power domain. If the `power_state` is invalid for the targeted power
+domain, the platform must return PSCI_E_INVALID_PARAMS as error. If this
+function is not implemented, then the generic implementation relies on
+`validate_power_state` function to translate the `power_state`.
+
+This function can also be used in case the platform wants to support local
+power state encoding for `power_state` parameter of PSCI_STAT_COUNT/RESIDENCY
+APIs as described in Section 5.18 of [PSCI].
 
 3.6  Interrupt Management framework (in BL31)
 ----------------------------------------------
@@ -2011,12 +2083,12 @@
 implementation. If more functionality is required, the needed library functions
 will need to be added to the local implementation.
 
-Versions of [FreeBSD] headers can be found in `include/stdlib`. Some of these
-headers have been cut down in order to simplify the implementation. In order to
-minimize changes to the header files, the [FreeBSD] layout has been maintained.
-The generic C library definitions can be found in `include/stdlib` with more
-system and machine specific declarations in `include/stdlib/sys` and
-`include/stdlib/machine`.
+Versions of [FreeBSD] headers can be found in `include/lib/stdlib`. Some of
+these headers have been cut down in order to simplify the implementation. In
+order to minimize changes to the header files, the [FreeBSD] layout has been
+maintained. The generic C library definitions can be found in
+`include/lib/stdlib` with more system and machine specific declarations in
+`include/lib/stdlib/sys` and `include/lib/stdlib/machine`.
 
 The local C library implementations can be found in `lib/stdlib`. In order to
 extend the C library these files may need to be modified. It is recommended to
diff --git a/docs/user-guide.md b/docs/user-guide.md
index e5e28a3..41a272f 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -8,27 +8,40 @@
 3.  [Tools](#3--tools)
 4.  [Getting the Trusted Firmware source code](#4--getting-the-trusted-firmware-source-code)
 5.  [Building the Trusted Firmware](#5--building-the-trusted-firmware)
-6.  [Building the rest of the software stack](#6--building-the-rest-of-the-software-stack)
+6.  [Building a FIP for Juno and FVP](#6--building-a-fip-for-juno-and-fvp)
 7.  [EL3 payloads alternative boot flow](#7--el3-payloads-alternative-boot-flow)
 8.  [Preloaded BL33 alternative boot flow](#8--preloaded-bl33-alternative-boot-flow)
-9.  [Preparing the images to run on FVP](#9--preparing-the-images-to-run-on-fvp)
-10. [Running the software on FVP](#10--running-the-software-on-fvp)
-11. [Running the software on Juno](#11--running-the-software-on-juno)
-12. [Changes required for booting Linux on FVP in GICv3 mode](#12--changes-required-for-booting-linux-on-fvp-in-gicv3-mode)
+9.  [Running the software on FVP](#9--running-the-software-on-fvp)
+10. [Running the software on Juno](#10--running-the-software-on-juno)
 
 
 1.  Introduction
 ----------------
 
-This document describes how to build ARM Trusted Firmware and run it with a
+This document describes how to build ARM Trusted Firmware (TF) and run it with a
 tested set of other software components using defined configurations on the Juno
 ARM development platform and ARM Fixed Virtual Platform (FVP) models. It is
 possible to use other software components, configurations and platforms but that
 is outside the scope of this document.
 
-This document should be used in conjunction with the [Firmware Design] and the
-[Instructions for using the Linaro software deliverables][Linaro SW Instructions].
+This document assumes that the reader has previous experience running a fully
+bootable Linux software stack on Juno or FVP using the prebuilt binaries and
+filesystems provided by [Linaro][Linaro Release Notes]. Further information may
+be found in the [Instructions for using the Linaro software deliverables][Linaro
+SW Instructions]. It also assumes that the user understands the role of the
+different software components required to boot a Linux system:
 
+*   Specific firmware images required by the platform (e.g. SCP firmware on Juno)
+*   Normal world bootloader (e.g. UEFI or U-Boot)
+*   Device tree
+*   Linux kernel image
+*   Root filesystem
+
+This document also assumes that the user is familiar with the FVP models and
+the different command line options available to launch the model.
+
+This document should be used in conjunction with the [Firmware Design].
+
 
 2.  Host machine requirements
 -----------------------------
@@ -48,53 +61,28 @@
 3.  Tools
 ---------
 
-In addition to the mandatory prerequisite tools listed in the [instructions for
-using the Linaro software deliverables][Linaro SW Instructions], the following
-optional tools may be needed:
+Install the required packages to build Trusted Firmware with the following
+command:
 
-*   `device-tree-compiler` package if you need to rebuild the Flattened Device
-    Tree (FDT) source files (`.dts` files) provided with this software.
+    sudo apt-get install build-essential gcc make git
 
-*   For debugging, ARM [Development Studio 5 (DS-5)][DS-5] v5.22.
+Download and install the AArch64 little-endian GCC cross compiler as indicated
+in the [Linaro instructions][Linaro SW Instructions].
 
+In addition, the following optional packages and tools may be needed:
 
-4.  Getting the Trusted Firmware source code
---------------------------------------------
-
-The Trusted Firmware (TF) source code can be obtained as part of the standard
-Linaro releases, which provide a full software stack, including TF, normal
-world firmware, Linux kernel and device tree, file system as well as any
-additional micro-controller firmware required by the platform. This TF version
-is tested with the [Linaro 15.10 Release][Linaro Release Notes].
-
-Note 1: Both the LSK kernel or the latest tracking kernel can be used with TF;
-choose the one that best suits your needs.
-
-Note 2: Currently to run the latest tracking kernel on FVP with GICv3 driver,
-some modifications are required to UEFI. Refer
-[here](#11--changes-required-for-booting-linux-on-fvp-in-gicv3-mode)
-for more details.
-
-The TF source code will then be in `arm-tf/`. This is the upstream git
-repository cloned from GitHub. The revision checked out by the `repo` tool is
-indicated by the manifest file. Depending on the manifest file you're using,
-this might not be the latest upstream version. To synchronize your copy of the
-repository and get the latest updates, use the following commands:
+*   `device-tree-compiler` package if you need to rebuild the Flattened Device
+    Tree (FDT) source files (`.dts` files) provided with this software.
 
-    # Change to the Trusted Firmware directory.
-    cd arm-tf
+*   `libssl-dev` package if Trusted Board Boot is enabled in the build.
 
-    # Download the latest code from GitHub.
-    git fetch github
+*   For debugging, ARM [Development Studio 5 (DS-5)][DS-5].
 
-    # Update your working copy to the latest master.
-    # This command will create a local branch master that tracks the remote
-    # branch master from GitHub.
-    git checkout --track github/master
 
+4.  Getting the Trusted Firmware source code
+--------------------------------------------
 
-Alternatively, the TF source code can be separately cloned from the upstream
-GitHub repository:
+Download the Trusted Firmware source code from Github:
 
     git clone https://github.com/ARM-software/arm-trusted-firmware.git
 
@@ -102,60 +90,42 @@
 5.  Building the Trusted Firmware
 ---------------------------------
 
-To build the Trusted Firmware images, change to the root directory of the
-Trusted Firmware source tree and follow these steps:
-
-1.  Set the compiler path, specify a Non-trusted Firmware image (BL33) and
-    a valid platform, and then build:
-
-        CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-linux-gnu- \
-        BL33=<path-to>/<bl33_image>                                \
-        make PLAT=<platform> all fip
+*   Before building Trusted Firmware, the environment variable `CROSS_COMPILE`
+    must point to the Linaro cross compiler:
 
-    If `PLAT` is not specified, `fvp` is assumed by default. See the "Summary of
-    build options" for more information on available build options.
+        export CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-linux-gnu-
 
-    The BL33 image corresponds to the software that is executed after switching
-    to the non-secure world. UEFI can be used as the BL33 image. Refer to the
-    "Building the rest of the software stack" section below.
+*   Change to the root directory of the Trusted Firmware source tree and build:
 
-    The TSP (Test Secure Payload), corresponding to the BL32 image, is not
-    compiled in by default. Refer to the "Building the Test Secure Payload"
-    section below.
+        make PLAT=<platform> all
 
-    By default this produces a release version of the build. To produce a debug
-    version instead, refer to the "Debugging options" section below.
+    Notes:
 
-    The build process creates products in a `build` directory tree, building
-    the objects and binaries for each boot loader stage in separate
-    sub-directories.  The following boot loader binary files are created from
-    the corresponding ELF files:
+    *   If `PLAT` is not specified, `fvp` is assumed by default. See the
+        "Summary of build options" for more information on available build
+        options.
 
-    *   `build/<platform>/<build-type>/bl1.bin`
-    *   `build/<platform>/<build-type>/bl2.bin`
-    *   `build/<platform>/<build-type>/bl31.bin`
+    *   The TSP (Test Secure Payload), corresponding to the BL32 image, is not
+        compiled in by default. Refer to the "Building the Test Secure Payload"
+        section below.
 
-    where `<platform>` is the name of the chosen platform and `<build-type>` is
-    either `debug` or `release`. A Firmware Image Package (FIP) will be created
-    as part of the build. It contains all boot loader images except for
-    `bl1.bin`.
+    *   By default this produces a release version of the build. To produce a
+        debug version instead, refer to the "Debugging options" section below.
 
-    *   `build/<platform>/<build-type>/fip.bin`
+    *   The build process creates products in a `build` directory tree, building
+        the objects and binaries for each boot loader stage in separate
+        sub-directories.  The following boot loader binary files are created
+        from the corresponding ELF files:
 
-    For more information on FIPs, see the "Firmware Image Package" section in
-    the [Firmware Design].
+        *   `build/<platform>/<build-type>/bl1.bin`
+        *   `build/<platform>/<build-type>/bl2.bin`
+        *   `build/<platform>/<build-type>/bl31.bin`
 
-2.  (Optional) Some platforms may require a SCP_BL2 image to boot. This image can
-    be included in the FIP when building the Trusted Firmware by specifying the
-    `SCP_BL2` build option:
+        where `<platform>` is the name of the chosen platform and `<build-type>`
+        is either `debug` or `release`. The actual number of images might differ
+        depending on the platform.
 
-        SCP_BL2=<path-to>/<scp_bl2_image>
-
-3.  Output binary files `bl1.bin` and `fip.bin` are both required to boot the
-    system. How these files are used is platform specific. Refer to the
-    platform documentation on how to use the firmware images.
-
-4.  (Optional) Build products for a specific build variant can be removed using:
+*   Build products for a specific build variant can be removed using:
 
         make DEBUG=<D> PLAT=<platform> clean
 
@@ -165,16 +135,6 @@
 
         make realclean
 
-5.  (Optional) Path to binary for certain BL stages (BL2, BL31 and BL32) can be
-    provided by specifying the BLx=<path-to>/<blx_image> where BLx is the BL stage.
-    This will bypass the build of the BL component from source, but will include
-    the specified binary in the final FIP image. Please note that BL32 will be
-    included in the build, only if the `SPD` build option is specified.
-
-    For example, specifying `BL2=<path-to>/<bl2_image>` in the build option,
-    will skip compilation of BL2 source in trusted firmware, but include the BL2
-    binary specified in the final FIP image.
-
 ### Summary of build options
 
 ARM Trusted Firmware build system supports the following build options. Unless
@@ -223,9 +183,6 @@
     optional. It is only needed if the platform makefile specifies that it
     is required in order to build the `fwu_fip` target.
 
-*   `CROSS_COMPILE`: Prefix to toolchain binaries. Please refer to examples in
-    this document for usage.
-
 *   `DEBUG`: Chooses between a debug and release build. It can take either 0
     (release) or 1 (debug) as values. 0 is the default.
 
@@ -423,6 +380,12 @@
     any register that is not part of the SBSA generic UART specification.
     Default value is 0 (a full PL011 compliant UART is present).
 
+*   `CTX_INCLUDE_AARCH32_REGS` : Boolean option that, when set to 1, will cause
+    the AArch32 system registers to be included when saving and restoring the
+    CPU context. The option must be set to 0 for AArch64-only platforms (that
+    is on hardware that does not implement AArch32, or at least not at EL1 and
+    higher ELs). Default value is 1.
+
 *   `CTX_INCLUDE_FPREGS`: Boolean option that, when set to 1, will cause the FP
     registers to be included when saving and restoring the CPU context. Default
     is 0.
@@ -445,6 +408,14 @@
 *   `HANDLE_EA_EL3_FIRST`: When defined External Aborts and SError Interrupts
     will be always trapped in EL3 i.e. in BL31 at runtime.
 
+*   `ENABLE_PMF`: Boolean option to enable support for optional Performance
+     Measurement Framework(PMF). Default is 0.
+
+*   `ENABLE_PSCI_STAT`: Boolean option to enable support for optional PSCI
+     functions `PSCI_STAT_RESIDENCY` and `PSCI_STAT_COUNT`. Default is 0.
+     Enabling this option enables the `ENABLE_PMF` build option as well.
+     The PMF is used for collecting the statistics.
+
 #### ARM development platform specific build options
 
 *   `ARM_TSP_RAM_LOCATION`: location of the TSP binary. Options:
@@ -525,79 +496,33 @@
 *   `FVP_USE_GIC_DRIVER`   : Selects the GIC driver to be built. Options:
     -    `FVP_GICV2`       : The GICv2 only driver is selected
     -    `FVP_GICV3`       : The GICv3 only driver is selected (default option)
-    -    `FVP_GICV3_LEGACY`: The Legacy GICv3 driver is selected (deprecated).
-
-    Note that if the FVP is configured for legacy VE memory map, then ARM
-    Trusted Firmware must be compiled with GICv2 only driver using
-    `FVP_USE_GIC_DRIVER=FVP_GICV2` build option.
+    -    `FVP_GICV3_LEGACY`: The Legacy GICv3 driver is selected (deprecated)
+          Note: If Trusted Firmware is compiled with this option on FVPs with
+          GICv3 hardware, then it configures the hardware to run in GICv2
+          emulation mode
 
 *   `FVP_CLUSTER_COUNT`    : Configures the cluster count to be used to
      build the topology tree within Trusted Firmware. By default the
      Trusted Firmware is configured for dual cluster topology and this option
      can be used to override the default value.
 
-### Creating a Firmware Image Package
+*   `FVP_USE_SP804_TIMER`  : Use the SP804 timer instead of the Generic Timer
+     for functions that wait for an arbitrary time length (udelay and mdelay).
+     The default value is 0.
 
-FIPs are automatically created as part of the build instructions described in
-the previous section. It is also possible to independently build the FIP
-creation tool and FIPs if required. To do this, follow these steps:
-
-Build the tool:
-
-    make -C tools/fip_create
-
-It is recommended to remove the build artifacts before rebuilding:
-
-    make -C tools/fip_create clean
-
-Create a Firmware package that contains existing BL2 and BL31 images:
-
-    # fip_create --help to print usage information
-    # fip_create <fip_name> <images to add> [--dump to show result]
-    ./tools/fip_create/fip_create fip.bin --dump \
-       --tb-fw build/<platform>/debug/bl2.bin --soc-fw build/<platform>/debug/bl31.bin
-
-     Firmware Image Package ToC:
-    ---------------------------
-    - Trusted Boot Firmware BL2: offset=0x88, size=0x81E8
-      file: 'build/<platform>/debug/bl2.bin'
-    - EL3 Runtime Firmware BL31: offset=0x8270, size=0xC218
-      file: 'build/<platform>/debug/bl31.bin'
-    ---------------------------
-    Creating "fip.bin"
-
-View the contents of an existing Firmware package:
-
-    ./tools/fip_create/fip_create fip.bin --dump
-
-     Firmware Image Package ToC:
-    ---------------------------
-    - Trusted Boot Firmware BL2: offset=0x88, size=0x81E8
-    - EL3 Runtime Firmware BL31: offset=0x8270, size=0xC218
-    ---------------------------
-
-Existing package entries can be individually updated:
-
-    # Change the BL2 from Debug to Release version
-    ./tools/fip_create/fip_create fip.bin --dump \
-      --tb-fw build/<platform>/release/bl2.bin
-
-    Firmware Image Package ToC:
-    ---------------------------
-    - Trusted Boot Firmware BL2: offset=0x88, size=0x7240
-      file: 'build/<platform>/release/bl2.bin'
-    - EL3 Runtime Firmware BL31: offset=0x72C8, size=0xC218
-    ---------------------------
-    Updating "fip.bin"
-
+*    `FVP_INTERCONNECT_DRIVER`: Selects the interconnect driver to be built. The
+     default interconnect driver depends on the value of `FVP_CLUSTER_COUNT` as
+     explained in the options below:
+     -    `FVP_CCI`           : The CCI driver is selected. This is the default
+                                if 0 < `FVP_CLUSTER_COUNT` <= 2.
+     -    `FVP_CCN`           : The CCN driver is selected. This is the default
+                                if `FVP_CLUSTER_COUNT` > 2.
 
 ### Debugging options
 
 To compile a debug version and make the build more verbose use
 
-    CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-linux-gnu- \
-    BL33=<path-to>/<bl33_image>                                \
-    make PLAT=<platform> DEBUG=1 V=1 all fip
+    make PLAT=<platform> DEBUG=1 V=1 all
 
 AArch64 GCC uses DWARF version 4 debugging symbols by default. Some tools (for
 example DS-5) might not support this and may need an older version of DWARF
@@ -615,9 +540,7 @@
 Extra debug options can be passed to the build system by setting `CFLAGS`:
 
     CFLAGS='-O0 -gdwarf-2'                                     \
-    CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-linux-gnu- \
-    BL33=<path-to>/<bl33_image>                                \
-    make PLAT=<platform> DEBUG=1 V=1 all fip
+    make PLAT=<platform> DEBUG=1 V=1 all
 
 It is also possible to introduce an infinite loop to help in debugging the
 post-BL2 phase of the Trusted Firmware. This can be done by rebuilding BL1 with
@@ -639,6 +562,7 @@
     # Resume execution
     continue
 
+
 ### Building the Test Secure Payload
 
 The TSP is coupled with a companion runtime service in the BL31 firmware,
@@ -647,49 +571,102 @@
 "Secure-EL1 Payloads and Dispatchers" section in the [Firmware Design].
 
 First clean the Trusted Firmware build directory to get rid of any previous
-BL31 binary. Then to build the TSP image and include it into the FIP use:
+BL31 binary. Then to build the TSP image use:
 
-    CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-linux-gnu- \
-    BL33=<path-to>/<bl33_image>                                \
-    make PLAT=<platform> SPD=tspd all fip
+    make PLAT=<platform> SPD=tspd all
 
 An additional boot loader binary file is created in the `build` directory:
 
-*   `build/<platform>/<build-type>/bl32.bin`
+    `build/<platform>/<build-type>/bl32.bin`
 
-The FIP will now contain the additional BL32 image. Here is an example
-output from an FVP build in release mode including BL32 and using
-`FVP_AARCH64_EFI.fd` as BL33 image:
 
-    Firmware Image Package ToC:
-    ---------------------------
-    - Trusted Boot Firmware BL2: offset=0xD8, size=0x6000
-      file: './build/fvp/release/bl2.bin'
-    - EL3 Runtime Firmware BL31: offset=0x60D8, size=0x9000
-      file: './build/fvp/release/bl31.bin'
-    - Secure Payload BL32 (Trusted OS): offset=0xF0D8, size=0x3000
-      file: './build/fvp/release/bl32.bin'
-    - Non-Trusted Firmware BL33: offset=0x120D8, size=0x280000
-      file: '../FVP_AARCH64_EFI.fd'
-    ---------------------------
-    Creating "build/fvp/release/fip.bin"
+### Checking source code style
 
+When making changes to the source for submission to the project, the source
+must be in compliance with the Linux style guide, and to assist with this check
+the project Makefile contains two targets, which both utilise the
+`checkpatch.pl` script that ships with the Linux source tree.
 
-### Building the Certificate Generation Tool
+To check the entire source tree, you must first download a copy of
+`checkpatch.pl` (or the full Linux source), set the `CHECKPATCH` environment
+variable to point to the script and build the target checkcodebase:
 
-The `cert_create` tool can be built separately through the following commands:
+    make CHECKPATCH=<path-to-linux>/linux/scripts/checkpatch.pl checkcodebase
 
-    $ cd tools/cert_create
-    $ make PLAT=<platform> [DEBUG=1] [V=1]
+To just check the style on the files that differ between your local branch and
+the remote master, use:
 
-`DEBUG=1` builds the tool in debug mode. `V=1` makes the build process more
-verbose. The following command should be used to obtain help about the tool:
+    make CHECKPATCH=<path-to-linux>/linux/scripts/checkpatch.pl checkpatch
 
-    $ ./cert_create -h
+If you wish to check your patch against something other than the remote master,
+set the `BASE_COMMIT` variable to your desired branch. By default, `BASE_COMMIT`
+is set to `origin/master`.
+
+
+### Building and using the FIP tool
+
+Firmware Image Package (FIP) is a packaging format used by the Trusted Firmware
+project to package firmware images in a single binary. The number and type of
+images that should be packed in a FIP is platform specific and may include TF
+images and other firmware images required by the platform. For example, most
+platforms require a BL33 image which corresponds to the normal world bootloader
+(e.g. UEFI or U-Boot).
+
+The TF build system provides the make target `fip` to create a FIP file for the
+specified platform using the FIP creation tool included in the TF project. For
+example, to build a FIP file for FVP, packaging TF images and a BL33 image:
+
+    make PLAT=fvp BL33=<path/to/bl33.bin> fip
 
-The `cert_create` tool is automatically built with the `fip` target when
-`GENERATE_COT=1`.
+The resulting FIP may be found in:
 
+    `build/fvp/<build-type>/fip.bin`
+
+For advanced operations on FIP files, it is also possible to independently build
+the tool and create or modify FIPs using this tool. To do this, follow these
+steps:
+
+It is recommended to remove old artifacts before building the tool:
+
+    make -C tools/fip_create clean
+
+Build the tool:
+
+    make [DEBUG=1] [V=1] fiptool
+
+The tool binary can be located in:
+
+    ./tools/fip_create/fip_create
+
+Invoking the tool with `--help` will print a help message with all available
+options.
+
+Example 1: create a new Firmware package `fip.bin` that contains BL2 and BL31:
+
+    ./tools/fip_create/fip_create \
+        --tb-fw build/<platform>/<build-type>/bl2.bin \
+        --soc-fw build/<platform>/<build-type>/bl31.bin \
+        fip.bin
+
+Example 2: view the contents of an existing Firmware package:
+
+    ./tools/fip_create/fip_create --dump <path-to>/fip.bin
+
+Example 3: update the entries of an existing Firmware package:
+
+    # Change the BL2 from Debug to Release version
+    ./tools/fip_create/fip_create \
+        --tb-fw build/<platform>/release/bl2.bin \
+        build/<platform>/debug/fip.bin
+
+Example 4: unpack all entries from an existing Firmware package:
+
+    # Images will be unpacked to the working directory
+    ./tools/fip_create/fip_create --unpack <path-to>/fip.bin
+
+More information about FIP can be found in the [Firmware Design document]
+[Firmware Design].
+
 
 ### Building FIP images with support for Trusted Board Boot
 
@@ -742,18 +719,17 @@
 
     Example of command line using RSA development keys:
 
-        CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-linux-gnu-      \
-        BL33=<path-to>/<bl33_image>                                     \
         MBEDTLS_DIR=<path of the directory containing mbed TLS sources> \
         make PLAT=<platform> TRUSTED_BOARD_BOOT=1 GENERATE_COT=1        \
         ARM_ROTPK_LOCATION=devel_rsa                                    \
         ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem        \
+        BL33=<path-to>/<bl33_image>                                     \
         all fip
 
-    The result of this build will be the bl1.bin and the fip.bin binaries, with
-    the difference that the FIP will include the certificates corresponding to
-    the Chain of Trust described in the TBBR-client document. These certificates
-    can also be found in the output build directory.
+    The result of this build will be the bl1.bin and the fip.bin binaries. This
+    FIP will include the certificates corresponding to the Chain of Trust
+    described in the TBBR-client document. These certificates can also be found
+    in the output build directory.
 
 3.  The optional FWU_FIP contains any additional images to be loaded from
     Non-Volatile storage during the [Firmware Update] process. To build the
@@ -766,15 +742,14 @@
     Example of Juno command line for generating both `fwu` and `fwu_fip`
     targets using RSA development:
 
-        CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf-       \
-        BL33=<path-to>/<bl33_image>                                     \
-        SCP_BL2=<path-to>/<scp_bl2_image>                               \
-        SCP_BL2U=<path-to>/<scp_bl2u_image>                             \
-        NS_BL2U=<path-to>/<ns_bl2u_image>                               \
         MBEDTLS_DIR=<path of the directory containing mbed TLS sources> \
         make PLAT=juno TRUSTED_BOARD_BOOT=1 GENERATE_COT=1              \
         ARM_ROTPK_LOCATION=devel_rsa                                    \
         ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem        \
+        BL33=<path-to>/<bl33_image>                                     \
+        SCP_BL2=<path-to>/<scp_bl2_image>                               \
+        SCP_BL2U=<path-to>/<scp_bl2u_image>                             \
+        NS_BL2U=<path-to>/<ns_bl2u_image>                               \
         all fip fwu_fip
 
     Note:   The BL2U image will be built by default and added to the FWU_FIP.
@@ -789,92 +764,75 @@
     Chain of Trust described in the TBBR-client document. These certificates
     can also be found in the output build directory.
 
-
-### Checking source code style
-
-When making changes to the source for submission to the project, the source
-must be in compliance with the Linux style guide, and to assist with this check
-the project Makefile contains two targets, which both utilise the
-`checkpatch.pl` script that ships with the Linux source tree.
-
-To check the entire source tree, you must first download a copy of
-`checkpatch.pl` (or the full Linux source), set the `CHECKPATCH` environment
-variable to point to the script and build the target checkcodebase:
-
-    make CHECKPATCH=<path-to-linux>/linux/scripts/checkpatch.pl checkcodebase
-
-To just check the style on the files that differ between your local branch and
-the remote master, use:
-
-    make CHECKPATCH=<path-to-linux>/linux/scripts/checkpatch.pl checkpatch
 
-If you wish to check your patch against something other than the remote master,
-set the `BASE_COMMIT` variable to your desired branch. By default, `BASE_COMMIT`
-is set to `origin/master`.
-
-
-6.  Building the rest of the software stack
--------------------------------------------
+### Building the Certificate Generation Tool
 
-The Linaro release provides a set of scripts that automate the process of
-building all components of the software stack. However, the scripts only support
-a limited number of Trusted Firmware build options. Therefore, it is recommended
-to modify these scripts to build all components except Trusted Firmware, and
-build Trusted Firmware separately as described in the section "Building the
-Trusted Firmware" above.
+The `cert_create` tool is built as part of the TF build process when the `fip`
+make target is specified and TBB is enabled (as described in the previous
+section), but it can also be built separately with the following command:
 
-The instructions below are targeted at an OpenEmbedded filesystem.
+    make PLAT=<platform> [DEBUG=1] [V=1] certtool
 
-1.  To exclude Trusted Firmware from the automated build process, edit the
-    variant file `build-scripts/variants/<platform>-oe`, where `<platform>`
-    is either `fvp` or `juno`. Add the following lines at the end of the file:
+Specifying the platform is mandatory since the tool is platform specific.
+`DEBUG=1` builds the tool in debug mode. `V=1` makes the build process more
+verbose. The following command should be used to obtain help about the tool:
 
-        # Disable ARM Trusted Firmware build
-        ARM_TF_BUILD_ENABLED=0
+    ./tools/cert_create/cert_create -h
 
-2.  Launch the build script:
 
-        CROSS_COMPILE=aarch64-linux-gnu- \
-        build-scripts/build-all.sh <platform>-oe
+6.  Building a FIP for Juno and FVP
+-----------------------------------
 
-### Preparing the Firmware Image Package
+This section provides Juno and FVP specific instructions to build Trusted
+Firmware, obtain the additional required firmware, and pack it all together in
+a single FIP binary. It assumes that a [Linaro Release][Linaro Release Notes]
+has been installed.
 
-The EDK2 binary should be specified as `BL33` in the `make` command line when
-building the Trusted Firmware. See the "Building the Trusted Firmware" section
-above. The EDK2 binary for use with the ARM Trusted Firmware can be found here:
+Note: follow the full instructions for one platform before switching to a
+different one. Mixing instructions for different platforms may result in
+corrupted binaries.
 
-    uefi/edk2/Build/ArmVExpress-FVP-AArch64-Minimal/DEBUG_GCC49/FV/FVP_AARCH64_EFI.fd   [for FVP]
-    uefi/edk2/Build/ArmJuno/DEBUG_GCC49/FV/BL33_AP_UEFI.fd                              [for Juno]
+1.  Clean the working directory
 
-### Building an alternative EDK2
+        make realclean
 
-*   By default, EDK2 is built in debug mode. To build a release version instead,
-    change the following line in the variant file:
+2.  Obtain SCP_BL2 (Juno) and BL33 (all platforms)
 
-        UEFI_BUILD_MODE=DEBUG
+    Use the fip_create tool to extract the SCP_BL2 and BL33 images from the FIP
+    package included in the Linaro release:
 
-    into:
+        # Build the fip_create tool
+        make [DEBUG=1] [V=1] fiptool
 
-        UEFI_BUILD_MODE=RELEASE
+        # Unpack firmware images from Linaro FIP
+        ./tools/fip_create/fip_create --unpack \
+             <path/to/linaro/release>/fip.bin
 
-*   On FVP, if legacy GICv2 locations are used, the EDK2 platform makefile must
-    be updated. This is required as EDK2 does not support probing for the GIC
-    location. To do this, first clean the EDK2 build directory:
+    The unpack operation will result in a set of binary images extracted to the
+    working directory. The SCP_BL2 image corresponds to `scp-fw.bin` and BL33
+    corresponds to `nt-fw.bin`.
 
-        build-scripts/build-uefi.sh fvp-oe clean
+    Note: the fip_create tool will complain if the images to be unpacked already
+    exist in the current directory. If that is the case, either delete those
+    files or use the `--force` option to overwrite.
 
-    Then edit the following file:
+3.  Build TF images and create a new FIP
 
-        uefi/edk2/ArmPlatformPkg/ArmVExpressPkg/ArmVExpress-FVP-AArch64.mak
+        # Juno
+        make PLAT=juno SCP_BL2=scp-fw.bin BL33=nt-fw.bin all fip
 
-    and add the following build flag into the `EDK2_MACROS` variable:
+        # FVP
+        make PLAT=fvp BL33=nt-fw.bin all fip
 
-        -D ARM_FVP_LEGACY_GICV2_LOCATION=1
+The resulting BL1 and FIP images may be found in:
 
-    Then rebuild everything as described above in step 2.
+    # Juno
+    ./build/juno/release/bl1.bin
+    ./build/juno/release/fip.bin
 
-    Finally rebuild the Trusted Firmware to generate a new FIP using the
-    instructions in the "Building the Trusted Firmware" section.
+    # FVP
+    ./build/fvp/release/bl1.bin
+    ./build/fvp/release/fip.bin
 
 
 7.  EL3 payloads alternative boot flow
@@ -1006,7 +964,6 @@
 will create a FIP without a BL33 and prepare to jump to a BL33 image loaded at
 address 0x80000000:
 
-    CROSS_COMPILE=<path-to>/bin/aarch64-linux-gnu- \
     make PRELOADED_BL33_BASE=0x80000000 PLAT=fvp all fip
 
 #### Boot of a preloaded bootwrapped kernel image on Base FVP
@@ -1039,62 +996,9 @@
 above in the EL3 payload boot flow section may be used to load the ELF file over
 JTAG on Juno.
 
-
-9.  Preparing the images to run on FVP
---------------------------------------
-
-Note: This section can be ignored when booting an EL3 payload, as no Flattened
-Device Tree or kernel image is needed in this case.
 
-### Obtaining the Flattened Device Trees
-
-Depending on the FVP configuration and Linux configuration used, different
-FDT files are required. FDTs for the Foundation and Base FVPs can be found in
-the Trusted Firmware source directory under `fdts/`. The Foundation FVP has a
-subset of the Base FVP components. For example, the Foundation FVP lacks CLCD
-and MMC support, and has only one CPU cluster.
-
-Note: It is not recommended to use the FDTs built along the kernel because not
-all FDTs are available from there.
-
-*   `fvp-base-gicv2-psci.dtb`
-
-    (Default) For use with both AEMv8 and Cortex-A57-A53 Base FVPs with
-    Base memory map configuration.
-
-*   `fvp-base-gicv2legacy-psci.dtb`
-
-    For use with AEMv8 Base FVP with legacy VE GIC memory map configuration.
-
-*   `fvp-base-gicv3-psci.dtb`
-
-    For use with both AEMv8 and Cortex-A57-A53 Base FVPs with Base memory map
-    configuration and Linux GICv3 support.
-
-*   `fvp-foundation-gicv2-psci.dtb`
-
-    (Default) For use with Foundation FVP with Base memory map configuration.
-
-*   `fvp-foundation-gicv2legacy-psci.dtb`
-
-    For use with Foundation FVP with legacy VE GIC memory map configuration.
-
-*   `fvp-foundation-gicv3-psci.dtb`
-
-    For use with Foundation FVP with Base memory map configuration and Linux
-    GICv3 support.
-
-Copy the chosen FDT blob as `fdt.dtb` to the directory from which the FVP
-is launched. Alternatively a symbolic link may be used.
-
-### Preparing the kernel image
-
-Copy the kernel image file `linux/arch/arm64/boot/Image` to the directory from
-which the FVP is launched. Alternatively a symbolic link may be used.
-
-
-10.  Running the software on FVP
---------------------------------
+9.  Running the software on FVP
+-------------------------------
 
 This version of the ARM Trusted Firmware has been tested on the following ARM
 FVPs (64-bit versions only).
@@ -1117,14 +1021,39 @@
 The Foundation FVP is a cut down version of the AArch64 Base FVP. It can be
 downloaded for free from [ARM's website][ARM FVP website].
 
-The Linaro release provides a script to run the software on FVP. However, it
-only supports a limited number of model parameter options. Therefore, it is
-recommended to launch the FVP manually for all use cases described below.
-
 Please refer to the FVP documentation for a detailed description of the model
 parameter options. A brief description of the important ones that affect the ARM
 Trusted Firmware and normal world software behavior is provided below.
 
+### Obtaining the Flattened Device Trees
+
+Depending on the FVP configuration and Linux configuration used, different
+FDT files are required. FDTs for the Foundation and Base FVPs can be found in
+the Trusted Firmware source directory under `fdts/`. The Foundation FVP has a
+subset of the Base FVP components. For example, the Foundation FVP lacks CLCD
+and MMC support, and has only one CPU cluster.
+
+Note: It is not recommended to use the FDTs built along the kernel because not
+all FDTs are available from there.
+
+*   `fvp-base-gicv2-psci.dtb`
+
+    For use with both AEMv8 and Cortex-A57-A53 Base FVPs with
+    Base memory map configuration.
+
+*   `fvp-base-gicv3-psci.dtb`
+
+    (Default) For use with both AEMv8 and Cortex-A57-A53 Base FVPs with Base
+    memory map configuration and Linux GICv3 support.
+
+*   `fvp-foundation-gicv2-psci.dtb`
+
+    For use with Foundation FVP with Base memory map configuration.
+
+*   `fvp-foundation-gicv3-psci.dtb`
+
+    (Default) For use with Foundation FVP with Base memory map configuration
+    and Linux GICv3 support.
 
 ### Running on the Foundation FVP with reset to BL1 entrypoint
 
@@ -1142,81 +1071,17 @@
     --data="<path-to>/<kernel-binary>"@0x80080000   \
     --block-device="<path-to>/<file-system-image>"
 
-1.  The `--data="<path-to-some-binary>"@0x...` parameters are used to load
-    binaries into memory.
-
-    *   BL1 is loaded at the start of the Trusted ROM.
-    *   The Firmware Image Package is loaded at the start of NOR FLASH0.
-    *   The Linux kernel image and device tree are loaded in DRAM.
-
-2.  The `--block-device` parameter is used to specify the path to the file
-    system image provided to Linux via VirtioBlock. Note that it must point to
-    the real file and that a symbolic link to this file cannot be used with the
-    FVP.
-
-The default use-case for the Foundation FVP is to enable the GICv3 device in
-the model but use the GICv2 FDT, in order for Linux to drive the GIC in GICv2
-emulation mode.
-
-### Notes regarding Base FVP configuration options
-
-Please refer to these notes in the subsequent "Running on the Base FVP"
-sections.
-
-1.  The `-C bp.flashloader0.fname` parameter is used to load a Firmware Image
-    Package at the start of NOR FLASH0 (see the "Building the Trusted Firmware"
-    section above).
-
-2.  Using `cache_state_modelled=1` makes booting very slow. The software will
-    still work (and run much faster) without this option but this will hide any
-    cache maintenance defects in the software.
-
-3.  The `-C bp.virtioblockdevice.image_path` parameter is used to specify the
-    path to the file system image provided to Linux via VirtioBlock. Note that
-    it must point to the real file and that a symbolic link to this file cannot
-    be used with the FVP. Ensure that the FVP doesn't output any error messages.
-    If the following error message is displayed:
-
-        ERROR: BlockDevice: Failed to open "<path-to>/<file-system-image>"!
-
-    then make sure the path to the file-system image in the model parameter is
-    correct and that read permission is correctly set on the file-system image
-    file.
-
-4.  Setting the `-C bp.secure_memory` parameter to `1` is only supported on
-    Base FVP versions 5.4 and newer. Setting this parameter to `0` is also
-    supported. The `-C bp.tzc_400.diagnostics=1` parameter is optional. It
-    instructs the FVP to provide some helpful information if a secure memory
-    violation occurs.
-
-5.  The `--data="<path-to-some-binary>"@<base-address-of-binary>` parameter is
-    used to load images into Base FVP memory. The base addresses used should
-    match the image base addresses used while linking the images. This parameter
-    is used to load the Linux kernel image and device tree into DRAM.
-
-6.  This and the following notes only apply when the firmware is built with
-    the `RESET_TO_BL31` option.
-
-    The `--data="<path-to><bl31|bl32|bl33-binary>"@<base-address-of-binary>`
-    parameter is needed to load the individual bootloader images in memory.
-    BL32 image is only needed if BL31 has been built to expect a Secure-EL1
-    Payload.
-
-7.  The `-C cluster<X>.cpu<Y>.RVBAR=@<base-address-of-bl31>` parameter, where
-    X and Y are the cluster and CPU numbers respectively, is used to set the
-    reset vector for each core.
-
-8.  Changing the default value of `ARM_TSP_RAM_LOCATION` will also require
-    changing the value of
-    `--data="<path-to><bl32-binary>"@<base-address-of-bl32>` to the new value of
-    `BL32_BASE`.
-
+Notes:
+*   BL1 is loaded at the start of the Trusted ROM.
+*   The Firmware Image Package is loaded at the start of NOR FLASH0.
+*   The Linux kernel image and device tree are loaded in DRAM.
+*   The default use-case for the Foundation FVP is to use the `--gicv3` option
+    and enable the GICv3 device in the model. Note that without this option,
+    the Foundation FVP defaults to legacy (Versatile Express) memory map which
+    is not supported by ARM Trusted Firmware.
 
 ### Running on the AEMv8 Base FVP with reset to BL1 entrypoint
 
-Please read "Notes regarding Base FVP configuration options" section above for
-information about some of the options to run the software.
-
 The following `FVP_Base_AEMv8A-AEMv8A` parameters should be used to boot Linux
 with 8 CPUs using the ARM Trusted Firmware.
 
@@ -1235,9 +1100,6 @@
 
 ### Running on the Cortex-A57-A53 Base FVP with reset to BL1 entrypoint
 
-Please read "Notes regarding Base FVP configuration options" section above for
-information about some of the options to run the software.
-
 The following `FVP_Base_Cortex-A57x4-A53x4` model parameters should be used to
 boot Linux with 8 CPUs using the ARM Trusted Firmware.
 
@@ -1254,9 +1116,6 @@
 
 ### Running on the AEMv8 Base FVP with reset to BL31 entrypoint
 
-Please read "Notes regarding Base FVP configuration options" section above for
-information about some of the options to run the software.
-
 The following `FVP_Base_AEMv8A-AEMv8A` parameters should be used to boot Linux
 with 8 CPUs using the ARM Trusted Firmware.
 
@@ -1282,10 +1141,24 @@
     --data cluster0.cpu0="<path-to>/<kernel-binary>"@0x80080000  \
     -C bp.virtioblockdevice.image_path="<path-to>/<file-system-image>"
 
-### Running on the Cortex-A57-A53 Base FVP with reset to BL31 entrypoint
+Notes:
 
-Please read "Notes regarding Base FVP configuration options" section above for
-information about some of the options to run the software.
+*   Since a FIP is not loaded when using BL31 as reset entrypoint, the
+    `--data="<path-to><bl31|bl32|bl33-binary>"@<base-address-of-binary>`
+    parameter is needed to load the individual bootloader images in memory.
+    BL32 image is only needed if BL31 has been built to expect a Secure-EL1
+    Payload.
+
+*   The `-C cluster<X>.cpu<Y>.RVBAR=@<base-address-of-bl31>` parameter, where
+    X and Y are the cluster and CPU numbers respectively, is used to set the
+    reset vector for each core.
+
+*   Changing the default value of `ARM_TSP_RAM_LOCATION` will also require
+    changing the value of
+    `--data="<path-to><bl32-binary>"@<base-address-of-bl32>` to the new value of
+    `BL32_BASE`.
+
+### Running on the Cortex-A57-A53 Base FVP with reset to BL31 entrypoint
 
 The following `FVP_Base_Cortex-A57x4-A53x4` model parameters should be used to
 boot Linux with 8 CPUs using the ARM Trusted Firmware.
@@ -1310,90 +1183,8 @@
     --data cluster0.cpu0="<path-to>/<kernel-binary>"@0x80080000  \
     -C bp.virtioblockdevice.image_path="<path-to>/<file-system-image>"
 
-### Configuring the GICv2 memory map
-
-The Base FVP models support GICv2 with the default model parameters at the
-following addresses. The Foundation FVP also supports these addresses when
-configured for GICv3 in GICv2 emulation mode.
-
-    GICv2 Distributor Interface     0x2f000000
-    GICv2 CPU Interface             0x2c000000
-    GICv2 Virtual CPU Interface     0x2c010000
-    GICv2 Hypervisor Interface      0x2c02f000
-
-The AEMv8 Base FVP can be configured to support GICv2 at addresses
-corresponding to the legacy (Versatile Express) memory map as follows. These are
-the default addresses when using the Foundation FVP in GICv2 mode.
-
-    GICv2 Distributor Interface     0x2c001000
-    GICv2 CPU Interface             0x2c002000
-    GICv2 Virtual CPU Interface     0x2c004000
-    GICv2 Hypervisor Interface      0x2c006000
-
-The choice of memory map is reflected in the build variant field (bits[15:12])
-in the `SYS_ID` register (Offset `0x0`) in the Versatile Express System
-registers memory map (`0x1c010000`).
-
-*   `SYS_ID.Build[15:12]`
 
-    `0x1` corresponds to the presence of the Base GIC memory map. This is the
-    default value on the Base FVPs.
-
-*   `SYS_ID.Build[15:12]`
-
-    `0x0` corresponds to the presence of the Legacy VE GIC memory map. This is
-    the default value on the Foundation FVP.
-
-This register can be configured as described in the following sections.
-
-NOTE: If the legacy VE GIC memory map is used, then Trusted Firmware must be
-compiled with the GICv2 only driver, and the corresponding FDT and BL33 images
-should be used.
-
-#### Configuring AEMv8 Foundation FVP GIC for legacy VE memory map
-
-The following parameters configure the Foundation FVP to use GICv2 with the
-legacy VE memory map:
-
-    <path-to>/Foundation_Platform             \
-    --cores=4                                 \
-    --secure-memory                           \
-    --visualization                           \
-    --no-gicv3                                \
-    --data="<path-to>/<bl1-binary>"@0x0       \
-    --data="<path-to>/<FIP-binary>"@0x8000000 \
-    --block-device="<path-to>/<file-system-image>"
-
-Explicit configuration of the `SYS_ID` register is not required.
-
-#### Configuring AEMv8 Base FVP GIC for legacy VE memory map
-
-The following parameters configure the AEMv8 Base FVP to use GICv2 with the
-legacy VE memory map. They must added to the parameters described in the
-"Running on the AEMv8 Base FVP" section above:
-
-    -C cluster0.gic.GICD-offset=0x1000                  \
-    -C cluster0.gic.GICC-offset=0x2000                  \
-    -C cluster0.gic.GICH-offset=0x4000                  \
-    -C cluster0.gic.GICH-other-CPU-offset=0x5000        \
-    -C cluster0.gic.GICV-offset=0x6000                  \
-    -C cluster0.gic.PERIPH-size=0x8000                  \
-    -C cluster1.gic.GICD-offset=0x1000                  \
-    -C cluster1.gic.GICC-offset=0x2000                  \
-    -C cluster1.gic.GICH-offset=0x4000                  \
-    -C cluster1.gic.GICH-other-CPU-offset=0x5000        \
-    -C cluster1.gic.GICV-offset=0x6000                  \
-    -C cluster1.gic.PERIPH-size=0x8000                  \
-    -C gic_distributor.GICD-alias=0x2c001000            \
-    -C gicv3.gicv2-only=1                               \
-    -C bp.variant=0x0
-
-The `bp.variant` parameter corresponds to the build variant field of the
-`SYS_ID` register.  Setting this to `0x0` allows the ARM Trusted Firmware to
-detect the legacy VE memory map while configuring the GIC.
-
-
-11.  Running the software on Juno
+10.  Running the software on Juno
 ---------------------------------
 
 This version of the ARM Trusted Firmware has been tested on Juno r0 and Juno r1.
@@ -1406,17 +1197,6 @@
 
 ### Preparing Trusted Firmware images
 
-The Juno platform requires a SCP_BL1 and a SCP_BL2 image to boot up. The
-SCP_BL1 image contains the ROM firmware that runs on the SCP (System Control
-Processor), whereas the SCP_BL2 image contains the SCP Runtime firmware. Both
-images are embedded within the Juno board recovery image, these are the files
-`bl0.bin` and `bl30.bin`, respectively. Please note that these filenames still
-use the old terminology.
-
-The SCP_BL2 file must be part of the FIP image. Therefore, its path must be
-supplied using the `SCP_BL2` variable on the command line when building the
-FIP. Please refer to the section "Building the Trusted Firmware".
-
 After building Trusted Firmware, the files `bl1.bin` and `fip.bin` need copying
 to the `SOFTWARE/` directory of the Juno SD card.
 
@@ -1430,67 +1210,16 @@
 ### Testing SYSTEM SUSPEND on Juno
 
 The SYSTEM SUSPEND is a PSCI API which can be used to implement system suspend
-to RAM. For more details refer to section 5.16 of [PSCI]. The [Linaro Release
-Notes] point to the required SCP and motherboard firmware binaries supporting
-this feature on Juno. The mainline linux kernel does not yet have support for
-this feature on Juno but it is queued to be merged in v4.4. Till that becomes
-available, the feature can be tested by using a custom kernel built from the
-following repository:
+to RAM. For more details refer to section 5.16 of [PSCI]. To test system suspend
+on Juno, at the linux shell prompt, issue the following command:
 
-    git clone git://git.kernel.org/pub/scm/linux/kernel/git/lpieralisi/linux.git
-    cd linux
-    git checkout firmware/psci-1.0
-
-Configure the linux kernel:
-
-    export CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-linux-gnu-
-    make ARCH=arm64 defconfig
-
-The feature is tested conveniently by using the RTC. Enable the RTC driver in
-menuconfig
-
-    make ARCH=arm64 menuconfig
-
-The PL031 RTC driver can be enabled at the following location in menuconfig
-
-    ARM AMBA PL031 RTC
-      |   Location:
-      |     -> Device Drivers
-      |       -> Real Time Clock
-
-Build the kernel
-
-    make ARCH=arm64 Image -j8
-
-Replace the kernel image in the `SOFTWARE/` directory of the Juno SD card with
-the `Image` from `arch/arm64/boot/` of the linux directory.
-
-Reset the board and wait for it to boot. At the shell prompt issue the
-following command:
-
-    echo +10 > /sys/class/rtc/rtc1/wakealarm
+    echo +10 > /sys/class/rtc/rtc0/wakealarm
     echo -n mem > /sys/power/state
 
 The Juno board should suspend to RAM and then wakeup after 10 seconds due to
 wakeup interrupt from RTC.
 
 
-12.  Changes required for booting Linux on FVP in GICv3 mode
-------------------------------------------------------------
-
-In case the TF FVP port is built with the build option
-`FVP_USE_GIC_DRIVER=FVP_GICV3`, then the GICv3 hardware cannot be used in
-GICv2 legacy mode. The default build of UEFI for FVP in
-[latest tracking kernel][Linaro Release Notes] configures GICv3 in GICv2 legacy
-mode. This can be changed by setting the build flag
-`gArmTokenSpaceGuid.PcdArmGicV3WithV2Legacy` to FALSE in
-`uefi/edk2/ArmPlatformPkg/ArmVExpressPkg/ArmVExpress-FVP-AArch64.dsc`.
-
-Recompile UEFI as mentioned [here][FVP Instructions].
-
-The GICv3 DTBs found in ARM Trusted Firmware source directory can be
-used to test the GICv3 kernel on the respective FVP models.
-
 - - - - - - - - - - - - - - - - - - - - - - - - - -
 
 _Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved._
@@ -1498,7 +1227,7 @@
 
 [Firmware Design]:             firmware-design.md
 [ARM FVP website]:             http://www.arm.com/fvp
-[Linaro Release Notes]:        https://community.arm.com/docs/DOC-10952#jive_content_id_Linaro_Release_1510
+[Linaro Release Notes]:        https://community.arm.com/docs/DOC-10952#jive_content_id_Linaro_Release_1602
 [ARM Platforms Portal]:        https://community.arm.com/groups/arm-development-platforms
 [Linaro SW Instructions]:      https://community.arm.com/docs/DOC-10803
 [Juno Instructions]:           https://community.arm.com/docs/DOC-10804
diff --git a/drivers/arm/ccn/ccn.c b/drivers/arm/ccn/ccn.c
index 28d2709..060acdd 100644
--- a/drivers/arm/ccn/ccn.c
+++ b/drivers/arm/ccn/ccn.c
@@ -254,6 +254,7 @@
 	assert(ccn_plat_desc);
 
 	FOR_EACH_PRESENT_MASTER_INTERFACE(iface_id, master_map) {
+		assert(iface_id < ccn_plat_desc->num_masters);
 
 		/* Convert the master ID into the node ID */
 		node_id = ccn_plat_desc->master_to_rn_id_map[iface_id];
@@ -501,3 +502,15 @@
 	}
 
 }
+
+/*******************************************************************************
+ * This function returns the part0 id from the peripheralID 0 register
+ * in CCN. This id can be used to distinguish the CCN variant present in the
+ * system.
+ ******************************************************************************/
+int ccn_get_part0_id(uintptr_t periphbase)
+{
+	assert(periphbase);
+	return (int)(mmio_read_64(periphbase
+			+ MN_PERIPH_ID_0_1_OFFSET) & 0xFF);
+}
diff --git a/drivers/arm/ccn/ccn_private.h b/drivers/arm/ccn/ccn_private.h
index 8b15472..fffa2ca 100644
--- a/drivers/arm/ccn/ccn_private.h
+++ b/drivers/arm/ccn/ccn_private.h
@@ -149,6 +149,7 @@
 #define MN_DDC_STAT_OFFSET	DOMAIN_CTRL_STAT_OFFSET
 #define MN_DDC_SET_OFFSET	DOMAIN_CTRL_SET_OFFSET
 #define MN_DDC_CLR_OFFSET	DOMAIN_CTRL_CLR_OFFSET
+#define MN_PERIPH_ID_0_1_OFFSET	0xFE0
 #define MN_ID_OFFSET		REGION_ID_OFFSET
 
 /* HNF System Address Map register bit masks and shifts */
diff --git a/drivers/auth/auth_mod.c b/drivers/auth/auth_mod.c
index 4184556..88ef0b0 100644
--- a/drivers/auth/auth_mod.c
+++ b/drivers/auth/auth_mod.c
@@ -199,8 +199,9 @@
 	}
 	return_if_error(rc);
 
-	/* If the PK is a hash of the key, retrieve the key from the image */
-	if (flags & ROTPK_IS_HASH) {
+	if (flags & (ROTPK_IS_HASH | ROTPK_NOT_DEPLOYED)) {
+		/* If the PK is a hash of the key or if the ROTPK is not
+		   deployed on the platform, retrieve the key from the image */
 		pk_hash_ptr = pk_ptr;
 		pk_hash_len = pk_len;
 		rc = img_parser_get_auth_param(img_desc->img_type,
@@ -215,9 +216,14 @@
 						 pk_ptr, pk_len);
 		return_if_error(rc);
 
-		/* Ask the crypto-module to verify the key hash */
-		rc = crypto_mod_verify_hash(pk_ptr, pk_len,
-					    pk_hash_ptr, pk_hash_len);
+		if (flags & ROTPK_NOT_DEPLOYED) {
+			NOTICE("ROTPK is not deployed on platform. "
+				"Skipping ROTPK verification.\n");
+		} else {
+			/* Ask the crypto-module to verify the key hash */
+			rc = crypto_mod_verify_hash(pk_ptr, pk_len,
+				    pk_hash_ptr, pk_hash_len);
+		}
 	} else {
 		/* Ask the crypto module to verify the signature */
 		rc = crypto_mod_verify_signature(data_ptr, data_len,
diff --git a/drivers/delay_timer/delay_timer.c b/drivers/delay_timer/delay_timer.c
index 0bee876..ed7ed52 100644
--- a/drivers/delay_timer/delay_timer.c
+++ b/drivers/delay_timer/delay_timer.c
@@ -48,19 +48,22 @@
 		(ops->clk_div != 0) &&
 		(ops->get_timer_value != 0));
 
-	uint32_t start, cnt, delta, delta_us;
+	uint32_t start, delta, total_delta;
 
-	/* counter is decreasing */
+	assert(usec < UINT32_MAX / ops->clk_div);
+
 	start = ops->get_timer_value();
+
+	total_delta = (usec * ops->clk_div) / ops->clk_mult;
+
 	do {
-		cnt = ops->get_timer_value();
-		if (cnt > start) {
-			delta = UINT32_MAX - cnt;
-			delta += start;
-		} else
-			delta = start - cnt;
-		delta_us = (delta * ops->clk_mult) / ops->clk_div;
-	} while (delta_us < usec);
+		/*
+		 * If the timer value wraps around, the subtraction will
+		 * overflow and it will still give the correct result.
+		 */
+		delta = start - ops->get_timer_value(); /* Decreasing counter */
+
+	} while (delta < total_delta);
 }
 
 /***********************************************************
diff --git a/plat/rockchip/common/plat_delay_timer.c b/drivers/delay_timer/generic_delay_timer.c
similarity index 64%
copy from plat/rockchip/common/plat_delay_timer.c
copy to drivers/delay_timer/generic_delay_timer.c
index 797ce05..4c364a3 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/drivers/delay_timer/generic_delay_timer.c
@@ -29,26 +29,54 @@
  */
 
 #include <arch_helpers.h>
+#include <assert.h>
+#include <bl_common.h>
+#include <debug.h>
 #include <delay_timer.h>
-#include <platform_def.h>
+#include <platform.h>
 
-static uint32_t plat_get_timer_value(void)
+/* Ticks elapsed in one second by a signal of 1 MHz */
+#define MHZ_TICKS_PER_SEC 1000000
+
+static timer_ops_t ops;
+
+static uint32_t get_timer_value(void)
 {
 	/*
 	 * Generic delay timer implementation expects the timer to be a down
 	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
+	 * by read_cntpct_el0() to simulate the down counter. The value is
+	 * clipped from 64 to 32 bits.
 	 */
 	return (uint32_t)(~read_cntpct_el0());
 }
 
+void generic_delay_timer_init_args(uint32_t mult, uint32_t div)
+{
+	ops.get_timer_value	= get_timer_value;
+	ops.clk_mult		= mult;
+	ops.clk_div		= div;
+
+	timer_init(&ops);
+
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+	VERBOSE("Generic delay timer configured with mult=%u and div=%u\n",
+		mult, div);
+}
 
-void plat_delay_timer_init(void)
+void generic_delay_timer_init(void)
 {
-	timer_init(&plat_timer_ops);
+	/* Value in ticks */
+	unsigned int mult = MHZ_TICKS_PER_SEC;
+
+	/* Value in ticks per second (Hz) */
+	unsigned int div  = plat_get_syscnt_freq2();
+
+	/* Reduce multiplier and divider by dividing them repeatedly by 10 */
+	while ((mult % 10 == 0) && (div % 10 == 0)) {
+		mult /= 10;
+		div /= 10;
+	}
+
+	generic_delay_timer_init_args(mult, div);
 }
+
diff --git a/drivers/emmc/emmc.c b/drivers/emmc/emmc.c
new file mode 100644
index 0000000..5fe28ef
--- /dev/null
+++ b/drivers/emmc/emmc.c
@@ -0,0 +1,347 @@
+/*
+ * 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.
+ *
+ * Defines a simple and generic interface to access eMMC device.
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <emmc.h>
+#include <errno.h>
+#include <string.h>
+
+static const emmc_ops_t *ops;
+static unsigned int emmc_ocr_value;
+static emmc_csd_t emmc_csd;
+
+static int emmc_device_state(void)
+{
+	emmc_cmd_t cmd;
+	int ret;
+
+	do {
+		memset(&cmd, 0, sizeof(emmc_cmd_t));
+		cmd.cmd_idx = EMMC_CMD13;
+		cmd.cmd_arg = EMMC_FIX_RCA << RCA_SHIFT_OFFSET;
+		cmd.resp_type = EMMC_RESPONSE_R1;
+		ret = ops->send_cmd(&cmd);
+		assert(ret == 0);
+		assert((cmd.resp_data[0] & STATUS_SWITCH_ERROR) == 0);
+		/* Ignore improbable errors in release builds */
+		(void)ret;
+	} while ((cmd.resp_data[0] & STATUS_READY_FOR_DATA) == 0);
+	return EMMC_GET_STATE(cmd.resp_data[0]);
+}
+
+static void emmc_set_ext_csd(unsigned int ext_cmd, unsigned int value)
+{
+	emmc_cmd_t cmd;
+	int ret, state;
+
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD6;
+	cmd.cmd_arg = EXTCSD_WRITE_BYTES | EXTCSD_CMD(ext_cmd) |
+		      EXTCSD_VALUE(value) | 1;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	/* wait to exit PRG state */
+	do {
+		state = emmc_device_state();
+	} while (state == EMMC_STATE_PRG);
+	/* Ignore improbable errors in release builds */
+	(void)ret;
+}
+
+static void emmc_set_ios(int clk, int bus_width)
+{
+	int ret;
+
+	/* set IO speed & IO bus width */
+	if (emmc_csd.spec_vers == 4)
+		emmc_set_ext_csd(CMD_EXTCSD_BUS_WIDTH, bus_width);
+	ret = ops->set_ios(clk, bus_width);
+	assert(ret == 0);
+	/* Ignore improbable errors in release builds */
+	(void)ret;
+}
+
+static int emmc_enumerate(int clk, int bus_width)
+{
+	emmc_cmd_t cmd;
+	int ret, state;
+
+	ops->init();
+
+	/* CMD0: reset to IDLE */
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD0;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	while (1) {
+		/* CMD1: get OCR register */
+		memset(&cmd, 0, sizeof(emmc_cmd_t));
+		cmd.cmd_idx = EMMC_CMD1;
+		cmd.cmd_arg = OCR_SECTOR_MODE | OCR_VDD_MIN_2V7 |
+			      OCR_VDD_MIN_1V7;
+		cmd.resp_type = EMMC_RESPONSE_R3;
+		ret = ops->send_cmd(&cmd);
+		assert(ret == 0);
+		emmc_ocr_value = cmd.resp_data[0];
+		if (emmc_ocr_value & OCR_POWERUP)
+			break;
+	}
+
+	/* CMD2: Card Identification */
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD2;
+	cmd.resp_type = EMMC_RESPONSE_R2;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	/* CMD3: Set Relative Address */
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD3;
+	cmd.cmd_arg = EMMC_FIX_RCA << RCA_SHIFT_OFFSET;
+	cmd.resp_type = EMMC_RESPONSE_R1;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	/* CMD9: CSD Register */
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD9;
+	cmd.cmd_arg = EMMC_FIX_RCA << RCA_SHIFT_OFFSET;
+	cmd.resp_type = EMMC_RESPONSE_R2;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+	memcpy(&emmc_csd, &cmd.resp_data, sizeof(cmd.resp_data));
+
+	/* CMD7: Select Card */
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD7;
+	cmd.cmd_arg = EMMC_FIX_RCA << RCA_SHIFT_OFFSET;
+	cmd.resp_type = EMMC_RESPONSE_R1;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+	/* wait to TRAN state */
+	do {
+		state = emmc_device_state();
+	} while (state != EMMC_STATE_TRAN);
+
+	emmc_set_ios(clk, bus_width);
+	return ret;
+}
+
+size_t emmc_read_blocks(int lba, uintptr_t buf, size_t size)
+{
+	emmc_cmd_t cmd;
+	int ret;
+
+	assert((ops != 0) &&
+	       (ops->read != 0) &&
+	       ((buf & EMMC_BLOCK_MASK) == 0) &&
+	       ((size & EMMC_BLOCK_MASK) == 0));
+
+	inv_dcache_range(buf, size);
+	ret = ops->prepare(lba, buf, size);
+	assert(ret == 0);
+
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	if (size > EMMC_BLOCK_SIZE)
+		cmd.cmd_idx = EMMC_CMD18;
+	else
+		cmd.cmd_idx = EMMC_CMD17;
+	if ((emmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE)
+		cmd.cmd_arg = lba * EMMC_BLOCK_SIZE;
+	else
+		cmd.cmd_arg = lba;
+	cmd.resp_type = EMMC_RESPONSE_R1;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	ret = ops->read(lba, buf, size);
+	assert(ret == 0);
+
+	/* wait buffer empty */
+	emmc_device_state();
+
+	if (size > EMMC_BLOCK_SIZE) {
+		memset(&cmd, 0, sizeof(emmc_cmd_t));
+		cmd.cmd_idx = EMMC_CMD12;
+		ret = ops->send_cmd(&cmd);
+		assert(ret == 0);
+	}
+	/* Ignore improbable errors in release builds */
+	(void)ret;
+	return size;
+}
+
+size_t emmc_write_blocks(int lba, const uintptr_t buf, size_t size)
+{
+	emmc_cmd_t cmd;
+	int ret;
+
+	assert((ops != 0) &&
+	       (ops->write != 0) &&
+	       ((buf & EMMC_BLOCK_MASK) == 0) &&
+	       ((size & EMMC_BLOCK_MASK) == 0));
+
+	clean_dcache_range(buf, size);
+	ret = ops->prepare(lba, buf, size);
+	assert(ret == 0);
+
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	if (size > EMMC_BLOCK_SIZE)
+		cmd.cmd_idx = EMMC_CMD25;
+	else
+		cmd.cmd_idx = EMMC_CMD24;
+	if ((emmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE)
+		cmd.cmd_arg = lba * EMMC_BLOCK_SIZE;
+	else
+		cmd.cmd_arg = lba;
+	cmd.resp_type = EMMC_RESPONSE_R1;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	ret = ops->write(lba, buf, size);
+	assert(ret == 0);
+
+	/* wait buffer empty */
+	emmc_device_state();
+
+	if (size > EMMC_BLOCK_SIZE) {
+		memset(&cmd, 0, sizeof(emmc_cmd_t));
+		cmd.cmd_idx = EMMC_CMD12;
+		ret = ops->send_cmd(&cmd);
+		assert(ret == 0);
+	}
+	/* Ignore improbable errors in release builds */
+	(void)ret;
+	return size;
+}
+
+size_t emmc_erase_blocks(int lba, size_t size)
+{
+	emmc_cmd_t cmd;
+	int ret, state;
+
+	assert(ops != 0);
+	assert((size != 0) && ((size % EMMC_BLOCK_SIZE) == 0));
+
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD35;
+	cmd.cmd_arg = lba;
+	cmd.resp_type = EMMC_RESPONSE_R1;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD36;
+	cmd.cmd_arg = lba + (size / EMMC_BLOCK_SIZE) - 1;
+	cmd.resp_type = EMMC_RESPONSE_R1;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	memset(&cmd, 0, sizeof(emmc_cmd_t));
+	cmd.cmd_idx = EMMC_CMD38;
+	cmd.resp_type = EMMC_RESPONSE_R1B;
+	ret = ops->send_cmd(&cmd);
+	assert(ret == 0);
+
+	/* wait to TRAN state */
+	do {
+		state = emmc_device_state();
+	} while (state != EMMC_STATE_TRAN);
+	/* Ignore improbable errors in release builds */
+	(void)ret;
+	return size;
+}
+
+static inline void emmc_rpmb_enable(void)
+{
+	emmc_set_ext_csd(CMD_EXTCSD_PARTITION_CONFIG,
+			PART_CFG_BOOT_PARTITION1_ENABLE |
+			PART_CFG_PARTITION1_ACCESS);
+}
+
+static inline void emmc_rpmb_disable(void)
+{
+	emmc_set_ext_csd(CMD_EXTCSD_PARTITION_CONFIG,
+			PART_CFG_BOOT_PARTITION1_ENABLE);
+}
+
+size_t emmc_rpmb_read_blocks(int lba, uintptr_t buf, size_t size)
+{
+	size_t size_read;
+
+	emmc_rpmb_enable();
+	size_read = emmc_read_blocks(lba, buf, size);
+	emmc_rpmb_disable();
+	return size_read;
+}
+
+size_t emmc_rpmb_write_blocks(int lba, const uintptr_t buf, size_t size)
+{
+	size_t size_written;
+
+	emmc_rpmb_enable();
+	size_written = emmc_write_blocks(lba, buf, size);
+	emmc_rpmb_disable();
+	return size_written;
+}
+
+size_t emmc_rpmb_erase_blocks(int lba, size_t size)
+{
+	size_t size_erased;
+
+	emmc_rpmb_enable();
+	size_erased = emmc_erase_blocks(lba, size);
+	emmc_rpmb_disable();
+	return size_erased;
+}
+
+void emmc_init(const emmc_ops_t *ops_ptr, int clk, int width)
+{
+	assert((ops_ptr != 0) &&
+	       (ops_ptr->init != 0) &&
+	       (ops_ptr->send_cmd != 0) &&
+	       (ops_ptr->set_ios != 0) &&
+	       (ops_ptr->prepare != 0) &&
+	       (ops_ptr->read != 0) &&
+	       (ops_ptr->write != 0) &&
+	       (clk != 0) &&
+	       ((width == EMMC_BUS_WIDTH_1) ||
+		(width == EMMC_BUS_WIDTH_4) ||
+		(width == EMMC_BUS_WIDTH_8)));
+	ops = ops_ptr;
+
+	emmc_enumerate(clk, width);
+}
diff --git a/drivers/gpio/gpio.c b/drivers/gpio/gpio.c
index c06172f..ef6bb9c 100644
--- a/drivers/gpio/gpio.c
+++ b/drivers/gpio/gpio.c
@@ -80,6 +80,26 @@
 	ops->set_value(gpio, value);
 }
 
+void gpio_set_pull(int gpio, int pull)
+{
+	assert(ops);
+	assert(ops->set_pull != 0);
+	assert((pull == GPIO_PULL_NONE) || (pull == GPIO_PULL_UP) ||
+	       (pull == GPIO_PULL_DOWN));
+	assert(gpio >= 0);
+
+	ops->set_pull(gpio, pull);
+}
+
+int gpio_get_pull(int gpio)
+{
+	assert(ops);
+	assert(ops->get_pull != 0);
+	assert(gpio >= 0);
+
+	return ops->get_pull(gpio);
+}
+
 /*
  * Initialize the gpio. The fields in the provided gpio
  * ops pointer must be valid.
diff --git a/drivers/io/io_block.c b/drivers/io/io_block.c
new file mode 100644
index 0000000..198b723
--- /dev/null
+++ b/drivers/io/io_block.c
@@ -0,0 +1,412 @@
+/*
+ * 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 <debug.h>
+#include <errno.h>
+#include <io_block.h>
+#include <io_driver.h>
+#include <io_storage.h>
+#include <platform_def.h>
+#include <string.h>
+
+typedef struct {
+	io_block_dev_spec_t	*dev_spec;
+	uintptr_t		base;
+	size_t			file_pos;
+	size_t			size;
+} block_dev_state_t;
+
+#define is_power_of_2(x)	((x != 0) && ((x & (x - 1)) == 0))
+
+io_type_t device_type_block(void);
+
+static int block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+		      io_entity_t *entity);
+static int block_seek(io_entity_t *entity, int mode, ssize_t offset);
+static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		      size_t *length_read);
+static int block_write(io_entity_t *entity, const uintptr_t buffer,
+		       size_t length, size_t *length_written);
+static int block_close(io_entity_t *entity);
+static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int block_dev_close(io_dev_info_t *dev_info);
+
+static const io_dev_connector_t block_dev_connector = {
+	.dev_open	= block_dev_open
+};
+
+static const io_dev_funcs_t block_dev_funcs = {
+	.type		= device_type_block,
+	.open		= block_open,
+	.seek		= block_seek,
+	.size		= NULL,
+	.read		= block_read,
+	.write		= block_write,
+	.close		= block_close,
+	.dev_init	= NULL,
+	.dev_close	= block_dev_close,
+};
+
+static block_dev_state_t state_pool[MAX_IO_BLOCK_DEVICES];
+static io_dev_info_t dev_info_pool[MAX_IO_BLOCK_DEVICES];
+
+/* Track number of allocated block state */
+static unsigned int block_dev_count;
+
+io_type_t device_type_block(void)
+{
+	return IO_TYPE_BLOCK;
+}
+
+/* Locate a block state in the pool, specified by address */
+static int find_first_block_state(const io_block_dev_spec_t *dev_spec,
+				  unsigned int *index_out)
+{
+	int result = -ENOENT;
+	for (int index = 0; index < MAX_IO_BLOCK_DEVICES; ++index) {
+		/* dev_spec is used as identifier since it's unique */
+		if (state_pool[index].dev_spec == dev_spec) {
+			result = 0;
+			*index_out = index;
+			break;
+		}
+	}
+	return result;
+}
+
+/* Allocate a device info from the pool and return a pointer to it */
+static int allocate_dev_info(io_dev_info_t **dev_info)
+{
+	int result = -ENOMEM;
+	assert(dev_info != NULL);
+
+	if (block_dev_count < MAX_IO_BLOCK_DEVICES) {
+		unsigned int index = 0;
+		result = find_first_block_state(NULL, &index);
+		assert(result == 0);
+		/* initialize dev_info */
+		dev_info_pool[index].funcs = &block_dev_funcs;
+		dev_info_pool[index].info = (uintptr_t)&state_pool[index];
+		*dev_info = &dev_info_pool[index];
+		++block_dev_count;
+	}
+
+	return result;
+}
+
+
+/* Release a device info to the pool */
+static int free_dev_info(io_dev_info_t *dev_info)
+{
+	int result;
+	unsigned int index = 0;
+	block_dev_state_t *state;
+	assert(dev_info != NULL);
+
+	state = (block_dev_state_t *)dev_info->info;
+	result = find_first_block_state(state->dev_spec, &index);
+	if (result ==  0) {
+		/* free if device info is valid */
+		memset(state, 0, sizeof(block_dev_state_t));
+		memset(dev_info, 0, sizeof(io_dev_info_t));
+		--block_dev_count;
+	}
+
+	return result;
+}
+
+static int block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+		      io_entity_t *entity)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *region;
+
+	assert((dev_info->info != (uintptr_t)NULL) &&
+	       (spec != (uintptr_t)NULL) &&
+	       (entity->info == (uintptr_t)NULL));
+
+	region = (io_block_spec_t *)spec;
+	cur = (block_dev_state_t *)dev_info->info;
+	assert(((region->offset % cur->dev_spec->block_size) == 0) &&
+	       ((region->length % cur->dev_spec->block_size) == 0));
+
+	cur->base = region->offset;
+	cur->size = region->length;
+	cur->file_pos = 0;
+
+	entity->info = (uintptr_t)cur;
+	return 0;
+}
+
+/* parameter offset is relative address at here */
+static int block_seek(io_entity_t *entity, int mode, ssize_t offset)
+{
+	block_dev_state_t *cur;
+
+	assert(entity->info != (uintptr_t)NULL);
+
+	cur = (block_dev_state_t *)entity->info;
+	assert((offset >= 0) && (offset < cur->size));
+
+	switch (mode) {
+	case IO_SEEK_SET:
+		cur->file_pos = offset;
+		break;
+	case IO_SEEK_CUR:
+		cur->file_pos += offset;
+		break;
+	default:
+		return -EINVAL;
+	}
+	assert(cur->file_pos < cur->size);
+	return 0;
+}
+
+static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+		      size_t *length_read)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *buf;
+	io_block_ops_t *ops;
+	size_t aligned_length, skip, count, left, padding, block_size;
+	int lba;
+
+	assert(entity->info != (uintptr_t)NULL);
+	cur = (block_dev_state_t *)entity->info;
+	ops = &(cur->dev_spec->ops);
+	buf = &(cur->dev_spec->buffer);
+	block_size = cur->dev_spec->block_size;
+	assert((length <= cur->size) &&
+	       (length > 0) &&
+	       (ops->read != 0));
+
+	skip = cur->file_pos % block_size;
+	aligned_length = ((skip + length) + (block_size - 1)) &
+			 ~(block_size - 1);
+	padding = aligned_length - (skip + length);
+	left = aligned_length;
+	do {
+		lba = (cur->file_pos + cur->base) / block_size;
+		if (left >= buf->length) {
+			/* Since left is larger, it's impossible to padding. */
+			if (skip) {
+				/*
+				 * The beginning address (file_pos) isn't
+				 * aligned with block size, we need to use
+				 * block buffer to read block. Since block
+				 * device is always relied on DMA operation.
+				 */
+				count = ops->read(lba, buf->offset,
+						  buf->length);
+			} else {
+				count = ops->read(lba, buffer, buf->length);
+			}
+			assert(count == buf->length);
+			cur->file_pos += count - skip;
+			if (skip) {
+				/*
+				 * Since it's not aligned with block size,
+				 * block buffer is used to store data.
+				 */
+				memcpy((void *)buffer,
+				       (void *)(buf->offset + skip),
+				       count - skip);
+			}
+			left = left - (count - skip);
+		} else {
+			if (skip || padding) {
+				/*
+				 * The beginning address (file_pos) isn't
+				 * aligned with block size, we have to read
+				 * full block by block buffer instead.
+				 * The size isn't aligned with block size.
+				 * Use block buffer to avoid overflow.
+				 */
+				count = ops->read(lba, buf->offset, left);
+			} else
+				count = ops->read(lba, buffer, left);
+			assert(count == left);
+			left = left - (skip + padding);
+			cur->file_pos += left;
+			if (skip || padding) {
+				/*
+				 * Since it's not aligned with block size,
+				 * block buffer is used to store data.
+				 */
+				memcpy((void *)buffer,
+				       (void *)(buf->offset + skip),
+				       left);
+			}
+			/* It's already the last block operation */
+			left = 0;
+		}
+		skip = cur->file_pos % block_size;
+	} while (left > 0);
+	*length_read = length;
+
+	return 0;
+}
+
+static int block_write(io_entity_t *entity, const uintptr_t buffer,
+		       size_t length, size_t *length_written)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *buf;
+	io_block_ops_t *ops;
+	size_t aligned_length, skip, count, left, padding, block_size;
+	int lba;
+
+	assert(entity->info != (uintptr_t)NULL);
+	cur = (block_dev_state_t *)entity->info;
+	ops = &(cur->dev_spec->ops);
+	buf = &(cur->dev_spec->buffer);
+	block_size = cur->dev_spec->block_size;
+	assert((length <= cur->size) &&
+	       (length > 0) &&
+	       (ops->read != 0) &&
+	       (ops->write != 0));
+
+	skip = cur->file_pos % block_size;
+	aligned_length = ((skip + length) + (block_size - 1)) &
+			 ~(block_size - 1);
+	padding = aligned_length - (skip + length);
+	left = aligned_length;
+	do {
+		lba = (cur->file_pos + cur->base) / block_size;
+		if (left >= buf->length) {
+			/* Since left is larger, it's impossible to padding. */
+			if (skip) {
+				/*
+				 * The beginning address (file_pos) isn't
+				 * aligned with block size, we need to use
+				 * block buffer to write block. Since block
+				 * device is always relied on DMA operation.
+				 */
+				count = ops->read(lba, buf->offset,
+						  buf->length);
+				assert(count == buf->length);
+				memcpy((void *)(buf->offset + skip),
+				       (void *)buffer,
+				       count - skip);
+				count = ops->write(lba, buf->offset,
+						   buf->length);
+			} else
+				count = ops->write(lba, buffer, buf->length);
+			assert(count == buf->length);
+			cur->file_pos += count - skip;
+			left = left - (count - skip);
+		} else {
+			if (skip || padding) {
+				/*
+				 * The beginning address (file_pos) isn't
+				 * aligned with block size, we need to avoid
+				 * poluate data in the beginning. Reading and
+				 * skipping the beginning is the only way.
+				 * The size isn't aligned with block size.
+				 * Use block buffer to avoid overflow.
+				 */
+				count = ops->read(lba, buf->offset, left);
+				assert(count == left);
+				memcpy((void *)(buf->offset + skip),
+				       (void *)buffer,
+				       left - skip - padding);
+				count = ops->write(lba, buf->offset, left);
+			} else
+				count = ops->write(lba, buffer, left);
+			assert(count == left);
+			cur->file_pos += left - (skip + padding);
+			/* It's already the last block operation */
+			left = 0;
+		}
+		skip = cur->file_pos % block_size;
+	} while (left > 0);
+	*length_written = length;
+	return 0;
+}
+
+static int block_close(io_entity_t *entity)
+{
+	entity->info = (uintptr_t)NULL;
+	return 0;
+}
+
+static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info)
+{
+	block_dev_state_t *cur;
+	io_block_spec_t *buffer;
+	io_dev_info_t *info;
+	size_t block_size;
+	int result;
+
+	assert(dev_info != NULL);
+	result = allocate_dev_info(&info);
+	if (result)
+		return -ENOENT;
+
+	cur = (block_dev_state_t *)info->info;
+	/* dev_spec is type of io_block_dev_spec_t. */
+	cur->dev_spec = (io_block_dev_spec_t *)dev_spec;
+	buffer = &(cur->dev_spec->buffer);
+	block_size = cur->dev_spec->block_size;
+	assert((block_size > 0) &&
+	       (is_power_of_2(block_size) != 0) &&
+	       ((buffer->offset % block_size) == 0) &&
+	       ((buffer->length % block_size) == 0));
+
+	*dev_info = info;	/* cast away const */
+	(void)block_size;
+	(void)buffer;
+	return 0;
+}
+
+static int block_dev_close(io_dev_info_t *dev_info)
+{
+	return free_dev_info(dev_info);
+}
+
+/* Exported functions */
+
+/* Register the Block driver with the IO abstraction */
+int register_io_dev_block(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	assert(dev_con != NULL);
+
+	/*
+	 * Since dev_info isn't really used in io_register_device, always
+	 * use the same device info at here instead.
+	 */
+	result = io_register_device(&dev_info_pool[0]);
+	if (result == 0)
+		*dev_con = &block_dev_connector;
+	return result;
+}
diff --git a/drivers/io/io_dummy.c b/drivers/io/io_dummy.c
new file mode 100644
index 0000000..e2458de
--- /dev/null
+++ b/drivers/io/io_dummy.c
@@ -0,0 +1,177 @@
+/*
+ * 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 <debug.h>
+#include <io_driver.h>
+#include <io_storage.h>
+#include <string.h>
+
+struct file_state {
+	int in_use;
+	size_t size;
+};
+
+static struct file_state current_file = {0};
+
+/* Identify the device type as dummy */
+io_type_t device_type_dummy(void)
+{
+	return IO_TYPE_DUMMY;
+}
+
+/* Dummy device functions */
+static int dummy_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int dummy_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity);
+static int dummy_block_len(io_entity_t *entity, size_t *length);
+static int dummy_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read);
+static int dummy_block_close(io_entity_t *entity);
+static int dummy_dev_close(io_dev_info_t *dev_info);
+
+
+static const io_dev_connector_t dummy_dev_connector = {
+	.dev_open = dummy_dev_open
+};
+
+
+static const io_dev_funcs_t dummy_dev_funcs = {
+	.type = device_type_dummy,
+	.open = dummy_block_open,
+	.seek = NULL,
+	.size = dummy_block_len,
+	.read = dummy_block_read,
+	.write = NULL,
+	.close = dummy_block_close,
+	.dev_init = NULL,
+	.dev_close = dummy_dev_close,
+};
+
+
+static const io_dev_info_t dummy_dev_info = {
+	.funcs = &dummy_dev_funcs,
+	.info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the dummy device */
+static int dummy_dev_open(const uintptr_t dev_spec __attribute__((unused)),
+			   io_dev_info_t **dev_info)
+{
+	assert(dev_info != NULL);
+	*dev_info = (io_dev_info_t *)&dummy_dev_info;
+
+	return 0;
+}
+
+
+/* Close a connection to the dummy device */
+static int dummy_dev_close(io_dev_info_t *dev_info)
+{
+	return 0;
+}
+
+
+/* Open a file on the dummy device */
+static int dummy_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+			     io_entity_t *entity)
+{
+	int result;
+	const io_block_spec_t *block_spec = (io_block_spec_t *)spec;
+
+	if (current_file.in_use == 0) {
+		assert(block_spec != NULL);
+		assert(entity != NULL);
+
+		current_file.in_use = 1;
+		current_file.size = block_spec->length;
+		entity->info = (uintptr_t)&current_file;
+		result = 0;
+	} else {
+		WARN("A Dummy device is already active. Close first.\n");
+		result = -ENOMEM;
+	}
+
+	return result;
+}
+
+
+/* Return the size of a file on the dummy device */
+static int dummy_block_len(io_entity_t *entity, size_t *length)
+{
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	*length =  ((struct file_state *)entity->info)->size;
+
+	return 0;
+}
+
+
+/* Read data from a file on the dummy device */
+static int dummy_block_read(io_entity_t *entity, uintptr_t buffer,
+			     size_t length, size_t *length_read)
+{
+	assert(length_read != NULL);
+
+	*length_read = length;
+
+	return 0;
+}
+
+
+/* Close a file on the dummy device */
+static int dummy_block_close(io_entity_t *entity)
+{
+	assert(entity != NULL);
+
+	entity->info = 0;
+	current_file.in_use = 0;
+
+	return 0;
+}
+
+
+/* Exported functions */
+
+/* Register the dummy driver with the IO abstraction */
+int register_io_dev_dummy(const io_dev_connector_t **dev_con)
+{
+	int result;
+
+	assert(dev_con != NULL);
+
+	result = io_register_device(&dummy_dev_info);
+	if (result == 0)
+		*dev_con = &dummy_dev_connector;
+
+	return result;
+}
diff --git a/fdts/fvp-base-gicv2legacy-psci.dtb b/fdts/fvp-base-gicv2legacy-psci.dtb
deleted file mode 100644
index 4c6f37a..0000000
--- a/fdts/fvp-base-gicv2legacy-psci.dtb
+++ /dev/null
Binary files differ
diff --git a/fdts/fvp-base-gicv2legacy-psci.dts b/fdts/fvp-base-gicv2legacy-psci.dts
deleted file mode 100644
index 5a7ce2f..0000000
--- a/fdts/fvp-base-gicv2legacy-psci.dts
+++ /dev/null
@@ -1,331 +0,0 @@
-/*
- * 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:
- *
- * 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.
- */
-
-/dts-v1/;
-
-/memreserve/ 0x80000000 0x00010000;
-
-/ {
-};
-
-/ {
-	model = "FVP Base";
-	compatible = "arm,vfp-base", "arm,vexpress";
-	interrupt-parent = <&gic>;
-	#address-cells = <2>;
-	#size-cells = <2>;
-
-	chosen { };
-
-	aliases {
-		serial0 = &v2m_serial0;
-		serial1 = &v2m_serial1;
-		serial2 = &v2m_serial2;
-		serial3 = &v2m_serial3;
-	};
-
-	psci {
-		compatible = "arm,psci-1.0", "arm,psci-0.2", "arm,psci";
-		method = "smc";
-		cpu_suspend = <0xc4000001>;
-		cpu_off = <0x84000002>;
-		cpu_on = <0xc4000003>;
-		sys_poweroff = <0x84000008>;
-		sys_reset = <0x84000009>;
-	};
-
-	cpus {
-		#address-cells = <2>;
-		#size-cells = <0>;
-
-		cpu-map {
-			cluster0 {
-				core0 {
-					cpu = <&CPU0>;
-				};
-				core1 {
-					cpu = <&CPU1>;
-				};
-				core2 {
-					cpu = <&CPU2>;
-				};
-				core3 {
-					cpu = <&CPU3>;
-				};
-			};
-
-			cluster1 {
-				core0 {
-					cpu = <&CPU4>;
-				};
-				core1 {
-					cpu = <&CPU5>;
-				};
-				core2 {
-					cpu = <&CPU6>;
-				};
-				core3 {
-					cpu = <&CPU7>;
-				};
-			};
-		};
-
-		idle-states {
-			entry-method = "arm,psci";
-
-			CPU_SLEEP_0: cpu-sleep-0 {
-				compatible = "arm,idle-state";
-				local-timer-stop;
-				arm,psci-suspend-param = <0x0010000>;
-				entry-latency-us = <40>;
-				exit-latency-us = <100>;
-				min-residency-us = <150>;
-			};
-
-			CLUSTER_SLEEP_0: cluster-sleep-0 {
-				compatible = "arm,idle-state";
-				local-timer-stop;
-				arm,psci-suspend-param = <0x1010000>;
-				entry-latency-us = <500>;
-				exit-latency-us = <1000>;
-				min-residency-us = <2500>;
-			};
-		};
-
-		CPU0:cpu@0 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x0>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU1:cpu@1 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x1>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU2:cpu@2 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x2>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU3:cpu@3 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x3>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU4:cpu@100 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x100>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU5:cpu@101 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x101>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU6:cpu@102 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x102>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU7:cpu@103 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x103>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		L2_0: l2-cache0 {
-			compatible = "cache";
-		};
-	};
-
-	memory@80000000 {
-		device_type = "memory";
-		reg = <0x00000000 0x80000000 0 0x7F000000>,
-		      <0x00000008 0x80000000 0 0x80000000>;
-	};
-
-	gic: interrupt-controller@2c001000 {
-		compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
-		#interrupt-cells = <3>;
-		#address-cells = <0>;
-		interrupt-controller;
-		reg = <0x0 0x2c001000 0 0x1000>,
-		      <0x0 0x2c002000 0 0x1000>,
-		      <0x0 0x2c004000 0 0x2000>,
-		      <0x0 0x2c006000 0 0x2000>;
-		interrupts = <1 9 0xf04>;
-	};
-
-	timer {
-		compatible = "arm,armv8-timer";
-		interrupts = <1 13 0xff01>,
-			     <1 14 0xff01>,
-			     <1 11 0xff01>,
-			     <1 10 0xff01>;
-		clock-frequency = <100000000>;
-	};
-
-	timer@2a810000 {
-			compatible = "arm,armv7-timer-mem";
-			reg = <0x0 0x2a810000 0x0 0x10000>;
-			clock-frequency = <100000000>;
-			#address-cells = <2>;
-			#size-cells = <2>;
-			ranges;
-			frame@2a830000 {
-				frame-number = <1>;
-				interrupts = <0 26 4>;
-				reg = <0x0 0x2a830000 0x0 0x10000>;
-			};
-	};
-
-	pmu {
-		compatible = "arm,armv8-pmuv3";
-		interrupts = <0 60 4>,
-			     <0 61 4>,
-			     <0 62 4>,
-			     <0 63 4>;
-	};
-
-	smb {
-		compatible = "simple-bus";
-
-		#address-cells = <2>;
-		#size-cells = <1>;
-		ranges = <0 0 0 0x08000000 0x04000000>,
-			 <1 0 0 0x14000000 0x04000000>,
-			 <2 0 0 0x18000000 0x04000000>,
-			 <3 0 0 0x1c000000 0x04000000>,
-			 <4 0 0 0x0c000000 0x04000000>,
-			 <5 0 0 0x10000000 0x04000000>;
-
-		#interrupt-cells = <1>;
-		interrupt-map-mask = <0 0 63>;
-		interrupt-map = <0 0  0 &gic 0  0 4>,
-				<0 0  1 &gic 0  1 4>,
-				<0 0  2 &gic 0  2 4>,
-				<0 0  3 &gic 0  3 4>,
-				<0 0  4 &gic 0  4 4>,
-				<0 0  5 &gic 0  5 4>,
-				<0 0  6 &gic 0  6 4>,
-				<0 0  7 &gic 0  7 4>,
-				<0 0  8 &gic 0  8 4>,
-				<0 0  9 &gic 0  9 4>,
-				<0 0 10 &gic 0 10 4>,
-				<0 0 11 &gic 0 11 4>,
-				<0 0 12 &gic 0 12 4>,
-				<0 0 13 &gic 0 13 4>,
-				<0 0 14 &gic 0 14 4>,
-				<0 0 15 &gic 0 15 4>,
-				<0 0 16 &gic 0 16 4>,
-				<0 0 17 &gic 0 17 4>,
-				<0 0 18 &gic 0 18 4>,
-				<0 0 19 &gic 0 19 4>,
-				<0 0 20 &gic 0 20 4>,
-				<0 0 21 &gic 0 21 4>,
-				<0 0 22 &gic 0 22 4>,
-				<0 0 23 &gic 0 23 4>,
-				<0 0 24 &gic 0 24 4>,
-				<0 0 25 &gic 0 25 4>,
-				<0 0 26 &gic 0 26 4>,
-				<0 0 27 &gic 0 27 4>,
-				<0 0 28 &gic 0 28 4>,
-				<0 0 29 &gic 0 29 4>,
-				<0 0 30 &gic 0 30 4>,
-				<0 0 31 &gic 0 31 4>,
-				<0 0 32 &gic 0 32 4>,
-				<0 0 33 &gic 0 33 4>,
-				<0 0 34 &gic 0 34 4>,
-				<0 0 35 &gic 0 35 4>,
-				<0 0 36 &gic 0 36 4>,
-				<0 0 37 &gic 0 37 4>,
-				<0 0 38 &gic 0 38 4>,
-				<0 0 39 &gic 0 39 4>,
-				<0 0 40 &gic 0 40 4>,
-				<0 0 41 &gic 0 41 4>,
-				<0 0 42 &gic 0 42 4>;
-
-		/include/ "rtsm_ve-motherboard.dtsi"
-	};
-
-	panels {
-		panel@0 {
-			compatible	= "panel";
-			mode		= "XVGA";
-			refresh		= <60>;
-			xres		= <1024>;
-			yres		= <768>;
-			pixclock	= <15748>;
-			left_margin	= <152>;
-			right_margin	= <48>;
-			upper_margin	= <23>;
-			lower_margin	= <3>;
-			hsync_len	= <104>;
-			vsync_len	= <4>;
-			sync		= <0>;
-			vmode		= "FB_VMODE_NONINTERLACED";
-			tim2		= "TIM2_BCD", "TIM2_IPC";
-			cntl		= "CNTL_LCDTFT", "CNTL_BGR", "CNTL_LCDVCOMP(1)";
-			caps		= "CLCD_CAP_5551", "CLCD_CAP_565", "CLCD_CAP_888";
-			bpp		= <16>;
-		};
-	};
-};
diff --git a/fdts/fvp-foundation-gicv2legacy-psci.dtb b/fdts/fvp-foundation-gicv2legacy-psci.dtb
deleted file mode 100644
index efded44..0000000
--- a/fdts/fvp-foundation-gicv2legacy-psci.dtb
+++ /dev/null
Binary files differ
diff --git a/fdts/fvp-foundation-gicv2legacy-psci.dts b/fdts/fvp-foundation-gicv2legacy-psci.dts
deleted file mode 100644
index e6e4012..0000000
--- a/fdts/fvp-foundation-gicv2legacy-psci.dts
+++ /dev/null
@@ -1,257 +0,0 @@
-/*
- * 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:
- *
- * 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 the 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.
- */
-
-/dts-v1/;
-
-/memreserve/ 0x80000000 0x00010000;
-
-/ {
-};
-
-/ {
-	model = "FVP Foundation";
-	compatible = "arm,fvp-base", "arm,vexpress";
-	interrupt-parent = <&gic>;
-	#address-cells = <2>;
-	#size-cells = <2>;
-
-	chosen { };
-
-	aliases {
-		serial0 = &v2m_serial0;
-		serial1 = &v2m_serial1;
-		serial2 = &v2m_serial2;
-		serial3 = &v2m_serial3;
-	};
-
-	psci {
-		compatible = "arm,psci-1.0", "arm,psci-0.2", "arm,psci";
-		method = "smc";
-		cpu_suspend = <0xc4000001>;
-		cpu_off = <0x84000002>;
-		cpu_on = <0xc4000003>;
-		sys_poweroff = <0x84000008>;
-		sys_reset = <0x84000009>;
-	};
-
-	cpus {
-		#address-cells = <2>;
-		#size-cells = <0>;
-
-		cpu-map {
-			cluster0 {
-				core0 {
-					cpu = <&CPU0>;
-				};
-				core1 {
-					cpu = <&CPU1>;
-				};
-				core2 {
-					cpu = <&CPU2>;
-				};
-				core3 {
-					cpu = <&CPU3>;
-				};
-			};
-		};
-
-		idle-states {
-			entry-method = "arm,psci";
-
-			CPU_SLEEP_0: cpu-sleep-0 {
-				compatible = "arm,idle-state";
-				local-timer-stop;
-				arm,psci-suspend-param = <0x0010000>;
-				entry-latency-us = <40>;
-				exit-latency-us = <100>;
-				min-residency-us = <150>;
-			};
-
-			CLUSTER_SLEEP_0: cluster-sleep-0 {
-				compatible = "arm,idle-state";
-				local-timer-stop;
-				arm,psci-suspend-param = <0x1010000>;
-				entry-latency-us = <500>;
-				exit-latency-us = <1000>;
-				min-residency-us = <2500>;
-			};
-		};
-
-		CPU0:cpu@0 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x0>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU1:cpu@1 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x1>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU2:cpu@2 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x2>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		CPU3:cpu@3 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x3>;
-			enable-method = "psci";
-			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
-			next-level-cache = <&L2_0>;
-		};
-
-		L2_0: l2-cache0 {
-			compatible = "cache";
-		};
-	};
-
-	memory@80000000 {
-		device_type = "memory";
-		reg = <0x00000000 0x80000000 0 0x7F000000>,
-		      <0x00000008 0x80000000 0 0x80000000>;
-	};
-
-	gic: interrupt-controller@2c001000 {
-		compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
-		#interrupt-cells = <3>;
-		#address-cells = <0>;
-		interrupt-controller;
-		reg = <0x0 0x2c001000 0 0x1000>,
-		      <0x0 0x2c002000 0 0x1000>,
-		      <0x0 0x2c004000 0 0x2000>,
-		      <0x0 0x2c006000 0 0x2000>;
-		interrupts = <1 9 0xf04>;
-	};
-
-	timer {
-		compatible = "arm,armv8-timer";
-		interrupts = <1 13 0xff01>,
-			     <1 14 0xff01>,
-			     <1 11 0xff01>,
-			     <1 10 0xff01>;
-		clock-frequency = <100000000>;
-	};
-
-	timer@2a810000 {
-			compatible = "arm,armv7-timer-mem";
-			reg = <0x0 0x2a810000 0x0 0x10000>;
-			clock-frequency = <100000000>;
-			#address-cells = <2>;
-			#size-cells = <2>;
-			ranges;
-			frame@2a830000 {
-				frame-number = <1>;
-				interrupts = <0 26 4>;
-				reg = <0x0 0x2a830000 0x0 0x10000>;
-			};
-	};
-
-	pmu {
-		compatible = "arm,armv8-pmuv3";
-		interrupts = <0 60 4>,
-			     <0 61 4>,
-			     <0 62 4>,
-			     <0 63 4>;
-	};
-
-	smb {
-		compatible = "simple-bus";
-
-		#address-cells = <2>;
-		#size-cells = <1>;
-		ranges = <0 0 0 0x08000000 0x04000000>,
-			 <1 0 0 0x14000000 0x04000000>,
-			 <2 0 0 0x18000000 0x04000000>,
-			 <3 0 0 0x1c000000 0x04000000>,
-			 <4 0 0 0x0c000000 0x04000000>,
-			 <5 0 0 0x10000000 0x04000000>;
-
-		#interrupt-cells = <1>;
-		interrupt-map-mask = <0 0 63>;
-		interrupt-map = <0 0  0 &gic 0  0 4>,
-				<0 0  1 &gic 0  1 4>,
-				<0 0  2 &gic 0  2 4>,
-				<0 0  3 &gic 0  3 4>,
-				<0 0  4 &gic 0  4 4>,
-				<0 0  5 &gic 0  5 4>,
-				<0 0  6 &gic 0  6 4>,
-				<0 0  7 &gic 0  7 4>,
-				<0 0  8 &gic 0  8 4>,
-				<0 0  9 &gic 0  9 4>,
-				<0 0 10 &gic 0 10 4>,
-				<0 0 11 &gic 0 11 4>,
-				<0 0 12 &gic 0 12 4>,
-				<0 0 13 &gic 0 13 4>,
-				<0 0 14 &gic 0 14 4>,
-				<0 0 15 &gic 0 15 4>,
-				<0 0 16 &gic 0 16 4>,
-				<0 0 17 &gic 0 17 4>,
-				<0 0 18 &gic 0 18 4>,
-				<0 0 19 &gic 0 19 4>,
-				<0 0 20 &gic 0 20 4>,
-				<0 0 21 &gic 0 21 4>,
-				<0 0 22 &gic 0 22 4>,
-				<0 0 23 &gic 0 23 4>,
-				<0 0 24 &gic 0 24 4>,
-				<0 0 25 &gic 0 25 4>,
-				<0 0 26 &gic 0 26 4>,
-				<0 0 27 &gic 0 27 4>,
-				<0 0 28 &gic 0 28 4>,
-				<0 0 29 &gic 0 29 4>,
-				<0 0 30 &gic 0 30 4>,
-				<0 0 31 &gic 0 31 4>,
-				<0 0 32 &gic 0 32 4>,
-				<0 0 33 &gic 0 33 4>,
-				<0 0 34 &gic 0 34 4>,
-				<0 0 35 &gic 0 35 4>,
-				<0 0 36 &gic 0 36 4>,
-				<0 0 37 &gic 0 37 4>,
-				<0 0 38 &gic 0 38 4>,
-				<0 0 39 &gic 0 39 4>,
-				<0 0 40 &gic 0 40 4>,
-				<0 0 41 &gic 0 41 4>,
-				<0 0 42 &gic 0 42 4>;
-
-		/include/ "fvp-foundation-motherboard.dtsi"
-	};
-};
diff --git a/include/bl31/services/psci.h b/include/bl31/services/psci.h
index acf0786..20aa52e 100644
--- a/include/bl31/services/psci.h
+++ b/include/bl31/services/psci.h
@@ -80,6 +80,10 @@
 #define PSCI_FEATURES			0x8400000A
 #define PSCI_SYSTEM_SUSPEND_AARCH32	0x8400000E
 #define PSCI_SYSTEM_SUSPEND_AARCH64	0xc400000E
+#define PSCI_STAT_RESIDENCY_AARCH32	0x84000010
+#define PSCI_STAT_RESIDENCY_AARCH64	0xc4000010
+#define PSCI_STAT_COUNT_AARCH32		0x84000011
+#define PSCI_STAT_COUNT_AARCH64		0xc4000011
 
 /* Macro to help build the psci capabilities bitfield */
 #define define_psci_cap(x)		(1 << (x & 0x1f))
@@ -87,7 +91,11 @@
 /*
  * Number of PSCI calls (above) implemented
  */
+#if ENABLE_PSCI_STAT
+#define PSCI_NUM_CALLS			22
+#else
 #define PSCI_NUM_CALLS			18
+#endif
 
 /*******************************************************************************
  * PSCI Migrate and friends
@@ -265,6 +273,8 @@
 	void (*pwr_domain_on_finish)(const psci_power_state_t *target_state);
 	void (*pwr_domain_suspend_finish)(
 				const psci_power_state_t *target_state);
+	void (*pwr_domain_pwr_down_wfi)(
+				const psci_power_state_t *target_state) __dead2;
 	void (*system_off)(void) __dead2;
 	void (*system_reset)(void) __dead2;
 	int (*validate_power_state)(unsigned int power_state,
@@ -272,6 +282,11 @@
 	int (*validate_ns_entrypoint)(uintptr_t ns_entrypoint);
 	void (*get_sys_suspend_power_state)(
 				    psci_power_state_t *req_state);
+	int (*get_pwr_lvl_state_idx)(plat_local_state_t pwr_domain_state,
+				    int pwrlvl);
+	int (*translate_power_state_by_mpidr)(u_register_t mpidr,
+				    unsigned int power_state,
+				    psci_power_state_t *output_state);
 } plat_psci_ops_t;
 
 /*******************************************************************************
diff --git a/include/common/asm_macros.S b/include/common/asm_macros.S
index a331c05..d4bd11e 100644
--- a/include/common/asm_macros.S
+++ b/include/common/asm_macros.S
@@ -66,11 +66,36 @@
 	b.ne	$label
 	.endm
 
+	/*
+	 * Declare the exception vector table, enforcing it is aligned on a
+	 * 2KB boundary, as required by the ARMv8 architecture.
+	 * Use zero bytes as the fill value to be stored in the padding bytes
+	 * so that it inserts illegal AArch64 instructions. This increases
+	 * security, robustness and potentially facilitates debugging.
+	 */
+	.macro vector_base  label
+	.section .vectors, "ax"
+	.align 11, 0
+	\label:
+	.endm
+
+	/*
+	 * Create an entry in the exception vector table, enforcing it is
+	 * aligned on a 128-byte boundary, as required by the ARMv8 architecture.
+	 * Use zero bytes as the fill value to be stored in the padding bytes
+	 * so that it inserts illegal AArch64 instructions. This increases
+	 * security, robustness and potentially facilitates debugging.
+	 */
+	.macro vector_entry  label
+	.section .vectors, "ax"
+	.align 7, 0
+	\label:
+	.endm
 
 	/*
-	 * This macro verifies that the a given vector doesn't exceed the
+	 * This macro verifies that the given vector doesn't exceed the
 	 * architectural limit of 32 instructions. This is meant to be placed
-	 * immedately after the last instruction in the vector. It takes the
+	 * immediately after the last instruction in the vector. It takes the
 	 * vector entry as the parameter
 	 */
 	.macro check_vector_size since
diff --git a/include/common/context.h b/include/common/context.h
index 0dfebe0..ec47f2a 100644
--- a/include/common/context.h
+++ b/include/common/context.h
@@ -91,48 +91,58 @@
 #define CTX_SYSREGS_OFFSET	(CTX_EL3STATE_OFFSET + CTX_EL3STATE_END)
 #define CTX_SPSR_EL1		0x0
 #define CTX_ELR_EL1		0x8
-#define CTX_SPSR_ABT		0x10
-#define CTX_SPSR_UND		0x18
-#define CTX_SPSR_IRQ		0x20
-#define CTX_SPSR_FIQ		0x28
-#define CTX_SCTLR_EL1		0x30
-#define CTX_ACTLR_EL1		0x38
-#define CTX_CPACR_EL1		0x40
-#define CTX_CSSELR_EL1		0x48
-#define CTX_SP_EL1		0x50
-#define CTX_ESR_EL1		0x58
-#define CTX_TTBR0_EL1		0x60
-#define CTX_TTBR1_EL1		0x68
-#define CTX_MAIR_EL1		0x70
-#define CTX_AMAIR_EL1		0x78
-#define CTX_TCR_EL1		0x80
-#define CTX_TPIDR_EL1		0x88
-#define CTX_TPIDR_EL0		0x90
-#define CTX_TPIDRRO_EL0		0x98
-#define CTX_DACR32_EL2		0xa0
-#define CTX_IFSR32_EL2		0xa8
-#define CTX_PAR_EL1		0xb0
-#define CTX_FAR_EL1		0xb8
-#define CTX_AFSR0_EL1		0xc0
-#define CTX_AFSR1_EL1		0xc8
-#define CTX_CONTEXTIDR_EL1	0xd0
-#define CTX_VBAR_EL1		0xd8
+#define CTX_SCTLR_EL1		0x10
+#define CTX_ACTLR_EL1		0x18
+#define CTX_CPACR_EL1		0x20
+#define CTX_CSSELR_EL1		0x28
+#define CTX_SP_EL1		0x30
+#define CTX_ESR_EL1		0x38
+#define CTX_TTBR0_EL1		0x40
+#define CTX_TTBR1_EL1		0x48
+#define CTX_MAIR_EL1		0x50
+#define CTX_AMAIR_EL1		0x58
+#define CTX_TCR_EL1		0x60
+#define CTX_TPIDR_EL1		0x68
+#define CTX_TPIDR_EL0		0x70
+#define CTX_TPIDRRO_EL0		0x78
+#define CTX_PAR_EL1		0x80
+#define CTX_FAR_EL1		0x88
+#define CTX_AFSR0_EL1		0x90
+#define CTX_AFSR1_EL1		0x98
+#define CTX_CONTEXTIDR_EL1	0xa0
+#define CTX_VBAR_EL1		0xa8
+
+/*
+ * If the platform is AArch64-only, there is no need to save and restore these
+ * AArch32 registers.
+ */
+#if CTX_INCLUDE_AARCH32_REGS
+#define CTX_SPSR_ABT		0xb0
+#define CTX_SPSR_UND		0xb8
+#define CTX_SPSR_IRQ		0xc0
+#define CTX_SPSR_FIQ		0xc8
+#define CTX_DACR32_EL2		0xd0
+#define CTX_IFSR32_EL2		0xd8
+#define CTX_FP_FPEXC32_EL2	0xe0
+#define CTX_TIMER_SYSREGS_OFF		0xf0 /* Align to the next 16 byte boundary */
+#else
+#define CTX_TIMER_SYSREGS_OFF		0xb0
+#endif /* __CTX_INCLUDE_AARCH32_REGS__ */
+
 /*
  * If the timer registers aren't saved and restored, we don't have to reserve
  * space for them in the context
  */
 #if NS_TIMER_SWITCH
-#define CTX_CNTP_CTL_EL0	0xe0
-#define CTX_CNTP_CVAL_EL0	0xe8
-#define CTX_CNTV_CTL_EL0	0xf0
-#define CTX_CNTV_CVAL_EL0	0xf8
-#define CTX_CNTKCTL_EL1		0x100
-#define CTX_FP_FPEXC32_EL2	0x108
-#define CTX_SYSREGS_END		0x110
+#define CTX_CNTP_CTL_EL0	(CTX_TIMER_SYSREGS_OFF + 0x0)
+#define CTX_CNTP_CVAL_EL0	(CTX_TIMER_SYSREGS_OFF + 0x8)
+#define CTX_CNTV_CTL_EL0	(CTX_TIMER_SYSREGS_OFF + 0x10)
+#define CTX_CNTV_CVAL_EL0	(CTX_TIMER_SYSREGS_OFF + 0x18)
+#define CTX_CNTKCTL_EL1		(CTX_TIMER_SYSREGS_OFF + 0x20)
+#define CTX_SYSREGS_END		(CTX_TIMER_SYSREGS_OFF + 0x30) /* Align to the next 16 byte boundary */
 #else
-#define CTX_FP_FPEXC32_EL2	0xe0
-#define CTX_SYSREGS_END		0xf0
-#endif
+#define CTX_SYSREGS_END		CTX_TIMER_SYSREGS_OFF
+#endif /* __NS_TIMER_SWITCH__ */
 
 /*******************************************************************************
  * Constants that allow assembler code to access members of and the 'fp_regs'
diff --git a/include/common/context_mgmt.h b/include/common/context_mgmt.h
index 11786a1..a76ecbe 100644
--- a/include/common/context_mgmt.h
+++ b/include/common/context_mgmt.h
@@ -55,7 +55,6 @@
 			     unsigned int security_state);
 void *cm_get_context(uint32_t security_state);
 void cm_set_context(void *context, uint32_t security_state);
-inline void cm_set_next_context(void *context);
 void cm_init_context(uint64_t mpidr,
 		     const struct entry_point_info *ep) __deprecated;
 void cm_init_my_context(const struct entry_point_info *ep);
@@ -80,7 +79,7 @@
  * return. This initializes the SP_EL3 to a pointer to a 'cpu_context' set for
  * the required security state
  ******************************************************************************/
-inline void cm_set_next_context(void *context)
+static inline void cm_set_next_context(void *context)
 {
 #if DEBUG
 	uint64_t sp_mode;
diff --git a/include/drivers/arm/ccn.h b/include/drivers/arm/ccn.h
index 2361596..85c45c6 100644
--- a/include/drivers/arm/ccn.h
+++ b/include/drivers/arm/ccn.h
@@ -51,6 +51,13 @@
 #define CCN_L3_RUN_MODE_HAM	0x2	/* HNF_PM_HALF */
 #define CCN_L3_RUN_MODE_FAM	0x3	/* HNF_PM_FULL */
 
+/* part 0 IDs for various CCN variants */
+#define CCN_502_PART0_ID	0x30
+#define CCN_504_PART0_ID	0x26
+#define CCN_505_PART0_ID	0x27
+#define CCN_508_PART0_ID	0x28
+#define CCN_512_PART0_ID	0x29
+
 /*
  * The following macro takes the value returned from a read of a HN-F P-state
  * status register and returns the retention state value.
@@ -107,6 +114,7 @@
 		 unsigned int top_addr_bit1,
 		 unsigned char three_sn_en);
 unsigned int ccn_get_l3_run_mode(void);
+int ccn_get_part0_id(uintptr_t periphbase);
 
 #endif /* __ASSEMBLY__ */
 #endif /* __CCN_H__ */
diff --git a/include/drivers/emmc.h b/include/drivers/emmc.h
new file mode 100644
index 0000000..61d4495
--- /dev/null
+++ b/include/drivers/emmc.h
@@ -0,0 +1,182 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __EMMC_H__
+#define __EMMC_H__
+
+#include <stdint.h>
+
+#define EMMC_BLOCK_SIZE			512
+#define EMMC_BLOCK_MASK			(EMMC_BLOCK_SIZE - 1)
+#define EMMC_BOOT_CLK_RATE		(400 * 1000)
+
+#define EMMC_CMD0			0
+#define EMMC_CMD1			1
+#define EMMC_CMD2			2
+#define EMMC_CMD3			3
+#define EMMC_CMD6			6
+#define EMMC_CMD7			7
+#define EMMC_CMD8			8
+#define EMMC_CMD9			9
+#define EMMC_CMD12			12
+#define EMMC_CMD13			13
+#define EMMC_CMD17			17
+#define EMMC_CMD18			18
+#define EMMC_CMD24			24
+#define EMMC_CMD25			25
+#define EMMC_CMD35			35
+#define EMMC_CMD36			36
+#define EMMC_CMD38			38
+
+#define OCR_POWERUP			(1 << 31)
+#define OCR_BYTE_MODE			(0 << 29)
+#define OCR_SECTOR_MODE			(2 << 29)
+#define OCR_ACCESS_MODE_MASK		(3 << 29)
+#define OCR_VDD_MIN_2V7			(0x1ff << 15)
+#define OCR_VDD_MIN_2V0			(0x7f << 8)
+#define OCR_VDD_MIN_1V7			(1 << 7)
+
+#define EMMC_RESPONSE_R1		1
+#define EMMC_RESPONSE_R1B		1
+#define EMMC_RESPONSE_R2		4
+#define EMMC_RESPONSE_R3		1
+#define EMMC_RESPONSE_R4		1
+#define EMMC_RESPONSE_R5		1
+
+#define EMMC_FIX_RCA			6	/* > 1 */
+#define RCA_SHIFT_OFFSET		16
+
+#define CMD_EXTCSD_PARTITION_CONFIG	179
+#define CMD_EXTCSD_BUS_WIDTH		183
+#define CMD_EXTCSD_HS_TIMING		185
+
+#define PART_CFG_BOOT_PARTITION1_ENABLE	(1 << 3)
+#define PART_CFG_PARTITION1_ACCESS	(1 << 0)
+
+/* values in EXT CSD register */
+#define EMMC_BUS_WIDTH_1		0
+#define EMMC_BUS_WIDTH_4		1
+#define EMMC_BUS_WIDTH_8		2
+#define EMMC_BOOT_MODE_BACKWARD		(0 << 3)
+#define EMMC_BOOT_MODE_HS_TIMING	(1 << 3)
+#define EMMC_BOOT_MODE_DDR		(2 << 3)
+
+#define EXTCSD_SET_CMD			(0 << 24)
+#define EXTCSD_SET_BITS			(1 << 24)
+#define EXTCSD_CLR_BITS			(2 << 24)
+#define EXTCSD_WRITE_BYTES		(3 << 24)
+#define EXTCSD_CMD(x)			(((x) & 0xff) << 16)
+#define EXTCSD_VALUE(x)			(((x) & 0xff) << 8)
+
+#define STATUS_CURRENT_STATE(x)		(((x) & 0xf) << 9)
+#define STATUS_READY_FOR_DATA		(1 << 8)
+#define STATUS_SWITCH_ERROR		(1 << 7)
+#define EMMC_GET_STATE(x)		(((x) >> 9) & 0xf)
+#define EMMC_STATE_IDLE			0
+#define EMMC_STATE_READY		1
+#define EMMC_STATE_IDENT		2
+#define EMMC_STATE_STBY			3
+#define EMMC_STATE_TRAN			4
+#define EMMC_STATE_DATA			5
+#define EMMC_STATE_RCV			6
+#define EMMC_STATE_PRG			7
+#define EMMC_STATE_DIS			8
+#define EMMC_STATE_BTST			9
+#define EMMC_STATE_SLP			10
+
+typedef struct emmc_cmd {
+	unsigned int	cmd_idx;
+	unsigned int	cmd_arg;
+	unsigned int	resp_type;
+	unsigned int	resp_data[4];
+} emmc_cmd_t;
+
+typedef struct emmc_ops {
+	void (*init)(void);
+	int (*send_cmd)(emmc_cmd_t *cmd);
+	int (*set_ios)(int clk, int width);
+	int (*prepare)(int lba, uintptr_t buf, size_t size);
+	int (*read)(int lba, uintptr_t buf, size_t size);
+	int (*write)(int lba, const uintptr_t buf, size_t size);
+} emmc_ops_t;
+
+typedef struct emmc_csd {
+	unsigned char		not_used:		1;
+	unsigned char		crc:			7;
+	unsigned char		ecc:			2;
+	unsigned char		file_format:		2;
+	unsigned char		tmp_write_protect:	1;
+	unsigned char		perm_write_protect:	1;
+	unsigned char		copy:			1;
+	unsigned char		file_format_grp:	1;
+
+	unsigned short		reserved_1:		5;
+	unsigned short		write_bl_partial:	1;
+	unsigned short		write_bl_len:		4;
+	unsigned short		r2w_factor:		3;
+	unsigned short		default_ecc:		2;
+	unsigned short		wp_grp_enable:		1;
+
+	unsigned int		wp_grp_size:		5;
+	unsigned int		erase_grp_mult:		5;
+	unsigned int		erase_grp_size:		5;
+	unsigned int		c_size_mult:		3;
+	unsigned int		vdd_w_curr_max:		3;
+	unsigned int		vdd_w_curr_min:		3;
+	unsigned int		vdd_r_curr_max:		3;
+	unsigned int		vdd_r_curr_min:		3;
+	unsigned int		c_size_low:		2;
+
+	unsigned int		c_size_high:		10;
+	unsigned int		reserved_2:		2;
+	unsigned int		dsr_imp:		1;
+	unsigned int		read_blk_misalign:	1;
+	unsigned int		write_blk_misalign:	1;
+	unsigned int		read_bl_partial:	1;
+	unsigned int		read_bl_len:		4;
+	unsigned int		ccc:			12;
+
+	unsigned int		tran_speed:		8;
+	unsigned int		nsac:			8;
+	unsigned int		taac:			8;
+	unsigned int		reserved_3:		2;
+	unsigned int		spec_vers:		4;
+	unsigned int		csd_structure:		2;
+} emmc_csd_t;
+
+size_t emmc_read_blocks(int lba, uintptr_t buf, size_t size);
+size_t emmc_write_blocks(int lba, const uintptr_t buf, size_t size);
+size_t emmc_erase_blocks(int lba, size_t size);
+size_t emmc_rpmb_read_blocks(int lba, uintptr_t buf, size_t size);
+size_t emmc_rpmb_write_blocks(int lba, const uintptr_t buf, size_t size);
+size_t emmc_rpmb_erase_blocks(int lba, size_t size);
+void emmc_init(const emmc_ops_t *ops, int clk, int bus_width);
+
+#endif	/* __EMMC_H__ */
diff --git a/plat/rockchip/common/plat_delay_timer.c b/include/drivers/generic_delay_timer.h
similarity index 72%
copy from plat/rockchip/common/plat_delay_timer.c
copy to include/drivers/generic_delay_timer.h
index 797ce05..1450162 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/include/drivers/generic_delay_timer.h
@@ -28,27 +28,13 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#ifndef __GENERIC_DELAY_TIMER_H__
+#define __GENERIC_DELAY_TIMER_H__
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
+#include <stdint.h>
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+void generic_delay_timer_init_args(uint32_t mult, uint32_t div);
+
+void generic_delay_timer_init(void);
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /* __GENERIC_DELAY_TIMER_H__ */
diff --git a/include/drivers/gpio.h b/include/drivers/gpio.h
index a5cb5c7..633b3f6 100644
--- a/include/drivers/gpio.h
+++ b/include/drivers/gpio.h
@@ -37,17 +37,25 @@
 #define GPIO_LEVEL_LOW		0
 #define GPIO_LEVEL_HIGH		1
 
+#define GPIO_PULL_NONE		0
+#define GPIO_PULL_UP		1
+#define GPIO_PULL_DOWN		2
+
 typedef struct gpio_ops {
 	int (*get_direction)(int gpio);
 	void (*set_direction)(int gpio, int direction);
 	int (*get_value)(int gpio);
 	void (*set_value)(int gpio, int value);
+	void (*set_pull)(int gpio, int pull);
+	int (*get_pull)(int gpio);
 } gpio_ops_t;
 
 int gpio_get_direction(int gpio);
 void gpio_set_direction(int gpio, int direction);
 int gpio_get_value(int gpio);
 void gpio_set_value(int gpio, int value);
+void gpio_set_pull(int gpio, int pull);
+int gpio_get_pull(int gpio);
 void gpio_init(const gpio_ops_t *ops);
 
 #endif	/* __GPIO_H__ */
diff --git a/plat/rockchip/common/plat_delay_timer.c b/include/drivers/io/io_block.h
similarity index 72%
copy from plat/rockchip/common/plat_delay_timer.c
copy to include/drivers/io/io_block.h
index 797ce05..ebf43cd 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/include/drivers/io/io_block.h
@@ -28,27 +28,25 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#ifndef __IO_BLOCK_H__
+#define __IO_BLOCK_H__
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
+#include <io_storage.h>
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+/* block devices ops */
+typedef struct io_block_ops {
+	size_t	(*read)(int lba, uintptr_t buf, size_t size);
+	size_t	(*write)(int lba, const uintptr_t buf, size_t size);
+} io_block_ops_t;
+
+typedef struct io_block_dev_spec {
+	io_block_spec_t	buffer;
+	io_block_ops_t	ops;
+	size_t		block_size;
+} io_block_dev_spec_t;
+
+struct io_dev_connector;
+
+int register_io_dev_block(const struct io_dev_connector **dev_con);
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /* __IO_BLOCK_H__ */
diff --git a/plat/rockchip/common/plat_delay_timer.c b/include/drivers/io/io_dummy.h
similarity index 72%
copy from plat/rockchip/common/plat_delay_timer.c
copy to include/drivers/io/io_dummy.h
index 797ce05..c7ee454 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/include/drivers/io/io_dummy.h
@@ -28,27 +28,9 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#ifndef __IO_DUMMY_H__
+#define __IO_DUMMY_H__
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
-
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+int register_io_dev_dummy(const struct io_dev_connector **dev_con);
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /* __IO_DUMMY_H__ */
diff --git a/include/drivers/io/io_storage.h b/include/drivers/io/io_storage.h
index 970ab2c..1b3028a 100644
--- a/include/drivers/io/io_storage.h
+++ b/include/drivers/io/io_storage.h
@@ -43,7 +43,9 @@
 	IO_TYPE_INVALID,
 	IO_TYPE_SEMIHOSTING,
 	IO_TYPE_MEMMAP,
+	IO_TYPE_DUMMY,
 	IO_TYPE_FIRMWARE_IMAGE_PACKAGE,
+	IO_TYPE_BLOCK,
 	IO_TYPE_MAX
 } io_type_t;
 
diff --git a/include/lib/bakery_lock.h b/include/lib/bakery_lock.h
index 8a53891..6b8157e 100644
--- a/include/lib/bakery_lock.h
+++ b/include/lib/bakery_lock.h
@@ -96,7 +96,7 @@
 
 #endif /* __USE_COHERENT_MEM__ */
 
-inline void bakery_lock_init(bakery_lock_t *bakery) {}
+static inline void bakery_lock_init(bakery_lock_t *bakery) {}
 void bakery_lock_get(bakery_lock_t *bakery);
 void bakery_lock_release(bakery_lock_t *bakery);
 
diff --git a/include/lib/cpus/aarch64/cortex_a57.h b/include/lib/cpus/aarch64/cortex_a57.h
index c512129..ac4ae57 100644
--- a/include/lib/cpus/aarch64/cortex_a57.h
+++ b/include/lib/cpus/aarch64/cortex_a57.h
@@ -61,9 +61,15 @@
  ******************************************************************************/
 #define CPUACTLR_EL1			S3_1_C15_C2_0	/* Instruction def. */
 
+#define CPUACTLR_DIS_LOAD_PASS_DMB	(1 << 59)
+#define CPUACTLR_GRE_NGRE_AS_NGNRE	(1 << 54)
 #define CPUACTLR_DIS_OVERREAD		(1 << 52)
 #define CPUACTLR_NO_ALLOC_WBWA		(1 << 49)
 #define CPUACTLR_DCC_AS_DCCI		(1 << 44)
+#define CPUACTLR_FORCE_FPSCR_FLUSH	(1 << 38)
+#define CPUACTLR_DIS_STREAMING		(3 << 27)
+#define CPUACTLR_DIS_L1_STREAMING	(3 << 25)
+#define CPUACTLR_DIS_INDIRECT_PREDICTOR	(1 << 4)
 
 /*******************************************************************************
  * L2 Control register specific definitions.
diff --git a/plat/rockchip/common/plat_delay_timer.c b/include/lib/cpus/aarch64/cortex_a73.h
similarity index 72%
copy from plat/rockchip/common/plat_delay_timer.c
copy to include/lib/cpus/aarch64/cortex_a73.h
index 797ce05..2ad0467 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/include/lib/cpus/aarch64/cortex_a73.h
@@ -28,27 +28,17 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#ifndef __CORTEX_A73_H__
+#define __CORTEX_A73_H__
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
+/* Cortex-A73 midr for revision 0 */
+#define CORTEX_A73_MIDR	0x410FD090
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+/*******************************************************************************
+ * CPU Extended Control register specific definitions.
+ ******************************************************************************/
+#define CORTEX_A73_CPUECTLR_EL1		S3_1_C15_C2_1	/* Instruction def. */
+
+#define CORTEX_A73_CPUECTLR_SMP_BIT	(1 << 6)
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /* __CORTEX_A73_H__ */
diff --git a/include/lib/libfdt/fdt.h b/include/lib/libfdt/fdt.h
new file mode 100644
index 0000000..6331998
--- /dev/null
+++ b/include/lib/libfdt/fdt.h
@@ -0,0 +1,119 @@
+#ifndef _FDT_H
+#define _FDT_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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.
+ */
+
+/*
+ * Portions copyright (c) 2016, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
+#ifndef __ASSEMBLY__
+
+#include <libfdt_env.h>
+
+
+struct fdt_header {
+	fdt32_t magic;			 /* magic word FDT_MAGIC */
+	fdt32_t totalsize;		 /* total size of DT block */
+	fdt32_t off_dt_struct;		 /* offset to structure */
+	fdt32_t off_dt_strings;		 /* offset to strings */
+	fdt32_t off_mem_rsvmap;		 /* offset to memory reserve map */
+	fdt32_t version;		 /* format version */
+	fdt32_t last_comp_version;	 /* last compatible version */
+
+	/* version 2 fields below */
+	fdt32_t boot_cpuid_phys;	 /* Which physical CPU id we're
+					    booting on */
+	/* version 3 fields below */
+	fdt32_t size_dt_strings;	 /* size of the strings block */
+
+	/* version 17 fields below */
+	fdt32_t size_dt_struct;		 /* size of the structure block */
+};
+
+struct fdt_reserve_entry {
+	fdt64_t address;
+	fdt64_t size;
+};
+
+struct fdt_node_header {
+	fdt32_t tag;
+	char name[];
+};
+
+struct fdt_property {
+	fdt32_t tag;
+	fdt32_t len;
+	fdt32_t nameoff;
+	char data[];
+};
+
+#endif /* !__ASSEMBLY */
+
+#define FDT_MAGIC	0xd00dfeed	/* 4: version, 4: total size */
+#define FDT_TAGSIZE	sizeof(fdt32_t)
+
+#define FDT_BEGIN_NODE	0x1		/* Start node: full name */
+#define FDT_END_NODE	0x2		/* End node */
+#define FDT_PROP	0x3		/* Property: name off,
+					   size, content */
+#define FDT_NOP		0x4		/* nop */
+#define FDT_END		0x9
+
+#define FDT_V1_SIZE	(7*sizeof(fdt32_t))
+#define FDT_V2_SIZE	(FDT_V1_SIZE + sizeof(fdt32_t))
+#define FDT_V3_SIZE	(FDT_V2_SIZE + sizeof(fdt32_t))
+#define FDT_V16_SIZE	FDT_V3_SIZE
+#define FDT_V17_SIZE	(FDT_V16_SIZE + sizeof(fdt32_t))
+
+#endif /* _FDT_H */
diff --git a/include/lib/libfdt/libfdt.h b/include/lib/libfdt/libfdt.h
new file mode 100644
index 0000000..74f4895
--- /dev/null
+++ b/include/lib/libfdt/libfdt.h
@@ -0,0 +1,1580 @@
+#ifndef _LIBFDT_H
+#define _LIBFDT_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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.
+ */
+
+/*
+ * Portions copyright (c) 2016, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
+#include <libfdt_env.h>
+#include <fdt.h>
+
+#define FDT_FIRST_SUPPORTED_VERSION	0x10
+#define FDT_LAST_SUPPORTED_VERSION	0x11
+
+/* Error codes: informative error codes */
+#define FDT_ERR_NOTFOUND	1
+	/* FDT_ERR_NOTFOUND: The requested node or property does not exist */
+#define FDT_ERR_EXISTS		2
+	/* FDT_ERR_EXISTS: Attemped to create a node or property which
+	 * already exists */
+#define FDT_ERR_NOSPACE		3
+	/* FDT_ERR_NOSPACE: Operation needed to expand the device
+	 * tree, but its buffer did not have sufficient space to
+	 * contain the expanded tree. Use fdt_open_into() to move the
+	 * device tree to a buffer with more space. */
+
+/* Error codes: codes for bad parameters */
+#define FDT_ERR_BADOFFSET	4
+	/* FDT_ERR_BADOFFSET: Function was passed a structure block
+	 * offset which is out-of-bounds, or which points to an
+	 * unsuitable part of the structure for the operation. */
+#define FDT_ERR_BADPATH		5
+	/* FDT_ERR_BADPATH: Function was passed a badly formatted path
+	 * (e.g. missing a leading / for a function which requires an
+	 * absolute path) */
+#define FDT_ERR_BADPHANDLE	6
+	/* FDT_ERR_BADPHANDLE: Function was passed an invalid phandle
+	 * value.  phandle values of 0 and -1 are not permitted. */
+#define FDT_ERR_BADSTATE	7
+	/* FDT_ERR_BADSTATE: Function was passed an incomplete device
+	 * tree created by the sequential-write functions, which is
+	 * not sufficiently complete for the requested operation. */
+
+/* Error codes: codes for bad device tree blobs */
+#define FDT_ERR_TRUNCATED	8
+	/* FDT_ERR_TRUNCATED: Structure block of the given device tree
+	 * ends without an FDT_END tag. */
+#define FDT_ERR_BADMAGIC	9
+	/* FDT_ERR_BADMAGIC: Given "device tree" appears not to be a
+	 * device tree at all - it is missing the flattened device
+	 * tree magic number. */
+#define FDT_ERR_BADVERSION	10
+	/* FDT_ERR_BADVERSION: Given device tree has a version which
+	 * can't be handled by the requested operation.  For
+	 * read-write functions, this may mean that fdt_open_into() is
+	 * required to convert the tree to the expected version. */
+#define FDT_ERR_BADSTRUCTURE	11
+	/* FDT_ERR_BADSTRUCTURE: Given device tree has a corrupt
+	 * structure block or other serious error (e.g. misnested
+	 * nodes, or subnodes preceding properties). */
+#define FDT_ERR_BADLAYOUT	12
+	/* FDT_ERR_BADLAYOUT: For read-write functions, the given
+	 * device tree has it's sub-blocks in an order that the
+	 * function can't handle (memory reserve map, then structure,
+	 * then strings).  Use fdt_open_into() to reorganize the tree
+	 * into a form suitable for the read-write operations. */
+
+/* "Can't happen" error indicating a bug in libfdt */
+#define FDT_ERR_INTERNAL	13
+	/* FDT_ERR_INTERNAL: libfdt has failed an internal assertion.
+	 * Should never be returned, if it is, it indicates a bug in
+	 * libfdt itself. */
+
+/* Errors in device tree content */
+#define FDT_ERR_BADNCELLS	14
+	/* FDT_ERR_BADNCELLS: Device tree has a #address-cells, #size-cells
+	 * or similar property with a bad format or value */
+
+#define FDT_ERR_MAX		14
+
+/**********************************************************************/
+/* Low-level functions (you probably don't need these)                */
+/**********************************************************************/
+
+const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int checklen);
+static inline void *fdt_offset_ptr_w(void *fdt, int offset, int checklen)
+{
+	return (void *)(uintptr_t)fdt_offset_ptr(fdt, offset, checklen);
+}
+
+uint32_t fdt_next_tag(const void *fdt, int offset, int *nextoffset);
+
+/**********************************************************************/
+/* Traversal functions                                                */
+/**********************************************************************/
+
+int fdt_next_node(const void *fdt, int offset, int *depth);
+
+/**
+ * fdt_first_subnode() - get offset of first direct subnode
+ *
+ * @fdt:	FDT blob
+ * @offset:	Offset of node to check
+ * @return offset of first subnode, or -FDT_ERR_NOTFOUND if there is none
+ */
+int fdt_first_subnode(const void *fdt, int offset);
+
+/**
+ * fdt_next_subnode() - get offset of next direct subnode
+ *
+ * After first calling fdt_first_subnode(), call this function repeatedly to
+ * get direct subnodes of a parent node.
+ *
+ * @fdt:	FDT blob
+ * @offset:	Offset of previous subnode
+ * @return offset of next subnode, or -FDT_ERR_NOTFOUND if there are no more
+ * subnodes
+ */
+int fdt_next_subnode(const void *fdt, int offset);
+
+/**********************************************************************/
+/* General functions                                                  */
+/**********************************************************************/
+
+#define fdt_get_header(fdt, field) \
+	(fdt32_to_cpu(((const struct fdt_header *)(fdt))->field))
+#define fdt_magic(fdt) 			(fdt_get_header(fdt, magic))
+#define fdt_totalsize(fdt)		(fdt_get_header(fdt, totalsize))
+#define fdt_off_dt_struct(fdt)		(fdt_get_header(fdt, off_dt_struct))
+#define fdt_off_dt_strings(fdt)		(fdt_get_header(fdt, off_dt_strings))
+#define fdt_off_mem_rsvmap(fdt)		(fdt_get_header(fdt, off_mem_rsvmap))
+#define fdt_version(fdt)		(fdt_get_header(fdt, version))
+#define fdt_last_comp_version(fdt) 	(fdt_get_header(fdt, last_comp_version))
+#define fdt_boot_cpuid_phys(fdt) 	(fdt_get_header(fdt, boot_cpuid_phys))
+#define fdt_size_dt_strings(fdt) 	(fdt_get_header(fdt, size_dt_strings))
+#define fdt_size_dt_struct(fdt)		(fdt_get_header(fdt, size_dt_struct))
+
+#define __fdt_set_hdr(name) \
+	static inline void fdt_set_##name(void *fdt, uint32_t val) \
+	{ \
+		struct fdt_header *fdth = (struct fdt_header*)fdt; \
+		fdth->name = cpu_to_fdt32(val); \
+	}
+__fdt_set_hdr(magic)
+__fdt_set_hdr(totalsize)
+__fdt_set_hdr(off_dt_struct)
+__fdt_set_hdr(off_dt_strings)
+__fdt_set_hdr(off_mem_rsvmap)
+__fdt_set_hdr(version)
+__fdt_set_hdr(last_comp_version)
+__fdt_set_hdr(boot_cpuid_phys)
+__fdt_set_hdr(size_dt_strings)
+__fdt_set_hdr(size_dt_struct)
+#undef __fdt_set_hdr
+
+/**
+ * fdt_check_header - sanity check a device tree or possible device tree
+ * @fdt: pointer to data which might be a flattened device tree
+ *
+ * fdt_check_header() checks that the given buffer contains what
+ * appears to be a flattened device tree with sane information in its
+ * header.
+ *
+ * returns:
+ *     0, if the buffer appears to contain a valid device tree
+ *     -FDT_ERR_BADMAGIC,
+ *     -FDT_ERR_BADVERSION,
+ *     -FDT_ERR_BADSTATE, standard meanings, as above
+ */
+int fdt_check_header(const void *fdt);
+
+/**
+ * fdt_move - move a device tree around in memory
+ * @fdt: pointer to the device tree to move
+ * @buf: pointer to memory where the device is to be moved
+ * @bufsize: size of the memory space at buf
+ *
+ * fdt_move() relocates, if possible, the device tree blob located at
+ * fdt to the buffer at buf of size bufsize.  The buffer may overlap
+ * with the existing device tree blob at fdt.  Therefore,
+ *     fdt_move(fdt, fdt, fdt_totalsize(fdt))
+ * should always succeed.
+ *
+ * returns:
+ *     0, on success
+ *     -FDT_ERR_NOSPACE, bufsize is insufficient to contain the device tree
+ *     -FDT_ERR_BADMAGIC,
+ *     -FDT_ERR_BADVERSION,
+ *     -FDT_ERR_BADSTATE, standard meanings
+ */
+int fdt_move(const void *fdt, void *buf, int bufsize);
+
+/**********************************************************************/
+/* Read-only functions                                                */
+/**********************************************************************/
+
+/**
+ * fdt_string - retrieve a string from the strings block of a device tree
+ * @fdt: pointer to the device tree blob
+ * @stroffset: offset of the string within the strings block (native endian)
+ *
+ * fdt_string() retrieves a pointer to a single string from the
+ * strings block of the device tree blob at fdt.
+ *
+ * returns:
+ *     a pointer to the string, on success
+ *     NULL, if stroffset is out of bounds
+ */
+const char *fdt_string(const void *fdt, int stroffset);
+
+/**
+ * fdt_num_mem_rsv - retrieve the number of memory reserve map entries
+ * @fdt: pointer to the device tree blob
+ *
+ * Returns the number of entries in the device tree blob's memory
+ * reservation map.  This does not include the terminating 0,0 entry
+ * or any other (0,0) entries reserved for expansion.
+ *
+ * returns:
+ *     the number of entries
+ */
+int fdt_num_mem_rsv(const void *fdt);
+
+/**
+ * fdt_get_mem_rsv - retrieve one memory reserve map entry
+ * @fdt: pointer to the device tree blob
+ * @address, @size: pointers to 64-bit variables
+ *
+ * On success, *address and *size will contain the address and size of
+ * the n-th reserve map entry from the device tree blob, in
+ * native-endian format.
+ *
+ * returns:
+ *     0, on success
+ *     -FDT_ERR_BADMAGIC,
+ *     -FDT_ERR_BADVERSION,
+ *     -FDT_ERR_BADSTATE, standard meanings
+ */
+int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size);
+
+/**
+ * fdt_subnode_offset_namelen - find a subnode based on substring
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ * @namelen: number of characters of name to consider
+ *
+ * Identical to fdt_subnode_offset(), but only examine the first
+ * namelen characters of name for matching the subnode name.  This is
+ * useful for finding subnodes based on a portion of a larger string,
+ * such as a full path.
+ */
+int fdt_subnode_offset_namelen(const void *fdt, int parentoffset,
+			       const char *name, int namelen);
+/**
+ * fdt_subnode_offset - find a subnode of a given node
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ *
+ * fdt_subnode_offset() finds a subnode of the node at structure block
+ * offset parentoffset with the given name.  name may include a unit
+ * address, in which case fdt_subnode_offset() will find the subnode
+ * with that unit address, or the unit address may be omitted, in
+ * which case fdt_subnode_offset() will find an arbitrary subnode
+ * whose name excluding unit address matches the given name.
+ *
+ * returns:
+ *	structure block offset of the requested subnode (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
+ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE tag
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_subnode_offset(const void *fdt, int parentoffset, const char *name);
+
+/**
+ * fdt_path_offset - find a tree node by its full path
+ * @fdt: pointer to the device tree blob
+ * @path: full path of the node to locate
+ *
+ * fdt_path_offset() finds a node of a given path in the device tree.
+ * Each path component may omit the unit address portion, but the
+ * results of this are undefined if any such path component is
+ * ambiguous (that is if there are multiple nodes at the relevant
+ * level matching the given component, differentiated only by unit
+ * address).
+ *
+ * returns:
+ *	structure block offset of the node with the requested path (>=0), on success
+ *	-FDT_ERR_BADPATH, given path does not begin with '/' or is invalid
+ *	-FDT_ERR_NOTFOUND, if the requested node does not exist
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_path_offset(const void *fdt, const char *path);
+
+/**
+ * fdt_get_name - retrieve the name of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of the starting node
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_get_name() retrieves the name (including unit address) of the
+ * device tree node at structure block offset nodeoffset.  If lenp is
+ * non-NULL, the length of this name is also returned, in the integer
+ * pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the node's name, on success
+ *		If lenp is non-NULL, *lenp contains the length of that name (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL *lenp contains an error code (<0):
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE, standard meanings
+ */
+const char *fdt_get_name(const void *fdt, int nodeoffset, int *lenp);
+
+/**
+ * fdt_first_property_offset - find the offset of a node's first property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of a node
+ *
+ * fdt_first_property_offset() finds the first property of the node at
+ * the given structure block offset.
+ *
+ * returns:
+ *	structure block offset of the property (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the requested node has no properties
+ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_BEGIN_NODE tag
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_first_property_offset(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_next_property_offset - step through a node's properties
+ * @fdt: pointer to the device tree blob
+ * @offset: structure block offset of a property
+ *
+ * fdt_next_property_offset() finds the property immediately after the
+ * one at the given structure block offset.  This will be a property
+ * of the same node as the given property.
+ *
+ * returns:
+ *	structure block offset of the next property (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the given property is the last in its node
+ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_PROP tag
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_next_property_offset(const void *fdt, int offset);
+
+/**
+ * fdt_get_property_by_offset - retrieve the property at a given offset
+ * @fdt: pointer to the device tree blob
+ * @offset: offset of the property to retrieve
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_get_property_by_offset() retrieves a pointer to the
+ * fdt_property structure within the device tree blob at the given
+ * offset.  If lenp is non-NULL, the length of the property value is
+ * also returned, in the integer pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the structure representing the property
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
+						      int offset,
+						      int *lenp);
+
+/**
+ * fdt_get_property_namelen - find a property based on substring
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @namelen: number of characters of name to consider
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * Identical to fdt_get_property_namelen(), but only examine the first
+ * namelen characters of name for matching the property name.
+ */
+const struct fdt_property *fdt_get_property_namelen(const void *fdt,
+						    int nodeoffset,
+						    const char *name,
+						    int namelen, int *lenp);
+
+/**
+ * fdt_get_property - find a given property in a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_get_property() retrieves a pointer to the fdt_property
+ * structure within the device tree blob corresponding to the property
+ * named 'name' of the node at offset nodeoffset.  If lenp is
+ * non-NULL, the length of the property value is also returned, in the
+ * integer pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the structure representing the property
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_NOTFOUND, node does not have named property
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const struct fdt_property *fdt_get_property(const void *fdt, int nodeoffset,
+					    const char *name, int *lenp);
+static inline struct fdt_property *fdt_get_property_w(void *fdt, int nodeoffset,
+						      const char *name,
+						      int *lenp)
+{
+	return (struct fdt_property *)(uintptr_t)
+		fdt_get_property(fdt, nodeoffset, name, lenp);
+}
+
+/**
+ * fdt_getprop_by_offset - retrieve the value of a property at a given offset
+ * @fdt: pointer to the device tree blob
+ * @ffset: offset of the property to read
+ * @namep: pointer to a string variable (will be overwritten) or NULL
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_getprop_by_offset() retrieves a pointer to the value of the
+ * property at structure block offset 'offset' (this will be a pointer
+ * to within the device blob itself, not a copy of the value).  If
+ * lenp is non-NULL, the length of the property value is also
+ * returned, in the integer pointed to by lenp.  If namep is non-NULL,
+ * the property's namne will also be returned in the char * pointed to
+ * by namep (this will be a pointer to within the device tree's string
+ * block, not a new copy of the name).
+ *
+ * returns:
+ *	pointer to the property's value
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *		if namep is non-NULL *namep contiains a pointer to the property
+ *		name.
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const void *fdt_getprop_by_offset(const void *fdt, int offset,
+				  const char **namep, int *lenp);
+
+/**
+ * fdt_getprop_namelen - get property value based on substring
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @namelen: number of characters of name to consider
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * Identical to fdt_getprop(), but only examine the first namelen
+ * characters of name for matching the property name.
+ */
+const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
+				const char *name, int namelen, int *lenp);
+
+/**
+ * fdt_getprop - retrieve the value of a given property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_getprop() retrieves a pointer to the value of the property
+ * named 'name' of the node at offset nodeoffset (this will be a
+ * pointer to within the device blob itself, not a copy of the value).
+ * If lenp is non-NULL, the length of the property value is also
+ * returned, in the integer pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the property's value
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_NOTFOUND, node does not have named property
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const void *fdt_getprop(const void *fdt, int nodeoffset,
+			const char *name, int *lenp);
+static inline void *fdt_getprop_w(void *fdt, int nodeoffset,
+				  const char *name, int *lenp)
+{
+	return (void *)(uintptr_t)fdt_getprop(fdt, nodeoffset, name, lenp);
+}
+
+/**
+ * fdt_get_phandle - retrieve the phandle of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of the node
+ *
+ * fdt_get_phandle() retrieves the phandle of the device tree node at
+ * structure block offset nodeoffset.
+ *
+ * returns:
+ *	the phandle of the node at nodeoffset, on success (!= 0, != -1)
+ *	0, if the node has no phandle, or another error occurs
+ */
+uint32_t fdt_get_phandle(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_get_alias_namelen - get alias based on substring
+ * @fdt: pointer to the device tree blob
+ * @name: name of the alias th look up
+ * @namelen: number of characters of name to consider
+ *
+ * Identical to fdt_get_alias(), but only examine the first namelen
+ * characters of name for matching the alias name.
+ */
+const char *fdt_get_alias_namelen(const void *fdt,
+				  const char *name, int namelen);
+
+/**
+ * fdt_get_alias - retreive the path referenced by a given alias
+ * @fdt: pointer to the device tree blob
+ * @name: name of the alias th look up
+ *
+ * fdt_get_alias() retrieves the value of a given alias.  That is, the
+ * value of the property named 'name' in the node /aliases.
+ *
+ * returns:
+ *	a pointer to the expansion of the alias named 'name', if it exists
+ *	NULL, if the given alias or the /aliases node does not exist
+ */
+const char *fdt_get_alias(const void *fdt, const char *name);
+
+/**
+ * fdt_get_path - determine the full path of a node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose path to find
+ * @buf: character buffer to contain the returned path (will be overwritten)
+ * @buflen: size of the character buffer at buf
+ *
+ * fdt_get_path() computes the full path of the node at offset
+ * nodeoffset, and records that path in the buffer at buf.
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset.
+ *
+ * returns:
+ *	0, on success
+ *		buf contains the absolute path of the node at
+ *		nodeoffset, as a NUL-terminated string.
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_NOSPACE, the path of the given node is longer than (bufsize-1)
+ *		characters and will not fit in the given buffer.
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen);
+
+/**
+ * fdt_supernode_atdepth_offset - find a specific ancestor of a node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose parent to find
+ * @supernodedepth: depth of the ancestor to find
+ * @nodedepth: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_supernode_atdepth_offset() finds an ancestor of the given node
+ * at a specific depth from the root (where the root itself has depth
+ * 0, its immediate subnodes depth 1 and so forth).  So
+ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, NULL);
+ * will always return 0, the offset of the root node.  If the node at
+ * nodeoffset has depth D, then:
+ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, D, NULL);
+ * will return nodeoffset itself.
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset.
+ *
+ * returns:
+
+ *	structure block offset of the node at node offset's ancestor
+ *		of depth supernodedepth (>=0), on success
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+*	-FDT_ERR_NOTFOUND, supernodedepth was greater than the depth of nodeoffset
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
+				 int supernodedepth, int *nodedepth);
+
+/**
+ * fdt_node_depth - find the depth of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose parent to find
+ *
+ * fdt_node_depth() finds the depth of a given node.  The root node
+ * has depth 0, its immediate subnodes depth 1 and so forth.
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset.
+ *
+ * returns:
+ *	depth of the node at nodeoffset (>=0), on success
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_depth(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_parent_offset - find the parent of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose parent to find
+ *
+ * fdt_parent_offset() locates the parent node of a given node (that
+ * is, it finds the offset of the node which contains the node at
+ * nodeoffset as a subnode).
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset, *twice*.
+ *
+ * returns:
+ *	structure block offset of the parent of the node at nodeoffset
+ *		(>=0), on success
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_parent_offset(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_node_offset_by_prop_value - find nodes with a given property value
+ * @fdt: pointer to the device tree blob
+ * @startoffset: only find nodes after this offset
+ * @propname: property name to check
+ * @propval: property value to search for
+ * @proplen: length of the value in propval
+ *
+ * fdt_node_offset_by_prop_value() returns the offset of the first
+ * node after startoffset, which has a property named propname whose
+ * value is of length proplen and has value equal to propval; or if
+ * startoffset is -1, the very first such node in the tree.
+ *
+ * To iterate through all nodes matching the criterion, the following
+ * idiom can be used:
+ *	offset = fdt_node_offset_by_prop_value(fdt, -1, propname,
+ *					       propval, proplen);
+ *	while (offset != -FDT_ERR_NOTFOUND) {
+ *		// other code here
+ *		offset = fdt_node_offset_by_prop_value(fdt, offset, propname,
+ *						       propval, proplen);
+ *	}
+ *
+ * Note the -1 in the first call to the function, if 0 is used here
+ * instead, the function will never locate the root node, even if it
+ * matches the criterion.
+ *
+ * returns:
+ *	structure block offset of the located node (>= 0, >startoffset),
+ *		 on success
+ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
+ *		tree after startoffset
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
+				  const char *propname,
+				  const void *propval, int proplen);
+
+/**
+ * fdt_node_offset_by_phandle - find the node with a given phandle
+ * @fdt: pointer to the device tree blob
+ * @phandle: phandle value
+ *
+ * fdt_node_offset_by_phandle() returns the offset of the node
+ * which has the given phandle value.  If there is more than one node
+ * in the tree with the given phandle (an invalid tree), results are
+ * undefined.
+ *
+ * returns:
+ *	structure block offset of the located node (>= 0), on success
+ *	-FDT_ERR_NOTFOUND, no node with that phandle exists
+ *	-FDT_ERR_BADPHANDLE, given phandle value was invalid (0 or -1)
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle);
+
+/**
+ * fdt_node_check_compatible: check a node's compatible property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of a tree node
+ * @compatible: string to match against
+ *
+ *
+ * fdt_node_check_compatible() returns 0 if the given node contains a
+ * 'compatible' property with the given string as one of its elements,
+ * it returns non-zero otherwise, or on error.
+ *
+ * returns:
+ *	0, if the node has a 'compatible' property listing the given string
+ *	1, if the node has a 'compatible' property, but it does not list
+ *		the given string
+ *	-FDT_ERR_NOTFOUND, if the given node has no 'compatible' property
+ * 	-FDT_ERR_BADOFFSET, if nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_check_compatible(const void *fdt, int nodeoffset,
+			      const char *compatible);
+
+/**
+ * fdt_node_offset_by_compatible - find nodes with a given 'compatible' value
+ * @fdt: pointer to the device tree blob
+ * @startoffset: only find nodes after this offset
+ * @compatible: 'compatible' string to match against
+ *
+ * fdt_node_offset_by_compatible() returns the offset of the first
+ * node after startoffset, which has a 'compatible' property which
+ * lists the given compatible string; or if startoffset is -1, the
+ * very first such node in the tree.
+ *
+ * To iterate through all nodes matching the criterion, the following
+ * idiom can be used:
+ *	offset = fdt_node_offset_by_compatible(fdt, -1, compatible);
+ *	while (offset != -FDT_ERR_NOTFOUND) {
+ *		// other code here
+ *		offset = fdt_node_offset_by_compatible(fdt, offset, compatible);
+ *	}
+ *
+ * Note the -1 in the first call to the function, if 0 is used here
+ * instead, the function will never locate the root node, even if it
+ * matches the criterion.
+ *
+ * returns:
+ *	structure block offset of the located node (>= 0, >startoffset),
+ *		 on success
+ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
+ *		tree after startoffset
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
+				  const char *compatible);
+
+/**
+ * fdt_stringlist_contains - check a string list property for a string
+ * @strlist: Property containing a list of strings to check
+ * @listlen: Length of property
+ * @str: String to search for
+ *
+ * This is a utility function provided for convenience. The list contains
+ * one or more strings, each terminated by \0, as is found in a device tree
+ * "compatible" property.
+ *
+ * @return: 1 if the string is found in the list, 0 not found, or invalid list
+ */
+int fdt_stringlist_contains(const char *strlist, int listlen, const char *str);
+
+/**********************************************************************/
+/* Read-only functions (addressing related)                           */
+/**********************************************************************/
+
+/**
+ * FDT_MAX_NCELLS - maximum value for #address-cells and #size-cells
+ *
+ * This is the maximum value for #address-cells, #size-cells and
+ * similar properties that will be processed by libfdt.  IEE1275
+ * requires that OF implementations handle values up to 4.
+ * Implementations may support larger values, but in practice higher
+ * values aren't used.
+ */
+#define FDT_MAX_NCELLS		4
+
+/**
+ * fdt_address_cells - retrieve address size for a bus represented in the tree
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to find the address size for
+ *
+ * When the node has a valid #address-cells property, returns its value.
+ *
+ * returns:
+ *	0 <= n < FDT_MAX_NCELLS, on success
+ *      2, if the node has no #address-cells property
+ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid #address-cells property
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_address_cells(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_size_cells - retrieve address range size for a bus represented in the
+ *                  tree
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to find the address range size for
+ *
+ * When the node has a valid #size-cells property, returns its value.
+ *
+ * returns:
+ *	0 <= n < FDT_MAX_NCELLS, on success
+ *      2, if the node has no #address-cells property
+ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid #size-cells property
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_size_cells(const void *fdt, int nodeoffset);
+
+
+/**********************************************************************/
+/* Write-in-place functions                                           */
+/**********************************************************************/
+
+/**
+ * fdt_setprop_inplace - change a property's value, but not its size
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: pointer to data to replace the property value with
+ * @len: length of the property value
+ *
+ * fdt_setprop_inplace() replaces the value of a given property with
+ * the data in val, of length len.  This function cannot change the
+ * size of a property, and so will only work if len is equal to the
+ * current length of the property.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the given property value, and will not alter or move any other part
+ * of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, if len is not equal to the property's current length
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
+			const void *val, int len);
+
+/**
+ * fdt_setprop_inplace_u32 - change the value of a 32-bit integer property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 32-bit integer value to replace the property with
+ *
+ * fdt_setprop_inplace_u32() replaces the value of a given property
+ * with the 32-bit integer value in val, converting val to big-endian
+ * if necessary.  This function cannot change the size of a property,
+ * and so will only work if the property already exists and has length
+ * 4.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the given property value, and will not alter or move any other part
+ * of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 4
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_inplace_u32(void *fdt, int nodeoffset,
+					  const char *name, uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_inplace_u64 - change the value of a 64-bit integer property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 64-bit integer value to replace the property with
+ *
+ * fdt_setprop_inplace_u64() replaces the value of a given property
+ * with the 64-bit integer value in val, converting val to big-endian
+ * if necessary.  This function cannot change the size of a property,
+ * and so will only work if the property already exists and has length
+ * 8.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the given property value, and will not alter or move any other part
+ * of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 8
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_inplace_u64(void *fdt, int nodeoffset,
+					  const char *name, uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_inplace_cell - change the value of a single-cell property
+ *
+ * This is an alternative name for fdt_setprop_inplace_u32()
+ */
+static inline int fdt_setprop_inplace_cell(void *fdt, int nodeoffset,
+					   const char *name, uint32_t val)
+{
+	return fdt_setprop_inplace_u32(fdt, nodeoffset, name, val);
+}
+
+/**
+ * fdt_nop_property - replace a property with nop tags
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to nop
+ * @name: name of the property to nop
+ *
+ * fdt_nop_property() will replace a given property's representation
+ * in the blob with FDT_NOP tags, effectively removing it from the
+ * tree.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the property, and will not alter or move any other part of the
+ * tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_nop_property(void *fdt, int nodeoffset, const char *name);
+
+/**
+ * fdt_nop_node - replace a node (subtree) with nop tags
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to nop
+ *
+ * fdt_nop_node() will replace a given node's representation in the
+ * blob, including all its subnodes, if any, with FDT_NOP tags,
+ * effectively removing it from the tree.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the node and its properties and subnodes, and will not alter or
+ * move any other part of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_nop_node(void *fdt, int nodeoffset);
+
+/**********************************************************************/
+/* Sequential write functions                                         */
+/**********************************************************************/
+
+int fdt_create(void *buf, int bufsize);
+int fdt_resize(void *fdt, void *buf, int bufsize);
+int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size);
+int fdt_finish_reservemap(void *fdt);
+int fdt_begin_node(void *fdt, const char *name);
+int fdt_property(void *fdt, const char *name, const void *val, int len);
+static inline int fdt_property_u32(void *fdt, const char *name, uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_property(fdt, name, &tmp, sizeof(tmp));
+}
+static inline int fdt_property_u64(void *fdt, const char *name, uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_property(fdt, name, &tmp, sizeof(tmp));
+}
+static inline int fdt_property_cell(void *fdt, const char *name, uint32_t val)
+{
+	return fdt_property_u32(fdt, name, val);
+}
+#define fdt_property_string(fdt, name, str) \
+	fdt_property(fdt, name, str, strlen(str)+1)
+int fdt_end_node(void *fdt);
+int fdt_finish(void *fdt);
+
+/**********************************************************************/
+/* Read-write functions                                               */
+/**********************************************************************/
+
+int fdt_create_empty_tree(void *buf, int bufsize);
+int fdt_open_into(const void *fdt, void *buf, int bufsize);
+int fdt_pack(void *fdt);
+
+/**
+ * fdt_add_mem_rsv - add one memory reserve map entry
+ * @fdt: pointer to the device tree blob
+ * @address, @size: 64-bit values (native endian)
+ *
+ * Adds a reserve map entry to the given blob reserving a region at
+ * address address of length size.
+ *
+ * This function will insert data into the reserve map and will
+ * therefore change the indexes of some entries in the table.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new reservation entry
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size);
+
+/**
+ * fdt_del_mem_rsv - remove a memory reserve map entry
+ * @fdt: pointer to the device tree blob
+ * @n: entry to remove
+ *
+ * fdt_del_mem_rsv() removes the n-th memory reserve map entry from
+ * the blob.
+ *
+ * This function will delete data from the reservation table and will
+ * therefore change the indexes of some entries in the table.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOTFOUND, there is no entry of the given index (i.e. there
+ *		are less than n+1 reserve map entries)
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_del_mem_rsv(void *fdt, int n);
+
+/**
+ * fdt_set_name - change the name of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of a node
+ * @name: name to give the node
+ *
+ * fdt_set_name() replaces the name (including unit address, if any)
+ * of the given node with the given string.  NOTE: this function can't
+ * efficiently check if the new name is unique amongst the given
+ * node's siblings; results are undefined if this function is invoked
+ * with a name equal to one of the given node's siblings.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob
+ *		to contain the new name
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE, standard meanings
+ */
+int fdt_set_name(void *fdt, int nodeoffset, const char *name);
+
+/**
+ * fdt_setprop - create or change a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: pointer to data to set the property value to
+ * @len: length of the property value
+ *
+ * fdt_setprop() sets the value of the named property in the given
+ * node to the given value and length, creating the property if it
+ * does not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_setprop(void *fdt, int nodeoffset, const char *name,
+		const void *val, int len);
+
+/**
+ * fdt_setprop_u32 - set a property to a 32-bit integer
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 32-bit integer value for the property (native endian)
+ *
+ * fdt_setprop_u32() sets the value of the named property in the given
+ * node to the given 32-bit integer value (converting to big-endian if
+ * necessary), or creates a new property with that value if it does
+ * not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_u32(void *fdt, int nodeoffset, const char *name,
+				  uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_u64 - set a property to a 64-bit integer
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 64-bit integer value for the property (native endian)
+ *
+ * fdt_setprop_u64() sets the value of the named property in the given
+ * node to the given 64-bit integer value (converting to big-endian if
+ * necessary), or creates a new property with that value if it does
+ * not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_u64(void *fdt, int nodeoffset, const char *name,
+				  uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_cell - set a property to a single cell value
+ *
+ * This is an alternative name for fdt_setprop_u32()
+ */
+static inline int fdt_setprop_cell(void *fdt, int nodeoffset, const char *name,
+				   uint32_t val)
+{
+	return fdt_setprop_u32(fdt, nodeoffset, name, val);
+}
+
+/**
+ * fdt_setprop_string - set a property to a string value
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @str: string value for the property
+ *
+ * fdt_setprop_string() sets the value of the named property in the
+ * given node to the given string value (using the length of the
+ * string to determine the new length of the property), or creates a
+ * new property with that value if it does not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+#define fdt_setprop_string(fdt, nodeoffset, name, str) \
+	fdt_setprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
+
+/**
+ * fdt_appendprop - append to or create a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to append to
+ * @val: pointer to data to append to the property value
+ * @len: length of the data to append to the property value
+ *
+ * fdt_appendprop() appends the value to the named property in the
+ * given node, creating the property if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
+		   const void *val, int len);
+
+/**
+ * fdt_appendprop_u32 - append a 32-bit integer value to a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 32-bit integer value to append to the property (native endian)
+ *
+ * fdt_appendprop_u32() appends the given 32-bit integer value
+ * (converting to big-endian if necessary) to the value of the named
+ * property in the given node, or creates a new property with that
+ * value if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_appendprop_u32(void *fdt, int nodeoffset,
+				     const char *name, uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_appendprop_u64 - append a 64-bit integer value to a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 64-bit integer value to append to the property (native endian)
+ *
+ * fdt_appendprop_u64() appends the given 64-bit integer value
+ * (converting to big-endian if necessary) to the value of the named
+ * property in the given node, or creates a new property with that
+ * value if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_appendprop_u64(void *fdt, int nodeoffset,
+				     const char *name, uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_appendprop_cell - append a single cell value to a property
+ *
+ * This is an alternative name for fdt_appendprop_u32()
+ */
+static inline int fdt_appendprop_cell(void *fdt, int nodeoffset,
+				      const char *name, uint32_t val)
+{
+	return fdt_appendprop_u32(fdt, nodeoffset, name, val);
+}
+
+/**
+ * fdt_appendprop_string - append a string to a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @str: string value to append to the property
+ *
+ * fdt_appendprop_string() appends the given string to the value of
+ * the named property in the given node, or creates a new property
+ * with that value if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+#define fdt_appendprop_string(fdt, nodeoffset, name, str) \
+	fdt_appendprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
+
+/**
+ * fdt_delprop - delete a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to nop
+ * @name: name of the property to nop
+ *
+ * fdt_del_property() will delete the given property.
+ *
+ * This function will delete data from the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_delprop(void *fdt, int nodeoffset, const char *name);
+
+/**
+ * fdt_add_subnode_namelen - creates a new node based on substring
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ * @namelen: number of characters of name to consider
+ *
+ * Identical to fdt_add_subnode(), but use only the first namelen
+ * characters of name as the name of the new node.  This is useful for
+ * creating subnodes based on a portion of a larger string, such as a
+ * full path.
+ */
+int fdt_add_subnode_namelen(void *fdt, int parentoffset,
+			    const char *name, int namelen);
+
+/**
+ * fdt_add_subnode - creates a new node
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ *
+ * fdt_add_subnode() creates a new node as a subnode of the node at
+ * structure block offset parentoffset, with the given name (which
+ * should include the unit address, if any).
+ *
+ * This function will insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+
+ * returns:
+ *	structure block offset of the created nodeequested subnode (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
+ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE tag
+ *	-FDT_ERR_EXISTS, if the node at parentoffset already has a subnode of
+ *		the given name
+ *	-FDT_ERR_NOSPACE, if there is insufficient free space in the
+ *		blob to contain the new node
+ *	-FDT_ERR_NOSPACE
+ *	-FDT_ERR_BADLAYOUT
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_add_subnode(void *fdt, int parentoffset, const char *name);
+
+/**
+ * fdt_del_node - delete a node (subtree)
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to nop
+ *
+ * fdt_del_node() will remove the given node, including all its
+ * subnodes if any, from the blob.
+ *
+ * This function will delete data from the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_del_node(void *fdt, int nodeoffset);
+
+/**********************************************************************/
+/* Debugging / informational functions                                */
+/**********************************************************************/
+
+const char *fdt_strerror(int errval);
+
+#endif /* _LIBFDT_H */
diff --git a/include/lib/libfdt/libfdt_env.h b/include/lib/libfdt/libfdt_env.h
new file mode 100644
index 0000000..9dea97d
--- /dev/null
+++ b/include/lib/libfdt/libfdt_env.h
@@ -0,0 +1,111 @@
+#ifndef _LIBFDT_ENV_H
+#define _LIBFDT_ENV_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+#ifdef __CHECKER__
+#define __force __attribute__((force))
+#define __bitwise __attribute__((bitwise))
+#else
+#define __force
+#define __bitwise
+#endif
+
+typedef uint16_t __bitwise fdt16_t;
+typedef uint32_t __bitwise fdt32_t;
+typedef uint64_t __bitwise fdt64_t;
+
+#define EXTRACT_BYTE(x, n)	((unsigned long long)((uint8_t *)&x)[n])
+#define CPU_TO_FDT16(x) ((EXTRACT_BYTE(x, 0) << 8) | EXTRACT_BYTE(x, 1))
+#define CPU_TO_FDT32(x) ((EXTRACT_BYTE(x, 0) << 24) | (EXTRACT_BYTE(x, 1) << 16) | \
+			 (EXTRACT_BYTE(x, 2) << 8) | EXTRACT_BYTE(x, 3))
+#define CPU_TO_FDT64(x) ((EXTRACT_BYTE(x, 0) << 56) | (EXTRACT_BYTE(x, 1) << 48) | \
+			 (EXTRACT_BYTE(x, 2) << 40) | (EXTRACT_BYTE(x, 3) << 32) | \
+			 (EXTRACT_BYTE(x, 4) << 24) | (EXTRACT_BYTE(x, 5) << 16) | \
+			 (EXTRACT_BYTE(x, 6) << 8) | EXTRACT_BYTE(x, 7))
+
+static inline uint16_t fdt16_to_cpu(fdt16_t x)
+{
+	return (__force uint16_t)CPU_TO_FDT16(x);
+}
+static inline fdt16_t cpu_to_fdt16(uint16_t x)
+{
+	return (__force fdt16_t)CPU_TO_FDT16(x);
+}
+
+static inline uint32_t fdt32_to_cpu(fdt32_t x)
+{
+	return (__force uint32_t)CPU_TO_FDT32(x);
+}
+static inline fdt32_t cpu_to_fdt32(uint32_t x)
+{
+	return (__force fdt32_t)CPU_TO_FDT32(x);
+}
+
+static inline uint64_t fdt64_to_cpu(fdt64_t x)
+{
+	return (__force uint64_t)CPU_TO_FDT64(x);
+}
+static inline fdt64_t cpu_to_fdt64(uint64_t x)
+{
+	return (__force fdt64_t)CPU_TO_FDT64(x);
+}
+#undef CPU_TO_FDT64
+#undef CPU_TO_FDT32
+#undef CPU_TO_FDT16
+#undef EXTRACT_BYTE
+
+#endif /* _LIBFDT_ENV_H */
diff --git a/include/lib/pmf.h b/include/lib/pmf.h
new file mode 100644
index 0000000..5f953b5
--- /dev/null
+++ b/include/lib/pmf.h
@@ -0,0 +1,199 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __PMF_H__
+#define __PMF_H__
+
+#include <cassert.h>
+#include <pmf_helpers.h>
+
+/*
+ * Constants used for/by PMF services.
+ */
+#define PMF_ARM_TIF_IMPL_ID	(0x41000000)
+#define PMF_TID_SHIFT		0
+#define PMF_TID_MASK		(0xFF << PMF_TID_SHIFT)
+#define PMF_SVC_ID_SHIFT	10
+#define PMF_SVC_ID_MASK		(0x3F << PMF_SVC_ID_SHIFT)
+#define PMF_IMPL_ID_SHIFT	24
+#define PMF_IMPL_ID_MASK	(0xFF << PMF_IMPL_ID_SHIFT)
+
+/*
+ * Flags passed to PMF_REGISTER_SERVICE
+ */
+#define PMF_STORE_ENABLE	(1 << 0)
+#define PMF_DUMP_ENABLE		(1 << 1)
+
+/*
+ * Flags passed to PMF_GET_TIMESTAMP_XXX
+ * and PMF_CAPTURE_TIMESTAMP
+ */
+#define PMF_CACHE_MAINT		(1 << 0)
+#define PMF_NO_CACHE_MAINT	0
+
+/*
+ * Defines for PMF SMC function ids.
+ */
+#define PMF_SMC_GET_TIMESTAMP_32	0x82000010
+#define PMF_SMC_GET_TIMESTAMP_64	0xC2000010
+#define PMF_NUM_SMC_CALLS		2
+
+/*
+ * The macros below are used to identify
+ * PMF calls from the SMC function ID.
+ */
+#define PMF_FID_MASK	0xffe0u
+#define PMF_FID_VALUE	0u
+#define is_pmf_fid(_fid)	(((_fid) & PMF_FID_MASK) == PMF_FID_VALUE)
+
+/* Following are the supported PMF service IDs */
+#define PMF_PSCI_STAT_SVC_ID	0
+
+#if ENABLE_PMF
+/*
+ * Convenience macros for capturing time-stamp.
+ */
+#define PMF_DECLARE_CAPTURE_TIMESTAMP(_name)			\
+	void pmf_capture_timestamp_with_cache_maint_ ## _name(	\
+				unsigned int tid,		\
+				unsigned long long ts);		\
+	void pmf_capture_timestamp_ ## _name(			\
+				unsigned int tid,		\
+				unsigned long long ts);
+
+#define PMF_CAPTURE_TIMESTAMP(_name, _tid, _flags)			\
+	do {								\
+		unsigned long long ts = read_cntpct_el0();		\
+		if ((_flags) & PMF_CACHE_MAINT)				\
+			pmf_capture_timestamp_with_cache_maint_ ## _name((_tid), ts);\
+		else							\
+			pmf_capture_timestamp_ ## _name((_tid), ts);	\
+	} while (0)
+
+#define PMF_CAPTURE_AND_GET_TIMESTAMP(_name, _tid, _flags, _tsval)	\
+	do {								\
+		(_tsval) = read_cntpct_el0();				\
+		CASSERT(sizeof(_tsval) == sizeof(unsigned long long), invalid_tsval_size);\
+		if ((_flags) & PMF_CACHE_MAINT)				\
+			pmf_capture_timestamp_with_cache_maint_ ## _name((_tid), (_tsval));\
+		else							\
+			pmf_capture_timestamp_ ## _name((_tid), (_tsval));\
+	} while (0)
+
+#define PMF_WRITE_TIMESTAMP(_name, _tid, _flags, _wrval)		\
+	do {								\
+		CASSERT(sizeof(_wrval) == sizeof(unsigned long long), invalid_wrval_size);\
+		if ((_flags) & PMF_CACHE_MAINT)				\
+			pmf_capture_timestamp_with_cache_maint_ ## _name((_tid), (_wrval));\
+		else							\
+			pmf_capture_timestamp_ ## _name((_tid), (_wrval));\
+	} while (0)
+
+/*
+ * Convenience macros for retrieving time-stamp.
+ */
+#define PMF_DECLARE_GET_TIMESTAMP(_name)			\
+	unsigned long long pmf_get_timestamp_by_index_ ## _name(\
+		unsigned int tid,				\
+		unsigned int cpuid,				\
+		unsigned int flags);				\
+	unsigned long long pmf_get_timestamp_by_mpidr_ ## _name(\
+		unsigned int tid,				\
+		u_register_t mpidr,				\
+		unsigned int flags);
+
+#define PMF_GET_TIMESTAMP_BY_MPIDR(_name, _tid, _mpidr, _flags, _tsval)\
+	_tsval = pmf_get_timestamp_by_mpidr_ ## _name(_tid, _mpidr, _flags)
+
+#define PMF_GET_TIMESTAMP_BY_INDEX(_name, _tid, _cpuid, _flags, _tsval)\
+	_tsval = pmf_get_timestamp_by_index_ ## _name(_tid, _cpuid, _flags)
+
+/* Convenience macros to register a PMF service.*/
+/*
+ * This macro is used to register a PMF Service. It allocates PMF memory
+ * and defines default service-specific PMF functions.
+ */
+#define PMF_REGISTER_SERVICE(_name, _svcid, _totalid, _flags)	\
+	PMF_ALLOCATE_TIMESTAMP_MEMORY(_name, _totalid)		\
+	PMF_DEFINE_CAPTURE_TIMESTAMP(_name, _flags)		\
+	PMF_DEFINE_GET_TIMESTAMP(_name)
+
+/*
+ * This macro is used to register a PMF service, including an
+ * SMC interface to that service.
+ */
+#define PMF_REGISTER_SERVICE_SMC(_name, _svcid, _totalid, _flags)\
+	PMF_REGISTER_SERVICE(_name, _svcid, _totalid, _flags)	\
+	PMF_DEFINE_SERVICE_DESC(_name, PMF_ARM_TIF_IMPL_ID,	\
+			_svcid, _totalid, NULL,			\
+			pmf_get_timestamp_by_mpidr_ ## _name)
+
+/*
+ * This macro is used to register a PMF service that has an SMC interface
+ * but provides its own service-specific PMF functions.
+ */
+#define PMF_REGISTER_SERVICE_SMC_OWN(_name, _implid, _svcid, _totalid,	\
+		 _init, _getts)						\
+	PMF_DEFINE_SERVICE_DESC(_name, _implid, _svcid, _totalid,	\
+		 _init, _getts)
+
+#else
+
+#define PMF_REGISTER_SERVICE(_name, _svcid, _totalid, _flags)
+#define PMF_REGISTER_SERVICE_SMC(_name, _svcid, _totalid, _flags)
+#define PMF_REGISTER_SERVICE_SMC_OWN(_name, _implid, _svcid, _totalid,	\
+				_init, _getts)
+#define PMF_DECLARE_CAPTURE_TIMESTAMP(_name)
+#define PMF_DECLARE_GET_TIMESTAMP(_name)
+#define PMF_CAPTURE_TIMESTAMP(_name, _tid, _flags)
+#define PMF_GET_TIMESTAMP_BY_MPIDR(_name, _tid, _mpidr, _flags, _tsval)
+#define PMF_GET_TIMESTAMP_BY_INDEX(_name, _tid, _cpuid, _flags, _tsval)
+
+#endif /* ENABLE_PMF */
+
+/*******************************************************************************
+ * Function & variable prototypes
+ ******************************************************************************/
+/* PMF common functions */
+int pmf_get_timestamp_smc(unsigned int tid,
+		u_register_t mpidr,
+		unsigned int flags,
+		unsigned long long *ts);
+int pmf_setup(void);
+uintptr_t pmf_smc_handler(unsigned int smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		u_register_t flags);
+
+#endif /* __PMF_H__ */
diff --git a/include/lib/pmf_helpers.h b/include/lib/pmf_helpers.h
new file mode 100644
index 0000000..1b8c507
--- /dev/null
+++ b/include/lib/pmf_helpers.h
@@ -0,0 +1,166 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __PMF_HELPERS_H__
+#define __PMF_HELPERS_H__
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <platform.h>
+#include <pmf.h>
+#include <stdint.h>
+#include <stddef.h>
+
+/*
+ * Prototype for PMF service functions.
+ */
+typedef int (*pmf_svc_init_t)(void);
+typedef unsigned long long (*pmf_svc_get_ts_t)(unsigned int tid,
+		 u_register_t mpidr,
+		 unsigned int flags);
+
+/*
+ * This is the definition of PMF service desc.
+ */
+typedef struct pmf_svc_desc {
+	/* Structure version information */
+	param_header_t h;
+
+	/* Name of the PMF service */
+	const char *name;
+
+	/* PMF service config: Implementer id, Service id and total id*/
+	unsigned int svc_config;
+
+	/* PMF service initialization handler */
+	pmf_svc_init_t init;
+
+	/* PMF service time-stamp retrieval handler */
+	pmf_svc_get_ts_t get_ts;
+} pmf_svc_desc_t;
+
+/*
+ * Convenience macro to allocate memory for a PMF service.
+ */
+#define PMF_ALLOCATE_TIMESTAMP_MEMORY(_name, _total_id)			\
+	static unsigned long long pmf_ts_mem_ ## _name[_total_id]	\
+	__section("pmf_timestamp_array") __used;
+
+/*
+ * Convenience macro to validate tid index for the given TS array.
+ */
+#define PMF_VALIDATE_TID(_name, _tid)	\
+	assert((_tid & PMF_TID_MASK) < (ARRAY_SIZE(pmf_ts_mem_ ## _name)))
+
+/*
+ * Convenience macros for capturing time-stamp.
+ */
+#define PMF_DEFINE_CAPTURE_TIMESTAMP(_name, _flags)			\
+	void pmf_capture_timestamp_ ## _name(				\
+			unsigned int tid,				\
+			unsigned long long ts)				\
+	{								\
+		CASSERT(_flags, select_proper_config);			\
+		PMF_VALIDATE_TID(_name, tid);				\
+		uintptr_t base_addr = (uintptr_t) pmf_ts_mem_ ## _name;	\
+		if ((_flags) & PMF_STORE_ENABLE)			\
+			__pmf_store_timestamp(base_addr, tid, ts);	\
+		if ((_flags) & PMF_DUMP_ENABLE)				\
+			__pmf_dump_timestamp(tid, ts);			\
+	}								\
+	void pmf_capture_timestamp_with_cache_maint_ ## _name(		\
+			unsigned int tid,				\
+			unsigned long long ts)				\
+	{								\
+		CASSERT(_flags, select_proper_config);			\
+		PMF_VALIDATE_TID(_name, tid);				\
+		uintptr_t base_addr = (uintptr_t) pmf_ts_mem_ ## _name;	\
+		if ((_flags) & PMF_STORE_ENABLE)			\
+			__pmf_store_timestamp_with_cache_maint(base_addr, tid, ts);\
+		if ((_flags) & PMF_DUMP_ENABLE)				\
+			__pmf_dump_timestamp(tid, ts);			\
+	}
+
+/*
+ * Convenience macros for retrieving time-stamp.
+ */
+#define PMF_DEFINE_GET_TIMESTAMP(_name)					\
+	unsigned long long pmf_get_timestamp_by_index_ ## _name(	\
+		unsigned int tid, unsigned int cpuid, unsigned int flags)\
+	{								\
+		PMF_VALIDATE_TID(_name, tid);				\
+		uintptr_t base_addr = (uintptr_t) pmf_ts_mem_ ## _name;	\
+		return __pmf_get_timestamp(base_addr, tid, cpuid, flags);\
+	}								\
+	unsigned long long pmf_get_timestamp_by_mpidr_ ## _name(	\
+		unsigned int tid, u_register_t mpidr, unsigned int flags)\
+	{								\
+		PMF_VALIDATE_TID(_name, tid);				\
+		uintptr_t base_addr = (uintptr_t) pmf_ts_mem_ ## _name;	\
+		return __pmf_get_timestamp(base_addr, tid,		\
+			plat_core_pos_by_mpidr(mpidr), flags);		\
+	}
+
+/*
+ * Convenience macro to register a PMF service.
+ * This is needed for services that require SMC handling.
+ */
+#define PMF_DEFINE_SERVICE_DESC(_name, _implid, _svcid, _totalid,	\
+		_init, _getts_by_mpidr) 				\
+	static const pmf_svc_desc_t __pmf_desc_ ## _name 		\
+	__section("pmf_svc_descs") __used = {		 		\
+		.h.type = PARAM_EP, 					\
+		.h.version = VERSION_1, 				\
+		.h.size = sizeof(pmf_svc_desc_t),			\
+		.h.attr = 0,						\
+		.name = #_name, 					\
+		.svc_config = ((((_implid) << PMF_IMPL_ID_SHIFT) &	\
+						PMF_IMPL_ID_MASK) |	\
+				(((_svcid) << PMF_SVC_ID_SHIFT) &	\
+						PMF_SVC_ID_MASK) |	\
+				(((_totalid) << PMF_TID_SHIFT) &	\
+						PMF_TID_MASK)),		\
+		.init = _init,						\
+		.get_ts = _getts_by_mpidr				\
+	};
+
+/* PMF internal functions */
+void __pmf_dump_timestamp(unsigned int tid, unsigned long long ts);
+void __pmf_store_timestamp(uintptr_t base_addr,
+		unsigned int tid,
+		unsigned long long ts);
+void __pmf_store_timestamp_with_cache_maint(uintptr_t base_addr,
+		unsigned int tid,
+		unsigned long long ts);
+unsigned long long __pmf_get_timestamp(uintptr_t base_addr,
+		unsigned int tid,
+		unsigned int cpuid,
+		unsigned int flags);
+#endif /* __PMF_HELPERS_H__ */
diff --git a/include/stdlib/assert.h b/include/lib/stdlib/assert.h
similarity index 100%
rename from include/stdlib/assert.h
rename to include/lib/stdlib/assert.h
diff --git a/include/stdlib/inttypes.h b/include/lib/stdlib/inttypes.h
similarity index 100%
rename from include/stdlib/inttypes.h
rename to include/lib/stdlib/inttypes.h
diff --git a/include/stdlib/machine/_inttypes.h b/include/lib/stdlib/machine/_inttypes.h
similarity index 100%
rename from include/stdlib/machine/_inttypes.h
rename to include/lib/stdlib/machine/_inttypes.h
diff --git a/include/stdlib/machine/_limits.h b/include/lib/stdlib/machine/_limits.h
similarity index 100%
rename from include/stdlib/machine/_limits.h
rename to include/lib/stdlib/machine/_limits.h
diff --git a/include/stdlib/machine/_stdint.h b/include/lib/stdlib/machine/_stdint.h
similarity index 100%
rename from include/stdlib/machine/_stdint.h
rename to include/lib/stdlib/machine/_stdint.h
diff --git a/include/stdlib/machine/_types.h b/include/lib/stdlib/machine/_types.h
similarity index 100%
rename from include/stdlib/machine/_types.h
rename to include/lib/stdlib/machine/_types.h
diff --git a/include/stdlib/stddef.h b/include/lib/stdlib/stddef.h
similarity index 100%
rename from include/stdlib/stddef.h
rename to include/lib/stdlib/stddef.h
diff --git a/include/stdlib/stdio.h b/include/lib/stdlib/stdio.h
similarity index 100%
rename from include/stdlib/stdio.h
rename to include/lib/stdlib/stdio.h
diff --git a/include/stdlib/stdlib.h b/include/lib/stdlib/stdlib.h
similarity index 100%
rename from include/stdlib/stdlib.h
rename to include/lib/stdlib/stdlib.h
diff --git a/include/stdlib/string.h b/include/lib/stdlib/string.h
similarity index 100%
rename from include/stdlib/string.h
rename to include/lib/stdlib/string.h
diff --git a/include/stdlib/strings.h b/include/lib/stdlib/strings.h
similarity index 100%
rename from include/stdlib/strings.h
rename to include/lib/stdlib/strings.h
diff --git a/include/stdlib/sys/_null.h b/include/lib/stdlib/sys/_null.h
similarity index 100%
rename from include/stdlib/sys/_null.h
rename to include/lib/stdlib/sys/_null.h
diff --git a/include/stdlib/sys/_stdint.h b/include/lib/stdlib/sys/_stdint.h
similarity index 100%
rename from include/stdlib/sys/_stdint.h
rename to include/lib/stdlib/sys/_stdint.h
diff --git a/include/stdlib/sys/_timespec.h b/include/lib/stdlib/sys/_timespec.h
similarity index 100%
rename from include/stdlib/sys/_timespec.h
rename to include/lib/stdlib/sys/_timespec.h
diff --git a/include/stdlib/sys/_types.h b/include/lib/stdlib/sys/_types.h
similarity index 100%
rename from include/stdlib/sys/_types.h
rename to include/lib/stdlib/sys/_types.h
diff --git a/include/stdlib/sys/cdefs.h b/include/lib/stdlib/sys/cdefs.h
similarity index 100%
rename from include/stdlib/sys/cdefs.h
rename to include/lib/stdlib/sys/cdefs.h
diff --git a/include/stdlib/sys/ctype.h b/include/lib/stdlib/sys/ctype.h
similarity index 100%
rename from include/stdlib/sys/ctype.h
rename to include/lib/stdlib/sys/ctype.h
diff --git a/include/stdlib/sys/errno.h b/include/lib/stdlib/sys/errno.h
similarity index 100%
rename from include/stdlib/sys/errno.h
rename to include/lib/stdlib/sys/errno.h
diff --git a/include/stdlib/sys/limits.h b/include/lib/stdlib/sys/limits.h
similarity index 100%
rename from include/stdlib/sys/limits.h
rename to include/lib/stdlib/sys/limits.h
diff --git a/include/stdlib/sys/stdarg.h b/include/lib/stdlib/sys/stdarg.h
similarity index 100%
rename from include/stdlib/sys/stdarg.h
rename to include/lib/stdlib/sys/stdarg.h
diff --git a/include/stdlib/sys/stdint.h b/include/lib/stdlib/sys/stdint.h
similarity index 100%
rename from include/stdlib/sys/stdint.h
rename to include/lib/stdlib/sys/stdint.h
diff --git a/include/stdlib/sys/timespec.h b/include/lib/stdlib/sys/timespec.h
similarity index 100%
rename from include/stdlib/sys/timespec.h
rename to include/lib/stdlib/sys/timespec.h
diff --git a/include/stdlib/sys/types.h b/include/lib/stdlib/sys/types.h
similarity index 100%
rename from include/stdlib/sys/types.h
rename to include/lib/stdlib/sys/types.h
diff --git a/include/stdlib/sys/uuid.h b/include/lib/stdlib/sys/uuid.h
similarity index 100%
rename from include/stdlib/sys/uuid.h
rename to include/lib/stdlib/sys/uuid.h
diff --git a/include/stdlib/time.h b/include/lib/stdlib/time.h
similarity index 100%
rename from include/stdlib/time.h
rename to include/lib/stdlib/time.h
diff --git a/include/stdlib/xlocale/_strings.h b/include/lib/stdlib/xlocale/_strings.h
similarity index 100%
rename from include/stdlib/xlocale/_strings.h
rename to include/lib/stdlib/xlocale/_strings.h
diff --git a/include/stdlib/xlocale/_time.h b/include/lib/stdlib/xlocale/_time.h
similarity index 100%
rename from include/stdlib/xlocale/_time.h
rename to include/lib/stdlib/xlocale/_time.h
diff --git a/include/lib/xlat_tables.h b/include/lib/xlat_tables.h
index abe46ed..7d57521 100644
--- a/include/lib/xlat_tables.h
+++ b/include/lib/xlat_tables.h
@@ -150,12 +150,6 @@
 	MT_MEMORY,
 	/* Values up to 7 are reserved to add new memory types in the future */
 
-	/*
-	 * The following values are organised so that a clear bit gives a more
-	 * restrictive mapping than a set bit, that way a bitwise-and of two
-	 * sets of attributes will never give an attribute which has greater
-	 * access rights than any of the original attributes.
-	 */
 	MT_RO		= 0 << MT_PERM_SHIFT,
 	MT_RW		= 1 << MT_PERM_SHIFT,
 
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 5857501..390721f 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -49,6 +49,9 @@
  * plat_get_rotpk_info() flags
  ******************************************************************************/
 #define ROTPK_IS_HASH			(1 << 0)
+/* Flag used to skip verification of the certificate ROTPK while the platform
+   ROTPK is not deployed */
+#define ROTPK_NOT_DEPLOYED		(1 << 1)
 
 /*******************************************************************************
  * Function declarations
@@ -56,7 +59,9 @@
 /*******************************************************************************
  * Mandatory common functions
  ******************************************************************************/
-uint64_t plat_get_syscnt_freq(void);
+unsigned long long plat_get_syscnt_freq(void) __deprecated;
+unsigned int plat_get_syscnt_freq2(void);
+
 int plat_get_image_source(unsigned int image_id,
 			uintptr_t *dev_handle,
 			uintptr_t *image_spec);
diff --git a/lib/cpus/aarch64/cortex_a57.S b/lib/cpus/aarch64/cortex_a57.S
index 4c0b8ce..60929a0 100644
--- a/lib/cpus/aarch64/cortex_a57.S
+++ b/lib/cpus/aarch64/cortex_a57.S
@@ -167,6 +167,142 @@
 	ret
 endfunc a57_disable_ldnp_overread
 
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #826974.
+	 * This applies only to revision <= r1p1 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Clobbers : x0 - x5
+	 * ---------------------------------------------------
+	 */
+func errata_a57_826974_wa
+	/*
+	 * Compare x0 against revision r1p1
+	 */
+	cmp	x0, #0x11
+	b.ls	apply_826974
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	b	print_revision_warning
+#else
+	ret
+#endif
+apply_826974:
+	mrs	x1, CPUACTLR_EL1
+	orr	x1, x1, #CPUACTLR_DIS_LOAD_PASS_DMB
+	msr	CPUACTLR_EL1, x1
+	ret
+endfunc errata_a57_826974_wa
+
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #826977.
+	 * This applies only to revision <= r1p1 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Clobbers : x0 - x5
+	 * ---------------------------------------------------
+	 */
+func errata_a57_826977_wa
+	/*
+	 * Compare x0 against revision r1p1
+	 */
+	cmp	x0, #0x11
+	b.ls	apply_826977
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	b	print_revision_warning
+#else
+	ret
+#endif
+apply_826977:
+	mrs	x1, CPUACTLR_EL1
+	orr	x1, x1, #CPUACTLR_GRE_NGRE_AS_NGNRE
+	msr	CPUACTLR_EL1, x1
+	ret
+endfunc errata_a57_826977_wa
+
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #828024.
+	 * This applies only to revision <= r1p1 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Clobbers : x0 - x5
+	 * ---------------------------------------------------
+	 */
+func errata_a57_828024_wa
+	/*
+	 * Compare x0 against revision r1p1
+	 */
+	cmp	x0, #0x11
+	b.ls	apply_828024
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	b	print_revision_warning
+#else
+	ret
+#endif
+apply_828024:
+	mrs	x1, CPUACTLR_EL1
+	/*
+	 * Setting the relevant bits in CPUACTLR_EL1 has to be done in 2
+	 * instructions here because the resulting bitmask doesn't fit in a
+	 * 16-bit value so it cannot be encoded in a single instruction.
+	 */
+	orr	x1, x1, #CPUACTLR_NO_ALLOC_WBWA
+	orr	x1, x1, #(CPUACTLR_DIS_L1_STREAMING | CPUACTLR_DIS_STREAMING)
+	msr	CPUACTLR_EL1, x1
+	ret
+endfunc errata_a57_828024_wa
+
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #829520.
+	 * This applies only to revision <= r1p2 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Clobbers : x0 - x5
+	 * ---------------------------------------------------
+	 */
+func errata_a57_829520_wa
+	/*
+	 * Compare x0 against revision r1p2
+	 */
+	cmp	x0, #0x12
+	b.ls	apply_829520
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	b	print_revision_warning
+#else
+	ret
+#endif
+apply_829520:
+	mrs	x1, CPUACTLR_EL1
+	orr	x1, x1, #CPUACTLR_DIS_INDIRECT_PREDICTOR
+	msr	CPUACTLR_EL1, x1
+	ret
+endfunc errata_a57_829520_wa
+
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #833471.
+	 * This applies only to revision <= r1p2 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Clobbers : x0 - x5
+	 * ---------------------------------------------------
+	 */
+func errata_a57_833471_wa
+	/*
+	 * Compare x0 against revision r1p2
+	 */
+	cmp	x0, #0x12
+	b.ls	apply_833471
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	b	print_revision_warning
+#else
+	ret
+#endif
+apply_833471:
+	mrs	x1, CPUACTLR_EL1
+	orr	x1, x1, #CPUACTLR_FORCE_FPSCR_FLUSH
+	msr	CPUACTLR_EL1, x1
+	ret
+endfunc errata_a57_833471_wa
+
 	/* -------------------------------------------------
 	 * The CPU Ops reset function for Cortex-A57.
 	 * Clobbers: x0-x5, x15, x19, x30
@@ -200,6 +336,31 @@
 	bl	a57_disable_ldnp_overread
 #endif
 
+#if ERRATA_A57_826974
+	mov	x0, x15
+	bl	errata_a57_826974_wa
+#endif
+
+#if ERRATA_A57_826977
+	mov	x0, x15
+	bl	errata_a57_826977_wa
+#endif
+
+#if ERRATA_A57_828024
+	mov	x0, x15
+	bl	errata_a57_828024_wa
+#endif
+
+#if ERRATA_A57_829520
+	mov	x0, x15
+	bl	errata_a57_829520_wa
+#endif
+
+#if ERRATA_A57_833471
+	mov	x0, x15
+	bl	errata_a57_833471_wa
+#endif
+
 	/* ---------------------------------------------
 	 * Enable the SMP bit.
 	 * ---------------------------------------------
diff --git a/lib/cpus/aarch64/cortex_a73.S b/lib/cpus/aarch64/cortex_a73.S
new file mode 100644
index 0000000..70b4c6a
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_a73.S
@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <arch.h>
+#include <asm_macros.S>
+#include <bl_common.h>
+#include <cortex_a73.h>
+#include <cpu_macros.S>
+#include <plat_macros.S>
+
+	/* ---------------------------------------------
+	 * Disable L1 data cache
+	 * ---------------------------------------------
+	 */
+func cortex_a73_disable_dcache
+	mrs	x1, sctlr_el3
+	bic	x1, x1, #SCTLR_C_BIT
+	msr	sctlr_el3, x1
+	isb
+	ret
+endfunc cortex_a73_disable_dcache
+
+	/* ---------------------------------------------
+	 * Disable intra-cluster coherency
+	 * ---------------------------------------------
+	 */
+func cortex_a73_disable_smp
+	mrs	x0, CORTEX_A73_CPUECTLR_EL1
+	bic	x0, x0, #CORTEX_A73_CPUECTLR_SMP_BIT
+	msr	CORTEX_A73_CPUECTLR_EL1, x0
+	isb
+	dsb	sy
+	ret
+endfunc cortex_a73_disable_smp
+
+func cortex_a73_reset_func
+	/* ---------------------------------------------
+	 * Enable the SMP bit.
+	 * Clobbers : x0
+	 * ---------------------------------------------
+	 */
+	mrs	x0, CORTEX_A73_CPUECTLR_EL1
+	orr	x0, x0, #CORTEX_A73_CPUECTLR_SMP_BIT
+	msr	CORTEX_A73_CPUECTLR_EL1, x0
+	isb
+	ret
+endfunc cortex_a73_reset_func
+
+func cortex_a73_core_pwr_dwn
+	mov	x18, x30
+
+	/* ---------------------------------------------
+	 * Turn off caches.
+	 * ---------------------------------------------
+	 */
+	bl	cortex_a73_disable_dcache
+
+	/* ---------------------------------------------
+	 * Flush L1 caches.
+	 * ---------------------------------------------
+	 */
+	mov	x0, #DCCISW
+	bl	dcsw_op_level1
+
+	/* ---------------------------------------------
+	 * Come out of intra cluster coherency
+	 * ---------------------------------------------
+	 */
+	mov	x30, x18
+	b	cortex_a73_disable_smp
+endfunc cortex_a73_core_pwr_dwn
+
+func cortex_a73_cluster_pwr_dwn
+	mov	x18, x30
+
+	/* ---------------------------------------------
+	 * Turn off caches.
+	 * ---------------------------------------------
+	 */
+	bl	cortex_a73_disable_dcache
+
+	/* ---------------------------------------------
+	 * Flush L1 caches.
+	 * ---------------------------------------------
+	 */
+	mov	x0, #DCCISW
+	bl	dcsw_op_level1
+
+	/* ---------------------------------------------
+	 * Disable the optional ACP.
+	 * ---------------------------------------------
+	 */
+	bl	plat_disable_acp
+
+	/* ---------------------------------------------
+	 * Flush L2 caches.
+	 * ---------------------------------------------
+	 */
+	mov	x0, #DCCISW
+	bl	dcsw_op_level2
+
+	/* ---------------------------------------------
+	 * Come out of intra cluster coherency
+	 * ---------------------------------------------
+	 */
+	mov	x30, x18
+	b	cortex_a73_disable_smp
+endfunc cortex_a73_cluster_pwr_dwn
+
+	/* ---------------------------------------------
+	 * This function provides cortex_a73 specific
+	 * register information for crash reporting.
+	 * It needs to return with x6 pointing to
+	 * a list of register names in ascii and
+	 * x8 - x15 having values of registers to be
+	 * reported.
+	 * ---------------------------------------------
+	 */
+.section .rodata.cortex_a73_regs, "aS"
+cortex_a73_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func cortex_a73_cpu_reg_dump
+	adr	x6, cortex_a73_regs
+	mrs	x8, CORTEX_A73_CPUECTLR_EL1
+	ret
+endfunc cortex_a73_cpu_reg_dump
+
+declare_cpu_ops cortex_a73, CORTEX_A73_MIDR
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index a3a08e1..0659bff 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -53,26 +53,47 @@
 $(eval $(call add_define,A57_DISABLE_NON_TEMPORAL_HINT))
 
 
-# CPU Errata Build flags. These should be enabled by the
-# platform if the errata needs to be applied.
+# CPU Errata Build flags.
+# These should be enabled by the platform if the erratum workaround needs to be
+# applied.
 
-# Flag to apply errata 826319 during reset. This errata applies only to
-# revision <= r0p2 of the Cortex A53 cpu.
+# Flag to apply erratum 826319 workaround during reset. This erratum applies
+# only to revision <= r0p2 of the Cortex A53 cpu.
 ERRATA_A53_826319	?=0
 
-# Flag to apply errata 836870 during reset. This errata applies only to
-# revision <= r0p3 of the Cortex A53 cpu. From r0p4 and onwards, this
-# errata is enabled by default.
+# Flag to apply erratum 836870 workaround during reset. This erratum applies
+# only to revision <= r0p3 of the Cortex A53 cpu. From r0p4 and onwards, this
+# erratum workaround is enabled by default.
 ERRATA_A53_836870	?=0
 
-# Flag to apply errata 806969 during reset. This errata applies only to
-# revision r0p0 of the Cortex A57 cpu.
+# Flag to apply erratum 806969 workaround during reset. This erratum applies
+# only to revision r0p0 of the Cortex A57 cpu.
 ERRATA_A57_806969	?=0
 
-# Flag to apply errata 813420 during reset. This errata applies only to
-# revision r0p0 of the Cortex A57 cpu.
+# Flag to apply erratum 813420  workaround during reset. This erratum applies
+# only to revision r0p0 of the Cortex A57 cpu.
 ERRATA_A57_813420	?=0
 
+# Flag to apply erratum 826974 workaround during reset. This erratum applies
+# only to revision <= r1p1 of the Cortex A57 cpu.
+ERRATA_A57_826974	?=0
+
+# Flag to apply erratum 826977 workaround during reset. This erratum applies
+# only to revision <= r1p1 of the Cortex A57 cpu.
+ERRATA_A57_826977	?=0
+
+# Flag to apply erratum 828024 workaround during reset. This erratum applies
+# only to revision <= r1p1 of the Cortex A57 cpu.
+ERRATA_A57_828024	?=0
+
+# Flag to apply erratum 829520 workaround during reset. This erratum applies
+# only to revision <= r1p2 of the Cortex A57 cpu.
+ERRATA_A57_829520	?=0
+
+# Flag to apply erratum 833471 workaround during reset. This erratum applies
+# only to revision <= r1p2 of the Cortex A57 cpu.
+ERRATA_A57_833471	?=0
+
 # Process ERRATA_A53_826319 flag
 $(eval $(call assert_boolean,ERRATA_A53_826319))
 $(eval $(call add_define,ERRATA_A53_826319))
@@ -88,3 +109,23 @@
 # Process ERRATA_A57_813420 flag
 $(eval $(call assert_boolean,ERRATA_A57_813420))
 $(eval $(call add_define,ERRATA_A57_813420))
+
+# Process ERRATA_A57_826974 flag
+$(eval $(call assert_boolean,ERRATA_A57_826974))
+$(eval $(call add_define,ERRATA_A57_826974))
+
+# Process ERRATA_A57_826977 flag
+$(eval $(call assert_boolean,ERRATA_A57_826977))
+$(eval $(call add_define,ERRATA_A57_826977))
+
+# Process ERRATA_A57_828024 flag
+$(eval $(call assert_boolean,ERRATA_A57_828024))
+$(eval $(call add_define,ERRATA_A57_828024))
+
+# Process ERRATA_A57_829520 flag
+$(eval $(call assert_boolean,ERRATA_A57_829520))
+$(eval $(call add_define,ERRATA_A57_829520))
+
+# Process ERRATA_A57_833471 flag
+$(eval $(call assert_boolean,ERRATA_A57_833471))
+$(eval $(call add_define,ERRATA_A57_833471))
diff --git a/lib/libfdt/fdt.c b/lib/libfdt/fdt.c
new file mode 100644
index 0000000..2ce6a44
--- /dev/null
+++ b/lib/libfdt/fdt.c
@@ -0,0 +1,250 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_check_header(const void *fdt)
+{
+	if (fdt_magic(fdt) == FDT_MAGIC) {
+		/* Complete tree */
+		if (fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
+			return -FDT_ERR_BADVERSION;
+		if (fdt_last_comp_version(fdt) > FDT_LAST_SUPPORTED_VERSION)
+			return -FDT_ERR_BADVERSION;
+	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
+		/* Unfinished sequential-write blob */
+		if (fdt_size_dt_struct(fdt) == 0)
+			return -FDT_ERR_BADSTATE;
+	} else {
+		return -FDT_ERR_BADMAGIC;
+	}
+
+	return 0;
+}
+
+const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len)
+{
+	const char *p;
+
+	if (fdt_version(fdt) >= 0x11)
+		if (((offset + len) < offset)
+		    || ((offset + len) > fdt_size_dt_struct(fdt)))
+			return NULL;
+
+	p = _fdt_offset_ptr(fdt, offset);
+
+	if (p + len < p)
+		return NULL;
+	return p;
+}
+
+uint32_t fdt_next_tag(const void *fdt, int startoffset, int *nextoffset)
+{
+	const fdt32_t *tagp, *lenp;
+	uint32_t tag;
+	int offset = startoffset;
+	const char *p;
+
+	*nextoffset = -FDT_ERR_TRUNCATED;
+	tagp = fdt_offset_ptr(fdt, offset, FDT_TAGSIZE);
+	if (!tagp)
+		return FDT_END; /* premature end */
+	tag = fdt32_to_cpu(*tagp);
+	offset += FDT_TAGSIZE;
+
+	*nextoffset = -FDT_ERR_BADSTRUCTURE;
+	switch (tag) {
+	case FDT_BEGIN_NODE:
+		/* skip name */
+		do {
+			p = fdt_offset_ptr(fdt, offset++, 1);
+		} while (p && (*p != '\0'));
+		if (!p)
+			return FDT_END; /* premature end */
+		break;
+
+	case FDT_PROP:
+		lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp));
+		if (!lenp)
+			return FDT_END; /* premature end */
+		/* skip-name offset, length and value */
+		offset += sizeof(struct fdt_property) - FDT_TAGSIZE
+			+ fdt32_to_cpu(*lenp);
+		break;
+
+	case FDT_END:
+	case FDT_END_NODE:
+	case FDT_NOP:
+		break;
+
+	default:
+		return FDT_END;
+	}
+
+	if (!fdt_offset_ptr(fdt, startoffset, offset - startoffset))
+		return FDT_END; /* premature end */
+
+	*nextoffset = FDT_TAGALIGN(offset);
+	return tag;
+}
+
+int _fdt_check_node_offset(const void *fdt, int offset)
+{
+	if ((offset < 0) || (offset % FDT_TAGSIZE)
+	    || (fdt_next_tag(fdt, offset, &offset) != FDT_BEGIN_NODE))
+		return -FDT_ERR_BADOFFSET;
+
+	return offset;
+}
+
+int _fdt_check_prop_offset(const void *fdt, int offset)
+{
+	if ((offset < 0) || (offset % FDT_TAGSIZE)
+	    || (fdt_next_tag(fdt, offset, &offset) != FDT_PROP))
+		return -FDT_ERR_BADOFFSET;
+
+	return offset;
+}
+
+int fdt_next_node(const void *fdt, int offset, int *depth)
+{
+	int nextoffset = 0;
+	uint32_t tag;
+
+	if (offset >= 0)
+		if ((nextoffset = _fdt_check_node_offset(fdt, offset)) < 0)
+			return nextoffset;
+
+	do {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+
+		switch (tag) {
+		case FDT_PROP:
+		case FDT_NOP:
+			break;
+
+		case FDT_BEGIN_NODE:
+			if (depth)
+				(*depth)++;
+			break;
+
+		case FDT_END_NODE:
+			if (depth && ((--(*depth)) < 0))
+				return nextoffset;
+			break;
+
+		case FDT_END:
+			if ((nextoffset >= 0)
+			    || ((nextoffset == -FDT_ERR_TRUNCATED) && !depth))
+				return -FDT_ERR_NOTFOUND;
+			else
+				return nextoffset;
+		}
+	} while (tag != FDT_BEGIN_NODE);
+
+	return offset;
+}
+
+int fdt_first_subnode(const void *fdt, int offset)
+{
+	int depth = 0;
+
+	offset = fdt_next_node(fdt, offset, &depth);
+	if (offset < 0 || depth != 1)
+		return -FDT_ERR_NOTFOUND;
+
+	return offset;
+}
+
+int fdt_next_subnode(const void *fdt, int offset)
+{
+	int depth = 1;
+
+	/*
+	 * With respect to the parent, the depth of the next subnode will be
+	 * the same as the last.
+	 */
+	do {
+		offset = fdt_next_node(fdt, offset, &depth);
+		if (offset < 0 || depth < 1)
+			return -FDT_ERR_NOTFOUND;
+	} while (depth > 1);
+
+	return offset;
+}
+
+const char *_fdt_find_string(const char *strtab, int tabsize, const char *s)
+{
+	int len = strlen(s) + 1;
+	const char *last = strtab + tabsize - len;
+	const char *p;
+
+	for (p = strtab; p <= last; p++)
+		if (memcmp(p, s, len) == 0)
+			return p;
+	return NULL;
+}
+
+int fdt_move(const void *fdt, void *buf, int bufsize)
+{
+	FDT_CHECK_HEADER(fdt);
+
+	if (fdt_totalsize(fdt) > bufsize)
+		return -FDT_ERR_NOSPACE;
+
+	memmove(buf, fdt, fdt_totalsize(fdt));
+	return 0;
+}
diff --git a/lib/libfdt/fdt_addresses.c b/lib/libfdt/fdt_addresses.c
new file mode 100644
index 0000000..eff4dbc
--- /dev/null
+++ b/lib/libfdt/fdt_addresses.c
@@ -0,0 +1,96 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2014 David Gibson <david@gibson.dropbear.id.au>
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_address_cells(const void *fdt, int nodeoffset)
+{
+	const fdt32_t *ac;
+	int val;
+	int len;
+
+	ac = fdt_getprop(fdt, nodeoffset, "#address-cells", &len);
+	if (!ac)
+		return 2;
+
+	if (len != sizeof(*ac))
+		return -FDT_ERR_BADNCELLS;
+
+	val = fdt32_to_cpu(*ac);
+	if ((val <= 0) || (val > FDT_MAX_NCELLS))
+		return -FDT_ERR_BADNCELLS;
+
+	return val;
+}
+
+int fdt_size_cells(const void *fdt, int nodeoffset)
+{
+	const fdt32_t *sc;
+	int val;
+	int len;
+
+	sc = fdt_getprop(fdt, nodeoffset, "#size-cells", &len);
+	if (!sc)
+		return 2;
+
+	if (len != sizeof(*sc))
+		return -FDT_ERR_BADNCELLS;
+
+	val = fdt32_to_cpu(*sc);
+	if ((val < 0) || (val > FDT_MAX_NCELLS))
+		return -FDT_ERR_BADNCELLS;
+
+	return val;
+}
diff --git a/lib/libfdt/fdt_empty_tree.c b/lib/libfdt/fdt_empty_tree.c
new file mode 100644
index 0000000..f72d13b
--- /dev/null
+++ b/lib/libfdt/fdt_empty_tree.c
@@ -0,0 +1,84 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2012 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_create_empty_tree(void *buf, int bufsize)
+{
+	int err;
+
+	err = fdt_create(buf, bufsize);
+	if (err)
+		return err;
+
+	err = fdt_finish_reservemap(buf);
+	if (err)
+		return err;
+
+	err = fdt_begin_node(buf, "");
+	if (err)
+		return err;
+
+	err =  fdt_end_node(buf);
+	if (err)
+		return err;
+
+	err = fdt_finish(buf);
+	if (err)
+		return err;
+
+	return fdt_open_into(buf, buf, bufsize);
+}
+
diff --git a/lib/libfdt/fdt_ro.c b/lib/libfdt/fdt_ro.c
new file mode 100644
index 0000000..50007f6
--- /dev/null
+++ b/lib/libfdt/fdt_ro.c
@@ -0,0 +1,573 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int _fdt_nodename_eq(const void *fdt, int offset,
+			    const char *s, int len)
+{
+	const char *p = fdt_offset_ptr(fdt, offset + FDT_TAGSIZE, len+1);
+
+	if (! p)
+		/* short match */
+		return 0;
+
+	if (memcmp(p, s, len) != 0)
+		return 0;
+
+	if (p[len] == '\0')
+		return 1;
+	else if (!memchr(s, '@', len) && (p[len] == '@'))
+		return 1;
+	else
+		return 0;
+}
+
+const char *fdt_string(const void *fdt, int stroffset)
+{
+	return (const char *)fdt + fdt_off_dt_strings(fdt) + stroffset;
+}
+
+static int _fdt_string_eq(const void *fdt, int stroffset,
+			  const char *s, int len)
+{
+	const char *p = fdt_string(fdt, stroffset);
+
+	return (strlen(p) == len) && (memcmp(p, s, len) == 0);
+}
+
+int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size)
+{
+	FDT_CHECK_HEADER(fdt);
+	*address = fdt64_to_cpu(_fdt_mem_rsv(fdt, n)->address);
+	*size = fdt64_to_cpu(_fdt_mem_rsv(fdt, n)->size);
+	return 0;
+}
+
+int fdt_num_mem_rsv(const void *fdt)
+{
+	int i = 0;
+
+	while (fdt64_to_cpu(_fdt_mem_rsv(fdt, i)->size) != 0)
+		i++;
+	return i;
+}
+
+static int _nextprop(const void *fdt, int offset)
+{
+	uint32_t tag;
+	int nextoffset;
+
+	do {
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+
+		switch (tag) {
+		case FDT_END:
+			if (nextoffset >= 0)
+				return -FDT_ERR_BADSTRUCTURE;
+			else
+				return nextoffset;
+
+		case FDT_PROP:
+			return offset;
+		}
+		offset = nextoffset;
+	} while (tag == FDT_NOP);
+
+	return -FDT_ERR_NOTFOUND;
+}
+
+int fdt_subnode_offset_namelen(const void *fdt, int offset,
+			       const char *name, int namelen)
+{
+	int depth;
+
+	FDT_CHECK_HEADER(fdt);
+
+	for (depth = 0;
+	     (offset >= 0) && (depth >= 0);
+	     offset = fdt_next_node(fdt, offset, &depth))
+		if ((depth == 1)
+		    && _fdt_nodename_eq(fdt, offset, name, namelen))
+			return offset;
+
+	if (depth < 0)
+		return -FDT_ERR_NOTFOUND;
+	return offset; /* error */
+}
+
+int fdt_subnode_offset(const void *fdt, int parentoffset,
+		       const char *name)
+{
+	return fdt_subnode_offset_namelen(fdt, parentoffset, name, strlen(name));
+}
+
+int fdt_path_offset(const void *fdt, const char *path)
+{
+	const char *end = path + strlen(path);
+	const char *p = path;
+	int offset = 0;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* see if we have an alias */
+	if (*path != '/') {
+		const char *q = strchr(path, '/');
+
+		if (!q)
+			q = end;
+
+		p = fdt_get_alias_namelen(fdt, p, q - p);
+		if (!p)
+			return -FDT_ERR_BADPATH;
+		offset = fdt_path_offset(fdt, p);
+
+		p = q;
+	}
+
+	while (*p) {
+		const char *q;
+
+		while (*p == '/')
+			p++;
+		if (! *p)
+			return offset;
+		q = strchr(p, '/');
+		if (! q)
+			q = end;
+
+		offset = fdt_subnode_offset_namelen(fdt, offset, p, q-p);
+		if (offset < 0)
+			return offset;
+
+		p = q;
+	}
+
+	return offset;
+}
+
+const char *fdt_get_name(const void *fdt, int nodeoffset, int *len)
+{
+	const struct fdt_node_header *nh = _fdt_offset_ptr(fdt, nodeoffset);
+	int err;
+
+	if (((err = fdt_check_header(fdt)) != 0)
+	    || ((err = _fdt_check_node_offset(fdt, nodeoffset)) < 0))
+			goto fail;
+
+	if (len)
+		*len = strlen(nh->name);
+
+	return nh->name;
+
+ fail:
+	if (len)
+		*len = err;
+	return NULL;
+}
+
+int fdt_first_property_offset(const void *fdt, int nodeoffset)
+{
+	int offset;
+
+	if ((offset = _fdt_check_node_offset(fdt, nodeoffset)) < 0)
+		return offset;
+
+	return _nextprop(fdt, offset);
+}
+
+int fdt_next_property_offset(const void *fdt, int offset)
+{
+	if ((offset = _fdt_check_prop_offset(fdt, offset)) < 0)
+		return offset;
+
+	return _nextprop(fdt, offset);
+}
+
+const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
+						      int offset,
+						      int *lenp)
+{
+	int err;
+	const struct fdt_property *prop;
+
+	if ((err = _fdt_check_prop_offset(fdt, offset)) < 0) {
+		if (lenp)
+			*lenp = err;
+		return NULL;
+	}
+
+	prop = _fdt_offset_ptr(fdt, offset);
+
+	if (lenp)
+		*lenp = fdt32_to_cpu(prop->len);
+
+	return prop;
+}
+
+const struct fdt_property *fdt_get_property_namelen(const void *fdt,
+						    int offset,
+						    const char *name,
+						    int namelen, int *lenp)
+{
+	for (offset = fdt_first_property_offset(fdt, offset);
+	     (offset >= 0);
+	     (offset = fdt_next_property_offset(fdt, offset))) {
+		const struct fdt_property *prop;
+
+		if (!(prop = fdt_get_property_by_offset(fdt, offset, lenp))) {
+			offset = -FDT_ERR_INTERNAL;
+			break;
+		}
+		if (_fdt_string_eq(fdt, fdt32_to_cpu(prop->nameoff),
+				   name, namelen))
+			return prop;
+	}
+
+	if (lenp)
+		*lenp = offset;
+	return NULL;
+}
+
+const struct fdt_property *fdt_get_property(const void *fdt,
+					    int nodeoffset,
+					    const char *name, int *lenp)
+{
+	return fdt_get_property_namelen(fdt, nodeoffset, name,
+					strlen(name), lenp);
+}
+
+const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
+				const char *name, int namelen, int *lenp)
+{
+	const struct fdt_property *prop;
+
+	prop = fdt_get_property_namelen(fdt, nodeoffset, name, namelen, lenp);
+	if (! prop)
+		return NULL;
+
+	return prop->data;
+}
+
+const void *fdt_getprop_by_offset(const void *fdt, int offset,
+				  const char **namep, int *lenp)
+{
+	const struct fdt_property *prop;
+
+	prop = fdt_get_property_by_offset(fdt, offset, lenp);
+	if (!prop)
+		return NULL;
+	if (namep)
+		*namep = fdt_string(fdt, fdt32_to_cpu(prop->nameoff));
+	return prop->data;
+}
+
+const void *fdt_getprop(const void *fdt, int nodeoffset,
+			const char *name, int *lenp)
+{
+	return fdt_getprop_namelen(fdt, nodeoffset, name, strlen(name), lenp);
+}
+
+uint32_t fdt_get_phandle(const void *fdt, int nodeoffset)
+{
+	const fdt32_t *php;
+	int len;
+
+	/* FIXME: This is a bit sub-optimal, since we potentially scan
+	 * over all the properties twice. */
+	php = fdt_getprop(fdt, nodeoffset, "phandle", &len);
+	if (!php || (len != sizeof(*php))) {
+		php = fdt_getprop(fdt, nodeoffset, "linux,phandle", &len);
+		if (!php || (len != sizeof(*php)))
+			return 0;
+	}
+
+	return fdt32_to_cpu(*php);
+}
+
+const char *fdt_get_alias_namelen(const void *fdt,
+				  const char *name, int namelen)
+{
+	int aliasoffset;
+
+	aliasoffset = fdt_path_offset(fdt, "/aliases");
+	if (aliasoffset < 0)
+		return NULL;
+
+	return fdt_getprop_namelen(fdt, aliasoffset, name, namelen, NULL);
+}
+
+const char *fdt_get_alias(const void *fdt, const char *name)
+{
+	return fdt_get_alias_namelen(fdt, name, strlen(name));
+}
+
+int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen)
+{
+	int pdepth = 0, p = 0;
+	int offset, depth, namelen;
+	const char *name;
+
+	FDT_CHECK_HEADER(fdt);
+
+	if (buflen < 2)
+		return -FDT_ERR_NOSPACE;
+
+	for (offset = 0, depth = 0;
+	     (offset >= 0) && (offset <= nodeoffset);
+	     offset = fdt_next_node(fdt, offset, &depth)) {
+		while (pdepth > depth) {
+			do {
+				p--;
+			} while (buf[p-1] != '/');
+			pdepth--;
+		}
+
+		if (pdepth >= depth) {
+			name = fdt_get_name(fdt, offset, &namelen);
+			if (!name)
+				return namelen;
+			if ((p + namelen + 1) <= buflen) {
+				memcpy(buf + p, name, namelen);
+				p += namelen;
+				buf[p++] = '/';
+				pdepth++;
+			}
+		}
+
+		if (offset == nodeoffset) {
+			if (pdepth < (depth + 1))
+				return -FDT_ERR_NOSPACE;
+
+			if (p > 1) /* special case so that root path is "/", not "" */
+				p--;
+			buf[p] = '\0';
+			return 0;
+		}
+	}
+
+	if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
+		return -FDT_ERR_BADOFFSET;
+	else if (offset == -FDT_ERR_BADOFFSET)
+		return -FDT_ERR_BADSTRUCTURE;
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
+				 int supernodedepth, int *nodedepth)
+{
+	int offset, depth;
+	int supernodeoffset = -FDT_ERR_INTERNAL;
+
+	FDT_CHECK_HEADER(fdt);
+
+	if (supernodedepth < 0)
+		return -FDT_ERR_NOTFOUND;
+
+	for (offset = 0, depth = 0;
+	     (offset >= 0) && (offset <= nodeoffset);
+	     offset = fdt_next_node(fdt, offset, &depth)) {
+		if (depth == supernodedepth)
+			supernodeoffset = offset;
+
+		if (offset == nodeoffset) {
+			if (nodedepth)
+				*nodedepth = depth;
+
+			if (supernodedepth > depth)
+				return -FDT_ERR_NOTFOUND;
+			else
+				return supernodeoffset;
+		}
+	}
+
+	if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
+		return -FDT_ERR_BADOFFSET;
+	else if (offset == -FDT_ERR_BADOFFSET)
+		return -FDT_ERR_BADSTRUCTURE;
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_node_depth(const void *fdt, int nodeoffset)
+{
+	int nodedepth;
+	int err;
+
+	err = fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, &nodedepth);
+	if (err)
+		return (err < 0) ? err : -FDT_ERR_INTERNAL;
+	return nodedepth;
+}
+
+int fdt_parent_offset(const void *fdt, int nodeoffset)
+{
+	int nodedepth = fdt_node_depth(fdt, nodeoffset);
+
+	if (nodedepth < 0)
+		return nodedepth;
+	return fdt_supernode_atdepth_offset(fdt, nodeoffset,
+					    nodedepth - 1, NULL);
+}
+
+int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
+				  const char *propname,
+				  const void *propval, int proplen)
+{
+	int offset;
+	const void *val;
+	int len;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we scan each
+	 * property of a node in fdt_getprop(), then if that didn't
+	 * find what we want, we scan over them again making our way
+	 * to the next node.  Still it's the easiest to implement
+	 * approach; performance can come later. */
+	for (offset = fdt_next_node(fdt, startoffset, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		val = fdt_getprop(fdt, offset, propname, &len);
+		if (val && (len == proplen)
+		    && (memcmp(val, propval, len) == 0))
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle)
+{
+	int offset;
+
+	if ((phandle == 0) || (phandle == -1))
+		return -FDT_ERR_BADPHANDLE;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we
+	 * potentially scan each property of a node in
+	 * fdt_get_phandle(), then if that didn't find what
+	 * we want, we scan over them again making our way to the next
+	 * node.  Still it's the easiest to implement approach;
+	 * performance can come later. */
+	for (offset = fdt_next_node(fdt, -1, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		if (fdt_get_phandle(fdt, offset) == phandle)
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_stringlist_contains(const char *strlist, int listlen, const char *str)
+{
+	int len = strlen(str);
+	const char *p;
+
+	while (listlen >= len) {
+		if (memcmp(str, strlist, len+1) == 0)
+			return 1;
+		p = memchr(strlist, '\0', listlen);
+		if (!p)
+			return 0; /* malformed strlist.. */
+		listlen -= (p-strlist) + 1;
+		strlist = p + 1;
+	}
+	return 0;
+}
+
+int fdt_node_check_compatible(const void *fdt, int nodeoffset,
+			      const char *compatible)
+{
+	const void *prop;
+	int len;
+
+	prop = fdt_getprop(fdt, nodeoffset, "compatible", &len);
+	if (!prop)
+		return len;
+	if (fdt_stringlist_contains(prop, len, compatible))
+		return 0;
+	else
+		return 1;
+}
+
+int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
+				  const char *compatible)
+{
+	int offset, err;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we scan each
+	 * property of a node in fdt_node_check_compatible(), then if
+	 * that didn't find what we want, we scan over them again
+	 * making our way to the next node.  Still it's the easiest to
+	 * implement approach; performance can come later. */
+	for (offset = fdt_next_node(fdt, startoffset, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		err = fdt_node_check_compatible(fdt, offset, compatible);
+		if ((err < 0) && (err != -FDT_ERR_NOTFOUND))
+			return err;
+		else if (err == 0)
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
diff --git a/lib/libfdt/fdt_rw.c b/lib/libfdt/fdt_rw.c
new file mode 100644
index 0000000..70adec6
--- /dev/null
+++ b/lib/libfdt/fdt_rw.c
@@ -0,0 +1,492 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int _fdt_blocks_misordered(const void *fdt,
+			      int mem_rsv_size, int struct_size)
+{
+	return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
+		|| (fdt_off_dt_struct(fdt) <
+		    (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
+		|| (fdt_off_dt_strings(fdt) <
+		    (fdt_off_dt_struct(fdt) + struct_size))
+		|| (fdt_totalsize(fdt) <
+		    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
+}
+
+static int _fdt_rw_check_header(void *fdt)
+{
+	FDT_CHECK_HEADER(fdt);
+
+	if (fdt_version(fdt) < 17)
+		return -FDT_ERR_BADVERSION;
+	if (_fdt_blocks_misordered(fdt, sizeof(struct fdt_reserve_entry),
+				   fdt_size_dt_struct(fdt)))
+		return -FDT_ERR_BADLAYOUT;
+	if (fdt_version(fdt) > 17)
+		fdt_set_version(fdt, 17);
+
+	return 0;
+}
+
+#define FDT_RW_CHECK_HEADER(fdt) \
+	{ \
+		int __err; \
+		if ((__err = _fdt_rw_check_header(fdt)) != 0) \
+			return __err; \
+	}
+
+static inline int _fdt_data_size(void *fdt)
+{
+	return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
+}
+
+static int _fdt_splice(void *fdt, void *splicepoint, int oldlen, int newlen)
+{
+	char *p = splicepoint;
+	char *end = (char *)fdt + _fdt_data_size(fdt);
+
+	if (((p + oldlen) < p) || ((p + oldlen) > end))
+		return -FDT_ERR_BADOFFSET;
+	if ((end - oldlen + newlen) > ((char *)fdt + fdt_totalsize(fdt)))
+		return -FDT_ERR_NOSPACE;
+	memmove(p + newlen, p + oldlen, end - p - oldlen);
+	return 0;
+}
+
+static int _fdt_splice_mem_rsv(void *fdt, struct fdt_reserve_entry *p,
+			       int oldn, int newn)
+{
+	int delta = (newn - oldn) * sizeof(*p);
+	int err;
+	err = _fdt_splice(fdt, p, oldn * sizeof(*p), newn * sizeof(*p));
+	if (err)
+		return err;
+	fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta);
+	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
+	return 0;
+}
+
+static int _fdt_splice_struct(void *fdt, void *p,
+			      int oldlen, int newlen)
+{
+	int delta = newlen - oldlen;
+	int err;
+
+	if ((err = _fdt_splice(fdt, p, oldlen, newlen)))
+		return err;
+
+	fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta);
+	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
+	return 0;
+}
+
+static int _fdt_splice_string(void *fdt, int newlen)
+{
+	void *p = (char *)fdt
+		+ fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
+	int err;
+
+	if ((err = _fdt_splice(fdt, p, 0, newlen)))
+		return err;
+
+	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen);
+	return 0;
+}
+
+static int _fdt_find_add_string(void *fdt, const char *s)
+{
+	char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
+	const char *p;
+	char *new;
+	int len = strlen(s) + 1;
+	int err;
+
+	p = _fdt_find_string(strtab, fdt_size_dt_strings(fdt), s);
+	if (p)
+		/* found it */
+		return (p - strtab);
+
+	new = strtab + fdt_size_dt_strings(fdt);
+	err = _fdt_splice_string(fdt, len);
+	if (err)
+		return err;
+
+	memcpy(new, s, len);
+	return (new - strtab);
+}
+
+int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
+{
+	struct fdt_reserve_entry *re;
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	re = _fdt_mem_rsv_w(fdt, fdt_num_mem_rsv(fdt));
+	err = _fdt_splice_mem_rsv(fdt, re, 0, 1);
+	if (err)
+		return err;
+
+	re->address = cpu_to_fdt64(address);
+	re->size = cpu_to_fdt64(size);
+	return 0;
+}
+
+int fdt_del_mem_rsv(void *fdt, int n)
+{
+	struct fdt_reserve_entry *re = _fdt_mem_rsv_w(fdt, n);
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	if (n >= fdt_num_mem_rsv(fdt))
+		return -FDT_ERR_NOTFOUND;
+
+	err = _fdt_splice_mem_rsv(fdt, re, 1, 0);
+	if (err)
+		return err;
+	return 0;
+}
+
+static int _fdt_resize_property(void *fdt, int nodeoffset, const char *name,
+				int len, struct fdt_property **prop)
+{
+	int oldlen;
+	int err;
+
+	*prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
+	if (! (*prop))
+		return oldlen;
+
+	if ((err = _fdt_splice_struct(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
+				      FDT_TAGALIGN(len))))
+		return err;
+
+	(*prop)->len = cpu_to_fdt32(len);
+	return 0;
+}
+
+static int _fdt_add_property(void *fdt, int nodeoffset, const char *name,
+			     int len, struct fdt_property **prop)
+{
+	int proplen;
+	int nextoffset;
+	int namestroff;
+	int err;
+
+	if ((nextoffset = _fdt_check_node_offset(fdt, nodeoffset)) < 0)
+		return nextoffset;
+
+	namestroff = _fdt_find_add_string(fdt, name);
+	if (namestroff < 0)
+		return namestroff;
+
+	*prop = _fdt_offset_ptr_w(fdt, nextoffset);
+	proplen = sizeof(**prop) + FDT_TAGALIGN(len);
+
+	err = _fdt_splice_struct(fdt, *prop, 0, proplen);
+	if (err)
+		return err;
+
+	(*prop)->tag = cpu_to_fdt32(FDT_PROP);
+	(*prop)->nameoff = cpu_to_fdt32(namestroff);
+	(*prop)->len = cpu_to_fdt32(len);
+	return 0;
+}
+
+int fdt_set_name(void *fdt, int nodeoffset, const char *name)
+{
+	char *namep;
+	int oldlen, newlen;
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
+	if (!namep)
+		return oldlen;
+
+	newlen = strlen(name);
+
+	err = _fdt_splice_struct(fdt, namep, FDT_TAGALIGN(oldlen+1),
+				 FDT_TAGALIGN(newlen+1));
+	if (err)
+		return err;
+
+	memcpy(namep, name, newlen+1);
+	return 0;
+}
+
+int fdt_setprop(void *fdt, int nodeoffset, const char *name,
+		const void *val, int len)
+{
+	struct fdt_property *prop;
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	err = _fdt_resize_property(fdt, nodeoffset, name, len, &prop);
+	if (err == -FDT_ERR_NOTFOUND)
+		err = _fdt_add_property(fdt, nodeoffset, name, len, &prop);
+	if (err)
+		return err;
+
+	memcpy(prop->data, val, len);
+	return 0;
+}
+
+int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
+		   const void *val, int len)
+{
+	struct fdt_property *prop;
+	int err, oldlen, newlen;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
+	if (prop) {
+		newlen = len + oldlen;
+		err = _fdt_splice_struct(fdt, prop->data,
+					 FDT_TAGALIGN(oldlen),
+					 FDT_TAGALIGN(newlen));
+		if (err)
+			return err;
+		prop->len = cpu_to_fdt32(newlen);
+		memcpy(prop->data + oldlen, val, len);
+	} else {
+		err = _fdt_add_property(fdt, nodeoffset, name, len, &prop);
+		if (err)
+			return err;
+		memcpy(prop->data, val, len);
+	}
+	return 0;
+}
+
+int fdt_delprop(void *fdt, int nodeoffset, const char *name)
+{
+	struct fdt_property *prop;
+	int len, proplen;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
+	if (! prop)
+		return len;
+
+	proplen = sizeof(*prop) + FDT_TAGALIGN(len);
+	return _fdt_splice_struct(fdt, prop, proplen, 0);
+}
+
+int fdt_add_subnode_namelen(void *fdt, int parentoffset,
+			    const char *name, int namelen)
+{
+	struct fdt_node_header *nh;
+	int offset, nextoffset;
+	int nodelen;
+	int err;
+	uint32_t tag;
+	fdt32_t *endtag;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
+	if (offset >= 0)
+		return -FDT_ERR_EXISTS;
+	else if (offset != -FDT_ERR_NOTFOUND)
+		return offset;
+
+	/* Try to place the new node after the parent's properties */
+	fdt_next_tag(fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */
+	do {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+	} while ((tag == FDT_PROP) || (tag == FDT_NOP));
+
+	nh = _fdt_offset_ptr_w(fdt, offset);
+	nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
+
+	err = _fdt_splice_struct(fdt, nh, 0, nodelen);
+	if (err)
+		return err;
+
+	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
+	memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
+	memcpy(nh->name, name, namelen);
+	endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
+	*endtag = cpu_to_fdt32(FDT_END_NODE);
+
+	return offset;
+}
+
+int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
+{
+	return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
+}
+
+int fdt_del_node(void *fdt, int nodeoffset)
+{
+	int endoffset;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	endoffset = _fdt_node_end_offset(fdt, nodeoffset);
+	if (endoffset < 0)
+		return endoffset;
+
+	return _fdt_splice_struct(fdt, _fdt_offset_ptr_w(fdt, nodeoffset),
+				  endoffset - nodeoffset, 0);
+}
+
+static void _fdt_packblocks(const char *old, char *new,
+			    int mem_rsv_size, int struct_size)
+{
+	int mem_rsv_off, struct_off, strings_off;
+
+	mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
+	struct_off = mem_rsv_off + mem_rsv_size;
+	strings_off = struct_off + struct_size;
+
+	memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
+	fdt_set_off_mem_rsvmap(new, mem_rsv_off);
+
+	memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
+	fdt_set_off_dt_struct(new, struct_off);
+	fdt_set_size_dt_struct(new, struct_size);
+
+	memmove(new + strings_off, old + fdt_off_dt_strings(old),
+		fdt_size_dt_strings(old));
+	fdt_set_off_dt_strings(new, strings_off);
+	fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
+}
+
+int fdt_open_into(const void *fdt, void *buf, int bufsize)
+{
+	int err;
+	int mem_rsv_size, struct_size;
+	int newsize;
+	const char *fdtstart = fdt;
+	const char *fdtend = fdtstart + fdt_totalsize(fdt);
+	char *tmp;
+
+	FDT_CHECK_HEADER(fdt);
+
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
+
+	if (fdt_version(fdt) >= 17) {
+		struct_size = fdt_size_dt_struct(fdt);
+	} else {
+		struct_size = 0;
+		while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
+			;
+		if (struct_size < 0)
+			return struct_size;
+	}
+
+	if (!_fdt_blocks_misordered(fdt, mem_rsv_size, struct_size)) {
+		/* no further work necessary */
+		err = fdt_move(fdt, buf, bufsize);
+		if (err)
+			return err;
+		fdt_set_version(buf, 17);
+		fdt_set_size_dt_struct(buf, struct_size);
+		fdt_set_totalsize(buf, bufsize);
+		return 0;
+	}
+
+	/* Need to reorder */
+	newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
+		+ struct_size + fdt_size_dt_strings(fdt);
+
+	if (bufsize < newsize)
+		return -FDT_ERR_NOSPACE;
+
+	/* First attempt to build converted tree at beginning of buffer */
+	tmp = buf;
+	/* But if that overlaps with the old tree... */
+	if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
+		/* Try right after the old tree instead */
+		tmp = (char *)(uintptr_t)fdtend;
+		if ((tmp + newsize) > ((char *)buf + bufsize))
+			return -FDT_ERR_NOSPACE;
+	}
+
+	_fdt_packblocks(fdt, tmp, mem_rsv_size, struct_size);
+	memmove(buf, tmp, newsize);
+
+	fdt_set_magic(buf, FDT_MAGIC);
+	fdt_set_totalsize(buf, bufsize);
+	fdt_set_version(buf, 17);
+	fdt_set_last_comp_version(buf, 16);
+	fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
+
+	return 0;
+}
+
+int fdt_pack(void *fdt)
+{
+	int mem_rsv_size;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
+	_fdt_packblocks(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt));
+	fdt_set_totalsize(fdt, _fdt_data_size(fdt));
+
+	return 0;
+}
diff --git a/lib/libfdt/fdt_strerror.c b/lib/libfdt/fdt_strerror.c
new file mode 100644
index 0000000..e6c3cee
--- /dev/null
+++ b/lib/libfdt/fdt_strerror.c
@@ -0,0 +1,96 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+struct fdt_errtabent {
+	const char *str;
+};
+
+#define FDT_ERRTABENT(val) \
+	[(val)] = { .str = #val, }
+
+static struct fdt_errtabent fdt_errtable[] = {
+	FDT_ERRTABENT(FDT_ERR_NOTFOUND),
+	FDT_ERRTABENT(FDT_ERR_EXISTS),
+	FDT_ERRTABENT(FDT_ERR_NOSPACE),
+
+	FDT_ERRTABENT(FDT_ERR_BADOFFSET),
+	FDT_ERRTABENT(FDT_ERR_BADPATH),
+	FDT_ERRTABENT(FDT_ERR_BADSTATE),
+
+	FDT_ERRTABENT(FDT_ERR_TRUNCATED),
+	FDT_ERRTABENT(FDT_ERR_BADMAGIC),
+	FDT_ERRTABENT(FDT_ERR_BADVERSION),
+	FDT_ERRTABENT(FDT_ERR_BADSTRUCTURE),
+	FDT_ERRTABENT(FDT_ERR_BADLAYOUT),
+};
+#define FDT_ERRTABSIZE	(sizeof(fdt_errtable) / sizeof(fdt_errtable[0]))
+
+const char *fdt_strerror(int errval)
+{
+	if (errval > 0)
+		return "<valid offset/length>";
+	else if (errval == 0)
+		return "<no error>";
+	else if (errval > -FDT_ERRTABSIZE) {
+		const char *s = fdt_errtable[-errval].str;
+
+		if (s)
+			return s;
+	}
+
+	return "<unknown error>";
+}
diff --git a/lib/libfdt/fdt_sw.c b/lib/libfdt/fdt_sw.c
new file mode 100644
index 0000000..6a80485
--- /dev/null
+++ b/lib/libfdt/fdt_sw.c
@@ -0,0 +1,288 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int _fdt_sw_check_header(void *fdt)
+{
+	if (fdt_magic(fdt) != FDT_SW_MAGIC)
+		return -FDT_ERR_BADMAGIC;
+	/* FIXME: should check more details about the header state */
+	return 0;
+}
+
+#define FDT_SW_CHECK_HEADER(fdt) \
+	{ \
+		int err; \
+		if ((err = _fdt_sw_check_header(fdt)) != 0) \
+			return err; \
+	}
+
+static void *_fdt_grab_space(void *fdt, size_t len)
+{
+	int offset = fdt_size_dt_struct(fdt);
+	int spaceleft;
+
+	spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
+		- fdt_size_dt_strings(fdt);
+
+	if ((offset + len < offset) || (offset + len > spaceleft))
+		return NULL;
+
+	fdt_set_size_dt_struct(fdt, offset + len);
+	return _fdt_offset_ptr_w(fdt, offset);
+}
+
+int fdt_create(void *buf, int bufsize)
+{
+	void *fdt = buf;
+
+	if (bufsize < sizeof(struct fdt_header))
+		return -FDT_ERR_NOSPACE;
+
+	memset(buf, 0, bufsize);
+
+	fdt_set_magic(fdt, FDT_SW_MAGIC);
+	fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
+	fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
+	fdt_set_totalsize(fdt,  bufsize);
+
+	fdt_set_off_mem_rsvmap(fdt, FDT_ALIGN(sizeof(struct fdt_header),
+					      sizeof(struct fdt_reserve_entry)));
+	fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
+	fdt_set_off_dt_strings(fdt, bufsize);
+
+	return 0;
+}
+
+int fdt_resize(void *fdt, void *buf, int bufsize)
+{
+	size_t headsize, tailsize;
+	char *oldtail, *newtail;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	headsize = fdt_off_dt_struct(fdt);
+	tailsize = fdt_size_dt_strings(fdt);
+
+	if ((headsize + tailsize) > bufsize)
+		return -FDT_ERR_NOSPACE;
+
+	oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
+	newtail = (char *)buf + bufsize - tailsize;
+
+	/* Two cases to avoid clobbering data if the old and new
+	 * buffers partially overlap */
+	if (buf <= fdt) {
+		memmove(buf, fdt, headsize);
+		memmove(newtail, oldtail, tailsize);
+	} else {
+		memmove(newtail, oldtail, tailsize);
+		memmove(buf, fdt, headsize);
+	}
+
+	fdt_set_off_dt_strings(buf, bufsize);
+	fdt_set_totalsize(buf, bufsize);
+
+	return 0;
+}
+
+int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
+{
+	struct fdt_reserve_entry *re;
+	int offset;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	if (fdt_size_dt_struct(fdt))
+		return -FDT_ERR_BADSTATE;
+
+	offset = fdt_off_dt_struct(fdt);
+	if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
+		return -FDT_ERR_NOSPACE;
+
+	re = (struct fdt_reserve_entry *)((char *)fdt + offset);
+	re->address = cpu_to_fdt64(addr);
+	re->size = cpu_to_fdt64(size);
+
+	fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
+
+	return 0;
+}
+
+int fdt_finish_reservemap(void *fdt)
+{
+	return fdt_add_reservemap_entry(fdt, 0, 0);
+}
+
+int fdt_begin_node(void *fdt, const char *name)
+{
+	struct fdt_node_header *nh;
+	int namelen = strlen(name) + 1;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	nh = _fdt_grab_space(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
+	if (! nh)
+		return -FDT_ERR_NOSPACE;
+
+	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
+	memcpy(nh->name, name, namelen);
+	return 0;
+}
+
+int fdt_end_node(void *fdt)
+{
+	fdt32_t *en;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	en = _fdt_grab_space(fdt, FDT_TAGSIZE);
+	if (! en)
+		return -FDT_ERR_NOSPACE;
+
+	*en = cpu_to_fdt32(FDT_END_NODE);
+	return 0;
+}
+
+static int _fdt_find_add_string(void *fdt, const char *s)
+{
+	char *strtab = (char *)fdt + fdt_totalsize(fdt);
+	const char *p;
+	int strtabsize = fdt_size_dt_strings(fdt);
+	int len = strlen(s) + 1;
+	int struct_top, offset;
+
+	p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
+	if (p)
+		return p - strtab;
+
+	/* Add it */
+	offset = -strtabsize - len;
+	struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	if (fdt_totalsize(fdt) + offset < struct_top)
+		return 0; /* no more room :( */
+
+	memcpy(strtab + offset, s, len);
+	fdt_set_size_dt_strings(fdt, strtabsize + len);
+	return offset;
+}
+
+int fdt_property(void *fdt, const char *name, const void *val, int len)
+{
+	struct fdt_property *prop;
+	int nameoff;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	nameoff = _fdt_find_add_string(fdt, name);
+	if (nameoff == 0)
+		return -FDT_ERR_NOSPACE;
+
+	prop = _fdt_grab_space(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
+	if (! prop)
+		return -FDT_ERR_NOSPACE;
+
+	prop->tag = cpu_to_fdt32(FDT_PROP);
+	prop->nameoff = cpu_to_fdt32(nameoff);
+	prop->len = cpu_to_fdt32(len);
+	memcpy(prop->data, val, len);
+	return 0;
+}
+
+int fdt_finish(void *fdt)
+{
+	char *p = (char *)fdt;
+	fdt32_t *end;
+	int oldstroffset, newstroffset;
+	uint32_t tag;
+	int offset, nextoffset;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	/* Add terminator */
+	end = _fdt_grab_space(fdt, sizeof(*end));
+	if (! end)
+		return -FDT_ERR_NOSPACE;
+	*end = cpu_to_fdt32(FDT_END);
+
+	/* Relocate the string table */
+	oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
+	newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
+	fdt_set_off_dt_strings(fdt, newstroffset);
+
+	/* Walk the structure, correcting string offsets */
+	offset = 0;
+	while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
+		if (tag == FDT_PROP) {
+			struct fdt_property *prop =
+				_fdt_offset_ptr_w(fdt, offset);
+			int nameoff;
+
+			nameoff = fdt32_to_cpu(prop->nameoff);
+			nameoff += fdt_size_dt_strings(fdt);
+			prop->nameoff = cpu_to_fdt32(nameoff);
+		}
+		offset = nextoffset;
+	}
+	if (nextoffset < 0)
+		return nextoffset;
+
+	/* Finally, adjust the header */
+	fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
+	fdt_set_magic(fdt, FDT_MAGIC);
+	return 0;
+}
diff --git a/lib/libfdt/fdt_wip.c b/lib/libfdt/fdt_wip.c
new file mode 100644
index 0000000..c5bbb68
--- /dev/null
+++ b/lib/libfdt/fdt_wip.c
@@ -0,0 +1,118 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
+			const void *val, int len)
+{
+	void *propval;
+	int proplen;
+
+	propval = fdt_getprop_w(fdt, nodeoffset, name, &proplen);
+	if (! propval)
+		return proplen;
+
+	if (proplen != len)
+		return -FDT_ERR_NOSPACE;
+
+	memcpy(propval, val, len);
+	return 0;
+}
+
+static void _fdt_nop_region(void *start, int len)
+{
+	fdt32_t *p;
+
+	for (p = start; (char *)p < ((char *)start + len); p++)
+		*p = cpu_to_fdt32(FDT_NOP);
+}
+
+int fdt_nop_property(void *fdt, int nodeoffset, const char *name)
+{
+	struct fdt_property *prop;
+	int len;
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
+	if (! prop)
+		return len;
+
+	_fdt_nop_region(prop, len + sizeof(*prop));
+
+	return 0;
+}
+
+int _fdt_node_end_offset(void *fdt, int offset)
+{
+	int depth = 0;
+
+	while ((offset >= 0) && (depth >= 0))
+		offset = fdt_next_node(fdt, offset, &depth);
+
+	return offset;
+}
+
+int fdt_nop_node(void *fdt, int nodeoffset)
+{
+	int endoffset;
+
+	endoffset = _fdt_node_end_offset(fdt, nodeoffset);
+	if (endoffset < 0)
+		return endoffset;
+
+	_fdt_nop_region(fdt_offset_ptr_w(fdt, nodeoffset, 0),
+			endoffset - nodeoffset);
+	return 0;
+}
diff --git a/lib/libfdt/libfdt.mk b/lib/libfdt/libfdt.mk
new file mode 100644
index 0000000..f2e8d09
--- /dev/null
+++ b/lib/libfdt/libfdt.mk
@@ -0,0 +1,41 @@
+#
+# 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.
+#
+
+LIBFDT_SRCS	:=	$(addprefix lib/libfdt/,	\
+			fdt.c				\
+			fdt_addresses.c			\
+			fdt_empty_tree.c		\
+			fdt_ro.c			\
+			fdt_rw.c			\
+			fdt_strerror.c			\
+			fdt_sw.c			\
+			fdt_wip.c)			\
+
+INCLUDES	+=	-Iinclude/lib/libfdt
diff --git a/lib/libfdt/libfdt_internal.h b/lib/libfdt/libfdt_internal.h
new file mode 100644
index 0000000..02cfa6f
--- /dev/null
+++ b/lib/libfdt/libfdt_internal.h
@@ -0,0 +1,95 @@
+#ifndef _LIBFDT_INTERNAL_H
+#define _LIBFDT_INTERNAL_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. 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.
+ *
+ *     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 OWNER 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 <fdt.h>
+
+#define FDT_ALIGN(x, a)		(((x) + (a) - 1) & ~((a) - 1))
+#define FDT_TAGALIGN(x)		(FDT_ALIGN((x), FDT_TAGSIZE))
+
+#define FDT_CHECK_HEADER(fdt) \
+	{ \
+		int __err; \
+		if ((__err = fdt_check_header(fdt)) != 0) \
+			return __err; \
+	}
+
+int _fdt_check_node_offset(const void *fdt, int offset);
+int _fdt_check_prop_offset(const void *fdt, int offset);
+const char *_fdt_find_string(const char *strtab, int tabsize, const char *s);
+int _fdt_node_end_offset(void *fdt, int nodeoffset);
+
+static inline const void *_fdt_offset_ptr(const void *fdt, int offset)
+{
+	return (const char *)fdt + fdt_off_dt_struct(fdt) + offset;
+}
+
+static inline void *_fdt_offset_ptr_w(void *fdt, int offset)
+{
+	return (void *)(uintptr_t)_fdt_offset_ptr(fdt, offset);
+}
+
+static inline const struct fdt_reserve_entry *_fdt_mem_rsv(const void *fdt, int n)
+{
+	const struct fdt_reserve_entry *rsv_table =
+		(const struct fdt_reserve_entry *)
+		((const char *)fdt + fdt_off_mem_rsvmap(fdt));
+
+	return rsv_table + n;
+}
+static inline struct fdt_reserve_entry *_fdt_mem_rsv_w(void *fdt, int n)
+{
+	return (void *)(uintptr_t)_fdt_mem_rsv(fdt, n);
+}
+
+#define FDT_SW_MAGIC		(~FDT_MAGIC)
+
+#endif /* _LIBFDT_INTERNAL_H */
diff --git a/lib/pmf/pmf_main.c b/lib/pmf/pmf_main.c
new file mode 100644
index 0000000..7ce1a9d
--- /dev/null
+++ b/lib/pmf/pmf_main.c
@@ -0,0 +1,280 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <platform.h>
+#include <pmf.h>
+#include <string.h>
+
+/*******************************************************************************
+ * The 'pmf_svc_descs' array holds the PMF service descriptors exported by
+ * services by placing them in the 'pmf_svc_descs' linker section.
+ * The 'pmf_svc_descs_indices' array holds the index of a descriptor in the
+ * 'pmf_svc_descs' array. The TIF[15:10] bits in the time-stamp id are used
+ * to get an index into the 'pmf_svc_descs_indices' array. This gives the
+ * index of the descriptor in the 'pmf_svc_descs' array  which contains the
+ * service function pointers.
+ ******************************************************************************/
+extern uintptr_t __PMF_SVC_DESCS_START__;
+extern uintptr_t __PMF_SVC_DESCS_END__;
+#define PMF_SVC_DESCS_START		((uintptr_t)(&__PMF_SVC_DESCS_START__))
+#define PMF_SVC_DESCS_END		((uintptr_t)(&__PMF_SVC_DESCS_END__))
+extern void *__PERCPU_TIMESTAMP_SIZE__;
+#define PMF_PERCPU_TIMESTAMP_SIZE	((uintptr_t)&__PERCPU_TIMESTAMP_SIZE__)
+extern uintptr_t __PMF_TIMESTAMP_START__;
+#define PMF_TIMESTAMP_ARRAY_START	((uintptr_t)&__PMF_TIMESTAMP_START__)
+extern uintptr_t __PMF_TIMESTAMP_END__;
+#define PMF_TIMESTAMP_ARRAY_END		((uintptr_t)&__PMF_TIMESTAMP_END__)
+
+#define PMF_SVC_DESCS_MAX		10
+
+/*
+ * This is used to traverse through registered PMF services.
+ */
+static pmf_svc_desc_t *pmf_svc_descs;
+
+/*
+ * This array is used to store registered PMF services in sorted order.
+ */
+static int pmf_svc_descs_indices[PMF_SVC_DESCS_MAX];
+
+/*
+ * This is used to track total number of successfully registered PMF services.
+ */
+static int pmf_num_services;
+
+/*
+ * This is the main PMF function that initialize registered
+ * PMF services and also sort them in ascending order.
+ */
+int pmf_setup(void)
+{
+	int rc, ii, jj = 0;
+	int pmf_svc_descs_num, temp_val;
+
+	/* If no PMF services are registered then simply bail out */
+	pmf_svc_descs_num = (PMF_SVC_DESCS_END - PMF_SVC_DESCS_START)/
+				 sizeof(pmf_svc_desc_t);
+	if (pmf_svc_descs_num == 0)
+		return 0;
+
+	assert(pmf_svc_descs_num < PMF_SVC_DESCS_MAX);
+
+	pmf_svc_descs = (pmf_svc_desc_t *) PMF_SVC_DESCS_START;
+	for (ii = 0; ii < pmf_svc_descs_num; ii++) {
+
+		assert(pmf_svc_descs[ii].get_ts);
+
+		/*
+		 * Call the initialization routine for this
+		 * PMF service, if it is defined.
+		 */
+		if (pmf_svc_descs[ii].init) {
+			rc = pmf_svc_descs[ii].init();
+			if (rc) {
+				WARN("Could not initialize PMF"
+					"service %s - skipping \n",
+					pmf_svc_descs[ii].name);
+				continue;
+			}
+		}
+
+		/* Update the pmf_svc_descs_indices array */
+		pmf_svc_descs_indices[jj++] = ii;
+	}
+
+	pmf_num_services = jj;
+
+	/*
+	 * Sort the successfully registered PMF services
+	 * according to service ID
+	 */
+	for (ii = 1; ii < pmf_num_services; ii++) {
+		for (jj = 0; jj < (pmf_num_services - ii); jj++) {
+			if ((pmf_svc_descs[jj].svc_config & PMF_SVC_ID_MASK) >
+				(pmf_svc_descs[jj + 1].svc_config &
+						PMF_SVC_ID_MASK)) {
+				temp_val = pmf_svc_descs_indices[jj];
+				pmf_svc_descs_indices[jj] =
+						pmf_svc_descs_indices[jj+1];
+				pmf_svc_descs_indices[jj+1] = temp_val;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * This function implements binary search to find registered
+ * PMF service based on Service ID provided in `tid` argument.
+ */
+static pmf_svc_desc_t *get_service(unsigned int tid)
+{
+	int low = 0;
+	int mid;
+	int high = pmf_num_services;
+	unsigned int svc_id = tid & PMF_SVC_ID_MASK;
+	int index;
+	unsigned int desc_svc_id;
+
+	if (pmf_num_services == 0)
+		return NULL;
+
+	assert(pmf_svc_descs);
+
+	do {
+		mid = (low + high) / 2;
+		index = pmf_svc_descs_indices[mid];
+
+		desc_svc_id = pmf_svc_descs[index].svc_config & PMF_SVC_ID_MASK;
+		if (svc_id < desc_svc_id)
+			high = mid - 1;
+		if (svc_id > desc_svc_id)
+			low = mid + 1;
+	} while ((svc_id != desc_svc_id) && (low <= high));
+
+	/*
+	 * Make sure the Service found supports the tid range.
+	 */
+	if ((svc_id == desc_svc_id) && ((tid & PMF_TID_MASK) <
+		(pmf_svc_descs[index].svc_config & PMF_TID_MASK)))
+		return (pmf_svc_desc_t *)&pmf_svc_descs[index];
+
+	return NULL;
+}
+
+/*
+ * This function gets the time-stamp value for the PMF services
+ * registered for SMC interface based on `tid` and `mpidr`.
+ */
+int pmf_get_timestamp_smc(unsigned int tid,
+		u_register_t mpidr,
+		unsigned int flags,
+		unsigned long long *ts_value)
+{
+	pmf_svc_desc_t *svc_desc;
+	assert(ts_value);
+
+	/* Search for registered service. */
+	svc_desc = get_service(tid);
+
+	if ((svc_desc == NULL) || (plat_core_pos_by_mpidr(mpidr) < 0)) {
+		*ts_value = 0;
+		return -EINVAL;
+	} else {
+		/* Call the service time-stamp handler. */
+		*ts_value = svc_desc->get_ts(tid, mpidr, flags);
+		return 0;
+	}
+}
+
+/*
+ * This function can be used to dump `ts` value for given `tid`.
+ * Assumption is that the console is already initialized.
+ */
+void __pmf_dump_timestamp(unsigned int tid, unsigned long long ts)
+{
+	tf_printf("PMF:cpu %u	tid %u	ts %llu\n",
+		plat_my_core_pos(), tid, ts);
+}
+
+/*
+ * This function calculate the address identified by
+ * `base_addr`, `tid` and `cpuid`.
+ */
+static inline uintptr_t calc_ts_addr(uintptr_t base_addr,
+		unsigned int tid,
+		unsigned int cpuid)
+{
+	assert(cpuid < PLATFORM_CORE_COUNT);
+	assert(base_addr >= PMF_TIMESTAMP_ARRAY_START);
+	assert(base_addr < ((PMF_TIMESTAMP_ARRAY_START +
+		PMF_PERCPU_TIMESTAMP_SIZE) - ((tid & PMF_TID_MASK) *
+		sizeof(unsigned long long))));
+
+	base_addr += ((cpuid * PMF_PERCPU_TIMESTAMP_SIZE) +
+		((tid & PMF_TID_MASK) * sizeof(unsigned long long)));
+
+	return base_addr;
+}
+
+/*
+ * This function stores the `ts` value to the storage identified by
+ * `base_addr`, `tid` and current cpu id.
+ * Note: The timestamp addresses are cache line aligned per cpu
+ * and only the owning CPU would ever write into it.
+ */
+void __pmf_store_timestamp(uintptr_t base_addr,
+			unsigned int tid,
+			unsigned long long ts)
+{
+	unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr,
+				 tid, plat_my_core_pos());
+	*ts_addr = ts;
+}
+
+/*
+ * This is the cached version of `pmf_store_my_timestamp`
+ * Note: The timestamp addresses are cache line aligned per cpu
+ * and only the owning CPU would ever write into it.
+ */
+void __pmf_store_timestamp_with_cache_maint(uintptr_t base_addr,
+			unsigned int tid,
+			unsigned long long ts)
+{
+	unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr,
+				 tid, plat_my_core_pos());
+	*ts_addr = ts;
+	flush_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long));
+}
+
+/*
+ * This function retrieves the `ts` value from the storage identified by
+ * `base_addr`, `tid` and `cpuid`.
+ * Note: The timestamp addresses are cache line aligned per cpu.
+ */
+unsigned long long __pmf_get_timestamp(uintptr_t base_addr,
+			unsigned int tid,
+			unsigned int cpuid,
+			unsigned int flags)
+{
+	assert(cpuid < PLATFORM_CORE_COUNT);
+	unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr,
+				tid, cpuid);
+
+	if (flags & PMF_CACHE_MAINT)
+		inv_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long));
+
+	return *ts_addr;
+}
diff --git a/lib/pmf/pmf_smc.c b/lib/pmf/pmf_smc.c
new file mode 100644
index 0000000..14190b7
--- /dev/null
+++ b/lib/pmf/pmf_smc.c
@@ -0,0 +1,91 @@
+/*
+ * 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 <debug.h>
+#include <platform.h>
+#include <pmf.h>
+#include <smcc_helpers.h>
+
+/*
+ * This function is responsible for handling all PMF SMC calls.
+ */
+uintptr_t pmf_smc_handler(unsigned int smc_fid,
+			u_register_t x1,
+			u_register_t x2,
+			u_register_t x3,
+			u_register_t x4,
+			void *cookie,
+			void *handle,
+			u_register_t flags)
+{
+	int rc;
+	unsigned long long ts_value;
+
+	if (((smc_fid >> FUNCID_CC_SHIFT) & FUNCID_CC_MASK) == SMC_32) {
+
+		x1 = (uint32_t)x1;
+		x2 = (uint32_t)x2;
+		x3 = (uint32_t)x3;
+
+		switch (smc_fid) {
+		case PMF_SMC_GET_TIMESTAMP_32:
+			/*
+			 * Return error code and the captured
+			 * time-stamp to the caller.
+			 * x0 --> error code.
+			 * x1 - x2 --> time-stamp value.
+			 */
+			rc = pmf_get_timestamp_smc(x1, x2, x3, &ts_value);
+			SMC_RET3(handle, rc, (uint32_t)ts_value,
+					(uint32_t)(ts_value >> 32));
+
+		default:
+			break;
+		}
+	} else {
+		switch (smc_fid) {
+		case PMF_SMC_GET_TIMESTAMP_64:
+			/*
+			 * Return error code and the captured
+			 * time-stamp to the caller.
+			 * x0 --> error code.
+			 * x1 --> time-stamp value.
+			 */
+			rc = pmf_get_timestamp_smc(x1, x2, x3, &ts_value);
+			SMC_RET2(handle, rc, ts_value);
+
+		default:
+			break;
+		}
+	}
+
+	WARN("Unimplemented PMF Call: 0x%x \n", smc_fid);
+	SMC_RET1(handle, SMC_UNK);
+}
diff --git a/lib/stdlib/stdlib.mk b/lib/stdlib/stdlib.mk
new file mode 100644
index 0000000..cdf3d29
--- /dev/null
+++ b/lib/stdlib/stdlib.mk
@@ -0,0 +1,47 @@
+#
+# 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.
+#
+
+STDLIB_SRCS	:=	$(addprefix lib/stdlib/,	\
+			abort.c				\
+			assert.c			\
+			exit.c				\
+			mem.c				\
+			printf.c			\
+			putchar.c			\
+			puts.c				\
+			sscanf.c			\
+			strchr.c			\
+			strcmp.c			\
+			strlen.c			\
+			strncmp.c			\
+			subr_prf.c)
+
+INCLUDES	+=	-Iinclude/lib/stdlib		\
+			-Iinclude/lib/stdlib/sys
diff --git a/lib/xlat_tables/xlat_tables_common.c b/lib/xlat_tables/xlat_tables_common.c
index d686c8d..fd10084 100644
--- a/lib/xlat_tables/xlat_tables_common.c
+++ b/lib/xlat_tables/xlat_tables_common.c
@@ -88,8 +88,8 @@
 {
 	mmap_region_t *mm = mmap;
 	mmap_region_t *mm_last = mm + ARRAY_SIZE(mmap) - 1;
-	unsigned long long pa_end = base_pa + size - 1;
-	uintptr_t va_end = base_va + size - 1;
+	unsigned long long end_pa = base_pa + size - 1;
+	uintptr_t end_va = base_va + size - 1;
 
 	assert(IS_PAGE_ALIGNED(base_pa));
 	assert(IS_PAGE_ALIGNED(base_va));
@@ -98,10 +98,74 @@
 	if (!size)
 		return;
 
+	assert(base_pa < end_pa); /* Check for overflows */
+	assert(base_va < end_va);
+
+#if DEBUG
+
+	/* Check for PAs and VAs overlaps with all other regions */
+	for (mm = mmap; mm->size; ++mm) {
+
+		uintptr_t mm_end_va = mm->base_va + mm->size - 1;
+
+		/*
+		 * Check if one of the regions is completely inside the other
+		 * one.
+		 */
+		int fully_overlapped_va =
+			((base_va >= mm->base_va) && (end_va <= mm_end_va)) ||
+			((mm->base_va >= base_va) && (mm_end_va <= end_va));
+
+		/*
+		 * Full VA overlaps are only allowed if both regions are
+		 * identity mapped (zero offset) or have the same VA to PA
+		 * offset. Also, make sure that it's not the exact same area.
+		 */
+		if (fully_overlapped_va) {
+			assert((mm->base_va - mm->base_pa) ==
+			       (base_va - base_pa));
+			assert((base_va != mm->base_va) || (size != mm->size));
+		} else {
+			/*
+			 * If the regions do not have fully overlapping VAs,
+			 * then they must have fully separated VAs and PAs.
+			 * Partial overlaps are not allowed
+			 */
+
+			unsigned long long mm_end_pa =
+						     mm->base_pa + mm->size - 1;
+
+			int separated_pa =
+				(end_pa < mm->base_pa) || (base_pa > mm_end_pa);
+			int separated_va =
+				(end_va < mm->base_va) || (base_va > mm_end_va);
+
+			assert(separated_va && separated_pa);
+		}
+	}
+
+	mm = mmap; /* Restore pointer to the start of the array */
+
+#endif /* DEBUG */
+
 	/* Find correct place in mmap to insert new region */
 	while (mm->base_va < base_va && mm->size)
 		++mm;
 
+	/*
+	 * If a section is contained inside another one with the same base
+	 * address, it must be placed after the one it is contained in:
+	 *
+	 * 1st |-----------------------|
+	 * 2nd |------------|
+	 * 3rd |------|
+	 *
+	 * This is required for mmap_region_attr() to get the attributes of the
+	 * small region correctly.
+	 */
+	while ((mm->base_va == base_va) && (mm->size > size))
+		++mm;
+
 	/* Make room for new region by moving other regions up by one place */
 	memmove(mm + 1, mm, (uintptr_t)mm_last - (uintptr_t)mm);
 
@@ -113,10 +177,10 @@
 	mm->size = size;
 	mm->attr = attr;
 
-	if (pa_end > xlat_max_pa)
-		xlat_max_pa = pa_end;
-	if (va_end > xlat_max_va)
-		xlat_max_va = va_end;
+	if (end_pa > xlat_max_pa)
+		xlat_max_pa = end_pa;
+	if (end_va > xlat_max_va)
+		xlat_max_va = end_va;
 }
 
 void mmap_add(const mmap_region_t *mm)
@@ -164,14 +228,33 @@
 	return desc;
 }
 
+/*
+ * Returns attributes of area at `base_va` with size `size`. It returns the
+ * attributes of the innermost region that contains it. If there are partial
+ * overlaps, it returns -1, as a smaller size is needed.
+ */
 static int mmap_region_attr(mmap_region_t *mm, uintptr_t base_va,
 					size_t size)
 {
-	int attr = mm->attr;
-	int old_mem_type, new_mem_type;
+	/* Don't assume that the area is contained in the first region */
+	int attr = -1;
 
-	for (;;) {
-		++mm;
+	/*
+	 * Get attributes from last (innermost) region that contains the
+	 * requested area. Don't stop as soon as one region doesn't contain it
+	 * because there may be other internal regions that contain this area:
+	 *
+	 * |-----------------------------1-----------------------------|
+	 * |----2----|     |-------3-------|    |----5----|
+	 *                   |--4--|
+	 *
+	 *                   |---| <- Area we want the attributes of.
+	 *
+	 * In this example, the area is contained in regions 1, 3 and 4 but not
+	 * in region 2. The loop shouldn't stop at region 2 as inner regions
+	 * have priority over outer regions, it should stop at region 5.
+	 */
+	for (;; ++mm) {
 
 		if (!mm->size)
 			return attr; /* Reached end of list */
@@ -182,27 +265,14 @@
 		if (mm->base_va + mm->size <= base_va)
 			continue; /* Next region has already been overtaken */
 
-		if ((mm->attr & attr) == attr)
+		if (mm->attr == attr)
 			continue; /* Region doesn't override attribs so skip */
 
-		/*
-		 * Update memory mapping attributes in 2 steps:
-		 * 1) Update access permissions and security state flags
-		 * 2) Update memory type.
-		 *
-		 * See xlat_tables.h for details about the attributes priority
-		 * system and the rules dictating whether attributes should be
-		 * updated.
-		 */
-		old_mem_type = MT_TYPE(attr);
-		new_mem_type = MT_TYPE(mm->attr);
-		attr &= mm->attr;
-		if (new_mem_type < old_mem_type)
-			attr = (attr & ~MT_TYPE_MASK) | new_mem_type;
-
 		if (mm->base_va > base_va ||
 			mm->base_va + mm->size < base_va + size)
 			return -1; /* Region doesn't fully cover our area */
+
+		attr = mm->attr;
 	}
 }
 
@@ -214,8 +284,9 @@
 	unsigned level_size_shift = L1_XLAT_ADDRESS_SHIFT - (level - 1) *
 						XLAT_TABLE_ENTRIES_SHIFT;
 	unsigned level_size = 1 << level_size_shift;
-	unsigned long long level_index_mask = XLAT_TABLE_ENTRIES_MASK <<
-							level_size_shift;
+	unsigned long long level_index_mask =
+		((unsigned long long) XLAT_TABLE_ENTRIES_MASK)
+		<< level_size_shift;
 
 	assert(level > 0 && level <= 3);
 
@@ -228,7 +299,7 @@
 			/* Done mapping regions; finish zeroing the table */
 			desc = INVALID_DESC;
 		} else if (mm->base_va + mm->size <= base_va) {
-			/* Area now after the region so skip it */
+			/* This area is after the region so get next region */
 			++mm;
 			continue;
 		}
@@ -237,24 +308,27 @@
 				(void *)base_va, level_size);
 
 		if (mm->base_va >= base_va + level_size) {
-			/* Next region is after area so nothing to map yet */
+			/* Next region is after this area. Nothing to map yet */
 			desc = INVALID_DESC;
-		} else if (mm->base_va <= base_va && mm->base_va + mm->size >=
-				base_va + level_size) {
-			/* Next region covers all of area */
+		} else {
+			/*
+			 * Try to get attributes of this area. It will fail if
+			 * there are partially overlapping regions. On success,
+			 * it will return the innermost region's attributes.
+			 */
 			int attr = mmap_region_attr(mm, base_va, level_size);
-			if (attr >= 0)
+			if (attr >= 0) {
 				desc = mmap_desc(attr,
 					base_va - mm->base_va + mm->base_pa,
 					level);
+			}
 		}
-		/* else Next region only partially covers area, so need */
 
 		if (desc == UNSET_DESC) {
 			/* Area not covered by a region so need finer table */
 			uint64_t *new_table = xlat_tables[next_xlat++];
 			assert(next_xlat <= MAX_XLAT_TABLES);
-			desc = TABLE_DESC | (unsigned long)new_table;
+			desc = TABLE_DESC | (uint64_t)new_table;
 
 			/* Recurse to fill in new table */
 			mm = init_xlation_table_inner(mm, base_va,
diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index 19b88ad..165e3aa 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@@ -215,11 +215,11 @@
 
 $(OBJ): $(2)
 	@echo "  CC      $$<"
-	$$(Q)$$(CC) $$(CFLAGS) -D$(IMAGE) -c $$< -o $$@
+	$$(Q)$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -D$(IMAGE) -c $$< -o $$@
 
 $(PREREQUISITES): $(2) | bl$(3)_dirs
 	@echo "  DEPS    $$@"
-	$$(Q)$$(CC) $$(CFLAGS) -M -MT $(OBJ) -MF $$@ $$<
+	$$(Q)$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -M -MT $(OBJ) -MF $$@ $$<
 
 ifdef IS_ANYTHING_TO_BUILD
 -include $(PREREQUISITES)
@@ -358,7 +358,7 @@
 else
 	@echo 'const char build_message[] = "Built : "$(BUILD_MESSAGE_TIMESTAMP); \
 	       const char version_string[] = "${VERSION_STRING}";' | \
-		$$(CC) $$(CFLAGS) -xc - -o $(BUILD_DIR)/build_message.o
+		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -xc - -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 8ac8246..fe5e8c1 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) $$(CFLAGS) -x c - -o $1
+		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -x c - -o $1
 endef
 
diff --git a/plat/arm/board/common/board_arm_trusted_boot.c b/plat/arm/board/common/board_arm_trusted_boot.c
index 7ae00cc..fd9e9f6 100644
--- a/plat/arm/board/common/board_arm_trusted_boot.c
+++ b/plat/arm/board/common/board_arm_trusted_boot.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:
@@ -180,10 +180,10 @@
 }
 
 /*
- * Store a new non-volatile counter value. On Juno and FVP, the non-volatile
- * counters are RO and cannot be modified. We expect the values in the
- * certificates to always match the RO values so that this function is never
- * called.
+ * Store a new non-volatile counter value. By default on ARM development
+ * platforms, the non-volatile counters are RO and cannot be modified. We expect
+ * the values in the certificates to always match the RO values so that this
+ * function is never called.
  *
  * Return: 0 = success, Otherwise = error
  */
diff --git a/plat/arm/board/fvp/fvp_bl2_setup.c b/plat/arm/board/fvp/fvp_bl2_setup.c
index 305309a..ee49c30 100644
--- a/plat/arm/board/fvp/fvp_bl2_setup.c
+++ b/plat/arm/board/fvp/fvp_bl2_setup.c
@@ -28,6 +28,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <generic_delay_timer.h>
 #include <mmio.h>
 #include <plat_arm.h>
 #include <sp804_delay_timer.h>
@@ -48,6 +49,7 @@
 {
 	arm_bl2_platform_setup();
 
+#if FVP_USE_SP804_TIMER
 	/* Enable the clock override for SP804 timer 0, which means that no
 	 * clock dividers are applied and the raw (35 MHz) clock will be used */
 	mmio_write_32(V2M_SP810_BASE, FVP_SP810_CTRL_TIM0_OV);
@@ -55,4 +57,7 @@
 	/* Initialize delay timer driver using SP804 dual timer 0 */
 	sp804_timer_init(V2M_SP804_TIMER0_BASE,
 			SP804_TIMER_CLKMULT, SP804_TIMER_CLKDIV);
+#else
+	generic_delay_timer_init();
+#endif /* FVP_USE_SP804_TIMER */
 }
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index 1de9999..0f557af 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -30,6 +30,7 @@
 
 #include <arm_config.h>
 #include <arm_def.h>
+#include <ccn.h>
 #include <debug.h>
 #include <gicv2.h>
 #include <mmio.h>
@@ -37,10 +38,6 @@
 #include <v2m_def.h>
 #include "../fvp_def.h"
 
-#if (FVP_USE_GIC_DRIVER == FVP_GICV2)
-extern gicv2_driver_data_t arm_gic_data;
-#endif
-
 /* Defines for GIC Driver build time selection */
 #define FVP_GICV2		1
 #define FVP_GICV3		2
@@ -65,7 +62,7 @@
 
 #define MAP_DEVICE2	MAP_REGION_FLAT(DEVICE2_BASE,			\
 					DEVICE2_SIZE,			\
-					MT_DEVICE | MT_RO | MT_SECURE)
+					MT_DEVICE | MT_RW | MT_SECURE)
 
 
 /*
@@ -159,26 +156,9 @@
 	 */
 	switch (bld) {
 	case BLD_GIC_VE_MMAP:
-#if IMAGE_BL31 || IMAGE_BL32
-#if FVP_USE_GIC_DRIVER == FVP_GICV2
-		/*
-		 * If the FVP implements the VE compatible memory map, then the
-		 * GICv2 driver must be included in the build. Update the platform
-		 * data with the correct GICv2 base addresses before it is used
-		 * to initialise the driver.
-		 *
-		 * This update of platform data is temporary and will be removed
-		 * once VE memory map for FVP is no longer supported by Trusted
-		 * Firmware.
-		 */
-		arm_gic_data.gicd_base = VE_GICD_BASE;
-		arm_gic_data.gicc_base = VE_GICC_BASE;
-
-#else
-		ERROR("Only GICv2 driver supported for VE memory map\n");
+		ERROR("Legacy Versatile Express memory map for GIC peripheral"
+				" is not supported\n");
 		panic();
-#endif /* __FVP_USE_GIC_DRIVER == FVP_GICV2__ */
-#endif /* __IMAGE_BL31 || IMAGE_BL32__ */
 		break;
 	case BLD_GIC_A53A57_MMAP:
 		break;
@@ -234,8 +214,16 @@
 
 void fvp_interconnect_init(void)
 {
-	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT)
+	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT) {
+#if FVP_INTERCONNECT_DRIVER == FVP_CCN
+		if (ccn_get_part0_id(PLAT_ARM_CCN_BASE) != CCN_502_PART0_ID) {
+			ERROR("Unrecognized CCN variant detected. Only CCN-502"
+					" is supported");
+			panic();
+		}
+#endif
 		plat_arm_interconnect_init();
+	}
 }
 
 void fvp_interconnect_enable(void)
diff --git a/plat/arm/board/fvp/fvp_def.h b/plat/arm/board/fvp/fvp_def.h
index b0f07ef..40d20fc 100644
--- a/plat/arm/board/fvp/fvp_def.h
+++ b/plat/arm/board/fvp/fvp_def.h
@@ -40,6 +40,10 @@
 
 #define FVP_PRIMARY_CPU			0x0
 
+/* Defines for the Interconnect build selection */
+#define FVP_CCI			1
+#define FVP_CCN			2
+
 /*******************************************************************************
  * FVP memory map related constants
  ******************************************************************************/
@@ -57,16 +61,23 @@
 #define DEVICE0_BASE			0x20000000
 #define DEVICE0_SIZE			0x0c200000
 
+/*
+ *  In case of FVP models with CCN, the CCN register space overlaps into
+ *  the NSRAM area.
+ */
+#if FVP_INTERCONNECT_DRIVER == FVP_CCN
+#define DEVICE1_BASE			0x2e000000
+#define DEVICE1_SIZE			0x1A00000
+#else
 #define DEVICE1_BASE			0x2f000000
 #define DEVICE1_SIZE			0x200000
-
+#define NSRAM_BASE			0x2e000000
+#define NSRAM_SIZE			0x10000
+#endif
 /* Devices in the second GB */
 #define DEVICE2_BASE			0x7fe00000
 #define DEVICE2_SIZE			0x00200000
 
-#define NSRAM_BASE			0x2e000000
-#define NSRAM_SIZE			0x10000
-
 #define PCIE_EXP_BASE			0x40000000
 #define TZRNG_BASE			0x7fe60000
 
diff --git a/plat/rockchip/common/plat_delay_timer.c b/plat/arm/board/fvp/fvp_trusted_boot.c
similarity index 61%
copy from plat/rockchip/common/plat_delay_timer.c
copy to plat/arm/board/fvp/fvp_trusted_boot.c
index 797ce05..7db5051 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/plat/arm/board/fvp/fvp_trusted_boot.c
@@ -28,27 +28,41 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#include <assert.h>
+#include <platform_oid.h>
+#include <stdint.h>
+#include <string.h>
+#include "fvp_def.h"
 
-static uint32_t plat_get_timer_value(void)
+/*
+ * Store a new non-volatile counter value. On some FVP versions, the
+ * non-volatile counters are RO. On these versions we expect the values in the
+ * certificates to always match the RO values so that this function is never
+ * called.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
 {
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
+	const char *oid;
+	uint32_t *nv_ctr_addr;
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+	assert(cookie != NULL);
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
+	oid = (const char *)cookie;
+	if (strcmp(oid, TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		nv_ctr_addr = (uint32_t *)TFW_NVCTR_BASE;
+	} else if (strcmp(oid, NON_TRUSTED_FW_NVCOUNTER_OID) == 0) {
+		nv_ctr_addr = (uint32_t *)NTFW_CTR_BASE;
+	} else {
+		return 1;
+	}
+
+	*(unsigned int *)nv_ctr_addr = nv_ctr;
+
+	/* Verify that the current value is the one we just wrote. */
+	if (nv_ctr != (unsigned int)(*nv_ctr_addr))
+		return 1;
+
+	return 0;
 }
diff --git a/plat/arm/board/fvp/include/plat_macros.S b/plat/arm/board/fvp/include/plat_macros.S
index e43c7ac..657a39b 100644
--- a/plat/arm/board/fvp/include/plat_macros.S
+++ b/plat/arm/board/fvp/include/plat_macros.S
@@ -64,7 +64,9 @@
 	mov_imm	x16, VE_GICD_BASE
 print_gic_regs:
 	arm_print_gic_regs
+#if FVP_INTERCONNECT_DRIVER == FVP_CCI
 	print_cci_regs
+#endif
 	.endm
 
 #endif /* __PLAT_MACROS_S__ */
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index ca8e35e..1e906d8 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -90,6 +90,10 @@
 #define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	3
 #define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	4
 
+/* CCN related constants. Only CCN 502 is currently supported */
+#define PLAT_ARM_CCN_BASE		0x2e000000
+#define PLAT_ARM_CLUSTER_TO_CCN_ID_MAP	1, 5, 7, 11
+
 /* System timer related constants */
 #define PLAT_ARM_NSTIMER_FRAME_ID		1
 
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 60ebe65..1ea9822 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -28,17 +28,33 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
-# Use the Legacy GICv3 driver on the FVP by default to maintain compatibility.
-FVP_USE_GIC_DRIVER	:= FVP_GICV3_LEGACY
+# Use the GICv3 driver on the FVP by default
+FVP_USE_GIC_DRIVER	:= FVP_GICV3
+# Use the SP804 timer instead of the generic one
+FVP_USE_SP804_TIMER	:= 0
+
+$(eval $(call assert_boolean,FVP_USE_SP804_TIMER))
+$(eval $(call add_define,FVP_USE_SP804_TIMER))
 
 # The FVP platform depends on this macro to build with correct GIC driver.
 $(eval $(call add_define,FVP_USE_GIC_DRIVER))
 
-# If FVP_CLUSTER_COUNT has been defined, pass it into the build system.
-ifdef FVP_CLUSTER_COUNT
+# Define default FVP_CLUSTER_COUNT to 2 and pass it into the build system.
+FVP_CLUSTER_COUNT	:= 2
 $(eval $(call add_define,FVP_CLUSTER_COUNT))
+
+# Sanity check the cluster count and if FVP_CLUSTER_COUNT <= 2,
+# choose the CCI driver , else the CCN driver
+ifeq ($(FVP_CLUSTER_COUNT), 0)
+$(error "Incorrect cluster count specified for FVP port")
+else ifeq ($(FVP_CLUSTER_COUNT),$(filter $(FVP_CLUSTER_COUNT),1 2))
+FVP_INTERCONNECT_DRIVER := FVP_CCI
+else
+FVP_INTERCONNECT_DRIVER := FVP_CCN
 endif
 
+$(eval $(call add_define,FVP_INTERCONNECT_DRIVER))
+
 # Choose the GIC sources depending upon the how the FVP will be invoked
 ifeq (${FVP_USE_GIC_DRIVER}, FVP_GICV3)
 FVP_GIC_SOURCES		:=	drivers/arm/gic/common/gic_common.c	\
@@ -62,8 +78,15 @@
 $(error "Incorrect GIC driver chosen on FVP port")
 endif
 
+ifeq (${FVP_INTERCONNECT_DRIVER}, FVP_CCI)
 FVP_INTERCONNECT_SOURCES	:= 	drivers/arm/cci/cci.c		\
 					plat/arm/common/arm_cci.c
+else ifeq (${FVP_INTERCONNECT_DRIVER}, FVP_CCN)
+FVP_INTERCONNECT_SOURCES	:= 	drivers/arm/ccn/ccn.c		\
+					plat/arm/common/arm_ccn.c
+else
+$(error "Incorrect CCN driver chosen on FVP port")
+endif
 
 FVP_SECURITY_SOURCES	:=	drivers/arm/tzc/tzc400.c		\
 				plat/arm/board/fvp/fvp_security.c	\
@@ -79,7 +102,8 @@
 				lib/cpus/aarch64/cortex_a35.S			\
 				lib/cpus/aarch64/cortex_a53.S			\
 				lib/cpus/aarch64/cortex_a57.S			\
-				lib/cpus/aarch64/cortex_a72.S
+				lib/cpus/aarch64/cortex_a72.S			\
+				lib/cpus/aarch64/cortex_a73.S
 
 BL1_SOURCES		+=	drivers/io/io_semihosting.c			\
 				lib/semihosting/semihosting.c			\
@@ -88,20 +112,27 @@
 				plat/arm/board/fvp/fvp_bl1_setup.c		\
 				plat/arm/board/fvp/fvp_err.c			\
 				plat/arm/board/fvp/fvp_io_storage.c		\
+				plat/arm/board/fvp/fvp_trusted_boot.c		\
 				${FVP_CPU_LIBS}					\
 				${FVP_INTERCONNECT_SOURCES}
 
 
-BL2_SOURCES		+=	drivers/arm/sp804/sp804_delay_timer.c		\
-				drivers/io/io_semihosting.c			\
+BL2_SOURCES		+=	drivers/io/io_semihosting.c			\
 				drivers/delay_timer/delay_timer.c		\
 				lib/semihosting/semihosting.c			\
 				lib/semihosting/aarch64/semihosting_call.S	\
 				plat/arm/board/fvp/fvp_bl2_setup.c		\
 				plat/arm/board/fvp/fvp_err.c			\
 				plat/arm/board/fvp/fvp_io_storage.c		\
+				plat/arm/board/fvp/fvp_trusted_boot.c		\
 				${FVP_SECURITY_SOURCES}
 
+ifeq (${FVP_USE_SP804_TIMER},1)
+BL2_SOURCES		+=	drivers/arm/sp804/sp804_delay_timer.c
+else
+BL2_SOURCES		+=	drivers/delay_timer/generic_delay_timer.c
+endif
+
 BL2U_SOURCES		+=	plat/arm/board/fvp/fvp_bl2u_setup.c		\
 				${FVP_SECURITY_SOURCES}
 
diff --git a/plat/arm/board/juno/juno_pm.c b/plat/arm/board/juno/juno_pm.c
index 4b956d2..cbf994a 100644
--- a/plat/arm/board/juno/juno_pm.c
+++ b/plat/arm/board/juno/juno_pm.c
@@ -59,6 +59,19 @@
 	return rc;
 }
 
+/*
+ * Custom `translate_power_state_by_mpidr` handler for Juno. Unlike in the
+ * `juno_validate_power_state`, we do not down-grade the system power
+ * domain level request in `power_state` as it will be used to query the
+ * PSCI_STAT_COUNT/RESIDENCY at the system power domain level.
+ */
+static int juno_translate_power_state_by_mpidr(u_register_t mpidr,
+		unsigned int power_state,
+		psci_power_state_t *output_state)
+{
+	return arm_validate_power_state(power_state, output_state);
+}
+
 /*******************************************************************************
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform will take care of registering the handlers with PSCI.
@@ -74,5 +87,6 @@
 	.system_reset			= css_system_reset,
 	.validate_power_state		= juno_validate_power_state,
 	.validate_ns_entrypoint		= arm_validate_ns_entrypoint,
-	.get_sys_suspend_power_state	= css_get_sys_suspend_power_state
+	.get_sys_suspend_power_state	= css_get_sys_suspend_power_state,
+	.translate_power_state_by_mpidr = juno_translate_power_state_by_mpidr
 };
diff --git a/plat/arm/common/aarch64/arm_common.c b/plat/arm/common/aarch64/arm_common.c
index 78b75b4..c4cc80e 100644
--- a/plat/arm/common/aarch64/arm_common.c
+++ b/plat/arm/common/aarch64/arm_common.c
@@ -29,6 +29,8 @@
  */
 #include <arch.h>
 #include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
 #include <mmio.h>
 #include <plat_arm.h>
 #include <platform_def.h>
@@ -40,6 +42,13 @@
 #pragma weak plat_get_ns_image_entrypoint
 #pragma weak plat_arm_get_mmap
 
+/* Conditionally provide a weak definition of plat_get_syscnt_freq2 to avoid
+ * conflicts with the definition in plat/common. */
+#if ERROR_DEPRECATED
+#pragma weak plat_get_syscnt_freq2
+#else
+#pragma weak plat_get_syscnt_freq
+#endif
 
 /*******************************************************************************
  * Macro generating the code for the function setting up the pagetables as per
@@ -161,3 +170,27 @@
 {
 	return plat_arm_mmap;
 }
+
+#ifdef ARM_SYS_CNTCTL_BASE
+
+#if ERROR_DEPRECATED
+unsigned int plat_get_syscnt_freq2(void)
+{
+	unsigned int counter_base_frequency;
+#else
+unsigned long long plat_get_syscnt_freq(void)
+{
+	unsigned long long counter_base_frequency;
+#endif /* ERROR_DEPRECATED */
+
+	/* Read the frequency from Frequency modes table */
+	counter_base_frequency = mmio_read_32(ARM_SYS_CNTCTL_BASE + CNTFID_OFF);
+
+	/* The first entry of the frequency modes table must not be 0 */
+	if (counter_base_frequency == 0)
+		panic();
+
+	return counter_base_frequency;
+}
+
+#endif /* ARM_SYS_CNTCTL_BASE */
diff --git a/plat/arm/common/arm_bl31_setup.c b/plat/arm/common/arm_bl31_setup.c
index 8fcfa77..8eb6818 100644
--- a/plat/arm/common/arm_bl31_setup.c
+++ b/plat/arm/common/arm_bl31_setup.c
@@ -34,7 +34,6 @@
 #include <assert.h>
 #include <bl_common.h>
 #include <console.h>
-#include <debug.h>
 #include <mmio.h>
 #include <plat_arm.h>
 #include <platform.h>
@@ -76,7 +75,6 @@
 #pragma weak bl31_platform_setup
 #pragma weak bl31_plat_arch_setup
 #pragma weak bl31_plat_get_next_image_ep_info
-#pragma weak plat_get_syscnt_freq
 
 
 /*******************************************************************************
@@ -268,17 +266,3 @@
 {
 	arm_bl31_plat_arch_setup();
 }
-
-uint64_t plat_get_syscnt_freq(void)
-{
-	uint64_t counter_base_frequency;
-
-	/* Read the frequency from Frequency modes table */
-	counter_base_frequency = mmio_read_32(ARM_SYS_CNTCTL_BASE + CNTFID_OFF);
-
-	/* The first entry of the frequency modes table must not be 0 */
-	if (counter_base_frequency == 0)
-		panic();
-
-	return counter_base_frequency;
-}
diff --git a/plat/arm/common/arm_ccn.c b/plat/arm/common/arm_ccn.c
index 5cb443a..fe10d7b 100644
--- a/plat/arm/common/arm_ccn.c
+++ b/plat/arm/common/arm_ccn.c
@@ -43,6 +43,9 @@
 	.master_to_rn_id_map = master_to_rn_id_map
 };
 
+CASSERT(PLAT_ARM_CLUSTER_COUNT == ARRAY_SIZE(master_to_rn_id_map),
+		assert_invalid_cluster_count_for_ccn_variant);
+
 /******************************************************************************
  * The following functions are defined as weak to allow a platform to override
  * the way ARM CCN driver is initialised and used.
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 0b28861..bcb3f6f 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -82,6 +82,9 @@
 $(eval $(call assert_boolean,ARM_BL31_IN_DRAM))
 $(eval $(call add_define,ARM_BL31_IN_DRAM))
 
+# Enable PSCI_STAT_COUNT/RESIDENCY APIs on ARM platforms
+ENABLE_PSCI_STAT = 1
+
 PLAT_INCLUDES		+=	-Iinclude/common/tbbr				\
 				-Iinclude/plat/arm/common			\
 				-Iinclude/plat/arm/common/aarch64
diff --git a/plat/arm/common/arm_gicv2.c b/plat/arm/common/arm_gicv2.c
index 76f04cd..2636d1c 100644
--- a/plat/arm/common/arm_gicv2.c
+++ b/plat/arm/common/arm_gicv2.c
@@ -47,17 +47,12 @@
  * On a GICv2 system, the Group 1 secure interrupts are treated as Group 0
  * interrupts.
  *****************************************************************************/
-const unsigned int g0_interrupt_array[] = {
+static const unsigned int g0_interrupt_array[] = {
 	PLAT_ARM_G1S_IRQS,
 	PLAT_ARM_G0_IRQS
 };
 
-/*
- * Ideally `arm_gic_data` structure definition should be a `const` but it is
- * kept as modifiable for overwriting with different GICD and GICC base when
- * running on FVP with VE memory map.
- */
-gicv2_driver_data_t arm_gic_data = {
+static const gicv2_driver_data_t arm_gic_data = {
 	.gicd_base = PLAT_ARM_GICD_BASE,
 	.gicc_base = PLAT_ARM_GICC_BASE,
 	.g0_interrupt_num = ARRAY_SIZE(g0_interrupt_array),
diff --git a/plat/arm/common/arm_gicv3.c b/plat/arm/common/arm_gicv3.c
index 33f8018..a20fd56 100644
--- a/plat/arm/common/arm_gicv3.c
+++ b/plat/arm/common/arm_gicv3.c
@@ -45,15 +45,15 @@
 #pragma weak plat_arm_gic_pcpu_init
 
 /* The GICv3 driver only needs to be initialized in EL3 */
-uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
+static uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
 
 /* Array of Group1 secure interrupts to be configured by the gic driver */
-const unsigned int g1s_interrupt_array[] = {
+static const unsigned int g1s_interrupt_array[] = {
 	PLAT_ARM_G1S_IRQS
 };
 
 /* Array of Group0 interrupts to be configured by the gic driver */
-const unsigned int g0_interrupt_array[] = {
+static const unsigned int g0_interrupt_array[] = {
 	PLAT_ARM_G0_IRQS
 };
 
diff --git a/plat/arm/common/arm_gicv3_legacy.c b/plat/arm/common/arm_gicv3_legacy.c
index 6170933..8396b60 100644
--- a/plat/arm/common/arm_gicv3_legacy.c
+++ b/plat/arm/common/arm_gicv3_legacy.c
@@ -48,7 +48,7 @@
  * In the GICv3 Legacy mode, the Group 1 secure interrupts are treated as Group
  * 0 interrupts.
  */
-const unsigned int irq_sec_array[] = {
+static const unsigned int irq_sec_array[] = {
 	PLAT_ARM_G0_IRQS,
 	PLAT_ARM_G1S_IRQS
 };
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index b6f94ac..801d937 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.c
@@ -262,8 +262,14 @@
 	assert(cpu_state == ARM_LOCAL_STATE_RET);
 
 	scr = read_scr_el3();
-	/* Enable PhysicalIRQ bit for NS world to wake the CPU */
-	write_scr_el3(scr | SCR_IRQ_BIT);
+	/*
+	 * Enable the Non secure interrupt to wake the CPU.
+	 * In GICv3 affinity routing mode, the non secure group1 interrupts use
+	 * the PhysicalFIQ at EL3 whereas in GICv2, it uses the PhysicalIRQ.
+	 * Enabling both the bits works for both GICv2 mode and GICv3 affinity
+	 * routing mode.
+	 */
+	write_scr_el3(scr | SCR_IRQ_BIT | SCR_FIQ_BIT);
 	isb();
 	dsb();
 	wfi();
diff --git a/plat/common/aarch64/plat_common.c b/plat/common/aarch64/plat_common.c
index 9070c61..4322341 100644
--- a/plat/common/aarch64/plat_common.c
+++ b/plat/common/aarch64/plat_common.c
@@ -40,6 +40,9 @@
 #pragma weak bl31_plat_enable_mmu
 #pragma weak bl32_plat_enable_mmu
 #pragma weak bl31_plat_runtime_setup
+#if !ERROR_DEPRECATED
+#pragma weak plat_get_syscnt_freq2
+#endif /* ERROR_DEPRECATED */
 
 void bl31_plat_enable_mmu(uint32_t flags)
 {
@@ -74,3 +77,14 @@
 }
 #endif
 
+
+#if !ERROR_DEPRECATED
+unsigned int plat_get_syscnt_freq2(void)
+{
+	unsigned long long freq = plat_get_syscnt_freq();
+
+	assert(freq >> 32 == 0);
+
+	return (unsigned int)freq;
+}
+#endif /* ERROR_DEPRECATED */
diff --git a/plat/mediatek/common/custom/oem_svc.c b/plat/mediatek/common/custom/oem_svc.c
new file mode 100644
index 0000000..6f31c53
--- /dev/null
+++ b/plat/mediatek/common/custom/oem_svc.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <oem_svc.h>
+#include <platform.h>
+#include <runtime_svc.h>
+#include <stdint.h>
+#include <uuid.h>
+
+/* OEM Service UUID */
+DEFINE_SVC_UUID(oem_svc_uid,
+		0xb943add0, 0x069d, 0x11e4, 0x91, 0x91,
+		0x08, 0x00, 0x20, 0x0c, 0x9a, 0x66);
+
+
+/* Setup OEM Services */
+static int32_t oem_svc_setup(void)
+{
+	/*
+	 * Invoke related module setup from here
+	 */
+
+	return 0;
+}
+
+/*******************************************************************************
+ * OEM top level handler for servicing SMCs.
+ ******************************************************************************/
+uint64_t oem_smc_handler(uint32_t smc_fid,
+			uint64_t x1,
+			uint64_t x2,
+			uint64_t x3,
+			uint64_t x4,
+			void *cookie,
+			void *handle,
+			uint64_t flags)
+{
+	uint64_t rc;
+
+	switch (smc_fid) {
+	default:
+		rc = SMC_UNK;
+		WARN("Unimplemented OEM Call: 0x%x\n", smc_fid);
+	}
+
+	SMC_RET1(handle, rc);
+}
+
+/*
+ * Top-level OEM Service SMC handler. This handler will in turn dispatch
+ * calls to related SMC handler
+ */
+uint64_t oem_svc_smc_handler(uint32_t smc_fid,
+			 uint64_t x1,
+			 uint64_t x2,
+			 uint64_t x3,
+			 uint64_t x4,
+			 void *cookie,
+			 void *handle,
+			 uint64_t flags)
+{
+	/*
+	 * Dispatch OEM calls to OEM Common handler and return its return value
+	 */
+	if (is_oem_fid(smc_fid)) {
+		return oem_smc_handler(smc_fid, x1, x2, x3, x4, cookie,
+					handle, flags);
+	}
+
+	switch (smc_fid) {
+	case OEM_SVC_CALL_COUNT:
+		/*
+		 * Return the number of OEM Service Calls.
+		 */
+		SMC_RET1(handle, OEM_SVC_NUM_CALLS);
+
+	case OEM_SVC_UID:
+		/* Return UID to the caller */
+		SMC_UUID_RET(handle, oem_svc_uid);
+
+	case OEM_SVC_VERSION:
+		/* Return the version of current implementation */
+		SMC_RET2(handle, OEM_VERSION_MAJOR, OEM_VERSION_MINOR);
+
+	default:
+		WARN("Unimplemented OEM Service Call: 0x%x\n", smc_fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
+
+/* Register OEM Service Calls as runtime service */
+DECLARE_RT_SVC(
+		oem_svc,
+		OEN_OEM_START,
+		OEN_OEM_END,
+		SMC_TYPE_FAST,
+		oem_svc_setup,
+		oem_svc_smc_handler
+);
diff --git a/plat/mediatek/common/custom/oem_svc.h b/plat/mediatek/common/custom/oem_svc.h
new file mode 100644
index 0000000..3b15e15
--- /dev/null
+++ b/plat/mediatek/common/custom/oem_svc.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __OEM_SVC_H__
+#define __OEM_SVC_H__
+
+/*******************************************************************************
+ * Defines for runtime services func ids
+ ******************************************************************************/
+/*
+ * Number of OEM calls (above) implemented.
+ */
+#define OEM_SVC_NUM_CALLS		3
+
+/*******************************************************************************
+ * Defines for OEM Service queries
+ ******************************************************************************/
+/* 0x83000000 - 0x8300FEFF is OEM service calls */
+#define OEM_SVC_CALL_COUNT		0x8300ff00
+#define OEM_SVC_UID			0x8300ff01
+/* 0x8300ff02 is reserved */
+#define OEM_SVC_VERSION			0x8300ff03
+/* 0x8300ff04 - 0x8300FFFF is reserved for future expansion */
+
+/* OEM Service Calls version numbers */
+#define OEM_VERSION_MAJOR		0x0
+#define OEM_VERSION_MINOR		0x1
+
+/* The macros below are used to identify OEM calls from the SMC function ID */
+/* SMC32 ID range from 0x83000000 to 0x83000FFF */
+/* SMC64 ID range from 0xC3000000 to 0xC3000FFF */
+#define OEM_FID_MASK			0xf000u
+#define OEM_FID_VALUE			0u
+#define is_oem_fid(_fid) \
+	(((_fid) & OEM_FID_MASK) == OEM_FID_VALUE)
+
+#define OEM_SVC_E_SUCCESS		0
+#define OEM_SVC_E_NOT_SUPPORTED		-1
+#define OEM_SVC_E_INVALID_PARAMS	-2
+
+#endif /* __OEM_SVC_H__ */
diff --git a/plat/mediatek/common/mtk_plat_common.c b/plat/mediatek/common/mtk_plat_common.c
new file mode 100644
index 0000000..6ffbfe0
--- /dev/null
+++ b/plat/mediatek/common/mtk_plat_common.c
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <arch_helpers.h>
+#include <arm_gic.h>
+#include <bl_common.h>
+#include <cci.h>
+#include <console.h>
+#include <debug.h>
+#include <mmio.h>
+#include <mtk_plat_common.h>
+#include <mtk_sip_svc.h>
+#include <platform.h>
+#include <plat_private.h>
+#include <xlat_tables.h>
+
+struct atf_arg_t gteearg;
+
+void clean_top_32b_of_param(uint32_t smc_fid,
+				uint64_t *px1,
+				uint64_t *px2,
+				uint64_t *px3,
+				uint64_t *px4)
+{
+	/* if parameters from SMC32. Clean top 32 bits */
+	if (0 == (smc_fid & SMC_AARCH64_BIT)) {
+		*px1 = *px1 & SMC32_PARAM_MASK;
+		*px2 = *px2 & SMC32_PARAM_MASK;
+		*px3 = *px3 & SMC32_PARAM_MASK;
+		*px4 = *px4 & SMC32_PARAM_MASK;
+	}
+}
+
+#if MTK_SIP_KERNEL_BOOT_ENABLE
+static struct kernel_info k_info;
+
+static void save_kernel_info(uint64_t pc,
+			uint64_t r0,
+			uint64_t r1,
+			uint64_t k32_64)
+{
+	k_info.k32_64 = k32_64;
+	k_info.pc = pc;
+
+	if (LINUX_KERNEL_32 ==  k32_64) {
+		/* for 32 bits kernel */
+		k_info.r0 = 0;
+		/* machtype */
+		k_info.r1 = r0;
+		/* tags */
+		k_info.r2 = r1;
+	} else {
+		/* for 64 bits kernel */
+		k_info.r0 = r0;
+		k_info.r1 = r1;
+	}
+}
+
+uint64_t get_kernel_info_pc(void)
+{
+	return k_info.pc;
+}
+
+uint64_t get_kernel_info_r0(void)
+{
+	return k_info.r0;
+}
+
+uint64_t get_kernel_info_r1(void)
+{
+	return k_info.r1;
+}
+
+uint64_t get_kernel_info_r2(void)
+{
+	return k_info.r2;
+}
+
+void boot_to_kernel(uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4)
+{
+	static uint8_t kernel_boot_once_flag;
+	/* only support in booting flow */
+	if (0 == kernel_boot_once_flag) {
+		kernel_boot_once_flag = 1;
+
+		console_init(gteearg.atf_log_port,
+			UART_CLOCK, UART_BAUDRATE);
+		INFO("save kernel info\n");
+		save_kernel_info(x1, x2, x3, x4);
+		bl31_prepare_kernel_entry(x4);
+		INFO("el3_exit\n");
+		console_uninit();
+	}
+}
+#endif
+
+uint32_t plat_get_spsr_for_bl33_entry(void)
+{
+	unsigned int mode;
+	uint32_t spsr;
+	unsigned int ee;
+	unsigned long daif;
+
+	INFO("Secondary bootloader is AArch32\n");
+	mode = MODE32_svc;
+	ee = 0;
+	/*
+	 * TODO: Choose async. exception bits if HYP mode is not
+	 * implemented according to the values of SCR.{AW, FW} bits
+	 */
+	daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+
+	spsr = SPSR_MODE32(mode, 0, ee, daif);
+	return spsr;
+}
diff --git a/plat/mediatek/common/mtk_plat_common.h b/plat/mediatek/common/mtk_plat_common.h
new file mode 100644
index 0000000..a3420f3
--- /dev/null
+++ b/plat/mediatek/common/mtk_plat_common.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __MTK_PLAT_COMMON_H__
+#define __MTK_PLAT_COMMON_H__
+#include <stdint.h>
+/*******************************************************************************
+ * Function and variable prototypes
+ ******************************************************************************/
+#define DEVINFO_SIZE 4
+#define LINUX_KERNEL_32 0
+#define SMC32_PARAM_MASK		(0xFFFFFFFF)
+
+struct atf_arg_t {
+	unsigned int atf_magic;
+	unsigned int tee_support;
+	unsigned int tee_entry;
+	unsigned int tee_boot_arg_addr;
+	unsigned int hwuid[4];     /* HW Unique id for t-base used */
+	unsigned int HRID[2];      /* HW random id for t-base used */
+	unsigned int atf_log_port;
+	unsigned int atf_log_baudrate;
+	unsigned int atf_log_buf_start;
+	unsigned int atf_log_buf_size;
+	unsigned int atf_irq_num;
+	unsigned int devinfo[DEVINFO_SIZE];
+	unsigned int atf_aee_debug_buf_start;
+	unsigned int atf_aee_debug_buf_size;
+};
+
+struct kernel_info {
+	uint64_t pc;
+	uint64_t r0;
+	uint64_t r1;
+	uint64_t r2;
+	uint64_t k32_64;
+};
+
+struct mtk_bl_param_t {
+	uint64_t bootarg_loc;
+	uint64_t bootarg_size;
+	uint64_t bl33_start_addr;
+	uint64_t tee_info_addr;
+};
+
+/* Declarations for mtk_plat_common.c */
+uint32_t plat_get_spsr_for_bl32_entry(void);
+uint32_t plat_get_spsr_for_bl33_entry(void);
+void clean_top_32b_of_param(uint32_t smc_fid, uint64_t *x1,
+				uint64_t *x2,
+				uint64_t *x3,
+				uint64_t *x4);
+void bl31_prepare_kernel_entry(uint64_t k32_64);
+void enable_ns_access_to_cpuectlr(void);
+void boot_to_kernel(uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4);
+uint64_t get_kernel_info_pc(void);
+uint64_t get_kernel_info_r0(void);
+uint64_t get_kernel_info_r1(void);
+uint64_t get_kernel_info_r2(void);
+
+extern struct atf_arg_t gteearg;
+#endif
diff --git a/plat/mediatek/common/mtk_sip_svc.c b/plat/mediatek/common/mtk_sip_svc.c
index cb10af5..021511e 100644
--- a/plat/mediatek/common/mtk_sip_svc.c
+++ b/plat/mediatek/common/mtk_sip_svc.c
@@ -28,8 +28,12 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 #include <assert.h>
+#include <console.h>
 #include <debug.h>
+#include <mmio.h>
+#include <mtk_plat_common.h>
 #include <mtk_sip_svc.h>
+#include <plat_sip_calls.h>
 #include <runtime_svc.h>
 #include <uuid.h>
 
@@ -38,41 +42,65 @@
 		0xf7582ba4, 0x4262, 0x4d7d, 0x80, 0xe5,
 		0x8f, 0x95, 0x05, 0x00, 0x0f, 0x3d);
 
+#pragma weak mediatek_plat_sip_handler
+uint64_t mediatek_plat_sip_handler(uint32_t smc_fid,
+				uint64_t x1,
+				uint64_t x2,
+				uint64_t x3,
+				uint64_t x4,
+				void *cookie,
+				void *handle,
+				uint64_t flags)
+{
+	ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
+	SMC_RET1(handle, SMC_UNK);
+}
+
 /*
  * This function handles Mediatek defined SiP Calls */
-static uint64_t mediatek_sip_handler(uint32_t smc_fid,
-				     uint64_t x1,
-				     uint64_t x2,
-				     uint64_t x3,
-				     uint64_t x4,
-				     void *cookie,
-				     void *handle)
+uint64_t mediatek_sip_handler(uint32_t smc_fid,
+			uint64_t x1,
+			uint64_t x2,
+			uint64_t x3,
+			uint64_t x4,
+			void *cookie,
+			void *handle,
+			uint64_t flags)
 {
-	uint64_t ret;
-
-	switch (smc_fid) {
-	case MTK_SIP_SET_AUTHORIZED_SECURE_REG:
-		ret = mt_sip_set_authorized_sreg((uint32_t)x1, (uint32_t)x2);
-		SMC_RET1(handle, ret);
-
-	case MTK_SIP_PWR_ON_MTCMOS:
-		ret = mt_sip_pwr_on_mtcmos((uint32_t)x1);
-		SMC_RET1(handle, ret);
+	uint32_t ns;
 
-	case MTK_SIP_PWR_OFF_MTCMOS:
-		ret = mt_sip_pwr_off_mtcmos((uint32_t)x1);
-		SMC_RET1(handle, ret);
+	/* if parameter is sent from SMC32. Clean top 32 bits */
+	clean_top_32b_of_param(smc_fid, &x1, &x2, &x3, &x4);
 
-	case MTK_SIP_PWR_MTCMOS_SUPPORT:
-		ret = mt_sip_pwr_mtcmos_support();
-		SMC_RET1(handle, ret);
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+	if (!ns) {
+		/* SiP SMC service secure world's call */
+		;
+	} else {
+		/* SiP SMC service normal world's call */
+		switch (smc_fid) {
+#if MTK_SIP_SET_AUTHORIZED_SECURE_REG_ENABLE
+		case MTK_SIP_SET_AUTHORIZED_SECURE_REG: {
+			/* only use ret here */
+			uint64_t ret;
 
-	default:
-		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
-		break;
+			ret = mt_sip_set_authorized_sreg((uint32_t)x1,
+				(uint32_t)x2);
+			SMC_RET1(handle, ret);
+		}
+#endif
+#if MTK_SIP_KERNEL_BOOT_ENABLE
+		case MTK_SIP_KERNEL_BOOT_AARCH32:
+			boot_to_kernel(x1, x2, x3, x4);
+			SMC_RET0(handle);
+#endif
+		}
 	}
 
-	SMC_RET1(handle, SMC_UNK);
+	return mediatek_plat_sip_handler(smc_fid, x1, x2, x3, x4,
+					cookie, handle, flags);
+
 }
 
 /*
@@ -87,17 +115,11 @@
 			 void *handle,
 			 uint64_t flags)
 {
-	uint32_t ns;
-
-	/* Determine which security state this SMC originated from */
-	ns = is_caller_non_secure(flags);
-	if (!ns)
-		SMC_RET1(handle, SMC_UNK);
-
 	switch (smc_fid) {
 	case SIP_SVC_CALL_COUNT:
 		/* Return the number of Mediatek SiP Service Calls. */
-		SMC_RET1(handle, MTK_SIP_NUM_CALLS);
+		SMC_RET1(handle,
+			 MTK_COMMON_SIP_NUM_CALLS + MTK_PLAT_SIP_NUM_CALLS);
 
 	case SIP_SVC_UID:
 		/* Return UID to the caller */
@@ -110,7 +132,7 @@
 
 	default:
 		return mediatek_sip_handler(smc_fid, x1, x2, x3, x4,
-			cookie, handle);
+			cookie, handle, flags);
 	}
 }
 
diff --git a/plat/mediatek/common/mtk_sip_svc.h b/plat/mediatek/common/mtk_sip_svc.h
index 94a6036..703e49a 100644
--- a/plat/mediatek/common/mtk_sip_svc.h
+++ b/plat/mediatek/common/mtk_sip_svc.h
@@ -42,19 +42,27 @@
 #define MTK_SIP_SVC_VERSION_MAJOR	0x0
 #define MTK_SIP_SVC_VERSION_MINOR	0x1
 
+#define SMC_AARCH64_BIT		0x40000000
+
 /* Number of Mediatek SiP Calls implemented */
-#define MTK_SIP_NUM_CALLS		4
+#define MTK_COMMON_SIP_NUM_CALLS	4
 
 /* Mediatek SiP Service Calls function IDs */
 #define MTK_SIP_SET_AUTHORIZED_SECURE_REG	0x82000001
-#define MTK_SIP_PWR_ON_MTCMOS			0x82000402
-#define MTK_SIP_PWR_OFF_MTCMOS			0x82000403
-#define MTK_SIP_PWR_MTCMOS_SUPPORT		0x82000404
+
+/* For MTK SMC from Secure OS */
+/* 0x82000000 - 0x820000FF & 0xC2000000 - 0xC20000FF */
+#define MTK_SIP_KERNEL_BOOT_AARCH32		0x82000200
+#define MTK_SIP_KERNEL_BOOT_AARCH64		0xC2000200
 
 /* Mediatek SiP Calls error code */
 enum {
 	MTK_SIP_E_SUCCESS = 0,
-	MTK_SIP_E_INVALID_PARAM = -1
+	MTK_SIP_E_INVALID_PARAM = -1,
+	MTK_SIP_E_NOT_SUPPORTED = -2,
+	MTK_SIP_E_INVALID_RANGE = -3,
+	MTK_SIP_E_PERMISSION_DENY = -4,
+	MTK_SIP_E_LOCK_FAIL = -5
 };
 
 /*
@@ -65,7 +73,5 @@
  * Return MTK_SIP_E_SUCCESS on success, and MTK_SIP_E_INVALID_PARAM on failure.
  */
 uint64_t mt_sip_set_authorized_sreg(uint32_t sreg, uint32_t val);
-uint64_t mt_sip_pwr_on_mtcmos(uint32_t val);
-uint64_t mt_sip_pwr_off_mtcmos(uint32_t val);
-uint64_t mt_sip_pwr_mtcmos_support(void);
+
 #endif /* __PLAT_SIP_SVC_H__ */
diff --git a/plat/mediatek/mt8173/aarch64/platform_common.c b/plat/mediatek/mt8173/aarch64/platform_common.c
index 23116f5..365df1b 100644
--- a/plat/mediatek/mt8173/aarch64/platform_common.c
+++ b/plat/mediatek/mt8173/aarch64/platform_common.c
@@ -84,7 +84,7 @@
 /* Define EL3 variants of the function initialising the MMU */
 DEFINE_CONFIGURE_MMU_EL(3)
 
-uint64_t plat_get_syscnt_freq(void)
+unsigned int plat_get_syscnt_freq2(void)
 {
 	return SYS_COUNTER_FREQ_IN_TICKS;
 }
diff --git a/plat/mediatek/mt8173/bl31_plat_setup.c b/plat/mediatek/mt8173/bl31_plat_setup.c
index 749009e..4626f81 100644
--- a/plat/mediatek/mt8173/bl31_plat_setup.c
+++ b/plat/mediatek/mt8173/bl31_plat_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -32,6 +32,7 @@
 #include <bl_common.h>
 #include <console.h>
 #include <debug.h>
+#include <generic_delay_timer.h>
 #include <mcucfg.h>
 #include <mmio.h>
 #include <mtcmos.h>
@@ -167,7 +168,7 @@
 	platform_setup_cpu();
 	platform_setup_sram();
 
-	plat_delay_timer_init();
+	generic_delay_timer_init();
 
 	/* Initialize the gic cpu and distributor interfaces */
 	plat_mt_gic_init();
diff --git a/plat/mediatek/mt8173/drivers/crypt/crypt.c b/plat/mediatek/mt8173/drivers/crypt/crypt.c
new file mode 100644
index 0000000..2fca0f5
--- /dev/null
+++ b/plat/mediatek/mt8173/drivers/crypt/crypt.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <delay_timer.h>
+#include <mmio.h>
+#include <mt8173_def.h>
+#include <mtk_sip_svc.h>
+
+#define crypt_read32(offset)	\
+	mmio_read_32((uintptr_t)(CRYPT_BASE+((offset) * 4)))
+
+#define crypt_write32(offset, value)    \
+	mmio_write_32((uintptr_t)(CRYPT_BASE + ((offset) * 4)), (uint32_t)value)
+
+#define GET_L32(x) ((uint32_t)(x & 0xffffffff))
+#define GET_H32(x) ((uint32_t)((x >> 32) & 0xffffffff))
+
+#define REG_INIT 0
+#define REG_MSC 4
+#define REG_TRIG 256
+#define REG_STAT 512
+#define REG_CLR 513
+#define REG_INT 514
+#define REG_P68 768
+#define REG_P69 769
+#define REG_P70 770
+#define REG_P71 771
+#define REG_P72 772
+#define REG_D20 820
+#define KEY_SIZE 160
+#define KEY_LEN 40
+
+/* Wait until crypt is completed */
+uint64_t crypt_wait(void)
+{
+	crypt_write32(REG_TRIG, 0);
+	while (crypt_read32(REG_STAT) == 0)
+		;
+	udelay(100);
+	crypt_write32(REG_CLR, crypt_read32(REG_STAT));
+	crypt_write32(REG_INT, 0);
+	return MTK_SIP_E_SUCCESS;
+}
+
+static uint32_t record[4];
+/* Copy encrypted key to crypt engine */
+uint64_t crypt_set_hdcp_key_ex(uint64_t x1, uint64_t x2, uint64_t x3)
+{
+	uint32_t i = (uint32_t)x1;
+	uint32_t j = 0;
+
+	if (i > KEY_LEN)
+		return MTK_SIP_E_INVALID_PARAM;
+
+	if (i < KEY_LEN) {
+		crypt_write32(REG_MSC, 0x80ff3800);
+		crypt_write32(REG_INIT, 0);
+		crypt_write32(REG_INIT, 0xF);
+		crypt_write32(REG_CLR, 1);
+		crypt_write32(REG_INT, 0);
+
+		crypt_write32(REG_P68, 0x70);
+		crypt_write32(REG_P69, 0x1C0);
+		crypt_write32(REG_P70, 0x30);
+		crypt_write32(REG_P71, 0x4);
+		crypt_wait();
+
+		crypt_write32(REG_D20 + 4 * i, GET_L32(x2));
+		crypt_write32(REG_D20 + 4 * i + 1, GET_H32(x2));
+		crypt_write32(REG_D20 + 4 * i + 2, GET_L32(x3));
+		crypt_write32(REG_D20 + 4 * i + 3, GET_H32(x3));
+
+		crypt_write32(REG_P69, 0);
+		crypt_write32(REG_P68, 0x20);
+		crypt_write32(REG_P71, 0x34 + 4 * i);
+		crypt_write32(REG_P72, 0x34 + 4 * i);
+		crypt_wait();
+
+		for (j = 0; j < 4; j++) {
+			crypt_write32(REG_P68, 0x71);
+			crypt_write32(REG_P69, 0x34 + 4 * i + j);
+			crypt_write32(REG_P70, record[j]);
+			crypt_wait();
+		}
+	}
+	/* Prepare data for next iteration */
+	record[0] = GET_L32(x2);
+	record[1] = GET_H32(x2);
+	record[2] = GET_L32(x3);
+	record[3] = GET_H32(x3);
+	return MTK_SIP_E_SUCCESS;
+}
+
+/* Set key to hdcp */
+uint64_t crypt_set_hdcp_key_num(uint32_t num)
+{
+	if (num > KEY_LEN)
+		return MTK_SIP_E_INVALID_PARAM;
+
+	crypt_write32(REG_P68, 0x6A);
+	crypt_write32(REG_P69, 0x34 + 4 * num);
+	crypt_wait();
+	return MTK_SIP_E_SUCCESS;
+}
+
+/* Clear key in crypt engine */
+uint64_t crypt_clear_hdcp_key(void)
+{
+	uint32_t i;
+
+	for (i = 0; i < KEY_SIZE; i++)
+		crypt_write32(REG_D20 + i, 0);
+	return MTK_SIP_E_SUCCESS;
+}
diff --git a/plat/rockchip/common/plat_delay_timer.c b/plat/mediatek/mt8173/drivers/crypt/crypt.h
similarity index 72%
copy from plat/rockchip/common/plat_delay_timer.c
copy to plat/mediatek/mt8173/drivers/crypt/crypt.h
index 797ce05..25e3077 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/plat/mediatek/mt8173/drivers/crypt/crypt.h
@@ -27,28 +27,14 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#ifndef __CRYPT_H__
+#define __CRYPT_H__
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#include <stdint.h>
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
-
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+/* crypt function prototype */
+uint64_t crypt_set_hdcp_key_ex(uint64_t x1, uint64_t x2, uint64_t x3);
+uint64_t crypt_set_hdcp_key_num(uint32_t num);
+uint64_t crypt_clear_hdcp_key(void);
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /* __CRYPT_H__ */
diff --git a/plat/mediatek/mt8173/include/mt8173_def.h b/plat/mediatek/mt8173/include/mt8173_def.h
index 0113d60..87e9c04 100644
--- a/plat/mediatek/mt8173/include/mt8173_def.h
+++ b/plat/mediatek/mt8173/include/mt8173_def.h
@@ -50,6 +50,7 @@
 #define MCUCFG_BASE		(IO_PHYS + 0x200000)
 #define APMIXED_BASE		(IO_PHYS + 0x209000)
 #define TRNG_BASE		(IO_PHYS + 0x20F000)
+#define CRYPT_BASE		(IO_PHYS + 0x210000)
 #define MT_GIC_BASE		(IO_PHYS + 0x220000)
 #define PLAT_MT_CCI_BASE	(IO_PHYS + 0x390000)
 
@@ -82,7 +83,6 @@
  * System counter frequency related constants
  ******************************************************************************/
 #define SYS_COUNTER_FREQ_IN_TICKS	13000000
-#define SYS_COUNTER_FREQ_IN_MHZ		13
 
 /*******************************************************************************
  * GIC-400 & interrupt handling related constants
diff --git a/plat/mediatek/mt8173/include/plat_private.h b/plat/mediatek/mt8173/include/plat_private.h
index bdde6a6..ae50e44 100644
--- a/plat/mediatek/mt8173/include/plat_private.h
+++ b/plat/mediatek/mt8173/include/plat_private.h
@@ -51,6 +51,4 @@
 /* Declarations for plat_topology.c */
 int mt_setup_topology(void);
 
-void plat_delay_timer_init(void);
-
 #endif /* __PLAT_PRIVATE_H__ */
diff --git a/plat/rockchip/common/plat_delay_timer.c b/plat/mediatek/mt8173/include/plat_sip_calls.h
similarity index 72%
copy from plat/rockchip/common/plat_delay_timer.c
copy to plat/mediatek/mt8173/include/plat_sip_calls.h
index 797ce05..0fd84b2 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/plat/mediatek/mt8173/include/plat_sip_calls.h
@@ -28,27 +28,19 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#ifndef __PLAT_SIP_CALLS_H__
+#define __PLAT_SIP_CALLS_H__
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
+/*******************************************************************************
+ * Plat SiP function constants
+ ******************************************************************************/
+#define MTK_PLAT_SIP_NUM_CALLS	6
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+#define MTK_SIP_PWR_ON_MTCMOS			0x82000402
+#define MTK_SIP_PWR_OFF_MTCMOS			0x82000403
+#define MTK_SIP_PWR_MTCMOS_SUPPORT		0x82000404
+#define MTK_SIP_SET_HDCP_KEY_NUM		0x82000405
+#define MTK_SIP_CLR_HDCP_KEY			0x82000406
+#define MTK_SIP_SET_HDCP_KEY_EX			0x82000407
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /* __PLAT_SIP_CALLS_H__ */
diff --git a/plat/mediatek/mt8173/plat_delay_timer.c b/plat/mediatek/mt8173/plat_delay_timer.c
deleted file mode 100644
index cc66b80..0000000
--- a/plat/mediatek/mt8173/plat_delay_timer.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this
- * list of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * Neither the name of ARM nor the names of its contributors may be used
- * to endorse or promote products derived from this software without specific
- * prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <mt8173_def.h>
-
-static uint32_t plat_get_timer_value(void)
-{
-	/* Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter. */
-	return (uint32_t)(~read_cntpct_el0());
-}
-
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
-
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
diff --git a/plat/mediatek/mt8173/plat_sip_calls.c b/plat/mediatek/mt8173/plat_sip_calls.c
index e12892f..6969ee3 100644
--- a/plat/mediatek/mt8173/plat_sip_calls.c
+++ b/plat/mediatek/mt8173/plat_sip_calls.c
@@ -27,9 +27,13 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#include <crypt.h>
+#include <debug.h>
 #include <mmio.h>
 #include <mtk_sip_svc.h>
 #include <mtcmos.h>
+#include <plat_sip_calls.h>
+#include <runtime_svc.h>
 
 /* Authorized secure register list */
 enum {
@@ -57,7 +61,7 @@
 	return MTK_SIP_E_INVALID_PARAM;
 }
 
-uint64_t mt_sip_pwr_on_mtcmos(uint32_t val)
+static uint64_t mt_sip_pwr_on_mtcmos(uint32_t val)
 {
 	uint32_t ret;
 
@@ -68,7 +72,7 @@
 		return MTK_SIP_E_SUCCESS;
 }
 
-uint64_t mt_sip_pwr_off_mtcmos(uint32_t val)
+static uint64_t mt_sip_pwr_off_mtcmos(uint32_t val)
 {
 	uint32_t ret;
 
@@ -79,7 +83,51 @@
 		return MTK_SIP_E_SUCCESS;
 }
 
-uint64_t mt_sip_pwr_mtcmos_support(void)
+static uint64_t mt_sip_pwr_mtcmos_support(void)
 {
 	return MTK_SIP_E_SUCCESS;
 }
+
+uint64_t mediatek_plat_sip_handler(uint32_t smc_fid,
+				   uint64_t x1,
+				   uint64_t x2,
+				   uint64_t x3,
+				   uint64_t x4,
+				   void *cookie,
+				   void *handle,
+				   uint64_t flags)
+{
+	uint64_t ret;
+
+	switch (smc_fid) {
+	case MTK_SIP_PWR_ON_MTCMOS:
+		ret = mt_sip_pwr_on_mtcmos((uint32_t)x1);
+		SMC_RET1(handle, ret);
+
+	case MTK_SIP_PWR_OFF_MTCMOS:
+		ret = mt_sip_pwr_off_mtcmos((uint32_t)x1);
+		SMC_RET1(handle, ret);
+
+	case MTK_SIP_PWR_MTCMOS_SUPPORT:
+		ret = mt_sip_pwr_mtcmos_support();
+		SMC_RET1(handle, ret);
+
+	case MTK_SIP_SET_HDCP_KEY_EX:
+		ret = crypt_set_hdcp_key_ex(x1, x2, x3);
+		SMC_RET1(handle, ret);
+
+	case MTK_SIP_SET_HDCP_KEY_NUM:
+		ret = crypt_set_hdcp_key_num((uint32_t)x1);
+		SMC_RET1(handle, ret);
+
+	case MTK_SIP_CLR_HDCP_KEY:
+		ret = crypt_clear_hdcp_key();
+		SMC_RET1(handle, ret);
+
+	default:
+		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
+		break;
+	}
+
+	SMC_RET1(handle, SMC_UNK);
+}
diff --git a/plat/mediatek/mt8173/platform.mk b/plat/mediatek/mt8173/platform.mk
index f0451b9..8f48230 100644
--- a/plat/mediatek/mt8173/platform.mk
+++ b/plat/mediatek/mt8173/platform.mk
@@ -32,6 +32,7 @@
 MTK_PLAT_SOC		:=	${MTK_PLAT}/${PLAT}
 
 PLAT_INCLUDES		:=	-I${MTK_PLAT}/common/				\
+				-I${MTK_PLAT_SOC}/drivers/crypt/		\
 				-I${MTK_PLAT_SOC}/drivers/mtcmos/		\
 				-I${MTK_PLAT_SOC}/drivers/pmic/			\
 				-I${MTK_PLAT_SOC}/drivers/rtc/			\
@@ -51,15 +52,18 @@
 				drivers/arm/gic/gic_v3.c			\
 				drivers/console/console.S			\
 				drivers/delay_timer/delay_timer.c		\
+				drivers/delay_timer/generic_delay_timer.c	\
 				lib/cpus/aarch64/aem_generic.S			\
 				lib/cpus/aarch64/cortex_a53.S			\
 				lib/cpus/aarch64/cortex_a57.S			\
 				lib/cpus/aarch64/cortex_a72.S			\
 				plat/common/aarch64/platform_mp_stack.S		\
+				${MTK_PLAT}/common/mtk_plat_common.c		\
 				${MTK_PLAT}/common/mtk_sip_svc.c		\
 				${MTK_PLAT_SOC}/aarch64/plat_helpers.S		\
 				${MTK_PLAT_SOC}/aarch64/platform_common.c	\
 				${MTK_PLAT_SOC}/bl31_plat_setup.c		\
+				${MTK_PLAT_SOC}/drivers/crypt/crypt.c		\
 				${MTK_PLAT_SOC}/drivers/mtcmos/mtcmos.c		\
 				${MTK_PLAT_SOC}/drivers/pmic/pmic_wrap_init.c	\
 				${MTK_PLAT_SOC}/drivers/rtc/rtc.c		\
@@ -69,7 +73,6 @@
 				${MTK_PLAT_SOC}/drivers/spm/spm_suspend.c	\
 				${MTK_PLAT_SOC}/drivers/timer/mt_cpuxgpt.c	\
 				${MTK_PLAT_SOC}/drivers/uart/8250_console.S	\
-				${MTK_PLAT_SOC}/plat_delay_timer.c		\
 				${MTK_PLAT_SOC}/plat_mt_gic.c			\
 				${MTK_PLAT_SOC}/plat_pm.c			\
 				${MTK_PLAT_SOC}/plat_sip_calls.c		\
@@ -88,3 +91,5 @@
 
 # indicate the reset vector address can be programmed
 PROGRAMMABLE_RESET_ADDRESS	:=	1
+
+$(eval $(call add_define,MTK_SIP_SET_AUTHORIZED_SECURE_REG_ENABLE))
diff --git a/plat/nvidia/tegra/include/tegra_private.h b/plat/nvidia/tegra/include/tegra_private.h
index 6fa943f..cf75d9f 100644
--- a/plat/nvidia/tegra/include/tegra_private.h
+++ b/plat/nvidia/tegra/include/tegra_private.h
@@ -52,7 +52,6 @@
 
 /* Declarations for plat_setup.c */
 const mmap_region_t *plat_get_mmio_map(void);
-uint64_t plat_get_syscnt_freq(void);
 
 /* Declarations for plat_secondary.c */
 void plat_secondary_setup(void);
diff --git a/plat/nvidia/tegra/soc/t132/plat_setup.c b/plat/nvidia/tegra/soc/t132/plat_setup.c
index 6ff2831..0d66413 100644
--- a/plat/nvidia/tegra/soc/t132/plat_setup.c
+++ b/plat/nvidia/tegra/soc/t132/plat_setup.c
@@ -74,7 +74,7 @@
 	return tegra_mmap;
 }
 
-uint64_t plat_get_syscnt_freq(void)
+unsigned int plat_get_syscnt_freq2(void)
 {
 	return 12000000;
 }
diff --git a/plat/nvidia/tegra/soc/t210/plat_setup.c b/plat/nvidia/tegra/soc/t210/plat_setup.c
index 3fce8a2..70a55c6 100644
--- a/plat/nvidia/tegra/soc/t210/plat_setup.c
+++ b/plat/nvidia/tegra/soc/t210/plat_setup.c
@@ -80,7 +80,7 @@
 /*******************************************************************************
  * Handler to get the System Counter Frequency
  ******************************************************************************/
-uint64_t plat_get_syscnt_freq(void)
+unsigned int plat_get_syscnt_freq2(void)
 {
 	return 19200000;
 }
diff --git a/plat/qemu/aarch64/plat_helpers.S b/plat/qemu/aarch64/plat_helpers.S
new file mode 100644
index 0000000..31d2c68
--- /dev/null
+++ b/plat/qemu/aarch64/plat_helpers.S
@@ -0,0 +1,140 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <platform_def.h>
+
+	.globl	plat_my_core_pos
+	.globl	plat_get_my_entrypoint
+	.globl	platform_mem_init
+	.globl	plat_qemu_calc_core_pos
+	.globl	plat_crash_console_init
+	.globl	plat_crash_console_putc
+	.globl  plat_secondary_cold_boot_setup
+	.globl  plat_get_my_entrypoint
+	.globl  plat_is_my_cpu_primary
+
+
+func plat_my_core_pos
+	mrs	x0, mpidr_el1
+	b	plat_qemu_calc_core_pos
+endfunc plat_my_core_pos
+
+/*
+ *  unsigned int plat_qemu_calc_core_pos(u_register_t mpidr);
+ *  With this function: CorePos = (ClusterId * 4) + CoreId
+ */
+func plat_qemu_calc_core_pos
+	and	x1, x0, #MPIDR_CPU_MASK
+	and	x0, x0, #MPIDR_CLUSTER_MASK
+	add	x0, x1, x0, LSR #6
+	ret
+endfunc plat_qemu_calc_core_pos
+
+	/* -----------------------------------------------------
+	 * unsigned int plat_is_my_cpu_primary (void);
+	 *
+	 * Find out whether the current cpu is the primary
+	 * cpu.
+	 * -----------------------------------------------------
+	 */
+func plat_is_my_cpu_primary
+	mrs	x0, mpidr_el1
+	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
+	cmp	x0, #QEMU_PRIMARY_CPU
+	cset	w0, eq
+	ret
+endfunc plat_is_my_cpu_primary
+
+	/* -----------------------------------------------------
+	 * void plat_secondary_cold_boot_setup (void);
+	 *
+	 * This function performs any platform specific actions
+	 * needed for a secondary cpu after a cold reset e.g
+	 * mark the cpu's presence, mechanism to place it in a
+	 * holding pen etc.
+	 * -----------------------------------------------------
+	 */
+func plat_secondary_cold_boot_setup
+	/* Calculate address of our hold entry */
+	bl	plat_my_core_pos
+	mov_imm	x2, PLAT_QEMU_HOLD_BASE
+
+	/* Wait until we have a go */
+poll_mailbox:
+	ldr	x1, [x2, x0]
+	cbz	x1, 1f
+	mov_imm	x0, PLAT_QEMU_TRUSTED_MAILBOX_BASE
+	ldr	x1, [x0]
+	br	x1
+1:
+	wfe
+	b	poll_mailbox
+endfunc plat_secondary_cold_boot_setup
+
+func plat_get_my_entrypoint
+	/* TODO support warm boot */
+	mov	x0, #0
+	ret
+endfunc plat_get_my_entrypoint
+
+func platform_mem_init
+	ret
+endfunc platform_mem_init
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_init(void)
+	 * Function to initialize the crash console
+	 * without a C Runtime to print crash report.
+	 * Clobber list : x0, x1, x2
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_init
+	mov_imm	x0, PLAT_QEMU_CRASH_UART_BASE
+	mov_imm	x1, PLAT_QEMU_CRASH_UART_CLK_IN_HZ
+	mov_imm	x2, PLAT_QEMU_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 : x1, x2
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_putc
+	mov_imm	x1, PLAT_QEMU_CRASH_UART_BASE
+	b	console_core_putc
+endfunc plat_crash_console_putc
+
+
diff --git a/plat/qemu/dt.c b/plat/qemu/dt.c
new file mode 100644
index 0000000..647ee01
--- /dev/null
+++ b/plat/qemu/dt.c
@@ -0,0 +1,119 @@
+/*
+ * 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 <console.h>
+#include <debug.h>
+#include <libfdt.h>
+#include <psci.h>
+#include "qemu_private.h"
+#include <string.h>
+
+static int append_psci_compatible(void *fdt, int offs, const char *str)
+{
+	return fdt_appendprop(fdt, offs, "compatible", str, strlen(str) + 1);
+}
+
+int dt_add_psci_node(void *fdt)
+{
+	int offs;
+
+	if (fdt_path_offset(fdt, "/psci") >= 0) {
+		WARN("PSCI Device Tree node already exists!\n");
+		return 0;
+	}
+
+	offs = fdt_path_offset(fdt, "/");
+	if (offs < 0)
+		return -1;
+	offs = fdt_add_subnode(fdt, offs, "psci");
+	if (offs < 0)
+		return -1;
+	if (append_psci_compatible(fdt, offs, "arm,psci-1.0"))
+		return -1;
+	if (append_psci_compatible(fdt, offs, "arm,psci-0.2"))
+		return -1;
+	if (append_psci_compatible(fdt, offs, "arm,psci"))
+		return -1;
+	if (fdt_setprop_string(fdt, offs, "method", "smc"))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "cpu_suspend", PSCI_CPU_SUSPEND_AARCH64))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "cpu_off", PSCI_CPU_OFF))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "cpu_on", PSCI_CPU_ON_AARCH64))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "sys_poweroff", PSCI_SYSTEM_OFF))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "sys_reset", PSCI_SYSTEM_RESET))
+		return -1;
+	return 0;
+}
+
+static int check_node_compat_prefix(void *fdt, int offs, const char *prefix)
+{
+	const size_t prefix_len = strlen(prefix);
+	size_t l;
+	int plen;
+	const char *prop;
+
+	prop = fdt_getprop(fdt, offs, "compatible", &plen);
+	if (!prop)
+		return -1;
+
+	while (plen > 0) {
+		if (memcmp(prop, prefix, prefix_len) == 0)
+			return 0; /* match */
+
+		l = strlen(prop) + 1;
+		prop += l;
+		plen -= l;
+	}
+
+	return -1;
+}
+
+int dt_add_psci_cpu_enable_methods(void *fdt)
+{
+	int offs = 0;
+
+	while (1) {
+		offs = fdt_next_node(fdt, offs, NULL);
+		if (offs < 0)
+			break;
+		if (fdt_getprop(fdt, offs, "enable-method", NULL))
+			continue; /* already set */
+		if (check_node_compat_prefix(fdt, offs, "arm,cortex-a"))
+			continue; /* no compatible */
+		if (fdt_setprop_string(fdt, offs, "enable-method", "psci"))
+			return -1;
+		/* Need to restart scanning as offsets may have changed */
+		offs = 0;
+	}
+	return 0;
+}
diff --git a/plat/rockchip/common/plat_delay_timer.c b/plat/qemu/include/plat_macros.S
similarity index 73%
rename from plat/rockchip/common/plat_delay_timer.c
rename to plat/qemu/include/plat_macros.S
index 797ce05..648d8ec 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/plat/qemu/include/plat_macros.S
@@ -27,28 +27,24 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#ifndef __PLAT_MACROS_S__
+#define __PLAT_MACROS_S__
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
+#include <arm_macros.S>
 #include <platform_def.h>
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
+	/* ---------------------------------------------
+	 * The below required platform porting macro
+	 * prints out relevant GIC and CCI registers
+	 * whenever an unhandled exception is taken in
+	 * BL31.
+	 * Clobbers: x0 - x10, x16, x17, sp
+	 * ---------------------------------------------
 	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
-
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+	.macro plat_crash_print_regs
+	mov_imm	x17, GICC_BASE
+	mov_imm	x16, GICD_BASE
+	arm_print_gic_regs
+	.endm
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /* __PLAT_MACROS_S__ */
diff --git a/plat/qemu/include/platform_def.h b/plat/qemu/include/platform_def.h
new file mode 100644
index 0000000..d983ce6
--- /dev/null
+++ b/plat/qemu/include/platform_def.h
@@ -0,0 +1,244 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __PLATFORM_DEF_H__
+#define __PLATFORM_DEF_H__
+
+#include <arch.h>
+#include <common_def.h>
+#include <tbbr_img_def.h>
+
+/* Special value used to verify platform parameters from BL2 to BL3-1 */
+#define QEMU_BL31_PLAT_PARAM_VAL	0x0f1e2d3c4b5a6978ULL
+
+#define PLATFORM_STACK_SIZE 0x1000
+
+#define PLATFORM_MAX_CPUS_PER_CLUSTER	4
+#define PLATFORM_CLUSTER_COUNT		2
+#define PLATFORM_CLUSTER0_CORE_COUNT	PLATFORM_MAX_CPUS_PER_CLUSTER
+#define PLATFORM_CLUSTER1_CORE_COUNT	PLATFORM_MAX_CPUS_PER_CLUSTER
+#define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER0_CORE_COUNT + \
+					 PLATFORM_CLUSTER1_CORE_COUNT)
+
+#define QEMU_PRIMARY_CPU		0
+
+#define PLAT_NUM_PWR_DOMAINS		(PLATFORM_CLUSTER_COUNT + \
+					PLATFORM_CORE_COUNT)
+#define PLAT_MAX_PWR_LVL		MPIDR_AFFLVL1
+
+#define PLAT_MAX_RET_STATE		1
+#define PLAT_MAX_OFF_STATE		2
+
+/* Local power state for power domains in Run state. */
+#define PLAT_LOCAL_STATE_RUN		0
+/* Local power state for retention. Valid only for CPU power domains */
+#define PLAT_LOCAL_STATE_RET		1
+/*
+ * Local power state for OFF/power-down. Valid for CPU and cluster power
+ * domains.
+ */
+#define PLAT_LOCAL_STATE_OFF		2
+
+/*
+ * Macros used to parse state information from State-ID if it is using the
+ * recommended encoding for State-ID.
+ */
+#define PLAT_LOCAL_PSTATE_WIDTH		4
+#define PLAT_LOCAL_PSTATE_MASK		((1 << PLAT_LOCAL_PSTATE_WIDTH) - 1)
+
+/*
+ * Some data must be aligned on the biggest cache line size in the platform.
+ * This is known only to the platform as it might have a combination of
+ * integrated and external caches.
+ */
+#define CACHE_WRITEBACK_SHIFT		6
+#define CACHE_WRITEBACK_GRANULE		(1 << CACHE_WRITEBACK_SHIFT)
+
+/*
+ * Partition memory into secure ROM, non-secure DRAM, secure "SRAM",
+ * and secure DRAM.
+ */
+#define SEC_ROM_BASE			0x00000000
+#define SEC_ROM_SIZE			0x00020000
+
+#define NS_DRAM0_BASE			0x40000000
+#define NS_DRAM0_SIZE			0x3de00000
+
+#define SEC_SRAM_BASE			0x0e000000
+#define SEC_SRAM_SIZE			0x00040000
+
+#define SEC_DRAM_BASE			0x0e100000
+#define SEC_DRAM_SIZE			0x00f00000
+
+/*
+ * ARM-TF lives in SRAM, partition it here
+ */
+
+#define SHARED_RAM_BASE			SEC_SRAM_BASE
+#define SHARED_RAM_SIZE			0x00001000
+
+#define PLAT_QEMU_TRUSTED_MAILBOX_BASE	SHARED_RAM_BASE
+#define PLAT_QEMU_TRUSTED_MAILBOX_SIZE	(8 + PLAT_QEMU_HOLD_SIZE)
+#define PLAT_QEMU_HOLD_BASE		(PLAT_QEMU_TRUSTED_MAILBOX_BASE + 8)
+#define PLAT_QEMU_HOLD_SIZE		(PLATFORM_CORE_COUNT * \
+					 PLAT_QEMU_HOLD_ENTRY_SIZE)
+#define PLAT_QEMU_HOLD_ENTRY_SIZE	8
+#define PLAT_QEMU_HOLD_STATE_WAIT	0
+#define PLAT_QEMU_HOLD_STATE_GO		1
+
+#define BL_RAM_BASE			(SHARED_RAM_BASE + SHARED_RAM_SIZE)
+#define BL_RAM_SIZE			(SEC_SRAM_SIZE - SHARED_RAM_SIZE)
+
+/*
+ * BL1 specific defines.
+ *
+ * BL1 RW data is relocated from ROM to RAM at runtime so we need 2 sets of
+ * addresses.
+ * Put BL1 RW at the top of the Secure SRAM. BL1_RW_BASE is calculated using
+ * the current BL1 RW debug size plus a little space for growth.
+ */
+#define BL1_RO_BASE			SEC_ROM_BASE
+#define BL1_RO_LIMIT			(SEC_ROM_BASE + SEC_ROM_SIZE)
+#define BL1_RW_BASE			(BL1_RW_LIMIT - 0x12000)
+#define BL1_RW_LIMIT			(BL_RAM_BASE + BL_RAM_SIZE)
+
+/*
+ * BL2 specific defines.
+ *
+ * Put BL2 just below BL3-1. BL2_BASE is calculated using the current BL2 debug
+ * size plus a little space for growth.
+ */
+#define BL2_BASE			(BL31_BASE - 0x1D000)
+#define BL2_LIMIT			BL31_BASE
+
+/*
+ * BL3-1 specific defines.
+ *
+ * Put BL3-1 at the top of the Trusted SRAM. BL31_BASE is calculated using the
+ * current BL3-1 debug size plus a little space for growth.
+ */
+#define BL31_BASE			(BL31_LIMIT - 0x20000)
+#define BL31_LIMIT			(BL_RAM_BASE + BL_RAM_SIZE)
+#define BL31_PROGBITS_LIMIT		BL1_RW_BASE
+
+
+/*
+ * BL3-2 specific defines.
+ *
+ * BL3-2 can execute from Secure SRAM, or Secure DRAM.
+ */
+#define BL32_SRAM_BASE			BL_RAM_BASE
+#define BL32_SRAM_LIMIT			BL31_BASE
+#define BL32_DRAM_BASE			SEC_DRAM_BASE
+#define BL32_DRAM_LIMIT			(SEC_DRAM_BASE + SEC_DRAM_SIZE)
+
+#define SEC_SRAM_ID			0
+#define SEC_DRAM_ID			1
+
+#if BL32_RAM_LOCATION_ID == SEC_SRAM_ID
+# define BL32_MEM_BASE			BL_RAM_BASE
+# define BL32_MEM_SIZE			BL_RAM_SIZE
+# define BL32_BASE			BL32_SRAM_BASE
+# define BL32_LIMIT			BL32_SRAM_LIMIT
+#elif BL32_RAM_LOCATION_ID == SEC_DRAM_ID
+# define BL32_MEM_BASE			SEC_DRAM_BASE
+# define BL32_MEM_SIZE			SEC_DRAM_SIZE
+# define BL32_BASE			BL32_DRAM_BASE
+# define BL32_LIMIT			BL32_DRAM_LIMIT
+#else
+# error "Unsupported BL32_RAM_LOCATION_ID value"
+#endif
+#define BL32_SIZE			(BL32_LIMIT - BL32_BASE)
+
+#define NS_IMAGE_OFFSET			0x60000000
+
+#define ADDR_SPACE_SIZE			(1ull << 32)
+#define MAX_MMAP_REGIONS		8
+#define MAX_XLAT_TABLES			6
+#define MAX_IO_DEVICES			3
+#define MAX_IO_HANDLES			4
+
+/*
+ * PL011 related constants
+ */
+#define UART0_BASE			0x09000000
+#define UART1_BASE			0x09040000
+#define UART0_CLK_IN_HZ			1
+#define UART1_CLK_IN_HZ			1
+
+#define PLAT_QEMU_BOOT_UART_BASE	UART0_BASE
+#define PLAT_QEMU_BOOT_UART_CLK_IN_HZ	UART0_CLK_IN_HZ
+
+#define PLAT_QEMU_CRASH_UART_BASE	UART1_BASE
+#define PLAT_QEMU_CRASH_UART_CLK_IN_HZ	UART1_CLK_IN_HZ
+
+#define PLAT_QEMU_CONSOLE_BAUDRATE	115200
+
+#define QEMU_FLASH0_BASE		0x04000000
+#define QEMU_FLASH0_SIZE		0x04000000
+
+#define PLAT_QEMU_FIP_BASE		QEMU_FLASH0_BASE
+#define PLAT_QEMU_FIP_MAX_SIZE		QEMU_FLASH0_SIZE
+
+#define DEVICE0_BASE			0x08000000
+#define DEVICE0_SIZE			0x00021000
+#define DEVICE1_BASE			0x09000000
+#define DEVICE1_SIZE			0x00011000
+
+/*
+ * GIC related constants
+ */
+
+#define GICD_BASE			0x8000000
+#define GICC_BASE			0x8010000
+#define GICR_BASE			0
+
+
+#define QEMU_IRQ_SEC_SGI_0		8
+#define QEMU_IRQ_SEC_SGI_1		9
+#define QEMU_IRQ_SEC_SGI_2		10
+#define QEMU_IRQ_SEC_SGI_3		11
+#define QEMU_IRQ_SEC_SGI_4		12
+#define QEMU_IRQ_SEC_SGI_5		13
+#define QEMU_IRQ_SEC_SGI_6		14
+#define QEMU_IRQ_SEC_SGI_7		15
+
+/*
+ * DT related constants
+ */
+#define PLAT_QEMU_DT_BASE		NS_DRAM0_BASE
+#define PLAT_QEMU_DT_MAX_SIZE		0x10000
+
+/*
+ * System counter
+ */
+#define SYS_COUNTER_FREQ_IN_TICKS	((1000 * 1000 * 1000) / 16)
+
+#endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/qemu/platform.mk b/plat/qemu/platform.mk
new file mode 100644
index 0000000..9542198
--- /dev/null
+++ b/plat/qemu/platform.mk
@@ -0,0 +1,99 @@
+#
+# Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# Neither the name of ARM nor the names of its contributors may be used
+# to endorse or promote products derived from this software without specific
+# prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+include lib/libfdt/libfdt.mk
+
+PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/		\
+				-Iinclude/plat/arm/common/aarch64/	\
+				-Iplat/qemu/include			\
+				-Iinclude/common/tbbr
+
+
+PLAT_BL_COMMON_SOURCES	:=	plat/qemu/qemu_common.c			\
+				drivers/arm/pl011/pl011_console.S	\
+				lib/xlat_tables/xlat_tables_common.c	\
+				lib/xlat_tables/aarch64/xlat_tables.c
+
+BL1_SOURCES		+=	drivers/io/io_semihosting.c		\
+				drivers/io/io_storage.c			\
+				drivers/io/io_fip.c			\
+				drivers/io/io_memmap.c			\
+				lib/semihosting/semihosting.c		\
+				lib/semihosting/aarch64/semihosting_call.S \
+				plat/qemu/qemu_io_storage.c		\
+				lib/cpus/aarch64/aem_generic.S		\
+				lib/cpus/aarch64/cortex_a53.S		\
+				lib/cpus/aarch64/cortex_a57.S		\
+				plat/common/aarch64/platform_mp_stack.S	\
+				plat/qemu/aarch64/plat_helpers.S	\
+				plat/qemu/qemu_bl1_setup.c
+
+BL2_SOURCES		+=	drivers/io/io_semihosting.c		\
+				drivers/io/io_storage.c			\
+				drivers/io/io_fip.c			\
+				drivers/io/io_memmap.c			\
+				plat/common/aarch64/platform_mp_stack.S	\
+				lib/semihosting/semihosting.c		\
+				lib/semihosting/aarch64/semihosting_call.S\
+				plat/qemu/qemu_io_storage.c		\
+				plat/qemu/aarch64/plat_helpers.S	\
+				plat/qemu/qemu_bl2_setup.c		\
+				plat/qemu/dt.c				\
+				$(LIBFDT_SRCS)
+
+BL31_SOURCES		+=	lib/cpus/aarch64/aem_generic.S		\
+				lib/cpus/aarch64/cortex_a53.S		\
+				lib/cpus/aarch64/cortex_a57.S		\
+				drivers/arm/gic/v2/gicv2_helpers.c	\
+				drivers/arm/gic/v2/gicv2_main.c		\
+				drivers/arm/gic/common/gic_common.c	\
+				plat/common/aarch64/platform_mp_stack.S	\
+				plat/common/aarch64/plat_psci_common.c	\
+				plat/common/aarch64/plat_common.c	\
+				plat/qemu/qemu_pm.c			\
+				plat/qemu/topology.c			\
+				plat/qemu/aarch64/plat_helpers.S	\
+				plat/qemu/qemu_bl31_setup.c		\
+				plat/qemu/qemu_gic.c
+
+# Disable the PSCI platform compatibility layer
+ENABLE_PLAT_COMPAT	:= 	0
+
+BL32_RAM_LOCATION	:=	tdram
+ifeq (${BL32_RAM_LOCATION}, tsram)
+  BL32_RAM_LOCATION_ID = SEC_SRAM_ID
+else ifeq (${BL32_RAM_LOCATION}, tdram)
+  BL32_RAM_LOCATION_ID = SEC_DRAM_ID
+else
+  $(error "Unsupported BL32_RAM_LOCATION value")
+endif
+
+# Process flags
+$(eval $(call add_define,BL32_RAM_LOCATION_ID))
diff --git a/plat/qemu/qemu_bl1_setup.c b/plat/qemu/qemu_bl1_setup.c
new file mode 100644
index 0000000..4438aac
--- /dev/null
+++ b/plat/qemu/qemu_bl1_setup.c
@@ -0,0 +1,148 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <bl_common.h>
+#include <console.h>
+#include <platform_def.h>
+#include "qemu_private.h"
+
+
+/*
+ * The next 2 constants identify the extents of the coherent memory region.
+ * These addresses are used by the MMU setup code and therefore they must be
+ * page-aligned.  It is the responsibility of the linker script to ensure that
+ * __COHERENT_RAM_START__ and __COHERENT_RAM_END__ linker symbols refer to
+ * page-aligned addresses.
+ */
+#define BL1_COHERENT_RAM_BASE (unsigned long)(&__COHERENT_RAM_START__)
+#define BL1_COHERENT_RAM_LIMIT (unsigned long)(&__COHERENT_RAM_END__)
+
+
+/*******************************************************************************
+ * Declarations of linker defined symbols which will tell us where BL1 lives
+ * in Trusted RAM
+ ******************************************************************************/
+extern uint64_t __BL1_RAM_START__;
+extern uint64_t __BL1_RAM_END__;
+#define BL1_RAM_BASE (uint64_t)(&__BL1_RAM_START__)
+#define BL1_RAM_LIMIT (uint64_t)(&__BL1_RAM_END__)
+
+/* Data structure which holds the extents of the trusted SRAM for BL1*/
+static meminfo_t bl1_tzram_layout;
+
+
+meminfo_t *bl1_plat_sec_mem_layout(void)
+{
+	return &bl1_tzram_layout;
+}
+
+/*******************************************************************************
+ * Perform any BL1 specific platform actions.
+ ******************************************************************************/
+void bl1_early_platform_setup(void)
+{
+	const size_t bl1_size = BL1_RAM_LIMIT - BL1_RAM_BASE;
+
+	/* Initialize the console to provide early debug support */
+	console_init(PLAT_QEMU_BOOT_UART_BASE, PLAT_QEMU_BOOT_UART_CLK_IN_HZ,
+		     PLAT_QEMU_CONSOLE_BAUDRATE);
+
+	/* Allow BL1 to see the whole Trusted RAM */
+	bl1_tzram_layout.total_base = BL_RAM_BASE;
+	bl1_tzram_layout.total_size = BL_RAM_SIZE;
+
+	/* Calculate how much RAM BL1 is using and how much remains free */
+	bl1_tzram_layout.free_base = BL_RAM_BASE;
+	bl1_tzram_layout.free_size = BL_RAM_SIZE;
+	reserve_mem(&bl1_tzram_layout.free_base, &bl1_tzram_layout.free_size,
+		    BL1_RAM_BASE, bl1_size);
+}
+
+/******************************************************************************
+ * Perform the very early platform specific architecture setup.  This only
+ * does basic initialization. Later architectural setup (bl1_arch_setup())
+ * does not do anything platform specific.
+ *****************************************************************************/
+void bl1_plat_arch_setup(void)
+{
+	qemu_configure_mmu_el3(bl1_tzram_layout.total_base,
+				bl1_tzram_layout.total_size,
+				BL1_RO_BASE, BL1_RO_LIMIT,
+				BL1_COHERENT_RAM_BASE, BL1_COHERENT_RAM_LIMIT);
+}
+
+void bl1_platform_setup(void)
+{
+	plat_qemu_io_setup();
+}
+
+/*******************************************************************************
+ * Function that takes a memory layout into which BL2 has been loaded and
+ * populates a new memory layout for BL2 that ensures that BL1's data sections
+ * resident in secure RAM are not visible to BL2.
+ ******************************************************************************/
+void bl1_init_bl2_mem_layout(const meminfo_t *bl1_mem_layout,
+			     meminfo_t *bl2_mem_layout)
+{
+	const size_t bl1_size = BL1_RAM_LIMIT - BL1_RAM_BASE;
+
+	assert(bl1_mem_layout != NULL);
+	assert(bl2_mem_layout != NULL);
+
+	/* Check that BL1's memory is lying outside of the free memory */
+	assert((BL1_RAM_LIMIT <= bl1_mem_layout->free_base) ||
+	       (BL1_RAM_BASE >= (bl1_mem_layout->free_base +
+				 bl1_mem_layout->free_size)));
+
+	/* Remove BL1 RW data from the scope of memory visible to BL2 */
+	*bl2_mem_layout = *bl1_mem_layout;
+	reserve_mem(&bl2_mem_layout->total_base,
+		    &bl2_mem_layout->total_size,
+		    BL1_RAM_BASE,
+		    bl1_size);
+
+	flush_dcache_range((unsigned long)bl2_mem_layout, sizeof(meminfo_t));
+}
+
+/*******************************************************************************
+ * Before calling this function BL2 is loaded in memory and its entrypoint
+ * is set by load_image. This is a placeholder for the platform to change
+ * the entrypoint of BL2 and set SPSR and security state.
+ * On ARM standard platforms we only set the security state of the entrypoint
+ ******************************************************************************/
+void bl1_plat_set_bl2_ep_info(image_info_t *bl2_image,
+				entry_point_info_t *bl2_ep)
+{
+	SET_SECURITY_STATE(bl2_ep->h.attr, SECURE);
+	bl2_ep->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+}
diff --git a/plat/qemu/qemu_bl2_setup.c b/plat/qemu/qemu_bl2_setup.c
new file mode 100644
index 0000000..24da2b6
--- /dev/null
+++ b/plat/qemu/qemu_bl2_setup.c
@@ -0,0 +1,327 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <arch_helpers.h>
+#include <bl_common.h>
+#include <console.h>
+#include <debug.h>
+#include <libfdt.h>
+#include <platform_def.h>
+#include "qemu_private.h"
+#include <string.h>
+
+
+/*
+ * The next 2 constants identify the extents of the code & RO data region.
+ * These addresses are used by the MMU setup code and therefore they must be
+ * page-aligned.  It is the responsibility of the linker script to ensure that
+ * __RO_START__ and __RO_END__ linker symbols refer to page-aligned addresses.
+ */
+#define BL2_RO_BASE (unsigned long)(&__RO_START__)
+#define BL2_RO_LIMIT (unsigned long)(&__RO_END__)
+
+/*
+ * The next 2 constants identify the extents of the coherent memory region.
+ * These addresses are used by the MMU setup code and therefore they must be
+ * page-aligned.  It is the responsibility of the linker script to ensure that
+ * __COHERENT_RAM_START__ and __COHERENT_RAM_END__ linker symbols refer to
+ * page-aligned addresses.
+ */
+#define BL2_COHERENT_RAM_BASE (unsigned long)(&__COHERENT_RAM_START__)
+#define BL2_COHERENT_RAM_LIMIT (unsigned long)(&__COHERENT_RAM_END__)
+
+/*******************************************************************************
+ * This structure represents the superset of information that is passed to
+ * BL3-1, e.g. while passing control to it from BL2, bl31_params
+ * and other platform specific params
+ ******************************************************************************/
+typedef struct bl2_to_bl31_params_mem {
+	bl31_params_t bl31_params;
+	image_info_t bl31_image_info;
+	image_info_t bl32_image_info;
+	image_info_t bl33_image_info;
+	entry_point_info_t bl33_ep_info;
+	entry_point_info_t bl32_ep_info;
+	entry_point_info_t bl31_ep_info;
+} bl2_to_bl31_params_mem_t;
+
+
+static bl2_to_bl31_params_mem_t bl31_params_mem;
+
+
+
+/* Data structure which holds the extents of the trusted SRAM for BL2 */
+static meminfo_t bl2_tzram_layout __aligned(CACHE_WRITEBACK_GRANULE);
+
+meminfo_t *bl2_plat_sec_mem_layout(void)
+{
+	return &bl2_tzram_layout;
+}
+
+/*******************************************************************************
+ * This function assigns a pointer to the memory that the platform has kept
+ * aside to pass platform specific and trusted firmware related information
+ * to BL31. This memory is allocated by allocating memory to
+ * bl2_to_bl31_params_mem_t structure which is a superset of all the
+ * structure whose information is passed to BL31
+ * NOTE: This function should be called only once and should be done
+ * before generating params to BL31
+ ******************************************************************************/
+bl31_params_t *bl2_plat_get_bl31_params(void)
+{
+	bl31_params_t *bl2_to_bl31_params;
+
+	/*
+	 * Initialise the memory for all the arguments that needs to
+	 * be passed to BL3-1
+	 */
+	memset(&bl31_params_mem, 0, sizeof(bl2_to_bl31_params_mem_t));
+
+	/* Assign memory for TF related information */
+	bl2_to_bl31_params = &bl31_params_mem.bl31_params;
+	SET_PARAM_HEAD(bl2_to_bl31_params, PARAM_BL31, VERSION_1, 0);
+
+	/* Fill BL3-1 related information */
+	bl2_to_bl31_params->bl31_image_info = &bl31_params_mem.bl31_image_info;
+	SET_PARAM_HEAD(bl2_to_bl31_params->bl31_image_info, PARAM_IMAGE_BINARY,
+		VERSION_1, 0);
+
+	/* Fill BL3-2 related information */
+	bl2_to_bl31_params->bl32_ep_info = &bl31_params_mem.bl32_ep_info;
+	SET_PARAM_HEAD(bl2_to_bl31_params->bl32_ep_info, PARAM_EP,
+		VERSION_1, 0);
+	bl2_to_bl31_params->bl32_image_info = &bl31_params_mem.bl32_image_info;
+	SET_PARAM_HEAD(bl2_to_bl31_params->bl32_image_info, PARAM_IMAGE_BINARY,
+		VERSION_1, 0);
+
+	/* Fill BL3-3 related information */
+	bl2_to_bl31_params->bl33_ep_info = &bl31_params_mem.bl33_ep_info;
+	SET_PARAM_HEAD(bl2_to_bl31_params->bl33_ep_info,
+		PARAM_EP, VERSION_1, 0);
+
+	/* BL3-3 expects to receive the primary CPU MPID (through x0) */
+	bl2_to_bl31_params->bl33_ep_info->args.arg0 = 0xffff & read_mpidr();
+
+	bl2_to_bl31_params->bl33_image_info = &bl31_params_mem.bl33_image_info;
+	SET_PARAM_HEAD(bl2_to_bl31_params->bl33_image_info, PARAM_IMAGE_BINARY,
+		VERSION_1, 0);
+
+	return bl2_to_bl31_params;
+}
+
+/* Flush the TF params and the TF plat params */
+void bl2_plat_flush_bl31_params(void)
+{
+	flush_dcache_range((unsigned long)&bl31_params_mem,
+			sizeof(bl2_to_bl31_params_mem_t));
+}
+
+/*******************************************************************************
+ * This function returns a pointer to the shared memory that the platform
+ * has kept to point to entry point information of BL31 to BL2
+ ******************************************************************************/
+struct entry_point_info *bl2_plat_get_bl31_ep_info(void)
+{
+#if DEBUG
+	bl31_params_mem.bl31_ep_info.args.arg1 = QEMU_BL31_PLAT_PARAM_VAL;
+#endif
+
+	return &bl31_params_mem.bl31_ep_info;
+}
+
+
+
+void bl2_early_platform_setup(meminfo_t *mem_layout)
+{
+	/* Initialize the console to provide early debug support */
+	console_init(PLAT_QEMU_BOOT_UART_BASE, PLAT_QEMU_BOOT_UART_CLK_IN_HZ,
+			PLAT_QEMU_CONSOLE_BAUDRATE);
+
+	/* Setup the BL2 memory layout */
+	bl2_tzram_layout = *mem_layout;
+
+	plat_qemu_io_setup();
+}
+
+static void security_setup(void)
+{
+	/*
+	 * This is where a TrustZone address space controller and other
+	 * security related peripherals, would be configured.
+	 */
+}
+
+static void update_dt(void)
+{
+	int ret;
+	void *fdt = (void *)(uintptr_t)PLAT_QEMU_DT_BASE;
+
+	ret = fdt_open_into(fdt, fdt, PLAT_QEMU_DT_MAX_SIZE);
+	if (ret < 0) {
+		ERROR("Invalid Device Tree at %p: error %d\n", fdt, ret);
+		return;
+	}
+
+	if (dt_add_psci_node(fdt)) {
+		ERROR("Failed to add PSCI Device Tree node\n");
+		return;
+	}
+
+	if (dt_add_psci_cpu_enable_methods(fdt)) {
+		ERROR("Failed to add PSCI cpu enable methods in Device Tree\n");
+		return;
+	}
+
+	ret = fdt_pack(fdt);
+	if (ret < 0)
+		ERROR("Failed to pack Device Tree at %p: error %d\n", fdt, ret);
+}
+
+void bl2_platform_setup(void)
+{
+	security_setup();
+	update_dt();
+
+	/* TODO Initialize timer */
+}
+
+void bl2_plat_arch_setup(void)
+{
+	qemu_configure_mmu_el1(bl2_tzram_layout.total_base,
+			      bl2_tzram_layout.total_size,
+			      BL2_RO_BASE, BL2_RO_LIMIT,
+			      BL2_COHERENT_RAM_BASE, BL2_COHERENT_RAM_LIMIT);
+}
+
+/*******************************************************************************
+ * Gets SPSR for BL32 entry
+ ******************************************************************************/
+static uint32_t qemu_get_spsr_for_bl32_entry(void)
+{
+	/*
+	 * The Secure Payload Dispatcher service is responsible for
+	 * setting the SPSR prior to entry into the BL3-2 image.
+	 */
+	return 0;
+}
+
+/*******************************************************************************
+ * Gets SPSR for BL33 entry
+ ******************************************************************************/
+static uint32_t qemu_get_spsr_for_bl33_entry(void)
+{
+	unsigned long el_status;
+	unsigned int mode;
+	uint32_t spsr;
+
+	/* Figure out what mode we enter the non-secure world in */
+	el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
+	el_status &= ID_AA64PFR0_ELX_MASK;
+
+	mode = (el_status) ? MODE_EL2 : MODE_EL1;
+
+	/*
+	 * TODO: Consider the possibility of specifying the SPSR in
+	 * the FIP ToC and allowing the platform to have a say as
+	 * well.
+	 */
+	spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	return spsr;
+}
+
+/*******************************************************************************
+ * Before calling this function BL3-1 is loaded in memory and its entrypoint
+ * is set by load_image. This is a placeholder for the platform to change
+ * the entrypoint of BL3-1 and set SPSR and security state.
+ * On ARM standard platforms we only set the security state of the entrypoint
+ ******************************************************************************/
+void bl2_plat_set_bl31_ep_info(image_info_t *bl31_image_info,
+					entry_point_info_t *bl31_ep_info)
+{
+	SET_SECURITY_STATE(bl31_ep_info->h.attr, SECURE);
+	bl31_ep_info->spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+}
+
+/*******************************************************************************
+ * Before calling this function BL3-2 is loaded in memory and its entrypoint
+ * is set by load_image. This is a placeholder for the platform to change
+ * the entrypoint of BL3-2 and set SPSR and security state.
+ * On ARM standard platforms we only set the security state of the entrypoint
+ ******************************************************************************/
+void bl2_plat_set_bl32_ep_info(image_info_t *bl32_image_info,
+					entry_point_info_t *bl32_ep_info)
+{
+	SET_SECURITY_STATE(bl32_ep_info->h.attr, SECURE);
+	bl32_ep_info->spsr = qemu_get_spsr_for_bl32_entry();
+}
+
+/*******************************************************************************
+ * Before calling this function BL3-3 is loaded in memory and its entrypoint
+ * is set by load_image. This is a placeholder for the platform to change
+ * the entrypoint of BL3-3 and set SPSR and security state.
+ * On ARM standard platforms we only set the security state of the entrypoint
+ ******************************************************************************/
+void bl2_plat_set_bl33_ep_info(image_info_t *image,
+					entry_point_info_t *bl33_ep_info)
+{
+
+	SET_SECURITY_STATE(bl33_ep_info->h.attr, NON_SECURE);
+	bl33_ep_info->spsr = qemu_get_spsr_for_bl33_entry();
+}
+
+/*******************************************************************************
+ * Populate the extents of memory available for loading BL32
+ ******************************************************************************/
+void bl2_plat_get_bl32_meminfo(meminfo_t *bl32_meminfo)
+{
+	/*
+	 * Populate the extents of memory available for loading BL32.
+	 */
+	bl32_meminfo->total_base = BL32_BASE;
+	bl32_meminfo->free_base = BL32_BASE;
+	bl32_meminfo->total_size = (BL32_MEM_BASE + BL32_MEM_SIZE) - BL32_BASE;
+	bl32_meminfo->free_size = (BL32_MEM_BASE + BL32_MEM_SIZE) - BL32_BASE;
+}
+
+/*******************************************************************************
+ * Populate the extents of memory available for loading BL33
+ ******************************************************************************/
+void bl2_plat_get_bl33_meminfo(meminfo_t *bl33_meminfo)
+{
+	bl33_meminfo->total_base = NS_DRAM0_BASE;
+	bl33_meminfo->total_size = NS_DRAM0_SIZE;
+	bl33_meminfo->free_base = NS_DRAM0_BASE;
+	bl33_meminfo->free_size = NS_DRAM0_SIZE;
+}
+
+unsigned long plat_get_ns_image_entrypoint(void)
+{
+	return NS_IMAGE_OFFSET;
+}
diff --git a/plat/qemu/qemu_bl31_setup.c b/plat/qemu/qemu_bl31_setup.c
new file mode 100644
index 0000000..7a48358
--- /dev/null
+++ b/plat/qemu/qemu_bl31_setup.c
@@ -0,0 +1,164 @@
+/*
+ * 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:
+ *
+ * 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 <bl_common.h>
+#include <console.h>
+#include <gicv2.h>
+#include <platform_def.h>
+#include "qemu_private.h"
+
+/*
+ * The next 3 constants identify the extents of the code, RO data region and the
+ * limit of the BL3-1 image.  These addresses are used by the MMU setup code and
+ * therefore they must be page-aligned.  It is the responsibility of the linker
+ * script to ensure that __RO_START__, __RO_END__ & __BL31_END__ linker symbols
+ * refer to page-aligned addresses.
+ */
+#define BL31_RO_BASE (unsigned long)(&__RO_START__)
+#define BL31_RO_LIMIT (unsigned long)(&__RO_END__)
+#define BL31_END (unsigned long)(&__BL31_END__)
+
+/*
+ * The next 2 constants identify the extents of the coherent memory region.
+ * These addresses are used by the MMU setup code and therefore they must be
+ * page-aligned.  It is the responsibility of the linker script to ensure that
+ * __COHERENT_RAM_START__ and __COHERENT_RAM_END__ linker symbols
+ * refer to page-aligned addresses.
+ */
+#define BL31_COHERENT_RAM_BASE (unsigned long)(&__COHERENT_RAM_START__)
+#define BL31_COHERENT_RAM_LIMIT (unsigned long)(&__COHERENT_RAM_END__)
+
+/*
+ * Placeholder variables for copying the arguments that have been passed to
+ * BL3-1 from BL2.
+ */
+static entry_point_info_t bl32_image_ep_info;
+static entry_point_info_t bl33_image_ep_info;
+
+/*******************************************************************************
+ * Perform any BL3-1 early platform setup.  Here is an opportunity to copy
+ * parameters passed by the calling EL (S-EL1 in BL2 & S-EL3 in BL1) before
+ * they are lost (potentially). This needs to be done before the MMU is
+ * initialized so that the memory layout can be used while creating page
+ * tables. BL2 has flushed this information to memory, so we are guaranteed
+ * to pick up good data.
+ ******************************************************************************/
+void bl31_early_platform_setup(bl31_params_t *from_bl2,
+				void *plat_params_from_bl2)
+{
+	/* Initialize the console to provide early debug support */
+	console_init(PLAT_QEMU_BOOT_UART_BASE, PLAT_QEMU_BOOT_UART_CLK_IN_HZ,
+			PLAT_QEMU_CONSOLE_BAUDRATE);
+
+	/*
+	 * Check params passed from BL2 should not be NULL,
+	 */
+	assert(from_bl2 != NULL);
+	assert(from_bl2->h.type == PARAM_BL31);
+	assert(from_bl2->h.version >= VERSION_1);
+	/*
+	 * In debug builds, we pass a special value in 'plat_params_from_bl2'
+	 * to verify platform parameters from BL2 to BL3-1.
+	 * In release builds, it's not used.
+	 */
+	assert(((unsigned long long)plat_params_from_bl2) ==
+		QEMU_BL31_PLAT_PARAM_VAL);
+
+	/*
+	 * Copy BL3-2 (if populated by BL2) and BL3-3 entry point information.
+	 * They are stored in Secure RAM, in BL2's address space.
+	 */
+	if (from_bl2->bl32_ep_info)
+		bl32_image_ep_info = *from_bl2->bl32_ep_info;
+	bl33_image_ep_info = *from_bl2->bl33_ep_info;
+}
+
+void bl31_plat_arch_setup(void)
+{
+	qemu_configure_mmu_el3(BL31_RO_BASE, (BL31_END - BL31_RO_BASE),
+			      BL31_RO_BASE, BL31_RO_LIMIT,
+			      BL31_COHERENT_RAM_BASE, BL31_COHERENT_RAM_LIMIT);
+}
+
+static const unsigned int irq_sec_array[] = {
+	QEMU_IRQ_SEC_SGI_0,
+	QEMU_IRQ_SEC_SGI_1,
+	QEMU_IRQ_SEC_SGI_2,
+	QEMU_IRQ_SEC_SGI_3,
+	QEMU_IRQ_SEC_SGI_4,
+	QEMU_IRQ_SEC_SGI_5,
+	QEMU_IRQ_SEC_SGI_6,
+	QEMU_IRQ_SEC_SGI_7,
+};
+
+static const struct gicv2_driver_data plat_gicv2_driver_data = {
+	.gicd_base = GICD_BASE,
+	.gicc_base = GICC_BASE,
+	.g0_interrupt_num = ARRAY_SIZE(irq_sec_array),
+	.g0_interrupt_array = irq_sec_array,
+};
+
+void bl31_platform_setup(void)
+{
+	/* Initialize the gic cpu and distributor interfaces */
+	gicv2_driver_init(&plat_gicv2_driver_data);
+	gicv2_distif_init();
+	gicv2_pcpu_distif_init();
+	gicv2_cpuif_enable();
+}
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	return SYS_COUNTER_FREQ_IN_TICKS;
+}
+
+/*******************************************************************************
+ * Return a pointer to the 'entry_point_info' structure of the next image
+ * for the security state specified. BL3-3 corresponds to the non-secure
+ * image type while BL3-2 corresponds to the secure image type. A NULL
+ * pointer is returned if the image does not exist.
+ ******************************************************************************/
+entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
+{
+	entry_point_info_t *next_image_info;
+
+	assert(sec_state_is_valid(type));
+	next_image_info = (type == NON_SECURE)
+			? &bl33_image_ep_info : &bl32_image_ep_info;
+	/*
+	 * None of the images on the ARM development platforms can have 0x0
+	 * as the entrypoint
+	 */
+	if (next_image_info->pc)
+		return next_image_info;
+	else
+		return NULL;
+}
diff --git a/plat/qemu/qemu_common.c b/plat/qemu/qemu_common.c
new file mode 100644
index 0000000..7ba1d34
--- /dev/null
+++ b/plat/qemu/qemu_common.c
@@ -0,0 +1,145 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch_helpers.h>
+#include <bl_common.h>
+#include <platform_def.h>
+#include "qemu_private.h"
+#include <xlat_tables.h>
+
+#define MAP_DEVICE0	MAP_REGION_FLAT(DEVICE0_BASE,			\
+					DEVICE0_SIZE,			\
+					MT_DEVICE | MT_RW | MT_SECURE)
+
+#ifdef DEVICE1_BASE
+#define MAP_DEVICE1	MAP_REGION_FLAT(DEVICE1_BASE,			\
+					DEVICE1_SIZE,			\
+					MT_DEVICE | MT_RW | MT_SECURE)
+#endif
+
+#ifdef DEVICE2_BASE
+#define MAP_DEVICE2	MAP_REGION_FLAT(DEVICE2_BASE,			\
+					DEVICE2_SIZE,			\
+					MT_DEVICE | MT_RO | MT_SECURE)
+#endif
+
+#define MAP_SHARED_RAM	MAP_REGION_FLAT(SHARED_RAM_BASE,		\
+					SHARED_RAM_SIZE,		\
+					MT_DEVICE  | MT_RW | MT_SECURE)
+
+#define MAP_BL32_MEM	MAP_REGION_FLAT(BL32_MEM_BASE, BL32_MEM_SIZE,	\
+					MT_MEMORY | MT_RW | MT_SECURE)
+
+#define MAP_NS_DRAM0	MAP_REGION_FLAT(NS_DRAM0_BASE, NS_DRAM0_SIZE,	\
+					MT_MEMORY | MT_RW | MT_NS)
+
+#define MAP_FLASH0	MAP_REGION_FLAT(QEMU_FLASH0_BASE, QEMU_FLASH0_SIZE, \
+					MT_MEMORY | MT_RO | MT_SECURE)
+
+/*
+ * Table of regions for various BL stages to map using the MMU.
+ * This doesn't include TZRAM as the 'mem_layout' argument passed to
+ * arm_configure_mmu_elx() will give the available subset of that,
+ */
+#if IMAGE_BL1
+static const mmap_region_t plat_qemu_mmap[] = {
+	MAP_FLASH0,
+	MAP_SHARED_RAM,
+	MAP_DEVICE0,
+#ifdef MAP_DEVICE1
+	MAP_DEVICE1,
+#endif
+#ifdef MAP_DEVICE2
+	MAP_DEVICE2,
+#endif
+	{0}
+};
+#endif
+#if IMAGE_BL2
+static const mmap_region_t plat_qemu_mmap[] = {
+	MAP_FLASH0,
+	MAP_SHARED_RAM,
+	MAP_DEVICE0,
+#ifdef MAP_DEVICE1
+	MAP_DEVICE1,
+#endif
+#ifdef MAP_DEVICE2
+	MAP_DEVICE2,
+#endif
+	MAP_NS_DRAM0,
+	MAP_BL32_MEM,
+	{0}
+};
+#endif
+#if IMAGE_BL31
+static const mmap_region_t plat_qemu_mmap[] = {
+	MAP_SHARED_RAM,
+	MAP_DEVICE0,
+#ifdef MAP_DEVICE1
+	MAP_DEVICE1,
+#endif
+	MAP_BL32_MEM,
+	{0}
+};
+#endif
+
+/*******************************************************************************
+ * Macro generating the code for the function setting up the pagetables as per
+ * the platform memory map & initialize the mmu, for the given exception level
+ ******************************************************************************/
+
+#define DEFINE_CONFIGURE_MMU_EL(_el)					\
+	void qemu_configure_mmu_el##_el(unsigned long total_base,	\
+				   unsigned long total_size,		\
+				   unsigned long ro_start,		\
+				   unsigned long ro_limit,		\
+				   unsigned long coh_start,		\
+				   unsigned long coh_limit)		\
+	{								\
+		mmap_add_region(total_base, total_base,			\
+				total_size,				\
+				MT_MEMORY | MT_RW | MT_SECURE);		\
+		mmap_add_region(ro_start, ro_start,			\
+				ro_limit - ro_start,			\
+				MT_MEMORY | MT_RO | MT_SECURE);		\
+		mmap_add_region(coh_start, coh_start,			\
+				coh_limit - coh_start,			\
+				MT_DEVICE | MT_RW | MT_SECURE);		\
+		mmap_add(plat_qemu_mmap);				\
+		init_xlat_tables();					\
+									\
+		enable_mmu_el##_el(0);					\
+	}
+
+/* Define EL1 and EL3 variants of the function initialising the MMU */
+DEFINE_CONFIGURE_MMU_EL(1)
+DEFINE_CONFIGURE_MMU_EL(3)
+
+
diff --git a/plat/qemu/qemu_gic.c b/plat/qemu/qemu_gic.c
new file mode 100644
index 0000000..44ce19c
--- /dev/null
+++ b/plat/qemu/qemu_gic.c
@@ -0,0 +1,92 @@
+/*
+ * 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:
+ *
+ * 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 <bl_common.h>
+#include <gicv2.h>
+#include <interrupt_mgmt.h>
+
+uint32_t plat_ic_get_pending_interrupt_id(void)
+{
+	return gicv2_get_pending_interrupt_id();
+}
+
+uint32_t plat_ic_get_pending_interrupt_type(void)
+{
+	return gicv2_get_pending_interrupt_type();
+}
+
+uint32_t plat_ic_acknowledge_interrupt(void)
+{
+	return gicv2_acknowledge_interrupt();
+}
+
+uint32_t plat_ic_get_interrupt_type(uint32_t id)
+{
+	uint32_t group;
+
+	group = gicv2_get_interrupt_group(id);
+
+	/* Assume that all secure interrupts are S-EL1 interrupts */
+	if (!group)
+		return INTR_TYPE_S_EL1;
+	else
+		return INTR_TYPE_NS;
+
+}
+
+void plat_ic_end_of_interrupt(uint32_t id)
+{
+	gicv2_end_of_interrupt(id);
+}
+
+uint32_t plat_interrupt_type_to_line(uint32_t type,
+				uint32_t security_state)
+{
+	assert(type == INTR_TYPE_S_EL1 ||
+	       type == INTR_TYPE_EL3 ||
+	       type == INTR_TYPE_NS);
+
+	assert(sec_state_is_valid(security_state));
+
+	/* Non-secure interrupts are signalled on the IRQ line always */
+	if (type == INTR_TYPE_NS)
+		return __builtin_ctz(SCR_IRQ_BIT);
+
+	/*
+	 * Secure interrupts are signalled using the IRQ line if the FIQ_EN
+	 * bit is not set else they are signalled using the FIQ line.
+	 */
+	if (gicv2_is_fiq_enabled())
+		return __builtin_ctz(SCR_FIQ_BIT);
+	else
+		return __builtin_ctz(SCR_IRQ_BIT);
+}
+
diff --git a/plat/qemu/qemu_io_storage.c b/plat/qemu/qemu_io_storage.c
new file mode 100644
index 0000000..b7951b7
--- /dev/null
+++ b/plat/qemu/qemu_io_storage.c
@@ -0,0 +1,378 @@
+/*
+ * 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:
+ *
+ * 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 <bl_common.h>		/* For ARRAY_SIZE */
+#include <debug.h>
+#include <firmware_image_package.h>
+#include <io_driver.h>
+#include <io_fip.h>
+#include <io_memmap.h>
+#include <io_semihosting.h>
+#include <io_storage.h>
+#include <platform_def.h>
+#include <semihosting.h>
+#include <string.h>
+
+/* Semihosting filenames */
+#define BL2_IMAGE_NAME			"bl2.bin"
+#define BL31_IMAGE_NAME			"bl31.bin"
+#define BL32_IMAGE_NAME			"bl32.bin"
+#define BL33_IMAGE_NAME			"bl33.bin"
+
+#if TRUSTED_BOARD_BOOT
+#define BL2_CERT_NAME			"bl2.crt"
+#define TRUSTED_KEY_CERT_NAME		"trusted_key.crt"
+#define BL31_KEY_CERT_NAME		"bl31_key.crt"
+#define BL32_KEY_CERT_NAME		"bl32_key.crt"
+#define BL33_KEY_CERT_NAME		"bl33_key.crt"
+#define BL31_CERT_NAME			"bl31.crt"
+#define BL32_CERT_NAME			"bl32.crt"
+#define BL33_CERT_NAME			"bl33.crt"
+#endif /* TRUSTED_BOARD_BOOT */
+
+
+
+/* IO devices */
+static const io_dev_connector_t *fip_dev_con;
+static uintptr_t fip_dev_handle;
+static const io_dev_connector_t *memmap_dev_con;
+static uintptr_t memmap_dev_handle;
+static const io_dev_connector_t *sh_dev_con;
+static uintptr_t sh_dev_handle;
+
+static const io_block_spec_t fip_block_spec = {
+	.offset = PLAT_QEMU_FIP_BASE,
+	.length = PLAT_QEMU_FIP_MAX_SIZE
+};
+
+static const io_uuid_spec_t bl2_uuid_spec = {
+	.uuid = UUID_TRUSTED_BOOT_FIRMWARE_BL2,
+};
+
+static const io_uuid_spec_t bl31_uuid_spec = {
+	.uuid = UUID_EL3_RUNTIME_FIRMWARE_BL31,
+};
+
+static const io_uuid_spec_t bl32_uuid_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32,
+};
+
+static const io_uuid_spec_t bl33_uuid_spec = {
+	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33,
+};
+
+#if TRUSTED_BOARD_BOOT
+static const io_uuid_spec_t bl2_cert_uuid_spec = {
+	.uuid = UUID_TRUSTED_BOOT_FIRMWARE_BL2_CERT,
+};
+
+static const io_uuid_spec_t trusted_key_cert_uuid_spec = {
+	.uuid = UUID_TRUSTED_KEY_CERT,
+};
+
+static const io_uuid_spec_t bl31_key_cert_uuid_spec = {
+	.uuid = UUID_EL3_RUNTIME_FIRMWARE_BL31_KEY_CERT,
+};
+
+static const io_uuid_spec_t bl32_key_cert_uuid_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_KEY_CERT,
+};
+
+static const io_uuid_spec_t bl33_key_cert_uuid_spec = {
+	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33_KEY_CERT,
+};
+
+static const io_uuid_spec_t bl31_cert_uuid_spec = {
+	.uuid = UUID_EL3_RUNTIME_FIRMWARE_BL31_CERT,
+};
+
+static const io_uuid_spec_t bl32_cert_uuid_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_CERT,
+};
+
+static const io_uuid_spec_t bl33_cert_uuid_spec = {
+	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33_CERT,
+};
+#endif /* TRUSTED_BOARD_BOOT */
+
+static const io_file_spec_t sh_file_spec[] = {
+	[BL2_IMAGE_ID] = {
+		.path = BL2_IMAGE_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL31_IMAGE_ID] = {
+		.path = BL31_IMAGE_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL32_IMAGE_ID] = {
+		.path = BL32_IMAGE_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL33_IMAGE_ID] = {
+		.path = BL33_IMAGE_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+#if TRUSTED_BOARD_BOOT
+	[BL2_CERT_ID] = {
+		.path = BL2_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[TRUSTED_KEY_CERT_ID] = {
+		.path = TRUSTED_KEY_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL31_KEY_CERT_ID] = {
+		.path = BL31_KEY_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL32_KEY_CERT_ID] = {
+		.path = BL32_KEY_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL33_KEY_CERT_ID] = {
+		.path = BL33_KEY_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL31_CERT_ID] = {
+		.path = BL31_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL32_CERT_ID] = {
+		.path = BL32_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL33_CERT_ID] = {
+		.path = BL33_CERT_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+#endif /* TRUSTED_BOARD_BOOT */
+};
+
+
+
+static int open_fip(const uintptr_t spec);
+static int open_memmap(const uintptr_t spec);
+
+struct plat_io_policy {
+	uintptr_t *dev_handle;
+	uintptr_t image_spec;
+	int (*check)(const uintptr_t spec);
+};
+
+/* By default, ARM platforms load images from the FIP */
+static const struct plat_io_policy policies[] = {
+	[FIP_IMAGE_ID] = {
+		&memmap_dev_handle,
+		(uintptr_t)&fip_block_spec,
+		open_memmap
+	},
+	[BL2_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl2_uuid_spec,
+		open_fip
+	},
+	[BL31_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl31_uuid_spec,
+		open_fip
+	},
+	[BL32_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl32_uuid_spec,
+		open_fip
+	},
+	[BL33_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl33_uuid_spec,
+		open_fip
+	},
+#if TRUSTED_BOARD_BOOT
+	[BL2_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl2_cert_uuid_spec,
+		open_fip
+	},
+	[TRUSTED_KEY_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&trusted_key_cert_uuid_spec,
+		open_fip
+	},
+	[BL31_KEY_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl31_key_cert_uuid_spec,
+		open_fip
+	},
+	[BL32_KEY_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl32_key_cert_uuid_spec,
+		open_fip
+	},
+	[BL33_KEY_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl33_key_cert_uuid_spec,
+		open_fip
+	},
+	[BL31_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl31_cert_uuid_spec,
+		open_fip
+	},
+	[BL32_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl32_cert_uuid_spec,
+		open_fip
+	},
+	[BL33_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl33_cert_uuid_spec,
+		open_fip
+	},
+#endif /* TRUSTED_BOARD_BOOT */
+};
+
+static int open_fip(const uintptr_t spec)
+{
+	int result;
+	uintptr_t local_image_handle;
+
+	/* See if a Firmware Image Package is available */
+	result = io_dev_init(fip_dev_handle, (uintptr_t)FIP_IMAGE_ID);
+	if (result == 0) {
+		result = io_open(fip_dev_handle, spec, &local_image_handle);
+		if (result == 0) {
+			VERBOSE("Using FIP\n");
+			io_close(local_image_handle);
+		}
+	}
+	return result;
+}
+
+static int open_memmap(const uintptr_t spec)
+{
+	int result;
+	uintptr_t local_image_handle;
+
+	result = io_dev_init(memmap_dev_handle, (uintptr_t)NULL);
+	if (result == 0) {
+		result = io_open(memmap_dev_handle, spec, &local_image_handle);
+		if (result == 0) {
+			VERBOSE("Using Memmap\n");
+			io_close(local_image_handle);
+		}
+	}
+	return result;
+}
+
+static int open_semihosting(const uintptr_t spec)
+{
+	int result;
+	uintptr_t local_image_handle;
+
+	/* See if the file exists on semi-hosting.*/
+	result = io_dev_init(sh_dev_handle, (uintptr_t)NULL);
+	if (result == 0) {
+		result = io_open(sh_dev_handle, spec, &local_image_handle);
+		if (result == 0) {
+			VERBOSE("Using Semi-hosting IO\n");
+			io_close(local_image_handle);
+		}
+	}
+	return result;
+}
+
+void plat_qemu_io_setup(void)
+{
+	int io_result;
+
+	io_result = register_io_dev_fip(&fip_dev_con);
+	assert(io_result == 0);
+
+	io_result = register_io_dev_memmap(&memmap_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to devices and cache the handles */
+	io_result = io_dev_open(fip_dev_con, (uintptr_t)NULL,
+				&fip_dev_handle);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(memmap_dev_con, (uintptr_t)NULL,
+				&memmap_dev_handle);
+	assert(io_result == 0);
+
+	/* Register the additional IO devices on this platform */
+	io_result = register_io_dev_sh(&sh_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to devices and cache the handles */
+	io_result = io_dev_open(sh_dev_con, (uintptr_t)NULL, &sh_dev_handle);
+	assert(io_result == 0);
+
+	/* Ignore improbable errors in release builds */
+	(void)io_result;
+}
+
+static int get_alt_image_source(unsigned int image_id, uintptr_t *dev_handle,
+				  uintptr_t *image_spec)
+{
+	int result = open_semihosting((const uintptr_t)&sh_file_spec[image_id]);
+
+	if (result == 0) {
+		*dev_handle = sh_dev_handle;
+		*image_spec = (uintptr_t)&sh_file_spec[image_id];
+	}
+
+	return result;
+}
+
+/*
+ * Return an IO device handle and specification which can be used to access
+ * an image. Use this to enforce platform load policy
+ */
+int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
+			  uintptr_t *image_spec)
+{
+	int result;
+	const struct plat_io_policy *policy;
+
+	assert(image_id < ARRAY_SIZE(policies));
+
+	policy = &policies[image_id];
+	result = policy->check(policy->image_spec);
+	if (result == 0) {
+		*image_spec = policy->image_spec;
+		*dev_handle = *(policy->dev_handle);
+	} else {
+		VERBOSE("Trying alternative IO\n");
+		result = get_alt_image_source(image_id, dev_handle, image_spec);
+	}
+
+	return result;
+}
diff --git a/plat/qemu/qemu_pm.c b/plat/qemu/qemu_pm.c
new file mode 100644
index 0000000..8114c16
--- /dev/null
+++ b/plat/qemu/qemu_pm.c
@@ -0,0 +1,253 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <gicv2.h>
+#include <platform_def.h>
+#include <platform.h>
+#include <psci.h>
+
+/*
+ * The secure entry point to be used on warm reset.
+ */
+static unsigned long secure_entrypoint;
+
+/* Make composite power state parameter till power level 0 */
+#if PSCI_EXTENDED_STATE_ID
+
+#define qemu_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type) \
+		(((lvl0_state) << PSTATE_ID_SHIFT) | \
+		 ((type) << PSTATE_TYPE_SHIFT))
+#else
+#define qemu_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type) \
+		(((lvl0_state) << PSTATE_ID_SHIFT) | \
+		 ((pwr_lvl) << PSTATE_PWR_LVL_SHIFT) | \
+		 ((type) << PSTATE_TYPE_SHIFT))
+#endif /* PSCI_EXTENDED_STATE_ID */
+
+
+#define qemu_make_pwrstate_lvl1(lvl1_state, lvl0_state, pwr_lvl, type) \
+		(((lvl1_state) << PLAT_LOCAL_PSTATE_WIDTH) | \
+		 qemu_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type))
+
+
+
+/*
+ *  The table storing the valid idle power states. Ensure that the
+ *  array entries are populated in ascending order of state-id to
+ *  enable us to use binary search during power state validation.
+ *  The table must be terminated by a NULL entry.
+ */
+static const unsigned int qemu_pm_idle_states[] = {
+	/* State-id - 0x01 */
+	qemu_make_pwrstate_lvl1(PLAT_LOCAL_STATE_RUN, PLAT_LOCAL_STATE_RET,
+				MPIDR_AFFLVL0, PSTATE_TYPE_STANDBY),
+	/* State-id - 0x02 */
+	qemu_make_pwrstate_lvl1(PLAT_LOCAL_STATE_RUN, PLAT_LOCAL_STATE_OFF,
+				MPIDR_AFFLVL0, PSTATE_TYPE_POWERDOWN),
+	/* State-id - 0x22 */
+	qemu_make_pwrstate_lvl1(PLAT_LOCAL_STATE_OFF, PLAT_LOCAL_STATE_OFF,
+				MPIDR_AFFLVL1, PSTATE_TYPE_POWERDOWN),
+	0,
+};
+
+/*******************************************************************************
+ * Platform handler called to check the validity of the power state
+ * parameter. The power state parameter has to be a composite power state.
+ ******************************************************************************/
+static int qemu_validate_power_state(unsigned int power_state,
+				psci_power_state_t *req_state)
+{
+	unsigned int state_id;
+	int i;
+
+	assert(req_state);
+
+	/*
+	 *  Currently we are using a linear search for finding the matching
+	 *  entry in the idle power state array. This can be made a binary
+	 *  search if the number of entries justify the additional complexity.
+	 */
+	for (i = 0; !!qemu_pm_idle_states[i]; i++) {
+		if (power_state == qemu_pm_idle_states[i])
+			break;
+	}
+
+	/* Return error if entry not found in the idle state array */
+	if (!qemu_pm_idle_states[i])
+		return PSCI_E_INVALID_PARAMS;
+
+	i = 0;
+	state_id = psci_get_pstate_id(power_state);
+
+	/* Parse the State ID and populate the state info parameter */
+	while (state_id) {
+		req_state->pwr_domain_state[i++] = state_id &
+						PLAT_LOCAL_PSTATE_MASK;
+		state_id >>= PLAT_LOCAL_PSTATE_WIDTH;
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * Platform handler called to check the validity of the non secure
+ * entrypoint.
+ ******************************************************************************/
+static int qemu_validate_ns_entrypoint(uintptr_t entrypoint)
+{
+	/*
+	 * Check if the non secure entrypoint lies within the non
+	 * secure DRAM.
+	 */
+	if ((entrypoint >= NS_DRAM0_BASE) &&
+	    (entrypoint < (NS_DRAM0_BASE + NS_DRAM0_SIZE)))
+		return PSCI_E_SUCCESS;
+	return PSCI_E_INVALID_ADDRESS;
+}
+
+/*******************************************************************************
+ * Platform handler called when a CPU is about to enter standby.
+ ******************************************************************************/
+static void qemu_cpu_standby(plat_local_state_t cpu_state)
+{
+
+	assert(cpu_state == PLAT_LOCAL_STATE_RET);
+
+	/*
+	 * Enter standby state
+	 * dsb is good practice before using wfi to enter low power states
+	 */
+	dsb();
+	wfi();
+}
+
+/*******************************************************************************
+ * Platform handler called when a power domain is about to be turned on. The
+ * mpidr determines the CPU to be turned on.
+ ******************************************************************************/
+static int qemu_pwr_domain_on(u_register_t mpidr)
+{
+	int rc = PSCI_E_SUCCESS;
+	unsigned pos = plat_core_pos_by_mpidr(mpidr);
+	uint64_t *hold_base = (uint64_t *)PLAT_QEMU_HOLD_BASE;
+
+	hold_base[pos] = PLAT_QEMU_HOLD_STATE_GO;
+	sev();
+
+	return rc;
+}
+
+/*******************************************************************************
+ * Platform handler called when a power domain is about to be turned off. The
+ * target_state encodes the power state that each level should transition to.
+ ******************************************************************************/
+void qemu_pwr_domain_off(const psci_power_state_t *target_state)
+{
+	assert(0);
+}
+
+/*******************************************************************************
+ * Platform handler called when a power domain is about to be suspended. The
+ * target_state encodes the power state that each level should transition to.
+ ******************************************************************************/
+void qemu_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	assert(0);
+}
+
+/*******************************************************************************
+ * Platform handler called when a power domain has just been powered on after
+ * being turned off earlier. The target_state encodes the low power state that
+ * each level has woken up from.
+ ******************************************************************************/
+void qemu_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	assert(target_state->pwr_domain_state[MPIDR_AFFLVL0] ==
+					PLAT_LOCAL_STATE_OFF);
+
+	/* TODO: This setup is needed only after a cold boot */
+	gicv2_pcpu_distif_init();
+
+	/* Enable the gic cpu interface */
+	gicv2_cpuif_enable();
+}
+
+/*******************************************************************************
+ * Platform handler called when a power domain has just been powered on after
+ * having been suspended earlier. The target_state encodes the low power state
+ * that each level has woken up from.
+ ******************************************************************************/
+void qemu_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	assert(0);
+}
+
+/*******************************************************************************
+ * Platform handlers to shutdown/reboot the system
+ ******************************************************************************/
+static void __dead2 qemu_system_off(void)
+{
+	ERROR("QEMU System Off: operation not handled.\n");
+	panic();
+}
+
+static void __dead2 qemu_system_reset(void)
+{
+	ERROR("QEMU System Reset: operation not handled.\n");
+	panic();
+}
+
+static const plat_psci_ops_t plat_qemu_psci_pm_ops = {
+	.cpu_standby = qemu_cpu_standby,
+	.pwr_domain_on = qemu_pwr_domain_on,
+	.pwr_domain_off = qemu_pwr_domain_off,
+	.pwr_domain_suspend = qemu_pwr_domain_suspend,
+	.pwr_domain_on_finish = qemu_pwr_domain_on_finish,
+	.pwr_domain_suspend_finish = qemu_pwr_domain_suspend_finish,
+	.system_off = qemu_system_off,
+	.system_reset = qemu_system_reset,
+	.validate_power_state = qemu_validate_power_state,
+	.validate_ns_entrypoint = qemu_validate_ns_entrypoint
+};
+
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const plat_psci_ops_t **psci_ops)
+{
+	uintptr_t *mailbox = (void *) PLAT_QEMU_TRUSTED_MAILBOX_BASE;
+
+	*mailbox = sec_entrypoint;
+	secure_entrypoint = (unsigned long) sec_entrypoint;
+	*psci_ops = &plat_qemu_psci_pm_ops;
+
+	return 0;
+}
diff --git a/plat/rockchip/common/plat_delay_timer.c b/plat/qemu/qemu_private.h
similarity index 66%
copy from plat/rockchip/common/plat_delay_timer.c
copy to plat/qemu/qemu_private.h
index 797ce05..2595e66 100644
--- a/plat/rockchip/common/plat_delay_timer.c
+++ b/plat/qemu/qemu_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -28,27 +28,23 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <delay_timer.h>
-#include <platform_def.h>
+#ifndef __QEMU_PRIVATE_H
+#define __QEMU_PRIVATE_H
 
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
+#include <sys/types.h>
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
+void qemu_configure_mmu_el1(unsigned long total_base, unsigned long total_size,
+			unsigned long ro_start, unsigned long ro_limit,
+			unsigned long coh_start, unsigned long coh_limit);
+
+void qemu_configure_mmu_el3(unsigned long total_base, unsigned long total_size,
+			unsigned long ro_start, unsigned long ro_limit,
+			unsigned long coh_start, unsigned long coh_limit);
+
+void plat_qemu_io_setup(void);
+unsigned int plat_qemu_calc_core_pos(u_register_t mpidr);
+
+int dt_add_psci_node(void *fdt);
+int dt_add_psci_cpu_enable_methods(void *fdt);
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#endif /*__QEMU_PRIVATE_H*/
diff --git a/plat/qemu/topology.c b/plat/qemu/topology.c
new file mode 100644
index 0000000..ea546ac
--- /dev/null
+++ b/plat/qemu/topology.c
@@ -0,0 +1,78 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <platform_def.h>
+#include "qemu_private.h"
+#include <sys/types.h>
+
+/* The power domain tree descriptor */
+static unsigned char power_domain_tree_desc[] = {
+	/* Number of root nodes */
+	PLATFORM_CLUSTER_COUNT,
+	/* Number of children for the first node */
+	PLATFORM_CLUSTER0_CORE_COUNT,
+	/* Number of children for the second node */
+	PLATFORM_CLUSTER1_CORE_COUNT,
+};
+
+/*******************************************************************************
+ * This function returns the ARM default topology tree information.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function implements a part of the critical interface between the psci
+ * generic layer and the platform that allows the former to query the platform
+ * to convert an MPIDR to a unique linear index. An error code (-1) is returned
+ * in case the MPIDR is invalid.
+ ******************************************************************************/
+int plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	unsigned int cluster_id, cpu_id;
+
+	mpidr &= MPIDR_AFFINITY_MASK;
+	if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK))
+		return -1;
+
+	cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+	cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
+
+	if (cluster_id >= PLATFORM_CLUSTER_COUNT)
+		return -1;
+
+	if (cpu_id >= PLATFORM_MAX_CPUS_PER_CLUSTER)
+		return -1;
+
+	return plat_qemu_calc_core_pos(mpidr);
+}
diff --git a/plat/rockchip/common/aarch64/plat_helpers.S b/plat/rockchip/common/aarch64/plat_helpers.S
index a90dcd7..1bbb614 100644
--- a/plat/rockchip/common/aarch64/plat_helpers.S
+++ b/plat/rockchip/common/aarch64/plat_helpers.S
@@ -189,6 +189,7 @@
 	 * cpus online or resume enterpoint
 	 * --------------------------------------------------------------------
 	 */
+	.align	16
 func platform_cpu_warmboot
 	mrs	x0, MPIDR_EL1
 	and	x1, x0, #MPIDR_CPU_MASK
@@ -207,12 +208,6 @@
 	add	x4, x4, x0, lsl #2
 	ldr	w1, [x4]
 	/* --------------------------------------------------------------------
-	 * get per cpuup boot addr
-         * --------------------------------------------------------------------
-	 */
-	adr	x5, cpuson_entry_point
-	ldr	x2, [x5, x0, lsl #3]
-	/* --------------------------------------------------------------------
 	 * check cpuon reason
          * --------------------------------------------------------------------
 	 */
@@ -231,8 +226,15 @@
 	wfe
 	b	wfe_loop
 boot_entry:
-	mov	w0, #0
-	str	w0, [x4]
+	mov	w1, #0
+	str	w1, [x4]
+	/* --------------------------------------------------------------------
+	 * get per cpuup boot addr
+	 * --------------------------------------------------------------------
+	 */
+	adr	x5, cpuson_entry_point
+	ldr	x2, [x5, x0, lsl #3]
+
 	br	x2
 endfunc platform_cpu_warmboot
 
@@ -248,5 +250,5 @@
 	.endr
 cpuson_flags:
 	.rept	PLATFORM_CORE_COUNT
-	.quad	0
+	.word	0
 	.endr
diff --git a/plat/rockchip/common/aarch64/platform_common.c b/plat/rockchip/common/aarch64/platform_common.c
index 3f912a4..6e9dab7 100644
--- a/plat/rockchip/common/aarch64/platform_common.c
+++ b/plat/rockchip/common/aarch64/platform_common.c
@@ -75,7 +75,7 @@
 /* Define EL3 variants of the function initialising the MMU */
 DEFINE_CONFIGURE_MMU_EL(3)
 
-uint64_t plat_get_syscnt_freq(void)
+unsigned int plat_get_syscnt_freq2(void)
 {
 	return SYS_COUNTER_FREQ_IN_TICKS;
 }
diff --git a/plat/rockchip/common/bl31_plat_setup.c b/plat/rockchip/common/bl31_plat_setup.c
index 30fb5ac..47a245a 100644
--- a/plat/rockchip/common/bl31_plat_setup.c
+++ b/plat/rockchip/common/bl31_plat_setup.c
@@ -33,6 +33,7 @@
 #include <bl_common.h>
 #include <console.h>
 #include <debug.h>
+#include <generic_delay_timer.h>
 #include <mmio.h>
 #include <platform.h>
 #include <plat_private.h>
@@ -119,6 +120,9 @@
 	 * Copy the code into pmusram.
 	 */
 	plat_rockchip_pmusram_prepare();
+
+	/* there may have some board sepcific message need to initialize */
+	params_early_setup(plat_params_from_bl2);
 }
 
 /*******************************************************************************
@@ -126,7 +130,7 @@
  ******************************************************************************/
 void bl31_platform_setup(void)
 {
-	plat_delay_timer_init();
+	generic_delay_timer_init();
 	plat_rockchip_soc_init();
 
 	/* Initialize the gic cpu and distributor interfaces */
diff --git a/plat/rockchip/common/drivers/pmu/pmu_com.h b/plat/rockchip/common/drivers/pmu/pmu_com.h
index 0cab53c..4cffb61 100644
--- a/plat/rockchip/common/drivers/pmu/pmu_com.h
+++ b/plat/rockchip/common/drivers/pmu/pmu_com.h
@@ -27,13 +27,19 @@
 #ifndef __PMU_COM_H__
 #define __PMU_COM_H__
 
+/*
+ * Use this macro to instantiate lock before it is used in below
+ * rockchip_pd_lock_xxx() macros
+ */
 DEFINE_BAKERY_LOCK(rockchip_pd_lock);
 
+/*
+ * These are wrapper macros to the powe domain Bakery Lock API.
+ */
+#define rockchip_pd_lock_init() bakery_lock_init(&rockchip_pd_lock)
 #define rockchip_pd_lock_get() bakery_lock_get(&rockchip_pd_lock)
-
 #define rockchip_pd_lock_rls() bakery_lock_release(&rockchip_pd_lock)
 
-#define rockchip_pd_lock_init() bakery_lock_init(&rockchip_pd_lock)
 /*****************************************************************************
  * power domain on or off
  *****************************************************************************/
diff --git a/plat/rockchip/common/include/plat_params.h b/plat/rockchip/common/include/plat_params.h
new file mode 100644
index 0000000..cad4535
--- /dev/null
+++ b/plat/rockchip/common/include/plat_params.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __PLAT_PARAMS_H__
+#define __PLAT_PARAMS_H__
+
+#include <stdint.h>
+
+/*
+ * We defined several plat parameter structs for BL2 to pass platform related
+ * parameters to Rockchip BL31 platform code.  All plat parameters start with
+ * a common header, which has a type field to indicate the parameter type, and
+ * a next pointer points to next parameter. If the parameter is the last one in
+ * the list, next pointer will points to NULL.  After the header comes the
+ * variable-sized members that describe the parameter. The picture below shows
+ * how the parameters are kept in memory.
+ *
+ * head of list  ---> +----------------+ --+
+ *                    |      type      |   |
+ *                    +----------------+   |--> struct bl31_plat_param
+ *               +----|      next      |   |
+ *               |    +----------------+ --+
+ *               |    | parameter data |
+ *               |    +----------------+
+ *               |
+ *               +--> +----------------+ --+
+ *                    |      type      |   |
+ *                    +----------------+   |--> struct bl31_plat_param
+ *           NULL <---|      next      |   |
+ *                    +----------------+ --+
+ *                    | parameter data |
+ *                    +----------------+
+ *
+ * Note: The SCTLR_EL3.A bit (Alignment fault check enable) of ARM TF is set,
+ * so be sure each parameter struct starts on 64-bit aligned address. If not,
+ * alignment fault will occur during accessing its data member.
+ */
+
+/* param type */
+enum {
+	PARAM_NONE = 0,
+	PARAM_RESET,
+	PARAM_POWEROFF,
+};
+
+struct gpio_info {
+	uint8_t polarity;
+	uint8_t direction;
+	uint8_t pull_mode;
+	uint32_t index;
+};
+
+/* common header for all plat parameter type */
+struct bl31_plat_param {
+	uint64_t type;
+	void *next;
+};
+
+struct bl31_gpio_param {
+	struct bl31_plat_param h;
+	struct gpio_info gpio;
+};
+
+#endif /* __PLAT_PARAMS_H__ */
diff --git a/plat/rockchip/common/include/plat_private.h b/plat/rockchip/common/include/plat_private.h
index 67fb827..031a341 100644
--- a/plat/rockchip/common/include/plat_private.h
+++ b/plat/rockchip/common/include/plat_private.h
@@ -77,7 +77,7 @@
 #endif
 
 #ifndef BITS_WITH_WMASK
-#define BITS_WITH_WMASK(msk, bits, shift)\
+#define BITS_WITH_WMASK(bits, msk, shift)\
 	(BITS_SHIFT(bits, shift) | BITS_SHIFT(msk, (shift + REG_MSK_SHIFT)))
 #endif
 
@@ -97,6 +97,8 @@
 
 void plat_delay_timer_init(void);
 
+void params_early_setup(void *plat_params_from_bl2);
+
 void plat_rockchip_gic_driver_init(void);
 void plat_rockchip_gic_init(void);
 void plat_rockchip_gic_cpuif_enable(void);
@@ -108,6 +110,12 @@
 void plat_rockchip_soc_init(void);
 void plat_setup_rockchip_pm_ops(struct rockchip_pm_ops_cb *ops);
 
+void platform_cpu_warmboot(void);
+
+void *plat_get_rockchip_gpio_reset(void);
+void *plat_get_rockchip_gpio_poweroff(void);
+void plat_rockchip_gpio_init(void);
+
 extern const unsigned char rockchip_power_domain_tree_desc[];
 
 extern void *pmu_cpuson_entrypoint_start;
diff --git a/plat/rockchip/common/params_setup.c b/plat/rockchip/common/params_setup.c
new file mode 100644
index 0000000..2a49556
--- /dev/null
+++ b/plat/rockchip/common/params_setup.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arm_gic.h>
+#include <assert.h>
+#include <bl_common.h>
+#include <console.h>
+#include <debug.h>
+#include <gpio.h>
+#include <mmio.h>
+#include <platform.h>
+#include <plat_params.h>
+#include <plat_private.h>
+#include <string.h>
+
+static struct bl31_plat_param *bl31_params_head;
+static struct bl31_gpio_param param_reset;
+static struct bl31_gpio_param param_poweroff;
+static struct gpio_info *rst_gpio;
+static struct gpio_info *poweroff_gpio;
+
+void *plat_get_rockchip_gpio_reset(void)
+{
+	return rst_gpio;
+}
+
+void *plat_get_rockchip_gpio_poweroff(void)
+{
+	return poweroff_gpio;
+}
+
+void params_early_setup(void *plat_param_from_bl2)
+{
+	struct bl31_plat_param *param;
+	struct bl31_plat_param *bl2_param;
+	struct bl31_gpio_param *gpio_param;
+
+	/* keep plat parameters for later processing if need */
+	bl2_param = (struct bl31_plat_param *)plat_param_from_bl2;
+	while (bl2_param) {
+		switch (bl2_param->type) {
+		case PARAM_RESET:
+			param = (struct bl31_plat_param *)&param_reset;
+			memcpy((void *)param, (void *)bl2_param,
+				sizeof(struct bl31_gpio_param));
+			gpio_param = (struct bl31_gpio_param *)param;
+			rst_gpio = &gpio_param->gpio;
+			break;
+		case PARAM_POWEROFF:
+			param = (struct bl31_plat_param *)&param_poweroff;
+			memcpy((void *)param, (void *)bl2_param,
+				sizeof(struct bl31_gpio_param));
+			gpio_param = (struct bl31_gpio_param *)param;
+			poweroff_gpio = &gpio_param->gpio;
+			break;
+		default:
+			NOTICE("not expected type found\n");
+			return; /* don't continue if unexpected type found */
+		}
+		param->next = bl31_params_head;
+		bl31_params_head = param;
+		bl2_param = bl2_param->next;
+	}
+}
diff --git a/plat/rockchip/common/plat_pm.c b/plat/rockchip/common/plat_pm.c
index fcd47a8..d20a683 100644
--- a/plat/rockchip/common/plat_pm.c
+++ b/plat/rockchip/common/plat_pm.c
@@ -52,7 +52,6 @@
 
 static void plat_rockchip_sys_pwr_domain_resume(void)
 {
-	plat_rockchip_gic_init();
 	if (rockchip_ops && rockchip_ops->sys_pwr_dm_resume)
 		rockchip_ops->sys_pwr_dm_resume();
 }
@@ -62,8 +61,6 @@
 	if (rockchip_ops && rockchip_ops->cores_pwr_dm_resume)
 		rockchip_ops->cores_pwr_dm_resume();
 
-	/* Enable the gic cpu interface */
-	plat_rockchip_gic_pcpu_init();
 	/* Program the gic per-cpu distributor or re-distributor interface */
 	plat_rockchip_gic_cpuif_enable();
 }
@@ -256,6 +253,16 @@
 }
 
 /*******************************************************************************
+ * RockChip handlers to power off the system
+ ******************************************************************************/
+static void __dead2 rockchip_system_poweroff(void)
+{
+	assert(rockchip_ops && rockchip_ops->system_off);
+
+	rockchip_ops->system_off();
+}
+
+/*******************************************************************************
  * Export the platform handlers via plat_rockchip_psci_pm_ops. The rockchip
  * standard
  * platform layer will take care of registering the handlers with PSCI.
@@ -268,6 +275,7 @@
 	.pwr_domain_on_finish = rockchip_pwr_domain_on_finish,
 	.pwr_domain_suspend_finish = rockchip_pwr_domain_suspend_finish,
 	.system_reset = rockchip_system_reset,
+	.system_off = rockchip_system_poweroff,
 	.validate_power_state = rockchip_validate_power_state,
 	.get_sys_suspend_power_state = rockchip_get_sys_suspend_power_state
 };
diff --git a/plat/rockchip/common/pmusram/pmu_sram.h b/plat/rockchip/common/pmusram/pmu_sram.h
index a2ab460..f290461 100644
--- a/plat/rockchip/common/pmusram/pmu_sram.h
+++ b/plat/rockchip/common/pmusram/pmu_sram.h
@@ -27,21 +27,14 @@
 #define __PMU_SRAM_H__
 
 /*****************************************************************************
- * cpu up status
- *****************************************************************************/
-#define PMU_SYS_SLP_MODE	0xa5
-#define PMU_SYS_ON_MODE		0x0
-
-/*****************************************************************************
  * define data offset in struct psram_data
  *****************************************************************************/
 #define PSRAM_DT_SP		0x0
 #define PSRAM_DT_DDR_FUNC	0x8
 #define PSRAM_DT_DDR_DATA	0x10
 #define PSRAM_DT_DDRFLAG	0x18
-#define PSRAM_DT_SYS_MODE	0x1c
-#define PSRAM_DT_MPIDR		0x20
-#define PSRAM_DT_END		0x24
+#define PSRAM_DT_MPIDR		0x1c
+#define PSRAM_DT_END		0x20
 /******************************************************************************
  * Allocate data region for struct psram_data_t in pmusram
  ******************************************************************************/
@@ -67,7 +60,6 @@
 	uint64_t ddr_func;
 	uint64_t ddr_data;
 	uint32_t ddr_flag;
-	uint32_t sys_mode;
 	uint32_t boot_mpidr;
 };
 
@@ -81,8 +73,6 @@
 	assert_psram_dt_ddr_data_offset_mistmatch);
 CASSERT(__builtin_offsetof(struct psram_data_t, ddr_flag) == PSRAM_DT_DDRFLAG,
 	assert_psram_dt_ddr_flag_offset_mistmatch);
-CASSERT(__builtin_offsetof(struct psram_data_t, sys_mode) == PSRAM_DT_SYS_MODE,
-	assert_psram_dt_sys_mode_offset_mistmatch);
 CASSERT(__builtin_offsetof(struct psram_data_t, boot_mpidr) == PSRAM_DT_MPIDR,
 	assert_psram_dt_mpidr_offset_mistmatch);
 void u32_align_cpy(uint32_t *dst, const uint32_t *src, size_t bytes);
diff --git a/plat/rockchip/common/pmusram/pmu_sram_cpus_on.S b/plat/rockchip/common/pmusram/pmu_sram_cpus_on.S
index 33a4646..9f94b0c 100644
--- a/plat/rockchip/common/pmusram/pmu_sram_cpus_on.S
+++ b/plat/rockchip/common/pmusram/pmu_sram_cpus_on.S
@@ -35,11 +35,6 @@
 func pmu_cpuson_entrypoint
 pmu_cpuson_entrypoint_start:
 	ldr	x5, psram_data
-	ldr	w0, [x5, #PSRAM_DT_SYS_MODE]
-	cmp	w0, #PMU_SYS_SLP_MODE
-	b.eq	check_wake_cpus
-	ldr	x6, warm_boot_func
-	br	x6
 check_wake_cpus:
 	mrs	x0, MPIDR_EL1
 	and	x1, x0, #MPIDR_CPU_MASK
@@ -74,8 +69,6 @@
 	.align	3
 psram_data:
 	.quad	PSRAM_DT_BASE
-warm_boot_func:
-	.quad	platform_cpu_warmboot
 sys_wakeup_entry:
 	.quad	psci_entrypoint
 pmu_cpuson_entrypoint_end:
diff --git a/plat/rockchip/rk3368/drivers/pmu/pmu.c b/plat/rockchip/rk3368/drivers/pmu/pmu.c
index e990634..53d333b 100644
--- a/plat/rockchip/rk3368/drivers/pmu/pmu.c
+++ b/plat/rockchip/rk3368/drivers/pmu/pmu.c
@@ -43,6 +43,8 @@
 static struct psram_data_t *psram_sleep_cfg =
 	(struct psram_data_t *)PSRAM_DT_BASE;
 
+static uint32_t cpu_warm_boot_addr;
+
 void rk3368_flash_l2_b(void)
 {
 	uint32_t wait_cnt = 0;
@@ -347,13 +349,14 @@
 	cpus_id_power_domain(cluster, cpu, pmu_pd_off, CKECK_WFEI_MSK);
 
 	cpuon_id = (cluster * PLATFORM_CLUSTER0_CORE_COUNT) + cpu;
+	assert(cpuon_id < PLATFORM_CORE_COUNT);
 	assert(cpuson_flags[cpuon_id] == 0);
 	cpuson_flags[cpuon_id] = PMU_CPU_HOTPLUG;
 	cpuson_entry_point[cpuon_id] = entrypoint;
 
 	/* Switch boot addr to pmusram */
 	mmio_write_32(SGRF_BASE + SGRF_SOC_CON(1 + cluster),
-		      (PMUSRAM_BASE >> CPU_BOOT_ADDR_ALIGN) |
+		      (cpu_warm_boot_addr >> CPU_BOOT_ADDR_ALIGN) |
 		      CPU_BOOT_ADDR_WMASK);
 	dsb();
 
@@ -368,19 +371,17 @@
 
 static int cores_pwr_domain_on_finish(void)
 {
-	uint32_t cpuon_id;
-
-	cpuon_id = plat_my_core_pos();
-	assert(cpuson_flags[cpuon_id] == 0);
-	cpuson_flags[cpuon_id] = 0x00;
-
 	return 0;
 }
 
 static int sys_pwr_domain_resume(void)
 {
-	psram_sleep_cfg->sys_mode = PMU_SYS_ON_MODE;
-
+	mmio_write_32(SGRF_BASE + SGRF_SOC_CON(1),
+		      (COLD_BOOT_BASE >> CPU_BOOT_ADDR_ALIGN) |
+		      CPU_BOOT_ADDR_WMASK);
+	mmio_write_32(SGRF_BASE + SGRF_SOC_CON(2),
+		      (COLD_BOOT_BASE >> CPU_BOOT_ADDR_ALIGN) |
+		      CPU_BOOT_ADDR_WMASK);
 	pm_plls_resume();
 	pmu_scu_b_pwrup();
 
@@ -392,7 +393,6 @@
 	nonboot_cpus_off();
 	pmu_set_sleep_mode();
 
-	psram_sleep_cfg->sys_mode = PMU_SYS_SLP_MODE;
 	psram_sleep_cfg->ddr_flag = 0;
 
 	return 0;
@@ -412,11 +412,12 @@
 
 	plat_setup_rockchip_pm_ops(&pm_ops);
 
+	/* register requires 32bits mode, switch it to 32 bits */
+	cpu_warm_boot_addr = (uint64_t)platform_cpu_warmboot;
+
 	for (cpu = 0; cpu < PLATFORM_CORE_COUNT; cpu++)
 		cpuson_flags[cpu] = 0;
 
-	psram_sleep_cfg->sys_mode = PMU_SYS_ON_MODE;
-
 	psram_sleep_cfg->boot_mpidr = read_mpidr_el1() & 0xffff;
 
 	nonboot_cpus_off();
diff --git a/plat/rockchip/rk3368/include/platform_def.h b/plat/rockchip/rk3368/include/platform_def.h
index 876cbfb..299704d 100644
--- a/plat/rockchip/rk3368/include/platform_def.h
+++ b/plat/rockchip/rk3368/include/platform_def.h
@@ -101,7 +101,7 @@
 /*
  * Put BL3-1 at the top of the Trusted RAM
  */
-#define BL31_BASE		(TZRAM_BASE + 0x8000)
+#define BL31_BASE		(TZRAM_BASE + 0x10000)
 #define BL31_LIMIT	(TZRAM_BASE + TZRAM_SIZE)
 
 /*******************************************************************************
diff --git a/plat/rockchip/rk3368/platform.mk b/plat/rockchip/rk3368/platform.mk
index 0d34cf4..50eda32 100644
--- a/plat/rockchip/rk3368/platform.mk
+++ b/plat/rockchip/rk3368/platform.mk
@@ -58,13 +58,14 @@
 				drivers/console/console.S			\
 				drivers/ti/uart/16550_console.S			\
 				drivers/delay_timer/delay_timer.c		\
+				drivers/delay_timer/generic_delay_timer.c	\
 				lib/cpus/aarch64/cortex_a53.S			\
 				plat/common/aarch64/platform_mp_stack.S		\
 				${RK_PLAT_COMMON}/aarch64/plat_helpers.S	\
 				${RK_PLAT_COMMON}/bl31_plat_setup.c		\
+				${RK_PLAT_COMMON}/params_setup.c                \
 				${RK_PLAT_COMMON}/pmusram/pmu_sram_cpus_on.S		\
 				${RK_PLAT_COMMON}/pmusram/pmu_sram.c		\
-				${RK_PLAT_COMMON}/plat_delay_timer.c		\
 				${RK_PLAT_COMMON}/plat_pm.c			\
 				${RK_PLAT_COMMON}/plat_topology.c		\
 				${RK_PLAT_COMMON}/aarch64/platform_common.c	\
diff --git a/plat/rockchip/rk3368/rk3368_def.h b/plat/rockchip/rk3368/rk3368_def.h
index 614f270..2242cee 100644
--- a/plat/rockchip/rk3368/rk3368_def.h
+++ b/plat/rockchip/rk3368/rk3368_def.h
@@ -89,7 +89,6 @@
  * System counter frequency related constants
  ******************************************************************************/
 #define SYS_COUNTER_FREQ_IN_TICKS	24000000
-#define SYS_COUNTER_FREQ_IN_MHZ		24
 
 /******************************************************************************
  * GIC-400 & interrupt handling related constants
diff --git a/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c b/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
new file mode 100644
index 0000000..eca9fbc
--- /dev/null
+++ b/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
@@ -0,0 +1,227 @@
+/*
+ * 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 <debug.h>
+#include <delay_timer.h>
+#include <errno.h>
+#include <gpio.h>
+#include <mmio.h>
+#include <platform.h>
+#include <platform_def.h>
+#include <plat_private.h>
+#include <soc.h>
+
+uint32_t gpio_port[] = {
+	GPIO0_BASE,
+	GPIO1_BASE,
+	GPIO2_BASE,
+	GPIO3_BASE,
+	GPIO4_BASE,
+};
+
+#define SWPORTA_DR	0x00
+#define SWPORTA_DDR	0x04
+#define EXT_PORTA	0x50
+
+#define PMU_GPIO_PORT0	0
+#define PMU_GPIO_PORT1	1
+
+#define PMU_GRF_GPIO0A_P	0x40
+#define GRF_GPIO2A_P		0xe040
+#define GPIO_P_MASK		0x03
+
+/*
+ * gpio clock disabled when not operate
+ * so need to enable gpio clock before operate gpio
+ * after setting, need to disable gpio clock
+ * gate 1: disable clock; 0: enable clock
+ */
+static void gpio_clk(int gpio, uint32_t gate)
+{
+	uint32_t port = gpio / 32;
+
+	assert(port < 5);
+
+	switch (port) {
+	case 0:
+		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
+			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+					      PCLK_GPIO0_GATE_SHIFT));
+		break;
+	case 1:
+		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
+			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+					      PCLK_GPIO1_GATE_SHIFT));
+		break;
+	case 2:
+		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+					      PCLK_GPIO2_GATE_SHIFT));
+		break;
+	case 3:
+		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+					      PCLK_GPIO3_GATE_SHIFT));
+
+		break;
+	case 4:
+		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+					      PCLK_GPIO4_GATE_SHIFT));
+		break;
+	default:
+		break;
+	}
+}
+
+static void set_pull(int gpio, int pull)
+{
+	uint32_t port = gpio / 32;
+	uint32_t num = gpio % 32;
+	uint32_t bank = num / 8;
+	uint32_t id = num % 8;
+
+	assert((port < 5) && (num < 32));
+
+	gpio_clk(gpio, 0);
+
+	/*
+	 * in gpio0a, gpio0b, gpio2c, gpio2d,
+	 * 00: Z
+	 * 01: pull down
+	 * 10: Z
+	 * 11: pull up
+	 * different with other gpio, so need to correct it
+	 */
+	if (((port == 0) && (bank < 2)) || ((port == 2) && (bank > 2))) {
+		if (pull == GPIO_PULL_UP)
+			pull = 3;
+		else if (pull == GPIO_PULL_DOWN)
+			pull = 1;
+		else
+			pull = 0;
+	}
+
+	if (port == PMU_GPIO_PORT0 || port == PMU_GPIO_PORT1) {
+		mmio_write_32(PMUGRF_BASE + PMU_GRF_GPIO0A_P +
+			      port * 16 + bank * 4,
+			      BITS_WITH_WMASK(pull, GPIO_P_MASK, id * 2));
+	} else {
+		mmio_write_32(GRF_BASE + GRF_GPIO2A_P +
+			      (port - 2) * 16 + bank * 4,
+			      BITS_WITH_WMASK(pull, GPIO_P_MASK, id * 2));
+	}
+	gpio_clk(gpio, 1);
+}
+
+static void set_direction(int gpio, int direction)
+{
+	uint32_t port = gpio / 32;
+	uint32_t num = gpio % 32;
+
+	assert((port < 5) && (num < 32));
+
+	gpio_clk(gpio, 0);
+
+	/*
+	 * in gpio.h
+	 * #define GPIO_DIR_OUT	0
+	 * #define GPIO_DIR_IN	1
+	 * but rk3399 gpio direction 1: output, 0: input
+	 * so need to revert direction value
+	 */
+	mmio_setbits_32(gpio_port[port] + SWPORTA_DDR, !direction << num);
+	gpio_clk(gpio, 1);
+}
+
+static int get_direction(int gpio)
+{
+	uint32_t port = gpio / 32;
+	uint32_t num = gpio % 32;
+	int direction;
+
+	assert((port < 5) && (num < 32));
+
+	gpio_clk(gpio, 0);
+
+	/*
+	 * in gpio.h
+	 * #define GPIO_DIR_OUT	0
+	 * #define GPIO_DIR_IN	1
+	 * but rk3399 gpio direction 1: output, 0: input
+	 * so need to revert direction value
+	 */
+	direction = !((mmio_read_32(gpio_port[port] +
+				    SWPORTA_DDR) >> num) & 0x1);
+	gpio_clk(gpio, 1);
+
+	return direction;
+}
+
+static int get_value(int gpio)
+{
+	uint32_t port = gpio / 32;
+	uint32_t num = gpio % 32;
+	int value;
+
+	assert((port < 5) && (num < 32));
+
+	gpio_clk(gpio, 0);
+	value = (mmio_read_32(gpio_port[port] + EXT_PORTA) >> num) & 0x1;
+	gpio_clk(gpio, 1);
+
+	return value;
+}
+
+static void set_value(int gpio, int value)
+{
+	uint32_t port = gpio / 32;
+	uint32_t num = gpio % 32;
+
+	assert((port < 5) && (num < 32));
+
+	gpio_clk(gpio, 0);
+	mmio_clrsetbits_32(gpio_port[port] + SWPORTA_DR, 1 << num,
+							 !!value << num);
+	gpio_clk(gpio, 0);
+}
+
+const gpio_ops_t rk3399_gpio_ops = {
+	.get_direction = get_direction,
+	.set_direction = set_direction,
+	.get_value = get_value,
+	.set_value = set_value,
+	.set_pull = set_pull,
+};
+
+void plat_rockchip_gpio_init(void)
+{
+	gpio_init(&rk3399_gpio_ops);
+}
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu.c b/plat/rockchip/rk3399/drivers/pmu/pmu.c
index 351de7d..9b95621 100644
--- a/plat/rockchip/rk3399/drivers/pmu/pmu.c
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu.c
@@ -34,9 +34,11 @@
 #include <debug.h>
 #include <delay_timer.h>
 #include <errno.h>
+#include <gpio.h>
 #include <mmio.h>
 #include <platform.h>
 #include <platform_def.h>
+#include <plat_params.h>
 #include <plat_private.h>
 #include <rk3399_def.h>
 #include <pmu_sram.h>
@@ -47,6 +49,8 @@
 static struct psram_data_t *psram_sleep_cfg =
 	(struct psram_data_t *)PSRAM_DT_BASE;
 
+static uint32_t cpu_warm_boot_addr;
+
 /*
  * There are two ways to powering on or off on core.
  * 1) Control it power domain into on or off in PMU_PWRDN_CON reg,
@@ -63,6 +67,53 @@
 #endif
 ;/* coheront */
 
+void rk3399_flash_l2_b(void)
+{
+	uint32_t wait_cnt = 0;
+
+	mmio_setbits_32(PMU_BASE + PMU_SFT_CON, BIT(L2_FLUSH_REQ_CLUSTER_B));
+	dsb();
+
+	while (!(mmio_read_32(PMU_BASE + PMU_CORE_PWR_ST) &
+		 BIT(L2_FLUSHDONE_CLUSTER_B))) {
+		wait_cnt++;
+		if (!(wait_cnt % MAX_WAIT_CONUT))
+			WARN("%s:reg %x,wait\n", __func__,
+			     mmio_read_32(PMU_BASE + PMU_CORE_PWR_ST));
+	}
+
+	mmio_clrbits_32(PMU_BASE + PMU_SFT_CON, BIT(L2_FLUSH_REQ_CLUSTER_B));
+}
+
+static void pmu_scu_b_pwrdn(void)
+{
+	uint32_t wait_cnt = 0;
+
+	if ((mmio_read_32(PMU_BASE + PMU_PWRDN_ST) &
+	     (BIT(PMU_A72_B0_PWRDWN_ST) | BIT(PMU_A72_B1_PWRDWN_ST))) !=
+	     (BIT(PMU_A72_B0_PWRDWN_ST) | BIT(PMU_A72_B1_PWRDWN_ST))) {
+		ERROR("%s: not all cpus is off\n", __func__);
+		return;
+	}
+
+	rk3399_flash_l2_b();
+
+	mmio_setbits_32(PMU_BASE + PMU_SFT_CON, BIT(ACINACTM_CLUSTER_B_CFG));
+
+	while (!(mmio_read_32(PMU_BASE + PMU_CORE_PWR_ST) &
+		 BIT(STANDBY_BY_WFIL2_CLUSTER_B))) {
+		wait_cnt++;
+		if (!(wait_cnt % MAX_WAIT_CONUT))
+			ERROR("%s:wait cluster-b l2(%x)\n", __func__,
+			      mmio_read_32(PMU_BASE + PMU_CORE_PWR_ST));
+	}
+}
+
+static void pmu_scu_b_pwrup(void)
+{
+	mmio_clrbits_32(PMU_BASE + PMU_SFT_CON, BIT(ACINACTM_CLUSTER_B_CFG));
+}
+
 void plat_rockchip_pmusram_prepare(void)
 {
 	uint32_t *sram_dst, *sram_src;
@@ -83,11 +134,13 @@
 
 static inline uint32_t get_cpus_pwr_domain_cfg_info(uint32_t cpu_id)
 {
+	assert(cpu_id < PLATFORM_CORE_COUNT);
 	return core_pm_cfg_info[cpu_id];
 }
 
 static inline void set_cpus_pwr_domain_cfg_info(uint32_t cpu_id, uint32_t value)
 {
+	assert(cpu_id < PLATFORM_CORE_COUNT);
 	core_pm_cfg_info[cpu_id] = value;
 #if !USE_COHERENT_MEM
 	flush_dcache_range((uintptr_t)&core_pm_cfg_info[cpu_id],
@@ -128,6 +181,7 @@
 
 		mmio_write_32(PMU_BASE + PMU_CORE_PM_CON(cpu_id),
 			      BIT(core_pm_sft_wakeup_en));
+		dsb();
 	}
 
 	return 0;
@@ -160,6 +214,7 @@
 			core_pm_value |= BIT(core_pm_int_wakeup_en);
 		mmio_write_32(PMU_BASE + PMU_CORE_PM_CON(cpu_id),
 			      core_pm_value);
+		dsb();
 	}
 
 	return 0;
@@ -183,6 +238,7 @@
 {
 	uint32_t cpu_id = plat_core_pos_by_mpidr(mpidr);
 
+	assert(cpu_id < PLATFORM_CORE_COUNT);
 	assert(cpuson_flags[cpu_id] == 0);
 	cpuson_flags[cpu_id] = PMU_CPU_HOTPLUG;
 	cpuson_entry_point[cpu_id] = entrypoint;
@@ -206,6 +262,7 @@
 {
 	uint32_t cpu_id = plat_my_core_pos();
 
+	assert(cpu_id < PLATFORM_CORE_COUNT);
 	assert(cpuson_flags[cpu_id] == 0);
 	cpuson_flags[cpu_id] = PMU_CPU_AUTO_PWRDN;
 	cpuson_entry_point[cpu_id] = (uintptr_t)psci_entrypoint;
@@ -220,9 +277,6 @@
 {
 	uint32_t cpu_id = plat_my_core_pos();
 
-	cpuson_flags[cpu_id] = 0;
-	cpuson_entry_point[cpu_id] = 0;
-
 	/* Disable core_pm */
 	mmio_write_32(PMU_BASE + PMU_CORE_PM_CON(cpu_id), CORES_PM_DISABLE);
 
@@ -233,9 +287,6 @@
 {
 	uint32_t cpu_id = plat_my_core_pos();
 
-	cpuson_flags[cpu_id] = 0;
-	cpuson_entry_point[cpu_id] = 0;
-
 	/* Disable core_pm */
 	mmio_write_32(PMU_BASE + PMU_CORE_PM_CON(cpu_id), CORES_PM_DISABLE);
 
@@ -246,36 +297,137 @@
 {
 	uint32_t slp_mode_cfg = 0;
 
+	mmio_write_32(PMU_BASE + PMU_CCI500_CON,
+		      BIT_WITH_WMSK(PMU_CLR_PREQ_CCI500_HW) |
+		      BIT_WITH_WMSK(PMU_CLR_QREQ_CCI500_HW) |
+		      BIT_WITH_WMSK(PMU_QGATING_CCI500_CFG));
+
-	slp_mode_cfg = PMU_PWR_MODE_EN |
-				   PMU_CPU0_PD_EN |
-				   PMU_L2_FLUSH_EN |
-				   PMU_L2_IDLE_EN |
-				   PMU_SCU_PD_EN |
-				   PMU_CLK_CORE_SRC_GATE_EN;
+	mmio_write_32(PMU_BASE + PMU_ADB400_CON,
+		      BIT_WITH_WMSK(PMU_CLR_CORE_L_HW) |
+		      BIT_WITH_WMSK(PMU_CLR_CORE_L_2GIC_HW) |
+		      BIT_WITH_WMSK(PMU_CLR_GIC2_CORE_L_HW));
+
+	mmio_write_32(PMUGRF_BASE + PMUGRF_GPIO1A_IOMUX,
+		      BIT_WITH_WMSK(AP_PWROFF));
+
+	slp_mode_cfg = BIT(PMU_PWR_MODE_EN) |
+		       BIT(PMU_POWER_OFF_REQ_CFG) |
+		       BIT(PMU_CPU0_PD_EN) |
+		       BIT(PMU_L2_FLUSH_EN) |
+		       BIT(PMU_L2_IDLE_EN) |
+		       BIT(PMU_SCU_PD_EN);
+
 	mmio_setbits_32(PMU_BASE + PMU_WKUP_CFG4, PMU_CLUSTER_L_WKUP_EN);
 	mmio_setbits_32(PMU_BASE + PMU_WKUP_CFG4, PMU_CLUSTER_B_WKUP_EN);
 	mmio_clrbits_32(PMU_BASE + PMU_WKUP_CFG4, PMU_GPIO_WKUP_EN);
+
 	mmio_write_32(PMU_BASE + PMU_PWRMODE_CON, slp_mode_cfg);
+
+	mmio_write_32(PMU_BASE + PMU_STABLE_CNT, CYCL_24M_CNT_MS(5));
+	mmio_write_32(PMU_BASE + PMU_SCU_L_PWRDN_CNT, CYCL_24M_CNT_MS(2));
+	mmio_write_32(PMU_BASE + PMU_SCU_L_PWRUP_CNT, CYCL_24M_CNT_MS(2));
+	mmio_write_32(PMU_BASE + PMU_SCU_B_PWRDN_CNT, CYCL_24M_CNT_MS(2));
+	mmio_write_32(PMU_BASE + PMU_SCU_B_PWRUP_CNT, CYCL_24M_CNT_MS(2));
 }
 
 static int sys_pwr_domain_suspend(void)
 {
 	sys_slp_config();
 	plls_suspend();
-	psram_sleep_cfg->sys_mode = PMU_SYS_SLP_MODE;
 	pmu_sgrf_rst_hld();
+
+	mmio_write_32(SGRF_BASE + SGRF_SOC_CON0_1(1),
+		      (PMUSRAM_BASE >> CPU_BOOT_ADDR_ALIGN) |
+		      CPU_BOOT_ADDR_WMASK);
+
+	pmu_scu_b_pwrdn();
+
+	mmio_write_32(PMU_BASE + PMU_ADB400_CON,
+		      BIT_WITH_WMSK(PMU_PWRDWN_REQ_CORE_B_2GIC_SW) |
+		      BIT_WITH_WMSK(PMU_PWRDWN_REQ_CORE_B_SW) |
+		      BIT_WITH_WMSK(PMU_PWRDWN_REQ_GIC2_CORE_B_SW));
+	dsb();
+	mmio_setbits_32(PMU_BASE + PMU_PWRDN_CON, BIT(PMU_SCU_B_PWRDWN_EN));
+
 	return 0;
 }
 
 static int sys_pwr_domain_resume(void)
 {
-	pmu_sgrf_rst_hld_release();
-	psram_sleep_cfg->sys_mode = PMU_SYS_ON_MODE;
+	pmu_sgrf_rst_hld();
+
+	mmio_write_32(SGRF_BASE + SGRF_SOC_CON0_1(1),
+		      (cpu_warm_boot_addr >> CPU_BOOT_ADDR_ALIGN) |
+		      CPU_BOOT_ADDR_WMASK);
+
 	plls_resume();
 
+	mmio_write_32(PMU_BASE + PMU_CCI500_CON,
+		      WMSK_BIT(PMU_CLR_PREQ_CCI500_HW) |
+		      WMSK_BIT(PMU_CLR_QREQ_CCI500_HW) |
+		      WMSK_BIT(PMU_QGATING_CCI500_CFG));
+
+	mmio_write_32(PMU_BASE + PMU_ADB400_CON,
+		      WMSK_BIT(PMU_CLR_CORE_L_HW) |
+		      WMSK_BIT(PMU_CLR_CORE_L_2GIC_HW) |
+		      WMSK_BIT(PMU_CLR_GIC2_CORE_L_HW));
+
+	mmio_clrbits_32(PMU_BASE + PMU_PWRDN_CON,
+			BIT(PMU_SCU_B_PWRDWN_EN));
+
+	mmio_write_32(PMU_BASE + PMU_ADB400_CON,
+		      WMSK_BIT(PMU_PWRDWN_REQ_CORE_B_2GIC_SW) |
+		      WMSK_BIT(PMU_PWRDWN_REQ_CORE_B_SW) |
+		      WMSK_BIT(PMU_PWRDWN_REQ_GIC2_CORE_B_SW));
+
+	pmu_scu_b_pwrup();
+
+	plat_rockchip_gic_cpuif_enable();
 	return 0;
 }
 
+void __dead2 soc_soft_reset(void)
+{
+	struct gpio_info *rst_gpio;
+
+	rst_gpio = (struct gpio_info *)plat_get_rockchip_gpio_reset();
+
+	if (rst_gpio) {
+		gpio_set_direction(rst_gpio->index, GPIO_DIR_OUT);
+		gpio_set_value(rst_gpio->index, rst_gpio->polarity);
+	} else {
+		soc_global_soft_reset();
+	}
+
+	while (1)
+		;
+}
+
+void __dead2 soc_system_off(void)
+{
+	struct gpio_info *poweroff_gpio;
+
+	poweroff_gpio = (struct gpio_info *)plat_get_rockchip_gpio_poweroff();
+
+	if (poweroff_gpio) {
+		/*
+		 * if use tsadc over temp pin(GPIO1A6) as shutdown gpio,
+		 * need to set this pin iomux back to gpio function
+		 */
+		if (poweroff_gpio->index == TSADC_INT_PIN) {
+			mmio_write_32(PMUGRF_BASE + PMUGRF_GPIO1A_IOMUX,
+				      GPIO1A6_IOMUX);
+		}
+		gpio_set_direction(poweroff_gpio->index, GPIO_DIR_OUT);
+		gpio_set_value(poweroff_gpio->index, poweroff_gpio->polarity);
+	} else {
+		WARN("Do nothing when system off\n");
+	}
+
+	while (1)
+		;
+}
+
 static struct rockchip_pm_ops_cb pm_ops = {
 	.cores_pwr_dm_on = cores_pwr_domain_on,
 	.cores_pwr_dm_off = cores_pwr_domain_off,
@@ -284,7 +436,8 @@
 	.cores_pwr_dm_resume = cores_pwr_domain_resume,
 	.sys_pwr_dm_suspend = sys_pwr_domain_suspend,
 	.sys_pwr_dm_resume = sys_pwr_domain_resume,
-	.sys_gbl_soft_reset = soc_global_soft_reset,
+	.sys_gbl_soft_reset = soc_soft_reset,
+	.system_off = soc_system_off,
 };
 
 void plat_rockchip_pmu_init(void)
@@ -294,19 +447,24 @@
 	rockchip_pd_lock_init();
 	plat_setup_rockchip_pm_ops(&pm_ops);
 
+	/* register requires 32bits mode, switch it to 32 bits */
+	cpu_warm_boot_addr = (uint64_t)platform_cpu_warmboot;
+
 	for (cpu = 0; cpu < PLATFORM_CORE_COUNT; cpu++)
 		cpuson_flags[cpu] = 0;
 
-	psram_sleep_cfg->sys_mode = PMU_SYS_ON_MODE;
-
+	psram_sleep_cfg->ddr_func = 0x00;
+	psram_sleep_cfg->ddr_data = 0x00;
+	psram_sleep_cfg->ddr_flag = 0x00;
 	psram_sleep_cfg->boot_mpidr = read_mpidr_el1() & 0xffff;
 
 	/* cpu boot from pmusram */
 	mmio_write_32(SGRF_BASE + SGRF_SOC_CON0_1(1),
-		      (PMUSRAM_BASE >> CPU_BOOT_ADDR_ALIGN) |
+		      (cpu_warm_boot_addr >> CPU_BOOT_ADDR_ALIGN) |
 		      CPU_BOOT_ADDR_WMASK);
 
 	nonboot_cpus_off();
+
 	INFO("%s(%d): pd status %x\n", __func__, __LINE__,
 	     mmio_read_32(PMU_BASE + PMU_PWRDN_ST));
 }
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu.h b/plat/rockchip/rk3399/drivers/pmu/pmu.h
index 053b6ee..8f935e9 100644
--- a/plat/rockchip/rk3399/drivers/pmu/pmu.h
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu.h
@@ -341,6 +341,25 @@
 
 	PMU_DDRCTL1_C_SYSREQ_CFG = 12,
 	PMU_DDR1_IO_RET_CFG,
+	DBG_PWRUP_B0_CFG = 15,
+
+	DBG_NOPWERDWN_L0_EN,
+	DBG_NOPWERDWN_L1_EN,
+	DBG_NOPWERDWN_L2_EN,
+	DBG_NOPWERDWN_L3_EN,
+
+	DBG_PWRUP_REQ_L_EN = 20,
+	CLUSTER_L_CLK_SRC_GATING_CFG,
+	L2_FLUSH_REQ_CLUSTER_L,
+	ACINACTM_CLUSTER_L_CFG,
+
+	DBG_NO_PWERDWN_B0_EN,
+	DBG_NO_PWERDWN_B1_EN,
+
+	DBG_PWRUP_REQ_B_EN = 28,
+	CLUSTER_B_CLK_SRC_GATING_CFG,
+	L2_FLUSH_REQ_CLUSTER_B,
+	ACINACTM_CLUSTER_B_CFG,
 };
 
 enum pmu_int_con {
@@ -638,12 +657,100 @@
 	PMU_IDLE_ACK_SDIOAUDIO,
 };
 
+enum pmu_cci500_con {
+	PMU_PREQ_CCI500_CFG_SW = 0,
+	PMU_CLR_PREQ_CCI500_HW,
+	PMU_PSTATE_CCI500_0,
+	PMU_PSTATE_CCI500_1,
+
+	PMU_PSTATE_CCI500_2,
+	PMU_QREQ_CCI500_CFG_SW,
+	PMU_CLR_QREQ_CCI500_HW,
+	PMU_QGATING_CCI500_CFG,
+
+	PMU_PREQ_CCI500_CFG_SW_WMSK = 16,
+	PMU_CLR_PREQ_CCI500_HW_WMSK,
+	PMU_PSTATE_CCI500_0_WMSK,
+	PMU_PSTATE_CCI500_1_WMSK,
+
+	PMU_PSTATE_CCI500_2_WMSK,
+	PMU_QREQ_CCI500_CFG_SW_WMSK,
+	PMU_CLR_QREQ_CCI500_HW_WMSK,
+	PMU_QGATING_CCI500_CFG_WMSK,
+};
+
+enum pmu_adb400_con {
+	PMU_PWRDWN_REQ_CXCS_SW = 0,
+	PMU_PWRDWN_REQ_CORE_L_SW,
+	PMU_PWRDWN_REQ_CORE_L_2GIC_SW,
+	PMU_PWRDWN_REQ_GIC2_CORE_L_SW,
+
+	PMU_PWRDWN_REQ_CORE_B_SW,
+	PMU_PWRDWN_REQ_CORE_B_2GIC_SW,
+	PMU_PWRDWN_REQ_GIC2_CORE_B_SW,
+
+	PMU_CLR_CXCS_HW = 8,
+	PMU_CLR_CORE_L_HW,
+	PMU_CLR_CORE_L_2GIC_HW,
+	PMU_CLR_GIC2_CORE_L_HW,
+
+	PMU_CLR_CORE_B_HW,
+	PMU_CLR_CORE_B_2GIC_HW,
+	PMU_CLR_GIC2_CORE_B_HW,
+
+	PMU_PWRDWN_REQ_CXCS_SW_WMSK = 16,
+	PMU_PWRDWN_REQ_CORE_L_SW_WMSK,
+	PMU_PWRDWN_REQ_CORE_L_2GIC_SW_WMSK,
+	PMU_PWRDWN_REQ_GIC2_CORE_L_SW_WMSK,
+
+	PMU_PWRDWN_REQ_CORE_B_SW_WMSK,
+	PMU_PWRDWN_REQ_CORE_B_2GIC_SW_WMSK,
+	PMU_PWRDWN_REQ_GIC2_CORE_B_SW_WMSK,
+
+	PMU_CLR_CXCS_HW_WMSK = 24,
+	PMU_CLR_CORE_L_HW_WMSK,
+	PMU_CLR_CORE_L_2GIC_HW_WMSK,
+	PMU_CLR_GIC2_CORE_L_HW_WMSK,
+
+	PMU_CLR_CORE_B_HW_WMSK,
+	PMU_CLR_CORE_B_2GIC_HW_WMSK,
+	PMU_CLR_GIC2_CORE_B_HW_WMSK,
+};
+
+enum pmu_adb400_st {
+	PMU_PWRDWN_REQ_CXCS_SW_ST = 0,
+	PMU_PWRDWN_REQ_CORE_L_SW_ST,
+	PMU_PWRDWN_REQ_CORE_L_2GIC_SW_ST,
+	PMU_PWRDWN_REQ_GIC2_CORE_L_SW_ST,
+
+	PMU_PWRDWN_REQ_CORE_B_SW_ST,
+	PMU_PWRDWN_REQ_CORE_B_2GIC_SW_ST,
+	PMU_PWRDWN_REQ_GIC2_CORE_B_SW_ST,
+
+	PMU_CLR_CXCS_HW_ST = 8,
+	PMU_CLR_CORE_L_HW_ST,
+	PMU_CLR_CORE_L_2GIC_HW_ST,
+	PMU_CLR_GIC2_CORE_L_HW_ST,
+
+	PMU_CLR_CORE_B_HW_ST,
+	PMU_CLR_CORE_B_2GIC_HW_ST,
+	PMU_CLR_GIC2_CORE_B_HW_ST,
+};
+
 enum pmu_pwrdn_con1 {
 	PMU_VD_SCU_L_PWRDN_EN = 0,
 	PMU_VD_SCU_B_PWRDN_EN,
 	PMU_VD_CENTER_PWRDN_EN,
 };
 
+enum pmu_core_pwr_st {
+	L2_FLUSHDONE_CLUSTER_L = 0,
+	STANDBY_BY_WFIL2_CLUSTER_L,
+
+	L2_FLUSHDONE_CLUSTER_B = 10,
+	STANDBY_BY_WFIL2_CLUSTER_B,
+};
+
 #define PMU_WKUP_CFG0		0x00
 #define PMU_WKUP_CFG1		0x04
 #define PMU_WKUP_CFG2		0x08
@@ -701,9 +808,14 @@
 #define PMU_NOC_AUTO_ENA	0xd8
 #define PMU_PWRDN_CON1		0xdc
 
+#define PMUGRF_GPIO1A_IOMUX	0x10
+#define AP_PWROFF		0x0a
+#define GPIO1A6_IOMUX		BITS_WITH_WMASK(0, 3, 12)
+#define TSADC_INT_PIN		38
 #define CORES_PM_DISABLE	0x0
 
 #define PD_CTR_LOOP		500
 #define CHK_CPU_LOOP		500
+#define MAX_WAIT_CONUT		1000
 
 #endif /* __PMU_H__ */
diff --git a/plat/rockchip/rk3399/drivers/soc/soc.c b/plat/rockchip/rk3399/drivers/soc/soc.c
index f8d66c2..bf2d441 100644
--- a/plat/rockchip/rk3399/drivers/soc/soc.c
+++ b/plat/rockchip/rk3399/drivers/soc/soc.c
@@ -55,6 +55,20 @@
 			MT_DEVICE | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(RK3399_UART2_BASE, RK3399_UART2_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(PMUGRF_BASE, PMUGRF_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(GPIO0_BASE, GPIO0_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(GPIO1_BASE, GPIO1_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(GPIO2_BASE, GPIO2_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(GPIO3_BASE, GPIO3_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(GPIO4_BASE, GPIO4_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(GRF_BASE, GRF_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE),
 	{ 0 }
 };
 
@@ -313,12 +327,13 @@
 {
 	mmio_write_32(PMUCRU_BASE + CRU_PMU_RSTHOLD_CON(1),
 		      CRU_PMU_SGRF_RST_RLS);
+
+	mmio_clrbits_32(CRU_BASE + CRU_GLB_RST_CON,
+			CRU_PMU_WDTRST_MSK | CRU_PMU_FIRST_SFTRST_MSK);
 }
 
 void  __dead2 soc_global_soft_reset(void)
 {
-	uint32_t temp_val;
-
 	set_pll_slow_mode(VPLL_ID);
 	set_pll_slow_mode(NPLL_ID);
 	set_pll_slow_mode(GPLL_ID);
@@ -326,9 +341,9 @@
 	set_pll_slow_mode(PPLL_ID);
 	set_pll_slow_mode(ABPLL_ID);
 	set_pll_slow_mode(ALPLL_ID);
-	temp_val = mmio_read_32(CRU_BASE + CRU_GLB_RST_CON) |
-		   PMU_RST_BY_FIRST_SFT;
-	mmio_write_32(CRU_BASE + CRU_GLB_RST_CON, temp_val);
+
+	dsb();
+
 	mmio_write_32(CRU_BASE + CRU_GLB_SRST_FST, GLB_SRST_FST_CFG_VAL);
 
 	/*
@@ -345,4 +360,5 @@
 	dma_secure_cfg(0);
 	sgrf_init();
 	soc_global_soft_reset_init();
+	plat_rockchip_gpio_init();
 }
diff --git a/plat/rockchip/rk3399/drivers/soc/soc.h b/plat/rockchip/rk3399/drivers/soc/soc.h
index 9100d02..4c6f000 100644
--- a/plat/rockchip/rk3399/drivers/soc/soc.h
+++ b/plat/rockchip/rk3399/drivers/soc/soc.h
@@ -52,14 +52,16 @@
 #define NO_PLL_BYPASS			(0x00)
 #define NO_PLL_PWRDN			(0x00)
 
-#define PLL_SLOW_MODE			BITS_WITH_WMASK(PLL_MODE_MSK,\
-						SLOW_MODE, PLL_MODE_SHIFT)
-#define PLL_BYPASS_MODE			BITS_WITH_WMASK(PLL_BYPASS_MSK,\
-						PLL_BYPASS, PLL_BYPASS_SHIFT)
-#define PLL_NO_BYPASS_MODE		BITS_WITH_WMASK(PLL_BYPASS_MSK,\
-						NO_PLL_BYPASS, PLL_BYPASS_SHIFT)
-#define PLL_NOMAL_MODE			BITS_WITH_WMASK(PLL_MODE_MSK,\
-						NORMAL_MODE, PLL_MODE_SHIFT)
+#define PLL_SLOW_MODE		BITS_WITH_WMASK(SLOW_MODE,\
+						PLL_MODE_MSK, PLL_MODE_SHIFT)
+#define PLL_BYPASS_MODE		BITS_WITH_WMASK(PLL_BYPASS,\
+						PLL_BYPASS_MSK,\
+						PLL_BYPASS_SHIFT)
+#define PLL_NO_BYPASS_MODE	BITS_WITH_WMASK(NO_PLL_BYPASS,\
+						PLL_BYPASS_MSK,\
+						PLL_BYPASS_SHIFT)
+#define PLL_NOMAL_MODE		BITS_WITH_WMASK(NORMAL_MODE,\
+						PLL_MODE_MSK, PLL_MODE_SHIFT)
 
 #define PLL_CON_COUNT			0x06
 #define CRU_CLKSEL_COUNT		0x108
@@ -70,6 +72,8 @@
 #define REG_SIZE			0x04
 #define REG_SOC_WMSK			0xffff0000
 
+#define CLK_GATE_MASK			0x01
+
 enum plls_id {
 	ALPLL_ID = 0,
 	ABPLL_ID,
@@ -100,6 +104,9 @@
 	uint32_t cru_clksel_con[CRU_CLKSEL_COUNT];
 };
 
+#define CYCL_24M_CNT_US(us)	(24 * us)
+#define CYCL_24M_CNT_MS(ms)	(ms * CYCL_24M_CNT_US(1000))
+
 /**************************************************
  * secure timer
  **************************************************/
@@ -147,6 +154,11 @@
 #define CRU_GLB_SRST_FST	0x0500
 #define CRU_GLB_SRST_SND	0x0504
 
+#define CRU_CLKGATE_CON(n)	(0x300 + n * 4)
+#define PCLK_GPIO2_GATE_SHIFT	3
+#define PCLK_GPIO3_GATE_SHIFT	4
+#define PCLK_GPIO4_GATE_SHIFT	5
+
 /**************************************************
  * pmu cru reg, offset
  **************************************************/
@@ -155,6 +167,17 @@
 #define CRU_PMU_SGRF_RST_HOLD		BIT_WITH_WMSK(6)
 /* reset hold release*/
 #define CRU_PMU_SGRF_RST_RLS		WMSK_BIT(6)
+
+#define CRU_PMU_WDTRST_MSK		(0x1 << 4)
+#define CRU_PMU_WDTRST_EN		0x0
+
+#define CRU_PMU_FIRST_SFTRST_MSK	(0x3 << 2)
+#define CRU_PMU_FIRST_SFTRST_EN		0x0
+
+#define CRU_PMU_CLKGATE_CON(n)		(0x100 + n * 4)
+#define PCLK_GPIO0_GATE_SHIFT		3
+#define PCLK_GPIO1_GATE_SHIFT		4
+
 /**************************************************
  * sgrf reg, offset
  **************************************************/
diff --git a/plat/rockchip/rk3399/include/platform_def.h b/plat/rockchip/rk3399/include/platform_def.h
index 13f3d50..5f04db9 100644
--- a/plat/rockchip/rk3399/include/platform_def.h
+++ b/plat/rockchip/rk3399/include/platform_def.h
@@ -101,7 +101,7 @@
 /*
  * Put BL3-1 at the top of the Trusted RAM
  */
-#define BL31_BASE		(TZRAM_BASE + 0x8000)
+#define BL31_BASE		(TZRAM_BASE + 0x10000)
 #define BL31_LIMIT	(TZRAM_BASE + TZRAM_SIZE)
 
 /*******************************************************************************
@@ -109,7 +109,7 @@
  ******************************************************************************/
 #define ADDR_SPACE_SIZE		(1ull << 32)
 #define MAX_XLAT_TABLES		20
-#define MAX_MMAP_REGIONS	16
+#define MAX_MMAP_REGIONS	20
 
 /*******************************************************************************
  * Declarations and constants to access the mailboxes safely. Each mailbox is
diff --git a/plat/rockchip/rk3399/platform.mk b/plat/rockchip/rk3399/platform.mk
index 6d7e134..b0ce56f 100644
--- a/plat/rockchip/rk3399/platform.mk
+++ b/plat/rockchip/rk3399/platform.mk
@@ -57,17 +57,20 @@
                                 drivers/console/console.S                       \
                                 drivers/ti/uart/16550_console.S                 \
                                 drivers/delay_timer/delay_timer.c               \
+                                drivers/delay_timer/generic_delay_timer.c	\
+				drivers/gpio/gpio.c				\
                                 lib/cpus/aarch64/cortex_a53.S                   \
                                 lib/cpus/aarch64/cortex_a72.S                   \
                                 plat/common/aarch64/platform_mp_stack.S         \
                                 ${RK_PLAT_COMMON}/aarch64/plat_helpers.S        \
                                 ${RK_PLAT_COMMON}/bl31_plat_setup.c             \
+                                ${RK_PLAT_COMMON}/params_setup.c                \
                                 ${RK_PLAT_COMMON}/pmusram/pmu_sram_cpus_on.S	\
 				${RK_PLAT_COMMON}/pmusram/pmu_sram.c		\
-                                ${RK_PLAT_COMMON}/plat_delay_timer.c            \
                                 ${RK_PLAT_COMMON}/plat_pm.c                     \
                                 ${RK_PLAT_COMMON}/plat_topology.c               \
                                 ${RK_PLAT_COMMON}/aarch64/platform_common.c        \
+				${RK_PLAT_SOC}/drivers/gpio/rk3399_gpio.c	\
                                 ${RK_PLAT_SOC}/drivers/pmu/pmu.c                \
                                 ${RK_PLAT_SOC}/drivers/soc/soc.c
 
diff --git a/plat/rockchip/rk3399/rk3399_def.h b/plat/rockchip/rk3399/rk3399_def.h
index 8562148..ed3a424 100644
--- a/plat/rockchip/rk3399/rk3399_def.h
+++ b/plat/rockchip/rk3399/rk3399_def.h
@@ -61,6 +61,27 @@
 #define PMUSRAM_SIZE		SIZE_K(64)
 #define PMUSRAM_RSIZE		SIZE_K(8)
 
+#define PMUGRF_BASE		0xff320000
+#define PMUGRF_SIZE		SIZE_K(64)
+
+#define GPIO0_BASE		0xff720000
+#define GPIO0_SIZE		SIZE_K(64)
+
+#define GPIO1_BASE		0xff730000
+#define GPIO1_SIZE		SIZE_K(64)
+
+#define GPIO2_BASE		0xff780000
+#define GPIO2_SIZE		SIZE_K(32)
+
+#define GPIO3_BASE		0xff788000
+#define GPIO3_SIZE		SIZE_K(32)
+
+#define GPIO4_BASE		0xff790000
+#define GPIO4_SIZE		SIZE_K(32)
+
+#define GRF_BASE		0xff770000
+#define GRF_SIZE		SIZE_K(64)
+
 /*
  * include i2c pmu/audio, pwm0-3 rkpwm0-3 uart_dbg,mailbox scr
  * 0xff650000 -0xff6c0000
@@ -79,14 +100,13 @@
 #define RK3399_UART2_BASE	(0xff1a0000)
 #define RK3399_UART2_SIZE	SIZE_K(64)
 
-#define RK3399_BAUDRATE		(1500000)
+#define RK3399_BAUDRATE		(115200)
 #define RK3399_UART_CLOCK	(24000000)
 
 /******************************************************************************
  * System counter frequency related constants
  ******************************************************************************/
 #define SYS_COUNTER_FREQ_IN_TICKS	24000000
-#define SYS_COUNTER_FREQ_IN_MHZ		24
 
 /* Base rockchip_platform compatible GIC memory map */
 #define BASE_GICD_BASE		(GIC500_BASE)
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index aefc827..1ba301d 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -291,9 +291,9 @@
 	mmio_write_32(IOU_SCNTRS_CONTROL, IOU_SCNTRS_CONTROL_EN);
 }
 
-uint64_t plat_get_syscnt_freq(void)
+unsigned int plat_get_syscnt_freq2(void)
 {
-	uint64_t counter_base_frequency;
+	unsigned int counter_base_frequency;
 
 	/* FIXME: Read the frequency from Frequency modes table */
 	counter_base_frequency = zynqmp_get_system_timer_freq();
diff --git a/services/spd/opteed/opteed_main.c b/services/spd/opteed/opteed_main.c
index 6b4b14a..9770fb9 100644
--- a/services/spd/opteed/opteed_main.c
+++ b/services/spd/opteed/opteed_main.c
@@ -148,7 +148,7 @@
 	 * state i.e whether AArch32 or AArch64. Assuming it's AArch32
 	 * for the time being.
 	 */
-	opteed_rw = OPTEE_AARCH32;
+	opteed_rw = OPTEE_AARCH64;
 	opteed_init_optee_ep_state(optee_ep_info,
 				opteed_rw,
 				optee_ep_info->pc,
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 8a2b81c..5090037 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -711,6 +711,16 @@
 	psci_acquire_pwr_domain_locks(end_pwrlvl,
 				      cpu_idx);
 
+#if ENABLE_PSCI_STAT
+	/*
+	 * Capture power up time-stamp.
+	 * No cache maintenance is required as caches are off
+	 * and writes are direct to the main memory.
+	 */
+	PMF_CAPTURE_TIMESTAMP(psci_svc, PSCI_STAT_ID_EXIT_LOW_PWR,
+		PMF_NO_CACHE_MAINT);
+#endif
+
 	psci_get_target_local_pwr_states(end_pwrlvl, &state_info);
 
 	/*
@@ -736,6 +746,16 @@
 	 */
 	psci_set_pwr_domains_to_run(end_pwrlvl);
 
+#if ENABLE_PSCI_STAT
+	/*
+	 * Update PSCI stats.
+	 * Caches are off when writing stats data on the power down path.
+	 * Since caches are now enabled, it's necessary to do cache
+	 * maintenance before reading that same data.
+	 */
+	psci_stats_update_pwr_up(end_pwrlvl, &state_info, PMF_CACHE_MAINT);
+#endif
+
 	/*
 	 * This loop releases the lock corresponding to each power level
 	 * in the reverse order to which they were acquired.
diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S
index 5f4f91c..f8c0afa 100644
--- a/services/std_svc/psci/psci_entry.S
+++ b/services/std_svc/psci/psci_entry.S
@@ -106,7 +106,6 @@
 func psci_power_down_wfi
 	dsb	sy		// ensure write buffer empty
 	wfi
-wfi_spill:
-	b	wfi_spill
+	bl	plat_panic_handler
 endfunc psci_power_down_wfi
 
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index e6cd3a3..86f45ca 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -47,7 +47,6 @@
 
 {
 	int rc;
-	unsigned int end_pwrlvl;
 	entry_point_info_t ep;
 
 	/* Determine if the cpu exists of not */
@@ -64,11 +63,7 @@
 	 * To turn this cpu on, specify which power
 	 * levels need to be turned on
 	 */
-	end_pwrlvl = PLAT_MAX_PWR_LVL;
-	rc = psci_cpu_on_start(target_cpu,
-			    &ep,
-			    end_pwrlvl);
-	return rc;
+	return psci_cpu_on_start(target_cpu, &ep);
 }
 
 unsigned int psci_version(void)
@@ -115,11 +110,32 @@
 		 */
 		cpu_pd_state = state_info.pwr_domain_state[PSCI_CPU_PWR_LVL];
 		psci_set_cpu_local_state(cpu_pd_state);
+
+#if ENABLE_PSCI_STAT
+		/*
+		 * Capture time-stamp before CPU standby
+		 * No cache maintenance is needed as caches
+		 * are ON through out the CPU standby operation.
+		 */
+		PMF_CAPTURE_TIMESTAMP(psci_svc, PSCI_STAT_ID_ENTER_LOW_PWR,
+			PMF_NO_CACHE_MAINT);
+#endif
+
 		psci_plat_pm_ops->cpu_standby(cpu_pd_state);
 
 		/* Upon exit from standby, set the state back to RUN. */
 		psci_set_cpu_local_state(PSCI_LOCAL_STATE_RUN);
 
+#if ENABLE_PSCI_STAT
+		/* Capture time-stamp after CPU standby */
+		PMF_CAPTURE_TIMESTAMP(psci_svc, PSCI_STAT_ID_EXIT_LOW_PWR,
+			PMF_NO_CACHE_MAINT);
+
+		/* Update PSCI stats */
+		psci_stats_update_pwr_up(PSCI_CPU_PWR_LVL, &state_info,
+			PMF_NO_CACHE_MAINT);
+#endif
+
 		return PSCI_E_SUCCESS;
 	}
 
@@ -373,6 +389,14 @@
 		case PSCI_FEATURES:
 			SMC_RET1(handle, psci_features(x1));
 
+#if ENABLE_PSCI_STAT
+		case PSCI_STAT_RESIDENCY_AARCH32:
+			SMC_RET1(handle, psci_stat_residency(x1, x2));
+
+		case PSCI_STAT_COUNT_AARCH32:
+			SMC_RET1(handle, psci_stat_count(x1, x2));
+#endif
+
 		default:
 			break;
 		}
@@ -398,6 +422,14 @@
 		case PSCI_SYSTEM_SUSPEND_AARCH64:
 			SMC_RET1(handle, psci_system_suspend(x1, x2));
 
+#if ENABLE_PSCI_STAT
+		case PSCI_STAT_RESIDENCY_AARCH64:
+			SMC_RET1(handle, psci_stat_residency(x1, x2));
+
+		case PSCI_STAT_COUNT_AARCH64:
+			SMC_RET1(handle, psci_stat_count(x1, x2));
+#endif
+
 		default:
 			break;
 		}
diff --git a/services/std_svc/psci/psci_off.c b/services/std_svc/psci/psci_off.c
index cef6668..36dab49 100644
--- a/services/std_svc/psci/psci_off.c
+++ b/services/std_svc/psci/psci_off.c
@@ -100,6 +100,11 @@
 	 */
 	psci_do_state_coordination(end_pwrlvl, &state_info);
 
+#if ENABLE_PSCI_STAT
+	/* Update the last cpu for each level till end_pwrlvl */
+	psci_stats_update_pwr_down(end_pwrlvl, &state_info);
+#endif
+
 	/*
 	 * Arch. management. Perform the necessary steps to flush all
 	 * cpu caches.
@@ -112,6 +117,16 @@
 	 */
 	psci_plat_pm_ops->pwr_domain_off(&state_info);
 
+#if ENABLE_PSCI_STAT
+	/*
+	 * Capture time-stamp while entering low power state.
+	 * No cache maintenance needed because caches are off
+	 * and writes are direct to main memory.
+	 */
+	PMF_CAPTURE_TIMESTAMP(psci_svc, PSCI_STAT_ID_ENTER_LOW_PWR,
+		PMF_NO_CACHE_MAINT);
+#endif
+
 exit:
 	/*
 	 * Release the locks corresponding to each power level in the
@@ -138,11 +153,16 @@
 		dsbish();
 		inv_cpu_data(psci_svc_cpu_data.aff_info_state);
 
-		/*
-		 * Enter a wfi loop which will allow the power controller to
-		 * physically power down this cpu.
-		 */
-		psci_power_down_wfi();
+		if (psci_plat_pm_ops->pwr_domain_pwr_down_wfi) {
+			/* This function must not return */
+			psci_plat_pm_ops->pwr_domain_pwr_down_wfi(&state_info);
+		} else {
+			/*
+			 * Enter a wfi loop which will allow the power
+			 * controller to physically power down this cpu.
+			 */
+			psci_power_down_wfi();
+		}
 	}
 
 	return rc;
diff --git a/services/std_svc/psci/psci_on.c b/services/std_svc/psci/psci_on.c
index 200e622..c8c36cd 100644
--- a/services/std_svc/psci/psci_on.c
+++ b/services/std_svc/psci/psci_on.c
@@ -67,13 +67,16 @@
  * platform handler as it can return error.
  ******************************************************************************/
 int psci_cpu_on_start(u_register_t target_cpu,
-		      entry_point_info_t *ep,
-		      unsigned int end_pwrlvl)
+		      entry_point_info_t *ep)
 {
 	int rc;
 	unsigned int target_idx = plat_core_pos_by_mpidr(target_cpu);
 	aff_info_state_t target_aff_state;
 
+	/* Calling function must supply valid input arguments */
+	assert((int) target_idx >= 0);
+	assert(ep != NULL);
+
 	/*
 	 * This function must only be called on platforms where the
 	 * CPU_ON platform hooks have been implemented.
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 4b91ad5..ffb0732 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -35,6 +35,7 @@
 #include <bakery_lock.h>
 #include <bl_common.h>
 #include <cpu_data.h>
+#include <pmf.h>
 #include <psci.h>
 #include <spinlock.h>
 
@@ -67,7 +68,9 @@
 			define_psci_cap(PSCI_AFFINITY_INFO_AARCH64) |	\
 			define_psci_cap(PSCI_MIG_AARCH64) |		\
 			define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64) |	\
-			define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64))
+			define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64) |	\
+			define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64) |	\
+			define_psci_cap(PSCI_STAT_COUNT_AARCH64))
 
 /*
  * Helper macros to get/set the fields of PSCI per-cpu data.
@@ -102,6 +105,15 @@
 #define is_cpu_standby_req(is_power_down_state, retn_lvl) \
 		(((!(is_power_down_state)) && ((retn_lvl) == 0)) ? 1 : 0)
 
+/* Following are used as ID's to capture time-stamp */
+#define PSCI_STAT_ID_ENTER_LOW_PWR		0
+#define PSCI_STAT_ID_EXIT_LOW_PWR		1
+#define PSCI_STAT_TOTAL_IDS			2
+
+/* Declare PMF service functions for PSCI */
+PMF_DECLARE_CAPTURE_TIMESTAMP(psci_svc)
+PMF_DECLARE_GET_TIMESTAMP(psci_svc)
+
 /*******************************************************************************
  * The following two data structures implement the power domain tree. The tree
  * is used to track the state of all the nodes i.e. power domain instances
@@ -203,8 +215,7 @@
 
 /* Private exported functions from psci_on.c */
 int psci_cpu_on_start(unsigned long target_cpu,
-		      entry_point_info_t *ep,
-		      unsigned int end_pwrlvl);
+		      entry_point_info_t *ep);
 
 void psci_cpu_on_finish(unsigned int cpu_idx,
 			psci_power_state_t *state_info);
@@ -229,4 +240,15 @@
 void __dead2 psci_system_off(void);
 void __dead2 psci_system_reset(void);
 
+/* Private exported functions from psci_stat.c */
+void psci_stats_update_pwr_down(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info);
+void psci_stats_update_pwr_up(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info,
+			unsigned int flags);
+u_register_t psci_stat_residency(u_register_t target_cpu,
+			unsigned int power_state);
+u_register_t psci_stat_count(u_register_t target_cpu,
+			unsigned int power_state);
+
 #endif /* __PSCI_PRIVATE_H__ */
diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c
index cd1bb09..975b257 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -252,5 +252,10 @@
 	if (psci_plat_pm_ops->system_reset)
 		psci_caps |=  define_psci_cap(PSCI_SYSTEM_RESET);
 
+#if ENABLE_PSCI_STAT
+	psci_caps |=  define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64);
+	psci_caps |=  define_psci_cap(PSCI_STAT_COUNT_AARCH64);
+#endif
+
 	return 0;
 }
diff --git a/services/std_svc/psci/psci_stat.c b/services/std_svc/psci/psci_stat.c
new file mode 100644
index 0000000..155bbb0
--- /dev/null
+++ b/services/std_svc/psci/psci_stat.c
@@ -0,0 +1,309 @@
+/*
+ * 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 <debug.h>
+#include <platform.h>
+#include <platform_def.h>
+#include "psci_private.h"
+
+#ifndef PLAT_MAX_PWR_LVL_STATES
+#define PLAT_MAX_PWR_LVL_STATES 2
+#endif
+
+/* Ticks elapsed in one second by a signal of 1 MHz */
+#define MHZ_TICKS_PER_SEC 1000000
+
+/* Following structure is used for PSCI STAT */
+typedef struct psci_stat {
+	u_register_t residency;
+	u_register_t count;
+} psci_stat_t;
+
+/*
+ * Following is used to keep track of the last cpu
+ * that goes to power down in non cpu power domains.
+ */
+static int last_cpu_in_non_cpu_pd[PSCI_NUM_NON_CPU_PWR_DOMAINS] = {-1};
+
+/*
+ * Following are used to store PSCI STAT values for
+ * CPU and non CPU power domains.
+ */
+static psci_stat_t psci_cpu_stat[PLATFORM_CORE_COUNT]
+				[PLAT_MAX_PWR_LVL_STATES];
+static psci_stat_t psci_non_cpu_stat[PSCI_NUM_NON_CPU_PWR_DOMAINS]
+				[PLAT_MAX_PWR_LVL_STATES];
+
+/* Register PMF PSCI service */
+PMF_REGISTER_SERVICE(psci_svc, PMF_PSCI_STAT_SVC_ID,
+	 PSCI_STAT_TOTAL_IDS, PMF_STORE_ENABLE)
+
+/* The divisor to use to convert raw timestamp into microseconds */
+u_register_t residency_div;
+
+/*
+ * This macro calculates the stats residency in microseconds,
+ * taking in account the wrap around condition.
+ */
+#define calc_stat_residency(_pwrupts, _pwrdnts, _res)		\
+	do {							\
+		if (_pwrupts < _pwrdnts)			\
+			_res = UINT64_MAX - _pwrdnts + _pwrupts;\
+		else						\
+			_res = _pwrupts - _pwrdnts;		\
+		/* Convert timestamp into microseconds */	\
+		_res = _res/residency_div;			\
+	} while (0)
+
+/*
+ * This functions returns the index into the `psci_stat_t` array given the
+ * local power state and power domain level. If the platform implements the
+ * `get_pwr_lvl_state_idx` pm hook, then that will be used to return the index.
+ */
+static int get_stat_idx(plat_local_state_t local_state, int pwr_lvl)
+{
+	int idx;
+
+	if (psci_plat_pm_ops->get_pwr_lvl_state_idx == NULL) {
+		assert(PLAT_MAX_PWR_LVL_STATES == 2);
+		if (is_local_state_retn(local_state))
+			return 0;
+
+		assert(is_local_state_off(local_state));
+		return 1;
+	}
+
+	idx = psci_plat_pm_ops->get_pwr_lvl_state_idx(local_state, pwr_lvl);
+	assert((idx >= 0) && (idx < PLAT_MAX_PWR_LVL_STATES));
+	return idx;
+}
+
+/*******************************************************************************
+ * This function is passed the target local power states for each power
+ * domain (state_info) between the current CPU domain and its ancestors until
+ * the target power level (end_pwrlvl).
+ *
+ * Then, for each level (apart from the CPU level) until the 'end_pwrlvl', it
+ * updates the `last_cpu_in_non_cpu_pd[]` with last power down cpu id.
+ *
+ * This function will only be invoked with data cache enabled and while
+ * powering down a core.
+ ******************************************************************************/
+void psci_stats_update_pwr_down(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info)
+{
+	int lvl, parent_idx, cpu_idx = plat_my_core_pos();
+
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
+	assert(state_info);
+
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1; lvl <= end_pwrlvl; lvl++) {
+
+		/* Break early if the target power state is RUN */
+		if (is_local_state_run(state_info->pwr_domain_state[lvl]))
+			break;
+
+		/*
+		 * The power domain is entering a low power state, so this is
+		 * the last CPU for this power domain
+		 */
+		last_cpu_in_non_cpu_pd[parent_idx] = cpu_idx;
+
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+
+}
+
+/*******************************************************************************
+ * This function updates the PSCI STATS(residency time and count) for CPU
+ * and NON-CPU power domains.
+ * It is called with caches enabled and locks acquired(for NON-CPU domain)
+ ******************************************************************************/
+void psci_stats_update_pwr_up(unsigned int end_pwrlvl,
+			const psci_power_state_t *state_info,
+			unsigned int flags)
+{
+	int parent_idx, cpu_idx = plat_my_core_pos();
+	int lvl, stat_idx;
+	plat_local_state_t local_state;
+	unsigned long long pwrup_ts = 0, pwrdn_ts = 0;
+	u_register_t residency;
+
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
+	assert(state_info);
+
+	/* Initialize the residency divisor if not already initialized */
+	if (!residency_div) {
+		/* Pre-calculate divisor so that it can be directly used to
+		   convert time-stamp into microseconds */
+		residency_div = read_cntfrq_el0() / MHZ_TICKS_PER_SEC;
+		assert(residency_div);
+	}
+
+	/* Get power down time-stamp for current CPU */
+	PMF_GET_TIMESTAMP_BY_INDEX(psci_svc, PSCI_STAT_ID_ENTER_LOW_PWR,
+			cpu_idx, flags, pwrdn_ts);
+
+	/* In the case of 1st power on just return */
+	if (!pwrdn_ts)
+		return;
+
+	/* Get power up time-stamp for current CPU */
+	PMF_GET_TIMESTAMP_BY_INDEX(psci_svc, PSCI_STAT_ID_EXIT_LOW_PWR,
+			cpu_idx, flags, pwrup_ts);
+
+	/* Get the index into the stats array */
+	local_state = state_info->pwr_domain_state[PSCI_CPU_PWR_LVL];
+	stat_idx = get_stat_idx(local_state, PSCI_CPU_PWR_LVL);
+
+	/* Calculate stats residency */
+	calc_stat_residency(pwrup_ts, pwrdn_ts, residency);
+
+	/* Update CPU stats. */
+	psci_cpu_stat[cpu_idx][stat_idx].residency += residency;
+	psci_cpu_stat[cpu_idx][stat_idx].count++;
+
+	/*
+	 * Check what power domains above CPU were off
+	 * prior to this CPU powering on.
+	 */
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+	for (lvl = PSCI_CPU_PWR_LVL + 1; lvl <= end_pwrlvl; lvl++) {
+		local_state = state_info->pwr_domain_state[lvl];
+		if (is_local_state_run(local_state)) {
+			/* Break early */
+			break;
+		}
+
+		assert(last_cpu_in_non_cpu_pd[parent_idx] != -1);
+
+		/* Get power down time-stamp for last CPU */
+		PMF_GET_TIMESTAMP_BY_INDEX(psci_svc, PSCI_STAT_ID_ENTER_LOW_PWR,
+				last_cpu_in_non_cpu_pd[parent_idx],
+				flags, pwrdn_ts);
+
+		/* Initialize back to reset value */
+		last_cpu_in_non_cpu_pd[parent_idx] = -1;
+
+		/* Get the index into the stats array */
+		stat_idx = get_stat_idx(local_state, lvl);
+
+		/* Calculate stats residency */
+		calc_stat_residency(pwrup_ts, pwrdn_ts, residency);
+
+		/* Update non cpu stats */
+		psci_non_cpu_stat[parent_idx][stat_idx].residency += residency;
+		psci_non_cpu_stat[parent_idx][stat_idx].count++;
+
+		parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+	}
+
+}
+
+/*******************************************************************************
+ * This function returns the appropriate count and residency time of the
+ * local state for the highest power level expressed in the `power_state`
+ * for the node represented by `target_cpu`.
+ ******************************************************************************/
+int psci_get_stat(u_register_t target_cpu, unsigned int power_state,
+			 psci_stat_t *psci_stat)
+{
+	int rc, pwrlvl, lvl, parent_idx, stat_idx, target_idx;
+	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
+	plat_local_state_t local_state;
+
+	/* Validate the target_cpu parameter and determine the cpu index */
+	target_idx = plat_core_pos_by_mpidr(target_cpu);
+	if (target_idx == -1)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Validate the power_state parameter */
+	if (!psci_plat_pm_ops->translate_power_state_by_mpidr)
+		rc = psci_validate_power_state(power_state, &state_info);
+	else
+		rc = psci_plat_pm_ops->translate_power_state_by_mpidr(
+				target_cpu, power_state, &state_info);
+
+	if (rc != PSCI_E_SUCCESS)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Find the highest power level */
+	pwrlvl = psci_find_target_suspend_lvl(&state_info);
+	if (pwrlvl == PSCI_INVALID_PWR_LVL)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Get the index into the stats array */
+	local_state = state_info.pwr_domain_state[pwrlvl];
+	stat_idx = get_stat_idx(local_state, pwrlvl);
+
+	if (pwrlvl > PSCI_CPU_PWR_LVL) {
+		/* Get the power domain index */
+		parent_idx = psci_cpu_pd_nodes[target_idx].parent_node;
+		for (lvl = PSCI_CPU_PWR_LVL + 1; lvl < pwrlvl; lvl++)
+			parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node;
+
+		/* Get the non cpu power domain stats */
+		*psci_stat = psci_non_cpu_stat[parent_idx][stat_idx];
+	} else {
+		/* Get the cpu power domain stats */
+		*psci_stat = psci_cpu_stat[target_idx][stat_idx];
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+/* This is the top level function for PSCI_STAT_RESIDENCY SMC. */
+u_register_t psci_stat_residency(u_register_t target_cpu,
+		unsigned int power_state)
+{
+	psci_stat_t psci_stat;
+
+	int rc = psci_get_stat(target_cpu, power_state, &psci_stat);
+	if (rc == PSCI_E_SUCCESS)
+		return psci_stat.residency;
+	else
+		return 0;
+}
+
+/* This is the top level function for PSCI_STAT_COUNT SMC. */
+u_register_t psci_stat_count(u_register_t target_cpu,
+	unsigned int power_state)
+{
+	psci_stat_t psci_stat;
+
+	int rc = psci_get_stat(target_cpu, power_state, &psci_stat);
+	if (rc == PSCI_E_SUCCESS)
+		return psci_stat.count;
+	else
+		return 0;
+}
diff --git a/services/std_svc/psci/psci_suspend.c b/services/std_svc/psci/psci_suspend.c
index bd0c5db..e6c8cd9 100644
--- a/services/std_svc/psci/psci_suspend.c
+++ b/services/std_svc/psci/psci_suspend.c
@@ -168,6 +168,11 @@
 	 */
 	psci_do_state_coordination(end_pwrlvl, state_info);
 
+#if ENABLE_PSCI_STAT
+	/* Update the last cpu for each level till end_pwrlvl */
+	psci_stats_update_pwr_down(end_pwrlvl, state_info);
+#endif
+
 	if (is_power_down_state)
 		psci_suspend_to_pwrdown_start(end_pwrlvl, ep, state_info);
 
@@ -179,6 +184,16 @@
 	 */
 	psci_plat_pm_ops->pwr_domain_suspend(state_info);
 
+#if ENABLE_PSCI_STAT
+	/*
+	 * Capture time-stamp while entering low power state.
+	 * No cache maintenance needed because caches are off
+	 * and writes are direct to main memory.
+	 */
+	PMF_CAPTURE_TIMESTAMP(psci_svc, PSCI_STAT_ID_ENTER_LOW_PWR,
+		PMF_NO_CACHE_MAINT);
+#endif
+
 exit:
 	/*
 	 * Release the locks corresponding to each power level in the
@@ -189,8 +204,13 @@
 	if (skip_wfi)
 		return;
 
-	if (is_power_down_state)
-		psci_power_down_wfi();
+	if (is_power_down_state) {
+		/* The function calls below must not return */
+		if (psci_plat_pm_ops->pwr_domain_pwr_down_wfi)
+			psci_plat_pm_ops->pwr_domain_pwr_down_wfi(state_info);
+		else
+			psci_power_down_wfi();
+	}
 
 	/*
 	 * We will reach here if only retention/standby states have been
@@ -214,7 +234,7 @@
 void psci_cpu_suspend_finish(unsigned int cpu_idx,
 			     psci_power_state_t *state_info)
 {
-	unsigned long long counter_freq;
+	unsigned int counter_freq;
 	unsigned int max_off_lvl;
 
 	/* Ensure we have been woken up from a suspended state */
@@ -238,7 +258,7 @@
 	psci_do_pwrup_cache_maintenance();
 
 	/* Re-init the cntfrq_el0 register */
-	counter_freq = plat_get_syscnt_freq();
+	counter_freq = plat_get_syscnt_freq2();
 	write_cntfrq_el0(counter_freq);
 
 	/*
diff --git a/tools/fip_create/Makefile b/tools/fip_create/Makefile
index 2e367c2..30e4b82 100644
--- a/tools/fip_create/Makefile
+++ b/tools/fip_create/Makefile
@@ -69,7 +69,7 @@
 # path. This avoids conflicts with definitions in the compiler standard
 # include path.
 #
-uuid.h : ../../include/stdlib/sys/uuid.h
+uuid.h : ../../include/lib/stdlib/sys/uuid.h
 	$(call SHELL_COPY,$<,$@)
 
 firmware_image_package.h : ../../include/common/firmware_image_package.h
diff --git a/tools/fip_create/fip_create.c b/tools/fip_create/fip_create.c
index 7bce348..48cb3f6 100644
--- a/tools/fip_create/fip_create.c
+++ b/tools/fip_create/fip_create.c
@@ -811,7 +811,7 @@
 	if (fip_filename == NULL) {
 		printf("ERROR: Missing FIP filename\n");
 		print_usage();
-		return 0;
+		return EINVAL;
 	}
 
 	/* Unpack images from FIP always takes precedence over packaging. In