diff --git a/src/platforms/f4discovery/platform.c b/src/platforms/f4discovery/platform.c
index 8e3b9d37..89b82c16 100644
--- a/src/platforms/f4discovery/platform.c
+++ b/src/platforms/f4discovery/platform.c
@@ -78,7 +78,9 @@ void platform_init(void)
 	GPIOC_OSPEEDR |= 0xA20;
 	gpio_mode_setup(JTAG_PORT, GPIO_MODE_OUTPUT,
 			GPIO_PUPD_NONE,
-			TMS_PIN | TCK_PIN | TDI_PIN);
+			TCK_PIN | TDI_PIN);
+	gpio_mode_setup(JTAG_PORT, GPIO_MODE_INPUT,
+			GPIO_PUPD_NONE, TMS_PIN);
 
 	gpio_mode_setup(TDO_PORT, GPIO_MODE_INPUT,
 			GPIO_PUPD_NONE,
diff --git a/src/platforms/hydrabus/platform.c b/src/platforms/hydrabus/platform.c
index c6a60c26..bf3db04b 100644
--- a/src/platforms/hydrabus/platform.c
+++ b/src/platforms/hydrabus/platform.c
@@ -63,7 +63,9 @@ void platform_init(void)
 	GPIOC_OSPEEDR |= 0xA20;
 	gpio_mode_setup(JTAG_PORT, GPIO_MODE_OUTPUT,
 			GPIO_PUPD_NONE,
-			TMS_PIN | TCK_PIN | TDI_PIN);
+			TCK_PIN | TDI_PIN);
+	gpio_mode_setup(JTAG_PORT, GPIO_MODE_INPUT,
+			GPIO_PUPD_NONE, TMS_PIN);
 
 	gpio_mode_setup(TDO_PORT, GPIO_MODE_INPUT,
 			GPIO_PUPD_NONE,
diff --git a/src/platforms/launchpad-icdi/platform.c b/src/platforms/launchpad-icdi/platform.c
index 5ecfb541..70bb72a0 100644
--- a/src/platforms/launchpad-icdi/platform.c
+++ b/src/platforms/launchpad-icdi/platform.c
@@ -62,7 +62,7 @@ platform_init(void)
 
 	gpio_enable_ahb_aperture();
 
-	gpio_mode_setup(TMS_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, TMS_PIN);
+	gpio_mode_setup(TMS_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, TMS_PIN);
 	gpio_mode_setup(TCK_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, TCK_PIN);
 	gpio_mode_setup(TDI_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, TDI_PIN);
 	gpio_mode_setup(TDO_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, TDO_PIN);
diff --git a/src/platforms/native/platform.c b/src/platforms/native/platform.c
index c69eea3b..7d750572 100644
--- a/src/platforms/native/platform.c
+++ b/src/platforms/native/platform.c
@@ -113,6 +113,11 @@ void platform_init(void)
 	gpio_set_mode(JTAG_PORT, GPIO_MODE_OUTPUT_50_MHZ,
 			GPIO_CNF_OUTPUT_PUSHPULL,
 			TMS_DIR_PIN | TMS_PIN | TCK_PIN | TDI_PIN);
+	gpio_set_mode(JTAG_PORT, GPIO_MODE_OUTPUT_50_MHZ,
+			GPIO_CNF_OUTPUT_PUSHPULL,
+			TMS_DIR_PIN | TCK_PIN | TDI_PIN);
+	gpio_set_mode(JTAG_PORT, GPIO_MODE_OUTPUT_50_MHZ,
+			GPIO_CNF_INPUT_FLOAT, TMS_PIN);
 	/* This needs some fixing... */
 	/* Toggle required to sort out line drivers... */
 	gpio_port_write(GPIOA, 0x8102);
diff --git a/src/platforms/native/platform.h b/src/platforms/native/platform.h
index a261cbca..59d69085 100644
--- a/src/platforms/native/platform.h
+++ b/src/platforms/native/platform.h
@@ -108,20 +108,27 @@
 #define LED_IDLE_RUN	LED_1
 #define LED_ERROR	LED_2
 
+# define SWD_CR   GPIO_CRL(SWDIO_PORT)
+# define SWD_CR_MULT (1 << (4 << 2))
+
 #define TMS_SET_MODE() do { \
 	gpio_set(TMS_DIR_PORT, TMS_DIR_PIN); \
 	gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ, \
 	              GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN); \
 } while(0)
 #define SWDIO_MODE_FLOAT() do { \
