X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=c23cee157d53751e0a24f59879f078b76ac83617;hp=a6aada32611ba26884ebf4e739f113e26ead3e3c;hb=d2033997871f8561d1406550c12508461f63de48;hpb=7e6445109763d47707e1d31620b3733b5137faba diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index a6aada3261..c23cee157d 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -53,36 +53,45 @@ #include -/* YUK! - but this is currently a global.... */ -extern struct jtag_interface *jtag_interface; +/* 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 = jtag_interface->swd; - if (dap->last_read != NULL) { + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); + 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 = jtag_interface->swd; + 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); } static int swd_run_inner(struct adiv5_dap *dap) { - const struct swd_driver *swd = jtag_interface->swd; + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); int retval; retval = swd->run(); @@ -95,16 +104,264 @@ static int swd_run_inner(struct adiv5_dap *dap) return retval; } +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); + 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) { - uint32_t dpidr; 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. */ @@ -113,40 +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 */ - jtag_interface->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->select = DP_SELECT_INVALID; + /* 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; - } else - dap->do_reconnect = true; + if (status == ERROR_OK) + status = dap_dp_init(dap); return status; } -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) @@ -157,68 +417,55 @@ static int swd_check_reconnect(struct adiv5_dap *dap) static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) { - const struct swd_driver *swd = jtag_interface->swd; + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); assert(swd); - swd->write_reg(swd_cmd(false, false, DP_ABORT), + /* 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) + return retval; + + 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 void swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg) -{ - /* Only register address 4 is banked. */ - if ((reg & 0xf) != 4) - return; - - 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; - - dap->select = sel; - - swd_queue_dp_write(dap, DP_SELECT, sel); -} - static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, uint32_t *data) { - const struct swd_driver *swd = jtag_interface->swd; - assert(swd); - int retval = swd_check_reconnect(dap); if (retval != ERROR_OK) return retval; - swd_queue_dp_bankselect(dap, reg); - swd->read_reg(swd_cmd(true, false, reg), data, 0); + retval = swd_multidrop_select(dap); + if (retval != ERROR_OK) + return retval; - 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, uint32_t data) { - const struct swd_driver *swd = jtag_interface->swd; + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); assert(swd); int retval = swd_check_reconnect(dap); if (retval != ERROR_OK) return retval; - swd_finish_read(dap); - swd_queue_dp_bankselect(dap, reg); - swd->write_reg(swd_cmd(false, false, reg), data, 0); + retval = swd_multidrop_select(dap); + if (retval != ERROR_OK) + return retval; - return check_sync(dap); + return swd_queue_dp_write_inner(dap, reg, data); } /** Select the AP register bank matching bits 7:4 of reg. */ -static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg) +static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg) { struct adiv5_dap *dap = ap->dap; uint32_t sel = ((uint32_t)ap->ap_num << 24) @@ -226,27 +473,37 @@ static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg) | (dap->select & DP_SELECT_DPBANK); if (sel == dap->select) - return; + return ERROR_OK; dap->select = sel; - 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; + + return retval; } static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg, uint32_t *data) { - const struct swd_driver *swd = jtag_interface->swd; - assert(swd); - struct adiv5_dap *dap = ap->dap; + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); + assert(swd); int retval = swd_check_reconnect(dap); if (retval != ERROR_OK) return retval; - swd_queue_ap_bankselect(ap, reg); - swd->read_reg(swd_cmd(true, true, reg), dap->last_read, ap->memaccess_tck); + 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); dap->last_read = data; return check_sync(dap); @@ -255,18 +512,25 @@ static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg, static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg, uint32_t data) { - const struct swd_driver *swd = jtag_interface->swd; - assert(swd); - struct adiv5_dap *dap = ap->dap; + 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_multidrop_select(dap); + if (retval != ERROR_OK) + return retval; + swd_finish_read(dap); - swd_queue_ap_bankselect(ap, reg); - swd->write_reg(swd_cmd(false, true, reg), data, ap->memaccess_tck); + + 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); return check_sync(dap); } @@ -274,84 +538,55 @@ 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); } +/** Put the SWJ-DP back to JTAG mode */ +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); + } + + /* flush the queue to shift out the sequence before exit */ + swd->run(); +} + const struct dap_ops swd_dap_ops = { + .connect = swd_connect, + .send_sequence = swd_send_sequence, .queue_dp_read = swd_queue_dp_read, .queue_dp_write = swd_queue_dp_write, .queue_ap_read = swd_queue_ap_read, .queue_ap_write = swd_queue_ap_write, .queue_ap_abort = swd_queue_ap_abort, .run = swd_run, + .quit = swd_quit, }; -/* - * This represents the bits which must be sent out on TMS/SWDIO to - * switch a DAP implemented using an SWJ-DP module into SWD mode. - * These bits are stored (and transmitted) LSB-first. - * - * See the DAP-Lite specification, section 2.2.5 for information - * about making the debug link select SWD or JTAG. (Similar info - * is in a few other ARM documents.) - */ -static const uint8_t jtag2swd_bitseq[] = { - /* More than 50 TCK/SWCLK cycles with TMS/SWDIO high, - * putting both JTAG and SWD logic into reset state. - */ - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - /* Switching sequence enables SWD and disables JTAG - * NOTE: bits in the DP's IDCODE may expose the need for - * an old/obsolete/deprecated sequence (0xb6 0xed). - */ - 0x9e, 0xe7, - /* More than 50 TCK/SWCLK cycles with TMS/SWDIO high, - * putting both JTAG and SWD logic into reset state. - */ - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, -}; - -/** - * Put the debug link into SWD mode, if the target supports it. - * The link's initial mode may be either JTAG (for example, - * with SWJ-DP after reset) or SWD. - * - * @param target Enters SWD mode (if possible). - * - * Note that targets using the JTAG-DP do not support SWD, and that - * some targets which could otherwise support it may have have been - * configured to disable SWD signaling - * - * @return ERROR_OK or else a fault code. - */ -int dap_to_swd(struct target *target) -{ - struct arm *arm = target_to_arm(target); - int retval; - - if (!arm->dap) { - LOG_ERROR("SWD mode is not available"); - return ERROR_FAIL; - } - - LOG_DEBUG("Enter SWD mode"); - - /* REVISIT it's ugly to need to make calls to a "jtag" - * subsystem if the link may not be in JTAG mode... - */ - - retval = jtag_add_tms_seq(8 * sizeof(jtag2swd_bitseq), - jtag2swd_bitseq, TAP_INVALID); - if (retval == ERROR_OK) - retval = jtag_execute_queue(); - - /* set up the DAP's ops vector for SWD mode. */ - arm->dap->ops = &swd_dap_ops; - - return retval; -} - static const struct command_registration swd_commands[] = { { /* @@ -375,21 +610,22 @@ static const struct command_registration swd_handlers[] = { .mode = COMMAND_ANY, .help = "SWD command group", .chain = swd_commands, + .usage = "", }, COMMAND_REGISTRATION_DONE }; static int swd_select(struct command_context *ctx) { + /* 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); - if (retval != ERROR_OK) return retval; - const struct swd_driver *swd = jtag_interface->swd; - /* be sure driver is in SWD mode; start * with hardware default TRN (1), it can be changed later */ @@ -404,33 +640,14 @@ static int swd_select(struct command_context *ctx) return retval; } - /* force DAP into SWD mode (not JTAG) */ - /*retval = dap_to_swd(target);*/ - - if (ctx->current_target) { - /* force DAP into SWD mode (not JTAG) */ - struct target *target = get_current_target(ctx); - retval = dap_to_swd(target); - } - return retval; } 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; - /* Force the DAP's ops vector for SWD mode. - * messy - is there a better way? */ - arm->dap->ops = &swd_dap_ops; - /* First connect after init is not reconnecting. */ - dap->do_reconnect = false; - - int retval = swd_connect(dap); - if (retval != ERROR_OK) - LOG_ERROR("SWD connect failed"); - return retval; + /* nothing done here, SWD is initialized + * together with the DAP */ + return ERROR_OK; } static struct transport swd_transport = {