platforms/hosted/jlink: rewrite frequency commands again

This time properly taking into account how the get and set speeds interact with the selected interface

src/platforms/hosted/jlink.c

@@ -42,6 +42,19 @@
 
 #include "cli.h"
 
+typedef struct jlink {
+	char fw_version[256U];
+	uint32_t hw_version;
+	uint32_t capabilities;
+	uint32_t available_interfaces;
+
+	struct jlink_interface_speed {
+		uint32_t base_frequency;
+		uint16_t min_divisor;
+		uint16_t current_divisor;
+	} interface_speed[JLINK_SELECT_IF_MAX_IF];
+} jlink_s;
+
 jlink_s jlink;
 
 /* J-Link protocol functions */
@@ -363,53 +376,116 @@ static bool jlink_get_interfaces(void)
 	return true;
 }
 
-static bool jlink_get_speeds(void)
+static bool jlink_get_interface_speed(const uint8_t interface)
 {
-	/* 
-	 * Fixme: this is not strictly correct, the reported base frequency and min divisor depend on the
-	 * interface selected, but we don't know which one is selected at this point, we are assuming JTAG
-	 * and that JTAG is the only interface that supports speed info/set.
-	 * Unfortunately, this does not look like it's documented on the docs we have access to.
-	*/
-
-	if (jlink.capabilities & JLINK_CAP_SPEED_INFO) {
-		uint8_t buffer[6];
-		if (!jlink_simple_query(JLINK_CMD_GET_SPEEDS, buffer, sizeof(buffer)))
+	if (!(jlink.capabilities & JLINK_CAP_SPEED_INFO)) {
+		DEBUG_WARN("J-Link does not support speed info command\n");
+		return false;
+	}
+
+	/* If the requested interface is invalid, bail out */
+	if (!jlink_interface_available(interface))
+		return false;
+
+	/* If the base frequency is non-zero, we've already read the speed info for the requested interface and can skip the query */
+	if (jlink.interface_speed[interface].base_frequency != 0U)
+		return true;
+
+	/* Get the selected interface */
+	const uint8_t selected_interface = jlink_selected_interface();
+
+	if (selected_interface != interface) {
+		/* If the selected interface doesn't match the requested interface, select it, let's hope this doesn't mess something up elsewhere */
+		DEBUG_WARN("Trying to get speed for interface %s but it is not selected, selecting it\n",
+			jlink_interface_to_string(interface));
+
+		/* select the requested interface */
+		if (!jlink_select_interface(interface))
 			return false;
+	}
 
-		jlink.base_frequency = read_le4(buffer, JLINK_GET_SPEEDS_BASE_FREQ_OFFSET);
-		jlink.min_divisor = read_le2(buffer, JLINK_GET_SPEEDS_MIN_DIV_OFFSET);
-		jlink.current_divisor = jlink.min_divisor;
+	/* Get the speed info for the selected interface */
+	uint8_t buffer[6];
+	if (!jlink_simple_query(JLINK_CMD_GET_SPEEDS, buffer, sizeof(buffer)))
+		return false;
 
-		DEBUG_INFO("Base frequency: %uHz\n\tMinimum divisor: %u\n", jlink.base_frequency, jlink.min_divisor);
-	} else
-		DEBUG_WARN("J-Link does not support speed info command\n");
+	struct jlink_interface_speed *const interface_speed = &jlink.interface_speed[interface];
+
+	interface_speed->base_frequency = read_le4(buffer, JLINK_GET_SPEEDS_BASE_FREQ_OFFSET);
+	interface_speed->min_divisor = read_le2(buffer, JLINK_GET_SPEEDS_MIN_DIV_OFFSET);
+
+	/* This is an assumption, if the J-Link was configured before we started, this may not be true, but we have no way to know */
+	interface_speed->current_divisor = interface_speed->min_divisor;
+
+	DEBUG_INFO("%s interface speed:\n\tBase frequency: %uHz\n\tMinimum divisor: %u\n",
+		jlink_interface_to_string(selected_interface), interface_speed->base_frequency, interface_speed->min_divisor);
+
+	// Restoring the selected interface seemed to cause issues sometimes (unsure if culprit)
+	// so we don't do it for now, it didn't seem to be necessary anyway
+	// if (selected_interface != interface) {
+	// 	/* If the selected interface didn't match the requested interface, restore the selected interface */
+	// 	DEBUG_WARN("Restoring selected interface to %s\n", jlink_interface_to_string(selected_interface));
+
+	// 	/* select the requested interface, we don't consider this a critical failure, as we got what we wanted */
+	// 	if (!jlink_select_interface(selected_interface))
+	// 		DEBUG_ERROR("Failed to restore selected interface to %s\n", jlink_interface_to_string(selected_interface));
+	// }
 
 	return true;
 }
 
