6 files changed, 232 insertions, 94 deletions

diff --git a/Documentation/devicetree/bindings/arm/atmel-at91.txt b/Documentation/devicetree/bindings/arm/atmel-at91.txt
index 4bf1b4da7659..99dee23c74a4 100644
--- a/Documentation/devicetree/bindings/arm/atmel-at91.txt
+++ b/Documentation/devicetree/bindings/arm/atmel-at91.txt
@@ -25,6 +25,7 @@ compatible: must be one of:
     o "atmel,at91sam9n12"
     o "atmel,at91sam9rl"
     o "atmel,at91sam9xe"
+    o "microchip,sam9x60"
  * "atmel,sama5" for SoCs using a Cortex-A5, shall be extended with the specific
    SoC family:
     o "atmel,sama5d2" shall be extended with the specific SoC compatible:
diff --git a/arch/arm/mach-at91/at91sam9.c b/arch/arm/mach-at91/at91sam9.c
index 3dbdef4d3cbf..c12563b09656 100644
--- a/arch/arm/mach-at91/at91sam9.c
+++ b/arch/arm/mach-at91/at91sam9.c
@@ -32,3 +32,21 @@ DT_MACHINE_START(at91sam_dt, "Atmel AT91SAM9")
         .init_machine   = at91sam9_init,
         .dt_compat      = at91_dt_board_compat,
 MACHINE_END
+
+static void __init sam9x60_init(void)
+{
+        of_platform_default_populate(NULL, NULL, NULL);
+
+        sam9x60_pm_init();
+}
+
+static const char *const sam9x60_dt_board_compat[] __initconst = {
+        "microchip,sam9x60",
+        NULL
+};
+
+DT_MACHINE_START(sam9x60_dt, "Microchip SAM9X60")
+        /* Maintainer: Microchip */
+        .init_machine   = sam9x60_init,
+        .dt_compat      = sam9x60_dt_board_compat,
+MACHINE_END
diff --git a/arch/arm/mach-at91/generic.h b/arch/arm/mach-at91/generic.h
index e2bd17237964..72b45accfa0f 100644
--- a/arch/arm/mach-at91/generic.h
+++ b/arch/arm/mach-at91/generic.h
@@ -14,11 +14,13 @@
 #ifdef CONFIG_PM
 extern void __init at91rm9200_pm_init(void);
 extern void __init at91sam9_pm_init(void);
+extern void __init sam9x60_pm_init(void);
 extern void __init sama5_pm_init(void);
 extern void __init sama5d2_pm_init(void);
 #else
 static inline void __init at91rm9200_pm_init(void) { }
 static inline void __init at91sam9_pm_init(void) { }
+static inline void __init sam9x60_pm_init(void) { }
 static inline void __init sama5_pm_init(void) { }
 static inline void __init sama5d2_pm_init(void) { }
 #endif
diff --git a/arch/arm/mach-at91/pm.c b/arch/arm/mach-at91/pm.c
index 2a757dcaa1a5..6c8147536f3d 100644
--- a/arch/arm/mach-at91/pm.c
+++ b/arch/arm/mach-at91/pm.c
@@ -39,6 +39,20 @@ extern void at91_pinctrl_gpio_suspend(void);
 extern void at91_pinctrl_gpio_resume(void);
 #endif
 
