diff --git a/scripts/stm32_mem.py b/scripts/stm32_mem.py
index a9695d39..1c8495ae 100755
--- a/scripts/stm32_mem.py
+++ b/scripts/stm32_mem.py
@@ -174,7 +174,10 @@ if __name__ == "__main__":
 		print("Failed to read device state! Assuming APP_IDLE")
 		state = dfu.STATE_APP_IDLE
 	if state == dfu.STATE_APP_IDLE:
-		dfudev.detach()
+		try:
+			dfudev.detach()
+		except:
+			pass
 		dfudev.release()
 		print("Invoking DFU Device")
 		timeout = 0
diff --git a/src/Makefile b/src/Makefile
index 696c7ce5..dd245704 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -8,7 +8,7 @@ MAKEFLAGS += --no-print-dir
 Q := @
 endif
 
-CFLAGS += -Wall -Wextra -Werror -Wno-char-subscripts -Wno-cast-function-type \
+CFLAGS += -Wall -Wextra -Werror -Wno-char-subscripts \
 	-std=gnu99 -g3 -MD \
 	-I. -Iinclude -Iplatforms/common -I$(PLATFORM_DIR)
 
diff --git a/src/command.c b/src/command.c
index 7eccaca8..15860636 100644
--- a/src/command.c
+++ b/src/command.c
@@ -43,17 +43,17 @@ struct command_s {
 	const char *help;
 };
 
-static bool cmd_version(void);
-static bool cmd_serial(void);
-static bool cmd_help(target *t);
+static bool cmd_version(target *t, int argc, char **argv);
+static bool cmd_serial(target *t, int argc, char **argv);
+static bool cmd_help(target *t, int argc, char **argv);
 
 static bool cmd_jtag_scan(target *t, int argc, char **argv);
-static bool cmd_swdp_scan(void);
-static bool cmd_targets(void);
-static bool cmd_morse(void);
+static bool cmd_swdp_scan(target *t, int argc, char **argv);
+static bool cmd_targets(target *t, int argc, char **argv);
+static bool cmd_morse(target *t, int argc, char **argv);
 static bool cmd_halt_timeout(target *t, int argc, const char **argv);
 static bool cmd_connect_srst(target *t, int argc, const char **argv);
-static bool cmd_hard_srst(void);
+static bool cmd_hard_srst(target *t, int argc, const char **argv);
 #ifdef PLATFORM_HAS_POWER_SWITCH
 static bool cmd_target_power(target *t, int argc, const char **argv);
 #endif
@@ -103,8 +103,8 @@ const struct command_s cmd_list[] = {
 	{NULL, NULL, NULL}
 };
 
-static bool connect_assert_srst;
-#ifdef PLATFORM_HAS_DEBUG
+bool connect_assert_srst;
+#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED)
 bool debug_bmp;
 #endif
 long cortexm_wait_timeout = 2000; /* Timeout to wait for Cortex to react on halt command. */
@@ -140,8 +140,11 @@ int command_process(target *t, char *cmd)
 	return target_command(t, argc, argv);
 }
 
-bool cmd_version(void)
+bool cmd_version(target *t, int argc, char **argv)
 {
+	(void)t;
+	(void)argc;
+	(void)argv;
 #if defined PC_HOSTED
 	gdb_outf("Black Magic Probe, PC-Hosted for " PLATFORM_IDENT
 			 ", Version " FIRMWARE_VERSION "\n");
@@ -155,8 +158,10 @@ bool cmd_version(void)
 	return true;
 }
 
-bool cmd_help(target *t)
+bool cmd_help(target *t, int argc, char **argv)
 {
+	(void)argc;
+	(void)argv;
 	const struct command_s *c;
 
 	gdb_out("General commands:\n");
@@ -207,13 +212,16 @@ static bool cmd_jtag_scan(target *t, int argc, char **argv)
 		gdb_out("JTAG device scan failed!\n");
 		return false;
 	}
-	cmd_targets();
+	cmd_targets(NULL, 0, NULL);
 	morse(NULL, false);
 	return true;
 }
 
-bool cmd_swdp_scan(void)
+bool cmd_swdp_scan(target *t, int argc, char **argv)
 {
+	(void)t;
+	(void)argc;
+	(void)argv;
 	gdb_outf("Target voltage: %s\n", platform_target_voltage());
 
 	if(connect_assert_srst)
@@ -239,7 +247,7 @@ bool cmd_swdp_scan(void)
 		return false;
 	}
 
-	cmd_targets();
+	cmd_targets(NULL, 0, NULL);
 	morse(NULL, false);
 	return true;
 
@@ -253,8 +261,11 @@ static void display_target(int i, target *t, void *context)
 			 (target_core_name(t)) ? target_core_name(t): "");
 }
 
-bool cmd_targets(void)
+bool cmd_targets(target *t, int argc, char **argv)
 {
+	(void)t;
+	(void)argc;
+	(void)argv;
 	gdb_out("Available Targets:\n");
 	gdb_out("No. Att Driver\n");
 	if (!target_foreach(display_target, NULL)) {
@@ -265,8 +276,11 @@ bool cmd_targets(void)
 	return true;
 }
 
-bool cmd_morse(void)
+bool cmd_morse(target *t, int argc, char **argv)
 {
+	(void)t;
+	(void)argc;
+	(void)argv;
 	if(morse_msg)
 		gdb_outf("%s\n", morse_msg);
 	return true;
@@ -319,8 +333,11 @@ static bool cmd_halt_timeout(target *t, int argc, const char **argv)
 	return true;
 }
 
-static bool cmd_hard_srst(void)
+static bool cmd_hard_srst(target *t, int argc, const char **argv)
 {
+	(void)t;
+	(void)argc;
+	(void)argv;
 	target_list_free();
 	platform_srst_set_val(true);
 	platform_srst_set_val(false);
@@ -438,8 +455,12 @@ static bool cmd_enter_bootldr(target *t, int argc, const char **argv)
 #endif
 
 #ifdef PLATFORM_HAS_PRINTSERIAL
-bool cmd_serial(void)
+bool cmd_serial(target *t, int argc, char **argv)
 {
+	(void) t;
+	(void) argc;
+	(void) argv;
+
 	print_serial();
 	return true;
 }
diff --git a/src/gdb_main.c b/src/gdb_main.c
index 1e2cb860..af1edf92 100644
--- a/src/gdb_main.c
+++ b/src/gdb_main.c
@@ -207,6 +207,36 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 			}
 			break;
 			}
+
+		/* Optional GDB packet support */
+		case 'p': { /* Read single register */
+			ERROR_IF_NO_TARGET();
+			uint32_t reg;
+			sscanf(pbuf, "p%" SCNx32, &reg);
+			uint8_t val[8];
+			size_t s = target_reg_read(cur_target, reg, val, sizeof(val));
+			if (s > 0) {
+				gdb_putpacket(hexify(pbuf, val, s), s * 2);
+			} else {
+				gdb_putpacketz("EFF");
+			}
+			break;
+			}
+		case 'P': { /* Write single register */
+			ERROR_IF_NO_TARGET();
+			uint32_t reg;
+			int n;
+			sscanf(pbuf, "P%" SCNx32 "=%n", &reg, &n);
+			uint8_t val[strlen(&pbuf[n])/2];
+			unhexify(val, pbuf + n, sizeof(val));
+			if (target_reg_write(cur_target, reg, val, sizeof(val)) > 0) {
+				gdb_putpacketz("OK");
+			} else {
+				gdb_putpacketz("EFF");
+			}
+			break;
+			}
+
 		case 'F':	/* Semihosting call finished */
 			if (in_syscall) {
 				return hostio_reply(tc, pbuf, size);
@@ -216,7 +246,6 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 			}
 			break;
 
