diff --git a/src/portable/espressif/esp32s2/dcd_esp32s2.c b/src/portable/espressif/esp32s2/dcd_esp32s2.c
index d49b69a10..21bad327a 100644
--- a/src/portable/espressif/esp32s2/dcd_esp32s2.c
+++ b/src/portable/espressif/esp32s2/dcd_esp32s2.c
@@ -71,6 +71,20 @@ static uint32_t _setup_packet[2];
 #define XFER_CTL_BASE(_ep, _dir) &xfer_status[_ep][_dir]
 static xfer_ctl_t xfer_status[EP_MAX][2];
 
+// Max number of IN EP FIFOs
+#define USB_FIFO_NUM 5
+
+// Keep count of how many FIFOs are in use
+static uint8_t dcd_allocated_fifos = 1; //FIFO0 is always in use
+
+// Will either return an unused FIFO number, or 0 if all are used.
+static uint8_t dcd_get_free_fifo(){
+  if (dcd_allocated_fifos < USB_FIFO_NUM) {
+    return dcd_allocated_fifos++;
+  }
+  return 0;
+}
+
 // Setup the control endpoint 0.
 static void bus_reset(void)
 {
@@ -271,8 +285,12 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt)
     // - Offset: GRXFSIZ + 16 + Size*(epnum-1)
     // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
 
+    uint8_t fifo_num = dcd_get_free_fifo();
+    TU_ASSERT(fifo_num != 0);
+
+    in_ep[epnum].diepctl &= ~(USB_D_TXFNUM1_M | USB_D_EPTYPE1_M | USB_DI_SETD0PID1 | USB_D_MPS1_M);
     in_ep[epnum].diepctl |= USB_D_USBACTEP1_M |
-                            epnum << USB_D_TXFNUM1_S |
+                            fifo_num << USB_D_TXFNUM1_S |
                             desc_edpt->bmAttributes.xfer << USB_D_EPTYPE1_S |
                             (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? (1 << USB_DI_SETD0PID1_S) : 0) |
                             desc_edpt->wMaxPacketSize.size << 0;
@@ -282,8 +300,8 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt)
     // Both TXFD and TXSA are in unit of 32-bit words.
     // IN FIFO 0 was configured during enumeration, hence the "+ 16".
     uint16_t const allocated_size = (USB0.grxfsiz & 0x0000ffff) + 16;
-    uint16_t const fifo_size = (EP_FIFO_SIZE/4 - allocated_size) / (EP_MAX-1);
-    uint32_t const fifo_offset = allocated_size + fifo_size*(epnum-1);
+    uint16_t const fifo_size = (EP_FIFO_SIZE/4 - allocated_size) / (USB_FIFO_NUM-1);
+    uint32_t const fifo_offset = allocated_size + fifo_size*(fifo_num-1);
 
     // DIEPTXF starts at FIFO #1.
     USB0.dieptxf[epnum - 1] = (fifo_size << USB_NPTXFDEP_S) | fifo_offset;
@@ -361,7 +379,8 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
     }
 
     // Flush the FIFO, and wait until we have confirmed it cleared.
-    USB0.grstctl |= ((epnum - 1) << USB_TXFNUM_S);
+    uint8_t const fifo_num = ((in_ep[epnum].diepctl >> USB_D_TXFNUM1_S) & USB_D_TXFNUM1_V);
+    USB0.grstctl |= (fifo_num << USB_TXFNUM_S);
     USB0.grstctl |= USB_TXFFLSH_M;
     while ((USB0.grstctl & USB_TXFFLSH_M) != 0) ;
   } else {
@@ -653,6 +672,8 @@ static void _dcd_int_handler(void* arg)
     // start of reset
     ESP_EARLY_LOGV(TAG, "dcd_int_handler - reset");
     USB0.gintsts = USB_USBRST_M;
+    // FIFOs will be reassigned when the endpoints are reopen
+    dcd_allocated_fifos = 1;
     bus_reset();
   }
 
@@ -681,6 +702,8 @@ static void _dcd_int_handler(void* arg)
     if (otg_int & USB_SESENDDET_M)
     {
       dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
+      // FIFOs will be reassigned when the endpoints are reopen
+      dcd_allocated_fifos = 1;
     }
 
     USB0.gotgint = otg_int;