1 files changed, 25 insertions, 10 deletions

diff --git a/arch/mips/netlogic/xlp/wakeup.c b/arch/mips/netlogic/xlp/wakeup.c
index cb90106..abb3e08 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);
 		}
 	}
 }