diff --git a/src/platforms/libftdi/platform.c b/src/platforms/libftdi/platform.c
index 93c89e9e..e2ceb59a 100644
--- a/src/platforms/libftdi/platform.c
+++ b/src/platforms/libftdi/platform.c
@@ -310,3 +310,8 @@ const char *platform_target_voltage(void)
 {
 	return "not supported";
 }
+
+void platform_adiv5_dp_defaults(void *arg)
+{
+	(void) arg;
+}
diff --git a/src/platforms/pc-hosted/platform.c b/src/platforms/pc-hosted/platform.c
index 53f95ba2..4be4cde4 100644
--- a/src/platforms/pc-hosted/platform.c
+++ b/src/platforms/pc-hosted/platform.c
@@ -181,3 +181,8 @@ const char *platform_target_voltage(void)
 
   return (char *)&construct[1];
 }
+
+void platform_adiv5_dp_defaults(void *arg)
+{
+	(void) arg;
+}
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
index b69b66a7..0d5012fd 100644
--- a/src/platforms/pc-stlinkv2/stlinkv2.c
+++ b/src/platforms/pc-stlinkv2/stlinkv2.c
@@ -1150,7 +1150,7 @@ uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	return response;
 }
 
-bool adiv5_ap_setup(int ap)
+static bool stlink_ap_setup(int ap)
 {
 	if (ap > 7)
 		return false;
@@ -1172,7 +1172,7 @@ bool adiv5_ap_setup(int ap)
 	return true;
 }
 
-void adiv5_ap_cleanup(int ap)
+static void stlink_ap_cleanup(int ap)
 {
        uint8_t cmd[16] = {
                STLINK_DEBUG_COMMAND,
@@ -1195,7 +1195,7 @@ static int stlink_usb_get_rw_status(bool verbose)
 	return stlink_usb_error_check(data, verbose);
 }
 
-void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
 	if (len == 0)
 		return;
@@ -1246,7 +1246,7 @@ void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	DEBUG_STLINK("\n");
 }
 
-void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+static void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 					  uint8_t *buffer)
 {
 	DEBUG_STLINK("Mem Write8 AP %d len %zu addr 0x%08" PRIx32 ": ",
@@ -1275,7 +1275,7 @@ void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 	}
 }
 
-void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+static void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 					   uint16_t *buffer)
 {
 	DEBUG_STLINK("Mem Write16 AP %d len %zu addr 0x%08" PRIx32 ": ",
@@ -1295,7 +1295,7 @@ void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 	stlink_usb_get_rw_status(true);
 }
 
-void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+static void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 					   uint32_t *buffer)
 {
 	DEBUG_STLINK("Mem Write32 AP %d len %zu addr 0x%08" PRIx32 ": ",
@@ -1350,15 +1350,9 @@ void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
 	stlink_usb_error_check(res, true);
 }
 
-void
-adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
-{
-	stlink_readmem(ap, dest, src, len);
-}
-
-void
-adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
-					  size_t len, enum align align)
+static void stlink_mem_write_sized(	ADIv5_AP_t *ap, uint32_t dest,
+									const void *src, size_t len,
+									enum align align)
 {
 	if (len == 0)
 		return;
@@ -1374,12 +1368,12 @@ adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 	}
 }
 
-void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+static void stlink_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
-	stlink_write_dp_register(ap->apsel, addr, value);
+       stlink_write_dp_register(ap->apsel, addr, value);
 }
 
-uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+static uint32_t stlink_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	uint32_t ret;
 	stlink_read_dp_register(ap->apsel, addr, &ret);
@@ -1426,3 +1420,16 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp)
 	return true;
 
 }
+
+void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
+{
+	dp->ap_regs_read = stlink_regs_read;
+	dp->ap_reg_read = stlink_reg_read;
+	dp->ap_reg_write = stlink_reg_write;
+	dp->ap_setup = stlink_ap_setup;
+	dp->ap_cleanup = stlink_ap_cleanup;
+	dp->ap_write = stlink_ap_write;
+	dp->ap_read = stlink_ap_read;
+	dp->mem_read = stlink_readmem;
+	dp->mem_write_sized = stlink_mem_write_sized;
+}
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 5f0de60b..edac3c78 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -399,8 +399,6 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 	}
 	return res;
 }
