From 7365a44989c7c8561c583f75ff30f7277e712ff3 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Fri, 20 Nov 2020 11:48:54 +0100
Subject: [PATCH 1/7] hosted: Implement SWJ test mode (-T).

Continious read/write-back some core register as NOP operation to allow scope
measurement of SWJ timing.
---
 src/platforms/pc/cl_utils.c | 26 ++++++++++++++++++++++++--
 src/platforms/pc/cl_utils.h |  3 ++-
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index e27fe59e..6283d561 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -31,6 +31,7 @@
 #include "version.h"
 #include "target.h"
 #include "target_internal.h"
+#include "cortexm.h"
 
 #include "cl_utils.h"
 #include "bmp_hosted.h"
@@ -140,6 +141,8 @@ static void cl_help(char **argv)
 	DEBUG_WARN("\t-C\t\t: Connect under reset\n");
 	DEBUG_WARN("\t-t\t\t: Scan SWD or JTAG and display information about \n"
 			   "\t\t\t  connected devices\n");
+	DEBUG_WARN("\t-T\t\t: Continious read/write-back some value to allow\n"
+			   "\t\t\t  timing insection of SWJ. Abort with ^C\n");
 	DEBUG_WARN("\t-e\t\t: Assume \"resistor SWD connection\" on FTDI: TDI\n"
                "\t\t\t  connected to TMS, TDO to TDI with eventual resistor\n");
 	DEBUG_WARN("\t-E\t\t: Erase flash until flash end or for given size\n");
@@ -163,7 +166,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	opt->opt_target_dev = 1;
 	opt->opt_flash_size = 16 * 1024 *1024;
 	opt->opt_flash_start = 0xffffffff;
-	while((c = getopt(argc, argv, "eEhHv:d:s:I:c:CnltVta:S:jpP:rR")) != -1) {
+	while((c = getopt(argc, argv, "eEhHv:d:s:I:c:CnltVtTa:S:jpP:rR")) != -1) {
 		switch(c) {
 		case 'c':
 			if (optarg)
@@ -212,6 +215,9 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			opt->opt_mode = BMP_MODE_TEST;
 			cl_debuglevel |= BMP_DEBUG_INFO | BMP_DEBUG_STDOUT;
 			break;
+		case 'T':
+			opt->opt_mode = BMP_MODE_SWJ_TEST;
+			break;
 		case 'V':
 			opt->opt_mode = BMP_MODE_FLASH_VERIFY;
 			break;
@@ -262,6 +268,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	}
 	/* Checks */
 	if ((opt->opt_flash_file) && ((opt->opt_mode == BMP_MODE_TEST ) ||
+								  (opt->opt_mode == BMP_MODE_SWJ_TEST) ||
 								  (opt->opt_mode == BMP_MODE_RESET))) {
 		DEBUG_WARN("Ignoring filename in reset/test mode\n");
 		opt->opt_flash_file = NULL;
@@ -369,7 +376,22 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	}
 	if (opt->opt_flash_start == 0xffffffff)
 		opt->opt_flash_start = flash_start;
-	if (opt->opt_mode == BMP_MODE_TEST)
+	if (opt->opt_mode == BMP_MODE_SWJ_TEST) {
+		switch (t->core[0]) {
+		case 'M':
+			DEBUG_WARN("Continious read/write-back DEMCR. Abort with ^C\n");
+			while(1) {
+				uint32_t demcr;
+				target_mem_read(t, &demcr, CORTEXM_DEMCR, 4);
+				target_mem_write32(t, CORTEXM_DEMCR, demcr);
+				platform_delay(1); /* To allow trigger*/
+			}
+		default:
+			DEBUG_WARN("No test for this core type yet\n");
+		}
+	}
+	if ((opt->opt_mode == BMP_MODE_TEST) ||
+		(opt->opt_mode == BMP_MODE_SWJ_TEST))
 		goto target_detach;
 	int read_file = -1;
 	if ((opt->opt_mode == BMP_MODE_FLASH_WRITE) ||
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index e7349c71..6cff3b81 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -33,7 +33,8 @@ enum bmp_cl_mode {
 	BMP_MODE_FLASH_ERASE,
 	BMP_MODE_FLASH_WRITE,
 	BMP_MODE_FLASH_READ,
-	BMP_MODE_FLASH_VERIFY
+	BMP_MODE_FLASH_VERIFY,
+	BMP_MODE_SWJ_TEST,
 };
 
 typedef struct BMP_CL_OPTIONS_s {

From 1ca9f234f7527396da91bfc1f98cb05c305c6472 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 19 Nov 2020 21:36:59 +0100
Subject: [PATCH 2/7] Infrastructure for setting maximum SWJ frequency.

Implement for BMP/firmware on STM32.
---
 src/command.c                           |  27 ++++++
 src/include/general.h                   |   4 +
 src/include/platform_support.h          |   3 +
 src/include/timing.h                    |   1 +
 src/platforms/common/jtagtap.c          | 110 +++++++++++++++------
 src/platforms/common/swdptap.c          | 124 ++++++++++++++++--------
 src/platforms/hosted/bmp_remote.c       |  34 +++++++
 src/platforms/hosted/bmp_remote.h       |   2 +
 src/platforms/hosted/platform.c         |  28 ++++++
 src/platforms/launchpad-icdi/platform.c |  11 +++
 src/platforms/launchpad-icdi/platform.h |   1 +
 src/platforms/pc/cl_utils.c             |  20 +++-
 src/platforms/pc/cl_utils.h             |   1 +
 src/platforms/stm32/timing_stm32.c      |  27 ++++++
 src/platforms/stm32/timing_stm32.h      |   1 +
 src/remote.c                            |   9 ++
 src/remote.h                            |   4 +
 17 files changed, 338 insertions(+), 69 deletions(-)

diff --git a/src/command.c b/src/command.c
index 6f7f1f00..9f15c211 100644
--- a/src/command.c
+++ b/src/command.c
@@ -49,6 +49,7 @@ static bool cmd_help(target *t, int argc, char **argv);
 
 static bool cmd_jtag_scan(target *t, int argc, char **argv);
 static bool cmd_swdp_scan(target *t, int argc, char **argv);
+static bool cmd_frequency(target *t, int argc, char **argv);
 static bool cmd_targets(target *t, int argc, char **argv);
 static bool cmd_morse(target *t, int argc, char **argv);
 static bool cmd_halt_timeout(target *t, int argc, const char **argv);
@@ -70,6 +71,7 @@ const struct command_s cmd_list[] = {
 	{"help", (cmd_handler)cmd_help, "Display help for monitor commands"},
 	{"jtag_scan", (cmd_handler)cmd_jtag_scan, "Scan JTAG chain for devices" },
 	{"swdp_scan", (cmd_handler)cmd_swdp_scan, "Scan SW-DP for devices" },
+	{"frequency", (cmd_handler)cmd_frequency, "set minimum high and low times" },
 	{"targets", (cmd_handler)cmd_targets, "Display list of available targets" },
 	{"morse", (cmd_handler)cmd_morse, "Display morse error message" },
 	{"halt_timeout", (cmd_handler)cmd_halt_timeout, "Timeout (ms) to wait until Cortex-M is halted: (Default 2000)" },
@@ -258,6 +260,31 @@ bool cmd_swdp_scan(target *t, int argc, char **argv)
 
 }
 
+bool cmd_frequency(target *t, int argc, char **argv)
+{
+	(void)t;
+	if (argc == 2) {
+		char *p;
+		uint32_t frequency = strtol(argv[1], &p, 10);
+		switch(*p) {
+		case 'k':
+			frequency *= 1000;
+			break;
+		case 'M':
+			frequency *= 1000*1000;
+			break;
+		}
+		platform_max_frequency_set(frequency);
+	}
+	uint32_t freq = platform_max_frequency_get();
+	if (freq == FREQ_FIXED)
+		gdb_outf("SWJ freq fixed\n");
+	else
+		gdb_outf("Max SWJ freq %08" PRIx32 "\n", freq);
+	return true;
+
+}
+
 static void display_target(int i, target *t, void *context)
 {
 	(void)context;
diff --git a/src/include/general.h b/src/include/general.h
index 429bf58f..ba86da22 100644
--- a/src/include/general.h
+++ b/src/include/general.h
@@ -37,6 +37,8 @@
 #include "platform.h"
 #include "platform_support.h"
 
+extern uint32_t delay_cnt;
+
 enum BMP_DEBUG {
 	BMP_DEBUG_NONE   =  0,
 	BMP_DEBUG_INFO   =  1,
@@ -48,6 +50,8 @@ enum BMP_DEBUG {
 	BMP_DEBUG_STDOUT = 0x8000,
 };
 
+#define FREQ_FIXED 0xffffffff
+
 #if PC_HOSTED == 0
 /* For BMP debug output on a firmware BMP platform, using
  * BMP PC-Hosted is the preferred way. Printing DEBUG_WARN
diff --git a/src/include/platform_support.h b/src/include/platform_support.h
index 968440a5..59720cb4 100644
--- a/src/include/platform_support.h
+++ b/src/include/platform_support.h
@@ -45,5 +45,8 @@ bool platform_srst_get_val(void);
 bool platform_target_get_power(void);
 void platform_target_set_power(bool power);
 void platform_request_boot(void);
+void platform_max_frequency_set(uint32_t frequency);
+uint32_t platform_max_frequency_get(void);
+
 #endif
 
diff --git a/src/include/timing.h b/src/include/timing.h
index 4a55f185..2d235081 100644
--- a/src/include/timing.h
+++ b/src/include/timing.h
@@ -24,6 +24,7 @@ struct platform_timeout {
 	uint32_t time;
 };
 
+extern int32_t swj_delay_cnt;
 uint32_t platform_time_ms(void);
 
 #endif /* __TIMING_H */
diff --git a/src/platforms/common/jtagtap.c b/src/platforms/common/jtagtap.c
index a319a6b2..24c858ff 100644
--- a/src/platforms/common/jtagtap.c
+++ b/src/platforms/common/jtagtap.c
@@ -70,12 +70,15 @@ static void jtagtap_reset(void)
 static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 {
 	uint16_t ret;
+	register volatile int32_t cnt;
 
 	gpio_set_val(TMS_PORT, TMS_PIN, dTMS);
 	gpio_set_val(TDI_PORT, TDI_PIN, dTDI);
 	gpio_set(TCK_PORT, TCK_PIN);
+	for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
 	ret = gpio_get(TDO_PORT, TDO_PIN);
 	gpio_clear(TCK_PORT, TCK_PIN);
+	for(cnt = swd_delay_cnt - 2; cnt > 0; cnt--);
 
 	//DEBUG("jtagtap_next(TMS = %d, TDI = %d) = %d\n", dTMS, dTDI, ret);
 
@@ -86,13 +89,27 @@ static 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);
+	register volatile int32_t cnt;
+	if (swd_delay_cnt) {
+		while(ticks) {
+			gpio_set_val(TMS_PORT, TMS_PIN, data);
+			gpio_set(TCK_PORT, TCK_PIN);
+			for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
+			MS >>= 1;
+			data = MS & 1;
+			ticks--;
+			gpio_clear(TCK_PORT, TCK_PIN);
+			for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
+		}
+	} else {
+		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);
+		}
 	}
 }
 
@@ -102,42 +119,81 @@ static void jtagtap_tdi_tdo_seq(
 	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;
+	register volatile int32_t cnt;
+	if (swd_delay_cnt) {
+		while(ticks > 1) {
+			gpio_set_val(TDI_PORT, TDI_PIN, *DI & index);
+			gpio_set(TCK_PORT, TCK_PIN);
+			for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
+			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);
+			for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
 		}
-		if(!(index <<= 1)) {
-			*DO = res;
-			res = 0;
-			index = 1;
-			DI++; DO++;
+	} else {
+		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);
 		}
-		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);
+			for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
 	if (gpio_get(TDO_PORT, TDO_PIN)) {
 		res |= index;
 	}
 	*DO = res;
 	gpio_clear(TCK_PORT, TCK_PIN);
+	for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
 }
 
 static 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++;
+	register volatile int32_t cnt;
+	if (swd_delay_cnt) {
+		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);
+			for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
+			if(!(index <<= 1)) {
+				index = 1;
+				DI++;
+			}
+			gpio_clear(TCK_PORT, TCK_PIN);
+			for(cnt = swd_delay_cnt -2 ; cnt > 0; cnt--);
+		}
+	} else {
+		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);
 		}
