From eee669a60a0cfa471ed891a3726d7e50529671c6 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 03:56:29 +0100
Subject: [PATCH 01/25] hosted/cli: Fixed the clang-tidy warnings on naming in
 cl_execute()

---
 src/platforms/hosted/cli.c | 56 +++++++++++++++++++-------------------
 1 file changed, 28 insertions(+), 28 deletions(-)

diff --git a/src/platforms/hosted/cli.c b/src/platforms/hosted/cli.c
index 321be131336..b4b1a059466 100644
--- a/src/platforms/hosted/cli.c
+++ b/src/platforms/hosted/cli.c
@@ -459,11 +459,11 @@ int cl_execute(bmda_cli_options_s *opt)
 		DEBUG_ERROR("Given target number %" PRIu32 " not available max %zu\n", opt->opt_target_dev, num_targets);
 		return -1;
 	}
-	target_s *t = target_attach_n(opt->opt_target_dev, &cl_controller);
+	target_s *target = target_attach_n(opt->opt_target_dev, &cl_controller);
 
 	int res = 0;
 	int read_file = -1;
-	if (!t) {
+	if (!target) {
 		DEBUG_ERROR("Can not attach to target %" PRIu32 "\n", opt->opt_target_dev);
 		res = -1;
 		goto target_detach;
@@ -471,14 +471,14 @@ int cl_execute(bmda_cli_options_s *opt)
 
 	/* List each defined RAM region */
 	size_t ram_regions = 0;
-	for (target_ram_s *r = t->ram; r; r = r->next)
+	for (target_ram_s *ram = target->ram; ram; ram = ram->next)
 		++ram_regions;
 
 	for (size_t region = 0; region < ram_regions; ++region) {
-		target_ram_s *r = t->ram;
-		for (size_t i = ram_regions - 1U; r; r = r->next, --i) {
+		target_ram_s *ram = target->ram;
+		for (size_t i = ram_regions - 1U; ram; ram = ram->next, --i) {
 			if (region == i) {
-				DEBUG_INFO("RAM   Start: 0x%08" PRIx32 " length = 0x%zx\n", r->start, r->length);
+				DEBUG_INFO("RAM   Start: 0x%08" PRIx32 " length = 0x%zx\n", ram->start, ram->length);
 				break;
 			}
 		}
@@ -490,20 +490,20 @@ int cl_execute(bmda_cli_options_s *opt)
 	size_t lowest_flash_size = 0;
 
 	size_t flash_regions = 0;
-	for (target_flash_s *f = t->flash; f; f = f->next) {
+	for (target_flash_s *flash = target->flash; flash; flash = flash->next) {
 		++flash_regions;
-		if (f->start < lowest_flash_start) {
-			lowest_flash_start = f->start;
-			lowest_flash_size = f->length;
+		if (flash->start < lowest_flash_start) {
+			lowest_flash_start = flash->start;
+			lowest_flash_size = flash->length;
 		}
 	}
 
 	for (size_t region = 0; region < flash_regions; ++region) {
-		target_flash_s *f = t->flash;
-		for (size_t i = flash_regions - 1U; f; f = f->next, --i) {
+		target_flash_s *flash = target->flash;
+		for (size_t i = flash_regions - 1U; flash; flash = flash->next, --i) {
 			if (region == i) {
-				DEBUG_INFO(
-					"Flash Start: 0x%08" PRIx32 " length = 0x%zx blocksize 0x%zx\n", f->start, f->length, f->blocksize);
+				DEBUG_INFO("Flash Start: 0x%08" PRIx32 " length = 0x%zx blocksize 0x%zx\n", flash->start, flash->length,
+					flash->blocksize);
 				break;
 			}
 		}
@@ -519,12 +519,12 @@ int cl_execute(bmda_cli_options_s *opt)
 		 * XXX: This is bugprone - any core, even if it's not just a Cortex-M* that
 		 * matches on the first letter triggers this
 		 */
-		if (t->core[0] == 'M') {
+		if (target->core[0] == 'M') {
 			DEBUG_WARN("Continuous read/write-back DEMCR. Abort with ^C\n");
 			while (true) {
 				uint32_t demcr;
-				target_mem_read(t, &demcr, CORTEXM_DEMCR, 4);
-				target_mem_write32(t, CORTEXM_DEMCR, demcr);
+				target_mem_read(target, &demcr, CORTEXM_DEMCR, 4);
+				target_mem_write32(target, CORTEXM_DEMCR, demcr);
 				platform_delay(1); /* To allow trigger */
 			}
 		} else
@@ -554,31 +554,31 @@ int cl_execute(bmda_cli_options_s *opt)
 		/* restrict to size given on command line */
 		map.size = opt->opt_flash_size;
 	if (opt->opt_monitor) {
-		res = command_process(t, opt->opt_monitor);
+		res = command_process(target, opt->opt_monitor);
 		if (res)
 			DEBUG_ERROR("Command \"%s\" failed\n", opt->opt_monitor);
 	}
 	if (opt->opt_mode == BMP_MODE_RESET)
-		target_reset(t);
+		target_reset(target);
 	else if (opt->opt_mode == BMP_MODE_FLASH_ERASE) {
 		DEBUG_INFO("Erase %zu bytes at 0x%08" PRIx32 "\n", opt->opt_flash_size, opt->opt_flash_start);
-		if (!target_flash_erase(t, opt->opt_flash_start, opt->opt_flash_size)) {
+		if (!target_flash_erase(target, opt->opt_flash_start, opt->opt_flash_size)) {
 			DEBUG_ERROR("Flash erase failed!\n");
 			res = -1;
 			goto free_map;
 		}
-		target_reset(t);
+		target_reset(target);
 	} else if (opt->opt_mode == BMP_MODE_FLASH_WRITE || opt->opt_mode == BMP_MODE_FLASH_WRITE_VERIFY) {
 		DEBUG_INFO("Erasing %zu bytes at 0x%08" PRIx32 "\n", map.size, opt->opt_flash_start);
 		const uint32_t start_time = platform_time_ms();
-		if (!target_flash_erase(t, opt->opt_flash_start, map.size)) {
+		if (!target_flash_erase(target, opt->opt_flash_start, map.size)) {
 			DEBUG_ERROR("Flash erase failed!\n");
 			res = -1;
 			goto free_map;
 		}
 		DEBUG_INFO("Flashing %zu bytes at 0x%08" PRIx32 "\n", map.size, opt->opt_flash_start);
 		/* Buffered write cares for padding*/
-		if (!target_flash_write(t, opt->opt_flash_start, map.data, map.size) || !target_flash_complete(t)) {
+		if (!target_flash_write(target, opt->opt_flash_start, map.data, map.size) || !target_flash_complete(target)) {
 			DEBUG_ERROR("Flashing failed!\n");
 			res = -1;
 			goto free_map;
@@ -588,7 +588,7 @@ int cl_execute(bmda_cli_options_s *opt)
 		DEBUG_WARN(
 			"Flash Write succeeded for %zu bytes, %8.3fkiB/s\n", map.size, (double)map.size / (end_time - start_time));
 		if (opt->opt_mode != BMP_MODE_FLASH_WRITE_VERIFY) {
-			target_reset(t);
+			target_reset(target);
 			goto free_map;
 		}
 	}
@@ -606,7 +606,7 @@ int cl_execute(bmda_cli_options_s *opt)
 		const uint32_t start_time = platform_time_ms();
 		for (size_t offset = 0; offset < size; offset += WORKSIZE) {
 			const size_t worksize = MIN(size - offset, WORKSIZE);
-			int n_read = target_mem_read(t, data, flash_src + offset, worksize);
+			int n_read = target_mem_read(target, data, flash_src + offset, worksize);
 			if (n_read) {
 				if (opt->opt_flash_size == 0) /* we reached end of flash */
 					DEBUG_INFO("Reached end of flash at size %" PRIu32 "\n", flash_src - opt->opt_flash_start);
@@ -642,7 +642,7 @@ int cl_execute(bmda_cli_options_s *opt)
 		DEBUG_WARN("Read/Verify succeeded for %zu bytes, %8.3fkiB/s\n", bytes_read,
 			(double)bytes_read / (end_time - start_time));
 		if (opt->opt_mode == BMP_MODE_FLASH_WRITE_VERIFY)
-			target_reset(t);
+			target_reset(target);
 	}
 free_map:
 	if (map.size)
@@ -650,8 +650,8 @@ int cl_execute(bmda_cli_options_s *opt)
 target_detach:
 	if (read_file != -1)
 		close(read_file);
-	if (t)
-		target_detach(t);
+	if (target)
+		target_detach(target);
 	target_list_free();
 	return res;
 }

From aa30de15cb3dbea97868ab94bd20f9217e4034fb Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 04:04:28 +0100
Subject: [PATCH 02/25] target_internal: Reduced the padding requirements on
 the target structure, per the advice of clang-tidy

---
 src/target/target_internal.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/target/target_internal.h b/src/target/target_internal.h
index 12ba49ef00f..f78cf00d300 100644
--- a/src/target/target_internal.h
+++ b/src/target/target_internal.h
@@ -100,7 +100,6 @@ struct breakwatch {
 #define MAX_CMDLINE 81
 
 struct target {
-	bool attached;
 	target_controller_s *tc;
 
 	/* Attach/Detach functions */
@@ -141,7 +140,7 @@ struct target {
 	bool flash_mode;
 
 	/* Target-defined options */
-	unsigned target_options;
+	uint32_t target_options;
 
 	void *target_storage;
 
@@ -176,6 +175,8 @@ struct target {
 	 * fallback to AP partno
 	 */
 	uint16_t part_id;
+
+	bool attached;
 };
 
 void target_print_progress(platform_timeout_s *timeout);

From 2048f8a57d25343e0716f087b2b15fa743f99e34 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 05:51:10 +0100
Subject: [PATCH 03/25] hosted/stlinkv2: Cleaned up the naming of
 stlink_adiv5_clear_error() and added protocol-level debug information to the
 function

---
 src/platforms/hosted/stlinkv2.c          | 3 ++-
 src/platforms/hosted/stlinkv2_jtag.c     | 2 +-
 src/platforms/hosted/stlinkv2_protocol.h | 2 +-
 src/platforms/hosted/stlinkv2_swd.c      | 2 +-
 4 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index e95e3dda3f8..8c8ac00e81d 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -504,8 +504,9 @@ int stlink_hwversion(void)
 	return stlink.ver_stlink;
 }
 
-uint32_t stlink_dp_error(adiv5_debug_port_s *dp, const bool protocol_recovery)
+uint32_t stlink_adiv5_clear_error(adiv5_debug_port_s *const dp, const bool protocol_recovery)
 {
+	DEBUG_PROBE("%s (protocol recovery: %s)\n", __func__, protocol_recovery ? "true" : "false");
 	if ((dp->version >= 2 && dp->fault) || protocol_recovery) {
 		/*
 		 * Note that on DPv2+ devices, during a protocol error condition
diff --git a/src/platforms/hosted/stlinkv2_jtag.c b/src/platforms/hosted/stlinkv2_jtag.c
index 4178b89e1f2..a8a8dcf9c33 100644
--- a/src/platforms/hosted/stlinkv2_jtag.c
+++ b/src/platforms/hosted/stlinkv2_jtag.c
@@ -76,7 +76,7 @@ static size_t stlink_read_idcodes(uint32_t *idcodes)
 
 void stlink_jtag_dp_init(adiv5_debug_port_s *dp)
 {
-	dp->error = stlink_dp_error;
+	dp->error = stlink_adiv5_clear_error;
 	dp->low_access = stlink_raw_access;
 	dp->abort = stlink_dp_abort;
 }
diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index 3635af10d4a..cae97a79092 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -236,7 +236,7 @@ bool stlink_leave_state(void);
 int stlink_usb_error_check(uint8_t *data, bool verbose);
 
 uint32_t stlink_raw_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, uint32_t value);
-uint32_t stlink_dp_error(adiv5_debug_port_s *dp, bool protocol_recovery);
+uint32_t stlink_adiv5_clear_error(adiv5_debug_port_s *dp, bool protocol_recovery);
 void stlink_dp_abort(adiv5_debug_port_s *dp, uint32_t abort);
 
 #endif /*PLATFORMS_HOSTED_STLINKV2_PROTOCOL_H*/
diff --git a/src/platforms/hosted/stlinkv2_swd.c b/src/platforms/hosted/stlinkv2_swd.c
index 621a7279302..9c4f6a5b239 100644
--- a/src/platforms/hosted/stlinkv2_swd.c
+++ b/src/platforms/hosted/stlinkv2_swd.c
@@ -47,7 +47,7 @@ bool stlink_swd_scan(void)
 	}
 
 	dp->dp_read = firmware_swdp_read;
-	dp->error = stlink_dp_error;
+	dp->error = stlink_adiv5_clear_error;
 	dp->low_access = stlink_raw_access;
 	dp->abort = stlink_dp_abort;
 

From 7b08293caaccf1ec782f4ae4f7ff9aa1b6c12463 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 05:53:33 +0100
Subject: [PATCH 04/25] hosted/stlinkv2: Added protocol-level debugging to
 stlink_raw_access() and proper handling for faulting accesses

---
 src/platforms/hosted/stlinkv2.c | 23 +++++++++++++++++------
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 8c8ac00e81d..76e4d3bc84b 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -585,11 +585,12 @@ static int stlink_write_dp_register(const uint16_t apsel, const uint16_t address
 	return stlink_usb_error_check(data, true);
 }
 
-uint32_t stlink_raw_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, uint32_t value)
+uint32_t stlink_raw_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, uint32_t request_value)
 {
-	uint32_t response = 0;
-	const int result = rnw ? stlink_read_dp_register(addr < 0x100U ? STLINK_DEBUG_PORT : 0U, addr, &response) :
-							 stlink_write_dp_register(addr < 0x100U ? STLINK_DEBUG_PORT : 0U, addr, value);
+	DEBUG_PROBE("%s: Attempting access to addr %04x\n", __func__, addr);
+	uint32_t result_value = 0;
+	const int result = rnw ? stlink_read_dp_register(addr < 0x100U ? STLINK_DEBUG_PORT : 0U, addr, &result_value) :
+							 stlink_write_dp_register(addr < 0x100U ? STLINK_DEBUG_PORT : 0U, addr, request_value);
 
 	if (result == STLINK_ERROR_WAIT) {
 		DEBUG_ERROR("SWD access resulted in wait, aborting\n");
@@ -599,13 +600,23 @@ uint32_t stlink_raw_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, u
 
 	if (result == STLINK_ERROR_DP_FAULT || result == STLINK_ERROR_AP_FAULT) {
 		DEBUG_ERROR("SWD access resulted in fault\n");
+		/* On fault, abort the request */
+		stlink_write_dp_register(STLINK_DEBUG_PORT, ADIV5_DP_ABORT,
+			ADIV5_DP_ABORT_ORUNERRCLR | ADIV5_DP_ABORT_WDERRCLR | ADIV5_DP_ABORT_STKERRCLR | ADIV5_DP_ABORT_STKCMPCLR);
 		dp->fault = SWDP_ACK_FAULT;
 		return 0;
 	}
 
-	if (result == STLINK_ERROR_FAIL)
+	if (result == STLINK_ERROR_FAIL) {
+		DEBUG_ERROR("SWD access has invalid ack %x\n", result);
 		raise_exception(EXCEPTION_ERROR, "SWD invalid ACK");
-	return response;
+	}
+
+	if (rnw)
+		DEBUG_PROBE("%s: addr %04x -> %08" PRIx32 "\n", __func__, addr, result_value);
+	else
+		DEBUG_PROBE("%s: addr %04x <- %08" PRIx32 "\n", __func__, addr, request_value);
+	return result_value;
 }
 
 static bool stlink_ap_setup(const uint8_t ap)

From 7454bec15b33fff80adb193510969b3b0e840489 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 05:57:40 +0100
Subject: [PATCH 05/25] hosted/stlinkv2: Moved all the stlink_usb_error_check()
 output definitions into one spot

---
 src/platforms/hosted/stlinkv2.c | 3 ---
 src/platforms/hosted/stlinkv2.h | 8 +++++---
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 76e4d3bc84b..04efdab7111 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -71,9 +71,6 @@ typedef struct stlink {
 	bool ap_error;
 } stlink_s;
 
-#define STLINK_ERROR_DP_FAULT (-2)
-#define STLINK_ERROR_AP_FAULT (-3)
-
 #define STLINK_V2_CPU_CLOCK_FREQ      (72U * 1000U * 1000U)
 #define STLINK_V2_JTAG_MUL_FACTOR     2U
 #define STLINK_V2_MAX_JTAG_CLOCK_FREQ (9U * 1000U * 1000U)
diff --git a/src/platforms/hosted/stlinkv2.h b/src/platforms/hosted/stlinkv2.h
index aea2a116ab5..73f8c62ec97 100644
--- a/src/platforms/hosted/stlinkv2.h
+++ b/src/platforms/hosted/stlinkv2.h
@@ -24,9 +24,11 @@
 
 #include "bmp_hosted.h"
 
-#define STLINK_ERROR_FAIL (-1)
-#define STLINK_ERROR_OK   0
-#define STLINK_ERROR_WAIT 1
+#define STLINK_ERROR_OK       0
+#define STLINK_ERROR_WAIT     1
+#define STLINK_ERROR_FAIL     (-1)
+#define STLINK_ERROR_DP_FAULT (-2)
+#define STLINK_ERROR_AP_FAULT (-3)
 
 bool stlink_init(void);
 bool stlink_swd_scan(void);

From 4275ad7f82f46c563c8810e0c04c2dbae96d9363 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 06:07:00 +0100
Subject: [PATCH 06/25] hosted/stlinkv2: Fixed some of the error checking done
 with stlink_usb_error_check()

---
 src/platforms/hosted/stlinkv2_jtag.c | 10 +++++-----
 src/platforms/hosted/stlinkv2_swd.c  |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2_jtag.c b/src/platforms/hosted/stlinkv2_jtag.c
index a8a8dcf9c33..b15cd5d64e5 100644
--- a/src/platforms/hosted/stlinkv2_jtag.c
+++ b/src/platforms/hosted/stlinkv2_jtag.c
@@ -25,7 +25,7 @@
 
 #define STLINK_JTAG_MAX_DEVS 2U
 
-static int stlink_enter_debug_jtag(void);
+static bool stlink_enter_debug_jtag(void);
 static size_t stlink_read_idcodes(uint32_t *idcodes);
 
 bool stlink_jtag_scan(void)
@@ -35,7 +35,7 @@ bool stlink_jtag_scan(void)
 
 	jtag_dev_count = 0;
 	memset(jtag_devs, 0, sizeof(jtag_devs));
-	if (stlink_enter_debug_jtag())
+	if (!stlink_enter_debug_jtag())
 		return false;
 	jtag_dev_count = stlink_read_idcodes(idcodes);
 	/* Check for known devices and handle accordingly */
@@ -54,20 +54,20 @@ bool stlink_jtag_scan(void)
 	return jtag_dev_count > 0;
 }
 
-static int stlink_enter_debug_jtag(void)
+static bool stlink_enter_debug_jtag(void)
 {
 	stlink_leave_state();
 	uint8_t data[2];
 	stlink_simple_request(
 		STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_ENTER, STLINK_DEBUG_ENTER_JTAG_NO_RESET, data, sizeof(data));
-	return stlink_usb_error_check(data, true);
+	return stlink_usb_error_check(data, true) == STLINK_ERROR_OK;
 }
 
 static size_t stlink_read_idcodes(uint32_t *idcodes)
 {
 	uint8_t data[12];
 	stlink_simple_query(STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READ_IDCODES, data, sizeof(data));
-	if (stlink_usb_error_check(data, true))
+	if (stlink_usb_error_check(data, true) != STLINK_ERROR_OK)
 		return 0;
 	idcodes[0] = data[4] | (data[5] << 8U) | (data[6] << 16U) | (data[7] << 24U);
 	idcodes[1] = data[8] | (data[9] << 8U) | (data[10] << 16U) | (data[11] << 24U);
diff --git a/src/platforms/hosted/stlinkv2_swd.c b/src/platforms/hosted/stlinkv2_swd.c
index 9c4f6a5b239..11b48114a3d 100644
--- a/src/platforms/hosted/stlinkv2_swd.c
+++ b/src/platforms/hosted/stlinkv2_swd.c
@@ -37,7 +37,7 @@ bool stlink_swd_scan(void)
 	};
 	uint8_t data[2];
 	stlink_send_recv_retry(&command, sizeof(command), data, sizeof(data));
-	if (stlink_usb_error_check(data, true))
+	if (stlink_usb_error_check(data, true) != STLINK_ERROR_OK)
 		return false;
 
 	adiv5_debug_port_s *dp = calloc(1, sizeof(*dp));

From b56aaa468a991759b13694c1496f057a756435b6 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 06:24:00 +0100
Subject: [PATCH 07/25] hosted/stlinkv2: Cleaned up the WAIT response handling

---
 src/platforms/hosted/stlinkv2.c          | 9 +++------
 src/platforms/hosted/stlinkv2_protocol.h | 4 ++--
 2 files changed, 5 insertions(+), 8 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 04efdab7111..1e936caed3e 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -132,13 +132,10 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("Failure powering DBG\n");
 		return STLINK_ERROR_WAIT;
-	case STLINK_SWD_AP_WAIT:
+	case STLINK_ERROR_AP_WAIT:
+	case STLINK_ERROR_DP_WAIT:
 		if (verbose)
-			DEBUG_WARN("Wait status SWD_AP_WAIT (0x%x)\n", STLINK_SWD_AP_WAIT);
-		return STLINK_ERROR_WAIT;
-	case STLINK_SWD_DP_WAIT:
-		if (verbose)
-			DEBUG_WARN("Wait status SWD_DP_WAIT (0x%x)\n", STLINK_SWD_DP_WAIT);
+			DEBUG_WARN("%s reported wait\n", data[0] == STLINK_ERROR_AP_WAIT ? "AP" : "DP");
 		return STLINK_ERROR_WAIT;
 	case STLINK_JTAG_WRITE_ERROR:
 		if (verbose)
diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index cae97a79092..54711172629 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -36,13 +36,13 @@
 #define STLINK_JTAG_COMMAND_ERROR      0x08U
 #define STLINK_JTAG_GET_IDCODE_ERROR   0x09U
 #define STLINK_JTAG_DBG_POWER_ERROR    0x0bU
-#define STLINK_SWD_AP_WAIT             0x10U
+#define STLINK_ERROR_AP_WAIT           0x10U
 #define STLINK_SWD_AP_FAULT            0x11U
 #define STLINK_SWD_AP_ERROR            0x12U
 #define STLINK_SWD_AP_PARITY_ERROR     0x13U
 #define STLINK_JTAG_WRITE_ERROR        0x0cU
 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0dU
-#define STLINK_SWD_DP_WAIT             0x14U
+#define STLINK_ERROR_DP_WAIT           0x14U
 #define STLINK_SWD_DP_FAULT            0x15U
 #define STLINK_SWD_DP_ERROR            0x16U
 #define STLINK_SWD_DP_PARITY_ERROR     0x17U

From 8ce6196d57a9d0fc2edd9726bf952aa44baf80dc Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 06:28:07 +0100
Subject: [PATCH 08/25] hosted/stlinkv2: Cleaned up the FAULT response
 handling, fixing a bug in the process

---
 src/platforms/hosted/stlinkv2.c          | 26 ++++++++----------------
 src/platforms/hosted/stlinkv2.h          | 12 ++++++-----
 src/platforms/hosted/stlinkv2_protocol.h |  4 ++--
 3 files changed, 18 insertions(+), 24 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 1e936caed3e..027d1095026 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -109,9 +109,15 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 	case STLINK_DEBUG_ERR_OK:
 		return STLINK_ERROR_OK;
 	case STLINK_DEBUG_ERR_FAULT:
+	case STLINK_ERROR_AP_FAULT:
+	case STLINK_ERROR_DP_FAULT:
+		stlink.ap_error |= data[0] == STLINK_ERROR_AP_FAULT;
 		if (verbose)
-			DEBUG_ERROR("SWD fault response (0x%x)\n", STLINK_DEBUG_ERR_FAULT);
-		return STLINK_ERROR_FAIL;
+			DEBUG_ERROR("%s reported fault\n",
+				data[0] == STLINK_DEBUG_ERR_FAULT    ? "Adaptor" :
+					data[0] == STLINK_ERROR_AP_FAULT ? "AP" :
+													   "DP");
+		return STLINK_ERROR_FAULT;
 	case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
 		if (verbose)
 			DEBUG_ERROR("Unknown JTAG chain\n");
@@ -145,16 +151,6 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("Write verify error, ignoring\n");
 		return STLINK_ERROR_OK;
-	case STLINK_SWD_AP_FAULT:
-		/* git://git.ac6.fr/openocd commit 657e3e885b9ee10
-			 * returns STLINK_ERROR_OK with the comment:
-			 * Change in error status when reading outside RAM.
-			 * This fix allows CDT plugin to visualize memory.
-			 */
-		stlink.ap_error = true;
-		if (verbose)
-			DEBUG_ERROR("STLINK_SWD_AP_FAULT\n");
-		return STLINK_ERROR_AP_FAULT;
 	case STLINK_SWD_AP_ERROR:
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_ERROR\n");
@@ -163,10 +159,6 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_PARITY_ERROR\n");
 		return STLINK_ERROR_FAIL;
-	case STLINK_SWD_DP_FAULT:
-		if (verbose)
-			DEBUG_ERROR("STLINK_SWD_DP_FAULT\n");
-		return STLINK_ERROR_FAIL;
 	case STLINK_SWD_DP_ERROR:
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_DP_ERROR\n");
@@ -592,7 +584,7 @@ uint32_t stlink_raw_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, u
 		return 0;
 	}
 
-	if (result == STLINK_ERROR_DP_FAULT || result == STLINK_ERROR_AP_FAULT) {
+	if (result == STLINK_ERROR_FAULT) {
 		DEBUG_ERROR("SWD access resulted in fault\n");
 		/* On fault, abort the request */
 		stlink_write_dp_register(STLINK_DEBUG_PORT, ADIV5_DP_ABORT,
diff --git a/src/platforms/hosted/stlinkv2.h b/src/platforms/hosted/stlinkv2.h
index 73f8c62ec97..b53c7c96f2a 100644
--- a/src/platforms/hosted/stlinkv2.h
+++ b/src/platforms/hosted/stlinkv2.h
@@ -24,11 +24,13 @@
 
 #include "bmp_hosted.h"
 
-#define STLINK_ERROR_OK       0
-#define STLINK_ERROR_WAIT     1
-#define STLINK_ERROR_FAIL     (-1)
-#define STLINK_ERROR_DP_FAULT (-2)
-#define STLINK_ERROR_AP_FAULT (-3)
+#define STLINK_ERROR_OK          0U
+#define STLINK_ERROR_WAIT        1U
+#define STLINK_ERROR_FAULT       2U
+#define STLINK_ERROR_PARITY      3U
+#define STLINK_ERROR_GENERAL     4U
+#define STLINK_ERROR_NO_RESPONSE 7U
+#define STLINK_ERROR_FAIL        (-1)
 
 bool stlink_init(void);
 bool stlink_swd_scan(void);
diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index 54711172629..df721048149 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -37,13 +37,13 @@
 #define STLINK_JTAG_GET_IDCODE_ERROR   0x09U
 #define STLINK_JTAG_DBG_POWER_ERROR    0x0bU
 #define STLINK_ERROR_AP_WAIT           0x10U
-#define STLINK_SWD_AP_FAULT            0x11U
+#define STLINK_ERROR_AP_FAULT          0x11U
 #define STLINK_SWD_AP_ERROR            0x12U
 #define STLINK_SWD_AP_PARITY_ERROR     0x13U
 #define STLINK_JTAG_WRITE_ERROR        0x0cU
 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0dU
 #define STLINK_ERROR_DP_WAIT           0x14U
-#define STLINK_SWD_DP_FAULT            0x15U
+#define STLINK_ERROR_DP_FAULT          0x15U
 #define STLINK_SWD_DP_ERROR            0x16U
 #define STLINK_SWD_DP_PARITY_ERROR     0x17U
 

From a99ed70cbd43441631d69671f9fc38bde0a2ab33 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 06:29:02 +0100
Subject: [PATCH 09/25] hosted/stlinkv2: Reordered the checks in
 stlink_usb_error_check() so they start to make more sense

---
 src/platforms/hosted/stlinkv2.c | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 027d1095026..ec48a714abb 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -108,6 +108,11 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 	switch (data[0]) {
 	case STLINK_DEBUG_ERR_OK:
 		return STLINK_ERROR_OK;
+	case STLINK_ERROR_AP_WAIT:
+	case STLINK_ERROR_DP_WAIT:
+		if (verbose)
+			DEBUG_WARN("%s reported wait\n", data[0] == STLINK_ERROR_AP_WAIT ? "AP" : "DP");
+		return STLINK_ERROR_WAIT;
 	case STLINK_DEBUG_ERR_FAULT:
 	case STLINK_ERROR_AP_FAULT:
 	case STLINK_ERROR_DP_FAULT:
@@ -138,11 +143,6 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("Failure powering DBG\n");
 		return STLINK_ERROR_WAIT;
-	case STLINK_ERROR_AP_WAIT:
-	case STLINK_ERROR_DP_WAIT:
-		if (verbose)
-			DEBUG_WARN("%s reported wait\n", data[0] == STLINK_ERROR_AP_WAIT ? "AP" : "DP");
-		return STLINK_ERROR_WAIT;
 	case STLINK_JTAG_WRITE_ERROR:
 		if (verbose)
 			DEBUG_ERROR("Write error\n");

From 928ea42c7f3249a3f1b01ce4716710f6ee9289e7 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 06:37:33 +0100
Subject: [PATCH 10/25] hosted/stlinkv2: Cleaned up the parity error response
 handling

---
 src/platforms/hosted/stlinkv2.c          | 17 +++++++++--------
 src/platforms/hosted/stlinkv2_protocol.h |  4 ++--
 2 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index ec48a714abb..e6215fb09bf 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -108,11 +108,13 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 	switch (data[0]) {
 	case STLINK_DEBUG_ERR_OK:
 		return STLINK_ERROR_OK;
+
 	case STLINK_ERROR_AP_WAIT:
 	case STLINK_ERROR_DP_WAIT:
 		if (verbose)
 			DEBUG_WARN("%s reported wait\n", data[0] == STLINK_ERROR_AP_WAIT ? "AP" : "DP");
 		return STLINK_ERROR_WAIT;
+
 	case STLINK_DEBUG_ERR_FAULT:
 	case STLINK_ERROR_AP_FAULT:
 	case STLINK_ERROR_DP_FAULT:
@@ -123,6 +125,13 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 					data[0] == STLINK_ERROR_AP_FAULT ? "AP" :
 													   "DP");
 		return STLINK_ERROR_FAULT;
+
+	case STLINK_ERROR_AP_PARITY:
+	case STLINK_ERROR_DP_PARITY:
+		if (verbose)
+			DEBUG_ERROR("Parity error on %s\n", data[0] == STLINK_ERROR_AP_PARITY ? "AP" : "DP");
+		return STLINK_ERROR_PARITY;
+
 	case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
 		if (verbose)
 			DEBUG_ERROR("Unknown JTAG chain\n");
@@ -155,19 +164,11 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_ERROR\n");
 		return STLINK_ERROR_FAIL;
-	case STLINK_SWD_AP_PARITY_ERROR:
-		if (verbose)
-			DEBUG_ERROR("STLINK_SWD_AP_PARITY_ERROR\n");
-		return STLINK_ERROR_FAIL;
 	case STLINK_SWD_DP_ERROR:
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_DP_ERROR\n");
 		raise_exception(EXCEPTION_ERROR, "STLINK_SWD_DP_ERROR");
 		return STLINK_ERROR_FAIL;
-	case STLINK_SWD_DP_PARITY_ERROR:
-		if (verbose)
-			DEBUG_ERROR("STLINK_SWD_DP_PARITY_ERROR\n");
-		return STLINK_ERROR_FAIL;
 	case STLINK_SWD_AP_WDATA_ERROR:
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_WDATA_ERROR\n");
diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index df721048149..164649e2c34 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -39,13 +39,13 @@
 #define STLINK_ERROR_AP_WAIT           0x10U
 #define STLINK_ERROR_AP_FAULT          0x11U
 #define STLINK_SWD_AP_ERROR            0x12U
-#define STLINK_SWD_AP_PARITY_ERROR     0x13U
+#define STLINK_ERROR_AP_PARITY         0x13U
 #define STLINK_JTAG_WRITE_ERROR        0x0cU
 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0dU
 #define STLINK_ERROR_DP_WAIT           0x14U
 #define STLINK_ERROR_DP_FAULT          0x15U
 #define STLINK_SWD_DP_ERROR            0x16U
-#define STLINK_SWD_DP_PARITY_ERROR     0x17U
+#define STLINK_ERROR_DP_PARITY         0x17U
 
 #define STLINK_SWD_AP_WDATA_ERROR      0x18U
 #define STLINK_SWD_AP_STICKY_ERROR     0x19U

From dbfd210378159906b873f8fb0525855c1e12f5e4 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 06:42:03 +0100
Subject: [PATCH 11/25] hosted/stlinkv2: Fixed the parity error handling in
 stlink_read_dp_register()

---
 src/platforms/hosted/stlinkv2.c | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index e6215fb09bf..6a097ee299d 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -532,7 +532,8 @@ void stlink_dp_abort(adiv5_debug_port_s *dp, uint32_t abort)
 	adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
 }
 
-static int stlink_read_dp_register(const uint16_t apsel, const uint16_t address, uint32_t *const reg)
+static int stlink_read_dp_register(
+	adiv5_debug_port_s *const dp, const uint16_t apsel, const uint16_t address, uint32_t *const reg)
 {
 	stlink_adiv5_reg_read_s request = {
 		.command = STLINK_DEBUG_COMMAND,
@@ -547,7 +548,11 @@ static int stlink_read_dp_register(const uint16_t apsel, const uint16_t address,
 	int result = stlink_send_recv_retry(&request, sizeof(request), data, sizeof(data));
 	if (result == STLINK_ERROR_OK)
 		*reg = read_le4(data, 4U);
-	else
+	else if (result == STLINK_ERROR_PARITY) {
+		dp->fault = 1;
+		DEBUG_ERROR("SWD access resulted in parity error\n");
+		raise_exception(EXCEPTION_ERROR, "SWD parity error");
+	} else
 		DEBUG_ERROR("%s error %d\n", __func__, result);
 	return result;
 }
@@ -576,7 +581,7 @@ uint32_t stlink_raw_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, u
 {
 	DEBUG_PROBE("%s: Attempting access to addr %04x\n", __func__, addr);
 	uint32_t result_value = 0;
-	const int result = rnw ? stlink_read_dp_register(addr < 0x100U ? STLINK_DEBUG_PORT : 0U, addr, &result_value) :
+	const int result = rnw ? stlink_read_dp_register(dp, addr < 0x100U ? STLINK_DEBUG_PORT : 0U, addr, &result_value) :
 							 stlink_write_dp_register(addr < 0x100U ? STLINK_DEBUG_PORT : 0U, addr, request_value);
 
 	if (result == STLINK_ERROR_WAIT) {
@@ -763,7 +768,7 @@ static void stlink_ap_write(adiv5_access_port_s *ap, uint16_t addr, uint32_t val
 static uint32_t stlink_ap_read(adiv5_access_port_s *ap, uint16_t addr)
 {
 	uint32_t ret = 0;
-	stlink_read_dp_register(ap->apsel, addr, &ret);
+	stlink_read_dp_register(ap->dp, ap->apsel, addr, &ret);
 	return ret;
 }
 

From 325e2a63ba9989712518d4f23d9467471982fe18 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 07:07:47 +0100
Subject: [PATCH 12/25] hosted/stlinkv2: Cleaned up the general error response
 handling

---
 src/platforms/hosted/stlinkv2.c          | 16 +++++++---------
 src/platforms/hosted/stlinkv2_protocol.h |  4 ++--
 2 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 6a097ee299d..7d4b6a0749c 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -132,6 +132,13 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 			DEBUG_ERROR("Parity error on %s\n", data[0] == STLINK_ERROR_AP_PARITY ? "AP" : "DP");
 		return STLINK_ERROR_PARITY;
 
+	case STLINK_ERROR_AP:
+	case STLINK_ERROR_DP:
+		if (verbose)
+			DEBUG_ERROR("%s reported error\n", data[0] == STLINK_ERROR_AP ? "AP" : "DP");
+		raise_exception(EXCEPTION_ERROR, "ST-Link error");
+		return STLINK_ERROR_GENERAL;
+
 	case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
 		if (verbose)
 			DEBUG_ERROR("Unknown JTAG chain\n");
@@ -160,15 +167,6 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("Write verify error, ignoring\n");
 		return STLINK_ERROR_OK;
-	case STLINK_SWD_AP_ERROR:
-		if (verbose)
-			DEBUG_ERROR("STLINK_SWD_AP_ERROR\n");
-		return STLINK_ERROR_FAIL;
-	case STLINK_SWD_DP_ERROR:
-		if (verbose)
-			DEBUG_ERROR("STLINK_SWD_DP_ERROR\n");
-		raise_exception(EXCEPTION_ERROR, "STLINK_SWD_DP_ERROR");
-		return STLINK_ERROR_FAIL;
 	case STLINK_SWD_AP_WDATA_ERROR:
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_WDATA_ERROR\n");
diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index 164649e2c34..f8332396a96 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -38,13 +38,13 @@
 #define STLINK_JTAG_DBG_POWER_ERROR    0x0bU
 #define STLINK_ERROR_AP_WAIT           0x10U
 #define STLINK_ERROR_AP_FAULT          0x11U
-#define STLINK_SWD_AP_ERROR            0x12U
+#define STLINK_ERROR_AP                0x12U
 #define STLINK_ERROR_AP_PARITY         0x13U
 #define STLINK_JTAG_WRITE_ERROR        0x0cU
 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0dU
 #define STLINK_ERROR_DP_WAIT           0x14U
 #define STLINK_ERROR_DP_FAULT          0x15U
-#define STLINK_SWD_DP_ERROR            0x16U
+#define STLINK_ERROR_DP                0x16U
 #define STLINK_ERROR_DP_PARITY         0x17U
 
 #define STLINK_SWD_AP_WDATA_ERROR      0x18U

From 22620ec35fb20e249b8ee326088ba91105d6f71c Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 07:09:53 +0100
Subject: [PATCH 13/25] hosted/stlinkv2_protocol: Cleaned up duplicated an
 unused error codes

---
 src/platforms/hosted/stlinkv2_protocol.h | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index f8332396a96..ead9e44b917 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -26,14 +26,11 @@
 #include <stdbool.h>
 #include "adiv5.h"
 
-#define STLINK_SWIM_ERR_OK             0x00U
-#define STLINK_SWIM_BUSY               0x01U
 #define STLINK_DEBUG_ERR_OK            0x80U
 #define STLINK_DEBUG_ERR_FAULT         0x81U
 #define STLINK_JTAG_UNKNOWN_JTAG_CHAIN 0x04U
 #define STLINK_NO_DEVICE_CONNECTED     0x05U
 #define STLINK_JTAG_COMMAND_ERROR      0x08U
-#define STLINK_JTAG_COMMAND_ERROR      0x08U
 #define STLINK_JTAG_GET_IDCODE_ERROR   0x09U
 #define STLINK_JTAG_DBG_POWER_ERROR    0x0bU
 #define STLINK_ERROR_AP_WAIT           0x10U
@@ -46,7 +43,6 @@
 #define STLINK_ERROR_DP_FAULT          0x15U
 #define STLINK_ERROR_DP                0x16U
 #define STLINK_ERROR_DP_PARITY         0x17U
-
 #define STLINK_SWD_AP_WDATA_ERROR      0x18U
 #define STLINK_SWD_AP_STICKY_ERROR     0x19U
 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1aU

From 1ab2fadc49b78077f9bd97e610d2640a3a14b98b Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 07:52:07 +0100
Subject: [PATCH 14/25] hosted/stlinkv2: Reworked the AP handling to properly
 take into account multi-drop targets

---
 src/platforms/hosted/platform.c |  5 +++
 src/platforms/hosted/stlinkv2.c | 60 ++++++++++++++++++++++++---------
 src/platforms/hosted/stlinkv2.h |  1 +
 src/target/adiv5.c              | 12 ++-----
 src/target/adiv5.h              |  2 --
 5 files changed, 52 insertions(+), 28 deletions(-)

diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index c6315f10297..74100736120 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -67,6 +67,11 @@ void gdb_ident(char *p, int count)
 
 static void exit_function(void)
 {
+#if HOSTED_BMP_ONLY == 0
+	if (bmda_probe_info.type == PROBE_TYPE_STLINK_V2)
+		stlink_deinit();
+#endif
+
 	libusb_exit_function(&bmda_probe_info);
 
 	switch (bmda_probe_info.type) {
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 7d4b6a0749c..1cc3a36d367 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -69,6 +69,7 @@ typedef struct stlink {
 	uint8_t ver_bridge;
 	uint16_t block_size;
 	bool ap_error;
+	uint16_t apsel;
 } stlink_s;
 
 #define STLINK_V2_CPU_CLOCK_FREQ      (72U * 1000U * 1000U)
@@ -79,12 +80,16 @@ typedef struct stlink {
 #define STLINK_V2_MAX_SWD_CLOCK_FREQ  (3600U * 1000U)
 #define STLINK_V2_MIN_SWD_CLOCK_FREQ  (4505U)
 
+#define STLINK_INVALID_AP 0xffffU
+
 static stlink_s stlink;
 
 static uint32_t stlink_v2_divisor;
 static unsigned int stlink_v3_freq[2];
 
 static int stlink_usb_get_rw_status(bool verbose);
+static bool stlink_ap_setup(uint8_t ap);
+static bool stlink_ap_cleanup(void);
 
 static stlink_mem_command_s stlink_memory_access(
 	const uint8_t operation, const uint32_t address, const uint16_t length, const uint8_t apsel)
@@ -466,10 +471,18 @@ bool stlink_init(void)
 		DEBUG_WARN("ST-Link board was in DFU mode. Restart\n");
 		return false;
 	}
+	stlink.apsel = STLINK_INVALID_AP;
 	stlink_reset_adaptor();
 	return true;
 }
 
+void stlink_deinit(void)
+{
+	if (stlink.apsel != STLINK_INVALID_AP)
+		stlink_ap_cleanup();
+	stlink_simple_query(STLINK_DEBUG_COMMAND, STLINK_DEBUG_EXIT, NULL, 0);
+}
+
 void stlink_nrst_set_val(bool assert)
 {
 	uint8_t data[2];
@@ -530,9 +543,25 @@ void stlink_dp_abort(adiv5_debug_port_s *dp, uint32_t abort)
 	adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
 }
 
+static bool stlink_ensure_ap(const uint16_t apsel)
+{
+	if (apsel != STLINK_DEBUG_PORT && apsel != stlink.apsel) {
+		if (stlink.apsel != STLINK_INVALID_AP) {
+			if (!stlink_ap_cleanup())
+				return false;
+		}
+		if (!stlink_ap_setup(apsel))
+			return false;
+		stlink.apsel = apsel;
+	}
+	return true;
+}
+
 static int stlink_read_dp_register(
 	adiv5_debug_port_s *const dp, const uint16_t apsel, const uint16_t address, uint32_t *const reg)
 {
+	if (!stlink_ensure_ap(apsel))
+		return STLINK_ERROR_GENERAL;
 	stlink_adiv5_reg_read_s request = {
 		.command = STLINK_DEBUG_COMMAND,
 		.operation = STLINK_DEBUG_APIV2_READ_DAP_REG,
@@ -557,6 +586,8 @@ static int stlink_read_dp_register(
 
 static int stlink_write_dp_register(const uint16_t apsel, const uint16_t address, const uint32_t value)
 {
+	if (!stlink_ensure_ap(apsel))
+		return STLINK_ERROR_GENERAL;
 	if (apsel == STLINK_DEBUG_PORT && address == 8U) {
 		stlink.dap_select = value;
 		DEBUG_PROBE("Caching SELECT 0x%02" PRIx32 "\n", value);
@@ -611,28 +642,22 @@ uint32_t stlink_raw_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, u
 
 static bool stlink_ap_setup(const uint8_t ap)
 {
-	if (ap > 7)
-		return false;
-	const stlink_simple_request_s command = {
-		STLINK_DEBUG_COMMAND,
-		STLINK_DEBUG_APIV2_INIT_AP,
-		ap,
-	};
 	uint8_t data[2];
 	DEBUG_PROBE("%s: AP %u\n", __func__, ap);
-	stlink_send_recv_retry(&command, sizeof(command), data, sizeof(data));
-	int res = stlink_usb_error_check(data, true);
+	stlink_simple_request(STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_INIT_AP, ap, data, sizeof(data));
+	const int res = stlink_usb_error_check(data, true);
 	if (res && stlink.ver_hw == 30)
 		DEBUG_WARN("ST-Link v3 only connects to STM8/32!\n");
-	return !res;
+	return res == STLINK_ERROR_OK;
 }
 
-static void stlink_ap_cleanup(const uint8_t ap)
+static bool stlink_ap_cleanup(void)
 {
 	uint8_t data[2];
-	stlink_simple_request(STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_CLOSE_AP_DBG, ap, data, sizeof(data));
-	DEBUG_PROBE("%s: AP %u\n", __func__, ap);
-	stlink_usb_error_check(data, true);
+	stlink_simple_request(STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_CLOSE_AP_DBG, stlink.apsel, data, sizeof(data));
+	DEBUG_PROBE("%s: AP %u\n", __func__, stlink.apsel);
+	stlink.apsel = STLINK_INVALID_AP;
+	return stlink_usb_error_check(data, true) == STLINK_ERROR_OK;
 }
 
 static int stlink_usb_get_rw_status(bool verbose)
@@ -650,6 +675,8 @@ static void stlink_mem_read(adiv5_access_port_s *ap, void *dest, uint32_t src, s
 		DEBUG_WARN("Too large!\n");
 		return;
 	}
+	if (!stlink_ensure_ap(ap->apsel))
+		raise_exception(EXCEPTION_ERROR, "ST-Link AP selection error");
 
 	uint8_t type;
 	if ((src & 1U) || (len & 1U))
@@ -692,6 +719,9 @@ static void stlink_mem_write(
 {
 	if (len == 0)
 		return;
+	if (!stlink_ensure_ap(ap->apsel))
+		raise_exception(EXCEPTION_ERROR, "ST-Link AP selection error");
+
 	const uint8_t *const data = (const uint8_t *)src;
 	/* Chunk the write up into stlink.block_size blocks */
 	for (size_t offset = 0; offset < len; offset += stlink.block_size) {
@@ -775,8 +805,6 @@ void stlink_adiv5_dp_init(adiv5_debug_port_s *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_mem_read;
diff --git a/src/platforms/hosted/stlinkv2.h b/src/platforms/hosted/stlinkv2.h
index b53c7c96f2a..dbee6a94214 100644
--- a/src/platforms/hosted/stlinkv2.h
+++ b/src/platforms/hosted/stlinkv2.h
@@ -33,6 +33,7 @@
 #define STLINK_ERROR_FAIL        (-1)
 
 bool stlink_init(void);
+void stlink_deinit(void);
 bool stlink_swd_scan(void);
 bool stlink_jtag_scan(void);
 int stlink_hwversion(void);
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 843bdf41706..13610f0a618 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -702,11 +702,6 @@ static void adiv5_component_probe(
 
 adiv5_access_port_s *adiv5_new_ap(adiv5_debug_port_s *dp, uint8_t apsel)
 {
-#if PC_HOSTED == 1
-	if (dp->ap_setup && !dp->ap_setup(apsel))
-		return NULL;
-#endif
-
 	adiv5_access_port_s tmpap = {0};
 	/* Assume valid and try to read IDR */
 	tmpap.dp = dp;
@@ -954,11 +949,8 @@ void adiv5_dp_init(adiv5_debug_port_s *const dp)
 		if (ap == NULL) {
 			/* Clear sticky errors in case scanning for this AP triggered any */
 			adiv5_dp_clear_sticky_errors(dp);
-#if PC_HOSTED == 1
-			if (dp->ap_cleanup)
-				dp->ap_cleanup(i);
-#endif
-			/* We have probably found all APs on this DP so no need to keep looking.
+			/*
+			 * We have probably found all APs on this DP so no need to keep looking.
 			 * Continue with rest of init function down below.
 			 */
 			if (++invalid_aps == 8U)
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index dfb7132650a..ac43f738705 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -241,8 +241,6 @@ struct adiv5_debug_port {
 	void (*abort)(adiv5_debug_port_s *dp, uint32_t abort);
 
 #if PC_HOSTED == 1
-	bool (*ap_setup)(uint8_t i);
-	void (*ap_cleanup)(uint8_t i);
 	void (*ap_regs_read)(adiv5_access_port_s *ap, void *data);
 	uint32_t (*ap_reg_read)(adiv5_access_port_s *ap, uint8_t reg_num);
 	void (*ap_reg_write)(adiv5_access_port_s *ap, uint8_t num, uint32_t value);

From 2d65aaea67816c373e00b6faed7029e5e9792cb2 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 07:56:57 +0100
Subject: [PATCH 15/25] hosted/stlinkv2: Implemented better handling for when
 stlink_ap_setup() fails

---
 src/platforms/hosted/stlinkv2.c          | 16 +++++++++++-----
 src/platforms/hosted/stlinkv2_protocol.h |  2 +-
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 1cc3a36d367..b16d45531bc 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -185,9 +185,10 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_STICKYORUN_ERROR\n");
 		return STLINK_ERROR_FAIL;
-	case STLINK_BAD_AP_ERROR:
-		/* ADIV5 probe 256 APs, most of them are non exisitant.*/
-		return STLINK_ERROR_FAIL;
+	case STLINK_ERROR_BAD_AP:
+		if (verbose)
+			DEBUG_ERROR("Failed to setup AP (bad AP)\n");
+		return STLINK_ERROR_GENERAL;
 	case STLINK_TOO_MANY_AP_ERROR:
 		/* TI TM4C duplicates AP. Error happens at AP9.*/
 		if (verbose)
@@ -646,8 +647,13 @@ static bool stlink_ap_setup(const uint8_t ap)
 	DEBUG_PROBE("%s: AP %u\n", __func__, ap);
 	stlink_simple_request(STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_INIT_AP, ap, data, sizeof(data));
 	const int res = stlink_usb_error_check(data, true);
-	if (res && stlink.ver_hw == 30)
-		DEBUG_WARN("ST-Link v3 only connects to STM8/32!\n");
+	if (res && stlink.ver_hw == 30) {
+		if (data[0] == STLINK_ERROR_BAD_AP)
+			DEBUG_WARN("ST-Link v3's only support up to AP 8, tried to setup AP %u\n", ap);
+		else
+			// XXX: What is this even trying to tell the user? What is the purpose of this message?
+			DEBUG_WARN("ST-Link v3 only connects to STM8/32!\n");
+	}
 	return res == STLINK_ERROR_OK;
 }
 
diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index ead9e44b917..c9f4a3d1263 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -46,7 +46,7 @@
 #define STLINK_SWD_AP_WDATA_ERROR      0x18U
 #define STLINK_SWD_AP_STICKY_ERROR     0x19U
 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1aU
-#define STLINK_BAD_AP_ERROR            0x1dU
+#define STLINK_ERROR_BAD_AP            0x1dU
 #define STLINK_TOO_MANY_AP_ERROR       0x29U
 #define STLINK_JTAG_UNKNOWN_CMD        0x42U
 

From bd440fd57030cd65fe532f16a82b3d83d0f90c97 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 08:01:54 +0100
Subject: [PATCH 16/25] hosted/stlinkv2: Cleaned up some variable naming in
 stlink_ap_setup()

---
 src/platforms/hosted/stlinkv2.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index b16d45531bc..8bc1c341bbd 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -646,15 +646,15 @@ static bool stlink_ap_setup(const uint8_t ap)
 	uint8_t data[2];
 	DEBUG_PROBE("%s: AP %u\n", __func__, ap);
 	stlink_simple_request(STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_INIT_AP, ap, data, sizeof(data));
-	const int res = stlink_usb_error_check(data, true);
-	if (res && stlink.ver_hw == 30) {
+	const int result = stlink_usb_error_check(data, true);
+	if (result && stlink.ver_hw == 30) {
 		if (data[0] == STLINK_ERROR_BAD_AP)
 			DEBUG_WARN("ST-Link v3's only support up to AP 8, tried to setup AP %u\n", ap);
 		else
 			// XXX: What is this even trying to tell the user? What is the purpose of this message?
 			DEBUG_WARN("ST-Link v3 only connects to STM8/32!\n");
 	}
-	return res == STLINK_ERROR_OK;
+	return result == STLINK_ERROR_OK;
 }
 
 static bool stlink_ap_cleanup(void)

From 0333e56924127b0bcc9201e1cee3e1806c6fc3bf Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 08:16:54 +0100
Subject: [PATCH 17/25] hosted/stlinkv2: Cleaned up a couple more of the error
 definitions

---
 src/platforms/hosted/stlinkv2.c          | 11 ++++++++---
 src/platforms/hosted/stlinkv2_protocol.h |  4 ++--
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 8bc1c341bbd..0bae70726cf 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -164,14 +164,17 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("Failure powering DBG\n");
 		return STLINK_ERROR_WAIT;
-	case STLINK_JTAG_WRITE_ERROR:
+
+	case STLINK_ERROR_WRITE:
 		if (verbose)
 			DEBUG_ERROR("Write error\n");
-		return STLINK_ERROR_FAIL;
-	case STLINK_JTAG_WRITE_VERIF_ERROR:
+		return STLINK_ERROR_GENERAL;
+
+	case STLINK_ERROR_WRITE_VERIFY:
 		if (verbose)
 			DEBUG_ERROR("Write verify error, ignoring\n");
 		return STLINK_ERROR_OK;
+
 	case STLINK_SWD_AP_WDATA_ERROR:
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_WDATA_ERROR\n");
@@ -185,10 +188,12 @@ int stlink_usb_error_check(uint8_t *const data, const bool verbose)
 		if (verbose)
 			DEBUG_ERROR("STLINK_SWD_AP_STICKYORUN_ERROR\n");
 		return STLINK_ERROR_FAIL;
+
 	case STLINK_ERROR_BAD_AP:
 		if (verbose)
 			DEBUG_ERROR("Failed to setup AP (bad AP)\n");
 		return STLINK_ERROR_GENERAL;
+
 	case STLINK_TOO_MANY_AP_ERROR:
 		/* TI TM4C duplicates AP. Error happens at AP9.*/
 		if (verbose)
diff --git a/src/platforms/hosted/stlinkv2_protocol.h b/src/platforms/hosted/stlinkv2_protocol.h
index c9f4a3d1263..bc7bb0d5dab 100644
--- a/src/platforms/hosted/stlinkv2_protocol.h
+++ b/src/platforms/hosted/stlinkv2_protocol.h
@@ -33,12 +33,12 @@
 #define STLINK_JTAG_COMMAND_ERROR      0x08U
 #define STLINK_JTAG_GET_IDCODE_ERROR   0x09U
 #define STLINK_JTAG_DBG_POWER_ERROR    0x0bU
+#define STLINK_ERROR_WRITE             0x0cU
+#define STLINK_ERROR_WRITE_VERIFY      0x0dU
 #define STLINK_ERROR_AP_WAIT           0x10U
 #define STLINK_ERROR_AP_FAULT          0x11U
 #define STLINK_ERROR_AP                0x12U
 #define STLINK_ERROR_AP_PARITY         0x13U
-#define STLINK_JTAG_WRITE_ERROR        0x0cU
-#define STLINK_JTAG_WRITE_VERIF_ERROR  0x0dU
 #define STLINK_ERROR_DP_WAIT           0x14U
 #define STLINK_ERROR_DP_FAULT          0x15U
 #define STLINK_ERROR_DP                0x16U

From ede069c4e5b813622f46a78c35d6148546d722c7 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 08:24:07 +0100
Subject: [PATCH 18/25] hosted/stlinkv2: Improved the protocol-level debugging
 available from stlink_ap_{read,write}()

---
 src/platforms/hosted/stlinkv2.c | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 0bae70726cf..2ecb50275fb 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -801,14 +801,16 @@ static void stlink_reg_write(adiv5_access_port_s *const ap, const uint8_t reg_nu
 
 static void stlink_ap_write(adiv5_access_port_s *ap, uint16_t addr, uint32_t value)
 {
+	DEBUG_PROBE("%s: addr %04x <- %08" PRIx32 "\n", __func__, addr, value);
 	stlink_write_dp_register(ap->apsel, addr, value);
 }
 
 static uint32_t stlink_ap_read(adiv5_access_port_s *ap, uint16_t addr)
 {
-	uint32_t ret = 0;
-	stlink_read_dp_register(ap->dp, ap->apsel, addr, &ret);
-	return ret;
+	uint32_t value = 0;
+	stlink_read_dp_register(ap->dp, ap->apsel, addr, &value);
+	DEBUG_PROBE("%s: addr %04x -> %08" PRIx32 "\n", __func__, addr, value);
+	return value;
 }
 
 void stlink_adiv5_dp_init(adiv5_debug_port_s *dp)

From 9870caee0abe6a4ee394f57fbb93489924f38e4e Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 08:24:35 +0100
Subject: [PATCH 19/25] adiv5: Cleaned up adiv5_dp_recoverable_access(), fixing
 the parameter name `rnw`

---
 src/target/adiv5.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index ac43f738705..6624822e96d 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -350,9 +350,9 @@ void adiv5_mem_write_sized(adiv5_access_port_s *ap, uint32_t dest, const void *s
 void adiv5_dp_write(adiv5_debug_port_s *dp, uint16_t addr, uint32_t value);
 #endif
 
-static inline uint32_t adiv5_dp_recoverable_access(adiv5_debug_port_s *dp, uint8_t RnW, uint16_t addr, uint32_t value)
+static inline uint32_t adiv5_dp_recoverable_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, uint32_t value)
 {
-	const uint32_t result = dp->low_access(dp, RnW, addr, value);
+	const uint32_t result = dp->low_access(dp, rnw, addr, value);
 	/* If the access results in the no-response response, retry after clearing the error state */
 	if (dp->fault == SWDP_ACK_NO_RESPONSE) {
 		uint32_t response;
@@ -360,7 +360,7 @@ static inline uint32_t adiv5_dp_recoverable_access(adiv5_debug_port_s *dp, uint8
 		swd_proc.seq_in_parity(&response, 32);
 		DEBUG_WARN("Recovering and re-trying access\n");
 		dp->error(dp, true);
-		return dp->low_access(dp, RnW, addr, value);
+		return dp->low_access(dp, rnw, addr, value);
 	}
 	return result;
 }

From 85bb78d594ac707a1c70558d59967dca8340fd7e Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Wed, 21 Jun 2023 09:27:48 +0100
Subject: [PATCH 20/25] hosted/platform: Improved the decoding of DP accesses

---
 src/platforms/hosted/platform.c | 42 +++++++++++++++++++++++++--------
 1 file changed, 32 insertions(+), 10 deletions(-)

diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 74100736120..fb82206d485 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -576,21 +576,43 @@ void platform_target_clk_output_enable(const bool enable)
 	}
 }
 
-static void decode_dp_access(const uint8_t addr, const uint8_t rnw)
+static void decode_dp_access(const uint8_t addr, const uint8_t rnw, const uint32_t value)
 {
+	/* How a DP address should be decoded depends on the bank that's presently selected, so make a note of that */
+	static uint8_t dp_bank = 0;
 	const char *reg = NULL;
+
+	/* Try to decode the requested address */
 	switch (addr) {
 	case 0x00U:
 		reg = rnw ? "DPIDR" : "ABORT";
 		break;
 	case 0x04U:
-		reg = "CTRL/STAT";
+		switch (dp_bank) {
+		case 0:
+			reg = rnw ? "STATUS" : "CTRL";
+			break;
+		case 1:
+			reg = "DLCR";
+			break;
+		case 2:
+			reg = "TARGETID";
+			break;
+		case 3:
+			reg = "DLPIDR";
+			break;
+		case 4:
+			reg = "EVENTSTAT";
+			break;
+		}
 		break;
 	case 0x08U:
+		if (!rnw)
+			dp_bank = value & 15U;
 		reg = rnw ? "RESEND" : "SELECT";
 		break;
 	case 0x0cU:
-		reg = "RDBUFF";
+		reg = rnw ? "RDBUFF" : "TARGETSEL";
 		break;
 	}
 
@@ -644,7 +666,7 @@ static void decode_ap_access(const uint8_t ap, const uint8_t addr)
 		DEBUG_PROTO("Reserved(%02x): ", addr);
 }
 
-static void decode_access(const uint16_t addr, const uint8_t rnw)
+static void decode_access(const uint16_t addr, const uint8_t rnw, const uint32_t value)
 {
 	if (rnw)
 		DEBUG_PROTO("Read ");
@@ -652,14 +674,14 @@ static void decode_access(const uint16_t addr, const uint8_t rnw)
 		DEBUG_PROTO("Write ");
 
 	if (addr < 0x100U)
-		decode_dp_access(addr & 0xffU, rnw);
+		decode_dp_access(addr & 0xffU, rnw, value);
 	else
 		decode_ap_access(addr >> 8U, addr & 0xffU);
 }
 
 void adiv5_dp_write(adiv5_debug_port_s *dp, uint16_t addr, uint32_t value)
 {
-	decode_access(addr, ADIV5_LOW_WRITE);
+	decode_access(addr, ADIV5_LOW_WRITE, value);
 	DEBUG_PROTO("0x%08" PRIx32 "\n", value);
 	dp->low_access(dp, ADIV5_LOW_WRITE, addr, value);
 }
@@ -667,7 +689,7 @@ void adiv5_dp_write(adiv5_debug_port_s *dp, uint16_t addr, uint32_t value)
 uint32_t adiv5_dp_read(adiv5_debug_port_s *dp, uint16_t addr)
 {
 	uint32_t ret = dp->dp_read(dp, addr);
-	decode_access(addr, ADIV5_LOW_READ);
+	decode_access(addr, ADIV5_LOW_READ, 0U);
 	DEBUG_PROTO("0x%08" PRIx32 "\n", ret);
 	return ret;
 }
@@ -682,7 +704,7 @@ uint32_t adiv5_dp_error(adiv5_debug_port_s *dp)
 uint32_t adiv5_dp_low_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr, uint32_t value)
 {
 	uint32_t ret = dp->low_access(dp, rnw, addr, value);
-	decode_access(addr, rnw);
+	decode_access(addr, rnw, value);
 	DEBUG_PROTO("0x%08" PRIx32 "\n", rnw ? ret : value);
 	return ret;
 }
@@ -690,14 +712,14 @@ uint32_t adiv5_dp_low_access(adiv5_debug_port_s *dp, uint8_t rnw, uint16_t addr,
 uint32_t adiv5_ap_read(adiv5_access_port_s *ap, uint16_t addr)
 {
 	uint32_t ret = ap->dp->ap_read(ap, addr);
-	decode_access(addr, ADIV5_LOW_READ);
+	decode_access(addr, ADIV5_LOW_READ, 0U);
 	DEBUG_PROTO("0x%08" PRIx32 "\n", ret);
 	return ret;
 }
 
 void adiv5_ap_write(adiv5_access_port_s *ap, uint16_t addr, uint32_t value)
 {
-	decode_access(addr, ADIV5_LOW_WRITE);
+	decode_access(addr, ADIV5_LOW_WRITE, value);
 	DEBUG_PROTO("0x%08" PRIx32 "\n", value);
 	ap->dp->ap_write(ap, addr, value);
 }

From 587b7934c0f2ef2816b94050ae6c3f204be6d350 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Thu, 22 Jun 2023 06:53:51 +0100
Subject: [PATCH 21/25] hosted/stlinkv2_swd: Cleaned up how entry into SWD is
 done

---
 src/platforms/hosted/stlinkv2_swd.c | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2_swd.c b/src/platforms/hosted/stlinkv2_swd.c
index 11b48114a3d..df46ac588ea 100644
--- a/src/platforms/hosted/stlinkv2_swd.c
+++ b/src/platforms/hosted/stlinkv2_swd.c
@@ -30,13 +30,9 @@ bool stlink_swd_scan(void)
 
 	stlink_leave_state();
 
-	const stlink_simple_request_s command = {
-		.command = STLINK_DEBUG_COMMAND,
-		.operation = STLINK_DEBUG_APIV2_ENTER,
-		.param = STLINK_DEBUG_ENTER_SWD_NO_RESET,
-	};
 	uint8_t data[2];
-	stlink_send_recv_retry(&command, sizeof(command), data, sizeof(data));
+	stlink_simple_request(
+		STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_ENTER, STLINK_DEBUG_ENTER_SWD_NO_RESET, data, sizeof(data));
 	if (stlink_usb_error_check(data, true) != STLINK_ERROR_OK)
 		return false;
 

From 8900991ea4fb66f1c2b9442ba23c5010786d7262 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Mon, 31 Jul 2023 00:47:50 +0100
Subject: [PATCH 22/25] hosted/stlinkv2: Cleaned up the error handling and
 reporting in stlink_send_recv_retry()

---
 src/platforms/hosted/stlinkv2.c | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 2ecb50275fb..26a1657f0e0 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -214,31 +214,31 @@ int stlink_send_recv_retry(
 	const void *const req_buffer, const size_t req_len, void *const rx_buffer, const size_t rx_len)
 {
 	uint32_t start = platform_time_ms();
-	int res;
 	int first_res = STLINK_ERROR_OK;
 	while (true) {
 		bmda_usb_transfer(bmda_probe_info.usb_link, req_buffer, req_len, rx_buffer, rx_len, BMDA_USB_NO_TIMEOUT);
-		res = stlink_usb_error_check(rx_buffer, false);
-		if (res == STLINK_ERROR_OK)
-			return res;
-		if (res == STLINK_ERROR_AP_FAULT && first_res == STLINK_ERROR_WAIT) {
+		int result = stlink_usb_error_check(rx_buffer, false);
+		if (result == STLINK_ERROR_OK)
+			return result;
+		if (result == STLINK_ERROR_AP_FAULT && first_res == STLINK_ERROR_WAIT) {
 			/*
 			 * ST-Link v3 while AP is busy answers once with ERROR_WAIT, then
 			 * with AP_FAULT and finally with ERROR_OK and the pending result.
 			 * Interpret AP_FAULT as AP_WAIT in this case.
 			 */
 			stlink.ap_error = false;
-			res = STLINK_ERROR_WAIT;
+			result = STLINK_ERROR_WAIT;
 		}
 		if (first_res == STLINK_ERROR_OK)
-			first_res = res;
+			first_res = result;
 		uint32_t now = platform_time_ms();
-		if (now - start > cortexm_wait_timeout || res != STLINK_ERROR_WAIT) {
-			DEBUG_ERROR("send_recv_retry failed.\n");
-			return res;
+		if (now - start > cortexm_wait_timeout || result != STLINK_ERROR_WAIT) {
+			DEBUG_ERROR("send_recv_retry failed (%d): ", result);
+			stlink_usb_error_check(rx_buffer, true);
+			return result;
 		}
 	}
-	return res;
+	return STLINK_ERROR_GENERAL;
 }
 
 static int stlink_read_retry(

From 8b499c7da2f535853b5890260f9c86c252b4a5f0 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Mon, 31 Jul 2023 00:48:15 +0100
Subject: [PATCH 23/25] hosted/stlinkv2: Cleaned up the error handling and
 reporting in stlink_read_retry()

---
 src/platforms/hosted/stlinkv2.c | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 26a1657f0e0..47f6cc66b87 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -245,20 +245,19 @@ static int stlink_read_retry(
 	const void *const req_buffer, const size_t req_len, void *const rx_buffer, const size_t rx_len)
 {
 	uint32_t start = platform_time_ms();
-	int res;
 	while (true) {
 		bmda_usb_transfer(bmda_probe_info.usb_link, req_buffer, req_len, rx_buffer, rx_len, BMDA_USB_NO_TIMEOUT);
-		res = stlink_usb_get_rw_status(false);
-		if (res == STLINK_ERROR_OK)
-			return res;
+		const int result = stlink_usb_get_rw_status(false);
+		if (result == STLINK_ERROR_OK)
+			return result;
 		uint32_t now = platform_time_ms();
-		if (now - start > 1000U || res != STLINK_ERROR_WAIT) {
-			DEBUG_ERROR("stlink_read_retry failed.\n");
+		if (now - start > 1000U || result != STLINK_ERROR_WAIT) {
+			DEBUG_ERROR("stlink_read_retry failed (%d): ", result);
 			stlink_usb_get_rw_status(true);
-			return res;
+			return result;
 		}
 	}
-	return res;
+	return STLINK_ERROR_GENERAL;
 }
 
 static int stlink_write_retry(

From 676419e1807721b1a744a2e147a47dcb0f0e26b0 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Mon, 31 Jul 2023 00:48:28 +0100
Subject: [PATCH 24/25] hosted/stlinkv2: Cleaned up the error handling and
 reporting in stlink_write_retry()

---
 src/platforms/hosted/stlinkv2.c | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 47f6cc66b87..53d9e664d9f 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -264,21 +264,21 @@ static int stlink_write_retry(
 	const void *const req_buffer, const size_t req_len, const void *const tx_buffer, const size_t tx_len)
 {
 	uint32_t start = platform_time_ms();
-	int res;
 	usb_link_s *link = bmda_probe_info.usb_link;
 	while (true) {
 		bmda_usb_transfer(link, req_buffer, req_len, NULL, 0, BMDA_USB_NO_TIMEOUT);
 		bmda_usb_transfer(link, tx_buffer, tx_len, NULL, 0, BMDA_USB_NO_TIMEOUT);
-		res = stlink_usb_get_rw_status(false);
-		if (res == STLINK_ERROR_OK)
-			return res;
+		const int result = stlink_usb_get_rw_status(false);
+		if (result == STLINK_ERROR_OK)
+			return result;
 		uint32_t now = platform_time_ms();
-		if (now - start > 1000U || res != STLINK_ERROR_WAIT) {
+		if (now - start > 1000U || result != STLINK_ERROR_WAIT) {
+			DEBUG_ERROR("stlink_write_retry failed (%d): ", result);
 			stlink_usb_get_rw_status(true);
-			return res;
+			return result;
 		}
 	}
-	return res;
+	return STLINK_ERROR_GENERAL;
 }
 
 int stlink_simple_query(const uint8_t command, const uint8_t operation, void *const rx_buffer, const size_t rx_len)

From 53b161cd33120d5738e6d9deec0ea6f4e9df162e Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Mon, 21 Aug 2023 12:15:45 +0100
Subject: [PATCH 25/25] hosted/stlinkv2_jtag: Implemented adiv5_dp_read() for
 ST-Link JTAG

---
 src/platforms/hosted/stlinkv2_jtag.c | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/platforms/hosted/stlinkv2_jtag.c b/src/platforms/hosted/stlinkv2_jtag.c
index b15cd5d64e5..6bae3a26e53 100644
--- a/src/platforms/hosted/stlinkv2_jtag.c
+++ b/src/platforms/hosted/stlinkv2_jtag.c
@@ -27,6 +27,7 @@
 
 static bool stlink_enter_debug_jtag(void);
 static size_t stlink_read_idcodes(uint32_t *idcodes);
+static uint32_t stlink_jtag_adiv5_dp_read(adiv5_debug_port_s *dp, uint16_t addr);
 
 bool stlink_jtag_scan(void)
 {
@@ -76,7 +77,13 @@ static size_t stlink_read_idcodes(uint32_t *idcodes)
 
 void stlink_jtag_dp_init(adiv5_debug_port_s *dp)
 {
+	dp->dp_read = stlink_jtag_adiv5_dp_read;
 	dp->error = stlink_adiv5_clear_error;
 	dp->low_access = stlink_raw_access;
 	dp->abort = stlink_dp_abort;
 }
+
+static uint32_t stlink_jtag_adiv5_dp_read(adiv5_debug_port_s *const dp, const uint16_t addr)
+{
+	return stlink_raw_access(dp, ADIV5_LOW_READ, addr, 0U);
+}