plat: imx8mq: Add workaround code for ERR11171 on imx8mq

This new workaround takes advantage of the per core IMR registers in GPC in
order to unmask the IRQ0, still generated by the 12bit in IOMUX_GPR register
(which now remains always set), so it can only wake up one core at the time.
Also, this entire workaround has now been moved here in TF-A, allowing the
kernel side to be minimal.

Another advantage this workaround brings is the removal of the 50us delay
(which was necessary before in gic_raise_softirq in kernel) by allowing the
core that is waking up to mask his own IRQ0 in the suspend finish callback.

One important change here is the way the cores are woken up in
dram_dvfs_handler. Since the wake up mechanism has changed from asserting the
12th bit in IOMUX_GPR and leaving the IMR1 1st bit on for each core to exactly
the reverse, that is, leaving the IOMUX_GPR 12th bit always set and then
masking/unmasking the IMR1 1st bit for each independent core, we need to use
the imx_gpc_core_wake to wake up the cores.

Also, the 50us udelay is moved to TF-A (inside imx_pwr_domain_off) from kernel
(gic_raise_softirq), since the new cpuidle workaround does not need it in order
to clean the IOMUX_GPC 12bit. For now, the udelay seems to be still needed
in order to delay the affinity info OFF for the dying core. This is something
that needs further investigation.

Signed-off-by: Abel Vesa <abel.vesa@nxp.com>
Signed-off-by: Jacky Bai <ping.bai@nxp.com>

8 files changed, 235 insertions, 12 deletions

diff --git a/plat/imx/common/imx_sip_svc.c b/plat/imx/common/imx_sip_svc.c
index c9c723f2..735cbe32 100644
--- a/plat/imx/common/imx_sip_svc.c
+++ b/plat/imx/common/imx_sip_svc.c
@@ -30,6 +30,9 @@ static uintptr_t imx_sip_handler(unsigned int smc_fid,
 	case IMX_SIP_GET_SOC_INFO:
 		SMC_RET1(handle, imx_soc_info_handler(smc_fid, x1, x2, x3));
 		break;
+	case IMX_SIP_GPC:
+		SMC_RET1(handle, imx_gpc_handler(smc_fid, x1, x2, x3));
+		break;
 #endif
 #if defined(PLAT_imx8mm) || defined(PLAT_imx8mn)
 	case IMX_SIP_DDR_DVFS:
diff --git a/plat/imx/common/include/imx_sip_svc.h b/plat/imx/common/include/imx_sip_svc.h
index dbf6ab87..38097070 100644
--- a/plat/imx/common/include/imx_sip_svc.h
+++ b/plat/imx/common/include/imx_sip_svc.h
@@ -41,6 +41,8 @@
 #if defined(PLAT_imx8mq)
 int imx_soc_info_handler(uint32_t smc_fid, u_register_t x1,
 			 u_register_t x2, u_register_t x3);
+int imx_gpc_handler(uint32_t smc_fid, u_register_t x1,
+		    u_register_t x2, u_register_t x3);
 #endif
 #if defined(PLAT_imx8mm) || defined(PLAT_imx8mn)
 int dram_dvfs_handler(uint32_t smc_fid, void *handle,
diff --git a/plat/imx/imx8m/gpc_common.c b/plat/imx/imx8m/gpc_common.c
index 3233a7c8..fa058cc8 100644
--- a/plat/imx/imx8m/gpc_common.c
+++ b/plat/imx/imx8m/gpc_common.c
@@ -179,6 +179,7 @@ static unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id)
 	return mmio_read_32(base + GICD_ISENABLER + (n << 2));
 }
 
+#pragma weak imx_set_sys_wakeup
 /*
  * 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
@@ -293,6 +294,7 @@ void imx_anamix_override(bool enter)
 	}
 }
 
+#pragma weak imx_gpc_handler
 int imx_gpc_handler(uint32_t smc_fid, u_register_t x1, u_register_t x2, u_register_t x3)
 {
 	switch(x1) {
diff --git a/plat/imx/imx8m/imx8m_psci_common.c b/plat/imx/imx8m/imx8m_psci_common.c
index 68ac23d5..fc9a585e 100644
--- a/plat/imx/imx8m/imx8m_psci_common.c
+++ b/plat/imx/imx8m/imx8m_psci_common.c
@@ -24,6 +24,7 @@
  * reuse below ones.
  */
 #pragma weak imx_validate_power_state