-		gpio_clear(TCK_PORT, TCK_PIN);
 	}
 }
diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 65f863ec..8043d8f8 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -22,6 +22,7 @@
 
 #include "general.h"
 #include "swdptap.h"
+#include "timing.h"
 
 enum {
 	SWDIO_STATUS_FLOAT = 0,
@@ -39,6 +40,7 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 static void swdptap_turnaround(int dir)
 {
 	static int olddir = SWDIO_STATUS_FLOAT;
+	register volatile int32_t cnt;
 
 	/* Don't turnaround if direction not changing */
 	if(dir == olddir) return;
@@ -51,10 +53,9 @@ static void swdptap_turnaround(int dir)
 	if(dir == SWDIO_STATUS_FLOAT)
 		SWDIO_MODE_FLOAT();
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	for(cnt = swd_delay_cnt; --cnt > 0;);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	for(cnt = swd_delay_cnt; --cnt > 0;);
 	if(dir == SWDIO_STATUS_DRIVE)
 		SWDIO_MODE_DRIVE();
 }
@@ -64,21 +65,30 @@ static uint32_t swdptap_seq_in(int ticks)
 	uint32_t index = 1;
 	uint32_t ret = 0;
 	int len = ticks;
+	register volatile int32_t cnt;
 
 	swdptap_turnaround(SWDIO_STATUS_FLOAT);
-	while (len--) {
-		int res;
-		res = gpio_get(SWDIO_PORT, SWDIO_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		if (res)
-			ret |= index;
-		index <<= 1;
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	if (swd_delay_cnt) {
+		while (len--) {
+			int res;
+			res = gpio_get(SWDIO_PORT, SWDIO_PIN);
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+			ret |= (res) ? index : 0;
+			index <<= 1;
+			gpio_clear(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+		}
+	} else {
+		volatile int res;
+		while (len--) {
+			res = gpio_get(SWDIO_PORT, SWDIO_PIN);
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			ret |= (res) ? index : 0;
+			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);
@@ -92,26 +102,35 @@ static bool swdptap_seq_in_parity(uint32_t *ret, int ticks)
 	uint32_t res = 0;
 	bool bit;
 	int len = ticks;
+	register volatile int32_t cnt;
 
 	swdptap_turnaround(SWDIO_STATUS_FLOAT);
-	while (len--) {
-		bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		if (bit)
-			res |= index;
-		index <<= 1;
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	if (swd_delay_cnt) {
+		while (len--) {
+			bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+			res |= (bit) ? index : 0;
+			index <<= 1;
+			gpio_clear(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+		}
+	} else {
+		while (len--) {
+			bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			res |= (bit) ? index : 0;
+			index <<= 1;
+			gpio_clear(SWCLK_PORT, SWCLK_PIN);
+		}
 	}
 	int parity = __builtin_popcount(res);
 	bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	if (bit)
-		parity++;
-	else
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
+	for(cnt = swd_delay_cnt; --cnt > 0;);
+	parity += (bit) ? 1 : 0;
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	for(cnt = swd_delay_cnt; --cnt > 0;);
 #ifdef DEBUG_SWD_BITS
 	for (int i = 0; i < len; i++)
 		DEBUG("%d", (res & (1 << i)) ? 1 : 0);
@@ -128,14 +147,25 @@ static void swdptap_seq_out(uint32_t MS, int ticks)
 	for (int i = 0; i < ticks; i++)
 		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
 #endif
+	register volatile int32_t cnt;
 	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
-	while (ticks--) {
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		MS >>= 1;
-		gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	if (swd_delay_cnt) {
+		while (ticks--) {
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+			MS >>= 1;
+			gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
+			gpio_clear(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+		}
+	} else {
+		while (ticks--) {
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			MS >>= 1;
+			gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
+			gpio_clear(SWCLK_PORT, SWCLK_PIN);
+		}
 	}
 }
 
@@ -146,20 +176,32 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 	for (int i = 0; i < ticks; i++)
 		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
 #endif
+	register volatile int32_t cnt;
 	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
 	MS >>= 1;
-	while (ticks--) {
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
-		MS >>= 1;
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	if (swd_delay_cnt) {
+		while (ticks--) {
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+			gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
+			MS >>= 1;
+			gpio_clear(SWCLK_PORT, SWCLK_PIN);
+			for(cnt = swd_delay_cnt; --cnt > 0;);
+		}
+	} else {
+		while (ticks--) {
+			gpio_set(SWCLK_PORT, SWCLK_PIN);
+			gpio_set_val(SWDIO_PORT, SWDIO_PIN, MS & 1);
+			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_set(SWCLK_PORT, SWCLK_PIN);
+	for(cnt = swd_delay_cnt; --cnt > 0;);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	for(cnt = swd_delay_cnt; --cnt > 0;);
 }
 
 swd_proc_t swd_proc;
diff --git a/src/platforms/hosted/bmp_remote.c b/src/platforms/hosted/bmp_remote.c
index b6d97f18..66d1b92a 100644
--- a/src/platforms/hosted/bmp_remote.c
+++ b/src/platforms/hosted/bmp_remote.c
@@ -128,6 +128,40 @@ bool remote_srst_get_val(void)
 	return (construct[1] == '1');
 }
 
+void remote_max_frequency_set(uint32_t freq)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, REMOTE_FREQ_SET_STR,
+				 freq);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		DEBUG_WARN("Update Firmware to allow to set max SWJ frequency\n");
+    }
+}
+
+uint32_t remote_max_frequency_get(void)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,"%s",
+				 REMOTE_FREQ_GET_STR);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR))
+		return FREQ_FIXED;
+
+	uint32_t freq[1];
+	unhexify(freq, (const char*)&construct[1], 4);
+	return freq[0];
+}
+
 const char *remote_target_voltage(void)
 {
 	static uint8_t construct[REMOTE_MAX_MSG_SIZE];
diff --git a/src/platforms/hosted/bmp_remote.h b/src/platforms/hosted/bmp_remote.h
index 5d135a38..c61cb026 100644
--- a/src/platforms/hosted/bmp_remote.h
+++ b/src/platforms/hosted/bmp_remote.h
@@ -37,6 +37,8 @@ const char *remote_target_voltage(void);
 void remote_target_set_power(bool power);
 void remote_srst_set_val(bool assert);
 bool remote_srst_get_val(void);
+void remote_max_frequency_set(uint32_t freq);
+uint32_t remote_max_frequency_get(void);
 const char *platform_target_voltage(void);
 void remote_adiv5_dp_defaults(ADIv5_DP_t *dp);
 void remote_add_jtag_dev(int i, const jtag_dev_t *jtag_dev);
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 8df0dbe5..8ee10169 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -109,6 +109,8 @@ void platform_init(int argc, char **argv)
 	default:
 		exit(-1);
 	}
+	if (cl_opts.opt_max_swj_frequency)
+		platform_max_frequency_set(cl_opts.opt_max_swj_frequency);
 	int ret = -1;
 	if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
 		ret = cl_execute(&cl_opts);
@@ -318,6 +320,32 @@ bool platform_srst_get_val(void)
 	return false;
 }
 
+void platform_max_frequency_set(uint32_t freq)
+{
+	if (!freq)
+		return;
+	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		remote_max_frequency_set(freq);
+		break;
+	default:
+		DEBUG_WARN("Setting max SWJ frequency not yet implemented\n");
+		break;
+	}
+}
+
+uint32_t platform_max_frequency_get(void)
+{
+	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		return remote_max_frequency_get();
+	default:
+		DEBUG_WARN("Reading max SWJ frequency not yet implemented\n");
+		break;
+	}
+	return false;
+}
+
 void platform_buffer_flush(void)
 {
 	switch (info.bmp_type) {
diff --git a/src/platforms/launchpad-icdi/platform.c b/src/platforms/launchpad-icdi/platform.c
index 5a78063c..b415336c 100644
--- a/src/platforms/launchpad-icdi/platform.c
+++ b/src/platforms/launchpad-icdi/platform.c
@@ -37,6 +37,8 @@ volatile platform_timeout * volatile head_timeout;
 uint8_t running_status;
 static volatile uint32_t time_ms;
 
+uint32_t swd_delay_cnt = 0;
+
 void sys_tick_handler(void)
 {
 	trace_tick();
@@ -140,3 +142,12 @@ void platform_request_boot(void)
 {
 }
 
+void platform_max_frequency_set(uint32_t freq)
+{
+	(void)freq;
+}
+
+uint32_t platform_max_frequency_get(void)
+{
+	return 0;
+}
diff --git a/src/platforms/launchpad-icdi/platform.h b/src/platforms/launchpad-icdi/platform.h
index 7ed96367..d86fcede 100644
--- a/src/platforms/launchpad-icdi/platform.h
+++ b/src/platforms/launchpad-icdi/platform.h
@@ -25,6 +25,7 @@
 #define PLATFORM_IDENT      "(Launchpad ICDI) "
 
 extern uint8_t running_status;
+extern uint32_t swd_delay_cnt;
 
 #define TMS_PORT	GPIOA_BASE
 #define TMS_PIN		GPIO3
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 6283d561..664d7eb8 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -138,6 +138,7 @@ static void cl_help(char **argv)
 	DEBUG_WARN("Run mode related options:\n");
 	DEBUG_WARN("\tDefault mode is to start the debug server at :2000\n");
 	DEBUG_WARN("\t-j\t\t: Use JTAG. SWD is default.\n");
+	DEBUG_WARN("\t-f\t\t: Set minimum high and low times of SWJ waveform.\n");
 	DEBUG_WARN("\t-C\t\t: Connect under reset\n");
 	DEBUG_WARN("\t-t\t\t: Scan SWD or JTAG and display information about \n"
 			   "\t\t\t  connected devices\n");
@@ -166,7 +167,8 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	opt->opt_target_dev = 1;
 	opt->opt_flash_size = 16 * 1024 *1024;
 	opt->opt_flash_start = 0xffffffff;
-	while((c = getopt(argc, argv, "eEhHv:d:s:I:c:CnltVtTa:S:jpP:rR")) != -1) {
+	opt->opt_max_swj_frequency = 4000000;
+	while((c = getopt(argc, argv, "eEhHv:d:f:s:I:c:CnltVtTa:S:jpP:rR")) != -1) {
 		switch(c) {
 		case 'c':
 			if (optarg)
@@ -200,6 +202,21 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			if (optarg)
 				opt->opt_device = optarg;
 			break;
+		case 'f':
+			if (optarg) {
+				char *p;
+				uint32_t frequency = strtol(optarg, &p, 10);
+				switch(*p) {
+				case 'k':
+					frequency *= 1000;
+					break;
+				case 'M':
+					frequency *= 1000*1000;
+					break;
+				}
+				opt->opt_max_swj_frequency = frequency;
+			}
+			break;
 		case 's':
 			if (optarg)
 				opt->opt_serial = optarg;
@@ -273,6 +290,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 		DEBUG_WARN("Ignoring filename in reset/test mode\n");
 		opt->opt_flash_file = NULL;
 	}
+	DEBUG_WARN("opt freq %" PRIu32 "\n", opt->opt_max_swj_frequency);
 }
 
 static void display_target(int i, target *t, void *context)
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index 6cff3b81..81aedc66 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -54,6 +54,7 @@ typedef struct BMP_CL_OPTIONS_s {
 	int opt_debuglevel;
 	int opt_target_dev;
 	uint32_t opt_flash_start;
+	uint32_t opt_max_swj_frequency;
 	size_t opt_flash_size;
 }BMP_CL_OPTIONS_t;
 
diff --git a/src/platforms/stm32/timing_stm32.c b/src/platforms/stm32/timing_stm32.c
index ca10de3f..dbf5cc3a 100644
--- a/src/platforms/stm32/timing_stm32.c
+++ b/src/platforms/stm32/timing_stm32.c
@@ -25,6 +25,7 @@
 
 uint8_t running_status;
 static volatile uint32_t time_ms;
+uint32_t swd_delay_cnt = 0;
 
 void platform_timing_init(void)
 {
@@ -60,3 +61,29 @@ uint32_t platform_time_ms(void)
 	return time_ms;
 }
 
+/* Assume some USED_SWD_CYCLES per clock
+ * and  CYCLES_PER_CNT Cycles per delay loop cnt with 2 delay loops per clock
+ */
+
+/* Values for STM32F103 at 72 MHz */
+#define USED_SWD_CYCLES 22
+#define CYCLES_PER_CNT 10
+void platform_max_frequency_set(uint32_t freq)
+{
+	int divisor = rcc_ahb_frequency - USED_SWD_CYCLES * freq;
+	if (divisor < 0) {
+		swd_delay_cnt = 0;
+		return;
+	}
+	divisor /= 2;
+	swd_delay_cnt = divisor/(CYCLES_PER_CNT * freq);
+	if ((swd_delay_cnt * (CYCLES_PER_CNT * freq)) < (unsigned int)divisor)
+		swd_delay_cnt++;
+}
+
+uint32_t platform_max_frequency_get(void)
+{
+	uint32_t ret = rcc_ahb_frequency;
+	ret /= USED_SWD_CYCLES + CYCLES_PER_CNT * swd_delay_cnt;
+	return ret;
+}
diff --git a/src/platforms/stm32/timing_stm32.h b/src/platforms/stm32/timing_stm32.h
index 76909019..9e5db18a 100644
--- a/src/platforms/stm32/timing_stm32.h
+++ b/src/platforms/stm32/timing_stm32.h
@@ -19,6 +19,7 @@
 #ifndef __TIMING_STM32_H
 #define __TIMING_STM32_H
 
+extern uint32_t swd_delay_cnt;
 extern uint8_t running_status;
 
 void platform_timing_init(void);
diff --git a/src/remote.c b/src/remote.c
index 10fc65ef..f24f8768 100644
--- a/src/remote.c
+++ b/src/remote.c
@@ -269,6 +269,7 @@ void remotePacketProcessGEN(uint8_t i, char *packet)
 
 {
 	(void)i;
+    uint32_t freq;
 	switch (packet[1]) {
     case REMOTE_VOLTAGE:
 		_respondS(REMOTE_RESP_OK,platform_target_voltage());
@@ -282,6 +283,14 @@ void remotePacketProcessGEN(uint8_t i, char *packet)
     case REMOTE_SRST_GET:
 		_respond(REMOTE_RESP_OK,platform_srst_get_val());
 		break;
+    case REMOTE_FREQ_SET:
+		platform_max_frequency_set( remotehston(8, packet + 2));
+		_respond(REMOTE_RESP_OK, 0);
+		break;
+    case REMOTE_FREQ_GET:
+		freq = platform_max_frequency_get();
+		_respond_buf(REMOTE_RESP_OK, (uint8_t*)&freq, 4);
+		break;
 
     case REMOTE_PWR_SET:
 #ifdef PLATFORM_HAS_POWER_SWITCH
diff --git a/src/remote.h b/src/remote.h
index 94eee009..65f3bb95 100644
--- a/src/remote.h
+++ b/src/remote.h
@@ -66,6 +66,8 @@
 #define REMOTE_TDITDO_TMS   'D'
 #define REMOTE_TDITDO_NOTMS 'd'
 #define REMOTE_IN_PAR       'I'
+#define REMOTE_FREQ_SET     'F'
+#define REMOTE_FREQ_GET     'f'
 #define REMOTE_IN           'i'
 #define REMOTE_NEXT         'N'
 #define REMOTE_OUT_PAR      'O'
@@ -106,6 +108,8 @@
 #define REMOTE_VOLTAGE_STR (char []){ REMOTE_SOM, REMOTE_GEN_PACKET, REMOTE_VOLTAGE, REMOTE_EOM, 0 }
 #define REMOTE_SRST_SET_STR (char []){ REMOTE_SOM, REMOTE_GEN_PACKET, REMOTE_SRST_SET, '%', 'c', REMOTE_EOM, 0 }
 #define REMOTE_SRST_GET_STR (char []){ REMOTE_SOM, REMOTE_GEN_PACKET, REMOTE_SRST_GET, REMOTE_EOM, 0 }
+#define REMOTE_FREQ_SET_STR (char []){ REMOTE_SOM, REMOTE_GEN_PACKET, REMOTE_FREQ_SET, '%', '0', '8', 'x', REMOTE_EOM, 0 }
+#define REMOTE_FREQ_GET_STR (char []){ REMOTE_SOM, REMOTE_GEN_PACKET, REMOTE_FREQ_GET, REMOTE_EOM, 0 }
 #define REMOTE_PWR_SET_STR (char []){ REMOTE_SOM, REMOTE_GEN_PACKET, REMOTE_PWR_SET, '%', 'c', REMOTE_EOM, 0 }
 #define REMOTE_PWR_GET_STR (char []){ REMOTE_SOM, REMOTE_GEN_PACKET, REMOTE_PWR_GET, REMOTE_EOM, 0 }
 

From 13e8a262e4b559e4697e9e470984d835b7ad5b3e Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Fri, 20 Nov 2020 13:10:54 +0100
Subject: [PATCH 3/7] SWJ Frequency: Implement for cmsis-dap

---
 src/platforms/hosted/cmsis_dap.c |  2 --
 src/platforms/hosted/cmsis_dap.h | 23 ++++++++++-------------
 src/platforms/hosted/dap.c       | 17 +++++++++++++----
 src/platforms/hosted/dap.h       |  1 -
 src/platforms/hosted/platform.c  |  6 ++++++
 src/platforms/pc/cl_utils.c      |  1 -
 6 files changed, 29 insertions(+), 21 deletions(-)

diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
index c1e9668f..06894174 100644
--- a/src/platforms/hosted/cmsis_dap.c
+++ b/src/platforms/hosted/cmsis_dap.c
@@ -271,7 +271,6 @@ int dap_enter_debug_swd(ADIv5_DP_t *dp)
 	if (!(dap_caps & DAP_CAP_SWD))
 		return -1;
 	mode =  DAP_CAP_SWD;
-	dap_swj_clock(2000000);
 	dap_transfer_configure(2, 128, 128);
 	dap_swd_configure(0);
 	dap_connect(false);
@@ -340,7 +339,6 @@ int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc)
 	mode =  DAP_CAP_JTAG;
 	dap_disconnect();
 	dap_connect(true);
-	dap_swj_clock(2000000);
 	jtag_proc->jtagtap_reset       = cmsis_dap_jtagtap_reset;
 	jtag_proc->jtagtap_next        = cmsis_dap_jtagtap_next;
 	jtag_proc->jtagtap_tms_seq     = cmsis_dap_jtagtap_tms_seq;
diff --git a/src/platforms/hosted/cmsis_dap.h b/src/platforms/hosted/cmsis_dap.h
index 6668ac3e..5451f8cf 100644
--- a/src/platforms/hosted/cmsis_dap.h
+++ b/src/platforms/hosted/cmsis_dap.h
@@ -29,6 +29,7 @@ void dap_exit_function(void);
 void dap_adiv5_dp_defaults(ADIv5_DP_t *dp);
 int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc);
 int dap_jtag_dp_init(ADIv5_DP_t *dp);
+uint32_t dap_swj_clock(uint32_t clock);
 #else
 int dap_init(bmp_info_t *info)
 {
@@ -36,19 +37,15 @@ int dap_init(bmp_info_t *info)
 	(void)info;
 	return -1;
 }
-int dap_enter_debug_swd(ADIv5_DP_t *dp) {(void)dp; return -1;}
-void dap_exit_function(void) {return;};
-void dap_adiv5_dp_defaults(ADIv5_DP_t *dp) {(void)dp; return; }
-int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc)
-{
-	(void)jtag_proc;
-	return -1;
-}
-int dap_jtag_dp_init(ADIv5_DP_t *dp)
-{
-	(void)dp;
-	return -1;
-}
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wunused-parameter"
+int dap_enter_debug_swd(ADIv5_DP_t *dp) {return -1;}
+uint32_t dap_swj_clock(uint32_t clock) {return 0;}
+void dap_exit_function(void) {};
+void dap_adiv5_dp_defaults(ADIv5_DP_t *dp) {};
+int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc) {return -1;}
+int dap_jtag_dp_init(ADIv5_DP_t *dp) {return -1;}
+# pragma GCC diagnostic pop
 
 #endif
 
diff --git a/src/platforms/hosted/dap.c b/src/platforms/hosted/dap.c
index 0f0efee8..6f9a23c1 100644
--- a/src/platforms/hosted/dap.c
+++ b/src/platforms/hosted/dap.c
@@ -203,18 +203,27 @@ void dap_disconnect(void)
 	dbg_dap_cmd(buf, sizeof(buf), 1);
 }
 
-//-----------------------------------------------------------------------------
-void dap_swj_clock(uint32_t clock)
+static uint32_t swj_clock;
+/* Set/Get JTAG/SWD clock frequency
+ *
+ * With clock == 0, return last set value.
+ */
+uint32_t dap_swj_clock(uint32_t clock)
 {
+	if (clock == 0)
+		return swj_clock;
 	uint8_t buf[5];
-
 	buf[0] = ID_DAP_SWJ_CLOCK;
 	buf[1] = clock & 0xff;
 	buf[2] = (clock >> 8) & 0xff;
 	buf[3] = (clock >> 16) & 0xff;
 	buf[4] = (clock >> 24) & 0xff;
 	dbg_dap_cmd(buf, sizeof(buf), 5);
-
+	if (buf[0])
+		DEBUG_WARN("dap_swj_clock failed\n");
+	else
+		swj_clock = clock;
+	return swj_clock;
 }
 
 //-----------------------------------------------------------------------------
diff --git a/src/platforms/hosted/dap.h b/src/platforms/hosted/dap.h
index 6a6fbb52..037e0f08 100644
--- a/src/platforms/hosted/dap.h
+++ b/src/platforms/hosted/dap.h
@@ -65,7 +65,6 @@ enum
 void dap_led(int index, int state);
 void dap_connect(bool jtag);
 void dap_disconnect(void);
-void dap_swj_clock(uint32_t clock);
 void dap_transfer_configure(uint8_t idle, uint16_t count, uint16_t retry);
 void dap_swd_configure(uint8_t cfg);
 int dap_info(int info, uint8_t *data, int size);
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 8ee10169..07aab72d 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -328,6 +328,9 @@ void platform_max_frequency_set(uint32_t freq)
 	case BMP_TYPE_BMP:
 		remote_max_frequency_set(freq);
 		break;
+	case BMP_TYPE_CMSIS_DAP:
+		dap_swj_clock(freq);
+		break;
 	default:
 		DEBUG_WARN("Setting max SWJ frequency not yet implemented\n");
 		break;
@@ -339,6 +342,9 @@ uint32_t platform_max_frequency_get(void)
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 		return remote_max_frequency_get();
+	case BMP_TYPE_CMSIS_DAP:
+		return dap_swj_clock(0);
+		break;
 	default:
 		DEBUG_WARN("Reading max SWJ frequency not yet implemented\n");
 		break;
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 664d7eb8..78ba5b52 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -290,7 +290,6 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 		DEBUG_WARN("Ignoring filename in reset/test mode\n");
 		opt->opt_flash_file = NULL;
 	}
