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 #include -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); } } }