target/adi_v5_swd: fix DP registers banking

ADIv6 brought more complicated rules for DP reg 0 banking.
Neither the original implementation [1] nor the later
modification [2] respected that the DP reg 0 is banked
for read only, not for write. Enforcing of an useless
SELECT write before a write to ABORT register may trigger
FAULT (CTRL/STAT bits ORUNDETECT and STICKYORUN are set)
or WAIT (DP is stalled by an outstanding previous operation)
and therefore make ABORT register virtually unusable
on some adapters (bitbang, CMSIS-DAP).

There are DP ABORT specific functions swd_queue_ap_abort()
and swd_clear_sticky_errors() which worked around the problem
using the lowest level swd->write_reg(). Using a specific
write procedure for a single DP register was error prone
(there are other DP_ABORT writes using swd_queue_dp_write_inner())
and also the Tcl command 'xx.dap dpreg 0 value' suffered
from unwanted SELECT write.

Other smaller discords in DP banking probably do not
influence normal DP operation however they may complicate
debugging in corner cases.

Adhere strictly to the DP banking rules for both ADI versions.

Fixes: [1] commit 72fb886 ("adiv6: add low level swd transport")
Fixes: [2] commit ee3fb5a ("target/arm_adi_v5: fix DP SELECT logic")
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Change-Id: I3328748c1c3e0661c5ecd6eb070ac519b190ace2
Reviewed-on: https://review.openocd.org/c/openocd/+/8154
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

src/target/adi_v5_swd.c

@@ -105,14 +105,13 @@ static inline int check_sync(struct adiv5_dap *dap)
 static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
 {
 	/* Only register address 0 (ADIv6 only) and 4 are banked. */
-	if ((reg & 0xf) > 4)
+	if (is_adiv6(dap) ? (reg & 0xf) > 4 : (reg & 0xf) != 4)
 		return ERROR_OK;
 
 	uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK;
 
-	/* DP register 0 is not mapped according to ADIv5
-	 * whereas ADIv6 ensures DPBANKSEL = 0 after line reset */
-	if ((dap->select_valid || ((reg & 0xf) == 0 && dap->select_dpbanksel_valid))
+	/* ADIv6 ensures DPBANKSEL = 0 after line reset */
+	if ((dap->select_valid || (is_adiv6(dap) && dap->select_dpbanksel_valid))
 			&& (sel == (dap->select & DP_SELECT_DPBANK)))
 		return ERROR_OK;
 
@@ -146,7 +145,7 @@ static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
 static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 		uint32_t data)
 {
-	int retval;
+	int retval = ERROR_OK;
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
 	assert(swd);
 
@@ -167,7 +166,11 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 	if (reg == DP_SELECT1)
 		dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull);
 
-	retval = swd_queue_dp_bankselect(dap, reg);
+	/* DP_ABORT write is not banked.
+	 * Prevent writing DP_SELECT before as it would fail on locked up DP */
+	if (reg != DP_ABORT)
+		retval = swd_queue_dp_bankselect(dap, reg);
+
 	if (retval == ERROR_OK) {
 		swd->write_reg(swd_cmd(false, false, reg), data, 0);