-	DEBUG_WARN("opt freq %" PRIu32 "\n", opt->opt_max_swj_frequency);
 }
 
 static void display_target(int i, target *t, void *context)

From 28a966a3e66665eaea34b8fc8217a05cfbfcecce Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Fri, 20 Nov 2020 14:07:48 +0100
Subject: [PATCH 4/7] frequency: Implement for ftdi.

For all 2232H types, use undivided 60 Mhz.
FIXME: Check FT2322C/D for unsymetric clock at 6 MHz!
---
 src/platforms/hosted/ftdi_bmp.c | 43 +++++++++++++++++++++++++++++++++
 src/platforms/hosted/ftdi_bmp.h |  4 +++
 src/platforms/hosted/platform.c |  5 ++++
 3 files changed, 52 insertions(+)

diff --git a/src/platforms/hosted/ftdi_bmp.c b/src/platforms/hosted/ftdi_bmp.c
index d7ea3d7b..0d15113b 100644
--- a/src/platforms/hosted/ftdi_bmp.c
+++ b/src/platforms/hosted/ftdi_bmp.c
@@ -376,6 +376,18 @@ int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info)
 	}
 	int index = 0;
 	ftdi_init[index++]= LOOPBACK_END; /* FT2232D gets upset otherwise*/
+	switch(ftdic->type) {
+	case TYPE_2232H:
+	case TYPE_4232H:
+	case TYPE_232H:
+		ftdi_init[index++] = EN_DIV_5;
+		break;
+	case TYPE_2232C:
+		break;
+	default:
+		DEBUG_WARN("FTDI Chip has no MPSSE\n");
+		goto error_2;
+	}
 	ftdi_init[index++]= TCK_DIVISOR;
 	/* Use CLK/2 for about 50 % SWDCLK duty cycle on FT2232c.*/
 	ftdi_init[index++]= 1;
