ARM: at91: pm: disable RC oscillator in ULP0

Disable RC oscillator in ULP0 as datasheet specifies.

Signed-off-by: Claudiu Beznea <claudiu.beznea@microchip.com>
Signed-off-by: Alexandre Belloni <alexandre.belloni@bootlin.com>
Signed-off-by: Ludovic Desroches <ludovic.desroches@microchip.com>

diff --git a/arch/arm/mach-at91/pm_suspend.S b/arch/arm/mach-at91/pm_suspend.S
index 8b18cad1dcf51be28cdf153725fe43ddfa0988df..5c33023f9129ecaa36669dfd12e9b3b8979be1d8 100644 (file)
--- a/arch/arm/mach-at91/pm_suspend.S
+++ b/arch/arm/mach-at91/pm_suspend.S
@@ -178,11 +178,46 @@ ENDPROC(at91_backup_mode)
        orr     tmp1, tmp1, #AT91_PMC_KEY
        str     tmp1, [pmc, #AT91_CKGR_MOR]
 
+       /* Save RC oscillator state */
+       ldr     tmp1, [pmc, #AT91_PMC_SR]
+       str     tmp1, .saved_osc_status
+       tst     tmp1, #AT91_PMC_MOSCRCS
+       bne     1f
+
+       /* Turn off RC oscillator */
+       ldr     tmp1, [pmc, #AT91_CKGR_MOR]
+       bic     tmp1, tmp1, #AT91_PMC_MOSCRCEN
+       bic     tmp1, tmp1, #AT91_PMC_KEY_MASK
+       orr     tmp1, tmp1, #AT91_PMC_KEY
+       str     tmp1, [pmc, #AT91_CKGR_MOR]
+
+       /* Wait main RC disabled done */
+2:     ldr     tmp1, [pmc, #AT91_PMC_SR]
+       tst     tmp1, #AT91_PMC_MOSCRCS
+       bne     2b
+
        /* Wait for interrupt */
-       at91_cpu_idle
+1:     at91_cpu_idle
 
-       /* Turn on the crystal oscillator */
+       /* Restore RC oscillator state */
+       ldr     tmp1, .saved_osc_status
+       tst     tmp1, #AT91_PMC_MOSCRCS
+       beq     4f
+
+       /* Turn on RC oscillator */
        ldr     tmp1, [pmc, #AT91_CKGR_MOR]
+       orr     tmp1, tmp1, #AT91_PMC_MOSCRCEN
+       bic     tmp1, tmp1, #AT91_PMC_KEY_MASK
+       orr     tmp1, tmp1, #AT91_PMC_KEY
+       str     tmp1, [pmc, #AT91_CKGR_MOR]
+
+       /* Wait main RC stabilization */
+3:     ldr     tmp1, [pmc, #AT91_PMC_SR]
+       tst     tmp1, #AT91_PMC_MOSCRCS
+       beq     3b
+
+       /* Turn on the crystal oscillator */
+4:     ldr     tmp1, [pmc, #AT91_CKGR_MOR]
        orr     tmp1, tmp1, #AT91_PMC_MOSCEN
        orr     tmp1, tmp1, #AT91_PMC_KEY
        str     tmp1, [pmc, #AT91_CKGR_MOR]