diff --git a/firmware/main.c b/firmware/main.c
index 02abf86..70b41d2 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -1811,10 +1811,19 @@ void main(void)
 #endif
 		}
 
+		// get current system config
+		uint8_t cfg = REG_SYS(CONFIG);
+		__bit usb_enabled = !!(cfg & REG_SYS_CONFIG_USB_DEBUG_EN);
 		// if we were asked to jump to USB IAP, do it
 		if (jump_to_usb_bootloader)
 			__asm__ ("ljmp _usb_bootloader_jump");
 
+		if (usb_initialized && !usb_enabled) {
+			usb_initialized = 0;
+			ticks = 0;
+			usb_disable();
+		}
+
 		// if the 20ms timer did not expire yet, check if we can
 		// powerdown, otherwise busyloop
 		if (!run_timed_tasks) {
@@ -1830,7 +1839,7 @@ void main(void)
 			if (i2c_idle && !scan_active
 			    && !p6_changed
 #if CONFIG_USB_STACK
-			    && usb_initialized  && usb_suspended
+			    && (usb_initialized || !usb_enabled) && usb_suspended
 #endif
 #if CONFIG_STOCK_FW
 			    && user_app_checked
@@ -1888,7 +1897,7 @@ void main(void)
 
 #if CONFIG_USB_STACK
 		// after 500ms, init usb
-		if (!usb_initialized && ticks > 500 / 20) {
+		if (!usb_initialized && usb_enabled && ticks > 500 / 20) {
 			usb_init();
 			usb_initialized = 1;
 		}
@@ -1918,14 +1927,17 @@ void main(void)
 				ext_int_assert();
 				delay_us(100);
 				ext_int_deassert();
-#if CONFIG_USB_STACK
-				usb_key_change = 1;
 
-				// USB wakeup
-				PAGESW = 1;
-				if (P1_UDCCTRL & BIT(2)) {
-					P1_UDCCTRL |= BIT(5);
-					P1_UDCCTRL &= ~BIT(5);
+#if CONFIG_USB_STACK
+				if (usb_enabled) {
+					usb_key_change = 1;
+
+					// USB wakeup
+					PAGESW = 1;
+					if (P1_UDCCTRL & BIT(2)) {
+						P1_UDCCTRL |= BIT(5);
+						P1_UDCCTRL &= ~BIT(5);
+					}
 				}
 #endif