diff --git a/Makefile b/Makefile
index 1894ccd..9e148fb 100644
--- a/Makefile
+++ b/Makefile
@@ -149,7 +149,7 @@
 				-D__ASSEMBLY__ -ffreestanding 			\
 				-Wa,--fatal-warnings
 TF_CFLAGS		+=	$(CPPFLAGS) $(TF_CFLAGS_$(ARCH))		\
-				-ffreestanding -Wall -std=c99 -Os		\
+				-ffreestanding -fno-builtin -Wall -std=c99 -Os	\
 				-ffunction-sections -fdata-sections
 
 LDFLAGS			+=	--fatal-warnings -O1
@@ -351,6 +351,10 @@
 ENABLE_PMF			:= 1
 endif
 
+ifneq (${FIP_ALIGN},0)
+FIP_ARGS += --align ${FIP_ALIGN}
+endif
+
 ################################################################################
 # Auxiliary tools (fiptool, cert_create, etc)
 ################################################################################
diff --git a/docs/user-guide.md b/docs/user-guide.md
index 056c414..ebdb5a2 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -1120,7 +1120,9 @@
 NOTE: The software will not work on Version 1.0 of the Foundation FVP.
 The commands below would report an `unhandled argument` error in this case.
 
-NOTE: The Foundation FVP does not provide a debugger interface.
+NOTE: FVPs can be launched with `--cadi-server` option such that a
+CADI-compliant debugger (for example, ARM DS-5) can connect to and control its
+execution.
 
 The Foundation FVP is a cut down version of the AArch64 Base FVP. It can be
 downloaded for free from [ARM's website][ARM FVP website].
@@ -1438,7 +1440,7 @@
 
 
 [Firmware Design]:             firmware-design.md
-[ARM FVP website]:             http://www.arm.com/fvp
+[ARM FVP website]:             https://developer.arm.com/products/system-design/fixed-virtual-platforms
 [Linaro Release Notes]:        https://community.arm.com/docs/DOC-10952#jive_content_id_Linaro_Release_1606
 [ARM Platforms Portal]:        https://community.arm.com/groups/arm-development-platforms
 [Linaro SW Instructions]:      https://community.arm.com/docs/DOC-10803
diff --git a/include/bl32/payloads/tlk.h b/include/bl32/payloads/tlk.h
index 6ce1eb6..d355313 100644
--- a/include/bl32/payloads/tlk.h
+++ b/include/bl32/payloads/tlk.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -31,6 +31,8 @@
 #ifndef __TLK_H__
 #define __TLK_H__
 
+#include <utils.h>
+
 /*
  * Generate function IDs for the Trusted OS/Apps
  */
@@ -51,13 +53,13 @@
  * SMC function IDs that TLK uses to signal various forms of completions
  * to the secure payload dispatcher.
  */
-#define TLK_REQUEST_DONE	(0x32000001 | (1 << 31))
-#define TLK_PREEMPTED		(0x32000002 | (1 << 31))
-#define TLK_ENTRY_DONE		(0x32000003 | (1 << 31))
-#define TLK_VA_TRANSLATE	(0x32000004 | (1 << 31))
-#define TLK_SUSPEND_DONE	(0x32000005 | (1 << 31))
-#define TLK_RESUME_DONE		(0x32000006 | (1 << 31))
-#define TLK_SYSTEM_OFF_DONE	(0x32000007 | (1 << 31))
+#define TLK_REQUEST_DONE	(0x32000001 | (ULL(1) << 31))
+#define TLK_PREEMPTED		(0x32000002 | (ULL(1) << 31))
+#define TLK_ENTRY_DONE		(0x32000003 | (ULL(1) << 31))
+#define TLK_VA_TRANSLATE	(0x32000004 | (ULL(1) << 31))
+#define TLK_SUSPEND_DONE	(0x32000005 | (ULL(1) << 31))
+#define TLK_RESUME_DONE		(0x32000006 | (ULL(1) << 31))
+#define TLK_SYSTEM_OFF_DONE	(0x32000007 | (ULL(1) << 31))
 
 /*
  * Trusted Application specific function IDs
diff --git a/include/lib/smcc.h b/include/lib/smcc.h
index c415ba1..2f562c5 100644
--- a/include/lib/smcc.h
+++ b/include/lib/smcc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -31,6 +31,8 @@
 #ifndef __SMCC_H__
 #define __SMCC_H__
 
+#include <utils.h>
+
 /*******************************************************************************
  * Bit definitions inside the function id as per the SMC calling convention
  ******************************************************************************/
@@ -57,7 +59,7 @@
 #define SMC_64				1
 #define SMC_32				0
 #define SMC_UNK				0xffffffff
-#define SMC_TYPE_FAST			1
+#define SMC_TYPE_FAST			ULL(1)
 #define SMC_TYPE_STD			0
 #define SMC_PREEMPTED		0xfffffffe
 /*******************************************************************************
diff --git a/include/lib/utils.h b/include/lib/utils.h
index f7af8f6..b6bc9af 100644
--- a/include/lib/utils.h
+++ b/include/lib/utils.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2017, 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:
@@ -66,4 +66,18 @@
 #define check_uptr_overflow(ptr, inc)		\
 	(((ptr) > UINTPTR_MAX - (inc)) ? 1 : 0)
 
+/*
+ * For those constants to be shared between C and other sources, apply a 'ull'
+ * suffix to the argument only in C, to avoid undefined or unintended behaviour.
+ *
+ * The GNU assembler and linker do not support the 'ull' suffix (it causes the
+ * build process to fail) therefore the suffix is omitted when used in linker
+ * scripts and assembler files.
+*/
+#if defined(__LINKER__) || defined(__ASSEMBLY__)
+# define ULL(_x)	(_x)
+#else
+# define ULL(_x)	(_x##ull)
+#endif
+
 #endif /* __UTILS_H__ */
diff --git a/include/plat/arm/board/common/board_css_def.h b/include/plat/arm/board/common/board_css_def.h
index 975f1fc..65e3d32 100644
--- a/include/plat/arm/board/common/board_css_def.h
+++ b/include/plat/arm/board/common/board_css_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2017, 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:
@@ -68,7 +68,7 @@
  * development platforms
  */
 
-#define PLAT_ARM_DRAM2_SIZE			MAKE_ULL(0x180000000)
+#define PLAT_ARM_DRAM2_SIZE			ULL(0x180000000)
 
 /* UART related constants */
 #define PLAT_ARM_BOOT_UART_BASE			SOC_CSS_UART0_BASE
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 6d7bcd1..4d26444 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2017, 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:
@@ -89,7 +89,7 @@
  *   - SCP TZC DRAM: If present, DRAM reserved for SCP use
  *   - AP TZC DRAM: The remaining TZC secured DRAM reserved for AP use
  */
-#define ARM_TZC_DRAM1_SIZE		MAKE_ULL(0x01000000)
+#define ARM_TZC_DRAM1_SIZE		ULL(0x01000000)
 
 #define ARM_SCP_TZC_DRAM1_BASE		(ARM_DRAM1_BASE +		\
 					 ARM_DRAM1_SIZE -		\
@@ -113,12 +113,12 @@
 #define ARM_NS_DRAM1_END		(ARM_NS_DRAM1_BASE +		\
 					 ARM_NS_DRAM1_SIZE - 1)
 
-#define ARM_DRAM1_BASE			MAKE_ULL(0x80000000)
-#define ARM_DRAM1_SIZE			MAKE_ULL(0x80000000)
+#define ARM_DRAM1_BASE			ULL(0x80000000)
+#define ARM_DRAM1_SIZE			ULL(0x80000000)
 #define ARM_DRAM1_END			(ARM_DRAM1_BASE +		\
 					 ARM_DRAM1_SIZE - 1)
 
-#define ARM_DRAM2_BASE			MAKE_ULL(0x880000000)
+#define ARM_DRAM2_BASE			ULL(0x880000000)
 #define ARM_DRAM2_SIZE			PLAT_ARM_DRAM2_SIZE
 #define ARM_DRAM2_END			(ARM_DRAM2_BASE +		\
 					 ARM_DRAM2_SIZE - 1)
