swd: handle various failure conditions

When communication with target fails for whatever reason, it makes
sense to do JTAG-to-SWD (in case the target got power-cycled or the
DAP method was reset anyhow), SWD line reset sequence (part of
JTAG-to-SWD already) and the mandatory IDCODE read. Schedule that to
be performed on the next poll.

Fix the return values for ftdi and jlink drivers to be consistent with
OpenOCD error codes and remove ad-hoc calls to perform DAP method
switching (as it's now done from the upper layer automatically).

Change-Id: Ie18797d4ce7ac43d8249f8f81f1064a2424e02be
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/2371
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-by: Mateusz Manowiecki <segmentation@fault.pl>

src/jtag/drivers/ftdi.c

@@ -655,11 +655,6 @@ static int ftdi_initialize(void)
 
 	freq = mpsse_set_frequency(mpsse_ctx, jtag_get_speed_khz() * 1000);
 
-	if (swd_mode)
-		ftdi_swd_switch_seq(NULL, JTAG_TO_SWD);
-	else
-		ftdi_swd_switch_seq(NULL, SWD_TO_JTAG);
-
 	return mpsse_flush(mpsse_ctx);
 }
 
@@ -981,7 +976,7 @@ static int ftdi_swd_run_queue(struct adiv5_dap *dap)
 						1 + 3 + (swd_cmd_queue[i].cmd & SWD_CMD_RnW ? 0 : 1), 32));
 
 		if (ack != SWD_ACK_OK) {
-			queued_retval = ack;
+			queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
 			goto skip;
 
 		} else if (swd_cmd_queue[i].cmd & SWD_CMD_RnW) {

src/jtag/drivers/jlink.c

@@ -590,12 +590,6 @@ static int jlink_init(void)
 		jlink_tap_execute();
 	}
 
-	if (swd_mode)
-		jlink_swd_switch_seq(NULL, JTAG_TO_SWD);
-	else
-		jlink_swd_switch_seq(NULL, SWD_TO_JTAG);
-	jlink_swd_run_queue(NULL);
-
 	return ERROR_OK;
 }
 
@@ -1664,9 +1658,9 @@ static int jlink_swd_run_queue(struct adiv5_dap *dap)
 		int ack = buf_get_u32(usb_in_buffer, pending_scan_results_buffer[i].first, 3);
 
 		if (ack != SWD_ACK_OK) {
-			LOG_ERROR("SWD ack not OK: %d %s", ack,
+			LOG_DEBUG("SWD ack not OK: %d %s", ack,
 				  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
-			queued_retval = ack;
+			queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
 			goto skip;
 		} else if (pending_scan_results_buffer[i].length) {
 			uint32_t data = buf_get_u32(usb_in_buffer, 3 + pending_scan_results_buffer[i].first, 32);

src/target/adi_v5_swd.c

@@ -70,6 +70,8 @@ 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)
 {
@@ -83,17 +85,48 @@ static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 static int swd_run_inner(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = jtag_interface->swd;
+	int retval;
 
-	int retval = swd->run(dap);
+	retval = swd->run(dap);
 
 	if (retval != ERROR_OK) {
 		/* fault response */
-		swd_clear_sticky_errors(dap);
+		dap->do_reconnect = true;
 	}
 
 	return retval;
 }
 
+static int swd_connect(struct adiv5_dap *dap)
+{
+	uint32_t idcode;
+	int status;
+
+	/* FIXME validate transport config ... is the
+	 * configured DAP present (check IDCODE)?
+	 * Is *only* one DAP configured?
+	 *
+	 * MUST READ IDCODE
+	 */
+
+	/* Note, debugport_init() does setup too */
+	jtag_interface->swd->switch_seq(dap, JTAG_TO_SWD);
+
+	swd_queue_dp_read(dap, DP_IDCODE, &idcode);
+
+	/* force clear all sticky faults */
+	swd_clear_sticky_errors(dap);
+
+	status = swd_run_inner(dap);
+
+	if (status == ERROR_OK) {
+		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
+		dap->do_reconnect = false;
+	}
+
+	return status;
+}
+
 static inline int check_sync(struct adiv5_dap *dap)
 {
 	return do_sync ? swd_run_inner(dap) : ERROR_OK;
@@ -138,7 +171,6 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 	return check_sync(dap);
 }
 
-
 static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data)
 {
@@ -172,6 +204,12 @@ static int swd_queue_ap_read(struct adiv5_dap *dap, unsigned reg,
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
+	if (dap->do_reconnect) {
+		int retval = swd_connect(dap);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
 	swd_queue_ap_bankselect(dap, reg);
 	swd->read_reg(dap, swd_cmd(true,  true, reg), dap->last_read);
 	dap->last_read = data;
@@ -408,33 +446,11 @@ static int swd_init(struct command_context *ctx)
 	struct target *target = get_current_target(ctx);
 	struct arm *arm = target_to_arm(target);
 	struct adiv5_dap *dap = arm->dap;
-	uint32_t idcode;
-	int status;
-
 	/* Force the DAP's ops vector for SWD mode.
 	 * messy - is there a better way? */
 	arm->dap->ops = &swd_dap_ops;
 
-	/* FIXME validate transport config ... is the
-	 * configured DAP present (check IDCODE)?
-	 * Is *only* one DAP configured?
-	 *
-	 * MUST READ IDCODE
-	 */
-
- /* Note, debugport_init() does setup too */
-
-	swd_queue_dp_read(dap, DP_IDCODE, &idcode);
-
-	/* force clear all sticky faults */
-	swd_clear_sticky_errors(dap);
-
-	status = swd_run(dap);
-
-	if (status == ERROR_OK)
-		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
-
-	return status;
+	return swd_connect(dap);
 }
 
 static struct transport swd_transport = {

src/target/arm_adi_v5.h

@@ -213,6 +213,12 @@ struct adiv5_dap {
 	 * the AHB-AP has strange byte ordering these processors, and we need to
 	 * swizzle appropriately. */
 	bool ti_be_32_quirks;
+
+	/**
+	 * Signals that an attempt to reestablish communication afresh
+	 * should be performed before the next access.
+	 */
+	bool do_reconnect;
 };
 
 /**