ARM: OMAP: Add initial 24xx suspend support

This patch adds support for omap24xx power domains and
allows suspend to work.

Please note that for some reason core power domain still
does not seem to idle.

Signed-off-by: Tony Lindgren <tony@atomide.com>

3 files changed, 566 insertions, 7 deletions

diff --git a/arch/arm/mach-omap2/Makefile b/arch/arm/mach-omap2/Makefile
index 7a8edd6cd198..266d88e77bdc 100644
--- a/arch/arm/mach-omap2/Makefile
+++ b/arch/arm/mach-omap2/Makefile
@@ -9,7 +9,7 @@ obj-y := irq.o id.o io.o sram-fn.o memory.o prcm.o clock.o mux.o devices.o \
 obj-$(CONFIG_OMAP_MPU_TIMER)            += timer-gp.o
 
 # Power Management
-obj-$(CONFIG_PM) += pm.o sleep.o
+obj-$(CONFIG_PM) += pm.o pm-domain.o sleep.o
 
 # Specific board support
 obj-$(CONFIG_MACH_OMAP_GENERIC)         += board-generic.o
diff --git a/arch/arm/mach-omap2/pm-domain.c b/arch/arm/mach-omap2/pm-domain.c
new file mode 100644
index 000000000000..5e20e740cde5
--- /dev/null
+++ b/arch/arm/mach-omap2/pm-domain.c
@@ -0,0 +1,300 @@
+/*
+ * linux/arch/arm/mach-omap2/pm-domain.c
+ *
+ * Power domain functions for OMAP2
+ *
+ * Copyright (C) 2006 Nokia Corporation
+ * Tony Lindgren <tony@atomide.com>
+ *
+ * Some code based on earlier OMAP2 sample PM code
+ * Copyright (C) 2005 Texas Instruments, Inc.
+ * Richard Woodruff <r-woodruff2@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/config.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+
+#include <asm/io.h>
+
+#include "prcm-regs.h"
+
+/* Power domain offsets */
+#define PM_MPU_OFFSET                   0x100
+#define PM_CORE_OFFSET                  0x200
+#define PM_GFX_OFFSET                   0x300
+#define PM_WKUP_OFFSET                  0x400           /* Autoidle only */
+#define PM_PLL_OFFSET                   0x500           /* Autoidle only */
+#define PM_DSP_OFFSET                   0x800
+#define PM_MDM_OFFSET                   0xc00
+
+/* Power domain wake-up dependency control register */
+#define PM_WKDEP_OFFSET                 0xc8
+#define         EN_MDM                  (1 << 5)
+#define         EN_WKUP                 (1 << 4)
+#define         EN_GFX                  (1 << 3)
+#define         EN_DSP                  (1 << 2)
+#define         EN_MPU                  (1 << 1)
+#define         EN_CORE                 (1 << 0)
+
+/* Core power domain state transition control register */
+#define PM_PWSTCTRL_OFFSET              0xe0
+#define         FORCESTATE              (1 << 18)       /* Only for DSP & GFX */
+#define         MEM4RETSTATE            (1 << 6)
+#define         MEM3RETSTATE            (1 << 5)
+#define         MEM2RETSTATE            (1 << 4)
+#define         MEM1RETSTATE            (1 << 3)
+#define         LOGICRETSTATE           (1 << 2)        /* Logic is retained */
+#define         POWERSTATE_OFF          0x3
+#define         POWERSTATE_RETENTION    0x1
+#define         POWERSTATE_ON           0x0
+
+/* Power domain state register */
+#define PM_PWSTST_OFFSET                0xe4
+
+/* Hardware supervised state transition control register */
+#define CM_CLKSTCTRL_OFFSET             0x48
+#define         AUTOSTAT_MPU            (1 << 0)        /* MPU */
+#define         AUTOSTAT_DSS            (1 << 2)        /* Core */
+#define         AUTOSTAT_L4             (1 << 1)        /* Core */
+#define         AUTOSTAT_L3             (1 << 0)        /* Core */
+#define         AUTOSTAT_GFX            (1 << 0)        /* GFX */
+#define         AUTOSTAT_IVA            (1 << 8)        /* 2420 IVA in DSP domain */
+#define         AUTOSTAT_DSP            (1 << 0)        /* DSP */
+#define         AUTOSTAT_MDM            (1 << 0)        /* MDM */
+
+/* Automatic control of interface clock idling */
+#define CM_AUTOIDLE1_OFFSET             0x30
+#define CM_AUTOIDLE2_OFFSET             0x34            /* Core only */
+#define CM_AUTOIDLE3_OFFSET             0x38            /* Core only */
+#define CM_AUTOIDLE4_OFFSET             0x3c            /* Core only */
+#define         AUTO_54M(x)             (((x) & 0x3) << 6)
+#define         AUTO_96M(x)             (((x) & 0x3) << 2)
+#define         AUTO_DPLL(x)            (((x) & 0x3) << 0)
+#define         AUTO_STOPPED            0x3
+#define         AUTO_BYPASS_FAST        0x2             /* DPLL only */
+#define         AUTO_BYPASS_LOW_POWER   0x1             /* DPLL only */
+#define         AUTO_DISABLED           0x0
+
+/* Voltage control PRCM_VOLTCTRL bits */
+#define         AUTO_EXTVOLT            (1 << 15)
+#define         FORCE_EXTVOLT           (1 << 14)
+#define         SETOFF_LEVEL(x)         (((x) & 0x3) << 12)
+#define         MEMRETCTRL              (1 << 8)
+#define         SETRET_LEVEL(x)         (((x) & 0x3) << 6)
+#define         VOLT_LEVEL(x)           (((x) & 0x3) << 0)
+
+#define OMAP24XX_PRCM_VBASE     IO_ADDRESS(OMAP24XX_PRCM_BASE)
+#define prcm_readl(r)           __raw_readl(OMAP24XX_PRCM_VBASE + (r))
+#define prcm_writel(v, r)       __raw_writel((v), OMAP24XX_PRCM_VBASE + (r))
+
+static u32 pmdomain_get_wakeup_dependencies(int domain_offset)
+{
+        return prcm_readl(domain_offset + PM_WKDEP_OFFSET);
+}
+
+static void pmdomain_set_wakeup_dependencies(u32 state, int domain_offset)
+{
+        prcm_writel(state, domain_offset + PM_WKDEP_OFFSET);
+}
+
+static u32 pmdomain_get_powerstate(int domain_offset)
+{
+        return prcm_readl(domain_offset + PM_PWSTCTRL_OFFSET);
+}
+
+static void pmdomain_set_powerstate(u32 state, int domain_offset)
+{
+        prcm_writel(state, domain_offset + PM_PWSTCTRL_OFFSET);
+}
+
+static u32 pmdomain_get_clock_autocontrol(int domain_offset)
+{
+        return prcm_readl(domain_offset + CM_CLKSTCTRL_OFFSET);
+}
+
+static void pmdomain_set_clock_autocontrol(u32 state, int domain_offset)
+{
+        prcm_writel(state, domain_offset + CM_CLKSTCTRL_OFFSET);
+}
+
+static u32 pmdomain_get_clock_autoidle1(int domain_offset)
+{
+        return prcm_readl(domain_offset + CM_AUTOIDLE1_OFFSET);
+}
+
+/* Core domain only */
+static u32 pmdomain_get_clock_autoidle2(int domain_offset)
+{
+        return prcm_readl(domain_offset + CM_AUTOIDLE2_OFFSET);
+}
+
+/* Core domain only */
+static u32 pmdomain_get_clock_autoidle3(int domain_offset)
+{
+        return prcm_readl(domain_offset + CM_AUTOIDLE3_OFFSET);
+}
+
+/* Core domain only */
+static u32 pmdomain_get_clock_autoidle4(int domain_offset)
+{
+        return prcm_readl(domain_offset + CM_AUTOIDLE4_OFFSET);
+}
+
+static void pmdomain_set_clock_autoidle1(u32 state, int domain_offset)
+{
+        prcm_writel(state, CM_AUTOIDLE1_OFFSET + domain_offset);
+}
+
+/* Core domain only */
+static void pmdomain_set_clock_autoidle2(u32 state, int domain_offset)
+{
+        prcm_writel(state, CM_AUTOIDLE2_OFFSET + domain_offset);
+}
+
+/* Core domain only */
+static void pmdomain_set_clock_autoidle3(u32 state, int domain_offset)
+{
+        prcm_writel(state, CM_AUTOIDLE3_OFFSET + domain_offset);
+}
+
+/* Core domain only */
+static void pmdomain_set_clock_autoidle4(u32 state, int domain_offset)
+{
+        prcm_writel(state, CM_AUTOIDLE4_OFFSET + domain_offset);
+}
+
+/*
+ * Configures power management domains to idle clocks automatically.
+ */
+void pmdomain_set_autoidle(void)
+{
+        u32 val;
+
+        /* Set PLL auto stop for 54M, 96M & DPLL */
+        pmdomain_set_clock_autoidle1(AUTO_54M(AUTO_STOPPED) |
+                                     AUTO_96M(AUTO_STOPPED) |
+                                     AUTO_DPLL(AUTO_STOPPED), PM_PLL_OFFSET);
+
+        /* External clock input control
+         * REVISIT: Should this be in clock framework?
+         */
+        PRCM_CLKSRC_CTRL |= (0x3 << 3);
+
+        /* Configure number of 32KHz clock cycles for sys_clk */
+        PRCM_CLKSSETUP = 0x00ff;
+
+        /* Configure automatic voltage transition */
+        PRCM_VOLTSETUP = 0;
+        val = PRCM_VOLTCTRL;
+        val &= ~(SETOFF_LEVEL(0x3) | VOLT_LEVEL(0x3));
+        val |= SETOFF_LEVEL(1) | VOLT_LEVEL(1) | AUTO_EXTVOLT;
+        PRCM_VOLTCTRL = val;
+
+        /* Disable emulation tools functional clock */
+        PRCM_CLKEMUL_CTRL = 0x0;
+
+        /* Set core memory retention state */
+        val = pmdomain_get_powerstate(PM_CORE_OFFSET);
+        if (cpu_is_omap2420()) {
+                val &= ~(0x7 << 3);
+                val |= (MEM3RETSTATE | MEM2RETSTATE | MEM1RETSTATE);
+        } else {
+                val &= ~(0xf << 3);
+                val |= (MEM4RETSTATE | MEM3RETSTATE | MEM2RETSTATE |
+                        MEM1RETSTATE);
+        }
+        pmdomain_set_powerstate(val, PM_CORE_OFFSET);
+
+        /* OCP interface smart idle. REVISIT: Enable autoidle bit0 ? */
+        val = SMS_SYSCONFIG;
+        val &= ~(0x3 << 3);
+        val |= (0x2 << 3) | (1 << 0);
+        SMS_SYSCONFIG |= val;
+
+        val = SDRC_SYSCONFIG;
+        val &= ~(0x3 << 3);
+        val |= (0x2 << 3);
+        SDRC_SYSCONFIG = val;
+
+        /* Configure L3 interface for smart idle.
+         * REVISIT: Enable autoidle bit0 ?
+         */
+        val = GPMC_SYSCONFIG;
+        val &= ~(0x3 << 3);
+        val |= (0x2 << 3) | (1 << 0);
+        GPMC_SYSCONFIG = val;
+
+        pmdomain_set_powerstate(LOGICRETSTATE | POWERSTATE_RETENTION,
+                                PM_MPU_OFFSET);
+        pmdomain_set_powerstate(POWERSTATE_RETENTION, PM_CORE_OFFSET);
+        if (!cpu_is_omap2420())
+                pmdomain_set_powerstate(POWERSTATE_RETENTION, PM_MDM_OFFSET);
+
+        /* Assume suspend function has saved the state for DSP and GFX */
+        pmdomain_set_powerstate(FORCESTATE | POWERSTATE_OFF, PM_DSP_OFFSET);
+        pmdomain_set_powerstate(FORCESTATE | POWERSTATE_OFF, PM_GFX_OFFSET);
+
+#if 0
+        /* REVISIT: Internal USB needs special handling */
+        force_standby_usb();
+        if (cpu_is_omap2430())
+                force_hsmmc();
+        sdram_self_refresh_on_idle_req(1);
+#endif
+
+        /* Enable clock auto control for all domains.
+         * Note that CORE domain includes also DSS, L4 & L3.
+         */
+        pmdomain_set_clock_autocontrol(AUTOSTAT_MPU, PM_MPU_OFFSET);
+        pmdomain_set_clock_autocontrol(AUTOSTAT_GFX, PM_GFX_OFFSET);
+        pmdomain_set_clock_autocontrol(AUTOSTAT_DSS | AUTOSTAT_L4 | AUTOSTAT_L3,
+                                       PM_CORE_OFFSET);
+        if (cpu_is_omap2420())
+                pmdomain_set_clock_autocontrol(AUTOSTAT_IVA | AUTOSTAT_DSP,
+                                               PM_DSP_OFFSET);
+        else {
+                pmdomain_set_clock_autocontrol(AUTOSTAT_DSP, PM_DSP_OFFSET);
+                pmdomain_set_clock_autocontrol(AUTOSTAT_MDM, PM_MDM_OFFSET);
+        }
+
+        /* Enable clock autoidle for all domains */
+        pmdomain_set_clock_autoidle1(0x2, PM_DSP_OFFSET);
+        if (cpu_is_omap2420()) {
+                pmdomain_set_clock_autoidle1(0xfffffff9, PM_CORE_OFFSET);
+                pmdomain_set_clock_autoidle2(0x7, PM_CORE_OFFSET);
+                pmdomain_set_clock_autoidle1(0x3f, PM_WKUP_OFFSET);
+        } else {
+                pmdomain_set_clock_autoidle1(0xeafffff1, PM_CORE_OFFSET);
+                pmdomain_set_clock_autoidle2(0xfff, PM_CORE_OFFSET);
+                pmdomain_set_clock_autoidle1(0x7f, PM_WKUP_OFFSET);
+                pmdomain_set_clock_autoidle1(0x3, PM_MDM_OFFSET);
+        }
+        pmdomain_set_clock_autoidle3(0x7, PM_CORE_OFFSET);
+        pmdomain_set_clock_autoidle4(0x1f, PM_CORE_OFFSET);
+}
+
+/*
+ * Initializes power domains by removing wake-up dependencies and powering
+ * down DSP and GFX. Gets called from PM init. Note that DSP and IVA code
+ * must re-enable DSP and GFX when used.
+ */
+void __init pmdomain_init(void)
+{
+        /* Remove all domain wakeup dependencies */
+        pmdomain_set_wakeup_dependencies(EN_WKUP | EN_CORE, PM_MPU_OFFSET);
+        pmdomain_set_wakeup_dependencies(0, PM_DSP_OFFSET);
+        pmdomain_set_wakeup_dependencies(0, PM_GFX_OFFSET);
+        pmdomain_set_wakeup_dependencies(EN_WKUP | EN_MPU, PM_CORE_OFFSET);
+        if (cpu_is_omap2430())
+                pmdomain_set_wakeup_dependencies(0, PM_MDM_OFFSET);
+
+        /* Power down DSP and GFX */
+        pmdomain_set_powerstate(POWERSTATE_OFF | FORCESTATE, PM_DSP_OFFSET);
+        pmdomain_set_powerstate(POWERSTATE_OFF | FORCESTATE, PM_GFX_OFFSET);
+}
diff --git a/arch/arm/mach-omap2/pm.c b/arch/arm/mach-omap2/pm.c
index 562168fa2b16..d7eee99b7e3f 100644
--- a/arch/arm/mach-omap2/pm.c
+++ b/arch/arm/mach-omap2/pm.c
@@ -23,6 +23,7 @@
 #include <linux/interrupt.h>
 #include <linux/sysfs.h>
 #include <linux/module.h>
