diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 9ce37f9926..4e31c99fa2 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -215,19 +215,19 @@ void dcd_init(uint8_t rhport)
   /* The RM mentions to use a special ordering of PDWN and FRES, but this isn't done in HAL.
    * Here, the RM is followed. */
 
-  for (uint32_t i = 0; i < 200; i++) { // should be a few us
+  for (volatile uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
   // 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
+  for (volatile uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
 
   USB->CNTR &= ~USB_CNTR_PDWN;
 
   // Wait startup time, for F042 and F070, this is <= 1 us.
-  for (uint32_t i = 0; i < 200; i++) { // should be a few us
+  for (volatile uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
   USB->CNTR = 0; // Enable USB
diff --git a/src/portable/st/stm32_fsdev/fsdev_common.h b/src/portable/st/stm32_fsdev/fsdev_common.h
index af5d8afab0..e8a6af8cb7 100644
--- a/src/portable/st/stm32_fsdev/fsdev_common.h
+++ b/src/portable/st/stm32_fsdev/fsdev_common.h
@@ -295,18 +295,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, u
 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, bEpIdx);
   regVal &= USB_EPTX_DTOGMASK;
-
-  /* toggle first bit ? */
-  if((USB_EPTX_DTOG1 & (wState))!= 0U)
-  {
-    regVal ^= USB_EPTX_DTOG1;
-  }
-  /* toggle second bit ?  */
-  if((USB_EPTX_DTOG2 & ((uint32_t)(wState)))!= 0U)
-  {
-    regVal ^= USB_EPTX_DTOG2;
-  }
-
+  regVal ^= wState;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
@@ -322,16 +311,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx
 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, bEpIdx);
   regVal &= USB_EPRX_DTOGMASK;
-
-  /* toggle first bit ? */
-  if((USB_EPRX_DTOG1 & wState)!= 0U) {
-    regVal ^= USB_EPRX_DTOG1;
-  }
-  /* toggle second bit ? */
-  if((USB_EPRX_DTOG2 & wState)!= 0U) {
-    regVal ^= USB_EPRX_DTOG2;
-  }
-
+  regVal ^= wState;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }