1 files changed, 51 insertions, 49 deletions

diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 362773247fbf..d9f06fbfa0bf 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -75,22 +75,17 @@ void arch_teardown_msi_irqs(struct pci_dev *dev)
 }
 #endif
 
-static void __msi_set_enable(struct pci_dev *dev, int pos, int enable)
+static void msi_set_enable(struct pci_dev *dev, int pos, int enable)
 {
         u16 control;
 
-        if (pos) {
-                pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
-                control &= ~PCI_MSI_FLAGS_ENABLE;
-                if (enable)
-                        control |= PCI_MSI_FLAGS_ENABLE;
-                pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
-        }
-}
+        BUG_ON(!pos);
 
-static void msi_set_enable(struct pci_dev *dev, int enable)
-{
-        __msi_set_enable(dev, pci_find_capability(dev, PCI_CAP_ID_MSI), enable);
+        pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
+        control &= ~PCI_MSI_FLAGS_ENABLE;
+        if (enable)
+                control |= PCI_MSI_FLAGS_ENABLE;
+        pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
 }
 
 static void msix_set_enable(struct pci_dev *dev, int enable)
@@ -131,9 +126,6 @@ static inline __attribute_const__ u32 msi_enabled_mask(u16 control)
  * mask all MSI interrupts by clearing the MSI enable bit does not work
  * reliably as devices without an INTx disable bit will then generate a
  * level IRQ which will never be cleared.
- *
- * Returns 1 if it succeeded in masking the interrupt and 0 if the device
- * doesn't support MSI masking.
  */
 static void msi_mask_irq(struct msi_desc *desc, u32 mask, u32 flag)
 {
@@ -303,7 +295,7 @@ static void __pci_restore_msi_state(struct pci_dev *dev)
         pos = entry->msi_attrib.pos;
 
         pci_intx_for_msi(dev, 0);
-        msi_set_enable(dev, 0);
+        msi_set_enable(dev, pos, 0);
         write_msi_msg(dev->irq, &entry->msg);
 
         pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
@@ -321,22 +313,22 @@ static void __pci_restore_msix_state(struct pci_dev *dev)
 
         if (!dev->msix_enabled)
                 return;
+        BUG_ON(list_empty(&dev->msi_list));
+        entry = list_entry(dev->msi_list.next, struct msi_desc, list);
+        pos = entry->msi_attrib.pos;
+        pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
 
         /* route the table */
         pci_intx_for_msi(dev, 0);
-        msix_set_enable(dev, 0);
+        control |= PCI_MSIX_FLAGS_ENABLE | PCI_MSIX_FLAGS_MASKALL;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
 
         list_for_each_entry(entry, &dev->msi_list, list) {
                 write_msi_msg(entry->irq, &entry->msg);
                 msix_mask_irq(entry, entry->masked);
         }
 
-        BUG_ON(list_empty(&dev->msi_list));
-        entry = list_entry(dev->msi_list.next, struct msi_desc, list);
-        pos = entry->msi_attrib.pos;
-        pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
         control &= ~PCI_MSIX_FLAGS_MASKALL;
-        control |= PCI_MSIX_FLAGS_ENABLE;
         pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
 }
 
@@ -365,9 +357,9 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
         u16 control;
         unsigned mask;
 
-        msi_set_enable(dev, 0); /* Ensure msi is disabled as I set it up */
-
         pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
+        msi_set_enable(dev, pos, 0);    /* Disable MSI during set up */
+
         pci_read_config_word(dev, msi_control_reg(pos), &control);
         /* MSI Entry Initialization */
         entry = alloc_msi_entry(dev);
@@ -381,7 +373,7 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
         entry->msi_attrib.default_irq = dev->irq;       /* Save IOAPIC IRQ */
         entry->msi_attrib.pos = pos;
 
-        entry->mask_pos = msi_mask_bits_reg(pos, entry->msi_attrib.is_64);
+        entry->mask_pos = msi_mask_reg(pos, entry->msi_attrib.is_64);
         /* All MSIs are unmasked by default, Mask them all */
         if (entry->msi_attrib.maskbit)
                 pci_read_config_dword(dev, entry->mask_pos, &entry->masked);
@@ -399,7 +391,7 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
 
         /* Set MSI enabled bits  */
         pci_intx_for_msi(dev, 0);
-        msi_set_enable(dev, 1);
+        msi_set_enable(dev, pos, 1);
         dev->msi_enabled = 1;
 
         dev->irq = entry->irq;
