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 b1560a2..ab9b1ad 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
@@ -107,16 +110,29 @@
 # 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}
 
 
 ################################################################################
@@ -193,26 +209,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 +232,6 @@
 				-Iinclude/lib/aarch64		\
 				-Iinclude/lib/cpus/aarch64	\
 				-Iinclude/plat/common		\
-				-Iinclude/stdlib		\
-				-Iinclude/stdlib/sys		\
 				${PLAT_INCLUDES}		\
 				${SPD_INCLUDES}
 
@@ -392,6 +395,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))
@@ -419,6 +423,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))
@@ -550,15 +555,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_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.
+	 * Current EL with SP0 : 0x0 - 0x200
 	 * -----------------------------------------------------
 	 */
-	.align	7
-early_exceptions:
-	/* -----------------------------------------------------
-	 * Current EL with SP0 : 0x0 - 0x180
-	 * -----------------------------------------------------
-	 */
-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/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..759761e 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
@@ -448,6 +448,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 +631,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 +1538,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 +1724,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
@@ -2011,12 +2044,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 03310c3..0bdedf9 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -380,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.
@@ -482,17 +488,27 @@
 *   `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.
 
+*   `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.
+
+*    `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
 
@@ -1014,30 +1030,22 @@
 
 *   `fvp-base-gicv2-psci.dtb`
 
-    (Default) For use with both AEMv8 and Cortex-A57-A53 Base FVPs with
+    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.
+    (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`
 
-    (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.
+    For use with Foundation FVP with Base memory map configuration.
 
 *   `fvp-foundation-gicv3-psci.dtb`
 
-    For use with Foundation FVP with Base memory map configuration and Linux
-    GICv3 support.
+    (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
 
@@ -1056,10 +1064,13 @@
     --block-device="<path-to>/<file-system-image>"
 
 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
 
@@ -1164,88 +1175,6 @@
     --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.
-
 
 10.  Running the software on Juno
 ---------------------------------
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());
 }
 
-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)
+void generic_delay_timer_init_args(uint32_t mult, uint32_t div)
 {
-	timer_init(&plat_timer_ops);
+	ops.get_timer_value	= get_timer_value;
+	ops.clk_mult		= mult;
+	ops.clk_div		= div;
+
+	timer_init(&ops);
+
+	VERBOSE("Generic delay timer configured with mult=%u and div=%u\n",
+		mult, div);
 }
+
+void generic_delay_timer_init(void)
+{
+	/* 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/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..95e7780 100644
--- a/include/bl31/services/psci.h
+++ b/include/bl31/services/psci.h
@@ -265,6 +265,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,
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
 
 	/*
-	 * This macro verifies that the a given vector doesn't exceed the
+	 * 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 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/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 plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+void generic_delay_timer_init(void);
+
+#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;
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+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);
+
+#endif /* __IO_BLOCK_H__ */
diff --git a/include/drivers/io/io_storage.h b/include/drivers/io/io_storage.h
index f7e5b86..1b3028a 100644
--- a/include/drivers/io/io_storage.h
+++ b/include/drivers/io/io_storage.h
@@ -45,6 +45,7 @@
 	IO_TYPE_MEMMAP,
 	IO_TYPE_DUMMY,
 	IO_TYPE_FIRMWARE_IMAGE_PACKAGE,
+	IO_TYPE_BLOCK,
 	IO_TYPE_MAX
 } io_type_t;
 
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. */
 
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+#define CORTEX_A73_CPUECTLR_SMP_BIT	(1 << 6)
+
+#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/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 42260e9..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
  ******************************************************************************/
-unsigned long long 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_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/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/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 54c528c..fd10084 100644
--- a/lib/xlat_tables/xlat_tables_common.c
+++ b/lib/xlat_tables/xlat_tables_common.c
@@ -284,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);
 
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/common/aarch64/arm_common.c b/plat/arm/common/aarch64/arm_common.c
index 74c65ed..c4cc80e 100644
--- a/plat/arm/common/aarch64/arm_common.c
+++ b/plat/arm/common/aarch64/arm_common.c
@@ -29,6 +29,7 @@
  */
 #include <arch.h>
 #include <arch_helpers.h>
+#include <assert.h>
 #include <debug.h>
 #include <mmio.h>
 #include <plat_arm.h>
@@ -40,7 +41,14 @@
 /* Weak definitions may be overridden in specific ARM standard platform */
 #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
@@ -163,9 +171,17 @@
 	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);
@@ -176,3 +192,5 @@
 
 	return counter_base_frequency;
 }
+
+#endif /* ARM_SYS_CNTCTL_BASE */
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_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;
+	uint32_t ns;
 
-	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);
+	/* 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_ON_MTCMOS:
-		ret = mt_sip_pwr_on_mtcmos((uint32_t)x1);
-		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;
 
-	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);
-
-	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 d83e147..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)
 
-unsigned long long 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());
-}
+/* 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);
 
-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);
-}
+#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 fb47f48..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);
-unsigned long long 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 5b10505..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;
 }
 
-unsigned long long 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 eecedb3..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
  ******************************************************************************/
-unsigned long long 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());
-}
+	.macro plat_crash_print_regs
+	mov_imm	x17, GICC_BASE
+	mov_imm	x16, GICD_BASE
+	arm_print_gic_regs
+	.endm
 
-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);
-}
+#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 plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
+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);
+
+#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 ba4d1a4..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)
 
-unsigned long long 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/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;
 
-	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_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));
+
+	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 f7da0e7..5f04db9 100644
--- a/plat/rockchip/rk3399/include/platform_def.h
+++ b/plat/rockchip/rk3399/include/platform_def.h
@@ -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 4ddb0dd..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
@@ -86,7 +107,6 @@
  * 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 6e5cee3..f89cdce 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -297,9 +297,9 @@
 	mmio_write_32(IOU_SCNTRS_CONTROL, IOU_SCNTRS_CONTROL_EN);
 }
 
-unsigned long long plat_get_syscnt_freq(void)
+unsigned int plat_get_syscnt_freq2(void)
 {
-	unsigned long long 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_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_off.c b/services/std_svc/psci/psci_off.c
index cef6668..686666d 100644
--- a/services/std_svc/psci/psci_off.c
+++ b/services/std_svc/psci/psci_off.c
@@ -138,11 +138,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_suspend.c b/services/std_svc/psci/psci_suspend.c
index bd0c5db..8c6ab6b 100644
--- a/services/std_svc/psci/psci_suspend.c
+++ b/services/std_svc/psci/psci_suspend.c
@@ -189,8 +189,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 +219,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 +243,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