plat: imx8mq: enable cpuidle support on iMX8MQ

Refact the PSCI  related code. Enable cpuidle support
on i.MX8MQ for CPU idle support.

Signed-off-by: Bai Ping <ping.bai@nxp.com>

6 files changed, 451 insertions, 366 deletions

diff --git a/plat/imx/common/include/fsl_sip.h b/plat/imx/common/include/fsl_sip.h
index 458e19f6..fbff22be 100644
--- a/plat/imx/common/include/fsl_sip.h
+++ b/plat/imx/common/include/fsl_sip.h
@@ -12,6 +12,7 @@
 #define FSL_SIP_CONFIG_GPC_UNMASK	0x01
 #define FSL_SIP_CONFIG_GPC_SET_WAKE	0x02
 #define FSL_SIP_CONFIG_GPC_PM_DOMAIN	0x03
+#define FSL_SIP_CONFIG_GPC_SET_AFF	0x04
 
 #define FSL_SIP_CPUFREQ			0xC2000001
 #define FSL_SIP_SET_CPUFREQ		0x00
diff --git a/plat/imx/imx8mq/ddrc.c b/plat/imx/imx8mq/ddrc.c
index f649dd98..2557d2b2 100644
--- a/plat/imx/imx8mq/ddrc.c
+++ b/plat/imx/imx8mq/ddrc.c
@@ -2599,7 +2599,7 @@ void lpddr4_cfg_phy(void)
 
 void ddrc_enter_retention(void)
 {
-	NOTICE("enter LPDDR4 retention\n");
+	INFO("enter LPDDR4 retention\n");
 
 	/* save the ddr phy trained csr in ocram */
 	if (!trained_csr_saved) {
@@ -2784,7 +2784,7 @@ void lpddr4_dvfs_hwffc(int init_vrcg, int init_fsp, int target_freq,
 {
 	uint32_t tmp, tmp_t;
 
-	NOTICE("hwffc enter\n");
+	INFO("hwffc enter\n");
 
 	/* step 1: hwffc flow enter, set the HWFCCCTL */
 	tmp = ((init_fsp << 4) & 0x10) | ((init_vrcg << 5) & 0x20) | 0x40;
@@ -2914,7 +2914,7 @@ void lpddr4_dvfs_hwffc(int init_vrcg, int init_fsp, int target_freq,
 	while ((mmio_read_32(IMX_DDRC_BASE + DDRC_STAT) & 0x3) != 0x1)
 		;
 
-	NOTICE("hwffc exit\n");
+	INFO("hwffc exit\n");
 }
 
 spinlock_t dfs_lock;
@@ -2958,10 +2958,14 @@ int lpddr4_dvfs_handler(uint32_t smc_fid,
 		/* trigger the IRQ */
 		for (int i = 0; i < 4; i++) {
 			int irq = irqs_used[i] % 32;
-			if (cpu_id != i && (online_cores & (0x1 << (i * 8))))
+			if (cpu_id != i && (online_cores & (0x1 << (i * 8)))) {
 				mmio_write_32(0x38800204 + (irqs_used[i] / 32) * 4, (1 << irq));
+				mmio_write_32(0x303a0040, mmio_read_32(0x303a0040) & ~(0x1));
+			}
 		}
 
+		mmio_write_32(0x30340004, mmio_read_32(0x30340004) | (1 << 12));
+
 		/* make sure all the core in WFE */
 		online_cores &= ~(0x1 << (cpu_id * 8));
 		while (1) {
@@ -2969,6 +2973,8 @@ int lpddr4_dvfs_handler(uint32_t smc_fid,
 				break;
 		}
 
+		mmio_write_32(0x30340004, mmio_read_32(0x30340004) & ~(1 << 12));
+
 		lpddr4_dvfs_hwffc(init_vrcg, init_fsp, target_freq, discamdrain);
 		init_fsp = (~init_fsp) & 0x1;
 		wait_ddrc_hwffc_done = false;
diff --git a/plat/imx/imx8mq/gpc.c b/plat/imx/imx8mq/gpc.c
index fb3ae7bb..3da8aadf 100644
--- a/plat/imx/imx8mq/gpc.c
+++ b/plat/imx/imx8mq/gpc.c
@@ -16,11 +16,6 @@
 #include <fsl_sip.h>
 #include <soc.h>
 
-#define GPC_LPCR_A53_BSC	0x0
-#define GPC_LPCR_A53_BSC2	0x108
-#define GPC_LPCR_A53_AD		0x4
-#define GPC_LPCR_M4		0x8
-#define GPC_SLPCR		0x14
 #define GPC_MST_CPU_MAPPING	0x18
 #define GPC_PGC_ACK_SEL_A53	0x24
 #define GPC_IMR1_CORE0_A53	0x30
@@ -48,43 +43,6 @@
 #define GPC_PGC_SCU_TIMMING	0x910
 #define GPC_SLOT0_CFG		0xb0
 
-#define LPCR_A53_BSC_CPU_CLK_ON_LPM	0x4000
-#define LPCR_A53_BSC_LPM0		0x3
-#define LPCR_A53_BSC_LPM1		0xc
-#define LPCR_A53_BSC2_LPM2		0x3
-#define LPCR_A53_BSC2_LPM3		0xc
-
-#define LPCR_A53_AD_L2PGE		0x80000000
-#define LPCR_A53_AD_EN_C3_PUP		0x8000000
-#define LPCR_A53_AD_EN_C3_IRQ_PUP	0x4000000
-#define LPCR_A53_AD_EN_C2_PUP		0x2000000
-#define LPCR_A53_AD_EN_C2_IRQ_PUP	0x1000000
-#define LPCR_A53_AD_EN_C3_PDN		0x80000
-#define LPCR_A53_AD_EN_C3_WFI_PDN	0x40000
-#define LPCR_A53_AD_EN_C2_PDN		0x20000
-#define LPCR_A53_AD_EN_C2_WFI_PDN	0x10000
-#define LPCR_A53_AD_EN_C1_PUP		0x800
-#define LPCR_A53_AD_EN_C1_IRQ_PUP	0x400
-#define LPCR_A53_AD_EN_C0_PUP		0x200
-#define LPCR_A53_AD_EN_C0_IRQ_PUP	0x100
-#define LPCR_A53_AD_EN_L2_WFI_PDN	0x20
-#define LPCR_A53_AD_EN_PLAT_PDN		0x10
-#define LPCR_A53_AD_EN_C1_PDN		0x8
-#define LPCR_A53_AD_EN_C1_WFI_PDN	0x4
-#define LPCR_A53_AD_EN_C0_PDN		0x2
-#define LPCR_A53_AD_EN_C0_WFI_PDN	0x1
-
-#define A53_LPM_WAIT			0x5
-#define A53_LPM_STOP			0xa
-
-#define SLPCR_EN_DSM			0x80000000
-#define SLPCR_RBC_EN			0x40000000
-#define SLPCR_VSTBY			0x4
-#define SLPCR_SBYOS			0x2
-#define SLPCR_BYPASS_PMIC_READY		0x1
-#define SLPCR_A53_FASTWUP_STOP		(1 << 17)
-#define SLPCR_A53_FASTWUP_WAIT		(1 << 16)
-
 #define GPC_CPU_PGC_SW_PUP_REQ		0xf0
 #define GPC_CPU_PGC_SW_PDN_REQ		0xfc
 #define BM_CPU_PGC_SW_PDN_PUP_REQ 	0x1
@@ -106,8 +64,26 @@
 #define ANAMIX_HW_DRAM_PLL_CFG0		0x60
 #define ANAMIX_HW_ANAMIX_MISC_CFG	0x70
 
-static uint32_t gpc_saved_imrs[128];
-static uint32_t gpc_wake_irqs[128];
+#define LPCR_A53_BSC			0x0
+#define LPCR_A53_BSC2			0x108
+#define LPCR_A53_AD			0x4 
+#define LPCR_M4				0x8
+#define SLPCR				0x14
+#define SLPCR_EN_DSM			(1 << 31)
+#define SLPCR_RBC_EN			(1 << 30)
+#define SLPCR_A53_FASTWUP_STOP		(1 << 17)
+#define SLPCR_A53_FASTWUP_WAIT		(1 << 16)
+#define SLPCR_VSTBY			(1 << 2)
+#define SLPCR_SBYOS			(1 << 1)
+#define SLPCR_BYPASS_PMIC_READY		0x1
+#define A53_LPM_WAIT			0x5
+#define A53_LPM_STOP			0xa
+
+#define GPC_SLOT0_CFG 		0xb0
+#define SRC_GPR1_OFFSET 	0x74
+
+static uint32_t gpc_saved_imrs[4];
+static uint32_t gpc_wake_irqs[4];
 static bool is_pcie1_power_down = true;
 
 static uint32_t gpc_pu_m_core_offset[11] = {
@@ -128,198 +104,286 @@ void imx_gpc_set_m_core_pgc(unsigned int offset, bool pdn)
 	mmio_write_32(IMX_GPC_BASE + offset, val);
 }
 
-void imx_gpc_set_lpm_mode(enum imx_cpu_pwr_mode mode)
+void imx_set_cpu_secure_entry(int core_id, uintptr_t sec_entrypoint)
 {
-	uint32_t val1, val2, val3, val4;
-
-	val1 = mmio_read_32(IMX_GPC_BASE + GPC_LPCR_A53_BSC);
-	val2 = mmio_read_32(IMX_GPC_BASE + GPC_LPCR_A53_BSC2);
-	val3 = mmio_read_32(IMX_GPC_BASE + GPC_SLPCR);
-	val4 = mmio_read_32(IMX_GPC_BASE + GPC_LPCR_A53_AD);
-
-	/* all core's LPM setting must be same */
-	val1 &= ~(LPCR_A53_BSC_LPM0 | LPCR_A53_BSC_LPM1);
-	val2 &= ~(LPCR_A53_BSC2_LPM2 | LPCR_A53_BSC2_LPM3);
-	val3 &= ~(SLPCR_EN_DSM | SLPCR_RBC_EN | SLPCR_VSTBY |
-		  SLPCR_SBYOS | SLPCR_BYPASS_PMIC_READY);
-	val4 |= (1 << 5);
-
-	switch(mode) {
-	case WAIT_CLOCKED:
-		break;
-	case WAIT_UNCLOCKED:
-		val1 |= A53_LPM_WAIT;
-		val2 |= A53_LPM_WAIT;
-		val1 &= ~LPCR_A53_BSC_CPU_CLK_ON_LPM;
-		break;
-	case STOP_POWER_ON:
-		val1 |= A53_LPM_STOP;
-		val2 |= A53_LPM_STOP;
-		val1 &= ~LPCR_A53_BSC_CPU_CLK_ON_LPM;
-		val3 |= SLPCR_EN_DSM;
-		val3 |= SLPCR_RBC_EN;
-		val3 |= SLPCR_BYPASS_PMIC_READY;
-		break;
-	case STOP_POWER_OFF:
-		val1 |= A53_LPM_STOP;
-		val2 |= A53_LPM_STOP;
-		val1 &= ~LPCR_A53_BSC_CPU_CLK_ON_LPM;
-		val3 |= SLPCR_EN_DSM;
-		val3 &= ~SLPCR_A53_FASTWUP_STOP;
-		val3 |= SLPCR_RBC_EN;
-		val3 |= SLPCR_VSTBY;
-		val3 |= SLPCR_SBYOS;
-		val3 |= SLPCR_BYPASS_PMIC_READY;
-		val4 &= ~(1 << 5);
-		break;
-	default:
-		break;
-	}
+	uint64_t temp_base;
 
-	mmio_write_32(IMX_GPC_BASE + GPC_LPCR_A53_BSC, val1);
-	mmio_write_32(IMX_GPC_BASE + GPC_LPCR_A53_BSC2, val2);
-	mmio_write_32(IMX_GPC_BASE + GPC_LPCR_A53_AD, val4);
-	mmio_write_32(IMX_GPC_BASE + GPC_SLPCR, val3);
+	temp_base = (uint64_t) sec_entrypoint;
+	temp_base >>= 2;
+
+	mmio_write_32(IMX_SRC_BASE + SRC_GPR1_OFFSET + (core_id << 3),
+		((uint32_t)(temp_base >> 22) & 0xffff));
+	mmio_write_32(IMX_SRC_BASE + SRC_GPR1_OFFSET + (core_id << 3) + 4,
+		((uint32_t)temp_base & 0x003fffff));
 }
 
-/* enable CORE LPM PDN/WUP in AD register */
-void imx_gpc_set_cpu_power_gate_by_lpm(unsigned int cpu, bool pdn)
+/* use wfi power down the core */
+void imx_set_cpu_pwr_off(int core_id)
 {
-	uint32_t mask, val;
-
-	val = mmio_read_32(IMX_GPC_BASE + GPC_LPCR_A53_AD);
-
-	switch(cpu) {
-	case 0:
-		mask = LPCR_A53_AD_EN_C0_PDN | LPCR_A53_AD_EN_C0_PUP;
-		break;
-	case 1:
-		mask = LPCR_A53_AD_EN_C1_PDN | LPCR_A53_AD_EN_C1_PUP;
-		break;
-	case 2:
-		mask = LPCR_A53_AD_EN_C2_PDN | LPCR_A53_AD_EN_C3_PUP;
-		break;
-	case 3:
-		mask = LPCR_A53_AD_EN_C3_PDN | LPCR_A53_AD_EN_C3_PUP;
-		break;
-	default:
-		return;
-	}
+	uint32_t val;
 
-	if (pdn)
-		val |= mask;
-	else
-		val &= ~mask;
+	/* enable the wfi power down of the core */
+	val = mmio_read_32(IMX_GPC_BASE + 0x4);
+	val |= (1 << (core_id < 2 ? core_id * 2 : (core_id - 2) * 2 + 16));
+	val |= 1 << (core_id + 20);
+	mmio_write_32(IMX_GPC_BASE + 0x4, val);
 
-	mmio_write_32(IMX_GPC_BASE + GPC_LPCR_A53_AD, val);
-}
+	/* assert the pcg pcr bit of the core */
+	val = mmio_read_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id);
+	val |= (1 << 0);
+	mmio_write_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id, val);
+};
 
-/* enable PLAT/SCU power down in AD register */
-void imx_gpc_set_plat_power_gate_by_lpm(bool pdn)
+/* use the sw method to power up the core */
+void imx_set_cpu_pwr_on(int core_id)
 {
 	uint32_t val;
 
-	val = mmio_read_32(IMX_GPC_BASE + GPC_LPCR_A53_AD);
-	val &= ~(LPCR_A53_AD_EN_PLAT_PDN | LPCR_A53_AD_L2PGE);
+	/* clear the wfi power down bit of the core */
+	val = mmio_read_32(IMX_GPC_BASE + 0x4);
+	val &= ~(1 << (core_id < 2 ? core_id * 2 : (core_id - 2) * 2 + 16));
+	mmio_write_32(IMX_GPC_BASE + 0x4, val);
 
-	if (pdn)
-		val |= LPCR_A53_AD_EN_PLAT_PDN | LPCR_A53_AD_L2PGE;
+	/* assert the ncpuporeset */
+	val = mmio_read_32(IMX_SRC_BASE + 0x8);
+	val &= ~(1 << core_id);
+	mmio_write_32(IMX_SRC_BASE + 0x8, val);
 
-	mmio_write_32(IMX_GPC_BASE + GPC_LPCR_A53_AD, val);
+	/* assert the pcg pcr bit of the core */
+	val = mmio_read_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id);
+	val |= (1 << 0);
+	mmio_write_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id, val);
+
+	/* sw power up the core */
+	val = mmio_read_32(IMX_GPC_BASE + 0xf0);
+	val |= (1 << core_id);
+	mmio_write_32(IMX_GPC_BASE + 0xf0, val);
+
+	/* wait for the power up finished */
+	while ((mmio_read_32(IMX_GPC_BASE + 0xf0) & (1 << core_id)) != 0)
+		;
+
+	/* deassert the pcg pcr bit of the core */
+	val = mmio_read_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id);
+	val &= ~(1 << 0);
+	mmio_write_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id, val);
+
+	/* deassert the ncpuporeset */
+	val = mmio_read_32(IMX_SRC_BASE + 0x8);
+	val |= (1 << core_id);
+	mmio_write_32(IMX_SRC_BASE + 0x8, val);
 }
 
