From: "Ang, Chee Hong" chee.hong.ang@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@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; +}