1 files changed, 21 insertions, 9 deletions

diff --git a/arch/arm/mach-imx/gpcv2.c b/arch/arm/mach-imx/gpcv2.c
index 7c4691eed110..e11c03b4d294 100644
--- a/arch/arm/mach-imx/gpcv2.c
+++ b/arch/arm/mach-imx/gpcv2.c
@@ -157,6 +157,7 @@ void imx_gpcv2_set_lpm_mode(u32 cpu, enum mxc_cpu_pwr_mode mode)
 {
 	unsigned long flags;
 	u32 val1, val2;
+	struct irq_desc *iomuxc_irq_desc;
 
 	spin_lock_irqsave(&gpcv2_lock, flags);
 
@@ -164,8 +165,7 @@ void imx_gpcv2_set_lpm_mode(u32 cpu, enum mxc_cpu_pwr_mode mode)
 	val2 = readl_relaxed(gpc_base + GPC_SLPCR);
 
 	/* core 0/1's LPM settings must be same */
-	val1 &= ~(BM_LPCR_A7_BSC_LPM0 | BM_LPCR_A7_BSC_LPM1 |
-		BM_LPCR_A7_BSC_IRQ_SRC_A7_WAKEUP);
+	val1 &= ~(BM_LPCR_A7_BSC_LPM0 | BM_LPCR_A7_BSC_LPM1);
 
 	val1 |= BM_LPCR_A7_BSC_CPU_CLK_ON_LPM;
 
@@ -174,17 +174,14 @@ void imx_gpcv2_set_lpm_mode(u32 cpu, enum mxc_cpu_pwr_mode mode)
 
 	switch (mode) {
 	case WAIT_CLOCKED:
-		val1 |= 0x3 << BP_LPCR_A7_BSC_IRQ_SRC;
 		break;
 	case WAIT_UNCLOCKED:
 		val1 |= A7_LPM_WAIT << BP_LPCR_A7_BSC_LPM0;
 		val1 &= ~BM_LPCR_A7_BSC_CPU_CLK_ON_LPM;
-		val1 |= 0x2 << BP_LPCR_A7_BSC_IRQ_SRC;
 		break;
 	case STOP_POWER_OFF:
 		val1 |= A7_LPM_STOP << BP_LPCR_A7_BSC_LPM0;
 		val1 &= ~BM_LPCR_A7_BSC_CPU_CLK_ON_LPM;
-		val1 |= 0x2 << BP_LPCR_A7_BSC_IRQ_SRC;
 		val2 |= BM_SLPCR_EN_DSM;
 		val2 |= BM_SLPCR_RBC_EN;
 		val2 |= BM_SLPCR_SBYOS;
@@ -194,9 +191,23 @@ void imx_gpcv2_set_lpm_mode(u32 cpu, enum mxc_cpu_pwr_mode mode)
 	default:
 		return;
 	}
-
+	/*
+	 * GPC: When improper low-power sequence is used,
+	 * the SoC enters low power mode before the ARM core executes WFI.
+	 *
+	 * Software workaround:
+	 * 1) Software should trigger IRQ #32 (IOMUX) to be always pending
+	 *    by setting IOMUX_GPR1_IRQ.
+	 * 2) Software should then unmask IRQ #32 in GPC before setting GPC
+	 *    Low-Power mode.
+	 * 3) Software should mask IRQ #32 right after GPC Low-Power mode
+	 *    is set.
+	 */
+	iomuxc_irq_desc = irq_to_desc(32);
+	imx_gpcv2_irq_unmask(&iomuxc_irq_desc->irq_data);
 	writel_relaxed(val1, gpc_base + GPC_LPCR_A7_BSC);
 	writel_relaxed(val2, gpc_base + GPC_SLPCR);
+	imx_gpcv2_irq_mask(&iomuxc_irq_desc->irq_data);
 
 	spin_unlock_irqrestore(&gpcv2_lock, flags);
 }
@@ -484,7 +495,7 @@ static int imx_pcie_regulator_notify(struct notifier_block *nb,
 void __init imx_gpcv2_init(void)
 {
 	struct device_node *np;
-	int i;
+	int i, val;
 
 	np = of_find_compatible_node(NULL, NULL, "fsl,imx7d-gpc");
 	gpc_base = of_iomap(np, 0);
@@ -512,8 +523,9 @@ void __init imx_gpcv2_init(void)
 	}
 
 	/* only external IRQs to wake up LPM and core 0/1 */
-	writel_relaxed(0x3 << BP_LPCR_A7_BSC_IRQ_SRC,
-		gpc_base + GPC_LPCR_A7_BSC);
+	val = readl_relaxed(gpc_base + GPC_LPCR_A7_BSC);
+	val |= BM_LPCR_A7_BSC_IRQ_SRC_A7_WAKEUP;
+	writel_relaxed(val, gpc_base + GPC_LPCR_A7_BSC);
 	/* mask m4 dsm trigger */
 	writel_relaxed(readl_relaxed(gpc_base + GPC_LPCR_M4) |
 		BM_LPCR_M4_MASK_DSM_TRIGGER, gpc_base + GPC_LPCR_M4);