-void imx_gpc_set_slot_ack(uint32_t index, enum imx_gpc_slot m_core,
-			  bool mode, bool ack)
+/* if out of lpm, we need to do reverse steps */
+void imx_set_cpu_lpm(int core_id, bool pdn)
 {
-	uint32_t val, shift;
+	uint32_t val;
 
-	if (index > 10) {
-		tf_printf("Invalid slot index\n");
-		return;
-	}
-	/* set slot */
-	val = mmio_read_32(IMX_GPC_BASE + GPC_SLOT0_CFG + index * 4);
-	val |= (mode + 1) << m_core * 2;
-	mmio_write_32(IMX_GPC_BASE + GPC_SLOT0_CFG + index * 4, val);
-	/* set ack */
-	if (ack) {
-		shift = m_core >= A53_SCU  ? 2 : 0;
-		val = mmio_read_32(IMX_GPC_BASE + GPC_PGC_ACK_SEL_A53);
-		/* clear dummy ack */
-		val &= ~(1 << (15 + (mode ? 16: 0)));
-		val |= 1 << (shift + (mode ? 16 : 0));
-		mmio_write_32(IMX_GPC_BASE + GPC_PGC_ACK_SEL_A53, val);
+	if (pdn) {
+		val = mmio_read_32(IMX_GPC_BASE + 0x4);
+		/* enable the core WFI power down */
+		val |= (1 << (core_id < 2 ? core_id * 2 : (core_id - 2) * 2 + 16));
+		val |= 1 << (core_id + 20);
+		/* enable the core IRQ wakeup */
+		val |= (core_id < 2 ? (1 << (core_id * 2 + 8)) : (1 << (core_id * 2 + 20)));
+		mmio_write_32(IMX_GPC_BASE + 0x4, val);
+
+		/* assert the pcg pcr bit of the core */
+		val = mmio_read_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id);
+		val |= (1 << 0);
+		mmio_write_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id, val);
+	} else {
+		val = mmio_read_32(IMX_GPC_BASE + 0x4);
+		/* disable the core WFI power down */
+		val &= ~(1 << (core_id < 2 ? core_id * 2 : (core_id - 2) * 2 + 16));
+		/* disable the core IRQ wakeup */
+		val &= ~(core_id < 2 ? (1 << (core_id * 2 + 8)) : (1 << (core_id * 2 + 20)));
+		mmio_write_32(IMX_GPC_BASE + 0x4, val);
+		/* deassert the pcg pcr bit of the core */
+		val = mmio_read_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id);
+		val &= ~(1 << 0);
+		mmio_write_32(IMX_GPC_BASE + 0x800 + 0x40 * core_id, val);
 	}
 }
 
