From c6a3b2479299fc8261fa65b60fa5172d7f7887c7 Mon Sep 17 00:00:00 2001
From: Jacky Bai <ping.bai@nxp.com>
Date: Thu, 26 Mar 2020 15:24:03 +0800
Subject: plat: imx8mp: imx8mp wait mode workaround

Add the i.MX8MP workaround for wait mode just for Alpha
release, this patch will be dropped in the future.

Signed-off-by: Jacky Bai <ping.bai@nxp.com>
---
 plat/imx/imx8m/imx8mp/imx8mp_psci.c | 46 +++++++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

(limited to 'plat/imx/imx8m/imx8mp/imx8mp_psci.c')

diff --git a/plat/imx/imx8m/imx8mp/imx8mp_psci.c b/plat/imx/imx8m/imx8mp/imx8mp_psci.c
index f541fc13..96bed88e 100644
--- a/plat/imx/imx8m/imx8mp/imx8mp_psci.c
+++ b/plat/imx/imx8m/imx8mp/imx8mp_psci.c
@@ -9,13 +9,59 @@
 #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 <dram.h>
 #include <gpc.h>
 #include <imx8m_psci.h>
 #include <plat_imx8.h>
 
+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);
+
+	/* TODO: Find out why this is still
+	 * needed in order not to break suspend */
+	udelay(50);
+}
+
+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))) {
+		if (!imx_m4_lpa_active()) {
+			imx_noc_wrapper_post_resume(core_id);
+			imx_anamix_override(false);
+			dram_exit_retention();
+			imx_set_sys_lpm(core_id, false);
+		}
+	imx_set_sys_wakeup(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))) {
+		/* mark this core as awake by masking IRQ0 */
+		imx_gpc_set_a53_core_awake(core_id);
+		imx_set_cpu_lpm(core_id, false);
+		plat_gic_cpuif_enable();
+	} else {
+		write_scr_el3(read_scr_el3() & (~SCR_FIQ_BIT));
+		isb();
+	}
+}
+
 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,
-- 
cgit v1.2.3