1 files changed, 13 insertions, 54 deletions

diff --git a/firmware/target/arm/tms320dm320/sansa-connect/usb-sansaconnect.c b/firmware/target/arm/tms320dm320/sansa-connect/usb-sansaconnect.c
index abe6622f0b..986efe374c 100644
--- a/firmware/target/arm/tms320dm320/sansa-connect/usb-sansaconnect.c
+++ b/firmware/target/arm/tms320dm320/sansa-connect/usb-sansaconnect.c
@@ -7,7 +7,7 @@
  *                     \/            \/     \/    \/            \/
  * $Id: $
  *
- * Copyright (C) 2011 by Tomasz Moń
+ * Copyright (C) 2011-2021 by Tomasz Moń
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -22,52 +22,12 @@
 #include "config.h"
 #include "system.h"
 #include "kernel.h"
-#include "usb.h"
-#ifdef HAVE_USBSTACK
-#include "usb_drv.h"
 #include "usb_core.h"
-#endif
-
-static bool usb_is_connected = false;
 
 static int usb_detect_callback(struct timeout *tmo)
 {
     (void)tmo;
-
-    if (IO_GIO_BITSET0 & (1 << 9))
-    {
-        /* Set GIO33 as normal output, drive it low */
-        IO_GIO_FSEL3 &= ~(0x0003);
-        IO_GIO_BITCLR2 = (1 << 1);
-
-        /* Disable M48XI crystal resonator */
-        IO_CLK_LPCTL1 |= 0x01;
-
-        /* Drive reset low */
-        IO_GIO_BITCLR0 = (1 << 7);
-
-        /* Disable VLYNQ clock */
-        IO_CLK_MOD2 &= ~(1 << 13);
-
-        usb_is_connected = false;
-    }
-    else
-    {
-        /* Enable M48XI crystal resonator */
-        IO_CLK_LPCTL1 &= ~(0x01);
-
-        /* Set GIO33 as CLKOUT1B */
-        IO_GIO_FSEL3 |= 0x0003;
-
-        /* Drive reset high */
-        IO_GIO_BITSET0 = (1 << 7);
-
-        /* Enable VLYNQ clock */
-        IO_CLK_MOD2 |= (1 << 13);
-
-        usb_is_connected = true;
-    }
- 
+    usb_status_event(usb_detect());
     return 0;
 }
 
@@ -82,20 +42,15 @@ void GIO9(void)
     timeout_register(&usb_oneshot, usb_detect_callback, HZ, 0);
 }
 
-bool usb_drv_connected(void)
-{
-    return false;
-}
-
 int usb_detect(void)
 {
-    if (usb_is_connected == true)
+    if (IO_GIO_BITSET0 & (1 << 9))
     {
-        return USB_INSERTED;
+        return USB_EXTRACTED;
     }
     else
     {
-        return USB_EXTRACTED;
+        return USB_INSERTED;
     }
 }
 
@@ -127,14 +82,18 @@ void usb_init_device(void)
 
     /* Enable USB insert detection interrupt */
     IO_INTC_EINT1 |= (1 << 14);
-
-    /* Check if USB is connected */
-    usb_detect_callback(NULL);
 }
 
 void usb_enable(bool on)
 {
-    (void)on;
+    if (on)
+    {
+        usb_core_init();
+    }
+    else
+    {
+        usb_core_exit();
+    }
 }
 
 void usb_attach(void)