[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....@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