Merge branch 'master' into update-host

14 files changed, 267 insertions, 118 deletions

diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index 052caef21..f144cda73 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -106,7 +106,7 @@ static inline uint16_t tu_max16 (uint16_t x, uint16_t y) { return (x > y) ? x :
 static inline uint32_t tu_max32 (uint32_t x, uint32_t y) { return (x > y) ? x : y; }
 
 // Align
-static inline uint32_t tu_align_n(uint32_t value, uint32_t alignment)
+static inline uint32_t tu_align(uint32_t value, uint32_t alignment)
 {
   return value & ((uint32_t) ~(alignment-1));
 }
@@ -215,8 +215,11 @@ static inline bool     tu_bit_test (uint32_t value, uint8_t pos) { return (value
 
 void tu_print_mem(void const *buf, uint16_t count, uint8_t indent);
 
-#ifndef tu_printf
-  #define tu_printf         printf
+#ifdef CFG_TUSB_DEBUG_PRINTF
+  extern int CFG_TUSB_DEBUG_PRINTF(const char *format, ...);
+  #define tu_printf    CFG_TUSB_DEBUG_PRINTF
+#else
+  #define tu_printf    printf
 #endif
 
 static inline
diff --git a/src/common/tusb_compiler.h b/src/common/tusb_compiler.h
index e403c749b..6f6aa3c39 100644
--- a/src/common/tusb_compiler.h
+++ b/src/common/tusb_compiler.h
@@ -99,7 +99,26 @@
   #define TU_BSWAP16(u16) (__builtin_bswap16(u16))
   #define TU_BSWAP32(u32) (__builtin_bswap32(u32))
 
-#else
+#elif defined(__ICCARM__)
+  #define TU_ATTR_ALIGNED(Bytes)        __attribute__ ((aligned(Bytes)))
+  #define TU_ATTR_SECTION(sec_name)     __attribute__ ((section(#sec_name)))
+  #define TU_ATTR_PACKED                __attribute__ ((packed))
+  #define TU_ATTR_PREPACKED
+  #define TU_ATTR_WEAK                  __attribute__ ((weak))
+  #define TU_ATTR_DEPRECATED(mess)      __attribute__ ((deprecated(mess))) // warn if function with this attribute is used
+  #define TU_ATTR_UNUSED                __attribute__ ((unused))           // Function/Variable is meant to be possibly unused
+  #define TU_ATTR_USED                  __attribute__ ((used))             // Function/Variable is meant to be used
+
+  // Endian conversion use well-known host to network (big endian) naming
+  #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+    #define TU_BYTE_ORDER TU_LITTLE_ENDIAN
+  #else
+    #define TU_BYTE_ORDER TU_BIG_ENDIAN
+  #endif
+
+  #define TU_BSWAP16(u16) (__iar_builtin_REV16(u16))
+  #define TU_BSWAP32(u32) (__iar_builtin_REV(u32))
+#else 
   #error "Compiler attribute porting is required"
 #endif
 
diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index 01e990311..6ab158cca 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -71,41 +71,46 @@ bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_si
   return true;
 }
 
+static inline uint16_t _ff_mod(uint16_t idx, uint16_t depth)
+{
+  return (idx < depth) ? idx : (idx-depth);
+}
+
 // retrieve data from fifo
-static void _tu_ff_pull(tu_fifo_t* f, void * buffer)
+static inline void _ff_pull(tu_fifo_t* f, void * buffer, uint16_t n)
 {
   memcpy(buffer,
          f->buffer + (f->rd_idx * f->item_size),
-         f->item_size);
+         f->item_size*n);
 
-  f->rd_idx = (f->rd_idx + 1) % f->depth;
-  f->count--;
+  f->rd_idx = _ff_mod(f->rd_idx + n, f->depth);
+  f->count -= n;
 }
 
 // send data to fifo
-static void _tu_ff_push(tu_fifo_t* f, void const * data)
+static inline void _ff_push(tu_fifo_t* f, void const * data, uint16_t n)
 {
-  memcpy( f->buffer + (f->wr_idx * f->item_size),
-          data,
-          f->item_size);
+  memcpy(f->buffer + (f->wr_idx * f->item_size),
+         data,
+         f->item_size*n);
 
-  f->wr_idx = (f->wr_idx + 1) % f->depth;
+  f->wr_idx = _ff_mod(f->wr_idx + n, f->depth);
 
   if (tu_fifo_full(f))
   {
-    f->rd_idx = f->wr_idx; // keep the full state (rd == wr && len = size)
+    f->rd_idx = f->wr_idx; // keep the full state (rd == wr && count = depth)
   }
   else
   {
-    f->count++;
+    f->count += n;
   }
 }
 
 /******************************************************************************/
 /*!
-    @brief Read one byte out of the RX buffer.
+    @brief Read one element out of the RX buffer.
 
-    This function will return the byte located at the array index of the
+    This function will return the element located at the array index of the
     read pointer, and then increment the read pointer index.  If the read
     pointer exceeds the maximum buffer size, it will roll over to zero.
 
@@ -123,7 +128,7 @@ bool tu_fifo_read(tu_fifo_t* f, void * buffer)
 
   tu_fifo_lock(f);
 
-  _tu_ff_pull(f, buffer);
+  _ff_pull(f, buffer, 1);
 
   tu_fifo_unlock(f);
 
@@ -132,8 +137,8 @@ bool tu_fifo_read(tu_fifo_t* f, void * buffer)
 
 /******************************************************************************/
 /*!
-    @brief This function will read n elements into the array index specified by
-    the write pointer and increment the write index. If the write index
+    @brief This function will read n elements from the array index specified by
+    the read pointer and increment the read index. If the read index
     exceeds the max buffer size, then it will roll over to zero.
 
     @param[in]  f
@@ -148,32 +153,37 @@ bool tu_fifo_read(tu_fifo_t* f, void * buffer)
 /******************************************************************************/
 uint16_t tu_fifo_read_n (tu_fifo_t* f, void * buffer, uint16_t count)
 {
-  if( tu_fifo_empty(f) ) return 0;
+  if(tu_fifo_empty(f)) return 0;
 
   tu_fifo_lock(f);
 
-  /* Limit up to fifo's count */
-  if ( count > f->count ) count = f->count;
-
-  uint8_t* buf8 = (uint8_t*) buffer;
-  uint16_t len = 0;
+  // Limit up to fifo's count
+  if(count > f->count) count = f->count;
 
-  while (len < count)
+  if(count + f->rd_idx <= f->depth)
   {
-    _tu_ff_pull(f, buf8);
+    _ff_pull(f, buffer, count);
+  }
+  else
+  {
+    uint16_t const part1 = f->depth - f->rd_idx;
 
-    len++;
-    buf8 += f->item_size;
+    // Part 1: from rd_idx to end
+    _ff_pull(f, buffer, part1);
+    buffer = ((uint8_t*) buffer) + part1*f->item_size;
+
+    // Part 2: start to remaining
+    _ff_pull(f, buffer, count-part1);
   }
 
   tu_fifo_unlock(f);
 
-  return len;
+  return count;
 }
 
 /******************************************************************************/
 /*!
-    @brief Reads one item without removing it from the FIFO
+    @brief Read one item without removing it from the FIFO
 
     @param[in]  f
                 Pointer to the FIFO buffer to manipulate
@@ -189,12 +199,16 @@ bool tu_fifo_peek_at(tu_fifo_t* f, uint16_t pos, void * p_buffer)
 {
   if ( pos >= f->count ) return false;
 
+  tu_fifo_lock(f);
+
   // rd_idx is pos=0
-  uint16_t index = (f->rd_idx + pos) % f->depth;
+  uint16_t index = _ff_mod(f->rd_idx + pos, f->depth);
   memcpy(p_buffer,
          f->buffer + (index * f->item_size),
          f->item_size);
 
+  tu_fifo_unlock(f);
+
   return true;
 }
 
@@ -221,7 +235,7 @@ bool tu_fifo_write (tu_fifo_t* f, const void * data)
 
   tu_fifo_lock(f);
 
-  _tu_ff_push(f, data);
+  _ff_push(f, data, 1);
 
   tu_fifo_unlock(f);
 
@@ -249,23 +263,42 @@ uint16_t tu_fifo_write_n (tu_fifo_t* f, const void * data, uint16_t count)
 
   tu_fifo_lock(f);
 
-  // Not overwritable limit up to full
-  if (!f->overwritable) count = tu_min16(count, tu_fifo_remaining(f));
-
   uint8_t const* buf8 = (uint8_t const*) data;
-  uint16_t len = 0;
 
-  while (len < count)
+  if (!f->overwritable)
   {
-    _tu_ff_push(f, buf8);
+    // Not overwritable limit up to full
+    count = tu_min16(count, tu_fifo_remaining(f));
+  }
+  else if (count > f->depth)
+  {
+    // Only copy last part
+    buf8 = buf8 + (count - f->depth) * f->item_size;
+    count = f->depth;
+    f->wr_idx = 0;
+    f->rd_idx = 0;
+    f->count = 0;
+  }
 
-    len++;
-    buf8 += f->item_size;
+  if (count + f->wr_idx <= f->depth )
+  {
+    _ff_push(f, buf8, count);
   }
+  else
+  {
+    uint16_t const part1 = f->depth - f->wr_idx;
 
+    // Part 1: from wr_idx to end
+    _ff_push(f, buf8, part1);
+    buf8 += part1*f->item_size;
+
+    // Part 2: start to remaining
+    _ff_push(f, buf8, count-part1);
+  }
+  
   tu_fifo_unlock(f);
 
-  return len;
+  return count;
 }
 
 /******************************************************************************/
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 06e724cb3..f7918c07e 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -339,6 +339,14 @@ static void usbd_reset(uint8_t rhport)
   }
 }
 
