stlink: add SWO tracing support

Enable reading the SWO trace output via STLinkv2 dongles that support
it.

This adds an optional initialization parameter "trace" with which the user
specifies a destination file where SWO trace output is appended as it comes in
as well as the trace module's source clock rate.

STLink will be configured for a 2MHz SWO data rate (STLink's highest
supported rate) if the source clock is > 2MHz, otherwise the source
clock is used as the data rate directly.

For example:

trace swo.log 168000000

If "trace" is specified with a usable file path, the stlink_usb driver will
attempt to configure and read SWO trace data as follows:
- on _run(), the target's TPI and TMI are configured and the STLinkv2 is told
  to enable tracing.  Only generic ARM TPI and TMI registers are
  configured, any MCU-specific settings (ex: pin routing) are the
  responsibility of the target firmware.  The configuration applied is
  based on the STLinkv2's capabilities (UART emulation).
- on _v2_get_status(), the trace data (if any) is fetched from the
  STLink after the target status is checked and the target is found to
  be running.
- on _halt(), the STLink is told to disable tracing.

When fetching trace data, the entire trace frame is written to the output file
and that data is flushed.  An external tool may be used to parse the
trace data into a more human-readable format.

Tested on ARM Cortex M4F and M3 MCUs (STM32F407 and STM32L152).

Change-Id: Ic3983d46c82ba77010c23b0e18ce7b275d917f12
Signed-off-by: Andrey Yurovsky <yurovsky@gmail.com>
Reviewed-on: http://openocd.zylin.com/1524
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>

doc/openocd.texi

@@ -3027,6 +3027,13 @@ The vendor ID and product ID of the device.
 @deffn {Config Command} {stlink_api} api_level
 Manually sets the stlink api used, valid options are 1 or 2. (@b{STLINK Only}).
 @end deffn
+
+@deffn {Config Command} {trace} output_file_path source_clock_hz
+Enable SWO tracing (if supported), trace data is appended to the specified
+output file and the file is created if it does not exist.  The source clock
+rate for the trace port must be specified, this is typically the CPU clock
+rate.
+@end deffn
 @end deffn
 
 @deffn {Interface Driver} {opendous}

src/jtag/drivers/stlink_usb.c

@@ -45,6 +45,7 @@
 #define STLINK_NULL_EP     0
 #define STLINK_RX_EP       (1|ENDPOINT_IN)
 #define STLINK_TX_EP       (2|ENDPOINT_OUT)
+#define STLINK_TRACE_EP    (3|ENDPOINT_IN)
 #define STLINK_SG_SIZE     (31)
 #define STLINK_DATA_SIZE   (4*128)
 #define STLINK_CMD_SIZE_V2 (16)
@@ -91,6 +92,17 @@ struct stlink_usb_handle_s {
 	uint16_t pid;
 	/** this is the currently used jtag api */
 	enum stlink_jtag_api_version jtag_api;
+	/** */
+	struct {
+		/** whether SWO tracing is enabled or not */
+		bool enabled;
+		/** trace data destination file */
+		FILE *output_f;
+		/** trace module source clock (for prescaler) */
+		uint32_t source_hz;
+		/** trace module clock prescaler */
+		uint32_t prescale;
+	} trace;
 };
 
 #define STLINK_DEBUG_ERR_OK            0x80
@@ -158,10 +170,18 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 
+#define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
+#define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
+#define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
+
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 
+#define STLINK_TRACE_SIZE               1024
+#define STLINK_TRACE_MAX_HZ             2000000
+#define STLINK_TRACE_MIN_VERSION        13
+
 /** */
 enum stlink_mode {
 	STLINK_MODE_UNKNOWN = 0,
@@ -305,6 +325,26 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 	return ERROR_OK;
 }
 
+/** */
+static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
+{
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	assert(h->version.stlink >= 2);
+
+	if (jtag_libusb_bulk_read(h->fd, STLINK_TRACE_EP, (char *)buf,
+				size, 1000) != size) {
+		LOG_ERROR("bulk trace read failed");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
 /** */
 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 {
@@ -779,6 +819,42 @@ static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 }
 
+/** */
+static void stlink_usb_trace_read(void *handle)
+{
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	if (h->trace.enabled && h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
+		int res;
+
+		stlink_usb_init_buffer(handle, STLINK_RX_EP, 10);
+
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+		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) {
+					/* Log retrieved trace output */
+					if (fwrite(buf, 1, size, h->trace.output_f) > 0)
+						fflush(h->trace.output_f);
+				}
+			}
+		}
+	}
+}
+
 static enum target_state stlink_usb_v2_get_status(void *handle)
 {
 	int result;
@@ -793,6 +869,8 @@ 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;
 }
 
@@ -886,6 +964,132 @@ static int stlink_usb_assert_srst(void *handle, int srst)
 	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 }
 
