[U-Boot] [PATCH v2 08/13] ARM: HYP/non-sec: add generic ARMv7 PSCI code

[Marc Zyngier]

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 at 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

diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
index 4069aa4..8027385 100644
--- a/arch/arm/cpu/armv7/Makefile
+++ b/arch/arm/cpu/armv7/Makefile
@@ -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/
 
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
new file mode 100644
index 0000000..3c70b9a
--- /dev/null
+++ b/arch/arm/cpu/armv7/psci.S
@@ -0,0 +1,113 @@
+/*
+ * Copyright (C) 2013 - ARM Ltd
+ * Author: Marc Zyngier <marc.zyngier at 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
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
new file mode 100644
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 at 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__ */
-- 
1.8.4.3