ARM: EXYNOS: add secure firmware support to AFTR mode code

* Move cp15 registers saving to exynos_save_cp15() helper and add
  additional helper usage to do_idle firmware method.

* Use resume firmware method instead of exynos_cpu_restore_register()
  and skip exynos_cpu_save_register() on boards with secure firmware
  enabled.

* Use sysram_ns_base_addr + 0x24/0x20 addresses instead of the default
  ones used by exynos_cpu_set_boot_vector() on boards with secure
  firmware enabled.

* Use do_idle firmware method instead of cpu_do_idle() on boards with
  secure firmware enabled.

Signed-off-by: Bartlomiej Zolnierkiewicz <b.zolnierkie@samsung.com>
Acked-by: Kyungmin Park <kyungmin.park@samsung.com>
Signed-off-by: Kukjin Kim <kgene.kim@samsung.com>

2 files changed, 29 insertions, 12 deletions

diff --git a/arch/arm/mach-exynos/firmware.c b/arch/arm/mach-exynos/firmware.c
index e57b7c3039a2..2c5bc6bfcbdf 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 3407fc162449..6796fce923e2 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();