1 files changed, 73 insertions, 41 deletions

diff --git a/arch/mips/kernel/smp-bmips.c b/arch/mips/kernel/smp-bmips.c
index 06bb5ed6d80a..b8bd9340c9c7 100644
--- a/arch/mips/kernel/smp-bmips.c
+++ b/arch/mips/kernel/smp-bmips.c
@@ -35,6 +35,7 @@
 #include <asm/bmips.h>
 #include <asm/traps.h>
 #include <asm/barrier.h>
+#include <asm/cpu-features.h>
 
 static int __maybe_unused max_cpus = 1;
 
@@ -42,6 +43,12 @@ static int __maybe_unused max_cpus = 1;
 int bmips_smp_enabled = 1;
 int bmips_cpu_offset;
 cpumask_t bmips_booted_mask;
+unsigned long bmips_tp1_irqs = IE_IRQ1;
+
+#define RESET_FROM_KSEG0                0x80080800
+#define RESET_FROM_KSEG1                0xa0080800
+
+static void bmips_set_reset_vec(int cpu, u32 val);
 
 #ifdef CONFIG_SMP
 
@@ -194,6 +201,9 @@ static void bmips_boot_secondary(int cpu, struct task_struct *idle)
         pr_info("SMP: Booting CPU%d...\n", cpu);
 
         if (cpumask_test_cpu(cpu, &bmips_booted_mask)) {
+                /* kseg1 might not exist if this CPU enabled XKS01 */
+                bmips_set_reset_vec(cpu, RESET_FROM_KSEG0);
+
                 switch (current_cpu_type()) {
                 case CPU_BMIPS4350:
                 case CPU_BMIPS4380:
@@ -203,8 +213,9 @@ static void bmips_boot_secondary(int cpu, struct task_struct *idle)
                         bmips5000_send_ipi_single(cpu, 0);
                         break;
                 }
-        }
-        else {
+        } else {
+                bmips_set_reset_vec(cpu, RESET_FROM_KSEG1);
+
                 switch (current_cpu_type()) {
                 case CPU_BMIPS4350:
                 case CPU_BMIPS4380:
@@ -213,17 +224,7 @@ static void bmips_boot_secondary(int cpu, struct task_struct *idle)
                                 set_c0_brcm_cmt_ctrl(0x01);
                         break;
                 case CPU_BMIPS5000:
-                        if (cpu & 0x01)
-                                write_c0_brcm_action(ACTION_BOOT_THREAD(cpu));
-                        else {
-                                /*
-                                 * core N thread 0 was already booted; just
-                                 * pulse the NMI line
-                                 */
-                                bmips_write_zscm_reg(0x210, 0xc0000000);
-                                udelay(10);
-                                bmips_write_zscm_reg(0x210, 0x00);
-                        }
+                        write_c0_brcm_action(ACTION_BOOT_THREAD(cpu));
                         break;
                 }
                 cpumask_set_cpu(cpu, &bmips_booted_mask);
@@ -235,31 +236,12 @@ static void bmips_boot_secondary(int cpu, struct task_struct *idle)
  */
 static void bmips_init_secondary(void)
 {
-        /* move NMI vector to kseg0, in case XKS01 is enabled */
-
-        void __iomem *cbr;
-        unsigned long old_vec;
-        unsigned long relo_vector;
-        int boot_cpu;
-
         switch (current_cpu_type()) {
         case CPU_BMIPS4350:
         case CPU_BMIPS4380:
-                cbr = BMIPS_GET_CBR();
-
-                boot_cpu = !!(read_c0_brcm_cmt_local() & (1 << 31));
-                relo_vector = boot_cpu ? BMIPS_RELO_VECTOR_CONTROL_0 :
-                                  BMIPS_RELO_VECTOR_CONTROL_1;
-
-                old_vec = __raw_readl(cbr + relo_vector);
-                __raw_writel(old_vec & ~0x20000000, cbr + relo_vector);
-
                 clear_c0_cause(smp_processor_id() ? C_SW1 : C_SW0);
                 break;
         case CPU_BMIPS5000:
-                write_c0_brcm_bootvec(read_c0_brcm_bootvec() &
-                        (smp_processor_id() & 0x01 ? ~0x20000000 : ~0x2000));
-
                 write_c0_brcm_action(ACTION_CLR_IPI(smp_processor_id(), 0));
                 break;
         }
@@ -276,7 +258,7 @@ static void bmips_smp_finish(void)
         write_c0_compare(read_c0_count() + mips_hpt_frequency / HZ);
 
         irq_enable_hazard();
-        set_c0_status(IE_SW0 | IE_SW1 | IE_IRQ1 | IE_IRQ5 | ST0_IE);
+        set_c0_status(IE_SW0 | IE_SW1 | bmips_tp1_irqs | IE_IRQ5 | ST0_IE);
         irq_enable_hazard();
 }
 
@@ -381,6 +363,7 @@ static int bmips_cpu_disable(void)
 
         set_cpu_online(cpu, false);
         cpu_clear(cpu, cpu_callin_map);
+        clear_c0_status(IE_IRQ5);
 
         local_flush_tlb_all();
         local_flush_icache_range(0, ~0);
@@ -405,7 +388,8 @@ void __ref play_dead(void)
          * IRQ handlers; this clears ST0_IE and returns immediately.
          */
         clear_c0_cause(CAUSEF_IV | C_SW0 | C_SW1);
-        change_c0_status(IE_IRQ5 | IE_IRQ1 | IE_SW0 | IE_SW1 | ST0_IE | ST0_BEV,
+        change_c0_status(
+                IE_IRQ5 | bmips_tp1_irqs | IE_SW0 | IE_SW1 | ST0_IE | ST0_BEV,
                 IE_SW0 | IE_SW1 | ST0_IE | ST0_BEV);
         irq_disable_hazard();
 
@@ -473,10 +457,61 @@ static inline void bmips_nmi_handler_setup(void)
                 &bmips_smp_int_vec_end);
 }
 
