ARM: HYP/non-sec: add generic ARMv7 PSCI code

Implement core support for PSCI. As this is generic code, it doesn't
implement anything really useful (all the functions are returning
Not Implemented).

Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
Acked-by: Ian Campbell <ijc@hellion.org.uk>

diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
index 232118d7f4135475c3489787211b3a04fe254080..735c4ad37e297dc316018ada487307853213cfcd 100644 (file)
--- a/arch/arm/cpu/armv7/Makefile
+++ b/arch/arm/cpu/armv7/Makefile
@@ -23,6 +23,10 @@ obj-y        += nonsec_virt.o
 obj-y  += virt-v7.o
 endif
 
+ifneq ($(CONFIG_ARMV7_PSCI),)
+obj-y  += psci.o
+endif
+
 obj-$(CONFIG_KONA) += kona-common/
 obj-$(CONFIG_OMAP_COMMON) += omap-common/
 obj-$(CONFIG_SYS_ARCH_TIMER) += arch_timer.o

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
new file mode 100644 (file)
index 0000000..bf11a34
--- /dev/null
+++ b/arch/arm/cpu/armv7/psci.S
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2013,2014 - ARM Ltd
+ * Author: Marc Zyngier <marc.zyngier@arm.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.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+#include <asm/psci.h>
+
+       .pushsection ._secure.text, "ax"
+
+       .arch_extension sec
+
+       .align  5
+       .globl _psci_vectors
+_psci_vectors:
+       b       default_psci_vector     @ reset
+       b       default_psci_vector     @ undef
+       b       _smc_psci               @ smc
+       b       default_psci_vector     @ pabort
+       b       default_psci_vector     @ dabort
+       b       default_psci_vector     @ hyp
+       b       default_psci_vector     @ irq
+       b       psci_fiq_enter          @ fiq
+
+ENTRY(psci_fiq_enter)
+       movs    pc, lr
+ENDPROC(psci_fiq_enter)
+.weak psci_fiq_enter
+
+ENTRY(default_psci_vector)
+       movs    pc, lr
+ENDPROC(default_psci_vector)
+.weak default_psci_vector
+
+ENTRY(psci_cpu_suspend)
+ENTRY(psci_cpu_off)
+ENTRY(psci_cpu_on)
+ENTRY(psci_migrate)
+       mov     r0, #ARM_PSCI_RET_NI    @ Return -1 (Not Implemented)
+       mov     pc, lr
+ENDPROC(psci_migrate)
+ENDPROC(psci_cpu_on)
+ENDPROC(psci_cpu_off)
+ENDPROC(psci_cpu_suspend)
+.weak psci_cpu_suspend
+.weak psci_cpu_off
+.weak psci_cpu_on
+.weak psci_migrate
+
+_psci_table:
+       .word   ARM_PSCI_FN_CPU_SUSPEND
+       .word   psci_cpu_suspend
+       .word   ARM_PSCI_FN_CPU_OFF
+       .word   psci_cpu_off
+       .word   ARM_PSCI_FN_CPU_ON
+       .word   psci_cpu_on
+       .word   ARM_PSCI_FN_MIGRATE
+       .word   psci_migrate
+       .word   0
+       .word   0
+
+_smc_psci:
+       push    {r4-r7,lr}
+
+       @ Switch to secure
+       mrc     p15, 0, r7, c1, c1, 0
+       bic     r4, r7, #1
+       mcr     p15, 0, r4, c1, c1, 0
+       isb
+
+       adr     r4, _psci_table
+1:     ldr     r5, [r4]                @ Load PSCI function ID
+       ldr     r6, [r4, #4]            @ Load target PC
+       cmp     r5, #0                  @ If reach the end, bail out
+       moveq   r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid)
+       beq     2f
+       cmp     r0, r5                  @ If not matching, try next entry
+       addne   r4, r4, #8
+       bne     1b
+
+       blx     r6                      @ Execute PSCI function
+
+       @ Switch back to non-secure
+2:     mcr     p15, 0, r7, c1, c1, 0
+
+       pop     {r4-r7, lr}
+       movs    pc, lr                  @ Return to the kernel
+
+       .popsection

diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
new file mode 100644 (file)
index 0000000..704b4b0
--- /dev/null
+++ b/arch/arm/include/asm/psci.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2013 - ARM Ltd
+ * Author: Marc Zyngier <marc.zyngier@arm.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.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ARM_PSCI_H__
+#define __ARM_PSCI_H__
+
+/* PSCI interface */
+#define ARM_PSCI_FN_BASE               0x95c1ba5e
+#define ARM_PSCI_FN(n)                 (ARM_PSCI_FN_BASE + (n))
+
+#define ARM_PSCI_FN_CPU_SUSPEND                ARM_PSCI_FN(0)
+#define ARM_PSCI_FN_CPU_OFF            ARM_PSCI_FN(1)
+#define ARM_PSCI_FN_CPU_ON             ARM_PSCI_FN(2)
+#define ARM_PSCI_FN_MIGRATE            ARM_PSCI_FN(3)
+
+#define ARM_PSCI_RET_SUCCESS           0
+#define ARM_PSCI_RET_NI                        (-1)
+#define ARM_PSCI_RET_INVAL             (-2)
+#define ARM_PSCI_RET_DENIED            (-3)
+
+#endif /* __ARM_PSCI_H__ */