+bool tud_task_event_ready(void)
+{
+  // Skip if stack is not initialized
+  if ( !tusb_inited() ) return false;
+
+  return !osal_queue_empty(_usbd_q);
+}
+
 /* USB Device Driver task
  * This top level thread manages all device controller event and delegates events to class-specific drivers.
  * This should be called periodically within the mainloop or rtos thread.
diff --git a/src/device/usbd.h b/src/device/usbd.h
index 713bee2b2..ecef63e18 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -47,6 +47,9 @@ bool tud_init (void);
 // Task function should be called in main/rtos loop
 void tud_task (void);
 
+// Check if there is pending events need proccessing by tud_task()
+bool tud_task_event_ready(void);
+
 // Interrupt handler, name alias to DCD
 #define tud_int_handler   dcd_int_handler
 
diff --git a/src/device/usbd_control.c b/src/device/usbd_control.c
index 353a66ad1..db0eb70a7 100644
--- a/src/device/usbd_control.c
+++ b/src/device/usbd_control.c
@@ -32,6 +32,10 @@
 #include "device/usbd_pvt.h"
 #include "dcd.h"
 
+#if CFG_TUSB_DEBUG >= 2
+extern void usbd_driver_print_control_complete_name(bool (*control_complete) (uint8_t, tusb_control_request_t const *));
+#endif
+
 enum
 {
   EDPT_CTRL_OUT = 0x00,
@@ -192,7 +196,6 @@ bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result
     if ( _ctrl_xfer.complete_cb )
     {
       #if CFG_TUSB_DEBUG >= 2
-      extern void usbd_driver_print_control_complete_name(bool (*control_complete) (uint8_t, tusb_control_request_t const *));
       usbd_driver_print_control_complete_name(_ctrl_xfer.complete_cb);
       #endif
 
diff --git a/src/osal/osal.h b/src/osal/osal.h
index 02dcf5367..c61f0c912 100644
--- a/src/osal/osal.h
+++ b/src/osal/osal.h
@@ -78,8 +78,9 @@ static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl);
 
 //------------- Queue -------------//
 static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef);
-static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data);
-static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, bool in_isr);
+static inline bool osal_queue_receive(osal_queue_t qhdl, void* data);
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr);
+static inline bool osal_queue_empty(osal_queue_t qhdl);
 
 #if 0  // TODO remove subtask related macros later
 // Sub Task
diff --git a/src/osal/osal_freertos.h b/src/osal/osal_freertos.h
index 240206e33..42ea1fd19 100644
--- a/src/osal/osal_freertos.h
+++ b/src/osal/osal_freertos.h
@@ -118,14 +118,19 @@ static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
   return xQueueCreateStatic(qdef->depth, qdef->item_sz, (uint8_t*) qdef->buf, &qdef->sq);
 }
 
-static inline bool osal_queue_receive(osal_queue_t const queue_hdl, void* data)
+static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 {
-  return xQueueReceive(queue_hdl, data, portMAX_DELAY);
+  return xQueueReceive(qhdl, data, portMAX_DELAY);
 }
 
-static inline bool osal_queue_send(osal_queue_t const queue_hdl, void const * data, bool in_isr)
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
 {
-  return in_isr ? xQueueSendToBackFromISR(queue_hdl, data, NULL) : xQueueSendToBack(queue_hdl, data, OSAL_TIMEOUT_WAIT_FOREVER);
+  return in_isr ? xQueueSendToBackFromISR(qhdl, data, NULL) : xQueueSendToBack(qhdl, data, OSAL_TIMEOUT_WAIT_FOREVER);
+}
+
+static inline bool osal_queue_empty(osal_queue_t qhdl)
+{
+  return uxQueueMessagesWaiting(qhdl) == 0;
 }
 
 #ifdef __cplusplus
diff --git a/src/osal/osal_mynewt.h b/src/osal/osal_mynewt.h
index e47e140c1..6882329c1 100644
--- a/src/osal/osal_mynewt.h
+++ b/src/osal/osal_mynewt.h
@@ -125,7 +125,7 @@ static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
   return (osal_queue_t) qdef;
 }
 
-static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
+static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 {
   struct os_event* ev;
   ev = os_eventq_get(&qhdl->evq);
@@ -137,7 +137,7 @@ static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
   return true;
 }
 
-static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, bool in_isr)
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
 {
   (void) in_isr;
 
@@ -161,6 +161,12 @@ static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, b
   return true;
 }
 
+static inline bool osal_queue_empty(osal_queue_t qhdl)
+{
+  return STAILQ_EMPTY(&qhdl->evq.evq_list);
+}
+
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/osal/osal_none.h b/src/osal/osal_none.h
index e7ce18079..4a5843f8e 100644
--- a/src/osal/osal_none.h
+++ b/src/osal/osal_none.h
@@ -142,7 +142,7 @@ typedef osal_queue_def_t* osal_queue_t;
     }\
   }
 
-// lock queue by disable usb isr
+// lock queue by disable USB interrupt
 static inline void _osal_q_lock(osal_queue_t qhdl)
 {
   (void) qhdl;
@@ -176,8 +176,7 @@ static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
   return (osal_queue_t) qdef;
 }
 
-// non blocking
-static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
+static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 {
   _osal_q_lock(qhdl);
   bool success = tu_fifo_read(&qhdl->ff, data);
@@ -186,7 +185,7 @@ static inline bool osal_queue_receive(osal_queue_t const qhdl, void* data)
   return success;
 }
 
-static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, bool in_isr)
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
 {
   if (!in_isr) {
     _osal_q_lock(qhdl);
@@ -203,6 +202,13 @@ static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, b
   return success;
 }
 
+static inline bool osal_queue_empty(osal_queue_t qhdl)
+{
+  // Skip queue lock/unlock since this function is primarily called
+  // with interrupt disabled before going into low power mode
+  return tu_fifo_empty(&qhdl->ff);
+}
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 12435ea23..7ca32c4f6 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -32,13 +32,6 @@
 #include "nrf_clock.h"
 #include "nrf_power.h"
 #include "nrfx_usbd_errata.h"
-
-#ifdef SOFTDEVICE_PRESENT
-// For enable/disable hfclk with SoftDevice
-#include "nrf_sdm.h"
-#include "nrf_soc.h"
-#endif
-
 #include "device/dcd.h"
 
 // TODO remove later
@@ -58,6 +51,11 @@ enum
                       USBD_INTENCLR_ENDISOIN_Msk | USBD_INTEN_ENDISOOUT_Msk
 };
 
+enum
+{
+  EP_COUNT = 8
+};
+
 // Transfer descriptor
 typedef struct
 {
@@ -76,36 +74,73 @@ typedef struct
 static struct
 {
   // All 8 endpoints including control IN & OUT (offset 1)
-  xfer_td_t xfer[8][2];
+  xfer_td_t xfer[EP_COUNT][2];
 
-  // Only one DMA can run at a time
-  volatile bool dma_running;
+  // Number of pending DMA that is started but not handled yet by dcd_int_handler().
+  // Since nRF can only carry one DMA can run at a time, this value is normally be either 0 or 1.
+  // However, in critical section with interrupt disabled, the DMA can be finished and added up
+  // until handled by dcd_init_handler() when exiting critical section.
+  volatile uint8_t dma_pending;
 }_dcd;
 
 /*------------------------------------------------------------------*/
 /* Control / Bulk / Interrupt (CBI) Transfer
  *------------------------------------------------------------------*/
 