-bool adiv5_ap_setup(int i);
-void adiv5_ap_cleanup(int i);
 
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 {
@@ -441,12 +439,32 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	return ap;
 }
 
+static void ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
+static uint32_t ap_read(ADIv5_AP_t *ap, uint16_t addr);
+static void mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
+static void mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align);
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
 	volatile bool probed = false;
 	volatile uint32_t ctrlstat = 0;
 	adiv5_dp_ref(dp);
-
+#if PC_HOSTED  == 1
+	platform_adiv5_dp_defaults(dp);
+	if (!dp->ap_write)
+		dp->ap_write = ap_write;
+	if (!dp->ap_read)
+		dp->ap_read = ap_read;
+	if (!dp->mem_read)
+		dp->mem_read = mem_read;
+	if (!dp->mem_write_sized)
+		dp->mem_write_sized = mem_write_sized;
+#else
+	dp->ap_write = ap_write;
+	dp->ap_read = ap_read;
+	dp->mem_read = mem_read;
+	dp->mem_write_sized = mem_write_sized;
+#endif
 	volatile struct exception e;
 	TRY_CATCH (e, EXCEPTION_TIMEOUT) {
 		ctrlstat = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
@@ -507,11 +525,18 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 	int void_aps = 0;
 	for(int i = 0; (i < 256) && (void_aps < 8); i++) {
 		ADIv5_AP_t *ap = NULL;
-		if (adiv5_ap_setup(i))
+#if PC_HOSTED == 1
+		if ((!dp->ap_setup) || dp->ap_setup(i))
 			ap = adiv5_new_ap(dp, i);
+#else
+		ap = adiv5_new_ap(dp, i);
+#endif
 		if (ap == NULL) {
 			void_aps++;
-			adiv5_ap_cleanup(i);
+#if PC_HOSTED == 1
+			if (dp->ap_cleanup)
+				dp->ap_cleanup(i);
+#endif
 			if (i == 0)
 				return;
 			else
@@ -519,7 +544,10 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		}
 		if (ap->base == last_base) {
 			DEBUG("AP %d: Duplicate base\n", i);
-			adiv5_ap_cleanup(i);
+#if PC_HOSTED == 1
+			if (dp->ap_cleanup)
+				dp->ap_cleanup(i);
+#endif
 			free(ap);
 			/* FIXME: Should we expect valid APs behind duplicate ones? */
 			return;
@@ -562,11 +590,6 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 #define ALIGNOF(x) (((x) & 3) == 0 ? ALIGN_WORD : \
                     (((x) & 1) == 0 ? ALIGN_HALFWORD : ALIGN_BYTE))
 
-#if !defined(JTAG_HL)
-
-bool adiv5_ap_setup(int i) {(void)i; return true;}
-void adiv5_ap_cleanup(int i) {(void)i;}
-
 /* Program the CSW and TAR for sequencial access at a given width */
 static void ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
 {
@@ -606,7 +629,7 @@ void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 	return (uint8_t *)dest + (1 << align);
 }
 
-void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+static void mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
 	uint32_t tmp;
 	uint32_t osrc = src;
@@ -636,8 +659,8 @@ void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	extract(dest, src, tmp, align);
 }
 
-void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
-					  size_t len, enum align align)
+static void mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align)
 {
 	uint32_t odest = dest;
 
@@ -671,14 +694,14 @@ void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 	}
 }
 
-void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+static void ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
 			((uint32_t)ap->apsel << 24)|(addr & 0xF0));
 	adiv5_dp_write(ap->dp, addr, value);
 }
 
-uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+static uint32_t ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	uint32_t ret;
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
@@ -686,7 +709,6 @@ uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 	ret = adiv5_dp_read(ap->dp, addr);
 	return ret;
 }
