/* SPDX-License-Identifier: GPL-2.0-only */
/*
 * arch/arm/mach-at91/pm_slow_clock.S
 *
 *  Copyright (C) 2006 Savin Zlobec
 *
 * AT91SAM9 support:
 *  Copyright (C) 2007 Anti Sullin <anti.sullin@artecdesign.ee>
 */
#include <linux/linkage.h>
#include <linux/clk/at91_pmc.h>
#include "pm.h"
#include "pm_data-offsets.h"

#ifdef CONFIG_CPU_V7
.arch armv7-a
#endif

#define SRAMC_SELF_FRESH_ACTIVE         0x01
#define SRAMC_SELF_FRESH_EXIT           0x00

pmc     .req    r0
tmp1    .req    r4
tmp2    .req    r5
tmp3    .req    r6

/*
 * Wait until master clock is ready (after switching master clock source)
 *
 * @r_mckid:    register holding master clock identifier
 *
 * Side effects: overwrites r7, r8
 */
        .macro wait_mckrdy r_mckid
#ifdef CONFIG_SOC_SAMA7
        cmp     \r_mckid, #0
        beq     1f
        mov     r7, #AT91_PMC_MCKXRDY
        b       2f
#endif
1:      mov     r7, #AT91_PMC_MCKRDY
2:      ldr     r8, [pmc, #AT91_PMC_SR]
        and     r8, r7
        cmp     r8, r7
        bne     2b
        .endm

/*
 * Wait until master oscillator has stabilized.
 *
 * Side effects: overwrites r7
 */
        .macro wait_moscrdy
1:      ldr     r7, [pmc, #AT91_PMC_SR]
        tst     r7, #AT91_PMC_MOSCS
        beq     1b
        .endm

/*
 * Wait for main oscillator selection is done
 *
 * Side effects: overwrites r7
 */
        .macro wait_moscsels
1:      ldr     r7, [pmc, #AT91_PMC_SR]
        tst     r7, #AT91_PMC_MOSCSELS
        beq     1b
        .endm

/*
 * Put the processor to enter the idle state
 *
 * Side effects: overwrites r7
 */
        .macro at91_cpu_idle

#if defined(CONFIG_CPU_V7)
        mov     r7, #AT91_PMC_PCK
        str     r7, [pmc, #AT91_PMC_SCDR]

        dsb

        wfi             @ Wait For Interrupt
#else
        mcr     p15, 0, tmp1, c7, c0, 4
#endif

        .endm

        .macro at91_backup_set_lpm reg
#ifdef CONFIG_SOC_SAMA7
        orr     \reg, \reg, #0x200000
#endif
        .endm

        .text

        .arm

#ifdef CONFIG_SOC_SAMA7
/**
 * Enable self-refresh
 *
 * Side effects: overwrites r2, r3, tmp1, tmp2, tmp3, r7
 */
.macro at91_sramc_self_refresh_ena
        ldr     r2, .sramc_base
        ldr     r3, .sramc_phy_base
        ldr     r7, .pm_mode

        dsb

        /* Disable all AXI ports. */
        ldr     tmp1, [r2, #UDDRC_PCTRL_0]
        bic     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_0]

        ldr     tmp1, [r2, #UDDRC_PCTRL_1]
        bic     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_1]

        ldr     tmp1, [r2, #UDDRC_PCTRL_2]
        bic     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_2]

        ldr     tmp1, [r2, #UDDRC_PCTRL_3]
        bic     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_3]

        ldr     tmp1, [r2, #UDDRC_PCTRL_4]
        bic     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_4]

sr_ena_1:
        /* Wait for all ports to disable. */
        ldr     tmp1, [r2, #UDDRC_PSTAT]
        ldr     tmp2, =UDDRC_PSTAT_ALL_PORTS
        tst     tmp1, tmp2
        bne     sr_ena_1

        /* Switch to self-refresh. */
        ldr     tmp1, [r2, #UDDRC_PWRCTL]
        orr     tmp1, tmp1, #UDDRC_PWRCTL_SELFREF_SW
        str     tmp1, [r2, #UDDRC_PWRCTL]

sr_ena_2:
        /* Wait for self-refresh enter. */
        ldr     tmp1, [r2, #UDDRC_STAT]
        bic     tmp1, tmp1, #~UDDRC_STAT_SELFREF_TYPE_MSK
        cmp     tmp1, #UDDRC_STAT_SELFREF_TYPE_SW
        bne     sr_ena_2

        /* Disable DX DLLs for non-backup modes. */
        cmp     r7, #AT91_PM_BACKUP
        beq     sr_ena_3

        /* Do not soft reset the AC DLL. */
        ldr     tmp1, [r3, DDR3PHY_ACDLLCR]
        bic     tmp1, tmp1, DDR3PHY_ACDLLCR_DLLSRST
        str     tmp1, [r3, DDR3PHY_ACDLLCR]

        /* Disable DX DLLs. */
        ldr     tmp1, [r3, #DDR3PHY_DX0DLLCR]
        orr     tmp1, tmp1, #DDR3PHY_DXDLLCR_DLLDIS
        str     tmp1, [r3, #DDR3PHY_DX0DLLCR]

        ldr     tmp1, [r3, #DDR3PHY_DX1DLLCR]
        orr     tmp1, tmp1, #DDR3PHY_DXDLLCR_DLLDIS
        str     tmp1, [r3, #DDR3PHY_DX1DLLCR]

sr_ena_3:
        /* Power down DDR PHY data receivers. */
        ldr     tmp1, [r3, #DDR3PHY_DXCCR]
        orr     tmp1, tmp1, #DDR3PHY_DXCCR_DXPDR
        str     tmp1, [r3, #DDR3PHY_DXCCR]

        /* Power down ADDR/CMD IO. */
        ldr     tmp1, [r3, #DDR3PHY_ACIOCR]
        orr     tmp1, tmp1, #DDR3PHY_ACIORC_ACPDD
        orr     tmp1, tmp1, #DDR3PHY_ACIOCR_CKPDD_CK0
        orr     tmp1, tmp1, #DDR3PHY_ACIOCR_CSPDD_CS0
        str     tmp1, [r3, #DDR3PHY_ACIOCR]

        /* Power down ODT. */
        ldr     tmp1, [r3, #DDR3PHY_DSGCR]
        orr     tmp1, tmp1, #DDR3PHY_DSGCR_ODTPDD_ODT0
        str     tmp1, [r3, #DDR3PHY_DSGCR]
.endm

/**
 * Disable self-refresh
 *
 * Side effects: overwrites r2, r3, tmp1, tmp2, tmp3
 */
.macro at91_sramc_self_refresh_dis
        ldr     r2, .sramc_base
        ldr     r3, .sramc_phy_base

        /* Power up DDR PHY data receivers. */
        ldr     tmp1, [r3, #DDR3PHY_DXCCR]
        bic     tmp1, tmp1, #DDR3PHY_DXCCR_DXPDR
        str     tmp1, [r3, #DDR3PHY_DXCCR]

        /* Power up the output of CK and CS pins. */
        ldr     tmp1, [r3, #DDR3PHY_ACIOCR]
        bic     tmp1, tmp1, #DDR3PHY_ACIORC_ACPDD
        bic     tmp1, tmp1, #DDR3PHY_ACIOCR_CKPDD_CK0
        bic     tmp1, tmp1, #DDR3PHY_ACIOCR_CSPDD_CS0
        str     tmp1, [r3, #DDR3PHY_ACIOCR]

        /* Power up ODT. */
        ldr     tmp1, [r3, #DDR3PHY_DSGCR]
        bic     tmp1, tmp1, #DDR3PHY_DSGCR_ODTPDD_ODT0
        str     tmp1, [r3, #DDR3PHY_DSGCR]

        /* Enable DX DLLs. */
        ldr     tmp1, [r3, #DDR3PHY_DX0DLLCR]
        bic     tmp1, tmp1, #DDR3PHY_DXDLLCR_DLLDIS
        str     tmp1, [r3, #DDR3PHY_DX0DLLCR]

        ldr     tmp1, [r3, #DDR3PHY_DX1DLLCR]
        bic     tmp1, tmp1, #DDR3PHY_DXDLLCR_DLLDIS
        str     tmp1, [r3, #DDR3PHY_DX1DLLCR]

        /* Enable quasi-dynamic programming. */
        mov     tmp1, #0
        str     tmp1, [r2, #UDDRC_SWCTRL]

        /* De-assert SDRAM initialization. */
        ldr     tmp1, [r2, #UDDRC_DFIMISC]
        bic     tmp1, tmp1, #UDDRC_DFIMISC_DFI_INIT_COMPLETE_EN
        str     tmp1, [r2, #UDDRC_DFIMISC]

        /* Quasi-dynamic programming done. */
        mov     tmp1, #UDDRC_SWCTRL_SW_DONE
        str     tmp1, [r2, #UDDRC_SWCTRL]

sr_dis_1:
        ldr     tmp1, [r2, #UDDRC_SWSTAT]
        tst     tmp1, #UDDRC_SWSTAT_SW_DONE_ACK
        beq     sr_dis_1

        /* DLL soft-reset + DLL lock wait + ITM reset */
        mov     tmp1, #(DDR3PHY_PIR_INIT | DDR3PHY_PIR_DLLSRST | \
                        DDR3PHY_PIR_DLLLOCK | DDR3PHY_PIR_ITMSRST)
        str     tmp1, [r3, #DDR3PHY_PIR]

sr_dis_4:
        /* Wait for it. */
        ldr     tmp1, [r3, #DDR3PHY_PGSR]
        tst     tmp1, #DDR3PHY_PGSR_IDONE
        beq     sr_dis_4

        /* Enable quasi-dynamic programming. */
        mov     tmp1, #0
        str     tmp1, [r2, #UDDRC_SWCTRL]

        /* Assert PHY init complete enable signal. */
        ldr     tmp1, [r2, #UDDRC_DFIMISC]
        orr     tmp1, tmp1, #UDDRC_DFIMISC_DFI_INIT_COMPLETE_EN
        str     tmp1, [r2, #UDDRC_DFIMISC]

        /* Programming is done. Set sw_done. */
        mov     tmp1, #UDDRC_SWCTRL_SW_DONE
        str     tmp1, [r2, #UDDRC_SWCTRL]

sr_dis_5:
        /* Wait for it. */
        ldr     tmp1, [r2, #UDDRC_SWSTAT]
        tst     tmp1, #UDDRC_SWSTAT_SW_DONE_ACK
        beq     sr_dis_5

        /* Trigger self-refresh exit. */
        ldr     tmp1, [r2, #UDDRC_PWRCTL]
        bic     tmp1, tmp1, #UDDRC_PWRCTL_SELFREF_SW
        str     tmp1, [r2, #UDDRC_PWRCTL]

sr_dis_6:
        /* Wait for self-refresh exit done. */
        ldr     tmp1, [r2, #UDDRC_STAT]
        bic     tmp1, tmp1, #~UDDRC_STAT_OPMODE_MSK
        cmp     tmp1, #UDDRC_STAT_OPMODE_NORMAL
        bne     sr_dis_6

        /* Enable all AXI ports. */
        ldr     tmp1, [r2, #UDDRC_PCTRL_0]
        orr     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_0]

        ldr     tmp1, [r2, #UDDRC_PCTRL_1]
        orr     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_1]

        ldr     tmp1, [r2, #UDDRC_PCTRL_2]
        orr     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_2]

        ldr     tmp1, [r2, #UDDRC_PCTRL_3]
        orr     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_3]

        ldr     tmp1, [r2, #UDDRC_PCTRL_4]
        orr     tmp1, tmp1, #0x1
        str     tmp1, [r2, #UDDRC_PCTRL_4]

        dsb
.endm
#else
/**
 * Enable self-refresh
 *
 * register usage:
 *      @r1: memory type
 *      @r2: base address of the sram controller
 *      @r3: temporary
 */
.macro at91_sramc_self_refresh_ena
        ldr     r1, .memtype
        ldr     r2, .sramc_base

        cmp     r1, #AT91_MEMCTRL_MC
        bne     sr_ena_ddrc_sf

        /* Active SDRAM self-refresh mode */
        mov     r3, #1
        str     r3, [r2, #AT91_MC_SDRAMC_SRR]
        b       sr_ena_exit

sr_ena_ddrc_sf:
        cmp     r1, #AT91_MEMCTRL_DDRSDR
        bne     sr_ena_sdramc_sf

        /*
         * DDR Memory controller
         */

        /* LPDDR1 --> force DDR2 mode during self-refresh */
        ldr     r3, [r2, #AT91_DDRSDRC_MDR]
        str     r3, .saved_sam9_mdr
        bic     r3, r3, #~AT91_DDRSDRC_MD
        cmp     r3, #AT91_DDRSDRC_MD_LOW_POWER_DDR
        ldreq   r3, [r2, #AT91_DDRSDRC_MDR]
        biceq   r3, r3, #AT91_DDRSDRC_MD
        orreq   r3, r3, #AT91_DDRSDRC_MD_DDR2
        streq   r3, [r2, #AT91_DDRSDRC_MDR]

        /* Active DDRC self-refresh mode */
        ldr     r3, [r2, #AT91_DDRSDRC_LPR]
        str     r3, .saved_sam9_lpr
        bic     r3, r3, #AT91_DDRSDRC_LPCB
        orr     r3, r3, #AT91_DDRSDRC_LPCB_SELF_REFRESH
        str     r3, [r2, #AT91_DDRSDRC_LPR]

        /* If using the 2nd ddr controller */
        ldr     r2, .sramc1_base
        cmp     r2, #0
        beq     sr_ena_no_2nd_ddrc

        ldr     r3, [r2, #AT91_DDRSDRC_MDR]
        str     r3, .saved_sam9_mdr1
        bic     r3, r3, #~AT91_DDRSDRC_MD
        cmp     r3, #AT91_DDRSDRC_MD_LOW_POWER_DDR
        ldreq   r3, [r2, #AT91_DDRSDRC_MDR]
        biceq   r3, r3, #AT91_DDRSDRC_MD
        orreq   r3, r3, #AT91_DDRSDRC_MD_DDR2
        streq   r3, [r2, #AT91_DDRSDRC_MDR]

        /* Active DDRC self-refresh mode */
        ldr     r3, [r2, #AT91_DDRSDRC_LPR]
        str     r3, .saved_sam9_lpr1
        bic     r3, r3, #AT91_DDRSDRC_LPCB
        orr     r3, r3, #AT91_DDRSDRC_LPCB_SELF_REFRESH
        str     r3, [r2, #AT91_DDRSDRC_LPR]

sr_ena_no_2nd_ddrc:
        b       sr_ena_exit

        /*
         * SDRAMC Memory controller
         */
sr_ena_sdramc_sf:
        /* Active SDRAMC self-refresh mode */
        ldr     r3, [r2, #AT91_SDRAMC_LPR]
        str     r3, .saved_sam9_lpr
        bic     r3, r3, #AT91_SDRAMC_LPCB
        orr     r3, r3, #AT91_SDRAMC_LPCB_SELF_REFRESH
        str     r3, [r2, #AT91_SDRAMC_LPR]

        ldr     r3, .saved_sam9_lpr
        str     r3, [r2, #AT91_SDRAMC_LPR]

sr_ena_exit:
.endm

/**
 * Disable self-refresh
 *
 * register usage:
 *      @r1: memory type
 *      @r2: base address of the sram controller
 *      @r3: temporary
 */
.macro at91_sramc_self_refresh_dis
        ldr     r1, .memtype
        ldr     r2, .sramc_base

        cmp     r1, #AT91_MEMCTRL_MC
        bne     sr_dis_ddrc_exit_sf

        /*
         * at91rm9200 Memory controller
         */

         /*
          * For exiting the self-refresh mode, do nothing,
          * automatically exit the self-refresh mode.
          */
        b       sr_dis_exit

sr_dis_ddrc_exit_sf:
        cmp     r1, #AT91_MEMCTRL_DDRSDR
        bne     sdramc_exit_sf

        /* DDR Memory controller */

        /* Restore MDR in case of LPDDR1 */
        ldr     r3, .saved_sam9_mdr
        str     r3, [r2, #AT91_DDRSDRC_MDR]
        /* Restore LPR on AT91 with DDRAM */
        ldr     r3, .saved_sam9_lpr
        str     r3, [r2, #AT91_DDRSDRC_LPR]

        /* If using the 2nd ddr controller */
        ldr     r2, .sramc1_base
        cmp     r2, #0
        ldrne   r3, .saved_sam9_mdr1
        strne   r3, [r2, #AT91_DDRSDRC_MDR]
        ldrne   r3, .saved_sam9_lpr1
        strne   r3, [r2, #AT91_DDRSDRC_LPR]

        b       sr_dis_exit

sdramc_exit_sf:
        /* SDRAMC Memory controller */
        ldr     r3, .saved_sam9_lpr
        str     r3, [r2, #AT91_SDRAMC_LPR]

sr_dis_exit:
.endm
#endif

.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]

        mov     tmp3, #0
        wait_mckrdy tmp3
        b       1f

0:
        /* Turn off the crystal oscillator */
        ldr     tmp1, [pmc, #AT91_CKGR_MOR]
        bic     tmp1, tmp1, #AT91_PMC_MOSCEN
        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 */
1:      at91_cpu_idle

        /* Check if ULP0 fast variant has been requested. */
        cmp     tmp2, #AT91_PM_ULP0_FAST
        bne     5f

        /* Set lowest prescaler for fast resume. */
        ldr     tmp3, .mckr_offset
        ldr     tmp1, [pmc, tmp3]
        bic     tmp1, tmp1, #AT91_PMC_PRES
        str     tmp1, [pmc, tmp3]

        mov     tmp3, #0
        wait_mckrdy tmp3
        b       6f

5:      /* 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]

        wait_moscrdy
6:
.endm

/**
 * Note: This procedure only applies on the platform which uses
 * the external crystal oscillator as a main clock source.
 */
.macro at91_pm_ulp1_mode
        ldr     pmc, .pmc_base
        ldr     tmp2, .mckr_offset
        mov     tmp3, #0

        /* Save RC oscillator state and check if it is enabled. */
        ldr     tmp1, [pmc, #AT91_PMC_SR]
        str     tmp1, .saved_osc_status
        tst     tmp1, #AT91_PMC_MOSCRCS
        bne     2f

        /* Enable 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 */
1:      ldr     tmp1, [pmc, #AT91_PMC_SR]
        tst     tmp1, #AT91_PMC_MOSCRCS
        beq     1b

        /* Switch the main clock source to 12-MHz RC oscillator */
2:      ldr     tmp1, [pmc, #AT91_CKGR_MOR]
        bic     tmp1, tmp1, #AT91_PMC_MOSCSEL
        bic     tmp1, tmp1, #AT91_PMC_KEY_MASK
        orr     tmp1, tmp1, #AT91_PMC_KEY
        str     tmp1, [pmc, #AT91_CKGR_MOR]

        wait_moscsels

        /* Disable the crystal oscillator */
        ldr     tmp1, [pmc, #AT91_CKGR_MOR]
        bic     tmp1, tmp1, #AT91_PMC_MOSCEN
        bic     tmp1, tmp1, #AT91_PMC_KEY_MASK
        orr     tmp1, tmp1, #AT91_PMC_KEY
        str     tmp1, [pmc, #AT91_CKGR_MOR]

        /* Switch the master clock source to main clock */
        ldr     tmp1, [pmc, tmp2]
        bic     tmp1, tmp1, #AT91_PMC_CSS
        orr     tmp1, tmp1, #AT91_PMC_CSS_MAIN
        str     tmp1, [pmc, tmp2]

        wait_mckrdy tmp3

        /* Enter the ULP1 mode by set WAITMODE bit in CKGR_MOR */
        ldr     tmp1, [pmc, #AT91_CKGR_MOR]
        orr     tmp1, tmp1, #AT91_PMC_WAITMODE
        bic     tmp1, tmp1, #AT91_PMC_KEY_MASK
        orr     tmp1, tmp1, #AT91_PMC_KEY
        str     tmp1, [pmc, #AT91_CKGR_MOR]

        /* Quirk for SAM9X60's PMC */
        nop
        nop

        wait_mckrdy tmp3

        /* Enable the crystal oscillator */
        ldr     tmp1, [pmc, #AT91_CKGR_MOR]
        orr     tmp1, tmp1, #AT91_PMC_MOSCEN
        bic     tmp1, tmp1, #AT91_PMC_KEY_MASK
        orr     tmp1, tmp1, #AT91_PMC_KEY
        str     tmp1, [pmc, #AT91_CKGR_MOR]

        wait_moscrdy

        /* Switch the master clock source to slow clock */
        ldr     tmp1, [pmc, tmp2]
        bic     tmp1, tmp1, #AT91_PMC_CSS
        str     tmp1, [pmc, tmp2]

        wait_mckrdy tmp3

        /* Switch main clock source to crystal oscillator */
        ldr     tmp1, [pmc, #AT91_CKGR_MOR]
        orr     tmp1, tmp1, #AT91_PMC_MOSCSEL
        bic     tmp1, tmp1, #AT91_PMC_KEY_MASK
        orr     tmp1, tmp1, #AT91_PMC_KEY
        str     tmp1, [pmc, #AT91_CKGR_MOR]

        wait_moscsels

        /* Switch the master clock source to main clock */
        ldr     tmp1, [pmc, tmp2]
        bic     tmp1, tmp1, #AT91_PMC_CSS
        orr     tmp1, tmp1, #AT91_PMC_CSS_MAIN
        str     tmp1, [pmc, tmp2]

        wait_mckrdy tmp3

        /* Restore RC oscillator state */
        ldr     tmp1, .saved_osc_status
        tst     tmp1, #AT91_PMC_MOSCRCS
        bne     3f

        /* Disable 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 RC oscillator disable done */
4:      ldr     tmp1, [pmc, #AT91_PMC_SR]
        tst     tmp1, #AT91_PMC_MOSCRCS
        bne     4b

3:
.endm

.macro at91_plla_disable
        /* Save PLLA setting and disable it */
        ldr     tmp1, .pmc_version
        cmp     tmp1, #AT91_PMC_V1
        beq     1f

#ifdef CONFIG_HAVE_AT91_SAM9X60_PLL
        /* Save PLLA settings. */
        ldr     tmp2, [pmc, #AT91_PMC_PLL_UPDT]
        bic     tmp2, tmp2, #AT91_PMC_PLL_UPDT_ID
        str     tmp2, [pmc, #AT91_PMC_PLL_UPDT]

        /* save acr */
        ldr     tmp2, [pmc, #AT91_PMC_PLL_ACR]
        str     tmp2, .saved_acr

        /* save div. */
        mov     tmp1, #0
        ldr     tmp2, [pmc, #AT91_PMC_PLL_CTRL0]
        bic     tmp2, tmp2, #0xffffff00
        orr     tmp1, tmp1, tmp2

        /* save mul. */
        ldr     tmp2, [pmc, #AT91_PMC_PLL_CTRL1]
        bic     tmp2, tmp2, #0xffffff
        orr     tmp1, tmp1, tmp2
        str     tmp1, .saved_pllar

        /* step 2. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_UPDT]
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_UPDATE
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_ID
        str     tmp1, [pmc, #AT91_PMC_PLL_UPDT]

        /* step 3. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_CTRL0]
        bic     tmp1, tmp1, #AT91_PMC_PLL_CTRL0_ENPLLCK
        orr     tmp1, tmp1, #AT91_PMC_PLL_CTRL0_ENPLL
        str     tmp1, [pmc, #AT91_PMC_PLL_CTRL0]

        /* step 4. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_UPDT]
        orr     tmp1, tmp1, #AT91_PMC_PLL_UPDT_UPDATE
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_ID
        str     tmp1, [pmc, #AT91_PMC_PLL_UPDT]

        /* step 5. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_CTRL0]
        bic     tmp1, tmp1, #AT91_PMC_PLL_CTRL0_ENPLL
        str     tmp1, [pmc, #AT91_PMC_PLL_CTRL0]

        /* step 7. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_UPDT]
        orr     tmp1, tmp1, #AT91_PMC_PLL_UPDT_UPDATE
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_ID
        str     tmp1, [pmc, #AT91_PMC_PLL_UPDT]

        b       2f
#endif

1:      /* 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]
2:
.endm

.macro at91_plla_enable
        ldr     tmp2, .saved_pllar
        ldr     tmp3, .pmc_version
        cmp     tmp3, #AT91_PMC_V1
        beq     4f

#ifdef CONFIG_HAVE_AT91_SAM9X60_PLL
        /* step 1. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_UPDT]
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_ID
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_UPDATE
        str     tmp1, [pmc, #AT91_PMC_PLL_UPDT]

        /* step 2. */
        ldr     tmp1, .saved_acr
        str     tmp1, [pmc, #AT91_PMC_PLL_ACR]

        /* step 3. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_CTRL1]
        mov     tmp3, tmp2
        bic     tmp3, tmp3, #0xffffff
        orr     tmp1, tmp1, tmp3
        str     tmp1, [pmc, #AT91_PMC_PLL_CTRL1]

        /* step 8. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_UPDT]
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_ID
        orr     tmp1, tmp1, #AT91_PMC_PLL_UPDT_UPDATE
        str     tmp1, [pmc, #AT91_PMC_PLL_UPDT]

        /* step 9. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_CTRL0]
        orr     tmp1, tmp1, #AT91_PMC_PLL_CTRL0_ENLOCK
        orr     tmp1, tmp1, #AT91_PMC_PLL_CTRL0_ENPLL
        orr     tmp1, tmp1, #AT91_PMC_PLL_CTRL0_ENPLLCK
        bic     tmp1, tmp1, #0xff
        mov     tmp3, tmp2
        bic     tmp3, tmp3, #0xffffff00
        orr     tmp1, tmp1, tmp3
        str     tmp1, [pmc, #AT91_PMC_PLL_CTRL0]

        /* step 10. */
        ldr     tmp1, [pmc, #AT91_PMC_PLL_UPDT]
        orr     tmp1, tmp1, #AT91_PMC_PLL_UPDT_UPDATE
        bic     tmp1, tmp1, #AT91_PMC_PLL_UPDT_ID
        str     tmp1, [pmc, #AT91_PMC_PLL_UPDT]

        /* step 11. */
3:      ldr     tmp1, [pmc, #AT91_PMC_PLL_ISR0]
        tst     tmp1, #0x1
        beq     3b
        b       2f
#endif

        /* Restore PLLA setting */
4:      str     tmp2, [pmc, #AT91_CKGR_PLLAR]

        /* Enable PLLA. */
        tst     tmp2, #(AT91_PMC_MUL &  0xff0000)
        bne     1f
        tst     tmp2, #(AT91_PMC_MUL & ~0xff0000)
        beq     2f

1:      ldr     tmp1, [pmc, #AT91_PMC_SR]
        tst     tmp1, #AT91_PMC_LOCKA
        beq     1b
2:
.endm

/**
 * at91_mckx_ps_enable: save MCK settings and switch it to main clock
 *
 * Side effects: overwrites tmp1, tmp2, tmp3
 */
.macro at91_mckx_ps_enable
#ifdef CONFIG_SOC_SAMA7
        ldr     pmc, .pmc_base
        ldr     tmp3, .mcks

        /* Start at MCK1 and go until MCKs */
        mov     tmp1, #1
e_loop:
        cmp     tmp1, tmp3
        bgt     e_done

        /* Write MCK ID to retrieve the settings. */
        str     tmp1, [pmc, #AT91_PMC_MCR_V2]
        ldr     tmp2, [pmc, #AT91_PMC_MCR_V2]

e_save_mck1:
        cmp     tmp1, #1
        bne     e_save_mck2
        str     tmp2, .saved_mck1
        b       e_ps

e_save_mck2:
        cmp     tmp1, #2
        bne     e_save_mck3
        str     tmp2, .saved_mck2
        b       e_ps

e_save_mck3:
        cmp     tmp1, #3
        bne     e_save_mck4
        str     tmp2, .saved_mck3
        b       e_ps

e_save_mck4:
        cmp     tmp1, #4
        bne     e_save_mck5
        str     tmp2, .saved_mck4
        b       e_ps

e_save_mck5:
        cmp     tmp1, #5
        bne     e_save_mck6
        str     tmp2, .saved_mck5
        b       e_ps

e_save_mck6:
        cmp     tmp1, #6
        bne     e_save_mck7
        str     tmp2, .saved_mck6
        b       e_ps

e_save_mck7:
        cmp     tmp1, #7
        bne     e_save_mck8
        str     tmp2, .saved_mck7
        b       e_ps

e_save_mck8:
        cmp     tmp1, #8
        bne     e_save_mck9
        str     tmp2, .saved_mck8
        b       e_ps

e_save_mck9:
        str     tmp2, .saved_mck9

e_ps:
        /* Use CSS=MAINCK and DIV=1. */
        bic     tmp2, tmp2, #AT91_PMC_MCR_V2_CSS
        bic     tmp2, tmp2, #AT91_PMC_MCR_V2_DIV
        orr     tmp2, tmp2, #AT91_PMC_MCR_V2_CSS_MAINCK
        orr     tmp2, tmp2, #AT91_PMC_MCR_V2_DIV1
        str     tmp2, [pmc, #AT91_PMC_MCR_V2]

        wait_mckrdy tmp1

        add     tmp1, tmp1, #1
        b       e_loop

e_done:
#endif
.endm

/**
 * at91_mckx_ps_restore: restore MCKx settings
 *
 * Side effects: overwrites tmp1, tmp2 and tmp3
 */
.macro at91_mckx_ps_restore
#ifdef CONFIG_SOC_SAMA7
        ldr     pmc, .pmc_base
        ldr     tmp2, .mcks

        /* Start from MCK1 and go up to MCKs */
        mov     tmp1, #1
r_loop:
        cmp     tmp1, tmp2
        bgt     r_done

r_save_mck1:
        cmp     tmp1, #1
        bne     r_save_mck2
        ldr     tmp2, .saved_mck1
        b       r_ps

r_save_mck2:
        cmp     tmp1, #2
        bne     r_save_mck3
        ldr     tmp2, .saved_mck2
        b       r_ps

r_save_mck3:
        cmp     tmp1, #3
        bne     r_save_mck4
        ldr     tmp2, .saved_mck3
        b       r_ps

r_save_mck4:
        cmp     tmp1, #4
        bne     r_save_mck5
        ldr     tmp2, .saved_mck4
        b       r_ps

r_save_mck5:
        cmp     tmp1, #5
        bne     r_save_mck6
        ldr     tmp2, .saved_mck5
        b       r_ps

r_save_mck6:
        cmp     tmp1, #6
        bne     r_save_mck7
        ldr     tmp2, .saved_mck6
        b       r_ps

r_save_mck7:
        cmp     tmp1, #7
        bne     r_save_mck8
        ldr     tmp2, .saved_mck7
        b       r_ps

r_save_mck8:
        cmp     tmp1, #8
        bne     r_save_mck9
        ldr     tmp2, .saved_mck8
        b       r_ps

r_save_mck9:
        ldr     tmp2, .saved_mck9

r_ps:
        /* Write MCK ID to retrieve the settings. */
        str     tmp1, [pmc, #AT91_PMC_MCR_V2]
        ldr     tmp3, [pmc, #AT91_PMC_MCR_V2]

        /* We need to restore CSS and DIV. */
        bic     tmp3, tmp3, #AT91_PMC_MCR_V2_CSS
        bic     tmp3, tmp3, #AT91_PMC_MCR_V2_DIV
        orr     tmp3, tmp3, tmp2
        bic     tmp3, tmp3, #AT91_PMC_MCR_V2_ID_MSK
        orr     tmp3, tmp3, tmp1
        orr     tmp3, tmp3, #AT91_PMC_MCR_V2_CMD
        str     tmp3, [pmc, #AT91_PMC_MCR_V2]

        wait_mckrdy tmp1

        add     tmp1, tmp1, #1
        ldr     tmp2, .mcks
        b       r_loop
r_done:
#endif
.endm

.macro at91_ulp_mode
        at91_mckx_ps_enable

        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 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]

        mov     tmp3, #0
        wait_mckrdy tmp3

        at91_plla_disable

        ldr     tmp3, .pm_mode
        cmp     tmp3, #AT91_PM_ULP1
        beq     ulp1_mode

        at91_pm_ulp0_mode
        b       ulp_exit

ulp1_mode:
        at91_pm_ulp1_mode
        b       ulp_exit

ulp_exit:
        ldr     pmc, .pmc_base

        at91_plla_enable

        /*
         * Restore master clock setting
         */
        ldr     tmp1, .mckr_offset
        ldr     tmp2, .saved_mckr
        str     tmp2, [pmc, tmp1]

        mov     tmp3, #0
        wait_mckrdy tmp3

        at91_mckx_ps_restore
.endm

.macro at91_backup_mode
        /* Switch the master clock source to slow clock. */
        ldr     pmc, .pmc_base
        ldr     tmp2, .mckr_offset
        ldr     tmp1, [pmc, tmp2]
        bic     tmp1, tmp1, #AT91_PMC_CSS
        str     tmp1, [pmc, tmp2]

        mov     tmp3, #0
        wait_mckrdy tmp3

        /*BUMEN*/
        ldr     r0, .sfrbu
        mov     tmp1, #0x1
        str     tmp1, [r0, #0x10]

        /* Wait for it. */
1:      ldr     tmp1, [r0, #0x10]
        tst     tmp1, #0x1
        beq     1b

        /* Shutdown */
        ldr     r0, .shdwc
        mov     tmp1, #0xA5000000
        add     tmp1, tmp1, #0x1
        at91_backup_set_lpm tmp1
        str     tmp1, [r0, #0]
.endm

/*
 * void at91_suspend_sram_fn(struct at91_pm_data*)
 * @input param:
 *      @r0: base address of struct at91_pm_data
 */
/* at91_pm_suspend_in_sram must be 8-byte aligned per the requirements of fncpy() */
        .align 3
ENTRY(at91_pm_suspend_in_sram)
        /* Save registers on stack */
        stmfd   sp!, {r4 - r12, lr}

        /* Drain write buffer */
        mov     tmp1, #0
        mcr     p15, 0, tmp1, c7, c10, 4

        /* Flush tlb. */
        mov     r4, #0
        mcr     p15, 0, r4, c8, c7, 0

        ldr     tmp1, [r0, #PM_DATA_PMC_MCKR_OFFSET]
        str     tmp1, .mckr_offset
        ldr     tmp1, [r0, #PM_DATA_PMC_VERSION]
        str     tmp1, .pmc_version
        ldr     tmp1, [r0, #PM_DATA_MEMCTRL]
        str     tmp1, .memtype
        ldr     tmp1, [r0, #PM_DATA_MODE]
        str     tmp1, .pm_mode
#ifdef CONFIG_SOC_SAMA7
        ldr     tmp1, [r0, #PM_DATA_PMC_MCKS]
        str     tmp1, .mcks
#endif

        /*
         * ldrne below are here to preload their address in the TLB as access
         * to RAM may be limited while in self-refresh.
         */
        ldr     tmp1, [r0, #PM_DATA_PMC]
        str     tmp1, .pmc_base
        cmp     tmp1, #0
        ldrne   tmp2, [tmp1, #0]

        ldr     tmp1, [r0, #PM_DATA_RAMC0]
        str     tmp1, .sramc_base
        cmp     tmp1, #0
        ldrne   tmp2, [tmp1, #0]

        ldr     tmp1, [r0, #PM_DATA_RAMC1]
        str     tmp1, .sramc1_base
        cmp     tmp1, #0
        ldrne   tmp2, [tmp1, #0]

#ifndef CONFIG_SOC_SAM_V4_V5
        /* ldrne below are here to preload their address in the TLB */
        ldr     tmp1, [r0, #PM_DATA_RAMC_PHY]
        str     tmp1, .sramc_phy_base
        cmp     tmp1, #0
        ldrne   tmp2, [tmp1, #0]

        ldr     tmp1, [r0, #PM_DATA_SHDWC]
        str     tmp1, .shdwc
        cmp     tmp1, #0
        ldrne   tmp2, [tmp1, #0]

        ldr     tmp1, [r0, #PM_DATA_SFRBU]
        str     tmp1, .sfrbu
        cmp     tmp1, #0
        ldrne   tmp2, [tmp1, #0x10]
#endif

        /* Active the self-refresh mode */
        at91_sramc_self_refresh_ena

        ldr     r0, .pm_mode
        cmp     r0, #AT91_PM_STANDBY
        beq     standby
        cmp     r0, #AT91_PM_BACKUP
        beq     backup_mode

        at91_ulp_mode
        b       exit_suspend

standby:
        /* Wait for interrupt */
        ldr     pmc, .pmc_base
        at91_cpu_idle
        b       exit_suspend

backup_mode:
        at91_backup_mode

exit_suspend:
        /* Exit the self-refresh mode */
        at91_sramc_self_refresh_dis

        /* Restore registers, and return */
        ldmfd   sp!, {r4 - r12, pc}
ENDPROC(at91_pm_suspend_in_sram)

.pmc_base:
        .word 0
.sramc_base:
        .word 0
.sramc1_base:
        .word 0
.sramc_phy_base:
        .word 0
.shdwc:
        .word 0
.sfrbu:
        .word 0
.memtype:
        .word 0
.pm_mode:
        .word 0
.mckr_offset:
        .word 0
.pmc_version:
        .word 0
#ifdef CONFIG_SOC_SAMA7
.mcks:
        .word 0
#endif
.saved_mckr:
        .word 0
.saved_acr:
        .word 0
.saved_pllar:
        .word 0
.saved_sam9_lpr:
        .word 0
.saved_sam9_lpr1:
        .word 0
.saved_sam9_mdr:
        .word 0
.saved_sam9_mdr1:
        .word 0
.saved_osc_status:
        .word 0
#ifdef CONFIG_SOC_SAMA7
.saved_mck1:
        .word 0
.saved_mck2:
        .word 0
.saved_mck3:
        .word 0
.saved_mck4:
        .word 0
.saved_mck5:
        .word 0
.saved_mck6:
        .word 0
.saved_mck7:
        .word 0
.saved_mck8:
        .word 0
.saved_mck9:
        .word 0
#endif

ENTRY(at91_pm_suspend_in_sram_sz)
        .word .-at91_pm_suspend_in_sram