diff --git a/include/plat/arm/soc/common/soc_css_def.h b/include/plat/arm/soc/common/soc_css_def.h
index f1396a6..316f8f9 100644
--- a/include/plat/arm/soc/common/soc_css_def.h
+++ b/include/plat/arm/soc/common/soc_css_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2017, 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:
@@ -98,7 +98,7 @@
  */
 
 /* 2MB used for SCP DDR retraining */
-#define PLAT_ARM_SCP_TZC_DRAM1_SIZE	MAKE_ULL(0x00200000)
+#define PLAT_ARM_SCP_TZC_DRAM1_SIZE	ULL(0x00200000)
 
 
 #endif /* __SOC_CSS_DEF_H__ */
diff --git a/include/plat/common/common_def.h b/include/plat/common/common_def.h
index 65f31e7..05588c1 100644
--- a/include/plat/common/common_def.h
+++ b/include/plat/common/common_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -54,26 +54,6 @@
  */
 #define FIRMWARE_WELCOME_STR		"Booting Trusted Firmware\n"
 
-/*
- * Some of the platform porting definitions use the 'ull' suffix in order to
- * avoid subtle integer overflow errors due to implicit integer type promotion
- * when working with 32-bit values.
- *
- * The TSP linker script includes some of these definitions to define the BL32
- * memory map, but the GNU LD does not support the 'ull' suffix, causing the
- * build process to fail. To solve this problem, the auxiliary macro MAKE_ULL(x)
- * will add the 'ull' suffix only when the macro __LINKER__  is not defined
- * (__LINKER__ is defined in the command line to preprocess the linker script).
- * Constants in the linker script will not have the 'ull' suffix, but this is
- * not a problem since the linker evaluates all constant expressions to 64 bit
- * (assuming the target architecture is 64 bit).
- */
-#ifndef __LINKER__
-  #define MAKE_ULL(x)			x##ull
-#else
-  #define MAKE_ULL(x)			x
-#endif
-
 #if LOAD_IMAGE_V2
 #define BL2_IMAGE_DESC {				\
 	.image_id = BL2_IMAGE_ID,			\
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index fc39819..0b93dfc 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -89,6 +89,9 @@
 # Build flag to treat usage of deprecated platform and framework APIs as error.
 ERROR_DEPRECATED		:= 0
 
+# Byte alignment that each component in FIP is aligned to
+FIP_ALIGN			:= 0
+
 # Default FIP file name
 FIP_NAME			:= fip.bin
 
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index d0898ad..bf5e03b 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2017, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -60,9 +60,9 @@
 #define PLAT_ARM_TRUSTED_DRAM_SIZE	0x02000000	/* 32 MB */
 
 /* No SCP in FVP */
-#define PLAT_ARM_SCP_TZC_DRAM1_SIZE	MAKE_ULL(0x0)
+#define PLAT_ARM_SCP_TZC_DRAM1_SIZE	ULL(0x0)
 
-#define PLAT_ARM_DRAM2_SIZE		MAKE_ULL(0x780000000)
+#define PLAT_ARM_DRAM2_SIZE		ULL(0x780000000)
 
 /*
  * Load address of BL33 for this platform port
diff --git a/plat/common/tbbr/plat_tbbr.c b/plat/common/tbbr/plat_tbbr.c
index 475564a..fde4d54 100644
--- a/plat/common/tbbr/plat_tbbr.c
+++ b/plat/common/tbbr/plat_tbbr.c
@@ -60,7 +60,7 @@
 	 * has been signed with the ROT key. Non Trusted NV counter
 	 * updates are unconditional.
 	 */
-	if (!trusted_nv_ctr || (trusted_nv_ctr && img_desc->parent == NULL))
+	if (!trusted_nv_ctr || img_desc->parent == NULL)
 		return plat_set_nv_ctr(cookie, nv_ctr);
 
 	/*
diff --git a/plat/mediatek/mt8173/aarch64/plat_helpers.S b/plat/mediatek/mt8173/aarch64/plat_helpers.S
index af3a407..3347f55 100644
--- a/plat/mediatek/mt8173/aarch64/plat_helpers.S
+++ b/plat/mediatek/mt8173/aarch64/plat_helpers.S
@@ -34,6 +34,7 @@
 	.globl	plat_secondary_cold_boot_setup
 	.globl	plat_report_exception
 	.globl	platform_is_primary_cpu
+	.globl  plat_my_core_pos
 	.globl	plat_crash_console_init
 	.globl	plat_crash_console_putc
 
@@ -59,6 +60,22 @@
 	ret
 endfunc platform_is_primary_cpu
 
+#if !ENABLE_PLAT_COMPAT
+	/* -----------------------------------------------------
+	 * unsigned int plat_my_core_pos(void);
+	 *
+	 * result: CorePos = CoreId + (ClusterId << 2)
+	 * -----------------------------------------------------
+	 */
+func plat_my_core_pos
+	mrs     x0, mpidr_el1
+	and     x1, x0, #MPIDR_CPU_MASK
+	and     x0, x0, #MPIDR_CLUSTER_MASK
+	add     x0, x1, x0, LSR #6
+	ret
+endfunc plat_my_core_pos
+#endif
+
 	/* ---------------------------------------------
 	 * int plat_crash_console_init(void)
 	 * Function to initialize the crash console
diff --git a/plat/mediatek/mt8173/bl31_plat_setup.c b/plat/mediatek/mt8173/bl31_plat_setup.c
index ef18d07..92c3494 100644
--- a/plat/mediatek/mt8173/bl31_plat_setup.c
+++ b/plat/mediatek/mt8173/bl31_plat_setup.c
@@ -27,7 +27,6 @@
  * 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 <common_def.h>
@@ -37,6 +36,7 @@
 #include <mcucfg.h>
 #include <mmio.h>
 #include <mtcmos.h>
+#include <plat_arm.h>
 #include <plat_private.h>
 #include <platform.h>
 #include <spm.h>
@@ -159,11 +159,13 @@
 	generic_delay_timer_init();
 
 	/* Initialize the gic cpu and distributor interfaces */
-	plat_mt_gic_init();
-	arm_gic_setup();
+	plat_arm_gic_driver_init();
+	plat_arm_gic_init();
 
+#if ENABLE_PLAT_COMPAT
 	/* Topologies are best known to the platform. */
 	mt_setup_topology();
+#endif
 
 	/* Initialize spm at boot time */
 	spm_boot_init();
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c b/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
index b89cd97..4cafd29 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
@@ -27,6 +27,7 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#include <arch.h>
 #include <mmio.h>
 #include <mt8173_def.h>
 #include <platform.h>
