armv7m_trace, stlink: provide APIs to capture trace with an adapter

Change-Id: I9d193dd5af382912e4fe838bd4f612cffd11b295
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/2540
Tested-by: jenkins

8 files changed, 235 insertions, 197 deletions

diff --git a/src/jtag/core.c b/src/jtag/core.c
index 8f3aa4d9..cb3e9265 100644
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -1824,3 +1824,25 @@ void adapter_deassert_reset(void)
 	else
 		LOG_ERROR("transport is not selected");
 }
+
+int adapter_config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
+			 uint32_t port_size, unsigned int *trace_freq)
+{
+	if (jtag->config_trace)
+		return jtag->config_trace(enabled, pin_protocol, port_size,
+					  trace_freq);
+	else if (enabled) {
+		LOG_ERROR("The selected interface does not support tracing");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+int adapter_poll_trace(uint8_t *buf, size_t *size)
+{
+	if (jtag->poll_trace)
+		return jtag->poll_trace(buf, size);
+
+	return ERROR_FAIL;
+}
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 331d30b7..9818dda6 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -124,12 +124,8 @@ struct stlink_usb_handle_s {
 	struct {
 		/** whether SWO tracing is enabled or not */
 		bool enabled;
-		/** trace data destination file */
-		FILE *output_f;
-		/** trace module source clock (for prescaler) */
+		/** trace module source clock */
 		uint32_t source_hz;
-		/** trace module clock prescaler */
-		uint32_t prescale;
 	} trace;
 	/** reconnect is needed next time we try to query the
 	 * status */
@@ -870,7 +866,7 @@ static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 }
 
 /** */
-static void stlink_usb_trace_read(void *handle)
+static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
 {
 	struct stlink_usb_handle_s *h = handle;
 
@@ -885,29 +881,20 @@ static void stlink_usb_trace_read(void *handle)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
 
 		res = stlink_usb_xfer(handle, h->databuf, 2);
-		if (res == ERROR_OK) {
-			uint8_t buf[STLINK_TRACE_SIZE];
-			size_t size = le_to_h_u16(h->databuf);
-
-			if (size > 0) {
-				size = size < sizeof(buf) ? size : sizeof(buf) - 1;
-
-				res = stlink_usb_read_trace(handle, buf, size);
-				if (res == ERROR_OK) {
-					if (h->trace.output_f) {
-						/* Log retrieved trace output */
-						if (fwrite(buf, 1, size, h->trace.output_f) > 0)
-							fflush(h->trace.output_f);
-					}
-				}
-			}
+		if (res != ERROR_OK)
+			return res;
+
+		size_t bytes_avail = le_to_h_u16(h->databuf);
+		*size = bytes_avail < *size ? bytes_avail : *size - 1;
+
+		if (*size > 0) {
+			res = stlink_usb_read_trace(handle, buf, *size);
+			if (res != ERROR_OK)
+				return res;
+			return ERROR_OK;
 		}
 	}
-}
-
-static int stlink_usb_trace_read_callback(void *handle)
-{
-	stlink_usb_trace_read(handle);
+	*size = 0;
 	return ERROR_OK;
 }
 
@@ -925,8 +912,6 @@ static enum target_state stlink_usb_v2_get_status(void *handle)
 	else if (status & S_RESET_ST)
 		return TARGET_RESET;
 
-	stlink_usb_trace_read(handle);
-
 	return TARGET_RUNNING;
 }
 
@@ -975,25 +960,6 @@ static enum target_state stlink_usb_state(void *handle)
 	return TARGET_UNKNOWN;
 }
 
-/** */
-static int stlink_usb_reset(void *handle)
-{
-	struct stlink_usb_handle_s *h = handle;
-
-	assert(handle != NULL);
-
-	stlink_usb_init_buffer(handle, h->rx_ep, 2);
-
-	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-
-	if (h->jtag_api == STLINK_JTAG_API_V1)
-		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
-	else
-		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
-
-	return stlink_cmd_allow_retry(handle, h->databuf, 2);
-}
-
 static int stlink_usb_assert_srst(void *handle, int srst)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -1013,62 +979,6 @@ static int stlink_usb_assert_srst(void *handle, int srst)
 }
 
 /** */
