Merge tag 'v5.10-rc1' into kvmarm-master/next

[linux-2.6-microblaze.git] / arch / arm / mach-at91 / pm_suspend.S

diff --git a/arch/arm/mach-at91/pm_suspend.S b/arch/arm/mach-at91/pm_suspend.S
index be9764e..0184de0 100644 (file)
--- 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