@@ -24,6 +24,10 @@ obj-y += nonsec_virt.o
obj-y += virt-v7.o
endif
+ifneq ($(CONFIG_ARMV7_PSCI),)
+obj-y += psci.o
+endif
+
obj-$(CONFIG_OMAP_COMMON) += omap-common/
obj-$(CONFIG_TEGRA) += tegra-common/
new file mode 100644
@@ -0,0 +1,113 @@
+/*
+ * 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/>.
+ */
+
+#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:
+ adr pc, . @ reset
+ adr pc, . @ undef
+ adr pc, _smc_psci @ smc
+ adr pc, . @ pabort
+ adr pc, . @ dabort
+ adr pc, . @ hyp
+ adr pc, . @ irq
+ adr pc, . @ fiq
+
+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
+
+_secure_stacks: @ Enough to save 16 registers per CPU
+ .skip 16*4*CONFIG_ARMV7_PSCI_NR_CPUS
+_secure_stack_base:
+
+_smc_psci:
+ @ Switch to secure mode
+ mrc p15, 0, sp, c1, c1, 0
+ bic sp, sp, #1
+ mcr p15, 0, sp, c1, c1, 0
+
+ adr sp, _secure_stack_base
+ mcr p15, 0, r0, c13, c0, 4 @ use TPIDRPRW as a tmp reg
+ mcr p15, 0, r1, c13, c0, 3 @ use TPIDRURO as a tmp reg
+ mrc p15, 0, r0, c0, c0, 5 @ MPIDR
+ and r1, r0, #3 @ cpu number in cluster
+ lsr r0, r0, #8
+ and r0, r0, #3 @ cluster number
+ mul r1, r1, r0 @ absolute cpu nr
+ sbc sp, sp, r1, lsl #6 @ sp = sp_base - 64*cpunr
+
+ mrc p15, 0, r0, c13, c0, 4 @ restore r0
+ mrc p15, 0, r1, c13, c0, 3 @ restore r1
+
+ push {r4-r12,lr}
+
+ 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
+ mvneq r0, #0 @ Return -1 (Not Implemented)
+ beq 2f
+ cmp r0, r5 @ If not matching, try next entry
+ addne r4, r4, #8
+ bne 1b
+ cmp r6, #0 @ Not implemented
+ moveq r0, #ARM_PSCI_RET_NI
+ beq 2f
+
+ blx r6 @ Execute PSCI function
+
+2: pop {r4-r12, lr}
+
+ @ Back to non-secure
+ mrc p15, 0, sp, c1, c1, 0
+ orr sp, sp, #1
+ mcr p15, 0, sp, c1, c1, 0
+ movs pc, lr @ Return to the kernel
+
+ .popsection
new file mode 100644
@@ -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__ */
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> --- arch/arm/cpu/armv7/Makefile | 4 ++ arch/arm/cpu/armv7/psci.S | 113 ++++++++++++++++++++++++++++++++++++++++++++ arch/arm/include/asm/psci.h | 35 ++++++++++++++ 3 files changed, 152 insertions(+) create mode 100644 arch/arm/cpu/armv7/psci.S create mode 100644 arch/arm/include/asm/psci.h