OMAP4: PM: Overwrite the default idle hook with custom omap_idle()

Create a custom omap_default_idle() to implement OMAP4 memory, IO
ordering requirements which can't be addressed with default
arch_idle() hook.

On OMAP4, this hook is being used by all CPUs with !CONFIG_CPUIDLE
and by secondary CPU with CONFIG_CPUIDLE.

Tested-by: Axel Haslam <axelhaslam@ti.com>
Signed-off-by: Santosh Shilimkar <santosh.shilimkar@ti.com>

3 files changed, 71 insertions, 45 deletions

diff --git a/arch/arm/mach-omap2/include/mach/omap4-common.h b/arch/arm/mach-omap2/include/mach/omap4-common.h
index 565d96a..e414ccd 100644
--- a/arch/arm/mach-omap2/include/mach/omap4-common.h
+++ b/arch/arm/mach-omap2/include/mach/omap4-common.h
@@ -64,6 +64,7 @@ extern void gic_dist_disable(void);
 extern u32 gic_cpu_read(u32 reg);
 extern void omap_smc1(u32 fn, u32 arg);
 extern void omap_bus_sync(void);
+extern void omap_do_wfi(void);
 
 extern bool gic_dist_disabled(void);
 extern void gic_timer_retrigger(void);
diff --git a/arch/arm/mach-omap2/pm44xx.c b/arch/arm/mach-omap2/pm44xx.c
index d4c754d..fbf6e5d 100644
--- a/arch/arm/mach-omap2/pm44xx.c
+++ b/arch/arm/mach-omap2/pm44xx.c
@@ -515,6 +515,24 @@ static irqreturn_t prcm_interrupt_handler (int irq, void *dev_id)
 }
 
 /**
+ * omap_default_idle() - implement a default idle for !CONFIG_CPUIDLE
+ *
+ * Implements OMAP4 memory, IO ordering requirements which can't be addressed
+ * with default arch_idle() hook. Used by all CPUs with !CONFIG_CPUIDLE and
+ * by secondary CPU with CONFIG_CPUIDLE.
+ */
+static void omap_default_idle(void)
+{
+	local_irq_disable();
+	local_fiq_disable();
+
+	omap_do_wfi();
+
+	local_fiq_enable();
+	local_irq_enable();
+}
+
+/**
  * omap4_pm_init - Init routine for OMAP4 PM
  *
  * Initializes all powerdomain and clockdomain target states
@@ -632,6 +650,9 @@ static int __init omap4_pm_init(void)
 	 /* Enable wakeup for PRCM IRQ for system wide suspend */
 	enable_irq_wake(OMAP44XX_IRQ_PRCM);
 
+	/* Overwrite the default arch_idle() */
+	pm_idle = omap_default_idle;
+
 	omap4_idle_init();
 
 err2:
diff --git a/arch/arm/mach-omap2/sleep44xx.S b/arch/arm/mach-omap2/sleep44xx.S
index 3c05afe..d9cfd80 100644
--- a/arch/arm/mach-omap2/sleep44xx.S
+++ b/arch/arm/mach-omap2/sleep44xx.S
@@ -233,11 +233,6 @@ wait:
 	smc	#0
 	dsb
 #endif
-#endif
-
-do_WFI:
-#ifdef CONFIG_CACHE_L2X0
-	/* Issue cache sync before WFI to drain L2X0 buffer */
 l2x_sync:
 	bl	omap4_get_l2cache_base
 	mov	r2, r0
@@ -248,47 +243,9 @@ sync:
 	ands	r0, r0, #0x1
 	bne	sync
 #endif
-	/* Drain interconnect write buffers. */
-	bl omap_bus_sync
-
-	/*
-	 * Execute an ISB instruction to ensure that all of the
-	 * CP15 register changes have been committed.
-	 */
-	isb
 
-	/*
-	 * Execute a barrier instruction to ensure that all cache,
-	 * TLB and branch predictor maintenance operations issued
-	 * by any CPU in the cluster have completed.
-	 */
-	dsb
-	dmb
-
-	/*
-	 * Execute a WFI instruction and wait until the
-	 * STANDBYWFI output is asserted to indicate that the
-	 * CPU is in idle and low power state. CPU can specualatively
-	 * prefetch the instructions so add NOPs after WFI. Sixteen
-	 * NOPs as per Cortex-A9 pipeline.
-	 */
-	wfi					@ Wait For Interrupt
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
+do_WFI:
+	bl	omap_do_wfi
 
 	/*
 	 * CPU is here when it failed to enter OFF/DORMANT or
@@ -554,4 +511,51 @@ ENTRY(omap_bus_sync)
 	ldmfd	sp!, {r9, pc}
 ENDPROC(omap_bus_sync)
 
+ENTRY(omap_do_wfi)
+	stmfd	sp!, {lr}
+	/* Drain interconnect write buffers. */
+	bl omap_bus_sync
+
+	/*
+	 * Execute an ISB instruction to ensure that all of the
+	 * CP15 register changes have been committed.
+	 */
+	isb
+
+	/*
+	 * Execute a barrier instruction to ensure that all cache,
+	 * TLB and branch predictor maintenance operations issued
+	 * by any CPU in the cluster have completed.
+	 */
+	dsb
+	dmb
+
+	/*
+	 * Execute a WFI instruction and wait until the
+	 * STANDBYWFI output is asserted to indicate that the
+	 * CPU is in idle and low power state. CPU can specualatively
+	 * prefetch the instructions so add NOPs after WFI. Sixteen
+	 * NOPs as per Cortex-A9 pipeline.
+	 */
+	wfi					@ Wait For Interrupt
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	ldmfd	sp!, {pc}
+ENDPROC(omap_do_wfi)
+
 #endif