plat: imx8m: refactor the code to make it reusable

for the i.MX8M SOCs, part of the code for gpc
and PSCI implementation can be reused and make it
common for all these SoCs. this patch extracts
the common part for reuse.
Signed-off-by: Jacky Bai <ping.bai@nxp.com>

plat/imx/imx8m/gpc_common.c

+/*
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <lib/psci/psci.h>
+
+#include <gpc.h>
+#include <imx8m_psci.h>
+#include <plat_imx8.h>
+
+static uint32_t gpc_imr_offset[] = { 0x30, 0x40, 0x1c0, 0x1d0, };
+
+#pragma weak imx_set_cpu_pwr_off
+#pragma weak imx_set_cpu_pwr_on
+#pragma weak imx_set_cpu_lpm
+#pragma weak imx_set_cluster_powerdown
+
+void imx_set_cpu_secure_entry(unsigned int core_id, uintptr_t sec_entrypoint)
+{
+	uint64_t temp_base;
+
+	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));
+}
+
+void imx_set_cpu_pwr_off(unsigned int core_id)
+{
+	/* enable the wfi power down of the core */
+	mmio_setbits_32(IMX_GPC_BASE + LPCR_A53_AD, COREx_WFI_PDN(core_id));
+	/* assert the pcg pcr bit of the core */
+	mmio_setbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
+}
+
+void imx_set_cpu_pwr_on(unsigned int core_id)
+{
+	/* clear the wfi power down bit of the core */
+	mmio_clrbits_32(IMX_GPC_BASE + LPCR_A53_AD, COREx_WFI_PDN(core_id));
+	/* assert the ncpuporeset */
+	mmio_clrbits_32(IMX_SRC_BASE + SRC_A53RCR1, (1 << core_id));
+	/* assert the pcg pcr bit of the core */
+	mmio_setbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
+	/* sw power up the core */
+	mmio_setbits_32(IMX_GPC_BASE + CPU_PGC_UP_TRG, (1 << core_id));
+
+	/* wait for the power up finished */
+	while ((mmio_read_32(IMX_GPC_BASE + CPU_PGC_UP_TRG) & (1 << core_id)) != 0)
+		;
+
+	/* deassert the pcg pcr bit of the core */
+	mmio_clrbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
+	/* deassert the ncpuporeset */
+	mmio_setbits_32(IMX_SRC_BASE + SRC_A53RCR1, (1 << core_id));
+}
+
+void imx_set_cpu_lpm(unsigned int core_id, bool pdn)
+{
+	if (pdn) {
+		/* enable the core WFI PDN & IRQ PUP */
+		mmio_setbits_32(IMX_GPC_BASE + LPCR_A53_AD, COREx_WFI_PDN(core_id) |
+				COREx_IRQ_WUP(core_id));
+		/* assert the pcg pcr bit of the core */
+		mmio_setbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
+	} else {
+		/* disbale CORE WFI PDN & IRQ PUP */
+		mmio_clrbits_32(IMX_GPC_BASE + LPCR_A53_AD, COREx_WFI_PDN(core_id) |
+				COREx_IRQ_WUP(core_id));
+		/* deassert the pcg pcr bit of the core */
+		mmio_clrbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
+	}
+}
+
+/*
+ * the plat and noc can only be power up & down by slot method,
+ * slot0: plat power down; slot1: noc power down; slot2: noc power up;
+ * slot3: plat power up. plat's pup&pdn ack is used by default. if
+ * noc is config to power down, then noc's pdn ack should be used.
+ */
+static void imx_a53_plat_slot_config(bool pdn)
+{
+	if (pdn) {
+		mmio_setbits_32(IMX_GPC_BASE + SLTx_CFG(0), PLAT_PDN_SLT_CTRL);
+		mmio_setbits_32(IMX_GPC_BASE + SLTx_CFG(3), PLAT_PUP_SLT_CTRL);
+		mmio_write_32(IMX_GPC_BASE + PGC_ACK_SEL_A53, A53_PLAT_PDN_ACK |
+			A53_PLAT_PUP_ACK);
+		mmio_setbits_32(IMX_GPC_BASE + PLAT_PGC_PCR, 0x1);
+	} else {
+		mmio_clrbits_32(IMX_GPC_BASE + SLTx_CFG(0), PLAT_PDN_SLT_CTRL);
+		mmio_clrbits_32(IMX_GPC_BASE + SLTx_CFG(3), PLAT_PUP_SLT_CTRL);
+		mmio_write_32(IMX_GPC_BASE + PGC_ACK_SEL_A53, A53_DUMMY_PUP_ACK |
+			A53_DUMMY_PDN_ACK);
+		mmio_clrbits_32(IMX_GPC_BASE + PLAT_PGC_PCR, 0x1);
+	}
+}
+
+void imx_set_cluster_standby(bool enter)
+{
+	/*
+	 * Enable BIT 6 of A53 AD register to make sure system
+	 * don't enter LPM mode.
+	 */
+	if (enter)
+		mmio_setbits_32(IMX_GPC_BASE + LPCR_A53_AD, (1 << 6));
+	else
+		mmio_clrbits_32(IMX_GPC_BASE + LPCR_A53_AD, (1 << 6));
+}
+
+/* i.mx8mq need to override it */
+void imx_set_cluster_powerdown(unsigned int last_core, uint8_t power_state)
+{
+	uint32_t val;
+
+	if (!is_local_state_run(power_state)) {
+		/* config C0~1's LPM, enable a53 clock off in LPM */
+		mmio_clrsetbits_32(IMX_GPC_BASE + LPCR_A53_BSC, A53_CLK_ON_LPM,
+			LPM_MODE(power_state));
+		/* config C2-3's LPM */
+		mmio_setbits_32(IMX_GPC_BASE + LPCR_A53_BSC2, LPM_MODE(power_state));
+
+		/* enable PLAT/SCU power down */
+		val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_AD);
+		val &= ~EN_L2_WFI_PDN;
+		/* L2 cache memory is on in WAIT mode */
+		if (is_local_state_off(power_state))
+			val |= (L2PGE | EN_PLAT_PDN);
+		else
+			val |= EN_PLAT_PDN;
+
+		mmio_write_32(IMX_GPC_BASE + LPCR_A53_AD, val);
+
+		imx_a53_plat_slot_config(true);
+	} else {
+		/* clear the slot and ack for cluster power down */
+		imx_a53_plat_slot_config(false);
+		/* reverse the cluster level setting */
+		mmio_clrsetbits_32(IMX_GPC_BASE + LPCR_A53_BSC, 0xf, A53_CLK_ON_LPM);
+		mmio_clrbits_32(IMX_GPC_BASE + LPCR_A53_BSC2, 0xf);
+
+		/* clear PLAT/SCU power down */
+		mmio_clrsetbits_32(IMX_GPC_BASE + LPCR_A53_AD, (L2PGE | EN_PLAT_PDN),
+			EN_L2_WFI_PDN);
+	}
+}
+
+static unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISENABLER_SHIFT;
+
+	return mmio_read_32(base + GICD_ISENABLER + (n << 2));
+}
+
+/*
+ * gic's clock will be gated in system suspend, so gic has no ability to
+ * to wakeup the system, we need to config the imr based on the irq
+ * enable status in gic, then gpc will monitor the wakeup irq
+ */
+void imx_set_sys_wakeup(unsigned int last_core, bool pdn)
+{
+	uint32_t irq_mask;
+	uintptr_t gicd_base = PLAT_GICD_BASE;
+
+	if (pdn)
+		mmio_clrsetbits_32(IMX_GPC_BASE + LPCR_A53_BSC, A53_CORE_WUP_SRC(last_core),
+			IRQ_SRC_A53_WUP);
+	else
+		mmio_clrsetbits_32(IMX_GPC_BASE + LPCR_A53_BSC, IRQ_SRC_A53_WUP,
+			A53_CORE_WUP_SRC(last_core));
+
+	/* clear last core's IMR based on GIC's mask setting */
+	for (int i = 0; i < IRQ_IMR_NUM; i++) {
+		if (pdn)
+			/* set the wakeup irq base GIC */
+			irq_mask = ~gicd_read_isenabler(gicd_base, 32 * (i + 1));
+		else
+			irq_mask = IMR_MASK_ALL;
+
+		mmio_write_32(IMX_GPC_BASE + gpc_imr_offset[last_core] + i * 4,
+			      irq_mask);
+	}
+}
+
+#pragma weak imx_noc_slot_config
+/*
+ * this function only need to be override by platform
+ * that support noc power down, for example: imx8mm.
+ *  otherwize, keep it empty.
+ */
+void imx_noc_slot_config(bool pdn)
+{
+
+}
+
+/* this is common for all imx8m soc */
+void imx_set_sys_lpm(unsigned int last_core, bool retention)
+{
+	uint32_t val;
+
+	val = mmio_read_32(IMX_GPC_BASE + SLPCR);
+	val &= ~(SLPCR_EN_DSM | SLPCR_VSTBY | SLPCR_SBYOS |
+		 SLPCR_BYPASS_PMIC_READY | SLPCR_A53_FASTWUP_STOP_MODE);
+
+	if (retention)
+		val |= (SLPCR_EN_DSM | SLPCR_VSTBY | SLPCR_SBYOS |
+			SLPCR_BYPASS_PMIC_READY | SLPCR_A53_FASTWUP_STOP_MODE);
+
+	mmio_write_32(IMX_GPC_BASE + SLPCR, val);
+
+	/* config the noc power down */
+	imx_noc_slot_config(retention);
+
+	/* config wakeup irqs' mask in gpc */
+	imx_set_sys_wakeup(last_core, retention);
+}
+
+void imx_set_rbc_count(void)
+{
+	mmio_setbits_32(IMX_GPC_BASE + SLPCR, SLPCR_RBC_EN |
+		(0x8 << SLPCR_RBC_COUNT_SHIFT));
+}
+
+void imx_clear_rbc_count(void)
+{
+	mmio_clrbits_32(IMX_GPC_BASE + SLPCR, SLPCR_RBC_EN |
+		(0x3f << SLPCR_RBC_COUNT_SHIFT));
+}