@@ -618,3 +630,34 @@ const char *libftdi_target_voltage(void)
 	}
 	return NULL;
 }
+
+static uint16_t divisor;
+void libftdi_max_frequency_set(uint32_t freq)
+{
+	uint32_t clock;
+	if (ftdic->type == TYPE_2232C)
+		clock = 12 * 1000 * 1000;
+	else
+		/* Undivided clock set during startup*/
+		clock = 60 * 1000 * 1000;
+	uint32_t div = (clock  + 2 * freq - 1)/ freq;
+	if ((div < 4) && (ftdic->type = TYPE_2232C))
+		div = 4; /* Avoid bad unsymetrict FT2232C clock at 6 MHz*/
+	divisor = div / 2 - 1;
+	uint8_t buf[3];
+	buf[0] = TCK_DIVISOR;
+	buf[1] = divisor & 0xff;
+	buf[2] = (divisor >> 8) & 0xff;
+	libftdi_buffer_write(buf, 3);
+}
+
+uint32_t libftdi_max_frequency_get(void)
+{
+	uint32_t clock;
+	if (ftdic->type == TYPE_2232C)
+		clock = 12 * 1000 * 1000;
+	else
+		/* Undivided clock set during startup*/
+		clock = 60 * 1000 * 1000;
+	return clock/ ( 2 *(divisor + 1));
+}
diff --git a/src/platforms/hosted/ftdi_bmp.h b/src/platforms/hosted/ftdi_bmp.h
index 12c90223..ffb9e866 100644
--- a/src/platforms/hosted/ftdi_bmp.h
+++ b/src/platforms/hosted/ftdi_bmp.h
@@ -119,6 +119,8 @@ const char *libftdi_target_voltage(void) {return "ERROR";};
 void libftdi_jtagtap_tdi_tdo_seq(
 	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks) {};
 bool  libftdi_swd_possible(bool *do_mpsse, bool *direct_bb_swd) {return false;};
