Merge branch 'master' into from_upstream

Conflicts:
	.github/workflows/snapshot.yml
	.gitmodules
	src/flash/nor/drivers.c
	src/helper/jep106.inc
	src/rtos/hwthread.c
	src/target/riscv/riscv.c
	src/target/target.c

Change-Id: I62f65e10d15dcda4c405d4042cce1d96f8e1680a

50 files changed, 1818 insertions, 990 deletions

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 19ba771..1d30747 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -48,7 +48,8 @@ TARGET_CORE_SRC = \
 	%D%/target_request.c \
 	%D%/testee.c \
 	%D%/semihosting_common.c \
-	%D%/smp.c
+	%D%/smp.c \
+	%D%/rtt.c
 
 ARMV4_5_SRC = \
 	%D%/armv4_5.c \
@@ -259,7 +260,8 @@ ARC_SRC = \
 	%D%/arc.h \
 	%D%/arc_cmd.h \
 	%D%/arc_jtag.h \
-	%D%/arc_mem.h
+	%D%/arc_mem.h \
+	%D%/rtt.h
 
 include %D%/openrisc/Makefile.am
 include %D%/riscv/Makefile.am
diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index c2100eb..6dede97 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -145,7 +145,7 @@ struct dap_cmd {
 struct dap_cmd_pool {
 	struct list_head lh;
 	struct dap_cmd cmd;
-} dap_cmd_pool;
+};
 
 static void log_dap_cmd(const char *header, struct dap_cmd *el)
 {
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index ee30ff7..b25181e 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -118,26 +118,69 @@ static int swd_connect(struct adiv5_dap *dap)
 		}
 	}
 
-	/* Note, debugport_init() does setup too */
-	swd->switch_seq(JTAG_TO_SWD);
 
-	/* Clear link state, including the SELECT cache. */
-	dap->do_reconnect = false;
-	dap_invalidate_cache(dap);
+	int64_t timeout = timeval_ms() + 500;
 
-	swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+	do {
+		/* Note, debugport_init() does setup too */
+		swd->switch_seq(JTAG_TO_SWD);
 
-	/* force clear all sticky faults */
-	swd_clear_sticky_errors(dap);
+		/* Clear link state, including the SELECT cache. */
+		dap->do_reconnect = false;
+		dap_invalidate_cache(dap);
+
+		status = swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+		if (status == ERROR_OK) {
+			status = swd_run_inner(dap);
+			if (status == ERROR_OK)
+				break;
+		}
+
+		alive_sleep(1);
+
+	} while (timeval_ms() < timeout);
+
+	if (status != ERROR_OK) {
+		LOG_ERROR("Error connecting DP: cannot read IDR");
+		return status;
+	}
+
+	LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+
+	do {
+		dap->do_reconnect = false;
 
-	status = swd_run_inner(dap);
+		/* force clear all sticky faults */
+		swd_clear_sticky_errors(dap);
+
+		status = swd_run_inner(dap);
+		if (status != ERROR_WAIT)
+			break;
+
+		alive_sleep(10);
+
+	} while (timeval_ms() < timeout);
+
+	/* IHI 0031E B4.3.2:
+	 * "A WAIT response must not be issued to the ...
+	 * ... writes to the ABORT register"
+	 * swd_clear_sticky_errors() writes to the ABORT register only.
+	 *
+	 * Unfortunately at least Microchip SAMD51/E53/E54 returns WAIT
+	 * in a corner case. Just try if ABORT resolves the problem.
+	 */
+	if (status == ERROR_WAIT) {
+		LOG_WARNING("Connecting DP: stalled AP operation, issuing ABORT");
 
-	if (status == ERROR_OK) {
-		LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
 		dap->do_reconnect = false;
+
+		swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+			DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+		status = swd_run_inner(dap);
+	}
+
+	if (status == ERROR_OK)
 		status = dap_dp_init(dap);
-	} else
-		dap->do_reconnect = true;
 
 	return status;
 }
diff --git a/src/target/arc.c b/src/target/arc.c
index e1b5764..ffe9745 100644
--- a/src/target/arc.c
+++ b/src/target/arc.c
@@ -48,6 +48,8 @@
  */
 
 
+static int arc_remove_watchpoint(struct target *target,
+	struct watchpoint *watchpoint);
 
 void arc_reg_data_type_add(struct target *target,
 		struct arc_reg_data_type *data_type)
@@ -303,7 +305,7 @@ static int arc_init_reg(struct target *target, struct reg *reg,
 	/* Initialize struct reg */
 	reg->name = reg_desc->name;
 	reg->size = 32; /* All register in ARC are 32-bit */
-	reg->value = &reg_desc->reg_value;
+	reg->value = reg_desc->reg_value;
 	reg->type = &arc_reg_type;
 	reg->arch_info = reg_desc;
 	reg->caller_save = true; /* @todo should be configurable. */
@@ -1696,6 +1698,7 @@ void arc_reset_actionpoints(struct target *target)
 	struct arc_common *arc = target_to_arc(target);
 	struct arc_actionpoint *ap_list = arc->actionpoints_list;
 	struct breakpoint *next_b;
+	struct watchpoint *next_w;
 
 	while (target->breakpoints) {
 		next_b = target->breakpoints->next;
@@ -1704,6 +1707,12 @@ void arc_reset_actionpoints(struct target *target)
 		free(target->breakpoints);
 		target->breakpoints = next_b;
 	}
+	while (target->watchpoints) {
+		next_w = target->watchpoints->next;
+		arc_remove_watchpoint(target, target->watchpoints);
+		free(target->watchpoints);
+		target->watchpoints = next_w;
+	}
 	for (unsigned int i = 0; i < arc->actionpoints_num; i++) {
 		if ((ap_list[i].used) && (ap_list[i].reg_address))
 			arc_remove_auxreg_actionpoint(target, ap_list[i].reg_address);
@@ -1800,6 +1809,159 @@ int arc_remove_auxreg_actionpoint(struct target *target, uint32_t auxreg_addr)
 	return retval;
 }
 
+
+static int arc_set_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	unsigned int wp_num;
+	struct arc_common *arc = target_to_arc(target);
+	struct arc_actionpoint *ap_list = arc->actionpoints_list;
+
+	if (watchpoint->set) {
+		LOG_WARNING("watchpoint already set");
+		return ERROR_OK;
+	}
+
+	for (wp_num = 0; wp_num < arc->actionpoints_num; wp_num++) {
+		if (!ap_list[wp_num].used)
+			break;
+	}
+
+	if (wp_num >= arc->actionpoints_num) {
+		LOG_ERROR("No free actionpoints, maximum amount is %u",
+				arc->actionpoints_num);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (watchpoint->length != 4) {
+		LOG_ERROR("Only watchpoints of length 4 are supported");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	int enable = AP_AC_TT_DISABLE;
+	switch (watchpoint->rw) {
+		case WPT_READ:
+			enable = AP_AC_TT_READ;
+			break;
+		case WPT_WRITE:
+			enable = AP_AC_TT_WRITE;
+			break;
+		case WPT_ACCESS:
+			enable = AP_AC_TT_READWRITE;
+			break;
+		default:
+			LOG_ERROR("BUG: watchpoint->rw neither read, write nor access");
+			return ERROR_FAIL;
+	}
+
+	int retval =  arc_configure_actionpoint(target, wp_num,
+					watchpoint->address, enable, AP_AC_AT_MEMORY_ADDR);
+
+	if (retval == ERROR_OK) {
+		watchpoint->set = wp_num + 1;
+		ap_list[wp_num].used = 1;
+		ap_list[wp_num].bp_value = watchpoint->address;
+		ap_list[wp_num].type = ARC_AP_WATCHPOINT;
+
+		LOG_DEBUG("wpid: %" PRIu32 ", wp_num %u wp_value 0x%" PRIx32,
+				watchpoint->unique_id, wp_num, ap_list[wp_num].bp_value);
+	}
+
+	return retval;
+}
+
+static int arc_unset_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	/* get pointers to arch-specific information */
+	struct arc_common *arc = target_to_arc(target);
+	struct arc_actionpoint *ap_list = arc->actionpoints_list;
+
+	if (!watchpoint->set) {
+		LOG_WARNING("watchpoint not set");
+		return ERROR_OK;
+	}
+
+	unsigned int wp_num = watchpoint->set - 1;
+	if ((watchpoint->set == 0) || (wp_num >= arc->actionpoints_num)) {
+		LOG_DEBUG("Invalid actionpoint ID: %u in watchpoint: %" PRIu32,
+				wp_num, watchpoint->unique_id);
+		return ERROR_OK;
+	}
+
+	int retval =  arc_configure_actionpoint(target, wp_num,
+				watchpoint->address, AP_AC_TT_DISABLE, AP_AC_AT_MEMORY_ADDR);
+
+	if (retval == ERROR_OK) {
+		watchpoint->set = 0;
+		ap_list[wp_num].used = 0;
+		ap_list[wp_num].bp_value = 0;
+
+		LOG_DEBUG("wpid: %" PRIu32 " - releasing actionpoint ID: %u",
+				watchpoint->unique_id, wp_num);
+	}
+
+	return retval;
+}
+
+static int arc_add_watchpoint(struct target *target,
+	struct watchpoint *watchpoint)
+{
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	CHECK_RETVAL(arc_set_watchpoint(target, watchpoint));
+
+	return ERROR_OK;
+}
+
+static int arc_remove_watchpoint(struct target *target,
+	struct watchpoint *watchpoint)
+{
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (watchpoint->set)
+		CHECK_RETVAL(arc_unset_watchpoint(target, watchpoint));
+
+	return ERROR_OK;
+}
+
+static int arc_hit_watchpoint(struct target *target, struct watchpoint **hit_watchpoint)
+{
+	assert(target);
+	assert(hit_watchpoint);
+
+	struct arc_actionpoint *actionpoint = NULL;
+	CHECK_RETVAL(get_current_actionpoint(target, &actionpoint));
+
+	if (actionpoint != NULL) {
+		if (!actionpoint->used)
+			LOG_WARNING("Target halted by unused actionpoint.");
+
+		/* If this check fails - that is some sort of an error in OpenOCD. */
+		if (actionpoint->type != ARC_AP_WATCHPOINT)
+			LOG_WARNING("Target halted by breakpoint, but is treated as a watchpoint.");
+
+		for (struct watchpoint *watchpoint = target->watchpoints;
+				watchpoint != NULL;
+				watchpoint = watchpoint->next) {
+			if (actionpoint->bp_value == watchpoint->address) {
+				*hit_watchpoint = watchpoint;
+				LOG_DEBUG("Hit watchpoint, wpid: %" PRIu32 ", watchpoint num: %i",
+							watchpoint->unique_id, watchpoint->set - 1);
+				return ERROR_OK;
+			}
+		}
+	}
+
+	return ERROR_FAIL;
+}
+
 /* Helper function which switches core to single_step mode by
  * doing aux r/w operations.  */
 int arc_config_step(struct target *target, int enable_step)
@@ -2106,9 +2268,9 @@ struct target_type arcv2_target = {
 	.add_context_breakpoint = NULL,
 	.add_hybrid_breakpoint = NULL,
 	.remove_breakpoint = arc_remove_breakpoint,
-	.add_watchpoint = NULL,
-	.remove_watchpoint = NULL,
-	.hit_watchpoint = NULL,
+	.add_watchpoint = arc_add_watchpoint,
+	.remove_watchpoint = arc_remove_watchpoint,
+	.hit_watchpoint = arc_hit_watchpoint,
 
 	.run_algorithm = NULL,
 	.start_algorithm = NULL,
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index d70d273..797f61c 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -1391,6 +1391,11 @@ static int arm7_9_full_context(struct target *target)
 	int retval;
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	struct arm *arm = &arm7_9->arm;
+	struct {
+		uint32_t value;
+		uint8_t *reg_p;
+	} read_cache[6 * (16 + 1)];
+	int read_cache_idx = 0;
 
 	LOG_DEBUG("-");
 
@@ -1433,10 +1438,12 @@ static int arm7_9_full_context(struct target *target)
 			for (j = 0; j < 15; j++) {
 				if (!ARMV4_5_CORE_REG_MODE(arm->core_cache,
 						armv4_5_number_to_mode(i), j).valid) {
-					reg_p[j] = (uint32_t *)ARMV4_5_CORE_REG_MODE(
+					read_cache[read_cache_idx].reg_p = ARMV4_5_CORE_REG_MODE(
 							arm->core_cache,
 							armv4_5_number_to_mode(i),
 							j).value;
+					reg_p[j] = &read_cache[read_cache_idx].value;
+					read_cache_idx++;
 					mask |= 1 << j;
 					ARMV4_5_CORE_REG_MODE(arm->core_cache,
 						armv4_5_number_to_mode(i),
@@ -1454,9 +1461,10 @@ static int arm7_9_full_context(struct target *target)
 			/* check if the PSR has to be read */
 			if (!ARMV4_5_CORE_REG_MODE(arm->core_cache, armv4_5_number_to_mode(i),
 					16).valid) {
-				arm7_9->read_xpsr(target,
-					(uint32_t *)ARMV4_5_CORE_REG_MODE(arm->core_cache,
-						armv4_5_number_to_mode(i), 16).value, 1);
+				read_cache[read_cache_idx].reg_p = ARMV4_5_CORE_REG_MODE(arm->core_cache,
+					armv4_5_number_to_mode(i), 16).value;
+				arm7_9->read_xpsr(target, &read_cache[read_cache_idx].value, 1);
+				read_cache_idx++;
 				ARMV4_5_CORE_REG_MODE(arm->core_cache, armv4_5_number_to_mode(i),
 					16).valid = true;
 				ARMV4_5_CORE_REG_MODE(arm->core_cache, armv4_5_number_to_mode(i),
@@ -1472,6 +1480,14 @@ static int arm7_9_full_context(struct target *target)
 	retval = jtag_execute_queue();
 	if (retval != ERROR_OK)
 		return retval;
+	/*
+	 * FIXME: regs in cache should be tagged as 'valid' only now,
+	 * not before the jtag_execute_queue()
+	 */
+	while (read_cache_idx) {
+		read_cache_idx--;
+		buf_set_u32(read_cache[read_cache_idx].reg_p, 0, 32, read_cache[read_cache_idx].value);
+	}
 	return ERROR_OK;
 }
 
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index 01685ab..10263f4 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -115,11 +115,9 @@ static inline int arm7tdmi_clock_out_inner(struct arm_jtag *jtag_info, uint32_t
 
 /* put an instruction in the ARM7TDMI pipeline or write the data bus,
  * and optionally read data
- *
- * FIXME remove the unused "deprecated" parameter
  */
 static inline int arm7tdmi_clock_out(struct arm_jtag *jtag_info,
-		uint32_t out, uint32_t *deprecated, int breakpoint)
+		uint32_t out, int breakpoint)
 {
 	int retval;
 	retval = arm_jtag_scann(jtag_info, 0x1, TAP_DRPAUSE);
@@ -246,35 +244,35 @@ static void arm7tdmi_change_to_arm(struct target *target,
 	 * to allow common handling of ARM and THUMB debugging */
 
 	/* fetch STR r0, [r0] */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, STR r0, [r0] in Execute (2) */
 	arm7tdmi_clock_data_in(jtag_info, r0);
 
 	/* MOV r0, r15 fetched, STR in Decode */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, STR r0, [r0] in Execute (2) */
 	arm7tdmi_clock_data_in(jtag_info, pc);
 
 	/* use pc-relative LDR to clear r0[1:0] (for switch to ARM mode) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, data for LDR r0, [PC, #0] */
-	arm7tdmi_clock_out(jtag_info, 0x0, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, 0x0, 0);
 	/* nothing fetched, data from previous cycle is written to register */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
 	/* fetch BX */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), 0);
 	/* NOP fetched, BX in Decode, MOV in Execute */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* NOP fetched, BX in Execute (1) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
 	jtag_execute_queue();
 
@@ -301,12 +299,12 @@ static void arm7tdmi_read_core_regs(struct target *target,
 	/* STMIA r0-15, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0);
 
 	/* fetch NOP, STM in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, STM in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	for (i = 0; i <= 15; i++) {
 		if (mask & (1 << i))
@@ -329,12 +327,12 @@ static void arm7tdmi_read_core_regs_target_buffer(struct target *target,
 	/* STMIA r0-15, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0);
 
 	/* fetch NOP, STM in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, STM in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	for (i = 0; i <= 15; i++) {
 		/* nothing fetched, STM still in EXECUTE (1 + i cycle), read databus */
@@ -360,14 +358,14 @@ static void arm7tdmi_read_xpsr(struct target *target, uint32_t *xpsr, int spsr)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* MRS r0, cpsr */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), 0);
 
 	/* STR r0, [r15] */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), 0);
 	/* fetch NOP, STR in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, STR in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, STR still in EXECUTE (2nd cycle) */
 	arm7tdmi_clock_data_in(jtag_info, xpsr);
 }
@@ -380,25 +378,25 @@ static void arm7tdmi_write_xpsr(struct target *target, uint32_t xpsr, int spsr)
 	LOG_DEBUG("xpsr: %8.8" PRIx32 ", spsr: %i", xpsr, spsr);
 
 	/* MSR1 fetched */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), 0);
 	/* MSR2 fetched, MSR1 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), 0);
 	/* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), 0);
 	/* nothing fetched, MSR1 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), 0);
 	/* nothing fetched, MSR2 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, MSR3 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* NOP fetched, MSR4 in EXECUTE (1) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, MSR4 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 }
 
 static void arm7tdmi_write_xpsr_im8(struct target *target,
@@ -410,13 +408,13 @@ static void arm7tdmi_write_xpsr_im8(struct target *target,
 	LOG_DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);
 
 	/* MSR fetched */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), 0);
 	/* NOP fetched, MSR in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* NOP fetched, MSR in EXECUTE (1) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, MSR in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 }
 
 static void arm7tdmi_write_core_regs(struct target *target,
@@ -429,7 +427,7 @@ static void arm7tdmi_write_core_regs(struct target *target,
 	/* LDMIA r0-15, [r0] at debug speed
 	* register values will start to appear on 4th DCLK
 	*/
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), 0);
 
 	/* fetch NOP, LDM in DECODE stage */
 	arm7tdmi_clock_out_inner(jtag_info, ARMV4_5_NOP, 0);
@@ -450,9 +448,9 @@ static void arm7tdmi_load_word_regs(struct target *target, uint32_t mask)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed load-multiple into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), 0);
 }
 
 static void arm7tdmi_load_hword_reg(struct target *target, int num)
@@ -461,9 +459,9 @@ static void arm7tdmi_load_hword_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed load half-word into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), 0);
 }
 
 static void arm7tdmi_load_byte_reg(struct target *target, int num)
@@ -472,9 +470,9 @@ static void arm7tdmi_load_byte_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed load byte into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), 0);
 }
 
 static void arm7tdmi_store_word_regs(struct target *target, uint32_t mask)
@@ -483,9 +481,9 @@ static void arm7tdmi_store_word_regs(struct target *target, uint32_t mask)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed store-multiple into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), 0);
 }
 
 static void arm7tdmi_store_hword_reg(struct target *target, int num)
@@ -494,9 +492,9 @@ static void arm7tdmi_store_hword_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed store half-word into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), 0);
 }
 
 static void arm7tdmi_store_byte_reg(struct target *target, int num)
@@ -505,9 +503,9 @@ static void arm7tdmi_store_byte_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed store byte into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), 0);
 }
 
 static void arm7tdmi_write_pc(struct target *target, uint32_t pc)
@@ -518,7 +516,7 @@ static void arm7tdmi_write_pc(struct target *target, uint32_t pc)
 	/* LDMIA r0-15, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), 0);
 	/* fetch NOP, LDM in DECODE stage */
 	arm7tdmi_clock_out_inner(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
@@ -540,7 +538,7 @@ static void arm7tdmi_branch_resume(struct target *target)
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
 	arm7tdmi_clock_out_inner(jtag_info, ARMV4_5_B(0xfffffa, 0), 0);
 }
 