-static int stlink_configure_target_trace_port(void *handle)
-{
-	int res;
-	uint32_t reg;
-	struct stlink_usb_handle_s *h = handle;
-
-	assert(handle != NULL);
-
-	/* configure the TPI */
-
-	/* enable the trace subsystem */
-	res = stlink_usb_v2_read_debug_reg(handle, DCB_DEMCR, &reg);
-	if (res != ERROR_OK)
-		goto out;
-	res = stlink_usb_write_debug_reg(handle, DCB_DEMCR, TRCENA|reg);
-	if (res != ERROR_OK)
-		goto out;
-	/* set the TPI clock prescaler */
-	res = stlink_usb_write_debug_reg(handle, TPIU_ACPR, h->trace.prescale);
-	if (res != ERROR_OK)
-		goto out;
-	/* select the pin protocol.  The STLinkv2 only supports asynchronous
-	 * UART emulation (NRZ) mode, so that's what we pick. */
-	res = stlink_usb_write_debug_reg(handle, TPIU_SPPR, 0x02);
-	if (res != ERROR_OK)
-		goto out;
-	/* disable continuous formatting */
-	res = stlink_usb_write_debug_reg(handle, TPIU_FFCR, (1<<8));
-	if (res != ERROR_OK)
-		goto out;
-
-	/* configure the ITM */
-
-	/* unlock access to the ITM registers */
-	res = stlink_usb_write_debug_reg(handle, ITM_LAR, 0xC5ACCE55);
-	if (res != ERROR_OK)
-		goto out;
-	/* enable trace with ATB ID 1 */
-	res = stlink_usb_write_debug_reg(handle, ITM_TCR, (1<<16)|(1<<0)|(1<<2));
-	if (res != ERROR_OK)
-		goto out;
-	/* trace privilege */
-	res = stlink_usb_write_debug_reg(handle, ITM_TPR, 1);
-	if (res != ERROR_OK)
-		goto out;
-	/* trace port enable (port 0) */
-	res = stlink_usb_write_debug_reg(handle, ITM_TER0, (1<<0));
-	if (res != ERROR_OK)
-		goto out;
-
-	res = ERROR_OK;
-out:
-	return res;
-}
-
-/** */
 static void stlink_usb_trace_disable(void *handle)
 {
 	int res = ERROR_OK;
@@ -1085,10 +995,8 @@ static void stlink_usb_trace_disable(void *handle)
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
 	res = stlink_usb_xfer(handle, h->databuf, 2);
 
-	if (res == ERROR_OK) {
+	if (res == ERROR_OK)
 		h->trace.enabled = false;
-		target_unregister_timer_callback(stlink_usb_trace_read_callback, handle);
-	}
 }
 
 
@@ -1101,38 +1009,20 @@ static int stlink_usb_trace_enable(void *handle)
 	assert(handle != NULL);
 
 	if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
-		uint32_t trace_hz;
-
-		res = stlink_configure_target_trace_port(handle);
-		if (res != ERROR_OK)
-			LOG_ERROR("Unable to configure tracing on target");
-
-		trace_hz = h->trace.prescale > 0 ?
-			h->trace.source_hz / (h->trace.prescale + 1) :
-			h->trace.source_hz;
-
 		stlink_usb_init_buffer(handle, h->rx_ep, 10);
 
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
 		h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
 		h->cmdidx += 2;
