Merge pull request #1828 from HiFiPhile/stm32_fsdev

stm32_fsdev & ISO EP buffer allocation improvements 

6 files changed, 724 insertions, 332 deletions

diff --git a/src/class/audio/audio_device.c b/src/class/audio/audio_device.c
index de5a20d6d..5bccc66b7 100644
--- a/src/class/audio/audio_device.c
+++ b/src/class/audio/audio_device.c
@@ -76,26 +76,19 @@
 // Linear buffer in case target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer
 // is available or driver is would need to be changed dramatically
 
-// Only STM32 synopsys and dcd_transdimension use non-linear buffer for now
-// Synopsys detection copied from dcd_synopsys.c (refactor later on)
-#if defined (STM32F105x8) || defined (STM32F105xB) || defined (STM32F105xC) || \
-    defined (STM32F107xB) || defined (STM32F107xC)
-#define STM32F1_SYNOPSYS
-#endif
-
-#if defined (STM32L475xx) || defined (STM32L476xx) ||                          \
-    defined (STM32L485xx) || defined (STM32L486xx) || defined (STM32L496xx) || \
-    defined (STM32L4R5xx) || defined (STM32L4R7xx) || defined (STM32L4R9xx) || \
-    defined (STM32L4S5xx) || defined (STM32L4S7xx) || defined (STM32L4S9xx)
-#define STM32L4_SYNOPSYS
-#endif
-
-#if (CFG_TUSB_MCU == OPT_MCU_STM32F1 && defined(STM32F1_SYNOPSYS)) || \
+// Only STM32 and dcd_transdimension use non-linear buffer for now
+#if CFG_TUSB_MCU == OPT_MCU_STM32F0                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32F1                               || \
     CFG_TUSB_MCU == OPT_MCU_STM32F2                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32F3                               || \
     CFG_TUSB_MCU == OPT_MCU_STM32F4                               || \
     CFG_TUSB_MCU == OPT_MCU_STM32F7                               || \
     CFG_TUSB_MCU == OPT_MCU_STM32H7                               || \
-    (CFG_TUSB_MCU == OPT_MCU_STM32L4 && defined(STM32L4_SYNOPSYS)) || \
+    CFG_TUSB_MCU == OPT_MCU_STM32L0                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32L1                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32L4                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32G4                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32WB                               || \
     CFG_TUSB_MCU == OPT_MCU_RX63X                                 || \
     CFG_TUSB_MCU == OPT_MCU_RX65X                                 || \
     CFG_TUSB_MCU == OPT_MCU_RX72N                                 || \
@@ -113,6 +106,33 @@
 #define  USE_LINEAR_BUFFER     1
 #endif
 
+#if defined(STM32F102x6) || defined(STM32F102xB) || \
+    defined(STM32F103x6) || defined(STM32F103xB) || \
+    defined(STM32F103xE) || defined(STM32F103xG)
+#define STM32F1_FSDEV
+#endif
+
+#if defined(STM32L412xx) || defined(STM32L422xx) || \
+    defined(STM32L432xx) || defined(STM32L433xx) || \
+    defined(STM32L442xx) || defined(STM32L443xx) || \
+    defined(STM32L452xx) || defined(STM32L462xx)
+#define STM32L4_FSDEV
+#endif
+
+// Temporarily put the check here for stm32_fsdev
+#if CFG_TUSB_MCU == OPT_MCU_STM32F0                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32F3                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32L0                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32L1                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32G4                               || \
+    CFG_TUSB_MCU == OPT_MCU_STM32WB                               || \
+    (TU_CHECK_MCU(OPT_MCU_STM32F1) && defined(STM32F1_FSDEV))     || \
+    (TU_CHECK_MCU(OPT_MCU_STM32L4) && defined(STM32L4_FSDEV))
+#define  USE_ISO_EP_ALLOCATION   1
+#else
+#define  USE_ISO_EP_ALLOCATION   0
+#endif
+
 // Declaration of buffers
 
 // Check for maximum supported numbers
@@ -1471,6 +1491,83 @@ uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 #endif
       }
 
+#if USE_ISO_EP_ALLOCATION
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN
+      uint8_t  ep_in = 0;
+      uint16_t ep_in_size = 0;
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_EP_OUT
+      uint8_t  ep_out = 0;
+      uint16_t ep_out_size = 0;
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+      uint8_t ep_fb = 0;
+  #endif
+
+      uint8_t const *p_desc = _audiod_fct[i].p_desc;
+      uint8_t const *p_desc_end = p_desc + _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;
+      while (p_desc < p_desc_end)
+      {
+        if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT)
+        {
+          tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+          if (desc_ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS)
+          {
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+            // Explicit feedback EP
+            if (desc_ep->bmAttributes.usage == 1)
+            {
+              ep_fb = desc_ep->bEndpointAddress;
+            }
+  #endif
+            // Data EP
+            if (desc_ep->bmAttributes.usage == 0)
+            {
+              if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN)
+              {
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN
+                ep_in = desc_ep->bEndpointAddress;
+                ep_in_size = TU_MAX(tu_edpt_packet_size(desc_ep), ep_in_size);
+  #endif
+              } else
+              {
+  #if CFG_TUD_AUDIO_ENABLE_EP_OUT
+                ep_out = desc_ep->bEndpointAddress;
+                ep_out_size = TU_MAX(tu_edpt_packet_size(desc_ep), ep_out_size);
+  #endif
+              }
+            }
+            
+          }
+        }
+        p_desc = tu_desc_next(p_desc);
+      }
+
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN
+      if (ep_in)
+      {
+        usbd_edpt_iso_alloc(rhport, ep_in, ep_in_size);
+      }
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_EP_OUT
+      if (ep_out)
+      {
+        usbd_edpt_iso_alloc(rhport, ep_out, ep_out_size);
+      }
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+      if (ep_fb)
+      {
+        usbd_edpt_iso_alloc(rhport, ep_fb, 4);
+      }
+  #endif
+
+#endif // USE_ISO_EP_ALLOCATION
+
       break;
     }
   }
@@ -1532,17 +1629,19 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
   if (audio->ep_in_as_intf_num == itf)
   {
     audio->ep_in_as_intf_num = 0;
+  #if !USE_ISO_EP_ALLOCATION
     usbd_edpt_close(rhport, audio->ep_in);
+  #endif
 
     // Clear FIFOs, since data is no longer valid
-#if !CFG_TUD_AUDIO_ENABLE_ENCODING
+  #if !CFG_TUD_AUDIO_ENABLE_ENCODING
     tu_fifo_clear(&audio->ep_in_ff);
-#else
+  #else
     for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++)
     {
       tu_fifo_clear(&audio->tx_supp_ff[cnt]);
     }
-#endif
+  #endif
 
     // Invoke callback - can be used to stop data sampling
     if (tud_audio_set_itf_close_EP_cb) TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request));
@@ -1550,23 +1649,25 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
     audio->ep_in = 0;                           // Necessary?
 
   }
-#endif
+#endif // CFG_TUD_AUDIO_ENABLE_EP_IN
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
   if (audio->ep_out_as_intf_num == itf)
   {
     audio->ep_out_as_intf_num = 0;
+  #if !USE_ISO_EP_ALLOCATION
     usbd_edpt_close(rhport, audio->ep_out);
+  #endif
 
     // Clear FIFOs, since data is no longer valid
-#if !CFG_TUD_AUDIO_ENABLE_DECODING
+  #if !CFG_TUD_AUDIO_ENABLE_DECODING
     tu_fifo_clear(&audio->ep_out_ff);
-#else
+  #else
     for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++)
     {
       tu_fifo_clear(&audio->rx_supp_ff[cnt]);
     }
-#endif
+  #endif
 
     // Invoke callback - can be used to stop data sampling
     if (tud_audio_set_itf_close_EP_cb) TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request));
@@ -1574,13 +1675,15 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
     audio->ep_out = 0;                          // Necessary?
 
     // Close corresponding feedback EP
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+    #if !USE_ISO_EP_ALLOCATION
     usbd_edpt_close(rhport, audio->ep_fb);
