PCIe:change the method to get the address of a requested capability in configuration...

[platform/kernel/u-boot.git] / drivers / pci / pci.c

diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 2c07158..ed113bf 100644 (file)
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -722,3 +722,68 @@ void pci_init(void)
        /* now call board specific pci_init()... */
        pci_init_board();
 }
+
+/* Returns the address of the requested capability structure within the
+ * device's PCI configuration space or 0 in case the device does not
+ * support it.
+ * */
+int pci_hose_find_capability(struct pci_controller *hose, pci_dev_t dev,
+                            int cap)
+{
+       int pos;
+       u8 hdr_type;
+
+       pci_hose_read_config_byte(hose, dev, PCI_HEADER_TYPE, &hdr_type);
+
+       pos = pci_hose_find_cap_start(hose, dev, hdr_type & 0x7F);
+
+       if (pos)
+               pos = pci_find_cap(hose, dev, pos, cap);
+
+       return pos;
+}
+
+/* Find the header pointer to the Capabilities*/
+int pci_hose_find_cap_start(struct pci_controller *hose, pci_dev_t dev,
+                           u8 hdr_type)
+{
+       u16 status;
+
+       pci_hose_read_config_word(hose, dev, PCI_STATUS, &status);
+
+       if (!(status & PCI_STATUS_CAP_LIST))
+               return 0;
+
+       switch (hdr_type) {
+       case PCI_HEADER_TYPE_NORMAL:
+       case PCI_HEADER_TYPE_BRIDGE:
+               return PCI_CAPABILITY_LIST;
+       case PCI_HEADER_TYPE_CARDBUS:
+               return PCI_CB_CAPABILITY_LIST;
+       default:
+               return 0;
+       }
+}
+
+int pci_find_cap(struct pci_controller *hose, pci_dev_t dev, int pos, int cap)
+{
+       int ttl = PCI_FIND_CAP_TTL;
+       u8 id;
+       u8 next_pos;
+
+       while (ttl--) {
+               pci_hose_read_config_byte(hose, dev, pos, &next_pos);
+               if (next_pos < CAP_START_POS)
+                       break;
+               next_pos &= ~3;
+               pos = (int) next_pos;
+               pci_hose_read_config_byte(hose, dev,
+                                         pos + PCI_CAP_LIST_ID, &id);
+               if (id == 0xff)
+                       break;
+               if (id == cap)
+                       return pos;
+               pos += PCI_CAP_LIST_NEXT;
+       }
+       return 0;
+}