diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c index 6406406aa..c031fd36e 100644 --- a/src/jtag/drivers/ftdi.c +++ b/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) { diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c index 40d91cc43..ca57ae848 100644 --- a/src/jtag/drivers/jlink.c +++ b/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); diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index 6e322fb41..1827c59db 100644 --- a/src/target/adi_v5_swd.c +++ b/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 = { diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h index dee3117e4..8d1260866 100644 --- a/src/target/arm_adi_v5.h +++ b/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; }; /**