-#endif
 
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 {
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index cbab7a6b..d592921c 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -129,6 +129,8 @@ enum align {
 	ALIGN_DWORD    = 3
 };
 
+typedef struct ADIv5_AP_s ADIv5_AP_t;
+
 /* Try to keep this somewhat absract for later adding SW-DP */
 typedef struct ADIv5_DP_s {
 	int refcnt;
@@ -142,6 +144,23 @@ typedef struct ADIv5_DP_s {
                                uint16_t addr, uint32_t value);
 	void (*abort)(struct ADIv5_DP_s *dp, uint32_t abort);
 
+#if PC_HOSTED == 1
+	bool (*ap_setup)(int i);
+	void (*ap_cleanup)(int i);
+    void (*ap_regs_read)(ADIv5_AP_t *ap, void *data);
+    uint32_t(*ap_reg_read)(ADIv5_AP_t *ap, int num);
+    void (*ap_reg_write)(ADIv5_AP_t *ap, int num, uint32_t value);
+	void (*read_block)(uint32_t addr, uint8_t *data, int size);
+	void (*dap_write_block_sized)(uint32_t addr, uint8_t *data,
+								  int size, enum align align);
+
+#endif
+	uint32_t (*ap_read)(ADIv5_AP_t *ap, uint16_t addr);
+	void (*ap_write)(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
+
+	void (*mem_read)(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
+	void (*mem_write_sized)(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align);
 #if PC_HOSTED == 1
 	jtag_dev_t *dev;
 	uint8_t fault;
@@ -174,7 +193,7 @@ static inline void adiv5_dp_abort(struct ADIv5_DP_s *dp, uint32_t abort)
 	return dp->abort(dp, abort);
 }
 
-typedef struct ADIv5_AP_s {
+struct ADIv5_AP_s {
 	int refcnt;
 
 	ADIv5_DP_t *dp;
@@ -183,25 +202,42 @@ typedef struct ADIv5_AP_s {
 	uint32_t idr;
 	uint32_t base;
 	uint32_t csw;
-} ADIv5_AP_t;
+};
+
+static inline uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+{
+	return ap->dp->ap_read(ap, addr);
+}
+
+static inline void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+{
+	return ap->dp->ap_write(ap, addr, value);
+}
+
+static inline void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src,
+								  size_t len)
+{
+	return ap->dp->mem_read(ap, dest, src, len);
+}
+static inline void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest,
+								  const void *src, size_t len, enum align align)
+{
+	return ap->dp->mem_write_sized(ap, dest, src, len, align);
+}
 
-void adiv5_dp_init(ADIv5_DP_t *dp);
 void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value);
-
+void adiv5_dp_init(ADIv5_DP_t *dp);
+void platform_adiv5_dp_defaults(ADIv5_DP_t *dp);
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel);
 void adiv5_ap_ref(ADIv5_AP_t *ap);
 void adiv5_ap_unref(ADIv5_AP_t *ap);
 
 void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
-uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr);
 
 void adiv5_jtag_dp_handler(jtag_dev_t *dev);
 int platform_jtag_dp_init(ADIv5_DP_t *dp);
 
-void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
-void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
-						   size_t len, enum align align);
 uint64_t adiv5_ap_read_pidr(ADIv5_AP_t *ap, uint32_t addr);
 void * extract(void *dest, uint32_t src, uint32_t val, enum align align);
 #endif
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 051befdf..a0abf852 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -478,88 +478,98 @@ static void cortexm_regs_read(target *t, void *data)
 	uint32_t *regs = data;
 	ADIv5_AP_t *ap = cortexm_ap(t);
 	unsigned i;
-#if defined(STLINKV2)
-	uint32_t base_regs[21];
-	extern void stlink_regs_read(ADIv5_AP_t *ap, void *data);
-	extern uint32_t stlink_reg_read(ADIv5_AP_t *ap, int idx);
-	stlink_regs_read(ap, base_regs);
-	for(i = 0; i < sizeof(regnum_cortex_m) / 4; i++)
-		*regs++ = base_regs[regnum_cortex_m[i]];
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(size_t t = 0; t < sizeof(regnum_cortex_mf) / 4; t++)
-			*regs++ = stlink_reg_read(ap, regnum_cortex_mf[t]);
-#else
-	/* FIXME: Describe what's really going on here */
-	adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
-
-	/* Map the banked data registers (0x10-0x1c) to the
-	 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
-	adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR, CORTEXM_DHCSR);
-
-	/* Walk the regnum_cortex_m array, reading the registers it
-	 * calls out. */
-	adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRSR), regnum_cortex_m[0]); /* Required to switch banks */
-	*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
-	for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
-		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
-		                    regnum_cortex_m[i]);
-		*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+#if PC_HOSTED == 1
+	if ((ap->dp->ap_reg_read) && (ap->dp->ap_regs_read)) {
+		uint32_t base_regs[21];
+		ap->dp->ap_regs_read(ap, base_regs);
+		for(i = 0; i < sizeof(regnum_cortex_m) / 4; i++)
+			*regs++ = base_regs[regnum_cortex_m[i]];
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(size_t t = 0; t < sizeof(regnum_cortex_mf) / 4; t++)
+			*regs++ = ap->dp->ap_reg_read(ap, regnum_cortex_mf[t]);
 	}
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
-			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-			                    ADIV5_AP_DB(DB_DCRSR),
-			                    regnum_cortex_mf[i]);
-			*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
-		}
+#else
+	if (0) {}
 #endif
