diff options
-rw-r--r-- | arch/arm/mach-exynos/firmware.c | 24 | ||||
-rw-r--r-- | arch/arm/mach-exynos/pm.c | 17 |
2 files changed, 29 insertions, 12 deletions
diff --git a/arch/arm/mach-exynos/firmware.c b/arch/arm/mach-exynos/firmware.c index e57b7c3..2c5bc6b 100644 --- a/arch/arm/mach-exynos/firmware.c +++ b/arch/arm/mach-exynos/firmware.c @@ -25,13 +25,28 @@ #include "smc.h" #define EXYNOS_SLEEP_MAGIC 0x00000bad +#define EXYNOS_AFTR_MAGIC 0xfcba0d10 #define EXYNOS_BOOT_ADDR 0x8 #define EXYNOS_BOOT_FLAG 0xc +static void exynos_save_cp15(void) +{ + /* Save Power control and Diagnostic registers */ + asm ("mrc p15, 0, %0, c15, c0, 0\n" + "mrc p15, 0, %1, c15, c0, 1\n" + : "=r" (cp15_save_power), "=r" (cp15_save_diag) + : : "cc"); +} + static int exynos_do_idle(unsigned long mode) { switch (mode) { case FW_DO_IDLE_AFTR: + if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) + exynos_save_cp15(); + __raw_writel(virt_to_phys(exynos_cpu_resume_ns), + sysram_ns_base_addr + 0x24); + __raw_writel(EXYNOS_AFTR_MAGIC, sysram_ns_base_addr + 0x20); exynos_smc(SMC_CMD_CPU0AFTR, 0, 0, 0); break; case FW_DO_IDLE_SLEEP: @@ -96,13 +111,8 @@ static int exynos_cpu_suspend(unsigned long arg) static int exynos_suspend(void) { - if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) { - /* Save Power control and Diagnostic registers */ - asm ("mrc p15, 0, %0, c15, c0, 0\n" - "mrc p15, 0, %1, c15, c0, 1\n" - : "=r" (cp15_save_power), "=r" (cp15_save_diag) - : : "cc"); - } + if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) + exynos_save_cp15(); writel(EXYNOS_SLEEP_MAGIC, sysram_ns_base_addr + EXYNOS_BOOT_FLAG); writel(virt_to_phys(exynos_cpu_resume_ns), diff --git a/arch/arm/mach-exynos/pm.c b/arch/arm/mach-exynos/pm.c index 3407fc1..6796fce 100644 --- a/arch/arm/mach-exynos/pm.c +++ b/arch/arm/mach-exynos/pm.c @@ -236,11 +236,19 @@ static void exynos_cpu_set_boot_vector(long flags) static int exynos_aftr_finisher(unsigned long flags) { + int ret; + 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(); + + ret = call_firmware_op(do_idle, FW_DO_IDLE_AFTR); + if (ret == -ENOSYS) { + if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) + exynos_cpu_save_register(); + exynos_cpu_set_boot_vector(S5P_CHECK_AFTR); + cpu_do_idle(); + } return 1; } @@ -250,14 +258,13 @@ 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(); + if (call_firmware_op(resume) == -ENOSYS) + exynos_cpu_restore_register(); } exynos_pm_central_resume(); |