ARM: at91: pm: add macros for plla disable/enable

Add macros for PLLA disable and enable (in disable macro the PLLA
state will also be saved). This prepares the field for PLLA disable/enable
for suspend/resume on SAM9X60.

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

diff --git a/arch/arm/mach-at91/pm_suspend.S b/arch/arm/mach-at91/pm_suspend.S
index bfb3aab8859e48d589a7df3ac1d903f44961e0bd..64460b4e0fc1bbfd6d6a2b63cca085bbf7470a20 100644 (file)
--- a/arch/arm/mach-at91/pm_suspend.S
+++ b/arch/arm/mach-at91/pm_suspend.S
@@ -46,15 +46,6 @@ tmp2 .req    r5
        beq     1b
        .endm
 
-/*
- * Wait until PLLA has locked.
- */
-       .macro wait_pllalock
-1:     ldr     tmp1, [pmc, #AT91_PMC_SR]
-       tst     tmp1, #AT91_PMC_LOCKA
-       beq     1b
-       .endm
-
 /*
  * Put the processor to enter the idle state
  */
@@ -336,6 +327,34 @@ ENDPROC(at91_backup_mode)
 3:
 .endm
 
+.macro at91_plla_disable
+       /* Save PLLA setting and disable it */
+       ldr     tmp1, [pmc, #AT91_CKGR_PLLAR]
+       str     tmp1, .saved_pllar
+
+       /* Disable PLLA. */
+       mov     tmp1, #AT91_PMC_PLLCOUNT
+       orr     tmp1, tmp1, #(1 << 29)          /* bit 29 always set */
+       str     tmp1, [pmc, #AT91_CKGR_PLLAR]
+.endm
+
+.macro at91_plla_enable
+       /* Restore PLLA setting */
+       ldr     tmp1, .saved_pllar
+       str     tmp1, [pmc, #AT91_CKGR_PLLAR]
+
+       /* Enable PLLA. */
+       tst     tmp1, #(AT91_PMC_MUL &  0xff0000)
+       bne     1f
+       tst     tmp1, #(AT91_PMC_MUL & ~0xff0000)
+       beq     2f
+
+1:     ldr     tmp1, [pmc, #AT91_PMC_SR]
+       tst     tmp1, #AT91_PMC_LOCKA
+       beq     1b
+2:
+.endm
+
 ENTRY(at91_ulp_mode)
        ldr     pmc, .pmc_base
        ldr     tmp2, .mckr_offset
@@ -352,13 +371,7 @@ ENTRY(at91_ulp_mode)
 
        wait_mckrdy
 
-       /* Save PLLA setting and disable it */
-       ldr     tmp1, [pmc, #AT91_CKGR_PLLAR]
-       str     tmp1, .saved_pllar
-
-       mov     tmp1, #AT91_PMC_PLLCOUNT
-       orr     tmp1, tmp1, #(1 << 29)          /* bit 29 always set */
-       str     tmp1, [pmc, #AT91_CKGR_PLLAR]
+       at91_plla_disable
 
        ldr     r0, .pm_mode
        cmp     r0, #AT91_PM_ULP1
@@ -374,17 +387,7 @@ ulp1_mode:
 ulp_exit:
        ldr     pmc, .pmc_base
 
-       /* Restore PLLA setting */
-       ldr     tmp1, .saved_pllar
-       str     tmp1, [pmc, #AT91_CKGR_PLLAR]
-
-       tst     tmp1, #(AT91_PMC_MUL &  0xff0000)
-       bne     3f
-       tst     tmp1, #(AT91_PMC_MUL & ~0xff0000)
-       beq     4f
-3:
-       wait_pllalock
-4:
+       at91_plla_enable
 
        /*
         * Restore master clock setting