fix stream read count computation

3 files changed, 101 insertions, 79 deletions

diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index 54d81dc0d..469725e72 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -43,67 +43,6 @@
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
 
-bool tu_edpt_stream_write_zlp_if_needed(tu_edpt_stream_t* s, uint32_t last_xferred_bytes)
-{
-  uint16_t const bulk_packet_size = (s->ep_speed == TUSB_SPEED_HIGH) ? TUSB_EPSIZE_BULK_HS : TUSB_EPSIZE_BULK_FS;
-
-  // ZLP condition: no pending data, last transferred bytes is multiple of packet size
-  TU_VERIFY( !tu_fifo_count(&s->ff) && last_xferred_bytes && (0 == (last_xferred_bytes & (bulk_packet_size-1))) );
-
-  if ( usbh_edpt_claim(s->daddr, s->ep_addr) )
-  {
-    TU_ASSERT( usbh_edpt_xfer(s->daddr, s->ep_addr, NULL, 0) );
-  }
-
-  return true;
-}
-
-uint32_t tu_edpt_stream_write_xfer(tu_edpt_stream_t* s)
-{
-  // skip if no data
-  TU_VERIFY( tu_fifo_count(&s->ff), 0 );
-
-  // Claim the endpoint
-  // uint8_t const rhport = 0;
-  // TU_VERIFY( usbd_edpt_claim(rhport, p_cdc->ep_in), 0 );
-  TU_VERIFY( usbh_edpt_claim(s->daddr, s->ep_addr) );
-
-  // Pull data from FIFO -> EP buf
-  uint16_t const count = tu_fifo_read_n(&s->ff, s->ep_buf, s->ep_bufsize);
-
-  if ( count )
-  {
-    //TU_ASSERT( usbd_edpt_xfer(rhport, p_cdc->ep_in, p_cdc->epin_buf, count), 0 );
-    TU_ASSERT( usbh_edpt_xfer(s->daddr, s->ep_addr, s->ep_buf, count), 0 );
-    return count;
-  }else
-  {
-    // Release endpoint since we don't make any transfer
-    // Note: data is dropped if terminal is not connected
-    //usbd_edpt_release(rhport, p_cdc->ep_in);
-
-    usbh_edpt_release(s->daddr, s->ep_addr);
-    return 0;
-  }
-}
-
-uint32_t tu_edpt_stream_write(tu_edpt_stream_t* s, void const *buffer, uint32_t bufsize)
-{
-  TU_VERIFY(bufsize); // TODO support ZLP
-
-  uint16_t ret = tu_fifo_write_n(&s->ff, buffer, (uint16_t) bufsize);
-
-  // flush if fifo has more than packet size or
-  // in rare case: fifo depth is configured too small (which never reach packet size)
-  uint16_t const bulk_packet_size = (s->ep_speed == TUSB_SPEED_HIGH) ? TUSB_EPSIZE_BULK_HS : TUSB_EPSIZE_BULK_FS;
-  if ( (tu_fifo_count(&s->ff) >= bulk_packet_size) || ( tu_fifo_depth(&s->ff) < bulk_packet_size) )
-  {
-    tu_edpt_stream_write_xfer(s);
-  }
-
-  return ret;
-}
-
 typedef struct {
   uint8_t daddr;
   uint8_t bInterfaceNumber;
@@ -379,6 +318,8 @@ void cdch_close(uint8_t daddr)
       //tu_memclr(p_cdc, sizeof(cdch_interface_t));
       p_cdc->daddr = 0;
       p_cdc->bInterfaceNumber = 0;
+      tu_edpt_stream_close(&p_cdc->stream.tx);
+      tu_edpt_stream_close(&p_cdc->stream.rx);
     }
   }
 }
diff --git a/src/common/tusb_private.h b/src/common/tusb_private.h
index 01078eed3..d0c7a84fb 100644
--- a/src/common/tusb_private.h
+++ b/src/common/tusb_private.h
@@ -87,7 +87,7 @@ bool tu_edpt_release(tu_edpt_state_t* ep_state, osal_mutex_t mutex);
 // Endpoint Stream
 //--------------------------------------------------------------------+
 
