Merge branch 'master' of git://git.denx.de/u-boot-uniphier

[platform/kernel/u-boot.git] / arch / arm / cpu / armv8 / fsl-layerscape / mp.c

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/mp.c b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
index 0d600db..f607c39 100644 (file)
--- a/arch/arm/cpu/armv8/fsl-layerscape/mp.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
@@ -104,6 +104,11 @@ int is_core_valid(unsigned int core)
        return !!((1 << core) & cpu_mask());
 }
 
+static int is_pos_valid(unsigned int pos)
+{
+       return !!((1 << pos) & cpu_pos_mask());
+}
+
 int is_core_online(u64 cpu_id)
 {
        u64 *table;
@@ -126,9 +131,9 @@ int cpu_disable(int nr)
        return 0;
 }
 
-int core_to_pos(int nr)
+static int core_to_pos(int nr)
 {
-       u32 cores = cpu_mask();
+       u32 cores = cpu_pos_mask();
        int i, count = 0;
 
        if (nr == 0) {
@@ -139,14 +144,17 @@ int core_to_pos(int nr)
        }
 
        for (i = 1; i < 32; i++) {
-               if (is_core_valid(i)) {
+               if (is_pos_valid(i)) {
                        count++;
                        if (count == nr)
                                break;
                }
        }
 
-       return count;
+       if (count != nr)
+               return -1;
+
+       return i;
 }
 
 int cpu_status(int nr)
@@ -192,6 +200,12 @@ int cpu_release(int nr, int argc, char * const argv[])
                           (unsigned long)table + SPIN_TABLE_ELEM_SIZE);
        asm volatile("dsb st");
        smp_kick_all_cpus();    /* only those with entry addr set will run */
+       /*
+        * When the first release command runs, all cores are set to go. Those
+        * without a valid entry address will be trapped by "wfe". "sev" kicks
+        * them off to check the address again. When set, they continue to run.
+        */
+       asm volatile("sev");
 
        return 0;
 }