ARM: EXYNOS: Fix suspend/resume sequences

[pandora-kernel.git] / arch / arm / mach-exynos / pm.c

diff --git a/arch/arm/mach-exynos/pm.c b/arch/arm/mach-exynos/pm.c
index 18646b7..abefacb 100644 (file)
--- a/arch/arm/mach-exynos/pm.c
+++ b/arch/arm/mach-exynos/pm.c
@@ -114,26 +114,6 @@ static int exynos_irq_set_wake(struct irq_data *data, unsigned int state)
 #define S5P_CHECK_AFTR  0xFCBA0D10
 #define S5P_CHECK_SLEEP 0x00000BAD
 
-/* Ext-GIC nIRQ/nFIQ is the only wakeup source in AFTR */
-static void exynos_set_wakeupmask(long mask)
-{
-       pmu_raw_writel(mask, S5P_WAKEUP_MASK);
-}
-
-static void exynos_cpu_set_boot_vector(long flags)
-{
-       __raw_writel(virt_to_phys(exynos_cpu_resume), EXYNOS_BOOT_VECTOR_ADDR);
-       __raw_writel(flags, EXYNOS_BOOT_VECTOR_FLAG);
-}
-
-void exynos_enter_aftr(void)
-{
-       exynos_set_wakeupmask(0x0000ff3e);
-       exynos_cpu_set_boot_vector(S5P_CHECK_AFTR);
-       /* Set value of power down register for aftr mode */
-       exynos_sys_powerdown_conf(SYS_AFTR);
-}
-
 /* For Cortex-A9 Diagnostic and Power control register */
 static unsigned int save_arm_register[2];
 
@@ -173,6 +153,82 @@ static void exynos_cpu_restore_register(void)
                      : "cc");
 }
 
+static void exynos_pm_central_suspend(void)
+{
+       unsigned long tmp;
+
+       /* Setting Central Sequence Register for power down mode */
+       tmp = pmu_raw_readl(S5P_CENTRAL_SEQ_CONFIGURATION);
+       tmp &= ~S5P_CENTRAL_LOWPWR_CFG;
+       pmu_raw_writel(tmp, S5P_CENTRAL_SEQ_CONFIGURATION);
+}
+
+static int exynos_pm_central_resume(void)
+{
+       unsigned long tmp;
+
+       /*
+        * If PMU failed while entering sleep mode, WFI will be
+        * ignored by PMU and then exiting cpu_do_idle().
+        * S5P_CENTRAL_LOWPWR_CFG bit will not be set automatically
+        * in this situation.
+        */
+       tmp = pmu_raw_readl(S5P_CENTRAL_SEQ_CONFIGURATION);
+       if (!(tmp & S5P_CENTRAL_LOWPWR_CFG)) {
+               tmp |= S5P_CENTRAL_LOWPWR_CFG;
+               pmu_raw_writel(tmp, S5P_CENTRAL_SEQ_CONFIGURATION);
+               /* clear the wakeup state register */
+               pmu_raw_writel(0x0, S5P_WAKEUP_STAT);
+               /* No need to perform below restore code */
+               return -1;
+       }
+
+       return 0;
+}
+
+/* Ext-GIC nIRQ/nFIQ is the only wakeup source in AFTR */
+static void exynos_set_wakeupmask(long mask)
+{
+       pmu_raw_writel(mask, S5P_WAKEUP_MASK);
+}
+
+static void exynos_cpu_set_boot_vector(long flags)
+{
+       __raw_writel(virt_to_phys(exynos_cpu_resume), EXYNOS_BOOT_VECTOR_ADDR);
+       __raw_writel(flags, EXYNOS_BOOT_VECTOR_FLAG);
+}
+
+static int exynos_aftr_finisher(unsigned long flags)
+{
+       exynos_set_wakeupmask(0x0000ff3e);
+       exynos_cpu_set_boot_vector(S5P_CHECK_AFTR);
+       /* Set value of power down register for aftr mode */
+       exynos_sys_powerdown_conf(SYS_AFTR);
+       cpu_do_idle();
+
+       return 1;
+}
+
+void exynos_enter_aftr(void)
+{
+       cpu_pm_enter();
+
+       exynos_pm_central_suspend();
+       if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9)
+               exynos_cpu_save_register();
+
+       cpu_suspend(0, exynos_aftr_finisher);
+
+       if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) {
+               scu_enable(S5P_VA_SCU);
+               exynos_cpu_restore_register();
+       }
+
+       exynos_pm_central_resume();
+
+       cpu_pm_exit();
+}
+
 static int exynos_cpu_suspend(unsigned long arg)
 {
 #ifdef CONFIG_CACHE_L2X0
@@ -217,16 +273,6 @@ static void exynos_pm_prepare(void)
        pmu_raw_writel(virt_to_phys(exynos_cpu_resume), S5P_INFORM0);
 }
 
