2 files changed, 13 insertions, 3 deletions

diff --git a/arch/mips/kernel/bmips_vec.S b/arch/mips/kernel/bmips_vec.S
index f739aedcb509..bd79c4f9bff4 100644
--- a/arch/mips/kernel/bmips_vec.S
+++ b/arch/mips/kernel/bmips_vec.S
@@ -54,7 +54,11 @@ LEAF(bmips_smp_movevec)
         /* set up CPU1 CBR; move BASE to 0xa000_0000 */
         li      k0, 0xff400000
         mtc0    k0, $22, 6
-        li      k1, CKSEG1 | BMIPS_RELO_VECTOR_CONTROL_1
+        /* set up relocation vector address based on thread ID */
+        mfc0    k1, $22, 3
+        srl     k1, 16
+        andi    k1, 0x8000
+        or      k1, CKSEG1 | BMIPS_RELO_VECTOR_CONTROL_0
         or      k0, k1
         li      k1, 0xa0080000
         sw      k1, 0(k0)
diff --git a/arch/mips/kernel/smp-bmips.c b/arch/mips/kernel/smp-bmips.c
index 89417c9c6aca..159abc8842d2 100644
--- a/arch/mips/kernel/smp-bmips.c
+++ b/arch/mips/kernel/smp-bmips.c
@@ -196,9 +196,15 @@ static void bmips_init_secondary(void)
 #if defined(CONFIG_CPU_BMIPS4350) || defined(CONFIG_CPU_BMIPS4380)
         void __iomem *cbr = BMIPS_GET_CBR();
         unsigned long old_vec;
+        unsigned long relo_vector;
+        int boot_cpu;
 
-        old_vec = __raw_readl(cbr + BMIPS_RELO_VECTOR_CONTROL_1);
-        __raw_writel(old_vec & ~0x20000000, cbr + BMIPS_RELO_VECTOR_CONTROL_1);
+        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);
 #elif defined(CONFIG_CPU_BMIPS5000)