2 files changed, 34 insertions, 8 deletions

diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index dbbe1a1905..ae7c71d939 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -320,16 +320,16 @@ void system_init(void)
         "mcr p15, 0, r0, c1, c0   \n"
         : : : "r0" );
 
-    CGU_COUNTA = 0xff;
+    CGU_COUNTA = CGU_LOCK_CNT;
     CGU_PLLA = AS3525_PLLA_SETTING;
-    CGU_PLLASUP = 0;        /* enable PLLA */
-    while(!(CGU_INTCTRL & (1<<0)));           /* wait until PLLA is locked */
-    
-#if (defined(USE_ROCKBOX_USB) && CONFIG_CPU==AS3525) || (AS3525_MCLK_SEL == AS3525_CLK_PLLB)
-    CGU_COUNTB = 0xff;
+    CGU_PLLASUP = 0;                          /* enable PLLA */
+    while(!(CGU_INTCTRL & CGU_PLLA_LOCK));    /* wait until PLLA is locked */
+
+#if AS3525_MCLK_SEL == AS3525_CLK_PLLB
+    CGU_COUNTB = CGU_LOCK_CNT;
     CGU_PLLB = AS3525_PLLB_SETTING;
-    CGU_PLLBSUP = 0;        /* enable PLLB */
-    while(!(CGU_INTCTRL & (1<<1)));           /* wait until PLLB is locked */
+    CGU_PLLBSUP = 0;                          /* enable PLLB */
+    while(!(CGU_INTCTRL & CGU_PLLB_LOCK));    /* wait until PLLB is locked */
 #endif
 
     /*  Set FCLK frequency */
diff --git a/firmware/target/arm/as3525/usb-drv-as3525.c b/firmware/target/arm/as3525/usb-drv-as3525.c
index 2cb1318f72..61a1265f42 100644
--- a/firmware/target/arm/as3525/usb-drv-as3525.c
+++ b/firmware/target/arm/as3525/usb-drv-as3525.c
@@ -306,6 +306,29 @@ static struct usb_dev_setup_buf dummy __attribute__((unused));
 static char rx_buf[1024];
 static char tx_buf[1024];
 
+#if AS3525_MCLK_SEL != AS3525_CLK_PLLB
+static inline void usb_enable_pll(void)
+{
+    CGU_COUNTB = CGU_LOCK_CNT;
+    CGU_PLLB = AS3525_PLLB_SETTING;
+    CGU_PLLBSUP = 0;                       /* enable PLLB */
+    while(!(CGU_INTCTRL & CGU_PLLB_LOCK)); /* wait until PLLB is locked */
+}
+
+static inline void usb_disable_pll(void)
+{
+    CGU_PLLBSUP = CGU_PLL_POWERDOWN;
+}
+#else
+static inline void usb_enable_pll(void)
+{
+}
+
+static inline void usb_disable_pll(void)
+{
+}
+#endif /* AS3525_MCLK_SEL != AS3525_CLK_PLLB */
+
 void usb_attach(void)
 {
     usb_enable(true);
@@ -448,6 +471,8 @@ void usb_drv_init(void)
 {
     logf("usb_drv_init() !!!!\n");
 
+    usb_enable_pll();
+
     /* length regulator: normal operation */
     ascodec_write(AS3514_CVDD_DCDC3, ascodec_read(AS3514_CVDD_DCDC3) | 1<<2);
 
@@ -524,6 +549,7 @@ void usb_drv_exit(void)
     CGU_PERI &= ~CGU_USB_CLOCK_ENABLE;
     /* Disable UVDD generating LDO */
     ascodec_write(AS3515_USB_UTIL, ascodec_read(AS3515_USB_UTIL) & ~(1<<4));
+    usb_disable_pll();
     logf("usb_drv_exit() !!!!\n");
 }