+    #endif
     audio->ep_fb = 0;
     tu_memclr(&audio->feedback, sizeof(audio->feedback));
-#endif
+  #endif
   }
-#endif
+#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT
 
   // Save current alternative interface setting
   audio->alt_setting[idxItf] = alt;
@@ -1605,8 +1708,11 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
         if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT)
         {
           tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+#if USE_ISO_EP_ALLOCATION
+          TU_ASSERT(usbd_edpt_iso_activate(rhport, desc_ep));
+#else
           TU_ASSERT(usbd_edpt_open(rhport, desc_ep));
-
+#endif
           uint8_t const ep_addr = desc_ep->bEndpointAddress;
 
           //TODO: We need to set EP non busy since this is not taken care of right now in ep_close() - THIS IS A WORKAROUND!
@@ -1621,11 +1727,11 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
             audio->ep_in_sz = tu_edpt_packet_size(desc_ep);
 
             // If software encoding is enabled, parse for the corresponding parameters - doing this here means only AS interfaces with EPs get scanned for parameters
-#if CFG_TUD_AUDIO_ENABLE_ENCODING
+  #if CFG_TUD_AUDIO_ENABLE_ENCODING
             audiod_parse_for_AS_params(audio, p_desc_parse_for_params, p_desc_end, itf);
 
             // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
+    #if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
             const uint16_t active_fifo_depth = (uint16_t) ((audio->tx_supp_ff_sz_max / audio->n_bytes_per_sampe_tx) * audio->n_bytes_per_sampe_tx);
             for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++)
             {
@@ -1633,9 +1739,8 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
             }
             audio->n_ff_used_tx = audio->n_channels_tx / audio->n_channels_per_ff_tx;
             TU_ASSERT( audio->n_ff_used_tx <= audio->n_tx_supp_ff );
-#endif
-
-#endif
+    #endif
+  #endif
 
             // Schedule first transmit if alternate interface is not zero i.e. streaming is disabled - in case no sample data is available a ZLP is loaded
             // It is necessary to trigger this here since the refill is done with an RX FIFO empty interrupt which can only trigger if something was in there
@@ -1652,11 +1757,11 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
             audio->ep_out_as_intf_num = itf;
             audio->ep_out_sz = tu_edpt_packet_size(desc_ep);
 
-#if CFG_TUD_AUDIO_ENABLE_DECODING
+  #if CFG_TUD_AUDIO_ENABLE_DECODING
             audiod_parse_for_AS_params(audio, p_desc_parse_for_params, p_desc_end, itf);
 
             // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
+    #if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
             const uint16_t active_fifo_depth = (audio->rx_supp_ff_sz_max / audio->n_bytes_per_sampe_rx) * audio->n_bytes_per_sampe_rx;
             for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++)
             {
@@ -1664,18 +1769,18 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
             }
             audio->n_ff_used_rx = audio->n_channels_rx / audio->n_channels_per_ff_rx;
             TU_ASSERT( audio->n_ff_used_rx <= audio->n_rx_supp_ff );
-#endif
-#endif
+    #endif
+  #endif
 
             // Prepare for incoming data
-#if USE_LINEAR_BUFFER_RX
+  #if USE_LINEAR_BUFFER_RX
             TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_out, audio->lin_buf_out, audio->ep_out_sz), false);
-#else
+  #else
             TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_out, &audio->ep_out_ff, audio->ep_out_sz), false);
-#endif
+  #endif
           }
 
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
           if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.usage == 1)   // Check if usage is explicit data feedback
           {
             audio->ep_fb = ep_addr;
@@ -1684,7 +1789,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
             // Enable SOF interrupt if callback is implemented
             if (tud_audio_feedback_interval_isr) usbd_sof_enable(rhport, true);
           }
-#endif
+  #endif
 #endif // CFG_TUD_AUDIO_ENABLE_EP_OUT
 
           foundEPs += 1;
@@ -1735,7 +1840,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
           default: break;
         }
       }
-#endif
+#endif // CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 
       // We are done - abort loop
       break;
diff --git a/src/device/dcd.h b/src/device/dcd.h
index 3a7e6c5df..93170732f 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -167,6 +167,12 @@ void dcd_edpt_stall           (uint8_t rhport, uint8_t ep_addr);
 // This API never calls with control endpoints, since it is auto cleared when receiving setup packet
 void dcd_edpt_clear_stall     (uint8_t rhport, uint8_t ep_addr);
 
+// Allocate packet buffer used by ISO endpoints
+// Some MCU need manual packet buffer allocation, we allocation largest size to avoid clustering
+TU_ATTR_WEAK bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size);
+
+// Configure and enable an ISO endpoint according to descriptor
+TU_ATTR_WEAK bool dcd_edpt_iso_activate(uint8_t rhport,  tusb_desc_endpoint_t const * p_endpoint_desc);
 //--------------------------------------------------------------------+
 // Event API (implemented by stack)
 //--------------------------------------------------------------------+
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 384daccb8..7733cc2e2 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -1382,4 +1382,31 @@ void usbd_sof_enable(uint8_t rhport, bool en)
   dcd_sof_enable(rhport, en);
 }
 
+bool usbd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size)
+{
+  rhport = _usbd_rhport;
+
+  TU_ASSERT(dcd_edpt_iso_alloc);
+  TU_ASSERT(tu_edpt_number(ep_addr) < CFG_TUD_ENDPPOINT_MAX);
+
+  return dcd_edpt_iso_alloc(rhport, ep_addr, largest_packet_size);
+}
+
+bool usbd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep)
+{
+  rhport = _usbd_rhport;
+
+  uint8_t const epnum = tu_edpt_number(desc_ep->bEndpointAddress);
+  uint8_t const dir   = tu_edpt_dir(desc_ep->bEndpointAddress);
+  
+  TU_ASSERT(dcd_edpt_iso_activate);
+  TU_ASSERT(epnum < CFG_TUD_ENDPPOINT_MAX);
+  TU_ASSERT(tu_edpt_validate(desc_ep, (tusb_speed_t) _usbd_dev.speed));
+  
+  _usbd_dev.ep_status[epnum][dir].stalled = false;
+  _usbd_dev.ep_status[epnum][dir].busy = false;
+  _usbd_dev.ep_status[epnum][dir].claimed = false;
+  return dcd_edpt_iso_activate(rhport, desc_ep);
+}
+
 #endif
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index 6fad46db3..f860ab0a1 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -96,6 +96,12 @@ void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr);
 // Check if endpoint is stalled
 bool usbd_edpt_stalled(uint8_t rhport, uint8_t ep_addr);
 
+// Allocate packet buffer used by ISO endpoints
+bool usbd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size);
+
+// Configure and enable an ISO endpoint according to descriptor
+bool usbd_edpt_iso_activate(uint8_t rhport,  tusb_desc_endpoint_t const * p_endpoint_desc);
+
 // Check if endpoint is ready (not busy and not stalled)
 TU_ATTR_ALWAYS_INLINE static inline
 bool usbd_edpt_ready(uint8_t rhport, uint8_t ep_addr)
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 54c3c95e7..78da9d0e5 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -6,6 +6,8 @@
  * Portions:
  * Copyright (c) 2016 STMicroelectronics
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ * Copyright (c) 2022 Simon Küppers (skuep)
+ * Copyright (c) 2022 HiFiPhile
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -64,10 +66,6 @@
  * - STALL handled, but not tested.
  *   - Does it work? No clue.
  * - All EP BTABLE buffers are created based on max packet size of first EP opened with that address.
- * - No isochronous endpoints
- * - Endpoint index is the ID of the endpoint
- *   - This means that priority is given to endpoints with lower ID numbers
- *   - Code is mixing up EP IX with EP ID. Everywhere.
  * - Packet buffer memory is copied in the interrupt.
  *   - This is better for performance, but means interrupts are disabled for longer
  *   - DMA may be the best choice, but it could also be pushed to the USBD task.
@@ -150,12 +148,6 @@
 #  define DCD_STM32_BTABLE_LENGTH (PMA_LENGTH - DCD_STM32_BTABLE_BASE)
 #endif
 
