From fd3a1558a2ae3f3636c25cbb22ae35138594e54e Mon Sep 17 00:00:00 2001
From: Lukasz Majewski <l.majewski@samsung.com>
Date: Thu, 10 Apr 2014 12:38:42 +0200
Subject: [PATCH] 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>
---
 arch/arm/include/asm/firmware.h |  2 +-
 arch/arm/mach-exynos/cpuidle.c  | 18 +++++++++++++-----
 arch/arm/mach-exynos/firmware.c | 17 +++++++++++++++--
 arch/arm/mach-exynos/smc.h      |  5 +++++
 4 files changed, 34 insertions(+), 8 deletions(-)

diff --git a/arch/arm/include/asm/firmware.h b/arch/arm/include/asm/firmware.h
index 1ee7523..4e2879f 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 17a18ff..df195ed 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 e224ae1..4eb3ee7 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 13a1dc8..cb458ac 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
-- 
2.7.4