From a4e20499c0c2da6c3bd13ce8b1c36db06ee15df0 Mon Sep 17 00:00:00 2001
From: Eric Price <eric.price@tuebingen.mpg.de>
Date: Tue, 16 Oct 2018 11:11:30 +0200
Subject: [PATCH] LP-602 significant change to USB layer. force complete USB
 stack reset on replug/reset from host device

---
 flight/pios/stm32f4xx/pios_usb.c     | 34 +++++++++-------------------
 flight/pios/stm32f4xx/pios_usbhook.c | 25 ++++++++++++++++----
 2 files changed, 32 insertions(+), 27 deletions(-)

diff --git a/flight/pios/stm32f4xx/pios_usb.c b/flight/pios/stm32f4xx/pios_usb.c
index 686120069..fb1c7b3ff 100644
--- a/flight/pios/stm32f4xx/pios_usb.c
+++ b/flight/pios/stm32f4xx/pios_usb.c
@@ -38,7 +38,7 @@
 #include <pios_helpers.h>
 
 /* Rx/Tx status */
-static uint8_t transfer_possible = 0;
+static volatile uint8_t transfer_possible = 0;
 
 #ifdef PIOS_INCLUDE_FREERTOS
 static void(*disconnection_cb_list[3]) (void);
@@ -148,6 +148,9 @@ out_fail:
  */
 int32_t PIOS_USB_ChangeConnectionState(bool connected)
 {
+#ifdef PIOS_INCLUDE_FREERTOS
+    static volatile uint8_t lastStatus = 2; // 2 is "no last status"
+#endif
     // In all cases: re-initialise USB HID driver
     if (connected) {
         transfer_possible = 1;
@@ -168,6 +171,12 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected)
 
 #ifdef PIOS_INCLUDE_FREERTOS
     raiseConnectionStateCallback(connected);
+    if (lastStatus != transfer_possible) {
+        if (lastStatus == 1) {
+            raiseDisconnectionCallbacks();
+        }
+        lastStatus = transfer_possible;
+    }
 #endif
 
     return 0;
@@ -187,28 +196,7 @@ bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id)
         return false;
     }
 
-    usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low;
-// Please note that checks of transfer_possible and the reconnection handling is
-// suppressed for non freertos mode (aka bootloader) as this is causing problems detecting connection and
-// broken communications.
-#ifdef PIOS_INCLUDE_FREERTOS
-    static bool lastStatus = false;
-    bool status    = usb_found != 0 && transfer_possible ? 1 : 0;
-    bool reconnect = false;
-    if (xSemaphoreTakeFromISR(usb_dev->statusCheckSemaphore, NULL) == pdTRUE) {
-        reconnect  = (lastStatus && !status);
-        lastStatus = status;
-        xSemaphoreGiveFromISR(usb_dev->statusCheckSemaphore, NULL);
-    }
-    if (reconnect) {
-        raiseDisconnectionCallbacks();
-    }
-    return status;
-
-#else
-    return usb_found;
-
-#endif
+    return transfer_possible;
 }
 
 /*
diff --git a/flight/pios/stm32f4xx/pios_usbhook.c b/flight/pios/stm32f4xx/pios_usbhook.c
index 0d58a5e10..d587905db 100644
--- a/flight/pios/stm32f4xx/pios_usbhook.c
+++ b/flight/pios/stm32f4xx/pios_usbhook.c
@@ -280,7 +280,9 @@ static USBD_DEVICE device_callbacks = {
 static void PIOS_USBHOOK_USR_Init(void)
 {
     PIOS_USB_ChangeConnectionState(false);
-    reconnect();
+    // reconnect dev logically on init (previously a call to reconnect())
+    DCD_DevDisconnect(&pios_usb_otg_core_handle);
+    DCD_DevConnect(&pios_usb_otg_core_handle);
 }
 
 static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed)
@@ -491,9 +493,24 @@ static USBD_Class_cb_TypeDef class_callbacks = {
 
 static void reconnect(void)
 {
-    /* Force a physical disconnect/reconnect */
-    DCD_DevDisconnect(&pios_usb_otg_core_handle);
-    DCD_DevConnect(&pios_usb_otg_core_handle);
+    static volatile bool in_reconnect = false;
+
+    /* Force a complete device reset. This can trigger a call to reconnect() so prevent recursion */
+    if (!in_reconnect) {
+        in_reconnect = true; // save since volatile and STM32F4 is single core
+        // disable USB device
+        DCD_DevDisconnect(&pios_usb_otg_core_handle);
+        USBD_DeInit(&pios_usb_otg_core_handle);
+        USB_OTG_StopDevice(&pios_usb_otg_core_handle);
+        // enable USB device
+        USBD_Init(&pios_usb_otg_core_handle,
+                  USB_OTG_FS_CORE_ID,
+                  &device_callbacks,
+                  &class_callbacks,
+                  &user_callbacks);
+
+        in_reconnect = false;
+    }
 }
 
 #endif /* PIOS_INCLUDE_USB */