armv7m: add generic trace support (TPIU, ITM, etc.)

This provides support for various trace-related subsystems in a
generic and expandable way.

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

11 files changed, 423 insertions, 12 deletions

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 554ffc1..331d30b 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -1031,16 +1031,16 @@ static int stlink_configure_target_trace_port(void *handle)
 	if (res != ERROR_OK)
 		goto out;
 	/* set the TPI clock prescaler */
-	res = stlink_usb_write_debug_reg(handle, TPI_ACPR, h->trace.prescale);
+	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, TPI_SPPR, 0x02);
+	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, TPI_FFCR, (1<<8));
+	res = stlink_usb_write_debug_reg(handle, TPIU_FFCR, (1<<8));
 	if (res != ERROR_OK)
 		goto out;
 
@@ -1059,7 +1059,7 @@ static int stlink_configure_target_trace_port(void *handle)
 	if (res != ERROR_OK)
 		goto out;
 	/* trace port enable (port 0) */
-	res = stlink_usb_write_debug_reg(handle, ITM_TER, (1<<0));
+	res = stlink_usb_write_debug_reg(handle, ITM_TER0, (1<<0));
 	if (res != ERROR_OK)
 		goto out;
 
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index bf80c64..c591103 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -79,6 +79,7 @@ ARMV6_SRC = \
 
 ARMV7_SRC = \
 	armv7m.c \
+	armv7m_trace.c \
 	cortex_m.c \
 	armv7a.c \
 	cortex_a.c
@@ -155,6 +156,7 @@ noinst_HEADERS = \
 	armv4_5_cache.h \
 	armv7a.h \
 	armv7m.h \
+	armv7m_trace.h \
 	avrt.h \
 	dsp563xx.h \
 	dsp563xx_once.h \
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 01fb69a..ccb3e54 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -635,6 +635,9 @@ int armv7m_init_arch_info(struct target *target, struct armv7m_common *armv7m)
 
 	armv7m->common_magic = ARMV7M_COMMON_MAGIC;
 	armv7m->fp_feature = FP_NONE;
+	armv7m->trace_config.trace_bus_id = 1;
+	/* Enable stimulus port #0 by default */
+	armv7m->trace_config.itm_ter[0] = 1;
 
 	arm->core_type = ARM_MODE_THREAD;
 	arm->arch_info = armv7m;
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index d28977e..1974460 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -29,6 +29,7 @@
 
 #include "arm_adi_v5.h"
 #include "arm.h"
+#include "armv7m_trace.h"
 
 extern const int armv7m_psp_reg_map[];
 extern const int armv7m_msp_reg_map[];
