[PATCH/v2] PCI: add pci_find_next_capability()

[Roland Dreier <rolandd@cisco.com>]

Greg,
I think you silently dropped the patch below.  This is the new version
that incorporates Matthew Wilcox's suggestion to avoid duplicating the
loop that iterates through capabilities.

Thanks,
  Roland



Some devices have more than one capability of the same type.  For
example, the PCI header for the PathScale InfiniPath looks like:

	04:01.0 InfiniBand: Unknown device 1fc1:000d (rev 02)
		Subsystem: Unknown device 1fc1:000d
		Flags: bus master, fast devsel, latency 0, IRQ 193
		Memory at fea00000 (64-bit, non-prefetchable) [size=2M]
		Capabilities: [c0] HyperTransport: Slave or Primary Interface
		Capabilities: [f8] HyperTransport: Interrupt Discovery and Configuration

There are _two_ HyperTransport capabilities, and the PathScale driver
wants to look at both of them.

The current pci_find_capability() API doesn't work for this, since it
only allows us to get to the first capability of a given type.  The
patch below introduces a new pci_find_next_capability(), which can be
used in a loop like

	for (pos = pci_find_capability(pdev, <ID>);
	     pos;
	     pos = pci_find_next_capability(pdev, pos, <ID>)) {
		/* ... */
	}

I made this an EXPORT_SYMBOL() instead of an EXPORT_SYMBOL_GPL() since
it is a trivial wrapper around existing PCI functions, and I'd rather
see people use a nice wrapper instead of recreating the function.

Signed-off-by: Roland Dreier <rolandd@cisco.com>

---

diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 259d247..b852959 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -62,11 +62,38 @@ pci_max_busnr(void)
 	return max;
 }
 
+static int __pci_find_next_cap(struct pci_bus *bus, unsigned int devfn, u8 pos, int cap)
+{
+	u8 id;
+	int ttl = 48;
+
+	while (ttl--) {
+		pci_bus_read_config_byte(bus, devfn, pos, &pos);
+		if (pos < 0x40)
+			break;
+		pos &= ~3;
+		pci_bus_read_config_byte(bus, devfn, pos + PCI_CAP_LIST_ID,
+					 &id);
+		if (id == 0xff)
+			break;
+		if (id == cap)
+			return pos;
+		pos += PCI_CAP_LIST_NEXT;
+	}
+	return 0;
+}
+
+int pci_find_next_capability(struct pci_dev *dev, u8 pos, int cap)
+{
+	return __pci_find_next_cap(dev->bus, dev->devfn,
+				   pos + PCI_CAP_LIST_NEXT, cap);
+}
+EXPORT_SYMBOL(pci_find_next_capability);
+
 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;
+	u8 pos;
 
 	pci_bus_read_config_word(bus, devfn, PCI_STATUS, &status);
 	if (!(status & PCI_STATUS_CAP_LIST))
@@ -75,24 +102,15 @@ static int __pci_bus_find_cap(struct pci
 	switch (hdr_type) {
 	case PCI_HEADER_TYPE_NORMAL:
 	case PCI_HEADER_TYPE_BRIDGE:
-		pci_bus_read_config_byte(bus, devfn, PCI_CAPABILITY_LIST, &pos);
+		pos = PCI_CAPABILITY_LIST;
 		break;
 	case PCI_HEADER_TYPE_CARDBUS:
-		pci_bus_read_config_byte(bus, devfn, PCI_CB_CAPABILITY_LIST, &pos);
+		pos = PCI_CB_CAPABILITY_LIST;
 		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;
+	return __pci_find_next_cap(bus, devfn, pos, cap);
 }
 
 /**
diff --git a/include/linux/pci.h b/include/linux/pci.h
index 7349058..8016d14 100644
--- a/include/linux/pci.h
+++ b/include/linux/pci.h
@@ -337,6 +337,7 @@ struct pci_dev *pci_find_device (unsigne
 struct pci_dev *pci_find_device_reverse (unsigned int vendor, unsigned int device, const struct pci_dev *from);
 struct pci_dev *pci_find_slot (unsigned int bus, unsigned int devfn);
 int pci_find_capability (struct pci_dev *dev, int cap);
+int pci_find_next_capability (struct pci_dev *dev, u8 pos, int cap);
 int pci_find_ext_capability (struct pci_dev *dev, int cap);
 struct pci_bus * pci_find_next_bus(const struct pci_bus *from);
 
@@ -546,6 +547,7 @@ static inline int pci_assign_resource(st
 static inline int pci_register_driver(struct pci_driver *drv) { return 0;}
 static inline void pci_unregister_driver(struct pci_driver *drv) { }
 static inline int pci_find_capability (struct pci_dev *dev, int cap) {return 0; }
+static inline int pci_find_next_capability (struct pci_dev *dev, u8 post, int cap) {return 0; }
 static inline int pci_find_ext_capability (struct pci_dev *dev, int cap) {return 0; }
 static inline const struct pci_device_id *pci_match_device(const struct pci_device_id *ids, const struct pci_dev *dev) { return NULL; }