-/* cpu: cpu index */
-void imx_gpc_set_core_pdn_pup_by_software(unsigned int cpu, bool pdn)
+/* SLOT 0 is used for A53 PLAT poewr down */
+/* SLOT 1 is used for A53 PLAT power up */
+/* SLOT 2 is used for A53 last core power up */
+/* when enter LPM power down, SCU's ACK is used */
+/* when out of LPM, last_core's ACK is used */
+void imx_pup_pdn_slot_config(int last_core, bool pdn)
 {
-	uint32_t val, index;
+	uint32_t slot_ack;
+	uint32_t slot0_cfg, slot1_cfg, slot2_cfg;
 
-	val = mmio_read_32(IMX_GPC_BASE	+ (pdn ?
-		GPC_CPU_PGC_SW_PDN_REQ : GPC_CPU_PGC_SW_PUP_REQ));
+	slot0_cfg = mmio_read_32(IMX_GPC_BASE + GPC_SLOT0_CFG);
+	slot1_cfg = mmio_read_32(IMX_GPC_BASE + GPC_SLOT0_CFG + 0x4);
+	slot2_cfg = mmio_read_32(IMX_GPC_BASE + GPC_SLOT0_CFG + 0x8);
+	slot_ack  = mmio_read_32(IMX_GPC_BASE + 0x24);
 
-	/*Set the core PCR bit before sw PUP/PDN trigger */
-	imx_gpc_set_m_core_pgc(GPC_ARM_PGC + cpu * 0x40, true);
+	if (pdn) {
+		/* PUP and PDN SLOT config */
+		slot0_cfg |= (1 << 8);
+		slot1_cfg |= (1 << 9);
+		slot2_cfg |= (0x2 << last_core * 2);
 
-	index = cpu;
-	val |= (BM_CPU_PGC_SW_PDN_PUP_REQ << index);
-	mmio_write_32(IMX_GPC_BASE + (pdn ?
-		GPC_CPU_PGC_SW_PDN_REQ : GPC_CPU_PGC_SW_PUP_REQ), val);
+		/*PDN ACK config */
+		slot_ack &= ~(1 << 15);
+		slot_ack |= (1 << 2);
 
-	while((mmio_read_32(IMX_GPC_BASE + (pdn ?
-		GPC_CPU_PGC_SW_PDN_REQ : GPC_CPU_PGC_SW_PUP_REQ)) &
-		BM_CPU_PGC_SW_PDN_PUP_REQ) != 0)
-		;
+		/*PUP ACK setting */
+		slot_ack &= ~(1 << 31);
+		slot_ack |= (last_core < 2 ? (1 << (last_core + 16)) :
+			    (1 << (last_core + 27)));
+
+	} else {
+		slot0_cfg &= ~(1 << 8);
+		slot1_cfg &= ~(1 << 9);
+		slot2_cfg &= ~(0x2 << last_core * 2);
+
+		slot_ack |= (1 << 15);
+		slot_ack &= ~(1 << 2);
+
+		slot_ack |= (1 << 31);
+		slot_ack &= ~(last_core < 2 ? (1 << (last_core + 16)) :
+			    (1 << (last_core + 27)));
+	}
 
-	/*Clear the core PCR bit after sw PUP/PDN trigger */
-	imx_gpc_set_m_core_pgc(GPC_ARM_PGC + cpu * 0x40, false);
+	mmio_write_32(IMX_GPC_BASE + GPC_SLOT0_CFG, slot0_cfg);
+	mmio_write_32(IMX_GPC_BASE + GPC_SLOT0_CFG + 0x4, slot1_cfg);
+	mmio_write_32(IMX_GPC_BASE + GPC_SLOT0_CFG + 0x8, slot2_cfg);
+	mmio_write_32(IMX_GPC_BASE + 0x24, slot_ack);
 }
 
