diff --git a/arch/powerpc/include/asm/fsl_pci.h b/arch/powerpc/include/asm/fsl_pci.h
index 749411c..5be718b 100644
--- a/arch/powerpc/include/asm/fsl_pci.h
+++ b/arch/powerpc/include/asm/fsl_pci.h
@@ -18,24 +18,6 @@
 /* Freescale-specific PCI config registers */
 #define FSL_PCI_PBFR		0x44
 
-#ifdef CONFIG_SYS_FSL_PCI_VER_3_X
-/* Currently only the PCIe capability is used, so hardcode the offset.
- * if more capabilities need to be justified, the capability link method
- * should be applied here
- */
-#define FSL_PCIE_CAP_ID		0x70
-#define PCI_DCR		0x78    /* PCIe Device Control Register */
-#define PCI_DSR		0x7a    /* PCIe Device Status Register */
-#define PCI_LSR		0x82    /* PCIe Link Status Register */
-#define PCI_LCR		0x80    /* PCIe Link Control Register */
-#else
-#define FSL_PCIE_CAP_ID		0x4c
-#define PCI_DCR		0x54    /* PCIe Device Control Register */
-#define PCI_DSR		0x56    /* PCIe Device Status Register */
-#define PCI_LSR		0x5e    /* PCIe Link Status Register */
-#define PCI_LCR		0x5c    /* PCIe Link Control Register */
-#endif
-
 #define FSL_PCIE_CFG_RDY	0x4b0
 #define FSL_PROG_IF_AGENT	0x1
 
diff --git a/drivers/pci/fsl_pci_init.c b/drivers/pci/fsl_pci_init.c
index d55db1a..1c202ca 100644
--- a/drivers/pci/fsl_pci_init.c
+++ b/drivers/pci/fsl_pci_init.c
@@ -295,6 +295,15 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 	int enabled, r, inbound = 0;
 	u16 ltssm;
 	u8 temp8, pcie_cap;
+	int pcie_cap_pos;
+	int pci_dcr;
+	int pci_dsr;
+	int pci_lsr;
+
+#if defined(CONFIG_FSL_PCIE_DISABLE_ASPM)
+	int pci_lcr;
+#endif
+
 	volatile ccsr_fsl_pci_t *pci = (ccsr_fsl_pci_t *)cfg_addr;
 	struct pci_region *reg = hose->regions + hose->region_count;
 	pci_dev_t dev = PCI_BDF(hose->first_busno, 0, 0);
@@ -367,7 +376,12 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 	hose->region_count++;
 
 	/* see if we are a PCIe or PCI controller */
-	pci_hose_read_config_byte(hose, dev, FSL_PCIE_CAP_ID, &pcie_cap);
+	pcie_cap_pos = pci_hose_find_capability(hose, dev, PCI_CAP_ID_EXP);
+	pci_dcr = pcie_cap_pos + 0x08;
+	pci_dsr = pcie_cap_pos + 0x0a;
+	pci_lsr = pcie_cap_pos + 0x12;
+
+	pci_hose_read_config_byte(hose, dev, pcie_cap_pos, &pcie_cap);
 
 #ifdef CONFIG_SRIO_PCIE_BOOT_MASTER
 	/* boot from PCIE --master */
@@ -406,15 +420,16 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 					 * - Master PERR (pci)
 					 * - ICCA (PCIe)
 					 */
-	pci_hose_read_config_dword(hose, dev, PCI_DCR, &temp32);
+	pci_hose_read_config_dword(hose, dev, pci_dcr, &temp32);
 	temp32 |= 0xf000e;		/* set URR, FER, NFER (but not CER) */
-	pci_hose_write_config_dword(hose, dev, PCI_DCR, temp32);
+	pci_hose_write_config_dword(hose, dev, pci_dcr, temp32);
 
 #if defined(CONFIG_FSL_PCIE_DISABLE_ASPM)
+	pci_lcr = pcie_cap_pos + 0x10;
 	temp32 = 0;
-	pci_hose_read_config_dword(hose, dev, PCI_LCR, &temp32);
+	pci_hose_read_config_dword(hose, dev, pci_lcr, &temp32);
 	temp32 &= ~0x03;		/* Disable ASPM  */
