diff --git a/drivers/staging/mt7621-pci/pci-mt7621.c b/drivers/staging/mt7621-pci/pci-mt7621.c
index b9d460a9c041..793bae0abbad 100644
--- a/drivers/staging/mt7621-pci/pci-mt7621.c
+++ b/drivers/staging/mt7621-pci/pci-mt7621.c
@@ -122,6 +122,7 @@ struct mt7621_pcie_port {
  * @ports: pointer to PCIe port information
  * @resets_inverted: depends on chip revision
  * reset lines are inverted.
+ * @link_status: link status of pcie device.
  */
 struct mt7621_pcie {
 	void __iomem *base;
@@ -136,6 +137,7 @@ struct mt7621_pcie {
 	unsigned long io_map_base;
 	struct list_head ports;
 	bool resets_inverted;
+	u32 link_status;
 };
 
 static inline u32 pcie_read(struct mt7621_pcie *pcie, u32 reg)
@@ -279,6 +281,29 @@ static void setup_cm_memory_region(struct mt7621_pcie *pcie)
 	}
 }
 
+static int mt7621_map_irq(const struct pci_dev *pdev, u8 slot, u8 pin)
+{
+	struct mt7621_pcie *pcie = pdev->bus->sysdata;
+	struct device *dev = pcie->dev;
+	int irq_map[PCIE_P2P_MAX];
+	int irq;
+	int n, i;
+
+	/* Assign IRQs */
+	n = 0;
+	for (i = 0; i < PCIE_P2P_MAX; i++)
+		if (pcie->link_status & BIT(i))
+			irq_map[n++] = of_irq_parse_and_map_pci(pdev, i, pin);
+
+	for (i = n; i < PCIE_P2P_MAX; i++)
+		irq_map[i] = -1;
+
+	irq = irq_map[slot];
+
+	dev_info(dev, "Bus=%d slot=%d irq=%d\n", pdev->bus->number, slot, irq);
+	return irq;
+}
+
 static int mt7621_pci_parse_request_of_pci_ranges(struct mt7621_pcie *pcie)
 {
 	struct device *dev = pcie->dev;
@@ -583,29 +608,29 @@ static void mt7621_pcie_enable_ports(struct mt7621_pcie *pcie)
 
 static int mt7621_pcie_init_virtual_bridges(struct mt7621_pcie *pcie)
 {
-	u32 pcie_link_status = 0;
 	u32 n;
 	int i;
 	u32 p2p_br_devnum[PCIE_P2P_MAX];
 	struct mt7621_pcie_port *port;
 
+	pcie->link_status = 0;
 	list_for_each_entry(port, &pcie->ports, list) {
 		u32 slot = port->slot;
 
 		if (port->enabled)
-			pcie_link_status |= BIT(slot);
+			pcie->link_status |= BIT(slot);
 	}
 
-	if (pcie_link_status == 0)
+	if (pcie->link_status == 0)
 		return -1;
 
 	n = 0;
 	for (i = 0; i < PCIE_P2P_MAX; i++)
-		if (pcie_link_status & BIT(i))
+		if (pcie->link_status & BIT(i))
 			p2p_br_devnum[i] = n++;
 
 	for (i = 0; i < PCIE_P2P_MAX; i++)
-		if ((pcie_link_status & BIT(i)) == 0)
+		if ((pcie->link_status & BIT(i)) == 0)
 			p2p_br_devnum[i] = n++;
 
 	pcie_rmw(pcie, RALINK_PCI_PCICFG_ADDR,
@@ -638,7 +663,7 @@ static int mt7621_pcie_register_host(struct pci_host_bridge *host,
 	host->busnr = pcie->busn.start;
 	host->dev.parent = pcie->dev;
 	host->ops = &mt7621_pci_ops;
-	host->map_irq = of_irq_parse_and_map_pci;
+	host->map_irq = mt7621_map_irq;
 	host->swizzle_irq = pci_common_swizzle;
 	host->sysdata = pcie;