-void imx_gpc_pre_suspend(bool arm_power_off)
+/* used for cpuidle support on imx8mq */
+void imx_set_cluster_standby(bool enter)
 {
-	unsigned int i;
+	uint32_t val;
+	val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_AD);
+	/*
+	 * Enable BIT 6 of A53 AD register to make sure
+	 * system don't enter LPM mode.
+	 */
+	if (enter)
+		val |= (1 << 6);
+	else
+		val &= ~(1 << 6);
 
-	/* set the LPM mode */
-	if (arm_power_off) {
-		/* enable core 0/1/2/3 power down/up with low power mode */
-		/* enable plat power down/up with low power mode */
-
-		/* 
-		 * to avoid confuse, we use slot 0~4 for power down.
-		 * slot 5~9 for power up.
-		 * power down slot sequence:
-		 * slot 0 -> CORE0,
-		 * SLOT 1 -> Mega/Fast mix,
-		 * SLOT 2 -> SCU
-		 *
-		 * SLOT 5 -> Mega/Fast mix,
-		 * SLOT 6 -> SCU
-		 * SLOT 7 -> CORE0
+	mmio_write_32(IMX_GPC_BASE  + LPCR_A53_AD, val);
+}
+
+/* only support the GPC STOP mode power off */
+/* set the BSC and BSC2 LPM bit, and other bit in AD */
+void imx_set_cluster_powerdown(int last_core, bool pdn)
+{
+	uint32_t val;
+
+	if (pdn) {
+		/*
+		 * config the LPM STOP mode, enable CPU clock
+		 * disable in LPM mode.
 		 */
-		 /* SCU slot ack as the power down ack */
-		 /* CORE0 slot ack as the power up ack */
-		imx_gpc_set_lpm_mode(STOP_POWER_OFF);
-		imx_gpc_set_slot_ack(0, A53_CORE0, false, false);
-		imx_gpc_set_slot_ack(2, A53_SCU, false, true);
-
-		imx_gpc_set_slot_ack(6, A53_SCU, true, false);
-		imx_gpc_set_slot_ack(7, A53_CORE0, true, true);
-		imx_gpc_set_m_core_pgc(GPC_ARM_PGC, true);
-		imx_gpc_set_m_core_pgc(GPC_SCU_PGC, true);
-
-		imx_gpc_set_cpu_power_gate_by_lpm(0, true);
-		imx_gpc_set_plat_power_gate_by_lpm(true);
+		val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_BSC);
+		val |= 0xa; /* enable the C0~1 LPM */
+		val &= ~(1 << 14); /* disable cpu clock in LPM */
+		mmio_write_32(IMX_GPC_BASE + LPCR_A53_BSC, val);
+
+		/* enable C2-3's LPM */
+		val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_BSC2);
+		val |= 0xa;
+		mmio_write_32(IMX_GPC_BASE + LPCR_A53_BSC2, val);
+
+		/* enable PLAT/SCU power down */
+		val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_AD);
+		val &= ~(1 << 5);
+		val |= ((1 << 31) | (1 << 4));
+		/* enable C0's LPM power down */
+		/*If cluster enter LPM, the last core's IRQ wakeup must be clear */
+		val &= ~(last_core < 2 ? (1 << (last_core * 2 + 8)) : (1 << (last_core * 2 + 20)));
+		/* enable the C0's LPM PUP */
+		val |= (last_core < 2 ? (1 << (last_core * 2 + 9)) : (1 << (last_core * 2 + 21)));
+		mmio_write_32(IMX_GPC_BASE + LPCR_A53_AD, val);
+
+		imx_pup_pdn_slot_config(last_core, true);
+
+		/* enable PLAT PGC */
+		val = mmio_read_32(IMX_GPC_BASE + 0x900);
+		val |= 0x1;
+		mmio_write_32(IMX_GPC_BASE + 0x900, val);
 	} else {
-		imx_gpc_set_lpm_mode(STOP_POWER_ON);
+		/* clear PLAT PGC */
+		val = mmio_read_32(IMX_GPC_BASE + 0x900);
+		val &= ~0x1;
+		mmio_write_32(IMX_GPC_BASE + 0x900, val);
+
+		/* clear the slot and ack for cluster power down */
+		imx_pup_pdn_slot_config(last_core, false);
+
+		/* reverse the cluster level setting */
+		val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_BSC);
+		val &= ~0xa; /* clear the C0~1 LPM */
+		val |= (1 << 14); /* disable cpu clock in LPM */
+		mmio_write_32(IMX_GPC_BASE + LPCR_A53_BSC, val);
+
+		val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_BSC2);
+		val &= ~0xa;
+		mmio_write_32(IMX_GPC_BASE + LPCR_A53_BSC2, val);
+
+		/* clear PLAT/SCU power down */
+		val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_AD);
+		val |= (1 << 5);
+		val &= ~((1 << 31) | (1 << 4));
+		/* disable C0's LPM PUP */
+		val &= ~(last_core < 2 ? (1 << (last_core * 2 + 9)) : (1 << (last_core * 2 + 21)));
+		mmio_write_32(IMX_GPC_BASE + LPCR_A53_AD, val);
 	}
+}
+
+/* only handle the SLPCR and DDR retention */
+/* config the PLLs override setting */
+void imx_set_sys_lpm(bool retention)
+{
+	uint32_t val;
+
+	/* set system DSM mode SLPCR(0x14) */
+	val = mmio_read_32(IMX_GPC_BASE + SLPCR);
+	val &= ~(SLPCR_EN_DSM | SLPCR_VSTBY | SLPCR_SBYOS |
+		 SLPCR_BYPASS_PMIC_READY | SLPCR_RBC_EN);
 
-	for (i = 0; i < 4; i++) {
-		gpc_saved_imrs[i] = mmio_read_32(IMX_GPC_BASE + GPC_IMR1_CORE0_A53 + i * 4);
-		mmio_write_32(IMX_GPC_BASE + GPC_IMR1_CORE0_A53 + i * 4, ~gpc_wake_irqs[i]);
+	if (retention) {
+		val |= (SLPCR_EN_DSM | SLPCR_VSTBY | SLPCR_SBYOS |
+			 SLPCR_BYPASS_PMIC_READY | SLPCR_RBC_EN);
+
+		/* DDR enter retention */
+		ddrc_enter_retention();
+	} else {
+		/* DDR exit retention */
+		ddrc_exit_retention();
 	}
 
+	mmio_write_32(IMX_GPC_BASE + 0x14, val);
+}
+
+void imx_set_rbc_count(void)
+{
+	uint32_t val;
+
+	val = mmio_read_32(IMX_GPC_BASE + SLPCR);
+	val |= (0x3f << 24);
+	mmio_write_32(IMX_GPC_BASE + SLPCR, val);
+}
+
+void imx_clear_rbc_count(void)
+{
+	uint32_t val;
+
+	val = mmio_read_32(IMX_GPC_BASE + SLPCR);
+	val &= ~(0x3f << 24);
+	mmio_write_32(IMX_GPC_BASE + SLPCR, val);
+
+}
+void imx_anamix_pre_suspend()
+{
 	/* override PLL/OSC to let ccm control them */
 	mmio_write_32(IMX_ANAMIX_BASE + ANAMIX_HW_AUDIO_PLL1_CFG0,
 		mmio_read_32(IMX_ANAMIX_BASE + ANAMIX_HW_AUDIO_PLL1_CFG0) | 0x140000);
@@ -347,31 +411,8 @@ void imx_gpc_pre_suspend(bool arm_power_off)
 		mmio_read_32(IMX_ANAMIX_BASE + ANAMIX_HW_ANAMIX_MISC_CFG) | 0xa);
 }
 
-void imx_gpc_post_resume(void)
+void imx_anamix_post_resume(void)
 {
-	int i;
-	/* set LPM mode WAIT CLOCKED */
-	imx_gpc_set_lpm_mode(WAIT_CLOCKED);
-	/* clear lpm power gate of core and plat */
-	imx_gpc_set_cpu_power_gate_by_lpm(0, false);
-	imx_gpc_set_plat_power_gate_by_lpm(false);
-	/* clear PGC PDN bit */
-	imx_gpc_set_m_core_pgc(GPC_ARM_PGC, false);
-	imx_gpc_set_m_core_pgc(GPC_SCU_PGC, false);
-
-	for (i = 0; i < 4; i++) {
-		mmio_write_32(IMX_GPC_BASE + GPC_IMR1_CORE0_A53 + i * 4, gpc_saved_imrs[i]);
-	}
-
-	/* skip slot m4 use , clear slots */
-	for(i = 0; i < 10; i ++) {
-		if (i == 1 || i == 5)
-			continue;
-		mmio_write_32(IMX_GPC_BASE +GPC_SLOT0_CFG + i * 0x4, 0x0);
-	}
-	/* set DUMMY PDN/PUP ACK by default for A53 domain */
-	mmio_write_32(IMX_GPC_BASE + GPC_PGC_ACK_SEL_A53, 1 << 31 | 1 << 15);
-
 	/* clear override of PLL/OSC */
 	mmio_write_32(IMX_ANAMIX_BASE + ANAMIX_HW_AUDIO_PLL1_CFG0,
 		mmio_read_32(IMX_ANAMIX_BASE + ANAMIX_HW_AUDIO_PLL1_CFG0) & ~0x140000);
@@ -399,6 +440,30 @@ void imx_gpc_post_resume(void)
 		mmio_read_32(IMX_ANAMIX_BASE + ANAMIX_HW_ANAMIX_MISC_CFG) & ~0xa);
 }
 
+/*
+ * On i.MX8MQ, only in system suspend mode, the A53 cluster can
+ * enter LPM mode and shutdown the A53 PLAT power domain. So LPM
+ * wakeup only used for system suspend. when system enter suspend,
+ * any A53 CORE can be the last core to suspend the system, But
+ * the LPM wakeup can only use the C0's IMR to wakeup A53 cluster
+ * from LPM, so save C0's IMRs before suspend, restore back after
+ * resume.
+ */
+void imx_set_lpm_wakeup(bool pdn)
+{
+	unsigned int i;
+
+	if (pdn) {
+		for (i = 0; i < 4; i++) {
+			gpc_saved_imrs[i] = mmio_read_32(IMX_GPC_BASE + GPC_IMR1_CORE0_A53 + i * 4);
+			mmio_write_32(IMX_GPC_BASE + GPC_IMR1_CORE0_A53 + i * 4, ~gpc_wake_irqs[i]);
+		}
+	} else {
+		for (i = 0; i < 4; i++) 
+			mmio_write_32(IMX_GPC_BASE + GPC_IMR1_CORE0_A53 + i * 4, gpc_saved_imrs[i]);
+	}
+}
+
 static void imx_gpc_hwirq_mask(unsigned int hwirq)
 {
 	uintptr_t reg;
@@ -431,6 +496,33 @@ static void imx_gpc_set_wake(uint32_t hwirq, unsigned int on)
 				 gpc_wake_irqs[idx] & ~mask;
 }
 
+static uint32_t mask_offset[] = {0x30, 0x40, 0x1c0, 0x1d0,};
+static void imx_gpc_set_affinity(uint32_t hwirq, unsigned cpu_idx)
+{
+	uint32_t offset = mask_offset[cpu_idx];
+
+	uintptr_t reg;
+	unsigned int val;
+
+	/*
+	 * using the mask/unmask bit as affinity function.unmask the
+	 * IMR bit to enable IRQ wakeup for this core.
+	 */
+	reg = IMX_GPC_BASE + offset + (hwirq / 32) * 4;
+	val = mmio_read_32(reg);
+	val &= ~(1 << hwirq % 32);
+	mmio_write_32(reg, val);
+
+	/* clear affinity of other core */
+	for (int i = 0; i < 4; i++) {
+		if (cpu_idx != i) {
+			val = mmio_read_32(IMX_GPC_BASE + mask_offset[i] + (hwirq / 32) * 4);
+			val |= (1 << hwirq % 32);
+			mmio_write_32(IMX_GPC_BASE + mask_offset[i] + (hwirq / 32) * 4, val);
+		}
+	}
+}
+
 static void imx_gpc_pm_domain_enable(uint32_t domain_id, uint32_t on)
 {
 	uint32_t val;
@@ -499,18 +591,21 @@ void imx_gpc_init(void)
 	 * avoid entering DSM mode by mistake.
 	 */
 	mmio_write_32(IMX_GPC_BASE + GPC_IMR1_CORE0_A53, ~0x1);
+	mmio_write_32(IMX_GPC_BASE + 0x40,  ~0x1);
+	mmio_write_32(IMX_GPC_BASE + 0x1c0, ~0x1);
+	mmio_write_32(IMX_GPC_BASE + 0x1d0, ~0x1);
 
 	/* use external IRQs to wakeup C0~C3 from LPM */
-	val = mmio_read_32(IMX_GPC_BASE + GPC_LPCR_A53_BSC);
-	val |= 0x70c00000;
+	val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_BSC);
+	val |= 0x40000000;
 	/* clear the MASTER0 LPM handshake */
 	val &= ~(1 << 6);
-	mmio_write_32(IMX_GPC_BASE + GPC_LPCR_A53_BSC, val);
+	mmio_write_32(IMX_GPC_BASE + LPCR_A53_BSC, val);
 
 	/* mask M4 DSM trigger if M4 is NOT enabled */
-	val = mmio_read_32(IMX_GPC_BASE + GPC_LPCR_M4);
+	val = mmio_read_32(IMX_GPC_BASE + LPCR_M4);
 	val |= 1 << 31;
-	mmio_write_32(IMX_GPC_BASE + GPC_LPCR_M4, val);
+	mmio_write_32(IMX_GPC_BASE + LPCR_M4, val);
 
 	/* set all mix/PU in A53 domain */
 	mmio_write_32(IMX_GPC_BASE + GPC_PGC_CPU_0_1_MAPPING, 0xfffd);
@@ -519,19 +614,13 @@ void imx_gpc_init(void)
 	mmio_write_32(IMX_GPC_BASE + GPC_PGC_SCU_TIMMING,
 		      (0x59 << 10) | 0x5B | (0x2 << 20));
 
-	/* set A53 core power up timming */
-	mmio_write_32(IMX_GPC_BASE + GPC_PGC_CORE0_TIMMING, 0x1a << 7);
-	mmio_write_32(IMX_GPC_BASE + GPC_PGC_CORE1_TIMMING, 0x1a << 7);
-	mmio_write_32(IMX_GPC_BASE + GPC_PGC_CORE2_TIMMING, 0x1a << 7);
-	mmio_write_32(IMX_GPC_BASE + GPC_PGC_CORE3_TIMMING, 0x1a << 7);
-
 	/* set DUMMY PDN/PUP ACK by default for A53 domain */
 	mmio_write_32(IMX_GPC_BASE + GPC_PGC_ACK_SEL_A53, 1 << 31 | 1 << 15);
 	/* clear DSM by default */
-	val = mmio_read_32(IMX_GPC_BASE + GPC_SLPCR);
+	val = mmio_read_32(IMX_GPC_BASE + SLPCR);
 	val &= ~(1 << 31);
 	/* TODO if M4 is not enabled, clear more SLPCR bits */
-	mmio_write_32(IMX_GPC_BASE + GPC_SLPCR, val);
+	mmio_write_32(IMX_GPC_BASE + SLPCR, val);
 
 	/*
 	 * USB PHY power up needs to make sure RESET bit in SRC is clear,
@@ -564,6 +653,9 @@ int imx_gpc_handler(uint32_t smc_fid,
 	case FSL_SIP_CONFIG_GPC_PM_DOMAIN:
 		imx_gpc_pm_domain_enable(x2, x3);
 		break;
+	case FSL_SIP_CONFIG_GPC_SET_AFF:
+		imx_gpc_set_affinity(x2, x3);
+		break;
 	default:
 		return SMC_UNK;
 	}
diff --git a/plat/imx/imx8mq/imx8m_psci.c b/plat/imx/imx8mq/imx8m_psci.c
index 6045e722..b43af4cc 100644
--- a/plat/imx/imx8mq/imx8m_psci.c
+++ b/plat/imx/imx8mq/imx8m_psci.c
@@ -38,17 +38,11 @@
 #include <mmio.h>
 #include <soc.h>
 
-#define SCR_A53RCR1_OFFSET 0x08
-#define SRC_GPR1_OFFSET 0x74
-#define ARM_PGC 0x800
-#define PGC_PCR 1
-#define GPC_CPU_PGC_SW_PUP_REQ 0xf0
-#define GPC_CPU_PGC_SW_PDN_REQ 0xfc
-
 #define SNVS_LPCR	0x38
 
-extern void imx_gpc_set_core_pdn_pup_by_software(unsigned int cpu, bool pdn);
-extern void imx_gpc_set_cpu_power_gate_by_wfi(unsigned int cpu, bool pdn);
+#define CORE_PWR_STATE(state) ((state)->pwr_domain_state[MPIDR_AFFLVL0])
+#define CLUSTER_PWR_STATE(state) ((state)->pwr_domain_state[MPIDR_AFFLVL1])
+#define SYSTEM_PWR_STATE(state) ((state)->pwr_domain_state[PLAT_MAX_PWR_LVL])
 
 const unsigned char imx_power_domain_tree_desc[] = {
 	/* number of root nodes */
@@ -58,54 +52,19 @@ const unsigned char imx_power_domain_tree_desc[] = {
 	PLATFORM_CLUSTER0_CORE_COUNT,
 };
 