+void libftdi_max_frequency_set(uint32_t freq) {};
+uint32_t libftdi_max_frequency_get(void) {return 0;};
 # pragma GCC diagnostic pop
 #else
 int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info);
@@ -131,6 +133,8 @@ const char *libftdi_target_voltage(void);
 void libftdi_jtagtap_tdi_tdo_seq(
 	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
 bool  libftdi_swd_possible(bool *do_mpsse, bool *direct_bb_swd);
+void libftdi_max_frequency_set(uint32_t freq);
+uint32_t libftdi_max_frequency_get(void);
 #endif
 
 #define MPSSE_SK 1
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 07aab72d..6578327f 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -331,6 +331,9 @@ void platform_max_frequency_set(uint32_t freq)
 	case BMP_TYPE_CMSIS_DAP:
 		dap_swj_clock(freq);
 		break;
+	case BMP_TYPE_LIBFTDI:
+		libftdi_max_frequency_set(freq);
+		break;
 	default:
 		DEBUG_WARN("Setting max SWJ frequency not yet implemented\n");
 		break;
@@ -345,6 +348,8 @@ uint32_t platform_max_frequency_get(void)
 	case BMP_TYPE_CMSIS_DAP:
 		return dap_swj_clock(0);
 		break;
+	case BMP_TYPE_LIBFTDI:
+		return libftdi_max_frequency_get();
 	default:
 		DEBUG_WARN("Reading max SWJ frequency not yet implemented\n");
 		break;

From de26ba6f73e733516c4e28844d4c6888d08693ce Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 29 Nov 2020 12:04:57 +0100
Subject: [PATCH 5/7] hosted: Add is_jtag to the info structure

Needed for different frequency setting procedures on some platforms.
---
 src/platforms/hosted/bmp_hosted.h | 1 +
 src/platforms/hosted/platform.c   | 2 ++
 2 files changed, 3 insertions(+)

diff --git a/src/platforms/hosted/bmp_hosted.h b/src/platforms/hosted/bmp_hosted.h
index 861971c4..721c648b 100644
--- a/src/platforms/hosted/bmp_hosted.h
+++ b/src/platforms/hosted/bmp_hosted.h
@@ -31,6 +31,7 @@ typedef struct bmp_info_s {
 	char manufacturer[128];
 	char product[128];
 	char version[128];
+	bool is_jtag;
 #if HOSTED_BMP_ONLY != 1
 	libusb_context *libusb_ctx;
 	struct ftdi_context *ftdic;
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 6578327f..9113d965 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -123,6 +123,7 @@ void platform_init(int argc, char **argv)
 
 int platform_adiv5_swdp_scan(void)
 {
+	info.is_jtag = false;
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
@@ -187,6 +188,7 @@ void platform_add_jtag_dev(int i, const jtag_dev_t *jtag_dev)
 
 int platform_jtag_scan(const uint8_t *lrlens)
 {
+	info.is_jtag = true;
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:

From fdc654cfb3d654879076e7ae759a9e2a7a13beaa Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 29 Nov 2020 12:00:50 +0100
Subject: [PATCH 6/7] stlink: Implementation of frequency setting.

---
 src/platforms/hosted/platform.c |   9 +-
 src/platforms/hosted/stlinkv2.c | 157 +++++++++++++++++++++-----------
 src/platforms/hosted/stlinkv2.h |   4 +
 3 files changed, 117 insertions(+), 53 deletions(-)

diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 9113d965..9b1638e7 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -109,8 +109,6 @@ void platform_init(int argc, char **argv)
 	default:
 		exit(-1);
 	}
-	if (cl_opts.opt_max_swj_frequency)
-		platform_max_frequency_set(cl_opts.opt_max_swj_frequency);
 	int ret = -1;
 	if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
 		ret = cl_execute(&cl_opts);
@@ -124,6 +122,7 @@ void platform_init(int argc, char **argv)
 int platform_adiv5_swdp_scan(void)
 {
 	info.is_jtag = false;
+	platform_max_frequency_set(cl_opts.opt_max_swj_frequency);
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
@@ -189,6 +188,7 @@ void platform_add_jtag_dev(int i, const jtag_dev_t *jtag_dev)
 int platform_jtag_scan(const uint8_t *lrlens)
 {
 	info.is_jtag = true;
+	platform_max_frequency_set(cl_opts.opt_max_swj_frequency);
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
@@ -336,6 +336,9 @@ void platform_max_frequency_set(uint32_t freq)
 	case BMP_TYPE_LIBFTDI:
 		libftdi_max_frequency_set(freq);
 		break;
+	case BMP_TYPE_STLINKV2:
+		stlink_max_frequency_set(&info, freq);
+		break;
 	default:
 		DEBUG_WARN("Setting max SWJ frequency not yet implemented\n");
 		break;
@@ -352,6 +355,8 @@ uint32_t platform_max_frequency_get(void)
 		break;
 	case BMP_TYPE_LIBFTDI:
 		return libftdi_max_frequency_get();
+	case BMP_TYPE_STLINKV2:
+		return stlink_max_frequency_get(&info);
 	default:
 		DEBUG_WARN("Reading max SWJ frequency not yet implemented\n");
 		break;
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 60213522..b22b58a0 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -200,7 +200,6 @@ typedef struct {
 	libusb_context* libusb_ctx;
 	uint16_t     vid;
 	uint16_t     pid;
-	uint8_t      transport_mode;
 	bool         srst;
 	uint8_t      dap_select;
 	uint8_t      ep_tx;
@@ -629,44 +628,6 @@ bool stlink_srst_get_val(void)
 	return Stlink.srst;
 }
 
-static bool stlink_set_freq_divisor(bmp_info_t *info, uint16_t divisor)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_APIV2_SWD_SET_FREQ,
-					  divisor & 0xff, divisor >> 8};
-	uint8_t data[2];
-	send_recv(info->usb_link, cmd, 16, data, 2);
-	if (stlink_usb_error_check(data, false))
-		return false;
-	return true;
-}
-
-static bool stlink3_set_freq_divisor(bmp_info_t *info, uint16_t divisor)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_APIV3_GET_COM_FREQ,
-					  Stlink.transport_mode};
-	uint8_t data[52];
-	send_recv(info->usb_link, cmd, 16, data, 52);
-	stlink_usb_error_check(data, true);
-	int size = data[8];
-	if (divisor > size)
-		divisor = size;
-	uint8_t *p = data + 12 + divisor * sizeof(uint32_t);
-	uint32_t freq = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
-	DEBUG_INFO("Selected %" PRId32 " khz\n", freq);
-	cmd[1] = STLINK_APIV3_SET_COM_FREQ;
-	cmd[2] = Stlink.transport_mode;
-	cmd[3] = 0;
-	p = data + 12 + divisor * sizeof(uint32_t);
-	cmd[4] = p[0];
-	cmd[5] = p[1];
-	cmd[6] = p[2];
-	cmd[7] = p[3];
-	send_recv(info->usb_link, cmd, 16, data, 8);
-	return true;
-}
-
 int stlink_hwversion(void)
 {
 	return Stlink.ver_stlink;
@@ -675,16 +636,10 @@ int stlink_hwversion(void)
 static int stlink_enter_debug_jtag(bmp_info_t *info)
 {
 	stlink_leave_state(info);
-	Stlink.transport_mode = STLINK_MODE_JTAG;
-	if (Stlink.ver_stlink == 3)
-		stlink3_set_freq_divisor(info, 4);
-	else
-		stlink_set_freq_divisor(info, 1);
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_DEBUG_APIV2_ENTER,
 					  STLINK_DEBUG_ENTER_JTAG_NO_RESET};
 	uint8_t data[2];
-	DEBUG_INFO("Enter JTAG\n");
 	send_recv(info->usb_link, cmd, 16, data, 2);
 	return stlink_usb_error_check(data, true);
 }
@@ -1085,16 +1040,10 @@ void stlink_adiv5_dp_defaults(ADIv5_DP_t *dp)
 int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
 {
 	stlink_leave_state(info);
-	Stlink.transport_mode = STLINK_MODE_SWD;
-	if (Stlink.ver_stlink == 3)
-		stlink3_set_freq_divisor(info, 2);
-	else
-		stlink_set_freq_divisor(info, 1);
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_DEBUG_APIV2_ENTER,
 					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
 	uint8_t data[2];
-	DEBUG_INFO("Enter SWD\n");
 	stlink_send_recv_retry(cmd, 16, data, 2);
 	if (stlink_usb_error_check(data, true))
 		return -1;
@@ -1107,3 +1056,109 @@ int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
 	stlink_dp_error(dp);
 	return 0;
 }
+
+#define V2_USED_SWD_CYCLES       20
+#define V2_CYCLES_PER_CNT        20
+#define V2_CLOCK_RATE      (72*1000*1000)
+/* Above values reproduce the known values for V2
+#include <stdio.h>
+
+int main(void)
+{
+    int divs[] = {0, 1,2,3,7,15,31,40,79,158,265,798};
+    for (int i = 0; i < (sizeof(divs) /sizeof(divs[0])); i++) {
+        float ret = 72.0 * 1000 * 1000 / (20 + 20 * divs[i]);
+        printf("%3d: %6.4f MHz\n", divs[i], ret/ 1000000);
+    }
+    return 0;
+}
+*/
+
+static int divisor;
+static unsigned int v3_freq[2];
+void stlink_max_frequency_set(bmp_info_t *info, uint32_t freq)
+{
+	if (Stlink.ver_hw == 30) {
+		uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+						   STLINK_APIV3_GET_COM_FREQ,
+						   info->is_jtag ? STLINK_MODE_JTAG : STLINK_MODE_SWD};
+		uint8_t data[52];
+		send_recv(info->usb_link, cmd, 16, data, 52);
+		stlink_usb_error_check(data, true);
+		volatile uint8_t *p = data + 12;
+		int i;
+		unsigned int last_freq = 0;
+		DEBUG_INFO("Available speed settings: ");
+		for (i = 0; i < STLINK_V3_MAX_FREQ_NB; i++) {
+			unsigned int new_freq = *p++;
+			new_freq |= *p++ <<  8;
+			new_freq |= *p++ << 16;
+			new_freq |= *p++ << 24;
+			if (!new_freq)
+				break;
+			else
+				last_freq = new_freq;
+			DEBUG_INFO("%s%d", (i)? "/": "", last_freq);
+			if ((freq / 1000) >= last_freq)
+				break;
+		}
+		DEBUG_INFO(" kHz for %s\n", (info->is_jtag) ? "JTAG" : "SWD");
+		cmd[1] = STLINK_APIV3_SET_COM_FREQ;
+		cmd[3] = 0;
+		cmd[4] = (last_freq >>  0) & 0xff;
+		cmd[5] = (last_freq >>  8) & 0xff;
+		cmd[6] = (last_freq >> 16) & 0xff;
+		cmd[7] = (last_freq >> 24) & 0xff;
+		send_recv(info->usb_link, cmd, 16, data, 8);
+		stlink_usb_error_check(data, true);
+		v3_freq[(info->is_jtag) ? 1 : 0] = last_freq * 1000;
+	} else {
+		uint8_t cmd[16];
+		cmd[0] = STLINK_DEBUG_COMMAND;
+		if (info->is_jtag) {
+			cmd[1] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
+			/*  V2_CLOCK_RATE / (4, 8, 16, ... 256)*/
+			int div  = (V2_CLOCK_RATE + (2 * freq) - 1) / (2 * freq);
+			if (div & (div -1)) {/* Round up */
+				int clz = __builtin_clz(div);
+				divisor = 1 << (32 - clz);
+			} else
+				divisor = div;
+			if (divisor < 4)
+				divisor = 4;
+			else if (divisor > 256)
+				divisor = 256;
+		} else {
+			cmd[1] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
+			divisor = V2_CLOCK_RATE + freq - 1;
+			divisor /= freq;
+			divisor -= V2_USED_SWD_CYCLES;
+			if (divisor < 0)
+				divisor = 0;
+			divisor /= V2_CYCLES_PER_CNT;
+		}
+		DEBUG_WARN("Divisor for %6.4f MHz is %" PRIu32 "\n",
+					   freq/1000000.0, divisor);
+		cmd[2] = divisor & 0xff;
+		cmd[3] = (divisor >> 8) & 0xff;
+		uint8_t data[2];
+		send_recv(info->usb_link, cmd, 16, data, 2);
+		if (stlink_usb_error_check(data, false))
+			DEBUG_WARN("Set frequency failed!\n");
+	}
+}
+
+uint32_t stlink_max_frequency_get(bmp_info_t *info)
+{
+	uint32_t ret = 0;
+	if (Stlink.ver_hw == 30) {
+		ret = v3_freq[(info->is_jtag) ? STLINK_MODE_JTAG : STLINK_MODE_SWD];
+	} else {
+		ret = V2_CLOCK_RATE;
+		if (info->is_jtag)
+			ret /= (2 * divisor);
+		else
+			ret /= (V2_USED_SWD_CYCLES + (V2_CYCLES_PER_CNT * divisor));
+	}
+	return ret;
+}
diff --git a/src/platforms/hosted/stlinkv2.h b/src/platforms/hosted/stlinkv2.h
index 6f639bc3..0d2a179c 100644
--- a/src/platforms/hosted/stlinkv2.h
+++ b/src/platforms/hosted/stlinkv2.h
@@ -37,6 +37,8 @@ void stlink_adiv5_dp_defaults(ADIv5_DP_t *dp) {};
 int stlink_jtag_dp_init(ADIv5_DP_t *dp) {return false;};
 int jtag_scan_stlinkv2(bmp_info_t *info, const uint8_t *irlens) {return 0;};
 void stlink_exit_function(bmp_info_t *info) {};
