X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=771b46090e175c2d91b694fda67517afb08e286b;hp=01782e5f4322936a666de23cbd617a9bb1f3af9e;hb=cf49e04429202e832ea642d1fdfec9570fd20f17;hpb=08d4411b59dd8bd0e7d8009003b71d23acbf6eee diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index 01782e5f43..771b46090e 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -55,68 +55,220 @@ #include +/* YUK! - but this is currently a global.... */ +extern struct jtag_interface *jtag_interface; +static bool do_sync; + +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(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) + uint32_t *data); + +static void swd_clear_sticky_errors(struct adiv5_dap *dap) { - /* REVISIT status return vs ack ... */ - return swd->read_reg(swd_cmd(true, false, reg), data); + const struct swd_driver *swd = jtag_interface->swd; + assert(swd); + + swd->write_reg(swd_cmd(false, false, DP_ABORT), + STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0); } -static int swd_queue_idcode_read(struct adiv5_dap *dap, - uint8_t *ack, uint32_t *data) +static int swd_run_inner(struct adiv5_dap *dap) { - int status = swd_queue_dp_read(dap, DP_IDCODE, data); - if (status < 0) - return status; - *ack = status; - /* ?? */ + const struct swd_driver *swd = jtag_interface->swd; + int retval; + + retval = swd->run(); + + if (retval != ERROR_OK) { + /* fault response */ + 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); + + /* Clear link state, including the SELECT cache. */ + dap->do_reconnect = false; + dap->select = DP_SELECT_INVALID; + + 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_dp_write)(struct adiv5_dap *dap, unsigned reg, - uint32_t data) +static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) { - /* REVISIT status return vs ack ... */ - return swd->write_reg(swd_cmd(false, false, reg), data); + const struct swd_driver *swd = jtag_interface->swd; + 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 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_ap_read)(struct adiv5_dap *dap, unsigned reg, +static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, uint32_t *data) { - /* REVISIT APSEL ... */ - /* REVISIT status return ... */ - return swd->read_reg(swd_cmd(true, true, reg), 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); + + return check_sync(dap); } -static int (swd_queue_ap_write)(struct adiv5_dap *dap, unsigned reg, +static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, uint32_t data) { - /* REVISIT APSEL ... */ - /* REVISIT status return ... */ - return swd->write_reg(swd_cmd(false, true, reg), 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(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_ap *ap, unsigned reg) +{ + struct adiv5_dap *dap = ap->dap; + uint32_t sel = ((uint32_t)ap->ap_num << 24) + | (reg & 0x000000F0) + | (dap->select & DP_SELECT_DPBANK); + + if (sel == dap->select) + return; + + dap->select = sel; + + swd_queue_dp_write(dap, DP_SELECT, sel); +} + +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; + + 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_abort)(struct adiv5_dap *dap, uint8_t *ack) +static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg, + uint32_t data) { - return ERROR_FAIL; + 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(ap, reg); + swd->write_reg(swd_cmd(false, true, reg), data, ap->memaccess_tck); + + return check_sync(dap); } /** Executes all queued DAP operations. */ static int swd_run(struct adiv5_dap *dap) { - /* for now the SWD interface hard-wires a zero-size queue. */ - - /* FIXME but we still need to check and scrub - * any hardware errors ... - */ - return ERROR_OK; + swd_finish_read(dap); + return swd_run_inner(dap); } const struct dap_ops swd_dap_ops = { - .is_swd = true, - - .queue_idcode_read = swd_queue_idcode_read, .queue_dp_read = swd_queue_dp_read, .queue_dp_write = swd_queue_dp_write, .queue_ap_read = swd_queue_ap_read, @@ -168,6 +320,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" @@ -185,8 +342,6 @@ int dap_to_swd(struct target *target) return retval; } - - COMMAND_HANDLER(handle_swd_wcr) { int retval; @@ -209,7 +364,7 @@ COMMAND_HANDLER(handle_swd_wcr) } command_print(CMD_CTX, - "turnaround=%d, prescale=%d", + "turnaround=%" PRIu32 ", prescale=%" PRIu32, WCR_TO_TRN(wcr), WCR_TO_PRESCALE(wcr)); return ERROR_OK; @@ -280,7 +435,6 @@ static const struct command_registration swd_handlers[] = { static int swd_select(struct command_context *ctx) { - struct target *target = get_current_target(ctx); int retval; retval = register_commands(ctx, NULL, swd_handlers); @@ -288,6 +442,8 @@ static int swd_select(struct command_context *ctx) 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 */ @@ -296,14 +452,20 @@ 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; } /* force DAP into SWD mode (not JTAG) */ - retval = dap_to_swd(target); + /*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; } @@ -313,27 +475,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; - - /* 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 */ - - uint8_t ack; - - status = swd_queue_idcode_read(dap, &ack, &idcode); - - if (status == ERROR_OK) - LOG_INFO("SWD IDCODE %#8.8x", idcode); - - return status; + /* Force the DAP's ops vector for SWD mode. + * messy - is there a better way? */ + arm->dap->ops = &swd_dap_ops; + return swd_connect(dap); } static struct transport swd_transport = {