diff --git a/class/cdc.c b/class/cdc.c
index e256520..dc79d81 100644
--- a/class/cdc.c
+++ b/class/cdc.c
@@ -8,6 +8,8 @@
 
 #include <blocking_io/blocking_fifo.h>
 
+#include <cmsis_gcc.h>
+
 // state
 static USB_CdcState cdcs = { 0 };
 static uint8_t tx_buffer[USB_CDC_PCKT_BUFSIZE];
diff --git a/class/cdc.h b/class/cdc.h
index 3b6aadd..68b9bdb 100644
--- a/class/cdc.h
+++ b/class/cdc.h
@@ -62,7 +62,7 @@ typedef struct {
 
 // ----------------
 
-#define USB_CDC_PCKT_BUFSIZE (64)
+#define USB_CDC_PCKT_BUFSIZE (72)
 
 // ----------------
 
diff --git a/usb_driver.c b/usb_driver.c
index 4317650..3d70cd4 100644
--- a/usb_driver.c
+++ b/usb_driver.c
@@ -265,10 +265,10 @@ void usbdrv_build_fifo() {
     for (uint8_t i = 0; i < USB_NUM_OF_ENDPOINTS; i++) {
         USBDRV_EpConfig *cfg = &gs.ep_IN[i];
         if (cfg->is_configured) {
-            cfg->fifo_size = CEIL4(MAX(USB_MIN_GROSS_TX_FIFO_SIZE, cfg->max_packet_size)); // correct FIFO size if necessary
-            cfg->fifo_address = next_fifo_addr;                                            // store FIFO address
-            cfg->zlp_next = false;                                                         // clear ZLP next
-            next_fifo_addr += cfg->fifo_size;                                              // advance next address
+            cfg->fifo_size = CEIL4(MAX(USB_MIN_GROSS_TX_FIFO_SIZE, 2 * cfg->max_packet_size + USB_FIFO_MARGIN)); // correct FIFO size if necessary
+            cfg->fifo_address = next_fifo_addr;                                                                  // store FIFO address
+            cfg->zlp_next = false;                                                                               // clear ZLP next
+            next_fifo_addr += cfg->fifo_size;                                                                    // advance next address
         }
     }
 }
@@ -346,8 +346,8 @@ void usbdrv_configure_endpoint(uint8_t ep, uint8_t dir, const USBDRV_EpConfig *c
         WRITE_FIELD(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_TXFNUM, ep);
 
         // store Tx FIFO size
-        uint32_t tx_fifo_config = ((cfg->fifo_size >> 2) << USB_OTG_DIEPTXF_INEPTXFD_Pos) | cfg->fifo_address; // combine size in DWORDs and address
-        *(USB_pDIEPTXF[ep]) = tx_fifo_config;                                                                  // save
+        uint32_t tx_fifo_config = ((cfg->fifo_size >> 2) << USB_OTG_DIEPTXF_INEPTXFD_Pos) | (cfg->fifo_address >> 2); // combine size in DWORDs and address
+        *(USB_pDIEPTXF[ep]) = tx_fifo_config;                                                                         // save
 
         // enable interrupt
         SET_BIT(USBD->DAINTMSK, 1 << ep);
@@ -477,9 +477,9 @@ uint32_t usbdrv_arm_IN_endpoint(uint8_t ep, const uint8_t *data, uint16_t len) {
 
     // determine final write size
     uint32_t freeSize = 0;
-    if (len > 0) { // only poll DTXFSTS if (len > 0) (see datasheet)
-        freeSize = USBINEP[ep].DTXFSTS * sizeof(uint32_t); // get free transmit buffer size
-    }
+    // if (len > 0) { // only poll DTXFSTS if (len > 0) (see datasheet)
+    freeSize = USBINEP[ep].DTXFSTS * sizeof(uint32_t); // get free transmit buffer size
+    //}
     uint16_t writeSize = MIN(freeSize, len); // limit transmit size to free size
 
     // calculate packet count based on max packet size
@@ -493,7 +493,7 @@ uint32_t usbdrv_arm_IN_endpoint(uint8_t ep, const uint8_t *data, uint16_t len) {
     gs.ep_IN[ep].zlp_next = (writeSize > 0) && ((writeSize % mps) == 0);
 
     // program DIEPTSIZ with transfer length
-    USBINEP[ep].DIEPTSIZ = (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | writeSize;
+    USBINEP[ep].DIEPTSIZ = (packet_count << USB_OTG_DIEPTSIZ_MULCNT_Pos) | (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | writeSize;
 
     // enable endpoint and cancel responding NAK
     SET_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_EPENA | USB_OTG_DIEPCTL_CNAK);
@@ -511,11 +511,11 @@ uint32_t usbdrv_arm_IN_endpoint(uint8_t ep, const uint8_t *data, uint16_t len) {
     }
 
     // push the remaining partial dword
-    uint8_t rem_bytes = len & 0b11;
+    uint8_t rem_bytes = writeSize & 0b11;
     if (rem_bytes > 0) {
         uint32_t rem_dword = 0;
-        uint16_t rem_start = len - rem_bytes;
-        for (int16_t i = len - 1; i >= rem_start; i--) {
+        uint16_t rem_start = writeSize - rem_bytes;
+        for (int16_t i = writeSize - 1; i >= rem_start; i--) {
             rem_dword = (rem_dword << 8) | data[i];
         }
         *p = rem_dword;