@@ -258,7 +259,9 @@
 {
 	unsigned long linear_id;
 
-	linear_id = platform_get_core_pos(mpidr);
+	linear_id = ((mpidr & MPIDR_CLUSTER_MASK) >> 6) |
+			(mpidr & MPIDR_CPU_MASK);
+
 	spm_lock_get();
 	if (is_hotplug_ready() == 0) {
 		spm_mcdi_wakeup_all_cores();
@@ -277,7 +280,9 @@
 {
 	unsigned long linear_id;
 
-	linear_id = platform_get_core_pos(mpidr);
+	linear_id = ((mpidr & MPIDR_CLUSTER_MASK) >> 6) |
+			(mpidr & MPIDR_CPU_MASK);
+
 	spm_lock_get();
 	if (is_hotplug_ready() == 0) {
 		spm_mcdi_wakeup_all_cores();
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
index d715735..4d8422f 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
@@ -511,7 +511,10 @@
 
 void spm_mcdi_finish_for_on_state(unsigned long mpidr, unsigned int afflvl)
 {
-	unsigned long linear_id = platform_get_core_pos(mpidr);
+	unsigned long linear_id;
+
+	linear_id = ((mpidr & MPIDR_CLUSTER_MASK) >> 6) |
+			(mpidr & MPIDR_CPU_MASK);
 
 	spm_lock_get();
 	spm_mcdi_clear_cputop_pwrctrl_for_cluster_on(mpidr);
diff --git a/plat/mediatek/mt8173/include/mt8173_def.h b/plat/mediatek/mt8173/include/mt8173_def.h
index 87e9c04..21c36b5 100644
--- a/plat/mediatek/mt8173/include/mt8173_def.h
+++ b/plat/mediatek/mt8173/include/mt8173_def.h
@@ -126,4 +126,51 @@
 #define MT_IRQ_SEC_SGI_6	14
 #define MT_IRQ_SEC_SGI_7	15
 
+/*
+ *  Macros for local power states in MTK platforms encoded by State-ID field
+ *  within the power-state parameter.
+ */
+/* Local power state for power domains in Run state. */
+#define MTK_LOCAL_STATE_RUN     0
+/* Local power state for retention. Valid only for CPU power domains */
+#define MTK_LOCAL_STATE_RET     1
+/* Local power state for OFF/power-down. Valid for CPU and cluster power
+ * domains
+ */
+#define MTK_LOCAL_STATE_OFF     2
+
+#if PSCI_EXTENDED_STATE_ID
+/*
+ * Macros used to parse state information from State-ID if it is using the
+ * recommended encoding for State-ID.
+ */
+#define MTK_LOCAL_PSTATE_WIDTH		4
+#define MTK_LOCAL_PSTATE_MASK		((1 << MTK_LOCAL_PSTATE_WIDTH) - 1)
+
+/* Macros to construct the composite power state */
+
+/* Make composite power state parameter till power level 0 */
+
+#define mtk_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type) \
+	(((lvl0_state) << PSTATE_ID_SHIFT) | ((type) << PSTATE_TYPE_SHIFT))
+#else
+#define mtk_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__ */
+
+/* Make composite power state parameter till power level 1 */
+#define mtk_make_pwrstate_lvl1(lvl1_state, lvl0_state, pwr_lvl, type) \
+		(((lvl1_state) << MTK_LOCAL_PSTATE_WIDTH) | \
+		mtk_make_pwrstate_lvl0(lvl0_state, pwr_lvl, type))
+
+/* Make composite power state parameter till power level 2 */
+#define mtk_make_pwrstate_lvl2( \
+		lvl2_state, lvl1_state, lvl0_state, pwr_lvl, type) \
+		(((lvl2_state) << (MTK_LOCAL_PSTATE_WIDTH * 2)) | \
+		mtk_make_pwrstate_lvl1(lvl1_state, lvl0_state, pwr_lvl, type))
+
+
 #endif /* __MT8173_DEF_H__ */
diff --git a/plat/mediatek/mt8173/include/plat_macros.S b/plat/mediatek/mt8173/include/plat_macros.S
index 018a790..f95fd5b 100644
--- a/plat/mediatek/mt8173/include/plat_macros.S
+++ b/plat/mediatek/mt8173/include/plat_macros.S
@@ -29,7 +29,8 @@
  */
 
 #include <cci.h>
-#include <gic_v2.h>
+#include <gic_common.h>
+#include <gicv2.h>
 #include <mt8173_def.h>
 
 .section .rodata.gic_reg_name, "aS"
diff --git a/plat/mediatek/mt8173/include/plat_private.h b/plat/mediatek/mt8173/include/plat_private.h
index ae50e44..9488b88 100644
--- a/plat/mediatek/mt8173/include/plat_private.h
+++ b/plat/mediatek/mt8173/include/plat_private.h
@@ -45,9 +45,6 @@
 void plat_cci_enable(void);
 void plat_cci_disable(void);
 
-/* Declarations for plat_mt_gic.c */
-void plat_mt_gic_init(void);
-
 /* Declarations for plat_topology.c */
 int mt_setup_topology(void);
 
diff --git a/plat/mediatek/mt8173/include/platform_def.h b/plat/mediatek/mt8173/include/platform_def.h
index 8f771e3..bb69920 100644
--- a/plat/mediatek/mt8173/include/platform_def.h
+++ b/plat/mediatek/mt8173/include/platform_def.h
@@ -31,6 +31,8 @@
 #ifndef __PLATFORM_DEF_H__
 #define __PLATFORM_DEF_H__
 
+#include "mt8173_def.h"
+
 
 /*******************************************************************************
  * Platform binary types for linking
@@ -56,6 +58,11 @@
 #define FIRMWARE_WELCOME_STR		"Booting Trusted Firmware\n"
 
 #define PLATFORM_MAX_AFFLVL		MPIDR_AFFLVL2
+#if !ENABLE_PLAT_COMPAT
+#define PLAT_MAX_PWR_LVL		2
+#define PLAT_MAX_RET_STATE		1
+#define PLAT_MAX_OFF_STATE		2
+#endif
 #define PLATFORM_SYSTEM_COUNT		1
 #define PLATFORM_CLUSTER_COUNT		2
 #define PLATFORM_CLUSTER0_CORE_COUNT	4
@@ -111,7 +118,8 @@
 /*******************************************************************************
  * Platform specific page table and MMU setup constants
  ******************************************************************************/
-#define ADDR_SPACE_SIZE		(1ull << 32)
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ull << 32)
 #define MAX_XLAT_TABLES		4
 #define MAX_MMAP_REGIONS	16
 
@@ -127,4 +135,19 @@
 #define CACHE_WRITEBACK_SHIFT	6
 #define CACHE_WRITEBACK_GRANULE	(1 << CACHE_WRITEBACK_SHIFT)
 
+
+#define PLAT_ARM_GICD_BASE      BASE_GICD_BASE
+#define PLAT_ARM_GICC_BASE      BASE_GICC_BASE
+
+#define PLAT_ARM_G1S_IRQS       MT_IRQ_SEC_SGI_0, \
+				MT_IRQ_SEC_SGI_1, \
+				MT_IRQ_SEC_SGI_2, \
+				MT_IRQ_SEC_SGI_3, \
+				MT_IRQ_SEC_SGI_4, \
+				MT_IRQ_SEC_SGI_5, \
+				MT_IRQ_SEC_SGI_6, \
+				MT_IRQ_SEC_SGI_7
+
+#define PLAT_ARM_G0_IRQS
+
 #endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/mediatek/mt8173/plat_pm.c b/plat/mediatek/mt8173/plat_pm.c
index 6bb8a9b..d727967 100644
--- a/plat/mediatek/mt8173/plat_pm.c
+++ b/plat/mediatek/mt8173/plat_pm.c
@@ -29,17 +29,18 @@
  */
 
 #include <arch_helpers.h>
-#include <arm_gic.h>
 #include <assert.h>
 #include <bakery_lock.h>
 #include <cci.h>
 #include <console.h>
 #include <debug.h>
 #include <errno.h>
+#include <gicv2.h>
 #include <mcucfg.h>
 #include <mmio.h>
 #include <mt8173_def.h>
 #include <mt_cpuxgpt.h> /* generic_timer_backup() */
+#include <plat_arm.h>
 #include <plat_private.h>
 #include <power_tracer.h>
 #include <psci.h>
@@ -49,6 +50,44 @@
 #include <spm_mcdi.h>
 #include <spm_suspend.h>
 
+#if !ENABLE_PLAT_COMPAT
+#define MTK_PWR_LVL0	0
+#define MTK_PWR_LVL1	1
+#define MTK_PWR_LVL2	2
+
+/* Macros to read the MTK power domain state */
+#define MTK_CORE_PWR_STATE(state)	(state)->pwr_domain_state[MTK_PWR_LVL0]
+#define MTK_CLUSTER_PWR_STATE(state)	(state)->pwr_domain_state[MTK_PWR_LVL1]
+#define MTK_SYSTEM_PWR_STATE(state)	((PLAT_MAX_PWR_LVL > MTK_PWR_LVL1) ?\
+			(state)->pwr_domain_state[MTK_PWR_LVL2] : 0)
+#endif
+
+#if PSCI_EXTENDED_STATE_ID
+/*
+ *  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.
+ */
+const unsigned int mtk_pm_idle_states[] = {
+	/* State-id - 0x001 */
+	mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_RUN, MTK_LOCAL_STATE_RUN,
+		MTK_LOCAL_STATE_RET, MTK_PWR_LVL0, PSTATE_TYPE_STANDBY),
+	/* State-id - 0x002 */
+	mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_RUN, MTK_LOCAL_STATE_RUN,
+		MTK_LOCAL_STATE_OFF, MTK_PWR_LVL0, PSTATE_TYPE_POWERDOWN),
+	/* State-id - 0x022 */
+	mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_RUN, MTK_LOCAL_STATE_OFF,
+		MTK_LOCAL_STATE_OFF, MTK_PWR_LVL1, PSTATE_TYPE_POWERDOWN),
+#if PLAT_MAX_PWR_LVL > MTK_PWR_LVL1
+	/* State-id - 0x222 */
+	mtk_make_pwrstate_lvl2(MTK_LOCAL_STATE_OFF, MTK_LOCAL_STATE_OFF,
+		MTK_LOCAL_STATE_OFF, MTK_PWR_LVL2, PSTATE_TYPE_POWERDOWN),
+#endif
+	0,
+};
+#endif
+
 struct core_context {
 	unsigned long timer_data[8];
 	unsigned int count;
@@ -219,6 +258,7 @@
 	mt_cpu_restore(mpidr);
 }
 