-	pci_hose_write_config_dword(hose, dev, PCI_LCR, temp32);
+	pci_hose_write_config_dword(hose, dev, pci_lcr, temp32);
 	udelay(1);
 #endif
 	if (pcie_cap == PCI_CAP_ID_EXP) {
@@ -494,7 +509,7 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 		out_be32(&pci->pme_msg_int_en, 0xffffffff);
 
 		/* Print the negotiated PCIe link width */
-		pci_hose_read_config_word(hose, dev, PCI_LSR, &temp16);
+		pci_hose_read_config_word(hose, dev, pci_lsr, &temp16);
 		printf("x%d, regs @ 0x%lx\n", (temp16 & 0x3f0 ) >> 4,
 			pci_info->regs);
 
@@ -541,9 +556,9 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 		out_be32(&pci->pme_msg_det, 0xffffffff);
 	out_be32(&pci->pedr, 0xffffffff);
 
-	pci_hose_read_config_word (hose, dev, PCI_DSR, &temp16);
+	pci_hose_read_config_word (hose, dev, pci_dsr, &temp16);
 	if (temp16) {
-		pci_hose_write_config_word(hose, dev, PCI_DSR, 0xffff);
+		pci_hose_write_config_word(hose, dev, pci_dsr, 0xffff);
 	}
 
 	pci_hose_read_config_word (hose, dev, PCI_SEC_STATUS, &temp16);
@@ -554,10 +569,12 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 
 int fsl_is_pci_agent(struct pci_controller *hose)
 {
+	int pcie_cap_pos;
 	u8 pcie_cap;
 	pci_dev_t dev = PCI_BDF(hose->first_busno, 0, 0);
 
-	pci_hose_read_config_byte(hose, dev, FSL_PCIE_CAP_ID, &pcie_cap);
+	pcie_cap_pos = pci_hose_find_capability(hose, dev, PCI_CAP_ID_EXP);
+	pci_hose_read_config_byte(hose, dev, pcie_cap_pos, &pcie_cap);
 	if (pcie_cap == PCI_CAP_ID_EXP) {
 		u8 header_type;
 
@@ -582,6 +599,7 @@ int fsl_pci_init_port(struct fsl_pci_info *pci_info,
 	volatile ccsr_fsl_pci_t *pci;
 	struct pci_region *r;
 	pci_dev_t dev = PCI_BDF(busno,0,0);
+	int pcie_cap_pos;
 	u8 pcie_cap;
 
 	pci = (ccsr_fsl_pci_t *) pci_info->regs;
@@ -631,11 +649,11 @@ int fsl_pci_init_port(struct fsl_pci_info *pci_info,
 #endif
 	}
 
-	pci_hose_read_config_byte(hose, dev, FSL_PCIE_CAP_ID, &pcie_cap);
+	pcie_cap_pos = pci_hose_find_capability(hose, dev, PCI_CAP_ID_EXP);
+	pci_hose_read_config_byte(hose, dev, pcie_cap_pos, &pcie_cap);
 	printf("PCI%s%x: Bus %02x - %02x\n", pcie_cap == PCI_CAP_ID_EXP ?
 		"e" : "", pci_info->pci_num,
 		hose->first_busno, hose->last_busno);
-
 	return(hose->last_busno + 1);
 }
 
@@ -643,13 +661,15 @@ int fsl_pci_init_port(struct fsl_pci_info *pci_info,
 void fsl_pci_config_unlock(struct pci_controller *hose)
 {
 	pci_dev_t dev = PCI_BDF(hose->first_busno,0,0);
+	int pcie_cap_pos;
 	u8 pcie_cap;
 	u16 pbfr;
 
 	if (!fsl_is_pci_agent(hose))
 		return;
 
-	pci_hose_read_config_byte(hose, dev, FSL_PCIE_CAP_ID, &pcie_cap);
+	pcie_cap_pos = pci_hose_find_capability(hose, dev, PCI_CAP_ID_EXP);
+	pci_hose_read_config_byte(hose, dev, pcie_cap_pos, &pcie_cap);
 	if (pcie_cap != 0x0) {
 		/* PCIe - set CFG_READY bit of Configuration Ready Register */
 		pci_hose_write_config_byte(hose, dev, FSL_PCIE_CFG_RDY, 0x1);
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 2c07158..ed113bf 100644
--- 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;
+}
diff --git a/include/pci.h b/include/pci.h
index 911ba89..d462479 100644
--- a/include/pci.h
+++ b/include/pci.h
@@ -410,6 +410,9 @@
 #define PCI_MAX_PCI_DEVICES	32
 #define PCI_MAX_PCI_FUNCTIONS	8
 
+#define PCI_FIND_CAP_TTL 0x48
+#define CAP_START_POS 0x40
+
 /* Include the ID list */
 
 #include <pci_ids.h>
@@ -647,6 +650,13 @@ extern int pci_hose_config_device(struct pci_controller *hose,
 				  pci_addr_t mem,
 				  unsigned long command);
 
+extern int pci_hose_find_capability(struct pci_controller *hose, pci_dev_t dev,
+				    int cap);
+extern int pci_hose_find_cap_start(struct pci_controller *hose, pci_dev_t dev,
+				   u8 hdr_type);
+extern int pci_find_cap(struct pci_controller *hose, pci_dev_t dev, int pos,
+			int cap);
+
 const char * pci_class_str(u8 class);
 int pci_last_busno(void);
 