@@ -556,53 +554,52 @@ static void arm7tdmi_branch_resume_thumb(struct target *target)
 	/* LDMIA r0, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), 0);
 
 	/* fetch NOP, LDM in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, LDM in EXECUTE stage (2nd cycle) */
-	arm7tdmi_clock_out(jtag_info,
-			buf_get_u32(arm->pc->value, 0, 32) | 1, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, buf_get_u32(arm->pc->value, 0, 32) | 1, 0);
 	/* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	/* Branch and eXchange */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_BX(0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_BX(0), 0);
 
 	embeddedice_read_reg(dbg_stat);
 
 	/* fetch NOP, BX in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	/* target is now in Thumb state */
 	embeddedice_read_reg(dbg_stat);
 
 	/* fetch NOP, BX in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	/* target is now in Thumb state */
 	embeddedice_read_reg(dbg_stat);
 
 	/* load r0 value */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0);
 	/* fetch NOP, LDR in Decode */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* fetch NOP, LDR in Execute */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, LDR in EXECUTE stage (2nd cycle) */
-	arm7tdmi_clock_out(jtag_info, buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32), 0);
 	/* nothing fetched, LDR in EXECUTE stage (3rd cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
 	embeddedice_read_reg(dbg_stat);
 
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f8), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f8), 0);
 }
 
 static void arm7tdmi_build_reg_cache(struct target *target)
diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index 95a4f7c..21fd689 100644
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -723,7 +723,7 @@ static int arm926ejs_target_create(struct target *target, Jim_Interp *interp)
 	return arm926ejs_init_arch_info(target, arm926ejs, target->tap);
 }
 
-void arm926ejs_deinit_target(struct target *target)
+static void arm926ejs_deinit_target(struct target *target)
 {
 	struct arm *arm = target_to_arm(target);
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index 8754c86..036e8ba 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -51,8 +51,8 @@
  */
 static uint8_t arm946e_preserve_cache;
 
-int arm946e_post_debug_entry(struct target *target);
-void arm946e_pre_restore_context(struct target *target);
+static int arm946e_post_debug_entry(struct target *target);
+static void arm946e_pre_restore_context(struct target *target);
 static int arm946e_read_cp15(struct target *target, int reg_addr, uint32_t *value);
 
 int arm946e_init_arch_info(struct target *target,
@@ -250,7 +250,7 @@ static uint32_t arm946e_cp15_get_csize(struct target *target, int idsel)
 	return csize ? 1 << (12 + (csize-3)) : 0;
 }
 
-uint32_t arm946e_invalidate_whole_dcache(struct target *target)
+static uint32_t arm946e_invalidate_whole_dcache(struct target *target)
 {
 	uint32_t csize = arm946e_cp15_get_csize(target, GET_DCACHE_SIZE);
 	if (csize == 0)
@@ -306,7 +306,7 @@ uint32_t arm946e_invalidate_whole_dcache(struct target *target)
 	return ERROR_OK;
 }
 
-uint32_t arm946e_invalidate_whole_icache(struct target *target)
+static uint32_t arm946e_invalidate_whole_icache(struct target *target)
 {
 	/* Check cache presence before flushing - avoid undefined behavior */
 	uint32_t csize = arm946e_cp15_get_csize(target, GET_ICACHE_SIZE);
@@ -327,7 +327,7 @@ uint32_t arm946e_invalidate_whole_icache(struct target *target)
 	return ERROR_OK;
 }
 
-int arm946e_post_debug_entry(struct target *target)
+static int arm946e_post_debug_entry(struct target *target)
 {
 	uint32_t ctr_reg = 0x0;
 	uint32_t retval = ERROR_OK;
@@ -368,7 +368,7 @@ int arm946e_post_debug_entry(struct target *target)
 	return ERROR_OK;
 }
 
-void arm946e_pre_restore_context(struct target *target)
+static void arm946e_pre_restore_context(struct target *target)
 {
 	uint32_t ctr_reg = 0x0;
 	uint32_t retval;
@@ -393,7 +393,7 @@ void arm946e_pre_restore_context(struct target *target)
 	}	/* if preserve_cache */
 }
 
-uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
+static uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
 	uint32_t size, uint32_t count)
 {
 	uint32_t cur_addr = 0x0;
@@ -458,7 +458,7 @@ uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
+static uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
 	uint32_t size, uint32_t count)
 {
 	uint32_t cur_addr = 0x0;
@@ -509,7 +509,7 @@ uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm946e_write_memory(struct target *target, target_addr_t address,
+static int arm946e_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
@@ -557,7 +557,7 @@ int arm946e_write_memory(struct target *target, target_addr_t address,
 
 }
 
-int arm946e_read_memory(struct target *target, target_addr_t address,
+static int arm946e_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index a09e269..59bb186 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -652,35 +652,22 @@ int dap_dp_init(struct adiv5_dap *dap)
 
 	LOG_DEBUG("%s", adiv5_dap_name(dap));
 
+	dap->do_reconnect = false;
 	dap_invalidate_cache(dap);
 
 	/*
 	 * Early initialize dap->dp_ctrl_stat.
-	 * In jtag mode only, if the following atomic reads fail and set the
-	 * sticky error, it will trigger the clearing of the sticky. Without this
-	 * initialization system and debug power would be disabled while clearing
-	 * the sticky error bit.
+	 * In jtag mode only, if the following queue run (in dap_dp_poll_register)
+	 * fails and sets the sticky error, it will trigger the clearing
+	 * of the sticky. Without this initialization system and debug power
+	 * would be disabled while clearing the sticky error bit.
 	 */
 	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 
-	for (size_t i = 0; i < 30; i++) {
-		/* DP initialization */
-
-		retval = dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
-		if (retval == ERROR_OK)
-			break;
-	}
-
 	/*
 	 * This write operation clears the sticky error bit in jtag mode only and
 	 * is ignored in swd mode. It also powers-up system and debug domains in
 	 * both jtag and swd modes, if not done before.
-	 * Actually we do not need to clear the sticky error here because it has
-	 * been already cleared (if it was set) in the previous atomic read. This
-	 * write could be removed, but this initial part of dap_dp_init() is the
-	 * result of years of fine tuning and there are strong concerns about any
-	 * unnecessary code change. It doesn't harm, so let's keep it here and
-	 * preserve the historical sequence of read/write operations!
 	 */
 	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat | SSTICKYERR);
 	if (retval != ERROR_OK)
@@ -732,6 +719,35 @@ int dap_dp_init(struct adiv5_dap *dap)
 }
 
 /**
+ * Initialize a DAP or do reconnect if DAP is not accessible.
+ *
+ * @param dap The DAP being initialized.
+ */
+int dap_dp_init_or_reconnect(struct adiv5_dap *dap)
+{
+	LOG_DEBUG("%s", adiv5_dap_name(dap));
+
+	/*
+	 * Early initialize dap->dp_ctrl_stat.
+	 * In jtag mode only, if the following atomic reads fail and set the
+	 * sticky error, it will trigger the clearing of the sticky. Without this
+	 * initialization system and debug power would be disabled while clearing
+	 * the sticky error bit.
+	 */
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
+
+	dap->do_reconnect = false;
+
+	dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
+	if (dap->do_reconnect) {
+		/* dap connect calls dap_dp_init() after transport dependent initialization */
+		return dap->ops->connect(dap);
+	} else {
+		return dap_dp_init(dap);
+	}
+}
+
+/**
  * Initialize a DAP.  This sets up the power domains, prepares the DP
  * for further use, and arranges to use AP #0 for all AP operations
  * until dap_ap-select() changes that policy.
@@ -1479,15 +1495,118 @@ int dap_info_command(struct command_invocation *cmd,
 
 enum adiv5_cfg_param {
 	CFG_DAP,
-	CFG_AP_NUM
+	CFG_AP_NUM,
+	CFG_BASEADDR,
+	CFG_CTIBASE, /* DEPRECATED */
 };
 
 static const Jim_Nvp nvp_config_opts[] = {
-	{ .name = "-dap",    .value = CFG_DAP },
-	{ .name = "-ap-num", .value = CFG_AP_NUM },
+	{ .name = "-dap",       .value = CFG_DAP },
+	{ .name = "-ap-num",    .value = CFG_AP_NUM },
+	{ .name = "-baseaddr",  .value = CFG_BASEADDR },
+	{ .name = "-ctibase",   .value = CFG_CTIBASE }, /* DEPRECATED */
 	{ .name = NULL, .value = -1 }
 };
 
+static int adiv5_jim_spot_configure(Jim_GetOptInfo *goi,
+		struct adiv5_dap **dap_p, int *ap_num_p, uint32_t *base_p)
+{
+	if (!goi->argc)
+		return JIM_OK;
+
+	Jim_SetEmptyResult(goi->interp);
+
+	Jim_Nvp *n;
+	int e = Jim_Nvp_name2value_obj(goi->interp, nvp_config_opts,
+				goi->argv[0], &n);
+	if (e != JIM_OK)
+		return JIM_CONTINUE;
+
+	/* base_p can be NULL, then '-baseaddr' option is treated as unknown */
+	if (!base_p && (n->value == CFG_BASEADDR || n->value == CFG_CTIBASE))
+		return JIM_CONTINUE;
+
+	e = Jim_GetOpt_Obj(goi, NULL);
+	if (e != JIM_OK)
+		return e;
+
+	switch (n->value) {
+	case CFG_DAP:
+		if (goi->isconfigure) {
+			Jim_Obj *o_t;
+			struct adiv5_dap *dap;
+			e = Jim_GetOpt_Obj(goi, &o_t);
+			if (e != JIM_OK)
+				return e;
+			dap = dap_instance_by_jim_obj(goi->interp, o_t);
+			if (!dap) {
+				Jim_SetResultString(goi->interp, "DAP name invalid!", -1);
+				return JIM_ERR;
+			}
+			if (*dap_p && *dap_p != dap) {
+				Jim_SetResultString(goi->interp,
+					"DAP assignment cannot be changed!", -1);
+				return JIM_ERR;
+			}
+			*dap_p = dap;
+		} else {
+			if (goi->argc)
+				goto err_no_param;
+			if (!*dap_p) {
+				Jim_SetResultString(goi->interp, "DAP not configured", -1);
+				return JIM_ERR;
+			}
+			Jim_SetResultString(goi->interp, adiv5_dap_name(*dap_p), -1);
+		}
+		break;
+
+	case CFG_AP_NUM:
+		if (goi->isconfigure) {
+			jim_wide ap_num;
+			e = Jim_GetOpt_Wide(goi, &ap_num);
+			if (e != JIM_OK)
+				return e;
+			if (ap_num < 0 || ap_num > DP_APSEL_MAX) {
+				Jim_SetResultString(goi->interp, "Invalid AP number!", -1);
+				return JIM_ERR;
+			}
+			*ap_num_p = ap_num;
+		} else {
+			if (goi->argc)
+				goto err_no_param;
+			if (*ap_num_p == DP_APSEL_INVALID) {
+				Jim_SetResultString(goi->interp, "AP number not configured", -1);
+				return JIM_ERR;
+			}
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, *ap_num_p));
+		}
+		break;
+
+	case CFG_CTIBASE:
+		LOG_WARNING("DEPRECATED! use \'-baseaddr' not \'-ctibase\'");
+		/* fall through */
+	case CFG_BASEADDR:
+		if (goi->isconfigure) {
+			jim_wide base;
+			e = Jim_GetOpt_Wide(goi, &base);
+			if (e != JIM_OK)
+				return e;
+			*base_p = (uint32_t)base;
+		} else {
+			if (goi->argc)
+				goto err_no_param;
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, *base_p));
+		}
+		break;
+	};
+
+	return JIM_OK;
+
+err_no_param:
+	Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "NO PARAMS");
+	return JIM_ERR;
+}
+
 int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
 {
 	struct adiv5_private_config *pc;
@@ -1502,90 +1621,19 @@ int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
 
 	target->has_dap = true;
 
-	if (goi->argc > 0) {
-		Jim_Nvp *n;
-
-		Jim_SetEmptyResult(goi->interp);
-
-		/* check first if topmost item is for us */
-		e = Jim_Nvp_name2value_obj(goi->interp, nvp_config_opts,
-								   goi->argv[0], &n);
-		if (e != JIM_OK)
-			return JIM_CONTINUE;
-
-		e = Jim_GetOpt_Obj(goi, NULL);
-		if (e != JIM_OK)
-			return e;
-
-		switch (n->value) {
-		case CFG_DAP:
-			if (goi->isconfigure) {
-				Jim_Obj *o_t;
-				struct adiv5_dap *dap;
-				e = Jim_GetOpt_Obj(goi, &o_t);
-				if (e != JIM_OK)
-					return e;
-				dap = dap_instance_by_jim_obj(goi->interp, o_t);
-				if (dap == NULL) {
-					Jim_SetResultString(goi->interp, "DAP name invalid!", -1);
-					return JIM_ERR;
-				}
-				if (pc->dap != NULL && pc->dap != dap) {
-					Jim_SetResultString(goi->interp,
-						"DAP assignment cannot be changed after target was created!", -1);
-					return JIM_ERR;
-				}
-				if (target->tap_configured) {
-					Jim_SetResultString(goi->interp,
-						"-chain-position and -dap configparams are mutually exclusive!", -1);
-					return JIM_ERR;
-				}
-				pc->dap = dap;
-				target->tap = dap->tap;
-				target->dap_configured = true;
-			} else {
-				if (goi->argc != 0) {
-					Jim_WrongNumArgs(goi->interp,
-										goi->argc, goi->argv,
-					"NO PARAMS");
-					return JIM_ERR;
-				}
-
-				if (pc->dap == NULL) {
-					Jim_SetResultString(goi->interp, "DAP not configured", -1);
-					return JIM_ERR;
-				}
-				Jim_SetResultString(goi->interp, adiv5_dap_name(pc->dap), -1);
-			}
-			break;
+	e = adiv5_jim_spot_configure(goi, &pc->dap, &pc->ap_num, NULL);
+	if (e != JIM_OK)
+		return e;
 
-		case CFG_AP_NUM:
-			if (goi->isconfigure) {
-				jim_wide ap_num;
-				e = Jim_GetOpt_Wide(goi, &ap_num);
-				if (e != JIM_OK)
-					return e;
-				if (ap_num < 0 || ap_num > DP_APSEL_MAX) {
-					Jim_SetResultString(goi->interp, "Invalid AP number!", -1);
-					return JIM_ERR;
-				}
-				pc->ap_num = ap_num;
-			} else {
-				if (goi->argc != 0) {
-					Jim_WrongNumArgs(goi->interp,
-									 goi->argc, goi->argv,
-					  "NO PARAMS");
-					return JIM_ERR;
-				}
-
-				if (pc->ap_num == DP_APSEL_INVALID) {
-					Jim_SetResultString(goi->interp, "AP number not configured", -1);
-					return JIM_ERR;
-				}
-				Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, pc->ap_num));
-			}
-			break;
+	if (pc->dap && !target->dap_configured) {
+		if (target->tap_configured) {
+			pc->dap = NULL;
+			Jim_SetResultString(goi->interp,
+				"-chain-position and -dap configparams are mutually exclusive!", -1);
+			return JIM_ERR;
 		}
+		target->tap = pc->dap->tap;
+		target->dap_configured = true;
 	}
 
 	return JIM_OK;
@@ -1602,6 +1650,19 @@ int adiv5_verify_config(struct adiv5_private_config *pc)
 	return ERROR_OK;
 }
 
+int adiv5_jim_mem_ap_spot_configure(struct adiv5_mem_ap_spot *cfg,
+		Jim_GetOptInfo *goi)
+{
+	return adiv5_jim_spot_configure(goi, &cfg->dap, &cfg->ap_num, &cfg->base);
+}
+
+int adiv5_mem_ap_spot_init(struct adiv5_mem_ap_spot *p)
+{
+	p->dap = NULL;
+	p->ap_num = DP_APSEL_INVALID;
+	p->base = 0;
+	return ERROR_OK;
+}
 
 COMMAND_HANDLER(handle_dap_info_command)
 {
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index ea71551..8edfaa8 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -550,6 +550,7 @@ int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
 
 /* Initialisation of the debug system, power domains and registers */
 int dap_dp_init(struct adiv5_dap *dap);
+int dap_dp_init_or_reconnect(struct adiv5_dap *dap);
 int mem_ap_init(struct adiv5_ap *ap);
 
 /* Invalidate cached DP select and cached TAR and CSW of all APs */
@@ -601,4 +602,14 @@ struct adiv5_private_config {
 extern int adiv5_verify_config(struct adiv5_private_config *pc);
 extern int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi);
 
+struct adiv5_mem_ap_spot {
+	struct adiv5_dap *dap;
+	int ap_num;
+	uint32_t base;
+};
+
+extern int adiv5_mem_ap_spot_init(struct adiv5_mem_ap_spot *p);
+extern int adiv5_jim_mem_ap_spot_configure(struct adiv5_mem_ap_spot *cfg,
+		Jim_GetOptInfo *goi);
+
 #endif /* OPENOCD_TARGET_ARM_ADI_V5_H */
diff --git a/src/target/arm_cti.c b/src/target/arm_cti.c
index 579bacb..689e9df 100644
--- a/src/target/arm_cti.c
+++ b/src/target/arm_cti.c
@@ -31,28 +31,21 @@
 #include "helper/command.h"
 
 struct arm_cti {
-	target_addr_t base;
-	struct adiv5_ap *ap;
-};
-
-struct arm_cti_object {
 	struct list_head lh;
-	struct arm_cti cti;
-	int ap_num;
 	char *name;
+	struct adiv5_mem_ap_spot spot;
 };
 
 static LIST_HEAD(all_cti);
 
 const char *arm_cti_name(struct arm_cti *self)
 {
-	struct arm_cti_object *obj = container_of(self, struct arm_cti_object, cti);
-	return obj->name;
+	return self->name;
 }
 
 struct arm_cti *cti_instance_by_jim_obj(Jim_Interp *interp, Jim_Obj *o)
 {
-	struct arm_cti_object *obj = NULL;
+	struct arm_cti *obj = NULL;
 	const char *name;
 	bool found = false;
 
@@ -66,16 +59,17 @@ struct arm_cti *cti_instance_by_jim_obj(Jim_Interp *interp, Jim_Obj *o)
 	}
 
 	if (found)
-		return &obj->cti;
+		return obj;
 	return NULL;
 }
 
 static int arm_cti_mod_reg_bits(struct arm_cti *self, unsigned int reg, uint32_t mask, uint32_t value)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
 	uint32_t tmp;
 
 	/* Read register */
-	int retval = mem_ap_read_atomic_u32(self->ap, self->base + reg, &tmp);
+	int retval = mem_ap_read_atomic_u32(ap, self->spot.base + reg, &tmp);
 	if (ERROR_OK != retval)
 		return retval;
 
@@ -85,26 +79,28 @@ static int arm_cti_mod_reg_bits(struct arm_cti *self, unsigned int reg, uint32_t
 	tmp |= value & mask;
 
 	/* write new value */
-	return mem_ap_write_atomic_u32(self->ap, self->base + reg, tmp);
+	return mem_ap_write_atomic_u32(ap, self->spot.base + reg, tmp);
 }
 
 int arm_cti_enable(struct arm_cti *self, bool enable)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
 	uint32_t val = enable ? 1 : 0;
 
-	return mem_ap_write_atomic_u32(self->ap, self->base + CTI_CTR, val);
+	return mem_ap_write_atomic_u32(ap, self->spot.base + CTI_CTR, val);
 }
 
 int arm_cti_ack_events(struct arm_cti *self, uint32_t event)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
 	int retval;
 	uint32_t tmp;
 