-// Since TinyUSB doesn't use SOF for now, and this interrupt too often (1ms interval)
-// We disable SOF for now until needed later on
-#ifndef USE_SOF
-#  define USE_SOF     0
-#endif
-
 /***************************************************
  * Checks, structs, defines, function definitions, etc.
  */
@@ -167,29 +159,43 @@ TU_VERIFY_STATIC(((DCD_STM32_BTABLE_BASE) + (DCD_STM32_BTABLE_LENGTH))<=(PMA_LEN
 
 TU_VERIFY_STATIC(((DCD_STM32_BTABLE_BASE) % 8) == 0, "BTABLE base must be aligned to 8 bytes");
 
+//--------------------------------------------------------------------+
+// MACRO CONSTANT TYPEDEF
+//--------------------------------------------------------------------+
+
 // One of these for every EP IN & OUT, uses a bit of RAM....
 typedef struct
 {
   uint8_t * buffer;
-  // tu_fifo_t * ff;  // TODO support dcd_edpt_xfer_fifo API
+  tu_fifo_t * ff;
   uint16_t total_len;
   uint16_t queued_len;
   uint16_t pma_ptr;
-  uint8_t max_packet_size;
-  uint8_t pma_alloc_size;
+  uint16_t max_packet_size;
+  uint16_t pma_alloc_size;
+  uint8_t ep_idx; // index for USB_EPnR register
 } xfer_ctl_t;
 
+// EP allocator
+typedef struct
+{
+  uint8_t ep_num;
+  uint8_t ep_type;
+  bool allocated[2];
+} ep_alloc_t;
+
 static xfer_ctl_t xfer_status[MAX_EP_COUNT][2];
 
-static inline xfer_ctl_t* xfer_ctl_ptr(uint32_t epnum, uint32_t dir)
-{
-  return &xfer_status[epnum][dir];
-}
+static ep_alloc_t ep_alloc_status[STFSDEV_EP_COUNT];
 
 static TU_ATTR_ALIGNED(4) uint32_t _setup_packet[6];
 
 static uint8_t remoteWakeCountdown; // When wake is requested
 
+//--------------------------------------------------------------------+
+// Prototypes
+//--------------------------------------------------------------------+
+
 // into the stack.
 static void dcd_handle_bus_reset(void);
 static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix);
@@ -201,23 +207,43 @@ static uint16_t ep_buf_ptr; ///< Points to first free memory location
 static void dcd_pma_alloc_reset(void);
 static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length);
 static void dcd_pma_free(uint8_t ep_addr);
+static void dcd_ep_free(uint8_t ep_addr);
+static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type);
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, size_t wNBytes);
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wNBytes);
 
-//static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wNBytes);
-//static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNBytes);
+static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wNBytes);
+static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNBytes);
+
+//--------------------------------------------------------------------+
+// Inline helper
+//--------------------------------------------------------------------+
+
+TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t* xfer_ctl_ptr(uint32_t ep_addr)
+{
+  uint8_t epnum = tu_edpt_number(ep_addr);
+  uint8_t dir = tu_edpt_dir(ep_addr);
+  // Fix -Werror=null-dereference
+  TU_ASSERT(epnum < MAX_EP_COUNT, &xfer_status[0][0]);
+
+  return &xfer_status[epnum][dir];
+}
 
 // Using a function due to better type checks
 // This seems better than having to do type casts everywhere else
-static inline void reg16_clear_bits(__IO uint16_t *reg, uint16_t mask) {
+TU_ATTR_ALWAYS_INLINE static inline void reg16_clear_bits(__IO uint16_t *reg, uint16_t mask) {
   *reg = (uint16_t)(*reg & ~mask);
 }
 
 // Bits in ISTR are cleared upon writing 0
-static inline void clear_istr_bits(uint16_t mask) {
+TU_ATTR_ALWAYS_INLINE static inline void clear_istr_bits(uint16_t mask) {
   USB->ISTR = ~mask;
 }
 
+//--------------------------------------------------------------------+
+// Controller API
+//--------------------------------------------------------------------+
+
 void dcd_init (uint8_t rhport)
 {
   /* Clocks should already be enabled */
@@ -230,7 +256,7 @@ void dcd_init (uint8_t rhport)
   {
     asm("NOP");
   }
-	// Perform USB peripheral reset
+  // Perform USB peripheral reset
   USB->CNTR = USB_CNTR_FRES | USB_CNTR_PDWN;
   for(uint32_t i = 0; i<200; i++) // should be a few us
   {
@@ -255,7 +281,7 @@ void dcd_init (uint8_t rhport)
     pcd_set_endpoint(USB,i,0u);
   }
 
-  USB->CNTR |= USB_CNTR_RESETM | (USE_SOF ? USB_CNTR_SOFM : 0) | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
+  USB->CNTR |= USB_CNTR_RESETM | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
   dcd_handle_bus_reset();
   
   // Enable pull-up if supported
@@ -300,7 +326,14 @@ void dcd_sof_enable(uint8_t rhport, bool en)
   (void) rhport;
   (void) en;
 
-  // TODO implement later
+  if (en)
+  {
+    USB->CNTR |= USB_CNTR_SOFM;
+  }
+  else
+  {
+    USB->CNTR &= (uint16_t) ~USB_CNTR_SOFM;
+  }
 }
 
 // Enable device interrupt
@@ -410,7 +443,7 @@ void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
   (void) dev_addr;
 
   // Respond with status
-  dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0);
+  dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK | 0x00, NULL, 0);
 
   // DCD can only set address after status for this request is complete.
   // do it at dcd_edpt0_status_complete()
@@ -451,10 +484,17 @@ static void dcd_handle_bus_reset(void)
   //__IO uint16_t * const epreg = &(EPREG(0));
   USB->DADDR = 0u; // disable USB peripheral by clearing the EF flag
 
-  // Clear all EPREG (or maybe this is automatic? I'm not sure)
+  
   for(uint32_t i=0; i<STFSDEV_EP_COUNT; i++)
   {
+    // Clear all EPREG (or maybe this is automatic? I'm not sure)
     pcd_set_endpoint(USB,i,0u);
+    
+    // Clear EP allocation status
+    ep_alloc_status[i].ep_num = 0xFF;
+    ep_alloc_status[i].ep_type = 0xFF;
+    ep_alloc_status[i].allocated[0] = false;
+    ep_alloc_status[i].allocated[1] = false;
   }
 
   dcd_pma_alloc_reset();
@@ -471,6 +511,7 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
 {
   uint32_t EPindex = wIstr & USB_ISTR_EP_ID;
   uint32_t wEPRegVal = pcd_get_endpoint(USB, EPindex);
+  uint8_t ep_addr = (wEPRegVal & USB_EPADDR_FIELD) | TUSB_DIR_IN_MASK;
 
   // Verify the CTR_TX bit is set. This was in the ST Micro code,
   // but I'm not sure it's actually necessary?
@@ -482,14 +523,14 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
   /* clear int flag */
   pcd_clear_tx_ep_ctr(USB, EPindex);
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(EPindex,TUSB_DIR_IN);
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
   if((xfer->total_len != xfer->queued_len)) /* TX not complete */
   {
       dcd_transmit_packet(xfer, EPindex);
   }
   else /* TX Complete */
   {
-    dcd_event_xfer_complete(0, (uint8_t)(0x80 + EPindex), xfer->total_len, XFER_RESULT_SUCCESS, true);
+    dcd_event_xfer_complete(0, ep_addr, xfer->total_len, XFER_RESULT_SUCCESS, true);
   }
 }
 
@@ -500,9 +541,9 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
 {
   uint32_t EPindex = wIstr & USB_ISTR_EP_ID;
   uint32_t wEPRegVal = pcd_get_endpoint(USB, EPindex);
-  uint32_t count = pcd_get_ep_rx_cnt(USB,EPindex);
+  uint8_t ep_addr = wEPRegVal & USB_EPADDR_FIELD;
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(EPindex,TUSB_DIR_OUT);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
 
   // Verify the CTR_RX bit is set. This was in the ST Micro code,
   // but I'm not sure it's actually necessary?
@@ -511,11 +552,12 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
     return;
   }
   