+#pragma weak imx_pwr_domain_off
 #pragma weak imx_domain_suspend
 #pragma weak imx_domain_suspend_finish
 #pragma weak imx_get_sys_suspend_power_state
diff --git a/plat/imx/imx8m/imx8mq/gpc.c b/plat/imx/imx8m/imx8mq/gpc.c
index 9b6e9951..c15c8739 100644
--- a/plat/imx/imx8m/imx8mq/gpc.c
+++ b/plat/imx/imx8m/imx8mq/gpc.c
@@ -8,16 +8,183 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/runtime_svc.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
+#include <lib/spinlock.h>
 #include <platform_def.h>
+#include <plat/common/platform.h>
 #include <services/std_svc.h>
 
 #include <gpc.h>
 
+#define FSL_SIP_CONFIG_GPC_MASK		0x00
+#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_CONFIG_GPC_CORE_WAKE	0x05
+
+static uint32_t gpc_saved_imrs[16];
+static uint32_t gpc_wake_irqs[4];
+static uint32_t gpc_imr_offset[] = {
+	IMX_GPC_BASE + IMR1_CORE0_A53,
+	IMX_GPC_BASE + IMR1_CORE1_A53,
+	IMX_GPC_BASE + IMR1_CORE2_A53,
+	IMX_GPC_BASE + IMR1_CORE3_A53,
+	IMX_GPC_BASE + IMR1_CORE0_M4,
+};
+
+spinlock_t gpc_imr_lock[4];
+
+static void gpc_imr_core_spin_lock(unsigned int core_id)
+{
+	spin_lock(&gpc_imr_lock[core_id]);
+}
+
+static void gpc_imr_core_spin_unlock(unsigned int core_id)
+{
+	spin_unlock(&gpc_imr_lock[core_id]);
+}
+
+static void gpc_save_imr_lpm(unsigned int core_id, unsigned int imr_idx)
+{
+	uint32_t reg = gpc_imr_offset[core_id] + imr_idx * 4;
+
+	gpc_imr_core_spin_lock(core_id);
+
+	gpc_saved_imrs[core_id + imr_idx * 4] = mmio_read_32(reg);
+	mmio_write_32(reg, ~gpc_wake_irqs[imr_idx]);
+
+	gpc_imr_core_spin_unlock(core_id);
+}
+
+static void gpc_restore_imr_lpm(unsigned int core_id, unsigned int imr_idx)
+{
+	uint32_t reg = gpc_imr_offset[core_id] + imr_idx * 4;
+	uint32_t val = gpc_saved_imrs[core_id + imr_idx * 4];
+
+	gpc_imr_core_spin_lock(core_id);
+
+	mmio_write_32(reg, val);
+
+	gpc_imr_core_spin_unlock(core_id);
+}
+
+/*
+ * 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_sys_wakeup(unsigned int last_core, bool pdn)
+{
+	unsigned int imr, core;
+
+	if (pdn)
+		for (imr = 0; imr < 4; imr++)
+			for (core = 0; core < 4; core++)
+				gpc_save_imr_lpm(core, imr);
+	else
+		for (imr = 0; imr < 4; imr++)
+			for (core = 0; core < 4; core++)
+				gpc_restore_imr_lpm(core, imr);
+}
+
+static void imx_gpc_hwirq_mask(unsigned int hwirq)
+{
+	uintptr_t reg;
+	unsigned int val;
+
+	gpc_imr_core_spin_lock(0);
+	reg = gpc_imr_offset[0] + (hwirq / 32) * 4;
+	val = mmio_read_32(reg);
+	val |= 1 << hwirq % 32;
+	mmio_write_32(reg, val);
+	gpc_imr_core_spin_unlock(0);
+}
+
+static void imx_gpc_hwirq_unmask(unsigned int hwirq)
+{
+	uintptr_t reg;
+	unsigned int val;
+
+	gpc_imr_core_spin_lock(0);
+	reg = gpc_imr_offset[0] + (hwirq / 32) * 4;
+	val = mmio_read_32(reg);
+	val &= ~(1 << hwirq % 32);
+	mmio_write_32(reg, val);
+	gpc_imr_core_spin_unlock(0);
+}
+
+static void imx_gpc_set_wake(uint32_t hwirq, unsigned int on)
+{
+	uint32_t mask, idx;
+
+	mask = 1 << hwirq % 32;
+	idx = hwirq / 32;
+	gpc_wake_irqs[idx] = on ? gpc_wake_irqs[idx] | mask :
+				 gpc_wake_irqs[idx] & ~mask;
+}
+
+static void imx_gpc_mask_irq0(uint32_t core_id, uint32_t mask)
+{
+	gpc_imr_core_spin_lock(core_id);
+	if (mask)
+		mmio_setbits_32(gpc_imr_offset[core_id], 1);
+	else
+		mmio_clrbits_32(gpc_imr_offset[core_id], 1);
+	dsb();
+	gpc_imr_core_spin_unlock(core_id);
+}
+
+void imx_gpc_core_wake(uint32_t cpumask)
+{
+	for (int i = 0; i < 4; i++)
+		if (cpumask & (1 << i))
+			imx_gpc_mask_irq0(i, false);
+}
+
+void imx_gpc_set_a53_core_awake(uint32_t core_id)
+{
+	imx_gpc_mask_irq0(core_id, true);
+}
+
+static void imx_gpc_set_affinity(uint32_t hwirq, unsigned 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.
+	 */
+	gpc_imr_core_spin_lock(cpu_idx);
+	reg = gpc_imr_offset[cpu_idx] + (hwirq / 32) * 4;
+	val = mmio_read_32(reg);
+	val &= ~(1 << hwirq % 32);
+	mmio_write_32(reg, val);
+	gpc_imr_core_spin_unlock(cpu_idx);
+
+	/* clear affinity of other core */
+	for (int i = 0; i < 4; i++) {
+		if (cpu_idx != i) {
+			gpc_imr_core_spin_lock(i);
+			reg = gpc_imr_offset[i] + (hwirq / 32) * 4;
+			val = mmio_read_32(reg);
+			val |= (1 << hwirq % 32);
+			mmio_write_32(reg, val);
+			gpc_imr_core_spin_unlock(i);
+		}
+	}
+}
+
 /* use wfi power down the core */
 void imx_set_cpu_pwr_off(unsigned int core_id)
 {
@@ -125,26 +292,53 @@ void imx_set_cluster_powerdown(unsigned int last_core, uint8_t power_state)
 	}
 }
 
