diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk
index 8b42565cdd..14d2a79015 100644
--- a/make/mcu/STM32F4.mk
+++ b/make/mcu/STM32F4.mk
@@ -154,6 +154,7 @@ MCU_COMMON_SRC = \
             drivers/stm32/adc_stm32f4xx.c \
             drivers/stm32/bus_i2c_stm32f4xx.c \
             drivers/stm32/bus_spi_stdperiph.c \
+            drivers/stm32/debug.c \
             drivers/stm32/dma_stm32f4xx.c \
             drivers/stm32/dshot_bitbang.c \
             drivers/stm32/dshot_bitbang_stdperiph.c \
diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk
index ee9268c38e..f45e194f4e 100644
--- a/make/mcu/STM32F7.mk
+++ b/make/mcu/STM32F7.mk
@@ -164,6 +164,7 @@ MCU_COMMON_SRC = \
             drivers/stm32/bus_i2c_hal_init.c \
             drivers/stm32/bus_i2c_hal.c \
             drivers/stm32/bus_spi_ll.c \
+            drivers/stm32/debug.c \
             drivers/stm32/dma_stm32f7xx.c \
             drivers/stm32/dshot_bitbang_ll.c \
             drivers/stm32/dshot_bitbang.c \
diff --git a/make/mcu/STM32G4.mk b/make/mcu/STM32G4.mk
index 5dd219ff7b..19267ae951 100644
--- a/make/mcu/STM32G4.mk
+++ b/make/mcu/STM32G4.mk
@@ -150,6 +150,7 @@ MCU_COMMON_SRC = \
             drivers/stm32/bus_i2c_hal_init.c \
             drivers/stm32/bus_i2c_hal.c \
             drivers/stm32/bus_spi_ll.c \
+            drivers/stm32/debug.c \
             drivers/stm32/dma_stm32g4xx.c \
             drivers/stm32/dshot_bitbang_ll.c \
             drivers/stm32/dshot_bitbang.c \
diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk
index e61c054da5..9327139089 100644
--- a/make/mcu/STM32H7.mk
+++ b/make/mcu/STM32H7.mk
@@ -306,6 +306,7 @@ MCU_COMMON_SRC = \
             drivers/stm32/bus_spi_ll.c \
             drivers/stm32/bus_quadspi_hal.c \
             drivers/stm32/bus_octospi_stm32h7xx.c \
+            drivers/stm32/debug.c \
             drivers/stm32/dma_stm32h7xx.c \
             drivers/stm32/dshot_bitbang_ll.c \
             drivers/stm32/dshot_bitbang.c \
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 59003bd85f..bf196aeca6 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -111,3 +111,5 @@ typedef enum {
 } debugType_e;
 
 extern const char * const debugModeNames[DEBUG_COUNT];
+
+void debugInit(void);
diff --git a/src/main/drivers/at32/debug.c b/src/main/drivers/at32/debug.c
new file mode 100644
index 0000000000..df1cc96180
--- /dev/null
+++ b/src/main/drivers/at32/debug.c
@@ -0,0 +1,38 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+#include "build/debug.h"
+#include "drivers/io.h"
+
+#ifdef DEBUG // DEBUG=GDB on command line
+void debugInit(void)
+{
+    IO_t io = IOGetByTag(DEFIO_TAG_E(PA13)); // SWDIO
+    if (IOGetOwner(io) == OWNER_FREE) {
+        IOInit(io, OWNER_SWD, 0);
+    }
+    io = IOGetByTag(DEFIO_TAG_E(PA14));      // SWCLK
+    if (IOGetOwner(io) == OWNER_FREE) {
+        IOInit(io, OWNER_SWD, 0);
+    }
+}
+#endif
diff --git a/src/main/drivers/stm32/debug.c b/src/main/drivers/stm32/debug.c
new file mode 100644
index 0000000000..df1cc96180
--- /dev/null
+++ b/src/main/drivers/stm32/debug.c
@@ -0,0 +1,38 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+#include "build/debug.h"
+#include "drivers/io.h"
+
+#ifdef DEBUG // DEBUG=GDB on command line
+void debugInit(void)
+{
+    IO_t io = IOGetByTag(DEFIO_TAG_E(PA13)); // SWDIO
+    if (IOGetOwner(io) == OWNER_FREE) {
+        IOInit(io, OWNER_SWD, 0);
+    }
+    io = IOGetByTag(DEFIO_TAG_E(PA14));      // SWCLK
+    if (IOGetOwner(io) == OWNER_FREE) {
+        IOInit(io, OWNER_SWD, 0);
+    }
+}
+#endif
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 04d1a561eb..c757ee34f0 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -256,20 +256,6 @@ static void sdCardAndFSInit(void)
 }
 #endif
 
-#ifdef USE_SWDIO
-static void swdPinsInit(void)
-{
-    IO_t io = IOGetByTag(DEFIO_TAG_E(PA13)); // SWDIO
-    if (IOGetOwner(io) == OWNER_FREE) {
-        IOInit(io, OWNER_SWD, 0);
-    }
-    io = IOGetByTag(DEFIO_TAG_E(PA14)); // SWCLK
-    if (IOGetOwner(io) == OWNER_FREE) {
-        IOInit(io, OWNER_SWD, 0);
-    }
-}
-#endif
-
 void init(void)
 {
 #ifdef SERIAL_PORT_COUNT
@@ -1030,8 +1016,8 @@ void init(void)
     spiInitBusDMA();
 #endif
 
-#ifdef USE_SWDIO
-    swdPinsInit();
+#ifdef DEBUG
+    debugInit();
 #endif
 
     unusedPinsInit();