Merge commit 'aa0d8f4b5de13f4fb7324a690a4bb7aa411e767f' into sam-update

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;

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;

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;}
+# pragma GCC diagnostic push
-void dap_exit_function(void) {return;};
+# pragma GCC diagnostic ignored "-Wunused-parameter"
-void dap_adiv5_dp_defaults(ADIv5_DP_t *dp) {(void)dp; return; }
+int dap_enter_debug_swd(ADIv5_DP_t *dp) {return -1;}
-int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc)
+uint32_t dap_swj_clock(uint32_t clock) {return 0;}
-{
+void dap_exit_function(void) {};
-	(void)jtag_proc;
+void dap_adiv5_dp_defaults(ADIv5_DP_t *dp) {};
-	return -1;
+int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc) {return -1;}
-}
+int dap_jtag_dp_init(ADIv5_DP_t *dp) {return -1;}
-int dap_jtag_dp_init(ADIv5_DP_t *dp)
+# pragma GCC diagnostic pop
-{
-	(void)dp;
-	return -1;
-}
 
 #endif
 

src/platforms/hosted/dap.c

@@ -203,18 +203,27 @@ void dap_disconnect(void)
 	dbg_dap_cmd(buf, sizeof(buf), 1);
 }
 
-//-----------------------------------------------------------------------------
+static uint32_t swj_clock;
-void dap_swj_clock(uint32_t 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;
 }
 
 //-----------------------------------------------------------------------------

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);

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));
+}

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

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);
+	emu_caps = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
-	DEBUG_INFO("Caps %" PRIx32 "\n", caps);
+	DEBUG_INFO("Caps %" PRIx32 "\n", emu_caps);
-	if (caps & JLINK_CAP_GET_HW_VERSION) {
+	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);
+	emu_speed_kHz = (res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24)) /
-	double freq_mhz = speed / 1000000.0;
+					 1000;
-	uint16_t divisor = res[4] | (res[5] << 8);
+	emu_min_divisor = res[4] | (res[5] << 8);
-	DEBUG_INFO("Emulator speed %3.1f MHz, Mindiv %d\n", freq_mhz, divisor);
+	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;
+}

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

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);
@@ -123,6 +121,8 @@ 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:
@@ -187,6 +187,8 @@ 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:
@@ -328,10 +330,30 @@ 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;
+	case BMP_TYPE_LIBFTDI:
+		libftdi_max_frequency_set(freq);
+		break;
+	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)
@@ -339,6 +361,15 @@ 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;
+	case BMP_TYPE_LIBFTDI:
+		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;

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;
+}

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

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)

src/target/cortexm.c