-  if((EPindex == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) /* Setup packet */
+  if((ep_addr == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) /* Setup packet */
   {
     // The setup_received function uses memcpy, so this must first copy the setup data into
     // user memory, to allow for the 32-bit access that memcpy performs.
     uint8_t userMemBuf[8];
+    uint32_t count = pcd_get_ep_rx_cnt(USB, EPindex);
     /* Get SETUP Packet*/
     if(count == 8) // Setup packet should always be 8 bytes. If not, ignore it, and try again.
     {
@@ -528,23 +570,33 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
   }
   else
   {
+    uint32_t count;
+    /* Read from correct register when ISOCHRONOUS (double buffered) */
+    if ( (wEPRegVal & USB_EP_DTOG_RX) && ( (wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) ) {
+      count = pcd_get_ep_tx_cnt(USB, EPindex);
+    } else {
+      count = pcd_get_ep_rx_cnt(USB, EPindex);
+    }
+
+    TU_ASSERT(count <= xfer->max_packet_size, /**/);
+
     // Clear RX CTR interrupt flag
-    if(EPindex != 0u)
+    if(ep_addr != 0u)
     {
       pcd_clear_rx_ep_ctr(USB, EPindex);
     }
 
     if (count != 0U)
     {
-#if 0 // TODO support dcd_edpt_xfer_fifo API
+      uint16_t addr = *pcd_ep_rx_address_ptr(USB, EPindex);
+
       if (xfer->ff)
       {
-        dcd_read_packet_memory_ff(xfer->ff, *pcd_ep_rx_address_ptr(USB,EPindex), count);
+        dcd_read_packet_memory_ff(xfer->ff, addr, count);
       }
       else
-#endif
       {
-        dcd_read_packet_memory(&(xfer->buffer[xfer->queued_len]), *pcd_ep_rx_address_ptr(USB,EPindex), count);
+        dcd_read_packet_memory(&(xfer->buffer[xfer->queued_len]), addr, count);
       }
 
       xfer->queued_len = (uint16_t)(xfer->queued_len + count);
@@ -553,7 +605,7 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
     if ((count < xfer->max_packet_size) || (xfer->queued_len == xfer->total_len))
     {
       /* RX COMPLETE */
-      dcd_event_xfer_complete(0, EPindex, xfer->queued_len, XFER_RESULT_SUCCESS, true);
+      dcd_event_xfer_complete(0, ep_addr, xfer->queued_len, XFER_RESULT_SUCCESS, true);
       // Though the host could still send, we don't know.
       // Does the bulk pipe need to be reset to valid to allow for a ZLP?
     }
@@ -561,21 +613,26 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
     {
       uint32_t remaining = (uint32_t)xfer->total_len - (uint32_t)xfer->queued_len;
       if(remaining >= xfer->max_packet_size) {
-        pcd_set_ep_rx_cnt(USB, EPindex,xfer->max_packet_size);
+        pcd_set_ep_rx_bufsize(USB, EPindex,xfer->max_packet_size);
       } else {
-        pcd_set_ep_rx_cnt(USB, EPindex,remaining);
+        pcd_set_ep_rx_bufsize(USB, EPindex,remaining);
+      }
+
+      if (!((wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS)) {
+        /* Set endpoint active again for receiving more data.
+         * Note that isochronous endpoints stay active always */
+        pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_VALID);
       }
-      pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_VALID);
     }
   }
 
   // For EP0, prepare to receive another SETUP packet.
   // Clear CTR last so that a new packet does not overwrite the packing being read.
   // (Based on the docs, it seems SETUP will always be accepted after CTR is cleared)
-  if(EPindex == 0u)
+  if(ep_addr == 0u)
   {
       // Always be prepared for a status packet...
-    pcd_set_ep_rx_cnt(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
+    pcd_set_ep_rx_bufsize(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
     pcd_clear_rx_ep_ctr(USB, EPindex);
   }
 }
@@ -612,6 +669,12 @@ void dcd_int_handler(uint8_t rhport) {
   // dcd_ep_ctr_handler(), so less need to loop here. The other interrupts shouldn't
   // be triggered repeatedly.
 
+  /* Put SOF flag at the beginning of ISR in case to get least amount of jitter if it is used for timing purposes */
+  if(int_status & USB_ISTR_SOF) {
+    clear_istr_bits(USB_ISTR_SOF);
+    dcd_event_sof(0, USB->FNR & USB_FNR_FN, true);
+  }
+
   if(int_status & USB_ISTR_RESET) {
     // USBRST is start of reset.
     clear_istr_bits(USB_ISTR_RESET);
@@ -649,13 +712,6 @@ void dcd_int_handler(uint8_t rhport) {
     dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
   }
 
-#if USE_SOF
-  if(int_status & USB_ISTR_SOF) {
-    clear_istr_bits(USB_ISTR_SOF);
-    dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
-  }
-#endif 
-
   if(int_status & USB_ISTR_ESOF) {
     if(remoteWakeCountdown == 1u)
     {
@@ -693,14 +749,15 @@ void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * re
 
 static void dcd_pma_alloc_reset(void)
 {
+  open_ep_count = 0;
   ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8*MAX_EP_COUNT; // 8 bytes per endpoint (two TX and two RX words, each)
   //TU_LOG2("dcd_pma_alloc_reset()\r\n");
   for(uint32_t i=0; i<MAX_EP_COUNT; i++)
   {
-    xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_alloc_size = 0U;
-    xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_alloc_size = 0U;
-    xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_ptr = 0U;
-    xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_ptr = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_alloc_size = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_alloc_size = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_ptr = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_ptr = 0U;
   }
 }
 
@@ -715,9 +772,7 @@ static void dcd_pma_alloc_reset(void)
  */
 static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
 {
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-  xfer_ctl_t* epXferCtl = xfer_ctl_ptr(epnum,dir);
+  xfer_ctl_t* epXferCtl = xfer_ctl_ptr(ep_addr);
 
   if(epXferCtl->pma_alloc_size != 0U)
   {
@@ -726,13 +781,15 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
     TU_ASSERT(length <= epXferCtl->pma_alloc_size, 0xFFFF);  // Verify no larger than previous alloc
     return epXferCtl->pma_ptr;
   }
-  
-  uint16_t addr = ep_buf_ptr; 
+
+  open_ep_count++;
+
+  uint16_t addr = ep_buf_ptr;
   ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
-  
+
   // Verify no overflow
   TU_ASSERT(ep_buf_ptr <= PMA_LENGTH, 0xFFFF);
-  
+
   epXferCtl->pma_ptr = addr;
   epXferCtl->pma_alloc_size = length;
   //TU_LOG2("dcd_pma_alloc(%x,%x)=%x\r\n",ep_addr,length,addr);
@@ -745,12 +802,9 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
  */
 static void dcd_pma_free(uint8_t ep_addr)
 {
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
   // Presently, this should never be called for EP0 IN/OUT
   TU_ASSERT(open_ep_count > 2, /**/);
-  TU_ASSERT(xfer_ctl_ptr(epnum,dir)->max_packet_size != 0, /**/);
+  TU_ASSERT(xfer_ctl_ptr(ep_addr)->max_packet_size != 0, /**/);
   open_ep_count--;
 
   // If count is 2, only EP0 should be open, so allocations can be mostly reset.
@@ -762,10 +816,83 @@ static void dcd_pma_free(uint8_t ep_addr)
     // Skip EP0
     for(uint32_t i=1; i<MAX_EP_COUNT; i++)
     {
-      xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_alloc_size = 0U;
-      xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_alloc_size = 0U;
-      xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_ptr = 0U;
-      xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_ptr = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_alloc_size = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_alloc_size = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_ptr = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_ptr = 0U;
+    }
+  }
+}
+
+/***
+ * Allocate hardware endpoint
+ */
+static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  for(uint8_t i = 0; i < STFSDEV_EP_COUNT; i++)
+  {
+    // Check if already allocated
+    if(ep_alloc_status[i].allocated[dir] &&
+       ep_alloc_status[i].ep_type == ep_type &&
+       ep_alloc_status[i].ep_num == epnum)
+    {
+      return i;
+    }
+
+    // If EP of current direction is not allocated
+    // Except for ISO endpoint, both direction should be free
+    if(!ep_alloc_status[i].allocated[dir] &&
+       (ep_type != TUSB_XFER_ISOCHRONOUS || !ep_alloc_status[i].allocated[dir ^ 1]))
+    {
+      // Check if EP number is the same
+      if(ep_alloc_status[i].ep_num == 0xFF ||
+         ep_alloc_status[i].ep_num == epnum)
+      {
+        // One EP pair has to be the same type
+        if(ep_alloc_status[i].ep_type == 0xFF ||
+           ep_alloc_status[i].ep_type == ep_type)
+        {
+          ep_alloc_status[i].ep_num = epnum;
+          ep_alloc_status[i].ep_type = ep_type;
+          ep_alloc_status[i].allocated[dir] = true;
+          
+          return i;
+        }
+      }
+    }
+  }
+
+  // Allocation failed
+  TU_ASSERT(0);
+}
+
+/***
+ * Free hardware endpoint
+ */
+static void dcd_ep_free(uint8_t ep_addr)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  for(uint8_t i = 0; i < STFSDEV_EP_COUNT; i++)
+  {
+    // Check if EP number & dir are the same
+    if(ep_alloc_status[i].ep_num == epnum && 
+       ep_alloc_status[i].allocated[dir] == dir)
+    {
+      ep_alloc_status[i].allocated[dir] = false;
+      // Reset entry if ISO endpoint or both direction are free
+      if(ep_alloc_status[i].ep_type == TUSB_XFER_ISOCHRONOUS ||
+         !ep_alloc_status[i].allocated[dir ^ 1])
+      {
+        ep_alloc_status[i].ep_num = 0xFF;
+        ep_alloc_status[i].ep_type = 0xFF;
+
+        return;
+      }
     }
   }
 }
@@ -776,27 +903,24 @@ static void dcd_pma_free(uint8_t ep_addr)
 bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 {
   (void)rhport;
-  uint8_t const epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
+  uint8_t const ep_idx = dcd_ep_alloc(p_endpoint_desc->bEndpointAddress, p_endpoint_desc->bmAttributes.xfer);
   uint8_t const dir   = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
-  const uint16_t epMaxPktSize = tu_edpt_packet_size(p_endpoint_desc);
+  const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
+  const uint16_t buffer_size = pcd_aligned_buffer_size(packet_size);
   uint16_t pma_addr;
   uint32_t wType;
-  
-  // Isochronous not supported (yet), and some other driver assumptions.
-  TU_ASSERT(p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS);
-  TU_ASSERT(epnum < MAX_EP_COUNT);
+
+  TU_ASSERT(ep_idx < STFSDEV_EP_COUNT);
+  TU_ASSERT(buffer_size <= 1024);
 
   // Set type
   switch(p_endpoint_desc->bmAttributes.xfer) {
   case TUSB_XFER_CONTROL:
     wType = USB_EP_CONTROL;
     break;
-#if (0)
-  case TUSB_XFER_ISOCHRONOUS: // FIXME: Not yet supported
+  case TUSB_XFER_ISOCHRONOUS:
     wType = USB_EP_ISOCHRONOUS;
     break;
-#endif
-
   case TUSB_XFER_BULK:
     wType = USB_EP_CONTROL;
     break;
@@ -809,30 +933,49 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
     TU_ASSERT(false);
   }
 
-  pcd_set_eptype(USB, epnum, wType);
-  pcd_set_ep_address(USB, epnum, epnum);
+  pcd_set_eptype(USB, ep_idx, wType);
+  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(p_endpoint_desc->bEndpointAddress));
   // Be normal, for now, instead of only accepting zero-byte packets (on control endpoint)
   // or being double-buffered (bulk endpoints)
   pcd_clear_ep_kind(USB,0);
 
-  pma_addr = dcd_pma_alloc(p_endpoint_desc->bEndpointAddress, epMaxPktSize);
+  /* Create a packet memory buffer area. For isochronous endpoints,
+   * use the same buffer as the double buffer, essentially disabling double buffering */
+  pma_addr = dcd_pma_alloc(p_endpoint_desc->bEndpointAddress, buffer_size);
 
-  if(dir == TUSB_DIR_IN)
+  if( (dir == TUSB_DIR_IN) || (wType == USB_EP_ISOCHRONOUS) )
   {
-    *pcd_ep_tx_address_ptr(USB, epnum) = pma_addr;
-    pcd_set_ep_tx_cnt(USB, epnum, epMaxPktSize);
-    pcd_clear_tx_dtog(USB, epnum);
-    pcd_set_ep_tx_status(USB,epnum,USB_EP_TX_NAK);
+    *pcd_ep_tx_address_ptr(USB, ep_idx) = pma_addr;
+    pcd_set_ep_tx_bufsize(USB, ep_idx, buffer_size);
+    pcd_clear_tx_dtog(USB, ep_idx);
   }
-  else
+
+  if( (dir == TUSB_DIR_OUT) || (wType == USB_EP_ISOCHRONOUS) )
+  {
+    *pcd_ep_rx_address_ptr(USB, ep_idx) = pma_addr;
+    pcd_set_ep_rx_bufsize(USB, ep_idx, buffer_size);
+    pcd_clear_rx_dtog(USB, ep_idx);
+  }
+
+  /* Enable endpoint */
+  if (dir == TUSB_DIR_IN)
+  {
+    if(wType == USB_EP_ISOCHRONOUS) {
+      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
+    } else {
+      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
+    }
+  } else
   {
-    *pcd_ep_rx_address_ptr(USB, epnum) = pma_addr;
-    pcd_set_ep_rx_cnt(USB, epnum, epMaxPktSize);
-    pcd_clear_rx_dtog(USB, epnum);
-    pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_NAK);
+    if(wType == USB_EP_ISOCHRONOUS) {
+      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
+    } else {
+      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
+    }
   }
 
-  xfer_ctl_ptr(epnum, dir)->max_packet_size = epMaxPktSize;
+  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = packet_size;
+  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->ep_idx = ep_idx;
 
   return true;
 }
@@ -853,21 +996,81 @@ void dcd_edpt_close_all (uint8_t rhport)
 void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
-  uint32_t const epnum = tu_edpt_number(ep_addr);
-  uint32_t const dir   = tu_edpt_dir(ep_addr);
-  
+
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
+
   if(dir == TUSB_DIR_IN)
   {
-    pcd_set_ep_tx_status(USB,epnum,USB_EP_TX_DIS);
+    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
   }
   else
   {
-    pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_DIS);
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
   }
 
+  dcd_ep_free(ep_addr);
+
   dcd_pma_free(ep_addr);
 }
 
+bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size)
+{
+  (void)rhport;
+  
+  TU_ASSERT(largest_packet_size <= 1024);
+  
+  uint8_t const ep_idx = dcd_ep_alloc(ep_addr, TUSB_XFER_ISOCHRONOUS);
+  const uint16_t buffer_size = pcd_aligned_buffer_size(largest_packet_size);
+
+  /* Create a packet memory buffer area. For isochronous endpoints,
+   * use the same buffer as the double buffer, essentially disabling double buffering */
+  uint16_t pma_addr = dcd_pma_alloc(ep_addr, buffer_size);
+
+  xfer_ctl_ptr(ep_addr)->ep_idx = ep_idx;
+
+  pcd_set_eptype(USB, ep_idx, USB_EP_ISOCHRONOUS);
+  
+  *pcd_ep_tx_address_ptr(USB, ep_idx) = pma_addr;
+  *pcd_ep_rx_address_ptr(USB, ep_idx) = pma_addr;
+  
+  return true;
+}
+
+bool dcd_edpt_iso_activate(uint8_t rhport,  tusb_desc_endpoint_t const * p_endpoint_desc)
+{
+  (void)rhport;
+  uint8_t const ep_idx = xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
+  const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
+  const uint16_t buffer_size = pcd_aligned_buffer_size(packet_size);
+
+  /* Disable endpoint */
+  if(dir == TUSB_DIR_IN)
+  {
+    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
+  }
+  else
+  {
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
+  }
+  
+  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(p_endpoint_desc->bEndpointAddress));
+  // Be normal, for now, instead of only accepting zero-byte packets (on control endpoint)
+  // or being double-buffered (bulk endpoints)
+  pcd_clear_ep_kind(USB,0);
+
+  pcd_set_ep_tx_bufsize(USB, ep_idx, buffer_size);
+  pcd_set_ep_rx_bufsize(USB, ep_idx, buffer_size);
+  pcd_clear_tx_dtog(USB, ep_idx);
+  pcd_clear_rx_dtog(USB, ep_idx);
+
+  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = packet_size;
+
+  return true;
+}
+
 // Currently, single-buffered, and only 64 bytes at a time (max)
 
 static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix)
