2 files changed, 77 insertions, 12 deletions

diff --git a/drivers/ssb/driver_mipscore.c b/drivers/ssb/driver_mipscore.c
index 3fd3e3b412b6..3c6feed46f6e 100644
--- a/drivers/ssb/driver_mipscore.c
+++ b/drivers/ssb/driver_mipscore.c
@@ -49,29 +49,54 @@ static const u32 ipsflag_irq_shift[] = {
 
 static inline u32 ssb_irqflag(struct ssb_device *dev)
 {
-        return ssb_read32(dev, SSB_TPSFLAG) & SSB_TPSFLAG_BPFLAG;
+        u32 tpsflag = ssb_read32(dev, SSB_TPSFLAG);
+        if (tpsflag)
+                return ssb_read32(dev, SSB_TPSFLAG) & SSB_TPSFLAG_BPFLAG;
+        else
+                /* not irq supported */
+                return 0x3f;
+}
+
+static struct ssb_device *find_device(struct ssb_device *rdev, int irqflag)
+{
+        struct ssb_bus *bus = rdev->bus;
+        int i;
+        for (i = 0; i < bus->nr_devices; i++) {
+                struct ssb_device *dev;
+                dev = &(bus->devices[i]);
+                if (ssb_irqflag(dev) == irqflag)
+                        return dev;
+        }
+        return NULL;
 }
 
 /* Get the MIPS IRQ assignment for a specified device.
  * If unassigned, 0 is returned.
+ * If disabled, 5 is returned.
+ * If not supported, 6 is returned.
  */
 unsigned int ssb_mips_irq(struct ssb_device *dev)
 {
         struct ssb_bus *bus = dev->bus;
+        struct ssb_device *mdev = bus->mipscore.dev;
         u32 irqflag;
         u32 ipsflag;
         u32 tmp;
         unsigned int irq;
 
         irqflag = ssb_irqflag(dev);
+        if (irqflag == 0x3f)
+                return 6;
         ipsflag = ssb_read32(bus->mipscore.dev, SSB_IPSFLAG);
         for (irq = 1; irq <= 4; irq++) {
                 tmp = ((ipsflag & ipsflag_irq_mask[irq]) >> ipsflag_irq_shift[irq]);
                 if (tmp == irqflag)
                         break;
         }
-        if (irq == 5)
-                irq = 0;
+        if (irq == 5) {
+                if ((1 << irqflag) & ssb_read32(mdev, SSB_INTVEC))
+                        irq = 0;
+        }
 
         return irq;
 }
@@ -97,25 +122,56 @@ static void set_irq(struct ssb_device *dev, unsigned int irq)
         struct ssb_device *mdev = bus->mipscore.dev;
         u32 irqflag = ssb_irqflag(dev);
 
+        BUG_ON(oldirq == 6);
+
         dev->irq = irq + 2;
 
-        ssb_dprintk(KERN_INFO PFX
-                    "set_irq: core 0x%04x, irq %d => %d\n",
-                    dev->id.coreid, oldirq, irq);
         /* clear the old irq */
         if (oldirq == 0)
                 ssb_write32(mdev, SSB_INTVEC, (~(1 << irqflag) & ssb_read32(mdev, SSB_INTVEC)));
-        else
+        else if (oldirq != 5)
                 clear_irq(bus, oldirq);
 
         /* assign the new one */
         if (irq == 0) {
                 ssb_write32(mdev, SSB_INTVEC, ((1 << irqflag) | ssb_read32(mdev, SSB_INTVEC)));
         } else {
+                u32 ipsflag = ssb_read32(mdev, SSB_IPSFLAG);
+                if ((ipsflag & ipsflag_irq_mask[irq]) != ipsflag_irq_mask[irq]) {
+                        u32 oldipsflag = (ipsflag & ipsflag_irq_mask[irq]) >> ipsflag_irq_shift[irq];
+                        struct ssb_device *olddev = find_device(dev, oldipsflag);
+                        if (olddev)
+                                set_irq(olddev, 0);
+                }
                 irqflag <<= ipsflag_irq_shift[irq];
-                irqflag |= (ssb_read32(mdev, SSB_IPSFLAG) & ~ipsflag_irq_mask[irq]);
+                irqflag |= (ipsflag & ~ipsflag_irq_mask[irq]);
                 ssb_write32(mdev, SSB_IPSFLAG, irqflag);
         }
+        ssb_dprintk(KERN_INFO PFX
+                    "set_irq: core 0x%04x, irq %d => %d\n",
+                    dev->id.coreid, oldirq+2, irq+2);
+}
+
+static void print_irq(struct ssb_device *dev, unsigned int irq)
+{
+        int i;
+        static const char *irq_name[] = {"2(S)", "3", "4", "5", "6", "D", "I"};
+        ssb_dprintk(KERN_INFO PFX
+                "core 0x%04x, irq :", dev->id.coreid);
+        for (i = 0; i <= 6; i++) {
+                ssb_dprintk(" %s%s", irq_name[i], i==irq?"*":" ");
+        }
+        ssb_dprintk("\n");
+}
+
+static void dump_irq(struct ssb_bus *bus)
+{
+        int i;
+        for (i = 0; i < bus->nr_devices; i++) {
+                struct ssb_device *dev;
+                dev = &(bus->devices[i]);
+                print_irq(dev, ssb_mips_irq(dev));
+        }
 }
 
 static void ssb_mips_serial_init(struct ssb_mipscore *mcore)