-		h_u32_to_le(h->cmdbuf+h->cmdidx, trace_hz);
+		h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
 		h->cmdidx += 4;
 
 		res = stlink_usb_xfer(handle, h->databuf, 2);
 
 		if (res == ERROR_OK)  {
 			h->trace.enabled = true;
-			LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", trace_hz);
-			/* We need the trace read function to be called at a
-			 * high-enough frequency to ensure reasonable
-			 * "timeliness" in processing ITM/DWT data.
-			 * TODO: An alternative could be using the asynchronous
-			 * features of the libusb-1.0 API to queue up one or more
-			 * reads in advance and requeue them once they are
-			 * completed. */
-			target_register_timer_callback(stlink_usb_trace_read_callback, 1, 1, handle);
+			LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
 		}
 	} else {
 		LOG_ERROR("Tracing is not supported by this version.");
@@ -1143,6 +1033,35 @@ static int stlink_usb_trace_enable(void *handle)
 }
 
 /** */
+static int stlink_usb_reset(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int retval;
+
+	assert(handle != NULL);
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+
+	if (h->jtag_api == STLINK_JTAG_API_V1)
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
+	else
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
+
+	retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (h->trace.enabled) {
+		stlink_usb_trace_disable(h);
+		return stlink_usb_trace_enable(h);
+	}
+
+	return ERROR_OK;
+}
+
+/** */
 static int stlink_usb_run(void *handle)
 {
 	int res;
@@ -1153,14 +1072,6 @@ static int stlink_usb_run(void *handle)
 	if (h->jtag_api == STLINK_JTAG_API_V2) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 
-		/* Try to start tracing, if requested */
-		if (res == ERROR_OK && h->trace.source_hz && !h->trace.enabled) {
-			if (stlink_usb_trace_enable(handle) == ERROR_OK)
-				LOG_DEBUG("Tracing: enabled");
-			else
-				LOG_ERROR("Tracing: enable failed");
-		}
-
 		return res;
 	}
 
@@ -1183,9 +1094,6 @@ static int stlink_usb_halt(void *handle)
 	if (h->jtag_api == STLINK_JTAG_API_V2) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 
-		if (res == ERROR_OK && h->trace.enabled)
-			stlink_usb_trace_disable(handle);
-
 		return res;
 	}
 
@@ -1816,17 +1724,6 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	/* set the used jtag api, this will default to the newest supported version */
 	h->jtag_api = api;
 
-	if (h->jtag_api >= 2 && param->trace_source_hz > 0) {
-		uint32_t prescale;
-
-		prescale = param->trace_source_hz > STLINK_TRACE_MAX_HZ ?
-			(param->trace_source_hz / STLINK_TRACE_MAX_HZ) - 1 : 0;
-
-		h->trace.output_f = param->trace_f;
-		h->trace.source_hz = param->trace_source_hz;
-		h->trace.prescale = prescale;
-	}
-
 	/* initialize the debug hardware */
 	err = stlink_usb_init_mode(h, param->connect_under_reset);
 
@@ -1872,6 +1769,36 @@ error_open:
 	return ERROR_FAIL;
 }
 