-	gpio_set_mode(SWDIO_PORT, GPIO_MODE_INPUT, \
-	              GPIO_CNF_INPUT_FLOAT, SWDIO_PIN); \
-	gpio_clear(SWDIO_DIR_PORT, SWDIO_DIR_PIN); \
+	uint32_t cr = SWD_CR; \
+	cr  &= ~(0xf * SWD_CR_MULT); \
+	cr  |=  (0x4 * SWD_CR_MULT); \
+	GPIO_BRR(SWDIO_DIR_PORT) = SWDIO_DIR_PIN; \
+	SWD_CR = cr; \
 } while(0)
 #define SWDIO_MODE_DRIVE() do { \
-	gpio_set(SWDIO_DIR_PORT, SWDIO_DIR_PIN); \
-	gpio_set_mode(SWDIO_PORT, GPIO_MODE_OUTPUT_50_MHZ, \
-	              GPIO_CNF_OUTPUT_PUSHPULL, SWDIO_PIN); \
+	uint32_t cr = SWD_CR; \
+	cr  &= ~(0xf * SWD_CR_MULT); \
+	cr  |=  (0x1 * SWD_CR_MULT); \
+	GPIO_BSRR(SWDIO_DIR_PORT) = SWDIO_DIR_PIN; \
+	SWD_CR = cr; \
 } while(0)
 #define UART_PIN_SETUP() do { \
 	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ, \
diff --git a/src/platforms/stlink/platform.c b/src/platforms/stlink/platform.c
index 591c2cba..a8797912 100644
--- a/src/platforms/stlink/platform.c
+++ b/src/platforms/stlink/platform.c
@@ -63,7 +63,7 @@ void platform_init(void)
 	}
 	/* Setup GPIO ports */
 	gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ,
-	              GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN);
+	              GPIO_CNF_INPUT_FLOAT, TMS_PIN);
 	gpio_set_mode(TCK_PORT, GPIO_MODE_OUTPUT_50_MHZ,
 	              GPIO_CNF_OUTPUT_PUSHPULL, TCK_PIN);
 	gpio_set_mode(TDI_PORT, GPIO_MODE_OUTPUT_50_MHZ,
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index cadb5d80..548a4c1d 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -71,16 +71,24 @@
 #define PLATFORM_HAS_TRACESWO	1
 #define NUM_TRACE_PACKETS		(128)		/* This is an 8K buffer */
 
+# define SWD_CR   GPIO_CRH(SWDIO_PORT)
+# define SWD_CR_MULT (1 << ((14 - 8) << 2))
+
 #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 SWDIO_MODE_FLOAT() 	do { \
+	uint32_t cr = SWD_CR; \
+	cr  &= ~(0xf * SWD_CR_MULT); \
+	cr  |=  (0x4 * SWD_CR_MULT); \
+	SWD_CR = cr; \
+} while(0)
+#define SWDIO_MODE_DRIVE() 	do { \
+	uint32_t cr = SWD_CR; \
+	cr  &= ~(0xf * SWD_CR_MULT); \
+	cr  |=  (0x1 * SWD_CR_MULT); \
+	SWD_CR = cr; \
+} while(0)
 #define UART_PIN_SETUP() \
 	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ, \
 	              GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN);
diff --git a/src/platforms/stm32/jtagtap.c b/src/platforms/stm32/jtagtap.c
index 1ee8cfe0..0b5d4b4f 100644
--- a/src/platforms/stm32/jtagtap.c
+++ b/src/platforms/stm32/jtagtap.c
@@ -66,3 +66,63 @@ inline uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 	return ret != 0;
 }
 
