X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=d92e2961ef02689572c746e869777f6fe99b7a7e;hp=d099f4e710021153b179c8bc4dcb0bb9d510010f;hb=0323c9bcbd608f74ab40f63eb62e0cdb4d022401;hpb=420a692e0fae34a59eb3c33932362812e33f5583

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index d099f4e710..d92e2961ef 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -166,22 +166,26 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 }
 
 /** Select the DP register bank matching bits 7:4 of reg. */
-static void swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
+static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
 {
 	/* Only register address 4 is banked. */
 	if ((reg & 0xf) != 4)
-		return;
+		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;
+		return ERROR_OK;
 
 	dap->select = sel;
 
-	swd_queue_dp_write(dap, DP_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,
@@ -194,7 +198,10 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
-	swd_queue_dp_bankselect(dap, reg);
+	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);
@@ -211,17 +218,29 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 		return retval;
 
 	swd_finish_read(dap);
-	if (reg == DP_SELECT)
+	if (reg == DP_SELECT) {
 		dap->select = data & (DP_SELECT_APSEL | DP_SELECT_APBANK | DP_SELECT_DPBANK);
-	else
-		swd_queue_dp_bankselect(dap, reg);
+
+		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. */
-static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
+static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
 	struct adiv5_dap *dap = ap->dap;
 	uint32_t sel = ((uint32_t)ap->ap_num << 24)
@@ -229,11 +248,15 @@ static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 			| (dap->select & DP_SELECT_DPBANK);
 
 	if (sel == dap->select)
-		return;
+		return ERROR_OK;
 
 	dap->select = sel;
 
-	swd_queue_dp_write(dap, DP_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_ap_read(struct adiv5_ap *ap, unsigned reg,
@@ -247,7 +270,10 @@ static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
-	swd_queue_ap_bankselect(ap, reg);
+	retval = swd_queue_ap_bankselect(ap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd->read_reg(swd_cmd(true,  true, reg), dap->last_read, ap->memaccess_tck);
 	dap->last_read = data;
 
@@ -266,7 +292,10 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
 		return retval;
 
 	swd_finish_read(dap);
-	swd_queue_ap_bankselect(ap, reg);
+	retval = swd_queue_ap_bankselect(ap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd->write_reg(swd_cmd(false,  true, reg), data, ap->memaccess_tck);
 
 	return check_sync(dap);