+void stlink_max_frequency_set(bmp_info_t *info, uint32_t freq) {};
+uint32_t stlink_max_frequency_get(bmp_info_t *info) {return 0;};
 # pragma GCC diagnostic pop
 #else
 int stlink_init(bmp_info_t *info);
@@ -49,5 +51,7 @@ void stlink_adiv5_dp_defaults(ADIv5_DP_t *dp);
 int stlink_jtag_dp_init(ADIv5_DP_t *dp);
 int jtag_scan_stlinkv2(bmp_info_t *info, const uint8_t *irlens);
 void stlink_exit_function(bmp_info_t *info);
+void stlink_max_frequency_set(bmp_info_t *info, uint32_t freq);
+uint32_t stlink_max_frequency_get(bmp_info_t *info);
 #endif
 #endif

From 6dbb5ff7ee3ccf6331a2e53e296cd569115fe4e5 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 28 Nov 2020 12:46:19 +0100
Subject: [PATCH 7/7] Frequency: Implementing for jlink.

SWD frequency is fixed for jlink according to the docs from 2010.
---
 src/platforms/hosted/jlink.c    | 51 ++++++++++++++++++++++++++-------
 src/platforms/hosted/jlink.h    | 47 ++++++++++++++++++++++++++++--
 src/platforms/hosted/platform.c | 13 +++++++++
 3 files changed, 99 insertions(+), 12 deletions(-)

