[U-Boot] [PATCH v1 2/3] ARM: socfpga: stratix10: Enable PSCI CPU_ON

[chee.hong.ang at intel.com]

From: "Ang, Chee Hong" <chee.hong.ang at intel.com>

Enable psci_cpu_on support for Stratix10. This PSCI function
will pass the cpu release address for CPU1-CPU3. Then send event
signal shall be triggered to get these CPUs running Linux code.

Signed-off-by: Ang, Chee Hong <chee.hong.ang at intel.com>
---
 arch/arm/mach-socfpga/psci.c | 35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/arch/arm/mach-socfpga/psci.c b/arch/arm/mach-socfpga/psci.c
index 9ef3931..0af3eb1 100644
--- a/arch/arm/mach-socfpga/psci.c
+++ b/arch/arm/mach-socfpga/psci.c
@@ -11,6 +11,9 @@
 #include <asm/arch/mailbox_s10.h>
 #include <asm/secure.h>
 
+static u64 psci_cpu_on_64_cpuid __secure_data;
+static u64 psci_cpu_on_64_entry_point __secure_data;
+
 void __noreturn __secure psci_system_reset(void)
 {
 	mbox_send_cmd_psci(MBOX_ID_UBOOT, MBOX_REBOOT_HPS,
@@ -19,3 +22,35 @@ void __noreturn __secure psci_system_reset(void)
 	while (1)
 		;
 }
+
+/* This function will handle multiple core release based PSCI */
+void __secure psci_cpu_on_64_mpidr(void)
+{
+	asm volatile(
+		".align	5			\n"
+		"1:	wfe			\n"
+		"	ldr	x0, [%0]	\n"
+		"	ldr	x1, [%1]	\n"
+		"	mrs	x2, mpidr_el1	\n"
+		"	and	x2, x2, #0xff	\n"
+		"	cmp	x0, x2		\n"
+		"	b.ne	1b		\n"
+		"	br	x1		\n"
+	: : "r"(&psci_cpu_on_64_cpuid), "r"(&psci_cpu_on_64_entry_point)
+	: "x0", "x1", "x2", "memory", "cc");
+}
+
+int __secure psci_cpu_on_64(u32 function_id, u64 cpuid, u64 entry_point)
+{
+	/* Releases all secondary CPUs to jump into psci_cpu_on_64_mpidr */
+	writeq(0, &psci_cpu_on_64_cpuid);
+	writeq(0, &psci_cpu_on_64_entry_point);
+	writeq((u64)&psci_cpu_on_64_mpidr, CPU_RELEASE_ADDR);
+
+	/* to store in global so psci_cpu_on_64_mpidr function can refer */
+	writeq(entry_point, &psci_cpu_on_64_entry_point);
+	writeq(cpuid, &psci_cpu_on_64_cpuid);
+	asm volatile("sev");
+
+	return ARM_PSCI_RET_SUCCESS;
+}
-- 
2.7.4