rapidio/tsi721: Fix interrupt mask when handling MSI

[pandora-kernel.git] / drivers / rapidio / devices / tsi721.c

diff --git a/drivers/rapidio/devices/tsi721.c b/drivers/rapidio/devices/tsi721.c
index 691b1ab..84eab3f 100644 (file)
--- a/drivers/rapidio/devices/tsi721.c
+++ b/drivers/rapidio/devices/tsi721.c
@@ -410,13 +410,14 @@ static void tsi721_db_dpc(struct work_struct *work)
         */
        mport = priv->mport;
 
-       wr_ptr = ioread32(priv->regs + TSI721_IDQ_WP(IDB_QUEUE));
-       rd_ptr = ioread32(priv->regs + TSI721_IDQ_RP(IDB_QUEUE));
+       wr_ptr = ioread32(priv->regs + TSI721_IDQ_WP(IDB_QUEUE)) % IDB_QSIZE;
+       rd_ptr = ioread32(priv->regs + TSI721_IDQ_RP(IDB_QUEUE)) % IDB_QSIZE;
 
        while (wr_ptr != rd_ptr) {
                idb_entry = (u64 *)(priv->idb_base +
                                        (TSI721_IDB_ENTRY_SIZE * rd_ptr));
                rd_ptr++;
+               rd_ptr %= IDB_QSIZE;
                idb.msg = *idb_entry;
                *idb_entry = 0;
 
@@ -438,6 +439,9 @@ static void tsi721_db_dpc(struct work_struct *work)
                                " info %4.4x\n", DBELL_SID(idb.bytes),
                                DBELL_TID(idb.bytes), DBELL_INF(idb.bytes));
                }
+
+               wr_ptr = ioread32(priv->regs +
+                                 TSI721_IDQ_WP(IDB_QUEUE)) % IDB_QSIZE;
        }
 
        iowrite32(rd_ptr & (IDB_QSIZE - 1),
@@ -448,6 +452,10 @@ static void tsi721_db_dpc(struct work_struct *work)
        regval |= TSI721_SR_CHINT_IDBQRCV;
        iowrite32(regval,
                priv->regs + TSI721_SR_CHINTE(IDB_QUEUE));
+
+       wr_ptr = ioread32(priv->regs + TSI721_IDQ_WP(IDB_QUEUE)) % IDB_QSIZE;
+       if (wr_ptr != rd_ptr)
+               schedule_work(&priv->idb_work);
 }
 
 /**
@@ -467,6 +475,10 @@ static irqreturn_t tsi721_irqhandler(int irq, void *ptr)
        u32 intval;
        u32 ch_inte;
 
+       /* For MSI mode disable all device-level interrupts */
+       if (priv->flags & TSI721_USING_MSI)
+               iowrite32(0, priv->regs + TSI721_DEV_INTE);
+
        dev_int = ioread32(priv->regs + TSI721_DEV_INT);
        if (!dev_int)
                return IRQ_NONE;
@@ -540,6 +552,13 @@ static irqreturn_t tsi721_irqhandler(int irq, void *ptr)
                        tsi721_pw_handler(mport);
        }
 
+       /* For MSI mode re-enable device-level interrupts */
+       if (priv->flags & TSI721_USING_MSI) {
+               dev_int = TSI721_DEV_INT_SR2PC_CH | TSI721_DEV_INT_SRIO |
+                       TSI721_DEV_INT_SMSG_CH;
+               iowrite32(dev_int, priv->regs + TSI721_DEV_INTE);
+       }
+
        return IRQ_HANDLED;
 }
 
@@ -2154,7 +2173,7 @@ static int __devinit tsi721_probe(struct pci_dev *pdev,
                                  const struct pci_device_id *id)
 {
        struct tsi721_device *priv;
-       int i, cap;
+       int cap;
        int err;
        u32 regval;
 
@@ -2174,12 +2193,15 @@ static int __devinit tsi721_probe(struct pci_dev *pdev,
        priv->pdev = pdev;
 
 #ifdef DEBUG
+       {
+       int i;
        for (i = 0; i <= PCI_STD_RESOURCE_END; i++) {
                dev_dbg(&pdev->dev, "res[%d] @ 0x%llx (0x%lx, 0x%lx)\n",
                        i, (unsigned long long)pci_resource_start(pdev, i),
                        (unsigned long)pci_resource_len(pdev, i),
                        pci_resource_flags(pdev, i));
        }
+       }
 #endif
        /*
         * Verify BAR configuration