@@ -427,11 +419,14 @@ static int msix_capability_init(struct pci_dev *dev,
         u8 bir;
         void __iomem *base;
 
-        msix_set_enable(dev, 0);/* Ensure msix is disabled as I set it up */
-
         pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
+        pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
+
+        /* Ensure MSI-X is disabled while it is set up */
+        control &= ~PCI_MSIX_FLAGS_ENABLE;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+
         /* Request & Map MSI-X table region */
-        pci_read_config_word(dev, msi_control_reg(pos), &control);
         nr_entries = multi_msix_capable(control);
 
         pci_read_config_dword(dev, msix_table_offset_reg(pos), &table_offset);
@@ -442,7 +437,6 @@ static int msix_capability_init(struct pci_dev *dev,
         if (base == NULL)
                 return -ENOMEM;
 
-        /* MSI-X Table Initialization */
         for (i = 0; i < nvec; i++) {
                 entry = alloc_msi_entry(dev);
                 if (!entry)
@@ -455,7 +449,6 @@ static int msix_capability_init(struct pci_dev *dev,
                 entry->msi_attrib.default_irq = dev->irq;
                 entry->msi_attrib.pos = pos;
                 entry->mask_base = base;
-                msix_mask_irq(entry, 1);
 
                 list_add_tail(&entry->list, &dev->msi_list);
         }
@@ -480,22 +473,31 @@ static int msix_capability_init(struct pci_dev *dev,
                 return ret;
         }
 
+        /*
+         * Some devices require MSI-X to be enabled before we can touch the
+         * MSI-X registers.  We need to mask all the vectors to prevent
+         * interrupts coming in before they're fully set up.
+         */
+        control |= PCI_MSIX_FLAGS_MASKALL | PCI_MSIX_FLAGS_ENABLE;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+
         i = 0;
         list_for_each_entry(entry, &dev->msi_list, list) {
                 entries[i].vector = entry->irq;
                 set_irq_msi(entry->irq, entry);
+                j = entries[i].entry;
+                entry->masked = readl(base + j * PCI_MSIX_ENTRY_SIZE +
+                                        PCI_MSIX_ENTRY_VECTOR_CTRL_OFFSET);
+                msix_mask_irq(entry, 1);
                 i++;
         }
-        /* Set MSI-X enabled bits */
+
+        /* Set MSI-X enabled bits and unmask the function */
         pci_intx_for_msi(dev, 0);
-        msix_set_enable(dev, 1);
         dev->msix_enabled = 1;
 
-        list_for_each_entry(entry, &dev->msi_list, list) {
-                int vector = entry->msi_attrib.entry_nr;
-                entry->masked = readl(base + vector * PCI_MSIX_ENTRY_SIZE +
-                                        PCI_MSIX_ENTRY_VECTOR_CTRL_OFFSET);
-        }
+        control &= ~PCI_MSIX_FLAGS_MASKALL;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
 
         return 0;
 }
@@ -596,17 +598,20 @@ void pci_msi_shutdown(struct pci_dev *dev)
         struct msi_desc *desc;
         u32 mask;
         u16 ctrl;
+        unsigned pos;
 
         if (!pci_msi_enable || !dev || !dev->msi_enabled)
                 return;
 
-        msi_set_enable(dev, 0);
+        BUG_ON(list_empty(&dev->msi_list));
+        desc = list_first_entry(&dev->msi_list, struct msi_desc, list);
+        pos = desc->msi_attrib.pos;
+
+        msi_set_enable(dev, pos, 0);
         pci_intx_for_msi(dev, 1);
         dev->msi_enabled = 0;
 
-        BUG_ON(list_empty(&dev->msi_list));
-        desc = list_first_entry(&dev->msi_list, struct msi_desc, list);
-        pci_read_config_word(dev, desc->msi_attrib.pos + PCI_MSI_FLAGS, &ctrl);
+        pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &ctrl);
         mask = msi_capable_mask(ctrl);
         msi_mask_irq(desc, mask, ~mask);
 
@@ -648,10 +653,7 @@ static int msi_free_irqs(struct pci_dev* dev)
 
         list_for_each_entry_safe(entry, tmp, &dev->msi_list, list) {
                 if (entry->msi_attrib.is_msix) {
-                        writel(1, entry->mask_base + entry->msi_attrib.entry_nr
-                                  * PCI_MSIX_ENTRY_SIZE
-                                  + PCI_MSIX_ENTRY_VECTOR_CTRL_OFFSET);
-
+                        msix_mask_irq(entry, 1);
                         if (list_is_last(&entry->list, &dev->msi_list))
                                 iounmap(entry->mask_base);
                 }
@@ -691,8 +693,8 @@ int pci_msix_table_size(struct pci_dev *dev)
  * indicates the successful configuration of MSI-X capability structure
  * with new allocated MSI-X irqs. A return of < 0 indicates a failure.
  * Or a return of > 0 indicates that driver request is exceeding the number
- * of irqs available. Driver should use the returned value to re-send
- * its request.
+ * of irqs or MSI-X vectors available. Driver should use the returned value to
+ * re-send its request.
  **/
 int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
 {
@@ -708,7 +710,7 @@ int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
 
         nr_entries = pci_msix_table_size(dev);
         if (nvec > nr_entries)
-                return -EINVAL;
+                return nr_entries;
 
         /* Check for any invalid entries */
         for (i = 0; i < nvec; i++) {