+#if ENABLE_PLAT_COMPAT
 /*******************************************************************************
 * Private function which is used to determine if any platform actions
 * should be performed for the specified affinity instance given its
@@ -270,11 +310,25 @@
 		wfi();
 	}
 }
+#else
+static void plat_cpu_standby(plat_local_state_t cpu_state)
+{
+	unsigned int scr;
+
+	scr = read_scr_el3();
+	write_scr_el3(scr | SCR_IRQ_BIT);
+	isb();
+	dsb();
+	wfi();
+	write_scr_el3(scr);
+}
+#endif
 
 /*******************************************************************************
  * MTK_platform handler called when an affinity instance is about to be turned
  * on. The level and mpidr determine the affinity instance.
  ******************************************************************************/
+#if ENABLE_PLAT_COMPAT
 static int plat_affinst_on(unsigned long mpidr,
 		    unsigned long sec_entrypoint,
 		    unsigned int afflvl,
@@ -308,6 +362,32 @@
 
 	return rc;
 }
+#else
+static uintptr_t secure_entrypoint;
+
+static int plat_power_domain_on(unsigned long mpidr)
+{
+	int rc = PSCI_E_SUCCESS;
+	unsigned long cpu_id;
+	unsigned long cluster_id;
+	uintptr_t rv;
+
+	cpu_id = mpidr & MPIDR_CPU_MASK;
+	cluster_id = mpidr & MPIDR_CLUSTER_MASK;
+
+	if (cluster_id)
+		rv = (uintptr_t)&mt8173_mcucfg->mp1_rv_addr[cpu_id].rv_addr_lw;
+	else
+		rv = (uintptr_t)&mt8173_mcucfg->mp0_rv_addr[cpu_id].rv_addr_lw;
+
+	mmio_write_32(rv, secure_entrypoint);
+	INFO("mt_on[%ld:%ld], entry %x\n",
+		cluster_id, cpu_id, mmio_read_32(rv));
+
+	spm_hotplug_on(mpidr);
+	return rc;
+}
+#endif
 
 /*******************************************************************************
  * MTK_platform handler called when an affinity instance is about to be turned
@@ -321,6 +401,7 @@
  * dealt with. So do not write & read global variables across calls. It will be
  * wise to do flush a write to the global to prevent unpredictable results.
  ******************************************************************************/
+#if ENABLE_PLAT_COMPAT
 static void plat_affinst_off(unsigned int afflvl, unsigned int state)
 {
 	unsigned long mpidr = read_mpidr_el1();
@@ -330,7 +411,7 @@
 		return;
 
 	/* Prevent interrupts from spuriously waking up this cpu */
-	arm_gic_cpuif_deactivate();
+	gicv2_cpuif_disable();
 
 	spm_hotplug_off(mpidr);
 
@@ -343,6 +424,26 @@
 		trace_power_flow(mpidr, CLUSTER_DOWN);
 	}
 }
+#else
+static void plat_power_domain_off(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
+
+	/* Prevent interrupts from spuriously waking up this cpu */
+	gicv2_cpuif_disable();
+
+	spm_hotplug_off(mpidr);
+
+	trace_power_flow(mpidr, CPU_DOWN);
+
+	if (MTK_CLUSTER_PWR_STATE(state) == MTK_LOCAL_STATE_OFF) {
+		/* Disable coherency if this cluster is to be turned off */
+		plat_cci_disable();
+
+		trace_power_flow(mpidr, CLUSTER_DOWN);
+	}
+}
+#endif
 
 /*******************************************************************************
  * MTK_platform handler called when an affinity instance is about to be
@@ -356,6 +457,7 @@
  * dealt with. So do not write & read global variables across calls. It will be
  * wise to do flush a write to the global to prevent unpredictable results.
  ******************************************************************************/
+#if ENABLE_PLAT_COMPAT
 static void plat_affinst_suspend(unsigned long sec_entrypoint,
 			  unsigned int afflvl,
 			  unsigned int state)
@@ -396,9 +498,50 @@
 		generic_timer_backup();
 		spm_system_suspend();
 		/* Prevent interrupts from spuriously waking up this cpu */
-		arm_gic_cpuif_deactivate();
+		gicv2_cpuif_disable();
 	}
 }
+#else
+static void plat_power_domain_suspend(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
+	unsigned long cluster_id;
+	unsigned long cpu_id;
+	uintptr_t rv;
+
+	cpu_id = mpidr & MPIDR_CPU_MASK;
+	cluster_id = mpidr & MPIDR_CLUSTER_MASK;
+
+	if (cluster_id)
+		rv = (uintptr_t)&mt8173_mcucfg->mp1_rv_addr[cpu_id].rv_addr_lw;
+	else
+		rv = (uintptr_t)&mt8173_mcucfg->mp0_rv_addr[cpu_id].rv_addr_lw;
+
+	mmio_write_32(rv, secure_entrypoint);
+
+	if (MTK_SYSTEM_PWR_STATE(state) != MTK_LOCAL_STATE_OFF) {
+		spm_mcdi_prepare_for_off_state(mpidr, MTK_PWR_LVL0);
+		if (MTK_CLUSTER_PWR_STATE(state) == MTK_LOCAL_STATE_OFF)
+			spm_mcdi_prepare_for_off_state(mpidr, MTK_PWR_LVL1);
+	}
+
+	mt_platform_save_context(mpidr);
+
+	/* Perform the common cluster specific operations */
+	if (MTK_CLUSTER_PWR_STATE(state) == MTK_LOCAL_STATE_OFF) {
+		/* Disable coherency if this cluster is to be turned off */
+		plat_cci_disable();
+	}
+
+	if (MTK_SYSTEM_PWR_STATE(state) == MTK_LOCAL_STATE_OFF) {
+		disable_scu(mpidr);
+		generic_timer_backup();
+		spm_system_suspend();
+		/* Prevent interrupts from spuriously waking up this cpu */
+		gicv2_cpuif_disable();
+	}
+}
+#endif
 
 /*******************************************************************************
  * MTK_platform handler called when an affinity instance has just been powered
@@ -407,6 +550,7 @@
  * was turned off prior to wakeup and do what's necessary to setup it up
  * correctly.
  ******************************************************************************/
+#if ENABLE_PLAT_COMPAT
 static void plat_affinst_on_finish(unsigned int afflvl, unsigned int state)
 {
 	unsigned long mpidr = read_mpidr_el1();
@@ -423,16 +567,45 @@
 	}
 
 	/* Enable the gic cpu interface */
-	arm_gic_cpuif_setup();
-	arm_gic_pcpu_distif_setup();
+	gicv2_cpuif_enable();
+	gicv2_pcpu_distif_init();
 	trace_power_flow(mpidr, CPU_UP);
 }