plat/imx/imx8m/imx8m_psci_common.c

+/*
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/psci/psci.h>
+
+#include <gpc.h>
+#include <imx8m_psci.h>
+#include <plat_imx8.h>
+
+/*
+ * below callback functions need to be override by i.mx8mq,
+ * for other i.mx8m soc, if no special requirement,
+ * reuse below ones.
+ */
+#pragma weak imx_validate_power_state
+#pragma weak imx_domain_suspend
+#pragma weak imx_domain_suspend_finish
+#pragma weak imx_get_sys_suspend_power_state
+
+int imx_validate_ns_entrypoint(uintptr_t ns_entrypoint)
+{
+	/* The non-secure entrypoint should be in RAM space */
+	if (ns_entrypoint < PLAT_NS_IMAGE_OFFSET)
+		return PSCI_E_INVALID_PARAMS;
+
+	return PSCI_E_SUCCESS;
+}
+
+int imx_pwr_domain_on(u_register_t mpidr)
+{
+	unsigned int core_id;
+	uint64_t base_addr = BL31_BASE;
+
+	core_id = MPIDR_AFFLVL0_VAL(mpidr);
+
+	imx_set_cpu_secure_entry(core_id, base_addr);
+	imx_set_cpu_pwr_on(core_id);
+
+	return PSCI_E_SUCCESS;
+}
+
+void imx_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	plat_gic_pcpu_init();
+	plat_gic_cpuif_enable();
+}
+
+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);
+
+	plat_gic_cpuif_disable();
+	imx_set_cpu_pwr_off(core_id);
+}
+
+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;
+
+	if (pwr_type == PSTATE_TYPE_STANDBY) {
+		CORE_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+		CLUSTER_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+	}
+
+	if (pwr_type == PSTATE_TYPE_POWERDOWN && state_id == 0x33) {
+		CORE_PWR_STATE(req_state) = PLAT_MAX_OFF_STATE;
+		CLUSTER_PWR_STATE(req_state) = PLAT_WAIT_RET_STATE;
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+void imx_cpu_standby(plat_local_state_t cpu_state)
+{
+	dsb();
+	write_scr_el3(read_scr_el3() | SCR_FIQ_BIT);
+	isb();
+
+	wfi();
+
+	write_scr_el3(read_scr_el3() & (~SCR_FIQ_BIT));
+	isb();
+}
+
+void imx_domain_suspend(const psci_power_state_t *target_state)
+{
+	uint64_t base_addr = BL31_BASE;
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL0_VAL(mpidr);
+
+	if (is_local_state_off(CORE_PWR_STATE(target_state))) {
+		plat_gic_cpuif_disable();
+		imx_set_cpu_secure_entry(core_id, base_addr);
+		imx_set_cpu_lpm(core_id, true);
+	} else {
+		dsb();
+		write_scr_el3(read_scr_el3() | SCR_FIQ_BIT);
+		isb();
+	}
+
+	if (!is_local_state_run(CLUSTER_PWR_STATE(target_state)))
+		imx_set_cluster_powerdown(core_id, CLUSTER_PWR_STATE(target_state));
+
+	if (is_local_state_off(SYSTEM_PWR_STATE(target_state)))
+		imx_set_sys_lpm(core_id, true);
+}
+
+void imx_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL0_VAL(mpidr);
+
+	if (is_local_state_off(SYSTEM_PWR_STATE(target_state)))
+		imx_set_sys_lpm(core_id, false);
+
+	if (!is_local_state_run(CLUSTER_PWR_STATE(target_state))) {
+		imx_clear_rbc_count();
+		imx_set_cluster_powerdown(core_id, PSCI_LOCAL_STATE_RUN);
+	}
+
+	if (is_local_state_off(CORE_PWR_STATE(target_state))) {
+		imx_set_cpu_lpm(core_id, false);
+		plat_gic_cpuif_enable();
+	} else {
+		write_scr_el3(read_scr_el3() & (~SCR_FIQ_BIT));
+		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_STOP_OFF_STATE;
+}
+
+void __dead2 imx_system_reset(void)
+{
+	uintptr_t wdog_base = IMX_WDOG_BASE;
+	unsigned int val;
+
+	/* WDOG_B reset */
+	val = mmio_read_16(wdog_base);
+#ifdef IMX_WDOG_B_RESET
+	val = (val & 0x00FF) | WDOG_WCR_WDZST | WDOG_WCR_WDE |
+		WDOG_WCR_WDT | WDOG_WCR_SRS;
+#else
+	val = (val & 0x00FF) | WDOG_WCR_WDZST | WDOG_WCR_SRS;
+#endif
+	mmio_write_16(wdog_base, val);
+
+	mmio_write_16(wdog_base + WDOG_WSR, 0x5555);
+	mmio_write_16(wdog_base + WDOG_WSR, 0xaaaa);
+	while (1)
+		;
+}
+
+void __dead2 imx_system_off(void)
+{
+	mmio_write_32(IMX_SNVS_BASE + SNVS_LPCR, SNVS_LPCR_SRTC_ENV |
+			SNVS_LPCR_DP_EN | SNVS_LPCR_TOP);
+
+	while (1)
+		;
+}
+
+void __dead2 imx_pwr_domain_pwr_down_wfi(const psci_power_state_t *target_state)
+{
+	/*
+	 * before enter WAIT or STOP mode with PLAT(SCU) power down,
+	 * rbc count need to be enabled to make sure PLAT is
+	 * power down successfully even if the the wakeup IRQ is pending
+	 * early before the power down sequence. the RBC counter is
+	 * drived by the 32K OSC, so delay 30us to make sure the counter
+	 * is really running.
+	 */
+	if (!is_local_state_run(CLUSTER_PWR_STATE(target_state))) {
+		imx_set_rbc_count();
+		udelay(30);
+	}
+
+	while (1)
+		wfi();
+}

plat/imx/imx8m/imx8mq/gpc.c

@@ -16,19 +16,6 @@
 
 #include <gpc.h>
 
-void imx_set_cpu_secure_entry(unsigned int core_id, uintptr_t sec_entrypoint)
-{
-	uint64_t temp_base;
-
-	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));
-}
-
 /* use wfi power down the core */
 void imx_set_cpu_pwr_off(unsigned int core_id)
 {
@@ -39,28 +26,6 @@ void imx_set_cpu_pwr_off(unsigned int core_id)
 	mmio_setbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
 };
 
