1 files changed, 55 insertions, 28 deletions

diff --git a/arch/mips/netlogic/xlp/wakeup.c b/arch/mips/netlogic/xlp/wakeup.c
index 44d923ff3846..cb9010642ac3 100644
--- a/arch/mips/netlogic/xlp/wakeup.c
+++ b/arch/mips/netlogic/xlp/wakeup.c
@@ -51,45 +51,72 @@
 #include <asm/netlogic/xlp-hal/xlp.h>
 #include <asm/netlogic/xlp-hal/sys.h>
 
-static void xlp_enable_secondary_cores(void)
+static int xlp_wakeup_core(uint64_t sysbase, int core)
 {
-        uint32_t core, value, coremask, syscoremask;
+        uint32_t coremask, value;
         int count;
 
-        /* read cores in reset from SYS block */
-        syscoremask = nlm_read_sys_reg(nlm_sys_base, SYS_CPU_RESET);
+        coremask = (1 << core);
 
-        /* update user specified */
-        nlm_coremask = nlm_coremask & (syscoremask | 1);
+        /* Enable CPU clock */
+        value = nlm_read_sys_reg(sysbase, SYS_CORE_DFS_DIS_CTRL);
+        value &= ~coremask;
+        nlm_write_sys_reg(sysbase, SYS_CORE_DFS_DIS_CTRL, value);
 
-        for (core = 1; core < 8; core++) {
-                coremask = 1 << core;
-                if ((nlm_coremask & coremask) == 0)
-                        continue;
+        /* Remove CPU Reset */
+        value = nlm_read_sys_reg(sysbase, SYS_CPU_RESET);
+        value &= ~coremask;
+        nlm_write_sys_reg(sysbase, SYS_CPU_RESET, value);
 
-                /* Enable CPU clock */
-                value = nlm_read_sys_reg(nlm_sys_base, SYS_CORE_DFS_DIS_CTRL);
-                value &= ~coremask;
-                nlm_write_sys_reg(nlm_sys_base, SYS_CORE_DFS_DIS_CTRL, value);
+        /* Poll for CPU to mark itself coherent */
+        count = 100000;
+        do {
+                value = nlm_read_sys_reg(sysbase, SYS_CPU_NONCOHERENT_MODE);
+        } while ((value & coremask) != 0 && --count > 0);
 
-                /* Remove CPU Reset */
-                value = nlm_read_sys_reg(nlm_sys_base, SYS_CPU_RESET);
-                value &= ~coremask;
-                nlm_write_sys_reg(nlm_sys_base, SYS_CPU_RESET, value);
+        return count != 0;
+}
+
+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;
+
+        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);
+                nodep = nlm_get_node(n);
+                syscoremask = nlm_read_sys_reg(nodep->sysbase, SYS_CPU_RESET);
+                if (n == 0)
+                        syscoremask |= 1;
+
+                for (core = 0; core < NLM_CORES_PER_NODE; core++) {
+                        /* see if the core exists */
+                        if ((syscoremask & (1 << core)) == 0)
+                                continue;
 
-                /* Poll for CPU to mark itself coherent */
-                count = 100000;
-                do {
-                        value = nlm_read_sys_reg(nlm_sys_base,
-                            SYS_CPU_NONCOHERENT_MODE);
-                } while ((value & coremask) != 0 && count-- > 0);
+                        /* see if at least the first thread is enabled */
+                        cpu = (n * NLM_CORES_PER_NODE + core)
+                                                * NLM_THREADS_PER_CORE;
+                        if (!cpumask_test_cpu(cpu, wakeup_mask))
+                                continue;
 
-                if (count == 0)
-                        pr_err("Failed to enable core %d\n", core);
+                        /* 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);
+                }
         }
 }
 
-void xlp_wakeup_secondary_cpus(void)
+void xlp_wakeup_secondary_cpus()
 {
         /*
          * In case of u-boot, the secondaries are in reset
@@ -98,5 +125,5 @@ void xlp_wakeup_secondary_cpus(void)
         xlp_boot_core0_siblings();
 
         /* now get other cores out of reset */
-        xlp_enable_secondary_cores();
+        xlp_enable_secondary_cores(&nlm_cpumask);
 }