1 files changed, 53 insertions, 12 deletions

diff --git a/arch/arm/mach-omap2/omap-secure.c b/arch/arm/mach-omap2/omap-secure.c
index f70d561f37f7..c2e1aecd07cc 100644
--- a/arch/arm/mach-omap2/omap-secure.c
+++ b/arch/arm/mach-omap2/omap-secure.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/arm-smccc.h>
+#include <linux/cpu_pm.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/io.h>
@@ -20,6 +21,7 @@
 
 #include "common.h"
 #include "omap-secure.h"
+#include "soc.h"
 
 static phys_addr_t omap_secure_memblock_base;
 
@@ -45,7 +47,7 @@ static void __init omap_optee_init_check(void)
 }
 
 /**
- * omap_sec_dispatcher: Routine to dispatch low power secure
+ * omap_secure_dispatcher - Routine to dispatch low power secure
  * service routines
  * @idx: The HAL API index
  * @flag: The flag indicating criticality of operation
@@ -57,8 +59,13 @@ static void __init omap_optee_init_check(void)
 u32 omap_secure_dispatcher(u32 idx, u32 flag, u32 nargs, u32 arg1, u32 arg2,
 							 u32 arg3, u32 arg4)
 {
+	static u32 buf[NR_CPUS][5];
+	u32 *param;
+	int cpu;
 	u32 ret;
-	u32 param[5];
+
+	cpu = get_cpu();
+	param = buf[cpu];
 
 	param[0] = nargs;
 	param[1] = arg1;
@@ -74,6 +81,8 @@ u32 omap_secure_dispatcher(u32 idx, u32 flag, u32 nargs, u32 arg1, u32 arg2,
 	outer_clean_range(__pa(param), __pa(param + 5));
 	ret = omap_smc2(idx, flag, __pa(param));
 
+	put_cpu();
+
 	return ret;
 }
 
@@ -109,16 +118,11 @@ int __init omap_secure_ram_reserve_memblock(void)
 	return 0;
 }
 
-phys_addr_t omap_secure_ram_mempool_base(void)
-{
-	return omap_secure_memblock_base;
-}
-
 #if defined(CONFIG_ARCH_OMAP3) && defined(CONFIG_PM)
-u32 omap3_save_secure_ram(void __iomem *addr, int size)
+u32 omap3_save_secure_ram(void *addr, int size)
 {
+	static u32 param[5];
 	u32 ret;
-	u32 param[5];
 
 	if (size != OMAP3_SAVE_SECURE_RAM_SZ)
 		return OMAP3_SAVE_SECURE_RAM_SZ;
@@ -148,11 +152,11 @@ u32 omap3_save_secure_ram(void __iomem *addr, int size)
  * NOTE: rx51_secure_dispatcher differs from omap_secure_dispatcher because
  *       it calling omap_smc3() instead omap_smc2() and param[0] is nargs+1
  */
-u32 rx51_secure_dispatcher(u32 idx, u32 process, u32 flag, u32 nargs,
+static u32 rx51_secure_dispatcher(u32 idx, u32 process, u32 flag, u32 nargs,
 			   u32 arg1, u32 arg2, u32 arg3, u32 arg4)
 {
+	static u32 param[5];
 	u32 ret;
-	u32 param[5];
 
 	param[0] = nargs+1; /* RX-51 needs number of arguments + 1 */
 	param[1] = arg1;
@@ -179,7 +183,7 @@ u32 rx51_secure_dispatcher(u32 idx, u32 process, u32 flag, u32 nargs,
 /**
  * rx51_secure_update_aux_cr: Routine to modify the contents of Auxiliary Control Register
  *  @set_bits: bits to set in ACR
- *  @clr_bits: bits to clear in ACR
+ *  @clear_bits: bits to clear in ACR
  *
  * Return the non-zero error value on failure.
 */
@@ -213,3 +217,40 @@ void __init omap_secure_init(void)
 {
 	omap_optee_init_check();
 }
+
+/*
+ * Dummy dispatcher call after core OSWR and MPU off. Updates the ROM return
+ * address after MMU has been re-enabled after CPU1 has been woken up again.
+ * Otherwise the ROM code will attempt to use the earlier physical return
+ * address that got set with MMU off when waking up CPU1. Only used on secure
+ * devices.
+ */
+static int cpu_notifier(struct notifier_block *nb, unsigned long cmd, void *v)
+{
+	switch (cmd) {
+	case CPU_CLUSTER_PM_EXIT:
+		omap_secure_dispatcher(OMAP4_PPA_SERVICE_0,
+				       FLAG_START_CRITICAL,
+				       0, 0, 0, 0, 0);
+		break;
+	default:
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block secure_notifier_block = {
+	.notifier_call = cpu_notifier,
+};
+
+static int __init secure_pm_init(void)
+{
+	if (omap_type() == OMAP2_DEVICE_TYPE_GP || !soc_is_omap44xx())
+		return 0;
+
+	cpu_pm_register_notifier(&secure_notifier_block);
+
+	return 0;
+}
+omap_arch_initcall(secure_pm_init);