target/adi_v5_swd: introduce swd_queue_dp_read/write_inner()

This is a preparatory change for swd multidrop, mostly refactoring.

Split swd_queue_dp_read/write() to inner and outer parts.
Use the inner parts in swd_queue_dp_bankselect(), swd_connect()
they do not need to check reconnect.
Use the outer parts exclusively in swd_dap_ops.

Rearrange the code to reduce forward declarations.

Change-Id: I47b7f0cb037e0032a267463f06ba02123ba96fe7
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: https://review.openocd.org/c/openocd/+/6139
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
Tested-by: jenkins

src/target/adi_v5_swd.c

@@ -56,6 +56,20 @@
 /* for debug, set do_sync to true to force synchronous transfers */
 static bool do_sync;
 
+
+static int swd_run(struct adiv5_dap *dap);
+static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
+		uint32_t data);
+
+
+static int swd_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
+{
+	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+	assert(swd);
+
+	return swd->switch_seq(seq);
+}
+
 static void swd_finish_read(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
@@ -65,11 +79,6 @@ static void swd_finish_read(struct adiv5_dap *dap)
 	}
 }
 
-static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
-		uint32_t data);
-static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
-		uint32_t *data);
-
 static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
@@ -94,6 +103,79 @@ static int swd_run_inner(struct adiv5_dap *dap)
 	return retval;
 }
 
+static inline int check_sync(struct adiv5_dap *dap)
+{
+	return do_sync ? swd_run_inner(dap) : ERROR_OK;
+}
+
+/** Select the DP register bank matching bits 7:4 of reg. */
+static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
+{
+	/* Only register address 4 is banked. */
+	if ((reg & 0xf) != 4)
+		return ERROR_OK;
+
+	uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
+	uint32_t sel = select_dp_bank
+			| (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK));
+
+	if (sel == dap->select)
+		return ERROR_OK;
+
+	dap->select = sel;
+
+	int retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
+	if (retval != ERROR_OK)
+		dap->select = DP_SELECT_INVALID;
+
+	return retval;
+}
+
+static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
+		uint32_t *data)
+{
+	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+	assert(swd);
+
+	int retval = swd_queue_dp_bankselect(dap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	swd->read_reg(swd_cmd(true, false, reg), data, 0);
+
+	return check_sync(dap);
+}
+
+static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
+		uint32_t data)
+{
+	int retval;
+	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+	assert(swd);
+
+	swd_finish_read(dap);
+
+	if (reg == DP_SELECT) {
+		dap->select = data & (DP_SELECT_APSEL | DP_SELECT_APBANK | DP_SELECT_DPBANK);
+
+		swd->write_reg(swd_cmd(false, false, reg), data, 0);
+
+		retval = check_sync(dap);
+		if (retval != ERROR_OK)
+			dap->select = DP_SELECT_INVALID;
+
+		return retval;
+	}
+
+	retval = swd_queue_dp_bankselect(dap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	swd->write_reg(swd_cmd(false, false, reg), data, 0);
+
+	return check_sync(dap);
+}
+
 static int swd_connect(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
@@ -130,7 +212,7 @@ static int swd_connect(struct adiv5_dap *dap)
 		dap->do_reconnect = false;
 		dap_invalidate_cache(dap);
 
-		status = swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+		status = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
 		if (status == ERROR_OK) {
 			status = swd_run_inner(dap);
 			if (status == ERROR_OK)
@@ -186,19 +268,6 @@ static int swd_connect(struct adiv5_dap *dap)
 	return status;
 }
 
-static int swd_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
-{
-	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-	assert(swd);
-
-	return swd->switch_seq(seq);
-}
-
-static inline int check_sync(struct adiv5_dap *dap)
-{
-	return do_sync ? swd_run_inner(dap) : ERROR_OK;
-}
-
 static int swd_check_reconnect(struct adiv5_dap *dap)
 {
 	if (dap->do_reconnect)
@@ -217,46 +286,14 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 	return check_sync(dap);
 }
 
-/** Select the DP register bank matching bits 7:4 of reg. */
-static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
-{
-	/* Only register address 4 is banked. */
-	if ((reg & 0xf) != 4)
-		return ERROR_OK;
-
-	uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
-	uint32_t sel = select_dp_bank
-			| (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK));
-
-	if (sel == dap->select)
-		return ERROR_OK;
-
-	dap->select = sel;
-
-	int retval = swd_queue_dp_write(dap, DP_SELECT, sel);
-	if (retval != ERROR_OK)
-		dap->select = DP_SELECT_INVALID;
-
-	return retval;
-}
-
 static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 		uint32_t *data)
 {
-	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-	assert(swd);
-
 	int retval = swd_check_reconnect(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = swd_queue_dp_bankselect(dap, reg);
+	return swd_queue_dp_read_inner(dap, reg, data);
-	if (retval != ERROR_OK)
-		return retval;
-
-	swd->read_reg(swd_cmd(true,  false, reg), data, 0);
-
-	return check_sync(dap);
 }
 
 static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
@@ -269,26 +306,7 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
-	swd_finish_read(dap);
+	return swd_queue_dp_write_inner(dap, reg, data);
-	if (reg == DP_SELECT) {
-		dap->select = data & (DP_SELECT_APSEL | DP_SELECT_APBANK | DP_SELECT_DPBANK);
-
-		swd->write_reg(swd_cmd(false,  false, reg), data, 0);
-
-		retval = check_sync(dap);
-		if (retval != ERROR_OK)
-			dap->select = DP_SELECT_INVALID;
-
-		return retval;
-	}
-
-	retval = swd_queue_dp_bankselect(dap, reg);
-	if (retval != ERROR_OK)
-		return retval;
-
-	swd->write_reg(swd_cmd(false,  false, reg), data, 0);
-
-	return check_sync(dap);
 }
 
 /** Select the AP register bank matching bits 7:4 of reg. */
@@ -304,7 +322,7 @@ static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 
 	dap->select = sel;
 
-	int retval = swd_queue_dp_write(dap, DP_SELECT, sel);
+	int retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
 	if (retval != ERROR_OK)
 		dap->select = DP_SELECT_INVALID;