Fix clearing of USB fifo empty interrupt and clear queues on disconne…

…ct (#393)
bitcraze · May 3, 2019 · f1011f1 · f1011f1
1 parent ede7efe
commit f1011f1
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 6 deletions.
diff --git a/src/hal/src/usb.c b/src/hal/src/usb.c
@@ -141,6 +141,20 @@ USBD_Usr_cb_TypeDef USR_cb =
 
 int command = 0xFF;
 
+static void resetUSB(void) {
+  portBASE_TYPE xTaskWokenByReceive = pdFALSE;
+
+  crtpSetLink(radiolinkGetLink());
+
+  if (isInit == true) {
+    // Empty queue
+    while (xQueueReceiveFromISR(usbDataTx, &outPacket, &xTaskWokenByReceive) == pdTRUE)
+      ;
+  }
+
+  USB_OTG_FlushTxFifo(&USB_OTG_dev, IN_EP);
+}
+
 static uint8_t usbd_cf_Setup(void *pdev , USB_SETUP_REQ  *req)
 {
   command = req->wIndex;
@@ -288,6 +302,7 @@ void USBD_USR_Init(void)
 */
 void USBD_USR_DeviceReset(uint8_t speed)
 {
+  resetUSB();
 }
 
 
@@ -308,7 +323,7 @@ void USBD_USR_DeviceConfigured(void)
 void USBD_USR_DeviceSuspended(void)
 {
   /* USB communication suspended (probably USB unplugged). Switch back to radiolink */
-  crtpSetLink(radiolinkGetLink());
+  resetUSB();
 }
 
 
@@ -339,7 +354,7 @@ void USBD_USR_DeviceConnected(void)
 */
 void USBD_USR_DeviceDisconnected(void)
 {
-  crtpSetLink(radiolinkGetLink());
+  resetUSB();
 }
 
 void usbInit(void)

diff --git a/src/lib/STM32_USB_OTG_Driver/src/usb_dcd_int.c b/src/lib/STM32_USB_OTG_Driver/src/usb_dcd_int.c
@@ -434,7 +434,6 @@ static uint32_t DCD_HandleInEP_ISR(USB_OTG_CORE_HANDLE *pdev)
 
   uint32_t ep_intr;
   uint32_t epnum = 0;
-  uint32_t fifoemptymsk;
   diepint.d32 = 0;
   ep_intr = USB_OTG_ReadDevAllInEPItr(pdev);
 
@@ -445,8 +444,6 @@ static uint32_t DCD_HandleInEP_ISR(USB_OTG_CORE_HANDLE *pdev)
       diepint.d32 = DCD_ReadDevInEP(pdev , epnum); /* Get In ITR status */
       if ( diepint.b.xfercompl )
       {
-        fifoemptymsk = 0x1 << epnum;
-        USB_OTG_MODIFY_REG32(&pdev->regs.DREGS->DIEPEMPMSK, fifoemptymsk, 0);
         CLEAR_IN_EP_INTR(epnum, xfercompl);
         /* TX COMPLETE */
         USBD_DCD_INT_fops->DataInStage(pdev , epnum);
@@ -481,7 +478,6 @@ static uint32_t DCD_HandleInEP_ISR(USB_OTG_CORE_HANDLE *pdev)
 
         DCD_WriteEmptyTxFifo(pdev , epnum);
 
-        CLEAR_IN_EP_INTR(epnum, emptyintr);
       }
     }
     epnum++;
@@ -686,6 +682,13 @@ static uint32_t DCD_WriteEmptyTxFifo(USB_OTG_CORE_HANDLE *pdev, uint32_t epnum)
     txstatus.d32 = USB_OTG_READ_REG32(&pdev->regs.INEP_REGS[epnum]->DTXFSTS);
   }
 
+  /* Switch off FIFO empty after all the data has been queued up */
+  if (len < ep->maxpacket)
+  {
+    uint32_t fifoemptymsk = 0x1 << epnum;
+    USB_OTG_MODIFY_REG32(&pdev->regs.DREGS->DIEPEMPMSK, fifoemptymsk, 0);    
+  }
+
   return 1;
 }