1 files changed, 27 insertions, 17 deletions

diff --git a/drivers/pci/host/pci-imx6.c b/drivers/pci/host/pci-imx6.c
index fdb95367721e..c93bcdf58a56 100644
--- a/drivers/pci/host/pci-imx6.c
+++ b/drivers/pci/host/pci-imx6.c
@@ -352,6 +352,23 @@ static int imx6_pcie_wait_for_link(struct pcie_port *pp)
         return 0;
 }
 
+static int imx6_pcie_wait_for_speed_change(struct pcie_port *pp)
+{
+        uint32_t tmp;
+        unsigned int retries;
+
+        for (retries = 0; retries < 200; retries++) {
+                tmp = readl(pp->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
+                /* Test if the speed change finished. */
+                if (!(tmp & PORT_LOGIC_SPEED_CHANGE))
+                        return 0;
+                usleep_range(100, 1000);
+        }
+
+        dev_err(pp->dev, "Speed change timeout\n");
+        return -EINVAL;
+}
+
 static irqreturn_t imx6_pcie_msi_handler(int irq, void *arg)
 {
         struct pcie_port *pp = arg;
@@ -363,7 +380,7 @@ static int imx6_pcie_start_link(struct pcie_port *pp)
 {
         struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
         uint32_t tmp;
-        int ret, count;
+        int ret;
 
         /*
          * Force Gen1 operation when starting the link.  In case the link is
@@ -397,29 +414,22 @@ static int imx6_pcie_start_link(struct pcie_port *pp)
         tmp |= PORT_LOGIC_SPEED_CHANGE;
         writel(tmp, pp->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
 
-        count = 200;
-        while (count--) {
-                tmp = readl(pp->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
-                /* Test if the speed change finished. */
-                if (!(tmp & PORT_LOGIC_SPEED_CHANGE))
-                        break;
-                usleep_range(100, 1000);
+        ret = imx6_pcie_wait_for_speed_change(pp);
+        if (ret) {
+                dev_err(pp->dev, "Failed to bring link up!\n");
+                return ret;
         }
 
         /* Make sure link training is finished as well! */
-        if (count)
-                ret = imx6_pcie_wait_for_link(pp);
-        else
-                ret = -EINVAL;
-
+        ret = imx6_pcie_wait_for_link(pp);
         if (ret) {
                 dev_err(pp->dev, "Failed to bring link up!\n");
-        } else {
-                tmp = readl(pp->dbi_base + 0x80);
-                dev_dbg(pp->dev, "Link up, Gen=%i\n", (tmp >> 16) & 0xf);
+                return ret;
         }
 
-        return ret;
+        tmp = readl(pp->dbi_base + 0x80);
+        dev_dbg(pp->dev, "Link up, Gen=%i\n", (tmp >> 16) & 0xf);
+        return 0;
 }
 
 static void imx6_pcie_host_init(struct pcie_port *pp)