1 files changed, 216 insertions, 9 deletions

diff --git a/arch/blackfin/mach-common/pm.c b/arch/blackfin/mach-common/pm.c
index 0be805ca423f..4fe6a2366b13 100644
--- a/arch/blackfin/mach-common/pm.c
+++ b/arch/blackfin/mach-common/pm.c
@@ -38,8 +38,9 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 
-#include <asm/dpmc.h>
 #include <asm/gpio.h>
+#include <asm/dma.h>
+#include <asm/dpmc.h>
 
 #ifdef CONFIG_PM_WAKEUP_GPIO_POLAR_H
 #define WAKEUP_TYPE     PM_WAKE_HIGH
@@ -61,16 +62,17 @@
 #define WAKEUP_TYPE     PM_WAKE_BOTH_EDGES
 #endif
 
+
 void bfin_pm_suspend_standby_enter(void)
 {
+        unsigned long flags;
+
 #ifdef CONFIG_PM_WAKEUP_BY_GPIO
         gpio_pm_wakeup_request(CONFIG_PM_WAKEUP_GPIO_NUMBER, WAKEUP_TYPE);
 #endif
 
-        u32 flags;
-
         local_irq_save(flags);
-        bfin_pm_setup();
+        bfin_pm_standby_setup();
 
 #ifdef CONFIG_PM_BFIN_SLEEP_DEEPER
         sleep_deeper(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
@@ -78,7 +80,7 @@ void bfin_pm_suspend_standby_enter(void)
         sleep_mode(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
 #endif
 
-        bfin_pm_restore();
+        bfin_pm_standby_restore();
 
 #if defined(CONFIG_BF54x) || defined(CONFIG_BF52x)  || defined(CONFIG_BF561)
         bfin_write_SIC_IWR0(IWR_ENABLE_ALL);
@@ -93,6 +95,195 @@ void bfin_pm_suspend_standby_enter(void)
         local_irq_restore(flags);
 }
 
+int bf53x_suspend_l1_mem(unsigned char *memptr)
+{
+        dma_memcpy(memptr, (const void *) L1_CODE_START, L1_CODE_LENGTH);
+        dma_memcpy(memptr + L1_CODE_LENGTH, (const void *) L1_DATA_A_START,
+                        L1_DATA_A_LENGTH);
+        dma_memcpy(memptr + L1_CODE_LENGTH + L1_DATA_A_LENGTH,
+                        (const void *) L1_DATA_B_START, L1_DATA_B_LENGTH);
+        memcpy(memptr + L1_CODE_LENGTH + L1_DATA_A_LENGTH +
+                        L1_DATA_B_LENGTH, (const void *) L1_SCRATCH_START,
+                        L1_SCRATCH_LENGTH);
+
+        return 0;
+}
+
+int bf53x_resume_l1_mem(unsigned char *memptr)
+{
+        dma_memcpy((void *) L1_CODE_START, memptr, L1_CODE_LENGTH);
+        dma_memcpy((void *) L1_DATA_A_START, memptr + L1_CODE_LENGTH,
+                        L1_DATA_A_LENGTH);
+        dma_memcpy((void *) L1_DATA_B_START, memptr + L1_CODE_LENGTH +
+                        L1_DATA_A_LENGTH, L1_DATA_B_LENGTH);
+        memcpy((void *) L1_SCRATCH_START, memptr + L1_CODE_LENGTH +
+                        L1_DATA_A_LENGTH + L1_DATA_B_LENGTH, L1_SCRATCH_LENGTH);
+
+        return 0;
+}
+
+#ifdef CONFIG_BFIN_WB
+static void flushinv_all_dcache(void)
+{
+        u32 way, bank, subbank, set;
+        u32 status, addr;
+        u32 dmem_ctl = bfin_read_DMEM_CONTROL();
+
+        for (bank = 0; bank < 2; ++bank) {
+                if (!(dmem_ctl & (1 << (DMC1_P - bank))))
+                        continue;
+
+                for (way = 0; way < 2; ++way)
+                        for (subbank = 0; subbank < 4; ++subbank)
+                                for (set = 0; set < 64; ++set) {
+
+                                        bfin_write_DTEST_COMMAND(
+                                                way << 26 |
+                                                bank << 23 |
+                                                subbank << 16 |
+                                                set << 5
+                                        );
+                                        CSYNC();
+                                        status = bfin_read_DTEST_DATA0();
+
+                                        /* only worry about valid/dirty entries */
+                                        if ((status & 0x3) != 0x3)
+                                                continue;
+
+                                        /* construct the address using the tag */
+                                        addr = (status & 0xFFFFC800) | (subbank << 12) | (set << 5);
+
+                                        /* flush it */
+                                        __asm__ __volatile__("FLUSHINV[%0];" : : "a"(addr));
+                                }
+        }
+}
+#endif
+
+static inline void dcache_disable(void)
+{
+#ifdef CONFIG_BFIN_DCACHE
+        unsigned long ctrl;
+
+#ifdef CONFIG_BFIN_WB
+        flushinv_all_dcache();
+#endif
+        SSYNC();
+        ctrl = bfin_read_DMEM_CONTROL();
+        ctrl &= ~ENDCPLB;
+        bfin_write_DMEM_CONTROL(ctrl);
+        SSYNC();
+#endif
+}
+
+static inline void dcache_enable(void)
+{
+#ifdef CONFIG_BFIN_DCACHE
+        unsigned long ctrl;
+        SSYNC();
+        ctrl = bfin_read_DMEM_CONTROL();
+        ctrl |= ENDCPLB;
+        bfin_write_DMEM_CONTROL(ctrl);
+        SSYNC();
+#endif
+}
+
+static inline void icache_disable(void)
+{
+#ifdef CONFIG_BFIN_ICACHE
+        unsigned long ctrl;
+        SSYNC();
+        ctrl = bfin_read_IMEM_CONTROL();
+        ctrl &= ~ENICPLB;
+        bfin_write_IMEM_CONTROL(ctrl);
+        SSYNC();
+#endif
+}
+
+static inline void icache_enable(void)
+{
+#ifdef CONFIG_BFIN_ICACHE
+        unsigned long ctrl;
+        SSYNC();
+        ctrl = bfin_read_IMEM_CONTROL();
+        ctrl |= ENICPLB;
+        bfin_write_IMEM_CONTROL(ctrl);
+        SSYNC();
+#endif
+}
+
+int bfin_pm_suspend_mem_enter(void)
+{
+        unsigned long flags;
+        int wakeup, ret;
+
+        unsigned char *memptr = kmalloc(L1_CODE_LENGTH + L1_DATA_A_LENGTH
+                                         + L1_DATA_B_LENGTH + L1_SCRATCH_LENGTH,
+                                          GFP_KERNEL);
+
+        if (memptr == NULL) {
+                panic("bf53x_suspend_l1_mem malloc failed");
+                return -ENOMEM;
+        }
+
+        wakeup = bfin_read_VR_CTL() & ~FREQ;
+        wakeup |= SCKELOW;
+
+        /* FIXME: merge this somehow with set_irq_wake */
+#ifdef CONFIG_PM_BFIN_WAKE_RTC
+        wakeup |= WAKE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_PH6
+        wakeup |= PHYWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_CAN
+        wakeup |= CANWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_GP
+        wakeup |= GPWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_USB
+        wakeup |= USBWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_KEYPAD
+        wakeup |= KPADWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_ROTARY
+        wakeup |= ROTWE;
+#endif
+
+        local_irq_save(flags);
+
+        ret = blackfin_dma_suspend();
+
+        if (ret) {
+                local_irq_restore(flags);
+                kfree(memptr);
+                return ret;
+        }
+
+        bfin_gpio_pm_hibernate_suspend();
+
+        dcache_disable();
+        icache_disable();
+        bf53x_suspend_l1_mem(memptr);
+
+        do_hibernate(wakeup);   /* Goodbye */
+
+        bf53x_resume_l1_mem(memptr);
+
+        icache_enable();
+        dcache_enable();
+
+        bfin_gpio_pm_hibernate_restore();
+        blackfin_dma_resume();
+
+        local_irq_restore(flags);
+        kfree(memptr);
+
+        return 0;
+}
+
 /*
  *      bfin_pm_valid - Tell the PM core that we only support the standby sleep
  *                      state
@@ -101,7 +292,24 @@ void bfin_pm_suspend_standby_enter(void)
  */
 static int bfin_pm_valid(suspend_state_t state)
 {
-        return (state == PM_SUSPEND_STANDBY);
+        return (state == PM_SUSPEND_STANDBY
+#ifndef BF533_FAMILY
+        /*
+         * On BF533/2/1:
+         * If we enter Hibernate the SCKE Pin is driven Low,
+         * so that the SDRAM enters Self Refresh Mode.
+         * However when the reset sequence that follows hibernate
+         * state is executed, SCKE is driven High, taking the
+         * SDRAM out of Self Refresh.
+         *
+         * If you reconfigure and access the SDRAM "very quickly",
+         * you are likely to avoid errors, otherwise the SDRAM
+         * start losing its contents.
+         * An external HW workaround is possible using logic gates.
+         */
+        || state == PM_SUSPEND_MEM
+#endif
+        );
 }
 
 /*
@@ -115,10 +323,9 @@ static int bfin_pm_enter(suspend_state_t state)
         case PM_SUSPEND_STANDBY:
                 bfin_pm_suspend_standby_enter();
                 break;
-
         case PM_SUSPEND_MEM:
-                return -ENOTSUPP;
-
+                bfin_pm_suspend_mem_enter();
+                break;
         default:
                 return -EINVAL;
         }