diff --git a/lib/usb/usb_f103.c b/lib/usb/usb_f103.c
index 26c27941..09d1bfca 100644
--- a/lib/usb/usb_f103.c
+++ b/lib/usb/usb_f103.c
@@ -343,4 +343,10 @@ static void stm32f103_poll(usbd_device *dev)
 			dev->user_callback_sof();
 		}
 	}
+
+	if (usbd_dev->user_callback_sof) {
+		BBIO_PERIPH(USB_CNTR_REG, USB_CNTR_SOFM) = 1;
+	} else {
+		BBIO_PERIPH(USB_CNTR_REG, USB_CNTR_SOFM) = 0;
+	}
 }
diff --git a/lib/usb/usb_f107.c b/lib/usb/usb_f107.c
index 32aecab6..d37de704 100644
--- a/lib/usb/usb_f107.c
+++ b/lib/usb/usb_f107.c
@@ -82,8 +82,7 @@ static usbd_device *stm32f107_usbd_init(void)
 			 OTG_FS_GINTMSK_RXFLVLM |
 			 OTG_FS_GINTMSK_IEPINT |
 			 OTG_FS_GINTMSK_USBSUSPM |
-			 OTG_FS_GINTMSK_WUIM |
-			 OTG_FS_GINTMSK_SOFM;
+			 OTG_FS_GINTMSK_WUIM;
 	OTG_FS_DAINTMSK = 0xF;
 	OTG_FS_DIEPMSK = OTG_FS_DIEPMSK_XFRCM;
 
diff --git a/lib/usb/usb_f207.c b/lib/usb/usb_f207.c
index b27d3cd2..498a83f6 100644
--- a/lib/usb/usb_f207.c
+++ b/lib/usb/usb_f207.c
@@ -82,8 +82,7 @@ static usbd_device *stm32f207_usbd_init(void)
 			 OTG_HS_GINTMSK_RXFLVLM |
 			 OTG_HS_GINTMSK_IEPINT |
 			 OTG_HS_GINTMSK_USBSUSPM |
-			 OTG_HS_GINTMSK_WUIM |
-			 OTG_HS_GINTMSK_SOFM;
+			 OTG_HS_GINTMSK_WUIM;
 	OTG_HS_DAINTMSK = 0xF;
 	OTG_HS_DIEPMSK = OTG_HS_DIEPMSK_XFRCM;
 
diff --git a/lib/usb/usb_fx07_common.c b/lib/usb/usb_fx07_common.c
index c54c7a32..234c9c1e 100644
--- a/lib/usb/usb_fx07_common.c
+++ b/lib/usb/usb_fx07_common.c
@@ -326,6 +326,12 @@ void stm32fx07_poll(usbd_device *usbd_dev)
 		}
 		REBASE(OTG_GINTSTS) = OTG_FS_GINTSTS_SOF;
 	}
+	
+	if (usbd_dev->user_callback_sof) {
+		BBIO_PERIPH(OTG_FS_GINTMSK, OTG_FS_GINTMSK_SOFM) = 1;
+	} else {
+		BBIO_PERIPH(OTG_FS_GINTMSK, OTG_FS_GINTMSK_SOFM) = 0;
+	}
 }
 
 void stm32fx07_disconnect(usbd_device *usbd_dev, bool disconnected)