-void imx8m_kill_cpu(unsigned int target_idx)
-{
-	unsigned int val1;
-
-	/* Disable the secondary core */
-	val1 = mmio_read_32(IMX_SRC_BASE + SCR_A53RCR1_OFFSET);
-	val1 &= ~(1 << target_idx);
-	mmio_write_32(IMX_SRC_BASE + SCR_A53RCR1_OFFSET, val1);
-
-	imx_gpc_set_core_pdn_pup_by_software(target_idx, true);
-}
-
 int imx_pwr_domain_on(u_register_t mpidr)
 {
-	int ret = PSCI_E_SUCCESS;
-	unsigned int cpu_id, reg;
-
+	unsigned int core_id;
 	uint64_t base_addr = BL31_BASE;
 
-	tf_printf("cpu on\n");
-	cpu_id = MPIDR_AFFLVL0_VAL(mpidr);
-
-	/* core power up sequence
-	 * 1. Assert nCPUPORESET
-	 * 2. power the core.
-	 * 3. Deassert reset.
-	 */
-	reg = mmio_read_32(IMX_SRC_BASE + SCR_A53RCR1_OFFSET);
-	reg &= ~(1 << cpu_id);
-	mmio_write_32(IMX_SRC_BASE + SCR_A53RCR1_OFFSET, reg);
-
-	/* Set CPU jump address */
-	if (cpu_id > 0) {
-		base_addr >>= 2;
-		mmio_write_32(IMX_SRC_BASE + SRC_GPR1_OFFSET + (cpu_id << 3),
-			((uint32_t)(base_addr >> 22) & 0xFFFF));
-		mmio_write_32(IMX_SRC_BASE + SRC_GPR1_OFFSET + (cpu_id << 3) + 4,
-			((uint32_t)base_addr & 0x003FFFFF));
-	}
-
-	imx_gpc_set_core_pdn_pup_by_software(cpu_id, false);
+	core_id = MPIDR_AFFLVL0_VAL(mpidr);
 
-	/* Kick CPU here */
-	reg = mmio_read_32(IMX_SRC_BASE + SCR_A53RCR1_OFFSET);
-	reg |= (1 << cpu_id);
-	mmio_write_32(IMX_SRC_BASE + SCR_A53RCR1_OFFSET, reg);
+	/* set the secure entrypoint */
+	imx_set_cpu_secure_entry(core_id, base_addr);
+	/* power up the core */
+	imx_set_cpu_pwr_on(core_id);
 
-	return ret;
+	return PSCI_E_SUCCESS;
 }
 
 void imx_pwr_domain_on_finish(const psci_power_state_t *target_state)
@@ -118,12 +77,21 @@ void imx_pwr_domain_on_finish(const psci_power_state_t *target_state)
 
 void imx_pwr_domain_off(const psci_power_state_t *target_state)
 {
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL0_VAL(mpidr);
+
+	/* disable the GIC cpu interface first */
 	plat_gic_cpuif_disable();
+	/* config the core for power down */
+	imx_set_cpu_pwr_off(core_id);
 }
 
 int imx_validate_ns_entrypoint(uintptr_t ns_entrypoint)
 {
-	/* TODO */
+	/* The non-secure entrypoint should be in RAM space */
+	if (ns_entrypoint < 0x40000000)
+		return PSCI_E_INVALID_PARAMS;
+
 	return PSCI_E_SUCCESS;
 }
 
