-+ u8 tmp8;
-+
-+ tmp32 = bcma_pcie2_read_config(bdev, 0, devfn, 0xdc, 4);
-+ tmp32 &= ~0xf;
-+ if (allow_gen2)
-+ tmp32 |= 2;
-+ else {
-+ /* force PCIE GEN1 */
-+ tmp32 |= 1;
-+ }
-+ bcma_pcie2_write_config(bdev, 0, devfn, 0xdc, 4, tmp32);
-+
-+ /* See if the port is in EP mode, indicated by header type 00 */
-+ tmp8 = bcma_pcie2_read_config(bdev, 0, devfn, PCI_HEADER_TYPE, 1);
-+ if (tmp8 != PCI_HEADER_TYPE_BRIDGE) {
-+ dev_info(&bdev->dev, "Port %d in End-Point mode - ignored\n",
-+ bdev->core_unit);
-+ return -ENODEV;
++ u16 tmp16;
++ u16 pos;
++ u8 nlw;
++ /*
++ * Setup callback (bcma_pcie2_setup) is called in pcibios_init_hw before
++ * creating bus root, so we don't have it here yet. On the other hand
++ * we really want to use pci_bus_find_capability helper to check NLW.
++ * Let's fake simple pci_bus just to query for capabilities.
++ */
++ struct pci_bus bus = {
++ .number = 0,
++ .ops = &bcma_pcie2_ops,
++ .sysdata = sys,
++ };
++
++ tmp32 = bcma_read32(bdev, BCMA_CORE_PCIE2_LINK_STATUS);
++ dev_dbg(&bdev->dev, "link status: 0x%08x\n", tmp32);
++
++ tmp32 = bcma_read32(bdev, BCMA_CORE_PCIE2_STRAP_STATUS);
++ dev_dbg(&bdev->dev, "strap status: 0x%08x\n", tmp32);
++
++ /* check link status to see if link is active */
++ pos = pci_bus_find_capability(&bus, 0, PCI_CAP_ID_EXP);
++ pci_bus_read_config_word(&bus, 0, pos + PCI_EXP_LNKSTA, &tmp16);
++ nlw = (tmp16 & PCI_EXP_LNKSTA_NLW) >> PCI_EXP_LNKSTA_NLW_SHIFT;
++
++ if (nlw == 0) {
++ /* try GEN 1 link speed */
++ tmp32 = bcma_pcie2_read_config(bdev, 0, 0,
++ PCI_LINK_STATUS_CTRL_2_OFFSET, 4);
++ if ((tmp32 & PCI_TARGET_LINK_SPEED_MASK) ==
++ PCI_TARGET_LINK_SPEED_GEN2) {
++ tmp32 &= ~PCI_TARGET_LINK_SPEED_MASK;
++ tmp32 |= PCI_TARGET_LINK_SPEED_GEN1;
++ bcma_pcie2_write_config(bdev, 0, 0,
++ PCI_LINK_STATUS_CTRL_2_OFFSET, 4, tmp32);
++ tmp32 = bcma_pcie2_read_config(bdev, 0, 0,
++ PCI_LINK_STATUS_CTRL_2_OFFSET, 4);
++ msleep(100);
++
++ pos = pci_bus_find_capability(&bus, 0, PCI_CAP_ID_EXP);
++ pci_bus_read_config_word(&bus, 0, pos + PCI_EXP_LNKSTA,
++ &tmp16);
++ nlw = (tmp16 & PCI_EXP_LNKSTA_NLW) >>
++ PCI_EXP_LNKSTA_NLW_SHIFT;
++ }