-static void exynos_pm_central_suspend(void)
-{
-       unsigned long tmp;
-
-       /* Setting Central Sequence Register for power down mode */
-       tmp = pmu_raw_readl(S5P_CENTRAL_SEQ_CONFIGURATION);
-       tmp &= ~S5P_CENTRAL_LOWPWR_CFG;
-       pmu_raw_writel(tmp, S5P_CENTRAL_SEQ_CONFIGURATION);
-}
-
 static int exynos_pm_suspend(void)
 {
        unsigned long tmp;
@@ -244,29 +290,6 @@ static int exynos_pm_suspend(void)
        return 0;
 }
 
-static int exynos_pm_central_resume(void)
-{
-       unsigned long tmp;
-
-       /*
-        * If PMU failed while entering sleep mode, WFI will be
-        * ignored by PMU and then exiting cpu_do_idle().
-        * S5P_CENTRAL_LOWPWR_CFG bit will not be set automatically
-        * in this situation.
-        */
-       tmp = pmu_raw_readl(S5P_CENTRAL_SEQ_CONFIGURATION);
-       if (!(tmp & S5P_CENTRAL_LOWPWR_CFG)) {
-               tmp |= S5P_CENTRAL_LOWPWR_CFG;
-               pmu_raw_writel(tmp, S5P_CENTRAL_SEQ_CONFIGURATION);
-               /* clear the wakeup state register */
-               pmu_raw_writel(0x0, S5P_WAKEUP_STAT);
-               /* No need to perform below restore code */
-               return -1;
-       }
-
-       return 0;
-}
-
 static void exynos_pm_resume(void)
 {
        if (exynos_pm_central_resume())
@@ -369,44 +392,10 @@ static const struct platform_suspend_ops exynos_suspend_ops = {
        .valid          = suspend_valid_only_mem,
 };
 
-static int exynos_cpu_pm_notifier(struct notifier_block *self,
-                                 unsigned long cmd, void *v)
-{
-       int cpu = smp_processor_id();
-
-       switch (cmd) {
-       case CPU_PM_ENTER:
-               if (cpu == 0) {
-                       exynos_pm_central_suspend();
-                       if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9)
-                               exynos_cpu_save_register();
-               }
-               break;
-
-       case CPU_PM_EXIT:
-               if (cpu == 0) {
-                       if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) {
-                               scu_enable(S5P_VA_SCU);
-                               exynos_cpu_restore_register();
-                       }
-                       exynos_pm_central_resume();
-               }
-               break;
-       }
-
-       return NOTIFY_OK;
-}
-
-static struct notifier_block exynos_cpu_pm_notifier_block = {
-       .notifier_call = exynos_cpu_pm_notifier,
-};
-
 void __init exynos_pm_init(void)
 {
        u32 tmp;
 
-       cpu_pm_register_notifier(&exynos_cpu_pm_notifier_block);
-
        /* Platform-specific GIC callback */
        gic_arch_extn.irq_set_wake = exynos_irq_set_wake;