diff --git a/hw/bsp/stm32f7/family.mk b/hw/bsp/stm32f7/family.mk
index e261b0467..bae7d6d5b 100644
--- a/hw/bsp/stm32f7/family.mk
+++ b/hw/bsp/stm32f7/family.mk
@@ -24,6 +24,7 @@ ifeq ($(PORT), 1)
     $(info "Using OTG_HS in FullSpeed mode")
   endif
 else
+  CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
   $(info "Using OTG_FS")
 endif
 
diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index 681a1b852..ebc408f5a 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -83,6 +83,10 @@ static tud_cdc_configure_fifo_t _cdcd_fifo_cfg;
 
 static bool _prep_out_transaction (cdcd_interface_t* p_cdc) {
   uint8_t const rhport = 0;
+
+  // Skip if usb is not ready yet
+  TU_VERIFY(tud_ready() && p_cdc->ep_out);
+
   uint16_t available = tu_fifo_remaining(&p_cdc->rx_ff);
 
   // Prepare for incoming data but only allow what we can store in the ring buffer.
@@ -116,6 +120,10 @@ bool tud_cdc_configure_fifo(tud_cdc_configure_fifo_t const* cfg) {
   return true;
 }
 
+bool tud_cdc_n_ready(uint8_t itf) {
+  return tud_ready() && _cdcd_itf[itf].ep_in != 0 && _cdcd_itf[itf].ep_out != 0;
+}
+
 bool tud_cdc_n_connected(uint8_t itf) {
   // DTR (bit 0) active  is considered as connected
   return tud_ready() && tu_bit_test(_cdcd_itf[itf].line_state, 0);
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index 34670517a..3ad7c8baf 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -61,6 +61,9 @@ bool tud_cdc_configure_fifo(tud_cdc_configure_fifo_t const* cfg);
 // Application API (Multiple Ports) i.e. CFG_TUD_CDC > 1
 //--------------------------------------------------------------------+
 
+// Check if interface is ready
+bool tud_cdc_n_ready(uint8_t itf);
+
 // Check if terminal is connected to this port
 bool tud_cdc_n_connected(uint8_t itf);
 
@@ -116,6 +119,11 @@ bool tud_cdc_n_write_clear(uint8_t itf);
 //--------------------------------------------------------------------+
 // Application API (Single Port)
 //--------------------------------------------------------------------+
+
+TU_ATTR_ALWAYS_INLINE static inline bool tud_cdc_ready(void) {
+  return tud_cdc_n_ready(0);
+}
+
 TU_ATTR_ALWAYS_INLINE static inline bool tud_cdc_connected(void) {
   return tud_cdc_n_connected(0);
 }
diff --git a/src/common/tusb_verify.h b/src/common/tusb_verify.h
index dde0550d3..4344575b7 100644
--- a/src/common/tusb_verify.h
+++ b/src/common/tusb_verify.h
@@ -56,8 +56,8 @@
  *   #define TU_VERIFY(cond)                  if(cond) return false;
  *   #define TU_VERIFY(cond,ret)              if(cond) return ret;
  *
- *   #define TU_ASSERT(cond)                  if(cond) {_MESS_FAILED(); TU_BREAKPOINT(), return false;}
- *   #define TU_ASSERT(cond,ret)              if(cond) {_MESS_FAILED(); TU_BREAKPOINT(), return ret;}
+ *   #define TU_ASSERT(cond)                  if(cond) {TU_MESS_FAILED(); TU_BREAKPOINT(), return false;}
+ *   #define TU_ASSERT(cond,ret)              if(cond) {TU_MESS_FAILED(); TU_BREAKPOINT(), return ret;}
  *------------------------------------------------------------------*/
 
 #ifdef __cplusplus
@@ -70,9 +70,9 @@
 
 #if CFG_TUSB_DEBUG
   #include <stdio.h>
-  #define _MESS_FAILED()    tu_printf("%s %d: ASSERT FAILED\r\n", __func__, __LINE__)
+  #define TU_MESS_FAILED()    tu_printf("%s %d: ASSERT FAILED\r\n", __func__, __LINE__)
 #else
-  #define _MESS_FAILED() do {} while (0)
+  #define TU_MESS_FAILED() do {} while (0)
 #endif
 
 // Halt CPU (breakpoint) when hitting error, only apply for Cortex M3, M4, M7, M33. M55
@@ -119,7 +119,7 @@
  *------------------------------------------------------------------*/
 #define TU_ASSERT_DEFINE(_cond, _ret)                                 \
   do {                                                                \
-    if ( !(_cond) ) { _MESS_FAILED(); TU_BREAKPOINT(); return _ret; } \
+    if ( !(_cond) ) { TU_MESS_FAILED(); TU_BREAKPOINT(); return _ret; } \
   } while(0)
 
 #define TU_ASSERT_1ARGS(_cond)         TU_ASSERT_DEFINE(_cond, false)
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 4105a71a4..4e723c8b9 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -747,17 +747,23 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
               _usbd_dev.speed = speed; // restore speed
             }
 
+            _usbd_dev.cfg_num = cfg_num;
+
             // Handle the new configuration and execute the corresponding callback
             if ( cfg_num ) {
               // switch to new configuration if not zero
-              TU_ASSERT( process_set_config(rhport, cfg_num) );
+              if (!process_set_config(rhport, cfg_num)) {
+                TU_MESS_FAILED();
+                TU_BREAKPOINT();
+                _usbd_dev.cfg_num = 0;
+                return false;
+              }
               if ( tud_mount_cb ) tud_mount_cb();
             } else {
               if ( tud_umount_cb ) tud_umount_cb();
             }
           }
 
-          _usbd_dev.cfg_num = cfg_num;
           tud_control_status(rhport, p_request);
         }
         break;