diff options
| -rw-r--r-- | arch/arm/mach-imx/pm-imx6.c | 19 |
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(); } |