usb: phy: omap-usb3: Improve DPLL parameter lookup code

#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

 */

# 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);

	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;

{

	u32			val;

	unsigned long		rate;

	enum sys_clk_rate	clk_index;

	struct usb_dpll_params *dpll_params;

	rate = clk_get_rate(phy->sys_clk);

	clk_index	= __get_sys_clk_index(rate);

	if (clk_index == CLK_RATE_UNDEFINED) {

		pr_err("dpll cannot be locked for sys clk freq:%luHz\n", rate);

	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);

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;