From 164e1e23419450ac8cd72af01051ec6fd299bd92 Mon Sep 17 00:00:00 2001
From: hathach <thach@tinyusb.org>
Date: Wed, 30 Jan 2019 14:13:07 +0700
Subject: [PATCH] re-init usb hardware when enable/disable SD

---
 ports/nrf/common-hal/bleio/Adapter.c | 15 ++++++++---
 ports/nrf/supervisor/usb.c           | 21 ++--------------
 supervisor/shared/usb/usb.c          | 37 +++++++++++++++++++++++++++-
 3 files changed, 50 insertions(+), 23 deletions(-)

diff --git a/ports/nrf/common-hal/bleio/Adapter.c b/ports/nrf/common-hal/bleio/Adapter.c
index cd116711eb..540659aa10 100644
--- a/ports/nrf/common-hal/bleio/Adapter.c
+++ b/ports/nrf/common-hal/bleio/Adapter.c
@@ -36,6 +36,8 @@
 #include "py/runtime.h"
 #include "shared-bindings/bleio/Adapter.h"
 
+#include "supervisor/usb.h"
+
 STATIC void softdevice_assert_handler(uint32_t id, uint32_t pc, uint32_t info) {
     mp_raise_msg_varg(&mp_type_AssertionError,
                       translate("Soft device assert, id: 0x%08lX, pc: 0x%08lX"), id, pc);
@@ -47,9 +49,6 @@ STATIC uint32_t ble_stack_enable(void) {
         .accuracy = NRF_CLOCK_LF_ACCURACY_20_PPM
     };
 
-    // The SD takes over the POWER IRQ and will fail if the IRQ is already in use
-    nrfx_power_uninit();
-
     uint32_t err_code = sd_softdevice_enable(&clock_config, softdevice_assert_handler);
     if (err_code != NRF_SUCCESS)
         return err_code;
@@ -101,9 +100,19 @@ void common_hal_bleio_adapter_set_enabled(bool enabled) {
 
     uint32_t err_code;
     if (enabled) {
+        // The SD takes over the POWER module and will fail if the module is already in use.
+        // Occurs when USB is initialized previously
+        nrfx_power_uninit();
+
         err_code = ble_stack_enable();
+
+        // Re-init USB hardware
+        init_usb_hardware();
     } else {
         err_code = sd_softdevice_disable();
+
+        // Re-init USB hardware
+        init_usb_hardware();
     }
 
     if (err_code != NRF_SUCCESS) {
diff --git a/ports/nrf/supervisor/usb.c b/ports/nrf/supervisor/usb.c
index 52eb07040d..5a4d5f1931 100644
--- a/ports/nrf/supervisor/usb.c
+++ b/ports/nrf/supervisor/usb.c
@@ -36,9 +36,8 @@
 #include "nrf_soc.h"
 #endif
 
-/* tinyusb function that handles power event (detected, ready, removed)
- * We must call it within SD's SOC event handler, or set it as power event handler if SD is not enabled.
- */
+// tinyusb function that handles power event (detected, ready, removed)
+// We must call it within SD's SOC event handler, or set it as power event handler if SD is not enabled.
 extern void tusb_hal_nrf_power_event(uint32_t event);
 
 void init_usb_hardware(void) {
@@ -46,10 +45,6 @@ void init_usb_hardware(void) {
     // 2 is max priority (0, 1 are reserved for SD)
     NVIC_SetPriority(USBD_IRQn, 2);
 
-    // USB power may already be ready at this time -> no event generated
-    // We need to invoke the handler based on the status initially
-    uint32_t usb_reg;
-
 #ifdef SOFTDEVICE_PRESENT
     uint8_t sd_en = false;
     (void) sd_softdevice_is_enabled(&sd_en);
@@ -58,8 +53,6 @@ void init_usb_hardware(void) {
         sd_power_usbdetected_enable(true);
         sd_power_usbpwrrdy_enable(true);
         sd_power_usbremoved_enable(true);
-
-        sd_power_usbregstatus_get(&usb_reg);
     }else
 #endif
     {
@@ -72,15 +65,5 @@ void init_usb_hardware(void) {
         nrfx_power_usbevt_init(&config);
 
         nrfx_power_usbevt_enable();
-
-        usb_reg = NRF_POWER->USBREGSTATUS;
-    }
-
-    if ( usb_reg & POWER_USBREGSTATUS_VBUSDETECT_Msk ) {
-        tusb_hal_nrf_power_event(NRFX_POWER_USB_EVT_DETECTED);
-    }
-
-    if ( usb_reg & POWER_USBREGSTATUS_OUTPUTRDY_Msk ) {
-        tusb_hal_nrf_power_event(NRFX_POWER_USB_EVT_READY);
     }
 }
diff --git a/supervisor/shared/usb/usb.c b/supervisor/shared/usb/usb.c
index b73c9f73fe..d15e1c4ffc 100644
--- a/supervisor/shared/usb/usb.c
+++ b/supervisor/shared/usb/usb.c
@@ -32,8 +32,19 @@
 #include "lib/utils/interrupt_char.h"
 #include "lib/mp-readline/readline.h"
 
+#include "nrfx_power.h"
+
+#ifdef SOFTDEVICE_PRESENT
+#include "nrf_sdm.h"
+#include "nrf_soc.h"
+#endif
+
 #include "tusb.h"
 
+// tinyusb function that handles power event (detected, ready, removed)
+// We must call it within SD's SOC event handler, or set it as power event handler if SD is not enabled.
+extern void tusb_hal_nrf_power_event(uint32_t event);
+
 // Serial number as hex characters. This writes directly to the USB
 // descriptor.
 extern uint16_t usb_serial_number[1 + COMMON_HAL_MCU_PROCESSOR_UID_LENGTH * 2];
@@ -60,6 +71,30 @@ bool usb_enabled(void) {
 
 void usb_init(void) {
     init_usb_hardware();
+
+    // USB power may already be ready at this time -> no event generated
+    // We need to invoke the handler based on the status initially
+    uint32_t usb_reg;
+
+#ifdef SOFTDEVICE_PRESENT
+    uint8_t sd_en = false;
+    (void) sd_softdevice_is_enabled(&sd_en);
+
+    if ( sd_en ) {
+        sd_power_usbregstatus_get(&usb_reg);
+    }else {
+        usb_reg = NRF_POWER->USBREGSTATUS;
+    }
+
+    if ( usb_reg & POWER_USBREGSTATUS_VBUSDETECT_Msk ) {
+        tusb_hal_nrf_power_event(NRFX_POWER_USB_EVT_DETECTED);
+    }
+
+    if ( usb_reg & POWER_USBREGSTATUS_OUTPUTRDY_Msk ) {
+        tusb_hal_nrf_power_event(NRFX_POWER_USB_EVT_READY);
+    }
+#endif
+
     load_serial_number();
 
     tusb_init();
@@ -74,7 +109,7 @@ void usb_init(void) {
 }
 
 void usb_background(void) {
-    if (tusb_inited()) {
+    if (usb_enabled()) {
         tud_task();
         tud_cdc_write_flush();
     }