diff --git a/src/platforms/native/platform.h b/src/platforms/native/platform.h
index d22e641..94e7fa3 100644
--- a/src/platforms/native/platform.h
+++ b/src/platforms/native/platform.h
@@ -95,6 +95,20 @@ extern usbd_device *usbdev;
 #define LED_IDLE_RUN	GPIO10
 #define LED_ERROR	GPIO11
 
+#define TMS_SET_MODE()                                          \
+    gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ,            \
+                  GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN);
+#define SWDIO_MODE_FLOAT()                              \
+    gpio_set_mode(SWDIO_PORT, GPIO_MODE_INPUT,          \
+                  GPIO_CNF_INPUT_FLOAT, SWDIO_PIN);
+#define SWDIO_MODE_DRIVE()                                              \
+    gpio_set_mode(SWDIO_PORT, GPIO_MODE_OUTPUT_50_MHZ,                  \
+                  GPIO_CNF_OUTPUT_PUSHPULL, SWDIO_PIN);
+
+#define UART_PIN_SETUP()                                                \
+    gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ,                \
+                  GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN);
+
 #define USB_DRIVER      stm32f103_usb_driver
 #define USB_IRQ         NVIC_USB_LP_CAN_RX0_IRQ
 #define USB_ISR         usb_lp_can_rx0_isr
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index ffea676..97e8697 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -81,6 +81,20 @@ extern usbd_device *usbdev;
 #define LED_UART	GPIO14
 #define LED_IDLE_RUN	GPIO8
 
+#define TMS_SET_MODE()                                          \
+    gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ,            \
+                  GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN);
+#define SWDIO_MODE_FLOAT()                              \
+    gpio_set_mode(SWDIO_PORT, GPIO_MODE_INPUT,          \
+                  GPIO_CNF_INPUT_FLOAT, SWDIO_PIN);
+#define SWDIO_MODE_DRIVE()                                              \
+    gpio_set_mode(SWDIO_PORT, GPIO_MODE_OUTPUT_50_MHZ,                  \
+                  GPIO_CNF_OUTPUT_PUSHPULL, SWDIO_PIN);
+
+#define UART_PIN_SETUP()                                                \
+    gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ,                \
+                  GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN);
+
 #define USB_DRIVER      stm32f103_usb_driver
 #define USB_IRQ         NVIC_USB_LP_CAN_RX0_IRQ
 #define USB_ISR          usb_lp_can_rx0_isr
diff --git a/src/platforms/stm32/jtagtap.c b/src/platforms/stm32/jtagtap.c
index a98c1d2..d6e298b 100644
--- a/src/platforms/stm32/jtagtap.c
+++ b/src/platforms/stm32/jtagtap.c
@@ -25,11 +25,11 @@
 #include "general.h"
 
 #include "jtagtap.h"
+#include "platform.h"
 
 int jtagtap_init(void)
 {
-	gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ, 
-		GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN); 
+	TMS_SET_MODE();
 
 	/* Go to JTAG mode for SWJ-DP */
 	for(int i = 0; i <= 50; i++) jtagtap_next(1, 0); /* Reset SW-DP */
diff --git a/src/platforms/stm32/swdptap.c b/src/platforms/stm32/swdptap.c
index dceb23e..72fb0f9 100644
--- a/src/platforms/stm32/swdptap.c
+++ b/src/platforms/stm32/swdptap.c
@@ -38,14 +38,12 @@ static void swdptap_turnaround(uint8_t dir)
 	if(dir == olddir) return;
 	olddir = dir;
 
-	if(dir) 
-		gpio_set_mode(SWDIO_PORT, GPIO_MODE_INPUT, 
-				GPIO_CNF_INPUT_FLOAT, SWDIO_PIN);
+	if(dir)
+		SWDIO_MODE_FLOAT();
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-	if(!dir) 
-		gpio_set_mode(SWDIO_PORT, GPIO_MODE_OUTPUT_50_MHZ, 
-				GPIO_CNF_OUTPUT_PUSHPULL, SWDIO_PIN);
+	if(!dir)
+		SWDIO_MODE_DRIVE();
 }
 
 static uint8_t swdptap_bit_in(void)
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index 68dac60..fac7b96 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -39,9 +39,7 @@ void usbuart_init(void)
 
 	rcc_peripheral_enable_clock(&USBUSART_APB_ENR, USBUSART_CLK_ENABLE);
 
-	/* UART TX to 'alternate function output push-pull' */
-	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ,
-		GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN);
+	UART_PIN_SETUP();
 
 	/* Setup UART parameters. */
 	usart_set_baudrate(USBUSART, 38400);
diff --git a/src/platforms/stm32_can/platform.h b/src/platforms/stm32_can/platform.h
index cf08b54..030855b 100644
--- a/src/platforms/stm32_can/platform.h
+++ b/src/platforms/stm32_can/platform.h
@@ -83,6 +83,20 @@ extern usbd_device *usbdev;
 #define LED_IDLE_RUN	GPIO1
 #define LED_ERROR	GPIO12
 
+#define TMS_SET_MODE()                                          \
+    gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ,            \
+                  GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN);
+#define SWDIO_MODE_FLOAT()                              \
+    gpio_set_mode(SWDIO_PORT, GPIO_MODE_INPUT,          \
+                  GPIO_CNF_INPUT_FLOAT, SWDIO_PIN);
+#define SWDIO_MODE_DRIVE()                                              \
+    gpio_set_mode(SWDIO_PORT, GPIO_MODE_OUTPUT_50_MHZ,                  \
+                  GPIO_CNF_OUTPUT_PUSHPULL, SWDIO_PIN);
+
+#define UART_PIN_SETUP()                                                \
+    gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ,                \
+                  GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN);
+
 #define USB_DRIVER      stm32f107_usb_driver
 #define USB_IRQ         NVIC_OTG_FS_IRQ
 #define USB_ISR         otg_fs_isr