X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=c23cee157d53751e0a24f59879f078b76ac83617;hp=a3735661b68459d6e59adf0c9415a0912bb58b32;hb=d2033997871f8561d1406550c12508461f63de48;hpb=be2d25efcc0132e02c76c304487a8759ca587b0c diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index a3735661b6..c23cee157d 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -53,28 +53,39 @@ #include +/* for debug, set do_sync to true to force synchronous transfers */ static bool do_sync; +static struct adiv5_dap *swd_multidrop_selected_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); - if (dap->last_read != NULL) { + if (dap->last_read) { swd->read_reg(swd_cmd(true, false, DP_RDBUFF), dap->last_read, 0); dap->last_read = NULL; } } -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); assert(swd); - swd->write_reg(swd_cmd(false, false, DP_ABORT), + swd->write_reg(swd_cmd(false, false, DP_ABORT), STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0); } @@ -93,17 +104,264 @@ static int swd_run_inner(struct adiv5_dap *dap) return retval; } -static int swd_connect(struct adiv5_dap *dap) +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); - uint32_t dpidr; + 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_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr, + uint32_t *dlpidr_ptr, bool clear_sticky) +{ + int retval; + uint32_t dpidr, dlpidr; + + assert(dap_is_multidrop(dap)); + + swd_send_sequence(dap, LINE_RESET); + + retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel); + if (retval != ERROR_OK) + return retval; + + retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr); + if (retval != ERROR_OK) + return retval; + + if (clear_sticky) { + /* Clear all sticky errors (including ORUN) */ + swd_clear_sticky_errors(dap); + } else { + /* Ideally just clear ORUN flag which is set by reset */ + retval = swd_queue_dp_write_inner(dap, DP_ABORT, ORUNERRCLR); + if (retval != ERROR_OK) + return retval; + } + + retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr); + if (retval != ERROR_OK) + return retval; + + retval = swd_run_inner(dap); + if (retval != ERROR_OK) + return retval; + + if ((dpidr & DP_DPIDR_VERSION_MASK) < (2UL << DP_DPIDR_VERSION_SHIFT)) { + LOG_INFO("Read DPIDR 0x%08" PRIx32 + " has version < 2. A non multidrop capable device connected?", + dpidr); + return ERROR_FAIL; + } + + /* TODO: check TARGETID if DLIPDR is same for more than one DP */ + uint32_t expected_dlpidr = DP_DLPIDR_PROTVSN | + (dap->multidrop_targetsel & DP_TARGETSEL_INSTANCEID_MASK); + if (dlpidr != expected_dlpidr) { + LOG_INFO("Read incorrect DLPIDR 0x%08" PRIx32 + " (possibly CTRL/STAT value)", + dlpidr); + return ERROR_FAIL; + } + + LOG_DEBUG_IO("Selected DP_TARGETSEL 0x%08" PRIx32, dap->multidrop_targetsel); + swd_multidrop_selected_dap = dap; + + if (dpidr_ptr) + *dpidr_ptr = dpidr; + + if (dlpidr_ptr) + *dlpidr_ptr = dlpidr; + + return retval; +} + +static int swd_multidrop_select(struct adiv5_dap *dap) +{ + if (!dap_is_multidrop(dap)) + return ERROR_OK; + + if (swd_multidrop_selected_dap == dap) + return ERROR_OK; + + int retval = ERROR_OK; + for (unsigned int retry = 0; ; retry++) { + bool clear_sticky = retry > 0; + + retval = swd_multidrop_select_inner(dap, NULL, NULL, clear_sticky); + if (retval == ERROR_OK) + break; + + swd_multidrop_selected_dap = NULL; + if (retry > 3) { + LOG_ERROR("Failed to select multidrop %s", adiv5_dap_name(dap)); + return retval; + } + + LOG_DEBUG("Failed to select multidrop %s, retrying...", + adiv5_dap_name(dap)); + } + + return retval; +} + +static int swd_connect_multidrop(struct adiv5_dap *dap) +{ + int retval; + uint32_t dpidr = 0xdeadbeef; + uint32_t dlpidr = 0xdeadbeef; + int64_t timeout = timeval_ms() + 500; + + do { + swd_send_sequence(dap, JTAG_TO_DORMANT); + swd_send_sequence(dap, DORMANT_TO_SWD); + + /* Clear link state, including the SELECT cache. */ + dap->do_reconnect = false; + dap_invalidate_cache(dap); + swd_multidrop_selected_dap = NULL; + + retval = swd_multidrop_select_inner(dap, &dpidr, &dlpidr, true); + if (retval == ERROR_OK) + break; + + alive_sleep(1); + + } while (timeval_ms() < timeout); + + if (retval != ERROR_OK) { + swd_multidrop_selected_dap = NULL; + LOG_ERROR("Failed to connect multidrop %s", adiv5_dap_name(dap)); + return retval; + } + + LOG_INFO("SWD DPIDR 0x%08" PRIx32 ", DLPIDR 0x%08" PRIx32, + dpidr, dlpidr); + + return retval; +} + +static int swd_connect_single(struct adiv5_dap *dap) +{ + int retval; + uint32_t dpidr = 0xdeadbeef; + int64_t timeout = timeval_ms() + 500; + + do { + swd_send_sequence(dap, JTAG_TO_SWD); + + /* Clear link state, including the SELECT cache. */ + dap->do_reconnect = false; + dap_invalidate_cache(dap); + + retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr); + if (retval == ERROR_OK) { + retval = swd_run_inner(dap); + if (retval == ERROR_OK) + break; + } + + alive_sleep(1); + + } while (timeval_ms() < timeout); + + if (retval != ERROR_OK) { + LOG_ERROR("Error connecting DP: cannot read IDR"); + return retval; + } + + LOG_INFO("SWD DPIDR 0x%08" PRIx32, dpidr); + + do { + dap->do_reconnect = false; + + /* force clear all sticky faults */ + swd_clear_sticky_errors(dap); + + retval = swd_run_inner(dap); + if (retval != ERROR_WAIT) + break; + + alive_sleep(10); + + } while (timeval_ms() < timeout); + + return retval; +} + +static int swd_connect(struct adiv5_dap *dap) +{ int status; /* FIXME validate transport config ... is the * configured DAP present (check IDCODE)? - * Is *only* one DAP configured? - * - * MUST READ DPIDR */ /* Check if we should reset srst already when connecting, but not if reconnecting. */ @@ -112,49 +370,43 @@ static int swd_connect(struct adiv5_dap *dap) if (jtag_reset_config & RESET_CNCT_UNDER_SRST) { if (jtag_reset_config & RESET_SRST_NO_GATING) - swd_add_reset(1); + adapter_assert_reset(); else LOG_WARNING("\'srst_nogate\' reset_config option is required"); } } - /* Note, debugport_init() does setup too */ - swd->switch_seq(JTAG_TO_SWD); + if (dap_is_multidrop(dap)) + status = swd_connect_multidrop(dap); + else + status = swd_connect_single(dap); - /* Clear link state, including the SELECT cache. */ - dap->do_reconnect = false; - dap_invalidate_cache(dap); + /* IHI 0031E B4.3.2: + * "A WAIT response must not be issued to the ... + * ... writes to the ABORT register" + * swd_clear_sticky_errors() writes to the ABORT register only. + * + * Unfortunately at least Microchip SAMD51/E53/E54 returns WAIT + * in a corner case. Just try if ABORT resolves the problem. + */ + if (status == ERROR_WAIT) { + LOG_WARNING("Connecting DP: stalled AP operation, issuing ABORT"); - swd_queue_dp_read(dap, DP_DPIDR, &dpidr); + dap->do_reconnect = false; - /* force clear all sticky faults */ - swd_clear_sticky_errors(dap); + status = swd_queue_dp_write_inner(dap, DP_ABORT, + DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR); - status = swd_run_inner(dap); + if (status == ERROR_OK) + status = swd_run_inner(dap); + } - if (status == ERROR_OK) { - LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr); - dap->do_reconnect = false; + if (status == ERROR_OK) status = dap_dp_init(dap); - } else - dap->do_reconnect = true; 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) @@ -168,51 +420,31 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) const struct swd_driver *swd = adiv5_dap_swd_driver(dap); assert(swd); - swd->write_reg(swd_cmd(false, false, DP_ABORT), - DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0); - 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); + /* TODO: Send DAPABORT in swd_multidrop_select_inner() + * in the case the multidrop dap is not selected? + * swd_queue_ap_abort() is not currently used anyway... + */ + int retval = swd_multidrop_select(dap); if (retval != ERROR_OK) - dap->select = DP_SELECT_INVALID; + return retval; - return retval; + swd->write_reg(swd_cmd(false, false, DP_ABORT), + DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0); + return check_sync(dap); } 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); + retval = swd_multidrop_select(dap); if (retval != ERROR_OK) return retval; - swd->read_reg(swd_cmd(true, false, reg), data, 0); - - return check_sync(dap); + return swd_queue_dp_read_inner(dap, reg, data); } static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, @@ -225,26 +457,11 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, if (retval != ERROR_OK) return retval; - 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); + retval = swd_multidrop_select(dap); if (retval != ERROR_OK) return retval; - swd->write_reg(swd_cmd(false, false, reg), data, 0); - - return check_sync(dap); + return swd_queue_dp_write_inner(dap, reg, data); } /** Select the AP register bank matching bits 7:4 of reg. */ @@ -260,7 +477,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; @@ -278,11 +495,15 @@ static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg, if (retval != ERROR_OK) return retval; + retval = swd_multidrop_select(dap); + if (retval != ERROR_OK) + return retval; + 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); + swd->read_reg(swd_cmd(true, true, reg), dap->last_read, ap->memaccess_tck); dap->last_read = data; return check_sync(dap); @@ -299,12 +520,17 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg, if (retval != ERROR_OK) return retval; + retval = swd_multidrop_select(dap); + if (retval != ERROR_OK) + return retval; + swd_finish_read(dap); + 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); + swd->write_reg(swd_cmd(false, true, reg), data, ap->memaccess_tck); return check_sync(dap); } @@ -312,7 +538,12 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg, /** Executes all queued DAP operations. */ static int swd_run(struct adiv5_dap *dap) { + int retval = swd_multidrop_select(dap); + if (retval != ERROR_OK) + return retval; + swd_finish_read(dap); + return swd_run_inner(dap); } @@ -320,9 +551,27 @@ static int swd_run(struct adiv5_dap *dap) static void swd_quit(struct adiv5_dap *dap) { const struct swd_driver *swd = adiv5_dap_swd_driver(dap); + static bool done; + + /* There is no difference if the sequence is sent at the last + * or the first swd_quit() call, send it just once */ + if (done) + return; + + done = true; + if (dap_is_multidrop(dap)) { + swd->switch_seq(SWD_TO_DORMANT); + /* Revisit! + * Leaving DPs in dormant state was tested and offers some safety + * against DPs mismatch in case of unintentional use of non-multidrop SWD. + * To put SWJ-DPs to power-on state issue + * swd->switch_seq(DORMANT_TO_JTAG); + */ + } else { + swd->switch_seq(SWD_TO_JTAG); + } - swd->switch_seq(SWD_TO_JTAG); - /* flush the queue before exit */ + /* flush the queue to shift out the sequence before exit */ swd->run(); } @@ -368,9 +617,9 @@ static const struct command_registration swd_handlers[] = { static int swd_select(struct command_context *ctx) { - /* FIXME: only place where global 'jtag_interface' is still needed */ - extern struct jtag_interface *jtag_interface; - const struct swd_driver *swd = jtag_interface->swd; + /* FIXME: only place where global 'adapter_driver' is still needed */ + extern struct adapter_driver *adapter_driver; + const struct swd_driver *swd = adapter_driver->swd_ops; int retval; retval = register_commands(ctx, NULL, swd_handlers);