X-Git-Url: https://review.openocd.org/gitweb?a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=68350424372bf6905fa38f456b47e7d23bb308c2;hb=d74f11dcd43914702ea7b011c4bbb7da8d4a2f5c;hp=f1fca40aed37832c853d610f8d023d3768226d75;hpb=44554698472aae5b02620eb91837118ada4d2bfe;p=openocd.git diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index f1fca40aed..6835042437 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -53,22 +53,33 @@ #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); @@ -93,59 +104,246 @@ 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 = 0xdeadbeef; - int status; + assert(swd); - /* FIXME validate transport config ... is the - * configured DAP present (check IDCODE)? - * Is *only* one DAP configured? - * - * MUST READ DPIDR - */ + int retval = swd_queue_dp_bankselect(dap, reg); + if (retval != ERROR_OK) + return retval; - /* Check if we should reset srst already when connecting, but not if reconnecting. */ - if (!dap->do_reconnect) { - enum reset_types jtag_reset_config = jtag_get_reset_config(); + swd->read_reg(swd_cmd(true, false, reg), data, 0); - if (jtag_reset_config & RESET_CNCT_UNDER_SRST) { - if (jtag_reset_config & RESET_SRST_NO_GATING) - adapter_assert_reset(); - else - LOG_WARNING("\'srst_nogate\' reset_config option is required"); + 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 { - /* Note, debugport_init() does setup too */ - swd->switch_seq(JTAG_TO_SWD); + 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; - status = swd_queue_dp_read(dap, DP_DPIDR, &dpidr); - if (status == ERROR_OK) { - status = swd_run_inner(dap); - if (status == ERROR_OK) + 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 { + if (dap->switch_through_dormant) { + swd_send_sequence(dap, JTAG_TO_DORMANT); + swd_send_sequence(dap, DORMANT_TO_SWD); + } else { + 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); + dap->switch_through_dormant = !dap->switch_through_dormant; } while (timeval_ms() < timeout); - if (status != ERROR_OK) { + if (retval != ERROR_OK) { LOG_ERROR("Error connecting DP: cannot read IDR"); - return status; + return retval; } - LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr); + LOG_INFO("SWD DPIDR 0x%08" PRIx32, dpidr); do { dap->do_reconnect = false; @@ -153,14 +351,42 @@ static int swd_connect(struct adiv5_dap *dap) /* force clear all sticky faults */ swd_clear_sticky_errors(dap); - status = swd_run_inner(dap); - if (status != ERROR_WAIT) + 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)? + */ + + /* Check if we should reset srst already when connecting, but not if reconnecting. */ + if (!dap->do_reconnect) { + enum reset_types jtag_reset_config = jtag_get_reset_config(); + + if (jtag_reset_config & RESET_CNCT_UNDER_SRST) { + if (jtag_reset_config & RESET_SRST_NO_GATING) + adapter_assert_reset(); + else + LOG_WARNING("\'srst_nogate\' reset_config option is required"); + } + } + + if (dap_is_multidrop(dap)) + status = swd_connect_multidrop(dap); + else + status = swd_connect_single(dap); + /* IHI 0031E B4.3.2: * "A WAIT response must not be issued to the ... * ... writes to the ABORT register" @@ -174,9 +400,11 @@ static int swd_connect(struct adiv5_dap *dap) dap->do_reconnect = false; - swd->write_reg(swd_cmd(false, false, DP_ABORT), - DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0); - status = swd_run_inner(dap); + status = swd_queue_dp_write_inner(dap, DP_ABORT, + DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR); + + if (status == ERROR_OK) + status = swd_run_inner(dap); } if (status == ERROR_OK) @@ -185,19 +413,6 @@ static int swd_connect(struct adiv5_dap *dap) 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) @@ -211,51 +426,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, @@ -268,26 +463,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. */ @@ -303,7 +483,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; @@ -321,6 +501,10 @@ 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; @@ -342,7 +526,12 @@ 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; @@ -355,7 +544,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); } @@ -363,9 +557,32 @@ 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 { + if (dap->switch_through_dormant) { + swd->switch_seq(SWD_TO_DORMANT); + 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(); }