@@ -878,21 +1081,27 @@ static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix)
   {
     len = xfer->max_packet_size;
   }
-  uint16_t oldAddr = *pcd_ep_tx_address_ptr(USB,ep_ix);
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
+  uint16_t ep_reg = pcd_get_endpoint(USB, ep_ix);
+  uint16_t addr_ptr = *pcd_ep_tx_address_ptr(USB,ep_ix);
+
   if (xfer->ff)
   {
-    dcd_write_packet_memory_ff(xfer->ff, oldAddr, len);
+    dcd_write_packet_memory_ff(xfer->ff, addr_ptr, len);
   }
   else
-#endif
   {
-    dcd_write_packet_memory(oldAddr, &(xfer->buffer[xfer->queued_len]), len);
+    dcd_write_packet_memory(addr_ptr, &(xfer->buffer[xfer->queued_len]), len);
   }
   xfer->queued_len = (uint16_t)(xfer->queued_len + len);
 
-  pcd_set_ep_tx_cnt(USB,ep_ix,len);
+  /* Write into correct register when ISOCHRONOUS (double buffered) */
+  if ( (ep_reg & USB_EP_DTOG_TX) && ( (ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) ) {
+    pcd_set_ep_rx_cnt(USB, ep_ix, len);
+  } else {
+    pcd_set_ep_tx_cnt(USB, ep_ix, len);
+  }
+
   pcd_set_ep_tx_status(USB, ep_ix, USB_EP_TX_VALID);
 }
 
