From e54a826745ae298a4eb555f3d76dfcdd571211b9 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 23 Jan 2018 20:15:16 +0100
Subject: [PATCH 1/6] common/swdptap.c: Speed up by "unrolling" swd.._seq_...()
 for GPIO.

Try to have sensible setup/hold times by evenntually duplicated or
logically useless port commands.
Loading code to RAM on a STM32L476 got up from 46 to 83 kB/sec.
---
 src/platforms/common/swdptap.c | 104 ++++++++++++++++++++++++++++++++-
 1 file changed, 103 insertions(+), 1 deletion(-)

diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 64cdc58a..321ce5b2 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -18,7 +18,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-/* This file implements the low-level SW-DP interface. */
+/* This file implements the SW-DP interface. */
 
 #include "general.h"
 #include "swdptap.h"
@@ -43,6 +43,7 @@ static void swdptap_turnaround(uint8_t dir)
 	if(dir)
 		SWDIO_MODE_FLOAT();
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	if(!dir)
 		SWDIO_MODE_DRIVE();
@@ -56,6 +57,7 @@ bool swdptap_bit_in(void)
 
 	ret = gpio_get(SWDIO_PORT, SWDIO_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 
 #ifdef DEBUG_SWD_BITS
@@ -65,6 +67,64 @@ bool swdptap_bit_in(void)
 	return ret != 0;
 }
 
+uint32_t
+swdptap_seq_in(int ticks)
+{
+	uint32_t index = 1;
+	uint32_t ret = 0;
+	int len = ticks;
+
+	swdptap_turnaround(1);
+	while (len--) {
+		int res;
+		res = gpio_get(SWDIO_PORT, SWDIO_PIN);
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
+		if (res)
+			ret |= index;
+		index <<= 1;
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	}
+
+#ifdef DEBUG_SWD_BITS
+	for (int i = 0; i < len; i++)
+		DEBUG("%d", (ret & (1 << i)) ? 1 : 0);
+#endif
+	return ret;
+}
+
+bool
+swdptap_seq_in_parity(uint32_t *ret, int ticks)
+{
+	uint32_t index = 1;
+	uint8_t parity = 0;
+	uint32_t res = 0;
+	bool bit;
+	int len = ticks;
+
+	swdptap_turnaround(1);
+	while (len--) {
+		bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
+		if (bit) {
+			res |= index;
+			parity ^= 1;
+		}
+		index <<= 1;
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	}
+	bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	if (bit)
+		parity ^= 1;
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+#ifdef DEBUG_SWD_BITS
+	for (int i = 0; i < len; i++)
+		DEBUG("%d", (res & (1 << i)) ? 1 : 0);
+#endif
+	*ret = res;
+	return parity;
+}
+
 void swdptap_bit_out(bool val)
 {
 #ifdef DEBUG_SWD_BITS
@@ -75,6 +135,48 @@ void swdptap_bit_out(bool val)
 
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 }
+void
+swdptap_seq_out(uint32_t MS, int ticks)
+{
+	int data = MS & 1;
+#ifdef DEBUG_SWD_BITS
+	for (int i = 0; i < ticks; i++)
+		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
+#endif
+	swdptap_turnaround(0);
+	while (ticks--) {
+		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
+		MS >>= 1;
+		data = MS & 1;
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	}
+}
 
+void
+swdptap_seq_out_parity(uint32_t MS, int ticks)
+{
+	uint8_t parity = 0;
+	int data = MS & 1;
+#ifdef DEBUG_SWD_BITS
+	for (int i = 0; i < ticks; i++)
+		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
+#endif
+	swdptap_turnaround(0);
+
+	while (ticks--) {
+		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
+		parity ^= MS;
+		MS >>= 1;
+		data = MS & 1;
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	}
+	gpio_set_val(SWDIO_PORT, SWDIO_PIN, parity & 1);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+}

From 7e3fe352ad4aed89522d808d76dbe868a470412e Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 23 Jan 2018 20:27:43 +0100
Subject: [PATCH 2/6] adiv5_swdp.c: Use swdptap_seq_out for initialiation
 sequence.

---
 src/platforms/common/swdptap.c |  7 +++++--
 src/target/adiv5_swdp.c        | 10 +++++-----
 2 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 321ce5b2..9a0d89c9 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -134,6 +134,7 @@ void swdptap_bit_out(bool val)
 	swdptap_turnaround(0);
 
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
@@ -149,9 +150,10 @@ swdptap_seq_out(uint32_t MS, int ticks)
 	swdptap_turnaround(0);
 	while (ticks--) {
 		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		MS >>= 1;
 		data = MS & 1;
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
 }
