[U-Boot] [PATCH 5/8] ARMv8: PCSI: Add generic ARMv8 PSCI code

[Arnab Basu]

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

This is largely ported from the similar code that exists for ARMv7

Signed-off-by: Arnab Basu <arnab.basu at freescale.com>
Reviewed-by: Bhupesh Sharma <bhupesh.sharma at freescale.com>
Cc: Marc Zyngier <marc.zyngier at arm.com>
---
 arch/arm/cpu/armv8/Makefile |    1 +
 arch/arm/cpu/armv8/psci.S   |  171 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 172 insertions(+), 0 deletions(-)
 create mode 100644 arch/arm/cpu/armv8/psci.S

diff --git a/arch/arm/cpu/armv8/Makefile b/arch/arm/cpu/armv8/Makefile
index 4f0ea87..8f6988d 100644
--- a/arch/arm/cpu/armv8/Makefile
+++ b/arch/arm/cpu/armv8/Makefile
@@ -15,3 +15,4 @@ obj-y	+= cache.o
 obj-y	+= tlb.o
 obj-y	+= transition.o
 obj-y	+= cpu-dt.o
+obj-$(CONFIG_ARMV8_PSCI)	+= psci.o
diff --git a/arch/arm/cpu/armv8/psci.S b/arch/arm/cpu/armv8/psci.S
new file mode 100644
index 0000000..5f4e3b2
--- /dev/null
+++ b/arch/arm/cpu/armv8/psci.S
@@ -0,0 +1,171 @@
+/*
+ * (C) Copyright 2014
+ * Arnab Basu <arnab.basu at freescale.com>
+ *
+ * Based on arch/arm/cpu/armv7/psci.S
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <linux/linkage.h>
+#include <asm/psci.h>
+
+.pushsection ._secure.text, "ax"
+
+ENTRY(psci_0_2_cpu_suspend_64)
+ENTRY(psci_0_2_cpu_on_64)
+ENTRY(psci_0_2_affinity_info_64)
+ENTRY(psci_0_2_migrate_64)
+ENTRY(psci_0_2_migrate_info_up_cpu_64)
+	mov	x0, #ARM_PSCI_RET_NI	/* Return -1 (Not Implemented) */
+	ret
+ENDPROC(psci_0_2_cpu_suspend_64)
+ENDPROC(psci_0_2_cpu_on_64)
+ENDPROC(psci_0_2_affinity_info_64)
+ENDPROC(psci_0_2_migrate_64)
+ENDPROC(psci_0_2_migrate_info_up_cpu_64)
+.weak psci_0_2_cpu_suspend_64
+.weak psci_0_2_cpu_on_64
+.weak psci_0_2_affinity_info_64
+.weak psci_0_2_migrate_64
+.weak psci_0_2_migrate_info_up_cpu_64
+
+ENTRY(psci_0_2_psci_version)
+	mov	x0, #2			/* Return Major = 0, Minor = 2*/
+	ret
+ENDPROC(psci_0_2_psci_version)
+
+.align 4
+_psci_0_2_table:
+	.quad	PSCI_0_2_FN_PSCI_VERSION
+	.quad	psci_0_2_psci_version
+	.quad	PSCI_0_2_FN64_CPU_SUSPEND
+	.quad	psci_0_2_cpu_suspend_64
+	.quad	PSCI_0_2_FN64_CPU_ON
+	.quad	psci_0_2_cpu_on_64
+	.quad	PSCI_0_2_FN64_AFFINITY_INFO
+	.quad	psci_0_2_affinity_info_64
+	.quad	PSCI_0_2_FN64_MIGRATE
+	.quad	psci_0_2_migrate_64
+	.quad	PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU
+	.quad	psci_0_2_migrate_info_up_cpu_64
+	.quad	0
+	.quad	0
+
+.macro	psci_enter
+	stp	x29, x30, [sp, #-16]!
+	stp	x27, x28, [sp, #-16]!
+	stp	x25, x26, [sp, #-16]!
+	stp	x23, x24, [sp, #-16]!
+	stp	x21, x22, [sp, #-16]!
+	stp	x19, x20, [sp, #-16]!
+	stp	x17, x18, [sp, #-16]!
+	stp	x15, x16, [sp, #-16]!
+	stp	x13, x14, [sp, #-16]!
+	stp	x11, x12, [sp, #-16]!
+	stp	x9, x10, [sp, #-16]!
+	stp	x7, x8, [sp, #-16]!
+	stp	x5, x6, [sp, #-16]!
+	mrs	x5, elr_el3
+	stp	x5, x4, [sp, #-16]!
+
+	/* EL0 and El1 will execute in secure */
+	mrs	x4, scr_el3
+	bic	x4, x4, #1
+	msr	scr_el3, x4
+.endm
+
+.macro	psci_return
+	/* EL0 and El1 will execute in non-secure */
+	mrs	x4, scr_el3
+	orr	x4, x4, #1
+	msr	scr_el3, x4
+
+	ldp	x5, x4, [sp], #16
+	msr	elr_el3, x5
+	ldp	x5, x6, [sp], #16
+	ldp	x7, x8, [sp], #16
+	ldp	x9, x10, [sp], #16
+	ldp	x11, x12, [sp], #16
+	ldp	x13, x14, [sp], #16
+	ldp	x15, x16, [sp], #16
+	ldp	x17, x18, [sp], #16
+	ldp	x19, x20, [sp], #16
+	ldp	x21, x22, [sp], #16
+	ldp	x23, x24, [sp], #16
+	ldp	x25, x26, [sp], #16
+	ldp	x27, x28, [sp], #16
+	ldp	x29, x30, [sp], #16
+	eret
+.endm
+
+ENTRY(_smc_psci)
+	psci_enter
+	adr	x4, _psci_0_2_table
+1:	ldr	x5, [x4]		/* Load PSCI function ID */
+	ldr	x6, [x4, #8]		/* Load target PC */
+	cmp	x5, #0			/* If reach the end, bail out */
+	b.eq	fn_not_found
+	cmp	x0, x5			/* If not matching, try next entry */
+	b.eq	fn_call
+	add	x4, x4, #16
+	b	1b
+
+fn_call:
+	blr	x6
+	psci_return
+
+fn_not_found:
+	mov	x0, #ARM_PSCI_RET_INVAL  /* Return -2 (Invalid) */
+	psci_return
+ENDPROC(_smc_psci)
+
+ENTRY(default_psci_vector)
+	eret
+ENDPROC(default_psci_vector)
+
+.align 2
+__handle_sync:
+	str 	x4, [sp, #-8]!
+	mrs	x4, esr_el3
+	ubfx	x4, x4, #26, #6
+	cmp	x4, #0x17
+	b.eq	smc_found
+	ldr	x4, [sp], #8
+	b	default_psci_vector
+smc_found:
+	ldr     x4, [sp], #8
+	b	_smc_psci
+
+/*
+ * PSCI Exception vectors.
+ */
+	.align	11
+	.globl	psci_vectors
+psci_vectors:
+	.align	7
+	b	default_psci_vector	/* Current EL Synchronous Thread */
+	.align	7
+	b	default_psci_vector	/* Current EL IRQ Thread */
+	.align	7
+	b	default_psci_vector	/* Current EL FIQ Thread */
+	.align	7
+	b	default_psci_vector	/* Current EL Error Thread */
+	.align	7
+	b	default_psci_vector	/* Current EL Synchronous Handler */
+	.align	7
+	b	default_psci_vector	/* Current EL IRQ Handler */
+	.align	7
+	b	default_psci_vector	/* Current EL FIQ Handler */
+	.align	7
+	b	default_psci_vector	/* Current EL Error Handler */
+	.align	7
+	b	__handle_sync		/* Lower EL Synchronous (64b) */
+	.align	7
+	b	default_psci_vector	/* Lower EL IRQ (64b) */
+	.align	7
+	b	default_psci_vector	/* Lower EL FIQ (64b) */
+	.align	7
+	b	default_psci_vector	/* Lower EL Error (64b) */
+
+.popsection
-- 
1.7.7.4