@@ -900,13 +1109,12 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
 {
   (void) rhport;
 
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
-  xfer_ctl_t * xfer = xfer_ctl_ptr(epnum,dir);
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
 
   xfer->buffer = buffer;
-  // xfer->ff     = NULL; // TODO support dcd_edpt_xfer_fifo API
+  xfer->ff     = NULL;
   xfer->total_len = total_bytes;
   xfer->queued_len = 0;
 
@@ -914,37 +1122,36 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   {
     // A setup token can occur immediately after an OUT STATUS packet so make sure we have a valid
     // buffer for the control endpoint.
-    if (epnum == 0 && buffer == NULL)
+    if (ep_idx == 0 && buffer == NULL)
     {
         xfer->buffer = (uint8_t*)_setup_packet;
     }
+
     if(total_bytes > xfer->max_packet_size)
     {
-      pcd_set_ep_rx_cnt(USB,epnum,xfer->max_packet_size);
+      pcd_set_ep_rx_bufsize(USB,ep_idx,xfer->max_packet_size);
     } else {
-      pcd_set_ep_rx_cnt(USB,epnum,total_bytes);
+      pcd_set_ep_rx_bufsize(USB,ep_idx,total_bytes);
     }
-    pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_VALID);
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_VALID);
   }
   else // IN
   {
-    dcd_transmit_packet(xfer,epnum);
+    dcd_transmit_packet(xfer,ep_idx);
   }
   return true;
 }
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
 bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes)
 {
   (void) rhport;
 
-  uint8_t const epnum = tu_edpt_number(ep_addr);
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const epnum = xfer->ep_idx;
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(epnum,dir);
-
   xfer->buffer = NULL;
-  // xfer->ff     = ff; // TODO support dcd_edpt_xfer_fifo API
+  xfer->ff     = ff;
   xfer->total_len = total_bytes;
   xfer->queued_len = 0;
 
@@ -952,9 +1159,9 @@ bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
   {
     if(total_bytes > xfer->max_packet_size)
     {
-      pcd_set_ep_rx_cnt(USB,epnum,xfer->max_packet_size);
+      pcd_set_ep_rx_bufsize(USB,epnum,xfer->max_packet_size);
     } else {
-      pcd_set_ep_rx_cnt(USB,epnum,total_bytes);
+      pcd_set_ep_rx_bufsize(USB,epnum,total_bytes);
     }
     pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_VALID);
   }
@@ -964,19 +1171,22 @@ bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
   }
   return true;
 }
-#endif
 
 void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
 
-  if (ep_addr & 0x80)
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
+
+  if (dir == TUSB_DIR_IN)
   { // IN
-    pcd_set_ep_tx_status(USB, ep_addr & 0x7F, USB_EP_TX_STALL);
+    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_STALL);
   }
   else
   { // OUT
-    pcd_set_ep_rx_status(USB, ep_addr, USB_EP_RX_STALL);
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_STALL);
   }
 }
 
@@ -984,21 +1194,26 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
 
-  if (ep_addr & 0x80)
-  { // IN
-    ep_addr &= 0x7F;
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
 
-    pcd_set_ep_tx_status(USB,ep_addr, USB_EP_TX_NAK);
+  if (dir == TUSB_DIR_IN)
+  { // IN
+    if (pcd_get_eptype(USB, ep_idx) !=  USB_EP_ISOCHRONOUS) {
+      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
+    }
 
     /* Reset to DATA0 if clearing stall condition. */
-    pcd_clear_tx_dtog(USB,ep_addr);
+    pcd_clear_tx_dtog(USB, ep_idx);
   }
   else
   { // OUT
+    if (pcd_get_eptype(USB, ep_idx) !=  USB_EP_ISOCHRONOUS) {
+      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
+    }
     /* Reset to DATA0 if clearing stall condition. */
-    pcd_clear_rx_dtog(USB,ep_addr);
-
-    pcd_set_ep_rx_status(USB,ep_addr, USB_EP_RX_NAK);
+    pcd_clear_rx_dtog(USB, ep_idx);
   }
 }
 
@@ -1015,8 +1230,7 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
   */
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, size_t wNBytes)
 {
-  uint32_t n =  ((uint32_t)wNBytes + 1U) >> 1U;
-  uint32_t i;
+  uint32_t n = (uint32_t)wNBytes >> 1U;
   uint16_t temp1, temp2;
   const uint8_t * srcVal;
 
@@ -1027,64 +1241,74 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, si
   srcVal = src;
   pdwVal = &pma[PMA_STRIDE*(dst>>1)];
 
-  for (i = n; i != 0; i--)
+  while (n--)
   {
-    temp1 = (uint16_t) *srcVal;
+    temp1 = (uint16_t)*srcVal;
     srcVal++;
-    temp2 = temp1 | ((uint16_t)((uint16_t) ((*srcVal) << 8U))) ;
+    temp2 = temp1 | ((uint16_t)(((uint16_t)(*srcVal)) << 8U)) ;
     *pdwVal = temp2;
     pdwVal += PMA_STRIDE;
     srcVal++;
   }
+
+  if (wNBytes & 0x01)
+  {
+    temp1 = *srcVal;
+    *pdwVal = temp1;
+  }
+
   return true;
 }
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
 /**
   * @brief Copy from FIFO to packet memory area (PMA).
   *        Uses byte-access of system memory and 16-bit access of packet memory
   * @param   wNBytes no. of bytes to be copied.
   * @retval None
   */