+#else
+void mtk_system_pwr_domain_resume(void);
+
+static void plat_power_domain_on_finish(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
+
+	assert(state->pwr_domain_state[MPIDR_AFFLVL0] == MTK_LOCAL_STATE_OFF);
+
+	if ((PLAT_MAX_PWR_LVL > MTK_PWR_LVL1) &&
+		(state->pwr_domain_state[MTK_PWR_LVL2] == MTK_LOCAL_STATE_OFF))
+		mtk_system_pwr_domain_resume();
+
+	if (state->pwr_domain_state[MPIDR_AFFLVL1] == MTK_LOCAL_STATE_OFF) {
+		plat_cci_enable();
+		trace_power_flow(mpidr, CLUSTER_UP);
+	}
+
+	if ((PLAT_MAX_PWR_LVL > MTK_PWR_LVL1) &&
+		(state->pwr_domain_state[MTK_PWR_LVL2] == MTK_LOCAL_STATE_OFF))
+		return;
+
+	/* Enable the gic cpu interface */
+	gicv2_cpuif_enable();
+	gicv2_pcpu_distif_init();
+	trace_power_flow(mpidr, CPU_UP);
+}
+#endif
 
 /*******************************************************************************
  * MTK_platform handler called when an affinity instance has just been powered
  * on after having been suspended earlier. The level and mpidr determine the
  * affinity instance.
  ******************************************************************************/
+#if ENABLE_PLAT_COMPAT
 static void plat_affinst_suspend_finish(unsigned int afflvl, unsigned int state)
 {
 	unsigned long mpidr = read_mpidr_el1();
@@ -443,8 +616,7 @@
 
 	if (afflvl >= MPIDR_AFFLVL2) {
 		/* Enable the gic cpu interface */
-		arm_gic_setup();
-		arm_gic_cpuif_setup();
+		plat_arm_gic_init();
 		spm_system_suspend_finish();
 		enable_scu(mpidr);
 	}
@@ -461,14 +633,56 @@
 	if (afflvl < MPIDR_AFFLVL2)
 		spm_mcdi_finish_for_on_state(mpidr, afflvl);
 
-	arm_gic_pcpu_distif_setup();
+	gicv2_pcpu_distif_init();
 }
+#else
+static void plat_power_domain_suspend_finish(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
 
+	if (state->pwr_domain_state[MTK_PWR_LVL0] == MTK_LOCAL_STATE_RET)
+		return;
+
+	if (MTK_SYSTEM_PWR_STATE(state) == MTK_LOCAL_STATE_OFF) {
+		/* Enable the gic cpu interface */
+		plat_arm_gic_init();
+		spm_system_suspend_finish();
+		enable_scu(mpidr);
+	}
+
+	/* Perform the common cluster specific operations */
+	if (MTK_CLUSTER_PWR_STATE(state) == MTK_LOCAL_STATE_OFF) {
+		/* Enable coherency if this cluster was off */
+		plat_cci_enable();
+	}
+
+	mt_platform_restore_context(mpidr);
+
+	if (MTK_SYSTEM_PWR_STATE(state) != MTK_LOCAL_STATE_OFF) {
+		spm_mcdi_finish_for_on_state(mpidr, MTK_PWR_LVL0);
+		if (MTK_CLUSTER_PWR_STATE(state) == MTK_LOCAL_STATE_OFF)
+			spm_mcdi_finish_for_on_state(mpidr, MTK_PWR_LVL1);
+	}
+
+	gicv2_pcpu_distif_init();
+}
+#endif
+
+#if ENABLE_PLAT_COMPAT
 static unsigned int plat_get_sys_suspend_power_state(void)
 {
 	/* StateID: 0, StateType: 1(power down), PowerLevel: 2(system) */
 	return psci_make_powerstate(0, 1, 2);
 }
+#else
+static void plat_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	assert(PLAT_MAX_PWR_LVL >= 2);
+
+	for (int i = MPIDR_AFFLVL0; i <= PLAT_MAX_PWR_LVL; i++)
+		req_state->pwr_domain_state[i] = MTK_LOCAL_STATE_OFF;
+}
+#endif
 
 /*******************************************************************************
  * MTK handlers to shutdown/reboot the system
@@ -500,6 +714,94 @@
 	panic();
 }
 
+#if !ENABLE_PLAT_COMPAT
+#if !PSCI_EXTENDED_STATE_ID
+static int plat_validate_power_state(unsigned int power_state,
+					psci_power_state_t *req_state)
+{
+	int pstate = psci_get_pstate_type(power_state);
+	int pwr_lvl = psci_get_pstate_pwrlvl(power_state);
+	int i;
+
+	assert(req_state);
+
+	if (pwr_lvl > PLAT_MAX_PWR_LVL)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Sanity check the requested state */
+	if (pstate == PSTATE_TYPE_STANDBY) {
+		/*
+		 * It's possible to enter standby only on power level 0
+		 * Ignore any other power level.
+		 */
+		if (pwr_lvl != 0)
+			return PSCI_E_INVALID_PARAMS;
+
+		req_state->pwr_domain_state[MTK_PWR_LVL0] =
+					MTK_LOCAL_STATE_RET;
+	} else {
+		for (i = 0; i <= pwr_lvl; i++)
+			req_state->pwr_domain_state[i] =
+					MTK_LOCAL_STATE_OFF;
+	}
+
+	/*
+	 * We expect the 'state id' to be zero.
+	 */
+	if (psci_get_pstate_id(power_state))
+		return PSCI_E_INVALID_PARAMS;
+
+	return PSCI_E_SUCCESS;
+}
+#else
+int plat_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; !!mtk_pm_idle_states[i]; i++) {
+		if (power_state == mtk_pm_idle_states[i])
+			break;
+	}
+
+	/* Return error if entry not found in the idle state array */
+	if (!mtk_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 &
+						MTK_LOCAL_PSTATE_MASK;
+		state_id >>= MTK_LOCAL_PSTATE_WIDTH;
+	}
+
+	return PSCI_E_SUCCESS;
+}
+#endif
+
+void mtk_system_pwr_domain_resume(void)
+{
+	console_init(MT8173_UART0_BASE, MT8173_UART_CLOCK, MT8173_BAUDRATE);
+
+	/* Assert system power domain is available on the platform */
+	assert(PLAT_MAX_PWR_LVL >= MTK_PWR_LVL2);
+
+	plat_arm_gic_init();
+}
+#endif
+
+#if ENABLE_PLAT_COMPAT
 /*******************************************************************************
  * Export the platform handlers to enable psci to invoke them
  ******************************************************************************/
@@ -524,3 +826,54 @@
 	*plat_ops = &plat_plat_pm_ops;
 	return 0;
 }
+#else
+static const plat_psci_ops_t plat_plat_pm_ops = {
+	.cpu_standby			= plat_cpu_standby,
+	.pwr_domain_on			= plat_power_domain_on,
+	.pwr_domain_on_finish		= plat_power_domain_on_finish,
+	.pwr_domain_off			= plat_power_domain_off,
+	.pwr_domain_suspend		= plat_power_domain_suspend,
+	.pwr_domain_suspend_finish	= plat_power_domain_suspend_finish,
+	.system_off			= plat_system_off,
+	.system_reset			= plat_system_reset,
+	.validate_power_state		= plat_validate_power_state,
+	.get_sys_suspend_power_state	= plat_get_sys_suspend_power_state,
+};
+
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const plat_psci_ops_t **psci_ops)
+{
+	*psci_ops = &plat_plat_pm_ops;
+	secure_entrypoint = sec_entrypoint;
+	return 0;
+}
+
+/*
+ * The PSCI generic code uses this API to let the platform participate in state
+ * coordination during a power management operation. It compares the platform
+ * specific local power states requested by each cpu for a given power domain
+ * and returns the coordinated target power state that the domain should
+ * enter. A platform assigns a number to a local power state. This default
+ * implementation assumes that the platform assigns these numbers in order of
+ * increasing depth of the power state i.e. for two power states X & Y, if X < Y
+ * then X represents a shallower power state than Y. As a result, the
+ * coordinated target local power state for a power domain will be the minimum
+ * of the requested local power states.
+ */
+plat_local_state_t plat_get_target_pwr_state(unsigned int lvl,
+					     const plat_local_state_t *states,
+					     unsigned int ncpu)
+{
+	plat_local_state_t target = PLAT_MAX_OFF_STATE, temp;
+
+	assert(ncpu);
+
+	do {
+		temp = *states++;
+		if (temp < target)
+			target = temp;
+	} while (--ncpu);
+
+	return target;
+}
+#endif
diff --git a/plat/mediatek/mt8173/plat_topology.c b/plat/mediatek/mt8173/plat_topology.c
index 96598b6..caab8e4 100644
--- a/plat/mediatek/mt8173/plat_topology.c
+++ b/plat/mediatek/mt8173/plat_topology.c
@@ -31,6 +31,7 @@
 #include <platform_def.h>
 #include <psci.h>
 
