X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=c080964d07a1275840406af7ad0e8a51143542aa;hp=31c219e59643d002ed575716233d9bd42729d985;hb=830d0c55c0920606366a15560d1945f1e1942744;hpb=d2bb14e36a03cc187ca090a0da418acccb4098b4 diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index 31c219e596..c080964d07 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -63,67 +63,114 @@ static void swd_finish_read(struct adiv5_dap *dap) { const struct swd_driver *swd = jtag_interface->swd; if (dap->last_read != NULL) { - swd->read_reg(dap, swd_cmd(true, false, DP_RDBUFF), 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; assert(swd); - swd->write_reg(dap, swd_cmd(false, false, DP_ABORT), - STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR); + 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; + int retval; - int retval = swd->run(dap); + retval = swd->run(); 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(JTAG_TO_SWD); + + /* Make sure we don't try to perform any other accesses before the DPIDR read. */ + dap->do_reconnect = false; + dap->select = 0; + + 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; + } else + dap->do_reconnect = true; + + 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) + return swd_connect(dap); + + return ERROR_OK; +} + static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) { const struct swd_driver *swd = jtag_interface->swd; assert(swd); - swd->write_reg(dap, swd_cmd(false, false, DP_ABORT), - DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR); + 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) { - uint32_t select_dp_bank = (reg & 0x000000F0) >> 4; - if (reg == DP_SELECT) return; - if (select_dp_bank == dap->dp_bank_value) + uint32_t select_dp_bank = (reg & 0x000000F0) >> 4; + uint32_t select = select_dp_bank + | (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK)); + + if (select == dap->select) return; - dap->dp_bank_value = select_dp_bank; - select_dp_bank |= dap->ap_current | dap->ap_bank_value; + dap->select = select; - swd_queue_dp_write(dap, DP_SELECT, select_dp_bank); + swd_queue_dp_write(dap, DP_SELECT, select); } static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, @@ -132,62 +179,83 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, 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(dap, swd_cmd(true, false, reg), data); + swd->read_reg(swd_cmd(true, false, reg), data, 0); return check_sync(dap); } - static int swd_queue_dp_write(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_finish_read(dap); swd_queue_dp_bankselect(dap, reg); - swd->write_reg(dap, swd_cmd(false, false, reg), data); + swd->write_reg(swd_cmd(false, false, reg), data, 0); return check_sync(dap); } /** Select the AP register bank matching bits 7:4 of reg. */ -static void swd_queue_ap_bankselect(struct adiv5_dap *dap, unsigned reg) +static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg) { - uint32_t select_ap_bank = reg & 0x000000F0; + struct adiv5_dap *dap = ap->dap; + uint32_t select = ((uint32_t)ap->ap_num << 24) + | (reg & 0x000000F0) + | (dap->select & DP_SELECT_DPBANK); - if (select_ap_bank == dap->ap_bank_value) + if (select == dap->select) return; - dap->ap_bank_value = select_ap_bank; - select_ap_bank |= dap->ap_current | dap->dp_bank_value; + dap->select = select; - swd_queue_dp_write(dap, DP_SELECT, select_ap_bank); + swd_queue_dp_write(dap, DP_SELECT, select); } -static int swd_queue_ap_read(struct adiv5_dap *dap, unsigned reg, +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); - swd_queue_ap_bankselect(dap, reg); - swd->read_reg(dap, swd_cmd(true, true, reg), dap->last_read); + struct adiv5_dap *dap = ap->dap; + + 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); dap->last_read = data; return check_sync(dap); } -static int swd_queue_ap_write(struct adiv5_dap *dap, 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; + + int retval = swd_check_reconnect(dap); + if (retval != ERROR_OK) + return retval; + swd_finish_read(dap); - swd_queue_ap_bankselect(dap, reg); - swd->write_reg(dap, swd_cmd(false, true, reg), data); + swd_queue_ap_bankselect(ap, reg); + swd->write_reg(swd_cmd(false, true, reg), data, ap->memaccess_tck); return check_sync(dap); } @@ -200,8 +268,6 @@ static int swd_run(struct adiv5_dap *dap) } const struct dap_ops swd_dap_ops = { - .is_swd = true, - .queue_dp_read = swd_queue_dp_read, .queue_dp_write = swd_queue_dp_write, .queue_ap_read = swd_queue_ap_read, @@ -253,6 +319,11 @@ 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" @@ -380,7 +451,7 @@ static int swd_select(struct command_context *ctx) return ERROR_FAIL; } - retval = swd->init(1); + retval = swd->init(); if (retval != ERROR_OK) { LOG_DEBUG("can't init SWD driver"); return retval; @@ -403,36 +474,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); - - /* this is a workaround to get polling working */ - jtag_add_reset(0, 0); - - return status; + return swd_connect(dap); } static struct transport swd_transport = {