+/** */
+static int stlink_configure_target_trace_port(void *handle)
+{
+	int res;
+	uint32_t reg;
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	/* 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, TPI_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, TPI_SPPR, 0b10);
+	if (res != ERROR_OK)
+		goto out;
+	/* disable continuous formatting */
+	res = stlink_usb_write_debug_reg(handle, TPI_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_TER, (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;
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
+
+	LOG_DEBUG("Tracing: disable\n");
+
+	stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
+	res = stlink_usb_xfer(handle, h->databuf, 2);
+
+	if (res == ERROR_OK)
+		h->trace.enabled = false;
+}
+
+
+/** */
+static int stlink_usb_trace_enable(void *handle)
+{
+	int res;
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	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\n");
+
+		trace_hz = h->trace.prescale > 0 ?
+			h->trace.source_hz / (h->trace.prescale + 1) :
+			h->trace.source_hz;
+
+		stlink_usb_init_buffer(handle, STLINK_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->cmdidx += 4;
+
+		res = stlink_usb_xfer(handle, h->databuf, 2);
+
+		if (res == ERROR_OK)  {
+			h->trace.enabled = true;
+			LOG_DEBUG("Tracing: recording at %uHz\n", trace_hz);
+		}
+	} else {
+		LOG_ERROR("Tracing is not supported by this version.");
+		res = ERROR_FAIL;
+	}
+
+	return res;
+}
+
 /** */
 static int stlink_usb_run(void *handle)
 {
@@ -896,8 +1100,19 @@ static int stlink_usb_run(void *handle)
 
 	h = (struct stlink_usb_handle_s *)handle;
 
-	if (h->jtag_api == STLINK_JTAG_API_V2)
+	if (h->jtag_api == STLINK_JTAG_API_V2) {
-		return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
+		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
+
+		/* Try to start tracing, if requested */
+		if (res == ERROR_OK && h->trace.output_f) {
+			if (stlink_usb_trace_enable(handle) == ERROR_OK)
+				LOG_DEBUG("Tracing: enabled\n");
+			else
+				LOG_ERROR("Tracing: enable failed\n");
+		}
+
+		return res;
+	}
 
 	stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 
@@ -922,8 +1137,14 @@ static int stlink_usb_halt(void *handle)
 
 	h = (struct stlink_usb_handle_s *)handle;
 
-	if (h->jtag_api == STLINK_JTAG_API_V2)
+	if (h->jtag_api == STLINK_JTAG_API_V2) {
-		return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
+		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;
+	}
 
 	stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 
@@ -1321,6 +1542,17 @@ 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_f && 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);
 

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, 0, false}, 0, 0 };
+static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, 0, false, NULL, 0}, 0, 0 };
 
 int hl_interface_open(enum hl_transports tr)
 {
@@ -114,6 +114,11 @@ 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;
+	}
+
 	return ERROR_OK;
 }
 
@@ -220,6 +225,30 @@ COMMAND_HANDLER(stlink_interface_handle_api_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(interface_handle_trace_command)
+{
+	FILE *f;
+	unsigned source_hz;
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	f = fopen(CMD_ARGV[0], "a");
+	if (!f)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], source_hz);
+	if (source_hz == 0) {
+		fclose(f);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	hl_if.param.trace_f = f;
+	hl_if.param.trace_source_hz = source_hz;
+
+	return ERROR_OK;
+}
+
 static const struct command_registration hl_interface_command_handlers[] = {
 	{
 	 .name = "hla_device_desc",
@@ -256,6 +285,13 @@ static const struct command_registration hl_interface_command_handlers[] = {
 	 .help = "set the desired stlink api level",
 	 .usage = "api version 1 or 2",
 	 },
+	 {
+	 .name = "trace",
+	 .handler = &interface_handle_trace_command,
+	 .mode = COMMAND_CONFIG,
+	 .help = "configure trace reception",
+	 .usage = "destination_path source_lock_hz",
+	 },
 	COMMAND_REGISTRATION_DONE
 };
 

src/jtag/hla/hla_interface.h

@@ -48,6 +48,10 @@ struct hl_interface_param_s {
 	int max_buffer;
 	/** */
 	bool connect_under_reset;
+	/** Output file for trace data (if any) */
+	FILE *trace_f;
+	/** Trace module source clock rate */
+	uint32_t trace_source_hz;
 };
 
 struct hl_interface_s {

src/target/cortex_m.h

@@ -33,6 +33,11 @@
 
 #define SYSTEM_CONTROL_BASE 0x400FE000
 
+#define ITM_TER		0xE0000E00
+#define ITM_TPR		0xE0000E40
+#define ITM_TCR		0xE0000E80
+#define ITM_LAR		0xE0000FB0
+
 #define CPUID		0xE000ED00
 /* Debug Control Block */
 #define DCB_DHCSR	0xE000EDF0
@@ -64,6 +69,14 @@
 #define FPU_FPCAR	0xE000EF38
 #define FPU_FPDSCR	0xE000EF3C
 
+#define TPI_SSPSR	0xE0040000
+#define TPI_CSPSR	0xE0040004
+#define TPI_ACPR	0xE0040010
+#define TPI_SPPR	0xE00400F0
+#define TPI_FFSR	0xE0040300
+#define TPI_FFCR	0xE0040304
+#define TPI_FSCR	0xE0040308
+
 /* DCB_DHCSR bit and field definitions */
 #define DBGKEY		(0xA05F << 16)
 #define C_DEBUGEN	(1 << 0)