1 files changed, 47 insertions, 9 deletions

diff --git a/arch/mips/pmc-sierra/msp71xx/msp_irq.c b/arch/mips/pmc-sierra/msp71xx/msp_irq.c
index 734d598a2e3a..4531c4a514bc 100644
--- a/arch/mips/pmc-sierra/msp71xx/msp_irq.c
+++ b/arch/mips/pmc-sierra/msp71xx/msp_irq.c
@@ -19,8 +19,6 @@
 
 #include <msp_int.h>
 
-extern void msp_int_handle(void);
-
 /* SLP bases systems */
 extern void msp_slp_irq_init(void);
 extern void msp_slp_irq_dispatch(void);
@@ -29,6 +27,18 @@ extern void msp_slp_irq_dispatch(void);
 extern void msp_cic_irq_init(void);
 extern void msp_cic_irq_dispatch(void);
 
+/* VSMP support init */
+extern void msp_vsmp_int_init(void);
+
+/* vectored interrupt implementation */
+
+/* SW0/1 interrupts are used for SMP/SMTC */
+static inline void mac0_int_dispatch(void) { do_IRQ(MSP_INT_MAC0); }
+static inline void mac1_int_dispatch(void) { do_IRQ(MSP_INT_MAC1); }
+static inline void mac2_int_dispatch(void) { do_IRQ(MSP_INT_SAR); }
+static inline void usb_int_dispatch(void)  { do_IRQ(MSP_INT_USB);  }
+static inline void sec_int_dispatch(void)  { do_IRQ(MSP_INT_SEC);  }
+
 /*
  * The PMC-Sierra MSP interrupts are arranged in a 3 level cascaded
  * hierarchical system.  The first level are the direct MIPS interrupts
@@ -96,29 +106,57 @@ asmlinkage void plat_irq_dispatch(struct pt_regs *regs)
                 do_IRQ(MSP_INT_SW1);
 }
 
-static struct irqaction cascade_msp = {
+static struct irqaction cic_cascade_msp = {
         .handler = no_action,
-        .name    = "MSP cascade"
+        .name    = "MSP CIC cascade"
 };
 
+static struct irqaction per_cascade_msp = {
+        .handler = no_action,
+        .name    = "MSP PER cascade"
+};
 
 void __init arch_init_irq(void)
 {
+        /* assume we'll be using vectored interrupt mode except in UP mode*/
+#ifdef CONFIG_MIPS_MT
+        BUG_ON(!cpu_has_vint);
+#endif
         /* initialize the 1st-level CPU based interrupt controller */
         mips_cpu_irq_init();
 
 #ifdef CONFIG_IRQ_MSP_CIC
         msp_cic_irq_init();
-
+#ifdef CONFIG_MIPS_MT
+        set_vi_handler(MSP_INT_CIC, msp_cic_irq_dispatch);
+        set_vi_handler(MSP_INT_MAC0, mac0_int_dispatch);
+        set_vi_handler(MSP_INT_MAC1, mac1_int_dispatch);
+        set_vi_handler(MSP_INT_SAR, mac2_int_dispatch);
+        set_vi_handler(MSP_INT_USB, usb_int_dispatch);
+        set_vi_handler(MSP_INT_SEC, sec_int_dispatch);
+#ifdef CONFIG_MIPS_MT_SMP
+        msp_vsmp_int_init();
+#elif defined CONFIG_MIPS_MT_SMTC
+        /*Set hwmask for all platform devices */
+        irq_hwmask[MSP_INT_MAC0] = C_IRQ0;
+        irq_hwmask[MSP_INT_MAC1] = C_IRQ1;
+        irq_hwmask[MSP_INT_USB] = C_IRQ2;
+        irq_hwmask[MSP_INT_SAR] = C_IRQ3;
+        irq_hwmask[MSP_INT_SEC] = C_IRQ5;
+
+#endif  /* CONFIG_MIPS_MT_SMP */
+#endif  /* CONFIG_MIPS_MT */
         /* setup the cascaded interrupts */
-        setup_irq(MSP_INT_CIC, &cascade_msp);
-        setup_irq(MSP_INT_PER, &cascade_msp);
+        setup_irq(MSP_INT_CIC, &cic_cascade_msp);
+        setup_irq(MSP_INT_PER, &per_cascade_msp);
+
 #else
         /* setup the 2nd-level SLP register based interrupt controller */
+        /* VSMP /SMTC support support is not enabled for SLP */
         msp_slp_irq_init();
 
         /* setup the cascaded SLP/PER interrupts */
-        setup_irq(MSP_INT_SLP, &cascade_msp);
-        setup_irq(MSP_INT_PER, &cascade_msp);
+        setup_irq(MSP_INT_SLP, &cic_cascade_msp);
+        setup_irq(MSP_INT_PER, &per_cascade_msp);
 #endif
 }