@@ -153,6 +154,8 @@ struct armv7m_common {
 	/* stlink is a high level adapter, does not support all functions */
 	bool stlink;
 
+	struct armv7m_trace_config trace_config;
+
 	/* Direct processor core register read and writes */
 	int (*load_core_reg_u32)(struct target *target, uint32_t num, uint32_t *value);
 	int (*store_core_reg_u32)(struct target *target, uint32_t num, uint32_t value);
diff --git a/src/target/armv7m_trace.c b/src/target/armv7m_trace.c
new file mode 100644
index 0000000..b1bbb31
--- /dev/null
+++ b/src/target/armv7m_trace.c
@@ -0,0 +1,295 @@
+/***************************************************************************
+ *   Copyright (C) 2015  Paul Fertser <fercerpav@gmail.com>                *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <target/target.h>
+#include <target/armv7m.h>
+#include <target/cortex_m.h>
+#include <target/armv7m_trace.h>
+
+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;
+	int prescaler;
+	int retval;
+
+	if (!trace_config->trace_freq) {
+		LOG_ERROR("Trace port frequency is 0, can't enable TPIU");
+		return ERROR_FAIL;
+	}
+
+	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 = 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;
+
+	retval = target_write_u32(target, TPIU_ACPR, prescaler - 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, TPIU_SPPR, trace_config->pin_protocol);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t ffcr;
+	retval = target_read_u32(target, TPIU_FFCR, &ffcr);
+	if (retval != ERROR_OK)
+		return retval;
+	if (trace_config->formatter)
+		ffcr |= (1 << 1);
+	else
+		ffcr &= ~(1 << 1);
+	retval = target_write_u32(target, TPIU_FFCR, ffcr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	target_call_event_callbacks(target, TARGET_EVENT_TRACE_CONFIG);
+
+	return ERROR_OK;
+}
+
+int armv7m_trace_itm_config(struct target *target)
+{
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	struct armv7m_trace_config *trace_config = &armv7m->trace_config;
+	int retval;
+
+	retval = target_write_u32(target, ITM_LAR, ITM_LAR_KEY);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Enable ITM, TXENA, set TraceBusID and other parameters */
+	retval = target_write_u32(target, ITM_TCR, (1 << 0) | (1 << 3) |
+				  (trace_config->itm_diff_timestamps << 1) |
+				  (trace_config->itm_synchro_packets << 2) |
+				  (trace_config->itm_async_timestamps << 4) |
+				  (trace_config->itm_ts_prescale << 8) |
+				  (trace_config->trace_bus_id << 16));
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (unsigned int i = 0; i < 8; i++) {
+		retval = target_write_u32(target, ITM_TER0 + i * 4,
+					  trace_config->itm_ter[i]);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static void close_trace_file(struct armv7m_common *armv7m)
+{
+	if (armv7m->trace_config.trace_file)
+		fclose(armv7m->trace_config.trace_file);
+	armv7m->trace_config.trace_file = NULL;
+}
+
+COMMAND_HANDLER(handle_tpiu_config_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+
+	unsigned int cmd_idx = 0;
+
+	if (CMD_ARGC == cmd_idx)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (!strcmp(CMD_ARGV[cmd_idx], "disable")) {
+		if (CMD_ARGC == cmd_idx + 1) {
+			close_trace_file(armv7m);
+
+			armv7m->trace_config.config_type = DISABLED;
+			if (CMD_CTX->mode == COMMAND_EXEC)
+				return armv7m_trace_tpiu_config(target);
+			else
+				return ERROR_OK;
+		}
+	} else if (!strcmp(CMD_ARGV[cmd_idx], "external") ||
+		   !strcmp(CMD_ARGV[cmd_idx], "internal")) {
+		close_trace_file(armv7m);
+
+		armv7m->trace_config.config_type = EXTERNAL;
+		if (!strcmp(CMD_ARGV[cmd_idx], "internal")) {
+			cmd_idx++;
+			if (CMD_ARGC == cmd_idx)
+				return ERROR_COMMAND_SYNTAX_ERROR;
+
+			armv7m->trace_config.config_type = INTERNAL;
+			armv7m->trace_config.trace_file = fopen(CMD_ARGV[cmd_idx], "ab");
+			if (!armv7m->trace_config.trace_file) {
+				LOG_ERROR("Can't open trace destination file");
+				return ERROR_FAIL;
+			}
+		}
+		cmd_idx++;
+		if (CMD_ARGC == cmd_idx)
+			return ERROR_COMMAND_SYNTAX_ERROR;
+
+		if (!strcmp(CMD_ARGV[cmd_idx], "sync")) {
+			armv7m->trace_config.pin_protocol = SYNC;
+
+			cmd_idx++;
+			if (CMD_ARGC == cmd_idx)
+				return ERROR_COMMAND_SYNTAX_ERROR;
+
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[cmd_idx], armv7m->trace_config.port_size);
+		} else {
+			if (!strcmp(CMD_ARGV[cmd_idx], "manchester"))
+				armv7m->trace_config.pin_protocol = ASYNC_MANCHESTER;
+			else if (!strcmp(CMD_ARGV[cmd_idx], "uart"))
+				armv7m->trace_config.pin_protocol = ASYNC_UART;
+			else
+				return ERROR_COMMAND_SYNTAX_ERROR;
+
+			cmd_idx++;
+			if (CMD_ARGC == cmd_idx)
+				return ERROR_COMMAND_SYNTAX_ERROR;
+
+			COMMAND_PARSE_ON_OFF(CMD_ARGV[cmd_idx], armv7m->trace_config.formatter);
+		}
+
+		cmd_idx++;
+		if (CMD_ARGC == cmd_idx)
+			return ERROR_COMMAND_SYNTAX_ERROR;
+
+		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[cmd_idx], armv7m->trace_config.traceclkin_freq);
+
+		cmd_idx++;
+		if (CMD_ARGC != cmd_idx) {
+			COMMAND_PARSE_NUMBER(uint, CMD_ARGV[cmd_idx], armv7m->trace_config.trace_freq);
+			cmd_idx++;
+		} else {
+			if (armv7m->trace_config.config_type != INTERNAL) {
+				LOG_ERROR("Trace port frequency can't be omitted in external capture mode");
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			}
+			armv7m->trace_config.trace_freq = 0;
+		}
+
+		if (CMD_ARGC == cmd_idx) {
+			if (CMD_CTX->mode == COMMAND_EXEC)
+				return armv7m_trace_tpiu_config(target);
+			else
+				return ERROR_OK;
+		}
+	}
+
+	return ERROR_COMMAND_SYNTAX_ERROR;
+}
+
+COMMAND_HANDLER(handle_itm_port_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	unsigned int reg_idx;
+	uint8_t port;
+	bool enable;
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0], port);
+	COMMAND_PARSE_ON_OFF(CMD_ARGV[1], enable);
+	reg_idx = port / 32;
+	port = port % 32;
+	if (enable)
+		armv7m->trace_config.itm_ter[reg_idx] |= (1 << port);
+	else
+		armv7m->trace_config.itm_ter[reg_idx] &= ~(1 << port);
+
+	if (CMD_CTX->mode == COMMAND_EXEC)
+		return armv7m_trace_itm_config(target);
+	else
+		return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_itm_ports_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	bool enable;
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_ON_OFF(CMD_ARGV[0], enable);
+	memset(armv7m->trace_config.itm_ter, enable ? 0xff : 0,
+	       sizeof(armv7m->trace_config.itm_ter));
+
+	if (CMD_CTX->mode == COMMAND_EXEC)
+		return armv7m_trace_itm_config(target);
+	else
+		return ERROR_OK;
+}
+
+static const struct command_registration tpiu_command_handlers[] = {
+	{
+		.name = "config",
+		.handler = handle_tpiu_config_command,
+		.mode = COMMAND_ANY,
+		.help = "Configure TPIU features",
+		.usage = "(disable | "
+		"((external | internal <filename>) "
+		"(sync <port width> | ((manchester | uart) <formatter enable>)) "
+		"<TRACECLKIN freq> [<trace freq>]))",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration itm_command_handlers[] = {
+	{
+		.name = "port",
+		.handler = handle_itm_port_command,
+		.mode = COMMAND_ANY,
+		.help = "Enable or disable ITM stimulus port",
+		.usage = "<port> (0|1|on|off)",
+	},
+	{
+		.name = "ports",
+		.handler = handle_itm_ports_command,
+		.mode = COMMAND_ANY,
+		.help = "Enable or disable all ITM stimulus ports",
+		.usage = "(0|1|on|off)",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+const struct command_registration armv7m_trace_command_handlers[] = {
+	{
+		.name = "tpiu",
+		.mode = COMMAND_ANY,
+		.help = "tpiu command group",
+		.usage = "",
+		.chain = tpiu_command_handlers,
+	},
+	{
+		.name = "itm",
+		.mode = COMMAND_ANY,
+		.help = "itm command group",
+		.usage = "",
+		.chain = itm_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
diff --git a/src/target/armv7m_trace.h b/src/target/armv7m_trace.h
new file mode 100644
index 0000000..96c1895
--- /dev/null
+++ b/src/target/armv7m_trace.h
@@ -0,0 +1,87 @@
+/***************************************************************************
+ *   Copyright (C) 2015  Paul Fertser <fercerpav@gmail.com>                *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ ***************************************************************************/
+
+#ifndef ARMV7M_TRACE_H
+#define ARMV7M_TRACE_H
+
+#include <command.h>
+
+/**
+ * @file
+ * Holds the interface to TPIU, ITM and DWT configuration functions.
+ */
+
+enum trace_config_type {
+	DISABLED,	/**< tracing is disabled */
+	EXTERNAL,	/**< trace output is captured externally */
+	INTERNAL	/**< trace output is handled by OpenOCD adapter driver */
+};
+
+enum tpio_pin_protocol {
+	SYNC,			/**< synchronous trace output */
+	ASYNC_MANCHESTER,	/**< asynchronous output with Manchester coding */
+	ASYNC_UART		/**< asynchronous output with NRZ coding */
+};
+
+enum itm_ts_prescaler {
+	ITM_TS_PRESCALE1,	/**< no prescaling for the timestamp counter */
+	ITM_TS_PRESCALE4,	/**< refclock divided by 4 for the timestamp counter */
+	ITM_TS_PRESCALE16,	/**< refclock divided by 16 for the timestamp counter */
+	ITM_TS_PRESCALE64,	/**< refclock divided by 64 for the timestamp counter */
+};
+
+struct armv7m_trace_config {
+	/** Currently active trace capture mode */
+	enum trace_config_type config_type;
+
+	/** Currently active trace output mode */
+	enum tpio_pin_protocol pin_protocol;
+	/** TPIU formatter enable/disable (in async mode) */
+	bool formatter;
+	/** Synchronous output port width */
+	uint32_t port_size;
+
+	/** Bitmask of currenty enabled ITM stimuli */
+	uint32_t itm_ter[8];
+	/** Identifier for multi-source trace stream formatting */
+	unsigned int trace_bus_id;
+	/** Prescaler for the timestamp counter */
+	enum itm_ts_prescaler itm_ts_prescale;
+	/** Enable differential timestamps */
+	bool itm_diff_timestamps;
+	/** Enable async timestamps model */
+	bool itm_async_timestamps;
+	/** Enable synchronisation packet transmission (for sync port only) */
+	bool itm_synchro_packets;
+
+	/** Current frequency of TRACECLKIN (usually matches HCLK) */
+	unsigned int traceclkin_freq;
+	/** Current frequency of trace port */
+	unsigned int trace_freq;
+	/** Handle to output trace data in INTERNAL capture mode */
+	FILE *trace_file;
+};
+
+extern const struct command_registration armv7m_trace_command_handlers[];
+
+/**
+ * Configure hardware accordingly to the current TPIU target settings
+ */
+int armv7m_trace_tpiu_config(struct target *target);
+/**
+ * Configure hardware accordingly to the current ITM target settings
+ */
+int armv7m_trace_itm_config(struct target *target);
+
+#endif
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 4dc92c8..9234824 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1932,6 +1932,16 @@ int cortex_m_examine(struct target *target)
 			armv7m->dap.tar_autoincr_block = (1 << 12);
 		}
 
+		/* Configure trace modules */
+		retval = target_write_u32(target, DCB_DEMCR, TRCENA | armv7m->demcr);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (armv7m->trace_config.config_type != DISABLED) {
+			armv7m_trace_tpiu_config(target);
+			armv7m_trace_itm_config(target);
+		}
+
 		/* NOTE: FPB and DWT are both optional. */
 
 		/* Setup FPB */
@@ -2325,6 +2335,9 @@ static const struct command_registration cortex_m_command_handlers[] = {
 		.chain = armv7m_command_handlers,
 	},
 	{
+		.chain = armv7m_trace_command_handlers,
+	},
+	{
 		.name = "cortex_m",
 		.mode = COMMAND_EXEC,
 		.help = "Cortex-M command group",
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index 28189e0..028b4c8 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -33,10 +33,11 @@
 
 #define SYSTEM_CONTROL_BASE 0x400FE000
 
-#define ITM_TER		0xE0000E00
+#define ITM_TER0	0xE0000E00
 #define ITM_TPR		0xE0000E40
 #define ITM_TCR		0xE0000E80
 #define ITM_LAR		0xE0000FB0
+#define ITM_LAR_KEY	0xC5ACCE55
 
 #define CPUID		0xE000ED00
 /* Debug Control Block */
@@ -69,13 +70,13 @@
 #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
+#define TPIU_SSPSR	0xE0040000
+#define TPIU_CSPSR	0xE0040004
+#define TPIU_ACPR	0xE0040010
+#define TPIU_SPPR	0xE00400F0
+#define TPIU_FFSR	0xE0040300
+#define TPIU_FFCR	0xE0040304
+#define TPIU_FSCR	0xE0040308
 
 /* DCB_DHCSR bit and field definitions */
 #define DBGKEY		(0xA05F << 16)
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index a05a99f..d0be966 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -772,6 +772,9 @@ static const struct command_registration adapter_command_handlers[] = {
 	{
 		.chain = arm_command_handlers,
 	},
+	{
+		.chain = armv7m_trace_command_handlers,
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/target.c b/src/target/target.c
index 5c88384..a8d3cba 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -217,6 +217,8 @@ static const Jim_Nvp nvp_target_event[] = {
 	{ .value = TARGET_EVENT_GDB_FLASH_ERASE_START, .name = "gdb-flash-erase-start" },
 	{ .value = TARGET_EVENT_GDB_FLASH_ERASE_END  , .name = "gdb-flash-erase-end" },
 
+	{ .value = TARGET_EVENT_TRACE_CONFIG, .name = "trace-config" },
+
 	{ .name = NULL, .value = -1 }
 };
 
diff --git a/src/target/target.h b/src/target/target.h
index fbce19f..9382720 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -266,6 +266,8 @@ enum target_event {
 	TARGET_EVENT_GDB_FLASH_ERASE_END,
 	TARGET_EVENT_GDB_FLASH_WRITE_START,
 	TARGET_EVENT_GDB_FLASH_WRITE_END,
+
+	TARGET_EVENT_TRACE_CONFIG,
 };
 
 struct target_event_action {