Add fsdev_deinit

Signed-off-by: Zixun LI <[email protected]>

4 files changed, 34 insertions, 0 deletions

diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 3c4e4fc9b..351916147 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -199,6 +199,14 @@ bool dcd_init(uint8_t rhport, const tusb_rhport_init_t* rh_init) {
   return true;
 }
 
+bool dcd_deinit(uint8_t rhport) {
+  (void)rhport;
+
+  fsdev_deinit();
+
+  return true;
+}
+
 void dcd_sof_enable(uint8_t rhport, bool en) {
   (void)rhport;
 
diff --git a/src/portable/st/stm32_fsdev/fsdev_common.c b/src/portable/st/stm32_fsdev/fsdev_common.c
index 5c7df6809..d0f3460b6 100644
--- a/src/portable/st/stm32_fsdev/fsdev_common.c
+++ b/src/portable/st/stm32_fsdev/fsdev_common.c
@@ -59,6 +59,21 @@ void fsdev_core_reset(void) {
   FSDEV_REG->ISTR = 0;
 }
 
+// De-initialize the USB Core
+void fsdev_deinit(void) {
+  // Disable all interrupts and force USB reset
+  FSDEV_REG->CNTR = USB_CNTR_FRES;
+
+  // Clear pending interrupts
+  FSDEV_REG->ISTR = 0;
+
+  // Put USB peripheral in power down mode
+  FSDEV_REG->CNTR = USB_CNTR_FRES | USB_CNTR_PDWN;
+  for (volatile uint32_t i = 0; i < 200; i++) { // should be a few us
+    asm("NOP");
+  }
+}
+
 
 //--------------------------------------------------------------------+
 // PMA read/write
diff --git a/src/portable/st/stm32_fsdev/fsdev_common.h b/src/portable/st/stm32_fsdev/fsdev_common.h
index 9cf61031d..0c67ee0c7 100644
--- a/src/portable/st/stm32_fsdev/fsdev_common.h
+++ b/src/portable/st/stm32_fsdev/fsdev_common.h
@@ -314,6 +314,9 @@ TU_ATTR_ALWAYS_INLINE static inline void btable_set_count(uint32_t ep_id, uint8_
 // Reset the USB Core
 void fsdev_core_reset(void);
 
+// De-initialize the USB Core
+void fsdev_deinit(void);
+
 // Aligned buffer size according to hardware
 uint16_t pma_align_buffer_size(uint16_t size, uint8_t* blsize, uint8_t* num_block);
 
diff --git a/src/portable/st/stm32_fsdev/hcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/hcd_stm32_fsdev.c
index 168366058..39b9ac2d3 100644
--- a/src/portable/st/stm32_fsdev/hcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/hcd_stm32_fsdev.c
@@ -226,6 +226,14 @@ bool hcd_init(uint8_t rhport, const tusb_rhport_init_t* rh_init) {
   return true;
 }
 
+bool hcd_deinit(uint8_t rhport) {
+  (void)rhport;
+
+  fsdev_deinit();
+
+  return true;
+}
+
 static void port_status_handler(uint8_t rhport, bool in_isr) {
   uint32_t const fnr_reg = FSDEV_REG->FNR;
   uint32_t const istr_reg = FSDEV_REG->ISTR;