pm-imx6.c: fix power off by stop modeColibri_iMX6_LinuxImageV2.5Beta3_20151215Apalis_iMX6_LinuxImageV2.5Beta3_20151215

With the 3.14.28 release of the kernel imx6_stop_mode_poweroff() does not
enter stop mode.
Thus the function does not switch of power but returns.

This seems to be caused by the second processor which has been stopped.
Set the WFI event mask for the stopped processor works around the issue.

Signed-off-by: Max Krummenacher <max.krummenacher@toradex.com>
Acked-by: Marcel Ziswiler <marcel.ziswiler@toradex.com>

1 files changed, 18 insertions, 1 deletions

diff --git a/arch/arm/mach-imx/pm-imx6.c b/arch/arm/mach-imx/pm-imx6.c
index 568b0c4490bc..b8ccb0975d6e 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -1062,7 +1062,24 @@ void __init imx6q_pm_init(void)
 
 void imx6_stop_mode_poweroff(void)
 {
-	imx6q_set_lpm(STOP_POWER_OFF);
+	struct irq_desc *iomuxc_irq_desc;
+	u32 val = readl_relaxed(ccm_base + CLPCR);
+
+	val &= ~BM_CLPCR_LPM;
+	/* mask the stopped processor, otherwise we will not enter stop mode */
+	val |= smp_processor_id() ? BM_CLPCR_MASK_CORE0_WFI : BM_CLPCR_MASK_CORE1_WFI;
+	val |= 0x2 << BP_CLPCR_LPM;
+	val |= 0x3 << BP_CLPCR_STBY_COUNT;
+	val |= BM_CLPCR_VSTBY;
+	val |= BM_CLPCR_SBYOS;
+	val |= BM_CLPCR_BYP_MMDC_CH1_LPM_HS;
+
+	iomuxc_irq_desc = irq_to_desc(32);
+	imx_gpc_irq_unmask(&iomuxc_irq_desc->irq_data);
+	writel_relaxed(val, ccm_base + CLPCR);
+
+	imx_gpc_irq_mask(&iomuxc_irq_desc->irq_data);
+	imx_gpc_mask_all();
 	cpu_do_idle();
 }