@@ -169,13 +171,14 @@ swdptap_seq_out_parity(uint32_t MS, int ticks)
 
 	while (ticks--) {
 		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		parity ^= MS;
 		MS >>= 1;
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		data = MS & 1;
 		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, parity & 1);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 6dfdfd4d..dd036549 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -53,12 +53,12 @@ int adiv5_swdp_scan(void)
 		return -1;
 
 	/* Switch from JTAG to SWD mode */
-	swdptap_seq_out(0xFFFF, 16);
-	for(int i = 0; i < 50; i++)
-		swdptap_bit_out(1);
+	swdptap_seq_out(0xFFFFFFFF, 16);
+	swdptap_seq_out(0xFFFFFFFF, 32);
+	swdptap_seq_out(0xFFFFFFFF, 18);
 	swdptap_seq_out(0xE79E, 16); /* 0b0111100111100111 */
-	for(int i = 0; i < 50; i++)
-		swdptap_bit_out(1);
+	swdptap_seq_out(0xFFFFFFFF, 32);
+	swdptap_seq_out(0xFFFFFFFF, 18);
 	swdptap_seq_out(0, 16);
 
 	/* Read the SW-DP IDCODE register to syncronise */

From 5548d54626a658b65f1f963cd5af54ddaf93fbf7 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 6 Mar 2018 18:52:23 +0100
Subject: [PATCH 3/6] common/swdptap: some clean up.

Remove superfluous transaction.
Use native variable size.
---
 src/platforms/common/swdptap.c |  4 ++--
 src/target/adiv5_swdp.c        | 11 ++++-------
 2 files changed, 6 insertions(+), 9 deletions(-)

diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 9a0d89c9..46d5a672 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -28,9 +28,9 @@ int swdptap_init(void)
 	return 0;
 }
 
-static void swdptap_turnaround(uint8_t dir)
+static void swdptap_turnaround(int dir)
 {
-	static uint8_t olddir = 0;
+	static int olddir = 0;
 
 	/* Don't turnaround if direction not changing */
 	if(dir == olddir) return;
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index dd036549..dcd46a6c 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -44,7 +44,7 @@ static void adiv5_swdp_abort(ADIv5_DP_t *dp, uint32_t abort);
 
 int adiv5_swdp_scan(void)
 {
-	uint8_t ack;
+	uint32_t ack;
 
 	target_list_free();
 	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
@@ -122,9 +122,9 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 {
 	bool APnDP = addr & ADIV5_APnDP;
 	addr &= 0xff;
-	uint8_t request = 0x81;
+	uint32_t request = 0x81;
 	uint32_t response = 0;
-	uint8_t ack;
+	uint32_t ack;
 	platform_timeout timeout;
 
 	if(APnDP && dp->fault) return 0;
@@ -141,7 +141,7 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	do {
 		swdptap_seq_out(request, 8);
 		ack = swdptap_seq_in(3);
-	} while (!platform_timeout_is_expired(&timeout) && ack == SWDP_ACK_WAIT);
+	} while (ack == SWDP_ACK_WAIT && !platform_timeout_is_expired(&timeout));
 
 	if (ack == SWDP_ACK_WAIT)
 		raise_exception(EXCEPTION_TIMEOUT, "SWDP ACK timeout");
@@ -161,9 +161,6 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		swdptap_seq_out_parity(value, 32);
 	}
 
-	/* REMOVE THIS */
-	swdptap_seq_out(0, 8);
-
 	return response;
 }
 

From 97561fc5cc29c52c60fcde0e52464881a85a19ff Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 6 Mar 2018 18:36:09 +0100
Subject: [PATCH 4/6] stlink: Decrease delay with SWD turn around for
 native,stlink and swlink.

---
 src/platforms/native/platform.h | 19 +++++++++++++------
 src/platforms/stlink/platform.h | 22 +++++++++++++++-------
 src/platforms/swlink/platform.h | 22 +++++++++++++++-------
 3 files changed, 43 insertions(+), 20 deletions(-)

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.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/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, \

From 59e03dea2789e1ee4698c9a1a5c1deeedf67ed5d Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 7 Mar 2018 15:57:00 +0100
Subject: [PATCH 5/6] Keep TMS floating until scanning.

NRF5x shares nRST with SWDIO and otherwise does not run until scan is done.
---
 src/platforms/common/swdptap.c          | 23 ++++++++++++++---------
 src/platforms/f4discovery/platform.c    |  4 +++-
 src/platforms/hydrabus/platform.c       |  4 +++-
 src/platforms/launchpad-icdi/platform.c |  2 +-
 src/platforms/native/platform.c         |  5 +++++
 src/platforms/stlink/platform.c         |  2 +-
 src/platforms/swlink/platform.c         |  2 +-
 7 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 46d5a672..9d9638de 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -23,6 +23,11 @@
 #include "general.h"
 #include "swdptap.h"
 
