3 files changed, 23 insertions, 12 deletions
diff --git a/src/lpc176x/internal.h b/src/lpc176x/internal.h
index 3adb7e19..23587011 100644
--- a/src/lpc176x/internal.h
+++ b/src/lpc176x/internal.h
@@ -23,5 +23,6 @@ int is_enabled_pclock(uint32_t pclk);
 void enable_pclock(uint32_t pclk);
 uint32_t get_pclock_frequency(uint32_t pclk);
 void gpio_peripheral(uint32_t gpio, int func, int pullup);
+void usb_disconnect(void);
 
 #endif // internal.h
diff --git a/src/lpc176x/main.c b/src/lpc176x/main.c
index f0fc6cde..7aa28552 100644
--- a/src/lpc176x/main.c
+++ b/src/lpc176x/main.c
@@ -6,6 +6,9 @@
 
 #include "autoconf.h" // CONFIG_CLOCK_FREQ
 #include "board/armcm_boot.h" // armcm_main
+#include "board/armcm_reset.h" // try_request_canboot
+#include "board/irq.h" // irq_disable
+#include "board/misc.h" // bootloader_request
 #include "internal.h" // enable_pclock
 #include "sched.h" // sched_main
 
@@ -37,6 +40,23 @@ DECL_INIT(watchdog_init);
  * misc functions
  ****************************************************************/
 
+// Try to reboot into bootloader
+void
+bootloader_request(void)
+{
+    if (!CONFIG_SMOOTHIEWARE_BOOTLOADER)
+        return;
+    try_request_canboot();
+    // Disable USB and pause for 5ms so host recognizes a disconnect
+    irq_disable();
+    if (CONFIG_USB)
+        usb_disconnect();
+    // The "LPC17xx-DFU-Bootloader" will enter the bootloader if the
+    // watchdog timeout flag is set.
+    LPC_WDT->WDMOD = 0x07;
+    NVIC_SystemReset();
+}
+
 // Check if a peripheral clock has been enabled
 int
 is_enabled_pclock(uint32_t pclk)
diff --git a/src/lpc176x/usbserial.c b/src/lpc176x/usbserial.c
index af7d66b5..858475eb 100644
--- a/src/lpc176x/usbserial.c
+++ b/src/lpc176x/usbserial.c
@@ -8,8 +8,6 @@
 #include "autoconf.h" // CONFIG_SMOOTHIEWARE_BOOTLOADER
 #include "board/armcm_boot.h" // armcm_enable_irq
 #include "board/armcm_timer.h" // udelay
-#include "board/armcm_reset.h" // try_request_canboot
-#include "board/irq.h" // irq_disable
 #include "board/misc.h" // timer_read_time
 #include "byteorder.h" // cpu_to_le32
 #include "command.h" // DECL_CONSTANT_STR
@@ -246,20 +244,12 @@ usb_set_configure(void)
     usb_irq_enable();
 }
 
+// Force a USB disconnect (used during reboot into bootloader)
 void
-bootloader_request(void)
+usb_disconnect(void)
 {
-    if (!CONFIG_SMOOTHIEWARE_BOOTLOADER)
-        return;
-    try_request_canboot();
-    // Disable USB and pause for 5ms so host recognizes a disconnect
-    irq_disable();
     sie_cmd_write(SIE_CMD_SET_DEVICE_STATUS, 0);
     udelay(5000);
-    // The "LPC17xx-DFU-Bootloader" will enter the bootloader if the
-    // watchdog timeout flag is set.
-    LPC_WDT->WDMOD = 0x07;
-    NVIC_SystemReset();
 }