+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();
+}
+