From: Antonio Borneo Date: Thu, 22 Jul 2021 13:08:36 +0000 (+0200) Subject: stlink: add queue in dap-direct mode X-Git-Tag: v0.12.0-rc1~465 X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=commitdiff_plain;h=76d109523199aa44bb36627ff46d91719566086a stlink: add queue in dap-direct mode Implement a minimalist queue for DP/AP commands and reorganize the code to use it. There is no performance improvement; the queue elements are still sent one-by-one on USB or on TCP during dap_run(). Change-Id: I8353563e59f883624bcc0fbe8b54955e4f27ccfa Signed-off-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/6601 Tested-by: jenkins Reviewed-by: Tarek BOCHKATI --- diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c index e83e84b957..92eb06d3ab 100644 --- a/src/jtag/drivers/stlink_usb.c +++ b/src/jtag/drivers/stlink_usb.c @@ -175,6 +175,43 @@ struct stlink_backend_s { int (*read_trace)(void *handle, const uint8_t *buf, int size); }; +/* TODO: make queue size dynamic */ +/* TODO: don't allocate queue for HLA */ +#define MAX_QUEUE_DEPTH (4096) + +enum queue_cmd { + CMD_DP_READ = 1, + CMD_DP_WRITE, + CMD_AP_READ, + CMD_AP_WRITE, +}; + +struct dap_queue { + enum queue_cmd cmd; + union { + struct dp_r { + unsigned int reg; + struct adiv5_dap *dap; + uint32_t *p_data; + } dp_r; + struct dp_w { + unsigned int reg; + struct adiv5_dap *dap; + uint32_t data; + } dp_w; + struct ap_r { + unsigned int reg; + struct adiv5_ap *ap; + uint32_t *p_data; + } ap_r; + struct ap_w { + unsigned int reg; + struct adiv5_ap *ap; + uint32_t data; + } ap_w; + }; +}; + /** */ struct stlink_usb_handle_s { /** */ @@ -218,6 +255,10 @@ struct stlink_usb_handle_s { /** reconnect is needed next time we try to query the * status */ bool reconnect_pending; + /** queue of dap_direct operations */ + struct dap_queue queue[MAX_QUEUE_DEPTH]; + /** first element available in the queue */ + unsigned int queue_index; }; /** */ @@ -3750,9 +3791,6 @@ static struct hl_interface_param_s stlink_dap_param; static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1); static int stlink_dap_error = ERROR_OK; -static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg, - uint32_t *data); - /** */ static int stlink_dap_record_error(int error) { @@ -3769,6 +3807,11 @@ static int stlink_dap_get_and_clear_error(void) return retval; } +static int stlink_dap_get_error(void) +{ + return stlink_dap_error; +} + static int stlink_usb_open_ap(void *handle, unsigned short apsel) { struct stlink_usb_handle_s *h = handle; @@ -3914,8 +3957,7 @@ static int stlink_dap_op_send_sequence(struct adiv5_dap *dap, enum swd_special_s } /** */ -static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg, - uint32_t *data) +static int stlink_dap_dp_read(struct adiv5_dap *dap, unsigned int reg, uint32_t *data) { uint32_t dummy; int retval; @@ -3926,10 +3968,6 @@ static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg, return ERROR_COMMAND_NOTFOUND; } - retval = stlink_dap_check_reconnect(dap); - if (retval != ERROR_OK) - return retval; - data = data ? data : &dummy; if (stlink_dap_handle->version.flags & STLINK_F_QUIRK_JTAG_DP_READ && stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) { @@ -3944,12 +3982,11 @@ static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg, STLINK_DEBUG_PORT_ACCESS, reg, data); } - return stlink_dap_record_error(retval); + return retval; } /** */ -static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg, - uint32_t data) +static int stlink_dap_dp_write(struct adiv5_dap *dap, unsigned int reg, uint32_t data) { int retval; @@ -3965,31 +4002,22 @@ static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg, data &= ~DP_SELECT_DPBANK; } - retval = stlink_dap_check_reconnect(dap); - if (retval != ERROR_OK) - return retval; - /* ST-Link does not like that we set CORUNDETECT */ if (reg == DP_CTRL_STAT) data &= ~CORUNDETECT; retval = stlink_write_dap_register(stlink_dap_handle, STLINK_DEBUG_PORT_ACCESS, reg, data); - return stlink_dap_record_error(retval); + return retval; } /** */ -static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned reg, - uint32_t *data) +static int stlink_dap_ap_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data) { struct adiv5_dap *dap = ap->dap; uint32_t dummy; int retval; - retval = stlink_dap_check_reconnect(dap); - if (retval != ERROR_OK) - return retval; - if (reg != AP_REG_IDR) { retval = stlink_dap_open_ap(ap->ap_num); if (retval != ERROR_OK) @@ -3999,20 +4027,15 @@ static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned reg, retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg, data); dap->stlink_flush_ap_write = false; - return stlink_dap_record_error(retval); + return retval; } /** */ -static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned reg, - uint32_t data) +static int stlink_dap_ap_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data) { struct adiv5_dap *dap = ap->dap; int retval; - retval = stlink_dap_check_reconnect(dap); - if (retval != ERROR_OK) - return retval; - retval = stlink_dap_open_ap(ap->ap_num); if (retval != ERROR_OK) return retval; @@ -4020,7 +4043,7 @@ static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned reg, retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg, data); dap->stlink_flush_ap_write = true; - return stlink_dap_record_error(retval); + return retval; } /** */ @@ -4030,8 +4053,47 @@ static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) return ERROR_OK; } +static void stlink_dap_run_internal(struct adiv5_dap *dap) +{ + int retval = stlink_dap_check_reconnect(dap); + if (retval != ERROR_OK) { + stlink_dap_handle->queue_index = 0; + stlink_dap_record_error(retval); + return; + } + + unsigned int i = stlink_dap_handle->queue_index; + struct dap_queue *q = &stlink_dap_handle->queue[0]; + + while (i && stlink_dap_get_error() == ERROR_OK) { + switch (q->cmd) { + case CMD_DP_READ: + retval = stlink_dap_dp_read(q->dp_r.dap, q->dp_r.reg, q->dp_r.p_data); + break; + case CMD_DP_WRITE: + retval = stlink_dap_dp_write(q->dp_w.dap, q->dp_w.reg, q->dp_w.data); + break; + case CMD_AP_READ: + retval = stlink_dap_ap_read(q->ap_r.ap, q->ap_r.reg, q->ap_r.p_data); + break; + case CMD_AP_WRITE: + retval = stlink_dap_ap_write(q->ap_w.ap, q->ap_w.reg, q->ap_w.data); + break; + default: + LOG_ERROR("ST-Link: Unknown queue command %d", q->cmd); + retval = ERROR_FAIL; + break; + } + stlink_dap_record_error(retval); + q++; + i--; + } + + stlink_dap_handle->queue_index = 0; +} + /** */ -static int stlink_dap_op_run(struct adiv5_dap *dap) +static int stlink_dap_run_finalize(struct adiv5_dap *dap) { uint32_t ctrlstat, pwrmask; int retval, saved_retval; @@ -4046,7 +4108,7 @@ static int stlink_dap_op_run(struct adiv5_dap *dap) */ if (dap->stlink_flush_ap_write) { dap->stlink_flush_ap_write = false; - retval = stlink_dap_op_queue_dp_read(dap, DP_RDBUFF, NULL); + retval = stlink_dap_dp_read(dap, DP_RDBUFF, NULL); if (retval != ERROR_OK) { dap->do_reconnect = true; return retval; @@ -4055,12 +4117,7 @@ static int stlink_dap_op_run(struct adiv5_dap *dap) saved_retval = stlink_dap_get_and_clear_error(); - retval = stlink_dap_op_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat); - if (retval != ERROR_OK) { - dap->do_reconnect = true; - return retval; - } - retval = stlink_dap_get_and_clear_error(); + retval = stlink_dap_dp_read(dap, DP_CTRL_STAT, &ctrlstat); if (retval != ERROR_OK) { LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect"); dap->do_reconnect = true; @@ -4069,15 +4126,10 @@ static int stlink_dap_op_run(struct adiv5_dap *dap) if (ctrlstat & SSTICKYERR) { if (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) - retval = stlink_dap_op_queue_dp_write(dap, DP_CTRL_STAT, + retval = stlink_dap_dp_write(dap, DP_CTRL_STAT, ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR)); else - retval = stlink_dap_op_queue_dp_write(dap, DP_ABORT, STKERRCLR); - if (retval != ERROR_OK) { - dap->do_reconnect = true; - return retval; - } - retval = stlink_dap_get_and_clear_error(); + retval = stlink_dap_dp_write(dap, DP_ABORT, STKERRCLR); if (retval != ERROR_OK) { dap->do_reconnect = true; return retval; @@ -4092,6 +4144,12 @@ static int stlink_dap_op_run(struct adiv5_dap *dap) return saved_retval; } +static int stlink_dap_op_queue_run(struct adiv5_dap *dap) +{ + stlink_dap_run_internal(dap); + return stlink_dap_run_finalize(dap); +} + /** */ static void stlink_dap_op_quit(struct adiv5_dap *dap) { @@ -4102,6 +4160,82 @@ static void stlink_dap_op_quit(struct adiv5_dap *dap) LOG_ERROR("Error closing APs"); } +static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned int reg, + uint32_t *data) +{ + if (stlink_dap_get_error() != ERROR_OK) + return ERROR_OK; + + unsigned int i = stlink_dap_handle->queue_index++; + struct dap_queue *q = &stlink_dap_handle->queue[i]; + q->cmd = CMD_DP_READ; + q->dp_r.reg = reg; + q->dp_r.dap = dap; + q->dp_r.p_data = data; + + if (i == MAX_QUEUE_DEPTH - 1) + stlink_dap_run_internal(dap); + + return ERROR_OK; +} + +static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned int reg, + uint32_t data) +{ + if (stlink_dap_get_error() != ERROR_OK) + return ERROR_OK; + + unsigned int i = stlink_dap_handle->queue_index++; + struct dap_queue *q = &stlink_dap_handle->queue[i]; + q->cmd = CMD_DP_WRITE; + q->dp_w.reg = reg; + q->dp_w.dap = dap; + q->dp_w.data = data; + + if (i == MAX_QUEUE_DEPTH - 1) + stlink_dap_run_internal(dap); + + return ERROR_OK; +} + +static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned int reg, + uint32_t *data) +{ + if (stlink_dap_get_error() != ERROR_OK) + return ERROR_OK; + + unsigned int i = stlink_dap_handle->queue_index++; + struct dap_queue *q = &stlink_dap_handle->queue[i]; + q->cmd = CMD_AP_READ; + q->ap_r.reg = reg; + q->ap_r.ap = ap; + q->ap_r.p_data = data; + + if (i == MAX_QUEUE_DEPTH - 1) + stlink_dap_run_internal(ap->dap); + + return ERROR_OK; +} + +static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned int reg, + uint32_t data) +{ + if (stlink_dap_get_error() != ERROR_OK) + return ERROR_OK; + + unsigned int i = stlink_dap_handle->queue_index++; + struct dap_queue *q = &stlink_dap_handle->queue[i]; + q->cmd = CMD_AP_WRITE; + q->ap_w.reg = reg; + q->ap_w.ap = ap; + q->ap_w.data = data; + + if (i == MAX_QUEUE_DEPTH - 1) + stlink_dap_run_internal(ap->dap); + + return ERROR_OK; +} + static int stlink_swim_op_srst(void) { return stlink_swim_generate_rst(stlink_dap_handle); @@ -4430,7 +4564,7 @@ static const struct dap_ops stlink_dap_ops = { .queue_ap_read = stlink_dap_op_queue_ap_read, .queue_ap_write = stlink_dap_op_queue_ap_write, .queue_ap_abort = stlink_dap_op_queue_ap_abort, - .run = stlink_dap_op_run, + .run = stlink_dap_op_queue_run, .sync = NULL, /* optional */ .quit = stlink_dap_op_quit, /* optional */ };