+void jtagtap_tms_seq(uint32_t MS, int ticks)
+{
+	gpio_set_val(TDI_PORT, TDI_PIN, 1);
+	int data = MS & 1;
+	while(ticks) {
+		gpio_set_val(TMS_PORT, TMS_PIN, data);
+		gpio_set(TCK_PORT, TCK_PIN);
+		MS >>= 1;
+		data = MS & 1;
+		ticks--;
+		gpio_clear(TCK_PORT, TCK_PIN);
+	}
+}
+
+void
+jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
+{
+	uint8_t index = 1;
+	gpio_set_val(TMS_PORT, TMS_PIN, 0);
+	uint8_t res = 0;
+	while(ticks > 1) {
+		gpio_set_val(TDI_PORT, TDI_PIN, *DI & index);
+		gpio_set(TCK_PORT, TCK_PIN);
+		if (gpio_get(TDO_PORT, TDO_PIN)) {
+			res |= index;
+		}
+		if(!(index <<= 1)) {
+			*DO = res;
+			res = 0;
+			index = 1;
+			DI++; DO++;
+		}
+		ticks--;
+		gpio_clear(TCK_PORT, TCK_PIN);
+	}
+	gpio_set_val(TMS_PORT, TMS_PIN, final_tms);
+	gpio_set_val(TDI_PORT, TDI_PIN, *DI & index);
+	gpio_set(TCK_PORT, TCK_PIN);
+	if (gpio_get(TDO_PORT, TDO_PIN)) {
+		res |= index;
+	}
+	*DO = res;
+	gpio_clear(TCK_PORT, TCK_PIN);
+}
+
+void
+jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI, int ticks)
+{
+	uint8_t index = 1;
+	while(ticks--) {
+		gpio_set_val(TMS_PORT, TMS_PIN, ticks? 0 : final_tms);
+		gpio_set_val(TDI_PORT, TDI_PIN, *DI & index);
+		gpio_set(TCK_PORT, TCK_PIN);
+		if(!(index <<= 1)) {
+			index = 1;
+			DI++;
+		}
+		gpio_clear(TCK_PORT, TCK_PIN);
+	}
+}
diff --git a/src/platforms/swlink/platform.c b/src/platforms/swlink/platform.c
index 651ab9b1..91150858 100644
--- a/src/platforms/swlink/platform.c
+++ b/src/platforms/swlink/platform.c
@@ -57,11 +57,11 @@ void platform_init(void)
 	data |= AFIO_MAPR_SWJ_CFG_JTAG_OFF_SW_OFF;
 	AFIO_MAPR = data;
 	/* Setup JTAG GPIO ports */
-	gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_10_MHZ,
-			GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN);
-	gpio_set_mode(TCK_PORT, GPIO_MODE_OUTPUT_10_MHZ,
+	gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ,
+			GPIO_CNF_INPUT_FLOAT, TMS_PIN);
+	gpio_set_mode(TCK_PORT, GPIO_MODE_OUTPUT_50_MHZ,
 			GPIO_CNF_OUTPUT_PUSHPULL, TCK_PIN);
-	gpio_set_mode(TDI_PORT, GPIO_MODE_OUTPUT_10_MHZ,
+	gpio_set_mode(TDI_PORT, GPIO_MODE_OUTPUT_50_MHZ,
 			GPIO_CNF_OUTPUT_PUSHPULL, TDI_PIN);
 
 	gpio_set_mode(TDO_PORT, GPIO_MODE_INPUT,
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index 748c31fd..436dd247 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -58,16 +58,24 @@
 #define LED_PORT_UART	GPIOC
 #define LED_UART	GPIO14
 
+# define SWD_CR   GPIO_CRH(SWDIO_PORT)
+# define SWD_CR_MULT (1 << ((13 - 8) << 2))
+
 #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 SWDIO_MODE_FLOAT() 	do { \
+	uint32_t cr = SWD_CR; \
+	cr  &= ~(0xf * SWD_CR_MULT); \
+	cr  |=  (0x4 * SWD_CR_MULT); \
+	SWD_CR = cr; \
+} while(0)
+#define SWDIO_MODE_DRIVE() 	do { \
+	uint32_t cr = SWD_CR; \
+	cr  &= ~(0xf * SWD_CR_MULT); \
+	cr  |=  (0x1 * SWD_CR_MULT); \
+	SWD_CR = cr; \
+} while(0)
 #define UART_PIN_SETUP() do { \
 	AFIO_MAPR |= AFIO_MAPR_USART1_REMAP; \
 	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ, \