+int imx_gpc_handler(uint32_t smc_fid,
+			  u_register_t x1,
+			  u_register_t x2,
+			  u_register_t x3)
+{
+	switch(x1) {
+	case FSL_SIP_CONFIG_GPC_CORE_WAKE:
+		imx_gpc_core_wake(x2);
+		break;
+	case FSL_SIP_CONFIG_GPC_SET_WAKE:
+		imx_gpc_set_wake(x2, x3);
+		break;
+	case FSL_SIP_CONFIG_GPC_MASK:
+		imx_gpc_hwirq_mask(x2);
+		break;
+	case FSL_SIP_CONFIG_GPC_UNMASK:
+		imx_gpc_hwirq_unmask(x2);
+		break;
+	case FSL_SIP_CONFIG_GPC_SET_AFF:
+		imx_gpc_set_affinity(x2, x3);
+		break;
+	default:
+		return SMC_UNK;
+	}
+
+	return 0;
+}
+
 void imx_gpc_init(void)
 {
 	uint32_t val;
-	int i;
+	int i, j;
+
 	/* mask all the interrupt by default */
-	for (i = 0; i < 4; i++) {
-		mmio_write_32(IMX_GPC_BASE + IMR1_CORE0_A53 + i * 4, ~0x0);
-		mmio_write_32(IMX_GPC_BASE + IMR1_CORE1_A53 + i * 4, ~0x0);
-		mmio_write_32(IMX_GPC_BASE + IMR1_CORE2_A53 + i * 4, ~0x0);
-		mmio_write_32(IMX_GPC_BASE + IMR1_CORE3_A53 + i * 4, ~0x0);
-		mmio_write_32(IMX_GPC_BASE + IMR1_CORE0_M4 + i * 4, ~0x0);
-	}
+	for (i = 0; i < 4; i++)
+		for (j = 0; j < 5; j++)
+			mmio_write_32(gpc_imr_offset[j] + i * 4, ~0x0);
+
 	/* Due to the hardware design requirement, need to make
 	 * sure GPR interrupt(#32) is unmasked during RUN mode to
 	 * avoid entering DSM mode by mistake.
 	 */
-	mmio_write_32(IMX_GPC_BASE + IMR1_CORE0_A53, 0xFFFFFFFE);
-	mmio_write_32(IMX_GPC_BASE + IMR1_CORE1_A53, 0xFFFFFFFE);
-	mmio_write_32(IMX_GPC_BASE + IMR1_CORE2_A53, 0xFFFFFFFE);
-	mmio_write_32(IMX_GPC_BASE + IMR1_CORE3_A53, 0xFFFFFFFE);
+	for (i = 0; i < 4; i++)
+		mmio_write_32(gpc_imr_offset[i], ~0x1);
+
+	/* leave the IOMUX_GPC bit 12 on for core wakeup */
+	mmio_setbits_32(IMX_IOMUX_GPR_BASE + 0x4, 1 << 12);
 
 	/* use external IRQs to wakeup C0~C3 from LPM */
 	val = mmio_read_32(IMX_GPC_BASE + LPCR_A53_BSC);
diff --git a/plat/imx/imx8m/imx8mq/imx8mq_psci.c b/plat/imx/imx8m/imx8mq/imx8mq_psci.c
index 78a6ea70..2d7befe2 100644
--- a/plat/imx/imx8m/imx8mq/imx8mq_psci.c
+++ b/plat/imx/imx8m/imx8mq/imx8mq_psci.c
@@ -9,6 +9,7 @@
 #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>
 
@@ -40,6 +41,19 @@ int imx_validate_power_state(unsigned int power_state,
 	return PSCI_E_SUCCESS;
 }
 
+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(const psci_power_state_t *target_state)
 {
 	uint64_t base_addr = BL31_BASE;
@@ -88,6 +102,8 @@ void imx_domain_suspend_finish(const psci_power_state_t *target_state)
 
 	/* check the core level power status */
 	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);
 		/* clear the core lpm setting */
 		imx_set_cpu_lpm(core_id, false);
 		/* enable the gic cpu interface */
diff --git a/plat/imx/imx8m/imx8mq/platform.mk b/plat/imx/imx8m/imx8mq/platform.mk
index 4dd659e3..fce4edc5 100644
--- a/plat/imx/imx8m/imx8mq/platform.mk
+++ b/plat/imx/imx8m/imx8mq/platform.mk
@@ -53,6 +53,7 @@ $(eval $(call add_define,STACK_IN_OCRAM_S))
 USE_COHERENT_MEM	:=	1
 RESET_TO_BL31		:=	1
 A53_DISABLE_NON_TEMPORAL_HINT := 0
+WARMBOOT_ENABLE_DCACHE_EARLY	:=	1
 
 ERRATA_A53_835769	:=	1
 ERRATA_A53_843419	:=	1
diff --git a/plat/imx/imx8m/include/gpc.h b/plat/imx/imx8m/include/gpc.h
index 3ea7d8ee..4e870303 100644
--- a/plat/imx/imx8m/include/gpc.h
+++ b/plat/imx/imx8m/include/gpc.h
@@ -135,4 +135,8 @@ void imx_clear_rbc_count(void);
 void imx_anamix_override(bool enter);
 void imx_gpc_pm_domain_enable(uint32_t domain_id, bool on);
 
+#if defined(PLAT_imx8mq)
+void imx_gpc_set_a53_core_awake(uint32_t core_id);
+#endif
+
 #endif /*IMX8M_GPC_H */