2 files changed, 17 insertions, 8 deletions

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 813af55..73169e1 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -3528,8 +3528,20 @@ static int stlink_config_trace(void *handle, bool enabled,
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
-			pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
+	if (!(h->version.flags & STLINK_F_HAS_TRACE)) {
+		LOG_ERROR("The attached ST-LINK version doesn't support trace");
+		return ERROR_FAIL;
+	}
+
+	if (!enabled) {
+		stlink_usb_trace_disable(h);
+		return ERROR_OK;
+	}
+
+	assert(trace_freq != NULL);
+	assert(prescaler != NULL);
+
+	if (pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART) {
 		LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
 		return ERROR_FAIL;
 	}
@@ -3538,14 +3550,12 @@ static int stlink_config_trace(void *handle, bool enabled,
 			STLINK_V3_TRACE_MAX_HZ : STLINK_TRACE_MAX_HZ;
 
 	/* Only concern ourselves with the frequency if the STlink is processing it. */
-	if (enabled && *trace_freq > max_trace_freq) {
+	if (*trace_freq > max_trace_freq) {
 		LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
 			  max_trace_freq);
 		return ERROR_FAIL;
 	}
 
-	stlink_usb_trace_disable(h);
-
 	if (!*trace_freq)
 		*trace_freq = max_trace_freq;
 
@@ -3567,8 +3577,7 @@ static int stlink_config_trace(void *handle, bool enabled,
 
 	*prescaler = presc;
 
-	if (!enabled)
-		return ERROR_OK;
+	stlink_usb_trace_disable(h);
 
 	h->trace.source_hz = *trace_freq;
 
diff --git a/src/target/armv7m_trace.c b/src/target/armv7m_trace.c
index 32b48a7..adfef2e 100644
--- a/src/target/armv7m_trace.c
+++ b/src/target/armv7m_trace.c
@@ -87,7 +87,7 @@ int armv7m_trace_tpiu_config(struct target *target)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	struct armv7m_trace_config *trace_config = &armv7m->trace_config;
-	uint16_t prescaler;
+	uint16_t prescaler = TPIU_ACPR_MAX_SWOSCALER + 1;
 	int retval;
 
 	target_unregister_timer_callback(armv7m_poll_trace, target);