1 files changed, 55 insertions, 3 deletions

diff --git a/arch/arm/mach-omap2/dpll3xxx.c b/arch/arm/mach-omap2/dpll3xxx.c
index 38207d5..2499afb 100644
--- a/arch/arm/mach-omap2/dpll3xxx.c
+++ b/arch/arm/mach-omap2/dpll3xxx.c
@@ -32,8 +32,10 @@
 #include <plat/clock.h>
 
 #include "clock.h"
+#include "cm2_44xx.h"
 #include "cm2xxx_3xxx.h"
 #include "cm-regbits-34xx.h"
+#include "cm-regbits-44xx.h"
 
 /* CM_AUTOIDLE_PLL*.AUTO_* bit values */
 #define DPLL_AUTOIDLE_DISABLE			0x0
@@ -61,24 +63,75 @@ static void _omap3_dpll_write_clken(struct clk *clk, u8 clken_bits)
 static int _omap3_wait_dpll_status(struct clk *clk, u8 state)
 {
 	const struct dpll_data *dd;
-	int i = 0;
+	int i;
 	int ret = -EINVAL;
+	bool first_time = true;
+	u32 reg;
+	u32 orig_cm_div_m2_dpll_usb;
+	u32 orig_cm_clkdcoldo_dpll_usb;
 
+retry:
 	dd = clk->dpll_data;
 
 	state <<= __ffs(dd->idlest_mask);
 
+	i = 0;
 	while (((__raw_readl(dd->idlest_reg) & dd->idlest_mask) != state) &&
 	       i < MAX_DPLL_WAIT_TRIES) {
 		i++;
 		udelay(1);
 	}
 
+	/* restore back old values if hit work-around */
+	if (!first_time) {
+		__raw_writel(orig_cm_div_m2_dpll_usb,
+				OMAP4430_CM_DIV_M2_DPLL_USB);
+		__raw_writel(orig_cm_clkdcoldo_dpll_usb,
+				OMAP4430_CM_CLKDCOLDO_DPLL_USB);
+	}
+
 	if (i == MAX_DPLL_WAIT_TRIES) {
 		printk(KERN_ERR "clock: %s failed transition to '%s'\n",
 		       clk->name, (state) ? "locked" : "bypassed");
-		/* Catch failing usb-dpll lock error */
+
+		/* Try Error Recovery: for failing usbdpll locking */
 		if (!strcmp(clk->name, "dpll_usb_ck")) {
+			reg = __raw_readl(dd->mult_div1_reg);
+
+			/* Put in MN bypass */
+			_omap3_dpll_write_clken(clk, DPLL_MN_BYPASS);
+			i = 0;
+			while (!(__raw_readl(dd->idlest_reg) & (1 << OMAP4430_ST_MN_BYPASS_SHIFT)) &&
+					i < MAX_DPLL_WAIT_TRIES) {
+				i++;
+				udelay(1);
+			}
+
+			/* MN bypass looses contents of CM_CLKSEL_DPLL_USB */
+			__raw_writel(reg, dd->mult_div1_reg);
+
+			/* Force generate request to PRCM: put in Force mode */
+
+			/* a) CM_DIV_M2_DPLL_USB.DPLL_CLKOUT_GATE_CTRL = 1 */
+			orig_cm_div_m2_dpll_usb = __raw_readl(OMAP4430_CM_DIV_M2_DPLL_USB);
+			__raw_writel(orig_cm_div_m2_dpll_usb |
+					(1 << OMAP4430_DPLL_CLKOUT_GATE_CTRL_SHIFT),
+					OMAP4430_CM_DIV_M2_DPLL_USB);
+
+			/* b) CM_CLKDCOLDO_DPLL_USB.DPLL_CLKDCOLDO_GATE_CTRL = 1 */
+			orig_cm_clkdcoldo_dpll_usb = __raw_readl(OMAP4430_CM_CLKDCOLDO_DPLL_USB);
+			__raw_writel(orig_cm_clkdcoldo_dpll_usb |
+					(1 << OMAP4430_DPLL_CLKDCOLDO_GATE_CTRL_SHIFT),
+					OMAP4430_CM_CLKDCOLDO_DPLL_USB);
+
+			/* Put back to locked mode */
+			_omap3_dpll_write_clken(clk, DPLL_LOCKED);
+
+			if (first_time) {
+				first_time = false;
+				goto retry;
+			}
+
 			pr_info("\n========== USB DPLL DUMP ===========\n");
 			pr_info("CM_CLKMODE_DPLL_USB         :%08x\n", omap_readl(0x4A008180));
 			pr_info("CM_IDLEST_DPLL_USB          :%08x\n", omap_readl(0x4A008184));
@@ -90,7 +143,6 @@ static int _omap3_wait_dpll_status(struct clk *clk, u8 state)
 			pr_info("CM_CLKDCOLDO_DPLL_USB       :%08x\n", omap_readl(0x4A0081B4));
 			pr_info("========== USB DPLL DUMP: End ===========\n");
 		}
-
 	} else {
 		pr_debug("clock: %s transition to '%s' in %d loops\n",
 			 clk->name, (state) ? "locked" : "bypassed", i);