-		/* Optional GDB packet support */
 		case '!':	/* Enable Extended GDB Protocol. */
 			/* This doesn't do anything, we support the extended
 			 * protocol anyway, but GDB will never send us a 'R'
diff --git a/src/include/target.h b/src/include/target.h
index ee3e5fb3..d052ab15 100644
--- a/src/include/target.h
+++ b/src/include/target.h
@@ -62,6 +62,8 @@ size_t target_regs_size(target *t);
 const char *target_tdesc(target *t);
 void target_regs_read(target *t, void *data);
 void target_regs_write(target *t, const void *data);
+ssize_t target_reg_read(target *t, int reg, void *data, size_t max);
+ssize_t target_reg_write(target *t, int reg, const void *data, size_t size);
 
 /* Halt/resume functions */
 enum target_halt_reason {
diff --git a/src/platforms/f4discovery/Makefile.inc b/src/platforms/f4discovery/Makefile.inc
index b04cf415..b36bf8f2 100644
--- a/src/platforms/f4discovery/Makefile.inc
+++ b/src/platforms/f4discovery/Makefile.inc
@@ -7,7 +7,7 @@ CFLAGS += -Istm32/include -mcpu=cortex-m4 -mthumb \
 	-DSTM32F4 -DF4DISCOVERY -I../libopencm3/include \
 	-Iplatforms/stm32
 
-LDFLAGS = -lopencm3_stm32f4 -Wl,--defsym,_stack=0x20010000 \
+LDFLAGS = -lopencm3_stm32f4 \
 	-Wl,-T,platforms/stm32/f4discovery.ld -nostartfiles -lc -lnosys \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m4 -Wl,-gc-sections \
 	-mfloat-abi=hard -mfpu=fpv4-sp-d16 \
diff --git a/src/platforms/hydrabus/Makefile.inc b/src/platforms/hydrabus/Makefile.inc
index 36973e0b..1c2bf7d1 100644
--- a/src/platforms/hydrabus/Makefile.inc
+++ b/src/platforms/hydrabus/Makefile.inc
@@ -7,7 +7,7 @@ CFLAGS += -Istm32/include -mcpu=cortex-m4 -mthumb \
 	-DSTM32F4 -DHYDRABUS -I../libopencm3/include \
 	-Iplatforms/stm32
 
-LDFLAGS = -lopencm3_stm32f4 -Wl,--defsym,_stack=0x20006000 \
+LDFLAGS = -lopencm3_stm32f4 \
 	-Wl,-T,platforms/stm32/f4discovery.ld -nostartfiles -lc -lnosys \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m4 -Wl,-gc-sections \
 	-mfloat-abi=hard -mfpu=fpv4-sp-d16 \
diff --git a/src/platforms/libftdi/Makefile.inc b/src/platforms/libftdi/Makefile.inc
index 0ddabe89..b890e860 100644
--- a/src/platforms/libftdi/Makefile.inc
+++ b/src/platforms/libftdi/Makefile.inc
@@ -3,7 +3,6 @@ CFLAGS += -DPC_HOSTED -DNO_LIBOPENCM3 -DENABLE_DEBUG
 LDFLAGS += -lftdi1
 ifneq (, $(findstring mingw, $(SYS)))
 LDFLAGS +=  -lusb-1.0 -lws2_32
-CFLAGS += -Wno-cast-function-type
 else ifneq (, $(findstring cygwin, $(SYS)))
 LDFLAGS +=  -lusb-1.0 -lws2_32
 endif
diff --git a/src/platforms/native/Makefile.inc b/src/platforms/native/Makefile.inc
index d8d552ba..1055570d 100644
--- a/src/platforms/native/Makefile.inc
+++ b/src/platforms/native/Makefile.inc
@@ -6,7 +6,7 @@ CFLAGS += -Istm32/include -mcpu=cortex-m3 -mthumb \
 	-DSTM32F1 -DBLACKMAGIC -I../libopencm3/include \
 	-Iplatforms/stm32
 
-LDFLAGS_BOOT := $(LDFLAGS) -lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
+LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs -lopencm3_stm32f1 \
 	-Wl,-T,platforms/stm32/blackmagic.ld -nostartfiles -lc \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
diff --git a/src/platforms/pc-stlinkv2/Makefile.inc b/src/platforms/pc-stlinkv2/Makefile.inc
index 8762885d..25fde894 100644
--- a/src/platforms/pc-stlinkv2/Makefile.inc
+++ b/src/platforms/pc-stlinkv2/Makefile.inc
@@ -5,7 +5,6 @@ CFLAGS +=-I ./target
 LDFLAGS += -lusb-1.0
 ifneq (, $(findstring mingw, $(SYS)))
 LDFLAGS += -lws2_32
-CFLAGS += -Wno-cast-function-type
 else ifneq (, $(findstring cygwin, $(SYS)))
 LDFLAGS += -lws2_32
 endif
diff --git a/src/platforms/pc-stlinkv2/README.md b/src/platforms/pc-stlinkv2/README.md
index 2d145ad0..ad2f78d8 100644
--- a/src/platforms/pc-stlinkv2/README.md
+++ b/src/platforms/pc-stlinkv2/README.md
@@ -5,11 +5,13 @@ functionality that BMP needs. This branch implements blackmagic debug probe
 for the STM Stlink as a proof of concept.
 Use at your own risk, but report or better fix problems.
 
-Run the resulting blackmagic_stlinkv2 executabel to start the gdb server
+Run the resulting blackmagic_stlinkv2 executable to start the gdb server
 
-CrosscCompling for windows with mingw succeeds.
+Crosscompling for windows with mingw succeeds.
 
-Drawback: JTAG does not work for chains with multiple devices.
+Drawback:
+- JTAG does not work for chains with multiple devices.
+- STLinkV3 does only work on STM32 devices.
 
 This branch may get forced push. In case of problems:
 - git reset --hard master
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
index 86c7a1ec..0e1723bd 100644
--- a/src/platforms/pc-stlinkv2/stlinkv2.c
+++ b/src/platforms/pc-stlinkv2/stlinkv2.c
@@ -84,6 +84,7 @@
 #define STLINK_SWD_AP_STICKY_ERROR     0x19
 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
 #define STLINK_BAD_AP_ERROR            0x1d
+#define STLINK_TOO_MANY_AP_ERROR       0x29
 #define STLINK_JTAG_UNKNOWN_CMD        0x42
 
 #define STLINK_CORE_RUNNING            0x80
@@ -502,6 +503,11 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 		case STLINK_BAD_AP_ERROR:
 			/* ADIV5 probe 256 APs, most of them are non exisitant.*/
 			return STLINK_ERROR_FAIL;
+		case STLINK_TOO_MANY_AP_ERROR:
+			/* TI TM4C duplicates AP. Error happens at AP9.*/
+			if (verbose)
+				DEBUG("STLINK_TOO_MANY_AP_ERROR\n");
+			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_UNKNOWN_CMD :
 			if (verbose)
 				DEBUG("STLINK_JTAG_UNKNOWN_CMD\n");
@@ -529,7 +535,7 @@ static int send_recv_retry(uint8_t *txbuf, size_t txsize,
 		gettimeofday(&now, NULL);
 		timersub(&now, &start, &diff);
 		if ((diff.tv_sec >= 1) || (res != STLINK_ERROR_WAIT)) {
-			DEBUG("write_retry failed");
+			DEBUG("write_retry failed. ");
 			return res;
 		}
 	}
@@ -552,7 +558,7 @@ static int read_retry(uint8_t *txbuf, size_t txsize,
 		gettimeofday(&now, NULL);
 		timersub(&now, &start, &diff);
 		if ((diff.tv_sec >= 1) || (res != STLINK_ERROR_WAIT)) {
-			DEBUG("read_retry failed");
+			DEBUG("read_retry failed. ");
 			return res;
 		}
 	}
@@ -1166,7 +1172,14 @@ bool adiv5_ap_setup(int ap)
 	uint8_t data[2];
 	send_recv_retry(cmd, 16, data, 2);
 	DEBUG_STLINK("Open AP %d\n", ap);
-	return (stlink_usb_error_check(data, true))? false: true;
+	int res = stlink_usb_error_check(data, true);
+	if (res) {
+		if (Stlink.ver_hw == 30) {
+			DEBUG("STLINKV3 only connects to STM8/32!\n");
+		}
+		return false;
+	}
+	return true;
 }
 
 void adiv5_ap_cleanup(int ap)
