From 84c2e044a9372945c5ef6b83ab49f9cb2ba74c0d Mon Sep 17 00:00:00 2001
From: Jiafei Pan <Jiafei.Pan@nxp.com>
Date: Wed, 21 Apr 2021 12:12:49 +0800
Subject: [PATCH] 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>
---
 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..730d7663d0 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.39.5