-	retval = mem_ap_write_atomic_u32(self->ap, self->base + CTI_INACK, event);
+	retval = mem_ap_write_atomic_u32(ap, self->spot.base + CTI_INACK, event);
 	if (retval == ERROR_OK) {
 		int64_t then = timeval_ms();
 		for (;;) {
-			retval = mem_ap_read_atomic_u32(self->ap, self->base + CTI_TROUT_STATUS, &tmp);
+			retval = mem_ap_read_atomic_u32(ap, self->spot.base + CTI_TROUT_STATUS, &tmp);
 			if (retval != ERROR_OK)
 				break;
 			if ((tmp & event) == 0)
@@ -138,15 +134,19 @@ int arm_cti_ungate_channel(struct arm_cti *self, uint32_t channel)
 
 int arm_cti_write_reg(struct arm_cti *self, unsigned int reg, uint32_t value)
 {
-	return mem_ap_write_atomic_u32(self->ap, self->base + reg, value);
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
+
+	return mem_ap_write_atomic_u32(ap, self->spot.base + reg, value);
 }
 
 int arm_cti_read_reg(struct arm_cti *self, unsigned int reg, uint32_t *p_value)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
+
 	if (p_value == NULL)
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 
-	return mem_ap_read_atomic_u32(self->ap, self->base + reg, p_value);
+	return mem_ap_read_atomic_u32(ap, self->spot.base + reg, p_value);
 }
 
 int arm_cti_pulse_channel(struct arm_cti *self, uint32_t channel)
@@ -225,7 +225,7 @@ static int cti_find_reg_offset(const char *name)
 
 int arm_cti_cleanup_all(void)
 {
-	struct arm_cti_object *obj, *tmp;
+	struct arm_cti *obj, *tmp;
 
 	list_for_each_entry_safe(obj, tmp, &all_cti, lh) {
 		free(obj->name);
@@ -237,16 +237,16 @@ int arm_cti_cleanup_all(void)
 
 COMMAND_HANDLER(handle_cti_dump)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
+	struct adiv5_ap *ap = dap_ap(cti->spot.dap, cti->spot.ap_num);
 	int retval = ERROR_OK;
 
 	for (int i = 0; (retval == ERROR_OK) && (i < (int)ARRAY_SIZE(cti_names)); i++)
-		retval = mem_ap_read_u32(cti->ap,
-				cti->base + cti_names[i].offset, cti_names[i].p_val);
+		retval = mem_ap_read_u32(ap,
+				cti->spot.base + cti_names[i].offset, cti_names[i].p_val);
 
 	if (retval == ERROR_OK)
-		retval = dap_run(cti->ap->dap);
+		retval = dap_run(ap->dap);
 
 	if (retval != ERROR_OK)
 		return JIM_ERR;
@@ -260,8 +260,7 @@ COMMAND_HANDLER(handle_cti_dump)
 
 COMMAND_HANDLER(handle_cti_enable)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	bool on_off;
 
 	if (CMD_ARGC != 1)
@@ -274,8 +273,7 @@ COMMAND_HANDLER(handle_cti_enable)
 
 COMMAND_HANDLER(handle_cti_testmode)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	bool on_off;
 
 	if (CMD_ARGC != 1)
@@ -288,8 +286,7 @@ COMMAND_HANDLER(handle_cti_testmode)
 
 COMMAND_HANDLER(handle_cti_write)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	int offset;
 	uint32_t value;
 
@@ -307,8 +304,7 @@ COMMAND_HANDLER(handle_cti_write)
 
 COMMAND_HANDLER(handle_cti_read)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	int offset;
 	int retval;
 	uint32_t value;
@@ -331,8 +327,7 @@ COMMAND_HANDLER(handle_cti_read)
 
 COMMAND_HANDLER(handle_cti_ack)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	uint32_t event;
 
 	if (CMD_ARGC != 1)
@@ -351,8 +346,7 @@ COMMAND_HANDLER(handle_cti_ack)
 
 COMMAND_HANDLER(handle_cti_channel)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	int retval = ERROR_OK;
 	uint32_t ch_num;
 
@@ -436,83 +430,26 @@ static const struct command_registration cti_instance_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-enum cti_cfg_param {
-	CFG_DAP,
-	CFG_AP_NUM,
-	CFG_CTIBASE
-};
-
-static const Jim_Nvp nvp_config_opts[] = {
-	{ .name = "-dap",     .value = CFG_DAP },
-	{ .name = "-ctibase", .value = CFG_CTIBASE },
-	{ .name = "-ap-num",  .value = CFG_AP_NUM },
-	{ .name = NULL, .value = -1 }
-};
-
-static int cti_configure(Jim_GetOptInfo *goi, struct arm_cti_object *cti)
+static int cti_configure(Jim_GetOptInfo *goi, struct arm_cti *cti)
 {
-	struct adiv5_dap *dap = NULL;
-	Jim_Nvp *n;
-	jim_wide w;
-	int e;
-
 	/* parse config or cget options ... */
 	while (goi->argc > 0) {
-		Jim_SetEmptyResult(goi->interp);
-
-		e = Jim_GetOpt_Nvp(goi, nvp_config_opts, &n);
-		if (e != JIM_OK) {
-			Jim_GetOpt_NvpUnknown(goi, nvp_config_opts, 0);
+		int e = adiv5_jim_mem_ap_spot_configure(&cti->spot, goi);
+		if (e != JIM_OK)
 			return e;
-		}
-		switch (n->value) {
-		case CFG_DAP: {
-			Jim_Obj *o_t;
-			e = Jim_GetOpt_Obj(goi, &o_t);
-			if (e != JIM_OK)
-				return e;
-			dap = dap_instance_by_jim_obj(goi->interp, o_t);
-			if (dap == NULL) {
-				Jim_SetResultString(goi->interp, "-dap is invalid", -1);
-				return JIM_ERR;
-			}
-			/* loop for more */
-			break;
-		}
-		case CFG_CTIBASE:
-			e = Jim_GetOpt_Wide(goi, &w);
-			if (e != JIM_OK)
-				return e;
-			cti->cti.base = (uint32_t)w;
-			/* loop for more */
-			break;
-
-		case CFG_AP_NUM:
-			e = Jim_GetOpt_Wide(goi, &w);
-			if (e != JIM_OK)
-				return e;
-			if (w < 0 || w > DP_APSEL_MAX) {
-				Jim_SetResultString(goi->interp, "-ap-num is invalid", -1);
-				return JIM_ERR;
-			}
-			cti->ap_num = (uint32_t)w;
-		}
 	}
 
-	if (dap == NULL) {
+	if (!cti->spot.dap) {
 		Jim_SetResultString(goi->interp, "-dap required when creating CTI", -1);
 		return JIM_ERR;
 	}
 
-	cti->cti.ap = dap_ap(dap, cti->ap_num);
-
 	return JIM_OK;
 }
-
 static int cti_create(Jim_GetOptInfo *goi)
 {
 	struct command_context *cmd_ctx;
-	static struct arm_cti_object *cti;
+	static struct arm_cti *cti;
 	Jim_Obj *new_cmd;
 	Jim_Cmd *cmd;
 	const char *cp;
@@ -536,10 +473,14 @@ static int cti_create(Jim_GetOptInfo *goi)
 	}
 
 	/* Create it */
-	cti = calloc(1, sizeof(struct arm_cti_object));
+	cti = calloc(1, sizeof(*cti));
 	if (cti == NULL)
 		return JIM_ERR;
 
+	adiv5_mem_ap_spot_init(&cti->spot);
+
+	/* Do the rest as "configure" options */
+	goi->isconfigure = 1;
 	e = cti_configure(goi, cti);
 	if (e != JIM_OK) {
 		free(cti);
@@ -593,7 +534,7 @@ static int jim_cti_create(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 
 static int jim_cti_names(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 {
-	struct arm_cti_object *obj;
+	struct arm_cti *obj;
 
 	if (argc != 1) {
 		Jim_WrongNumArgs(interp, 1, argv, "Too many parameters");
diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 61f1e78..723be57 100644
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -315,7 +315,7 @@ int arm_semihosting(struct target *target, int *retval)
 		if (0 <= semihosting->op && semihosting->op <= 0x31) {
 			*retval = semihosting_common(target);
 			if (*retval != ERROR_OK) {
-				LOG_ERROR("Failed semihosting operation");
+				LOG_ERROR("Failed semihosting operation (0x%02X)", semihosting->op);
 				return 0;
 			}
 		} else {
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 7da28e3..b725853 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -856,6 +856,9 @@ COMMAND_HANDLER(handle_armv4_5_reg_command)
 		char *sep = "\n";
 		char *shadow = "";
 
+		if (!arm_mode_data[mode].n_indices)
+			continue;
+
 		/* label this bank of registers (or shadows) */
 		switch (arm_mode_data[mode].psr) {
 			case ARM_MODE_SYS:
@@ -869,6 +872,7 @@ COMMAND_HANDLER(handle_armv4_5_reg_command)
 					continue;
 			/* FALLTHROUGH */
 			case ARM_MODE_MON:
+			case ARM_MODE_1176_MON:
 				if (arm->core_type != ARM_CORE_TYPE_SEC_EXT
 					&& arm->core_type != ARM_CORE_TYPE_VIRT_EXT)
 					continue;
diff --git a/src/target/armv7a.c b/src/target/armv7a.c
index c36744d..abca335 100644
--- a/src/target/armv7a.c
+++ b/src/target/armv7a.c
@@ -589,7 +589,7 @@ static const struct command_registration l2_cache_commands[] = {
 
 };
 
-const struct command_registration l2x_cache_command_handlers[] = {
+static const struct command_registration l2x_cache_command_handlers[] = {
 	{
 		.name = "cache_config",
 		.mode = COMMAND_EXEC,
diff --git a/src/target/armv7a_cache.c b/src/target/armv7a_cache.c
index e5f1fb0..fa6df2a 100644
--- a/src/target/armv7a_cache.c
+++ b/src/target/armv7a_cache.c
@@ -572,7 +572,7 @@ static const struct command_registration arm7a_l1_i_cache_commands[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-const struct command_registration arm7a_l1_di_cache_group_handlers[] = {
+static const struct command_registration arm7a_l1_di_cache_group_handlers[] = {
 	{
 		.name = "info",
 		.handler = arm7a_l1_cache_info_cmd,
@@ -597,7 +597,7 @@ const struct command_registration arm7a_l1_di_cache_group_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-const struct command_registration arm7a_cache_group_handlers[] = {
+static const struct command_registration arm7a_cache_group_handlers[] = {
 	{
 		.name = "auto",
 		.handler = arm7a_cache_disable_auto_cmd,
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index ea6ee61..f14ce0d 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -14,6 +14,9 @@
  *   Copyright (C) 2018 by Liviu Ionescu                                   *
  *   <ilg@livius.net>                                                      *
  *                                                                         *
+ *   Copyright (C) 2019 by Tomas Vanek                                     *
+ *   vanekt@fbl.cz                                                         *
+ *                                                                         *
  *   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     *
@@ -108,10 +111,19 @@ static const struct {
 	{ ARMV7M_MSP, "msp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
 	{ ARMV7M_PSP, "psp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
 
+	/* A working register for packing/unpacking special regs, hidden from gdb */
+	{ ARMV7M_PMSK_BPRI_FLTMSK_CTRL, "pmsk_bpri_fltmsk_ctrl", 32, REG_TYPE_INT, NULL, NULL },
+
+	/* WARNING: If you use armv7m_write_core_reg() on one of 4 following
+	 * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL
+	 * cache only and are not flushed to CPU HW register.
+	 * To trigger write to CPU HW register, add
+	 *		armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,);
+	 */
 	{ ARMV7M_PRIMASK, "primask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 	{ ARMV7M_BASEPRI, "basepri", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 	{ ARMV7M_FAULTMASK, "faultmask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
-	{ ARMV7M_CONTROL, "control", 2, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
+	{ ARMV7M_CONTROL, "control", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 
 	{ ARMV7M_D0, "d0", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 	{ ARMV7M_D1, "d1", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
@@ -150,6 +162,9 @@ int armv7m_restore_context(struct target *target)
 	if (armv7m->pre_restore_context)
 		armv7m->pre_restore_context(target);
 
+	/* The descending order of register writes is crucial for correct
+	 * packing of ARMV7M_PMSK_BPRI_FLTMSK_CTRL!
+	 * See also comments in the register table above */
 	for (i = cache->num_regs - 1; i >= 0; i--) {
 		if (cache->reg_list[i].dirty) {
 			armv7m->arm.write_core_reg(target, &cache->reg_list[i], i,
@@ -211,87 +226,195 @@ static int armv7m_set_core_reg(struct reg *reg, uint8_t *buf)
 	return ERROR_OK;
 }
 
+static uint32_t armv7m_map_id_to_regsel(unsigned int arm_reg_id)
+{
+	switch (arm_reg_id) {
+	case ARMV7M_R0 ... ARMV7M_R14:
+	case ARMV7M_PC:
+	case ARMV7M_xPSR:
+	case ARMV7M_MSP:
+	case ARMV7M_PSP:
+		/* NOTE:  we "know" here that the register identifiers
+		 * match the Cortex-M DCRSR.REGSEL selectors values
+		 * for R0..R14, PC, xPSR, MSP, and PSP.
+		 */
+		return arm_reg_id;
+
+	case ARMV7M_PMSK_BPRI_FLTMSK_CTRL:
+		return ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL;
+
+	case ARMV7M_FPSCR:
+		return ARMV7M_REGSEL_FPSCR;
+
+	case ARMV7M_D0 ... ARMV7M_D15:
+		return ARMV7M_REGSEL_S0 + 2 * (arm_reg_id - ARMV7M_D0);
+
+	default:
+		LOG_ERROR("Bad register ID %u", arm_reg_id);
+		return arm_reg_id;
+	}
+}
+
+static bool armv7m_map_reg_packing(unsigned int arm_reg_id,
+					unsigned int *reg32_id, uint32_t *offset)
+{
+	switch (arm_reg_id) {
+
+	case ARMV7M_PRIMASK:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 0;
+		return true;
+	case ARMV7M_BASEPRI:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 1;
+		return true;
+	case ARMV7M_FAULTMASK:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 2;
+		return true;
+	case ARMV7M_CONTROL:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 3;
+		return true;
+
+	default:
+		return false;
+	}
+}
+
 static int armv7m_read_core_reg(struct target *target, struct reg *r,
 	int num, enum arm_mode mode)
 {
 	uint32_t reg_value;
 	int retval;
-	struct arm_reg *armv7m_core_reg;
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 
 	assert(num < (int)armv7m->arm.core_cache->num_regs);
+	assert(num == (int)r->number);
 
-	armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info;
+	/* If a code calls read_reg, it expects the cache is no more dirty.
+	 * Clear the dirty flag regardless of the later read succeeds or not
+	 * to prevent unwanted cache flush after a read error */
+	r->dirty = false;
+
+	if (r->size <= 8) {
+		/* any 8-bit or shorter register is packed */
+		uint32_t offset = 0;	/* silence false gcc warning */
+		unsigned int reg32_id;
+
+		bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
+		assert(is_packed);
+		struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
+
+		/* Read 32-bit container register if not cached */
+		if (!r32->valid) {
+			retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+		/* Copy required bits of 32-bit container register */
+		buf_cpy(r32->value + offset, r->value, r->size);
 
-	if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) {
-		/* map D0..D15 to S0..S31 */
-		size_t regidx = ARMV7M_S0 + 2 * (armv7m_core_reg->num - ARMV7M_D0);
-		retval = armv7m->load_core_reg_u32(target, regidx, &reg_value);
-		if (retval != ERROR_OK)
-			return retval;
-		buf_set_u32(armv7m->arm.core_cache->reg_list[num].value,
-			    0, 32, reg_value);
-		retval = armv7m->load_core_reg_u32(target, regidx + 1, &reg_value);
-		if (retval != ERROR_OK)
-			return retval;
-		buf_set_u32(armv7m->arm.core_cache->reg_list[num].value + 4,
-			    0, 32, reg_value);
 	} else {
-		retval = armv7m->load_core_reg_u32(target,
-						   armv7m_core_reg->num, &reg_value);
+		assert(r->size == 32 || r->size == 64);
+
+		struct arm_reg *armv7m_core_reg = r->arch_info;
+		uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
+
+		retval = armv7m->load_core_reg_u32(target, regsel, &reg_value);
 		if (retval != ERROR_OK)
 			return retval;
-		buf_set_u32(armv7m->arm.core_cache->reg_list[num].value, 0, 32, reg_value);
+		buf_set_u32(r->value, 0, 32, reg_value);
+
+		if (r->size == 64) {
+			retval = armv7m->load_core_reg_u32(target, regsel + 1, &reg_value);
+			if (retval != ERROR_OK) {
+				r->valid = false;
+				return retval;
+			}
+			buf_set_u32(r->value + 4, 0, 32, reg_value);
+
+			uint64_t q = buf_get_u64(r->value, 0, 64);
+			LOG_DEBUG("read %s value 0x%016" PRIx64, r->name, q);
+		} else {
+			LOG_DEBUG("read %s value 0x%08" PRIx32, r->name, reg_value);
+		}
 	}
 
-	armv7m->arm.core_cache->reg_list[num].valid = true;
-	armv7m->arm.core_cache->reg_list[num].dirty = false;
+	r->valid = true;
 
-	return retval;
+	return ERROR_OK;
 }
 
 static int armv7m_write_core_reg(struct target *target, struct reg *r,
 	int num, enum arm_mode mode, uint8_t *value)
 {
 	int retval;
-	struct arm_reg *armv7m_core_reg;
+	uint32_t t;
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 
 	assert(num < (int)armv7m->arm.core_cache->num_regs);
+	assert(num == (int)r->number);
 
-	armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info;
+	if (value != r->value) {
+		/* If we are not flushing the cache, store the new value to the cache */
+		buf_cpy(value, r->value, r->size);
+	}
 
-	if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) {
-		/* map D0..D15 to S0..S31 */
-		size_t regidx = ARMV7M_S0 + 2 * (armv7m_core_reg->num - ARMV7M_D0);
+	if (r->size <= 8) {
+		/* any 8-bit or shorter register is packed */
+		uint32_t offset = 0;	/* silence false gcc warning */
+		unsigned int reg32_id;
 
-		uint32_t t = buf_get_u32(value, 0, 32);
-		retval = armv7m->store_core_reg_u32(target, regidx, t);
-		if (retval != ERROR_OK)
-			goto out_error;
+		bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
+		assert(is_packed);
+		struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
+
+		if (!r32->valid) {
+			/* Before merging with other parts ensure the 32-bit register is valid */
+			retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+		/* Write a part to the 32-bit container register */
+		buf_cpy(value, r32->value + offset, r->size);
+		r32->dirty = true;
 
-		t = buf_get_u32(value + 4, 0, 32);
-		retval = armv7m->store_core_reg_u32(target, regidx + 1, t);
-		if (retval != ERROR_OK)
-			goto out_error;
 	} else {
-		uint32_t t = buf_get_u32(value, 0, 32);
+		assert(r->size == 32 || r->size == 64);
+
+		struct arm_reg *armv7m_core_reg = r->arch_info;
+		uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
 
-		LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num, t);
-		retval = armv7m->store_core_reg_u32(target, armv7m_core_reg->num, t);
+		t = buf_get_u32(value, 0, 32);
+		retval = armv7m->store_core_reg_u32(target, regsel, t);
 		if (retval != ERROR_OK)
 			goto out_error;
+
+		if (r->size == 64) {
+			t = buf_get_u32(value + 4, 0, 32);
+			retval = armv7m->store_core_reg_u32(target, regsel + 1, t);
+			if (retval != ERROR_OK)
+				goto out_error;
+
+			uint64_t q = buf_get_u64(value, 0, 64);
+			LOG_DEBUG("write %s value 0x%016" PRIx64, r->name, q);
+		} else {
+			LOG_DEBUG("write %s value 0x%08" PRIx32, r->name, t);
+		}
 	}
 
-	armv7m->arm.core_cache->reg_list[num].valid = true;
-	armv7m->arm.core_cache->reg_list[num].dirty = false;
+	r->valid = true;
+	r->dirty = false;
 
 	return ERROR_OK;
 
 out_error:
-	LOG_ERROR("Error setting register");
-	armv7m->arm.core_cache->reg_list[num].dirty = armv7m->arm.core_cache->reg_list[num].valid;
-	return ERROR_JTAG_DEVICE_ERROR;
+	r->dirty = true;
+	LOG_ERROR("Error setting register %s", r->name);
+	return retval;
 }
 
 /**
@@ -370,8 +493,7 @@ int armv7m_start_algorithm(struct target *target,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	/* refresh core register cache
-	 * Not needed if core register cache is always consistent with target process state */
+	/* Store all non-debug execution registers to armv7m_algorithm_info context */
 	for (unsigned i = 0; i < armv7m->arm.core_cache->num_regs; i++) {
 
 		armv7m_algorithm_info->context[i] = buf_get_u32(
@@ -618,12 +740,10 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target)
 
 		reg_list[i].name = armv7m_regs[i].name;
 		reg_list[i].size = armv7m_regs[i].bits;
-		size_t storage_size = DIV_ROUND_UP(armv7m_regs[i].bits, 8);
-		if (storage_size < 4)
-			storage_size = 4;
-		reg_list[i].value = calloc(1, storage_size);
+		reg_list[i].value = arch_info[i].value;
 		reg_list[i].dirty = false;
 		reg_list[i].valid = false;
+		reg_list[i].hidden = i == ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 		reg_list[i].type = &armv7m_reg_type;
 		reg_list[i].arch_info = &arch_info[i];
 
@@ -632,6 +752,9 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target)
 		reg_list[i].exist = true;
 		reg_list[i].caller_save = true;	/* gdb defaults to true */
 
+		if (reg_list[i].hidden)
+			continue;
+
 		feature = calloc(1, sizeof(struct reg_feature));
 		if (feature) {
 			feature->name = armv7m_regs[i].feature;
@@ -671,7 +794,6 @@ void armv7m_free_reg_cache(struct target *target)
 
 		free(reg->feature);
 		free(reg->reg_data_type);
-		free(reg->value);
 	}
 
 	free(cache->reg_list[0].arch_info);
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index 01bf19e..db6f8bc 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -34,75 +34,115 @@ extern const int armv7m_msp_reg_map[];
 
 const char *armv7m_exception_string(int number);
 
+/* Cortex-M DCRSR.REGSEL selectors */
+enum {
+	ARMV7M_REGSEL_R0,
+	ARMV7M_REGSEL_R1,
+	ARMV7M_REGSEL_R2,
+	ARMV7M_REGSEL_R3,
+
+	ARMV7M_REGSEL_R4,
+	ARMV7M_REGSEL_R5,
+	ARMV7M_REGSEL_R6,
+	ARMV7M_REGSEL_R7,
+
+	ARMV7M_REGSEL_R8,
+	ARMV7M_REGSEL_R9,
+	ARMV7M_REGSEL_R10,
+	ARMV7M_REGSEL_R11,
+
+	ARMV7M_REGSEL_R12,
+	ARMV7M_REGSEL_R13,
+	ARMV7M_REGSEL_R14,
+	ARMV7M_REGSEL_PC = 15,
+
+	ARMV7M_REGSEL_xPSR = 16,
+	ARMV7M_REGSEL_MSP,
+	ARMV7M_REGSEL_PSP,
+
+	ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL = 0x14,
+	ARMV7M_REGSEL_FPSCR = 0x21,
+
+	/* 32bit Floating-point registers */
+	ARMV7M_REGSEL_S0 = 0x40,
+	ARMV7M_REGSEL_S1,
+	ARMV7M_REGSEL_S2,
+	ARMV7M_REGSEL_S3,
+	ARMV7M_REGSEL_S4,
+	ARMV7M_REGSEL_S5,
+	ARMV7M_REGSEL_S6,
+	ARMV7M_REGSEL_S7,
+	ARMV7M_REGSEL_S8,
+	ARMV7M_REGSEL_S9,
+	ARMV7M_REGSEL_S10,
+	ARMV7M_REGSEL_S11,
+	ARMV7M_REGSEL_S12,
+	ARMV7M_REGSEL_S13,
+	ARMV7M_REGSEL_S14,
+	ARMV7M_REGSEL_S15,
+	ARMV7M_REGSEL_S16,
+	ARMV7M_REGSEL_S17,
+	ARMV7M_REGSEL_S18,
+	ARMV7M_REGSEL_S19,
+	ARMV7M_REGSEL_S20,
+	ARMV7M_REGSEL_S21,
+	ARMV7M_REGSEL_S22,
+	ARMV7M_REGSEL_S23,
+	ARMV7M_REGSEL_S24,
+	ARMV7M_REGSEL_S25,
+	ARMV7M_REGSEL_S26,
+	ARMV7M_REGSEL_S27,
+	ARMV7M_REGSEL_S28,
+	ARMV7M_REGSEL_S29,
+	ARMV7M_REGSEL_S30,
+	ARMV7M_REGSEL_S31,
+};
+
 /* offsets into armv7m core register cache */
 enum {
 	/* for convenience, the first set of indices match
-	 * the Cortex-M3/-M4 DCRSR selectors
+	 * the Cortex-M DCRSR.REGSEL selectors
+	 */
+	ARMV7M_R0 = ARMV7M_REGSEL_R0,
+	ARMV7M_R1 = ARMV7M_REGSEL_R1,
+	ARMV7M_R2 = ARMV7M_REGSEL_R2,
+	ARMV7M_R3 = ARMV7M_REGSEL_R3,
+
+	ARMV7M_R4 = ARMV7M_REGSEL_R4,
+	ARMV7M_R5 = ARMV7M_REGSEL_R5,
+	ARMV7M_R6 = ARMV7M_REGSEL_R6,
+	ARMV7M_R7 = ARMV7M_REGSEL_R7,
+
+	ARMV7M_R8 = ARMV7M_REGSEL_R8,
+	ARMV7M_R9 = ARMV7M_REGSEL_R9,
+	ARMV7M_R10 = ARMV7M_REGSEL_R10,
+	ARMV7M_R11 = ARMV7M_REGSEL_R11,
+
+	ARMV7M_R12 = ARMV7M_REGSEL_R12,
+	ARMV7M_R13 = ARMV7M_REGSEL_R13,
+	ARMV7M_R14 = ARMV7M_REGSEL_R14,
+	ARMV7M_PC = ARMV7M_REGSEL_PC,
+
+	ARMV7M_xPSR = ARMV7M_REGSEL_xPSR,
+	ARMV7M_MSP = ARMV7M_REGSEL_MSP,
+	ARMV7M_PSP = ARMV7M_REGSEL_PSP,
+
+	/* following indices are arbitrary, do not match DCRSR.REGSEL selectors */
+
+	/* working register for packing/unpacking special regs, hidden from gdb */
+	ARMV7M_PMSK_BPRI_FLTMSK_CTRL,
+
+	/* WARNING: If you use armv7m_write_core_reg() on one of 4 following
+	 * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL
+	 * cache only and are not flushed to CPU HW register.
+	 * To trigger write to CPU HW register, add
+	 *		armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,);
 	 */
-	ARMV7M_R0,
-	ARMV7M_R1,
-	ARMV7M_R2,
-	ARMV7M_R3,
-
-	ARMV7M_R4,
-	ARMV7M_R5,
-	ARMV7M_R6,
-	ARMV7M_R7,
-
-	ARMV7M_R8,
-	ARMV7M_R9,
-	ARMV7M_R10,
-	ARMV7M_R11,
-
-	ARMV7M_R12,
-	ARMV7M_R13,
-	ARMV7M_R14,
-	ARMV7M_PC = 15,
-
-	ARMV7M_xPSR = 16,
-	ARMV7M_MSP,
-	ARMV7M_PSP,
-
-	/* this next set of indices is arbitrary */
 	ARMV7M_PRIMASK,
 	ARMV7M_BASEPRI,
 	ARMV7M_FAULTMASK,
 	ARMV7M_CONTROL,
 
-	/* 32bit Floating-point registers */
-	ARMV7M_S0,
-	ARMV7M_S1,
-	ARMV7M_S2,
-	ARMV7M_S3,
-	ARMV7M_S4,
-	ARMV7M_S5,
-	ARMV7M_S6,
-	ARMV7M_S7,
-	ARMV7M_S8,
-	ARMV7M_S9,
-	ARMV7M_S10,
-	ARMV7M_S11,
-	ARMV7M_S12,
-	ARMV7M_S13,
-	ARMV7M_S14,
-	ARMV7M_S15,
-	ARMV7M_S16,
-	ARMV7M_S17,
-	ARMV7M_S18,
-	ARMV7M_S19,
-	ARMV7M_S20,
-	ARMV7M_S21,
-	ARMV7M_S22,
-	ARMV7M_S23,
-	ARMV7M_S24,
-	ARMV7M_S25,
-	ARMV7M_S26,
-	ARMV7M_S27,
-	ARMV7M_S28,
-	ARMV7M_S29,
-	ARMV7M_S30,
-	ARMV7M_S31,
-
 	/* 64bit Floating-point registers */
 	ARMV7M_D0,
 	ARMV7M_D1,
@@ -121,10 +161,8 @@ enum {
 	ARMV7M_D14,
 	ARMV7M_D15,
 
-	/* Floating-point status registers */
-	ARMV7M_FPSID,
+	/* Floating-point status register */
 	ARMV7M_FPSCR,
-	ARMV7M_FPEXC,
 
 	ARMV7M_LAST_REG,
 };
@@ -137,7 +175,7 @@ enum {
 };
 
 #define ARMV7M_NUM_CORE_REGS (ARMV7M_xPSR + 1)
-#define ARMV7M_NUM_CORE_REGS_NOFP (ARMV7M_NUM_CORE_REGS + 6)
+#define ARMV7M_NUM_CORE_REGS_NOFP (ARMV7M_CONTROL + 1)
 
 #define ARMV7M_COMMON_MAGIC 0x2A452A45
 
@@ -159,8 +197,8 @@ struct armv7m_common {
 	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);
+	int (*load_core_reg_u32)(struct target *target, uint32_t regsel, uint32_t *value);
+	int (*store_core_reg_u32)(struct target *target, uint32_t regsel, uint32_t value);
 
 	int (*examine_debug_reason)(struct target *target);
 	int (*post_debug_entry)(struct target *target);
diff --git a/src/target/armv7m_trace.c b/src/target/armv7m_trace.c
index 6b368f7..10f1422 100644
--- a/src/target/armv7m_trace.c
+++ b/src/target/armv7m_trace.c
@@ -40,13 +40,43 @@ static int armv7m_poll_trace(void *target)
 
 	target_call_trace_callbacks(target, size, buf);
 
-	if (armv7m->trace_config.trace_file != NULL) {
-		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;
+	switch (armv7m->trace_config.internal_channel) {
+	case TRACE_INTERNAL_CHANNEL_FILE:
+		if (armv7m->trace_config.trace_file != NULL) {
+			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;
+			}
+		}
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCP:
+		if (armv7m->trace_config.trace_service != NULL) {
+			/* broadcast to all service connections */
+			struct connection *connection = armv7m->trace_config.trace_service->connections;
+			retval = ERROR_OK;
+			while (connection) {
+				if (connection_write(connection, buf, size) != (int) size)
+					retval = ERROR_FAIL;
+
+				connection = connection->next;
+			}
+
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Error streaming the trace to TCP/IP port");
+				return ERROR_FAIL;
+			}
 		}
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCL_ONLY:
+		/* nothing to do :
+		 * the trace data is sent to TCL by calling the target_call_trace_callbacks
+		 **/
+		break;
+	default:
+		LOG_ERROR("unsupported trace internal channel");
+		return ERROR_FAIL;
 	}
 
 	return ERROR_OK;
@@ -152,11 +182,71 @@ int armv7m_trace_itm_config(struct target *target)
 	return ERROR_OK;
 }
 
-static void close_trace_file(struct armv7m_common *armv7m)
+static void close_trace_channel(struct armv7m_common *armv7m)
 {
-	if (armv7m->trace_config.trace_file)
-		fclose(armv7m->trace_config.trace_file);
-	armv7m->trace_config.trace_file = NULL;
+	switch (armv7m->trace_config.internal_channel) {
+	case TRACE_INTERNAL_CHANNEL_FILE:
+		if (armv7m->trace_config.trace_file)
+			fclose(armv7m->trace_config.trace_file);
+		armv7m->trace_config.trace_file = NULL;
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCP:
+		if (armv7m->trace_config.trace_service)
+			remove_service(armv7m->trace_config.trace_service->name, armv7m->trace_config.trace_service->port);
+		armv7m->trace_config.trace_service = NULL;
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCL_ONLY:
+		/* nothing to do:
+		 * the trace polling is disabled in the beginning of armv7m_trace_tpiu_config
+		 **/
+		break;
+	default:
+		LOG_ERROR("unsupported trace internal channel");
+	}
+}
+
+static int trace_new_connection(struct connection *connection)
+{
+	/* nothing to do */
+	return ERROR_OK;
+}
+
+static int trace_input(struct connection *connection)
+{
+	/* create a dummy buffer to check if the connection is still active */
+	const int buf_len = 100;
+	unsigned char buf[buf_len];
+	int bytes_read = connection_read(connection, buf, buf_len);
+
+	if (bytes_read == 0)
+		return ERROR_SERVER_REMOTE_CLOSED;
+	else if (bytes_read == -1) {
+		LOG_ERROR("error during read: %s", strerror(errno));
+		return ERROR_SERVER_REMOTE_CLOSED;
+	}
+
+	return ERROR_OK;
+}
+
+static int trace_connection_closed(struct connection *connection)
+{
+	/* nothing to do, no connection->priv to free */
+	return ERROR_OK;
+}
+
+extern struct command_context *global_cmd_ctx;
+
+int armv7m_trace_tpiu_exit(struct target *target)
+{
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+
+	if (global_cmd_ctx->mode == COMMAND_CONFIG ||
+		armv7m->trace_config.config_type == TRACE_CONFIG_TYPE_DISABLED)
+		return ERROR_OK;
+
+	close_trace_channel(armv7m);
+	armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_DISABLED;
+	return armv7m_trace_tpiu_config(target);
 }
 
 COMMAND_HANDLER(handle_tpiu_config_command)
@@ -170,7 +260,7 @@ COMMAND_HANDLER(handle_tpiu_config_command)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	if (!strcmp(CMD_ARGV[cmd_idx], "disable")) {
 		if (CMD_ARGC == cmd_idx + 1) {
-			close_trace_file(armv7m);
+			close_trace_channel(armv7m);
 
 			armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_DISABLED;
 			if (CMD_CTX->mode == COMMAND_EXEC)
@@ -180,7 +270,7 @@ COMMAND_HANDLER(handle_tpiu_config_command)
 		}
 	} else if (!strcmp(CMD_ARGV[cmd_idx], "external") ||
 		   !strcmp(CMD_ARGV[cmd_idx], "internal")) {
-		close_trace_file(armv7m);
+		close_trace_channel(armv7m);
 
 		armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_EXTERNAL;
 		if (!strcmp(CMD_ARGV[cmd_idx], "internal")) {
@@ -189,12 +279,26 @@ COMMAND_HANDLER(handle_tpiu_config_command)
 				return ERROR_COMMAND_SYNTAX_ERROR;
 
 			armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_INTERNAL;
+			armv7m->trace_config.internal_channel = TRACE_INTERNAL_CHANNEL_TCL_ONLY;
 
 			if (strcmp(CMD_ARGV[cmd_idx], "-") != 0) {
-				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;
+				if (CMD_ARGV[cmd_idx][0] == ':') {
+					armv7m->trace_config.internal_channel = TRACE_INTERNAL_CHANNEL_TCP;
+
+					int ret = add_service("armv7m_trace", &(CMD_ARGV[cmd_idx][1]),
+							CONNECTION_LIMIT_UNLIMITED, trace_new_connection, trace_input,
+							trace_connection_closed, NULL, &armv7m->trace_config.trace_service);
+					if (ret != ERROR_OK) {
+						LOG_ERROR("Can't configure trace TCP port");
+						return ERROR_FAIL;
+					}
+				} else {
+					armv7m->trace_config.internal_channel = TRACE_INTERNAL_CHANNEL_FILE;
+					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;
+					}
 				}
 			}
 		}
@@ -306,7 +410,7 @@ static const struct command_registration tpiu_command_handlers[] = {
 		.mode = COMMAND_ANY,
 		.help = "Configure TPIU features",
 		.usage = "(disable | "
-		"((external | internal <filename>) "
+		"((external | internal (<filename> | <:port> | -)) "
 		"(sync <port width> | ((manchester | uart) <formatter enable>)) "
 		"<TRACECLKIN freq> [<trace freq>]))",
 	},
diff --git a/src/target/armv7m_trace.h b/src/target/armv7m_trace.h
index e5879fb..cdf79e7 100644
--- a/src/target/armv7m_trace.h
+++ b/src/target/armv7m_trace.h
@@ -18,6 +18,7 @@
 #ifndef OPENOCD_TARGET_ARMV7M_TRACE_H
 #define OPENOCD_TARGET_ARMV7M_TRACE_H
 
+#include <server/server.h>
 #include <target/target.h>
 #include <command.h>
 
@@ -32,8 +33,14 @@ enum trace_config_type {
 	TRACE_CONFIG_TYPE_INTERNAL	/**< trace output is handled by OpenOCD adapter driver */
 };
 
+enum trace_internal_channel {
+	TRACE_INTERNAL_CHANNEL_TCL_ONLY,	/** trace data is sent only to 'tcl_trace'  */
+	TRACE_INTERNAL_CHANNEL_FILE,		/** trace data is appended to a file */
+	TRACE_INTERNAL_CHANNEL_TCP			/** trace data is appended to a TCP/IP port*/
+};
+
 enum tpiu_pin_protocol {
-	TPIU_PIN_PROTOCOL_SYNC,			/**< synchronous trace output */
+	TPIU_PIN_PROTOCOL_SYNC,				/**< synchronous trace output */
 	TPIU_PIN_PROTOCOL_ASYNC_MANCHESTER,	/**< asynchronous output with Manchester coding */
 	TPIU_PIN_PROTOCOL_ASYNC_UART		/**< asynchronous output with NRZ coding */
 };
@@ -49,6 +56,9 @@ struct armv7m_trace_config {
 	/** Currently active trace capture mode */
 	enum trace_config_type config_type;
 
+	/** The used channel when internal mode is selected */
+	enum trace_internal_channel internal_channel;
+
 	/** Currently active trace output mode */
 	enum tpiu_pin_protocol pin_protocol;
 	/** TPIU formatter enable/disable (in async mode) */
@@ -73,8 +83,10 @@ struct armv7m_trace_config {
 	unsigned int traceclkin_freq;
 	/** Current frequency of trace port */
 	unsigned int trace_freq;
-	/** Handle to output trace data in INTERNAL capture mode */
+	/** Handle to output trace data in INTERNAL capture mode via file */
 	FILE *trace_file;
+	/** Handle to output trace data in INTERNAL capture mode via tcp */
+	struct service *trace_service;
 };
 
 extern const struct command_registration armv7m_trace_command_handlers[];
@@ -84,6 +96,10 @@ extern const struct command_registration armv7m_trace_command_handlers[];
  */
 int armv7m_trace_tpiu_config(struct target *target);
 /**
+ * Disable TPIU data gathering at exit
+ */
+int armv7m_trace_tpiu_exit(struct target *target);
+/**
  * Configure hardware accordingly to the current ITM target settings
  */
 int armv7m_trace_itm_config(struct target *target);
diff --git a/src/target/armv8.c b/src/target/armv8.c
index ab60cd3..95efdc9 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1126,7 +1126,7 @@ int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
 	return ERROR_OK;
 }
 
-int armv8_aarch64_state(struct target *target)
+static int armv8_aarch64_state(struct target *target)
 {
 	struct arm *arm = target_to_arm(target);
 
diff --git a/src/target/avr32_ap7k.c b/src/target/avr32_ap7k.c
index 6221059..b0c0875 100644
--- a/src/target/avr32_ap7k.c
+++ b/src/target/avr32_ap7k.c
@@ -63,7 +63,7 @@ static const struct avr32_core_reg
 static int avr32_read_core_reg(struct target *target, int num);
 static int avr32_write_core_reg(struct target *target, int num);
 
-int avr32_ap7k_save_context(struct target *target)
+static int avr32_ap7k_save_context(struct target *target)
 {
 	int retval, i;
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
@@ -80,7 +80,7 @@ int avr32_ap7k_save_context(struct target *target)
 	return ERROR_OK;
 }
 
-int avr32_ap7k_restore_context(struct target *target)
+static int avr32_ap7k_restore_context(struct target *target)
 {
 	int i;
 
@@ -555,7 +555,7 @@ static int avr32_ap7k_examine(struct target *target)
 	return ERROR_OK;
 }
 
-int avr32_ap7k_arch_state(struct target *target)
+static int avr32_ap7k_arch_state(struct target *target)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 
@@ -565,7 +565,7 @@ int avr32_ap7k_arch_state(struct target *target)
 	return ERROR_OK;
 }
 
-int avr32_ap7k_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
+static int avr32_ap7k_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 		int *reg_list_size, enum target_register_class reg_class)
 {
 #if 0
diff --git a/src/target/avr32_jtag.c b/src/target/avr32_jtag.c
index 64ebf12..62c8f98 100644
--- a/src/target/avr32_jtag.c
+++ b/src/target/avr32_jtag.c
@@ -55,7 +55,7 @@ static int avr32_jtag_set_instr(struct avr32_jtag *jtag_info, int new_instr)
 	return ERROR_OK;
 }
 
-int avr32_jtag_nexus_set_address(struct avr32_jtag *jtag_info,
+static int avr32_jtag_nexus_set_address(struct avr32_jtag *jtag_info,
 		uint32_t addr, int mode)
 {
 	struct scan_field fields[2];
@@ -92,7 +92,7 @@ int avr32_jtag_nexus_set_address(struct avr32_jtag *jtag_info,
 }
 
 
-int avr32_jtag_nexus_read_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_nexus_read_data(struct avr32_jtag *jtag_info,
 	uint32_t *pdata)
 {
 
@@ -129,7 +129,7 @@ int avr32_jtag_nexus_read_data(struct avr32_jtag *jtag_info,
 	return ERROR_OK;
 }
 
-int avr32_jtag_nexus_write_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_nexus_write_data(struct avr32_jtag *jtag_info,
 		uint32_t data)
 {
 
@@ -184,7 +184,7 @@ int avr32_jtag_nexus_write(struct avr32_jtag *jtag_info,
 	return avr32_jtag_nexus_write_data(jtag_info, value);
 }
 
-int avr32_jtag_mwa_set_address(struct avr32_jtag *jtag_info, int slave,
+static int avr32_jtag_mwa_set_address(struct avr32_jtag *jtag_info, int slave,
 		uint32_t addr, int mode)
 {
 	struct scan_field fields[2];
@@ -223,7 +223,7 @@ int avr32_jtag_mwa_set_address(struct avr32_jtag *jtag_info, int slave,
 	return ERROR_OK;
 }
 
-int avr32_jtag_mwa_read_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_mwa_read_data(struct avr32_jtag *jtag_info,
 	uint32_t *pdata)
 {
 
@@ -260,7 +260,7 @@ int avr32_jtag_mwa_read_data(struct avr32_jtag *jtag_info,
 	return ERROR_OK;
 }
 
-int avr32_jtag_mwa_write_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_mwa_write_data(struct avr32_jtag *jtag_info,
 	uint32_t data)
 {
 
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index bd8e49f..d27c298 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -1115,7 +1115,8 @@ static int cortex_a_post_debug_entry(struct target *target)
 	return ERROR_OK;
 }
 
-int cortex_a_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
+static int cortex_a_set_dscr_bits(struct target *target,
+		unsigned long bit_mask, unsigned long value)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	uint32_t dscr;
@@ -1680,10 +1681,10 @@ static int cortex_a_assert_reset(struct target *target)
 		 */
 
 		/*
-		 * FIXME: fix reset when transport is SWD. This is a temporary
+		 * FIXME: fix reset when transport is not JTAG. This is a temporary
 		 * work-around for release v0.10 that is not intended to stay!
 		 */
-		if (transport_is_swd() ||
+		if (!transport_is_jtag() ||
 				(target->reset_halt && (jtag_get_reset_config() & RESET_SRST_NO_GATING)))
 			adapter_assert_reset();
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 55664a7..ac308b4 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -39,6 +39,7 @@
 #include "arm_opcodes.h"
 #include "arm_semihosting.h"
 #include <helper/time_support.h>
+#include <rtt/rtt.h>
 
 /* NOTE:  most of this should work fine for the Cortex-M1 and
  * Cortex-M0 cores too, although they're ARMv6-M not ARMv7-M.
@@ -56,8 +57,8 @@ static int cortex_m_store_core_reg_u32(struct target *target,
 		uint32_t num, uint32_t value);
 static void cortex_m_dwt_free(struct target *target);
 
-static int cortexm_dap_read_coreregister_u32(struct target *target,
-	uint32_t *value, int regnum)
+static int cortex_m_load_core_reg_u32(struct target *target,
+		uint32_t regsel, uint32_t *value)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
@@ -71,7 +72,7 @@ static int cortexm_dap_read_coreregister_u32(struct target *target,
 			return retval;
 	}
 
-	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DCRSR, regnum);
+	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DCRSR, regsel);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -89,8 +90,8 @@ static int cortexm_dap_read_coreregister_u32(struct target *target,
 	return retval;
 }
 
-static int cortexm_dap_write_coreregister_u32(struct target *target,
-	uint32_t value, int regnum)
+static int cortex_m_store_core_reg_u32(struct target *target,
+		uint32_t regsel, uint32_t value)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
@@ -108,7 +109,7 @@ static int cortexm_dap_write_coreregister_u32(struct target *target,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRSR, regnum | DCRSR_WnR);
+	retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRSR, regsel | DCRSR_WnR);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -527,12 +528,6 @@ static int cortex_m_debug_entry(struct target *target)
 	r = arm->cpsr;
 	xPSR = buf_get_u32(r->value, 0, 32);
 
-	/* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
-	if (xPSR & 0xf00) {
-		r->dirty = r->valid;
-		cortex_m_store_core_reg_u32(target, 16, xPSR & ~0xff);
-	}
-
 	/* Are we in an exception handler */
 	if (xPSR & 0x1FF) {
 		armv7m->exception_number = (xPSR & 0x1FF);
@@ -541,7 +536,7 @@ static int cortex_m_debug_entry(struct target *target)
 		arm->map = armv7m_msp_reg_map;
 	} else {
 		unsigned control = buf_get_u32(arm->core_cache
-				->reg_list[ARMV7M_CONTROL].value, 0, 2);
+				->reg_list[ARMV7M_CONTROL].value, 0, 3);
 
 		/* is this thread privileged? */
 		arm->core_mode = control & 1
@@ -656,13 +651,9 @@ static int cortex_m_poll(struct target *target)
 		}
 	}
 
-	/* REVISIT when S_SLEEP is set, it's in a Sleep or DeepSleep state.
-	 * How best to model low power modes?
-	 */
-
 	if (target->state == TARGET_UNKNOWN) {
-		/* check if processor is retiring instructions */
-		if (cortex_m->dcb_dhcsr & S_RETIRE_ST) {
+		/* check if processor is retiring instructions or sleeping */
+		if (cortex_m->dcb_dhcsr & S_RETIRE_ST || cortex_m->dcb_dhcsr & S_SLEEP) {
 			target->state = TARGET_RUNNING;
 			retval = ERROR_OK;
 		}
@@ -827,15 +818,19 @@ static int cortex_m_resume(struct target *target, int current,
 		 * in parallel with disabled interrupts can cause local faults
 		 * to not be taken.
 		 *
-		 * REVISIT this clearly breaks non-debug execution, since the
-		 * PRIMASK register state isn't saved/restored...  workaround
-		 * by never resuming app code after debug execution.
+		 * This breaks non-debug (application) execution if not
+		 * called from armv7m_start_algorithm() which saves registers.
 		 */
 		buf_set_u32(r->value, 0, 1, 1);
 		r->dirty = true;
 		r->valid = true;
 
-		/* Make sure we are in Thumb mode */
+		/* Make sure we are in Thumb mode, set xPSR.T bit */
+		/* armv7m_start_algorithm() initializes entire xPSR register.
+		 * This duplicity handles the case when cortex_m_resume()
+		 * is used with the debug_execution flag directly,
+		 * not called through armv7m_start_algorithm().
+		 */
 		r = armv7m->arm.cpsr;
 		buf_set_u32(r->value, 24, 1, 1);
 		r->dirty = true;
@@ -1208,11 +1203,13 @@ static int cortex_m_assert_reset(struct target *target)
 		if (retval3 != ERROR_OK)
 			LOG_DEBUG("Ignoring AP write error right after reset");
 
-		retval3 = dap_dp_init(armv7m->debug_ap->dap);
-		if (retval3 != ERROR_OK)
+		retval3 = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
+		if (retval3 != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
-
-		else {
+			/* The error return value must not be propagated in this case.
+			 * SYSRESETREQ or VECTRESET have been possibly triggered
+			 * so reset processing should continue */
+		} else {
 			/* I do not know why this is necessary, but it
 			 * fixes strange effects (step/resume cause NMI
 			 * after reset) on LM3S6918 -- Michael Schwingen
@@ -1255,7 +1252,8 @@ static int cortex_m_deassert_reset(struct target *target)
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
 	    !(jtag_reset_config & RESET_SRST_NO_GATING) &&
 		target_was_examined(target)) {
-		int retval = dap_dp_init(armv7m->debug_ap->dap);
+
+		int retval = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
 			return retval;
@@ -1413,7 +1411,7 @@ int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpo
 	return cortex_m_unset_breakpoint(target, breakpoint);
 }
 
-int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
 	int dwt_num = 0;
 	struct cortex_m_common *cortex_m = target_to_cm(target);
@@ -1496,7 +1494,7 @@ int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint
 	return ERROR_OK;
 }
 
-int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct cortex_m_dwt_comparator *comparator;
@@ -1610,176 +1608,6 @@ void cortex_m_enable_watchpoints(struct target *target)
 	}
 }
 
-static int cortex_m_load_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t *value)
-{
-	int retval;
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-		case 0 ... 18:
-			/* read a normal core register */
-			retval = cortexm_dap_read_coreregister_u32(target, value, num);
-
-			if (retval != ERROR_OK) {
-				LOG_ERROR("JTAG failure %i", retval);
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
-			LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
-			break;
-
-		case ARMV7M_FPSCR:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRSR, 0x21);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_read_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("load from FPSCR  value 0x%" PRIx32, *value);
-			break;
-
-		case ARMV7M_S0 ... ARMV7M_S31:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRSR, num - ARMV7M_S0 + 0x40);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_read_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("load from FPU reg S%d  value 0x%" PRIx32,
-				  (int)(num - ARMV7M_S0), *value);
-			break;
-
-		case ARMV7M_PRIMASK:
-		case ARMV7M_BASEPRI:
-		case ARMV7M_FAULTMASK:
-		case ARMV7M_CONTROL:
-			/* Cortex-M3 packages these four registers as bitfields
-			 * in one Debug Core register.  So say r0 and r2 docs;
-			 * it was removed from r1 docs, but still works.
-			 */
-			cortexm_dap_read_coreregister_u32(target, value, 20);
-
-			switch (num) {
-				case ARMV7M_PRIMASK:
-					*value = buf_get_u32((uint8_t *)value, 0, 1);
-					break;
-
-				case ARMV7M_BASEPRI:
-					*value = buf_get_u32((uint8_t *)value, 8, 8);
-					break;
-
-				case ARMV7M_FAULTMASK:
-					*value = buf_get_u32((uint8_t *)value, 16, 1);
-					break;
-
-				case ARMV7M_CONTROL:
-					*value = buf_get_u32((uint8_t *)value, 24, 2);
-					break;
-			}
-
-			LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "", (int)num, *value);
-			break;
-
-		default:
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
-}
-
-static int cortex_m_store_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t value)
-{
-	int retval;
-	uint32_t reg;
-	struct armv7m_common *armv7m = target_to_armv7m(target);
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-		case 0 ... 18:
-			retval = cortexm_dap_write_coreregister_u32(target, value, num);
-			if (retval != ERROR_OK) {
-				struct reg *r;
-
-				LOG_ERROR("JTAG failure");
-				r = armv7m->arm.core_cache->reg_list + num;
-				r->dirty = r->valid;
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
-			LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
-			break;
-
-		case ARMV7M_FPSCR:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, DCB_DCRSR, 0x21 | (1<<16));
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("write FPSCR value 0x%" PRIx32, value);
-			break;
-
-		case ARMV7M_S0 ... ARMV7M_S31:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, DCB_DCRSR, (num - ARMV7M_S0 + 0x40) | (1<<16));
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("write FPU reg S%d  value 0x%" PRIx32,
-				  (int)(num - ARMV7M_S0), value);
-			break;
-
-		case ARMV7M_PRIMASK:
-		case ARMV7M_BASEPRI:
-		case ARMV7M_FAULTMASK:
-		case ARMV7M_CONTROL:
-			/* Cortex-M3 packages these four registers as bitfields
-			 * in one Debug Core register.  So say r0 and r2 docs;
-			 * it was removed from r1 docs, but still works.
-			 */
-			cortexm_dap_read_coreregister_u32(target, &reg, 20);
-
-			switch (num) {
-				case ARMV7M_PRIMASK:
-					buf_set_u32((uint8_t *)&reg, 0, 1, value);
-					break;
-
-				case ARMV7M_BASEPRI:
-					buf_set_u32((uint8_t *)&reg, 8, 8, value);
-					break;
-
-				case ARMV7M_FAULTMASK:
-					buf_set_u32((uint8_t *)&reg, 16, 1, value);
-					break;
-
-				case ARMV7M_CONTROL:
-					buf_set_u32((uint8_t *)&reg, 24, 2, value);
-					break;
-			}
-
-			cortexm_dap_write_coreregister_u32(target, reg, 20);
-
-			LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
-			break;
-
-		default:
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
-}
-
 static int cortex_m_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
@@ -1820,6 +1648,8 @@ void cortex_m_deinit_target(struct target *target)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 
+	armv7m_trace_tpiu_exit(target);
+
 	free(cortex_m->fp_comparator_list);
 
 	cortex_m_dwt_free(target);
@@ -1835,28 +1665,23 @@ int cortex_m_profiling(struct target *target, uint32_t *samples,
 	struct timeval timeout, now;
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	uint32_t reg_value;
-	bool use_pcsr = false;
-	int retval = ERROR_OK;
-	struct reg *reg;
-
-	gettimeofday(&timeout, NULL);
-	timeval_add_time(&timeout, seconds, 0);
+	int retval;
 
 	retval = target_read_u32(target, DWT_PCSR, &reg_value);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error while reading PCSR");
 		return retval;
 	}
-
-	if (reg_value != 0) {
-		use_pcsr = true;
-		LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
-	} else {
-		LOG_INFO("Starting profiling. Halting and resuming the"
-			 " target as often as we can...");
-		reg = register_get_by_name(target->reg_cache, "pc", 1);
+	if (reg_value == 0) {
+		LOG_INFO("PCSR sampling not supported on this processor.");
+		return target_profiling_default(target, samples, max_num_samples, num_samples, seconds);
 	}
 
+	gettimeofday(&timeout, NULL);
+	timeval_add_time(&timeout, seconds, 0);
+
+	LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
+
 	/* Make sure the target is running */
 	target_poll(target);
 	if (target->state == TARGET_HALTED)
@@ -1870,40 +1695,21 @@ int cortex_m_profiling(struct target *target, uint32_t *samples,
 	uint32_t sample_count = 0;
 
 	for (;;) {
-		if (use_pcsr) {
-			if (armv7m && armv7m->debug_ap) {
-				uint32_t read_count = max_num_samples - sample_count;
-				if (read_count > 1024)
-					read_count = 1024;
-
-				retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
-							(void *)&samples[sample_count],
-							4, read_count, DWT_PCSR);
-				sample_count += read_count;
-			} else {
-				target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
-			}
+		if (armv7m && armv7m->debug_ap) {
+			uint32_t read_count = max_num_samples - sample_count;
+			if (read_count > 1024)
+				read_count = 1024;
+
+			retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
+						(void *)&samples[sample_count],
+						4, read_count, DWT_PCSR);
+			sample_count += read_count;
 		} else {
-			target_poll(target);
-			if (target->state == TARGET_HALTED) {
-				reg_value = buf_get_u32(reg->value, 0, 32);
-				/* current pc, addr = 0, do not handle breakpoints, not debugging */
-				retval = target_resume(target, 1, 0, 0, 0);
-				samples[sample_count++] = reg_value;
-				target_poll(target);
-				alive_sleep(10); /* sleep 10ms, i.e. <100 samples/second. */
-			} else if (target->state == TARGET_RUNNING) {
-				/* We want to quickly sample the PC. */
-				retval = target_halt(target);
-			} else {
-				LOG_INFO("Target not halted or running");
-				retval = ERROR_OK;
-				break;
-			}
+			target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
 		}
 
 		if (retval != ERROR_OK) {
-			LOG_ERROR("Error while reading %s", use_pcsr ? "PCSR" : "target pc");
+			LOG_ERROR("Error while reading PCSR");
 			return retval;
 		}
 
@@ -2013,7 +1819,7 @@ static void cortex_m_dwt_addreg(struct target *t, struct reg *r, const struct dw
 	r->type = &dwt_reg_type;
 }
 
-void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
+static void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
 {
 	uint32_t dwtcr;
 	struct reg_cache *cache;
@@ -2242,7 +2048,6 @@ int cortex_m_examine(struct target *target)
 			for (idx = ARMV7M_NUM_CORE_REGS_NOFP;
 			     idx < armv7m->arm.core_cache->num_regs;
 			     idx++) {
-				free(armv7m->arm.core_cache->reg_list[idx].value);
 				free(armv7m->arm.core_cache->reg_list[idx].feature);
 				free(armv7m->arm.core_cache->reg_list[idx].reg_data_type);
 			}
@@ -2687,6 +2492,9 @@ static const struct command_registration cortex_m_command_handlers[] = {
 		.usage = "",
 		.chain = cortex_m_exec_command_handlers,
 	},
+	{
+		.chain = rtt_target_command_handlers,
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index 415a6c2..b470fbd 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -233,13 +233,10 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoint);
 int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint);
 int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpoint);
-int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint);
-int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint);
 int cortex_m_add_watchpoint(struct target *target, struct watchpoint *watchpoint);
 int cortex_m_remove_watchpoint(struct target *target, struct watchpoint *watchpoint);
 void cortex_m_enable_breakpoints(struct target *target);
 void cortex_m_enable_watchpoints(struct target *target);
-void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target);
 void cortex_m_deinit_target(struct target *target);
 int cortex_m_profiling(struct target *target, uint32_t *samples,
 	uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds);
diff --git a/src/target/dsp5680xx.c b/src/target/dsp5680xx.c
index d6107ab..ee26d24 100644
--- a/src/target/dsp5680xx.c
+++ b/src/target/dsp5680xx.c
@@ -40,7 +40,7 @@ struct dsp5680xx_common dsp5680xx_context;
 #define CHECK_HALT(target) if (target->state != TARGET_HALTED) HALT_FAIL
 #define check_halt_and_debug(target) { CHECK_HALT(target); CHECK_DBG; }
 
-int dsp5680xx_execute_queue(void)
+static int dsp5680xx_execute_queue(void)
 {
 	int retval;
 
@@ -892,12 +892,6 @@ static int dsp5680xx_arch_state(struct target *target)
 	return ERROR_OK;
 }
 
-int dsp5680xx_target_status(struct target *target, uint8_t *jtag_st,
-			    uint16_t *eonce_st)
-{
-	return target->state;
-}
-
 static int dsp5680xx_assert_reset(struct target *target)
 {
 	target->state = TARGET_RESET;
@@ -1555,7 +1549,7 @@ static int perl_crc(const uint8_t *buff8, uint32_t word_count)
  *
  * @return
  */
-int dsp5680xx_f_SIM_reset(struct target *target)
+static int dsp5680xx_f_SIM_reset(struct target *target)
 {
 	int retval = ERROR_OK;
 
@@ -1978,7 +1972,8 @@ int dsp5680xx_f_erase(struct target *target, int first, int last)
  * 0x0000001E  0xA961		 bra	 *-30
  */
 
-const uint16_t pgm_write_pflash[] = { 0x8A46, 0x0013, 0x807D, 0xE700,
+static const uint16_t pgm_write_pflash[] = {
+		0x8A46, 0x0013, 0x807D, 0xE700,
 		0xE700, 0x8A44, 0xFFFE, 0x017B,
 		0xE700, 0xF514, 0x8563, 0x8646,
 		0x0020, 0x0014, 0x8646, 0x0080,
@@ -1988,7 +1983,7 @@ const uint16_t pgm_write_pflash[] = { 0x8A46, 0x0013, 0x807D, 0xE700,
 		0x0013, 0x0010, 0xA961
 };
 
-const uint32_t pgm_write_pflash_length = 31;
+static const uint32_t pgm_write_pflash_length = 31;
 
 int dsp5680xx_f_wr(struct target *t, const uint8_t *b, uint32_t a, uint32_t count,
 		   int is_flash_lock)
diff --git a/src/target/etm.c b/src/target/etm.c
index 5d079ff..faa941f 100644
--- a/src/target/etm.c
+++ b/src/target/etm.c
@@ -279,7 +279,7 @@ static void etm_reg_add(unsigned bcd_vers, struct arm_jtag *jtag_info,
 
 		reg->name = r->name;
 		reg->size = r->size;
-		reg->value = &ereg->value;
+		reg->value = ereg->value;
 		reg->arch_info = ereg;
 		reg->type = &etm_scan6_type;
 		reg++;
@@ -650,7 +650,6 @@ static struct etm_capture_driver *etm_capture_drivers[] = {
 
 static int etm_read_instruction(struct etm_context *ctx, struct arm_instruction *instruction)
 {
-	int i;
 	int section = -1;
 	size_t size_read;
 	uint32_t opcode;
@@ -660,7 +659,7 @@ static int etm_read_instruction(struct etm_context *ctx, struct arm_instruction
 		return ERROR_TRACE_IMAGE_UNAVAILABLE;
 
 	/* search for the section the current instruction belongs to */
-	for (i = 0; i < ctx->image->num_sections; i++) {
+	for (unsigned int i = 0; i < ctx->image->num_sections; i++) {
 		if ((ctx->image->sections[i].base_address <= ctx->current_pc) &&
 			(ctx->image->sections[i].base_address + ctx->image->sections[i].size >
 			ctx->current_pc)) {
@@ -1683,15 +1682,15 @@ COMMAND_HANDLER(handle_etm_image_command)
 	}
 
 	etm_ctx->image = malloc(sizeof(struct image));
-	etm_ctx->image->base_address_set = 0;
-	etm_ctx->image->start_address_set = 0;
+	etm_ctx->image->base_address_set = false;
+	etm_ctx->image->start_address_set = false;
 
 	/* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
 	if (CMD_ARGC >= 2) {
-		etm_ctx->image->base_address_set = 1;
+		etm_ctx->image->base_address_set = true;
 		COMMAND_PARSE_NUMBER(llong, CMD_ARGV[1], etm_ctx->image->base_address);
 	} else
-		etm_ctx->image->base_address_set = 0;
+		etm_ctx->image->base_address_set = false;
 
 	if (image_open(etm_ctx->image, CMD_ARGV[0],
 		(CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL) != ERROR_OK) {
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index f0dc572..3d41387 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -39,6 +39,7 @@
 #include "cortex_m.h"
 #include "arm_semihosting.h"
 #include "target_request.h"
+#include <rtt/rtt.h>
 
 #define savedDCRDR  dbgbase  /* FIXME: using target->dbgbase to preserve DCRDR */
 
@@ -51,184 +52,17 @@ static inline struct hl_interface_s *target_to_adapter(struct target *target)
 }
 
 static int adapter_load_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t *value)
+		uint32_t regsel, uint32_t *value)
 {
-	int retval;
 	struct hl_interface_s *adapter = target_to_adapter(target);
-
-	LOG_DEBUG("%s", __func__);
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-	case 0 ... 18:
-		/* read a normal core register */
-		retval = adapter->layout->api->read_reg(adapter->handle, num, value);
-
-		if (retval != ERROR_OK) {
-			LOG_ERROR("JTAG failure %i", retval);
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
-		break;
-
-	case ARMV7M_FPSCR:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, 33);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_read_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("load from FPSCR  value 0x%" PRIx32, *value);
-		break;
-
-	case ARMV7M_S0 ... ARMV7M_S31:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, num-ARMV7M_S0+64);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_read_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("load from FPU reg S%d  value 0x%" PRIx32,
-			  (int)(num - ARMV7M_S0), *value);
-		break;
-
-	case ARMV7M_PRIMASK:
-	case ARMV7M_BASEPRI:
-	case ARMV7M_FAULTMASK:
-	case ARMV7M_CONTROL:
-		/* Cortex-M3 packages these four registers as bitfields
-		 * in one Debug Core register.  So say r0 and r2 docs;
-		 * it was removed from r1 docs, but still works.
-		 */
-		retval = adapter->layout->api->read_reg(adapter->handle, 20, value);
-		if (retval != ERROR_OK)
-			return retval;
-
-		switch (num) {
-		case ARMV7M_PRIMASK:
-			*value = buf_get_u32((uint8_t *) value, 0, 1);
-			break;
-
-		case ARMV7M_BASEPRI:
-			*value = buf_get_u32((uint8_t *) value, 8, 8);
-			break;
-
-		case ARMV7M_FAULTMASK:
-			*value = buf_get_u32((uint8_t *) value, 16, 1);
-			break;
-
-		case ARMV7M_CONTROL:
-			*value = buf_get_u32((uint8_t *) value, 24, 2);
-			break;
-		}
-
-		LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "",
-			  (int)num, *value);
-		break;
-
-	default:
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
+	return adapter->layout->api->read_reg(adapter->handle, regsel, value);
 }
 
 static int adapter_store_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t value)
+		uint32_t regsel, uint32_t value)
 {
-	int retval;
-	uint32_t reg;
-	struct armv7m_common *armv7m = target_to_armv7m(target);
 	struct hl_interface_s *adapter = target_to_adapter(target);
-
-	LOG_DEBUG("%s", __func__);
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-	case 0 ... 18:
-		retval = adapter->layout->api->write_reg(adapter->handle, num, value);
-
-		if (retval != ERROR_OK) {
-			struct reg *r;
-
-			LOG_ERROR("JTAG failure");
-			r = armv7m->arm.core_cache->reg_list + num;
-			r->dirty = r->valid;
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
-		break;
-
-	case ARMV7M_FPSCR:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, 33 | (1<<16));
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("write FPSCR value 0x%" PRIx32, value);
-		break;
-
-	case ARMV7M_S0 ... ARMV7M_S31:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, (num-ARMV7M_S0+64) | (1<<16));
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("write FPU reg S%d  value 0x%" PRIx32,
-			  (int)(num - ARMV7M_S0), value);
-		break;
-
-	case ARMV7M_PRIMASK:
-	case ARMV7M_BASEPRI:
-	case ARMV7M_FAULTMASK:
-	case ARMV7M_CONTROL:
-		/* Cortex-M3 packages these four registers as bitfields
-		 * in one Debug Core register.  So say r0 and r2 docs;
-		 * it was removed from r1 docs, but still works.
-		 */
-
-		adapter->layout->api->read_reg(adapter->handle, 20, &reg);
-
-		switch (num) {
-		case ARMV7M_PRIMASK:
-			buf_set_u32((uint8_t *) &reg, 0, 1, value);
-			break;
-
-		case ARMV7M_BASEPRI:
-			buf_set_u32((uint8_t *) &reg, 8, 8, value);
-			break;
-
-		case ARMV7M_FAULTMASK:
-			buf_set_u32((uint8_t *) &reg, 16, 1, value);
-			break;
-
-		case ARMV7M_CONTROL:
-			buf_set_u32((uint8_t *) &reg, 24, 2, value);
-			break;
-		}
-
-		adapter->layout->api->write_reg(adapter->handle, 20, reg);
-
-		LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
-		break;
-
-	default:
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
+	return adapter->layout->api->write_reg(adapter->handle, regsel, value);
 }
 
 static int adapter_examine_debug_reason(struct target *target)
@@ -433,7 +267,7 @@ static int adapter_debug_entry(struct target *target)
 		arm->map = armv7m_msp_reg_map;
 	} else {
 		unsigned control = buf_get_u32(arm->core_cache
-				->reg_list[ARMV7M_CONTROL].value, 0, 2);
+				->reg_list[ARMV7M_CONTROL].value, 0, 3);
 
 		/* is this thread privileged? */
 		arm->core_mode = control & 1
@@ -793,6 +627,9 @@ static const struct command_registration adapter_command_handlers[] = {
 	{
 		.chain = armv7m_trace_command_handlers,
 	},
+	{
+		.chain = rtt_target_command_handlers,
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/image.c b/src/target/image.c
index 68262e9..fd5eff8 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -124,7 +124,6 @@ static int image_ihex_buffer_complete_inner(struct image *image,
 	uint32_t full_address;
 	uint32_t cooked_bytes;
 	bool end_rec = false;
-	int i;
 
 	/* we can't determine the number of sections that we'll have to create ahead of time,
 	 * so we locally hold them until parsing is finished */
@@ -207,7 +206,7 @@ static int image_ihex_buffer_complete_inner(struct image *image,
 
 				/* copy section information */
 				image->sections = malloc(sizeof(struct imagesection) * image->num_sections);
-				for (i = 0; i < image->num_sections; i++) {
+				for (unsigned int i = 0; i < image->num_sections; i++) {
 					image->sections[i].private = section[i].private;
 					image->sections[i].base_address = section[i].base_address;
 					image->sections[i].size = section[i].size;
@@ -294,7 +293,7 @@ static int image_ihex_buffer_complete_inner(struct image *image,
 				cal_checksum += (uint8_t)start_address;
 				bytes_read += 8;
 
-				image->start_address_set = 1;
+				image->start_address_set = true;
 				image->start_address = be_to_h_u32((uint8_t *)&start_address);
 			} else {
 				LOG_ERROR("unhandled IHEX record type: %i", (int)record_type);
@@ -471,7 +470,7 @@ static int image_elf_read_headers(struct image *image)
 		}
 	}
 
-	image->start_address_set = 1;
+	image->start_address_set = true;
 	image->start_address = field32(elf, elf->header->e_entry);
 
 	return ERROR_OK;
@@ -529,7 +528,6 @@ static int image_mot_buffer_complete_inner(struct image *image,
 	uint32_t full_address;
 	uint32_t cooked_bytes;
 	bool end_rec = false;
-	int i;
 
 	/* we can't determine the number of sections that we'll have to create ahead of time,
 	 * so we locally hold them until parsing is finished */
@@ -658,7 +656,7 @@ static int image_mot_buffer_complete_inner(struct image *image,
 
 				/* copy section information */
 				image->sections = malloc(sizeof(struct imagesection) * image->num_sections);
-				for (i = 0; i < image->num_sections; i++) {
+				for (unsigned int i = 0; i < image->num_sections; i++) {
 					image->sections[i].private = section[i].private;
 					image->sections[i].base_address = section[i].base_address;
 					image->sections[i].size = section[i].size;
@@ -821,21 +819,20 @@ int image_open(struct image *image, const char *url, const char *type_string)
 		}
 	} else if (image->type == IMAGE_BUILDER) {
 		image->num_sections = 0;
-		image->base_address_set = 0;
+		image->base_address_set = false;
 		image->sections = NULL;
 		image->type_private = NULL;
 	}
 
 	if (image->base_address_set) {
 		/* relocate */
-		int section;
-		for (section = 0; section < image->num_sections; section++)
+		for (unsigned int section = 0; section < image->num_sections; section++)
 			image->sections[section].base_address += image->base_address;
 											/* we're done relocating. The two statements below are mainly
 											* for documentation purposes: stop anyone from empirically
 											* thinking they should use these values henceforth. */
 		image->base_address = 0;
-		image->base_address_set = 0;
+		image->base_address_set = false;
 	}
 
 	return retval;
@@ -1009,9 +1006,7 @@ void image_close(struct image *image)
 		free(image_mot->buffer);
 		image_mot->buffer = NULL;
 	} else if (image->type == IMAGE_BUILDER) {
-		int i;
-
-		for (i = 0; i < image->num_sections; i++) {
+		for (unsigned int i = 0; i < image->num_sections; i++) {
 			free(image->sections[i].private);
 			image->sections[i].private = NULL;
 		}
@@ -1024,7 +1019,7 @@ void image_close(struct image *image)
 	image->sections = NULL;
 }
 
-int image_calculate_checksum(uint8_t *buffer, uint32_t nbytes, uint32_t *checksum)
+int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes, uint32_t *checksum)
 {
 	uint32_t crc = 0xffffffff;
 	LOG_DEBUG("Calculating checksum");
diff --git a/src/target/image.h b/src/target/image.h
index 9907a5f..53c27d8 100644
--- a/src/target/image.h
+++ b/src/target/image.h
@@ -55,11 +55,11 @@ struct imagesection {
 struct image {
 	enum image_type type;		/* image type (plain, ihex, ...) */
 	void *type_private;		/* type private data */
-	int num_sections;		/* number of sections contained in the image */
+	unsigned int num_sections;		/* number of sections contained in the image */
 	struct imagesection *sections;	/* array of sections */
-	int base_address_set;	/* whether the image has a base address set (for relocation purposes) */
+	bool base_address_set;	/* whether the image has a base address set (for relocation purposes) */
 	long long base_address;		/* base address, if one is set */
-	int start_address_set;	/* whether the image has a start address (entry point) associated */
+	bool start_address_set;	/* whether the image has a start address (entry point) associated */
 	uint32_t start_address;		/* start address, if one is set */
 };
 
@@ -99,7 +99,7 @@ void image_close(struct image *image);
 int image_add_section(struct image *image, uint32_t base, uint32_t size,
 		int flags, uint8_t const *data);
 
-int image_calculate_checksum(uint8_t *buffer, uint32_t nbytes,
+int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes,
 		uint32_t *checksum);
 
 #define ERROR_IMAGE_FORMAT_ERROR	(-1400)
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 9bac40e..d6bd1c5 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -120,7 +120,7 @@ static void mips32_pracc_finish(struct mips_ejtag *ejtag_info)
 	mips_ejtag_drscan_32_out(ejtag_info, ctrl);
 }
 
-int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
+static int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 {
 	uint32_t jt_code = MIPS32_J(ejtag_info->isa, MIPS32_PRACC_TEXT);
 	pracc_swap16_array(ejtag_info, &jt_code, 1);
@@ -453,7 +453,7 @@ exit:
 	return retval;
 }
 
-int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
+static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 {
 	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 7544afe..4b049fb 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -58,7 +58,7 @@ int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info)
 	return mips_ejtag_drscan_32(ejtag_info, &ejtag_info->idcode);
 }
 
-int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
+static int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
 {
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_IMPCODE);
 
@@ -119,7 +119,8 @@ int mips_ejtag_drscan_64(struct mips_ejtag *ejtag_info, uint64_t *data)
 	return ERROR_OK;
 }
 
-void mips_ejtag_drscan_32_queued(struct mips_ejtag *ejtag_info, uint32_t data_out, uint8_t *data_in)
+static void mips_ejtag_drscan_32_queued(struct mips_ejtag *ejtag_info,
+		uint32_t data_out, uint8_t *data_in)
 {
 	assert(ejtag_info->tap != NULL);
 	struct jtag_tap *tap = ejtag_info->tap;
diff --git a/src/target/mips_mips64.c b/src/target/mips_mips64.c
index f941af5..0fc0897 100644
--- a/src/target/mips_mips64.c
+++ b/src/target/mips_mips64.c
@@ -62,7 +62,7 @@ static int mips_mips64_debug_entry(struct target *target)
 	mips_mips64_examine_debug_reason(target);
 
 	LOG_DEBUG("entered debug state at PC 0x%" PRIx64 ", target->state: %s",
-		  *(uint64_t *)pc->value, target_state_name(target));
+		  buf_get_u64(pc->value, 0, 64), target_state_name(target));
 
 	return ERROR_OK;
 }
diff --git a/src/target/nds32.c b/src/target/nds32.c
index 487e19c..add66b2 100644
--- a/src/target/nds32.c
+++ b/src/target/nds32.c
@@ -2496,6 +2496,12 @@ int nds32_profiling(struct target *target, uint32_t *samples,
 	struct aice_port_s *aice = target_to_aice(target);
 	struct nds32 *nds32 = target_to_nds32(target);
 
+	/* REVISIT: can nds32 profile without halting? */
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target %s is not halted (profiling)", target->cmd_name);
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
 	if (max_num_samples < iteration)
 		iteration = max_num_samples;
 
diff --git a/src/target/nds32_tlb.c b/src/target/nds32_tlb.c
index c4bce1a..93a9241 100644
--- a/src/target/nds32_tlb.c
+++ b/src/target/nds32_tlb.c
@@ -31,7 +31,7 @@ int nds32_probe_tlb(struct nds32 *nds32, const target_addr_t virtual_address,
 	return aice_read_tlb(aice, virtual_address, physical_address);
 }
 
-struct page_table_walker_info_s page_table_info[PAGE_SIZE_NUM] = {
+static struct page_table_walker_info_s page_table_info[PAGE_SIZE_NUM] = {
 	/* 4K page */
 	{0xFFC00000, 20, 0x003FF000, 10, 0x00000FFF, 0xFFFFF000, 0xFFFFF000, 0xFFFFF000},
 	/* 8K page */
diff --git a/src/target/nds32_v3.c b/src/target/nds32_v3.c
index e5d146b..f9cd47a 100644
--- a/src/target/nds32_v3.c
+++ b/src/target/nds32_v3.c
@@ -404,7 +404,7 @@ static int nds32_v3_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-struct nds32_v3_common_callback nds32_v3_common_callback = {
+static struct nds32_v3_common_callback nds32_v3_common_callback = {
 	.check_interrupt_stack = nds32_v3_check_interrupt_stack,
 	.restore_interrupt_stack = nds32_v3_restore_interrupt_stack,
 	.activate_hardware_breakpoint = nds32_v3_activate_hardware_breakpoint,
diff --git a/src/target/nds32_v3m.c b/src/target/nds32_v3m.c
index 86903a5..952d0eb 100644
--- a/src/target/nds32_v3m.c
+++ b/src/target/nds32_v3m.c
@@ -379,7 +379,7 @@ static int nds32_v3m_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-struct nds32_v3_common_callback nds32_v3m_common_callback = {
+static struct nds32_v3_common_callback nds32_v3m_common_callback = {
 	.check_interrupt_stack = nds32_v3m_check_interrupt_stack,
 	.restore_interrupt_stack = nds32_v3m_restore_interrupt_stack,
 	.activate_hardware_breakpoint = nds32_v3m_activate_hardware_breakpoint,
diff --git a/src/target/openrisc/jsp_server.c b/src/target/openrisc/jsp_server.c
index 1d05944..b4b2566 100644
--- a/src/target/openrisc/jsp_server.c
+++ b/src/target/openrisc/jsp_server.c
@@ -57,7 +57,7 @@ static int telnet_write(struct connection *connection, const void *data, int len
 	return ERROR_SERVER_REMOTE_CLOSED;
 }
 
-int jsp_poll_read(void *priv)
+static int jsp_poll_read(void *priv)
 {
 	struct jsp_service *jsp_service = (struct jsp_service *)priv;
 	unsigned char out_buffer[10];
@@ -207,7 +207,8 @@ int jsp_init(struct or1k_jtag *jtag_info, char *banner)
 		jsp_new_connection,
 		jsp_input,
 		jsp_connection_closed,
-		jsp_service);
+		jsp_service,
+		NULL);
 }
 
 COMMAND_HANDLER(handle_jsp_port_command)
diff --git a/src/target/openrisc/or1k.c b/src/target/openrisc/or1k.c
index d685359..5b8d7de 100644
--- a/src/target/openrisc/or1k.c
+++ b/src/target/openrisc/or1k.c
@@ -1200,7 +1200,7 @@ static int or1k_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 
 }
 
-int or1k_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
+static int or1k_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
 {
 	return ERROR_FAIL;
 }
diff --git a/src/target/quark_d20xx.c b/src/target/quark_d20xx.c
index 42d3b8c..9169379 100644
--- a/src/target/quark_d20xx.c
+++ b/src/target/quark_d20xx.c
@@ -43,7 +43,7 @@
 #include "lakemont.h"
 #include "x86_32_common.h"
 
-int quark_d20xx_target_create(struct target *t, Jim_Interp *interp)
+static int quark_d20xx_target_create(struct target *t, Jim_Interp *interp)
 {
 	struct x86_32_common *x86_32 = calloc(1, sizeof(struct x86_32_common));
 	if (x86_32 == NULL) {
@@ -56,7 +56,7 @@ int quark_d20xx_target_create(struct target *t, Jim_Interp *interp)
 	return ERROR_OK;
 }
 
-int quark_d20xx_init_target(struct command_context *cmd_ctx, struct target *t)
+static int quark_d20xx_init_target(struct command_context *cmd_ctx, struct target *t)
 {
 	return lakemont_init_target(cmd_ctx, t);
 }
diff --git a/src/target/register.h b/src/target/register.h
index 7c53d6e..5f1c25f 100644
--- a/src/target/register.h
+++ b/src/target/register.h
@@ -127,13 +127,15 @@ struct reg {
 	bool caller_save;
 	/* Pointer to place where the value is stored, in the format understood by
 	 * the binarybuffer.h functions. */
-	void *value;
+	uint8_t *value;
 	/* The stored value needs to be written to the target. */
 	bool dirty;
 	/* When true, value is valid. */
 	bool valid;
 	/* When false, the register doesn't actually exist in the target. */
 	bool exist;
+	/* Hide the register from gdb and omit it in 'reg' cmd output */
+	bool hidden;
 	/* Size of the register in bits. */
 	uint32_t size;
 	/* Used for generating XML description of registers. Can be set to NULL for
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 2c34795..9978931 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -4835,7 +4835,7 @@ int riscv_init_registers(struct target *target)
 			assert(reg_name < info->reg_names + target->reg_cache->num_regs *
 					max_reg_name_len);
 		}
-		r->value = &info->reg_cache_values[number];
+		r->value = info->reg_cache_values[number];
 	}
 
 	return ERROR_OK;
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 8e098c0..aa9614a 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -101,8 +101,8 @@ typedef struct {
 
 	/* OpenOCD's register cache points into here. This is not per-hart because
 	 * we just invalidate the entire cache when we change which hart is
-	 * selected. */
-	uint64_t reg_cache_values[RISCV_MAX_REGISTERS];
+	 * selected. Use an array of 8 uint8_t per register. */
+	uint8_t reg_cache_values[RISCV_MAX_REGISTERS][8];
 
 	/* Single buffer that contains all register names, instead of calling
 	 * malloc for each register. Needs to be freed when reg_list is freed. */
diff --git a/src/target/riscv/riscv_semihosting.c b/src/target/riscv/riscv_semihosting.c
index 3048709..c39f030 100644
--- a/src/target/riscv/riscv_semihosting.c
+++ b/src/target/riscv/riscv_semihosting.c
@@ -143,7 +143,7 @@ semihosting_result_t riscv_semihosting(struct target *target, int *retval)
 		if (0 <= semihosting->op && semihosting->op <= 0x31) {
 			*retval = semihosting_common(target);
 			if (*retval != ERROR_OK) {
-				LOG_ERROR("Failed semihosting operation");
+				LOG_ERROR("Failed semihosting operation (0x%02X)", semihosting->op);
 				return SEMI_ERROR;
 			}
 		} else {
diff --git a/src/target/rtt.c b/src/target/rtt.c
new file mode 100644
index 0000000..7e556e1
--- /dev/null
+++ b/src/target/rtt.c
@@ -0,0 +1,424 @@
+/*
+ * Copyright (C) 2016-2020 by Marc Schink <dev@zapb.de>
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+#include <helper/log.h>
+#include <helper/binarybuffer.h>
+#include <helper/command.h>
+#include <rtt/rtt.h>
+
+#include "target.h"
+
+static int read_rtt_channel(struct target *target,
+		const struct rtt_control *ctrl, unsigned int channel_index,
+		enum rtt_channel_type type, struct rtt_channel *channel)
+{
+	int ret;
+	uint8_t buf[RTT_CHANNEL_SIZE];
+	target_addr_t address;
+
+	address = ctrl->address + RTT_CB_SIZE + (channel_index * RTT_CHANNEL_SIZE);
+
+	if (type == RTT_CHANNEL_TYPE_DOWN)
+		address += ctrl->num_up_channels * RTT_CHANNEL_SIZE;
+
+	ret = target_read_buffer(target, address, RTT_CHANNEL_SIZE, buf);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	channel->address = address;
+	channel->name_addr = buf_get_u32(buf + 0, 0, 32);
+	channel->buffer_addr = buf_get_u32(buf + 4, 0, 32);
+	channel->size = buf_get_u32(buf + 8, 0, 32);
+	channel->write_pos = buf_get_u32(buf + 12, 0, 32);
+	channel->read_pos = buf_get_u32(buf + 16, 0, 32);
+	channel->flags = buf_get_u32(buf + 20, 0, 32);
+
+	return ERROR_OK;
+}
+
+int target_rtt_start(struct target *target, const struct rtt_control *ctrl,
+		void *user_data)
+{
+	return ERROR_OK;
+}
+
+int target_rtt_stop(struct target *target, void *user_data)
+{
+	return ERROR_OK;
+}
+
+static int read_channel_name(struct target *target, target_addr_t address,
+		char *name, size_t length)
+{
+	size_t offset;
+
+	offset = 0;
+
+	while (offset < length) {
+		int ret;
+		size_t read_length;
+
+		read_length = MIN(32, length - offset);
+		ret = target_read_buffer(target, address + offset, read_length,
+			(uint8_t *)name + offset);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		if (memchr(name + offset, '\0', read_length))
+			return ERROR_OK;
+
+		offset += read_length;
+	}
+
+	name[length - 1] = '\0';
+
+	return ERROR_OK;
+}
+
+static int write_to_channel(struct target *target,
+		const struct rtt_channel *channel, const uint8_t *buffer,
+		size_t *length)
+{
+	int ret;
+	uint32_t len;
+
+	if (!*length)
+		return ERROR_OK;
+
+	if (channel->write_pos == channel->read_pos) {
+		uint32_t first_length;
+
+		len = MIN(*length, channel->size - 1);
+		first_length = MIN(len, channel->size - channel->write_pos);
+
+		ret = target_write_buffer(target,
+			channel->buffer_addr + channel->write_pos, first_length,
+			buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		ret = target_write_buffer(target, channel->buffer_addr,
+			len - first_length, buffer + first_length);
+
+		if (ret != ERROR_OK)
+			return ret;
+	} else if (channel->write_pos < channel->read_pos) {
+		len = MIN(*length, channel->read_pos - channel->write_pos - 1);
+
+		if (!len) {
+			*length = 0;
+			return ERROR_OK;
+		}
+
+		ret = target_write_buffer(target,
+			channel->buffer_addr + channel->write_pos, len, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+	} else {
+		uint32_t first_length;
+
+		len = MIN(*length,
+			channel->size - channel->write_pos + channel->read_pos - 1);
+
+		if (!len) {
+			*length = 0;
+			return ERROR_OK;
+		}
+
+		first_length = MIN(len, channel->size - channel->write_pos);
+
+		ret = target_write_buffer(target,
+			channel->buffer_addr + channel->write_pos, first_length,
+			buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		buffer = buffer + first_length;
+
+		ret = target_write_buffer(target, channel->buffer_addr,
+			len - first_length, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+	}
+
+	ret = target_write_u32(target, channel->address + 12,
+		(channel->write_pos + len) % channel->size);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	*length = len;
+
+	return ERROR_OK;
+}
+
+static bool channel_is_active(const struct rtt_channel *channel)
+{
+	if (!channel)
+		return false;
+
+	if (!channel->size)
+		return false;
+
+	return true;
+}
+
+int target_rtt_write_callback(struct target *target, struct rtt_control *ctrl,
+		unsigned int channel_index, const uint8_t *buffer, size_t *length,
+		void *user_data)
+{
+	int ret;
+	struct rtt_channel channel;
+
+	ret = read_rtt_channel(target, ctrl, channel_index,
+		RTT_CHANNEL_TYPE_DOWN, &channel);
+
+	if (ret != ERROR_OK) {
+		LOG_ERROR("rtt: Failed to read down-channel %u description",
+			channel_index);
+		return ret;
+	}
+
+	if (!channel_is_active(&channel)) {
+		LOG_WARNING("rtt: Down-channel %u is not active", channel_index);
+		return ERROR_OK;
+	}
+
+	if (channel.size < RTT_CHANNEL_BUFFER_MIN_SIZE) {
+		LOG_WARNING("rtt: Down-channel %u is not large enough",
+			channel_index);
+		return ERROR_OK;
+	}
+
+	ret = write_to_channel(target, &channel, buffer, length);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	LOG_DEBUG("rtt: Wrote %zu bytes into down-channel %u", *length,
+		channel_index);
+
+	return ERROR_OK;
+}
+
+int target_rtt_read_control_block(struct target *target,
+		target_addr_t address, struct rtt_control *ctrl, void *user_data)
+{
+	int ret;
+	uint8_t buf[RTT_CB_SIZE];
+
+	ret = target_read_buffer(target, address, RTT_CB_SIZE, buf);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	memcpy(ctrl->id, buf, RTT_CB_MAX_ID_LENGTH);
+	ctrl->id[RTT_CB_MAX_ID_LENGTH - 1] = '\0';
+	ctrl->num_up_channels = buf_get_u32(buf + RTT_CB_MAX_ID_LENGTH + 0,
+		0, 32);
+	ctrl->num_down_channels = buf_get_u32(buf + RTT_CB_MAX_ID_LENGTH + 4,
+		0, 32);
+
+	return ERROR_OK;
+}
+
+int target_rtt_find_control_block(struct target *target,
+		target_addr_t *address, size_t size, const char *id, bool *found,
+		void *user_data)
+{
+	uint8_t buf[1024];
+
+	*found = false;
+
+	size_t j = 0;
+	size_t cb_offset = 0;
+	const size_t id_length = strlen(id);
+
+	LOG_INFO("rtt: Searching for control block '%s'", id);
+
+	for (target_addr_t addr = 0; addr < size; addr = addr + sizeof(buf)) {
+		int ret;
+
+		const size_t buf_size = MIN(sizeof(buf), size - addr);
+		ret = target_read_buffer(target, *address + addr, buf_size, buf);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		size_t start = 0;
+		size_t i = 0;
+
+		while (i < buf_size) {
+			if (buf[i] != id[j]) {
+				start++;
+				cb_offset++;
+				i = start;
+				j = 0;
+
+				continue;
+			}
+
+			i++;
+			j++;
+
+			if (j == id_length) {
+				*address = *address + cb_offset;
+				*found = true;
+				return ERROR_OK;
+			}
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int target_rtt_read_channel_info(struct target *target,
+		const struct rtt_control *ctrl, unsigned int channel_index,
+		enum rtt_channel_type type, struct rtt_channel_info *info,
+		void *user_data)
+{
+	int ret;
+	struct rtt_channel channel;
+
+	ret = read_rtt_channel(target, ctrl, channel_index, type, &channel);
+
+	if (ret != ERROR_OK) {
+		LOG_ERROR("rtt: Failed to read channel %u description",
+			channel_index);
+		return ret;
+	}
+
+	ret = read_channel_name(target, channel.name_addr, info->name,
+		info->name_length);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	info->size = channel.size;
+	info->flags = channel.flags;
+
+	return ERROR_OK;
+}
+
+static int read_from_channel(struct target *target,
+		const struct rtt_channel *channel, uint8_t *buffer,
+		size_t *length)
+{
+	int ret;
+	uint32_t len;
+
+	if (!*length)
+		return ERROR_OK;
+
+	if (channel->read_pos == channel->write_pos) {
+		len = 0;
+	} else if (channel->read_pos < channel->write_pos) {
+		len = MIN(*length, channel->write_pos - channel->read_pos);
+
+		ret = target_read_buffer(target,
+			channel->buffer_addr + channel->read_pos, len, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+	} else {
+		uint32_t first_length;
+
+		len = MIN(*length,
+			channel->size - channel->read_pos + channel->write_pos);
+		first_length = MIN(len, channel->size - channel->read_pos);
+
+		ret = target_read_buffer(target,
+			channel->buffer_addr + channel->read_pos, first_length, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		ret = target_read_buffer(target, channel->buffer_addr,
+			len - first_length, buffer + first_length);
+
+		if (ret != ERROR_OK)
+			return ret;
+	}
+
+	if (len > 0) {
+		ret = target_write_u32(target, channel->address + 16,
+			(channel->read_pos + len) % channel->size);
+
+		if (ret != ERROR_OK)
+			return ret;
+	}
+
+	*length = len;
+
+	return ERROR_OK;
+}
+
+int target_rtt_read_callback(struct target *target,
+		const struct rtt_control *ctrl, struct rtt_sink_list **sinks,
+		size_t num_channels, void *user_data)
+{
+	num_channels = MIN(num_channels, ctrl->num_up_channels);
+
+	for (size_t i = 0; i < num_channels; i++) {
+		int ret;
+		struct rtt_channel channel;
+		uint8_t buffer[1024];
+		size_t length;
+
+		if (!sinks[i])
+			continue;
+
+		ret = read_rtt_channel(target, ctrl, i, RTT_CHANNEL_TYPE_UP,
+			&channel);
+
+		if (ret != ERROR_OK) {
+			LOG_ERROR("rtt: Failed to read up-channel %zu description", i);
+			return ret;
+		}
+
+		if (!channel_is_active(&channel)) {
+			LOG_WARNING("rtt: Up-channel %zu is not active", i);
+			continue;
+		}
+
+		if (channel.size < RTT_CHANNEL_BUFFER_MIN_SIZE) {
+			LOG_WARNING("rtt: Up-channel %zu is not large enough", i);
+			continue;
+		}
+
+		length = sizeof(buffer);
+		ret = read_from_channel(target, &channel, buffer, &length);
+
+		if (ret != ERROR_OK) {
+			LOG_ERROR("rtt: Failed to read from up-channel %zu", i);
+			return ret;
+		}
+
+		for (struct rtt_sink_list *sink = sinks[i]; sink; sink = sink->next)
+			sink->read(i, buffer, length, sink->user_data);
+	}
+
+	return ERROR_OK;
+}
diff --git a/src/target/rtt.h b/src/target/rtt.h
new file mode 100644
index 0000000..0122475
--- /dev/null
+++ b/src/target/rtt.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2016-2020 by Marc Schink <dev@zapb.de>
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef OPENOCD_TARGET_RTT_H
+#define OPENOCD_TARGET_RTT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <target/target.h>
+#include <rtt/rtt.h>
+
+int target_rtt_start(struct target *target, const struct rtt_control *ctrl,
+		void *user_data);
+int target_rtt_stop(struct target *target, void *user_data);
+int target_rtt_find_control_block(struct target *target,
+		target_addr_t *address, size_t size, const char *id, bool *found,
+		void *user_data);
+int target_rtt_read_control_block(struct target *target,
+		target_addr_t address, struct rtt_control *ctrl, void *user_data);
+int target_rtt_write_callback(struct target *target,
+		struct rtt_control *ctrl, unsigned int channel_index,
+		const uint8_t *buffer, size_t *length, void *user_data);
+int target_rtt_read_callback(struct target *target,
+		const struct rtt_control *ctrl, struct rtt_sink_list **sinks,
+		size_t length, void *user_data);
+int target_rtt_read_channel_info(struct target *target,
+		const struct rtt_control *ctrl, unsigned int channel_index,
+		enum rtt_channel_type type, struct rtt_channel_info *info,
+		void *user_data);
+
+#endif /* OPENOCD_TARGET_RTT_H */
diff --git a/src/target/stm8.c b/src/target/stm8.c
index 78bf6a2..e99b3c2 100644
--- a/src/target/stm8.c
+++ b/src/target/stm8.c
@@ -1945,7 +1945,7 @@ static int stm8_run_algorithm(struct target *target, int num_mem_params,
 	return ERROR_OK;
 }
 
-int stm8_jim_configure(struct target *target, Jim_GetOptInfo *goi)
+static int stm8_jim_configure(struct target *target, Jim_GetOptInfo *goi)
 {
 	struct stm8_common *stm8 = target_to_stm8(target);
 	jim_wide w;
diff --git a/src/target/target.c b/src/target/target.c
index 9e27f33..4ea6aca 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -72,8 +72,6 @@ static int target_get_gdb_fileio_info_default(struct target *target,
 		struct gdb_fileio_info *fileio_info);
 static int target_gdb_fileio_end_default(struct target *target, int retcode,
 		int fileio_errno, bool ctrl_c);
-static int target_profiling_default(struct target *target, uint32_t *samples,
-		uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds);
 
 /* targets */
 extern struct target_type arm7tdmi_target;
@@ -156,8 +154,8 @@ static struct target_type *target_types[] = {
 struct target *all_targets;
 static struct target_event_callback *target_event_callbacks;
 static struct target_timer_callback *target_timer_callbacks;
-LIST_HEAD(target_reset_callback_list);
-LIST_HEAD(target_trace_callback_list);
+static LIST_HEAD(target_reset_callback_list);
+static LIST_HEAD(target_trace_callback_list);
 static const int polling_interval = TARGET_DEFAULT_POLLING_INTERVAL;
 
 static const Jim_Nvp nvp_assert[] = {
@@ -768,9 +766,11 @@ int target_examine(void)
 		if (target->defer_examine)
 			continue;
 
-		retval = target_examine_one(target);
-		if (retval != ERROR_OK)
-			return retval;
+		int retval2 = target_examine_one(target);
+		if (retval2 != ERROR_OK) {
+			LOG_WARNING("target %s examination failed", target_name(target));
+			retval = retval2;
+		}
 	}
 	return retval;
 }
@@ -1031,11 +1031,11 @@ int target_run_flash_async_algorithm(struct target *target,
 			 * programming. The exact delay shouldn't matter as long as it's
 			 * less than buffer size / flash speed. This is very unlikely to
 			 * run when using high latency connections such as USB. */
-			alive_sleep(10);
+			alive_sleep(2);
 
 			/* to stop an infinite loop on some targets check and increment a timeout
 			 * this issue was observed on a stellaris using the new ICDI interface */
-			if (timeout++ >= 500) {
+			if (timeout++ >= 2500) {
 				LOG_ERROR("timeout waiting for algorithm, a target reset is recommended");
 				return ERROR_FLASH_OPERATION_FAILED;
 			}
@@ -1049,6 +1049,10 @@ int target_run_flash_async_algorithm(struct target *target,
 		if (thisrun_bytes > count * block_size)
 			thisrun_bytes = count * block_size;
 
+		/* Force end of large blocks to be word aligned */
+		if (thisrun_bytes >= 16)
+			thisrun_bytes -= (rp + thisrun_bytes) & 0x03;
+
 		/* Write data to fifo */
 		retval = target_write_buffer(target, wp, thisrun_bytes, buffer);
 		if (retval != ERROR_OK)
@@ -1098,6 +1102,156 @@ int target_run_flash_async_algorithm(struct target *target,
 	return retval;
 }
 
+int target_run_read_async_algorithm(struct target *target,
+		uint8_t *buffer, uint32_t count, int block_size,
+		int num_mem_params, struct mem_param *mem_params,
+		int num_reg_params, struct reg_param *reg_params,
+		uint32_t buffer_start, uint32_t buffer_size,
+		uint32_t entry_point, uint32_t exit_point, void *arch_info)
+{
+	int retval;
+	int timeout = 0;
+
+	const uint8_t *buffer_orig = buffer;
+
+	/* Set up working area. First word is write pointer, second word is read pointer,
+	 * rest is fifo data area. */
+	uint32_t wp_addr = buffer_start;
+	uint32_t rp_addr = buffer_start + 4;
+	uint32_t fifo_start_addr = buffer_start + 8;
+	uint32_t fifo_end_addr = buffer_start + buffer_size;
+
+	uint32_t wp = fifo_start_addr;
+	uint32_t rp = fifo_start_addr;
+
+	/* validate block_size is 2^n */
+	assert(!block_size || !(block_size & (block_size - 1)));
+
+	retval = target_write_u32(target, wp_addr, wp);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, rp_addr, rp);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Start up algorithm on target */
+	retval = target_start_algorithm(target, num_mem_params, mem_params,
+			num_reg_params, reg_params,
+			entry_point,
+			exit_point,
+			arch_info);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("error starting target flash read algorithm");
+		return retval;
+	}
+
+	while (count > 0) {
+		retval = target_read_u32(target, wp_addr, &wp);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("failed to get write pointer");
+			break;
+		}
+
+		LOG_DEBUG("offs 0x%zx count 0x%" PRIx32 " wp 0x%" PRIx32 " rp 0x%" PRIx32,
+			(size_t)(buffer - buffer_orig), count, wp, rp);
+
+		if (wp == 0) {
+			LOG_ERROR("flash read algorithm aborted by target");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (((wp - fifo_start_addr) & (block_size - 1)) || wp < fifo_start_addr || wp >= fifo_end_addr) {
+			LOG_ERROR("corrupted fifo write pointer 0x%" PRIx32, wp);
+			break;
+		}
+
+		/* Count the number of bytes available in the fifo without
+		 * crossing the wrap around. */
+		uint32_t thisrun_bytes;
+		if (wp >= rp)
+			thisrun_bytes = wp - rp;
+		else
+			thisrun_bytes = fifo_end_addr - rp;
+
+		if (thisrun_bytes == 0) {
+			/* Throttle polling a bit if transfer is (much) faster than flash
+			 * reading. The exact delay shouldn't matter as long as it's
+			 * less than buffer size / flash speed. This is very unlikely to
+			 * run when using high latency connections such as USB. */
+			alive_sleep(2);
+
+			/* to stop an infinite loop on some targets check and increment a timeout
+			 * this issue was observed on a stellaris using the new ICDI interface */
+			if (timeout++ >= 2500) {
+				LOG_ERROR("timeout waiting for algorithm, a target reset is recommended");
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			continue;
+		}
+
+		/* Reset our timeout */
+		timeout = 0;
+
+		/* Limit to the amount of data we actually want to read */
+		if (thisrun_bytes > count * block_size)
+			thisrun_bytes = count * block_size;
+
+		/* Force end of large blocks to be word aligned */
+		if (thisrun_bytes >= 16)
+			thisrun_bytes -= (rp + thisrun_bytes) & 0x03;
+
+		/* Read data from fifo */
+		retval = target_read_buffer(target, rp, thisrun_bytes, buffer);
+		if (retval != ERROR_OK)
+			break;
+
+		/* Update counters and wrap write pointer */
+		buffer += thisrun_bytes;
+		count -= thisrun_bytes / block_size;
+		rp += thisrun_bytes;
+		if (rp >= fifo_end_addr)
+			rp = fifo_start_addr;
+
+		/* Store updated write pointer to target */
+		retval = target_write_u32(target, rp_addr, rp);
+		if (retval != ERROR_OK)
+			break;
+
+		/* Avoid GDB timeouts */
+		keep_alive();
+
+	}
+
+	if (retval != ERROR_OK) {
+		/* abort flash write algorithm on target */
+		target_write_u32(target, rp_addr, 0);
+	}
+
+	int retval2 = target_wait_algorithm(target, num_mem_params, mem_params,
+			num_reg_params, reg_params,
+			exit_point,
+			10000,
+			arch_info);
+
+	if (retval2 != ERROR_OK) {
+		LOG_ERROR("error waiting for target flash write algorithm");
+		retval = retval2;
+	}
+
+	if (retval == ERROR_OK) {
+		/* check if algorithm set wp = 0 after fifo writer loop finished */
+		retval = target_read_u32(target, wp_addr, &wp);
+		if (retval == ERROR_OK && wp == 0) {
+			LOG_ERROR("flash read algorithm aborted by target");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+	return retval;
+}
+
 int target_read_memory(struct target *target,
 		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
@@ -1265,10 +1419,10 @@ int target_get_gdb_reg_list_noread(struct target *target,
 bool target_supports_gdb_connection(struct target *target)
 {
 	/*
-	 * based on current code, we can simply exclude all the targets that
-	 * don't provide get_gdb_reg_list; this could change with new targets.
+	 * exclude all the targets that don't provide get_gdb_reg_list
+	 * or that have explicit gdb_max_connection == 0
 	 */
-	return !!target->type->get_gdb_reg_list;
+	return !!target->type->get_gdb_reg_list && !!target->gdb_max_connections;
 }
 
 int target_step(struct target *target,
@@ -1328,13 +1482,9 @@ unsigned target_data_bits(struct target *target)
 	return 32;
 }
 
-int target_profiling(struct target *target, uint32_t *samples,
+static int target_profiling(struct target *target, uint32_t *samples,
 			uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
 {
-	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (profiling)", target->cmd_name);
-		return ERROR_TARGET_NOT_HALTED;
-	}
 	return target->type->profiling(target, samples, max_num_samples,
 			num_samples, seconds);
 }
@@ -2154,7 +2304,7 @@ static int target_gdb_fileio_end_default(struct target *target,
 	return ERROR_OK;
 }
 
-static int target_profiling_default(struct target *target, uint32_t *samples,
+int target_profiling_default(struct target *target, uint32_t *samples,
 		uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
 {
 	struct timeval timeout, now;
@@ -2905,7 +3055,7 @@ COMMAND_HANDLER(handle_reg_command)
 			for (i = 0, reg = cache->reg_list;
 					i < cache->num_regs;
 					i++, reg++, count++) {
-				if (reg->exist == false)
+				if (reg->exist == false || reg->hidden)
 					continue;
 				/* only print cached values if they are valid */
 				if (reg->exist) {
@@ -3441,11 +3591,11 @@ static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
 		target_addr_t addr;
 		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image->base_address = addr;
-		image->base_address_set = 1;
+		image->base_address_set = true;
 	} else
-		image->base_address_set = 0;
+		image->base_address_set = false;
 
-	image->start_address_set = 0;
+	image->start_address_set = false;
 
 	if (CMD_ARGC >= 4)
 		COMMAND_PARSE_ADDRESS(CMD_ARGV[3], *min_address);
@@ -3468,7 +3618,6 @@ COMMAND_HANDLER(handle_load_image_command)
 	uint32_t image_size;
 	target_addr_t min_address = 0;
 	target_addr_t max_address = -1;
-	int i;
 	struct image image;
 
 	int retval = CALL_COMMAND_HANDLER(parse_load_image_command_CMD_ARGV,
@@ -3486,7 +3635,7 @@ COMMAND_HANDLER(handle_load_image_command)
 
 	image_size = 0x0;
 	retval = ERROR_OK;
-	for (i = 0; i < image.num_sections; i++) {
+	for (unsigned int i = 0; i < image.num_sections; i++) {
 		buffer = malloc(image.sections[i].size);
 		if (buffer == NULL) {
 			command_print(CMD,
@@ -3619,7 +3768,6 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 	uint8_t *buffer;
 	size_t buf_cnt;
 	uint32_t image_size;
-	int i;
 	int retval;
 	uint32_t checksum = 0;
 	uint32_t mem_checksum = 0;
@@ -3643,13 +3791,13 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 		target_addr_t addr;
 		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image.base_address = addr;
-		image.base_address_set = 1;
+		image.base_address_set = true;
 	} else {
-		image.base_address_set = 0;
+		image.base_address_set = false;
 		image.base_address = 0x0;
 	}
 
-	image.start_address_set = 0;
+	image.start_address_set = false;
 
 	retval = image_open(&image, CMD_ARGV[0], (CMD_ARGC == 3) ? CMD_ARGV[2] : NULL);
 	if (retval != ERROR_OK)
@@ -3658,12 +3806,12 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 	image_size = 0x0;
 	int diffs = 0;
 	retval = ERROR_OK;
-	for (i = 0; i < image.num_sections; i++) {
+	for (unsigned int i = 0; i < image.num_sections; i++) {
 		buffer = malloc(image.sections[i].size);
 		if (buffer == NULL) {
 			command_print(CMD,
-					"error allocating buffer for section (%d bytes)",
-					(int)(image.sections[i].size));
+					"error allocating buffer for section (%" PRIu32 " bytes)",
+					image.sections[i].size);
 			break;
 		}
 		retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt);
@@ -4134,6 +4282,7 @@ COMMAND_HANDLER(handle_profile_command)
 	uint32_t offset;
 	uint32_t num_of_samples;
 	int retval = ERROR_OK;
+	bool halted_before_profiling = target->state == TARGET_HALTED;
 
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], offset);
 
@@ -4164,12 +4313,23 @@ COMMAND_HANDLER(handle_profile_command)
 		free(samples);
 		return retval;
 	}
-	if (target->state == TARGET_RUNNING) {
+
+	if (target->state == TARGET_RUNNING && halted_before_profiling) {
+		/* The target was halted before we started and is running now. Halt it,
+		 * for consistency. */
 		retval = target_halt(target);
 		if (retval != ERROR_OK) {
 			free(samples);
 			return retval;
 		}
+	} else if (target->state == TARGET_HALTED && !halted_before_profiling) {
+		/* The target was running before we started and is halted now. Resume
+		 * it, for consistency. */
+		retval = target_resume(target, 1, 0, 0, 0);
+		if (retval != ERROR_OK) {
+			free(samples);
+			return retval;
+		}
 	}
 
 	retval = target_poll(target);
@@ -4678,6 +4838,7 @@ enum target_cfg_param {
 	TCFG_RTOS,
 	TCFG_DEFER_EXAMINE,
 	TCFG_GDB_PORT,
+	TCFG_GDB_MAX_CONNECTIONS,
 };
 
 static Jim_Nvp nvp_config_opts[] = {
@@ -4694,6 +4855,7 @@ static Jim_Nvp nvp_config_opts[] = {
 	{ .name = "-rtos",             .value = TCFG_RTOS },
 	{ .name = "-defer-examine",    .value = TCFG_DEFER_EXAMINE },
 	{ .name = "-gdb-port",         .value = TCFG_GDB_PORT },
+	{ .name = "-gdb-max-connections",   .value = TCFG_GDB_MAX_CONNECTIONS },
 	{ .name = NULL, .value = -1 }
 };
 
@@ -5001,6 +5163,25 @@ no_params:
 			Jim_SetResultString(goi->interp, target->gdb_port_override ? : "undefined", -1);
 			/* loop for more */
 			break;
+
+		case TCFG_GDB_MAX_CONNECTIONS:
+			if (goi->isconfigure) {
+				struct command_context *cmd_ctx = current_command_context(goi->interp);
+				if (cmd_ctx->mode != COMMAND_CONFIG) {
+					Jim_SetResultString(goi->interp, "-gdb-max-conenctions must be configured before 'init'", -1);
+					return JIM_ERR;
+				}
+
+				e = Jim_GetOpt_Wide(goi, &w);
+				if (e != JIM_OK)
+					return e;
+				target->gdb_max_connections = (w < 0) ? CONNECTION_LIMIT_UNLIMITED : (int)w;
+			} else {
+				if (goi->argc != 0)
+					goto no_params;
+			}
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->gdb_max_connections));
+			break;
 		}
 	} /* while (goi->argc) */
 
@@ -5581,6 +5762,7 @@ static int target_create(Jim_GetOptInfo *goi)
 	target->rtos_auto_detect = false;
 
 	target->gdb_port_override = NULL;
+	target->gdb_max_connections = 1;
 
 	/* Do the rest as "configure" options */
 	goi->isconfigure = 1;
@@ -5703,7 +5885,9 @@ static int jim_target_current(Jim_Interp *interp, int argc, Jim_Obj *const *argv
 	struct command_context *cmd_ctx = current_command_context(interp);
 	assert(cmd_ctx != NULL);
 
-	Jim_SetResultString(interp, target_name(get_current_target(cmd_ctx)), -1);
+	struct target *target = get_current_target_or_null(cmd_ctx);
+	if (target)
+		Jim_SetResultString(interp, target_name(target), -1);
 	return JIM_OK;
 }
 
@@ -5875,7 +6059,6 @@ COMMAND_HANDLER(handle_fast_load_image_command)
 	uint32_t image_size;
 	target_addr_t min_address = 0;
 	target_addr_t max_address = -1;
-	int i;
 
 	struct image image;
 
@@ -5901,7 +6084,7 @@ COMMAND_HANDLER(handle_fast_load_image_command)
 		return ERROR_FAIL;
 	}
 	memset(fastload, 0, sizeof(struct FastLoad)*image.num_sections);
-	for (i = 0; i < image.num_sections; i++) {
+	for (unsigned int i = 0; i < image.num_sections; i++) {
 		buffer = malloc(image.sections[i].size);
 		if (buffer == NULL) {
 			command_print(CMD, "error allocating buffer for section (%d bytes)",
diff --git a/src/target/target.h b/src/target/target.h
index 4e8897c..f6044c5 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -47,7 +47,7 @@ struct gdb_fileio_info;
 
 /*
  * TARGET_UNKNOWN = 0: we don't know anything about the target yet
- * TARGET_RUNNING = 1: the target is executing user code
+ * TARGET_RUNNING = 1: the target is executing or ready to execute user code
  * TARGET_HALTED  = 2: the target is not executing code, and ready to talk to the
  * debugger. on an xscale it means that the debug handler is executing
  * TARGET_RESET   = 3: the target is being held in reset (only a temporary state,
@@ -211,6 +211,8 @@ struct target {
 
 	char *gdb_port_override;			/* target-specific override for gdb_port */
 
+	int gdb_max_connections;			/* max number of simultaneous gdb connections */
+
 	/* The semihosting information, extracted from the target. */
 	struct semihosting *semihosting;
 };
@@ -576,6 +578,18 @@ int target_run_flash_async_algorithm(struct target *target,
 		void *arch_info);
 
 /**
+ * This routine is a wrapper for asynchronous algorithms.
+ *
+ */
+int target_run_read_async_algorithm(struct target *target,
+		uint8_t *buffer, uint32_t count, int block_size,
+		int num_mem_params, struct mem_param *mem_params,
+		int num_reg_params, struct reg_param *reg_params,
+		uint32_t buffer_start, uint32_t buffer_size,
+		uint32_t entry_point, uint32_t exit_point,
+		void *arch_info);
+
+/**
  * Read @a count items of @a size bytes from the memory of @a target at
  * the @a address given.
  *
@@ -755,6 +769,9 @@ void target_handle_md_output(struct command_invocation *cmd,
 	struct target *target, target_addr_t address, unsigned size,
 	unsigned count, const uint8_t *buffer, bool include_address);
 
+int target_profiling_default(struct target *target, uint32_t *samples, uint32_t
+		max_num_samples, uint32_t *num_samples, uint32_t seconds);
+
 #define ERROR_TARGET_INVALID	(-300)
 #define ERROR_TARGET_INIT_FAILED (-301)
 #define ERROR_TARGET_TIMEOUT	(-302)
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 6d1d426..b25999d 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -2582,7 +2582,6 @@ static int xscale_read_instruction(struct target *target, uint32_t pc,
 	struct arm_instruction *instruction)
 {
 	struct xscale_common *const xscale = target_to_xscale(target);
-	int i;
 	int section = -1;
 	size_t size_read;
 	uint32_t opcode;
@@ -2592,7 +2591,7 @@ static int xscale_read_instruction(struct target *target, uint32_t pc,
 		return ERROR_TRACE_IMAGE_UNAVAILABLE;
 
 	/* search for the section the current instruction belongs to */
-	for (i = 0; i < xscale->trace.image->num_sections; i++) {
+	for (unsigned int i = 0; i < xscale->trace.image->num_sections; i++) {
 		if ((xscale->trace.image->sections[i].base_address <= pc) &&
 			(xscale->trace.image->sections[i].base_address +
 			xscale->trace.image->sections[i].size > pc)) {
@@ -2883,7 +2882,7 @@ static void xscale_build_reg_cache(struct target *target)
 	/* fill in values for the xscale reg cache */
 	(*cache_p)->name = "XScale registers";
 	(*cache_p)->next = NULL;
-	(*cache_p)->reg_list = malloc(num_regs * sizeof(struct reg));
+	(*cache_p)->reg_list = calloc(num_regs, sizeof(struct reg));
 	(*cache_p)->num_regs = num_regs;
 
 	for (i = 0; i < num_regs; i++) {
@@ -3428,15 +3427,15 @@ COMMAND_HANDLER(xscale_handle_trace_image_command)
 	}
 
 	xscale->trace.image = malloc(sizeof(struct image));
-	xscale->trace.image->base_address_set = 0;
-	xscale->trace.image->start_address_set = 0;
+	xscale->trace.image->base_address_set = false;
+	xscale->trace.image->start_address_set = false;
 
 	/* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
 	if (CMD_ARGC >= 2) {
-		xscale->trace.image->base_address_set = 1;
+		xscale->trace.image->base_address_set = true;
 		COMMAND_PARSE_NUMBER(llong, CMD_ARGV[1], xscale->trace.image->base_address);
 	} else
-		xscale->trace.image->base_address_set = 0;
+		xscale->trace.image->base_address_set = false;
 
 	if (image_open(xscale->trace.image, CMD_ARGV[0],
 		(CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL) != ERROR_OK) {