+struct reset_vec_info {
+        int cpu;
+        u32 val;
+};
+
+static void bmips_set_reset_vec_remote(void *vinfo)
+{
+        struct reset_vec_info *info = vinfo;
+        int shift = info->cpu & 0x01 ? 16 : 0;
+        u32 mask = ~(0xffff << shift), val = info->val >> 16;
+
+        preempt_disable();
+        if (smp_processor_id() > 0) {
+                smp_call_function_single(0, &bmips_set_reset_vec_remote,
+                                         info, 1);
+        } else {
+                if (info->cpu & 0x02) {
+                        /* BMIPS5200 "should" use mask/shift, but it's buggy */
+                        bmips_write_zscm_reg(0xa0, (val << 16) | val);
+                        bmips_read_zscm_reg(0xa0);
+                } else {
+                        write_c0_brcm_bootvec((read_c0_brcm_bootvec() & mask) |
+                                              (val << shift));
+                }
+        }
+        preempt_enable();
+}
+
+static void bmips_set_reset_vec(int cpu, u32 val)
+{
+        struct reset_vec_info info;
+
+        if (current_cpu_type() == CPU_BMIPS5000) {
+                /* this needs to run from CPU0 (which is always online) */
+                info.cpu = cpu;
+                info.val = val;
+                bmips_set_reset_vec_remote(&info);
+        } else {
+                void __iomem *cbr = BMIPS_GET_CBR();
+
+                if (cpu == 0)
+                        __raw_writel(val, cbr + BMIPS_RELO_VECTOR_CONTROL_0);
+                else {
+                        if (current_cpu_type() != CPU_BMIPS4380)
+                                return;
+                        __raw_writel(val, cbr + BMIPS_RELO_VECTOR_CONTROL_1);
+                }
+        }
+        __sync();
+        back_to_back_c0_hazard();
+}
+
 void bmips_ebase_setup(void)
 {
         unsigned long new_ebase = ebase;
-        void __iomem __maybe_unused *cbr;
 
         BUG_ON(ebase != CKSEG0);
 
@@ -496,15 +531,14 @@ void bmips_ebase_setup(void)
                         &bmips_smp_int_vec, 0x80);
                 __sync();
                 return;
+        case CPU_BMIPS3300:
         case CPU_BMIPS4380:
                 /*
                  * 0x8000_0000: reset/NMI (initially in kseg1)
                  * 0x8000_0400: normal vectors
                  */
                 new_ebase = 0x80000400;
-                cbr = BMIPS_GET_CBR();
-                __raw_writel(0x80080800, cbr + BMIPS_RELO_VECTOR_CONTROL_0);
-                __raw_writel(0xa0080800, cbr + BMIPS_RELO_VECTOR_CONTROL_1);
+                bmips_set_reset_vec(0, RESET_FROM_KSEG0);
                 break;
         case CPU_BMIPS5000:
                 /*
@@ -512,10 +546,8 @@ void bmips_ebase_setup(void)
                  * 0x8000_1000: normal vectors
                  */
                 new_ebase = 0x80001000;
-                write_c0_brcm_bootvec(0xa0088008);
+                bmips_set_reset_vec(0, RESET_FROM_KSEG0);
                 write_c0_ebase(new_ebase);
-                if (max_cpus > 2)
-                        bmips_write_zscm_reg(0xa0, 0xa008a008);
                 break;
         default:
                 return;