-
-// THIS FUNCTION IS UNTESTED
-
 static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wNBytes)
 {
   // Since we copy from a ring buffer FIFO, a wrap might occur making it necessary to conduct two copies
-  // Check for first linear part
-  void * src;
-  uint16_t len = tu_fifo_get_linear_read_info(ff, 0, &src, wNBytes);  // We want to read from the FIFO        - THIS FUNCTION CHANGED!!!
-  TU_VERIFY(len && dcd_write_packet_memory(dst, src, len));           // and write it into the PMA
-  tu_fifo_advance_read_pointer(ff, len);
-
-  // Check for wrapped part
-  if (len < wNBytes)
+  tu_fifo_buffer_info_t info;
+  tu_fifo_get_read_info(ff, &info); 
+  
+  uint16_t cnt_lin =  TU_MIN(wNBytes, info.len_lin);
+  uint16_t cnt_wrap = TU_MIN(wNBytes - cnt_lin, info.len_wrap);
+  
+  // We want to read from the FIFO and write it into the PMA, if LIN part is ODD and has WRAPPED part,
+  // last lin byte will be combined with wrapped part
+  // To ensure PMA is always access 16bit aligned (dst aligned to 16 bit)
+  if((cnt_lin & 0x01) && cnt_wrap)
   {
-    // Get remaining wrapped length
-    uint16_t len2 = tu_fifo_get_linear_read_info(ff, 0, &src, wNBytes - len);
-    TU_VERIFY(len2);
+    // Copy first linear part
+    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin &~0x01);
+    dst += cnt_lin &~0x01;
+
+    // Copy last linear byte & first wrapped byte
+    uint16_t tmp = ((uint8_t*)info.ptr_lin)[cnt_lin - 1] | ((uint16_t)(((uint8_t*)info.ptr_wrap)[0]) << 8U);
+    dcd_write_packet_memory(dst, &tmp, 2);
+    dst += 2;
 
-    // Update destination pointer
-    dst += len;
+    // Copy rest of wrapped byte
+    dcd_write_packet_memory(dst, ((uint8_t*)info.ptr_wrap) + 1, cnt_wrap - 1);
+  }
+  else
+  {
+    // Copy linear part
+    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin);
+    dst += info.len_lin;
 
-    // Since PMA is accessed 16-bit wise we need to handle the case when a 16 bit value was split
-    if (len % 2)    // If len is uneven there is a byte left to copy
+    if(info.len_wrap)
     {
-      // Since PMA can accessed only 16 bit-wise we copy the last byte again
-      tu_fifo_backward_read_pointer(ff, 1);                 // Move one byte back and copy two bytes for the PMA
-      tu_fifo_read_n(ff, (void *) &pma[PMA_STRIDE*(dst>>1)], 2);     // Since EP FIFOs must be of item size 1 this is safe to do
-      dst++;
-      len2--;
+      // Copy wrapped byte
+      dcd_write_packet_memory(dst, info.ptr_wrap, cnt_wrap);
     }
-
-    TU_VERIFY(dcd_write_packet_memory(dst, src, len2));
-    tu_fifo_advance_write_pointer(ff, len2);
   }
 
+  tu_fifo_advance_read_pointer(ff, cnt_lin + cnt_wrap);
+
   return true;
 }
-#endif
 
 /**
   * @brief Copy a buffer from packet memory area (PMA) to user memory area.
@@ -1095,7 +1319,6 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wN
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wNBytes)
 {
   uint32_t n = (uint32_t)wNBytes >> 1U;
-  uint32_t i;
   // The GCC optimizer will combine access to 32-bit sizes if we let it. Force
   // it volatile so that it won't do that.
   __IO const uint16_t *pdwVal;
@@ -1104,7 +1327,7 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wN
   pdwVal = &pma[PMA_STRIDE*(src>>1)];
   uint8_t *dstVal = (uint8_t*)dst;
 
-  for (i = n; i != 0U; i--)
+  while (n--)
   {
     temp = *pdwVal;
     pdwVal += PMA_STRIDE;
@@ -1112,7 +1335,7 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wN
     *dstVal++ = ((temp >> 8) & 0xFF);
   }
 
-  if (wNBytes % 2)
+  if (wNBytes & 0x01)
   {
     temp = *pdwVal;
     pdwVal += PMA_STRIDE;
@@ -1121,52 +1344,59 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wN
   return true;
 }
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
 /**
   * @brief Copy a buffer from user packet memory area (PMA) to FIFO.
   *        Uses byte-access of system memory and 16-bit access of packet memory
   * @param   wNBytes no. of bytes to be copied.
   * @retval None
   */
-
-// THIS FUNCTION IS UNTESTED
-
 static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNBytes)
 {
   // Since we copy into a ring buffer FIFO, a wrap might occur making it necessary to conduct two copies
   // Check for first linear part
-  void * dst;
-  uint16_t len = tu_fifo_get_linear_write_info(ff, 0, &dst, wNBytes);           // THIS FUNCTION CHANGED!!!!
-  TU_VERIFY(len && dcd_read_packet_memory(dst, src, len));
-  tu_fifo_advance_write_pointer(ff, len);
+  tu_fifo_buffer_info_t info;
+  tu_fifo_get_write_info(ff, &info);  // We want to read from the FIFO
+
+  uint16_t cnt_lin =  TU_MIN(wNBytes, info.len_lin);
+  uint16_t cnt_wrap = TU_MIN(wNBytes - cnt_lin, info.len_wrap);
 
-  // Check for wrapped part
-  if (len < wNBytes)
+  // We want to read from PMA and write it into the FIFO, if LIN part is ODD and has WRAPPED part,
+  // last lin byte will be combined with wrapped part
+  // To ensure PMA is always access 16bit aligned (src aligned to 16 bit)
+  if((cnt_lin & 0x01) && cnt_wrap)
   {
-    // Get remaining wrapped length
-    uint16_t len2 = tu_fifo_get_linear_write_info(ff, 0, &dst, wNBytes - len);
-    TU_VERIFY(len2);
+    // Copy first linear part
+    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin &~0x01);
+    src += cnt_lin &~0x01;
 
-    // Update source pointer
-    src += len;
+    // Copy last linear byte & first wrapped byte
+    uint16_t tmp;
+    dcd_read_packet_memory(&tmp, src, 2);
+    
+    ((uint8_t*)info.ptr_lin)[cnt_lin - 1] = (uint8_t)tmp;
+    ((uint8_t*)info.ptr_wrap)[0] = (uint8_t)(tmp >> 8U);
+    src += 2;
 
-    // Since PMA is accessed 16-bit wise we need to handle the case when a 16 bit value was split
-    if (len % 2)    // If len is uneven there is a byte left to copy
+    // Copy rest of wrapped byte
+    dcd_read_packet_memory(((uint8_t*)info.ptr_wrap) + 1, src, cnt_wrap - 1);
+  }
+  else
+  {
+    // Copy linear part
+    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin);
+    src += cnt_lin;
+
+    if(info.len_wrap)
     {
-      uint32_t temp = pma[PMA_STRIDE*(src>>1)];
-      *((uint8_t *)dst++) = ((temp >> 8) & 0xFF);
-      src++;
-      len2--;
+      // Copy wrapped byte
+      dcd_read_packet_memory(info.ptr_wrap, src, cnt_wrap);
     }
-
-    TU_VERIFY(dcd_read_packet_memory(dst, src, len2));
-    tu_fifo_advance_write_pointer(ff, len2);
   }
 
+  tu_fifo_advance_write_pointer(ff, cnt_lin + cnt_wrap);
+
   return true;
 }
 
 #endif
 
-#endif
-
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
index 920d12c1b..039fb13d2 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
@@ -120,76 +120,89 @@
 static __IO uint16_t * const pma = (__IO uint16_t*)USB_PMAADDR;
 
 // prototypes
-static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum);
-static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum);
-static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wRegValue);
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx);
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx);
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue);
 
+/* Aligned buffer size according to hardware */
+TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t size)
+{
+  /* The STM32 full speed USB peripheral supports only a limited set of
+   * buffer sizes given by the RX buffer entry format in the USB_BTABLE. */
+  uint16_t blocksize = (size > 62) ? 32 : 2;
+
+  // Round up while dividing requested size by blocksize
+  uint16_t numblocks = (size + blocksize - 1) / blocksize ;
+
+  return numblocks * blocksize;
+}
 
 /* SetENDPOINT */