-// Init an stream, should only called once
+// Init an stream, should only be called once
 bool tu_edpt_stream_init(tu_edpt_stream_t* s, bool is_host, bool is_tx, bool overwritable,
                          void* ff_buf, uint16_t ff_bufsize, uint8_t* ep_buf, uint16_t ep_bufsize);
 
@@ -102,6 +102,13 @@ void tu_edpt_stream_open(tu_edpt_stream_t* s, uint8_t hwid, tusb_desc_endpoint_t
   s->ep_packetsize = tu_edpt_packet_size(desc_ep);
 }
 
+TU_ATTR_ALWAYS_INLINE static inline
+void tu_edpt_stream_close(tu_edpt_stream_t* s)
+{
+  s->hwid = 0;
+  s->ep_addr = 0;
+}
+
 // Clear fifo
 TU_ATTR_ALWAYS_INLINE static inline
 bool tu_edpt_stream_clear(tu_edpt_stream_t* s)
@@ -109,7 +116,9 @@ bool tu_edpt_stream_clear(tu_edpt_stream_t* s)
   return tu_fifo_clear(&s->ff);
 }
 
-//------------- Write -------------//
+//--------------------------------------------------------------------+
+// Stream Write
+//--------------------------------------------------------------------+
 
 // Write to stream
 uint32_t tu_edpt_stream_write(tu_edpt_stream_t* s, void const *buffer, uint32_t bufsize);
@@ -117,6 +126,9 @@ uint32_t tu_edpt_stream_write(tu_edpt_stream_t* s, void const *buffer, uint32_t
 // Start an usb transfer if endpoint is not busy
 uint32_t tu_edpt_stream_write_xfer(tu_edpt_stream_t* s);
 
+// Start an zero-length packet if needed
+bool tu_edpt_stream_write_zlp_if_needed(tu_edpt_stream_t* s, uint32_t last_xferred_bytes);
+
 // Get the number of bytes available for writing
 TU_ATTR_ALWAYS_INLINE static inline
 uint32_t tu_edpt_stream_write_available(tu_edpt_stream_t* s)
@@ -124,8 +136,9 @@ uint32_t tu_edpt_stream_write_available(tu_edpt_stream_t* s)
   return (uint32_t) tu_fifo_remaining(&s->ff);
 }
 
-
-//------------- Read -------------//
+//--------------------------------------------------------------------+
+// Stream Read
+//--------------------------------------------------------------------+
 
 // Read from stream
 uint32_t tu_edpt_stream_read(tu_edpt_stream_t* s, void* buffer, uint32_t bufsize);
diff --git a/src/tusb.c b/src/tusb.c
index 05e0d4c1f..0d0e84a49 100644
--- a/src/tusb.c
+++ b/src/tusb.c
@@ -224,9 +224,86 @@ bool tu_edpt_stream_init(tu_edpt_stream_t* s, bool is_host, bool is_tx, bool ove
   return true;
 }
 
-//------------- Stream Write -------------//
+TU_ATTR_ALWAYS_INLINE static inline
+bool stream_claim(tu_edpt_stream_t* s)
+{
+  if (s->is_host)
+  {
+    #if CFG_TUH_ENABLED
+    return usbh_edpt_claim(s->daddr, s->ep_addr);
+    #endif
+  }else
+  {
+    #if CFG_TUD_ENABLED
+    return usbd_edpt_claim(s->rhport, s->ep_addr);
+    #endif
+  }
+
+  return false;
+}
 
