staging: mt7621-pci: factor out 'mt7621_pcie_enable_port' function

Function 'mt7621_pcie_enable_ports' tries to enable all PCI ports.
To make it more readable the single port initialization part has
been factor out into a new 'mt7621_pcie_enable_port' function.

Signed-off-by: Sergio Paracuellos <sergio.paracuellos@gmail.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/staging/mt7621-pci/pci-mt7621.c b/drivers/staging/mt7621-pci/pci-mt7621.c
index 5f7ff70e289f905faf4c93f42834af5834a36ff8..a069d7f5d8d9ac6b01e1fe4852dd30a5072ac88d 100644 (file)
--- a/drivers/staging/mt7621-pci/pci-mt7621.c
+++ b/drivers/staging/mt7621-pci/pci-mt7621.c
@@ -628,54 +628,64 @@ static void mt7621_pcie_init_ports(struct mt7621_pcie *pcie)
        }
 }
 
+static int mt7621_pcie_enable_port(struct mt7621_pcie_port *port)
+{
+       struct mt7621_pcie *pcie = port->pcie;
+       u32 slot = port->slot;
+       u32 offset = MT7621_PCIE_OFFSET + (slot * MT7621_NEXT_PORT);
+       u32 val;
+       int err;
+
+       /* assert port PERST_N */
+       val = pcie_read(pcie, RALINK_PCI_PCICFG_ADDR);
+       val |= PCIE_PORT_PERST(slot);
+       pcie_write(pcie, val, RALINK_PCI_PCICFG_ADDR);
+
+       /* de-assert port PERST_N */
+       val = pcie_read(pcie, RALINK_PCI_PCICFG_ADDR);
+       val &= ~PCIE_PORT_PERST(slot);
+       pcie_write(pcie, val, RALINK_PCI_PCICFG_ADDR);
+
+       /* 100ms timeout value should be enough for Gen1 training */
+       err = readl_poll_timeout(port->base + RALINK_PCI_STATUS,
+                                val, !!(val & PCIE_PORT_LINKUP),
+                                20, 100 * USEC_PER_MSEC);
+       if (err)
+               return -ETIMEDOUT;
+
+       /* enable pcie interrupt */
+       val = pcie_read(pcie, RALINK_PCI_PCIMSK_ADDR);
+       val |= PCIE_PORT_INT_EN(slot);
+       pcie_write(pcie, val, RALINK_PCI_PCIMSK_ADDR);
+
+       /* map 2G DDR region */
+       pcie_write(pcie, PCIE_BAR_MAP_MAX | PCIE_BAR_ENABLE,
+                  offset + RALINK_PCI_BAR0SETUP_ADDR);
+       pcie_write(pcie, MEMORY_BASE,
+                  offset + RALINK_PCI_IMBASEBAR0_ADDR);
+
+       /* configure class code and revision ID */
+       pcie_write(pcie, PCIE_CLASS_CODE | PCIE_REVISION_ID,
+                  offset + RALINK_PCI_CLASS);
+
+       return 0;
+}
+
 static void mt7621_pcie_enable_ports(struct mt7621_pcie *pcie)
 {
        struct device *dev = pcie->dev;
        struct mt7621_pcie_port *port;
        u8 num_slots_enabled = 0;
-       u32 offset;
        u32 slot;
        u32 val;
-       int err;
 
        list_for_each_entry(port, &pcie->ports, list) {
-               slot = port->slot;
-               offset = MT7621_PCIE_OFFSET + (slot * MT7621_NEXT_PORT);
-
                if (port->enabled) {
-                       /* assert port PERST_N */
-                       val = pcie_read(pcie, RALINK_PCI_PCICFG_ADDR);
-                       val |= PCIE_PORT_PERST(slot);
-                       pcie_write(pcie, val, RALINK_PCI_PCICFG_ADDR);
-
-                       /* de-assert port PERST_N */
-                       val = pcie_read(pcie, RALINK_PCI_PCICFG_ADDR);
-                       val &= ~PCIE_PORT_PERST(slot);
-                       pcie_write(pcie, val, RALINK_PCI_PCICFG_ADDR);
-
-                       /* 100ms timeout value should be enough for Gen1 training */
-                       err = readl_poll_timeout(port->base + RALINK_PCI_STATUS,
-                                                val,!!(val & PCIE_PORT_LINKUP),
-                                                20, 100 * USEC_PER_MSEC);
-                       if (err) {
+                       if (!mt7621_pcie_enable_port(port)) {
                                dev_err(dev, "de-assert port %d PERST_N\n",
-                                       slot);
+                                       port->slot);
                                continue;
                        }
-
-                       /* enable pcie interrupt */
-                       val = pcie_read(pcie, RALINK_PCI_PCIMSK_ADDR);
-                       val |= PCIE_PORT_INT_EN(slot);
-                       pcie_write(pcie, val, RALINK_PCI_PCIMSK_ADDR);
-
-                       /* map 2G DDR region */
-                       pcie_write(pcie, PCIE_BAR_MAP_MAX | PCIE_BAR_ENABLE,
-                                  offset + RALINK_PCI_BAR0SETUP_ADDR);
-                       pcie_write(pcie, MEMORY_BASE,
-                                  offset + RALINK_PCI_IMBASEBAR0_ADDR);
-                       /* configure class code and revision ID */
-                       pcie_write(pcie, PCIE_CLASS_CODE | PCIE_REVISION_ID,
-                                  offset + RALINK_PCI_CLASS);
                        dev_info(dev, "PCIE%d enabled\n", slot);
                        num_slots_enabled++;
                }