1 files changed, 41 insertions, 15 deletions

diff --git a/drivers/usb/phy/omap_usb_phy.c b/drivers/usb/phy/omap_usb_phy.c
index f9069c7f9c3..1993da16ea1 100644
--- a/drivers/usb/phy/omap_usb_phy.c
+++ b/drivers/usb/phy/omap_usb_phy.c
@@ -23,7 +23,7 @@
 #include "../host/xhci.h"
 
 #ifdef CONFIG_OMAP_USB3PHY1_HOST
-struct usb_dpll_params {
+struct usb3_dpll_params {
 	u16	m;
 	u8	n;
 	u8	freq:3;
@@ -31,17 +31,39 @@ struct usb_dpll_params {
 	u32	mf;
 };
 
-#define	NUM_USB_CLKS		6
+struct usb3_dpll_map {
+	unsigned long rate;
+	struct usb3_dpll_params params;
+	struct usb3_dpll_map *dpll_map;
+};
 
-static struct usb_dpll_params omap_usb3_dpll_params[NUM_USB_CLKS] = {
-	{1250, 5, 4, 20, 0},		/* 12 MHz */
-	{3125, 20, 4, 20, 0},		/* 16.8 MHz */
-	{1172, 8, 4, 20, 65537},	/* 19.2 MHz */
-	{1250, 12, 4, 20, 0},		/* 26 MHz */
-	{3125, 47, 4, 20, 92843},	/* 38.4 MHz */
-	{1000, 7, 4, 10, 0},        /* 20 MHz */
+static struct usb3_dpll_map dpll_map_usb[] = {
+	{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 */
+	{ },					/* Terminator */
 };
 
+static struct usb3_dpll_params *omap_usb3_get_dpll_params(void)
+{
+	unsigned long rate;
+	struct usb3_dpll_map *dpll_map = dpll_map_usb;
+
+	rate = get_sys_clk_freq();
+
+	for (; dpll_map->rate; dpll_map++) {
+		if (rate == dpll_map->rate)
+			return &dpll_map->params;
+	}
+
+	dev_err(phy->dev, "No DPLL configuration for %lu Hz SYS CLK\n", rate);
+
+	return NULL;
+}
+
 static void omap_usb_dpll_relock(struct omap_usb3_phy *phy_regs)
 {
 	u32 val;
@@ -56,32 +78,36 @@ static void omap_usb_dpll_relock(struct omap_usb3_phy *phy_regs)
 
 static void omap_usb_dpll_lock(struct omap_usb3_phy *phy_regs)
 {
-	u32 clk_index = get_sys_clk_index();
+	struct usb3_dpll_params	*dpll_params;
 	u32 val;
 
+	dpll_params = omap_usb3_get_dpll_params();
+	if (!dpll_params)
+		return;
+
 	val = readl(&phy_regs->pll_config_1);
 	val &= ~PLL_REGN_MASK;
-	val |= omap_usb3_dpll_params[clk_index].n << PLL_REGN_SHIFT;
+	val |= dpll_params->n << PLL_REGN_SHIFT;
 	writel(val, &phy_regs->pll_config_1);
 
 	val = readl(&phy_regs->pll_config_2);
 	val &= ~PLL_SELFREQDCO_MASK;
-	val |= omap_usb3_dpll_params[clk_index].freq << PLL_SELFREQDCO_SHIFT;
+	val |= dpll_params->freq << PLL_SELFREQDCO_SHIFT;
 	writel(val, &phy_regs->pll_config_2);
 
 	val = readl(&phy_regs->pll_config_1);
 	val &= ~PLL_REGM_MASK;
-	val |= omap_usb3_dpll_params[clk_index].m << PLL_REGM_SHIFT;
+	val |= dpll_params->m << PLL_REGM_SHIFT;
 	writel(val, &phy_regs->pll_config_1);
 
 	val = readl(&phy_regs->pll_config_4);
 	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;
 	writel(val, &phy_regs->pll_config_4);
 
 	val = readl(&phy_regs->pll_config_3);
 	val &= ~PLL_SD_MASK;
-	val |= omap_usb3_dpll_params[clk_index].sd << PLL_SD_SHIFT;
+	val |= dpll_params->sd << PLL_SD_SHIFT;
 	writel(val, &phy_regs->pll_config_3);
 
 	omap_usb_dpll_relock(phy_regs);