@@ -445,8 +445,16 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 			PROBE(lpc11xx_probe); /* LPC24C11 */
 			PROBE(lpc43xx_probe);
 		} else if (ap->ap_partno == 0x4c4) { /* Cortex-M4 ROM */
-			PROBE(lpc43xx_probe);
+			/* The LPC546xx and LPC43xx parts present with the same AP ROM Part
+			Number, so we need to probe both. Unfortunately, when probing for
+			the LPC43xx when the target is actually an LPC546xx, the memory
+			location checked is illegal for the LPC546xx and puts the chip into
+			Lockup, requiring a RST pulse to recover. Instead, make sure to
+			probe for the LPC546xx first, which experimentally doesn't harm
+			LPC43xx detection. */
 			PROBE(lpc546xx_probe);
+
+			PROBE(lpc43xx_probe);
 			PROBE(kinetis_probe); /* Older K-series */
 		} else if (ap->ap_partno == 0x4cb) { /* Cortex-M23 ROM */
 			PROBE(gd32f1_probe); /* GD32E23x uses GD32F1 peripherals */

src/target/lpc546xx.c

@@ -27,36 +27,53 @@
 
 #define LPC546XX_CHIPID 0x40000FF8
 
-#define IAP_ENTRYPOINT_LOCATION	0x03000204
+#define IAP_ENTRYPOINT_LOCATION 0x03000204
 
 #define LPC546XX_ETBAHB_SRAM_BASE 0x20000000
-#define LPC546XX_ETBAHB_SRAM_SIZE (160*1024)
+
+/* only SRAM0 bank is enabled after reset */
+#define LPC546XX_ETBAHB_SRAM_SIZE (64 * 1024)
 
 #define LPC546XX_WDT_MODE 0x4000C000
-#define LPC546XX_WDT_CNT  0x4000C004
+#define LPC546XX_WDT_CNT 0x4000C004
 #define LPC546XX_WDT_FEED 0x4000C008
 #define LPC546XX_WDT_PERIOD_MAX 0xFFFFFF
 #define LPC546XX_WDT_PROTECT (1 << 4)
 
-#define IAP_RAM_SIZE	LPC546XX_ETBAHB_SRAM_SIZE
+#define IAP_RAM_SIZE LPC546XX_ETBAHB_SRAM_SIZE
-#define IAP_RAM_BASE	LPC546XX_ETBAHB_SRAM_BASE
+#define IAP_RAM_BASE LPC546XX_ETBAHB_SRAM_BASE
 
-#define IAP_PGM_CHUNKSIZE	4096
+#define IAP_PGM_CHUNKSIZE 4096
 
-#define FLASH_NUM_SECTOR	15
+static bool lpc546xx_cmd_erase_mass(target *t, int argc, const char *argv[]);
-
+static bool lpc546xx_cmd_erase_sector(target *t, int argc, const char *argv[]);
-static bool lpc546xx_cmd_erase(target *t, int argc, const char *argv[]);
+static bool lpc546xx_cmd_read_partid(target *t, int argc, const char *argv[]);
+static bool lpc546xx_cmd_read_uid(target *t, int argc, const char *argv[]);
+static bool lpc546xx_cmd_reset_attach(target *t, int argc, const char *argv[]);
 static bool lpc546xx_cmd_reset(target *t, int argc, const char *argv[]);
+static bool lpc546xx_cmd_write_sector(target *t, int argc, const char *argv[]);
+
+static void lpc546xx_reset_attach(target *t);
 static int lpc546xx_flash_init(target *t);
 static int lpc546xx_flash_erase(struct target_flash *f, target_addr addr, size_t len);
-static void lpc546xx_set_internal_clock(target *t);
 static void lpc546xx_wdt_set_period(target *t);
 static void lpc546xx_wdt_pet(target *t);
 
 const struct command_s lpc546xx_cmd_list[] = {
-	{"erase_mass", lpc546xx_cmd_erase, "Erase entire flash memory"},
+	{ "erase_mass", lpc546xx_cmd_erase_mass, "Erase entire flash memory" },
-	{"reset", lpc546xx_cmd_reset, "Reset target"},
+	{ "erase_sector", lpc546xx_cmd_erase_sector,
-	{NULL, NULL, NULL}
+	  "Erase a sector by number" },
+	{ "read_partid", lpc546xx_cmd_read_partid,
+	  "Read out the 32-bit part ID using IAP." },
+	{ "read_uid", lpc546xx_cmd_read_uid, "Read out the 16-byte UID." },
+	{ "reset_attach", lpc546xx_cmd_reset_attach,
+	  "Reset target. Reset debug registers. Re-attach debugger. This restores "
+	  "the chip to the very start of program execution, after the ROM "
+	  "bootloader." },
+	{ "reset", lpc546xx_cmd_reset, "Reset target" },
+	{ "write_sector", lpc546xx_cmd_write_sector,
+	  "Write incrementing data 8-bit values across a previously erased sector" },
+	{ NULL, NULL, NULL }
 };
 
 void lpc546xx_add_flash(target *t, uint32_t iap_entry,
@@ -65,6 +82,11 @@ void lpc546xx_add_flash(target *t, uint32_t iap_entry,
 {
 	struct lpc_flash *lf = lpc_add_flash(t, addr, len);
 	lf->f.erase = lpc546xx_flash_erase;
+
+	/* LPC546xx devices require the checksum value written into the vector table
+	in sector 0 */
+	lf->f.write = lpc_flash_write_magic_vect;
+
 	lf->f.blocksize = erasesize;
 	lf->f.buf_size = IAP_PGM_CHUNKSIZE;
 	lf->bank = 0;
@@ -78,7 +100,6 @@ void lpc546xx_add_flash(target *t, uint32_t iap_entry,
 bool lpc546xx_probe(target *t)
 {
 	uint32_t chipid;
-	uint32_t iap_entry;
 	uint32_t flash_size;
 
 	chipid = target_mem_read32(t, LPC546XX_CHIPID);
@@ -132,10 +153,12 @@ bool lpc546xx_probe(target *t)
 		return false;
 	}
 
-	iap_entry = target_mem_read32(t,
+	lpc546xx_add_flash(t, IAP_ENTRYPOINT_LOCATION, 0, 0x0, flash_size,
-				      IAP_ENTRYPOINT_LOCATION);
+			   0x8000);
-	lpc546xx_add_flash(t, iap_entry, 0, 0x0,
+
-			   flash_size, 0x8000);
+	/* Note: upper 96kB is only usable after enabling the appropriate control
+	register bits, see LPC546xx User Manual: 7.5.19 AHB Clock Control register 0
+	*/
 	target_add_ram(t, 0x20000000, 0x28000);
 	target_add_commands(t, lpc546xx_cmd_list, "Lpc546xx");
 	t->target_options |= CORTEXM_TOPT_INHIBIT_SRST;
@@ -143,7 +166,88 @@ bool lpc546xx_probe(target *t)
 	return true;
 }
 
-/* Reset all major systems _except_ debug */
+static void lpc546xx_reset_attach(target *t)
+{
+	/* To reset the LPC546xx into a usable state, we need to reset and let it
+	step once, then attach the debug probe again. Otherwise the ROM bootloader
+	is mapped to address 0x0, we can't perform flash operations on sector 0,
+	and reading memory from sector 0 will return the contents of the ROM
+	bootloader, not the flash */
+	target_reset(t);
+	target_halt_resume(t, false);
+	cortexm_attach(t);
+}
+
+static bool lpc546xx_cmd_erase_mass(target *t, int argc, const char *argv[])
+{
+	(void)argc;
+	(void)argv;
+
+	int result = lpc546xx_flash_erase(t->flash, t->flash->start,
+					  t->flash->length);
+
+	if (result != 0) {
+		tc_printf(t, "Error erasing flash: %d\n", result);
+		return false;
+	}
+
+	tc_printf(t, "Erase OK.\n");
+
+	return true;
+}
+
+static bool lpc546xx_cmd_erase_sector(target *t, int argc, const char *argv[])
+{
+	if (argc > 1) {
+		uint32_t sector_addr = strtoul(argv[1], NULL, 0);
+		sector_addr *= t->flash->blocksize;
+		int retval = lpc546xx_flash_erase(t->flash, sector_addr, 1);
+		return retval == 0;
+	}
+	return -1;
+}
+
+static bool lpc546xx_cmd_read_partid(target *t, int argc, const char *argv[])
+{
+	(void)argc;
+	(void)argv;
+	struct lpc_flash *f = (struct lpc_flash *)t->flash;
+	uint32_t partid[4];
+	if (lpc_iap_call(f, partid, IAP_CMD_PARTID))
+		return false;
+	tc_printf(t, "PART ID: 0x%08x\n", partid[0]);
+	return true;
+}
+
+static bool lpc546xx_cmd_read_uid(target *t, int argc, const char *argv[])
+{
+	(void)argc;
+	(void)argv;
+	struct lpc_flash *f = (struct lpc_flash *)t->flash;
+	uint8_t uid[16];
+	if (lpc_iap_call(f, uid, IAP_CMD_READUID))
+		return false;
+	tc_printf(t, "UID: 0x");
+	for (uint32_t i = 0; i < sizeof(uid); ++i)
+		tc_printf(t, "%02x", uid[i]);
+	tc_printf(t, "\n");
+	return true;
+}
+
+/* Reset everything, including debug; single step past the ROM bootloader so the
+system is in a sane state */
+static bool lpc546xx_cmd_reset_attach(target *t, int argc, const char *argv[])
+{
+	(void)argc;
+	(void)argv;
+
+	lpc546xx_reset_attach(t);
+
+	return true;
+}
+
+/* Reset all major systems _except_ debug. Note that this will leave the system
+with the ROM bootloader mapped to 0x0 */
 static bool lpc546xx_cmd_reset(target *t, int argc, const char *argv[])
 {
 	(void)argc;
@@ -160,38 +264,44 @@ static bool lpc546xx_cmd_reset(target *t, int argc, const char *argv[])
 	return true;
 }
 
-static bool lpc546xx_cmd_erase(target *t, int argc, const char *argv[])
+static bool lpc546xx_cmd_write_sector(target *t, int argc, const char *argv[])
 {
-	(void)argc;
+	if (argc > 1) {
-	(void)argv;
+		const uint32_t sector_size = t->flash->blocksize;
+		uint32_t sector_addr = strtoul(argv[1], NULL, 0);
+		sector_addr *= sector_size;
 
-	lpc546xx_flash_init(t);
+		int retval = lpc546xx_flash_erase(t->flash, sector_addr, 1);
-	struct lpc_flash *f = (struct lpc_flash *)t->flash;
+		if (retval != 0) {
+			return retval;
+		}
 
-	if (lpc_iap_call(f, NULL, IAP_CMD_PREPARE, 0, FLASH_NUM_SECTOR-1))
+		uint8_t *buf = calloc(1, sector_size);
-		return false;
+		for (uint32_t i = 0; i < sector_size; i++) {
+			buf[i] = i & 0xff;
+		}
 
-	if (lpc_iap_call(f, NULL, IAP_CMD_ERASE, 0, FLASH_NUM_SECTOR-1, CPU_CLK_KHZ))
+		retval = lpc_flash_write_magic_vect(t->flash, sector_addr, buf,
-		return false;
+						    sector_size);
 
-	tc_printf(t, "Erase OK.\n");
+		free(buf);
 
-	return true;
+		return retval == 0;
+	}
+	return -1;
 }
 
 static int lpc546xx_flash_init(target *t)
 {
+	/* Reset the chip. It's unfortunate but we need to make sure the ROM
+	bootloader is no longer mapped to 0x0 or flash blank check won't work after
+	erasing that sector. Resetting will also set the main clock back to default
+	12MHZ FRO; that value is required for some IAP routines. */
+	lpc546xx_reset_attach(t);
+
 	/* Deal with WDT */
 	lpc546xx_wdt_set_period(t);
 
-	/* /\* Force internal clock *\/ */
-	lpc546xx_set_internal_clock(t);
-
-	/* Initialize flash IAP */
-	struct lpc_flash *f = (struct lpc_flash *)t->flash;
-	if (lpc_iap_call(f, NULL, IAP_CMD_INIT))
-		return -1;
-
 	return 0;
 }
 
@@ -203,12 +313,6 @@ static int lpc546xx_flash_erase(struct target_flash *tf, target_addr addr, size_
 	return lpc_flash_erase(tf, addr, len);
 }
 
-static void lpc546xx_set_internal_clock(target *t)
-{
-	/* Switch to 12 Mhz FRO */
-	target_mem_write32(t, 0x40000000 + 0x248, 0);
-}
-
 static void lpc546xx_wdt_set_period(target *t)
 {
 	/* Check if WDT is on */

src/target/lpc_common.c

@@ -109,6 +109,11 @@ enum iap_status lpc_iap_call(struct lpc_flash *f, void *result, enum iap_cmd cmd
 	if (result != NULL)
 		memcpy(result, param.result, sizeof(param.result));
 
+	if (param.status != IAP_STATUS_CMD_SUCCESS) {
+		DEBUG_WARN("IAP failure code %d for cmd %d\n",
+			   (enum iap_status)param.status, cmd);
+	}
+
 	return param.status;
 }
 
@@ -165,8 +170,10 @@ int lpc_flash_write_magic_vect(struct target_flash *f,
 		uint32_t *w = (uint32_t *)src;
 		uint32_t sum = 0;
 
+		/* compute checksum of first 7 vectors */
 		for (unsigned i = 0; i < 7; i++)
 			sum += w[i];
+		/* two's complement is written to 8'th vector */
 		w[7] = ~sum + 1;
 	}
 	return lpc_flash_write(f, dest, src, len);