+struct at91_soc_pm {
+        int (*config_shdwc_ws)(void __iomem *shdwc, u32 *mode, u32 *polarity);
+        int (*config_pmc_ws)(void __iomem *pmc, u32 mode, u32 polarity);
+        const struct of_device_id *ws_ids;
+        struct at91_pm_data data;
+};
+
+static struct at91_soc_pm soc_pm = {
+        .data = {
+                .standby_mode = AT91_PM_STANDBY,
+                .suspend_mode = AT91_PM_ULP0,
+        },
+};
+
 static const match_table_t pm_modes __initconst = {
         { AT91_PM_STANDBY, "standby" },
         { AT91_PM_ULP0, "ulp0" },
@@ -47,16 +61,11 @@ static const match_table_t pm_modes __initconst = {
         { -1, NULL },
 };
 
-static struct at91_pm_data pm_data = {
-        .standby_mode = AT91_PM_STANDBY,
-        .suspend_mode = AT91_PM_ULP0,
-};
-
 #define at91_ramc_read(id, field) \
-        __raw_readl(pm_data.ramc[id] + field)
+        __raw_readl(soc_pm.data.ramc[id] + field)
 
 #define at91_ramc_write(id, field, value) \
-        __raw_writel(value, pm_data.ramc[id] + field)
+        __raw_writel(value, soc_pm.data.ramc[id] + field)
 
 static int at91_pm_valid_state(suspend_state_t state)
 {
@@ -91,6 +100,8 @@ static const struct wakeup_source_info ws_info[] = {
         { .pmc_fsmr_bit = AT91_PMC_RTCAL,       .shdwc_mr_bit = BIT(17) },
         { .pmc_fsmr_bit = AT91_PMC_USBAL },
         { .pmc_fsmr_bit = AT91_PMC_SDMMC_CD },
+        { .pmc_fsmr_bit = AT91_PMC_RTTAL },
+        { .pmc_fsmr_bit = AT91_PMC_RXLP_MCE },
 };
 
 static const struct of_device_id sama5d2_ws_ids[] = {
@@ -105,6 +116,17 @@ static const struct of_device_id sama5d2_ws_ids[] = {
         { /* sentinel */ }
 };
 
+static const struct of_device_id sam9x60_ws_ids[] = {
+        { .compatible = "atmel,at91sam9x5-rtc",         .data = &ws_info[1] },
+        { .compatible = "atmel,at91rm9200-ohci",        .data = &ws_info[2] },
+        { .compatible = "usb-ohci",                     .data = &ws_info[2] },
+        { .compatible = "atmel,at91sam9g45-ehci",       .data = &ws_info[2] },
+        { .compatible = "usb-ehci",                     .data = &ws_info[2] },
+        { .compatible = "atmel,at91sam9260-rtt",        .data = &ws_info[4] },
+        { .compatible = "cdns,sam9x60-macb",            .data = &ws_info[5] },
+        { /* sentinel */ }
+};
+
 static int at91_pm_config_ws(unsigned int pm_mode, bool set)
 {
         const struct wakeup_source_info *wsi;
@@ -116,24 +138,22 @@ static int at91_pm_config_ws(unsigned int pm_mode, bool set)
         if (pm_mode != AT91_PM_ULP1)
                 return 0;
 
-        if (!pm_data.pmc || !pm_data.shdwc)
+        if (!soc_pm.data.pmc || !soc_pm.data.shdwc || !soc_pm.ws_ids)
                 return -EPERM;
 
         if (!set) {
-                writel(mode, pm_data.pmc + AT91_PMC_FSMR);
+                writel(mode, soc_pm.data.pmc + AT91_PMC_FSMR);
                 return 0;
         }
 
-        /* SHDWC.WUIR */
-        val = readl(pm_data.shdwc + 0x0c);
-        mode |= (val & 0x3ff);
-        polarity |= ((val >> 16) & 0x3ff);
+        if (soc_pm.config_shdwc_ws)
+                soc_pm.config_shdwc_ws(soc_pm.data.shdwc, &mode, &polarity);
 
         /* SHDWC.MR */
-        val = readl(pm_data.shdwc + 0x04);
+        val = readl(soc_pm.data.shdwc + 0x04);
 
         /* Loop through defined wakeup sources. */
-        for_each_matching_node_and_match(np, sama5d2_ws_ids, &match) {
+        for_each_matching_node_and_match(np, soc_pm.ws_ids, &match) {
                 pdev = of_find_device_by_node(np);
                 if (!pdev)
                         continue;
@@ -155,8 +175,8 @@ put_device:
         }
 
         if (mode) {
-                writel(mode, pm_data.pmc + AT91_PMC_FSMR);
-                writel(polarity, pm_data.pmc + AT91_PMC_FSPR);
+                if (soc_pm.config_pmc_ws)
+                        soc_pm.config_pmc_ws(soc_pm.data.pmc, mode, polarity);
         } else {
                 pr_err("AT91: PM: no ULP1 wakeup sources found!");
         }
@@ -164,6 +184,34 @@ put_device:
         return mode ? 0 : -EPERM;
 }
 
+static int at91_sama5d2_config_shdwc_ws(void __iomem *shdwc, u32 *mode,
+                                        u32 *polarity)
+{
+        u32 val;
+
+        /* SHDWC.WUIR */
+        val = readl(shdwc + 0x0c);
+        *mode |= (val & 0x3ff);
+        *polarity |= ((val >> 16) & 0x3ff);
+
+        return 0;
+}
+
+static int at91_sama5d2_config_pmc_ws(void __iomem *pmc, u32 mode, u32 polarity)
+{
+        writel(mode, pmc + AT91_PMC_FSMR);
+        writel(polarity, pmc + AT91_PMC_FSPR);
+
+        return 0;
+}
+
+static int at91_sam9x60_config_pmc_ws(void __iomem *pmc, u32 mode, u32 polarity)
+{
+        writel(mode, pmc + AT91_PMC_FSMR);
+
+        return 0;
+}
+
 /*
  * Called after processes are frozen, but before we shutdown devices.
  */
@@ -171,18 +219,18 @@ static int at91_pm_begin(suspend_state_t state)
 {
         switch (state) {
         case PM_SUSPEND_MEM:
-                pm_data.mode = pm_data.suspend_mode;
+                soc_pm.data.mode = soc_pm.data.suspend_mode;
                 break;
 
         case PM_SUSPEND_STANDBY:
-                pm_data.mode = pm_data.standby_mode;
+                soc_pm.data.mode = soc_pm.data.standby_mode;
                 break;
 
         default:
-                pm_data.mode = -1;
+                soc_pm.data.mode = -1;
         }
 
-        return at91_pm_config_ws(pm_data.mode, true);
+        return at91_pm_config_ws(soc_pm.data.mode, true);
 }
 
 /*
@@ -194,10 +242,10 @@ static int at91_pm_verify_clocks(void)
         unsigned long scsr;
         int i;
 
-        scsr = readl(pm_data.pmc + AT91_PMC_SCSR);
+        scsr = readl(soc_pm.data.pmc + AT91_PMC_SCSR);
 
         /* USB must not be using PLLB */
-        if ((scsr & pm_data.uhp_udp_mask) != 0) {
+        if ((scsr & soc_pm.data.uhp_udp_mask) != 0) {
                 pr_err("AT91: PM - Suspend-to-RAM with USB still active\n");
                 return 0;
         }
@@ -208,7 +256,7 @@ static int at91_pm_verify_clocks(void)
 
                 if ((scsr & (AT91_PMC_PCK0 << i)) == 0)
                         continue;
-                css = readl(pm_data.pmc + AT91_PMC_PCKR(i)) & AT91_PMC_CSS;
+                css = readl(soc_pm.data.pmc + AT91_PMC_PCKR(i)) & AT91_PMC_CSS;
                 if (css != AT91_PMC_CSS_SLOW) {
                         pr_err("AT91: PM - Suspend-to-RAM with PCK%d src %d\n", i, css);
                         return 0;
@@ -230,7 +278,7 @@ static int at91_pm_verify_clocks(void)
  */
 int at91_suspend_entering_slow_clock(void)
 {
-        return (pm_data.mode >= AT91_PM_ULP0);
+        return (soc_pm.data.mode >= AT91_PM_ULP0);
 }
 EXPORT_SYMBOL(at91_suspend_entering_slow_clock);
 
@@ -243,14 +291,14 @@ static int at91_suspend_finish(unsigned long val)
         flush_cache_all();
         outer_disable();
 
-        at91_suspend_sram_fn(&pm_data);
+        at91_suspend_sram_fn(&soc_pm.data);
 
         return 0;
 }
 
 static void at91_pm_suspend(suspend_state_t state)
 {
-        if (pm_data.mode == AT91_PM_BACKUP) {
+        if (soc_pm.data.mode == AT91_PM_BACKUP) {
                 pm_bu->suspended = 1;
 
                 cpu_suspend(0, at91_suspend_finish);
@@ -289,7 +337,7 @@ static int at91_pm_enter(suspend_state_t state)
                 /*
                  * Ensure that clocks are in a valid state.
                  */
-                if (pm_data.mode >= AT91_PM_ULP0 &&
+                if (soc_pm.data.mode >= AT91_PM_ULP0 &&
                     !at91_pm_verify_clocks())
                         goto error;
 
@@ -318,7 +366,7 @@ error:
  */
 static void at91_pm_end(void)
 {
-        at91_pm_config_ws(pm_data.mode, false);
+        at91_pm_config_ws(soc_pm.data.mode, false);
 }
 
 
@@ -351,7 +399,7 @@ static void at91rm9200_standby(void)
                 "    str    %2, [%1, %3]\n\t"
                 "    mcr    p15, 0, %0, c7, c0, 4\n\t"
                 :
-                : "r" (0), "r" (pm_data.ramc[0]),
+                : "r" (0), "r" (soc_pm.data.ramc[0]),
                   "r" (1), "r" (AT91_MC_SDRAMC_SRR));
 }
 
@@ -374,7 +422,7 @@ static void at91_ddr_standby(void)
                 at91_ramc_write(0, AT91_DDRSDRC_MDR, mdr);
         }
 
-        if (pm_data.ramc[1]) {
+        if (soc_pm.data.ramc[1]) {
                 saved_lpr1 = at91_ramc_read(1, AT91_DDRSDRC_LPR);
                 lpr1 = saved_lpr1 & ~AT91_DDRSDRC_LPCB;
                 lpr1 |= AT91_DDRSDRC_LPCB_SELF_REFRESH;
@@ -392,14 +440,14 @@ static void at91_ddr_standby(void)
 
         /* self-refresh mode now */
         at91_ramc_write(0, AT91_DDRSDRC_LPR, lpr0);
-        if (pm_data.ramc[1])
+        if (soc_pm.data.ramc[1])
                 at91_ramc_write(1, AT91_DDRSDRC_LPR, lpr1);
 
         cpu_do_idle();
 
         at91_ramc_write(0, AT91_DDRSDRC_MDR, saved_mdr0);
         at91_ramc_write(0, AT91_DDRSDRC_LPR, saved_lpr0);
-        if (pm_data.ramc[1]) {
+        if (soc_pm.data.ramc[1]) {
                 at91_ramc_write(0, AT91_DDRSDRC_MDR, saved_mdr1);
                 at91_ramc_write(1, AT91_DDRSDRC_LPR, saved_lpr1);
         }
@@ -429,7 +477,7 @@ static void at91sam9_sdram_standby(void)
         u32 lpr0, lpr1 = 0;
         u32 saved_lpr0, saved_lpr1 = 0;
 
-        if (pm_data.ramc[1]) {
+        if (soc_pm.data.ramc[1]) {
                 saved_lpr1 = at91_ramc_read(1, AT91_SDRAMC_LPR);
                 lpr1 = saved_lpr1 & ~AT91_SDRAMC_LPCB;
                 lpr1 |= AT91_SDRAMC_LPCB_SELF_REFRESH;
@@ -441,13 +489,13 @@ static void at91sam9_sdram_standby(void)
 
         /* self-refresh mode now */
         at91_ramc_write(0, AT91_SDRAMC_LPR, lpr0);
-        if (pm_data.ramc[1])
+        if (soc_pm.data.ramc[1])
                 at91_ramc_write(1, AT91_SDRAMC_LPR, lpr1);
 
         cpu_do_idle();
 
         at91_ramc_write(0, AT91_SDRAMC_LPR, saved_lpr0);
-        if (pm_data.ramc[1])
+        if (soc_pm.data.ramc[1])
                 at91_ramc_write(1, AT91_SDRAMC_LPR, saved_lpr1);
 }
 
@@ -480,14 +528,14 @@ static __init void at91_dt_ramc(void)
         const struct ramc_info *ramc;
 
         for_each_matching_node_and_match(np, ramc_ids, &of_id) {
-                pm_data.ramc[idx] = of_iomap(np, 0);
-                if (!pm_data.ramc[idx])
+                soc_pm.data.ramc[idx] = of_iomap(np, 0);
+                if (!soc_pm.data.ramc[idx])
                         panic(pr_fmt("unable to map ramc[%d] cpu registers\n"), idx);
 
                 ramc = of_id->data;
                 if (!standby)
                         standby = ramc->idle;
-                pm_data.memctrl = ramc->memctrl;
+                soc_pm.data.memctrl = ramc->memctrl;
 
                 idx++;
         }
@@ -509,12 +557,17 @@ static void at91rm9200_idle(void)
          * Disable the processor clock.  The processor will be automatically
          * re-enabled by an interrupt or by a reset.
          */
-        writel(AT91_PMC_PCK, pm_data.pmc + AT91_PMC_SCDR);
+        writel(AT91_PMC_PCK, soc_pm.data.pmc + AT91_PMC_SCDR);
+}
+
+static void at91sam9x60_idle(void)
+{
+        cpu_do_idle();
 }
 
 static void at91sam9_idle(void)
 {
-        writel(AT91_PMC_PCK, pm_data.pmc + AT91_PMC_SCDR);
+        writel(AT91_PMC_PCK, soc_pm.data.pmc + AT91_PMC_SCDR);
         cpu_do_idle();
 }
 
@@ -566,8 +619,8 @@ static void __init at91_pm_sram_init(void)
 
 static bool __init at91_is_pm_mode_active(int pm_mode)
 {
-        return (pm_data.standby_mode == pm_mode ||
-                pm_data.suspend_mode == pm_mode);
+        return (soc_pm.data.standby_mode == pm_mode ||
+                soc_pm.data.suspend_mode == pm_mode);
 }
 
 static int __init at91_pm_backup_init(void)
@@ -577,6 +630,9 @@ static int __init at91_pm_backup_init(void)
         struct platform_device *pdev = NULL;
         int ret = -ENODEV;
 
+        if (!IS_ENABLED(CONFIG_SOC_SAMA5D2))
+                return -EPERM;
+
         if (!at91_is_pm_mode_active(AT91_PM_BACKUP))
                 return 0;
 
@@ -586,7 +642,7 @@ static int __init at91_pm_backup_init(void)
                 return ret;
         }
 
-        pm_data.sfrbu = of_iomap(np, 0);
+        soc_pm.data.sfrbu = of_iomap(np, 0);
         of_node_put(np);
 
         np = of_find_compatible_node(NULL, NULL, "atmel,sama5d2-securam");
@@ -622,8 +678,8 @@ static int __init at91_pm_backup_init(void)
 securam_fail:
         put_device(&pdev->dev);
 securam_fail_no_ref_dev:
-        iounmap(pm_data.sfrbu);
-        pm_data.sfrbu = NULL;
+        iounmap(soc_pm.data.sfrbu);
+        soc_pm.data.sfrbu = NULL;
         return ret;
 }
 
@@ -632,10 +688,10 @@ static void __init at91_pm_use_default_mode(int pm_mode)
         if (pm_mode != AT91_PM_ULP1 && pm_mode != AT91_PM_BACKUP)
                 return;
 
-        if (pm_data.standby_mode == pm_mode)
-                pm_data.standby_mode = AT91_PM_ULP0;
-        if (pm_data.suspend_mode == pm_mode)
-                pm_data.suspend_mode = AT91_PM_ULP0;
+        if (soc_pm.data.standby_mode == pm_mode)
+                soc_pm.data.standby_mode = AT91_PM_ULP0;
+        if (soc_pm.data.suspend_mode == pm_mode)
+                soc_pm.data.suspend_mode = AT91_PM_ULP0;
 }
 
 static void __init at91_pm_modes_init(void)
@@ -653,7 +709,7 @@ static void __init at91_pm_modes_init(void)
                 goto ulp1_default;
         }
 
-        pm_data.shdwc = of_iomap(np, 0);
+        soc_pm.data.shdwc = of_iomap(np, 0);
         of_node_put(np);
 
         ret = at91_pm_backup_init();
@@ -667,8 +723,8 @@ static void __init at91_pm_modes_init(void)
         return;
 
 unmap:
-        iounmap(pm_data.shdwc);
-        pm_data.shdwc = NULL;
+        iounmap(soc_pm.data.shdwc);
+        soc_pm.data.shdwc = NULL;
 ulp1_default:
         at91_pm_use_default_mode(AT91_PM_ULP1);
 backup_default:
@@ -711,14 +767,14 @@ static void __init at91_pm_init(void (*pm_idle)(void))
                 platform_device_register(&at91_cpuidle_device);
 
         pmc_np = of_find_matching_node_and_match(NULL, atmel_pmc_ids, &of_id);
-        pm_data.pmc = of_iomap(pmc_np, 0);
-        if (!pm_data.pmc) {
+        soc_pm.data.pmc = of_iomap(pmc_np, 0);
+        if (!soc_pm.data.pmc) {
                 pr_err("AT91: PM not supported, PMC not found\n");
                 return;
         }
 
         pmc = of_id->data;
-        pm_data.uhp_udp_mask = pmc->uhp_udp_mask;
+        soc_pm.data.uhp_udp_mask = pmc->uhp_udp_mask;
 
         if (pm_idle)
                 arm_pm_idle = pm_idle;
@@ -728,8 +784,8 @@ static void __init at91_pm_init(void (*pm_idle)(void))
         if (at91_suspend_sram_fn) {
                 suspend_set_ops(&at91_pm_ops);
                 pr_info("AT91: PM: standby: %s, suspend: %s\n",
-                        pm_modes[pm_data.standby_mode].pattern,
-                        pm_modes[pm_data.suspend_mode].pattern);
+                        pm_modes[soc_pm.data.standby_mode].pattern,
+                        pm_modes[soc_pm.data.suspend_mode].pattern);
         } else {
                 pr_info("AT91: PM not supported, due to no SRAM allocated\n");
         }
@@ -750,6 +806,19 @@ void __init at91rm9200_pm_init(void)
         at91_pm_init(at91rm9200_idle);
 }
 
+void __init sam9x60_pm_init(void)
+{
+        if (!IS_ENABLED(CONFIG_SOC_AT91SAM9))
+                return;
+
+        at91_pm_modes_init();
+        at91_dt_ramc();
+        at91_pm_init(at91sam9x60_idle);
+
+        soc_pm.ws_ids = sam9x60_ws_ids;
+        soc_pm.config_pmc_ws = at91_sam9x60_config_pmc_ws;
+}
+
 void __init at91sam9_pm_init(void)
 {
         if (!IS_ENABLED(CONFIG_SOC_AT91SAM9))
@@ -775,6 +844,10 @@ void __init sama5d2_pm_init(void)
 
         at91_pm_modes_init();
         sama5_pm_init();
+
+        soc_pm.ws_ids = sama5d2_ws_ids;
+        soc_pm.config_shdwc_ws = at91_sama5d2_config_shdwc_ws;
+        soc_pm.config_pmc_ws = at91_sama5d2_config_pmc_ws;
 }
 
 static int __init at91_pm_modes_select(char *str)
@@ -795,8 +868,8 @@ static int __init at91_pm_modes_select(char *str)
         if (suspend < 0)
                 return 0;
 
-        pm_data.standby_mode = standby;
-        pm_data.suspend_mode = suspend;
+        soc_pm.data.standby_mode = standby;
+        soc_pm.data.suspend_mode = suspend;
 
         return 0;
 }
diff --git a/arch/arm/mach-at91/pm_suspend.S b/arch/arm/mach-at91/pm_suspend.S
index bfe1c4d06901..77e29309cc6e 100644
--- a/arch/arm/mach-at91/pm_suspend.S
+++ b/arch/arm/mach-at91/pm_suspend.S
@@ -51,15 +51,6 @@ tmp2	.req	r5
         .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
  */
         .macro at91_cpu_idle
@@ -178,11 +169,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]
@@ -197,8 +223,26 @@ ENDPROC(at91_backup_mode)
 .macro at91_pm_ulp1_mode
         ldr     pmc, .pmc_base
 
-        /* Switch the main clock source to 12-MHz RC oscillator */
+        /* 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
@@ -262,6 +306,25 @@ ENDPROC(at91_backup_mode)
         str     tmp1, [pmc, #AT91_PMC_MCKR]
 
         wait_mckrdy
+
+        /* 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
 
 ENTRY(at91_ulp_mode)
@@ -279,14 +342,6 @@ 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]
-
         ldr     r0, .pm_mode
         cmp     r0, #AT91_PM_ULP1
         beq     ulp1_mode
@@ -301,18 +356,6 @@ 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:
-
         /*
          * Restore master clock setting
          */
@@ -465,8 +508,6 @@ ENDPROC(at91_sramc_self_refresh)
         .word 0
 .saved_mckr:
         .word 0
-.saved_pllar:
-        .word 0
 .saved_sam9_lpr:
         .word 0
 .saved_sam9_lpr1:
@@ -475,6 +516,8 @@ ENDPROC(at91_sramc_self_refresh)
         .word 0
 .saved_sam9_mdr1:
         .word 0
+.saved_osc_status:
+        .word 0
 
 ENTRY(at91_pm_suspend_in_sram_sz)
         .word .-at91_pm_suspend_in_sram
diff --git a/include/linux/clk/at91_pmc.h b/include/linux/clk/at91_pmc.h
index 0c53f26ae3d3..44e8fc30b889 100644
--- a/include/linux/clk/at91_pmc.h
+++ b/include/linux/clk/at91_pmc.h
@@ -161,6 +161,7 @@
 
 #define AT91_PMC_FSMR           0x70            /* Fast Startup Mode Register */
 #define AT91_PMC_FSTT(n)        BIT(n)
+#define AT91_PMC_RTTAL          BIT(16)
 #define AT91_PMC_RTCAL          BIT(17)         /* RTC Alarm Enable */
 #define AT91_PMC_USBAL          BIT(18)         /* USB Resume Enable */
 #define AT91_PMC_SDMMC_CD       BIT(19)         /* SDMMC Card Detect Enable */