diff --git a/usb/device/USBDevice/EndpointResolver.cpp b/usb/device/USBDevice/EndpointResolver.cpp
index e06b892960..05ced02866 100644
--- a/usb/device/USBDevice/EndpointResolver.cpp
+++ b/usb/device/USBDevice/EndpointResolver.cpp
@@ -44,9 +44,9 @@ void EndpointResolver::endpoint_ctrl(uint32_t size)
     endpoint_out(USB_EP_TYPE_CTRL, size);
 }
 
-usb_ep_t EndpointResolver::endpoint_in(usb_ep_type_t type, uint32_t size)
+usb_ep_t EndpointResolver::next_free_endpoint(bool in_not_out, usb_ep_type_t type, uint32_t size)
 {
-    int index = next_index(type, true);
+    int index = next_index(type, in_not_out);
     if (index < 0) {
         _valid = false;
         return 0;
@@ -57,21 +57,16 @@ usb_ep_t EndpointResolver::endpoint_in(usb_ep_type_t type, uint32_t size)
     _used |= 1 << index;
 
     return index_to_endpoint(index);
+
+}
+usb_ep_t EndpointResolver::endpoint_in(usb_ep_type_t type, uint32_t size)
+{
+    return next_free_endpoint(true, type, size);
 }
 
 usb_ep_t EndpointResolver::endpoint_out(usb_ep_type_t type, uint32_t size)
 {
-    int index = next_index(type, false);
-    if (index < 0) {
-        _valid = false;
-        return 0;
-    }
-
-    const usb_ep_entry_t &entry = _table->table[index_to_logical(index)];
-    _cost += entry.base_cost + entry.byte_cost * size;
-    _used |= 1 << index;
-
-    return index_to_endpoint(index);
+    return next_free_endpoint(false, type, size);
 }
 
 bool EndpointResolver::valid()
diff --git a/usb/device/USBDevice/EndpointResolver.h b/usb/device/USBDevice/EndpointResolver.h
index 04f7d188f8..a0a9a93e8f 100644
--- a/usb/device/USBDevice/EndpointResolver.h
+++ b/usb/device/USBDevice/EndpointResolver.h
@@ -62,6 +62,11 @@ public:
      */
     usb_ep_t endpoint_out(usb_ep_type_t type, uint32_t size);
 
+    /**
+     * Get next free endpoint
+     */
+    usb_ep_t next_free_endpoint(bool in_not_out, usb_ep_type_t type, uint32_t size);
+
     /**
      * Check if the endpoint configuration created so far is valid
      *