cpuidle:exynos:AFTR: Use secure monitor calls to enable entering AFTR

Secure monitor calls are necessary to enter the AFTR low power mode on the
trats2 device.

Without them it is not possible to access certain CP15 registers (e.g.
Power Control Register).

Change-Id: I8f91acb80acdbd43c86a679d69037be041d76309
Signed-off-by: Lukasz Majewski <l.majewski@samsung.com>

4 files changed, 34 insertions, 8 deletions

diff --git a/arch/arm/include/asm/firmware.h b/arch/arm/include/asm/firmware.h
index 1ee7523f6be..4e2879f6053 100644
--- a/arch/arm/include/asm/firmware.h
+++ b/arch/arm/include/asm/firmware.h
@@ -24,7 +24,7 @@ struct firmware_ops {
 	/*
 	 * Enters CPU idle mode
 	 */
-	int (*do_idle)(void);
+	int (*do_idle)(int);
 	/*
 	 * Sets boot address of specified physical CPU
 	 */
diff --git a/arch/arm/mach-exynos/cpuidle.c b/arch/arm/mach-exynos/cpuidle.c
index 17a18ff3d71..df195ed0891 100644
--- a/arch/arm/mach-exynos/cpuidle.c
+++ b/arch/arm/mach-exynos/cpuidle.c
@@ -21,12 +21,14 @@
 #include <asm/suspend.h>
 #include <asm/unified.h>
 #include <asm/cpuidle.h>
+#include <asm/firmware.h>
 #include <mach/regs-clock.h>
 #include <mach/regs-pmu.h>
 
 #include <plat/cpu.h>
 
 #include "common.h"
+#include "smc.h"
 
 #define REG_DIRECTGO_ADDR	(samsung_rev() == EXYNOS4210_REV_1_1 ? \
 			S5P_INFORM7 : (samsung_rev() == EXYNOS4210_REV_1_0 ? \
@@ -67,28 +69,34 @@ static void exynos4_set_wakeupmask(void)
 	__raw_writel(0x0000ff3e, S5P_WAKEUP_MASK);
 }
 
-static unsigned int g_pwr_ctrl, g_diag_reg;
+static unsigned int cp15_regs[2];
 
 static void save_cpu_arch_register(void)
 {
 	/*read power control register*/
-	asm("mrc p15, 0, %0, c15, c0, 0" : "=r"(g_pwr_ctrl) : : "cc");
+	asm("mrc p15, 0, %0, c15, c0, 0" : "=r"(cp15_regs[0]) : : "cc");
 	/*read diagnostic register*/
-	asm("mrc p15, 0, %0, c15, c0, 1" : "=r"(g_diag_reg) : : "cc");
+	asm("mrc p15, 0, %0, c15, c0, 1" : "=r"(cp15_regs[1]) : : "cc");
 	return;
 }
 
 static void restore_cpu_arch_register(void)
 {
+	if (call_firmware_op(c15resume, cp15_regs) == 0)
+		return;
+
 	/*write power control register*/
-	asm("mcr p15, 0, %0, c15, c0, 0" : : "r"(g_pwr_ctrl) : "cc");
+	asm("mcr p15, 0, %0, c15, c0, 0" : : "r"(cp15_regs[0]) : "cc");
 	/*write diagnostic register*/
-	asm("mcr p15, 0, %0, c15, c0, 1" : : "r"(g_diag_reg) : "cc");
+	asm("mcr p15, 0, %0, c15, c0, 1" : : "r"(cp15_regs[1]) : "cc");
 	return;
 }
 
 static int idle_finisher(unsigned long flags)
 {
+	if (call_firmware_op(do_idle, EXYNOS_DO_IDLE_AFTR) == 0)
+		return 1;
+
 	cpu_do_idle();
 	return 1;
 }
diff --git a/arch/arm/mach-exynos/firmware.c b/arch/arm/mach-exynos/firmware.c
index e224ae1bcd7..4eb3ee77a6a 100644
--- a/arch/arm/mach-exynos/firmware.c
+++ b/arch/arm/mach-exynos/firmware.c
@@ -19,13 +19,26 @@
 
 #include <mach/map.h>
 
+#include "common.h"
 #include "smc.h"
 
 #define EXYNOS_SLEEP_MAGIC	0x00000BAD
+#define EXYNOS_AFTR_MAGIC	0xFCBA0D10
 
-static int exynos_do_idle(void)
+static int exynos_do_idle(int mode)
 {
-	exynos_smc(SMC_CMD_SLEEP, 0, 0, 0);
+	switch (mode) {
+	case EXYNOS_DO_IDLE_AFTR:
+		__raw_writel(virt_to_phys(s3c_cpu_resume), S5P_VA_SYSRAM_NS +
+			     0x24);
+		__raw_writel(EXYNOS_AFTR_MAGIC, S5P_VA_SYSRAM_NS + 0x20);
+		exynos_smc(SMC_CMD_CPU0AFTR, 0, 0, 0);
+
+		break;
+	case EXYNOS_DO_IDLE_NORMAL:
+		exynos_smc(SMC_CMD_SLEEP, 0, 0, 0);
+	}
+
 	return 0;
 }
 
diff --git a/arch/arm/mach-exynos/smc.h b/arch/arm/mach-exynos/smc.h
index 13a1dc8ecbf..cb458ac59ad 100644
--- a/arch/arm/mach-exynos/smc.h
+++ b/arch/arm/mach-exynos/smc.h
@@ -28,4 +28,9 @@
 
 extern void exynos_smc(u32 cmd, u32 arg1, u32 arg2, u32 arg3);
 
+enum {
+	EXYNOS_DO_IDLE_NORMAL,
+	EXYNOS_DO_IDLE_AFTR,
+};
+
 #endif