ti: dwc3: Enable USB clocks on-demand

index c94a807..e28b795 100644
@@ -648,6 +648,14 @@ static inline void enable_clock_domain(u32 const clkctrl_reg, u32 enable_mode)
 	debug("Enable clock domain - %x\n", clkctrl_reg);
 }
 
+static inline void disable_clock_domain(u32 const clkctrl_reg)
+{
+	clrsetbits_le32(clkctrl_reg, CD_CLKCTRL_CLKTRCTRL_MASK,
+			CD_CLKCTRL_CLKTRCTRL_SW_SLEEP <<
+			CD_CLKCTRL_CLKTRCTRL_SHIFT);
+	debug("Disable clock domain - %x\n", clkctrl_reg);
+}
+
 static inline void wait_for_clk_enable(u32 clkctrl_addr)
 {
 	u32 clkctrl, idlest = MODULE_CLKCTRL_IDLEST_DISABLED;

@@ -677,6 +685,34 @@ static inline void enable_clock_module(u32 const clkctrl_addr, u32 enable_mode,
 		wait_for_clk_enable(clkctrl_addr);
 }
 
+static inline void wait_for_clk_disable(u32 clkctrl_addr)
+{
+	u32 clkctrl, idlest = MODULE_CLKCTRL_IDLEST_FULLY_FUNCTIONAL;
+	u32 bound = LDELAY;
+
+	while ((idlest != MODULE_CLKCTRL_IDLEST_DISABLED)) {
+		clkctrl = readl(clkctrl_addr);
+		idlest = (clkctrl & MODULE_CLKCTRL_IDLEST_MASK) >>
+			 MODULE_CLKCTRL_IDLEST_SHIFT;
+		if (--bound == 0) {
+			printf("Clock disable failed for 0x%x idlest 0x%x\n",
+			       clkctrl_addr, clkctrl);
+			return;
+		}
+	}
+}
+
+static inline void disable_clock_module(u32 const clkctrl_addr,
+					u32 wait_for_disable)
+{
+	clrsetbits_le32(clkctrl_addr, MODULE_CLKCTRL_MODULEMODE_MASK,
+			MODULE_CLKCTRL_MODULEMODE_SW_DISABLE <<
+			MODULE_CLKCTRL_MODULEMODE_SHIFT);
+	debug("Disable clock module - %x\n", clkctrl_addr);
+	if (wait_for_disable)
+		wait_for_clk_disable(clkctrl_addr);
+}
+
 void freq_update_core(void)
 {
 	u32 freq_config1 = 0;

@@ -800,6 +836,23 @@ void do_enable_clocks(u32 const *clk_domains,
 	}
 }
 
+void do_disable_clocks(u32 const *clk_domains,
+			    u32 const *clk_modules_disable,
+			    u8 wait_for_disable)
+{
+	u32 i, max = 100;
+
+
+	/* Clock modules that need to be put in SW_DISABLE */
+	for (i = 0; (i < max) && clk_modules_disable[i]; i++)
+		disable_clock_module(clk_modules_disable[i],
+				     wait_for_disable);
+
+	/* Put the clock domains in SW_SLEEP mode */
+	for (i = 0; (i < max) && clk_domains[i]; i++)
+		disable_clock_domain(clk_domains[i]);
+}
+
 void prcm_init(void)
 {
 	switch (omap_hw_init_context()) {

index 462a9ee..224fbf0 100644
@@ -577,6 +577,10 @@ void do_enable_clocks(u32 const *clk_domains,
 		      u32 const *clk_modules_explicit_en,
 		      u8 wait_for_enable);
 
+void do_disable_clocks(u32 const *clk_domains,
+		       u32 const *clk_modules_disable,
+		       u8 wait_for_disable);
+
 void setup_post_dividers(u32 const base,
 			const struct dpll_params *params);
 u32 omap_ddr_clk(void);

index 11440ac..1085bb3 100644
@@ -581,6 +581,103 @@ void enable_basic_uboot_clocks(void)
 			 1);
 }
 
+#ifdef CONFIG_USB_DWC3
+void enable_usb_clocks(int index)
+{
+	u32 cm_l3init_usb_otg_ss_clkctrl = 0;
+
+	if (index == 0) {
+		cm_l3init_usb_otg_ss_clkctrl =
+			(*prcm)->cm_l3init_usb_otg_ss1_clkctrl;
+		/* Enable 960 MHz clock for dwc3 */
+		setbits_le32((*prcm)->cm_l3init_usb_otg_ss1_clkctrl,
+			     OPTFCLKEN_REFCLK960M);
+
+		/* Enable 32 KHz clock for dwc3 */
+		setbits_le32((*prcm)->cm_coreaon_usb_phy1_core_clkctrl,
+			     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
+	} else if (index == 1) {
+		cm_l3init_usb_otg_ss_clkctrl =
+			(*prcm)->cm_l3init_usb_otg_ss2_clkctrl;
+		/* Enable 960 MHz clock for dwc3 */
+		setbits_le32((*prcm)->cm_l3init_usb_otg_ss2_clkctrl,
+			     OPTFCLKEN_REFCLK960M);
+
+		/* Enable 32 KHz clock for dwc3 */
+		setbits_le32((*prcm)->cm_coreaon_usb_phy2_core_clkctrl,
+			     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
+
+		/* Enable 60 MHz clock for USB2PHY2 */
+		setbits_le32((*prcm)->cm_coreaon_l3init_60m_gfclk_clkctrl,
+			     L3INIT_CLKCTRL_OPTFCLKEN_60M_GFCLK);
+	}
+
+	u32 const clk_domains_usb[] = {
+		0
+	};
+
+	u32 const clk_modules_hw_auto_usb[] = {
+		(*prcm)->cm_l3init_ocp2scp1_clkctrl,
+		cm_l3init_usb_otg_ss_clkctrl,
+		0
+	};
+
+	u32 const clk_modules_explicit_en_usb[] = {
+		0
+	};
+
+	do_enable_clocks(clk_domains_usb,
+			 clk_modules_hw_auto_usb,
+			 clk_modules_explicit_en_usb,
+			 1);
+}
+
+void disable_usb_clocks(int index)
+{
+	u32 cm_l3init_usb_otg_ss_clkctrl = 0;
+
+	if (index == 0) {
+		cm_l3init_usb_otg_ss_clkctrl =
+			(*prcm)->cm_l3init_usb_otg_ss1_clkctrl;
+		/* Disable 960 MHz clock for dwc3 */
+		clrbits_le32((*prcm)->cm_l3init_usb_otg_ss1_clkctrl,
+			     OPTFCLKEN_REFCLK960M);
+
+		/* Disable 32 KHz clock for dwc3 */
+		clrbits_le32((*prcm)->cm_coreaon_usb_phy1_core_clkctrl,
+			     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
+	} else if (index == 1) {
+		cm_l3init_usb_otg_ss_clkctrl =
+			(*prcm)->cm_l3init_usb_otg_ss2_clkctrl;
+		/* Disable 960 MHz clock for dwc3 */
+		clrbits_le32((*prcm)->cm_l3init_usb_otg_ss2_clkctrl,
+			     OPTFCLKEN_REFCLK960M);
+
+		/* Disable 32 KHz clock for dwc3 */
+		clrbits_le32((*prcm)->cm_coreaon_usb_phy2_core_clkctrl,
+			     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
+
+		/* Disable 60 MHz clock for USB2PHY2 */
+		clrbits_le32((*prcm)->cm_coreaon_l3init_60m_gfclk_clkctrl,
+			     L3INIT_CLKCTRL_OPTFCLKEN_60M_GFCLK);
+	}
+
+	u32 const clk_domains_usb[] = {
+		0
+	};
+
+	u32 const clk_modules_disable[] = {
+		(*prcm)->cm_l3init_ocp2scp1_clkctrl,
+		cm_l3init_usb_otg_ss_clkctrl,
+		0
+	};
+
+	do_disable_clocks(clk_domains_usb,
+			  clk_modules_disable,
+			  1);
+}
+#endif
+
 const struct ctrl_ioregs ioregs_omap5430 = {
 	.ctrl_ddrch = DDR_IO_I_34OHM_SR_FASTEST_WD_DQ_NO_PULL_DQS_PULL_DOWN,
 	.ctrl_lpddr2ch = DDR_IO_I_34OHM_SR_FASTEST_WD_CK_CKE_NCS_CA_PULL_DOWN,

index 224fbf0..527e143 100644
@@ -587,6 +587,12 @@ u32 omap_ddr_clk(void);
 u32 get_sys_clk_index(void);
 void enable_basic_clocks(void);
 void enable_basic_uboot_clocks(void);
+
+#ifdef CONFIG_USB_DWC3
+void enable_usb_clocks(int index);
+void disable_usb_clocks(int index);
+#endif
+
 void scale_vcores(struct vcores_data const *);
 u32 get_offset_code(u32 volt_offset, struct pmic_data *pmic);
 void do_scale_vcore(u32 vcore_reg, u32 volt_mv, struct pmic_data *pmic);

index ec7d468..f6a4cee 100644
@@ -144,6 +144,34 @@ static inline void enable_clock_module(u32 *const clkctrl_addr, u32 enable_mode,
 		wait_for_clk_enable(clkctrl_addr);
 }
 
+static inline void wait_for_clk_disable(u32 *clkctrl_addr)
+{
+	u32 clkctrl, idlest = MODULE_CLKCTRL_IDLEST_FULLY_FUNCTIONAL;
+	u32 bound = LDELAY;
+
+	while ((idlest != MODULE_CLKCTRL_IDLEST_DISABLED)) {
+		clkctrl = readl(clkctrl_addr);
+		idlest = (clkctrl & MODULE_CLKCTRL_IDLEST_MASK) >>
+			 MODULE_CLKCTRL_IDLEST_SHIFT;
+		if (--bound == 0) {
+			printf("Clock disable failed for 0x%p idlest 0x%x\n",
+			       clkctrl_addr, clkctrl);
+			return;
+		}
+	}
+}
+
+static inline void disable_clock_module(u32 *const clkctrl_addr,
+					u32 wait_for_disable)
+{
+	clrsetbits_le32(clkctrl_addr, MODULE_CLKCTRL_MODULEMODE_MASK,
+			MODULE_CLKCTRL_MODULEMODE_SW_DISABLE <<
+			MODULE_CLKCTRL_MODULEMODE_SHIFT);
+	debug("Disable clock module - %p\n", clkctrl_addr);
+	if (wait_for_disable)
+		wait_for_clk_disable(clkctrl_addr);
+}
+
 static inline void enable_clock_domain(u32 *const clkctrl_reg, u32 enable_mode)
 {
 	clrsetbits_le32(clkctrl_reg, CD_CLKCTRL_CLKTRCTRL_MASK,

@@ -151,6 +179,14 @@ static inline void enable_clock_domain(u32 *const clkctrl_reg, u32 enable_mode)
 	debug("Enable clock domain - %p\n", clkctrl_reg);
 }
 
+static inline void disable_clock_domain(u32 *const clkctrl_reg)
+{
+	clrsetbits_le32(clkctrl_reg, CD_CLKCTRL_CLKTRCTRL_MASK,
+			CD_CLKCTRL_CLKTRCTRL_SW_SLEEP <<
+			CD_CLKCTRL_CLKTRCTRL_SHIFT);
+	debug("Disable clock domain - %p\n", clkctrl_reg);
+}
+
 void do_enable_clocks(u32 *const *clk_domains,
 		      u32 *const *clk_modules_explicit_en, u8 wait_for_enable)
 {

@@ -170,6 +206,23 @@ void do_enable_clocks(u32 *const *clk_domains,
 	};
 }
 
+void do_disable_clocks(u32 *const *clk_domains,
+			    u32 *const *clk_modules_disable,
+			    u8 wait_for_disable)
+{
+	u32 i, max = 100;
+
+
+	/* Clock modules that need to be put in SW_DISABLE */
+	for (i = 0; (i < max) && clk_modules_disable[i]; i++)
+		disable_clock_module(clk_modules_disable[i],
+				     wait_for_disable);
+
+	/* Put the clock domains in SW_SLEEP mode */
+	for (i = 0; (i < max) && clk_domains[i]; i++)
+		disable_clock_domain(clk_domains[i]);
+}
+
 /*
  * Before scaling up the clocks we need to have the PMIC scale up the
  * voltages first.  This will be dependent on which PMIC is in use

index 4af6b57..a6d2419 100644
@@ -112,5 +112,6 @@ void do_setup_dpll(const struct dpll_regs *, const struct dpll_params *);
 void prcm_init(void);
 void enable_basic_clocks(void);
 void do_enable_clocks(u32 *const *, u32 *const *, u8);
+void do_disable_clocks(u32 *const *, u32 *const *, u8);
 
 #endif

index b1c0025..cd8931e 100644
@@ -134,3 +134,73 @@ void enable_basic_clocks(void)
 	/* For OPP100 the mac clock should be /5. */
 	writel(0x4, &cmdpll->clkselmacclk);
 }
+
+#ifdef CONFIG_USB_DWC3
+void enable_usb_clocks(int index)
+{
+	u32 *usbclkctrl = 0;
+	u32 *usbphyocp2scpclkctrl = 0;
+
+	if (index == 0) {
+		usbclkctrl = &cmper->usb0clkctrl;
+		usbphyocp2scpclkctrl = &cmper->usbphyocp2scp0clkctrl;
+		setbits_le32(&cmper->usb0clkctrl,
+			     USBOTGSSX_CLKCTRL_OPTFCLKEN_REFCLK960);
+		setbits_le32(&cmwkup->usbphy0clkctrl,
+			     USBPHY0_CLKCTRL_OPTFCLKEN_CLK32K);
+	} else if (index == 1) {
+		usbclkctrl = &cmper->usb1clkctrl;
+		usbphyocp2scpclkctrl = &cmper->usbphyocp2scp1clkctrl;
+		setbits_le32(&cmper->usb1clkctrl,
+			     USBOTGSSX_CLKCTRL_OPTFCLKEN_REFCLK960);
+		setbits_le32(&cmwkup->usbphy1clkctrl,
+			     USBPHY0_CLKCTRL_OPTFCLKEN_CLK32K);
+	}
+
+	u32 *const clk_domains_usb[] = {
+		0
+	};
+
+	u32 *const clk_modules_explicit_en_usb[] = {
+		usbclkctrl,
+		usbphyocp2scpclkctrl,
+		0
+	};
+
+	do_enable_clocks(clk_domains_usb, clk_modules_explicit_en_usb, 1);
+}
+
+void disable_usb_clocks(int index)
+{
+	u32 *usbclkctrl = 0;
+	u32 *usbphyocp2scpclkctrl = 0;
+
+	if (index == 0) {
+		usbclkctrl = &cmper->usb0clkctrl;
+		usbphyocp2scpclkctrl = &cmper->usbphyocp2scp0clkctrl;
+		clrbits_le32(&cmper->usb0clkctrl,
+			     USBOTGSSX_CLKCTRL_OPTFCLKEN_REFCLK960);
+		clrbits_le32(&cmwkup->usbphy0clkctrl,
+			     USBPHY0_CLKCTRL_OPTFCLKEN_CLK32K);
+	} else if (index == 1) {
+		usbclkctrl = &cmper->usb1clkctrl;
+		usbphyocp2scpclkctrl = &cmper->usbphyocp2scp1clkctrl;
+		clrbits_le32(&cmper->usb1clkctrl,
+			     USBOTGSSX_CLKCTRL_OPTFCLKEN_REFCLK960);
+		clrbits_le32(&cmwkup->usbphy1clkctrl,
+			     USBPHY0_CLKCTRL_OPTFCLKEN_CLK32K);
+	}
+
+	u32 *const clk_domains_usb[] = {
+		0
+	};
+
+	u32 *const clk_modules_disable_usb[] = {
+		usbclkctrl,
+		usbphyocp2scpclkctrl,
+		0
+	};
+
+	do_disable_clocks(clk_domains_usb, clk_modules_disable_usb, 1);
+}
+#endif

index 91b614a..275cf7b 100644
@@ -42,3 +42,8 @@ void am33xx_spl_board_init(void);
 int am335x_get_efuse_mpu_max_freq(struct ctrl_dev *cdev);
 int am335x_get_tps65910_mpu_vdd(int sil_rev, int frequency);
 #endif
+
+#ifdef CONFIG_USB_DWC3
+void enable_usb_clocks(int index);
+void disable_usb_clocks(int index);
+#endif

index 1454976..770726c 100644
@@ -713,6 +713,7 @@ static struct ti_usb_phy_device usb_phy2_device = {
 
 int board_usb_init(int index, enum usb_init_type init)
 {
+	enable_usb_clocks(index);
 	switch (index) {
 	case 0:
 		if (init == USB_INIT_DEVICE) {

@@ -759,6 +760,7 @@ int board_usb_cleanup(int index, enum usb_init_type init)
 	default:
 		printf("Invalid Controller Index\n");
 	}
+	disable_usb_clocks(index);
 
 	return 0;
 }

index bcf8cf2..042f9ab 100644
@@ -356,10 +356,12 @@ static struct ti_usb_phy_device usb_phy2_device = {
 
 int board_usb_init(int index, enum usb_init_type init)
 {
+	enable_usb_clocks(index);
 	switch (index) {
 	case 0:
 		if (init == USB_INIT_DEVICE) {
 			printf("port %d can't be used as device\n", index);
+			disable_usb_clocks(index);
 			return -EINVAL;
 		} else {
 			usb_otg_ss1.dr_mode = USB_DR_MODE_HOST;

@@ -379,6 +381,7 @@ int board_usb_init(int index, enum usb_init_type init)
 			usb_otg_ss2_glue.vbus_id_status = OMAP_DWC3_VBUS_VALID;
 		} else {
 			printf("port %d can't be used as host\n", index);
+			disable_usb_clocks(index);
 			return -EINVAL;
 		}
 

@@ -405,6 +408,7 @@ int board_usb_cleanup(int index, enum usb_init_type init)
 	default:
 		printf("Invalid Controller Index\n");
 	}
+	disable_usb_clocks(index);
 	return 0;
 }
 

index 4849694..04fc8ed 100644
@@ -144,6 +144,7 @@ static struct ti_usb_phy_device usb_phy2_device = {
 
 int board_usb_init(int index, enum usb_init_type init)
 {
+	enable_usb_clocks(index);
 	switch (index) {
 	case 0:
 		if (init == USB_INIT_DEVICE) {

@@ -190,6 +191,7 @@ int board_usb_cleanup(int index, enum usb_init_type init)
 	default:
 		printf("Invalid Controller Index\n");
 	}
+	disable_usb_clocks(index);
 	return 0;
 }
 

index d0d0e0e..659877c 100644
@@ -95,6 +95,7 @@ int board_usb_init(int index, enum usb_init_type init)
 		usb_otg_ss_glue.vbus_id_status = OMAP_DWC3_ID_GROUND;
 	}
 
+	enable_usb_clocks(index);
 	ti_usb_phy_uboot_init(&usb_phy_device);
 	dwc3_omap_uboot_init(&usb_otg_ss_glue);
 	dwc3_uboot_init(&usb_otg_ss);

@@ -112,6 +113,7 @@ int board_usb_cleanup(int index, enum usb_init_type init)
 	ti_usb_phy_uboot_exit(index);
 	dwc3_uboot_exit(index);
 	dwc3_omap_uboot_exit(index);
+	disable_usb_clocks(index);
 
 	return 0;
 }

index cd8931e..30b4867 100644
@@ -111,21 +111,9 @@ void enable_basic_clocks(void)
 		&cmper->emifclkctrl,
 		&cmper->otfaemifclkctrl,
 		&cmper->qspiclkctrl,
-		&cmper->usb0clkctrl,
-		&cmper->usbphyocp2scp0clkctrl,
-		&cmper->usb1clkctrl,
-		&cmper->usbphyocp2scp1clkctrl,
 		0
 	};
 
-	setbits_le32(&cmper->usb0clkctrl,
-		     USBOTGSSX_CLKCTRL_OPTFCLKEN_REFCLK960);
-	setbits_le32(&cmwkup->usbphy0clkctrl,
-		     USBPHY0_CLKCTRL_OPTFCLKEN_CLK32K);
-	setbits_le32(&cmper->usb1clkctrl,
-		     USBOTGSSX_CLKCTRL_OPTFCLKEN_REFCLK960);
-	setbits_le32(&cmwkup->usbphy1clkctrl,
-		     USBPHY0_CLKCTRL_OPTFCLKEN_CLK32K);
 	do_enable_clocks(clk_domains, clk_modules_explicit_en, 1);
 
 	/* Select the Master osc clk as Timer2 clock source */

index 1085bb3..19f2c43 100644
@@ -460,13 +460,6 @@ void enable_basic_clocks(void)
 		(*prcm)->cm_l4per_gpio6_clkctrl,
 		(*prcm)->cm_l4per_gpio7_clkctrl,
 		(*prcm)->cm_l4per_gpio8_clkctrl,
-#if defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_XHCI_OMAP)
-		(*prcm)->cm_l3init_ocp2scp1_clkctrl,
-		(*prcm)->cm_l3init_usb_otg_ss1_clkctrl,
-#if defined(CONFIG_DRA7XX) || defined(CONFIG_AM57XX)
-		(*prcm)->cm_l3init_usb_otg_ss2_clkctrl,
-#endif
-#endif
 		0
 	};
 

@@ -498,29 +491,6 @@ void enable_basic_clocks(void)
 	setbits_le32((*prcm)->cm_l3init_hsmmc2_clkctrl,
 			HSMMC_CLKCTRL_CLKSEL_MASK);
 
-#if defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_XHCI_OMAP)
-	/* Enable 960 MHz clock for dwc3 */
-	setbits_le32((*prcm)->cm_l3init_usb_otg_ss1_clkctrl,
-		     OPTFCLKEN_REFCLK960M);
-
-	/* Enable 32 KHz clock for dwc3 */
-	setbits_le32((*prcm)->cm_coreaon_usb_phy1_core_clkctrl,
-		     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
-#if defined(CONFIG_DRA7XX) || defined(CONFIG_AM57XX)
-	/* Enable 960 MHz clock for dwc3 */
-	setbits_le32((*prcm)->cm_l3init_usb_otg_ss2_clkctrl,
-		     OPTFCLKEN_REFCLK960M);
-
-	/* Enable 32 KHz clock for dwc3 */
-	setbits_le32((*prcm)->cm_coreaon_usb_phy2_core_clkctrl,
-		     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
-
-	/* Enable 60 MHz clock for USB2PHY2 */
-	setbits_le32((*prcm)->cm_coreaon_l3init_60m_gfclk_clkctrl,
-		     L3INIT_CLKCTRL_OPTFCLKEN_60M_GFCLK);
-#endif
-#endif
-
 	/* Set the correct clock dividers for mmc */
 	setbits_le32((*prcm)->cm_l3init_hsmmc1_clkctrl,
 			HSMMC_CLKCTRL_CLKSEL_DIV_MASK);