+int stlink_config_trace(void *handle, bool enabled, enum tpio_pin_protocol pin_protocol,
+			uint32_t port_size, unsigned int *trace_freq)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (enabled && (h->jtag_api < 2 || pin_protocol != ASYNC_UART)) {
+		LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
+		return ERROR_FAIL;
+	}
+
+	if (!enabled) {
+		stlink_usb_trace_disable(h);
+		return ERROR_OK;
+	}
+
+	if (*trace_freq > STLINK_TRACE_MAX_HZ) {
+		LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
+			  STLINK_TRACE_MAX_HZ);
+		return ERROR_FAIL;
+	}
+
+	stlink_usb_trace_disable(h);
+
+	if (!*trace_freq)
+		*trace_freq = STLINK_TRACE_MAX_HZ;
+	h->trace.source_hz = *trace_freq;
+
+	return stlink_usb_trace_enable(h);
+}
+
 /** */
 struct hl_layout_api_s stlink_usb_layout_api = {
 	/** */
@@ -1908,4 +1835,8 @@ struct hl_layout_api_s stlink_usb_layout_api = {
 	.override_target = stlink_usb_override_target,
 	/** */
 	.speed = stlink_speed,
+	/** */
+	.config_trace = stlink_config_trace,
+	/** */
+	.poll_trace = stlink_usb_trace_read,
 };
diff --git a/src/jtag/hla/hla_interface.c b/src/jtag/hla/hla_interface.c
index 85f083ce..a208e9fd 100644
--- a/src/jtag/hla/hla_interface.c
+++ b/src/jtag/hla/hla_interface.c
@@ -37,7 +37,7 @@
 
 #include <target/target.h>
 
-static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, false, NULL, 0, -1}, 0, 0 };
+static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, false, -1}, 0, 0 };
 
 int hl_interface_open(enum hl_transports tr)
 {
@@ -118,12 +118,6 @@ static int hl_interface_quit(void)
 {
 	LOG_DEBUG("hl_interface_quit");
 
-	if (hl_if.param.trace_f) {
-		fclose(hl_if.param.trace_f);
-		hl_if.param.trace_f = NULL;
-	}
-	hl_if.param.trace_source_hz = 0;
-
 	if (hl_if.layout->api->close)
 		hl_if.layout->api->close(hl_if.handle);
 
@@ -194,6 +188,28 @@ int hl_interface_override_target(const char **targetname)
 	return ERROR_FAIL;
 }
 
+int hl_interface_config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
+			      uint32_t port_size, unsigned int *trace_freq)
+{
+	if (hl_if.layout->api->config_trace)
+		return hl_if.layout->api->config_trace(hl_if.handle, enabled, pin_protocol,
+						       port_size, trace_freq);
+	else if (enabled) {
+		LOG_ERROR("The selected interface does not support tracing");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+int hl_interface_poll_trace(uint8_t *buf, size_t *size)
+{
+	if (hl_if.layout->api->poll_trace)
+		return hl_if.layout->api->poll_trace(hl_if.handle, buf, size);
+
+	return ERROR_FAIL;
+}
+
 COMMAND_HANDLER(hl_interface_handle_device_desc_command)
 {
 	LOG_DEBUG("hl_interface_handle_device_desc_command");
@@ -263,31 +279,6 @@ COMMAND_HANDLER(hl_interface_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(interface_handle_trace_command)
-{
-	FILE *f = NULL;
-	unsigned source_hz;
-
-	if ((CMD_ARGC < 1) || (CMD_ARGC > 2))
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], source_hz);
-	if (source_hz == 0) {
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	if (CMD_ARGC == 2) {
-		f = fopen(CMD_ARGV[1], "a");
-		if (!f)
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	hl_if.param.trace_f = f;
-	hl_if.param.trace_source_hz = source_hz;
-
-	return ERROR_OK;
-}
-
 COMMAND_HANDLER(interface_handle_hla_command)
 {
 	if (CMD_ARGC != 1)
@@ -333,13 +324,6 @@ static const struct command_registration hl_interface_command_handlers[] = {
 	 .usage = "(vid pid)* ",
 	 },
 	 {
-	 .name = "trace",
-	 .handler = &interface_handle_trace_command,
-	 .mode = COMMAND_CONFIG,
-	 .help = "configure trace reception",
-	 .usage = "source_lock_hz [destination_path]",
-	 },
-	 {
 	 .name = "hla_command",
 	 .handler = &interface_handle_hla_command,
 	 .mode = COMMAND_EXEC,
@@ -360,4 +344,6 @@ struct jtag_interface hl_interface = {
 	.speed = &hl_interface_speed,
 	.khz = &hl_interface_khz,
 	.speed_div = &hl_interface_speed_div,
+	.config_trace = &hl_interface_config_trace,
+	.poll_trace = &hl_interface_poll_trace,
 };
diff --git a/src/jtag/hla/hla_interface.h b/src/jtag/hla/hla_interface.h
index aac1be30..a1559257 100644
--- a/src/jtag/hla/hla_interface.h
+++ b/src/jtag/hla/hla_interface.h
@@ -46,10 +46,6 @@ struct hl_interface_param_s {
 	enum hl_transports transport;
 	/** */
 	bool connect_under_reset;
-	/** Output file for trace data (if any) */
-	FILE *trace_f;
-	/** Trace module source clock rate */
-	uint32_t trace_source_hz;
 	/** Initial interface clock clock speed */
 	int initial_interface_speed;
 };
diff --git a/src/jtag/hla/hla_layout.h b/src/jtag/hla/hla_layout.h
index e989f665..8833d6c8 100644
--- a/src/jtag/hla/hla_layout.h
+++ b/src/jtag/hla/hla_layout.h
@@ -24,6 +24,8 @@
 #ifndef _HL_LAYOUT_H
 #define _HL_LAYOUT_H
 
+#include <target/armv7m_trace.h>
+
 /** */
 struct hl_interface_s;
 struct hl_interface_param_s;
@@ -79,6 +81,31 @@ struct hl_layout_api_s {
 	int (*custom_command) (void *handle, const char *command);
 	/** */
 	int (*speed)(void *handle, int khz, bool query);
+	/**
+	 * Configure trace parameters for the adapter
+	 *
+	 * @param handle A handle to adapter
+	 * @param enabled Whether to enable trace
+	 * @param pin_protocol Configured pin protocol
+	 * @param port_size Trace port width for sync mode
+	 * @param trace_freq A pointer to the configured trace
+	 * frequency; if it points to 0, the adapter driver must write
+	 * its maximum supported rate there
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*config_trace)(void *handle, bool enabled, enum tpio_pin_protocol pin_protocol,
+			    uint32_t port_size, unsigned int *trace_freq);
+	/**
+	 * Poll for new trace data
+	 *
+	 * @param handle A handle to adapter
+	 * @param buf A pointer to buffer to store received data
+	 * @param size A pointer to buffer size; must be filled with
+	 * the actual amount of bytes written
+	 *
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*poll_trace)(void *handle, uint8_t *buf, size_t *size);
 	/** */
 	enum target_state (*state) (void *fd);
 };
diff --git a/src/jtag/interface.h b/src/jtag/interface.h
index c7130ccf..e7b20148 100644
--- a/src/jtag/interface.h
+++ b/src/jtag/interface.h
@@ -28,6 +28,7 @@
 #define OPENOCD_JTAG_INTERFACE_H
 
 #include <jtag/jtag.h>
+#include <target/armv7m_trace.h>
 
 /* @file
  * The "Cable Helper API" is what the cable drivers can use to help
@@ -298,11 +299,39 @@ struct jtag_interface {
 	 * @returns ERROR_OK on success, or an error code on failure.
 	 */
 	int (*srst_asserted)(int *srst_asserted);
+
+	/**
+	 * Configure trace parameters for the adapter
+	 *
+	 * @param enabled Whether to enable trace
+	 * @param pin_protocol Configured pin protocol
+	 * @param port_size Trace port width for sync mode
+	 * @param trace_freq A pointer to the configured trace
+	 * frequency; if it points to 0, the adapter driver must write
+	 * its maximum supported rate there
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*config_trace)(bool enabled, enum tpio_pin_protocol pin_protocol,
+			    uint32_t port_size, unsigned int *trace_freq);
+
+	/**
+	 * Poll for new trace data
+	 *
+	 * @param buf A pointer to buffer to store received data
+	 * @param size A pointer to buffer size; must be filled with
+	 * the actual amount of bytes written
+	 *
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*poll_trace)(uint8_t *buf, size_t *size);
 };
 
 extern const char * const jtag_only[];
 
 void adapter_assert_reset(void);
 void adapter_deassert_reset(void);
+int adapter_config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
+			 uint32_t port_size, unsigned int *trace_freq);
+int adapter_poll_trace(uint8_t *buf, size_t *size);
 
 #endif /* OPENOCD_JTAG_INTERFACE_H */
diff --git a/src/target/armv7m_trace.c b/src/target/armv7m_trace.c
index b1bbb31c..eb07a6e6 100644
--- a/src/target/armv7m_trace.c
+++ b/src/target/armv7m_trace.c
@@ -20,6 +20,30 @@
 #include <target/armv7m.h>
 #include <target/cortex_m.h>
 #include <target/armv7m_trace.h>
+#include <jtag/interface.h>
+
+#define TRACE_BUF_SIZE	4096
+
+static int armv7m_poll_trace(void *target)
+{
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	uint8_t buf[TRACE_BUF_SIZE];
+	size_t size = sizeof(buf);
+	int retval;
+
+	retval = adapter_poll_trace(buf, &size);
+	if (retval != ERROR_OK || !size)
+		return retval;
+
+	if (fwrite(buf, 1, size, armv7m->trace_config.trace_file) == size)
+		fflush(armv7m->trace_config.trace_file);
+	else {
+		LOG_ERROR("Error writing to the trace destination file");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
 
 int armv7m_trace_tpiu_config(struct target *target)
 {
@@ -28,19 +52,38 @@ int armv7m_trace_tpiu_config(struct target *target)
 	int prescaler;
 	int retval;
 
+	target_unregister_timer_callback(armv7m_poll_trace, target);
+
+
+	retval = adapter_config_trace(trace_config->config_type == INTERNAL,
+				      trace_config->pin_protocol,
+				      trace_config->port_size,
+				      &trace_config->trace_freq);
+	if (retval != ERROR_OK)
+		return retval;
+
 	if (!trace_config->trace_freq) {
 		LOG_ERROR("Trace port frequency is 0, can't enable TPIU");
 		return ERROR_FAIL;
 	}
 
+	prescaler = trace_config->traceclkin_freq / trace_config->trace_freq;
+
 	if (trace_config->traceclkin_freq % trace_config->trace_freq) {
-		LOG_ERROR("Can not calculate an integer divisor to get %u trace port frequency from %u TRACECLKIN frequency",
-			  trace_config->trace_freq, trace_config->traceclkin_freq);
-		return ERROR_FAIL;
+		prescaler++;
+		int trace_freq = trace_config->traceclkin_freq / prescaler;
+		LOG_INFO("Can not obtain %u trace port frequency from %u TRACECLKIN frequency, using %u instead",
+			  trace_config->trace_freq, trace_config->traceclkin_freq,
+			  trace_freq);
+		trace_config->trace_freq = trace_freq;
+		retval = adapter_config_trace(trace_config->config_type == INTERNAL,
+					      trace_config->pin_protocol,
+					      trace_config->port_size,
+					      &trace_config->trace_freq);
+		if (retval != ERROR_OK)
+			return retval;
 	}
 
-	prescaler = trace_config->traceclkin_freq / trace_config->trace_freq;
-
 	retval = target_write_u32(target, TPIU_CSPSR, 1 << trace_config->port_size);
 	if (retval != ERROR_OK)
 		return retval;
@@ -65,6 +108,9 @@ int armv7m_trace_tpiu_config(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 
+	if (trace_config->config_type == INTERNAL)
+		target_register_timer_callback(armv7m_poll_trace, 1, 1, target);
+
 	target_call_event_callbacks(target, TARGET_EVENT_TRACE_CONFIG);
 
 	return ERROR_OK;
diff --git a/src/target/armv7m_trace.h b/src/target/armv7m_trace.h
index 96c18958..eae72f55 100644
--- a/src/target/armv7m_trace.h
+++ b/src/target/armv7m_trace.h
@@ -15,6 +15,7 @@
 #ifndef ARMV7M_TRACE_H
 #define ARMV7M_TRACE_H
 
+#include <target/target.h>
 #include <command.h>
 
 /**