@@ -1230,6 +1243,15 @@ void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 		for (size_t i = 0; i < len ; i++) {
 			DEBUG_STLINK("%02x", *p++);
 		}
+	} else {
+		/* FIXME: What is the right measure when failing?
+		 *
+		 * E.g. TM4C129 gets here when NRF probe reads 0x10000010
+		 * Approach taken:
+		 * Fill the memory with some fixed pattern so hopefully
+		 * the caller notices the error*/
+		DEBUG("stlink_readmem failed\n");
+		memset(dest, 0xff, len);
 	}
 	DEBUG_STLINK("\n");
 }
diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index 4bde36c6..9dc971a6 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -6,8 +6,7 @@ OPT_FLAGS = -Os
 CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-DSTM32F1 -DDISCOVERY_STLINK -I../libopencm3/include \
 	-I platforms/stm32
-LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs \
-	-lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
+LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs -lopencm3_stm32f1 \
 	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
diff --git a/src/platforms/stlink/stlink_common.c b/src/platforms/stlink/stlink_common.c
index b23a345d..bee7a149 100644
--- a/src/platforms/stlink/stlink_common.c
+++ b/src/platforms/stlink/stlink_common.c
@@ -38,16 +38,20 @@ uint32_t detect_rev(void)
 	rcc_periph_clock_enable(RCC_CRC);
 	/* First, get Board revision by pulling PC13/14 up. Read
 	 *  11 for ST-Link V1, e.g. on VL Discovery, tag as rev 0
+	 *  11 for Baite, PB11 pulled high,          tag as rev 1
 	 *  00 for ST-Link V2, e.g. on F4 Discovery, tag as rev 1
 	 *  01 for ST-Link V2, else,                 tag as rev 1
 	 */
 	gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
 				  GPIO_CNF_INPUT_PULL_UPDOWN, GPIO14 | GPIO13);
 	gpio_set(GPIOC, GPIO14 | GPIO13);
+	gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
+				  GPIO_CNF_INPUT_PULL_UPDOWN, GPIO11);
+	gpio_clear(GPIOB, GPIO11);
 	for (int i = 0; i < 100; i ++)
 		res = gpio_get(GPIOC, GPIO13);
 	if (res)
-		rev = 0;
+		rev = (gpio_get(GPIOB, GPIO11))? 1 : 0;
 	else {
 		/* Check for V2.1 boards.
 		 * PA15/TDI is USE_RENUM, pulled with 10 k to U5V on V2.1,
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index ddcda807..83f1d38a 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -260,9 +260,13 @@ enum {
 int rdi_write(int fn, const char *buf, size_t len)
 {
 	(void)fn;
+#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED)
 	if (debug_bmp)
 		return len - usbuart_debug_write(buf, len);
-
+#else
+	(void)buf;
+	(void)len;
+#endif
 	return 0;
 }
 
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index 19941ab5..9c4394db 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -6,8 +6,7 @@ OPT_FLAGS = -Os
 CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-DSTM32F1 -DDISCOVERY_SWLINK -I../libopencm3/include \
 	-I platforms/stm32
-LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs \
-	-lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
+LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs -lopencm3_stm32f1 \
 	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc\
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 66c45a9d..f56ce0cf 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -262,7 +262,7 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 	uint64_t pidr = 0;
 	uint32_t cidr = 0;
 	bool res = false;
-#if defined(ENABLE_DEBUG)
+#if defined(ENABLE_DEBUG) && defined(PLATFORM_HAS_DEBUG)
 	char indent[recursion];
 
 	for(int i = 0; i < recursion; i++) indent[i] = ' ';
@@ -304,7 +304,7 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 	/* ROM table */
 	if (cid_class == cidc_romtab) {
 		/* Check SYSMEM bit */
-#ifdef ENABLE_DEBUG
+#if defined(ENABLE_DEBUG) && defined(PLATFORM_HAS_DEBUG)
 		uint32_t memtype = adiv5_mem_read32(ap, addr | ADIV5_ROM_MEMTYPE) &
 			ADIV5_ROM_MEMTYPE_SYSMEM;
 
@@ -485,6 +485,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		DEBUG("TARGETID %08" PRIx32 "\n", dp->targetid);
 	}
 	/* Probe for APs on this DP */
+	uint32_t last_base = 0;
 	for(int i = 0; i < 256; i++) {
 		ADIv5_AP_t *ap = NULL;
 		if (adiv5_ap_setup(i))
@@ -496,6 +497,13 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 			else
 				continue;
 		}
+		if (ap->base == last_base) {
+			DEBUG("AP %d: Duplicate base\n", i);
+			adiv5_ap_cleanup(i);
+			/* FIXME: Should we expect valid APs behind duplicate ones? */
+			return;
+		}
+		last_base = ap->base;
 		extern void kinetis_mdm_probe(ADIv5_AP_t *);
 		kinetis_mdm_probe(ap);
 
diff --git a/src/target/cortexa.c b/src/target/cortexa.c
index 2e7e723d..2d1f2724 100644
--- a/src/target/cortexa.c
+++ b/src/target/cortexa.c
@@ -43,6 +43,9 @@ static void cortexa_regs_read(target *t, void *data);
 static void cortexa_regs_write(target *t, const void *data);
 static void cortexa_regs_read_internal(target *t);
 static void cortexa_regs_write_internal(target *t);
+static ssize_t cortexa_reg_read(target *t, int reg, void *data, size_t max);
+static ssize_t cortexa_reg_write(target *t, int reg, const void *data, size_t max);
+
 
 static void cortexa_reset(target *t);
 static enum target_halt_reason cortexa_halt_poll(target *t, target_addr *watch);
@@ -357,6 +360,8 @@ bool cortexa_probe(ADIv5_AP_t *apb, uint32_t debug_base)
 	t->tdesc = tdesc_cortex_a;
 	t->regs_read = cortexa_regs_read;
 	t->regs_write = cortexa_regs_write;
+	t->reg_read = cortexa_reg_read;
+	t->reg_write = cortexa_reg_write;
 
 	t->reset = cortexa_reset;
 	t->halt_request = cortexa_halt_request;
@@ -468,6 +473,47 @@ static void cortexa_regs_write(target *t, const void *data)
 	memcpy(&priv->reg_cache, data, t->regs_size);
 }
 
