Merge pull request #1828 from HiFiPhile/stm32_fsdev

stm32_fsdev & ISO EP buffer allocation improvements 

15 files changed, 1739 insertions, 332 deletions

diff --git a/examples/device/CMakeLists.txt b/examples/device/CMakeLists.txt
index edf5ab805..5520209e0 100644
--- a/examples/device/CMakeLists.txt
+++ b/examples/device/CMakeLists.txt
@@ -8,6 +8,7 @@ family_initialize_project(tinyusb_device_examples ${CMAKE_CURRENT_LIST_DIR})
 # family_add_subdirectory will filter what to actually add based on selected FAMILY
 family_add_subdirectory(audio_4_channel_mic)
 family_add_subdirectory(audio_test)
+family_add_subdirectory(audio_test_multi_rate)
 family_add_subdirectory(board_test)
 family_add_subdirectory(cdc_dual_ports)
 family_add_subdirectory(cdc_msc)
diff --git a/examples/device/audio_test_multi_rate/CMakeLists.txt b/examples/device/audio_test_multi_rate/CMakeLists.txt
new file mode 100644
index 000000000..cb321f9a8
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/CMakeLists.txt
@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.5)
+
+include(${CMAKE_CURRENT_SOURCE_DIR}/../../../hw/bsp/family_support.cmake)
+
+# gets PROJECT name for the example (e.g. <BOARD>-<DIR_NAME>)
+family_get_project_name(PROJECT ${CMAKE_CURRENT_LIST_DIR})
+
+project(${PROJECT})
+
+# Checks this example is valid for the family and initializes the project
+family_initialize_project(${PROJECT} ${CMAKE_CURRENT_LIST_DIR})
+
+add_executable(${PROJECT})
+
+# Example source
+target_sources(${PROJECT} PUBLIC
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/usb_descriptors.c
+)
+
+# Example include
+target_include_directories(${PROJECT} PUBLIC
+  ${CMAKE_CURRENT_SOURCE_DIR}/src
+)
+
+# Configure compilation flags and libraries for the example... see the corresponding function
+# in hw/bsp/FAMILY/family.cmake for details.
+family_configure_device_example(${PROJECT})
\ No newline at end of file
diff --git a/examples/device/audio_test_multi_rate/Makefile b/examples/device/audio_test_multi_rate/Makefile
new file mode 100644
index 000000000..5a455078e
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/Makefile
@@ -0,0 +1,12 @@
+include ../../../tools/top.mk
+include ../../make.mk
+
+INC += \
+  src \
+  $(TOP)/hw \
+
+# Example source
+EXAMPLE_SOURCE += $(wildcard src/*.c)
+SRC_C += $(addprefix $(CURRENT_PATH)/, $(EXAMPLE_SOURCE))
+
+include ../../rules.mk
diff --git a/examples/device/audio_test_multi_rate/skip.txt b/examples/device/audio_test_multi_rate/skip.txt
new file mode 100644
index 000000000..ae9b57f1f
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/skip.txt
@@ -0,0 +1,3 @@
+mcu:SAMD11
+mcu:SAME5X
+mcu:SAMG
\ No newline at end of file
diff --git a/examples/device/audio_test_multi_rate/src/main.c b/examples/device/audio_test_multi_rate/src/main.c
new file mode 100644
index 000000000..0e11a509b
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/src/main.c
@@ -0,0 +1,521 @@
+/* 
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2020 Reinhard Panhuber
+ * 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
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ */
+
+/* plot_audio_samples.py requires following modules:
+ * $ sudo apt install libportaudio
+ * $ pip3 install sounddevice matplotlib
+ *
+ * Then run
+ * $ python3 plot_audio_samples.py
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "bsp/board.h"
+#include "tusb.h"
+#include "usb_descriptors.h"
+
+//--------------------------------------------------------------------+
+// MACRO CONSTANT TYPEDEF PROTYPES
+//--------------------------------------------------------------------+
+
+/* Blink pattern
+ * - 250 ms  : device not mounted
+ * - 1000 ms : device mounted
+ * - 2500 ms : device is suspended
+ */
+enum  {
+  BLINK_NOT_MOUNTED = 250,
+  BLINK_MOUNTED = 1000,
+  BLINK_SUSPENDED = 2500,
+};
+
+static uint32_t blink_interval_ms = BLINK_NOT_MOUNTED;
+
+// Audio controls
+// Current states
+bool mute[CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX + 1]; 				          // +1 for master channel 0
+uint16_t volume[CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX + 1]; 					// +1 for master channel 0
+uint32_t sampFreq;
+uint8_t bytesPerSample;
+uint8_t clkValid;
+
+// Range states
+// List of supported sample rates
+static const uint32_t sampleRatesList[] =
+{
+    32000, 48000, 96000
+};
+
+#define N_sampleRates  TU_ARRAY_SIZE(sampleRatesList)
+
+// Bytes per format of every Alt settings
+static const uint8_t bytesPerSampleAltList[CFG_TUD_AUDIO_FUNC_1_N_FORMATS] =
+{
+    CFG_TUD_AUDIO_FUNC_1_FORMAT_1_N_BYTES_PER_SAMPLE_TX,
+    CFG_TUD_AUDIO_FUNC_1_FORMAT_2_N_BYTES_PER_SAMPLE_TX,
+};
+
+audio_control_range_2_n_t(1) volumeRng[CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX+1]; 			// Volume range state
+
+
+// Audio test data
+CFG_TUSB_MEM_ALIGN uint8_t test_buffer_audio[CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX];
+uint16_t startVal = 0;
+
+void led_blinking_task(void);
+void audio_task(void);
+
+/*------------- MAIN -------------*/
+int main(void)
+{
+  board_init();
+
+  // init device stack on configured roothub port
+  tud_init(BOARD_TUD_RHPORT);
+
+  // Init values
+  sampFreq = sampleRatesList[0];
+  clkValid = 1;
+
+  while (1)
+  {
+    tud_task(); // tinyusb device task
+    led_blinking_task();
+    audio_task();
+  }
+
+
+  return 0;
+}
+
+//--------------------------------------------------------------------+
+// Device callbacks
+//--------------------------------------------------------------------+
+
+// Invoked when device is mounted
+void tud_mount_cb(void)
+{
+  blink_interval_ms = BLINK_MOUNTED;
+}
+
+// Invoked when device is unmounted
+void tud_umount_cb(void)
+{
+  blink_interval_ms = BLINK_NOT_MOUNTED;
+}
+
+// Invoked when usb bus is suspended
+// remote_wakeup_en : if host allow us  to perform remote wakeup
+// Within 7ms, device must draw an average of current less than 2.5 mA from bus
+void tud_suspend_cb(bool remote_wakeup_en)
+{
+  (void) remote_wakeup_en;
+  blink_interval_ms = BLINK_SUSPENDED;
+}
+
+// Invoked when usb bus is resumed
+void tud_resume_cb(void)
+{
+  blink_interval_ms = BLINK_MOUNTED;
+}
+
+//--------------------------------------------------------------------+
+// AUDIO Task
+//--------------------------------------------------------------------+
+
+void audio_task(void)
+{
+  // Yet to be filled - e.g. put meas data into TX FIFOs etc.
+  // asm("nop");
+}
+
+//--------------------------------------------------------------------+
+// Application Callback API Implementations
+//--------------------------------------------------------------------+
+
+// Invoked when set interface is called, typically on start/stop streaming or format change
+bool tud_audio_set_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void)rhport;
+  //uint8_t const itf = tu_u16_low(tu_le16toh(p_request->wIndex));
+  uint8_t const alt = tu_u16_low(tu_le16toh(p_request->wValue));
+  
+  // Clear buffer when streaming format is changed
+  if(alt != 0)
+  {
+    bytesPerSample = bytesPerSampleAltList[alt-1];
+  }
+  return true;
+}
+
+// Invoked when audio class specific set request received for an EP
+bool tud_audio_set_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff)
+{
+  (void) rhport;
+  (void) pBuff;
+
+  // We do not support any set range requests here, only current value requests
+  TU_VERIFY(p_request->bRequest == AUDIO_CS_REQ_CUR);
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t ep = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) ep;
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific set request received for an interface
+bool tud_audio_set_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff)
+{
+  (void) rhport;
+  (void) pBuff;
+
+  // We do not support any set range requests here, only current value requests
+  TU_VERIFY(p_request->bRequest == AUDIO_CS_REQ_CUR);
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t itf = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) itf;
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific set request received for an entity
+bool tud_audio_set_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t itf = TU_U16_LOW(p_request->wIndex);
+  uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
+
+  (void) itf;
+
+  // We do not support any set range requests here, only current value requests
+  TU_VERIFY(p_request->bRequest == AUDIO_CS_REQ_CUR);
+
+  // If request is for our feature unit
+  if ( entityID == UAC2_ENTITY_FEATURE_UNIT )
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_FU_CTRL_MUTE:
+        // Request uses format layout 1
+        TU_VERIFY(p_request->wLength == sizeof(audio_control_cur_1_t));
+
+        mute[channelNum] = ((audio_control_cur_1_t*) pBuff)->bCur;
+
+        TU_LOG2("    Set Mute: %d of channel: %u\r\n", mute[channelNum], channelNum);
+      return true;
+
+      case AUDIO_FU_CTRL_VOLUME:
+        // Request uses format layout 2
+        TU_VERIFY(p_request->wLength == sizeof(audio_control_cur_2_t));
+
+        volume[channelNum] = (uint16_t) ((audio_control_cur_2_t*) pBuff)->bCur;
+
+        TU_LOG2("    Set Volume: %d dB of channel: %u\r\n", volume[channelNum], channelNum);
+      return true;
+
+        // Unknown/Unsupported control
+      default:
+        TU_BREAKPOINT();
+      return false;
+    }
+  }
+  
+  // Clock Source unit
+  if ( entityID == UAC2_ENTITY_CLOCK )
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_CS_CTRL_SAM_FREQ:
+        TU_VERIFY(p_request->wLength == sizeof(audio_control_cur_4_t));
+
+        sampFreq = (uint32_t)((audio_control_cur_4_t *)pBuff)->bCur;
+
+        TU_LOG2("Clock set current freq: %d\r\n", sampFreq);
+
+        return true;
+      break;
+
+      // Unknown/Unsupported control
+      default:
+        TU_BREAKPOINT();
+        return false;
+    }
+  }
+  
+  return false;    // Yet not implemented
+}
+
+// Invoked when audio class specific get request received for an EP
+bool tud_audio_get_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t ep = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) ep;
+
+  //	return tud_control_xfer(rhport, p_request, &tmp, 1);
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific get request received for an interface
+bool tud_audio_get_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t itf = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) itf;
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific get request received for an entity
+bool tud_audio_get_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  // uint8_t itf = TU_U16_LOW(p_request->wIndex); 			// Since we have only one audio function implemented, we do not need the itf value
+  uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
+
+  // Input terminal (Microphone input)
+  if (entityID == UAC2_ENTITY_INPUT_TERMINAL)
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_TE_CTRL_CONNECTOR:
+      {
+        // The terminal connector control only has a get request with only the CUR attribute.
+        audio_desc_channel_cluster_t ret;
+
+        // Those are dummy values for now
+        ret.bNrChannels = 1;
+        ret.bmChannelConfig = 0;
+        ret.iChannelNames = 0;
+
+        TU_LOG2("    Get terminal connector\r\n");
+
+        return tud_audio_buffer_and_schedule_control_xfer(rhport, p_request, (void*) &ret, sizeof(ret));
+      }
+      break;
+
+        // Unknown/Unsupported control selector
+      default:
+        TU_BREAKPOINT();
+        return false;
+    }
+  }
+
+  // Feature unit
+  if (entityID == UAC2_ENTITY_FEATURE_UNIT)
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_FU_CTRL_MUTE:
+        // Audio control mute cur parameter block consists of only one byte - we thus can send it right away
+        // There does not exist a range parameter block for mute
+        TU_LOG2("    Get Mute of channel: %u\r\n", channelNum);
+        return tud_control_xfer(rhport, p_request, &mute[channelNum], 1);
+
+      case AUDIO_FU_CTRL_VOLUME:
+        switch ( p_request->bRequest )
+        {
+          case AUDIO_CS_REQ_CUR:
+            TU_LOG2("    Get Volume of channel: %u\r\n", channelNum);
+            return tud_control_xfer(rhport, p_request, &volume[channelNum], sizeof(volume[channelNum]));
+
+          case AUDIO_CS_REQ_RANGE:
+            TU_LOG2("    Get Volume range of channel: %u\r\n", channelNum);
+
+            // Copy values - only for testing - better is version below
+            audio_control_range_2_n_t(1)
+            ret;
+
+            ret.wNumSubRanges = 1;
+            ret.subrange[0].bMin = -90;    // -90 dB
+            ret.subrange[0].bMax = 30;		// +30 dB
+            ret.subrange[0].bRes = 1; 		// 1 dB steps
+
+            return tud_audio_buffer_and_schedule_control_xfer(rhport, p_request, (void*) &ret, sizeof(ret));
+
+            // Unknown/Unsupported control
+          default:
+            TU_BREAKPOINT();
+            return false;
+        }
+      break;
+
+        // Unknown/Unsupported control
+      default:
+        TU_BREAKPOINT();
+        return false;
+    }
+  }
+
+  // Clock Source unit
+  if ( entityID == UAC2_ENTITY_CLOCK )
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_CS_CTRL_SAM_FREQ:
+        // channelNum is always zero in this case
+        switch ( p_request->bRequest )
+        {
+          case AUDIO_CS_REQ_CUR:
+            TU_LOG2("    Get Sample Freq.\r\n");
+            return tud_control_xfer(rhport, p_request, &sampFreq, sizeof(sampFreq));
+
+          case AUDIO_CS_REQ_RANGE:
+          {
+            TU_LOG2("    Get Sample Freq. range\r\n");
+            audio_control_range_4_n_t(N_sampleRates) rangef =
+            {
+                .wNumSubRanges = tu_htole16(N_sampleRates)
+            };
+            TU_LOG1("Clock get %d freq ranges\r\n", N_sampleRates);
+            for(uint8_t i = 0; i < N_sampleRates; i++)
+            {
+                rangef.subrange[i].bMin = (int32_t)sampleRatesList[i];
+                rangef.subrange[i].bMax = (int32_t)sampleRatesList[i];
+                rangef.subrange[i].bRes = 0;
+                TU_LOG1("Range %d (%d, %d, %d)\r\n", i, (int)rangef.subrange[i].bMin, (int)rangef.subrange[i].bMax, (int)rangef.subrange[i].bRes);
+            }
+            return tud_audio_buffer_and_schedule_control_xfer(rhport, p_request, &rangef, sizeof(rangef));
+          }
+           // Unknown/Unsupported control
+          default:
+            TU_BREAKPOINT();
+            return false;
+        }
+      break;
+
+      case AUDIO_CS_CTRL_CLK_VALID:
+        // Only cur attribute exists for this request
+        TU_LOG2("    Get Sample Freq. valid\r\n");
+        return tud_control_xfer(rhport, p_request, &clkValid, sizeof(clkValid));
+
+      // Unknown/Unsupported control
+      default:
+        TU_BREAKPOINT();
+        return false;
+    }
+  }
+
+  TU_LOG2("  Unsupported entity: %d\r\n", entityID);
+  return false; 	// Yet not implemented
+}
+
+bool tud_audio_tx_done_pre_load_cb(uint8_t rhport, uint8_t itf, uint8_t ep_in, uint8_t cur_alt_setting)
+{
+  (void) rhport;
+  (void) itf;
+  (void) ep_in;
+  (void) cur_alt_setting;
+
+  tud_audio_write((uint8_t *)test_buffer_audio, (uint16_t)(sampFreq / (TUD_OPT_HIGH_SPEED ? 8000 : 1000) * bytesPerSample));
+
+  return true;
+}
+
+bool tud_audio_tx_done_post_load_cb(uint8_t rhport, uint16_t n_bytes_copied, uint8_t itf, uint8_t ep_in, uint8_t cur_alt_setting)
+{
+  (void) rhport;
+  (void) n_bytes_copied;
+  (void) itf;
+  (void) ep_in;
+  (void) cur_alt_setting;
+
+  // 16bit
+  if(bytesPerSample == 2)
+  {
+    uint16_t* pData_16 = (uint16_t*)((void*)test_buffer_audio);
+    for (size_t cnt = 0; cnt < sampFreq / (TUD_OPT_HIGH_SPEED ? 8000 : 1000); cnt++)
+    {
+      pData_16[cnt] = startVal++;
+    }
+  }
+  // 24bit in 32bit slot
+  else if(bytesPerSample == 4)
+  {
+    uint32_t* pData_32 = (uint32_t*)((void*)test_buffer_audio);
+    for (size_t cnt = 0; cnt < sampFreq / (TUD_OPT_HIGH_SPEED ? 8000 : 1000); cnt++)
+    {
+      pData_32[cnt] = (uint32_t)startVal++ << 16U;
+    }
+  }
+  
+  return true;
+}
+
+bool tud_audio_set_itf_close_EP_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void) rhport;
+  (void) p_request;
+  startVal = 0;
+
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// BLINKING TASK
+//--------------------------------------------------------------------+
+void led_blinking_task(void)
+{
+  static uint32_t start_ms = 0;
+  static bool led_state = false;
+
+  // Blink every interval ms
+  if ( board_millis() - start_ms < blink_interval_ms) return; // not enough time
+  start_ms += blink_interval_ms;
+
+  board_led_write(led_state);
+  led_state = 1 - led_state; // toggle
+}
diff --git a/examples/device/audio_test_multi_rate/src/plot_audio_samples.py b/examples/device/audio_test_multi_rate/src/plot_audio_samples.py
new file mode 100644
index 000000000..4a5f15a9c
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/src/plot_audio_samples.py
@@ -0,0 +1,38 @@
+import sounddevice as sd
+import matplotlib.pyplot as plt
+import numpy as np
+import platform
+import csv
+
+if __name__ == '__main__':
+
+    # If you got "ValueError: No input device matching", that is because your PC name example device
+    # differently from tested list below. Uncomment the next line to see full list and try to pick correct one
+    # print(sd.query_devices())
+
+    fs = 96000           # Sample rate
+    duration = 100e-3    # Duration of recording
+
+    if platform.system() == 'Windows':
+        # MME is needed since there are more than one MicNode device APIs (at least in Windows)
+        device = 'Microphone (MicNode) MME'
+    elif platform.system() == 'Darwin':
+        device = 'MicNode'
+    else:
+        device ='default'
+
+    myrecording = sd.rec(int(duration * fs), samplerate=fs, channels=1, dtype='int16', device=device)
+    print('Waiting...')
+    sd.wait()  # Wait until recording is finished
+    print('Done!')
+
+    time = np.arange(0, duration, 1 / fs)  # time vector
+    plt.plot(time, myrecording)
+    plt.xlabel('Time [s]')
+    plt.ylabel('Amplitude')
+    plt.title('MicNode')
+    plt.show()
+    
+    samples = np.array(myrecording)
+    np.savetxt('Output.csv', samples, delimiter=",", fmt='%s')
+    
\ No newline at end of file
diff --git a/examples/device/audio_test_multi_rate/src/tusb_config.h b/examples/device/audio_test_multi_rate/src/tusb_config.h
new file mode 100644
index 000000000..d8b6511d6
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/src/tusb_config.h
@@ -0,0 +1,141 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ */
+
+#ifndef _TUSB_CONFIG_H_
+#define _TUSB_CONFIG_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "usb_descriptors.h"
+
+//--------------------------------------------------------------------+
+// Board Specific Configuration
+//--------------------------------------------------------------------+
+
+// RHPort number used for device can be defined by board.mk, default to port 0
+#ifndef BOARD_TUD_RHPORT
+#define BOARD_TUD_RHPORT      0
+#endif
+
+// RHPort max operational speed can defined by board.mk
+#ifndef BOARD_TUD_MAX_SPEED
+#define BOARD_TUD_MAX_SPEED   OPT_MODE_DEFAULT_SPEED
+#endif
+
+//--------------------------------------------------------------------
+// COMMON CONFIGURATION
+//--------------------------------------------------------------------
+
+// defined by compiler flags for flexibility
+#ifndef CFG_TUSB_MCU
+#error CFG_TUSB_MCU must be defined
+#endif
+
+#ifndef CFG_TUSB_OS
+#define CFG_TUSB_OS                 OPT_OS_NONE
+#endif
+
+#ifndef CFG_TUSB_DEBUG
+#define CFG_TUSB_DEBUG              0
+#endif
+
+// Enable Device stack
+#define CFG_TUD_ENABLED       1
+
+// Default is max speed that hardware controller could support with on-chip PHY
+#define CFG_TUD_MAX_SPEED     BOARD_TUD_MAX_SPEED
+
+// CFG_TUSB_DEBUG is defined by compiler in DEBUG build
+// #define CFG_TUSB_DEBUG           0
+
+/* USB DMA on some MCUs can only access a specific SRAM region with restriction on alignment.
+ * Tinyusb use follows macros to declare transferring memory so that they can be put
+ * into those specific section.
+ * e.g
+ * - CFG_TUSB_MEM SECTION : __attribute__ (( section(".usb_ram") ))
+ * - CFG_TUSB_MEM_ALIGN   : __attribute__ ((aligned(4)))
+ */
+#ifndef CFG_TUSB_MEM_SECTION
+#define CFG_TUSB_MEM_SECTION
+#endif
+
+#ifndef CFG_TUSB_MEM_ALIGN
+#define CFG_TUSB_MEM_ALIGN          __attribute__ ((aligned(4)))
+#endif
+
+//--------------------------------------------------------------------
+// DEVICE CONFIGURATION
+//--------------------------------------------------------------------
+
+#ifndef CFG_TUD_ENDPOINT0_SIZE
+#define CFG_TUD_ENDPOINT0_SIZE    64
+#endif
+
+//------------- CLASS -------------//
+#define CFG_TUD_AUDIO             1
+#define CFG_TUD_CDC               0
+#define CFG_TUD_MSC               0
+#define CFG_TUD_HID               0
+#define CFG_TUD_MIDI              0
+#define CFG_TUD_VENDOR            0
+
+//--------------------------------------------------------------------
+// AUDIO CLASS DRIVER CONFIGURATION
+//--------------------------------------------------------------------
+
+#define CFG_TUD_AUDIO_FUNC_1_MAX_SAMPLE_RATE                         96000
+
+// How many formats are used, need to adjust USB descriptor if changed
+#define CFG_TUD_AUDIO_FUNC_1_N_FORMATS                               2
+  
+// 16bit in 16bit slots
+#define CFG_TUD_AUDIO_FUNC_1_FORMAT_1_N_BYTES_PER_SAMPLE_TX          2
+#define CFG_TUD_AUDIO_FUNC_1_FORMAT_1_RESOLUTION_RX                  16
+
+// 24bit in 32bit slots
+#define CFG_TUD_AUDIO_FUNC_1_FORMAT_2_N_BYTES_PER_SAMPLE_TX          4
+#define CFG_TUD_AUDIO_FUNC_1_FORMAT_2_RESOLUTION_RX                  24
+
+// Have a look into audio_device.h for all configurations
+
+#define CFG_TUD_AUDIO_FUNC_1_DESC_LEN                                 TUD_AUDIO_MIC_ONE_CH_2_FORMAT_DESC_LEN
+#define CFG_TUD_AUDIO_FUNC_1_N_AS_INT                                 1                                       // Number of Standard AS Interface Descriptors (4.9.1) defined per audio function - this is required to be able to remember the current alternate settings of these interfaces - We restrict us here to have a constant number for all audio functions (which means this has to be the maximum number of AS interfaces an audio function has and a second audio function with less AS interfaces just wastes a few bytes)
+#define CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ                              64                                      // Size of control request buffer
+
+#define CFG_TUD_AUDIO_ENABLE_EP_IN                                    1
+#define CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX                            1                                       // Driver gets this info from the descriptors - we define it here to use it to setup the descriptors and to do calculations with it below - be aware: for different number of channels you need another descriptor!
+
+#define CFG_TUD_AUDIO_FUNC_1_FORMAT_1_EP_SZ_IN                        TUD_AUDIO_EP_SIZE(CFG_TUD_AUDIO_FUNC_1_MAX_SAMPLE_RATE, CFG_TUD_AUDIO_FUNC_1_FORMAT_1_N_BYTES_PER_SAMPLE_TX, CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX)
+#define CFG_TUD_AUDIO_FUNC_1_FORMAT_2_EP_SZ_IN                        TUD_AUDIO_EP_SIZE(CFG_TUD_AUDIO_FUNC_1_MAX_SAMPLE_RATE, CFG_TUD_AUDIO_FUNC_1_FORMAT_2_N_BYTES_PER_SAMPLE_TX, CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX)
+
+#define CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX                             TU_MAX(CFG_TUD_AUDIO_FUNC_1_FORMAT_1_EP_SZ_IN, CFG_TUD_AUDIO_FUNC_1_FORMAT_2_EP_SZ_IN)                  // Maximum EP IN size for all AS alternate settings used
+#define CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ                          CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _TUSB_CONFIG_H_ */
diff --git a/examples/device/audio_test_multi_rate/src/usb_descriptors.c b/examples/device/audio_test_multi_rate/src/usb_descriptors.c
new file mode 100644
index 000000000..92e791a72
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/src/usb_descriptors.c
@@ -0,0 +1,169 @@
+/* 
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ * 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
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ */
+
+#include "tusb.h"
+#include "usb_descriptors.h"
+
+/* A combination of interfaces must have a unique product id, since PC will save device driver after the first plug.
+ * Same VID/PID with different interface e.g MSC (first), then CDC (later) will possibly cause system error on PC.
+ *
+ * Auto ProductID layout's Bitmap:
+ *   [MSB]     AUDIO | MIDI | HID | MSC | CDC          [LSB]
+ */
+#define _PID_MAP(itf, n)  ( (CFG_TUD_##itf) << (n) )
+#define USB_PID           (0x4000 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1) | _PID_MAP(HID, 2) | \
+    _PID_MAP(MIDI, 3) | _PID_MAP(AUDIO, 4) | _PID_MAP(VENDOR, 5) )
+
+//--------------------------------------------------------------------+
+// Device Descriptors
+//--------------------------------------------------------------------+
+tusb_desc_device_t const desc_device =
+{
+    .bLength            = sizeof(tusb_desc_device_t),
+    .bDescriptorType    = TUSB_DESC_DEVICE,
+    .bcdUSB             = 0x0200,
+
+    // Use Interface Association Descriptor (IAD) for CDC
+    // As required by USB Specs IAD's subclass must be common class (2) and protocol must be IAD (1)
+    .bDeviceClass       = TUSB_CLASS_MISC,
+    .bDeviceSubClass    = MISC_SUBCLASS_COMMON,
+    .bDeviceProtocol    = MISC_PROTOCOL_IAD,
+    .bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
+
+    .idVendor           = 0xCafe,
+    .idProduct          = USB_PID,
+    .bcdDevice          = 0x0100,
+
+    .iManufacturer      = 0x01,
+    .iProduct           = 0x02,
+    .iSerialNumber      = 0x03,
+
+    .bNumConfigurations = 0x01
+};
+
+// Invoked when received GET DEVICE DESCRIPTOR
+// Application return pointer to descriptor
+uint8_t const * tud_descriptor_device_cb(void)
+{
+  return (uint8_t const *) &desc_device;
+}
+
+//--------------------------------------------------------------------+
+// Configuration Descriptor
+//--------------------------------------------------------------------+
+enum
+{
+  ITF_NUM_AUDIO_CONTROL = 0,
+  ITF_NUM_AUDIO_STREAMING,
+  ITF_NUM_TOTAL
+};
+
+#define CONFIG_TOTAL_LEN    	(TUD_CONFIG_DESC_LEN + CFG_TUD_AUDIO * TUD_AUDIO_MIC_ONE_CH_2_FORMAT_DESC_LEN)
+
+#if CFG_TUSB_MCU == OPT_MCU_LPC175X_6X || CFG_TUSB_MCU == OPT_MCU_LPC177X_8X || CFG_TUSB_MCU == OPT_MCU_LPC40XX
+  // LPC 17xx and 40xx endpoint type (bulk/interrupt/iso) are fixed by its number
+  // 0 control, 1 In, 2 Bulk, 3 Iso, 4 In etc ...
+  #define EPNUM_AUDIO   0x03
+
+#elif TU_CHECK_MCU(OPT_MCU_NRF5X)
+  // nRF5x ISO can only be endpoint 8
+  #define EPNUM_AUDIO   0x08
+
+#else
+  #define EPNUM_AUDIO   0x01
+#endif
+
+uint8_t const desc_configuration[] =
+{
+    // Interface count, string index, total length, attribute, power in mA
+    TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 100),
+
+    // Interface number, string index, EP Out & EP In address, EP size
+    TUD_AUDIO_MIC_ONE_CH_2_FORMAT_DESCRIPTOR(/*_itfnum*/ ITF_NUM_AUDIO_CONTROL, /*_stridx*/ 0, /*_epin*/ 0x80 | EPNUM_AUDIO)
+};
+
+TU_VERIFY_STATIC(sizeof(desc_configuration) == CONFIG_TOTAL_LEN, "Incorrect size");
+
+// Invoked when received GET CONFIGURATION DESCRIPTOR
+// Application return pointer to descriptor
+// Descriptor contents must exist long enough for transfer to complete
+uint8_t const * tud_descriptor_configuration_cb(uint8_t index)
+{
+  (void) index; // for multiple configurations
+  return desc_configuration;
+}
+
+//--------------------------------------------------------------------+
+// String Descriptors
+//--------------------------------------------------------------------+
+
+// array of pointer to string descriptors
+char const* string_desc_arr [] =
+{
+    (const char[]) { 0x09, 0x04 }, 	// 0: is supported language is English (0x0409)
+    "PaniRCorp",                   	// 1: Manufacturer
+    "MicNode",              		// 2: Product
+    "123456",                      	// 3: Serials, should use chip ID
+    "UAC2",                 	 	// 4: Audio Interface
+};
+
+static uint16_t _desc_str[32];
+
+// Invoked when received GET STRING DESCRIPTOR request
+// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete
+uint16_t const* tud_descriptor_string_cb(uint8_t index, uint16_t langid)
+{
+  (void) langid;
+
+  uint8_t chr_count;
+
+  if ( index == 0)
+  {
+    memcpy(&_desc_str[1], string_desc_arr[0], 2);
+    chr_count = 1;
+  }else
+  {
+    // Convert ASCII string into UTF-16
+
+    if ( !(index < sizeof(string_desc_arr)/sizeof(string_desc_arr[0])) ) return NULL;
+
+    const char* str = string_desc_arr[index];
+
+    // Cap at max char
+    chr_count = (uint8_t) strlen(str);
+    if ( chr_count > 31 ) chr_count = 31;
+
+    for(uint8_t i=0; i<chr_count; i++)
+    {
+      _desc_str[1+i] = str[i];
+    }
+  }
+
+  // first byte is length (including header), second byte is string type
+  _desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8 ) | (2*chr_count + 2));
+
+  return _desc_str;
+}
diff --git a/examples/device/audio_test_multi_rate/src/usb_descriptors.h b/examples/device/audio_test_multi_rate/src/usb_descriptors.h
new file mode 100644
index 000000000..77e2fe6c1
--- /dev/null
+++ b/examples/device/audio_test_multi_rate/src/usb_descriptors.h
@@ -0,0 +1,102 @@
+/* 
+ * The MIT License (MIT)
+ *
+ * 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
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ */
+
+#ifndef _USB_DESCRIPTORS_H_
+#define _USB_DESCRIPTORS_H_
+
+// #include "tusb.h"
+
+// Unit numbers are arbitrary selected
+#define UAC2_ENTITY_CLOCK                   0x04
+#define UAC2_ENTITY_INPUT_TERMINAL          0x01
+#define UAC2_ENTITY_OUTPUT_TERMINAL         0x03
+#define UAC2_ENTITY_FEATURE_UNIT            0x02
+
+
+#define TUD_AUDIO_MIC_ONE_CH_2_FORMAT_DESC_LEN (TUD_AUDIO_DESC_IAD_LEN\
+    + TUD_AUDIO_DESC_STD_AC_LEN\
+    + TUD_AUDIO_DESC_CS_AC_LEN\
+    + TUD_AUDIO_DESC_CLK_SRC_LEN\
+    + TUD_AUDIO_DESC_INPUT_TERM_LEN\
+    + TUD_AUDIO_DESC_OUTPUT_TERM_LEN\
+    + TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN\
+    /* Interface 1, Alternate 0 */\
+    + TUD_AUDIO_DESC_STD_AS_INT_LEN\
+    /* Interface 1, Alternate 1 */\
+    + TUD_AUDIO_DESC_STD_AS_INT_LEN\
+    + TUD_AUDIO_DESC_CS_AS_INT_LEN\
+    + TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN\
+    + TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN\
+    + TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN\
+    /* Interface 1, Alternate 2 */\
+    + TUD_AUDIO_DESC_STD_AS_INT_LEN\
+    + TUD_AUDIO_DESC_CS_AS_INT_LEN\
+    + TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN\
+    + TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN\
+    + TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN)
+
+
+#define TUD_AUDIO_MIC_ONE_CH_2_FORMAT_DESCRIPTOR(_itfnum, _stridx, _epin) \
+  /* Standard Interface Association Descriptor (IAD) */\
+  TUD_AUDIO_DESC_IAD(/*_firstitfs*/ _itfnum, /*_nitfs*/ 0x02, /*_stridx*/ 0x00),\
+  /* Standard AC Interface Descriptor(4.7.1) */\
+  TUD_AUDIO_DESC_STD_AC(/*_itfnum*/ _itfnum, /*_nEPs*/ 0x00, /*_stridx*/ _stridx),\
+  /* Class-Specific AC Interface Header Descriptor(4.7.2) */\
+  TUD_AUDIO_DESC_CS_AC(/*_bcdADC*/ 0x0200, /*_category*/ AUDIO_FUNC_MICROPHONE, /*_totallen*/ TUD_AUDIO_DESC_CLK_SRC_LEN+TUD_AUDIO_DESC_INPUT_TERM_LEN+TUD_AUDIO_DESC_OUTPUT_TERM_LEN+TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN, /*_ctrl*/ AUDIO_CS_AS_INTERFACE_CTRL_LATENCY_POS),\
+  /* Clock Source Descriptor(4.7.2.1) */\
+  TUD_AUDIO_DESC_CLK_SRC(/*_clkid*/ UAC2_ENTITY_CLOCK, /*_attr*/ AUDIO_CLOCK_SOURCE_ATT_INT_PRO_CLK, /*_ctrl*/ AUDIO_CTRL_RW << AUDIO_CLOCK_SOURCE_CTRL_CLK_FRQ_POS | AUDIO_CTRL_R << AUDIO_CLOCK_SOURCE_CTRL_CLK_VAL_POS, /*_assocTerm*/ 0x01,  /*_stridx*/ 0x00),\
+  /* Input Terminal Descriptor(4.7.2.4) */\
+  TUD_AUDIO_DESC_INPUT_TERM(/*_termid*/ UAC2_ENTITY_INPUT_TERMINAL, /*_termtype*/ AUDIO_TERM_TYPE_IN_GENERIC_MIC, /*_assocTerm*/ UAC2_ENTITY_OUTPUT_TERMINAL, /*_clkid*/ UAC2_ENTITY_CLOCK, /*_nchannelslogical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_idxchannelnames*/ 0x00, /*_ctrl*/ AUDIO_CTRL_R << AUDIO_IN_TERM_CTRL_CONNECTOR_POS, /*_stridx*/ 0x00),\
+  /* Output Terminal Descriptor(4.7.2.5) */\
+  TUD_AUDIO_DESC_OUTPUT_TERM(/*_termid*/ UAC2_ENTITY_OUTPUT_TERMINAL, /*_termtype*/ AUDIO_TERM_TYPE_USB_STREAMING, /*_assocTerm*/ UAC2_ENTITY_INPUT_TERMINAL, /*_srcid*/ UAC2_ENTITY_FEATURE_UNIT, /*_clkid*/ UAC2_ENTITY_CLOCK, /*_ctrl*/ 0x0000, /*_stridx*/ 0x00),\
+  /* Feature Unit Descriptor(4.7.2.8) */\
+  TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL(/*_unitid*/ UAC2_ENTITY_FEATURE_UNIT, /*_srcid*/ 0x01, /*_ctrlch0master*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_ctrlch1*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_stridx*/ 0x00),\
+  /* Standard AS Interface Descriptor(4.9.1) */\
+  /* Interface 1, Alternate 0 - default alternate setting with 0 bandwidth */\
+  TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum)+1), /*_altset*/ 0x00, /*_nEPs*/ 0x00, /*_stridx*/ 0x00),\
+  /* Standard AS Interface Descriptor(4.9.1) */\
+  /* Interface 1, Alternate 1 - alternate interface for data streaming */\
+  TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum)+1), /*_altset*/ 0x01, /*_nEPs*/ 0x01, /*_stridx*/ 0x00),\
+  /* Class-Specific AS Interface Descriptor(4.9.2) */\
+  TUD_AUDIO_DESC_CS_AS_INT(/*_termid*/ UAC2_ENTITY_OUTPUT_TERMINAL, /*_ctrl*/ AUDIO_CTRL_NONE, /*_formattype*/ AUDIO_FORMAT_TYPE_I, /*_formats*/ AUDIO_DATA_FORMAT_TYPE_I_PCM, /*_nchannelsphysical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_stridx*/ 0x00),\
+  /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\
+  TUD_AUDIO_DESC_TYPE_I_FORMAT(CFG_TUD_AUDIO_FUNC_1_FORMAT_1_N_BYTES_PER_SAMPLE_TX, CFG_TUD_AUDIO_FUNC_1_FORMAT_1_RESOLUTION_RX),\
+  /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\
+  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ CFG_TUD_AUDIO_FUNC_1_FORMAT_1_EP_SZ_IN, /*_interval*/ 0x01),\
+  /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\
+  TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000),\
+  /* Interface 1, Alternate 2 - alternate interface for data streaming */\
+  TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum)+1), /*_altset*/ 0x02, /*_nEPs*/ 0x01, /*_stridx*/ 0x00),\
+  /* Class-Specific AS Interface Descriptor(4.9.2) */\
+  TUD_AUDIO_DESC_CS_AS_INT(/*_termid*/ UAC2_ENTITY_OUTPUT_TERMINAL, /*_ctrl*/ AUDIO_CTRL_NONE, /*_formattype*/ AUDIO_FORMAT_TYPE_I, /*_formats*/ AUDIO_DATA_FORMAT_TYPE_I_PCM, /*_nchannelsphysical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_stridx*/ 0x00),\
+  /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\
+  TUD_AUDIO_DESC_TYPE_I_FORMAT(CFG_TUD_AUDIO_FUNC_1_FORMAT_2_N_BYTES_PER_SAMPLE_TX, CFG_TUD_AUDIO_FUNC_1_FORMAT_2_RESOLUTION_RX),\
+  /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\
+  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ CFG_TUD_AUDIO_FUNC_1_FORMAT_2_EP_SZ_IN, /*_interval*/ 0x01),\
+  /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\
+  TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000)
+
+
+#endif
\ No newline at end of file
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_ */