[PATCH boot-wrapper-aarch64 4/4] psci: implement PSCI v0.2

[zlim@broadcom.com (Zi Shen Lim)]

In this simple implementation:
* cpu_suspend is not supported, and as such is not fully
  compliant with the spec.
* The system_{off,reset} functions currently just spin on
  one CPU, when it should instead power {down,cycle}
  the system.

Signed-off-by: Zi Shen Lim <zlim@broadcom.com>
---
 Makefile.am |   4 +-
 psci.S      | 158 ++++++++++++++++++++++++++++++++++++++++++++++++++++--------
 2 files changed, 138 insertions(+), 24 deletions(-)

diff --git a/Makefile.am b/Makefile.am
index 4f5bfdd..471462a 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -23,10 +23,8 @@ BOOTLOADER	:= boot.S
 if PSCI
 BOOTMETHOD	:= psci.o
 PSCI_NODE	:= psci {				\
-			compatible = \"arm,psci\";	\
+			compatible = \"arm,psci-0.2\";	\
 			method = \"smc\";		\
-			cpu_on = <0xc4000003>;		\
-			cpu_off = <0x84000002>;		\
 		   };
 CPU_NODES	:= $(shell $(top_srcdir)/gen-cpu-nodes.sh $(CPU_IDS))
 CPUS_NODE	:= cpus {		\
diff --git a/psci.S b/psci.S
index c51e125..b8afb0e 100644
--- a/psci.S
+++ b/psci.S
@@ -8,13 +8,26 @@
  */
 #include "common.S"
 
-#define PSCI_CPU_OFF		0x84000002
-#define PSCI_CPU_ON		0xc4000003
-
-#define PSCI_RET_SUCCESS	0
-#define PSCI_RET_NOT_IMPL	(-1)
-#define PSCI_RET_INVALID	(-2)
-#define PSCI_RET_DENIED		(-3)
+#define PSCI_VERSION			0x84000000
+#define PSCI_CPU_SUSPEND		0xc4000001
+#define PSCI_CPU_OFF			0x84000002
+#define PSCI_CPU_ON			0xc4000003
+#define PSCI_AFFINITY_INFO		0xc4000004
+#define PSCI_MIGRATE			0xc4000005
+#define PSCI_MIGRATE_INFO_TYPE		0x84000006
+#define PSCI_MIGRATE_INFO_UP_CPU	0xc4000007
+#define PSCI_SYSTEM_OFF			0x84000008
+#define PSCI_SYSTEM_RESET		0x84000009
+
+#define PSCI_RET_SUCCESS		0
+#define PSCI_RET_NOT_SUPPORTED		(-1)
+#define PSCI_RET_INVALID_PARAMETERS	(-2)
+#define PSCI_RET_DENIED			(-3)
+#define PSCI_RET_ALREADY_ON		(-4)
+#define PSCI_RET_ON_PENDING		(-5)
+#define PSCI_RET_INTERNAL_FAILURE	(-6)
+#define PSCI_RET_NOT_PRESENT		(-7)
+#define PSCI_RET_DISABLED		(-8)
 
 #ifndef CPU_IDS
 #error No CPU MPIDRs provided.
@@ -77,6 +90,16 @@ branch_table:
 	.quad ADDR_INVALID
 	.endr
 
+#define AFFINFO_ON		0
+#define AFFINFO_OFF		1
+#define AFFINFO_ON_PENDING	2
+#define AFFINFO_DISABLED	PSCI_RET_DISABLED
+
+affinfo_table:
+	.rept (nr_cpus)
+	.quad AFFINFO_DISABLED
+	.endr
+
 	.text
 
 	.globl start_no_el3
@@ -86,10 +109,18 @@ err_exception:
 	b err_exception
 
 psci_call32:
-	mov	w0, PSCI_RET_NOT_IMPL
+	mov	w0, PSCI_RET_NOT_SUPPORTED
 	eret
 
 psci_call64:
+	ldr	x7, =PSCI_VERSION
+	cmp	x0, x7
+	b.eq	psci_version
+
+	ldr	x7, =PSCI_CPU_SUSPEND
+	cmp	x0, x7
+	b.eq	psci_not_supported	// XXX: not spec compliant
+
 	ldr	x7, =PSCI_CPU_OFF
 	cmp	x0, x7
 	b.eq	psci_cpu_off
@@ -98,7 +129,36 @@ psci_call64:
 	cmp	x0, x7
 	b.eq	psci_cpu_on
 
-	mov	x0, PSCI_RET_NOT_IMPL
+	ldr	x7, =PSCI_AFFINITY_INFO
+	cmp	x0, x7
+	b.eq	psci_affinity_info
+
+	ldr	x7, =PSCI_MIGRATE
+	cmp	x0, x7
+	b.eq	psci_not_supported
+
+	ldr	x7, =PSCI_MIGRATE_INFO_TYPE
+	cmp	x0, x7
+	b.eq	psci_migrate_info_type
+
+	ldr	x7, =PSCI_MIGRATE_INFO_UP_CPU
+	cmp	x0, x7
+	b.eq	psci_not_supported
+
+	ldr	x7, =PSCI_SYSTEM_OFF
+	cmp	x0, x7
+	b.eq	psci_system_off
+
+	ldr	x7, =PSCI_SYSTEM_RESET
+	cmp	x0, x7
+	b.eq	psci_system_reset
+
+psci_not_supported:
+	mov	x0, PSCI_RET_NOT_SUPPORTED
+	eret
+
+psci_version:
+	mov	x0, #((0 << 16) | (2 << 0))
 	eret
 
 /*
@@ -109,6 +169,11 @@ psci_cpu_off:
 	ldr	x1, =MPIDR_ID_BITS
 	and	x0, x0, x1
 	bl	find_logical_id
+
+	adr	x1, affinfo_table
+	mov	x2, #AFFINFO_OFF
+	str	x2, [x1, x0, lsl #3]
+
 	adr	x1, branch_table
 	mov	x2, #ADDR_INVALID
 	str	x2, [x1, x0, lsl #3]
@@ -118,39 +183,85 @@ psci_cpu_off:
 /*
  * x1 - target cpu
  * x2 - address
+ * x3 - context id (Note: currently ignored)
  */
 psci_cpu_on:
-	mov	x15, x30
 	mov	x14, x2
-	mov	x0, x1
 
+	mov	x0, x1
+	mov	x15, x30
 	bl	find_logical_id
+	mov	x30, x15
 	cmp	x0, #MPIDR_INVALID
 	b.eq	1f
 
+	adr	x1, affinfo_table
+	ldr	x2, [x1, x0, lsl #3]
+	cmp	x2, #AFFINFO_ON
+	b.eq	2f
+	cmp	x2, #AFFINFO_ON_PENDING
+	b.eq	3f
+	cmp	x2, #AFFINFO_DISABLED
+	b.eq	1f
+
+	mov	x2, #AFFINFO_ON_PENDING
+	str	x2, [x1, x0, lsl #3]
+
 	adr	x3, branch_table
 	add	x3, x3, x0, lsl #3
 
-	ldr	x4, =ADDR_INVALID
-
 	ldxr	x5, [x3]
-	cmp	x4, x5
-	b.ne	1f
+	cmp	x5, #ADDR_INVALID
+	b.ne	2f
 
 	stxr	w4, x14, [x3]
-	cbnz	w4, 1f
+	cbnz	w4, 2f
 
 	dsb	ishst
 	sev
 
 	mov	x0, #PSCI_RET_SUCCESS
-	mov	x30, x15
 	eret
 
-1:	mov	x0, #PSCI_RET_DENIED
+1:	mov	x0, #PSCI_RET_INVALID_PARAMETERS
+	eret
+
+2:	mov	x0, #PSCI_RET_ALREADY_ON
+	eret
+
+3:	mov	x0, #PSCI_RET_ON_PENDING
+	eret
+
+/*
+ * x1 - target affinity (same as target cpu)
+ * x2 - lowest affinity level (Note: only 0 is supported)
+ */
+psci_affinity_info:
+	cbnz	x2, 1f
+
+	mov	x0, x1
+	mov	x15, x30
+	bl	find_logical_id
 	mov	x30, x15
+	cmp	x0, #MPIDR_INVALID
+	b.eq	1f
+
+	adr	x3, affinfo_table
+	ldr	x0, [x3, x0, lsl #3]
 	eret
 
+1:	mov	x0, #PSCI_RET_INVALID_PARAMETERS
+	eret
+
+psci_migrate_info_type:
+	mov	x0, #2	// Trusted OS not present, doesn't require migration.
+	eret
+
+psci_system_off:
+	b	spin_dead	// XXX: need to power down system
+
+psci_system_reset:
+	b	spin_dead	// XXX: need to power cycle system
 
 /*
  * Takes masked MPIDR in x0, returns logical id in x0
@@ -208,16 +319,21 @@ spin:
 	cmp	x0, #MPIDR_INVALID
 	b.eq	spin_dead
 
-	adr	x1, branch_table
-	mov	x3, #ADDR_INVALID
+	adr	x15, affinfo_table
+	mov	x14, #AFFINFO_OFF
+	str	x14, [x15, x0, lsl #3]
 
+	adr	x1, branch_table
 	add	x1, x1, x0, lsl #3
 
 1:	wfe
 	ldr	x2, [x1]
-	cmp	x2, x3
+	cmp	x2, #ADDR_INVALID
 	b.eq	1b
 
+	mov	x14, #AFFINFO_ON
+	str	x14, [x15, x0, lsl #3]
+
 	ldr	x0, =SCTLR_EL2_RESET
 	msr	sctlr_el2, x0
 
-- 
2.1.0