diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index bdbdbc2..56ddbb1 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -72,10 +72,20 @@
 
 2.1 Common mandatory modifications
 ----------------------------------
-A platform port must enable the Memory Management Unit (MMU) with identity
-mapped page tables, and enable both the instruction and data caches for each BL
-stage. In ARM standard platforms, each BL stage configures the MMU in
-the platform-specific architecture setup function, `blX_plat_arch_setup()`.
+
+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_helpers.c`
+and `lib/aarch64/xlat_tables.c`) 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 use an identity mapping.
+
+In ARM standard platforms, each BL stage configures the MMU in the
+platform-specific architecture setup function, `blX_plat_arch_setup()`, and uses
+an identity mapping for all addresses.
 
 If the build option `USE_COHERENT_MEM` is enabled, each platform can allocate a
 block of identity mapped secure memory with Device-nGnRE attributes aligned to
@@ -1860,7 +1870,7 @@
     Return   : int
 
 This API is used by the crash reporting mechanism to initialize the crash
-console. It should only use the general purpose registers x0 to x2 to do the
+console. It must only use the general purpose registers x0 to x4 to do the
 initialization and returns 1 on success.
 
 ### Function : plat_crash_console_putc
@@ -1869,7 +1879,7 @@
     Return   : int
 
 This API is used by the crash reporting mechanism to print a character on the
-designated crash console. It should only use general purpose registers x1 and
+designated crash console. It must only use general purpose registers x1 and
 x2 to do its work. The parameter and the return value are in general purpose
 register x0.
 
diff --git a/docs/user-guide.md b/docs/user-guide.md
index 894d69b..e692913 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -859,6 +859,84 @@
 *   MMU disabled;
 *   Caches disabled.
 
+### Booting an EL3 payload
+
+The EL3 payload image is a standalone image and is not part of the FIP. It is
+not loaded by the Trusted Firmware. Therefore, there are 2 possible scenarios:
+
+*   The EL3 payload may reside in non-volatile memory (NVM) and execute in
+    place. In this case, booting it is just a matter of specifying the right
+    address in NVM through `EL3_PAYLOAD_BASE` when building the TF.
+
+*   The EL3 payload needs to be loaded in volatile memory (e.g. DRAM) at
+    run-time.
+
+To help in the latter scenario, the `SPIN_ON_BL1_EXIT=1` build option can be
+used. The infinite loop that it introduces in BL1 stops execution at the right
+moment for a debugger to take control of the target and load the payload (for
+example, over JTAG).
+
+It is expected that this loading method will work in most cases, as a debugger
+connection is usually available in a pre-production system. The user is free to
+use any other platform-specific mechanism to load the EL3 payload, though.
+
+#### Booting an EL3 payload on FVP
+
+The EL3 payloads boot flow requires the CPU's mailbox to be cleared at reset for
+the secondary CPUs holding pen to work properly. Unfortunately, its reset value
+is undefined on the FVP platform and the FVP platform code doesn't clear it.
+Therefore, one must modify the way the model is normally invoked in order to
+clear the mailbox at start-up.
+
+One way to do that is to create an 8-byte file containing all zero bytes using
+the following command:
+
+    dd if=/dev/zero of=mailbox.dat bs=1 count=8
+
+and pre-load it into the FVP memory at the mailbox address (i.e. `0x04000000`)
+using the following model parameters:
+
+    --data cluster0.cpu0=mailbox.dat@0x04000000   [Base FVPs]
+    --data=mailbox.dat@0x04000000                 [Foundation FVP]
+
+To provide the model with the EL3 payload image, the following methods may be
+used:
+
+1.  If the EL3 payload is able to execute in place, it may be programmed into
+    flash memory. On Base Cortex and AEM FVPs, the following model parameter
+    loads it at the base address of the NOR FLASH1 (the NOR FLASH0 is already
+    used for the FIP):
+
+        -C bp.flashloader1.fname="/path/to/el3-payload"
+
+    On Foundation FVP, there is no flash loader component and the EL3 payload
+    may be programmed anywhere in flash using method 3 below.
+
+2.  When using the `SPIN_ON_BL1_EXIT=1` loading method, the following DS-5
+    command may be used to load the EL3 payload ELF image over JTAG:
+
+        load /path/to/el3-payload.elf
+
+3.  The EL3 payload may be pre-loaded in volatile memory using the following
+    model parameters:
+
+        --data cluster0.cpu0="/path/to/el3-payload"@address  [Base FVPs]
+        --data="/path/to/el3-payload"@address                [Foundation FVP]
+
+    The address provided to the FVP must match the `EL3_PAYLOAD_BASE` address
+    used when building the Trusted Firmware.
+
+#### Booting an EL3 payload on Juno
+
+If the EL3 payload is able to execute in place, it may be programmed in flash
+memory by adding an entry in the `SITE1/HBI0262x/images.txt` configuration file
+on the Juno SD card (where `x` depends on the revision of the Juno board).
+Refer to the [Juno Getting Started Guide], section 2.3 "Flash memory
+programming" for more information.
+
+Alternatively, the same DS-5 command mentioned in the FVP section above can
+be used to load the EL3 payload's ELF file over JTAG on Juno.
+
 
 8.  Preparing the images to run on FVP
 --------------------------------------
@@ -1211,38 +1289,6 @@
 `SYS_ID` register.  Setting this to `0x0` allows the ARM Trusted Firmware to
 detect the legacy VE memory map while configuring the GIC.
 
-### Booting an EL3 payload on FVP
-
-Booting an EL3 payload on FVP requires a couple of changes to the way the
-model is normally invoked.
-
-First of all, the EL3 payload image is not part of the FIP and is not loaded by
-the Trusted Firmware. Therefore, it must be loaded in memory some other way.
-There are 2 ways of doing that:
-
-1.  It can be loaded over JTAG at the appropriate time. The infinite loop
-    introduced in BL1 when compiling the Trusted Firmware with
-    `SPIN_ON_BL1_EXIT=1` stops execution at the right moment for a debugger to
-    take control of the target and load the payload.
-
-2.  It can be pre-loaded in the FVP memory using the following model parameter:
-
-        --data="<path-to-binary>"@<base-address-of-binary>
-
-    The base address provided to the FVP must match the `EL3_PAYLOAD_BASE`
-    address used when building the Trusted Firmware.
-
-Secondly, the EL3 payloads boot flow requires the CPUs mailbox to be cleared
-at reset for the secondary CPUs holding pen to work properly. Unfortunately,
-its reset value is undefined on FVP. One way to clear it is to create an
-8-byte file containing all zero bytes and pre-load it into the FVP memory at the
-mailbox address (i.e. `0x04000000`) using the same `--data` FVP parameter as
-described above.
-
-The following command creates such a file called `mailbox.dat`:
-
-    dd if=/dev/zero of=mailbox.dat bs=1 count=8
-
 
 10.  Running the software on Juno
 ---------------------------------
@@ -1344,7 +1390,7 @@
 
 - - - - - - - - - - - - - - - - - - - - - - - - - -
 
-_Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved._
+_Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved._
 
 
 [Firmware Design]:             firmware-design.md
diff --git a/drivers/arm/pl011/pl011_console.S b/drivers/arm/pl011/pl011_console.S
index 47608da..f29f895 100644
--- a/drivers/arm/pl011/pl011_console.S
+++ b/drivers/arm/pl011/pl011_console.S
@@ -54,7 +54,7 @@
 	 *     w1 - Uart clock in Hz
 	 *     w2 - Baud rate
 	 * Out: return 1 on success else 0 on error
-	 * Clobber list : x1, x2
+	 * Clobber list : x1, x2, x3, x4
 	 * -----------------------------------------------
 	 */
 func console_core_init
@@ -64,6 +64,20 @@
 	/* Check baud rate and uart clock for sanity */
 	cbz	w1, core_init_fail
 	cbz	w2, core_init_fail
+	/* Disable uart before programming */
+	ldr	w3, [x0, #UARTCR]
+	mov	w4, #PL011_UARTCR_UARTEN
+	bic	w3, w3, w4
+	str	w3, [x0, #UARTCR]
+	/* Flush the transmit FIFO */
+	ldr	w3, [x0, #UARTLCR_H]
+	mov	w4, #PL011_UARTLCR_H_FEN
+	bic	w3, w3, w4
+	str	w3, [x0, #UARTLCR_H]
+	/* Wait for the end of Tx of current character */
+busy_loop:
+	ldr	w3, [x0, #UARTFR]
+	tbnz	w3, #PL011_UARTFR_BUSY_BIT, busy_loop
 	/* Program the baudrate */
 	/* Divisor =  (Uart clock * 4) / baudrate */
 	lsl	w1, w1, #2
diff --git a/drivers/console/console.S b/drivers/console/console.S
index b772363..797b564 100644
--- a/drivers/console/console.S
+++ b/drivers/console/console.S
@@ -54,7 +54,7 @@
 	 *     w1 - Uart clock in Hz
 	 *     w2 - Baud rate
 	 * out: return 1 on success else 0 on error
-	 * Clobber list : x1 - x3
+	 * Clobber list : x1 - x4
 	 * -----------------------------------------------
 	 */
 func console_init
diff --git a/include/drivers/arm/pl011.h b/include/drivers/arm/pl011.h
index d5ea890..ce6cdcf 100644
--- a/include/drivers/arm/pl011.h
+++ b/include/drivers/arm/pl011.h
@@ -71,6 +71,7 @@
 
 #define PL011_UARTFR_TXFF_BIT	5	/* Transmit FIFO full bit in UARTFR register */
 #define PL011_UARTFR_RXFE_BIT	4	/* Receive FIFO empty bit in UARTFR register */
+#define PL011_UARTFR_BUSY_BIT	3	/* UART busy bit in UARTFR register */
 
 /* Control reg bits */
 #if !PL011_GENERIC_UART
diff --git a/plat/arm/common/aarch64/arm_helpers.S b/plat/arm/common/aarch64/arm_helpers.S
index 87179da..a0338f1 100644
--- a/plat/arm/common/aarch64/arm_helpers.S
+++ b/plat/arm/common/aarch64/arm_helpers.S
@@ -66,7 +66,7 @@
 	 * 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
+	 * Clobber list : x0 - x4
 	 * ---------------------------------------------
 	 */
 func plat_crash_console_init
diff --git a/plat/mediatek/common/mtk_sip_svc.c b/plat/mediatek/common/mtk_sip_svc.c
index af28080..cb10af5 100644
--- a/plat/mediatek/common/mtk_sip_svc.c
+++ b/plat/mediatek/common/mtk_sip_svc.c
@@ -55,6 +55,18 @@
 		ret = mt_sip_set_authorized_sreg((uint32_t)x1, (uint32_t)x2);
 		SMC_RET1(handle, ret);
 
+	case MTK_SIP_PWR_ON_MTCMOS:
+		ret = mt_sip_pwr_on_mtcmos((uint32_t)x1);
+		SMC_RET1(handle, ret);
+
+	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;
diff --git a/plat/mediatek/common/mtk_sip_svc.h b/plat/mediatek/common/mtk_sip_svc.h
index eb1c2e6..94a6036 100644
--- a/plat/mediatek/common/mtk_sip_svc.h
+++ b/plat/mediatek/common/mtk_sip_svc.h
@@ -43,10 +43,13 @@
 #define MTK_SIP_SVC_VERSION_MINOR	0x1
 
 /* Number of Mediatek SiP Calls implemented */
-#define MTK_SIP_NUM_CALLS		1
+#define MTK_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
 
 /* Mediatek SiP Calls error code */
 enum {
@@ -62,5 +65,7 @@
  * 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/plat_helpers.S b/plat/mediatek/mt8173/aarch64/plat_helpers.S
index 99a054c..af3a407 100644
--- a/plat/mediatek/mt8173/aarch64/plat_helpers.S
+++ b/plat/mediatek/mt8173/aarch64/plat_helpers.S
@@ -63,7 +63,7 @@
 	 * 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
+	 * Clobber list : x0 - x4
 	 * ---------------------------------------------
 	 */
 func plat_crash_console_init
diff --git a/plat/mediatek/mt8173/bl31_plat_setup.c b/plat/mediatek/mt8173/bl31_plat_setup.c
index 03ec690..ec95143 100644
--- a/plat/mediatek/mt8173/bl31_plat_setup.c
+++ b/plat/mediatek/mt8173/bl31_plat_setup.c
@@ -96,6 +96,16 @@
 	/* set LITTLE cores arm64 boot mode */
 	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->mp0_rv_addr[0].rv_addr_hw,
 		MP0_CPUCFG_64BIT);
+
+	/* enable dcm control */
+	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->bus_fabric_dcm_ctrl,
+		ADB400_GRP_DCM_EN | CCI400_GRP_DCM_EN | ADBCLK_GRP_DCM_EN |
+		EMICLK_GRP_DCM_EN | ACLK_GRP_DCM_EN | L2C_IDLE_DCM_EN |
+		INFRACLK_PSYS_DYNAMIC_CG_EN);
+	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->l2c_sram_ctrl,
+		L2C_SRAM_DCM_EN);
+	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->cci_clk_ctrl,
+		MCU_BUS_DCM_EN);
 }
 
 /*******************************************************************************
diff --git a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c
index f7a1b07..265685f 100644
--- a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c
+++ b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c
@@ -29,8 +29,11 @@
  */
 #include <mmio.h>
 #include <mt8173_def.h>
+#include <debug.h>
 #include <mtcmos.h>
 #include <spm.h>
+#include <spm_mcdi.h>
+#include <delay_timer.h>
 
 enum {
 	SRAM_ISOINT_B	= 1U << 6,
@@ -64,6 +67,63 @@
 	AUD_PWR_STA_MASK   = 0x1 << 24,
 };
 
+#define SPM_VDE_PWR_CON				0x0210
+#define SPM_MFG_PWR_CON				0x0214
+#define SPM_VEN_PWR_CON				0x0230
+#define SPM_ISP_PWR_CON				0x0238
+#define SPM_DIS_PWR_CON				0x023c
+#define SPM_VEN2_PWR_CON			0x0298
+#define SPM_AUDIO_PWR_CON			0x029c
+#define SPM_MFG_2D_PWR_CON			0x02c0
+#define SPM_MFG_ASYNC_PWR_CON			0x02c4
+#define SPM_USB_PWR_CON				0x02cc
+
+#define MTCMOS_CTRL_SUCCESS			0
+#define MTCMOS_CTRL_ERROR			-1
+
+#define MTCMOS_CTRL_EN				(0x1 << 18)
+
+#define VDE_PWR_ON				0
+#define VEN_PWR_ON				1
+#define ISP_PWR_ON				2
+#define DIS_PWR_ON				3
+#define VEN2_PWR_ON				4
+#define AUDIO_PWR_ON				5
+#define MFG_ASYNC_PWR_ON			6
+#define MFG_2D_PWR_ON				7
+#define MFG_PWR_ON				8
+#define USB_PWR_ON				9
+
+#define VDE_PWR_OFF				10
+#define VEN_PWR_OFF				11
+#define ISP_PWR_OFF				12
+#define DIS_PWR_OFF				13
+#define VEN2_PWR_OFF				14
+#define AUDIO_PWR_OFF				15
+#define MFG_ASYNC_PWR_OFF			16
+#define MFG_2D_PWR_OFF				17
+#define MFG_PWR_OFF				18
+#define USB_PWR_OFF				19
+
+#define VDE_PWR_CON_PWR_STA			7
+#define VEN_PWR_CON_PWR_STA			21
+#define ISP_PWR_CON_PWR_STA			5
+#define DIS_PWR_CON_PWR_STA			3
+#define VEN2_PWR_CON_PWR_STA			20
+#define AUDIO_PWR_CON_PWR_STA			24
+#define MFG_ASYNC_PWR_CON_PWR_STA		23
+#define MFG_2D_PWR_CON_PWR_STA			22
+#define MFG_PWR_CON_PWR_STA			4
+#define USB_PWR_CON_PWR_STA			25
+
+/*
+ * Timeout if the ack is not signled after 1 second.
+ * According to designer, one mtcmos operation should be done
+ * around 10us.
+ */
+#define MTCMOS_ACK_POLLING_MAX_COUNT			10000
+#define MTCMOS_ACK_POLLING_INTERVAL			10
+
 static void mtcmos_ctrl_little_off(unsigned int linear_id)
 {
 	uint32_t reg_pwr_con;
@@ -120,3 +180,117 @@
 	mtcmos_ctrl_little_off(2);
 	mtcmos_ctrl_little_off(3);
 }
+
+uint32_t wait_mtcmos_ack(uint32_t on, uint32_t mtcmos_sta, uint32_t spm_pwr_sta)
+{
+	int i = 0;
+	uint32_t cmp, pwr_sta, pwr_sta_2nd;
+
+	while (1) {
+		cmp = (mmio_read_32(SPM_PCM_PASR_DPD_3) >> mtcmos_sta) & 1;
+		pwr_sta = (mmio_read_32(SPM_PWR_STATUS) >> spm_pwr_sta) & 1;
+		pwr_sta_2nd =
+			(mmio_read_32(SPM_PWR_STATUS_2ND) >> spm_pwr_sta) & 1;
+		if ((cmp == on) && (pwr_sta == on) && (pwr_sta_2nd == on)) {
+			mmio_write_32(SPM_PCM_RESERVE2, 0);
+			return MTCMOS_CTRL_SUCCESS;
+		}
+		udelay(MTCMOS_ACK_POLLING_INTERVAL);
+		i++;
+		if (i > MTCMOS_ACK_POLLING_MAX_COUNT) {
+			INFO("MTCMOS control failed(%d), SPM_PWR_STA(%d),\n"
+				"SPM_PCM_RESERVE=0x%x,SPM_PCM_RESERVE2=0x%x,\n"
+				"SPM_PWR_STATUS=0x%x,SPM_PWR_STATUS_2ND=0x%x\n"
+				"SPM_PCM_PASR_DPD_3 = 0x%x\n",
+				on, spm_pwr_sta, mmio_read_32(SPM_PCM_RESERVE),
+				mmio_read_32(SPM_PCM_RESERVE2),
+				mmio_read_32(SPM_PWR_STATUS),
+				mmio_read_32(SPM_PWR_STATUS_2ND),
+				mmio_read_32(SPM_PCM_PASR_DPD_3));
+			mmio_write_32(SPM_PCM_RESERVE2, 0);
+			return MTCMOS_CTRL_ERROR;
+		}
+	}
+}
+
+uint32_t mtcmos_non_cpu_ctrl(uint32_t on, uint32_t mtcmos_num)
+{
+	uint32_t ret = MTCMOS_CTRL_SUCCESS;
+	uint32_t power_on;
+	uint32_t power_off;
+	uint32_t power_status;
+
+	spm_lock_get();
+	spm_mcdi_prepare_for_mtcmos();
+	mmio_setbits_32(SPM_PCM_RESERVE, MTCMOS_CTRL_EN);
+
+	switch (mtcmos_num) {
+	case SPM_VDE_PWR_CON:
+		power_on = VDE_PWR_ON;
+		power_off = VDE_PWR_OFF;
+		power_status = VDE_PWR_CON_PWR_STA;
+		break;
+	case SPM_MFG_PWR_CON:
+		power_on = MFG_PWR_ON;
+		power_off = MFG_PWR_OFF;
+		power_status = MFG_PWR_CON_PWR_STA;
+		break;
+	case SPM_VEN_PWR_CON:
+		power_on = VEN_PWR_ON;
+		power_off = VEN_PWR_OFF;
+		power_status = VEN_PWR_CON_PWR_STA;
+		break;
+	case SPM_ISP_PWR_CON:
+		power_on = ISP_PWR_ON;
+		power_off = ISP_PWR_OFF;
+		power_status = ISP_PWR_CON_PWR_STA;
+		break;
+	case SPM_DIS_PWR_CON:
+		power_on = DIS_PWR_ON;
+		power_off = DIS_PWR_OFF;
+		power_status = DIS_PWR_CON_PWR_STA;
+		break;
+	case SPM_VEN2_PWR_CON:
+		power_on = VEN2_PWR_ON;
+		power_off = VEN2_PWR_OFF;
+		power_status = VEN2_PWR_CON_PWR_STA;
+		break;
+	case SPM_AUDIO_PWR_CON:
+		power_on = AUDIO_PWR_ON;
+		power_off = AUDIO_PWR_OFF;
+		power_status = AUDIO_PWR_CON_PWR_STA;
+		break;
+	case SPM_MFG_2D_PWR_CON:
+		power_on = MFG_2D_PWR_ON;
+		power_off = MFG_2D_PWR_OFF;
+		power_status = MFG_2D_PWR_CON_PWR_STA;
+		break;
+	case SPM_MFG_ASYNC_PWR_CON:
+		power_on = MFG_ASYNC_PWR_ON;
+		power_off = MFG_ASYNC_PWR_OFF;
+		power_status = MFG_ASYNC_PWR_CON_PWR_STA;
+		break;
+	case SPM_USB_PWR_CON:
+		power_on = USB_PWR_ON;
+		power_off = USB_PWR_OFF;
+		power_status = USB_PWR_CON_PWR_STA;
+		break;
+	default:
+		ret = MTCMOS_CTRL_ERROR;
+		INFO("No mapping MTCMOS(%d), ret = %d\n", mtcmos_num, ret);
+		break;
+	}
+
+	if (ret == MTCMOS_CTRL_SUCCESS) {
+		mmio_setbits_32(SPM_PCM_RESERVE2, on ?
+			(1 << power_on) : (1 << power_off));
+		ret = wait_mtcmos_ack(on, power_on, power_status);
+		VERBOSE("0x%x(%d), PWR_STATUS(0x%x), ret(%d)\n",
+			power_on, on, mmio_read_32(SPM_PWR_STATUS), ret);
+	}
+
+	mmio_clrbits_32(SPM_PCM_RESERVE, MTCMOS_CTRL_EN);
+	spm_lock_release();
+
+	return ret;
+}
diff --git a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h
index ddcda78..4641f46 100644
--- a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h
+++ b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h
@@ -37,5 +37,6 @@
  * during CPU_ON psci call.
  */
 void mtcmos_little_cpu_off(void);
+uint32_t mtcmos_non_cpu_ctrl(uint32_t on, uint32_t mtcmos_num);
 
 #endif /* __MTCMOS_H__ */
diff --git a/plat/mediatek/mt8173/drivers/spm/spm.c b/plat/mediatek/mt8173/drivers/spm/spm.c
index 2801925..45defd8 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm.c
@@ -386,6 +386,8 @@
 {
 	/* Only CPU0 is online during boot, initialize cpu online reserve bit */
 	mmio_write_32(SPM_PCM_RESERVE, 0xFE);
+	mmio_clrbits_32(AP_PLL_CON3, 0xFFFFF);
+	mmio_clrbits_32(AP_PLL_CON4, 0xF);
 	spm_lock_init();
 	spm_register_init();
 }
diff --git a/plat/mediatek/mt8173/drivers/spm/spm.h b/plat/mediatek/mt8173/drivers/spm/spm.h
index f1e7674..f371fd8 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm.h
+++ b/plat/mediatek/mt8173/drivers/spm/spm.h
@@ -129,6 +129,9 @@
 #define SPM_SLEEP_CA15_WFI2_EN			(SPM_BASE + 0xf18)
 #define SPM_SLEEP_CA15_WFI3_EN			(SPM_BASE + 0xf1c)
 
+#define AP_PLL_CON3		0x1020900c
+#define AP_PLL_CON4		0x10209010
+
 #define SPM_PROJECT_CODE	0xb16
 
 #define SPM_REGWR_EN		(1U << 0)
@@ -213,6 +216,7 @@
 #define WAKE_SRC_USB_PDN	(1 << 15)
 #define WAKE_SRC_AFE		(1 << 20)
 #define WAKE_SRC_THERM		(1 << 21)
+#define WAKE_SRC_CIRQ		(1 << 22)
 #define WAKE_SRC_SYSPWREQ	(1 << 24)
 #define WAKE_SRC_SEJ		(1 << 27)
 #define WAKE_SRC_ALL_MD32	(1 << 28)
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c b/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
index fccd5a2..b89cd97 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
@@ -40,7 +40,7 @@
  * This driver controls the cpu power in cpu hotplug flow.
  */
 
-#define PCM_HOTPLUG_VALID_MASK	0x00ff0000
+#define PCM_HOTPLUG_VALID_MASK	0x0000ff00
 #define PCM_HOTPLUG_VALID_SHIFT	0x8
 
 /**********************************************************
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
index 11f4a4a..3043327 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
@@ -43,175 +43,199 @@
  * This driver controls the cpu power in cpu idle power saving state.
  */
 
-#define WAKE_SRC_FOR_MCDI	(WAKE_SRC_SYSPWREQ | WAKE_SRC_CPU_IRQ)
+#define WAKE_SRC_FOR_MCDI \
+	(WAKE_SRC_KP | WAKE_SRC_GPT | WAKE_SRC_EINT |		\
+	 WAKE_SRC_MD32 | WAKE_SRC_USB_CD | WAKE_SRC_USB_PDN |	\
+	 WAKE_SRC_AFE | WAKE_SRC_THERM | WAKE_SRC_CIRQ |	\
+	 WAKE_SRC_SYSPWREQ | WAKE_SRC_CPU_IRQ)
 #define PCM_MCDI_HANDSHAKE_SYNC	0xbeefbeef
 #define PCM_MCDI_HANDSHAKE_ACK	0xdeaddead
 #define PCM_MCDI_UPDATE_INFORM	0xabcdabcd
 #define PCM_MCDI_CKECK_DONE	0x12345678
 #define PCM_MCDI_ALL_CORE_AWAKE	0x0
 #define PCM_MCDI_OFFLOADED	0xaa55aa55
+#define PCM_MCDI_CA72_CPUTOP_PWRCTL	(0x1 << 16)
+#define PCM_MCDI_CA53_CPUTOP_PWRCTL	(0x1 << 17)
+#define PCM_MCDI_CA72_PWRSTA_SHIFT	16
+#define PCM_MCDI_CA53_PWRSTA_SHIFT	9
 
 static const unsigned int mcdi_binary[] = {
-	0x1212841f, 0xe2e00036, 0xe2e0003e, 0x1380201f, 0xe2e0003c, 0xe2a00000,
-	0x1b80001f, 0x20000080, 0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c,
-	0xe2e0004c, 0xe2e0004d, 0xf0000000, 0x17c07c1f, 0xe2e0004f, 0xe2e0006f,
-	0xe2e0002f, 0xe2a00001, 0x1b80001f, 0x20000080, 0xe2e0002e, 0xe2e0003e,
-	0xe2e00032, 0xf0000000, 0x17c07c1f, 0x1212841f, 0xe2e00026, 0xe2e0002e,
-	0x1380201f, 0x1a00001f, 0x100062b4, 0x1910001f, 0x100062b4, 0x81322804,
-	0xe2000004, 0x81202804, 0xe2000004, 0x1b80001f, 0x20000034, 0x1910001f,
-	0x100062b4, 0x81142804, 0xd8000524, 0x17c07c1f, 0xe2e0000e, 0xe2e0000c,
-	0xe2e0000d, 0xf0000000, 0x17c07c1f, 0xe2e0002d, 0x1a00001f, 0x100062b4,
-	0x1910001f, 0x100062b4, 0xa1002804, 0xe2000004, 0xa1122804, 0xe2000004,
-	0x1b80001f, 0x20000080, 0x1910001f, 0x100062b4, 0x81142804, 0xd82007c4,
-	0x17c07c1f, 0xe2e0002f, 0xe2e0002b, 0xe2e00023, 0x1380201f, 0xe2e00022,
-	0xf0000000, 0x17c07c1f, 0x18c0001f, 0x10006b6c, 0x1910001f, 0x10006b6c,
-	0xa1002804, 0xe0c00004, 0xf0000000, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
+	0x1a10001f, 0x10006b04, 0x1890001f, 0x10006b6c, 0x1a40001f, 0x10006210,
+	0x18d0001f, 0x10006210, 0x81002001, 0xd82001c4, 0x17c07c1f, 0xa0900402,
+	0xc2401520, 0x17c07c1f, 0x81052001, 0xd8200284, 0x17c07c1f, 0x80b00402,
+	0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x10006230, 0x18d0001f, 0x10006230,
+	0x8100a001, 0xd82003c4, 0x17c07c1f, 0xa0908402, 0xc2401520, 0x17c07c1f,
+	0x8105a001, 0xd8200484, 0x17c07c1f, 0x80b08402, 0xc2401b60, 0x17c07c1f,
+	0x1a40001f, 0x10006238, 0x18d0001f, 0x10006238, 0x81012001, 0xd82005c4,
+	0x17c07c1f, 0xa0910402, 0xc2401520, 0x17c07c1f, 0x81062001, 0xd8200684,
+	0x17c07c1f, 0x80b10402, 0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x1000623c,
+	0x18d0001f, 0x1000623c, 0x8101a001, 0xd82007c4, 0x17c07c1f, 0xa0918402,
+	0xc2401520, 0x17c07c1f, 0x8106a001, 0xd8200884, 0x17c07c1f, 0x80b18402,
+	0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x10006298, 0x18d0001f, 0x10006298,
+	0x81022001, 0xd82009c4, 0x17c07c1f, 0xa0920402, 0xc2401520, 0x17c07c1f,
+	0x81072001, 0xd8200a84, 0x17c07c1f, 0x80b20402, 0xc2401b60, 0x17c07c1f,
+	0x1a40001f, 0x1000629c, 0x18d0001f, 0x1000629c, 0x8102a001, 0xd8200bc4,
+	0x17c07c1f, 0xa0928402, 0xc2401520, 0x17c07c1f, 0x8107a001, 0xd8200c84,
+	0x17c07c1f, 0x80b28402, 0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x100062c4,
+	0x18d0001f, 0x100062c4, 0x81032001, 0xd8200dc4, 0x17c07c1f, 0xa0930402,
+	0xc2401520, 0x17c07c1f, 0x81082001, 0xd8200e84, 0x17c07c1f, 0x80b30402,
+	0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x100062c0, 0x18d0001f, 0x100062c0,
+	0x8103a001, 0xd8200fc4, 0x17c07c1f, 0xa0938402, 0xc2401520, 0x17c07c1f,
+	0x8108a001, 0xd8201084, 0x17c07c1f, 0x80b38402, 0xc2401b60, 0x17c07c1f,
+	0x1a40001f, 0x10006214, 0x18d0001f, 0x10006214, 0x81042001, 0xd82011c4,
+	0x17c07c1f, 0xa0940402, 0xc2401520, 0x17c07c1f, 0x81092001, 0xd8201284,
+	0x17c07c1f, 0x80b40402, 0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x100062cc,
+	0x18d0001f, 0x100062cc, 0x8104a001, 0xd82013c4, 0x17c07c1f, 0xa0948402,
+	0xc2401520, 0x17c07c1f, 0x8109a001, 0xd8201484, 0x17c07c1f, 0x80b48402,
+	0xc2401b60, 0x17c07c1f, 0x1900001f, 0x10006b6c, 0xe1000002, 0xf0000000,
+	0x17c07c1f, 0xa8c00003, 0x00000004, 0xe2400003, 0xa8c00003, 0x00000008,
+	0xe2400003, 0x1b80001f, 0x00000020, 0x88c00003, 0xffffffef, 0xe2400003,
+	0x88c00003, 0xfffffffd, 0xe2400003, 0xa8c00003, 0x00000001, 0xe2400003,
+	0x88c00003, 0xfffff0ff, 0xe2400003, 0x1b80001f, 0x20000080, 0x1a90001f,
+	0x10001220, 0x69200009, 0x1000623c, 0xd8001964, 0x17c07c1f, 0x69200009,
+	0x10006214, 0xd8001a44, 0x17c07c1f, 0xd0001ae0, 0x17c07c1f, 0x1900001f,
+	0x10001220, 0x8a80000a, 0xfffffff9, 0xe100000a, 0xd0001ae0, 0x17c07c1f,
+	0x1900001f, 0x10001220, 0x8a80000a, 0xff1fbfff, 0xe100000a, 0x1b80001f,
+	0x20000080, 0xf0000000, 0x17c07c1f, 0x1a90001f, 0x10001220, 0x69200009,
+	0x1000623c, 0xd8001ce4, 0x17c07c1f, 0x69200009, 0x10006214, 0xd8001dc4,
+	0x17c07c1f, 0xd0001e60, 0x17c07c1f, 0x1900001f, 0x10001220, 0xaa80000a,
+	0x00000006, 0xe100000a, 0xd0001e60, 0x17c07c1f, 0x1900001f, 0x10001220,
+	0xaa80000a, 0x00e04000, 0xe100000a, 0x1b80001f, 0x20000080, 0x69200009,
+	0x10006214, 0xd8001fc4, 0x17c07c1f, 0xa8c00003, 0x00000f00, 0xe2400003,
+	0xd0002020, 0x17c07c1f, 0xa8c00003, 0x00003f00, 0xe2400003, 0x1b80001f,
+	0x20000080, 0xa8c00003, 0x00000002, 0xe2400003, 0x88c00003, 0xfffffffe,
+	0xe2400003, 0xa8c00003, 0x00000010, 0xe2400003, 0x88c00003, 0xfffffffb,
+	0xe2400003, 0x88c00003, 0xfffffff7, 0xe2400003, 0xf0000000, 0x17c07c1f,
+	0xe2e00036, 0xe2e0003e, 0x1b80001f, 0x00000020, 0xe2e0003c, 0xe8208000,
+	0x10006244, 0x00000000, 0x1b80001f, 0x20000080, 0xe2e0007c, 0x1b80001f,
+	0x20000003, 0xe2e0005c, 0xe2e0004c, 0xe2e0004d, 0xf0000000, 0x17c07c1f,
+	0xe2e0004f, 0xe2e0006f, 0xe2e0002f, 0xe8208000, 0x10006244, 0x00000001,
+	0x1b80001f, 0x20000080, 0xe2e0002e, 0xe2e0003e, 0xe2e0003a, 0xe2e00032,
+	0x1b80001f, 0x00000020, 0x1910001f, 0x10006b6c, 0x09000004, 0x00100000,
+	0x1a10001f, 0x10006b6c, 0xe2000004, 0xf0000000, 0x17c07c1f, 0xe2e00036,
+	0xe2e0003e, 0x1b80001f, 0x00000020, 0xe2e0003c, 0xe2a00000, 0x1b80001f,
+	0x20000080, 0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c, 0xe2e0004c,
+	0xe2e0004d, 0xf0000000, 0x17c07c1f, 0xe2e0004f, 0xe2e0006f, 0xe2e0002f,
+	0xe2a00001, 0x1b80001f, 0x20000080, 0xe2e0002e, 0xe2e0003e, 0xe2e0003a,
+	0xe2e00032, 0xf0000000, 0x17c07c1f, 0xe2e00026, 0xe2e0002e, 0x1b80001f,
+	0x00000020, 0x1a00001f, 0x100062b4, 0x1910001f, 0x100062b4, 0x81322804,
+	0xe2000004, 0x81202804, 0xe2000004, 0x1b80001f, 0x20000080, 0xe2e0000e,
+	0xe2e0000c, 0xe2e0000d, 0xf0000000, 0x17c07c1f, 0xe2e0002d, 0x1a00001f,
+	0x100062b4, 0x1910001f, 0x100062b4, 0xa1002804, 0xe2000004, 0xa1122804,
+	0xe2000004, 0x1b80001f, 0x20000080, 0xe2e0002f, 0xe2e0002b, 0xe2e00023,
+	0x1b80001f, 0x00000020, 0xe2e00022, 0xf0000000, 0x17c07c1f, 0x1900001f,
+	0x1020020c, 0x1a10001f, 0x1020020c, 0xaa000008, 0x00000001, 0xe1000008,
+	0x1910001f, 0x10006720, 0x820c9001, 0xd82030c8, 0x17c07c1f, 0x1900001f,
+	0x10001220, 0x1a10001f, 0x10001220, 0xa21f0408, 0xe1000008, 0x1b80001f,
+	0x20000080, 0xe2e0006d, 0xe2e0002d, 0x1a00001f, 0x100062b8, 0x1910001f,
+	0x100062b8, 0xa9000004, 0x00000001, 0xe2000004, 0x1b80001f, 0x20000080,
+	0xe2e0002c, 0xe2e0003c, 0xe2e0003e, 0xe2e0003a, 0xe2e00032, 0x1b80001f,
+	0x00000020, 0x1900001f, 0x10006404, 0x1a10001f, 0x10006404, 0xa2168408,
+	0xe1000008, 0xf0000000, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8a000008,
+	0x00003030, 0xb900010c, 0x01000001, 0xd8203bc4, 0x17c07c1f, 0x1900001f,
+	0x10006404, 0x1a10001f, 0x10006404, 0x8a000008, 0x0000dfff, 0xe1000008,
+	0xe2e00036, 0xe2e0003e, 0x1b80001f, 0x00000020, 0xe2e0002e, 0x1a00001f,
+	0x100062b8, 0x1910001f, 0x100062b8, 0x89000004, 0x0000fffe, 0xe2000004,
+	0x1b80001f, 0x20000080, 0xe2e0006e, 0xe2e0004e, 0xe2e0004c, 0xe2e0004d,
+	0x1900001f, 0x10001220, 0x1a10001f, 0x10001220, 0x8a000008, 0xbfffffff,
+	0xe1000008, 0x1b80001f, 0x20000080, 0x1900001f, 0x1020020c, 0x1a10001f,
+	0x1020020c, 0x8a000008, 0xfffffffe, 0xe1000008, 0xf0000000, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x1840001f, 0x00000001, 0x11407c1f, 0xe8208000,
 	0x10006b6c, 0xa0000000, 0xe8208000, 0x10006310, 0x0b160008, 0x1900001f,
 	0x000f7bde, 0x1a00001f, 0x10200268, 0xe2000004, 0xe8208000, 0x10006600,
-	0x00000000, 0xc2800940, 0x1280041f, 0x1b00001f, 0x21000001, 0x1b80001f,
-	0xd0010000, 0xc2800940, 0x1290841f, 0x69200006, 0xbeefbeef, 0xd8204764,
-	0x17c07c1f, 0xc2800940, 0x1291041f, 0x1910001f, 0x10006358, 0x810b1001,
-	0xd80043e4, 0x17c07c1f, 0x1980001f, 0xdeaddead, 0x69200006, 0xabcdabcd,
-	0xd82044c4, 0x17c07c1f, 0xc2800940, 0x1291841f, 0x88900001, 0x10006814,
+	0x00000000, 0x69200006, 0xbeefbeef, 0xd82045e4, 0x17c07c1f, 0x1910001f,
+	0x10006358, 0x810b1001, 0xd80042a4, 0x17c07c1f, 0x1980001f, 0xdeaddead,
+	0x69200006, 0xabcdabcd, 0xd8204384, 0x17c07c1f, 0x88900001, 0x10006814,
 	0x1910001f, 0x10006400, 0x81271002, 0x1880001f, 0x10006600, 0xe0800004,
-	0x1910001f, 0x10006358, 0x810b1001, 0xd8004684, 0x17c07c1f, 0x1980001f,
-	0x12345678, 0x60a07c05, 0x89100002, 0x10006600, 0x80801001, 0xd8007142,
-	0x17c07c1f, 0xc2800940, 0x1292041f, 0x1a10001f, 0x10006720, 0x82002001,
-	0x82201408, 0xd8204a08, 0x17c07c1f, 0x1a40001f, 0x10006200, 0x1a80001f,
-	0x1000625c, 0xc2400200, 0x17c07c1f, 0xa1400405, 0xc2800940, 0x1292841f,
-	0x1a10001f, 0x10006720, 0x8200a001, 0x82209408, 0xd8204be8, 0x17c07c1f,
-	0x1a40001f, 0x10006218, 0x1a80001f, 0x10006264, 0xc2400200, 0x17c07c1f,
-	0xa1508405, 0xc2800940, 0x1293041f, 0x1a10001f, 0x10006720, 0x82012001,
-	0x82211408, 0xd8204dc8, 0x17c07c1f, 0x1a40001f, 0x1000621c, 0x1a80001f,
-	0x1000626c, 0xc2400200, 0x17c07c1f, 0xa1510405, 0x1a10001f, 0x10006720,
-	0x8201a001, 0x82219408, 0xd8204f68, 0x17c07c1f, 0x1a40001f, 0x10006220,
-	0x1a80001f, 0x10006274, 0xc2400200, 0x17c07c1f, 0xa1518405, 0x1a10001f,
-	0x10006720, 0x82022001, 0x82221408, 0xd82050e8, 0x17c07c1f, 0x1a40001f,
-	0x100062a0, 0x1280041f, 0xc2400660, 0x17c07c1f, 0xa1520405, 0x1a10001f,
-	0x10006720, 0x8202a001, 0x82229408, 0xd8205268, 0x17c07c1f, 0x1a40001f,
-	0x100062a4, 0x1290841f, 0xc2400660, 0x17c07c1f, 0xa1528405, 0x1a10001f,
-	0x10006720, 0x82032001, 0x82231408, 0xd82053e8, 0x17c07c1f, 0x1a40001f,
-	0x100062a8, 0x1291041f, 0xc2400660, 0x17c07c1f, 0xa1530405, 0x1a10001f,
-	0x10006720, 0x8203a001, 0x82239408, 0xd8205568, 0x17c07c1f, 0x1a40001f,
-	0x100062ac, 0x1291841f, 0xc2400660, 0x17c07c1f, 0xa1538405, 0x1b80001f,
-	0x20000208, 0xd82070cc, 0x17c07c1f, 0x81001401, 0xd8205964, 0x17c07c1f,
-	0x1a10001f, 0x10006918, 0x81002001, 0xb1042081, 0xb1003081, 0xb10c3081,
-	0xd8205964, 0x17c07c1f, 0x1a40001f, 0x10006200, 0x1a80001f, 0x1000625c,
-	0xc2400000, 0x17c07c1f, 0x89400005, 0xfffffffe, 0xe8208000, 0x10006f00,
-	0x00000000, 0xe8208000, 0x10006b30, 0x00000000, 0xe8208000, 0x100063e0,
-	0x00000001, 0x81009401, 0xd8205cc4, 0x17c07c1f, 0x1a10001f, 0x10006918,
-	0x8100a001, 0xb104a081, 0xb1003081, 0xd8205cc4, 0x17c07c1f, 0x1a40001f,
-	0x10006218, 0x1a80001f, 0x10006264, 0xc2400000, 0x17c07c1f, 0x89400005,
-	0xfffffffd, 0xe8208000, 0x10006f04, 0x00000000, 0xe8208000, 0x10006b34,
-	0x00000000, 0xe8208000, 0x100063e0, 0x00000002, 0x81011401, 0xd8206024,
-	0x17c07c1f, 0x1a10001f, 0x10006918, 0x81012001, 0xb1052081, 0xb1003081,
-	0xd8206024, 0x17c07c1f, 0x1a40001f, 0x1000621c, 0x1a80001f, 0x1000626c,
-	0xc2400000, 0x17c07c1f, 0x89400005, 0xfffffffb, 0xe8208000, 0x10006f08,
-	0x00000000, 0xe8208000, 0x10006b38, 0x00000000, 0xe8208000, 0x100063e0,
-	0x00000004, 0x81019401, 0xd8206384, 0x17c07c1f, 0x1a10001f, 0x10006918,
-	0x8101a001, 0xb105a081, 0xb1003081, 0xd8206384, 0x17c07c1f, 0x1a40001f,
-	0x10006220, 0x1a80001f, 0x10006274, 0xc2400000, 0x17c07c1f, 0x89400005,
-	0xfffffff7, 0xe8208000, 0x10006f0c, 0x00000000, 0xe8208000, 0x10006b3c,
-	0x00000000, 0xe8208000, 0x100063e0, 0x00000008, 0x81021401, 0xd82066c4,
-	0x17c07c1f, 0x1a10001f, 0x10006918, 0x81022001, 0xb1062081, 0xb1003081,
-	0xd82066c4, 0x17c07c1f, 0x1a40001f, 0x100062a0, 0x1280041f, 0xc2400360,
-	0x17c07c1f, 0x89400005, 0xffffffef, 0xe8208000, 0x10006f10, 0x00000000,
-	0xe8208000, 0x10006b40, 0x00000000, 0xe8208000, 0x100063e0, 0x00000010,
-	0x81029401, 0xd8206a04, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8102a001,
-	0xb106a081, 0xb1003081, 0xd8206a04, 0x17c07c1f, 0x1a40001f, 0x100062a4,
-	0x1290841f, 0xc2400360, 0x17c07c1f, 0x89400005, 0xffffffdf, 0xe8208000,
+	0x1910001f, 0x10006358, 0x810b1001, 0xd8004504, 0x17c07c1f, 0x1980001f,
+	0x12345678, 0x60a07c05, 0x89100002, 0x10006600, 0x80801001, 0xd8007e02,
+	0x17c07c1f, 0x1890001f, 0x10006b00, 0x82090801, 0xc8800008, 0x17c07c1f,
+	0x1b00001f, 0x3fffe7ff, 0x8a00000c, 0x3fffe7ff, 0xd8204228, 0x17c07c1f,
+	0x1b80001f, 0xd0010000, 0x1a10001f, 0x10006720, 0x82002001, 0x82201408,
+	0xd82049e8, 0x17c07c1f, 0x1a40001f, 0x10006200, 0x1a80001f, 0x1000625c,
+	0xc24029a0, 0x17c07c1f, 0xa1400405, 0x1a10001f, 0x10006720, 0x8200a001,
+	0x82209408, 0xd8204b88, 0x17c07c1f, 0x1a40001f, 0x10006218, 0x1a80001f,
+	0x10006264, 0xc24029a0, 0x17c07c1f, 0xa1508405, 0x1a10001f, 0x10006720,
+	0x82012001, 0x82211408, 0xd8204d28, 0x17c07c1f, 0x1a40001f, 0x1000621c,
+	0x1a80001f, 0x1000626c, 0xc24029a0, 0x17c07c1f, 0xa1510405, 0x1a10001f,
+	0x10006720, 0x8201a001, 0x82219408, 0xd8204ec8, 0x17c07c1f, 0x1a40001f,
+	0x10006220, 0x1a80001f, 0x10006274, 0xc24029a0, 0x17c07c1f, 0xa1518405,
+	0x1a10001f, 0x10006720, 0x82022001, 0x82221408, 0xd8205048, 0x17c07c1f,
+	0x1a40001f, 0x100062a0, 0x1280041f, 0xc2402d80, 0x17c07c1f, 0xa1520405,
+	0x1a10001f, 0x10006720, 0x8202a001, 0x82229408, 0xd82051c8, 0x17c07c1f,
+	0x1a40001f, 0x100062a4, 0x1290841f, 0xc2402d80, 0x17c07c1f, 0xa1528405,
+	0x1a10001f, 0x10006720, 0x82032001, 0x82231408, 0xd82052a8, 0x17c07c1f,
+	0xa1530405, 0x1a10001f, 0x10006720, 0x8203a001, 0x82239408, 0xd8205388,
+	0x17c07c1f, 0xa1538405, 0x1a10001f, 0x10006b00, 0x8108a001, 0xd8205ec4,
+	0x17c07c1f, 0x1a10001f, 0x10006610, 0x8104a001, 0xb1052081, 0xb105a081,
+	0xb1062081, 0xd8005984, 0x17c07c1f, 0x81042001, 0xd8205984, 0x17c07c1f,
+	0x1900001f, 0x1020002c, 0x1a10001f, 0x1020002c, 0xaa000008, 0x00000010,
+	0xe1000008, 0x1910001f, 0x10006720, 0x820c1001, 0xd8205668, 0x17c07c1f,
+	0x1900001f, 0x10001250, 0x1a10001f, 0x10001250, 0xa2110408, 0xe1000008,
+	0x1b80001f, 0x20000080, 0x1900001f, 0x10001220, 0x1a10001f, 0x10001220,
+	0xa21e8408, 0xe1000008, 0x1b80001f, 0x20000080, 0x1a40001f, 0x10006208,
+	0xc24024c0, 0x17c07c1f, 0x1910001f, 0x10006610, 0x81041001, 0xd8005ec4,
+	0x17c07c1f, 0x1a10001f, 0x10006918, 0x8a000008, 0x00000f0f, 0xb900010c,
+	0x1fffe7ff, 0xd8205ec4, 0x17c07c1f, 0x1a40001f, 0x10006208, 0xc2402280,
+	0x17c07c1f, 0x1900001f, 0x10001250, 0x1a10001f, 0x10001250, 0x8a000008,
+	0xfffffffb, 0xe1000008, 0x1b80001f, 0x20000080, 0x1900001f, 0x10001220,
+	0x1a10001f, 0x10001220, 0x8a000008, 0xdfffffff, 0xe1000008, 0x1b80001f,
+	0x20000080, 0x1900001f, 0x1020002c, 0x1a10001f, 0x1020002c, 0x8a000008,
+	0xffffffef, 0xe1000008, 0x1a10001f, 0x10006b00, 0x81082001, 0xd8206144,
+	0x17c07c1f, 0x1a10001f, 0x10006610, 0x81082001, 0xb108a081, 0xd8006144,
+	0x17c07c1f, 0x1a10001f, 0x10006610, 0x8107a001, 0xd8206144, 0x17c07c1f,
+	0x1a40001f, 0x100062b0, 0xc2402fe0, 0x17c07c1f, 0x1b80001f, 0x20000208,
+	0xd8207dcc, 0x17c07c1f, 0x1910001f, 0x10006610, 0x81079001, 0xd80062e4,
+	0x17c07c1f, 0x1a40001f, 0x100062b0, 0xc24035a0, 0x17c07c1f, 0x81001401,
+	0xd8206664, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x81002001, 0xb1042081,
+	0xb900008c, 0x1fffe7ff, 0xd8206664, 0x17c07c1f, 0x1a40001f, 0x10006200,
+	0x1a80001f, 0x1000625c, 0xc24027a0, 0x17c07c1f, 0x89400005, 0xfffffffe,
+	0xe8208000, 0x10006f00, 0x00000000, 0xe8208000, 0x10006b30, 0x00000000,
+	0xe8208000, 0x100063e0, 0x00000001, 0x81009401, 0xd82069e4, 0x17c07c1f,
+	0x1a10001f, 0x10006918, 0x8100a001, 0xb104a081, 0xb900008c, 0x01000001,
+	0xd82069e4, 0x17c07c1f, 0x1a40001f, 0x10006218, 0x1a80001f, 0x10006264,
+	0xc24027a0, 0x17c07c1f, 0x89400005, 0xfffffffd, 0xe8208000, 0x10006f04,
+	0x00000000, 0xe8208000, 0x10006b34, 0x00000000, 0xe8208000, 0x100063e0,
+	0x00000002, 0x81011401, 0xd8206d64, 0x17c07c1f, 0x1a10001f, 0x10006918,
+	0x81012001, 0xb1052081, 0xb900008c, 0x01000001, 0xd8206d64, 0x17c07c1f,
+	0x1a40001f, 0x1000621c, 0x1a80001f, 0x1000626c, 0xc24027a0, 0x17c07c1f,
+	0x89400005, 0xfffffffb, 0xe8208000, 0x10006f08, 0x00000000, 0xe8208000,
+	0x10006b38, 0x00000000, 0xe8208000, 0x100063e0, 0x00000004, 0x81019401,
+	0xd82070e4, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8101a001, 0xb105a081,
+	0xb900008c, 0x01000001, 0xd82070e4, 0x17c07c1f, 0x1a40001f, 0x10006220,
+	0x1a80001f, 0x10006274, 0xc24027a0, 0x17c07c1f, 0x89400005, 0xfffffff7,
+	0xe8208000, 0x10006f0c, 0x00000000, 0xe8208000, 0x10006b3c, 0x00000000,
+	0xe8208000, 0x100063e0, 0x00000008, 0x1910001f, 0x10006610, 0x81079001,
+	0xd8207844, 0x17c07c1f, 0x81021401, 0xd82074e4, 0x17c07c1f, 0x1a10001f,
+	0x10006918, 0x81022001, 0xb1062081, 0xb900008c, 0x01000001, 0xd82074e4,
+	0x17c07c1f, 0x1a40001f, 0x100062a0, 0x1280041f, 0xc2402b20, 0x17c07c1f,
+	0x89400005, 0xffffffef, 0xe8208000, 0x10006f10, 0x00000000, 0xe8208000,
+	0x10006b40, 0x00000000, 0xe8208000, 0x100063e0, 0x00000010, 0x81029401,
+	0xd8207844, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8102a001, 0xb106a081,
+	0xb900008c, 0x01000001, 0xd8207844, 0x17c07c1f, 0x1a40001f, 0x100062a4,
+	0x1290841f, 0xc2402b20, 0x17c07c1f, 0x89400005, 0xffffffdf, 0xe8208000,
 	0x10006f14, 0x00000000, 0xe8208000, 0x10006b44, 0x00000000, 0xe8208000,
-	0x100063e0, 0x00000020, 0x81031401, 0xd8206d44, 0x17c07c1f, 0x1a10001f,
-	0x10006918, 0x81032001, 0xb1072081, 0xb1003081, 0xd8206d44, 0x17c07c1f,
-	0x1a40001f, 0x100062a8, 0x1291041f, 0xc2400360, 0x17c07c1f, 0x89400005,
-	0xffffffbf, 0xe8208000, 0x10006f18, 0x00000000, 0xe8208000, 0x10006b48,
-	0x00000000, 0xe8208000, 0x100063e0, 0x00000040, 0x81039401, 0xd8207084,
-	0x17c07c1f, 0x1a10001f, 0x10006918, 0x8103a001, 0xb107a081, 0xb1003081,
-	0xd8207084, 0x17c07c1f, 0x1a40001f, 0x100062ac, 0x1291841f, 0xc2400360,
-	0x17c07c1f, 0x89400005, 0xffffff7f, 0xe8208000, 0x10006f1c, 0x00000000,
-	0xe8208000, 0x10006b4c, 0x00000000, 0xe8208000, 0x100063e0, 0x00000080,
-	0xc2800940, 0x1293841f, 0xd0004260, 0x17c07c1f, 0xc2800940, 0x1294041f,
+	0x100063e0, 0x00000020, 0x81031401, 0xd8207b04, 0x17c07c1f, 0x1a10001f,
+	0x10006918, 0x81032001, 0xb1072081, 0xb900008c, 0x01000001, 0xd8207b04,
+	0x17c07c1f, 0x89400005, 0xffffffbf, 0xe8208000, 0x10006f18, 0x00000000,
+	0xe8208000, 0x10006b48, 0x00000000, 0xe8208000, 0x100063e0, 0x00000040,
+	0x81039401, 0xd8207dc4, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8103a001,
+	0xb107a081, 0xb900008c, 0x01000001, 0xd8207dc4, 0x17c07c1f, 0x89400005,
+	0xffffff7f, 0xe8208000, 0x10006f1c, 0x00000000, 0xe8208000, 0x10006b4c,
+	0x00000000, 0xe8208000, 0x100063e0, 0x00000080, 0xd0004220, 0x17c07c1f,
 	0xe8208000, 0x10006600, 0x00000000, 0x1ac0001f, 0x55aa55aa, 0x1940001f,
-	0xaa55aa55, 0xc2800940, 0x1294841f, 0x1b80001f, 0x00001000, 0xf0000000,
-	0x17c07c1f
+	0xaa55aa55, 0x1b80001f, 0x00001000, 0xf0000000, 0x17c07c1f
 };
 
 static const struct pcm_desc mcdi_pcm = {
-	.version = "pcm_mcdi_v0.5_20140721_mt8173_v03.04_20150507",
+	.version = "pcm_mcdi_mt8173_20151203_v6",
 	.base = mcdi_binary,
-	.size = 919,
+	.size = 1019,
 	.sess = 2,
 	.replace = 0,
 };
@@ -239,8 +263,8 @@
 	    && ((mmio_read_32(SPM_CLK_CON) & CC_DISABLE_DORM_PWR) == 0)) {
 		/* MCDI is offload? */
 		INFO("%s: SPM_SLEEP_CPU_WAKEUP_EVENT:%x, SPM_CLK_CON %x",
-		     __func__, mmio_read_32(SPM_SLEEP_CPU_WAKEUP_EVENT),
-		     mmio_read_32(SPM_CLK_CON));
+			__func__, mmio_read_32(SPM_SLEEP_CPU_WAKEUP_EVENT),
+			mmio_read_32(SPM_CLK_CON));
 		return;
 	}
 	/* Inform SPM that CPU wants to program CPU_WAKEUP_EVENT and
@@ -301,7 +325,7 @@
 	clear_all_ready();
 }
 
-void spm_mcdi_wfi_sel_enter(unsigned long mpidr)
+static void spm_mcdi_wfi_sel_enter(unsigned long mpidr)
 {
 	int core_id_val = mpidr & MPIDR_CPU_MASK;
 	int cluster_id = (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS;
@@ -352,7 +376,7 @@
 	}
 }
 
-void spm_mcdi_wfi_sel_leave(unsigned long mpidr)
+static void spm_mcdi_wfi_sel_leave(unsigned long mpidr)
 {
 	int core_id_val = mpidr & MPIDR_CPU_MASK;
 	int cluster_id = (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS;
@@ -400,10 +424,72 @@
 		default:
 			break;
 		}
+	}
+}
+
+static void spm_mcdi_set_cputop_pwrctrl_for_cluster_off(unsigned long mpidr)
+{
+	unsigned long cluster_id = mpidr & MPIDR_CLUSTER_MASK;
+	unsigned long cpu_id = mpidr & MPIDR_CPU_MASK;
+	unsigned int pwr_status, shift, i, flag = 0;
+
+	pwr_status = mmio_read_32(SPM_PWR_STATUS) |
+				 mmio_read_32(SPM_PWR_STATUS_2ND);
+
+	if (cluster_id) {
+		for (i = 0; i < PLATFORM_CLUSTER1_CORE_COUNT; i++) {
+			if (i == cpu_id)
+				continue;
+			shift = i + PCM_MCDI_CA72_PWRSTA_SHIFT;
+			flag |= (pwr_status & (1 << shift)) >> shift;
+		}
+		if (!flag)
+			mmio_setbits_32(SPM_PCM_RESERVE,
+					PCM_MCDI_CA72_CPUTOP_PWRCTL);
+	} else {
+		for (i = 0; i < PLATFORM_CLUSTER0_CORE_COUNT; i++) {
+			if (i == cpu_id)
+				continue;
+			shift = i + PCM_MCDI_CA53_PWRSTA_SHIFT;
+			flag |= (pwr_status & (1 << shift)) >> shift;
+		}
+		if (!flag)
+			mmio_setbits_32(SPM_PCM_RESERVE,
+					PCM_MCDI_CA53_CPUTOP_PWRCTL);
+	}
+}
+
+static void spm_mcdi_clear_cputop_pwrctrl_for_cluster_on(unsigned long mpidr)
+{
+	unsigned long cluster_id = mpidr & MPIDR_CLUSTER_MASK;
+
+	if (cluster_id)
+		mmio_clrbits_32(SPM_PCM_RESERVE,
+				PCM_MCDI_CA72_CPUTOP_PWRCTL);
+	else
+		mmio_clrbits_32(SPM_PCM_RESERVE,
+				PCM_MCDI_CA53_CPUTOP_PWRCTL);
+}
+
+void spm_mcdi_prepare_for_mtcmos(void)
+{
+	const struct pcm_desc *pcmdesc = spm_mcdi.pcmdesc;
+	struct pwr_ctrl *pwrctrl = spm_mcdi.pwrctrl;
+
+	if (is_mcdi_ready() == 0) {
+		if (is_hotplug_ready() == 1)
+			spm_clear_hotplug();
+		set_pwrctrl_pcm_flags(pwrctrl, 0);
+		spm_reset_and_init_pcm();
+		spm_kick_im_to_fetch(pcmdesc);
+		spm_set_power_control(pwrctrl);
+		spm_set_wakeup_event(pwrctrl);
+		spm_kick_pcm_to_run(pwrctrl);
+		set_mcdi_ready();
 	}
 }
 
-void spm_mcdi_prepare(unsigned long mpidr)
+void spm_mcdi_prepare_for_off_state(unsigned long mpidr, unsigned int afflvl)
 {
 	const struct pcm_desc *pcmdesc = spm_mcdi.pcmdesc;
 	struct pwr_ctrl *pwrctrl = spm_mcdi.pwrctrl;
@@ -421,14 +507,17 @@
 		set_mcdi_ready();
 	}
 	spm_mcdi_wfi_sel_enter(mpidr);
+	if (afflvl == MPIDR_AFFLVL1)
+		spm_mcdi_set_cputop_pwrctrl_for_cluster_off(mpidr);
 	spm_lock_release();
 }
 
-void spm_mcdi_finish(unsigned long mpidr)
+void spm_mcdi_finish_for_on_state(unsigned long mpidr, unsigned int afflvl)
 {
 	unsigned long linear_id = platform_get_core_pos(mpidr);
 
 	spm_lock_get();
+	spm_mcdi_clear_cputop_pwrctrl_for_cluster_on(mpidr);
 	spm_mcdi_wfi_sel_leave(mpidr);
 	mmio_write_32(SPM_PCM_SW_INT_CLEAR, (0x1 << linear_id));
 	spm_lock_release();
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h
index 4df71d6..4d1f990 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h
+++ b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h
@@ -31,9 +31,8 @@
 #define __SPM_MCDI_H__
 
 void spm_mcdi_wakeup_all_cores(void);
-void spm_mcdi_wfi_sel_enter(unsigned long mpidr);
-void spm_mcdi_wfi_sel_leave(unsigned long mpidr);
-void spm_mcdi_prepare(unsigned long mpidr);
-void spm_mcdi_finish(unsigned long mpidr);
+void spm_mcdi_prepare_for_mtcmos(void);
+void spm_mcdi_prepare_for_off_state(unsigned long mpidr, unsigned int afflvl);
+void spm_mcdi_finish_for_on_state(unsigned long mpidr, unsigned int afflvl);
 
 #endif /* __SPM_MCDI_H__ */
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_suspend.c b/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
index b4d0a60..6bf37f3 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
@@ -61,81 +61,81 @@
  **********************************************************/
 static const unsigned int suspend_binary_ca7[] = {
 	0x81f58407, 0x81f68407, 0x803a0400, 0x803a8400, 0x1b80001f, 0x20000000,
-	0x80300400, 0x80318400, 0x80328400, 0xa1d28407, 0x81f20407, 0x81409801,
-	0xd8000245, 0x17c07c1f, 0x18c0001f, 0x10006234, 0xc0c031a0, 0x1200041f,
+	0x80300400, 0x80318400, 0x80328400, 0xa1d28407, 0x81f20407, 0x81009801,
+	0xd8000244, 0x17c07c1f, 0x18c0001f, 0x10006234, 0xc0c032e0, 0x1200041f,
 	0x80310400, 0x1b80001f, 0x2000000a, 0xa0110400, 0x18c0001f, 0x100062c8,
 	0xe0e00010, 0xe0e00030, 0xe0e00070, 0xe0e000f0, 0x1b80001f, 0x2000001a,
 	0xe0e00ff0, 0xe8208000, 0x10006354, 0xfffe7fff, 0xe8208000, 0x10006834,
-	0x00000010, 0x81f00407, 0xa1dd0407, 0x81fd0407, 0xc2803780, 0x1290041f,
-	0x8880000c, 0x2f7be75f, 0xd8200642, 0x17c07c1f, 0x1b00001f, 0x7fffe7ff,
-	0xd0000680, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xf0000000, 0x17c07c1f,
-	0x80880001, 0xd8000762, 0x17c07c1f, 0xd00027a0, 0x1200041f, 0xe8208000,
-	0x10006834, 0x00000000, 0x1b00001f, 0x3fffe7ff, 0x1b80001f, 0x20000004,
-	0xd820092c, 0x17c07c1f, 0xe8208000, 0x10006834, 0x00000010, 0xd00011a0,
-	0x17c07c1f, 0x18c0001f, 0x10006608, 0x1910001f, 0x10006608, 0x813b0404,
-	0xe0c00004, 0x1880001f, 0x10006320, 0xc0c03680, 0xe080000f, 0xd8200b23,
-	0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xd00011a0, 0x17c07c1f, 0xe080001f,
-	0xe8208000, 0x10006354, 0xffffffff, 0x18c0001f, 0x100062c8, 0xe0e000f0,
-	0xe0e00030, 0xe0e00000, 0x81409801, 0xd8000fe5, 0x17c07c1f, 0x18c0001f,
-	0x10004094, 0x1910001f, 0x1020e374, 0xe0c00004, 0x18c0001f, 0x10004098,
-	0x1910001f, 0x1020e378, 0xe0c00004, 0x18c0001f, 0x10011094, 0x1910001f,
-	0x10213374, 0xe0c00004, 0x18c0001f, 0x10011098, 0x1910001f, 0x10213378,
-	0xe0c00004, 0x1910001f, 0x10213378, 0x18c0001f, 0x10006234, 0xc0c03360,
-	0x17c07c1f, 0xc2803780, 0x1290841f, 0xa1d20407, 0x81f28407, 0xa1d68407,
-	0xa0128400, 0xa0118400, 0xa0100400, 0xa01a8400, 0xa01a0400, 0x19c0001f,
-	0x001c239f, 0x1b00001f, 0x3fffefff, 0xf0000000, 0x17c07c1f, 0x808d8001,
-	0xd8201422, 0x17c07c1f, 0x803d8400, 0x1b80001f, 0x2000001a, 0x80340400,
-	0x17c07c1f, 0x17c07c1f, 0x80310400, 0x81fa0407, 0x81f18407, 0x81f08407,
-	0xa1dc0407, 0x1b80001f, 0x200000b6, 0xd00020e0, 0x17c07c1f, 0x1880001f,
-	0x20000208, 0x81411801, 0xd8001605, 0x17c07c1f, 0xe8208000, 0x1000f600,
-	0xd2000000, 0x1380081f, 0x18c0001f, 0x10006240, 0xe0e00016, 0xe0e0001e,
-	0xe0e0000e, 0xe0e0000f, 0x80368400, 0x1380081f, 0x80370400, 0x1380081f,
-	0x80360400, 0x803e0400, 0x1380081f, 0x80380400, 0x803b0400, 0xa01d8400,
-	0x1b80001f, 0x20000034, 0x803d8400, 0x1b80001f, 0x20000152, 0x803d0400,
-	0x1380081f, 0x18c0001f, 0x1000f5c8, 0x1910001f, 0x1000f5c8, 0xa1000404,
-	0xe0c00004, 0x18c0001f, 0x100125c8, 0x1910001f, 0x100125c8, 0xa1000404,
-	0xe0c00004, 0x1910001f, 0x100125c8, 0x80340400, 0x17c07c1f, 0x17c07c1f,
-	0x80310400, 0xe8208000, 0x10000044, 0x00000100, 0x1b80001f, 0x20000068,
-	0x1b80001f, 0x2000000a, 0x18c0001f, 0x10006240, 0xe0e0000d, 0xd8001e65,
-	0x17c07c1f, 0x18c0001f, 0x100040f4, 0x1910001f, 0x100040f4, 0xa11c8404,
-	0xe0c00004, 0x1b80001f, 0x2000000a, 0x813c8404, 0xe0c00004, 0x18c0001f,
-	0x100110f4, 0x1910001f, 0x100110f4, 0xa11c8404, 0xe0c00004, 0x1b80001f,
-	0x2000000a, 0x813c8404, 0xe0c00004, 0x1b80001f, 0x20000100, 0x81fa0407,
-	0x81f18407, 0x81f08407, 0xe8208000, 0x10006354, 0xfffe7b47, 0x18c0001f,
-	0x65930003, 0xc0c03080, 0x17c07c1f, 0xa1d80407, 0xa1dc0407, 0x18c0001f,
-	0x10006608, 0x1910001f, 0x10006608, 0xa11b0404, 0xe0c00004, 0xc2803780,
-	0x1291041f, 0x8880000c, 0x2f7be75f, 0xd8202222, 0x17c07c1f, 0x1b00001f,
-	0x3fffe7ff, 0xd0002260, 0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xf0000000,
-	0x17c07c1f, 0x1890001f, 0x10006608, 0x808b0801, 0xd8202502, 0x17c07c1f,
-	0x1880001f, 0x10006320, 0xc0c03400, 0xe080000f, 0xd8002663, 0x17c07c1f,
-	0xe080001f, 0xa1da0407, 0x81fc0407, 0xa0110400, 0xa0140400, 0xa01d8400,
-	0xd0002fc0, 0x17c07c1f, 0x1b80001f, 0x20000fdf, 0x1890001f, 0x10006608,
-	0x80c98801, 0x810a8801, 0x10918c1f, 0xa0939002, 0x8080080d, 0xd82027a2,
-	0x12007c1f, 0x1b00001f, 0x3fffe7ff, 0x1b80001f, 0x20000004, 0xd800304c,
-	0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xd0003040, 0x17c07c1f, 0x81f80407,
-	0x81fc0407, 0x18c0001f, 0x65930006, 0xc0c03080, 0x17c07c1f, 0x18c0001f,
-	0x65930007, 0xc0c03080, 0x17c07c1f, 0x1880001f, 0x10006320, 0xc0c03400,
-	0xe080000f, 0xd8002663, 0x17c07c1f, 0xe080001f, 0x18c0001f, 0x65930005,
-	0xc0c03080, 0x17c07c1f, 0xa1da0407, 0xe8208000, 0x10000048, 0x00000100,
-	0x1b80001f, 0x20000068, 0xa0110400, 0xa0140400, 0x18c0001f, 0x1000f5c8,
-	0x1910001f, 0x1000f5c8, 0x81200404, 0xe0c00004, 0x18c0001f, 0x100125c8,
-	0x1910001f, 0x100125c8, 0x81200404, 0xe0c00004, 0x1910001f, 0x100125c8,
-	0xa01d0400, 0xa01b0400, 0xa0180400, 0x803d8400, 0xa01e0400, 0xa0160400,
-	0xa0170400, 0xa0168400, 0x1b80001f, 0x20000104, 0x81411801, 0xd8002f85,
-	0x17c07c1f, 0x18c0001f, 0x10006240, 0xc0c03360, 0x17c07c1f, 0xe8208000,
-	0x1000f600, 0xd2000001, 0xd8000768, 0x17c07c1f, 0xc2803780, 0x1291841f,
-	0x1b00001f, 0x7ffff7ff, 0xf0000000, 0x17c07c1f, 0x1900001f, 0x10006830,
-	0xe1000003, 0x18c0001f, 0x10006834, 0xe0e00000, 0xe0e00001, 0xf0000000,
-	0x17c07c1f, 0xe0f07f16, 0x1380201f, 0xe0f07f1e, 0x1380201f, 0xe0f07f0e,
-	0x1b80001f, 0x20000104, 0xe0f07f0c, 0xe0f07f0d, 0xe0f07e0d, 0xe0f07c0d,
-	0xe0f0780d, 0xf0000000, 0xe0f0700d, 0xe0f07f0d, 0xe0f07f0f, 0xe0f07f1e,
-	0xf0000000, 0xe0f07f12, 0x11407c1f, 0x81f08407, 0x81f18407, 0x1b80001f,
-	0x20000001, 0xa1d08407, 0xa1d18407, 0x1392841f, 0x812ab401, 0x80ebb401,
-	0xa0c00c04, 0xd8203603, 0x17c07c1f, 0x80c01403, 0xd8203423, 0x01400405,
-	0x1900001f, 0x10006814, 0xf0000000, 0xe1000003, 0xa1d00407, 0x1b80001f,
-	0x20000208, 0x80ea3401, 0x1a00001f, 0x10006814, 0xf0000000, 0xe2000003,
-	0x18c0001f, 0x10006b6c, 0x1910001f, 0x10006b6c, 0xa1002804, 0xf0000000,
-	0xe0c00004, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
+	0x00000010, 0x81f00407, 0xa1dd0407, 0x81fd0407, 0xc2803800, 0x1290041f,
+	0x8880000c, 0x2f7be75f, 0xd8200722, 0x17c07c1f, 0xd82006a9, 0x17c07c1f,
+	0xe8208000, 0x10006814, 0x00000001, 0xc2803800, 0x1293841f, 0x1b00001f,
+	0x7fffe7ff, 0xd0000760, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xf0000000,
+	0x17c07c1f, 0x80880001, 0xd8000842, 0x17c07c1f, 0xd00028e0, 0x1200041f,
+	0xe8208000, 0x10006834, 0x00000000, 0x1b00001f, 0x3fffe7ff, 0x1b80001f,
+	0x20000004, 0xd8200a0c, 0x17c07c1f, 0xe8208000, 0x10006834, 0x00000010,
+	0xd0001280, 0x17c07c1f, 0x18c0001f, 0x10006608, 0x1910001f, 0x10006608,
+	0x813b0404, 0xe0c00004, 0x1880001f, 0x10006320, 0xc0c03760, 0xe080000f,
+	0xd8200c03, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xd0001280, 0x17c07c1f,
+	0xe080001f, 0xe8208000, 0x10006354, 0xffffffff, 0x18c0001f, 0x100062c8,
+	0xe0e000f0, 0xe0e00030, 0xe0e00000, 0x81009801, 0xd80010c4, 0x17c07c1f,
+	0x18c0001f, 0x10004094, 0x1910001f, 0x1020e374, 0xe0c00004, 0x18c0001f,
+	0x10004098, 0x1910001f, 0x1020e378, 0xe0c00004, 0x18c0001f, 0x10011094,
+	0x1910001f, 0x10213374, 0xe0c00004, 0x18c0001f, 0x10011098, 0x1910001f,
+	0x10213378, 0xe0c00004, 0x1910001f, 0x10213378, 0x18c0001f, 0x10006234,
+	0xc0c034a0, 0x17c07c1f, 0xc2803800, 0x1290841f, 0xa1d20407, 0x81f28407,
+	0xa1d68407, 0xa0128400, 0xa0118400, 0xa0100400, 0xa01a8400, 0xa01a0400,
+	0x19c0001f, 0x001c239f, 0x1b00001f, 0x3fffefff, 0xf0000000, 0x17c07c1f,
+	0x808d8001, 0xd8201502, 0x17c07c1f, 0x803d8400, 0x1b80001f, 0x2000001a,
+	0x80340400, 0x17c07c1f, 0x17c07c1f, 0x80310400, 0x81fa0407, 0x81f18407,
+	0x81f08407, 0xa1dc0407, 0x1b80001f, 0x200000b6, 0xd0002220, 0x17c07c1f,
+	0x1880001f, 0x20000208, 0x81011801, 0xd80016e4, 0x17c07c1f, 0xe8208000,
+	0x1000f600, 0xd2000000, 0x1380081f, 0x18c0001f, 0x10006240, 0xe0e00016,
+	0xe0e0001e, 0xe0e0000e, 0xe0e0000f, 0x80368400, 0x1380081f, 0x80370400,
+	0x1380081f, 0x80360400, 0x803e0400, 0x1380081f, 0x80380400, 0x803b0400,
+	0xa01d8400, 0x1b80001f, 0x20000034, 0x803d8400, 0x1b80001f, 0x20000152,
+	0x803d0400, 0x1380081f, 0x18c0001f, 0x1000f5c8, 0x1910001f, 0x1000f5c8,
+	0xa1000404, 0xe0c00004, 0x18c0001f, 0x100125c8, 0x1910001f, 0x100125c8,
+	0xa1000404, 0xe0c00004, 0x1910001f, 0x100125c8, 0x80340400, 0x17c07c1f,
+	0x17c07c1f, 0x80310400, 0xe8208000, 0x10000044, 0x00000100, 0x1b80001f,
+	0x20000068, 0x1b80001f, 0x2000000a, 0x18c0001f, 0x10006240, 0xe0e0000d,
+	0x81011801, 0xd8001f64, 0x17c07c1f, 0x18c0001f, 0x100040f4, 0x1910001f,
+	0x100040f4, 0xa11c8404, 0xe0c00004, 0x1b80001f, 0x2000000a, 0x813c8404,
+	0xe0c00004, 0x18c0001f, 0x100110f4, 0x1910001f, 0x100110f4, 0xa11c8404,
+	0xe0c00004, 0x1b80001f, 0x2000000a, 0x813c8404, 0xe0c00004, 0x1b80001f,
+	0x20000100, 0x81fa0407, 0x81f18407, 0x81f08407, 0xe8208000, 0x10006354,
+	0xfffe7b47, 0x18c0001f, 0x65930003, 0xc0c031c0, 0x17c07c1f, 0xc2803800,
+	0x1293041f, 0xa1d80407, 0xa1dc0407, 0x18c0001f, 0x10006608, 0x1910001f,
+	0x10006608, 0xa11b0404, 0xe0c00004, 0xc2803800, 0x1291041f, 0x8880000c,
+	0x2f7be75f, 0xd8202362, 0x17c07c1f, 0x1b00001f, 0x3fffe7ff, 0xd00023a0,
+	0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xf0000000, 0x17c07c1f, 0x1890001f,
+	0x10006608, 0x808b0801, 0xd8202642, 0x17c07c1f, 0x1880001f, 0x10006320,
+	0xc0c03540, 0xe080000f, 0xd80027a3, 0x17c07c1f, 0xe080001f, 0xa1da0407,
+	0x81fc0407, 0xa0110400, 0xa0140400, 0xa01d8400, 0xd0003100, 0x17c07c1f,
+	0x1b80001f, 0x20000fdf, 0x1890001f, 0x10006608, 0x80c98801, 0x810a8801,
+	0x10918c1f, 0xa0939002, 0x8080080d, 0xd82028e2, 0x12007c1f, 0x1b00001f,
+	0x3fffe7ff, 0x1b80001f, 0x20000004, 0xd800318c, 0x17c07c1f, 0x1b00001f,
+	0xbfffe7ff, 0xd0003180, 0x17c07c1f, 0x81f80407, 0x81fc0407, 0x18c0001f,
+	0x65930006, 0xc0c031c0, 0x17c07c1f, 0x18c0001f, 0x65930007, 0xc0c031c0,
+	0x17c07c1f, 0x1880001f, 0x10006320, 0xc0c03540, 0xe080000f, 0xd80027a3,
+	0x17c07c1f, 0xe080001f, 0x18c0001f, 0x65930005, 0xc0c031c0, 0x17c07c1f,
+	0xa1da0407, 0xe8208000, 0x10000048, 0x00000100, 0x1b80001f, 0x20000068,
+	0xa0110400, 0xa0140400, 0x18c0001f, 0x1000f5c8, 0x1910001f, 0x1000f5c8,
+	0x81200404, 0xe0c00004, 0x18c0001f, 0x100125c8, 0x1910001f, 0x100125c8,
+	0x81200404, 0xe0c00004, 0x1910001f, 0x100125c8, 0xa01d0400, 0xa01b0400,
+	0xa0180400, 0x803d8400, 0xa01e0400, 0xa0160400, 0xa0170400, 0xa0168400,
+	0x1b80001f, 0x20000104, 0x81011801, 0xd80030c4, 0x17c07c1f, 0x18c0001f,
+	0x10006240, 0xc0c034a0, 0x17c07c1f, 0xe8208000, 0x1000f600, 0xd2000001,
+	0xd8000848, 0x17c07c1f, 0xc2803800, 0x1291841f, 0x1b00001f, 0x7ffff7ff,
+	0xf0000000, 0x17c07c1f, 0x1900001f, 0x10006830, 0xe1000003, 0x18c0001f,
+	0x10006834, 0xe0e00000, 0xe0e00001, 0xf0000000, 0x17c07c1f, 0xe0f07f16,
+	0x1380201f, 0xe0f07f1e, 0x1380201f, 0xe0f07f0e, 0x1b80001f, 0x20000104,
+	0xe0f07f0c, 0xe0f07f0d, 0xe0f07e0d, 0xe0f07c0d, 0xe0f0780d, 0xf0000000,
+	0xe0f0700d, 0xe0f07f0d, 0xe0f07f0f, 0xe0f07f1e, 0xf0000000, 0xe0f07f12,
+	0x11407c1f, 0x81f08407, 0x81f18407, 0x1b80001f, 0x20000001, 0xa1d08407,
+	0xa1d18407, 0x1392841f, 0x812ab401, 0x80ebb401, 0xa0c00c04, 0xd8203743,
+	0x17c07c1f, 0x80c01403, 0xd8203563, 0x01400405, 0xf0000000, 0xa1d00407,
+	0x1b80001f, 0x20000208, 0x80ea3401, 0xf0000000, 0x18c0001f, 0x10006b6c,
+	0x1910001f, 0x10006b6c, 0xa1002804, 0xf0000000, 0xe0c00004, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
@@ -148,75 +148,78 @@
 	0x17c07c1f, 0x17c07c1f, 0x1840001f, 0x00000001, 0xa1d48407, 0x1990001f,
 	0x10006b08, 0x1a50001f, 0x10006610, 0x8246a401, 0xe8208000, 0x10006b6c,
 	0x00000000, 0x1b00001f, 0x2f7be75f, 0x81469801, 0xd8004305, 0x17c07c1f,
-	0x1b80001f, 0xd00f0000, 0x8880000c, 0x2f7be75f, 0xd8005da2, 0x17c07c1f,
+	0x1b80001f, 0xd00f0000, 0x8880000c, 0x2f7be75f, 0xd8005fa2, 0x17c07c1f,
 	0xd0004340, 0x17c07c1f, 0x1b80001f, 0x500f0000, 0xe8208000, 0x10006354,
-	0xfffe7b47, 0xc0c06940, 0x81401801, 0xd80048e5, 0x17c07c1f, 0x81f60407,
-	0x18c0001f, 0x10006200, 0xc0c05e60, 0x12807c1f, 0xe8208000, 0x1000625c,
-	0x00000001, 0x1b80001f, 0x20000080, 0xc0c05e60, 0x1280041f, 0x18c0001f,
-	0x10006204, 0xc0c06200, 0x1280041f, 0x18c0001f, 0x10006208, 0xc0c05e60,
+	0xfffe7b47, 0xc0c06c00, 0x81401801, 0xd80048e5, 0x17c07c1f, 0x81f60407,
+	0x18c0001f, 0x10006200, 0xc0c06060, 0x12807c1f, 0xe8208000, 0x1000625c,
+	0x00000001, 0x1b80001f, 0x20000080, 0xc0c06060, 0x1280041f, 0x18c0001f,
+	0x10006204, 0xc0c06400, 0x1280041f, 0x18c0001f, 0x10006208, 0xc0c06060,
 	0x12807c1f, 0xe8208000, 0x10006244, 0x00000001, 0x1b80001f, 0x20000080,
-	0xc0c05e60, 0x1280041f, 0x18d0001f, 0x10200200, 0x18c0001f, 0x10006290,
-	0xc0c05e60, 0x1280041f, 0xe8208000, 0x10006404, 0x00003101, 0xc2803780,
+	0xc0c06060, 0x1280041f, 0x18d0001f, 0x10200200, 0x18c0001f, 0x10006290,
+	0xc0c06060, 0x1280041f, 0xe8208000, 0x10006404, 0x00003101, 0xc2803800,
 	0x1292041f, 0x81469801, 0xd8204a45, 0x17c07c1f, 0x1b00001f, 0x2f7be75f,
-	0x1b80001f, 0x30000004, 0x8880000c, 0x2f7be75f, 0xd8005802, 0x17c07c1f,
-	0xc0c064c0, 0x17c07c1f, 0x18c0001f, 0x10006294, 0xe0f07fff, 0xe0e00fff,
+	0x1b80001f, 0x30000004, 0x8880000c, 0x2f7be75f, 0xd8005a02, 0x17c07c1f,
+	0xc0c06780, 0x17c07c1f, 0x18c0001f, 0x10006294, 0xe0f07fff, 0xe0e00fff,
 	0xe0e000ff, 0x81449801, 0xd8004c85, 0x17c07c1f, 0x1a00001f, 0x10006604,
-	0xe2200003, 0xc0c06580, 0x17c07c1f, 0xe2200005, 0xc0c06580, 0x17c07c1f,
+	0xe2200003, 0xc0c06840, 0x17c07c1f, 0xe2200005, 0xc0c06840, 0x17c07c1f,
 	0xa1d38407, 0xa1d98407, 0x1800001f, 0x00000012, 0x1800001f, 0x00000e12,
 	0x1800001f, 0x03800e12, 0x1800001f, 0x038e0e12, 0xe8208000, 0x10006310,
-	0x0b1600f8, 0x1b00001f, 0xbfffe7ff, 0x1b80001f, 0x90100000, 0x80c00400,
-	0xd8204fa3, 0xa1d58407, 0xa1dd8407, 0x1b00001f, 0x3fffefff, 0xd0004e60,
-	0x17c07c1f, 0x1890001f, 0x100063e8, 0x88c0000c, 0x2f7be75f, 0xd80051c3,
-	0x17c07c1f, 0x80c40001, 0xd8005143, 0x17c07c1f, 0x1b00001f, 0xbfffe7ff,
-	0xd0005180, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xd0004e60, 0x17c07c1f,
-	0x80c40001, 0xd82052c3, 0x17c07c1f, 0xa1de0407, 0x1b00001f, 0x7fffe7ff,
-	0xd0004e60, 0x17c07c1f, 0x18c0001f, 0x10006294, 0xe0e001fe, 0xe0e003fc,
-	0xe0e007f8, 0xe0e00ff0, 0x1b80001f, 0x20000020, 0xe0f07ff0, 0xe0f07f00,
-	0x81449801, 0xd80055a5, 0x17c07c1f, 0x1a00001f, 0x10006604, 0xe2200002,
-	0xc0c06580, 0x17c07c1f, 0xe2200004, 0xc0c06580, 0x17c07c1f, 0x1b80001f,
-	0x200016a8, 0x1800001f, 0x03800e12, 0x1b80001f, 0x20000300, 0x1800001f,
-	0x00000e12, 0x1b80001f, 0x20000300, 0x1800001f, 0x00000012, 0x1b80001f,
-	0x20000104, 0x10007c1f, 0x81f38407, 0x81f98407, 0x81f90407, 0x81f40407,
-	0x1b80001f, 0x200016a8, 0x81401801, 0xd8005da5, 0x17c07c1f, 0xe8208000,
-	0x10006404, 0x00002101, 0x18c0001f, 0x10006290, 0x1212841f, 0xc0c05fe0,
-	0x12807c1f, 0xc0c05fe0, 0x1280041f, 0x18c0001f, 0x10006208, 0x1212841f,
-	0xc0c05fe0, 0x12807c1f, 0xe8208000, 0x10006244, 0x00000000, 0x1b80001f,
-	0x20000080, 0xc0c05fe0, 0x1280041f, 0xe8208000, 0x10200268, 0x000ffffe,
-	0x18c0001f, 0x10006204, 0x1212841f, 0xc0c06340, 0x1280041f, 0x18c0001f,
-	0x10006200, 0x1212841f, 0xc0c05fe0, 0x12807c1f, 0xe8208000, 0x1000625c,
-	0x00000000, 0x1b80001f, 0x20000080, 0xc0c05fe0, 0x1280041f, 0x19c0001f,
-	0x01411820, 0x1ac0001f, 0x55aa55aa, 0x10007c1f, 0xf0000000, 0xd8005f0a,
-	0x17c07c1f, 0xe2e0004f, 0xe2e0006f, 0xe2e0002f, 0xd8205faa, 0x17c07c1f,
-	0xe2e0002e, 0xe2e0003e, 0xe2e00032, 0xf0000000, 0x17c07c1f, 0xd80060aa,
-	0x17c07c1f, 0xe2e00036, 0xe2e0003e, 0x1380201f, 0xe2e0003c, 0xd82061ca,
-	0x17c07c1f, 0x1380201f, 0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c,
-	0xe2e0004c, 0xe2e0004d, 0xf0000000, 0x17c07c1f, 0xd8206309, 0x17c07c1f,
-	0xe2e0000d, 0xe2e0000c, 0xe2e0001c, 0xe2e0001e, 0xe2e00016, 0xe2e00012,
-	0xf0000000, 0x17c07c1f, 0xd8206489, 0x17c07c1f, 0xe2e00016, 0x1380201f,
-	0xe2e0001e, 0x1380201f, 0xe2e0001c, 0x1380201f, 0xe2e0000c, 0xe2e0000d,
-	0xf0000000, 0x17c07c1f, 0xa1d40407, 0x1391841f, 0xa1d90407, 0x1393041f,
-	0xf0000000, 0x17c07c1f, 0x18d0001f, 0x10006604, 0x10cf8c1f, 0xd8206583,
-	0x17c07c1f, 0xf0000000, 0x17c07c1f, 0xe8208000, 0x11008014, 0x00000002,
-	0xe8208000, 0x11008020, 0x00000101, 0xe8208000, 0x11008004, 0x000000d0,
-	0x1a00001f, 0x11008000, 0xd800684a, 0xe220005d, 0xd820686a, 0xe2200000,
-	0xe2200001, 0xe8208000, 0x11008024, 0x00000001, 0x1b80001f, 0x20000424,
-	0xf0000000, 0x17c07c1f, 0xa1d10407, 0x1b80001f, 0x20000020, 0xf0000000,
-	0x17c07c1f
+	0x0b1600f8, 0x1940001f, 0x00000000, 0x12407c1f, 0x1b00001f, 0xbfffe7ff,
+	0x1b80001f, 0x90100000, 0x17c07c1f, 0xd8004fc5, 0x17c07c1f, 0x8247b001,
+	0x1940001f, 0xffffffff, 0x80c00400, 0xd82050c3, 0xa1d58407, 0xa1dd8407,
+	0x1b00001f, 0x3fffefff, 0xd0004ec0, 0x17c07c1f, 0x1890001f, 0x100063e8,
+	0x88c0000c, 0x2f7be75f, 0xd80052e3, 0x17c07c1f, 0x80c40001, 0xd8005263,
+	0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xd00052a0, 0x17c07c1f, 0x1b00001f,
+	0x7ffff7ff, 0xd0004ec0, 0x17c07c1f, 0x80c40001, 0xd82053e3, 0x17c07c1f,
+	0xa1de0407, 0x1b00001f, 0x7fffe7ff, 0xd0004ec0, 0x17c07c1f, 0xe8208000,
+	0x10006814, 0x00000000, 0x18c0001f, 0x10006b00, 0xe0e00000, 0xe0c00009,
+	0x18c0001f, 0x10006294, 0xe0e001fe, 0xe0e003fc, 0xe0e007f8, 0xe0e00ff0,
+	0x1b80001f, 0x20000020, 0xe0f07ff0, 0xe0f07f00, 0x81449801, 0xd80057a5,
+	0x17c07c1f, 0x1a00001f, 0x10006604, 0xe2200002, 0xc0c06840, 0x17c07c1f,
+	0xe2200004, 0xc0c06840, 0x17c07c1f, 0x1b80001f, 0x200016a8, 0x1800001f,
+	0x03800e12, 0x1b80001f, 0x20000300, 0x1800001f, 0x00000e12, 0x1b80001f,
+	0x20000300, 0x1800001f, 0x00000012, 0x1b80001f, 0x20000104, 0x10007c1f,
+	0x81f38407, 0x81f98407, 0x81f90407, 0x81f40407, 0x1b80001f, 0x200016a8,
+	0x81401801, 0xd8005fa5, 0x17c07c1f, 0xe8208000, 0x10006404, 0x00002101,
+	0x18c0001f, 0x10006290, 0x1212841f, 0xc0c061e0, 0x12807c1f, 0xc0c061e0,
+	0x1280041f, 0x18c0001f, 0x10006208, 0x1212841f, 0xc0c061e0, 0x12807c1f,
+	0xe8208000, 0x10006244, 0x00000000, 0x1b80001f, 0x20000080, 0xc0c061e0,
+	0x1280041f, 0xe8208000, 0x10200268, 0x000ffffe, 0x18c0001f, 0x10006204,
+	0x1212841f, 0xc0c065a0, 0x1280041f, 0x18c0001f, 0x10006200, 0x1212841f,
+	0xc0c061e0, 0x12807c1f, 0xe8208000, 0x1000625c, 0x00000000, 0x1b80001f,
+	0x20000080, 0xc0c061e0, 0x1280041f, 0x19c0001f, 0x01411820, 0x1ac0001f,
+	0x55aa55aa, 0x10007c1f, 0xf0000000, 0xd800610a, 0x17c07c1f, 0xe2e0004f,
+	0xe2e0006f, 0xe2e0002f, 0xd82061aa, 0x17c07c1f, 0xe2e0002e, 0xe2e0003e,
+	0xe2e00032, 0xf0000000, 0x17c07c1f, 0xd80062aa, 0x17c07c1f, 0xe2e00036,
+	0xe2e0003e, 0x1380201f, 0xe2e0003c, 0xd82063ca, 0x17c07c1f, 0x1380201f,
+	0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c, 0xe2e0004c, 0xe2e0004d,
+	0xf0000000, 0x17c07c1f, 0x1a50001f, 0x10006610, 0x8246a401, 0xd8206569,
+	0x17c07c1f, 0xe2e0000d, 0xe2e0000c, 0xe2e0001c, 0xe2e0001e, 0xe2e00016,
+	0xe2e00012, 0xf0000000, 0x17c07c1f, 0x1a50001f, 0x10006610, 0x8246a401,
+	0xd8206749, 0x17c07c1f, 0xe2e00016, 0x1380201f, 0xe2e0001e, 0x1380201f,
+	0xe2e0001c, 0x1380201f, 0xe2e0000c, 0xe2e0000d, 0xf0000000, 0x17c07c1f,
+	0xa1d40407, 0x1391841f, 0xa1d90407, 0x1393041f, 0xf0000000, 0x17c07c1f,
+	0x18d0001f, 0x10006604, 0x10cf8c1f, 0xd8206843, 0x17c07c1f, 0xf0000000,
+	0x17c07c1f, 0xe8208000, 0x11008014, 0x00000002, 0xe8208000, 0x11008020,
+	0x00000101, 0xe8208000, 0x11008004, 0x000000d0, 0x1a00001f, 0x11008000,
+	0xd8006b0a, 0xe220005d, 0xd8206b2a, 0xe2200000, 0xe2200001, 0xe8208000,
+	0x11008024, 0x00000001, 0x1b80001f, 0x20000424, 0xf0000000, 0x17c07c1f,
+	0xa1d10407, 0x1b80001f, 0x20000020, 0xf0000000, 0x17c07c1f
 };
 
 /*
  * PCM binary for suspend scenario
  */
 static const struct pcm_desc suspend_pcm_ca7 = {
-	.version = "pcm_suspend_20150805_V1",
+	.version = "pcm_suspend_20150917_V4",
 	.base = suspend_binary_ca7,
-	.size = 847,
+	.size = 869,
 	.sess = 2,
 	.replace = 0,
 	.vec0 = EVENT_VEC(11, 1, 0, 0),
-	.vec1 = EVENT_VEC(12, 1, 0, 54),
-	.vec2 = EVENT_VEC(30, 1, 0, 143),
-	.vec3 = EVENT_VEC(31, 1, 0, 277),
+	.vec1 = EVENT_VEC(12, 1, 0, 61),
+	.vec2 = EVENT_VEC(30, 1, 0, 150),
+	.vec3 = EVENT_VEC(31, 1, 0, 287),
 };
 
 /*
diff --git a/plat/mediatek/mt8173/include/mcucfg.h b/plat/mediatek/mt8173/include/mcucfg.h
index d2474ac..7c837ba 100644
--- a/plat/mediatek/mt8173/include/mcucfg.h
+++ b/plat/mediatek/mt8173/include/mcucfg.h
@@ -176,4 +176,70 @@
 	MP1_L2RSTDISABLE = 1 << MP1_L2RSTDISABLE_SHIFT
 };
 
+/* cci clock control related */
+enum {
+	MCU_BUS_DCM_EN	= 1 << 8
+};
+
+/* l2c sram control related */
+enum {
+	L2C_SRAM_DCM_EN = 1 << 0
+};
+
+/* bus fabric dcm control related */
+enum {
+	PSYS_ADB400_DCM_EN		= 1 << 29,
+	GPU_ADB400_DCM_EN		= 1 << 28,
+
+	EMI1_ADB400_DCM_EN		= 1 << 27,
+	EMI_ADB400_DCM_EN		= 1 << 26,
+	INFRA_ADB400_DCM_EN		= 1 << 25,
+	L2C_ADB400_DCM_EN		= 1 << 24,
+
+	MP0_ADB400_DCM_EN		= 1 << 23,
+	CCI400_CK_ONLY_DCM_EN		= 1 << 22,
+	L2C_IDLE_DCM_EN			= 1 << 21,
+
+	CA15U_ADB_DYNAMIC_CG_EN		= 1 << 19,
+	CA7L_ADB_DYNAMIC_CG_EN		= 1 << 18,
+	L2C_ADB_DYNAMIC_CG_EN		= 1 << 17,
+
+	EMICLK_EMI1_DYNAMIC_CG_EN	= 1 << 12,
+
+	INFRACLK_PSYS_DYNAMIC_CG_EN	= 1 << 11,
+	EMICLK_GPU_DYNAMIC_CG_EN	= 1 << 10,
+	EMICLK_EMI_DYNAMIC_CG_EN	= 1 << 8,
+
+	CCI400_SLV_RW_DCM_EN		= 1 << 7,
+	CCI400_SLV_DCM_EN		= 1 << 5,
+
+	ACLK_PSYS_DYNAMIC_CG_EN		= 1 << 3,
+	ACLK_GPU_DYNAMIC_CG_EN		= 1 << 2,
+	ACLK_EMI_DYNAMIC_CG_EN		= 1 << 1,
+	ACLK_INFRA_DYNAMIC_CG_EN	= 1 << 0,
+
+	/* adb400 related */
+	ADB400_GRP_DCM_EN = PSYS_ADB400_DCM_EN | GPU_ADB400_DCM_EN |
+			    EMI1_ADB400_DCM_EN | EMI_ADB400_DCM_EN |
+			    INFRA_ADB400_DCM_EN | L2C_ADB400_DCM_EN |
+			    MP0_ADB400_DCM_EN,
+
+	/* cci400 related */
+	CCI400_GRP_DCM_EN = CCI400_CK_ONLY_DCM_EN | CCI400_SLV_RW_DCM_EN |
+			    CCI400_SLV_DCM_EN,
+
+	/* adb clock related */
+	ADBCLK_GRP_DCM_EN = CA15U_ADB_DYNAMIC_CG_EN | CA7L_ADB_DYNAMIC_CG_EN |
+			    L2C_ADB_DYNAMIC_CG_EN,
+
+	/* emi clock related */
+	EMICLK_GRP_DCM_EN = EMICLK_EMI1_DYNAMIC_CG_EN |
+			    EMICLK_GPU_DYNAMIC_CG_EN |
+			    EMICLK_EMI_DYNAMIC_CG_EN,
+
+	/* bus clock related */
+	ACLK_GRP_DCM_EN = ACLK_PSYS_DYNAMIC_CG_EN | ACLK_GPU_DYNAMIC_CG_EN |
+			  ACLK_EMI_DYNAMIC_CG_EN | ACLK_INFRA_DYNAMIC_CG_EN,
+};
+
 #endif  /* __MCUCFG_H__ */
diff --git a/plat/mediatek/mt8173/plat_pm.c b/plat/mediatek/mt8173/plat_pm.c
index a84d031..f28f8ed 100644
--- a/plat/mediatek/mt8173/plat_pm.c
+++ b/plat/mediatek/mt8173/plat_pm.c
@@ -380,8 +380,8 @@
 
 	mmio_write_32(rv, sec_entrypoint);
 
-	if (afflvl == MPIDR_AFFLVL0)
-		spm_mcdi_prepare(mpidr);
+	if (afflvl < MPIDR_AFFLVL2)
+		spm_mcdi_prepare_for_off_state(mpidr, afflvl);
 
 	if (afflvl >= MPIDR_AFFLVL0)
 		mt_platform_save_context(mpidr);
@@ -390,12 +390,10 @@
 	if (afflvl >= MPIDR_AFFLVL1) {
 		/* Disable coherency if this cluster is to be turned off */
 		plat_cci_disable();
-		disable_scu(mpidr);
-
-		trace_power_flow(mpidr, CLUSTER_SUSPEND);
 	}
 
 	if (afflvl >= MPIDR_AFFLVL2) {
+		disable_scu(mpidr);
 		generic_timer_backup();
 		spm_system_suspend();
 		/* Prevent interrupts from spuriously waking up this cpu */
@@ -420,8 +418,6 @@
 
 	/* Perform the common cluster specific operations */
 	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
 		/* Enable coherency if this cluster was off */
 		plat_cci_enable();
 		trace_power_flow(mpidr, CLUSTER_UP);
@@ -451,22 +447,20 @@
 		arm_gic_setup();
 		arm_gic_cpuif_setup();
 		spm_system_suspend_finish();
+		enable_scu(mpidr);
 	}
 
 	/* Perform the common cluster specific operations */
 	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
 		/* Enable coherency if this cluster was off */
 		plat_cci_enable();
-		trace_power_flow(mpidr, CLUSTER_UP);
 	}
 
 	if (afflvl >= MPIDR_AFFLVL0)
 		mt_platform_restore_context(mpidr);
 
-	if (afflvl == MPIDR_AFFLVL0)
-		spm_mcdi_finish(mpidr);
+	if (afflvl < MPIDR_AFFLVL2)
+		spm_mcdi_finish_for_on_state(mpidr, afflvl);
 
 	arm_gic_pcpu_distif_setup();
 }
diff --git a/plat/mediatek/mt8173/plat_sip_calls.c b/plat/mediatek/mt8173/plat_sip_calls.c
index 92deb60..e12892f 100644
--- a/plat/mediatek/mt8173/plat_sip_calls.c
+++ b/plat/mediatek/mt8173/plat_sip_calls.c
@@ -29,6 +29,7 @@
  */
 #include <mmio.h>
 #include <mtk_sip_svc.h>
+#include <mtcmos.h>
 
 /* Authorized secure register list */
 enum {
@@ -55,3 +56,30 @@
 
 	return MTK_SIP_E_INVALID_PARAM;
 }
+
+uint64_t mt_sip_pwr_on_mtcmos(uint32_t val)
+{
+	uint32_t ret;
+
+	ret = mtcmos_non_cpu_ctrl(1, val);
+	if (ret)
+		return MTK_SIP_E_INVALID_PARAM;
+	else
+		return MTK_SIP_E_SUCCESS;
+}
+
+uint64_t mt_sip_pwr_off_mtcmos(uint32_t val)
+{
+	uint32_t ret;
+
+	ret = mtcmos_non_cpu_ctrl(0, val);
+	if (ret)
+		return MTK_SIP_E_INVALID_PARAM;
+	else
+		return MTK_SIP_E_SUCCESS;
+}
+
+uint64_t mt_sip_pwr_mtcmos_support(void)
+{
+	return MTK_SIP_E_SUCCESS;
+}
diff --git a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
index a4caf5e..905c4c5 100644
--- a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
+++ b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
@@ -186,7 +186,7 @@
 	 * 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
+	 * Clobber list : x0 - x4
 	 * ---------------------------------------------
 	 */
 func plat_crash_console_init