+#if ENABLE_PLAT_COMPAT
 unsigned int plat_get_aff_count(unsigned int aff_lvl, unsigned long mpidr)
 {
 	/* Report 1 (absent) instance at levels higher that the cluster level */
@@ -54,3 +55,55 @@
 	/* [TODO] Make topology configurable via SCC */
 	return 0;
 }
+#else
+
+const unsigned char mtk_power_domain_tree_desc[] = {
+	/* No of root nodes */
+	PLATFORM_SYSTEM_COUNT,
+	/* No of children for the root node */
+	PLATFORM_CLUSTER_COUNT,
+	/* No of children for the first cluster node */
+	PLATFORM_CLUSTER0_CORE_COUNT,
+	/* No of children for the second cluster node */
+	PLATFORM_CLUSTER1_CORE_COUNT
+};
+
+/*******************************************************************************
+ * This function returns the MT8173 default topology tree information.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return mtk_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;
+
+	/*
+	 * Validate cpu_id by checking whether it represents a CPU in
+	 * one of the two clusters present on the platform.
+	 */
+	if (cpu_id >= PLATFORM_MAX_CPUS_PER_CLUSTER)
+		return -1;
+
+	return (cpu_id + (cluster_id * 4));
+}
+#endif
diff --git a/plat/mediatek/mt8173/platform.mk b/plat/mediatek/mt8173/platform.mk
index 54ca475..e59125a 100644
--- a/plat/mediatek/mt8173/platform.mk
+++ b/plat/mediatek/mt8173/platform.mk
@@ -33,6 +33,8 @@
 
 PLAT_INCLUDES		:=	-I${MTK_PLAT}/common/				\
 				-I${MTK_PLAT}/common/drivers/uart/		\
+				-Iinclude/plat/arm/common			\
+				-Iinclude/plat/arm/common/aarch64		\
 				-I${MTK_PLAT_SOC}/drivers/crypt/		\
 				-I${MTK_PLAT_SOC}/drivers/mtcmos/		\
 				-I${MTK_PLAT_SOC}/drivers/pmic/			\
@@ -44,12 +46,13 @@
 PLAT_BL_COMMON_SOURCES	:=	lib/xlat_tables/xlat_tables_common.c		\
 				lib/xlat_tables/aarch64/xlat_tables.c		\
 				plat/common/aarch64/plat_common.c		\
-				plat/common/plat_gic.c
+				plat/arm/common/arm_gicv2.c			\
+				plat/common/plat_gicv2.c
 
 BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
-				drivers/arm/gic/arm_gic.c			\
-				drivers/arm/gic/gic_v2.c			\
-				drivers/arm/gic/gic_v3.c			\
+				drivers/arm/gic/common/gic_common.c		\
+				drivers/arm/gic/v2/gicv2_main.c			\
+				drivers/arm/gic/v2/gicv2_helpers.c		\
 				drivers/console/aarch64/console.S		\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
@@ -73,7 +76,6 @@
 				${MTK_PLAT_SOC}/drivers/spm/spm_mcdi.c		\
 				${MTK_PLAT_SOC}/drivers/spm/spm_suspend.c	\
 				${MTK_PLAT_SOC}/drivers/timer/mt_cpuxgpt.c	\
-				${MTK_PLAT_SOC}/plat_mt_gic.c			\
 				${MTK_PLAT_SOC}/plat_pm.c			\
 				${MTK_PLAT_SOC}/plat_sip_calls.c		\
 				${MTK_PLAT_SOC}/plat_topology.c			\
diff --git a/services/spd/trusty/smcall.h b/services/spd/trusty/smcall.h
index 7e876c8..a1d91e5 100644
--- a/services/spd/trusty/smcall.h
+++ b/services/spd/trusty/smcall.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2017, 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:
@@ -40,11 +40,12 @@
 #define SMC_ENTITY(smc_nr)	(((smc_nr) & 0x3F000000) >> 24)
 #define SMC_FUNCTION(smc_nr)	((smc_nr) & 0x0000FFFF)
 
-#define SMC_NR(entity, fn, fastcall, smc64) ((((fastcall) & 0x1) << 31) | \
-					     (((smc64) & 0x1) << 30) | \
-					     (((entity) & 0x3F) << 24) | \
-					     ((fn) & 0xFFFF) \
-					    )
+#define SMC_NR(entity, fn, fastcall, smc64)			\
+		(((((unsigned int) (fastcall)) & 0x1) << 31) |	\
+		(((smc64) & 0x1) << 30) |			\
+		(((entity) & 0x3F) << 24) |			\
+		((fn) & 0xFFFF)					\
+		)
 
 #define SMC_FASTCALL_NR(entity, fn)	SMC_NR((entity), (fn), 1, 0)
 #define SMC_STDCALL_NR(entity, fn)	SMC_NR((entity), (fn), 0, 0)
diff --git a/tools/fiptool/fiptool.c b/tools/fiptool/fiptool.c
index 6145e26..865aeae 100644
--- a/tools/fiptool/fiptool.c
+++ b/tools/fiptool/fiptool.c
@@ -50,6 +50,7 @@
 
 #define OPT_TOC_ENTRY 0
 #define OPT_PLAT_TOC_FLAGS 1
+#define OPT_ALIGN 2
 
 static image_desc_t *lookup_image_desc_from_uuid(const uuid_t *uuid);
 static image_t *lookup_image_from_uuid(const uuid_t *uuid);
@@ -160,6 +161,12 @@
 	return memset(xmalloc(size, msg), 0, size);
 }
 
