ARM: at91: pm: add support for ULP0 fast wakeup

ULP0 fast improves suspend/resume time with few milliseconds the drawback
being the power consumption. The mean values measured for suspend/resume
time are as follows (measured on SAMA5D2 Xplained board), ULP0 compared
with fast ULP0:
- ulp0 fast: suspend time: 169 ms, resume time: 216 ms
- ulp0     : suspend time: 197 ms, resume time: 258 ms

Current consumption while suspended (measured on SAMA5D2 Xplained board):
- ulp0 fast: 730uA
- ulp0     : 270uA

Signed-off-by: Claudiu Beznea <claudiu.beznea@microchip.com>
Signed-off-by: Alexandre Belloni <alexandre.belloni@bootlin.com>
Link: https://lore.kernel.org/r/1596616610-15460-2-git-send-email-claudiu.beznea@microchip.com

1 files changed, 37 insertions, 4 deletions

diff --git a/arch/arm/mach-at91/pm_suspend.S b/arch/arm/mach-at91/pm_suspend.S
index be9764e8d3fa..0184de05c1be 100644
--- a/arch/arm/mach-at91/pm_suspend.S
+++ b/arch/arm/mach-at91/pm_suspend.S
@@ -164,7 +164,22 @@ ENDPROC(at91_backup_mode)
 
 .macro at91_pm_ulp0_mode
 	ldr	pmc, .pmc_base
+	ldr	tmp2, .pm_mode
+	ldr	tmp3, .mckr_offset
+
+	/* Check if ULP0 fast variant has been requested. */
+	cmp	tmp2, #AT91_PM_ULP0_FAST
+	bne	0f
+
+	/* Set highest prescaler for power saving */
+	ldr	tmp1, [pmc, tmp3]
+	bic	tmp1, tmp1, #AT91_PMC_PRES
+	orr	tmp1, tmp1, #AT91_PMC_PRES_64
+	str	tmp1, [pmc, tmp3]
+	wait_mckrdy
+	b	1f
 
+0:
 	/* Turn off the crystal oscillator */
 	ldr	tmp1, [pmc, #AT91_CKGR_MOR]
 	bic	tmp1, tmp1, #AT91_PMC_MOSCEN
@@ -192,7 +207,18 @@ ENDPROC(at91_backup_mode)
 	/* Wait for interrupt */
 1:	at91_cpu_idle
 
-	/* Restore RC oscillator state */
+	/* Check if ULP0 fast variant has been requested. */
+	cmp	tmp2, #AT91_PM_ULP0_FAST
+	bne	5f
+
+	/* Set lowest prescaler for fast resume. */
+	ldr	tmp1, [pmc, tmp3]
+	bic	tmp1, tmp1, #AT91_PMC_PRES
+	str	tmp1, [pmc, tmp3]
+	wait_mckrdy
+	b	6f
+
+5:	/* Restore RC oscillator state */
 	ldr	tmp1, .saved_osc_status
 	tst	tmp1, #AT91_PMC_MOSCRCS
 	beq	4f
@@ -216,6 +242,7 @@ ENDPROC(at91_backup_mode)
 	str	tmp1, [pmc, #AT91_CKGR_MOR]
 
 	wait_moscrdy
+6:
 .endm
 
 /**
@@ -473,23 +500,29 @@ ENDPROC(at91_backup_mode)
 ENTRY(at91_ulp_mode)
 	ldr	pmc, .pmc_base
 	ldr	tmp2, .mckr_offset
+	ldr	tmp3, .pm_mode
 
 	/* Save Master clock setting */
 	ldr	tmp1, [pmc, tmp2]
 	str	tmp1, .saved_mckr
 
 	/*
-	 * Set the Master clock source to slow clock
+	 * Set master clock source to:
+	 * - MAINCK if using ULP0 fast variant
+	 * - slow clock, otherwise
 	 */
 	bic	tmp1, tmp1, #AT91_PMC_CSS
+	cmp	tmp3, #AT91_PM_ULP0_FAST
+	bne	save_mck
+	orr	tmp1, tmp1, #AT91_PMC_CSS_MAIN
+save_mck:
 	str	tmp1, [pmc, tmp2]
 
 	wait_mckrdy
 
 	at91_plla_disable
 
-	ldr	r0, .pm_mode
-	cmp	r0, #AT91_PM_ULP1
+	cmp	tmp3, #AT91_PM_ULP1
 	beq	ulp1_mode
 
 	at91_pm_ulp0_mode