4 files changed, 43 insertions, 13 deletions

diff --git a/arch/arm/mach-keystone/Makefile b/arch/arm/mach-keystone/Makefile
index 3f6b8ab82235..ddc52b05dc84 100644
--- a/arch/arm/mach-keystone/Makefile
+++ b/arch/arm/mach-keystone/Makefile
@@ -1,2 +1,6 @@
-obj-y                                   := keystone.o
+obj-y                                   := keystone.o smc.o
+
+plus_sec := $(call as-instr,.arch_extension sec,+sec)
+AFLAGS_smc.o                            :=-Wa,-march=armv7-a$(plus_sec)
+
 obj-$(CONFIG_SMP)                       += platsmp.o
diff --git a/arch/arm/mach-keystone/keystone.h b/arch/arm/mach-keystone/keystone.h
index 43a1b4789a6e..60bef9dedb12 100644
--- a/arch/arm/mach-keystone/keystone.h
+++ b/arch/arm/mach-keystone/keystone.h
@@ -11,7 +11,13 @@
 #ifndef __KEYSTONE_H__
 #define __KEYSTONE_H__
 
+#define KEYSTONE_MON_CPU_UP_IDX         0x00
+
+#ifndef __ASSEMBLER__
+
 extern struct smp_operations keystone_smp_ops;
 extern void secondary_startup(void);
+extern u32 keystone_cpu_smc(u32 command, u32 cpu, u32 addr);
 
+#endif /* __ASSEMBLER__ */
 #endif /* __KEYSTONE_H__ */
diff --git a/arch/arm/mach-keystone/platsmp.c b/arch/arm/mach-keystone/platsmp.c
index 630ab3bd5f78..1d4181e1daf2 100644
--- a/arch/arm/mach-keystone/platsmp.c
+++ b/arch/arm/mach-keystone/platsmp.c
@@ -30,18 +30,9 @@ static int __cpuinit keystone_smp_boot_secondary(unsigned int cpu,
         pr_debug("keystone-smp: booting cpu %d, vector %08lx\n",
                  cpu, start);
 
-        asm volatile (
-                "mov    r0, #0\n"       /* power on cmd */
-                "mov    r1, %1\n"       /* cpu          */
-                "mov    r2, %2\n"       /* start        */
-                ".inst  0xe1600070\n"   /* smc #0       */
-                "mov    %0, r0\n"
-                : "=r" (error)
-                : "r"(cpu), "r"(start)
-                : "cc", "r0", "r1", "r2", "memory"
-        );
-
-        pr_debug("keystone-smp: monitor returned %d\n", error);
+        error = keystone_cpu_smc(KEYSTONE_MON_CPU_UP_IDX, cpu, start);
+        if (error)
+                pr_err("CPU %d bringup failed with %d\n", cpu, error);
 
         return error;
 }
diff --git a/arch/arm/mach-keystone/smc.S b/arch/arm/mach-keystone/smc.S
new file mode 100644
index 000000000000..9b9e4f7b241e
--- /dev/null
+++ b/arch/arm/mach-keystone/smc.S
@@ -0,0 +1,29 @@
+/*
+ * Keystone Secure APIs
+ *
+ * Copyright (C) 2013 Texas Instruments, Inc.
+ *      Santosh Shilimkar <santosh.shilimkar@ti.com>
+ *
+ * This program is free software,you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/linkage.h>
+
+/**
+ * u32 keystone_cpu_smc(u32 command, u32 cpu, u32 addr)
+ *
+ * Low level CPU monitor API
+ * @command:    Monitor command.
+ * @cpu:        CPU Number
+ * @addr:       Kernel jump address for boot CPU
+ *
+ * Return: Non zero value on failure
+ */
+ENTRY(keystone_cpu_smc)
+        stmfd   sp!, {r4-r12, lr}
+        smc     #0
+        dsb
+        ldmfd   sp!, {r4-r12, pc}
+ENDPROC(keystone_cpu_smc)