-/* use the sw method to power up the core */
-void imx_set_cpu_pwr_on(unsigned int core_id)
-{
-	/* clear the wfi power down bit of the core */
-	mmio_clrbits_32(IMX_GPC_BASE + LPCR_A53_AD, COREx_WFI_PDN(core_id));
-	/* assert the ncpuporeset */
-	mmio_clrbits_32(IMX_SRC_BASE + SRC_A53RCR1, (1 << core_id));
-	/* assert the pcg pcr bit of the core */
-	mmio_setbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
-	/* sw power up the core */
-	mmio_setbits_32(IMX_GPC_BASE + CPU_PGC_UP_TRG, (1 << core_id));
-
-	/* wait for the power up finished */
-	while ((mmio_read_32(IMX_GPC_BASE + CPU_PGC_UP_TRG) & (1 << core_id)) != 0)
-		;
-
-	/* deassert the pcg pcr bit of the core */
-	mmio_clrbits_32(IMX_GPC_BASE + COREx_PGC_PCR(core_id), 0x1);
-	/* deassert the ncpuporeset */
-	mmio_setbits_32(IMX_SRC_BASE + SRC_A53RCR1, (1 << core_id));
-}
-
 /* if out of lpm, we need to do reverse steps */
 void imx_set_cpu_lpm(unsigned int core_id, bool pdn)
 {
@@ -79,11 +44,6 @@ void imx_set_cpu_lpm(unsigned int core_id, bool pdn)
 	}
 }
 