+	else {
+		/* FIXME: Describe what's really going on here */
+		adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
+
+		/* Map the banked data registers (0x10-0x1c) to the
+		 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR,
+							CORTEXM_DHCSR);
+
+		/* Walk the regnum_cortex_m array, reading the registers it
+		 * calls out. */
+		adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRSR), regnum_cortex_m[0]);
+       /* Required to switch banks */
+		*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+		for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
+			regnum_cortex_m[i]);
+		*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+		}
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
+				adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+									ADIV5_AP_DB(DB_DCRSR),
+									regnum_cortex_mf[i]);
+				*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+			}
+		}
 }
 
 static void cortexm_regs_write(target *t, const void *data)
 {
 	const uint32_t *regs = data;
 	ADIv5_AP_t *ap = cortexm_ap(t);
-#if defined(STLINKV2)
-	extern void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val);
-	for(size_t z = 0; z < sizeof(regnum_cortex_m) / 4; z++) {
-		stlink_reg_write(ap, regnum_cortex_m[z], *regs);
-		regs++;
-	}
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(size_t z = 0; z < sizeof(regnum_cortex_mf) / 4; z++) {
-			stlink_reg_write(ap, regnum_cortex_mf[z], *regs);
+#if PC_HOSTED == 1
+	if (ap->dp->ap_reg_write) {
+		for (size_t z = 0; z < sizeof(regnum_cortex_m) / 4; z++) {
+			ap->dp->ap_reg_write(ap, regnum_cortex_m[z], *regs);
 			regs++;
+		}
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(size_t z = 0; z < sizeof(regnum_cortex_mf) / 4; z++) {
+				ap->dp->ap_reg_write(ap, regnum_cortex_mf[z], *regs);
+				regs++;
+			}
 	}
 #else
-	unsigned i;
-
-	/* FIXME: Describe what's really going on here */
-	adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
-
-	/* Map the banked data registers (0x10-0x1c) to the
-	 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
-	adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR, CORTEXM_DHCSR);
-
-	/* Walk the regnum_cortex_m array, writing the registers it
-	 * calls out. */
-	adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRDR), *regs++); /* Required to switch banks */
-	adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
-	                    0x10000 | regnum_cortex_m[0]);
-	for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
-		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-		                    ADIV5_AP_DB(DB_DCRDR), *regs++);
-		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
-		                    0x10000 | regnum_cortex_m[i]);
-	}
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
-			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-			                    ADIV5_AP_DB(DB_DCRDR), *regs++);
-			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-			                    ADIV5_AP_DB(DB_DCRSR),
-			                    0x10000 | regnum_cortex_mf[i]);
-		}
+	if (0) {}
 #endif
+	else {
+		unsigned i;
+
+		/* FIXME: Describe what's really going on here */
+		adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
+
+		/* Map the banked data registers (0x10-0x1c) to the
+		 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR,
+							CORTEXM_DHCSR);
+		/* Walk the regnum_cortex_m array, writing the registers it
+		 * calls out. */
+		adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRDR), *regs++);
+        /* Required to switch banks */
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
+							0x10000 | regnum_cortex_m[0]);
+		for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+								ADIV5_AP_DB(DB_DCRDR), *regs++);
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
+								0x10000 | regnum_cortex_m[i]);
+		}
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
+				adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+									ADIV5_AP_DB(DB_DCRDR), *regs++);
+				adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+									ADIV5_AP_DB(DB_DCRSR),
+									0x10000 | regnum_cortex_mf[i]);
+			}
+	}
 }
 
 int cortexm_mem_write_sized(