+static void xfwrite(void *buf, size_t size, FILE *fp, const char *filename)
+{
+	if (fwrite(buf, 1, size, fp) != size)
+		log_errx("Failed to write %s", filename);
+}
+
 static image_desc_t *new_image_desc(const uuid_t *uuid,
     const char *name, const char *cmdline_name)
 {
@@ -202,11 +209,10 @@
 {
 	image_desc_t **p = &image_desc_head;
 
-	assert(lookup_image_desc_from_uuid(&desc->uuid) == NULL);
-
 	while (*p)
 		p = &(*p)->next;
 
+	assert(*p == NULL);
 	*p = desc;
 	nr_image_descs++;
 }
@@ -244,16 +250,32 @@
 {
 	image_t **p = &image_head;
 
-	assert(lookup_image_from_uuid(&image->uuid) == NULL);
-
 	while (*p)
 		p = &(*p)->next;
 
+	assert(*p == NULL);
 	*p = image;
 
 	nr_images++;
 }
 
+static void replace_image(image_t *image)
+{
+	image_t **p = &image_head;
+
+	while (*p) {
+		if (!memcmp(&(*p)->toc_e.uuid, &image->toc_e.uuid,
+			    sizeof(image->toc_e.uuid)))
+			break;
+		p = &(*p)->next;
+	}
+
+	assert(*p != NULL);
+
+	image->next = (*p)->next;
+	*p = image;
+}
+
 static void free_image(image_t *image)
 {
 	free(image->buffer);
@@ -262,20 +284,20 @@
 
 static void remove_image(image_t *image)
 {
-	image_t *tmp = image_head, *prev;
+	image_t *tmp, **p = &image_head;
 
-	if (tmp == image) {
-		image_head = tmp->next;
-		free_image(tmp);
-	} else {
-		while (tmp != NULL && tmp != image) {
-			prev = tmp;
-			tmp = tmp->next;
-		}
-		assert(tmp != NULL);
-		prev->next = tmp->next;
-		free_image(tmp);
+	while (*p) {
+		if (*p == image)
+			break;
+		p = &(*p)->next;
 	}
+
+	assert(*p != NULL);
+
+	tmp = *p;
+	*p = tmp->next;
+	free_image(tmp);
+
 	nr_images--;
 }
 
@@ -317,7 +339,7 @@
 	image_t *image;
 
 	for (image = image_head; image != NULL; image = image->next)
-		if (memcmp(&image->uuid, uuid, sizeof(uuid_t)) == 0)
+		if (!memcmp(&image->toc_e.uuid, uuid, sizeof(*uuid)))
 			return image;
 	return NULL;
 }
@@ -410,7 +432,7 @@
 		 */
 		image = xzalloc(sizeof(*image),
 		    "failed to allocate memory for image");
-		memcpy(&image->uuid, &toc_entry->uuid, sizeof(uuid_t));
+		image->toc_e = *toc_entry;
 		image->buffer = xmalloc(toc_entry->size,
 		    "failed to allocate image buffer, is FIP file corrupted?");
 		/* Overflow checks before memory copy. */
@@ -421,17 +443,16 @@
 
 		memcpy(image->buffer, buf + toc_entry->offset_address,
 		    toc_entry->size);
-		image->size = toc_entry->size;
 
 		/* If this is an unknown image, create a descriptor for it. */
-		desc = lookup_image_desc_from_uuid(&image->uuid);
+		desc = lookup_image_desc_from_uuid(&toc_entry->uuid);
 		if (desc == NULL) {
 			char name[_UUID_STR_LEN + 1], filename[PATH_MAX];
 
-			uuid_to_str(name, sizeof(name), &image->uuid);
+			uuid_to_str(name, sizeof(name), &toc_entry->uuid);
 			snprintf(filename, sizeof(filename), "%s%s",
 			    name, ".bin");
-			desc = new_image_desc(&image->uuid, name, "blob");
+			desc = new_image_desc(&toc_entry->uuid, name, "blob");
 			desc->action = DO_UNPACK;
 			desc->action_arg = xstrdup(filename,
 			    "failed to allocate memory for blob filename");
@@ -466,11 +487,11 @@
 		log_errx("fstat %s", filename);
 
 	image = xzalloc(sizeof(*image), "failed to allocate memory for image");
-	memcpy(&image->uuid, uuid, sizeof(uuid_t));
+	image->toc_e.uuid = *uuid;
 	image->buffer = xmalloc(st.st_size, "failed to allocate image buffer");
 	if (fread(image->buffer, 1, st.st_size, fp) != st.st_size)
 		log_errx("Failed to read %s", filename);
-	image->size = st.st_size;
+	image->toc_e.size = st.st_size;
 
 	fclose(fp);
 	return image;
@@ -483,8 +504,7 @@
 	fp = fopen(filename, "w");
 	if (fp == NULL)
 		log_err("fopen");
-	if (fwrite(image->buffer, 1, image->size, fp) != image->size)
-		log_errx("Failed to write %s", filename);
+	xfwrite(image->buffer, image->toc_e.size, fp, filename);
 	fclose(fp);
 	return 0;
 }
@@ -525,8 +545,6 @@
 static int info_cmd(int argc, char *argv[])
 {
 	image_t *image;
-	uint64_t image_offset;
-	uint64_t image_size;
 	fip_toc_header_t toc_header;
 
 	if (argc != 2)
@@ -544,31 +562,24 @@
 		    (unsigned long long)toc_header.flags);
 	}
 
-	image_offset = sizeof(fip_toc_header_t) +
-	    (sizeof(fip_toc_entry_t) * (nr_images + 1));
-
 	for (image = image_head; image != NULL; image = image->next) {
 		image_desc_t *desc;
 
-		desc = lookup_image_desc_from_uuid(&image->uuid);
+		desc = lookup_image_desc_from_uuid(&image->toc_e.uuid);
 		assert(desc != NULL);
-		printf("%s: ", desc->name);
-		image_size = image->size;
-		printf("offset=0x%llX, size=0x%llX",
-		    (unsigned long long)image_offset,
-		    (unsigned long long)image_size);
-		if (desc != NULL)
-			printf(", cmdline=\"--%s\"",
-			    desc->cmdline_name);
+		printf("%s: offset=0x%llX, size=0x%llX, cmdline=\"--%s\"",
+		       desc->name,
+		       (unsigned long long)image->toc_e.offset_address,
+		       (unsigned long long)image->toc_e.size,
+		       desc->cmdline_name);
 		if (verbose) {
 			unsigned char md[SHA256_DIGEST_LENGTH];
 
-			SHA256(image->buffer, image_size, md);
+			SHA256(image->buffer, image->toc_e.size, md);
 			printf(", sha256=");
 			md_print(md, sizeof(md));
 		}
 		putchar('\n');
-		image_offset += image_size;
 	}
 
 	free_images();
@@ -581,19 +592,14 @@
 	exit(1);
 }
 
-static int pack_images(const char *filename, uint64_t toc_flags)
+static int pack_images(const char *filename, uint64_t toc_flags, unsigned long align)
 {
 	FILE *fp;
 	image_t *image;
 	fip_toc_header_t *toc_header;
 	fip_toc_entry_t *toc_entry;
 	char *buf;
-	uint64_t entry_offset, buf_size, payload_size;
-
-	/* Calculate total payload size and allocate scratch buffer. */
-	payload_size = 0;
-	for (image = image_head; image != NULL; image = image->next)
-		payload_size += image->size;
+	uint64_t entry_offset, buf_size, payload_size = 0;
 
 	buf_size = sizeof(fip_toc_header_t) +
 	    sizeof(fip_toc_entry_t) * (nr_images + 1);
@@ -611,19 +617,16 @@
 
 	entry_offset = buf_size;
 	for (image = image_head; image != NULL; image = image->next) {
-		memcpy(&toc_entry->uuid, &image->uuid, sizeof(uuid_t));
-		toc_entry->offset_address = entry_offset;
-		toc_entry->size = image->size;
-		toc_entry->flags = 0;
-		entry_offset += toc_entry->size;
-		toc_entry++;
+		payload_size += image->toc_e.size;
+		entry_offset = (entry_offset + align - 1) & ~(align - 1);
+		image->toc_e.offset_address = entry_offset;
+		*toc_entry++ = image->toc_e;
+		entry_offset += image->toc_e.size;
 	}
 
 	/* Append a null uuid entry to mark the end of ToC entries. */
-	memcpy(&toc_entry->uuid, &uuid_null, sizeof(uuid_t));
+	memset(toc_entry, 0, sizeof(*toc_entry));
 	toc_entry->offset_address = entry_offset;
-	toc_entry->size = 0;
-	toc_entry->flags = 0;
 
 	/* Generate the FIP file. */
 	fp = fopen(filename, "w");
@@ -633,16 +636,18 @@
 	if (verbose)
 		log_dbgx("Metadata size: %zu bytes", buf_size);
 
-	if (fwrite(buf, 1, buf_size, fp) != buf_size)
-		log_errx("Failed to write image to %s", filename);
+	xfwrite(buf, buf_size, fp, filename);
 	free(buf);
 
 	if (verbose)
 		log_dbgx("Payload size: %zu bytes", payload_size);
 
-	for (image = image_head; image != NULL; image = image->next)
-		if (fwrite(image->buffer, 1, image->size, fp) != image->size)
-			log_errx("Failed to write image to %s", filename);
+	for (image = image_head; image != NULL; image = image->next) {
+		if (fseek(fp, image->toc_e.offset_address, SEEK_SET))
+			log_errx("Failed to set file position");
+
+		xfwrite(image->buffer, image->toc_e.size, fp, filename);
+	}
 
 	fclose(fp);
 	return 0;
@@ -675,8 +680,7 @@
 				    desc->cmdline_name,
 				    desc->action_arg);
 			}