-//------------- Stream Read -------------//
+
+//--------------------------------------------------------------------+
+// Stream Write
+//--------------------------------------------------------------------+
+
+bool tu_edpt_stream_write_zlp_if_needed(tu_edpt_stream_t* s, uint32_t last_xferred_bytes)
+{
+  // ZLP condition: no pending data, last transferred bytes is multiple of packet size
+  TU_VERIFY( !tu_fifo_count(&s->ff) && last_xferred_bytes && (0 == (last_xferred_bytes & (s->ep_packetsize-1))) );
+
+  TU_VERIFY( stream_claim(s) );
+  TU_ASSERT( usbh_edpt_xfer(s->daddr, s->ep_addr, NULL, 0) );
+
+  return true;
+}
+
+uint32_t tu_edpt_stream_write_xfer(tu_edpt_stream_t* s)
+{
+  // skip if no data
+  TU_VERIFY( tu_fifo_count(&s->ff), 0 );
+
+  // Claim the endpoint
+  TU_VERIFY( stream_claim(s), 0 );
+
+  // Pull data from FIFO -> EP buf
+  uint16_t const count = tu_fifo_read_n(&s->ff, s->ep_buf, s->ep_bufsize);
+
+  if ( count )
+  {
+    //TU_ASSERT( usbd_edpt_xfer(rhport, p_cdc->ep_in, p_cdc->epin_buf, count), 0 );
+    TU_ASSERT( usbh_edpt_xfer(s->daddr, s->ep_addr, s->ep_buf, count), 0 );
+    return count;
+  }else
+  {
+    // Release endpoint since we don't make any transfer
+    // Note: data is dropped if terminal is not connected
+    //usbd_edpt_release(rhport, p_cdc->ep_in);
+
+    usbh_edpt_release(s->daddr, s->ep_addr);
+    return 0;
+  }
+}
+
+uint32_t tu_edpt_stream_write(tu_edpt_stream_t* s, void const *buffer, uint32_t bufsize)
+{
+  TU_VERIFY(bufsize); // TODO support ZLP
+
+  uint16_t ret = tu_fifo_write_n(&s->ff, buffer, (uint16_t) bufsize);
+
+  // flush if fifo has more than packet size or
+  // in rare case: fifo depth is configured too small (which never reach packet size)
+  if ( (tu_fifo_count(&s->ff) >= s->ep_packetsize) || (tu_fifo_depth(&s->ff) < s->ep_packetsize) )
+  {
+    tu_edpt_stream_write_xfer(s);
+  }
+
+  return ret;
+}
+
+//--------------------------------------------------------------------+
+// Stream Read
+//--------------------------------------------------------------------+
 
 uint32_t tu_edpt_stream_read_xfer(tu_edpt_stream_t* s)
 {
@@ -239,17 +316,7 @@ uint32_t tu_edpt_stream_read_xfer(tu_edpt_stream_t* s)
   TU_VERIFY(available >= s->ep_packetsize);
 
   // claim endpoint
-  if (s->is_host)
-  {
-    #if CFG_TUH_ENABLED
-    TU_VERIFY(usbh_edpt_claim(s->daddr, s->ep_addr), 0);
-    #endif
-  }else
-  {
-    #if CFG_TUD_ENABLED
-    TU_VERIFY(usbd_edpt_claim(s->rhport, s->ep_addr), 0);
-    #endif
-  }
+  TU_VERIFY(stream_claim(s), 0);
 
   // get available again since fifo can be changed before endpoint is claimed
   available = tu_fifo_remaining(&s->ff);
@@ -257,7 +324,7 @@ uint32_t tu_edpt_stream_read_xfer(tu_edpt_stream_t* s)
   if ( available >= s->ep_packetsize )
   {
     // multiple of packet size limit by ep bufsize
-    uint16_t count = (uint16_t) (available & (s->ep_packetsize -1));
+    uint16_t count = (uint16_t) (available & ~(s->ep_packetsize -1));
     count = tu_min16(count, s->ep_bufsize);
 
     if (s->is_host)
@@ -301,6 +368,7 @@ uint32_t tu_edpt_stream_read(tu_edpt_stream_t* s, void* buffer, uint32_t bufsize
 //--------------------------------------------------------------------+
 // Debug
 //--------------------------------------------------------------------+
+
 #if CFG_TUSB_DEBUG
 #include <ctype.h>