-void imx_set_sys_wakeup(unsigned int last_core, bool pdn)
-{
-	/* TODO */
-}
-
 void imx_pup_pdn_slot_config(int last_core, bool pdn)
 {
 	if (pdn) {
@@ -105,18 +65,6 @@ void imx_pup_pdn_slot_config(int last_core, bool pdn)
 	}
 }
 
-void imx_set_cluster_standby(bool retention)
-{
-	/*
-	 * Enable BIT 6 of A53 AD register to make sure system
-	 * don't enter LPM mode.
-	 */
-	if (retention)
-		mmio_setbits_32(IMX_GPC_BASE + LPCR_A53_AD, (1 << 6));
-	else
-		mmio_clrbits_32(IMX_GPC_BASE + LPCR_A53_AD, (1 << 6));
-}
-
 void imx_set_cluster_powerdown(unsigned int last_core, uint8_t power_state)
 {
 	uint32_t val;
@@ -166,34 +114,6 @@ void imx_set_cluster_powerdown(unsigned int last_core, uint8_t power_state)
 	}
 }
 
-/* config the system level power mode */
-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);
-
-	if (retention)
-		val |= (SLPCR_EN_DSM | SLPCR_VSTBY | SLPCR_SBYOS |
-			 SLPCR_BYPASS_PMIC_READY | SLPCR_RBC_EN |
-			 SLPCR_A53_FASTWUP_STOP_MODE);
-
-	mmio_write_32(IMX_GPC_BASE + SLPCR, val);
-}
-
-void imx_set_rbc_count(void)
-{
-	mmio_setbits_32(IMX_GPC_BASE + SLPCR, 0x3f << SLPCR_RBC_COUNT_SHIFT);
-}
-
-void imx_clear_rbc_count(void)
-{
-	mmio_clrbits_32(IMX_GPC_BASE + SLPCR, 0x3f << SLPCR_RBC_COUNT_SHIFT);
-}
-
 void imx_gpc_init(void)
 {
 	uint32_t val;

plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c

@@ -15,6 +15,7 @@
 #include <context.h>
 #include <drivers/arm/tzc380.h>
 #include <drivers/console.h>
+#include <drivers/generic_delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
 #include <lib/xlat_tables/xlat_tables.h>
@@ -128,6 +129,8 @@ void bl31_plat_arch_setup(void)
 
 void bl31_platform_setup(void)
 {
+	generic_delay_timer_init();
+
 	/* init the GICv3 cpu and distributor interface */
 	plat_gic_driver_init();
 	plat_gic_init();

plat/imx/imx8m/imx8mq/imx8mq_psci.c

@@ -13,55 +13,9 @@
 #include <lib/psci/psci.h>
 
 #include <gpc.h>
+#include <imx8m_psci.h>
 #include <plat_imx8.h>
 
-#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])
-
-int imx_pwr_domain_on(u_register_t mpidr)
-{
-	unsigned int core_id;
-	uint64_t base_addr = BL31_BASE;
-
-	core_id = MPIDR_AFFLVL0_VAL(mpidr);
-
-	/* 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 PSCI_E_SUCCESS;
-}
-
-void imx_pwr_domain_on_finish(const psci_power_state_t *target_state)
-{
-	/* program the GIC per cpu dist and rdist interface */
-	plat_gic_pcpu_init();
-	/* enable the GICv3 cpu interface */
-	plat_gic_cpuif_enable();
-}
-
-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)
-{
-	/* The non-secure entrypoint should be in RAM space */
-	if (ns_entrypoint < PLAT_NS_IMAGE_OFFSET)
-		return PSCI_E_INVALID_PARAMS;
-
-	return PSCI_E_SUCCESS;
-}
-
 int imx_validate_power_state(unsigned int power_state,
 			 psci_power_state_t *req_state)
 {
@@ -85,18 +39,6 @@ int imx_validate_power_state(unsigned int power_state,
 	return PSCI_E_SUCCESS;
 }
 
-void imx_cpu_standby(plat_local_state_t cpu_state)
-{
-	dsb();
-	write_scr_el3(read_scr_el3() | SCR_FIQ_BIT);
-	isb();
-
-	wfi();
-
-	write_scr_el3(read_scr_el3() & (~SCR_FIQ_BIT));
-	isb();
-}
-
 void imx_domain_suspend(const psci_power_state_t *target_state)
 {
 	uint64_t base_addr = BL31_BASE;
@@ -120,7 +62,7 @@ void imx_domain_suspend(const psci_power_state_t *target_state)
 		imx_set_cluster_standby(true);
 
 	if (is_local_state_retn(SYSTEM_PWR_STATE(target_state))) {
-		imx_set_sys_lpm(true);
+		imx_set_sys_lpm(core_id, true);
 	}
 }
 
@@ -131,7 +73,7 @@ void imx_domain_suspend_finish(const psci_power_state_t *target_state)
 
 	/* check the system level status */
 	if (is_local_state_retn(SYSTEM_PWR_STATE(target_state))) {
-		imx_set_sys_lpm(false);
+		imx_set_sys_lpm(core_id, false);
 		imx_clear_rbc_count();
 	}
 
@@ -163,47 +105,6 @@ void imx_get_sys_suspend_power_state(psci_power_state_t *req_state)
 	req_state->pwr_domain_state[PLAT_MAX_PWR_LVL] = PLAT_MAX_RET_STATE;
 }
 
-void __dead2 imx_system_reset(void)
-{
-	uintptr_t wdog_base = IMX_WDOG_BASE;
-	unsigned int val;
-
-	/* WDOG_B reset */
-	val = mmio_read_16(wdog_base);
-#ifdef IMX_WDOG_B_RESET
-	val = (val & 0x00FF) | WDOG_WCR_WDZST | WDOG_WCR_WDE |
-		WDOG_WCR_WDT | WDOG_WCR_SRS;
-#else
-	val = (val & 0x00FF) | WDOG_WCR_WDZST | WDOG_WCR_SRS;
-#endif
-	mmio_write_16(wdog_base, val);
-
-	mmio_write_16(wdog_base + WDOG_WSR, 0x5555);
-	mmio_write_16(wdog_base + WDOG_WSR, 0xaaaa);
-	while (1)
-		;
-}
-
-
-
-void __dead2 imx_system_off(void)
-{
-	mmio_write_32(IMX_SNVS_BASE + SNVS_LPCR, SNVS_LPCR_SRTC_ENV |
-			SNVS_LPCR_DP_EN | SNVS_LPCR_TOP);
-
-	while (1)
-		;
-}
-
-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,

plat/imx/imx8m/imx8mq/include/platform_def.h

@@ -26,6 +26,7 @@
 #define PLAT_MAX_OFF_STATE		U(4)
 #define PLAT_MAX_RET_STATE		U(1)
 
+#define PLAT_WAIT_RET_STATE		PLAT_MAX_RET_STATE
 #define PLAT_WAIT_OFF_STATE		U(2)
 #define PLAT_STOP_OFF_STATE		U(3)
 

plat/imx/imx8m/imx8mq/platform.mk

@@ -20,6 +20,8 @@ IMX_GIC_SOURCES		:=	drivers/arm/gic/v3/gicv3_helpers.c	\
 BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c	\
 				plat/imx/imx8m/imx8mq/imx8mq_psci.c		\
+				plat/imx/imx8m/gpc_common.c			\
+				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mq/gpc.c			\
 				plat/imx/common/imx8_topology.c			\
 				plat/imx/common/imx_uart_console.S		\
@@ -28,6 +30,8 @@ BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/console/aarch64/console.S		\
 				drivers/arm/tzc/tzc380.c			\
+				drivers/delay_timer/delay_timer.c		\
+				drivers/delay_timer/generic_delay_timer.c	\
 				${IMX_GIC_SOURCES}
 
 USE_COHERENT_MEM	:=	1

plat/imx/imx8m/include/gpc.h

 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,15 +29,18 @@
 #define PU_PGC_DN_TRG			0x104
 #define A53_CORE0_PGC			0x800
 #define A53_PLAT_PGC			0x900
+#define PLAT_PGC_PCR			0x900
 #define PGC_SCU_TIMING			0x910
 
 #define MASK_DSM_TRIGGER_A53		BIT(31)
 #define IRQ_SRC_A53_WUP			BIT(30)
+#define IRQ_SRC_A53_WUP_SHIFT		30
 #define IRQ_SRC_C1			BIT(29)
 #define IRQ_SRC_C0			BIT(28)
 #define IRQ_SRC_C3			BIT(23)
 #define IRQ_SRC_C2			BIT(22)
 #define CPU_CLOCK_ON_LPM		BIT(14)
+#define A53_CLK_ON_LPM			BIT(14)
 #define MASTER0_LPM_HSK			BIT(6)
 
 #define L2PGE				BIT(31)
@@ -52,12 +55,16 @@
 #define SLPCR_SBYOS			BIT(1)
 #define SLPCR_BYPASS_PMIC_READY		BIT(0)
 #define SLPCR_RBC_COUNT_SHIFT		24
+#define SLPCR_STBY_COUNT_SHFT		3
 
 #define A53_DUMMY_PDN_ACK		BIT(15)
 #define A53_DUMMY_PUP_ACK		BIT(31)
 #define A53_PLAT_PDN_ACK		BIT(2)
 #define A53_PLAT_PUP_ACK		BIT(18)
 
+#define PLAT_PUP_SLT_CTRL		BIT(9)
+#define PLAT_PDN_SLT_CTRL		BIT(8)
+
 #define SLT_PLAT_PDN			BIT(8)
 #define SLT_PLAT_PUP			BIT(9)
 
@@ -65,6 +72,7 @@
 #define A53_LPM_MASK	U(0xF)
 #define A53_LPM_WAIT	U(0x5)
 #define A53_LPM_STOP	U(0xA)
+#define LPM_MODE(local_state)		((local_state) == PLAT_WAIT_RET_STATE ? A53_LPM_WAIT : A53_LPM_STOP)
 
 #define DSM_MODE_MASK	BIT(31)
 
@@ -76,6 +84,9 @@
 #define SLTx_CFG(n)			((SLT0_CFG + ((n) * 4)))
 #define SLT_COREx_PUP(core_id)		(0x2 << ((core_id) * 2))
 
+#define IRQ_IMR_NUM	4
+#define IMR_MASK_ALL	0xffffffff
+
 /* function declare */
 void imx_gpc_init(void);
 void imx_set_cpu_secure_entry(unsigned int core_index, uintptr_t sec_entrypoint);
@@ -84,7 +95,9 @@ void imx_set_cpu_pwr_on(unsigned int core_index);
 void imx_set_cpu_lpm(unsigned int core_index, bool pdn);
 void imx_set_cluster_standby(bool retention);
 void imx_set_cluster_powerdown(unsigned int last_core, uint8_t power_state);
-void imx_set_sys_lpm(bool retention);
+void imx_noc_slot_config(bool pdn);
+void imx_set_sys_wakeup(unsigned int last_core, bool pdn);
+void imx_set_sys_lpm(unsigned last_core, bool retention);
 void imx_set_rbc_count(void);
 void imx_clear_rbc_count(void);
 

plat/imx/imx8m/include/imx8m_psci.h

+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX8M_PSCI_H
+#define IMX8M_PSCI_H
+
+#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])
+
+int imx_pwr_domain_on(u_register_t mpidr);
+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);
+int imx_validate_ns_entrypoint(uintptr_t ns_entrypoint);
+int imx_validate_power_state(unsigned int power_state, psci_power_state_t *rq_state);
+void imx_cpu_standby(plat_local_state_t cpu_state);
+void imx_domain_suspend(const psci_power_state_t *target_state);
+void imx_domain_suspend_finish(const psci_power_state_t *target_state);
+void imx_get_sys_suspend_power_state(psci_power_state_t *req_state);
+void __dead2 imx_system_reset(void);
+void __dead2 imx_system_off(void);
+void __dead2 imx_pwr_domain_pwr_down_wfi(const psci_power_state_t *target_state);
+
+#endif /* IMX8M_PSCI_H */