+static ssize_t ptr_for_reg(target *t, int reg, void **r)
+{
+	struct cortexa_priv *priv = (struct cortexa_priv *)t->priv;
+	switch (reg) {
+	case 0 ... 15:
+		*r = &priv->reg_cache.r[reg];
+		return 4;
+	case 16:
+		*r = &priv->reg_cache.cpsr;
+		return 4;
+	case 17:
+		*r = &priv->reg_cache.fpscr;
+		return 4;
+	case 18 ... 33:
+		*r = &priv->reg_cache.d[reg - 18];
+		return 8;
+	default:
+		return -1;
+	}
+}
+
+static ssize_t cortexa_reg_read(target *t, int reg, void *data, size_t max)
+{
+	void *r = NULL;
+	size_t s = ptr_for_reg(t, reg, &r);
+	if (s > max)
+		return -1;
+	memcpy(data, r, s);
+	return s;
+}
+
+static ssize_t cortexa_reg_write(target *t, int reg, const void *data, size_t max)
+{
+	void *r = NULL;
+	size_t s = ptr_for_reg(t, reg, &r);
+	if (s > max)
+		return -1;
+	memcpy(r, data, s);
+	return s;
+}
+
 static void cortexa_regs_read_internal(target *t)
 {
 	struct cortexa_priv *priv = (struct cortexa_priv *)t->priv;
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index a67b5994..17017aa3 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -53,6 +53,8 @@ const struct command_s cortexm_cmd_list[] = {
 static void cortexm_regs_read(target *t, void *data);
 static void cortexm_regs_write(target *t, const void *data);
 static uint32_t cortexm_pc_read(target *t);
+ssize_t cortexm_reg_read(target *t, int reg, void *data, size_t max);
+ssize_t cortexm_reg_write(target *t, int reg, const void *data, size_t max);
 
 static void cortexm_reset(target *t);
 static enum target_halt_reason cortexm_halt_poll(target *t, target_addr *watch);
@@ -308,6 +310,8 @@ bool cortexm_probe(ADIv5_AP_t *ap, bool forced)
 	t->tdesc = tdesc_cortex_m;
 	t->regs_read = cortexm_regs_read;
 	t->regs_write = cortexm_regs_write;
+	t->reg_read = cortexm_reg_read;
+	t->reg_write = cortexm_reg_write;
 
 	t->reset = cortexm_reset;
 	t->halt_request = cortexm_halt_request;
@@ -544,6 +548,39 @@ int cortexm_mem_write_sized(
 	return target_check_error(t);
 }
 
+int dcrsr_regnum(target *t, unsigned reg)
+{
+	if (reg < sizeof(regnum_cortex_m) / 4) {
+		return regnum_cortex_m[reg];
+	} else if ((t->target_options & TOPT_FLAVOUR_V7MF) &&
+	           (reg < (sizeof(regnum_cortex_m) +
+	                   sizeof(regnum_cortex_mf) / 4))) {
+		return regnum_cortex_mf[reg - sizeof(regnum_cortex_m)/4];
+	} else {
+		return -1;
+	}
+}
+ssize_t cortexm_reg_read(target *t, int reg, void *data, size_t max)
+{
+	if (max < 4)
+		return -1;
+	uint32_t *r = data;
+	target_mem_write32(t, CORTEXM_DCRSR, dcrsr_regnum(t, reg));
+	*r = target_mem_read32(t, CORTEXM_DCRDR);
+	return 4;
+}
+
+ssize_t cortexm_reg_write(target *t, int reg, const void *data, size_t max)
+{
+	if (max < 4)
+		return -1;
+	const uint32_t *r = data;
+	target_mem_write32(t, CORTEXM_DCRDR, *r);
+	target_mem_write32(t, CORTEXM_DCRSR, CORTEXM_DCRSR_REGWnR |
+	                                     dcrsr_regnum(t, reg));
+	return 4;
+}
+
 static uint32_t cortexm_pc_read(target *t)
 {
 	target_mem_write32(t, CORTEXM_DCRSR, 0x0F);
diff --git a/src/target/cortexm.h b/src/target/cortexm.h
index 9c763b74..927d9d66 100644
--- a/src/target/cortexm.h
+++ b/src/target/cortexm.h
@@ -144,10 +144,12 @@ extern long cortexm_wait_timeout;
 #define CORTEXM_FPB_CTRL_KEY		(1 << 1)
 #define CORTEXM_FPB_CTRL_ENABLE		(1 << 0)
 
-/* Data Watchpoint and Trace Mask Register (DWT_MASKx) */
-#define CORTEXM_DWT_MASK_BYTE		(0 << 0)
-#define CORTEXM_DWT_MASK_HALFWORD	(1 << 0)
-#define CORTEXM_DWT_MASK_WORD		(3 << 0)
+/* Data Watchpoint and Trace Mask Register (DWT_MASKx)
+*  The value here is the number of address bits we mask out */
+#define CORTEXM_DWT_MASK_BYTE		(0)
+#define CORTEXM_DWT_MASK_HALFWORD	(1)
+#define CORTEXM_DWT_MASK_WORD		(2)
+#define CORTEXM_DWT_MASK_DWORD		(3)
 
 /* Data Watchpoint and Trace Function Register (DWT_FUNCTIONx) */
 #define CORTEXM_DWT_FUNC_MATCHED	(1 << 24)
diff --git a/src/target/efm32.c b/src/target/efm32.c
index 4405d99a..6bd114e5 100644
--- a/src/target/efm32.c
+++ b/src/target/efm32.c
@@ -53,9 +53,9 @@ static const uint16_t efm32_flash_write_stub[] = {
 #include "flashstub/efm32.stub"
 };
 
-static bool efm32_cmd_erase_all(target *t);
-static bool efm32_cmd_serial(target *t);
-static bool efm32_cmd_efm_info(target *t);
+static bool efm32_cmd_erase_all(target *t, int argc, const char **argv);
+static bool efm32_cmd_serial(target *t, int argc, const char **argv);
+static bool efm32_cmd_efm_info(target *t, int argc, const char **argv);
 
 const struct command_s efm32_cmd_list[] = {
 	{"erase_mass", (cmd_handler)efm32_cmd_erase_all, "Erase entire flash memory"},
@@ -698,8 +698,10 @@ static int efm32_flash_write(struct target_flash *f,
 /**
  * Uses the MSC ERASEMAIN0 command to erase the entire flash
  */
-static bool efm32_cmd_erase_all(target *t)
+static bool efm32_cmd_erase_all(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	efm32_device_t const* device = efm32_get_device(t->driver[2] - 32);
 	if (device == NULL) {
 		return true;
@@ -732,8 +734,10 @@ static bool efm32_cmd_erase_all(target *t)
 /**
  * Reads the 40-bit unique number
  */
-static bool efm32_cmd_serial(target *t)
+static bool efm32_cmd_serial(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint64_t unique = 0;
 	uint8_t di_version = t->driver[0] - 48; /* di version hidden in driver str */
 
@@ -755,8 +759,10 @@ static bool efm32_cmd_serial(target *t)
 /**
  * Prints various information we know about the device
  */
-static bool efm32_cmd_efm_info(target *t)
+static bool efm32_cmd_efm_info(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint8_t di_version  = t->driver[0] - 48; /* hidden in driver str */
 
 	switch (di_version) {
diff --git a/src/target/kinetis.c b/src/target/kinetis.c
index b7ec7e4e..e0b1530b 100644
--- a/src/target/kinetis.c
+++ b/src/target/kinetis.c
@@ -343,8 +343,8 @@ static int kl_gen_flash_done(struct target_flash *f)
 #define KINETIS_MDM_IDR_K22F 0x1c0000
 #define KINETIS_MDM_IDR_KZ03 0x1c0020
 
-static bool kinetis_mdm_cmd_erase_mass(target *t);
-static bool kinetis_mdm_cmd_ke04_mode(target *t);
+static bool kinetis_mdm_cmd_erase_mass(target *t, int argc, const char **argv);
+static bool kinetis_mdm_cmd_ke04_mode(target *t, int argc, const char **argv);
 
 const struct command_s kinetis_mdm_cmd_list[] = {
 	{"erase_mass", (cmd_handler)kinetis_mdm_cmd_erase_mass, "Erase entire flash memory"},
@@ -352,11 +352,6 @@ const struct command_s kinetis_mdm_cmd_list[] = {
 	{NULL, NULL, NULL}
 };
 
-static bool nop_function(void)
-{
-	return true;
-}
-
 enum target_halt_reason mdm_halt_poll(target *t, target_addr *watch)
 {
 	(void)t; (void)watch;
@@ -383,19 +378,7 @@ void kinetis_mdm_probe(ADIv5_AP_t *ap)
 	t->priv_free = (void*)adiv5_ap_unref;
 
 	t->driver = "Kinetis Recovery (MDM-AP)";
-	t->attach = (void*)nop_function;
-	t->detach = (void*)nop_function;
-	t->check_error = (void*)nop_function;
-	t->mem_read = (void*)nop_function;
-	t->mem_write = (void*)nop_function;
 	t->regs_size = 4;
-	t->regs_read = (void*)nop_function;
-	t->regs_write = (void*)nop_function;
-	t->reset = (void*)nop_function;
-	t->halt_request = (void*)nop_function;
-	t->halt_poll = mdm_halt_poll;
-	t->halt_resume = (void*)nop_function;
-
 	target_add_commands(t, kinetis_mdm_cmd_list, t->driver);
 }
 
@@ -412,15 +395,19 @@ void kinetis_mdm_probe(ADIv5_AP_t *ap)
 /* This is needed as a separate command, as there's no way to  *
  * tell a KE04 from other kinetis in kinetis_mdm_probe()       */
 static bool ke04_mode = false;
-static bool kinetis_mdm_cmd_ke04_mode(target *t)
+static bool kinetis_mdm_cmd_ke04_mode(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	/* Set a flag to ignore part of the status and assert reset */
 	ke04_mode = true;
 	tc_printf(t, "Mass erase for KE04 now allowed\n");
 	return true;
 }
-static bool kinetis_mdm_cmd_erase_mass(target *t)
+static bool kinetis_mdm_cmd_erase_mass(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	ADIv5_AP_t *ap = t->priv;
 
 	/* Keep the MCU in reset as stated in KL25PxxM48SF0RM */
diff --git a/src/target/msp432.c b/src/target/msp432.c
index 96acfed2..f1b3eb37 100644
--- a/src/target/msp432.c
+++ b/src/target/msp432.c
@@ -135,9 +135,9 @@ static inline uint32_t msp432_sector_unprotect(struct msp432_flash *mf, target_a
 
 /* Optional commands handlers */
 /* Erase all of main flash */
-static bool msp432_cmd_erase_main(target *t);
+static bool msp432_cmd_erase_main(target *t, int argc, const char **argv);
 /* Erase a single (4KB) sector */
-static bool msp432_cmd_sector_erase(target *t, int argc, char *argv[]);
+static bool msp432_cmd_sector_erase(target *t, int argc, const char **argv);
 
 /* Optional commands structure*/
 const struct command_s msp432_cmd_list[] = {
@@ -308,8 +308,10 @@ static int msp432_flash_write(struct target_flash *f, target_addr dest,
 }
 
 /* Optional commands handlers */
-static bool msp432_cmd_erase_main(target *t)
+static bool msp432_cmd_erase_main(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	/* The mass erase routine in ROM will also erase the Info Flash. */
 	/* Usually, this is not wanted, so go sector by sector...        */
 
@@ -327,7 +329,7 @@ static bool msp432_cmd_erase_main(target *t)
 	return ret;
 }
 
-static bool msp432_cmd_sector_erase(target *t, int argc, char *argv[])
+static bool msp432_cmd_sector_erase(target *t, int argc, const char **argv)
 {
 	if (argc < 2)
 		tc_printf(t, "usage: monitor sector_erase <addr>\n");
diff --git a/src/target/nrf51.c b/src/target/nrf51.c
index 48885bcc..3b004244 100644
--- a/src/target/nrf51.c
+++ b/src/target/nrf51.c
@@ -30,14 +30,14 @@ static int nrf51_flash_erase(struct target_flash *f, target_addr addr, size_t le
 static int nrf51_flash_write(struct target_flash *f,
                              target_addr dest, const void *src, size_t len);
 
-static bool nrf51_cmd_erase_all(target *t);
-static bool nrf51_cmd_read_hwid(target *t);
-static bool nrf51_cmd_read_fwid(target *t);
-static bool nrf51_cmd_read_deviceid(target *t);
-static bool nrf51_cmd_read_deviceaddr(target *t);
-static bool nrf51_cmd_read_deviceinfo(target *t);
-static bool nrf51_cmd_read_help(target *t);
-static bool nrf51_cmd_read(target *t, int argc, const char *argv[]);
+static bool nrf51_cmd_erase_all(target *t, int argc, const char **argv);
+static bool nrf51_cmd_read_hwid(target *t, int argc, const char **argv);
+static bool nrf51_cmd_read_fwid(target *t, int argc, const char **argv);
+static bool nrf51_cmd_read_deviceid(target *t, int argc, const char **argv);
+static bool nrf51_cmd_read_deviceaddr(target *t, int argc, const char **argv);
+static bool nrf51_cmd_read_deviceinfo(target *t, int argc, const char **argv);
+static bool nrf51_cmd_read_help(target *t, int argc, const char **argv);
+static bool nrf51_cmd_read(target *t, int argc, const char **argv);
 
 const struct command_s nrf51_cmd_list[] = {
 	{"erase_mass", (cmd_handler)nrf51_cmd_erase_all, "Erase entire flash memory"},
@@ -215,8 +215,10 @@ static int nrf51_flash_write(struct target_flash *f,
 	return 0;
 }
 
-static bool nrf51_cmd_erase_all(target *t)
+static bool nrf51_cmd_erase_all(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	tc_printf(t, "erase..\n");
 
 	/* Enable erase */
@@ -238,22 +240,28 @@ static bool nrf51_cmd_erase_all(target *t)
 	return true;
 }
 
-static bool nrf51_cmd_read_hwid(target *t)
+static bool nrf51_cmd_read_hwid(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint32_t hwid = target_mem_read32(t, NRF51_FICR_CONFIGID) & 0xFFFF;
 	tc_printf(t, "Hardware ID: 0x%04X\n", hwid);
 
 	return true;
 }
-static bool nrf51_cmd_read_fwid(target *t)
+static bool nrf51_cmd_read_fwid(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint32_t fwid = (target_mem_read32(t, NRF51_FICR_CONFIGID) >> 16) & 0xFFFF;
 	tc_printf(t, "Firmware ID: 0x%04X\n", fwid);
 
 	return true;
 }
-static bool nrf51_cmd_read_deviceid(target *t)
+static bool nrf51_cmd_read_deviceid(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint32_t deviceid_low = target_mem_read32(t, NRF51_FICR_DEVICEID_LOW);
 	uint32_t deviceid_high = target_mem_read32(t, NRF51_FICR_DEVICEID_HIGH);
 
@@ -262,8 +270,10 @@ static bool nrf51_cmd_read_deviceid(target *t)
 	return true;
 }
 
-static bool nrf51_cmd_read_deviceinfo(target *t)
+static bool nrf51_cmd_read_deviceinfo(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	struct deviceinfo{
 		uint32_t part;
 		union{
@@ -311,8 +321,10 @@ static bool nrf51_cmd_read_deviceinfo(target *t)
 	return true;
 }
 
-static bool nrf51_cmd_read_deviceaddr(target *t)
+static bool nrf51_cmd_read_deviceaddr(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint32_t addr_type = target_mem_read32(t, NRF51_FICR_DEVICEADDRTYPE);
 	uint32_t addr_low = target_mem_read32(t, NRF51_FICR_DEVICEADDR_LOW);
 	uint32_t addr_high = target_mem_read32(t, NRF51_FICR_DEVICEADDR_HIGH) & 0xFFFF;
@@ -325,8 +337,10 @@ static bool nrf51_cmd_read_deviceaddr(target *t)
 
 	return true;
 }
-static bool nrf51_cmd_read_help(target *t)
+static bool nrf51_cmd_read_help(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	const struct command_s *c;
 
 	tc_printf(t, "Read commands:\n");
@@ -335,7 +349,7 @@ static bool nrf51_cmd_read_help(target *t)
 
 	return true;
 }
-static bool nrf51_cmd_read(target *t, int argc, const char *argv[])
+static bool nrf51_cmd_read(target *t, int argc, const char **argv)
 {
 	const struct command_s *c;
 	if (argc > 1) {
@@ -347,24 +361,19 @@ static bool nrf51_cmd_read(target *t, int argc, const char *argv[])
 				return !c->handler(t, argc - 1, &argv[1]);
 		}
 	}
-	return nrf51_cmd_read_help(t);
+	return nrf51_cmd_read_help(t, 0, NULL);
 }
 
 #include "adiv5.h"
 #define NRF52_MDM_IDR 0x02880000
 
-static bool nrf51_mdm_cmd_erase_mass(target *t);
+static bool nrf51_mdm_cmd_erase_mass(target *t, int argc, const char **argv);
 
 const struct command_s nrf51_mdm_cmd_list[] = {
 	{"erase_mass", (cmd_handler)nrf51_mdm_cmd_erase_mass, "Erase entire flash memory"},
 	{NULL, NULL, NULL}
 };
 
-static bool nop_function(void)
-{
-	return true;
-}
-
 void nrf51_mdm_probe(ADIv5_AP_t *ap)
 {
 	switch(ap->idr) {
@@ -384,19 +393,7 @@ void nrf51_mdm_probe(ADIv5_AP_t *ap)
 	t->priv_free = (void*)adiv5_ap_unref;
 
 	t->driver = "Nordic nRF52 Access Port";
-	t->attach = (void*)nop_function;
-	t->detach = (void*)nop_function;
-	t->check_error = (void*)nop_function;
-	t->mem_read = (void*)nop_function;
-	t->mem_write = (void*)nop_function;
 	t->regs_size = 4;
-	t->regs_read = (void*)nop_function;
-	t->regs_write = (void*)nop_function;
-	t->reset = (void*)nop_function;
-	t->halt_request = (void*)nop_function;
-	//t->halt_poll = mdm_halt_poll;
-	t->halt_resume = (void*)nop_function;
-
 	target_add_commands(t, nrf51_mdm_cmd_list, t->driver);
 }
 
@@ -407,8 +404,10 @@ void nrf51_mdm_probe(ADIv5_AP_t *ap)
 #define MDM_PROT_EN  ADIV5_AP_REG(0x0C)
 
 
-static bool nrf51_mdm_cmd_erase_mass(target *t)
+static bool nrf51_mdm_cmd_erase_mass(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	ADIv5_AP_t *ap = t->priv;
 
 	uint32_t status = adiv5_ap_read(ap, MDM_STATUS);
diff --git a/src/target/sam3x.c b/src/target/sam3x.c
index 4abea750..2dae6029 100644
--- a/src/target/sam3x.c
+++ b/src/target/sam3x.c
@@ -33,8 +33,8 @@ static int sam3_flash_erase(struct target_flash *f, target_addr addr, size_t len
 static int sam3x_flash_write(struct target_flash *f, target_addr dest,
                              const void *src, size_t len);
 
-static bool sam3x_cmd_gpnvm_get(target *t);
-static bool sam3x_cmd_gpnvm_set(target *t, int argc, char *argv[]);
+static bool sam3x_cmd_gpnvm_get(target *t, int argc, const char **argv);
+static bool sam3x_cmd_gpnvm_set(target *t, int argc, const char **argv);
 
 const struct command_s sam3x_cmd_list[] = {
 	{"gpnvm_get", (cmd_handler)sam3x_cmd_gpnvm_get, "Get GPVNM value"},
@@ -338,8 +338,10 @@ static int sam3x_flash_write(struct target_flash *f, target_addr dest,
 	return 0;
 }
 
-static bool sam3x_cmd_gpnvm_get(target *t)
+static bool sam3x_cmd_gpnvm_get(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint32_t base = sam3x_flash_base(t);
 
 	sam3x_flash_cmd(t, base, EEFC_FCR_FCMD_GGPB, 0);
@@ -348,7 +350,7 @@ static bool sam3x_cmd_gpnvm_get(target *t)
 	return true;
 }
 
-static bool sam3x_cmd_gpnvm_set(target *t, int argc, char *argv[])
+static bool sam3x_cmd_gpnvm_set(target *t, int argc, const char **argv)
 {
 	uint32_t bit, cmd;
 	uint32_t base = sam3x_flash_base(t);
@@ -361,7 +363,7 @@ static bool sam3x_cmd_gpnvm_set(target *t, int argc, char *argv[])
 	cmd = atol(argv[2]) ? EEFC_FCR_FCMD_SGPB : EEFC_FCR_FCMD_CGPB;
 
 	sam3x_flash_cmd(t, base, cmd, bit);
-	sam3x_cmd_gpnvm_get(t);
+	sam3x_cmd_gpnvm_get(t, 0, NULL);
 
 	return true;
 }
diff --git a/src/target/samd.c b/src/target/samd.c
index 3117f2ae..f5d4282d 100644
--- a/src/target/samd.c
+++ b/src/target/samd.c
@@ -41,15 +41,15 @@ static int samd_flash_erase(struct target_flash *t, target_addr addr, size_t len
 static int samd_flash_write(struct target_flash *f,
                             target_addr dest, const void *src, size_t len);
 
-static bool samd_cmd_erase_all(target *t);
-static bool samd_cmd_lock_flash(target *t);
-static bool samd_cmd_unlock_flash(target *t);
-static bool samd_cmd_unlock_bootprot(target *t);
-static bool samd_cmd_lock_bootprot(target *t);
-static bool samd_cmd_read_userrow(target *t);
-static bool samd_cmd_serial(target *t);
-static bool samd_cmd_mbist(target *t);
-static bool samd_cmd_ssb(target *t);
+static bool samd_cmd_erase_all(target *t, int argc, const char **argv);
+static bool samd_cmd_lock_flash(target *t, int argc, const char **argv);
+static bool samd_cmd_unlock_flash(target *t, int argc, const char **argv);
+static bool samd_cmd_unlock_bootprot(target *t, int argc, const char **argv);
+static bool samd_cmd_lock_bootprot(target *t, int argc, const char **argv);
+static bool samd_cmd_read_userrow(target *t, int argc, const char **argv);
+static bool samd_cmd_serial(target *t, int argc, const char **argv);
+static bool samd_cmd_mbist(target *t, int argc, const char **argv);
+static bool samd_cmd_ssb(target *t, int argc, const char **argv);
 
 const struct command_s samd_cmd_list[] = {
 	{"erase_mass", (cmd_handler)samd_cmd_erase_all, "Erase entire flash memory"},
@@ -273,8 +273,7 @@ samd20_revB_halt_resume(target *t, bool step)
  * function allows users to attach on a temporary basis so they can
  * rescue the device.
  */
-static bool
-samd_protected_attach(target *t)
+static bool samd_protected_attach(target *t)
 {
 	/**
 	 * TODO: Notify the user that we're not really attached and
@@ -525,8 +524,10 @@ static int samd_flash_write(struct target_flash *f,
 /**
  * Uses the Device Service Unit to erase the entire flash
  */
-static bool samd_cmd_erase_all(target *t)
+static bool samd_cmd_erase_all(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	/* Clear the DSU status bits */
 	target_mem_write32(t, SAMD_DSU_CTRLSTAT,
 	                   SAMD_STATUSA_DONE | SAMD_STATUSA_PERR |
@@ -566,8 +567,9 @@ static bool samd_cmd_erase_all(target *t)
  *
  * 0x0000 = Lock, 0xFFFF = Unlock (default)
  */
-static bool samd_set_flashlock(target *t, uint16_t value)
+static bool samd_set_flashlock(target *t, uint16_t value, const char **argv)
 {
+	(void)argv;
 	uint32_t high = target_mem_read32(t, SAMD_NVM_USER_ROW_HIGH);
 	uint32_t low = target_mem_read32(t, SAMD_NVM_USER_ROW_LOW);
 
@@ -598,18 +600,23 @@ static bool samd_set_flashlock(target *t, uint16_t value)
 	return true;
 }
 
-static bool samd_cmd_lock_flash(target *t)
+static bool samd_cmd_lock_flash(target *t, int argc, const char **argv)
 {
-	return samd_set_flashlock(t, 0x0000);
+	(void)argc;
+	(void)argv;
+	return samd_set_flashlock(t, 0x0000, NULL);
 }
 
-static bool samd_cmd_unlock_flash(target *t)
+static bool samd_cmd_unlock_flash(target *t, int argc, const char **argv)
 {
-	return samd_set_flashlock(t, 0xFFFF);
+	(void)argc;
+	(void)argv;
+	return samd_set_flashlock(t, 0xFFFF, NULL);
 }
 
-static bool samd_set_bootprot(target *t, uint16_t value)
+static bool samd_set_bootprot(target *t, uint16_t value, const char **argv)
 {
+	(void)argv;
 	uint32_t high = target_mem_read32(t, SAMD_NVM_USER_ROW_HIGH);
 	uint32_t low = target_mem_read32(t, SAMD_NVM_USER_ROW_LOW);
 
@@ -640,18 +647,24 @@ static bool samd_set_bootprot(target *t, uint16_t value)
 	return true;
 }
 
-static bool samd_cmd_lock_bootprot(target *t)
+static bool samd_cmd_lock_bootprot(target *t, int argc, const char **argv)
 {
-	return samd_set_bootprot(t, 0);
+	(void)argc;
+	(void)argv;
+	return samd_set_bootprot(t, 0, NULL);
 }
 
-static bool samd_cmd_unlock_bootprot(target *t)
+static bool samd_cmd_unlock_bootprot(target *t, int argc, const char **argv)
 {
-	return samd_set_bootprot(t, 7);
+	(void)argc;
+	(void)argv;
+	return samd_set_bootprot(t, 7, NULL);
 }
 
-static bool samd_cmd_read_userrow(target *t)
+static bool samd_cmd_read_userrow(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	tc_printf(t, "User Row: 0x%08x%08x\n",
 		target_mem_read32(t, SAMD_NVM_USER_ROW_HIGH),
 		target_mem_read32(t, SAMD_NVM_USER_ROW_LOW));
@@ -662,8 +675,10 @@ static bool samd_cmd_read_userrow(target *t)
 /**
  * Reads the 128-bit serial number from the NVM
  */
-static bool samd_cmd_serial(target *t)
+static bool samd_cmd_serial(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	tc_printf(t, "Serial Number: 0x");
 
 	for (uint32_t i = 0; i < 4; i++) {
@@ -693,8 +708,10 @@ static uint32_t samd_flash_size(target *t)
 /**
  * Runs the Memory Built In Self Test (MBIST)
  */
-static bool samd_cmd_mbist(target *t)
+static bool samd_cmd_mbist(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	/* Write the memory parameters to the DSU */
 	target_mem_write32(t, SAMD_DSU_ADDRESS, 0);
 	target_mem_write32(t, SAMD_DSU_LENGTH, samd_flash_size(t));
@@ -731,8 +748,10 @@ static bool samd_cmd_mbist(target *t)
 /**
  * Sets the security bit
  */
-static bool samd_cmd_ssb(target *t)
+static bool samd_cmd_ssb(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	/* Issue the ssb command */
 	target_mem_write32(t, SAMD_NVMC_CTRLA,
 	                   SAMD_CTRLA_CMD_KEY | SAMD_CTRLA_CMD_SSB);
diff --git a/src/target/stm32f1.c b/src/target/stm32f1.c
index 0719dd0a..0774257a 100644
--- a/src/target/stm32f1.c
+++ b/src/target/stm32f1.c
@@ -39,8 +39,8 @@
 #include "target_internal.h"
 #include "cortexm.h"
 
-static bool stm32f1_cmd_erase_mass(target *t);
-static bool stm32f1_cmd_option(target *t, int argc, char *argv[]);
+static bool stm32f1_cmd_erase_mass(target *t, int argc, const char **argv);
+static bool stm32f1_cmd_option(target *t, int argc, const char **argv);
 
 const struct command_s stm32f1_cmd_list[] = {
 	{"erase_mass", (cmd_handler)stm32f1_cmd_erase_mass, "Erase entire flash memory"},
@@ -249,8 +249,10 @@ static int stm32f1_flash_write(struct target_flash *f,
 	return 0;
 }
 
-static bool stm32f1_cmd_erase_mass(target *t)
+static bool stm32f1_cmd_erase_mass(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	stm32f1_flash_unlock(t);
 
 	/* Flash mass erase start instruction */
@@ -326,7 +328,7 @@ static bool stm32f1_option_write(target *t, uint32_t addr, uint16_t value)
 	return true;
 }
 
-static bool stm32f1_cmd_option(target *t, int argc, char *argv[])
+static bool stm32f1_cmd_option(target *t, int argc, const char **argv)
 {
 	uint32_t addr, val;
 	uint32_t flash_obp_rdp_key;
diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index 338cb10d..10b71023 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -37,7 +37,7 @@
 #include "target_internal.h"
 #include "cortexm.h"
 
-static bool stm32f4_cmd_erase_mass(target *t);
+static bool stm32f4_cmd_erase_mass(target *t, int argc, const char **argv);
 static bool stm32f4_cmd_option(target *t, int argc, char *argv[]);
 static bool stm32f4_cmd_psize(target *t, int argc, char *argv[]);
 
@@ -456,8 +456,10 @@ static int stm32f4_flash_write(struct target_flash *f,
 	return 0;
 }
 
-static bool stm32f4_cmd_erase_mass(target *t)
+static bool stm32f4_cmd_erase_mass(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	const char spinner[] = "|/-\\";
 	int spinindex = 0;
 	struct target_flash *f = t->flash;
diff --git a/src/target/stm32h7.c b/src/target/stm32h7.c
index 15d9614d..f9e0ac1f 100644
--- a/src/target/stm32h7.c
+++ b/src/target/stm32h7.c
@@ -30,12 +30,12 @@
 #include "target_internal.h"
 #include "cortexm.h"
 
-static bool stm32h7_cmd_erase_mass(target *t);
+static bool stm32h7_cmd_erase_mass(target *t, int argc, const char **argv);
 /* static bool stm32h7_cmd_option(target *t, int argc, char *argv[]); */
-static bool stm32h7_uid(target *t);
-static bool stm32h7_crc(target *t);
+static bool stm32h7_uid(target *t, int argc, const char **argv);
+static bool stm32h7_crc(target *t, int argc, const char **argv);
 static bool stm32h7_cmd_psize(target *t, int argc, char *argv[]);
-static bool stm32h7_cmd_rev(target *t);
+static bool stm32h7_cmd_rev(target *t, int argc, const char **argv);
 
 const struct command_s stm32h7_cmd_list[] = {
 	{"erase_mass", (cmd_handler)stm32h7_cmd_erase_mass,
@@ -430,8 +430,10 @@ static bool stm32h7_cmd_erase(target *t, int bank_mask)
 	return result;
 }
 
-static bool stm32h7_cmd_erase_mass(target *t)
+static bool stm32h7_cmd_erase_mass(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	tc_printf(t, "Erasing flash... This may take a few seconds.  ");
 	return stm32h7_cmd_erase(t, 3);
 }
@@ -439,8 +441,10 @@ static bool stm32h7_cmd_erase_mass(target *t)
 /* Print the Unique device ID.
  * Can be reused for other STM32 devices With uid as parameter.
  */
-static bool stm32h7_uid(target *t)
+static bool stm32h7_uid(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	uint32_t uid = 0x1ff1e800;
 	int i;
 	tc_printf(t, "0x");
@@ -483,8 +487,10 @@ static int stm32h7_crc_bank(target *t, uint32_t bank)
 	return 0;
 }
 
-static bool stm32h7_crc(target *t)
+static bool stm32h7_crc(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	if (stm32h7_crc_bank(t, BANK1_START) ) return false;
 	uint32_t crc1 = target_mem_read32(t, FPEC1_BASE + FLASH_CRCDATA);
 	if (stm32h7_crc_bank(t, BANK2_START) ) return false;
@@ -494,6 +500,8 @@ static bool stm32h7_crc(target *t)
 }
 static bool stm32h7_cmd_psize(target *t, int argc, char *argv[])
 {
+	(void)argc;
+	(void)argv;
 	if (argc == 1) {
 		enum align psize = ALIGN_DWORD;
 		for (struct target_flash *f = t->flash; f; f = f->next) {
@@ -538,8 +546,10 @@ static const struct stm32h7xx_rev {
 	{ 0x2001, 'X' },
 	{ 0x2003, 'V' }
 };
-static bool stm32h7_cmd_rev(target *t)
+static bool stm32h7_cmd_rev(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	/* DBGMCU identity code register */
 	uint32_t dbgmcu_idc = target_mem_read32(t, DBGMCU_IDC);
 	uint16_t rev_id = (dbgmcu_idc >> 16) & 0xFFFF;
diff --git a/src/target/stm32l4.c b/src/target/stm32l4.c
index 0a0a6d20..02375ff7 100644
--- a/src/target/stm32l4.c
+++ b/src/target/stm32l4.c
@@ -38,9 +38,9 @@
 #include "target_internal.h"
 #include "cortexm.h"
 
-static bool stm32l4_cmd_erase_mass(target *t);
-static bool stm32l4_cmd_erase_bank1(target *t);
-static bool stm32l4_cmd_erase_bank2(target *t);
+static bool stm32l4_cmd_erase_mass(target *t, int argc, const char **argv);
+static bool stm32l4_cmd_erase_bank1(target *t, int argc, const char **argv);
+static bool stm32l4_cmd_erase_bank2(target *t, int argc, const char **argv);
 static bool stm32l4_cmd_option(target *t, int argc, char *argv[]);
 
 const struct command_s stm32l4_cmd_list[] = {
@@ -484,18 +484,24 @@ static bool stm32l4_cmd_erase(target *t, uint32_t action)
 	return true;
 }
 
-static bool stm32l4_cmd_erase_mass(target *t)
+static bool stm32l4_cmd_erase_mass(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	return stm32l4_cmd_erase(t, FLASH_CR_MER1 | FLASH_CR_MER2);
 }
 
-static bool stm32l4_cmd_erase_bank1(target *t)
+static bool stm32l4_cmd_erase_bank1(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	return stm32l4_cmd_erase(t, FLASH_CR_MER1);
 }
 
-static bool stm32l4_cmd_erase_bank2(target *t)
+static bool stm32l4_cmd_erase_bank2(target *t, int argc, const char **argv)
 {
+	(void)argc;
+	(void)argv;
 	return stm32l4_cmd_erase(t, FLASH_CR_MER2);
 }
 
diff --git a/src/target/target.c b/src/target/target.c
index db396a38..9ae1e9e0 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -30,6 +30,11 @@ static int target_flash_write_buffered(struct target_flash *f,
                                        target_addr dest, const void *src, size_t len);
 static int target_flash_done_buffered(struct target_flash *f);
 
+static bool nop_function(void)
+{
+	return true;
+}
+
 target *target_new(void)
 {
 	target *t = (void*)calloc(1, sizeof(*t));
@@ -47,6 +52,20 @@ target *target_new(void)
 		target_list = t;
 	}
 
+	t->attach = (void*)nop_function;
+	t->detach = (void*)nop_function;
+	t->check_error = (void*)nop_function;
+	t->mem_read = (void*)nop_function;
+	t->mem_write = (void*)nop_function;
+	t->reg_read = (void*)nop_function;
+	t->reg_write = (void*)nop_function;
+	t->regs_read = (void*)nop_function;
+	t->regs_write = (void*)nop_function;
+	t->reset = (void*)nop_function;
+	t->halt_request = (void*)nop_function;
+	t->halt_poll = (void*)nop_function;
+	t->halt_resume = (void*)nop_function;
+
 	return t;
 }
 
@@ -337,8 +356,36 @@ int target_mem_write(target *t, target_addr dest, const void *src, size_t len)
 }
 
 /* Register access functions */
-void target_regs_read(target *t, void *data) { t->regs_read(t, data); }
-void target_regs_write(target *t, const void *data) { t->regs_write(t, data); }
+ssize_t target_reg_read(target *t, int reg, void *data, size_t max)
+{
+	return t->reg_read(t, reg, data, max);
+}
+
+ssize_t target_reg_write(target *t, int reg, const void *data, size_t size)
+{
+	return t->reg_write(t, reg, data, size);
+}
+
+void target_regs_read(target *t, void *data)
+{
+	if (t->regs_read) {
+		t->regs_read(t, data);
+		return;
+	}
+	for (size_t x = 0, i = 0; x < t->regs_size; ) {
+		x += t->reg_read(t, i++, data + x, t->regs_size - x);
+	}
+}
+void target_regs_write(target *t, const void *data)
+{
+	if (t->regs_write) {
+		t->regs_write(t, data);
+		return;
+	}
+	for (size_t x = 0, i = 0; x < t->regs_size; ) {
+		x += t->reg_write(t, i++, data + x, t->regs_size - x);
+	}
+}
 
 /* Halt/resume functions */
 void target_reset(target *t) { t->reset(t); }
diff --git a/src/target/target_internal.h b/src/target/target_internal.h
index 5f4e41e7..69981f3f 100644
--- a/src/target/target_internal.h
+++ b/src/target/target_internal.h
@@ -92,6 +92,8 @@ struct target_s {
 	const char *tdesc;
 	void (*regs_read)(target *t, void *data);
 	void (*regs_write)(target *t, const void *data);
+	ssize_t (*reg_read)(target *t, int reg, void *data, size_t max);
+	ssize_t (*reg_write)(target *t, int reg, const void *data, size_t size);
 
 	/* Halt/resume functions */
 	void (*reset)(target *t);