MIPS: smp-cps: Allow booting of CPU other than VP0 within a core

[cascardo/linux.git] / arch / mips / kernel / smp-cps.c

diff --git a/arch/mips/kernel/smp-cps.c b/arch/mips/kernel/smp-cps.c
index 1061bd2..006e99d 100644 (file)
--- a/arch/mips/kernel/smp-cps.c
+++ b/arch/mips/kernel/smp-cps.c
@@ -206,7 +206,7 @@ err_out:
        }
 }
 
-static void boot_core(unsigned core)
+static void boot_core(unsigned int core, unsigned int vpe_id)
 {
        u32 access, stat, seq_state;
        unsigned timeout;
@@ -233,8 +233,9 @@ static void boot_core(unsigned core)
                mips_cpc_lock_other(core);
 
                if (mips_cm_revision() >= CM_REV_CM3) {
-                       /* Run VP0 following the reset */
-                       write_cpc_co_vp_run(0x1);
+                       /* Run only the requested VP following the reset */
+                       write_cpc_co_vp_stop(0xf);
+                       write_cpc_co_vp_run(1 << vpe_id);
 
                        /*
                         * Ensure that the VP_RUN register is written before the
@@ -306,7 +307,7 @@ static void cps_boot_secondary(int cpu, struct task_struct *idle)
 
        if (!test_bit(core, core_power)) {
                /* Boot a VPE on a powered down core */
-               boot_core(core);
+               boot_core(core, vpe_id);
                goto out;
        }
 
@@ -359,8 +360,12 @@ static void cps_init_secondary(void)
                BUG_ON(ident != mips_cm_vp_id(smp_processor_id()));
        }
 
-       change_c0_status(ST0_IM, STATUSF_IP2 | STATUSF_IP3 | STATUSF_IP4 |
-                                STATUSF_IP5 | STATUSF_IP6 | STATUSF_IP7);
+       if (cpu_has_veic)
+               clear_c0_status(ST0_IM);
+       else
+               change_c0_status(ST0_IM, STATUSF_IP2 | STATUSF_IP3 |
+                                        STATUSF_IP4 | STATUSF_IP5 |
+                                        STATUSF_IP6 | STATUSF_IP7);
 }
 
 static void cps_smp_finish(void)