1 files changed, 37 insertions, 0 deletions

diff --git a/firmware/target/arm/s5l8700/usb-nano2g-6g.c b/firmware/target/arm/s5l8700/usb-nano2g-6g.c
index 7f5d38a5ae..b070f2be3b 100644
--- a/firmware/target/arm/s5l8700/usb-nano2g-6g.c
+++ b/firmware/target/arm/s5l8700/usb-nano2g-6g.c
@@ -21,9 +21,12 @@
 
 #include "config.h"
 #include "usb.h"
+#include "usb-s3c6400x.h"
+#include "cpu.h"
 
 #ifdef HAVE_USBSTACK
 #include "usb_core.h"
+#include "usb_drv.h"
 #include "power.h"
 
 void usb_enable(bool on)
@@ -38,6 +41,24 @@ int usb_detect(void)
         return USB_INSERTED;
     return USB_EXTRACTED;
 }
+
+void usb_init_device(void)
+{
+    /* Power up the core clocks to allow writing
+       to some registers needed to power it down */
+    PCGCCTL = 0;
+#if CONFIG_CPU==S5L8701
+    PWRCON &= ~0x4000;
+    PWRCONEXT &= ~0x800;
+    INTMSK |= INTMSK_USB_OTG;
+#elif CONFIG_CPU==S5L8702
+    PWRCON(0) &= ~0x4;
+    PWRCON(1) &= ~0x8;
+    VIC0INTENABLE |= 1 << 19;
+#endif
+
+    usb_drv_exit();
+}
 #else
 void usb_enable(bool on)
 {
@@ -48,4 +69,20 @@ int usb_detect(void)
 {
     return USB_EXTRACTED;
 }
+
+void usb_init_device(void)
+{
+    DCTL = DCTL_pwronprgdone | DCTL_sftdiscon;
+
+    ORSTCON = 1;  /* Put the PHY into reset (needed to get current down) */
+    PCGCCTL = 1;  /* Shut down PHY clock */
+    OPHYPWR = 0xF;  /* PHY: Power down */
+
+#if CONFIG_CPU==S5L8701
+    PWRCON |= 0x4000;
+    PWRCONEXT |= 0x800;
+#elif CONFIG_CPU==S5L8702
+    PWRCON(0) |= 0x4;
+    PWRCON(1) |= 0x8;
+#endif
 #endif