[U-Boot] [PATCH 5/9] 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   | 109 ++++++++++++++++++++++++++++++++++++++++++++
 psci/Makefile               |  67 +++++++++++++++++++++++++++
 3 files changed, 180 insertions(+)
 create mode 100644 arch/arm/cpu/armv7/psci.S
 create mode 100644 psci/Makefile

diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
index 8fac06c..24b6cb9 100644
--- 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_PSCI_BUILD),)
+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..3cfb147
--- /dev/null
+++ b/arch/arm/cpu/armv7/psci.S
@@ -0,0 +1,109 @@
+/*
+ * 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>
+
+	.arch_extension	sec
+
+	.globl	_start		@ Keep linker happy...
+_start:
+	.align	5
+_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)
+	mvn	r0, #0		@ 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	0x95c10000
+	.word	psci_cpu_suspend
+	.word	0x95c10001
+	.word	psci_cpu_off
+	.word	0x95c10002
+	.word	psci_cpu_on
+	.word	0x95c10003
+	.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
+	mvneq	r0, #0
+	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
diff --git a/psci/Makefile b/psci/Makefile
new file mode 100644
index 0000000..7cc8b29
--- /dev/null
+++ b/psci/Makefile
@@ -0,0 +1,67 @@
+#
+# (C) Copyright 2000-2011
+# Wolfgang Denk, DENX Software Engineering, wd at denx.de.
+#
+# (C) Copyright 2011
+# Daniel Schwierzeck, daniel.schwierzeck at googlemail.com.
+#
+# (C) Copyright 2011
+# Texas Instruments Incorporated - http://www.ti.com/
+# Aneesh V <aneesh at ti.com>
+#
+# (C) Copyright 2013 ARM Ltd
+# Marc Zyngier <marc.zyngier at arm.com>
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+# Based on top-level Makefile.
+#
+
+CONFIG_PSCI_BUILD := y
+export CONFIG_PSCI_BUILD
+
+PSCI_BIN := u-boot-psci
+
+include $(TOPDIR)/config.mk
+
+obj := $(OBJTREE)/psci/
+
+PSCI_OBJS := $(CPUDIR)/psci.o
+PSCI_OBJS += $(CPUDIR)/nonsec_virt.o
+PSCI_OBJS += $(SOC_PSCI)
+
+# Linker Script
+LDSCRIPT := $(TOPDIR)/$(CPUDIR)/$(SOC)/u-boot-psci.lds
+
+build := -f $(TOPDIR)/scripts/Makefile.build -C
+
+# Special flags for CPP when processing the linker script.
+# Pass the version down so we can handle backwards compatibility
+# on the fly.
+LDPPFLAGS += \
+	-include $(TOPDIR)/include/u-boot/u-boot.lds.h \
+	-include $(OBJTREE)/include/config.h \
+	-DCPUDIR=$(CPUDIR) \
+	$(shell $(LD) --version | \
+	  sed -ne 's/GNU ld version \([0-9][0-9]*\)\.\([0-9][0-9]*\).*/-DLD_MAJOR=\1 -DLD_MINOR=\2/p')
+
+all:	$(PSCI_BIN)
+
+$(PSCI_BIN):	depend $(PSCI_OBJS) u-boot-psci.lds
+	$(LD) $(LDFLAGS) $(LDFLAGS_$(@F))		\
+	      $(PSCI_OBJS) -T u-boot-psci.lds	\
+	      -Map $(PSCI_BIN).map -o $(PSCI_BIN)
+
+$(PSCI_OBJS):	depend
+	$(MAKE) $(build) $(SRCTREE)/$(dir $@)
+	mkdir -p $(dir $@)
+	mv $(SRCTREE)/$@ $@
+
+u-boot-psci.lds: $(LDSCRIPT) depend
+	$(CPP) $(CPPFLAGS) $(LDPPFLAGS) -I$(obj). -ansi -D__ASSEMBLY__ -P - < $< > $@
+
+depend:	$(obj).depend
+.PHONY: depend
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
-- 
1.8.2.3