diff --git a/src/platforms/hosted/jlink.c b/src/platforms/hosted/jlink.c
index caf8bf90..d1d1220f 100644
--- a/src/platforms/hosted/jlink.c
+++ b/src/platforms/hosted/jlink.c
@@ -40,14 +40,19 @@
 #define USB_VID_SEGGER_0105  0x0105
 #define USB_VID_SEGGER_1020  0x1020
 
+static uint32_t emu_caps;
+static uint32_t emu_speed_kHz;
+static uint16_t emu_min_divisor;
+static uint16_t emu_current_divisor;
+
 static void jlink_print_caps(bmp_info_t *info)
 {
 	uint8_t cmd[1] = {CMD_GET_CAPS};
 	uint8_t res[4];
 	send_recv(info->usb_link, cmd, 1, res, sizeof(res));
-	uint32_t caps = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
-	DEBUG_INFO("Caps %" PRIx32 "\n", caps);
-	if (caps & JLINK_CAP_GET_HW_VERSION) {
+	emu_caps = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
+	DEBUG_INFO("Caps %" PRIx32 "\n", emu_caps);
+	if (emu_caps & JLINK_CAP_GET_HW_VERSION) {
 		uint8_t cmd[1] = {CMD_GET_HW_VERSION};
 		send_recv(info->usb_link, cmd, 1, NULL, 0);
 		send_recv(info->usb_link, NULL, 0, res, sizeof(res));
@@ -57,13 +62,15 @@ static void jlink_print_caps(bmp_info_t *info)
 }
 static void jlink_print_speed(bmp_info_t *info)
 {
-	uint8_t cmd[1] = {CMD_GET_SPEED};
+	uint8_t cmd[1] = {CMD_GET_SPEEDS};
 	uint8_t res[6];
 	send_recv(info->usb_link, cmd, 1, res, sizeof(res));
-	uint32_t speed = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
-	double freq_mhz = speed / 1000000.0;
-	uint16_t divisor = res[4] | (res[5] << 8);
-	DEBUG_INFO("Emulator speed %3.1f MHz, Mindiv %d\n", freq_mhz, divisor);
+	emu_speed_kHz = (res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24)) /
+					 1000;
+	emu_min_divisor = res[4] | (res[5] << 8);
+	DEBUG_INFO("Emulator speed %d kHz, Mindiv %d%s\n", emu_speed_kHz,
+			   emu_min_divisor,
+			   (emu_caps & JLINK_CAP_GET_SPEEDS) ? "" : ", fixed");
 }
 
 static void jlink_print_version(bmp_info_t *info)
@@ -98,8 +105,8 @@ static void jlink_print_interfaces(bmp_info_t *info)
 static void jlink_info(bmp_info_t *info)
 {
 	jlink_print_version(info);
-	jlink_print_speed(info);
 	jlink_print_caps(info);
+	jlink_print_speed(info);
 	jlink_print_interfaces(info);
 }
 
@@ -214,7 +221,7 @@ int jlink_init(bmp_info_t *info)
 const char *jlink_target_voltage(bmp_info_t *info)
 {
         uint8_t cmd[1] = {CMD_GET_HW_STATUS};
-        uint8_t res[8];
+		uint8_t res[8];
         send_recv(info->usb_link, cmd, 1, res, sizeof(res));
         uint16_t mVolt = res[0] | (res[1] << 8);
         static char ret[7];
@@ -238,3 +245,27 @@ bool jlink_srst_get_val(bmp_info_t *info) {
         send_recv(info->usb_link, cmd, 1, res, sizeof(res));
         return !(res[6]);
 }
+
+void jlink_max_frequency_set(bmp_info_t *info, uint32_t freq)
+{
+	if (!(emu_caps &  JLINK_CAP_GET_SPEEDS))
+		return;
+	if (!info->is_jtag)
+		return;
+	uint16_t freq_kHz = freq /1000;
+	uint16_t divisor = (emu_speed_kHz + freq_kHz - 1) / freq_kHz;
+	if (divisor < emu_min_divisor)
+		divisor = emu_min_divisor;
+	emu_current_divisor = divisor;
+	uint16_t speed_kHz = emu_speed_kHz / divisor;
+	uint8_t cmd[3] = {CMD_SET_SPEED, speed_kHz & 0xff, speed_kHz >> 8};
+	DEBUG_WARN("Set Speed %d\n", speed_kHz);
+	send_recv(info->usb_link, cmd, 3, NULL, 0);
+}
+
+uint32_t jlink_max_frequency_get(bmp_info_t *info)
+{
+	if ((emu_caps & JLINK_CAP_GET_SPEEDS) && (info->is_jtag))
+		return (emu_speed_kHz * 1000L)/ emu_current_divisor;
+	return FREQ_FIXED;
+}
diff --git a/src/platforms/hosted/jlink.h b/src/platforms/hosted/jlink.h
index c79b8683..5504c67e 100644
--- a/src/platforms/hosted/jlink.h
+++ b/src/platforms/hosted/jlink.h
@@ -24,8 +24,9 @@
 
 /** @cond PRIVATE */
 #define CMD_GET_VERSION         0x01
+#define CMD_SET_SPEED           0x05
 #define CMD_GET_HW_STATUS       0x07
-#define CMD_GET_SPEED           0xc0
+#define CMD_GET_SPEEDS          0xc0
 #define CMD_GET_SELECT_IF       0xc7
 #define CMD_HW_JTAG3            0xcf
 #define CMD_HW_RESET0           0xdc
@@ -37,7 +38,8 @@
 #define JLINK_IF_GET_ACTIVE    0xfe
 #define JLINK_IF_GET_AVAILABLE 0xff
 
-#define JLINK_CAP_GET_HW_VERSION  2
+#define JLINK_CAP_GET_SPEEDS     (1 <<  9)
+#define JLINK_CAP_GET_HW_VERSION (1 <<  1)
 #define JLINK_IF_JTAG             1
 #define JLINK_IF_SWD              2
 
@@ -53,13 +55,54 @@ int jlink_jtagtap_init(bmp_info_t *info, jtag_proc_t *jtag_proc) {return 0;};
 const char *jlink_target_voltage(bmp_info_t *info) {return "ERROR";};
 void jlink_srst_set_val(bmp_info_t *info, bool assert) {};
 bool jlink_srst_get_val(bmp_info_t *info) {return true;};
+void jlink_max_frequency_set(bmp_info_t *info, uint32_t freq) {};
+uint32_t jlink_max_frequency_get(bmp_info_t *info) {return 0;};
 # pragma GCC diagnostic pop
 #else
+/** Device capabilities. (from openocd*/
+enum jaylink_device_capability {
+        /** Device supports retrieval of the hardware version. */
+        JAYLINK_DEV_CAP_GET_HW_VERSION = 1,
+        /** Device supports adaptive clocking. */
+        JAYLINK_DEV_CAP_ADAPTIVE_CLOCKING = 3,
+        /** Device supports reading configuration data. */
+        JAYLINK_DEV_CAP_READ_CONFIG = 4,
+        /** Device supports writing configuration data. */
+        JAYLINK_DEV_CAP_WRITE_CONFIG = 5,
+        /** Device supports retrieval of target interface speeds. */
+        JAYLINK_DEV_CAP_GET_SPEEDS = 9,
+        /** Device supports retrieval of free memory size. */
+        JAYLINK_DEV_CAP_GET_FREE_MEMORY = 11,
+        /** Device supports retrieval of hardware information. */
+        JAYLINK_DEV_CAP_GET_HW_INFO = 12,
+        /** Device supports the setting of the target power supply. */
+        JAYLINK_DEV_CAP_SET_TARGET_POWER = 13,
+        /** Device supports target interface selection. */
+        JAYLINK_DEV_CAP_SELECT_TIF = 17,
+        /** Device supports retrieval of counter values. */
+        JAYLINK_DEV_CAP_GET_COUNTERS = 19,
+        /** Device supports capturing of SWO trace data. */
+        JAYLINK_DEV_CAP_SWO = 23,
+        /** Device supports file I/O operations. */
+        JAYLINK_DEV_CAP_FILE_IO = 26,
+        /** Device supports registration of connections. */
+        JAYLINK_DEV_CAP_REGISTER = 27,
+        /** Device supports retrieval of extended capabilities. */
+        JAYLINK_DEV_CAP_GET_EXT_CAPS = 31,
+        /** Device supports EMUCOM. */
+        JAYLINK_DEV_CAP_EMUCOM = 33,
+        /** Device supports ethernet connectivity. */
+        JAYLINK_DEV_CAP_ETHERNET = 38
+};
+
+
 int jlink_init(bmp_info_t *info);
 int jlink_swdp_scan(bmp_info_t *info);
 int jlink_jtagtap_init(bmp_info_t *info, jtag_proc_t *jtag_proc);
 const char *jlink_target_voltage(bmp_info_t *info);
 void jlink_srst_set_val(bmp_info_t *info, bool assert);
 bool jlink_srst_get_val(bmp_info_t *info);
+void jlink_max_frequency_set(bmp_info_t *info, uint32_t freq);
+uint32_t jlink_max_frequency_get(bmp_info_t *info);
 #endif
 #endif
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 9b1638e7..5e55603d 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -339,10 +339,21 @@ void platform_max_frequency_set(uint32_t freq)
 	case BMP_TYPE_STLINKV2:
 		stlink_max_frequency_set(&info, freq);
 		break;
+	case BMP_TYPE_JLINK:
+		jlink_max_frequency_set(&info, freq);
+		break;
 	default:
 		DEBUG_WARN("Setting max SWJ frequency not yet implemented\n");
 		break;
 	}
+	uint32_t max_freq = platform_max_frequency_get();
+	if (max_freq == FREQ_FIXED)
+		DEBUG_INFO("Device has fixed frequency for %s\n",
+				   (info.is_jtag) ? "JTAG" : "SWD" );
+	else
+		DEBUG_INFO("Speed set to %7.4f MHz for %s\n",
+				   platform_max_frequency_get() / 1000000.0,
+				   (info.is_jtag) ? "JTAG" : "SWD" );
 }
 
 uint32_t platform_max_frequency_get(void)
@@ -357,6 +368,8 @@ uint32_t platform_max_frequency_get(void)
 		return libftdi_max_frequency_get();
 	case BMP_TYPE_STLINKV2:
 		return stlink_max_frequency_get(&info);
+	case BMP_TYPE_JLINK:
+		return jlink_max_frequency_get(&info);
 	default:
 		DEBUG_WARN("Reading max SWJ frequency not yet implemented\n");
 		break;