[PATCH 2/7] pm: at91: pm_suspend: add the WFI support for ARMv7

[Wenyou Yang]

Signed-off-by: Wenyou Yang <wenyou.yang at atmel.com>
---
 arch/arm/mach-at91/pm_suspend.S |   54 ++++++++++++++++++++++++++++++++++++++-
 1 file changed, 53 insertions(+), 1 deletion(-)

diff --git a/arch/arm/mach-at91/pm_suspend.S b/arch/arm/mach-at91/pm_suspend.S
index 122a3f1..e796722 100644
--- a/arch/arm/mach-at91/pm_suspend.S
+++ b/arch/arm/mach-at91/pm_suspend.S
@@ -53,6 +53,58 @@ mode	.req	r6
 	beq	1b
 	.endm
 
+/*
+ * Put the processor to enter the WFI state
+ */
+	.macro _do_wfi
+
+#if defined(CONFIG_CPU_V7)
+	/*
+	 * Execute an ISB instruction to flush the pipeline to ensure
+	 * that all of operations have beem completed.
+	 */
+	isb
+
+	/*
+	 * Execute an ISB instruction to ensure that all of the
+	 * CP15 register changes have been committed.
+	 */
+	dsb
+	dmb
+
+	/* Disable the processor's clock */
+	mov	tmp1, #AT91_PMC_PCK
+	str	tmp1, [pmc, #AT91_PMC_SCDR]
+
+	/* Execute a WFI instruction */
+	wfi	@ Wait For Interrupt
+
+	/*
+	 * CPU can specualatively prefetch the instructions
+	 * so add NOPs after WFI. Sixteen NOPs as Cortex-A5 pipeline.
+	 */
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+#else
+	mcr	p15, 0, tmp1, c7, c0, 4
+#endif
+
+	.endm
+
 	.text
 
 /*
@@ -181,7 +233,7 @@ sdr_sr_done:
 
 skip_disable_main_clock:
 	/* Wait for interrupt */
-	mcr	p15, 0, tmp1, c7, c0, 4
+	_do_wfi
 
 	tst	mode, #AT91_PM_SLOW_CLOCK
 	beq	skip_enable_main_clock
-- 
1.7.9.5