1 files changed, 100 insertions, 15 deletions

diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index a21e1c292ee4..a8523294e4ae 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -2050,10 +2050,100 @@ DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_NVIDIA,
                         PCI_DEVICE_ID_NVIDIA_NVENET_15,
                         nvenet_msi_disable);
 
-static void __devinit nv_msi_ht_cap_quirk(struct pci_dev *dev)
+static void __devinit nv_ht_enable_msi_mapping(struct pci_dev *dev)
 {
         struct pci_dev *host_bridge;
+        int pos;
+        int i, dev_no;
+        int found = 0;
+
+        dev_no = dev->devfn >> 3;
+        for (i = dev_no; i >= 0; i--) {
+                host_bridge = pci_get_slot(dev->bus, PCI_DEVFN(i, 0));
+                if (!host_bridge)
+                        continue;
+
+                pos = pci_find_ht_capability(host_bridge, HT_CAPTYPE_SLAVE);
+                if (pos != 0) {
+                        found = 1;
+                        break;
+                }
+                pci_dev_put(host_bridge);
+        }
+
+        if (!found)
+                return;
+
+        /* root did that ! */
+        if (msi_ht_cap_enabled(host_bridge))
+                goto out;
+
+        ht_enable_msi_mapping(dev);
+
+out:
+        pci_dev_put(host_bridge);
+}
+
+static void __devinit ht_disable_msi_mapping(struct pci_dev *dev)
+{
+        int pos, ttl = 48;
+
+        pos = pci_find_ht_capability(dev, HT_CAPTYPE_MSI_MAPPING);
+        while (pos && ttl--) {
+                u8 flags;
+
+                if (pci_read_config_byte(dev, pos + HT_MSI_FLAGS,
+                                         &flags) == 0) {
+                        dev_info(&dev->dev, "Enabling HT MSI Mapping\n");
+
+                        pci_write_config_byte(dev, pos + HT_MSI_FLAGS,
+                                              flags & ~HT_MSI_FLAGS_ENABLE);
+                }
+                pos = pci_find_next_ht_capability(dev, pos,
+                                                  HT_CAPTYPE_MSI_MAPPING);
+        }
+}
+
+static int __devinit ht_check_msi_mapping(struct pci_dev *dev)
+{
         int pos, ttl = 48;
+        int found = 0;
+
+        /* check if there is HT MSI cap or enabled on this device */
+        pos = pci_find_ht_capability(dev, HT_CAPTYPE_MSI_MAPPING);
+        while (pos && ttl--) {
+                u8 flags;
+
+                if (found < 1)
+                        found = 1;
+                if (pci_read_config_byte(dev, pos + HT_MSI_FLAGS,
+                                         &flags) == 0) {
+                        if (flags & HT_MSI_FLAGS_ENABLE) {
+                                if (found < 2) {
+                                        found = 2;
+                                        break;
+                                }
+                        }
+                }
+                pos = pci_find_next_ht_capability(dev, pos,
+                                                  HT_CAPTYPE_MSI_MAPPING);
+        }
+
+        return found;
+}
+
+static void __devinit nv_msi_ht_cap_quirk(struct pci_dev *dev)
+{
+        struct pci_dev *host_bridge;
+        int pos;
+        int found;
+
+        /* check if there is HT MSI cap or enabled on this device */
+        found = ht_check_msi_mapping(dev);
+
+        /* no HT MSI CAP */
+        if (found == 0)
+                return;
 
         /*
          * HT MSI mapping should be disabled on devices that are below
@@ -2069,24 +2159,19 @@ static void __devinit nv_msi_ht_cap_quirk(struct pci_dev *dev)
         pos = pci_find_ht_capability(host_bridge, HT_CAPTYPE_SLAVE);
         if (pos != 0) {
                 /* Host bridge is to HT */
-                ht_enable_msi_mapping(dev);
+                if (found == 1) {
+                        /* it is not enabled, try to enable it */
+                        nv_ht_enable_msi_mapping(dev);
+                }
                 return;
         }
 
-        /* Host bridge is not to HT, disable HT MSI mapping on this device */
-        pos = pci_find_ht_capability(dev, HT_CAPTYPE_MSI_MAPPING);
-        while (pos && ttl--) {
-                u8 flags;
+        /* HT MSI is not enabled */
+        if (found == 1)
+                return;
 
-                if (pci_read_config_byte(dev, pos + HT_MSI_FLAGS,
-                                         &flags) == 0) {
-                        dev_info(&dev->dev, "Disabling HT MSI mapping");
-                        pci_write_config_byte(dev, pos + HT_MSI_FLAGS,
-                                              flags & ~HT_MSI_FLAGS_ENABLE);
-                }
-                pos = pci_find_next_ht_capability(dev, pos,
-                                                  HT_CAPTYPE_MSI_MAPPING);
-        }
+        /* Host bridge is not to HT, disable HT MSI mapping on this device */
+        ht_disable_msi_mapping(dev);
 }
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_NVIDIA, PCI_ANY_ID, nv_msi_ht_cap_quirk);
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_AL, PCI_ANY_ID, nv_msi_ht_cap_quirk);