@@ -131,21 +99,23 @@ int imx_validate_power_state(unsigned int power_state,
 			 psci_power_state_t *req_state)
 {
 	int pwr_lvl = psci_get_pstate_pwrlvl(power_state);
+	int pwr_type = psci_get_pstate_type(power_state);
+	int state_id = psci_get_pstate_id(power_state);
 
 	if (pwr_lvl > PLAT_MAX_PWR_LVL)
 		return PSCI_E_INVALID_PARAMS;
 
-	/* Sanity check the requested afflvl */
-	if (psci_get_pstate_type(power_state) == PSTATE_TYPE_STANDBY) {
-		if (pwr_lvl != MPIDR_AFFLVL0)
-			return PSCI_E_INVALID_PARAMS;
-		/* power domain in standby state */
-		req_state->pwr_domain_state[pwr_lvl] = PLAT_MAX_RET_STATE;
+	if (pwr_type == PSTATE_TYPE_STANDBY) {
+		CORE_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+		CLUSTER_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+	}
 
-		return PSCI_E_SUCCESS;
+	if (pwr_type == PSTATE_TYPE_POWERDOWN && state_id == 0x33) {
+		CORE_PWR_STATE(req_state) = PLAT_MAX_OFF_STATE;
+		CLUSTER_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
 	}
 
-	return 0;
+	return PSCI_E_SUCCESS;
 }
 
 void imx_cpu_standby(plat_local_state_t cpu_state)
@@ -163,38 +133,74 @@ void imx_cpu_standby(plat_local_state_t cpu_state)
 void imx_domain_suspend(const psci_power_state_t *target_state)
 {
 	uint64_t base_addr = BL31_BASE;
-	plat_gic_cpuif_disable();
-
-	ddrc_enter_retention();
-	/* imx gpc pre suspend */
-	imx_gpc_pre_suspend(true);
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL0_VAL(mpidr);
+
+	if (is_local_state_off(CORE_PWR_STATE(target_state))) {
+		/* disable the cpu interface */
+		plat_gic_cpuif_disable();
+		imx_set_cpu_secure_entry(core_id, base_addr);
+		imx_set_cpu_lpm(core_id, true);
+	} else {
+		/* TODO cluster level clock gate off ? */
+		dsb();
+		write_scr_el3(read_scr_el3() | 0x4);
+		isb();
+	}
 
-	base_addr >>= 2;
-	mmio_write_32(IMX_SRC_BASE + SRC_GPR1_OFFSET ,
-			((uint32_t)(base_addr >> 22) & 0xFFFF));
-	mmio_write_32(IMX_SRC_BASE + SRC_GPR1_OFFSET + 4,
-			((uint32_t)base_addr & 0x003FFFFF));
+	if (is_local_state_off(CLUSTER_PWR_STATE(target_state)))
+		imx_set_cluster_powerdown(core_id, true);
+	else
+		imx_set_cluster_standby(true);
 
+	if (is_local_state_retn(SYSTEM_PWR_STATE(target_state))) {
+		/* put DDR into retention mode */
+		imx_set_sys_lpm(true);
+		imx_anamix_pre_suspend();
+	}
 }
 
 void imx_domain_suspend_finish(const psci_power_state_t *target_state)
 {
-	ddrc_exit_retention();
-	/* imx gpc post resume */
-	imx_gpc_post_resume();
-	/* enable the GICv3 cpu interface */
-	plat_gic_cpuif_enable();
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL0_VAL(mpidr);
+
+	/* check the system level status */
+	if (is_local_state_retn(SYSTEM_PWR_STATE(target_state))) {
+		imx_set_sys_lpm(false);
+		imx_anamix_post_resume();
+		imx_clear_rbc_count();
+	}
+
+	/* check the cluster level power status */
+	if (is_local_state_off(CLUSTER_PWR_STATE(target_state)))
+		imx_set_cluster_powerdown(core_id, false);
+	else
+		imx_set_cluster_standby(false);
+
+	/* check the core level power status */
+	if (is_local_state_off(CORE_PWR_STATE(target_state))) {
+		/* clear the core lpm setting */
+		imx_set_cpu_lpm(core_id, false);
+		/* enable the gic cpu interface */
+		plat_gic_cpuif_enable();
+	} else {
+		write_scr_el3(read_scr_el3() & (~0x4));
+		isb();
+	}
 }
 
 void imx_get_sys_suspend_power_state(psci_power_state_t *req_state)
 {
 	unsigned int i;
 
-	for (i = IMX_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++)
-		req_state->pwr_domain_state[i] = PLAT_MAX_RET_STATE;
+	for (i = IMX_PWR_LVL0; i < PLAT_MAX_PWR_LVL; i++)
+		req_state->pwr_domain_state[i] = PLAT_STOP_OFF_STATE;
+
+	req_state->pwr_domain_state[PLAT_MAX_PWR_LVL] = PLAT_MAX_RET_STATE;
 }
 
-void  __attribute__((noreturn)) imx_system_reset(void)
+void __dead2 imx_system_reset(void)
 {
 	uintptr_t wdog_base = IMX_WDOG_BASE;
 	unsigned int val;
@@ -214,7 +220,7 @@ void  __attribute__((noreturn)) imx_system_reset(void)
 		;
 }
 
-void __attribute__((noreturn)) imx_system_off(void)
+void __dead2 imx_system_off(void)
 {
 	mmio_write_32(IMX_SNVS_BASE + SNVS_LPCR, 0x61);
 
@@ -224,6 +230,15 @@ void __attribute__((noreturn)) imx_system_off(void)
 		;
 }
 
+void __dead2 imx_pwr_domain_pwr_down_wfi(const psci_power_state_t *target_state)
+{
+	if (is_local_state_off(CLUSTER_PWR_STATE(target_state)))
+		imx_set_rbc_count();
+
+	while (1)
+		wfi();
+}
+
 static const plat_psci_ops_t imx_plat_psci_ops = {
 	.pwr_domain_on = imx_pwr_domain_on,
 	.pwr_domain_on_finish = imx_pwr_domain_on_finish,
@@ -233,6 +248,7 @@ static const plat_psci_ops_t imx_plat_psci_ops = {
 	.cpu_standby = imx_cpu_standby,
 	.pwr_domain_suspend = imx_domain_suspend,
 	.pwr_domain_suspend_finish = imx_domain_suspend_finish,
+	.pwr_domain_pwr_down_wfi = imx_pwr_domain_pwr_down_wfi,
 	.get_sys_suspend_power_state = imx_get_sys_suspend_power_state,
 	.system_reset = imx_system_reset,
 	.system_off = imx_system_off,
diff --git a/plat/imx/imx8mq/include/platform_def.h b/plat/imx/imx8mq/include/platform_def.h
index faff2021..1021fd25 100644
--- a/plat/imx/imx8mq/include/platform_def.h
+++ b/plat/imx/imx8mq/include/platform_def.h
@@ -16,9 +16,12 @@
 
 #define PWR_DOMAIN_AT_MAX_LVL		1
 #define PLAT_MAX_PWR_LVL		2
-#define PLAT_MAX_OFF_STATE		2
+#define PLAT_MAX_OFF_STATE		4
 #define PLAT_MAX_RET_STATE		1
 
+#define PLAT_WAIT_OFF_STATE		2
+#define PLAT_STOP_OFF_STATE		3
+
 #define BL31_BASE			0x910000
 #define BL31_LIMIT			0x920000
 #define BL32_BASE			0xfe000000
diff --git a/plat/imx/imx8mq/include/soc.h b/plat/imx/imx8mq/include/soc.h
index 65a49d00..af6fc09e 100644
--- a/plat/imx/imx8mq/include/soc.h
+++ b/plat/imx/imx8mq/include/soc.h
@@ -7,55 +7,22 @@
 #ifndef __IMX_SOC_H
 #define __IMX_SOC_H
 
-enum imx_cpu_pwr_mode {
-	WAIT_CLOCKED,			/* wfi only */
-	WAIT_UNCLOCKED,			/* WAIT */
-	WAIT_UNCLOCKED_POWER_OFF,	/* WAIT + SRPG */
-	STOP_POWER_ON,			/* just STOP */
-	STOP_POWER_OFF,			/* STOP + SRPG */
-};
-
-enum imx_gpc_slot {
-	A53_CORE0,
-	A53_CORE1,
-	A53_CORE2,
-	A53_CORE3,
-	A53_SCU,
-};
-
-enum imx_gpc_pu_slot {
-	FAST_MEGA_MIX,
-	MIPI_PHY,
-	PCIE1_PHY,
-	OTG1_PHY,
-	OTG2_PHY,
-	RESERVED,
-	CORE1_M4,
-	DDR1_PHY,
-	DDR2_PHY,
-	GPU,
-	VPU,
-	HDMI_PHY,
-	DSIP,
-	MIPI_CSI1,
-	MIPI_CSI2,
-	PCIE2_PHY,
-};
-
 void imx_gpc_set_m_core_pgc(unsigned int cpu, bool pdn);
-void imx_gpc_set_lpm_mode(enum imx_cpu_pwr_mode mode);
-void imx_gpc_set_cpu_power_gate_by_lpm(unsigned int cpu, bool pdn);
-void imx_gpc_set_plat_power_gate_by_lpm(bool pdn);
-void imx_gpc_set_core_pdn_pup_by_software(unsigned int cpu, bool pdn);
-void imx_gpc_set_cpu_ppower_gate_by_wfi(unsigned int cpu, bool pdn);
-void imx_gpc_pre_suspend(bool arm_power_off);
-void imx_gpc_post_resume(void);
+void imx_anamix_pre_suspend(void);
+void imx_anamix_post_resume(void);
 void imx_gpc_init(void);
 
+void imx_set_cpu_secure_entry(int cpu_id, uintptr_t sec_entrypoint);
+void imx_set_cpu_pwr_off(int cpu_id);
+void imx_set_cpu_pwr_on(int cpu_id);
+void imx_set_cpu_lpm(int cpu_id, bool pdn);
+void imx_set_cluster_standby(bool pdn);
+void imx_set_cluster_powerdown(int last_core, bool pdn);
+void imx_set_sys_lpm(bool retention);
+void imx_set_rbc_count(void);
+void imx_clear_rbc_count(void);
+
 void ddrc_enter_retention(void);
 void ddrc_exit_retention(void);
 
-void imx_enable_cpu(unsigned int cpu, bool enable);
-int imx_is_m4_enabled(void);
-void imx_set_cpu_jump_addr(unsigned int cpu, void *jump_addr);
 #endif /* __IMX_SOC_H */