5 files changed, 59 insertions, 3 deletions

diff --git a/firmware/target/arm/imx233/clkctrl-imx233.c b/firmware/target/arm/imx233/clkctrl-imx233.c
index ee77a77493..976a9f9d3b 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.c
+++ b/firmware/target/arm/imx233/clkctrl-imx233.c
@@ -118,3 +118,11 @@ void imx233_set_bypass_pll(enum imx233_clock_t clk, bool bypass)
         __REG_CLR(HW_CLKCTRL_CLKSEQ) = msk;
 }
 
+void imx233_enable_usb_pll(bool enable)
+{
+    if(enable)
+        __REG_SET(HW_CLKCTRL_PLLCTRL0) = HW_CLKCTRL_PLLCTRL0__EN_USB_CLKS;
+    else
+        __REG_CLR(HW_CLKCTRL_PLLCTRL0) = HW_CLKCTRL_PLLCTRL0__EN_USB_CLKS;
+}
+
diff --git a/firmware/target/arm/imx233/clkctrl-imx233.h b/firmware/target/arm/imx233/clkctrl-imx233.h
index f1a51e25b2..7aa4f716e8 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.h
+++ b/firmware/target/arm/imx233/clkctrl-imx233.h
@@ -28,8 +28,9 @@
 #define HW_CLKCTRL_BASE     0x80040000
 
 #define HW_CLKCTRL_PLLCTRL0 (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x0))
-#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BP 20
-#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BM (3 << 20)
+#define HW_CLKCTRL_PLLCTRL0__EN_USB_CLKS    (1 << 18)
+#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BP     20
+#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BM     (3 << 20)
 
 #define HW_CLKCTRL_PLLCTRL1 (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x10))
 
@@ -90,5 +91,6 @@ void imx233_set_clock_divisor(enum imx233_clock_t clk, int div);
 /* call with fracdiv=0 to disable it */
 void imx233_set_fractional_divisor(enum imx233_clock_t clk, int fracdiv);
 void imx233_set_bypass_pll(enum imx233_clock_t clk, bool bypass);
+void imx233_enable_usb_pll(bool enable);
 
 #endif /* CLKCTRL_IMX233_H */
diff --git a/firmware/target/arm/imx233/system-imx233.c b/firmware/target/arm/imx233/system-imx233.c
index d27bb1b7d2..7b8e0c858e 100644
--- a/firmware/target/arm/imx233/system-imx233.c
+++ b/firmware/target/arm/imx233/system-imx233.c
@@ -253,3 +253,25 @@ void set_cpu_frequency(long frequency)
     }
 }
 #endif
+
+void imx233_enable_usb_controller(bool enable)
+{
+    if(enable)
+        __REG_CLR(HW_DIGCTL_CTRL) = HW_DIGCTL_CTRL__USB_CLKGATE;
+    else
+        __REG_SET(HW_DIGCTL_CTRL) = HW_DIGCTL_CTRL__USB_CLKGATE;
+}
+
+void imx233_enable_usb_phy(bool enable)
+{
+    if(enable)
+    {
+        __REG_CLR(HW_USBPHY_CTRL) = __BLOCK_CLKGATE | __BLOCK_SFTRST;
+        __REG_CLR(HW_USBPHY_PWD) = HW_USBPHY_PWD__ALL;
+    }
+    else
+    {
+        __REG_SET(HW_USBPHY_PWD) = HW_USBPHY_PWD__ALL;
+        __REG_SET(HW_USBPHY_CTRL) = __BLOCK_CLKGATE | __BLOCK_SFTRST;
+    }
+}
diff --git a/firmware/target/arm/imx233/system-target.h b/firmware/target/arm/imx233/system-target.h
index fd817a9950..29c175175b 100644
--- a/firmware/target/arm/imx233/system-target.h
+++ b/firmware/target/arm/imx233/system-target.h
@@ -28,9 +28,21 @@
 #include "clock-target.h" /* CPUFREQ_* are defined here */
 #include "power-imx233.h"
 
+/* Digital control */
 #define HW_DIGCTL_BASE          0x8001C000
+#define HW_DIGCTL_CTRL          (*(volatile uint32_t *)(HW_DIGCTL_BASE + 0))
+#define HW_DIGCTL_CTRL__USB_CLKGATE (1 << 2)
+
 #define HW_DIGCTL_MICROSECONDS  (*(volatile uint32_t *)(HW_DIGCTL_BASE + 0xC0))
 
+/* USB Phy */
+#define HW_USBPHY_BASE          0x8007C000 
+#define HW_USBPHY_PWD           (*(volatile uint32_t *)(HW_USBPHY_BASE + 0))
+#define HW_USBPHY_PWD__ALL      (7 << 10 | 0xf << 17)
+
+#define HW_USBPHY_CTRL          (*(volatile uint32_t *)(HW_USBPHY_BASE + 0x30))
+
+/* Interrupt collector */
 #define HW_ICOLL_BASE           0x80000000
 
 #define HW_ICOLL_VECTOR         (*(volatile uint32_t *)(HW_ICOLL_BASE + 0x0))
@@ -89,6 +101,8 @@ void udelay(unsigned us);
 bool imx233_us_elapsed(uint32_t ref, unsigned us_delay);
 void imx233_reset_block(volatile uint32_t *block_reg);
 void power_off(void);
+void imx233_enable_usb_controller(bool enable);
+void imx233_enable_usb_phy(bool enable);
 
 void udelay(unsigned usecs);
 
diff --git a/firmware/target/arm/imx233/usb-imx233.c b/firmware/target/arm/imx233/usb-imx233.c
index c2d355ad88..c275f76916 100644
--- a/firmware/target/arm/imx233/usb-imx233.c
+++ b/firmware/target/arm/imx233/usb-imx233.c
@@ -28,6 +28,7 @@
 #include "usb-target.h"
 #include "system.h"
 #include "system-target.h"
+#include "clkctrl-imx233.h"
 
 
 void usb_insert_int(void)
@@ -77,9 +78,18 @@ bool usb_plugged(void)
 
 void usb_enable(bool on)
 {
-    /* FIXME: power up/down usb phy and pll usb */
     if(on)
+    {
+        imx233_enable_usb_pll(true);
+        imx233_enable_usb_phy(true);
+        imx233_enable_usb_controller(true);
         usb_core_init();
+    }
     else
+    {
         usb_core_exit();
+        imx233_enable_usb_controller(false);
+        imx233_enable_usb_phy(false);
+        imx233_enable_usb_pll(false);
+    }
 }