+// NVIC_GetEnableIRQ is only available in CMSIS v5
+#ifndef NVIC_GetEnableIRQ
+static inline uint32_t NVIC_GetEnableIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    return((uint32_t)(((NVIC->ISER[(((uint32_t)(int32_t)IRQn) >> 5UL)] & (1UL << (((uint32_t)(int32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
+  }
+  else
+  {
+    return(0U);
+  }
+}
+#endif
+
 // helper to start DMA
 static void edpt_dma_start(volatile uint32_t* reg_startep)
 {
   // Only one dma can be active
-  if ( _dcd.dma_running )
+  if ( _dcd.dma_pending )
   {
     if (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk)
     {
-      // If called within ISR, use usbd task to defer later
+      // Called within ISR, use usbd task to defer later
       usbd_defer_func( (osal_task_func_t) edpt_dma_start, (void*) reg_startep, true );
       return;
     }
     else
     {
-      // Otherwise simply block wait
-      while ( _dcd.dma_running )  { }
+      if ( __get_PRIMASK() || !NVIC_GetEnableIRQ(USBD_IRQn) )
+      {
+        // Called in critical section with interrupt disabled. We have to manually check
+        // for the DMA complete by comparing current pending DMA with number of ENDED Events
+        uint32_t ended = 0;
+
+        while ( _dcd.dma_pending < ((uint8_t) ended) )
+        {
+          ended = NRF_USBD->EVENTS_ENDISOIN + NRF_USBD->EVENTS_ENDISOOUT;
+
+          for (uint8_t i=0; i<EP_COUNT; i++)
+          {
+            ended += NRF_USBD->EVENTS_ENDEPIN[i] + NRF_USBD->EVENTS_ENDEPOUT[i];
+          }
+        }
+      }else
+      {
+        // Called in non-critical thread-mode, should be 99% of the time.
+        // Should be safe to blocking wait until previous DMA transfer complete
+        while ( _dcd.dma_pending ) { }
+      }
     }
   }
 
-  _dcd.dma_running = true;
+  _dcd.dma_pending++;
 
   (*reg_startep) = 1;
   __ISB(); __DSB();
@@ -114,8 +149,8 @@ static void edpt_dma_start(volatile uint32_t* reg_startep)
 // DMA is complete
 static void edpt_dma_end(void)
 {
-  TU_ASSERT(_dcd.dma_running, );
-  _dcd.dma_running = false;
+  TU_ASSERT(_dcd.dma_pending, );
+  _dcd.dma_pending = 0;
 }
 
 // helper getting td
@@ -289,9 +324,11 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
 
   if ( control_status )
   {
-    // Status Phase also require Easy DMA has to be free as well !!!!
+    // Status Phase also requires Easy DMA has to be available as well !!!!
+    // However TASKS_EP0STATUS doesn't trigger any DMA transfer and got ENDED event subsequently
+    // Therefore dma_running state will be corrected right away
     edpt_dma_start(&NRF_USBD->TASKS_EP0STATUS);
-    edpt_dma_end();
+    if (_dcd.dma_pending) _dcd.dma_pending--; // correct the dma_running++ in dma start
 
     // The nRF doesn't interrupt on status transmit so we queue up a success response.
     dcd_event_xfer_complete(0, ep_addr, 0, XFER_RESULT_SUCCESS, false);
@@ -564,9 +601,26 @@ void dcd_int_handler(uint8_t rhport)
 // HFCLK helper
 //--------------------------------------------------------------------+
 #ifdef SOFTDEVICE_PRESENT
-// check if SD is present and enabled
-static bool is_sd_enabled(void)
+
+// For enable/disable hfclk with SoftDevice
+#include "nrf_mbr.h"
+#include "nrf_sdm.h"
+#include "nrf_soc.h"
+
+#ifndef SD_MAGIC_NUMBER
+  #define SD_MAGIC_NUMBER   0x51B1E5DB
+#endif
+
+static inline bool is_sd_existed(void)
 {
+  return *((uint32_t*)(SOFTDEVICE_INFO_STRUCT_ADDRESS+4)) == SD_MAGIC_NUMBER;
+}
+
+// check if SD is existed and enabled
+static inline bool is_sd_enabled(void)
+{
+  if ( !is_sd_existed() ) return false;
+
   uint8_t sd_en = false;
   (void) sd_softdevice_is_enabled(&sd_en);
   return sd_en;
diff --git a/src/portable/nuvoton/nuc505/dcd_nuc505.c b/src/portable/nuvoton/nuc505/dcd_nuc505.c
index 02f0b35a6..e67b69ab2 100644
--- a/src/portable/nuvoton/nuc505/dcd_nuc505.c
+++ b/src/portable/nuvoton/nuc505/dcd_nuc505.c
@@ -182,7 +182,10 @@ static void dcd_userEP_in_xfer(struct xfer_ctl_t *xfer, USBD_EP_T *ep)
   /* provided buffers are thankfully 32-bit aligned, allowing most data to be transfered as 32-bit */
   while (countdown > 3)
   {
-    ep->EPDAT = *(uint32_t *)xfer->data_ptr;
+    uint32_t u32;
+    memcpy(&u32, xfer->data_ptr, 4);
+
+    ep->EPDAT = u32;
     xfer->data_ptr += 4; countdown -= 4;
   }
   while (countdown--)
diff --git a/src/portable/nxp/transdimension/dcd_transdimension.c b/src/portable/nxp/transdimension/dcd_transdimension.c
index fcedc1c0c..7baf43e23 100644
--- a/src/portable/nxp/transdimension/dcd_transdimension.c
+++ b/src/portable/nxp/transdimension/dcd_transdimension.c
@@ -273,7 +273,8 @@ typedef struct {
   dcd_qtd_t qtd[QHD_MAX] TU_ATTR_ALIGNED(32); // for portability, TinyUSB only queue 1 TD for each Qhd
 }dcd_data_t;
 
-static dcd_data_t _dcd_data CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(2048);
+CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(2048)
+static dcd_data_t _dcd_data;
 
 //--------------------------------------------------------------------+
 // CONTROLLER API
@@ -478,7 +479,8 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t t
 
   // Force the CPU to flush the buffer. We increase the size by 32 because the call aligns the
   // address to 32-byte boundaries.
-  CleanInvalidateDCache_by_Addr((uint32_t*) buffer, total_bytes + 31);
+  // void* cast to suppress cast-align warning, buffer must be
+  CleanInvalidateDCache_by_Addr((uint32_t*) tu_align((uint32_t) buffer, 4), total_bytes + 31);
 
   //------------- Prepare qtd -------------//
   qtd_init(p_qtd, buffer, total_bytes);
diff --git a/src/portable/st/synopsys/dcd_synopsys.c b/src/portable/st/synopsys/dcd_synopsys.c
index 4e0d6f8c4..717a59db6 100644
--- a/src/portable/st/synopsys/dcd_synopsys.c
+++ b/src/portable/st/synopsys/dcd_synopsys.c
@@ -195,7 +195,7 @@ void dcd_init (uint8_t rhport)
 
   // Programming model begins in the last section of the chapter on the USB
   // peripheral in each Reference Manual.
-  USB_OTG_FS->GAHBCFG |= USB_OTG_GAHBCFG_TXFELVL | USB_OTG_GAHBCFG_GINT;
+  USB_OTG_FS->GAHBCFG |= USB_OTG_GAHBCFG_GINT;
 
   // No HNP/SRP (no OTG support), program timeout later, turnaround
   // programmed for 32+ MHz.
@@ -374,7 +374,6 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
                             ((total_bytes & USB_OTG_DIEPTSIZ_XFRSIZ_Msk) << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);
 
     in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_EPENA | USB_OTG_DIEPCTL_CNAK;
-
     // Enable fifo empty interrupt only if there are something to put in the fifo.
     if(total_bytes != 0) {
       dev->DIEPEMPMSK |= (1 << epnum);
@@ -539,45 +538,29 @@ static void receive_packet(xfer_ctl_t * xfer, /* USB_OTG_OUTEndpointTypeDef * ou
   xfer->short_packet = (xfer_size < xfer->max_size);
 }
 
-// Write a data packet to EPIN FIFO
-static void transmit_packet(xfer_ctl_t * xfer, USB_OTG_INEndpointTypeDef * in_ep, uint8_t fifo_num) {
+// Write a single data packet to EPIN FIFO
+static void write_fifo_packet(uint8_t fifo_num, uint8_t * src, uint16_t len){
   usb_fifo_t tx_fifo = FIFO_BASE(fifo_num);
 
-  uint16_t remaining = (in_ep->DIEPTSIZ & USB_OTG_DIEPTSIZ_XFRSIZ_Msk) >> USB_OTG_DIEPTSIZ_XFRSIZ_Pos;
-  xfer->queued_len = xfer->total_len - remaining;
-
-  uint16_t to_xfer_size = (remaining > xfer->max_size) ? xfer->max_size : remaining;
-  uint8_t to_xfer_rem = to_xfer_size % 4;
-  uint16_t to_xfer_size_aligned = to_xfer_size - to_xfer_rem;
-
-  // Buffer might not be aligned to 32b, so we need to force alignment
-  // by copying to a temp var.
-  uint8_t * base = (xfer->buffer + xfer->queued_len);
-
-  // This for loop always runs at least once- skip if less than 4 bytes
-  // to send off.
-  if(to_xfer_size >= 4) {
-    for(uint16_t i = 0; i < to_xfer_size_aligned; i += 4) {
-      uint32_t tmp = base[i] | (base[i + 1] << 8) | \
-        (base[i + 2] << 16) | (base[i + 3] << 24);
-      (* tx_fifo) = tmp;
-    }
+  // Pushing full available 32 bit words to fifo
+  uint16_t full_words = len >> 2;
+  for(uint16_t i = 0; i < full_words; i++){
+    *tx_fifo = (src[3] << 24) | (src[2] << 16) | (src[1] << 8) | src[0];
+    src += 4;
   }
 
-  // Do not read beyond end of buffer if not divisible by 4.
-  if(to_xfer_rem != 0) {
-    uint32_t tmp = 0;
-    uint8_t * last_32b_bound = base + to_xfer_size_aligned;
-
-    tmp |= last_32b_bound[0];
-    if(to_xfer_rem > 1) {
-      tmp |= (last_32b_bound[1] << 8);
+  // Write the remaining 1-3 bytes into fifo
+  uint8_t bytes_rem = len & 0x03;
+  if(bytes_rem){
+    uint32_t tmp_word = 0;
+    tmp_word |= src[0];
+    if(bytes_rem > 1){
+      tmp_word |= src[1] << 8;
     }
-    if(to_xfer_rem > 2) {
-      tmp |= (last_32b_bound[2] << 16);
+    if(bytes_rem > 2){
+      tmp_word |= src[2] << 16;
     }
-
-    (* tx_fifo) = tmp;
+    *tx_fifo = tmp_word;
   }
 }
 
@@ -669,6 +652,7 @@ static void handle_epin_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_INEndpointType
 
     if ( dev->DAINT & (1 << (USB_OTG_DAINT_IEPINT_Pos + n)) )
     {
+
       // IN XFER complete (entire xfer).
       if ( in_ep[n].DIEPINT & USB_OTG_DIEPINT_XFRC )
       {
@@ -677,17 +661,36 @@ static void handle_epin_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_INEndpointType
       }
 
       // XFER FIFO empty
-      if ( in_ep[n].DIEPINT & USB_OTG_DIEPINT_TXFE )
+      if ( (in_ep[n].DIEPINT & USB_OTG_DIEPINT_TXFE) && (dev->DIEPEMPMSK & (1 << n)) )
       {
         // DIEPINT's TXFE bit is read-only, software cannot clear it.
         // It will only be cleared by hardware when written bytes is more than
         // - 64 bytes or
         // - Half of TX FIFO size (configured by DIEPTXF)
 
-        transmit_packet(xfer, &in_ep[n], n);
+        uint16_t remaining_packets = (in_ep[n].DIEPTSIZ & USB_OTG_DIEPTSIZ_PKTCNT_Msk) >> USB_OTG_DIEPTSIZ_PKTCNT_Pos;
+
+        // Process every single packet (only whole packets can be written to fifo)
+        for(uint16_t i = 0; i < remaining_packets; i++){
+          uint16_t remaining_bytes = (in_ep[n].DIEPTSIZ & USB_OTG_DIEPTSIZ_XFRSIZ_Msk) >> USB_OTG_DIEPTSIZ_XFRSIZ_Pos;
+          // Packet can not be larger than ep max size
+          uint16_t packet_size = tu_min16(remaining_bytes, xfer->max_size);
+
+          // It's only possible to write full packets into FIFO. Therefore DTXFSTS register of current
+          // EP has to be checked if the buffer can take another WHOLE packet
+          if(packet_size > ((in_ep[n].DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV_Msk) << 2)){
+            break;
+          }
+
+          // TODO: queued_len can be removed later
+          xfer->queued_len = xfer->total_len - remaining_bytes;
+
+          // Push packet to Tx-FIFO
+          write_fifo_packet(n, (xfer->buffer + xfer->queued_len), packet_size);
+        }
 
         // Turn off TXFE if all bytes are written.
-        if (xfer->queued_len == xfer->total_len)
+        if (((in_ep[n].DIEPTSIZ & USB_OTG_DIEPTSIZ_XFRSIZ_Msk) >> USB_OTG_DIEPTSIZ_XFRSIZ_Pos) == 0)
         {
           dev->DIEPEMPMSK &= ~(1 << n);
         }