-bool jlink_set_jtag_speed(const uint32_t frequency)
+bool jlink_set_interface_speed(const uint8_t interface, const uint32_t frequency)
 {
-	/* Fixme: see note on jlink_get_speeds */
+	if (!(jlink.capabilities & JLINK_CAP_SPEED_INFO)) {
+		DEBUG_WARN("J-Link does not support speed info command\n");
+		return false;
+	}
+
+	/* Get the selected interface */
+	const uint8_t selected_interface = jlink_selected_interface();
+
+	if (selected_interface != interface) {
+		/* If the selected interface doesn't match the requested interface, select it, let's hope this doesn't mess something up elsewhere */
+		DEBUG_WARN("Trying to set speed for interface %s but it is not selected, selecting it\n",
+			jlink_interface_to_string(interface));
 
-	if (!(jlink.capabilities & JLINK_CAP_SPEED_INFO))
+		/* select the requested interface */
+		if (!jlink_select_interface(interface))
+			return false;
+	}
+
+	/* Ensure we have the speed info for the selected interface */
+	if (!jlink_get_interface_speed(interface))
 		return false;
 
+	struct jlink_interface_speed *const interface_speed = &jlink.interface_speed[interface];
+
 	/* Find the divisor that gets us closest to the requested frequency */
-	uint16_t divisor = (jlink.base_frequency + frequency - 1U) / frequency;
+	uint16_t divisor = (interface_speed->base_frequency + frequency - 1U) / frequency;
 
 	/* Bound the divisor to the min divisor */
-	if (divisor < jlink.min_divisor)
-		divisor = jlink.min_divisor;
+	if (divisor < interface_speed->min_divisor)
+		divisor = interface_speed->min_divisor;
 
 	/* Get the approximate frequency we'll actually be running at, convert to kHz in the process */
-	const uint16_t frequency_khz = (jlink.base_frequency / jlink.current_divisor) / 1000U;
+	const uint16_t frequency_khz = (interface_speed->base_frequency / interface_speed->current_divisor) / 1000U;
 
 	if (!jlink_simple_request_16(JLINK_CMD_SET_SPEED, frequency_khz, NULL, 0))
-		return false;
+		return false; /* note that we don't restore the selected interface here, for simplicity sake */
 
-	/* Update the current divisor for frquency calculations */
-	jlink.current_divisor = divisor;
+	/* Update the current divisor for frequency calculations */
+	interface_speed->current_divisor = divisor;
+
+	// Restoring the selected interface seemed to cause issues sometimes (unsure if culprit)
+	// so we don't do it for now, it didn't seem to be necessary anyway
+	// if (selected_interface != interface) {
+	// 	/* If the selected interface didn't match the requested interface, restore the selected interface */
+	// 	DEBUG_WARN("Restoring selected interface to %s\n", jlink_interface_to_string(selected_interface));
+
+	// 	/* select the requested interface, we don't consider this a critical failure, as we got what we wanted */
+	// 	if (!jlink_select_interface(selected_interface))
+	// 		DEBUG_ERROR("Failed to restore selected interface to %s\n", jlink_interface_to_string(selected_interface));
+	// }
 
 	return true;
 }
@@ -482,7 +558,7 @@ bool jlink_init(void)
 		libusb_close(info.usb_link->device_handle);
 		return false;
 	}
-	if (!jlink_get_capabilities() || !jlink_get_version() || !jlink_get_interfaces() || !jlink_get_speeds()) {
+	if (!jlink_get_capabilities() || !jlink_get_version() || !jlink_get_interfaces()) {
 		DEBUG_ERROR("Failed to read J-Link information\n");
 		libusb_release_interface(info.usb_link->device_handle, info.usb_link->interface);
 		libusb_close(info.usb_link->device_handle);
@@ -527,22 +603,24 @@ bool jlink_nrst_get_val(void)
 
 void jlink_max_frequency_set(const uint32_t frequency)
 {
-	/* Fixme: see note on jlink_get_speeds */
+	const uint8_t bmda_interface = info.is_jtag ? JLINK_SELECT_IF_JTAG : JLINK_SELECT_IF_SWD;
 
-	if (!(jlink.capabilities & JLINK_CAP_SPEED_INFO) || !info.is_jtag)
-		return;
-
-	jlink_set_jtag_speed(frequency);
+	if (!jlink_set_interface_speed(bmda_interface, frequency))
+		DEBUG_ERROR("Failed to set J-Link %s interface speed\n", jlink_interface_to_string(bmda_interface));
 }
 
 uint32_t jlink_max_frequency_get(void)
 {
-	/* Fixme: see note on jlink_get_speeds */
+	const uint8_t bmda_interface = info.is_jtag ? JLINK_SELECT_IF_JTAG : JLINK_SELECT_IF_SWD;
 
-	if (!(jlink.capabilities & JLINK_CAP_SPEED_INFO) || !info.is_jtag)
+	/* Ensure we have the speed info for the requested interface */
+	if (!jlink_get_interface_speed(bmda_interface)) {
+		DEBUG_ERROR("No speed info available for interface %s\n", jlink_interface_to_string(bmda_interface));
 		return FREQ_FIXED;
+	}
 
-	return jlink.base_frequency / jlink.current_divisor;
+	struct jlink_interface_speed *const interface_speed = &jlink.interface_speed[bmda_interface];
+	return interface_speed->base_frequency / interface_speed->current_divisor;
 }
 
 bool jlink_target_set_power(const bool power)

src/platforms/hosted/jlink.h

@@ -24,16 +24,6 @@
 
 #include "bmp_hosted.h"
 
-typedef struct jlink {
-	char fw_version[256U];
-	uint32_t hw_version;
-	uint32_t capabilities;
-	uint32_t available_interfaces;
-	uint32_t base_frequency;
-	uint16_t min_divisor;
-	uint16_t current_divisor;
-} jlink_s;
-
 bool jlink_select_interface(const uint8_t interface);
 
 /* BMDA interface */