-			remove_image(old_image);
-			add_image(new_image);
+			replace_image(new_image);
 		} else {
 			if (verbose)
 				log_dbgx("Adding image %s",
@@ -699,6 +703,24 @@
 	*toc_flags |= flags << 32;
 }
 
+static int is_power_of_2(unsigned long x)
+{
+	return x && !(x & (x - 1));
+}
+
+static unsigned long get_image_align(char *arg)
+{
+	char *endptr;
+	unsigned long align;
+
+	errno = 0;
+	align = strtoul(arg, &endptr, 10);
+	if (*endptr != '\0' || !is_power_of_2(align) || errno != 0)
+		log_errx("Invalid alignment: %s", arg);
+
+	return align;
+}
+
 static void parse_blob_opt(char *arg, uuid_t *uuid, char *filename, size_t len)
 {
 	char *p;
@@ -719,6 +741,7 @@
 	struct option *opts = NULL;
 	size_t nr_opts = 0;
 	unsigned long long toc_flags = 0;
+	unsigned long align = 1;
 
 	if (argc < 2)
 		create_usage();
@@ -726,6 +749,7 @@
 	opts = fill_common_opts(opts, &nr_opts, required_argument);
 	opts = add_opt(opts, &nr_opts, "plat-toc-flags", required_argument,
 	    OPT_PLAT_TOC_FLAGS);
+	opts = add_opt(opts, &nr_opts, "align", required_argument, OPT_ALIGN);
 	opts = add_opt(opts, &nr_opts, "blob", required_argument, 'b');
 	opts = add_opt(opts, &nr_opts, NULL, 0, 0);
 
@@ -747,6 +771,9 @@
 		case OPT_PLAT_TOC_FLAGS:
 			parse_plat_toc_flags(optarg, &toc_flags);
 			break;
+		case OPT_ALIGN:
+			align = get_image_align(optarg);
+			break;
 		case 'b': {
 			char name[_UUID_STR_LEN + 1];
 			char filename[PATH_MAX] = { 0 };
@@ -782,7 +809,7 @@
 
 	update_fip();
 
-	pack_images(argv[0], toc_flags);
+	pack_images(argv[0], toc_flags, align);
 	free_images();
 	return 0;
 }
@@ -791,8 +818,10 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool create [--blob uuid=...,file=...] "
-	    "[--plat-toc-flags <value>] [opts] FIP_FILENAME\n");
+	printf("fiptool create [opts] FIP_FILENAME\n");
+	printf("\n");
+	printf("Options:\n");
+	printf("  --align <value>\t\tEach image is aligned to <value> (default: 1).\n");
 	printf("  --blob uuid=...,file=...\tAdd an image with the given UUID "
 	    "pointed to by file.\n");
 	printf("  --plat-toc-flags <value>\t16-bit platform specific flag field "
@@ -812,12 +841,14 @@
 	char outfile[PATH_MAX] = { 0 };
 	fip_toc_header_t toc_header = { 0 };
 	unsigned long long toc_flags = 0;
+	unsigned long align = 1;
 	int pflag = 0;
 
 	if (argc < 2)
 		update_usage();
 
 	opts = fill_common_opts(opts, &nr_opts, required_argument);
+	opts = add_opt(opts, &nr_opts, "align", required_argument, OPT_ALIGN);
 	opts = add_opt(opts, &nr_opts, "blob", required_argument, 'b');
 	opts = add_opt(opts, &nr_opts, "out", required_argument, 'o');
 	opts = add_opt(opts, &nr_opts, "plat-toc-flags", required_argument,
@@ -865,6 +896,9 @@
 			set_image_desc_action(desc, DO_PACK, filename);
 			break;
 		}
+		case OPT_ALIGN:
+			align = get_image_align(optarg);
+			break;
 		case 'o':
 			snprintf(outfile, sizeof(outfile), "%s", optarg);
 			break;
@@ -882,7 +916,7 @@
 	if (outfile[0] == '\0')
 		snprintf(outfile, sizeof(outfile), "%s", argv[0]);
 
-	if (access(outfile, F_OK) == 0)
+	if (access(argv[0], F_OK) == 0)
 		parse_fip(argv[0], &toc_header);
 
 	if (pflag)
@@ -891,7 +925,7 @@
 
 	update_fip();
 
-	pack_images(outfile, toc_flags);
+	pack_images(outfile, toc_flags, align);
 	free_images();
 	return 0;
 }
@@ -900,8 +934,10 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool update [--blob uuid=...,file=...] [--out FIP_FILENAME] "
-	    "[--plat-toc-flags <value>] [opts] FIP_FILENAME\n");
+	printf("fiptool update [opts] FIP_FILENAME\n");
+	printf("\n");
+	printf("Options:\n");
+	printf("  --align <value>\t\tEach image is aligned to <value> (default: 1).\n");
 	printf("  --blob uuid=...,file=...\tAdd or update an image "
 	    "with the given UUID pointed to by file.\n");
 	printf("  --out FIP_FILENAME\t\tSet an alternative output FIP file.\n");
@@ -1037,8 +1073,9 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool unpack [--blob uuid=...,file=...] [--force] "
-	    "[--out <path>] [opts] FIP_FILENAME\n");
+	printf("fiptool unpack [opts] FIP_FILENAME\n");
+	printf("\n");
+	printf("Options:\n");
 	printf("  --blob uuid=...,file=...\tUnpack an image with the given UUID "
 	    "to file.\n");
 	printf("  --force\t\t\tIf the output file already exists, use --force to "
@@ -1061,12 +1098,14 @@
 	char outfile[PATH_MAX] = { 0 };
 	fip_toc_header_t toc_header;
 	image_desc_t *desc;
+	unsigned long align = 1;
 	int fflag = 0;
 
 	if (argc < 2)
 		remove_usage();
 
 	opts = fill_common_opts(opts, &nr_opts, no_argument);
+	opts = add_opt(opts, &nr_opts, "align", required_argument, OPT_ALIGN);
 	opts = add_opt(opts, &nr_opts, "blob", required_argument, 'b');
 	opts = add_opt(opts, &nr_opts, "force", no_argument, 'f');
 	opts = add_opt(opts, &nr_opts, "out", required_argument, 'o');
@@ -1087,6 +1126,9 @@
 			set_image_desc_action(desc, DO_REMOVE, NULL);
 			break;
 		}
+		case OPT_ALIGN:
+			align = get_image_align(optarg);
+			break;
 		case 'b': {
 			char name[_UUID_STR_LEN + 1], filename[PATH_MAX];
 			uuid_t uuid = { 0 };
@@ -1151,7 +1193,7 @@
 		}
 	}
 
-	pack_images(outfile, toc_header.flags);
+	pack_images(outfile, toc_header.flags, align);
 	free_images();
 	return 0;
 }
@@ -1160,8 +1202,10 @@
 {
 	toc_entry_t *toc_entry = toc_entries;
 
-	printf("fiptool remove [--blob uuid=...] [--force] "
-	    "[--out FIP_FILENAME] [opts] FIP_FILENAME\n");
+	printf("fiptool remove [opts] FIP_FILENAME\n");
+	printf("\n");
+	printf("Options:\n");
+	printf("  --align <value>\tEach image is aligned to <value> (default: 1).\n");
 	printf("  --blob uuid=...\tRemove an image with the given UUID.\n");
 	printf("  --force\t\tIf the output FIP file already exists, use --force to "
 	    "overwrite it.\n");
@@ -1211,7 +1255,7 @@
 
 static void usage(void)
 {
-	printf("usage: [--verbose] fiptool <command> [<args>]\n");
+	printf("usage: fiptool [--verbose] <command> [<args>]\n");
 	printf("Global options supported:\n");
 	printf("  --verbose\tEnable verbose output for all commands.\n");
 	fputc('\n', stderr);
diff --git a/tools/fiptool/fiptool.h b/tools/fiptool/fiptool.h
index 6ace400..be0c6f0 100644
--- a/tools/fiptool/fiptool.h
+++ b/tools/fiptool/fiptool.h
@@ -34,6 +34,7 @@
 #include <stddef.h>
 #include <stdint.h>
 
+#include "firmware_image_package.h"
 #include "uuid.h"
 
 #define NELEM(x) (sizeof (x) / sizeof *(x))
@@ -61,10 +62,9 @@
 } image_desc_t;
 
 typedef struct image {
-	uuid_t             uuid;
-	size_t             size;
-	void              *buffer;
-	struct image      *next;
+	struct fip_toc_entry toc_e;
+	void                *buffer;
+	struct image        *next;
 } image_t;
 
 typedef struct cmd {