interesting
/
riscv-openocd
mirror of https://github.com/riscv/riscv-openocd.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

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 <borneo.antonio@gmail.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/6601
Tested-by: jenkins
Reviewed-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>
This commit is contained in:
Antonio Borneo 2021-07-22 15:08:36 +02:00
parent fd0b4dd15f
commit 76d1095231
1 changed files with 181 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

228
src/jtag/drivers/stlink_usb.c
View File

@ -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 */
};