From 8305058cf5cddac8f53f0ddca13d4bf762c2046c Mon Sep 17 00:00:00 2001
From: CTCaer <ctcaer@gmail.com>
Date: Sun, 15 Nov 2020 14:42:01 +0200
Subject: [PATCH] clock: Move PLLU init/deinit from USB to clock

---
 bdk/soc/clock.c | 26 ++++++++++++++++++++++++++
 bdk/soc/clock.h |  2 ++
 bdk/usb/usbd.c  | 23 ++++-------------------
 3 files changed, 32 insertions(+), 19 deletions(-)

diff --git a/bdk/soc/clock.c b/bdk/soc/clock.c
index 94146f2..227f595 100644
--- a/bdk/soc/clock.c
+++ b/bdk/soc/clock.c
@@ -362,6 +362,32 @@ static void _clock_disable_pllc4(u32 mask)
 	pllc4_enabled = 0;
 }
 
+void clock_enable_pllu()
+{
+	// Configure PLLU.
+	CLOCK(CLK_RST_CONTROLLER_PLLU_MISC) |= (1 << 29); // Disable reference clock.
+	u32 pllu_cfg = (CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) & 0xFFE00000) | (1 << 24) | (1 << 16) | (0x19 << 8) | 2;
+	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) = pllu_cfg;
+	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) = pllu_cfg | (1 << 30); // Enable.
+
+	// Wait for PLL to stabilize.
+	u32 timeout = (u32)TMR(TIMERUS_CNTR_1US) + 1300;
+	while (!(CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) & (1 << 27))) // PLL_LOCK.
+		if ((u32)TMR(TIMERUS_CNTR_1US) > timeout)
+			break;
+	usleep(10);
+
+	// Enable PLLU USB/HSIC/ICUSB/48M.
+	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) |= 0x2E00000;
+}
+
+void clock_disable_pllu()
+{
+	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) &= ~0x2E00000;  // Disable PLLU USB/HSIC/ICUSB/48M.
+	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) &= ~0x40000000; // Disable PLLU.
+	CLOCK(CLK_RST_CONTROLLER_PLLU_MISC) &= ~0x20000000; // Enable reference clock.
+}
+
 static int _clock_sdmmc_is_reset(u32 id)
 {
 	switch (id)
diff --git a/bdk/soc/clock.h b/bdk/soc/clock.h
index 3edb5e8..fbce54e 100644
--- a/bdk/soc/clock.h
+++ b/bdk/soc/clock.h
@@ -478,6 +478,8 @@ void clock_enable_pwm();
 void clock_disable_pwm();
 void clock_enable_pllc(u32 divn);
 void clock_disable_pllc();
+void clock_enable_pllu();
+void clock_disable_pllu();
 void clock_sdmmc_config_clock_source(u32 *pclock, u32 id, u32 val);
 void clock_sdmmc_get_card_clock_div(u32 *pclock, u16 *pdivisor, u32 type);
 int  clock_sdmmc_is_not_reset_and_enabled(u32 id);
diff --git a/bdk/usb/usbd.c b/bdk/usb/usbd.c
index b43b790..5dbfe4e 100644
--- a/bdk/usb/usbd.c
+++ b/bdk/usb/usbd.c
@@ -247,21 +247,8 @@ int usb_device_init()
 	if (usb_init_done)
 		return 0;
 
-	// Configure PLLU.
-	CLOCK(CLK_RST_CONTROLLER_PLLU_MISC) |= (1 << 29); // Disable reference clock.
-	u32 pllu_cfg = (((((CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) >> 8 << 8) | 2) & 0xFFFF00FF) | ((0x19 << 8) & 0xFFFF)) & 0xFFE0FFFF) | (1 << 16) | (1 << 24);
-	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) = pllu_cfg;
-	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) = pllu_cfg | (1 << 30); // Enable.
-
-	// Wait for PLL to stabilize.
-	u32 timeout = (u32)TMR(TIMERUS_CNTR_1US) + 1300;
-	while (!(CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) & (1 << 27))) // PLL_LOCK.
-		if ((u32)TMR(TIMERUS_CNTR_1US) > timeout)
-			break;
-	usleep(10);
-
-	// Enable PLLU USB/HSIC/ICUSB/48M.
-	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) |= 0x2E00000;
+	// Configure and enable PLLU.
+	clock_enable_pllu();
 
 	// Enable USBD clock.
 	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) = BIT(CLK_L_USBD);
@@ -453,10 +440,8 @@ static void _usb_device_power_down()
 	// Disable USBD clock.
 	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_CLR) = BIT(CLK_L_USBD);
 
-	// Completely disable PLLU.
-	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) &= ~0x2E00000;  // Disable PLLU USB/HSIC/ICUSB/48M.
-	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) &= ~0x40000000; // Disable PLLU.
-	CLOCK(CLK_RST_CONTROLLER_PLLU_MISC) &= ~0x20000000; // Enable reference clock.
+	// Disable PLLU.
+	clock_disable_pllu();
 
 	usb_init_done = false;
 }