+#include <linux/delay.h>
 
 #include <asm/io.h>
 #include <asm/irq.h>
@@ -36,11 +37,18 @@
 #include <asm/arch/sram.h>
 #include <asm/arch/pm.h>
 
+#include "prcm-regs.h"
+
 static struct clk *vclk;
 static void (*omap2_sram_idle)(void);
 static void (*omap2_sram_suspend)(int dllctrl, int cpu_rev);
 static void (*saved_idle)(void);
 
+extern void __init pmdomain_init(void);
+extern void pmdomain_set_autoidle(void);
+
+static unsigned int omap24xx_sleep_save[OMAP24XX_SLEEP_SAVE_SIZE];
+
 void omap2_pm_idle(void)
 {
         local_irq_disable();
@@ -87,23 +95,272 @@ static int omap2_pm_prepare(suspend_state_t state)
         return error;
 }
 
+#define INT0_WAKE_MASK  (OMAP_IRQ_BIT(INT_24XX_GPIO_BANK1) |    \
+                        OMAP_IRQ_BIT(INT_24XX_GPIO_BANK2) |     \
+                        OMAP_IRQ_BIT(INT_24XX_GPIO_BANK3))
+
+#define INT1_WAKE_MASK  (OMAP_IRQ_BIT(INT_24XX_GPIO_BANK4))
+
+#define INT2_WAKE_MASK  (OMAP_IRQ_BIT(INT_24XX_UART1_IRQ) |     \
+                        OMAP_IRQ_BIT(INT_24XX_UART2_IRQ) |      \
+                        OMAP_IRQ_BIT(INT_24XX_UART3_IRQ))
+
+#define preg(reg)       printk("%s\t(0x%p):\t0x%08x\n", #reg, &reg, reg);
+
+static void omap2_pm_debug(char * desc)
+{
+        printk("%s:\n", desc);
+
+        preg(CM_CLKSTCTRL_MPU);
+        preg(CM_CLKSTCTRL_CORE);
+        preg(CM_CLKSTCTRL_GFX);
+        preg(CM_CLKSTCTRL_DSP);
+        preg(CM_CLKSTCTRL_MDM);
+
+        preg(PM_PWSTCTRL_MPU);
+        preg(PM_PWSTCTRL_CORE);
+        preg(PM_PWSTCTRL_GFX);
+        preg(PM_PWSTCTRL_DSP);
+        preg(PM_PWSTCTRL_MDM);
+
+        preg(PM_PWSTST_MPU);
+        preg(PM_PWSTST_CORE);
+        preg(PM_PWSTST_GFX);
+        preg(PM_PWSTST_DSP);
+        preg(PM_PWSTST_MDM);
+
+        preg(CM_AUTOIDLE1_CORE);
+        preg(CM_AUTOIDLE2_CORE);
+        preg(CM_AUTOIDLE3_CORE);
+        preg(CM_AUTOIDLE4_CORE);
+        preg(CM_AUTOIDLE_WKUP);
+        preg(CM_AUTOIDLE_PLL);
+        preg(CM_AUTOIDLE_DSP);
+        preg(CM_AUTOIDLE_MDM);
+
+        preg(CM_ICLKEN1_CORE);
+        preg(CM_ICLKEN2_CORE);
+        preg(CM_ICLKEN3_CORE);
+        preg(CM_ICLKEN4_CORE);
+        preg(CM_ICLKEN_GFX);
+        preg(CM_ICLKEN_WKUP);
+        preg(CM_ICLKEN_DSP);
+        preg(CM_ICLKEN_MDM);
+
+        preg(CM_IDLEST1_CORE);
+        preg(CM_IDLEST2_CORE);
+        preg(CM_IDLEST3_CORE);
+        preg(CM_IDLEST4_CORE);
+        preg(CM_IDLEST_GFX);
+        preg(CM_IDLEST_WKUP);
+        preg(CM_IDLEST_CKGEN);
+        preg(CM_IDLEST_DSP);
+        preg(CM_IDLEST_MDM);
+
+        preg(RM_RSTST_MPU);
+        preg(RM_RSTST_GFX);
+        preg(RM_RSTST_WKUP);
+        preg(RM_RSTST_DSP);
+        preg(RM_RSTST_MDM);
+
+        preg(PM_WKDEP_MPU);
+        preg(PM_WKDEP_CORE);
+        preg(PM_WKDEP_GFX);
+        preg(PM_WKDEP_DSP);
+        preg(PM_WKDEP_MDM);
+
+        preg(CM_FCLKEN_WKUP);
+        preg(CM_ICLKEN_WKUP);
+        preg(CM_IDLEST_WKUP);
+        preg(CM_AUTOIDLE_WKUP);
+        preg(CM_CLKSEL_WKUP);
+
+        preg(PM_WKEN_WKUP);
+        preg(PM_WKST_WKUP);
+}
+
+static inline void omap2_pm_save_registers(void)
+{
+        /* Save interrupt registers */
+        OMAP24XX_SAVE(INTC_MIR0);
+        OMAP24XX_SAVE(INTC_MIR1);
+        OMAP24XX_SAVE(INTC_MIR2);
+
+        /* Save power control registers */
+        OMAP24XX_SAVE(CM_CLKSTCTRL_MPU);
+        OMAP24XX_SAVE(CM_CLKSTCTRL_CORE);
+        OMAP24XX_SAVE(CM_CLKSTCTRL_GFX);
+        OMAP24XX_SAVE(CM_CLKSTCTRL_DSP);
+        OMAP24XX_SAVE(CM_CLKSTCTRL_MDM);
+
+        /* Save power state registers */
+        OMAP24XX_SAVE(PM_PWSTCTRL_MPU);
+        OMAP24XX_SAVE(PM_PWSTCTRL_CORE);
+        OMAP24XX_SAVE(PM_PWSTCTRL_GFX);
+        OMAP24XX_SAVE(PM_PWSTCTRL_DSP);
+        OMAP24XX_SAVE(PM_PWSTCTRL_MDM);
+
+        /* Save autoidle registers */
+        OMAP24XX_SAVE(CM_AUTOIDLE1_CORE);
+        OMAP24XX_SAVE(CM_AUTOIDLE2_CORE);
+        OMAP24XX_SAVE(CM_AUTOIDLE3_CORE);
+        OMAP24XX_SAVE(CM_AUTOIDLE4_CORE);
+        OMAP24XX_SAVE(CM_AUTOIDLE_WKUP);
+        OMAP24XX_SAVE(CM_AUTOIDLE_PLL);
+        OMAP24XX_SAVE(CM_AUTOIDLE_DSP);
+        OMAP24XX_SAVE(CM_AUTOIDLE_MDM);
+
+        /* Save idle state registers */
+        OMAP24XX_SAVE(CM_IDLEST1_CORE);
+        OMAP24XX_SAVE(CM_IDLEST2_CORE);
+        OMAP24XX_SAVE(CM_IDLEST3_CORE);
+        OMAP24XX_SAVE(CM_IDLEST4_CORE);
+        OMAP24XX_SAVE(CM_IDLEST_GFX);
+        OMAP24XX_SAVE(CM_IDLEST_WKUP);
+        OMAP24XX_SAVE(CM_IDLEST_CKGEN);
+        OMAP24XX_SAVE(CM_IDLEST_DSP);
+        OMAP24XX_SAVE(CM_IDLEST_MDM);
+
+        /* Save clock registers */
+        OMAP24XX_SAVE(CM_FCLKEN1_CORE);
+        OMAP24XX_SAVE(CM_FCLKEN2_CORE);
+        OMAP24XX_SAVE(CM_ICLKEN1_CORE);
+        OMAP24XX_SAVE(CM_ICLKEN2_CORE);
+        OMAP24XX_SAVE(CM_ICLKEN3_CORE);
+        OMAP24XX_SAVE(CM_ICLKEN4_CORE);
+}
+
+static inline void omap2_pm_restore_registers(void)
+{
+        /* Restore clock state registers */
+        OMAP24XX_RESTORE(CM_CLKSTCTRL_MPU);
+        OMAP24XX_RESTORE(CM_CLKSTCTRL_CORE);
+        OMAP24XX_RESTORE(CM_CLKSTCTRL_GFX);
+        OMAP24XX_RESTORE(CM_CLKSTCTRL_DSP);
+        OMAP24XX_RESTORE(CM_CLKSTCTRL_MDM);
+
+        /* Restore power state registers */
+        OMAP24XX_RESTORE(PM_PWSTCTRL_MPU);
+        OMAP24XX_RESTORE(PM_PWSTCTRL_CORE);
+        OMAP24XX_RESTORE(PM_PWSTCTRL_GFX);
+        OMAP24XX_RESTORE(PM_PWSTCTRL_DSP);
+        OMAP24XX_RESTORE(PM_PWSTCTRL_MDM);
+
+        /* Restore idle state registers */
+        OMAP24XX_RESTORE(CM_IDLEST1_CORE);
+        OMAP24XX_RESTORE(CM_IDLEST2_CORE);
+        OMAP24XX_RESTORE(CM_IDLEST3_CORE);
+        OMAP24XX_RESTORE(CM_IDLEST4_CORE);
+        OMAP24XX_RESTORE(CM_IDLEST_GFX);
+        OMAP24XX_RESTORE(CM_IDLEST_WKUP);
+        OMAP24XX_RESTORE(CM_IDLEST_CKGEN);
+        OMAP24XX_RESTORE(CM_IDLEST_DSP);
+        OMAP24XX_RESTORE(CM_IDLEST_MDM);
+
+        /* Restore autoidle registers */
+        OMAP24XX_RESTORE(CM_AUTOIDLE1_CORE);
+        OMAP24XX_RESTORE(CM_AUTOIDLE2_CORE);
+        OMAP24XX_RESTORE(CM_AUTOIDLE3_CORE);
+        OMAP24XX_RESTORE(CM_AUTOIDLE4_CORE);
+        OMAP24XX_RESTORE(CM_AUTOIDLE_WKUP);
+        OMAP24XX_RESTORE(CM_AUTOIDLE_PLL);
+        OMAP24XX_RESTORE(CM_AUTOIDLE_DSP);
+        OMAP24XX_RESTORE(CM_AUTOIDLE_MDM);
+
+        /* Restore clock registers */
+        OMAP24XX_RESTORE(CM_FCLKEN1_CORE);
+        OMAP24XX_RESTORE(CM_FCLKEN2_CORE);
+        OMAP24XX_RESTORE(CM_ICLKEN1_CORE);
+        OMAP24XX_RESTORE(CM_ICLKEN2_CORE);
+        OMAP24XX_RESTORE(CM_ICLKEN3_CORE);
+        OMAP24XX_RESTORE(CM_ICLKEN4_CORE);
+
+        /* REVISIT: Clear interrupts here */
+
+        /* Restore interrupt registers */
+        OMAP24XX_RESTORE(INTC_MIR0);
+        OMAP24XX_RESTORE(INTC_MIR1);
+        OMAP24XX_RESTORE(INTC_MIR2);
+}
+
+static int omap2_pm_suspend(void)
+{
+        int processor_type = 0;
+
+        /* REVISIT: 0x21 or 0x26? */
+        if (cpu_is_omap2420())
+                processor_type = 0x21;
+
+        if (!processor_type)
+                return -ENOTSUPP;
+
+        local_irq_disable();
+        local_fiq_disable();
+
+        omap2_pm_save_registers();
+
+        /* Disable interrupts except for the wake events */
+        INTC_MIR_SET0 = 0xffffffff & ~INT0_WAKE_MASK;
+        INTC_MIR_SET1 = 0xffffffff & ~INT1_WAKE_MASK;
+        INTC_MIR_SET2 = 0xffffffff & ~INT2_WAKE_MASK;
+
+        pmdomain_set_autoidle();
+
+        /* Clear old wake-up events */
+        PM_WKST1_CORE = 0;
+        PM_WKST2_CORE = 0;
+        PM_WKST_WKUP = 0;
+
+        /* Enable wake-up events */
+        PM_WKEN1_CORE = (1 << 22) | (1 << 21);  /* UART1 & 2 */
+        PM_WKEN2_CORE = (1 << 2);               /* UART3 */
+        PM_WKEN_WKUP = (1 << 2) | (1 << 0);     /* GPIO & GPT1 */
+
+        /* Disable clocks except for CM_ICLKEN2_CORE. It gets disabled
+         * in the SRAM suspend code */
+        CM_FCLKEN1_CORE = 0;
+        CM_FCLKEN2_CORE = 0;
+        CM_ICLKEN1_CORE = 0;
+        CM_ICLKEN3_CORE = 0;
+        CM_ICLKEN4_CORE = 0;
+
+        omap2_pm_debug("Status before suspend");
+
+        /* Must wait for serial buffers to clear */
+        mdelay(200);
+
+        /* Jump to SRAM suspend code
+         * REVISIT: When is this SDRC_DLLB_CTRL?
+         */
+        omap2_sram_suspend(SDRC_DLLA_CTRL, processor_type);
+
+        /* Back from sleep */
+        omap2_pm_restore_registers();
+
+        local_fiq_enable();
+        local_irq_enable();
+
+        return 0;
+}
+
 static int omap2_pm_enter(suspend_state_t state)
 {
+        int ret = 0;
+
         switch (state)
         {
         case PM_SUSPEND_STANDBY:
         case PM_SUSPEND_MEM:
-                /* FIXME: Add suspend */
+                ret = omap2_pm_suspend();
                 break;
-
         case PM_SUSPEND_DISK:
-                return -ENOTSUPP;
-
+                ret = -ENOTSUPP;
+                break;
         default:
-                return -EINVAL;
+                ret = -EINVAL;
         }
 
-        return 0;
+        return ret;
 }
 
 static int omap2_pm_finish(suspend_state_t state)
@@ -143,6 +400,8 @@ int __init omap2_pm_init(void)
         pm_set_ops(&omap_pm_ops);
         pm_idle = omap2_pm_idle;
 
+        pmdomain_init();
+
         return 0;
 }