[PATCH] armv8: layerscape: add PSCI support for cpu release

[Jiafei Pan]

For cpu release command, check whether PSCI is supported firstly,
if supported, use PSCI to kick off secondary cores, otherwise still
use spin table.

Signed-off-by: Jiafei Pan <Jiafei.Pan at nxp.com>
---
 arch/arm/cpu/armv8/fsl-layerscape/cpu.c |  2 +-
 arch/arm/cpu/armv8/fsl-layerscape/cpu.h |  1 +
 arch/arm/cpu/armv8/fsl-layerscape/mp.c  | 49 +++++++++++++++++--------
 3 files changed, 36 insertions(+), 16 deletions(-)

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 270a72e550..d0103fc881 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -1063,7 +1063,7 @@ int cpu_eth_init(struct bd_info *bis)
 	return error;
 }
 
-static inline int check_psci(void)
+int check_psci(void)
 {
 	unsigned int psci_ver;
 
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.h b/arch/arm/cpu/armv8/fsl-layerscape/cpu.h
index dca5fd0f7d..45da95831e 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.h
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.h
@@ -6,3 +6,4 @@
 int fsl_qoriq_core_to_cluster(unsigned int core);
 u32 initiator_type(u32 cluster, int init_id);
 u32 cpu_mask(void);
+int check_psci(void);
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/mp.c b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
index 5ac545f9df..d6d839c31c 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/mp.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
@@ -10,10 +10,12 @@
 #include <asm/cache.h>
 #include <asm/global_data.h>
 #include <asm/io.h>
+#include <asm/ptrace.h>
 #include <asm/system.h>
 #include <asm/arch/mp.h>
 #include <asm/arch/soc.h>
 #include <linux/delay.h>
+#include <linux/psci.h>
 #include "cpu.h"
 #include <asm/arch-fsl-layerscape/soc.h>
 #include <efi_loader.h>
@@ -301,24 +303,41 @@ int cpu_release(u32 nr, int argc, char *const argv[])
 	u64 *table = get_spin_tbl_addr();
 	int pos;
 
-	pos = core_to_pos(nr);
-	if (pos <= 0)
-		return -1;
-
-	table += pos * WORDS_PER_SPIN_TABLE_ENTRY;
 	boot_addr = simple_strtoull(argv[0], NULL, 16);
-	table[SPIN_TABLE_ELEM_ENTRY_ADDR_IDX] = boot_addr;
-	flush_dcache_range((unsigned long)table,
+
+	if (check_psci()) {
+		/* SPIN Table is used */
+		pos = core_to_pos(nr);
+		if (pos <= 0)
+			return -1;
+
+		table += pos * WORDS_PER_SPIN_TABLE_ENTRY;
+		table[SPIN_TABLE_ELEM_ENTRY_ADDR_IDX] = boot_addr;
+		flush_dcache_range((unsigned long)table,
 			   (unsigned long)table + SPIN_TABLE_ELEM_SIZE);
-	asm volatile("dsb st");
+		asm volatile("dsb st");
 
-	/*
-	 * The secondary CPUs polling the spin-table above for a non-zero
-	 * value. To save power "wfe" is called. Thus call "sev" here to
-	 * wake the CPUs and let them check the spin-table again (see
-	 * slave_cpu loop in lowlevel.S)
-	 */
-	asm volatile("sev");
+		/*
+		 * The secondary CPUs polling the spin-table above for a non-zero
+		 * value. To save power "wfe" is called. Thus call "sev" here to
+		 * wake the CPUs and let them check the spin-table again (see
+		 * slave_cpu loop in lowlevel.S)
+		 */
+		asm volatile("sev");
+	} else {
+		/* Use PSCI to kick the core */
+		struct pt_regs regs;
+
+		printf("begin to kick cpu core #%d to address %llx\n",
+				nr, boot_addr);
+		regs.regs[0] = PSCI_0_2_FN64_CPU_ON;
+		regs.regs[1] = nr;
+		regs.regs[2] = boot_addr;
+		regs.regs[3] = 0;
+		smc_call(&regs);
+		if (regs.regs[0])
+			return -1;
+	}
 
 	return 0;
 }
-- 
2.17.1