+enum {
+	SWDIO_STATUS_FLOAT = 0,
+	SWDIO_STATUS_DRIVE
+};
+
 int swdptap_init(void)
 {
 	return 0;
@@ -30,7 +35,7 @@ int swdptap_init(void)
 
 static void swdptap_turnaround(int dir)
 {
-	static int olddir = 0;
+	static int olddir = SWDIO_STATUS_FLOAT;
 
 	/* Don't turnaround if direction not changing */
 	if(dir == olddir) return;
@@ -40,12 +45,12 @@ static void swdptap_turnaround(int dir)
 	DEBUG("%s", dir ? "\n-> ":"\n<- ");
 #endif
 
-	if(dir)
+	if(dir == SWDIO_STATUS_FLOAT)
 		SWDIO_MODE_FLOAT();
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-	if(!dir)
+	if(dir == SWDIO_STATUS_DRIVE)
 		SWDIO_MODE_DRIVE();
 }
 
@@ -53,7 +58,7 @@ bool swdptap_bit_in(void)
 {
 	uint16_t ret;
 
-	swdptap_turnaround(1);
+	swdptap_turnaround(SWDIO_STATUS_FLOAT);
 
 	ret = gpio_get(SWDIO_PORT, SWDIO_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
@@ -74,7 +79,7 @@ swdptap_seq_in(int ticks)
 	uint32_t ret = 0;
 	int len = ticks;
 
-	swdptap_turnaround(1);
+	swdptap_turnaround(SWDIO_STATUS_FLOAT);
 	while (len--) {
 		int res;
 		res = gpio_get(SWDIO_PORT, SWDIO_PIN);
@@ -101,7 +106,7 @@ swdptap_seq_in_parity(uint32_t *ret, int ticks)
 	bool bit;
 	int len = ticks;
 
-	swdptap_turnaround(1);
+	swdptap_turnaround(SWDIO_STATUS_FLOAT);
 	while (len--) {
 		bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
 		gpio_set(SWCLK_PORT, SWCLK_PIN);
@@ -131,7 +136,7 @@ void swdptap_bit_out(bool val)
 	DEBUG("%d", val);
 #endif
 
-	swdptap_turnaround(0);
+	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
@@ -147,7 +152,7 @@ swdptap_seq_out(uint32_t MS, int ticks)
 	for (int i = 0; i < ticks; i++)
 		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
 #endif
-	swdptap_turnaround(0);
+	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 	while (ticks--) {
 		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
 		MS >>= 1;
@@ -167,7 +172,7 @@ swdptap_seq_out_parity(uint32_t MS, int ticks)
 	for (int i = 0; i < ticks; i++)
 		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
 #endif
-	swdptap_turnaround(0);
+	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 
 	while (ticks--) {
 		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
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/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/swlink/platform.c b/src/platforms/swlink/platform.c
index 651ab9b1..15458336 100644
--- a/src/platforms/swlink/platform.c
+++ b/src/platforms/swlink/platform.c
@@ -58,7 +58,7 @@ void platform_init(void)
 	AFIO_MAPR = data;
 	/* Setup JTAG GPIO ports */
 	gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_10_MHZ,
-			GPIO_CNF_OUTPUT_PUSHPULL, TMS_PIN);
+			GPIO_CNF_INPUT_FLOAT, TMS_PIN);
 	gpio_set_mode(TCK_PORT, GPIO_MODE_OUTPUT_10_MHZ,
 			GPIO_CNF_OUTPUT_PUSHPULL, TCK_PIN);
 	gpio_set_mode(TDI_PORT, GPIO_MODE_OUTPUT_10_MHZ,

From 7a7266a0f7e38a9359f84fe1fb34453e8b2b16ae Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 7 Mar 2018 18:49:19 +0100
Subject: [PATCH 6/6] Speed up JTAG.

---
 src/platforms/stm32/jtagtap.c   | 60 +++++++++++++++++++++++++++++++++
 src/platforms/swlink/platform.c |  6 ++--
 2 files changed, 63 insertions(+), 3 deletions(-)

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 15458336..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_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ,
 			GPIO_CNF_INPUT_FLOAT, TMS_PIN);
-	gpio_set_mode(TCK_PORT, GPIO_MODE_OUTPUT_10_MHZ,
+	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,