-static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wRegValue)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue)
 {
-  __O uint16_t *reg = (__O uint16_t *)((&USBx->EP0R) + bEpNum*2u);
+  __O uint16_t *reg = (__O uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
   *reg = (uint16_t)wRegValue;
 }
 
 /* GetENDPOINT */
-static inline uint16_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpNum) {
-  __I uint16_t *reg = (__I uint16_t *)((&USBx->EP0R) + bEpNum*2u);
+TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx) {
+  __I uint16_t *reg = (__I uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
   return *reg;
 }
 
-static inline void pcd_set_eptype(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wType)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_eptype(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wType)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= (uint32_t)USB_EP_T_MASK;
   regVal |= wType;
   regVal |= USB_EP_CTR_RX | USB_EP_CTR_TX; // These clear on write0, so must set high
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EP_T_FIELD;
   return regVal;
 }
 /**
   * @brief  Clears bit CTR_RX / CTR_TX in the endpoint register.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal &= ~USB_EP_CTR_RX;
   regVal |= USB_EP_CTR_TX; // preserve CTR_TX (clears on writing 0)
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
-static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpNum)
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal &= ~USB_EP_CTR_TX;
   regVal |= USB_EP_CTR_RX; // preserve CTR_RX (clears on writing 0)
-  pcd_set_endpoint(USBx, bEpNum,regVal);
+  pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 /**
   * @brief  gets counter of the tx buffer.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval Counter value
   */
-static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  __I uint16_t *regPtr = pcd_ep_tx_cnt_ptr(USBx, bEpNum);
+  __I uint16_t *regPtr = pcd_ep_tx_cnt_ptr(USBx, bEpIdx);
   return *regPtr & 0x3ffU;
 }
 
-static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  __I uint16_t *regPtr = pcd_ep_rx_cnt_ptr(USBx, bEpNum);
+  __I uint16_t *regPtr = pcd_ep_rx_cnt_ptr(USBx, bEpIdx);
   return *regPtr & 0x3ffU;
 }
 
@@ -200,49 +213,36 @@ static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpNum)
   * @param  wNBlocks no. of Blocks.
   * @retval None
   */
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_cnt_reg(__O uint16_t * pdwReg, size_t wCount)
+{
+  /* We assume that the buffer size is already aligned to hardware requirements. */
+  uint16_t blocksize = (wCount > 62) ? 1 : 0;
+  uint16_t numblocks = wCount / (blocksize ? 32 : 2);
 
-static inline void pcd_set_ep_cnt_rx_reg(__O uint16_t * pdwReg, size_t wCount)  {
-  uint32_t wNBlocks;
-  if(wCount > 62u)
-  {
-    wNBlocks = wCount >> 5u;
-    if((wCount & 0x1fU) == 0u)
-    {
-      wNBlocks--;
-    }
-    wNBlocks = wNBlocks << 10u;
-    wNBlocks |= 0x8000u; // Mark block size as 32byte
-    *pdwReg = (uint16_t)wNBlocks;
-  }
-  else
-  {
-    wNBlocks = wCount >> 1u;
-    if((wCount & 0x1U) != 0u)
-    {
-      wNBlocks++;
-    }
-    *pdwReg = (uint16_t)((wNBlocks) << 10u);
-  }
-}
+  /* There should be no remainder in the above calculation */
+  TU_ASSERT((wCount - (numblocks * (blocksize ? 32 : 2))) == 0, /**/);
 
+  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
+  *pdwReg = (blocksize << 15) | ((numblocks - blocksize) << 10);
+}
 
 /**
   * @brief  Sets address in an endpoint register.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @param  bAddr Address.
   * @retval None
   */
-static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t bAddr)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t bAddr)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= bAddr;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpNum,regVal);
+  pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 
-static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
 {
   size_t total_word_offset = (((USBx)->BTABLE)>>1) + x;
   total_word_offset *= PMA_STRIDE;
@@ -250,46 +250,61 @@ static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
 }
 
 // Pointers to the PMA table entries (using the ARM address space)
-static inline __IO uint16_t* pcd_ep_tx_address_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_address_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+{
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 0u);
+}
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+{
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 1u);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_address_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  return pcd_btable_word_ptr(USBx,(bEpNum)*4u + 0u);
+  return  pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 2u);
 }
-static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  return pcd_btable_word_ptr(USBx,(bEpNum)*4u + 1u);
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 3u);
 }
 
-static inline __IO uint16_t* pcd_ep_rx_address_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  return  pcd_btable_word_ptr(USBx,(bEpNum)*4u + 2u);
+  __IO uint16_t * reg = pcd_ep_tx_cnt_ptr(USBx, bEpIdx);
+  *reg = (uint16_t) (*reg & (uint16_t) ~0x3FFU) | (wCount & 0x3FFU);
 }
 
-static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  return pcd_btable_word_ptr(USBx,(bEpNum)*4u + 3u);
+  __IO uint16_t * reg = pcd_ep_rx_cnt_ptr(USBx, bEpIdx);
+  *reg = (uint16_t) (*reg & (uint16_t) ~0x3FFU) | (wCount & 0x3FFU);
 }
 
-static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_bufsize(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  *pcd_ep_tx_cnt_ptr(USBx, bEpNum) = (uint16_t)wCount;
+  __IO uint16_t *pdwReg = pcd_ep_tx_cnt_ptr((USBx),(bEpIdx));
+  wCount = pcd_aligned_buffer_size(wCount);
+  pcd_set_ep_cnt_reg(pdwReg, wCount);
 }
 
-static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  __IO uint16_t *pdwReg = pcd_ep_rx_cnt_ptr((USBx),(bEpNum));
-  pcd_set_ep_cnt_rx_reg(pdwReg, wCount);
+  __IO uint16_t *pdwReg = pcd_ep_rx_cnt_ptr((USBx),(bEpIdx));
+  wCount = pcd_aligned_buffer_size(wCount);
+  pcd_set_ep_cnt_reg(pdwReg, wCount);
 }
 
 /**
   * @brief  sets the status for tx transfer (bits STAT_TX[1:0]).
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @param  wState new state
   * @retval None
   */
-static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wState)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPTX_DTOGMASK;
 
   /* toggle first bit ? */
@@ -302,21 +317,22 @@ static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpNum, ui
   {
     regVal ^= USB_EPTX_DTOG2;
   }
+
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 } /* pcd_set_ep_tx_status */
 
 /**
   * @brief  sets the status for rx transfer (bits STAT_TX[1:0])
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @param  wState new state
   * @retval None
   */
 
-static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wState)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPRX_DTOGMASK;
 
   /* toggle first bit ? */
@@ -329,13 +345,14 @@ static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum, ui
   {
     regVal ^= USB_EPRX_DTOG2;
   }
+
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 } /* pcd_set_ep_rx_status */
 
-static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   return (regVal & USB_EPRX_STAT) >> (12u);
 } /* pcd_get_ep_rx_status */
 
@@ -343,71 +360,71 @@ static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum
 /**
   * @brief  Toggles DTOG_RX / DTOG_TX bit in the endpoint register.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_RX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_TX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
 /**
   * @brief  Clears DTOG_RX / DTOG_TX bit in the endpoint register.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
 
-static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   if((regVal & USB_EP_DTOG_RX) != 0)
   {
-    pcd_rx_dtog(USBx,bEpNum);
+    pcd_rx_dtog(USBx,bEpIdx);
   }
 }
 
-static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   if((regVal & USB_EP_DTOG_TX) != 0)
   {
-    pcd_tx_dtog(USBx,bEpNum);
+    pcd_tx_dtog(USBx,bEpIdx);
   }
 }
 
 /**
   * @brief  set & clear EP_KIND bit.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
 
-static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal |= USB_EP_KIND;
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
-static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPKIND_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
 // This checks if the device has "LPM"
@@ -421,6 +438,7 @@ static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpNum)
      USB_ISTR_RESET | USB_ISTR_SOF | USB_ISTR_ESOF | USB_ISTR_L1REQ_FORCED )
 
 // Number of endpoints in hardware
+// TODO should use TUP_DCD_ENDPOINT_MAX
 #define STFSDEV_EP_COUNT (8u)
 
 #endif /* PORTABLE_ST_STM32F0_DCD_STM32F0_FSDEV_PVT_ST_H_ */