1 files changed, 25 insertions, 10 deletions

diff --git a/arch/mips/netlogic/xlp/wakeup.c b/arch/mips/netlogic/xlp/wakeup.c
index cb9010642ac3..abb3e08cc052 100644
--- a/arch/mips/netlogic/xlp/wakeup.c
+++ b/arch/mips/netlogic/xlp/wakeup.c
@@ -51,7 +51,7 @@
 #include <asm/netlogic/xlp-hal/xlp.h>
 #include <asm/netlogic/xlp-hal/sys.h>
 
-static int xlp_wakeup_core(uint64_t sysbase, int core)
+static int xlp_wakeup_core(uint64_t sysbase, int node, int core)
 {
         uint32_t coremask, value;
         int count;
@@ -82,36 +82,51 @@ static void xlp_enable_secondary_cores(const cpumask_t *wakeup_mask)
         struct nlm_soc_info *nodep;
         uint64_t syspcibase;
         uint32_t syscoremask;
-        int core, n, cpu;
+        int core, n, cpu, count, val;
 
         for (n = 0; n < NLM_NR_NODES; n++) {
                 syspcibase = nlm_get_sys_pcibase(n);
                 if (nlm_read_reg(syspcibase, 0) == 0xffffffff)
                         break;
 
-                /* read cores in reset from SYS and account for boot cpu */
-                nlm_node_init(n);
+                /* read cores in reset from SYS */
+                if (n != 0)
+                        nlm_node_init(n);
                 nodep = nlm_get_node(n);
                 syscoremask = nlm_read_sys_reg(nodep->sysbase, SYS_CPU_RESET);
-                if (n == 0)
+                /* The boot cpu */
+                if (n == 0) {
                         syscoremask |= 1;
+                        nodep->coremask = 1;
+                }
 
                 for (core = 0; core < NLM_CORES_PER_NODE; core++) {
+                        /* we will be on node 0 core 0 */
+                        if (n == 0 && core == 0)
+                                continue;
+
                         /* see if the core exists */
                         if ((syscoremask & (1 << core)) == 0)
                                 continue;
 
-                        /* see if at least the first thread is enabled */
+                        /* see if at least the first hw thread is enabled */
                         cpu = (n * NLM_CORES_PER_NODE + core)
                                                 * NLM_THREADS_PER_CORE;
                         if (!cpumask_test_cpu(cpu, wakeup_mask))
                                 continue;
 
                         /* wake up the core */
-                        if (xlp_wakeup_core(nodep->sysbase, core))
-                                nodep->coremask |= 1u << core;
-                        else
-                                pr_err("Failed to enable core %d\n", core);
+                        if (!xlp_wakeup_core(nodep->sysbase, n, core))
+                                continue;
+
+                        /* core is up */
+                        nodep->coremask |= 1u << core;
+
+                        /* spin until the first hw thread sets its ready */
+                        count = 0x20000000;
+                        do {
+                                val = *(volatile int *)&nlm_cpu_ready[cpu];
+                        } while (val == 0 && --count > 0);
                 }
         }
 }