1 files changed, 38 insertions, 49 deletions

diff --git a/drivers/usb/phy/phy-omap-usb3.c b/drivers/usb/phy/phy-omap-usb3.c
index a2fb30bbb971..fc15694d3031 100644
--- a/drivers/usb/phy/phy-omap-usb3.c
+++ b/drivers/usb/phy/phy-omap-usb3.c
@@ -27,7 +27,6 @@
 #include <linux/delay.h>
 #include <linux/usb/omap_control_usb.h>
 
-#define NUM_SYS_CLKS            6
 #define PLL_STATUS              0x00000004
 #define PLL_GO                  0x00000008
 #define PLL_CONFIGURATION1      0x0000000C
@@ -57,26 +56,32 @@
  */
 # define PLL_IDLE_TIME  100;
 
-enum sys_clk_rate {
-        CLK_RATE_UNDEFINED = -1,
-        CLK_RATE_12MHZ,
-        CLK_RATE_16MHZ,
-        CLK_RATE_19MHZ,
-        CLK_RATE_20MHZ,
-        CLK_RATE_26MHZ,
-        CLK_RATE_38MHZ
+struct usb_dpll_map {
+        unsigned long rate;
+        struct usb_dpll_params params;
 };
 
-static struct usb_dpll_params omap_usb3_dpll_params[NUM_SYS_CLKS] = {
-        {1250, 5, 4, 20, 0},            /* 12 MHz */
-        {3125, 20, 4, 20, 0},           /* 16.8 MHz */
-        {1172, 8, 4, 20, 65537},        /* 19.2 MHz */
-        {1000, 7, 4, 10, 0},            /* 20 MHz */
-        {1250, 12, 4, 20, 0},           /* 26 MHz */
-        {3125, 47, 4, 20, 92843},       /* 38.4 MHz */
-
+static struct usb_dpll_map dpll_map[] = {
+        {12000000, {1250, 5, 4, 20, 0} },       /* 12 MHz */
+        {16800000, {3125, 20, 4, 20, 0} },      /* 16.8 MHz */
+        {19200000, {1172, 8, 4, 20, 65537} },   /* 19.2 MHz */
+        {20000000, {1000, 7, 4, 10, 0} },       /* 20 MHz */
+        {26000000, {1250, 12, 4, 20, 0} },      /* 26 MHz */
+        {38400000, {3125, 47, 4, 20, 92843} },  /* 38.4 MHz */
 };
 
+static struct usb_dpll_params *omap_usb3_get_dpll_params(unsigned long rate)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(dpll_map); i++) {
+                if (rate == dpll_map[i].rate)
+                        return &dpll_map[i].params;
+        }
+
+        return 0;
+}
+
 static int omap_usb3_suspend(struct usb_phy *x, int suspend)
 {
         struct omap_usb *phy = phy_to_omapusb(x);
@@ -116,26 +121,6 @@ static int omap_usb3_suspend(struct usb_phy *x, int suspend)
         return 0;
 }
 
-static inline enum sys_clk_rate __get_sys_clk_index(unsigned long rate)
-{
-        switch (rate) {
-        case 12000000:
-                return CLK_RATE_12MHZ;
-        case 16800000:
-                return CLK_RATE_16MHZ;
-        case 19200000:
-                return CLK_RATE_19MHZ;
-        case 20000000:
-                return CLK_RATE_20MHZ;
-        case 26000000:
-                return CLK_RATE_26MHZ;
-        case 38400000:
-                return CLK_RATE_38MHZ;
-        default:
-                return CLK_RATE_UNDEFINED;
-        }
-}
-
 static void omap_usb_dpll_relock(struct omap_usb *phy)
 {
         u32             val;
@@ -155,39 +140,39 @@ static int omap_usb_dpll_lock(struct omap_usb *phy)
 {
         u32                     val;
         unsigned long           rate;
-        enum sys_clk_rate       clk_index;
-
-        rate            = clk_get_rate(phy->sys_clk);
-        clk_index       = __get_sys_clk_index(rate);
+        struct usb_dpll_params *dpll_params;
 
-        if (clk_index == CLK_RATE_UNDEFINED) {
-                pr_err("dpll cannot be locked for sys clk freq:%luHz\n", rate);
+        rate = clk_get_rate(phy->sys_clk);
+        dpll_params = omap_usb3_get_dpll_params(rate);
+        if (!dpll_params) {
+                dev_err(phy->dev,
+                          "No DPLL configuration for %lu Hz SYS CLK\n", rate);
                 return -EINVAL;
         }
 
         val = omap_usb_readl(phy->pll_ctrl_base, PLL_CONFIGURATION1);
         val &= ~PLL_REGN_MASK;
-        val |= omap_usb3_dpll_params[clk_index].n << PLL_REGN_SHIFT;
+        val |= dpll_params->n << PLL_REGN_SHIFT;
         omap_usb_writel(phy->pll_ctrl_base, PLL_CONFIGURATION1, val);
 
         val = omap_usb_readl(phy->pll_ctrl_base, PLL_CONFIGURATION2);
         val &= ~PLL_SELFREQDCO_MASK;
-        val |= omap_usb3_dpll_params[clk_index].freq << PLL_SELFREQDCO_SHIFT;
+        val |= dpll_params->freq << PLL_SELFREQDCO_SHIFT;
         omap_usb_writel(phy->pll_ctrl_base, PLL_CONFIGURATION2, val);
 
         val = omap_usb_readl(phy->pll_ctrl_base, PLL_CONFIGURATION1);
         val &= ~PLL_REGM_MASK;
-        val |= omap_usb3_dpll_params[clk_index].m << PLL_REGM_SHIFT;
+        val |= dpll_params->m << PLL_REGM_SHIFT;
         omap_usb_writel(phy->pll_ctrl_base, PLL_CONFIGURATION1, val);
 
         val = omap_usb_readl(phy->pll_ctrl_base, PLL_CONFIGURATION4);
         val &= ~PLL_REGM_F_MASK;
-        val |= omap_usb3_dpll_params[clk_index].mf << PLL_REGM_F_SHIFT;
+        val |= dpll_params->mf << PLL_REGM_F_SHIFT;
         omap_usb_writel(phy->pll_ctrl_base, PLL_CONFIGURATION4, val);
 
         val = omap_usb_readl(phy->pll_ctrl_base, PLL_CONFIGURATION3);
         val &= ~PLL_SD_MASK;
-        val |= omap_usb3_dpll_params[clk_index].sd << PLL_SD_SHIFT;
+        val |= dpll_params->sd << PLL_SD_SHIFT;
         omap_usb_writel(phy->pll_ctrl_base, PLL_CONFIGURATION3, val);
 
         omap_usb_dpll_relock(phy);
@@ -198,8 +183,12 @@ static int omap_usb_dpll_lock(struct omap_usb *phy)
 static int omap_usb3_init(struct usb_phy *x)
 {
         struct omap_usb *phy = phy_to_omapusb(x);
+        int ret;
+
+        ret = omap_usb_dpll_lock(phy);
+        if (ret)
+                return ret;
 
-        omap_usb_dpll_lock(phy);
         omap_control_usb3_phy_power(phy->control_dev, 1);
 
         return 0;