From 85a0ebec2d68819efa82484933cf826d4fa6650a Mon Sep 17 00:00:00 2001
From: whoenig <whoenig@users.noreply.github.com>
Date: Fri, 12 Feb 2021 10:17:43 +0100
Subject: [PATCH] Add flow control for USB connection. (#691)

* Add flow control for USB connection.

RX:
 * return BUSY, if receiving queue is full
 * prepare for Rx after a packet was consumed
TX:
 * use blocking queue calls

* Protect and fix the USB flow control calls

Fixes issue #688.

Co-authored-by: Wolfgang Hoenig <whoenig@caltech.edu>
Co-authored-by: Arnaud Taffanel <arnaud@bitcraze.io>
---
 src/hal/src/usb.c     | 50 +++++++++++++++++++++++++++++++++++++------
 src/hal/src/usblink.c |  8 +++----
 2 files changed, 47 insertions(+), 11 deletions(-)

diff --git a/src/hal/src/usb.c b/src/hal/src/usb.c
index 4750b321b6..ec2ee5e956 100644
--- a/src/hal/src/usb.c
+++ b/src/hal/src/usb.c
@@ -53,6 +53,7 @@ NO_DMA_CCM_SAFE_ZERO_INIT __ALIGN_BEGIN USB_OTG_CORE_HANDLE    USB_OTG_dev __ALI
 
 static bool isInit = false;
 static bool doingTransfer = false;
+static bool rxStopped = true;
 
 // This should probably be reduced to a CRTP packet size
 static xQueueHandle usbDataRx;
@@ -158,6 +159,9 @@ static void resetUSB(void) {
   }
 
   USB_OTG_FlushTxFifo(&USB_OTG_dev, IN_EP);
+
+  rxStopped = true;
+  doingTransfer = false;
 }
 
 static uint8_t usbd_cf_Setup(void *pdev , USB_SETUP_REQ  *req)
@@ -165,6 +169,14 @@ static uint8_t usbd_cf_Setup(void *pdev , USB_SETUP_REQ  *req)
   command = req->wIndex;
   if (command == 0x01) {
     crtpSetLink(usblinkGetLink());
+
+    if (rxStopped && !xQueueIsQueueFullFromISR(usbDataRx)) {
+      DCD_EP_PrepareRx(&USB_OTG_dev,
+                      OUT_EP,
+                      (uint8_t*)(inPacket.data),
+                      USB_RX_TX_PACKET_SIZE);
+      rxStopped = false;
+    }
   } else {
     crtpSetLink(radiolinkGetLink());
   }
@@ -192,6 +204,7 @@ static uint8_t  usbd_cf_Init (void  *pdev,
                    OUT_EP,
                    (uint8_t*)(inPacket.data),
                    USB_RX_TX_PACKET_SIZE);
+  rxStopped = false;
 
   return USBD_OK;
 }
@@ -271,20 +284,30 @@ static uint8_t  usbd_cf_SOF (void *pdev)
   */
 static uint8_t  usbd_cf_DataOut (void *pdev, uint8_t epnum)
 {
+  uint8_t result;
   portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
 
   /* Get the received data buffer and update the counter */
   inPacket.size = ((USB_OTG_CORE_HANDLE*)pdev)->dev.out_ep[epnum].xfer_count;
 
-  xQueueSendFromISR(usbDataRx, &inPacket, &xHigherPriorityTaskWoken);
+  if (xQueueSendFromISR(usbDataRx, &inPacket, &xHigherPriorityTaskWoken) == pdTRUE) {
+    result = USBD_OK;
+  } else {
+    result = USBD_BUSY;
+  }
 
-  /* Prepare Out endpoint to receive next packet */
-  DCD_EP_PrepareRx(pdev,
-                   OUT_EP,
-                   (uint8_t*)(inPacket.data),
-                   USB_RX_TX_PACKET_SIZE);
+  if (!xQueueIsQueueFullFromISR(usbDataRx)) {
+    /* Prepare Out endpoint to receive next packet */
+    DCD_EP_PrepareRx(pdev,
+                     OUT_EP,
+                     (uint8_t*)(inPacket.data),
+                     USB_RX_TX_PACKET_SIZE);
+    rxStopped = false;
+  } else {
+    rxStopped = true;
+  }
 
-  return USBD_OK;
+  return result;
 }
 
 /**
@@ -397,6 +420,19 @@ bool usbGetDataBlocking(USBPacket *in)
 {
   while (xQueueReceive(usbDataRx, in, portMAX_DELAY) != pdTRUE)
     ; // Don't return until we get some data on the USB
+
+  // Disabling USB interrupt to make sure we can check and re-enable the endpoint
+  // if it is not currently accepting data (ie. can happen if the RX queue was full)
+  NVIC_DisableIRQ(OTG_FS_IRQn);
+  if (rxStopped) {
+    DCD_EP_PrepareRx(&USB_OTG_dev,
+                    OUT_EP,
+                    (uint8_t*)(inPacket.data),
+                    USB_RX_TX_PACKET_SIZE);
+    rxStopped = false;
+  }
+  NVIC_EnableIRQ(OTG_FS_IRQn);
+
   return true;
 }
 
diff --git a/src/hal/src/usblink.c b/src/hal/src/usblink.c
index ff018ff09b..05958a83ec 100644
--- a/src/hal/src/usblink.c
+++ b/src/hal/src/usblink.c
@@ -50,7 +50,7 @@ static uint8_t sendBuffer[64];
 
 static int usblinkSendPacket(CRTPPacket *p);
 static int usblinkSetEnable(bool enable);
-static int usblinkReceiveCRTPPacket(CRTPPacket *p);
+static int usblinkReceivePacket(CRTPPacket *p);
 
 STATIC_MEM_TASK_ALLOC(usblinkTask, USBLINK_TASK_STACKSIZE);
 
@@ -58,7 +58,7 @@ static struct crtpLinkOperations usblinkOp =
 {
   .setEnable         = usblinkSetEnable,
   .sendPacket        = usblinkSendPacket,
-  .receivePacket     = usblinkReceiveCRTPPacket,
+  .receivePacket     = usblinkReceivePacket,
 };
 
 /* Radio task handles the CRTP packet transfers as well as the radio link
@@ -76,12 +76,12 @@ static void usblinkTask(void *param)
     p.size = usbIn.size - 1;
     memcpy(&p.raw, usbIn.data, usbIn.size);
     // This queuing will copy a CRTP packet size from usbIn
-    ASSERT(xQueueSend(crtpPacketDelivery, &p, 0) == pdTRUE);
+    xQueueSend(crtpPacketDelivery, &p, portMAX_DELAY);
   }
 
 }
 
-static int usblinkReceiveCRTPPacket(CRTPPacket *p)
+static int usblinkReceivePacket(CRTPPacket *p)
 {
   if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
   {