armv8: layerscape: add PSCI support for cpu release

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@nxp.com>
[Fixed checkpatch alignment CHECKs]
Signed-off-by: Priyanka Jain <priyanka.jain@nxp.com>

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 270a72e550c..d0103fc8811 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 dca5fd0f7dc..45da95831e6 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 5ac545f9df8..730d7663d0f 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;
 }