summaryrefslogtreecommitdiffstats
path: root/arch/arm/cpu
diff options
context:
space:
mode:
Diffstat (limited to 'arch/arm/cpu')
-rw-r--r--arch/arm/cpu/Makefile4
-rw-r--r--arch/arm/cpu/psci.c311
-rw-r--r--arch/arm/cpu/sm.c266
-rw-r--r--arch/arm/cpu/sm_as.S168
-rw-r--r--arch/arm/cpu/smccc-call.S64
5 files changed, 813 insertions, 0 deletions
diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index d8cb187..13b4f95 100644
--- a/arch/arm/cpu/Makefile
+++ b/arch/arm/cpu/Makefile
@@ -34,6 +34,10 @@ ifeq ($(CONFIG_MMU),)
obj-y += no-mmu.o
endif
+obj-$(CONFIG_ARM_PSCI) += psci.o
+obj-$(CONFIG_ARM_SECURE_MONITOR) += smccc-call.o
+obj-$(CONFIG_ARM_SECURE_MONITOR) += sm.o sm_as.o
+
obj-$(CONFIG_CPU_32v4T) += cache-armv4.o
pbl-$(CONFIG_CPU_32v4T) += cache-armv4.o
obj-$(CONFIG_CPU_32v5) += cache-armv5.o
diff --git a/arch/arm/cpu/psci.c b/arch/arm/cpu/psci.c
new file mode 100644
index 0000000..d650c23
--- /dev/null
+++ b/arch/arm/cpu/psci.c
@@ -0,0 +1,311 @@
+/*
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; version 2.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ */
+
+#define pr_fmt(fmt) "psci: " fmt
+
+#include <common.h>
+#include <asm/psci.h>
+#include <asm/arm-smccc.h>
+#include <asm/secure.h>
+#include <asm/system.h>
+#include <restart.h>
+#include <globalvar.h>
+#include <init.h>
+#include <magicvar.h>
+
+#ifdef CONFIG_ARM_PSCI_DEBUG
+
+/*
+ * PSCI debugging functions. Board code can specify a putc() function
+ * which is used for debugging output. Beware that this function is
+ * called while the kernel is running. This means the kernel could have
+ * turned off clocks, configured other baudrates and other stuff that
+ * might confuse the putc function. So it can well be that the debugging
+ * code itself is the problem when somethings not working. You have been
+ * warned.
+ */
+
+static void (*__putc)(void *ctx, int c);
+static void *putc_ctx;
+
+void psci_set_putc(void (*putcf)(void *ctx, int c), void *ctx)
+{
+ __putc = putcf;
+ putc_ctx = ctx;
+}
+
+void psci_putc(char c)
+{
+ if (__putc)
+ __putc(putc_ctx, c);
+}
+
+int psci_puts(const char *str)
+{
+ int n = 0;
+
+ while (*str) {
+ if (*str == '\n')
+ psci_putc('\r');
+
+ psci_putc(*str);
+ str++;
+ n++;
+ }
+
+ return n;
+}
+
+int psci_printf(const char *fmt, ...)
+{
+ va_list args;
+ uint i;
+ char printbuffer[128];
+
+ va_start(args, fmt);
+ i = vsprintf(printbuffer, fmt, args);
+ va_end(args);
+
+ psci_puts(printbuffer);
+
+ return i;
+}
+#endif
+
+static struct psci_ops *psci_ops;
+
+void psci_set_ops(struct psci_ops *ops)
+{
+ psci_ops = ops;
+}
+
+static unsigned long psci_version(void)
+{
+ psci_printf("%s\n", __func__);
+ return ARM_PSCI_VER_1_0;
+}
+
+static unsigned long psci_cpu_suspend(u32 power_state, unsigned long entry,
+ u32 context_id)
+{
+ psci_printf("%s\n", __func__);
+
+ if (psci_ops->cpu_off)
+ return psci_ops->cpu_suspend(power_state, entry, context_id);
+
+ return ARM_PSCI_RET_NOT_SUPPORTED;
+}
+
+static unsigned long psci_cpu_off(void)
+{
+ psci_printf("%s\n", __func__);
+
+ if (psci_ops->cpu_off)
+ return psci_ops->cpu_off();
+
+ return ARM_PSCI_RET_NOT_SUPPORTED;
+}
+
+static unsigned long cpu_entry[ARM_SECURE_MAX_CPU];
+static unsigned long context[ARM_SECURE_MAX_CPU];
+
+static unsigned long psci_cpu_on(u32 cpu_id, unsigned long entry, u32 context_id)
+{
+ psci_printf("%s: %d 0x%08lx\n", __func__, cpu_id, entry);
+
+ if (cpu_id >= ARM_SECURE_MAX_CPU)
+ return ARM_PSCI_RET_INVAL;
+
+ cpu_entry[cpu_id] = entry;
+ context[cpu_id] = context_id;
+ dsb();
+
+ if (psci_ops->cpu_on)
+ return psci_ops->cpu_on(cpu_id);
+
+ return ARM_PSCI_RET_NOT_SUPPORTED;
+}
+
+static unsigned long psci_system_off(void)
+{
+ psci_printf("%s\n", __func__);
+
+ if (psci_ops->system_reset)
+ psci_ops->system_reset();
+
+ while(1);
+
+ return 0;
+}
+
+static unsigned long psci_system_reset(void)
+{
+ psci_printf("%s\n", __func__);
+
+ if (psci_ops->system_reset)
+ psci_ops->system_reset();
+
+ restart_machine();
+}
+
+void psci_entry(u32 r0, u32 r1, u32 r2, u32 r3, u32 r4, u32 r5, u32 r6,
+ struct arm_smccc_res *res)
+{
+ int mmuon;
+ unsigned long ttb;
+
+ mmuon = get_cr() & CR_M;
+ asm volatile ("mrc p15, 0, %0, c2, c0, 0" : "=r"(ttb));
+
+ psci_printf("%s entry, function: 0x%08x\n", __func__, r0);
+
+ switch (r0) {
+ case ARM_PSCI_0_2_FN_PSCI_VERSION:
+ res->a0 = psci_version();
+ break;
+ case ARM_PSCI_0_2_FN_CPU_SUSPEND:
+ res->a0 = psci_cpu_suspend(r1, r2, r3);
+ break;
+ case ARM_PSCI_0_2_FN_CPU_OFF:
+ res->a0 = psci_cpu_off();
+ break;
+ case ARM_PSCI_0_2_FN_CPU_ON:
+ res->a0 = psci_cpu_on(r1, r2, r3);
+ break;
+ case ARM_PSCI_0_2_FN_SYSTEM_OFF:
+ psci_system_off();
+ break;
+ case ARM_PSCI_0_2_FN_SYSTEM_RESET:
+ psci_system_reset();
+ break;
+ default:
+ res->a0 = ARM_PSCI_RET_NOT_SUPPORTED;
+ break;
+ }
+}
+
+static int of_psci_fixup(struct device_node *root, void *unused)
+{
+ struct device_node *psci;
+ int ret;
+
+ if (bootm_arm_security_state() < ARM_STATE_NONSECURE)
+ return 0;
+
+ psci = of_create_node(root, "/psci");
+ if (!psci)
+ return -EINVAL;
+
+ ret = of_set_property(psci, "compatible", "arm,psci-1.0",
+ strlen("arm,psci-1.0") + 1, 1);
+ if (ret)
+ return ret;
+
+ ret = of_set_property(psci, "method", "smc",
+ strlen("smc") + 1, 1);
+ if (ret)
+ return ret;
+
+ return 0;
+}
+
+int psci_cpu_entry_c(void)
+{
+ void (*entry)(u32 context);
+ int cpu;
+ u32 context_id;
+
+ __armv7_secure_monitor_install();
+ cpu = psci_get_cpu_id();
+ entry = (void *)cpu_entry[cpu];
+ context_id = context[cpu];
+
+ if (bootm_arm_security_state() == ARM_STATE_HYP)
+ armv7_switch_to_hyp();
+
+ psci_printf("core #%d enter function 0x%p\n", cpu, entry);
+
+ entry(context_id);
+
+ while (1);
+}
+
+static int armv7_psci_init(void)
+{
+ return of_register_fixup(of_psci_fixup, NULL);
+}
+device_initcall(armv7_psci_init);
+
+#ifdef DEBUG
+
+#include <command.h>
+#include <getopt.h>
+#include "mmu.h"
+
+void second_entry(void)
+{
+ struct arm_smccc_res res;
+
+ psci_printf("2nd CPU online, now turn off again\n");
+
+ arm_smccc_smc(ARM_PSCI_0_2_FN_CPU_OFF,
+ 0, 0, 0, 0, 0, 0, 0, &res);
+
+ psci_printf("2nd CPU still alive?\n");
+
+ while (1);
+}
+
+static int do_smc(int argc, char *argv[])
+{
+ int opt;
+ struct arm_smccc_res res = {
+ .a0 = 0xdeadbee0,
+ .a1 = 0xdeadbee1,
+ .a2 = 0xdeadbee2,
+ .a3 = 0xdeadbee3,
+ };
+
+ while ((opt = getopt(argc, argv, "nicz")) > 0) {
+ switch (opt) {
+ case 'n':
+ armv7_secure_monitor_install();
+ break;
+ case 'i':
+ arm_smccc_smc(ARM_PSCI_0_2_FN_PSCI_VERSION,
+ 0, 0, 0, 0, 0, 0, 0, &res);
+ printf("found psci version %ld.%ld\n", res.a0 >> 16, res.a0 & 0xffff);
+ break;
+ case 'c':
+ arm_smccc_smc(ARM_PSCI_0_2_FN_CPU_ON,
+ 1, (unsigned long)second_entry, 0, 0, 0, 0, 0, &res);
+ break;
+ }
+ }
+
+
+ return 0;
+}
+BAREBOX_CMD_HELP_START(smc)
+BAREBOX_CMD_HELP_TEXT("Secure monitor code test command")
+BAREBOX_CMD_HELP_TEXT("")
+BAREBOX_CMD_HELP_TEXT("Options:")
+BAREBOX_CMD_HELP_OPT ("-n", "Install secure monitor and switch to nonsecure mode")
+BAREBOX_CMD_HELP_OPT ("-i", "Show information about installed PSCI version")
+BAREBOX_CMD_HELP_OPT ("-c", "Start secondary CPU core")
+BAREBOX_CMD_HELP_OPT ("-z", "Turn off secondary CPU core")
+BAREBOX_CMD_HELP_END
+
+BAREBOX_CMD_START(smc)
+ .cmd = do_smc,
+ BAREBOX_CMD_DESC("secure monitor test command")
+BAREBOX_CMD_END
+#endif \ No newline at end of file
diff --git a/arch/arm/cpu/sm.c b/arch/arm/cpu/sm.c
new file mode 100644
index 0000000..5808dfd
--- /dev/null
+++ b/arch/arm/cpu/sm.c
@@ -0,0 +1,266 @@
+/*
+ * (C) Copyright 2013
+ * Andre Przywara, Linaro <andre.przywara@linaro.org>
+ *
+ * Routines to transition ARMv7 processors from secure into non-secure state
+ * and from non-secure SVC into HYP mode
+ * needed to enable ARMv7 virtualization for current hypervisors
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+#define pr_fmt(fmt) "secure: " fmt
+
+#include <common.h>
+#include <io.h>
+#include <asm/gic.h>
+#include <asm/system.h>
+#include <init.h>
+#include <globalvar.h>
+#include <asm/arm-smccc.h>
+#include <asm-generic/sections.h>
+#include <asm/secure.h>
+
+#include "mmu.h"
+
+/* valid bits in CBAR register / PERIPHBASE value */
+#define CBAR_MASK 0xFFFF8000
+
+static unsigned int read_id_pfr1(void)
+{
+ unsigned int reg;
+
+ asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
+ return reg;
+}
+
+static u32 read_nsacr(void)
+{
+ unsigned int reg;
+
+ asm("mrc p15, 0, %0, c1, c1, 2\n" : "=r"(reg));
+ return reg;
+}
+
+static void write_nsacr(u32 val)
+{
+ asm("mcr p15, 0, %0, c1, c1, 2" : : "r"(val));
+}
+
+static void write_mvbar(u32 val)
+{
+ asm("mcr p15, 0, %0, c12, c0, 1" : : "r"(val));
+}
+
+static unsigned long get_cbar(void)
+{
+ unsigned periphbase;
+
+ /* get the GIC base address from the CBAR register */
+ asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase));
+
+ /* the PERIPHBASE can be mapped above 4 GB (lower 8 bits used to
+ * encode this). Bail out here since we cannot access this without
+ * enabling paging.
+ */
+ if ((periphbase & 0xff) != 0) {
+ pr_err("PERIPHBASE is above 4 GB, no access.\n");
+ return -1;
+ }
+
+ return periphbase & CBAR_MASK;
+}
+
+static unsigned long get_gicd_base_address(void)
+{
+ return get_cbar() + GIC_DIST_OFFSET;
+}
+
+static int cpu_is_virt_capable(void)
+{
+ return read_id_pfr1() & (1 << 12);
+}
+
+static unsigned long get_gicc_base_address(void)
+{
+ unsigned long adr = get_cbar();
+
+ if (cpu_is_virt_capable())
+ adr += GIC_CPU_OFFSET_A15;
+ else
+ adr += GIC_CPU_OFFSET_A9;
+
+ return adr;
+}
+
+#define GICD_IGROUPRn 0x0080
+
+int armv7_init_nonsec(void)
+{
+ void __iomem *gicd = IOMEM(get_gicd_base_address());
+ unsigned itlinesnr, i;
+ u32 val;
+
+ /*
+ * the SCR register will be set directly in the monitor mode handler,
+ * according to the spec one should not tinker with it in secure state
+ * in SVC mode. Do not try to read it once in non-secure state,
+ * any access to it will trap.
+ */
+
+ /* enable the GIC distributor */
+ val = readl(gicd + GICD_CTLR);
+ val |= 0x3;
+ writel(val, gicd + GICD_CTLR);
+
+ /* TYPER[4:0] contains an encoded number of available interrupts */
+ itlinesnr = readl(gicd + GICD_TYPER) & 0x1f;
+
+ /*
+ * Set all bits in the GIC group registers to one to allow access
+ * from non-secure state. The first 32 interrupts are private per
+ * CPU and will be set later when enabling the GIC for each core
+ */
+ for (i = 1; i <= itlinesnr; i++)
+ writel(0xffffffff, gicd + GICD_IGROUPRn + 4 * i);
+
+ return 0;
+}
+
+/*
+ * armv7_secure_monitor_install - install secure monitor
+ *
+ * This function is entered in secure mode. It installs the secure
+ * monitor code and enters it using a smc call. This function is executed
+ * on every CPU. We leave this function returns in nonsecure mode.
+ */
+int __armv7_secure_monitor_install(void)
+{
+ struct arm_smccc_res res;
+ void __iomem *gicd = IOMEM(get_gicd_base_address());
+ void __iomem *gicc = IOMEM(get_gicc_base_address());
+ u32 nsacr;
+
+ writel(0xffffffff, gicd + GICD_IGROUPRn);
+
+ writel(0x3, gicc + GICC_CTLR);
+ writel(0xff, gicc + GICC_PMR);
+
+ nsacr = read_nsacr();
+ nsacr |= 0x00043fff; /* all copros allowed in non-secure mode */
+ write_nsacr(nsacr);
+
+ write_mvbar((unsigned long)secure_monitor_init_vectors);
+
+ isb();
+
+ /* Initialize the secure monitor */
+ arm_smccc_smc(0, 0, 0, 0, 0, 0, 0, 0, &res);
+
+ /* We're in nonsecure mode now */
+
+ return 0;
+}
+
+static bool armv7_have_security_extensions(void)
+{
+ return (read_id_pfr1() & 0xf0) != 0;
+}
+
+/*
+ * armv7_secure_monitor_install - install secure monitor
+ *
+ * This function is entered in secure mode. It installs the secure
+ * monitor code and enters it using a smc call. This function is executed
+ * once on the primary CPU only. We leave this function returns in nonsecure
+ * mode.
+ */
+int armv7_secure_monitor_install(void)
+{
+ int mmuon;
+ unsigned long ttb, vbar;
+
+ if (!armv7_have_security_extensions()) {
+ pr_err("Security extensions not implemented.\n");
+ return -EINVAL;
+ }
+
+ mmuon = get_cr() & CR_M;
+
+ vbar = get_vbar();
+
+ asm volatile ("mrc p15, 0, %0, c2, c0, 0" : "=r"(ttb));
+
+ armv7_init_nonsec();
+ __armv7_secure_monitor_install();
+
+ asm volatile ("mcr p15, 0, %0, c2, c0, 0" : : "r"(ttb));
+
+ set_vbar(vbar);
+
+ if (mmuon) {
+ /*
+ * If the MMU was already turned on in secure mode, enable it in
+ * non-secure mode aswell
+ */
+ __mmu_cache_on();
+ }
+
+ pr_debug("Initialized secure monitor\n");
+
+ return 0;
+}
+
+/*
+ * of_secure_monitor_fixup - reserve memory region for secure monitor
+ *
+ * We currently do not support putting the secure monitor into onchip RAM,
+ * hence it runs in SDRAM and we must reserve the memory region so that we
+ * won't get overwritten from the Kernel.
+ * Beware: despite the name this is not secure in any way. The Kernel obeys
+ * the reserve map, but only because it's nice. It could always overwrite the
+ * secure monitor and hijack secure mode.
+ */
+static int of_secure_monitor_fixup(struct device_node *root, void *unused)
+{
+ unsigned long res_start, res_end;
+
+ res_start = (unsigned long)_stext;
+ res_end = (unsigned long)__bss_stop;
+
+ of_add_reserve_entry(res_start, res_end);
+
+ pr_debug("Reserved memory range from 0x%08lx to 0x%08lx\n", res_start, res_end);
+
+ return 0;
+}
+
+static enum arm_security_state bootm_secure_state;
+
+static const char * const bootm_secure_state_names[] = {
+ [ARM_STATE_SECURE] = "secure",
+ [ARM_STATE_NONSECURE] = "nonsecure",
+ [ARM_STATE_HYP] = "hyp",
+};
+
+enum arm_security_state bootm_arm_security_state(void)
+{
+ return bootm_secure_state;
+}
+
+const char *bootm_arm_security_state_name(enum arm_security_state state)
+{
+ return bootm_secure_state_names[state];
+}
+
+static int sm_init(void)
+{
+ of_register_fixup(of_secure_monitor_fixup, NULL);
+
+ globalvar_add_simple_enum("bootm.secure_state",
+ (unsigned int *)&bootm_secure_state,
+ bootm_secure_state_names,
+ ARRAY_SIZE(bootm_secure_state_names));
+
+ return 0;
+}
+device_initcall(sm_init); \ No newline at end of file
diff --git a/arch/arm/cpu/sm_as.S b/arch/arm/cpu/sm_as.S
new file mode 100644
index 0000000..09580e7
--- /dev/null
+++ b/arch/arm/cpu/sm_as.S
@@ -0,0 +1,168 @@
+#include <linux/linkage.h>
+#include <asm/ptrace.h>
+#include <asm-generic/memory_layout.h>
+#include <asm/secure.h>
+#include <asm/system.h>
+
+.arch_extension sec
+.arch_extension virt
+
+ .section ".text","ax"
+ .arm
+
+ .align 5
+.globl secure_monitor_init_vectors
+secure_monitor_init_vectors:
+1: b 1b /* reset */
+1: b 1b /* undefined instruction */
+ b secure_monitor_init /* software interrupt (SWI) */
+1: b 1b /* prefetch abort */
+1: b 1b /* data abort */
+1: b 1b /* (reserved) */
+1: b 1b /* irq (interrupt) */
+1: b 1b /* fiq (fast interrupt) */
+
+#define CPUID_ARM_GENTIMER_MASK (0xF << CPUID_ARM_GENTIMER_SHIFT)
+#define CPUID_ARM_GENTIMER_SHIFT 16
+
+#define CPUID_ARM_VIRT_MASK (0xF << CPUID_ARM_VIRT_SHIFT)
+#define CPUID_ARM_VIRT_SHIFT 12
+
+.macro is_cpu_virt_capable tmp
+ mrc p15, 0, \tmp, c0, c1, 1 @ read ID_PFR1
+ and \tmp, \tmp, #CPUID_ARM_VIRT_MASK @ mask virtualization bits
+ cmp \tmp, #(1 << CPUID_ARM_VIRT_SHIFT)
+.endm
+
+@ Requires dense and single-cluster CPU ID space
+ENTRY(psci_get_cpu_id)
+ mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */
+ and r0, r0, #0xff /* return CPU ID in cluster */
+ bx lr
+ENDPROC(psci_get_cpu_id)
+
+ENTRY(secure_monitor_stack_setup)
+ mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */
+ and r0, r0, #0xff /* CPU ID => r0 */
+
+ @ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT)
+ ldr r1, =__secure_stack_end
+ sub r0, r1, r0, LSL #ARM_SECURE_STACK_SHIFT
+ sub r0, r0, #4 @ Save space for target PC
+
+ mov sp, r0
+ bx lr
+ENDPROC(secure_monitor_stack_setup)
+
+secure_monitor_init:
+ mov r3, lr
+
+ bl secure_monitor_stack_setup
+
+ push {r4-r7}
+ mov r7, r3
+ ldr r5, =secure_monitor_vectors @ Switch MVBAR to secure_monitor_vectors
+ mcr p15, 0, r5, c12, c0, 1
+ isb
+
+#ifdef CONFIG_MMU
+ mrc p15, 0, r5, c1, c0, 0
+ tst r5, #CR_M
+ beq 1f
+ bl __mmu_cache_off
+1:
+#endif
+ mrc p15, 0, r5, c1, c1, 0 @ read SCR
+ bic r5, r5, #0x4a @ clear IRQ, EA, nET bits
+ orr r5, r5, #0x31 @ enable NS, AW, FW bits
+ @ FIQ preserved for secure mode
+ mov r6, #SVC_MODE @ default mode is SVC
+
+ is_cpu_virt_capable r4
+
+ orreq r5, r5, #0x100 @ allow HVC instruction
+
+ mcr p15, 0, r5, c1, c1, 0 @ write SCR (with NS bit set)
+ isb
+
+ mrceq p15, 0, r0, c12, c0, 1 @ get MVBAR value
+ mcreq p15, 4, r0, c12, c0, 0 @ write HVBAR
+
+ bne 1f
+
+ @ Reset CNTVOFF to 0 before leaving monitor mode
+ mrc p15, 0, r4, c0, c1, 1 @ read ID_PFR1
+ ands r4, r4, #CPUID_ARM_GENTIMER_MASK @ test arch timer bits
+ movne r4, #0
+ mcrrne p15, 4, r4, r4, c14 @ Reset CNTVOFF to zero
+1:
+ mov lr, r7
+ mov ip, #(PSR_F_BIT | PSR_I_BIT | PSR_A_BIT) @ Set A, I and F
+ tst lr, #1 @ Check for Thumb PC
+ orrne ip, ip, #PSR_T_BIT @ Set T if Thumb
+ orr ip, ip, r6 @ Slot target mode in
+ msr spsr_cxfs, ip @ Set full SPSR
+ pop {r4-r7}
+ movs pc, lr @ ERET to non-secure
+
+ .align 5
+secure_monitor_vectors:
+1: b 1b /* reset */
+1: b 1b /* undefined instruction */
+ b secure_monitor /* software interrupt (SWI) */
+1: b 1b /* prefetch abort */
+1: b 1b /* data abort */
+1: b hyp_trap /* (reserved) */
+1: b 1b /* irq (interrupt) */
+1: b 1b /* fiq (fast interrupt) */
+
+secure_monitor:
+ push {r4-r7,lr}
+
+ @ Switch to secure mode
+ mrc p15, 0, r7, c1, c1, 0
+ bic r4, r7, #1
+ mcr p15, 0, r4, c1, c1, 0
+ isb
+
+ /* r0-r6: Arguments */
+ sub sp, sp, #4*4 @ allocate result structure on stack
+ mov r12, sp
+ push {r4-r6, r12}
+ bl psci_entry
+ pop {r4-r6, r12}
+ ldm r12, {r0-r3}
+ add sp, sp, #4*4
+ /* r0-r3: results, r4-r14: preserved */
+
+ @ back to non-secure
+ mcr p15, 0, r7, c1, c1, 0
+
+ pop {r4-r7, lr}
+ movs pc, lr
+
+hyp_trap:
+ mrs lr, elr_hyp @ for older asm: .byte 0x00, 0xe3, 0x0e, 0xe1
+ mov pc, lr @ do no switch modes, but
+ @ return to caller
+
+ENTRY(armv7_switch_to_hyp)
+ mov r0, lr
+ mov r1, sp @ save SVC copy of LR and SP
+ isb
+ hvc #0 @ for older asm: .byte 0x70, 0x00, 0x40, 0xe1
+ mov sp, r1
+ mov lr, r0 @ restore SVC copy of LR and SP
+
+ bx lr
+ENDPROC(armv7_switch_to_hyp)
+
+ENTRY(psci_cpu_entry)
+ mrc p15, 0, r0, c1, c0, 1 @ ACTLR
+ orr r0, r0, #(1 << 6) @ Set SMP bit
+ mcr p15, 0, r0, c1, c0, 1 @ ACTLR
+
+ bl secure_monitor_stack_setup
+ bl psci_cpu_entry_c
+
+ENDPROC(psci_cpu_entry)
diff --git a/arch/arm/cpu/smccc-call.S b/arch/arm/cpu/smccc-call.S
new file mode 100644
index 0000000..c2781b1
--- /dev/null
+++ b/arch/arm/cpu/smccc-call.S
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/linkage.h>
+
+#include <asm/unwind.h>
+
+ .arch_extension sec
+ .arch_extension virt
+ .arm
+
+ /*
+ * Wrap c macros in asm macros to delay expansion until after the
+ * SMCCC asm macro is expanded.
+ */
+ .macro SMCCC_SMC
+ smc #0
+ .endm
+
+ .macro SMCCC_HVC
+ hvc #0
+ .endm
+
+ .macro SMCCC instr
+UNWIND( .fnstart)
+ mov r12, sp
+ push {r4-r7}
+UNWIND( .save {r4-r7})
+ ldm r12, {r4-r7}
+ \instr
+ pop {r4-r7}
+ ldr r12, [sp, #(4 * 4)]
+ stm r12, {r0-r3}
+ bx lr
+UNWIND( .fnend)
+ .endm
+
+/*
+ * void smccc_smc(unsigned long a0, unsigned long a1, unsigned long a2,
+ * unsigned long a3, unsigned long a4, unsigned long a5,
+ * unsigned long a6, unsigned long a7, struct arm_smccc_res *res)
+ */
+ENTRY(arm_smccc_smc)
+ SMCCC SMCCC_SMC
+ENDPROC(arm_smccc_smc)
+
+/*
+ * void smccc_hvc(unsigned long a0, unsigned long a1, unsigned long a2,
+ * unsigned long a3, unsigned long a4, unsigned long a5,
+ * unsigned long a6, unsigned long a7, struct arm_smccc_res *res)
+ */
+ENTRY(arm_smccc_hvc)
+ SMCCC SMCCC_HVC
+ENDPROC(arm_smccc_hvc)