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;
 }