1 files changed, 51 insertions, 20 deletions

diff --git a/src/class/bth/bth_device.c b/src/class/bth/bth_device.c
index cbf6e1332..45cbf2d98 100755
--- a/src/class/bth/bth_device.c
+++ b/src/class/bth/bth_device.c
@@ -37,25 +37,29 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_ev;
   uint8_t ep_acl_in;
+  uint16_t ep_acl_in_pkt_sz;
   uint8_t ep_acl_out;
   uint8_t ep_voice[2];  // Not used yet
   uint8_t ep_voice_size[2][CFG_TUD_BTH_ISO_ALT_COUNT];
 
-  // Endpoint Transfer buffer
-  CFG_TUSB_MEM_ALIGN bt_hci_cmd_t hci_cmd;
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_BTH_DATA_EPSIZE];
-
+  // Previous amount of bytes sent when issuing ZLP
+  uint32_t prev_xferred_bytes;
 } btd_interface_t;
 
+typedef struct {
+  TUD_EPBUF_DEF(epout_buf, CFG_TUD_BTH_DATA_EPSIZE);
+  TUD_EPBUF_TYPE_DEF(bt_hci_cmd_t, hci_cmd);
+} btd_epbuf_t;
+
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-CFG_TUD_MEM_SECTION btd_interface_t _btd_itf;
+static btd_interface_t _btd_itf;
+CFG_TUD_MEM_SECTION static btd_epbuf_t _btd_epbuf;
 
 static bool bt_tx_data(uint8_t ep, void *data, uint16_t len)
 {
@@ -127,14 +131,28 @@ uint16_t btd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_
   TU_ASSERT(usbd_edpt_open(rhport, desc_ep), 0);
   _btd_itf.ep_ev = desc_ep->bEndpointAddress;
 
+  desc_ep = (tusb_desc_endpoint_t const *)tu_desc_next(desc_ep);
+
   // Open endpoint pair
-  TU_ASSERT(usbd_open_edpt_pair(rhport, tu_desc_next(desc_ep), 2, TUSB_XFER_BULK, &_btd_itf.ep_acl_out,
-                                &_btd_itf.ep_acl_in), 0);
+  TU_ASSERT(usbd_open_edpt_pair(rhport, (uint8_t const *)desc_ep, 2,
+                                TUSB_XFER_BULK, &_btd_itf.ep_acl_out,
+                                &_btd_itf.ep_acl_in),
+            0);
 
-  itf_desc = (tusb_desc_interface_t const *)tu_desc_next(tu_desc_next(tu_desc_next(desc_ep)));
+  // Save acl in endpoint max packet size
+  tusb_desc_endpoint_t const *desc_ep_acl_in = desc_ep;
+  for (size_t p = 0; p < 2; p++) {
+    if (tu_edpt_dir(desc_ep_acl_in->bEndpointAddress) == TUSB_DIR_IN) {
+      _btd_itf.ep_acl_in_pkt_sz = tu_edpt_packet_size(desc_ep_acl_in);
+      break;
+    }
+    desc_ep_acl_in = (tusb_desc_endpoint_t const *)tu_desc_next(desc_ep_acl_in);
+  }
+
+  itf_desc = (tusb_desc_interface_t const *)tu_desc_next(tu_desc_next(desc_ep));
 
   // Prepare for incoming data from host
-  TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_itf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE), 0);
+  TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_epbuf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE), 0);
 
   drv_len = hci_itf_size;
 
@@ -225,30 +243,30 @@ bool btd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t c
     }
     else return false;
 
-    return tud_control_xfer(rhport, request, &_btd_itf.hci_cmd, sizeof(_btd_itf.hci_cmd));
+    return tud_control_xfer(rhport, request, &_btd_epbuf.hci_cmd, sizeof(bt_hci_cmd_t));
   }
   else if ( stage == CONTROL_STAGE_DATA )
   {
     // Handle class request only
     TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
-    if (tud_bt_hci_cmd_cb) tud_bt_hci_cmd_cb(&_btd_itf.hci_cmd, tu_min16(request->wLength, sizeof(_btd_itf.hci_cmd)));
+    if (tud_bt_hci_cmd_cb) {
+      tud_bt_hci_cmd_cb(&_btd_epbuf.hci_cmd, tu_min16(request->wLength, sizeof(bt_hci_cmd_t)));
+      }
   }
 
   return true;
 }
 
-bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
-  (void)result;
-
+bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result,
+                 uint32_t xferred_bytes) {
   // received new data from host
   if (ep_addr == _btd_itf.ep_acl_out)
   {
-    if (tud_bt_acl_data_received_cb) tud_bt_acl_data_received_cb(_btd_itf.epout_buf, xferred_bytes);
+    if (tud_bt_acl_data_received_cb) tud_bt_acl_data_received_cb(_btd_epbuf.epout_buf, xferred_bytes);
 
     // prepare for next data
-    TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_itf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE));
+    TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_epbuf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE));
   }
   else if (ep_addr == _btd_itf.ep_ev)
   {
@@ -256,7 +274,20 @@ bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t
   }
   else if (ep_addr == _btd_itf.ep_acl_in)
   {
-    if (tud_bt_acl_data_sent_cb) tud_bt_acl_data_sent_cb((uint16_t)xferred_bytes);
+    if ((result == XFER_RESULT_SUCCESS) && (xferred_bytes > 0) &&
+        ((xferred_bytes & (_btd_itf.ep_acl_in_pkt_sz - 1)) == 0)) {
+      // Save number of transferred bytes
+      _btd_itf.prev_xferred_bytes = xferred_bytes;
+
+      // Send zero-length packet
+      tud_bt_acl_data_send(NULL, 0);
+    } else if (tud_bt_acl_data_sent_cb) {
+      if (xferred_bytes == 0) {
+        xferred_bytes = _btd_itf.prev_xferred_bytes;
+        _btd_itf.prev_xferred_bytes = 0;
+      }
+      tud_bt_acl_data_sent_cb((uint16_t)xferred_bytes);
+    }
   }
 
   return true;