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] 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,