X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=c23cee157d53751e0a24f59879f078b76ac83617;hp=1b743657c231427abfad10fe0b95e6f365d945b2;hb=HEAD;hpb=5fd4f2954d6673e5b2fd740da681439425794abc diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index 1b743657c2..1231005884 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -48,6 +48,8 @@ static bool do_sync; static struct adiv5_dap *swd_multidrop_selected_dap; +static bool swd_multidrop_in_swd_state; + static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg, uint32_t data); @@ -82,16 +84,8 @@ static void swd_clear_sticky_errors(struct adiv5_dap *dap) static int swd_run_inner(struct adiv5_dap *dap) { const struct swd_driver *swd = adiv5_dap_swd_driver(dap); - int retval; - - retval = swd->run(); - if (retval != ERROR_OK) { - /* fault response */ - dap->do_reconnect = true; - } - - return retval; + return swd->run(); } static inline int check_sync(struct adiv5_dap *dap) @@ -99,27 +93,30 @@ 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. */ +/** Select the DP register bank */ static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg) { - /* Only register address 0 and 4 are banked. */ - if ((reg & 0xf) > 4) + /* Only register address 0 (ADIv6 only) and 4 are banked. */ + if (is_adiv6(dap) ? (reg & 0xf) > 4 : (reg & 0xf) != 4) return ERROR_OK; - uint64_t sel = (reg & 0x000000F0) >> 4; - if (dap->select != DP_SELECT_INVALID) - sel |= dap->select & ~0xfULL; + uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK; - if (sel == dap->select) + /* ADIv6 ensures DPBANKSEL = 0 after line reset */ + if ((dap->select_valid || (is_adiv6(dap) && dap->select_dpbanksel_valid)) + && (sel == (dap->select & DP_SELECT_DPBANK))) return ERROR_OK; - dap->select = sel; + /* Use the AP part of dap->select regardless of dap->select_valid: + * if !dap->select_valid + * dap->select contains a speculative value likely going to be used + * in the following swd_queue_ap_bankselect() */ + sel |= (uint32_t)(dap->select & SELECT_AP_MASK); - int retval = swd_queue_dp_write_inner(dap, DP_SELECT, (uint32_t)sel); - if (retval != ERROR_OK) - dap->select = DP_SELECT_INVALID; + LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, sel); - return retval; + /* dap->select cache gets updated in the following call */ + return swd_queue_dp_write_inner(dap, DP_SELECT, sel); } static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg, @@ -140,31 +137,42 @@ static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg, static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg, uint32_t data) { - int retval; + int retval = ERROR_OK; const struct swd_driver *swd = adiv5_dap_swd_driver(dap); assert(swd); swd_finish_read(dap); if (reg == DP_SELECT) { - dap->select = data & (ADIV5_DP_SELECT_APSEL | ADIV5_DP_SELECT_APBANK | DP_SELECT_DPBANK); + dap->select = data | (dap->select & (0xffffffffull << 32)); swd->write_reg(swd_cmd(false, false, reg), data, 0); retval = check_sync(dap); - if (retval != ERROR_OK) - dap->select = DP_SELECT_INVALID; + dap->select_valid = (retval == ERROR_OK); + dap->select_dpbanksel_valid = dap->select_valid; return retval; } - retval = swd_queue_dp_bankselect(dap, reg); - if (retval != ERROR_OK) - return retval; + if (reg == DP_SELECT1) + dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull); - swd->write_reg(swd_cmd(false, false, reg), data, 0); + /* DP_ABORT write is not banked. + * Prevent writing DP_SELECT before as it would fail on locked up DP */ + if (reg != DP_ABORT) + retval = swd_queue_dp_bankselect(dap, reg); - return check_sync(dap); + if (retval == ERROR_OK) { + swd->write_reg(swd_cmd(false, false, reg), data, 0); + + retval = check_sync(dap); + } + + if (reg == DP_SELECT1) + dap->select1_valid = (retval == ERROR_OK); + + return retval; } @@ -176,20 +184,26 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr assert(dap_is_multidrop(dap)); - swd_send_sequence(dap, LINE_RESET); - /* From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset - * sequence": - * - line reset sets DP_SELECT_DPBANK to zero; - * - read of DP_DPIDR takes the connection out of reset; - * - write of DP_TARGETSEL keeps the connection in reset; - * - other accesses return protocol error (SWDIO not driven by target). - * - * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to - * skip the write to DP_SELECT, avoiding the protocol error. Set again - * dap->select to DP_SELECT_INVALID because the rest of the register is - * unknown after line reset. + /* Send JTAG_TO_DORMANT and DORMANT_TO_SWD just once + * and then use shorter LINE_RESET until communication fails */ + if (!swd_multidrop_in_swd_state) { + swd_send_sequence(dap, JTAG_TO_DORMANT); + swd_send_sequence(dap, DORMANT_TO_SWD); + } else { + swd_send_sequence(dap, LINE_RESET); + } + + /* + * Zero dap->select and set dap->select_dpbanksel_valid + * to skip the write to DP_SELECT before DPIDR read, avoiding + * the protocol error. + * Clear the other validity flags because the rest of the DP + * SELECT and SELECT1 registers is unknown after line reset. */ dap->select = 0; + dap->select_dpbanksel_valid = true; + dap->select_valid = false; + dap->select1_valid = false; retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel); if (retval != ERROR_OK) @@ -209,8 +223,6 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr return retval; } - dap->select = DP_SELECT_INVALID; - retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr); if (retval != ERROR_OK) return retval; @@ -238,6 +250,7 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr LOG_DEBUG_IO("Selected DP_TARGETSEL 0x%08" PRIx32, dap->multidrop_targetsel); swd_multidrop_selected_dap = dap; + swd_multidrop_in_swd_state = true; if (dpidr_ptr) *dpidr_ptr = dpidr; @@ -267,15 +280,15 @@ static int swd_multidrop_select(struct adiv5_dap *dap) swd_multidrop_selected_dap = NULL; if (retry > 3) { LOG_ERROR("Failed to select multidrop %s", adiv5_dap_name(dap)); + dap->do_reconnect = true; return retval; } LOG_DEBUG("Failed to select multidrop %s, retrying...", adiv5_dap_name(dap)); - /* we going to retry localy, do not ask for full reconnect */ - dap->do_reconnect = false; } + dap->do_reconnect = false; return retval; } @@ -287,8 +300,9 @@ static int swd_connect_multidrop(struct adiv5_dap *dap) int64_t timeout = timeval_ms() + 500; do { - swd_send_sequence(dap, JTAG_TO_DORMANT); - swd_send_sequence(dap, DORMANT_TO_SWD); + /* Do not make any assumptions about SWD state in case of reconnect */ + if (dap->do_reconnect) + swd_multidrop_in_swd_state = false; /* Clear link state, including the SELECT cache. */ dap->do_reconnect = false; @@ -299,6 +313,7 @@ static int swd_connect_multidrop(struct adiv5_dap *dap) if (retval == ERROR_OK) break; + swd_multidrop_in_swd_state = false; alive_sleep(1); } while (timeval_ms() < timeout); @@ -309,6 +324,7 @@ static int swd_connect_multidrop(struct adiv5_dap *dap) return retval; } + swd_multidrop_in_swd_state = true; LOG_INFO("SWD DPIDR 0x%08" PRIx32 ", DLPIDR 0x%08" PRIx32, dpidr, dlpidr); @@ -335,19 +351,20 @@ static int swd_connect_single(struct adiv5_dap *dap) /* The sequences to enter in SWD (JTAG_TO_SWD and DORMANT_TO_SWD) end * with a SWD line reset sequence (50 clk with SWDIO high). - * From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset - * sequence": - * - line reset sets DP_SELECT_DPBANK to zero; + * From ARM IHI 0031F ADIv5.2 and ARM IHI 0074C ADIv6.0, + * chapter B4.3.3 "Connection and line reset sequence": + * - DPv3 (ADIv6) only: line reset sets DP_SELECT_DPBANK to zero; * - read of DP_DPIDR takes the connection out of reset; * - write of DP_TARGETSEL keeps the connection in reset; * - other accesses return protocol error (SWDIO not driven by target). * - * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to - * skip the write to DP_SELECT, avoiding the protocol error. Set again - * dap->select to DP_SELECT_INVALID because the rest of the register is - * unknown after line reset. + * dap_invalidate_cache() sets dap->select to zero and all validity + * flags to invalid. Set dap->select_dpbanksel_valid only + * to skip the write to DP_SELECT, avoiding the protocol error. + * Read DP_DPIDR to get out of reset. */ - dap->select = 0; + dap->select_dpbanksel_valid = true; + retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr); if (retval == ERROR_OK) { retval = swd_run_inner(dap); @@ -360,8 +377,6 @@ static int swd_connect_single(struct adiv5_dap *dap) dap->switch_through_dormant = !dap->switch_through_dormant; } while (timeval_ms() < timeout); - dap->select = DP_SELECT_INVALID; - if (retval != ERROR_OK) { LOG_ERROR("Error connecting DP: cannot read IDR"); return retval; @@ -386,6 +401,13 @@ static int swd_connect_single(struct adiv5_dap *dap) return retval; } +static int swd_pre_connect(struct adiv5_dap *dap) +{ + swd_multidrop_in_swd_state = false; + + return ERROR_OK; +} + static int swd_connect(struct adiv5_dap *dap) { int status; @@ -494,49 +516,55 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, return swd_queue_dp_write_inner(dap, reg, data); } -/** Select the AP register bank matching bits 7:4 of reg. */ +/** Select the AP register bank */ static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg) { int retval; struct adiv5_dap *dap = ap->dap; uint64_t sel; - if (is_adiv6(dap)) { + if (is_adiv6(dap)) sel = ap->ap_num | (reg & 0x00000FF0); - if (sel == (dap->select & ~0xfULL)) - return ERROR_OK; - - if (dap->select != DP_SELECT_INVALID) - sel |= dap->select & 0xf; - dap->select = sel; - LOG_DEBUG("AP BANKSEL: %" PRIx64, sel); - - retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel); + else + sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK); - if (retval == ERROR_OK && dap->asize > 32) - retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32)); + uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK; - if (retval != ERROR_OK) - dap->select = DP_SELECT_INVALID; + bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull); + bool set_select1 = is_adiv6(dap) && dap->asize > 32 + && (!dap->select1_valid + || sel_diff & (0xffffffffull << 32)); - return retval; + if (set_select && set_select1) { + /* Prepare DP bank for DP_SELECT1 now to save one write */ + sel |= (DP_SELECT1 & 0x000000f0) >> 4; + } else { + /* Use the DP part of dap->select regardless of dap->select_valid: + * if !dap->select_valid + * dap->select contains a speculative value likely going to be used + * in the following swd_queue_dp_bankselect(). + * Moreover dap->select_valid should never be false here as a DP bank + * is always selected before selecting an AP bank */ + sel |= dap->select & DP_SELECT_DPBANK; } - /* ADIv5 */ - sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK); - if (dap->select != DP_SELECT_INVALID) - sel |= dap->select & DP_SELECT_DPBANK; + if (set_select) { + LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel); - if (sel == dap->select) - return ERROR_OK; + retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel); + if (retval != ERROR_OK) + return retval; + } - dap->select = sel; + if (set_select1) { + LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32)); - retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel); - if (retval != ERROR_OK) - dap->select = DP_SELECT_INVALID; + retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32)); + if (retval != ERROR_OK) + return retval; + } - return retval; + return ERROR_OK; } static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg, @@ -599,7 +627,13 @@ static int swd_run(struct adiv5_dap *dap) swd_finish_read(dap); - return swd_run_inner(dap); + retval = swd_run_inner(dap); + if (retval != ERROR_OK) { + /* fault response */ + dap->do_reconnect = true; + } + + return retval; } /** Put the SWJ-DP back to JTAG mode */ @@ -615,7 +649,12 @@ static void swd_quit(struct adiv5_dap *dap) done = true; if (dap_is_multidrop(dap)) { + /* Emit the switch seq to dormant state regardless the state mirrored + * in swd_multidrop_in_swd_state. Doing so ensures robust operation + * in the case the variable is out of sync. + * Sending SWD_TO_DORMANT makes no change if the DP is already dormant. */ swd->switch_seq(SWD_TO_DORMANT); + swd_multidrop_in_swd_state = false; /* Revisit! * Leaving DPs in dormant state was tested and offers some safety * against DPs mismatch in case of unintentional use of non-multidrop SWD. @@ -636,6 +675,7 @@ static void swd_quit(struct adiv5_dap *dap) } const struct dap_ops swd_dap_ops = { + .pre_connect_init = swd_pre_connect, .connect = swd_connect, .send_sequence = swd_send_sequence, .queue_dp_read = swd_queue_dp_read, @@ -666,6 +706,7 @@ static const struct command_registration swd_commands[] = { "['-ignore-version'] " "['-ignore-bypass'] " "['-ircapture' number] " + "['-ir-bypass' number] " "['-mask' number]", }, COMMAND_REGISTRATION_DONE