[PATCH] PCI: Fix pci_bus_find_capability()

pci_bus_find_capability() is currently broken.  It checks the wrong
device's hdr_type for being a cardbus bridge or not.  This patch pulls
the guts of pci_bus_find_capability() and pci_find_capability() into a
new function __pci_bus_find_cap() and changes these two functions to
call it.

diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 0ed0c02fc5ab7ce0b7b889599d333b3d5efa98e6..d0d81018eedba1877a803c9db49887712cf5877f 100644 (file)
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -67,6 +67,39 @@ pci_max_busnr(void)
        return max;
 }
 
+static int __pci_bus_find_cap(struct pci_bus *bus, unsigned int devfn, u8 hdr_type, int cap)
+{
+       u16 status;
+       u8 pos, id;
+       int ttl = 48;
+
+       pci_bus_read_config_word(bus, devfn, PCI_STATUS, &status);
+       if (!(status & PCI_STATUS_CAP_LIST))
+               return 0;
+
+       switch (hdr_type) {
+       case PCI_HEADER_TYPE_NORMAL:
+       case PCI_HEADER_TYPE_BRIDGE:
+               pci_bus_read_config_byte(bus, devfn, PCI_CAPABILITY_LIST, &pos);
+               break;
+       case PCI_HEADER_TYPE_CARDBUS:
+               pci_bus_read_config_byte(bus, devfn, PCI_CB_CAPABILITY_LIST, &pos);
+               break;
+       default:
+               return 0;
+       }
+       while (ttl-- && pos >= 0x40) {
+               pos &= ~3;
+               pci_bus_read_config_byte(bus, devfn, pos + PCI_CAP_LIST_ID, &id);
+               if (id == 0xff)
+                       break;
+               if (id == cap)
+                       return pos;
+               pci_bus_read_config_byte(bus, devfn, pos + PCI_CAP_LIST_NEXT, &pos);
+       }
+       return 0;
+}
+
 /**
  * pci_find_capability - query for devices' capabilities 
  * @dev: PCI device to query
@@ -94,34 +127,7 @@ pci_max_busnr(void)
 int
 pci_find_capability(struct pci_dev *dev, int cap)
 {
-       u16 status;
-       u8 pos, id;
-       int ttl = 48;
-
-       pci_read_config_word(dev, PCI_STATUS, &status);
-       if (!(status & PCI_STATUS_CAP_LIST))
-               return 0;
-       switch (dev->hdr_type) {
-       case PCI_HEADER_TYPE_NORMAL:
-       case PCI_HEADER_TYPE_BRIDGE:
-               pci_read_config_byte(dev, PCI_CAPABILITY_LIST, &pos);
-               break;
-       case PCI_HEADER_TYPE_CARDBUS:
-               pci_read_config_byte(dev, PCI_CB_CAPABILITY_LIST, &pos);
-               break;
-       default:
-               return 0;
-       }
-       while (ttl-- && pos >= 0x40) {
-               pos &= ~3;
-               pci_read_config_byte(dev, pos + PCI_CAP_LIST_ID, &id);
-               if (id == 0xff)
-                       break;
-               if (id == cap)
-                       return pos;
-               pci_read_config_byte(dev, pos + PCI_CAP_LIST_NEXT, &pos);
-       }
-       return 0;
+       return __pci_bus_find_cap(dev->bus, dev->devfn, dev->hdr_type, cap);
 }
 
 /**
@@ -139,35 +145,11 @@ pci_find_capability(struct pci_dev *dev, int cap)
  */
 int pci_bus_find_capability(struct pci_bus *bus, unsigned int devfn, int cap)
 {
-       u16 status;
-       u8 pos, id;
-       int ttl = 48;
-       struct pci_dev *dev = bus->self;
+       u8 hdr_type;
 
-       pci_bus_read_config_word(bus, devfn, PCI_STATUS, &status);
-       if (!(status & PCI_STATUS_CAP_LIST))
-               return 0;
-       switch (dev->hdr_type) {
-       case PCI_HEADER_TYPE_NORMAL:
-       case PCI_HEADER_TYPE_BRIDGE:
-               pci_bus_read_config_byte(bus, devfn, PCI_CAPABILITY_LIST, &pos);
-               break;
-       case PCI_HEADER_TYPE_CARDBUS:
-               pci_bus_read_config_byte(bus, devfn, PCI_CB_CAPABILITY_LIST, &pos);
-               break;
-       default:
-               return 0;
-       }
-       while (ttl-- && pos >= 0x40) {
-               pos &= ~3;
-               pci_bus_read_config_byte(bus, devfn, pos + PCI_CAP_LIST_ID, &id);
-               if (id == 0xff)
-                       break;
-               if (id == cap)
-                       return pos;
-               pci_bus_read_config_byte(bus, devfn, pos + PCI_CAP_LIST_NEXT, &pos);
-       }
-       return 0;
+       pci_bus_read_config_byte(bus, devfn, PCI_HEADER_TYPE, &hdr_type);
+
+       return __pci_bus_find_cap(bus, devfn, hdr_type & 0x7f, cap);
 }
 
 /**