1 files changed, 29 insertions, 37 deletions

diff --git a/arch/arm/mach-pxa/pxa25x.c b/arch/arm/mach-pxa/pxa25x.c
index 305452b56e91..25d17a1dab78 100644
--- a/arch/arm/mach-pxa/pxa25x.c
+++ b/arch/arm/mach-pxa/pxa25x.c
@@ -36,6 +36,12 @@
 #include "devices.h"
 #include "clock.h"
 
+int cpu_is_pxa26x(void)
+{
+        return cpu_is_pxa250() && ((BOOT_DEF & 0x8) == 0);
+}
+EXPORT_SYMBOL_GPL(cpu_is_pxa26x);
+
 /*
  * Various clock factors driven by the CCCR register.
  */
@@ -203,48 +209,21 @@ static struct clk pxa25x_clks[] = {
  * More ones like CP and general purpose register values are preserved
  * with the stack pointer in sleep.S.
  */
-enum {  SLEEP_SAVE_PGSR0, SLEEP_SAVE_PGSR1, SLEEP_SAVE_PGSR2,
-
-        SLEEP_SAVE_GAFR0_L, SLEEP_SAVE_GAFR0_U,
-        SLEEP_SAVE_GAFR1_L, SLEEP_SAVE_GAFR1_U,
-        SLEEP_SAVE_GAFR2_L, SLEEP_SAVE_GAFR2_U,
-
+enum {
         SLEEP_SAVE_PSTR,
-
         SLEEP_SAVE_CKEN,
-
         SLEEP_SAVE_COUNT
 };
 
 
 static void pxa25x_cpu_pm_save(unsigned long *sleep_save)
 {
-        SAVE(PGSR0); SAVE(PGSR1); SAVE(PGSR2);
-
-        SAVE(GAFR0_L); SAVE(GAFR0_U);
-        SAVE(GAFR1_L); SAVE(GAFR1_U);
-        SAVE(GAFR2_L); SAVE(GAFR2_U);
-
         SAVE(CKEN);
         SAVE(PSTR);
-
-        /* Clear GPIO transition detect bits */
-        GEDR0 = GEDR0; GEDR1 = GEDR1; GEDR2 = GEDR2;
 }
 
 static void pxa25x_cpu_pm_restore(unsigned long *sleep_save)
 {
-        /* ensure not to come back here if it wasn't intended */
-        PSPR = 0;
-
-        /* restore registers */
-        RESTORE(GAFR0_L); RESTORE(GAFR0_U);
-        RESTORE(GAFR1_L); RESTORE(GAFR1_U);
-        RESTORE(GAFR2_L); RESTORE(GAFR2_U);
-        RESTORE(PGSR0); RESTORE(PGSR1); RESTORE(PGSR2);
-
-        PSSR = PSSR_RDH | PSSR_PH;
-
         RESTORE(CKEN);
         RESTORE(PSTR);
 }
@@ -256,19 +235,32 @@ static void pxa25x_cpu_pm_enter(suspend_state_t state)
 
         switch (state) {
         case PM_SUSPEND_MEM:
-                /* set resume return address */
-                PSPR = virt_to_phys(pxa_cpu_resume);
                 pxa25x_cpu_suspend(PWRMODE_SLEEP);
                 break;
         }
 }
 
+static int pxa25x_cpu_pm_prepare(void)
+{
+        /* set resume return address */
+        PSPR = virt_to_phys(pxa_cpu_resume);
+        return 0;
+}
+
+static void pxa25x_cpu_pm_finish(void)
+{
+        /* ensure not to come back here if it wasn't intended */
+        PSPR = 0;
+}
+
 static struct pxa_cpu_pm_fns pxa25x_cpu_pm_fns = {
         .save_count     = SLEEP_SAVE_COUNT,
         .valid          = suspend_valid_only_mem,
         .save           = pxa25x_cpu_pm_save,
         .restore        = pxa25x_cpu_pm_restore,
         .enter          = pxa25x_cpu_pm_enter,
+        .prepare        = pxa25x_cpu_pm_prepare,
+        .finish         = pxa25x_cpu_pm_finish,
 };
 
 static void __init pxa25x_init_pm(void)
@@ -330,6 +322,8 @@ static struct sys_device pxa25x_sysdev[] = {
         {
                 .cls    = &pxa_irq_sysclass,
         }, {
+                .cls    = &pxa2xx_mfp_sysclass,
+        }, {
                 .cls    = &pxa_gpio_sysclass,
         },
 };
@@ -338,11 +332,7 @@ static int __init pxa25x_init(void)
 {
         int i, ret = 0;
 
-        /* Only add HWUART for PXA255/26x; PXA210/250/27x do not have it. */
-        if (cpu_is_pxa255())
-                clks_register(&pxa25x_hwuart_clk, 1);
-
-        if (cpu_is_pxa21x() || cpu_is_pxa25x()) {
+        if (cpu_is_pxa25x()) {
 
                 reset_status = RCSR;
 
@@ -365,9 +355,11 @@ static int __init pxa25x_init(void)
                         return ret;
         }
 
-        /* Only add HWUART for PXA255/26x; PXA210/250/27x do not have it. */
-        if (cpu_is_pxa255())
+        /* Only add HWUART for PXA255/26x; PXA210/250 do not have it. */
+        if (cpu_is_pxa255() || cpu_is_pxa26x()) {
+                clks_register(&pxa25x_hwuart_clk, 1);
                 ret = platform_device_register(&pxa_device_hwuart);
+        }
 
         return ret;
 }