@@ -197,16 +253,23 @@ void ssb_mipscore_init(struct ssb_mipscore *mcore)
 
         /* Assign IRQs to all cores on the bus, start with irq line 2, because serial usually takes 1 */
         for (irq = 2, i = 0; i < bus->nr_devices; i++) {
+                int mips_irq;
                 dev = &(bus->devices[i]);
-                dev->irq = ssb_mips_irq(dev) + 2;
+                mips_irq = ssb_mips_irq(dev);
+                if (mips_irq > 4)
+                        dev->irq = 0;
+                else
+                        dev->irq = mips_irq + 2;
+                if (dev->irq > 5)
+                        continue;
                 switch (dev->id.coreid) {
                 case SSB_DEV_USB11_HOST:
                         /* shouldn't need a separate irq line for non-4710, most of them have a proper
                          * external usb controller on the pci */
                         if ((bus->chip_id == 0x4710) && (irq <= 4)) {
                                 set_irq(dev, irq++);
-                                break;
                         }
+                        break;
                         /* fallthrough */
                 case SSB_DEV_PCI:
                 case SSB_DEV_ETHERNET:
@@ -220,6 +283,8 @@ void ssb_mipscore_init(struct ssb_mipscore *mcore)
                         }
                 }
         }
+        ssb_dprintk(KERN_INFO PFX "after irq reconfiguration\n");
+        dump_irq(bus);
 
         ssb_mips_serial_init(mcore);
         ssb_mips_flash_detect(mcore);
diff --git a/drivers/ssb/pcmcia.c b/drivers/ssb/pcmcia.c
index d288608d2206..100e7a5c5ea1 100644
--- a/drivers/ssb/pcmcia.c
+++ b/drivers/ssb/pcmcia.c
@@ -583,7 +583,7 @@ static int ssb_pcmcia_sprom_write_all(struct ssb_bus *bus, const u16 *sprom)
                         ssb_printk(".");
                 err = ssb_pcmcia_sprom_write(bus, i, sprom[i]);
                 if (err) {
-                        ssb_printk("\n" KERN_NOTICE PFX
+                        ssb_printk(KERN_NOTICE PFX
                                    "Failed to write to SPROM.\n");
                         failed = 1;
                         break;
@@ -591,7 +591,7 @@ static int ssb_pcmcia_sprom_write_all(struct ssb_bus *bus, const u16 *sprom)
         }
         err = ssb_pcmcia_sprom_command(bus, SSB_PCMCIA_SPROMCTL_WRITEDIS);
         if (err) {
-                ssb_printk("\n" KERN_NOTICE PFX
+                ssb_printk(KERN_NOTICE PFX
                            "Could not disable SPROM write access.\n");
                 failed = 1;
         }