stlink: collapse consecutive mem AP r/w in a single command

Detect a sequence of memory AP operations that can be issued as a
single stlink command.
This improves the data throughput during memory transfer.

Change-Id: Ifa4488513346fc7cd0c9317b7d24ef510ccfd959
Signed-off-by: Antonio Borneo <borneo.antonio@gmail.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/6604
Tested-by: jenkins
Reviewed-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>
This commit is contained in:
Antonio Borneo 2021-07-25 17:31:53 +02:00
parent 7920110665
commit da15f9f8c2
1 changed files with 94 additions and 42 deletions

View File

@ -4132,6 +4132,95 @@ static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
return ERROR_OK; return ERROR_OK;
} }
static int stlink_usb_buf_rw_segment(void *handle, const struct dap_queue *q, unsigned int count)
{
uint32_t bufsize = count * CMD_MEM_AP_2_SIZE(q[0].cmd);
uint8_t buf[bufsize];
uint8_t ap_num = q[0].mem_ap.ap->ap_num;
uint32_t addr = q[0].mem_ap.addr;
uint32_t csw = q[0].mem_ap.csw;
int retval = stlink_dap_open_ap(ap_num);
if (retval != ERROR_OK)
return retval;
switch (q[0].cmd) {
case CMD_MEM_AP_WRITE8:
for (unsigned int i = 0; i < count; i++)
buf[i] = q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 3);
return stlink_usb_write_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
case CMD_MEM_AP_WRITE16:
for (unsigned int i = 0; i < count; i++)
h_u16_to_le(&buf[2 * i], q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 2));
return stlink_usb_write_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
case CMD_MEM_AP_WRITE32:
for (unsigned int i = 0; i < count; i++)
h_u32_to_le(&buf[4 * i], q[i].mem_ap.data);
return stlink_usb_write_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
case CMD_MEM_AP_READ8:
retval = stlink_usb_read_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
if (retval == ERROR_OK)
for (unsigned int i = 0; i < count; i++)
*q[i].mem_ap.p_data = buf[i] << 8 * (q[i].mem_ap.addr & 3);
return retval;
case CMD_MEM_AP_READ16:
retval = stlink_usb_read_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
if (retval == ERROR_OK)
for (unsigned int i = 0; i < count; i++)
*q[i].mem_ap.p_data = le_to_h_u16(&buf[2 * i]) << 8 * (q[i].mem_ap.addr & 2);
return retval;
case CMD_MEM_AP_READ32:
retval = stlink_usb_read_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
if (retval == ERROR_OK)
for (unsigned int i = 0; i < count; i++)
*q[i].mem_ap.p_data = le_to_h_u32(&buf[4 * i]);
return retval;
default:
return ERROR_FAIL;
};
}
static int stlink_usb_count_buf_rw_queue(const struct dap_queue *q, unsigned int len)
{
uint32_t incr = CMD_MEM_AP_2_SIZE(q[0].cmd);
unsigned int len_max;
if (incr == 1)
len_max = stlink_usb_block(stlink_dap_handle);
else
len_max = STLINK_MAX_RW16_32 / incr;
if (len > len_max)
len = len_max;
for (unsigned int i = 1; i < len; i++)
if (q[i].cmd != q[0].cmd ||
q[i].mem_ap.ap != q[0].mem_ap.ap ||
q[i].mem_ap.csw != q[0].mem_ap.csw ||
q[i].mem_ap.addr != q[i - 1].mem_ap.addr + incr)
return i;
return len;
}
static int stlink_usb_mem_rw_queue(void *handle, const struct dap_queue *q, unsigned int len, unsigned int *skip)
{
unsigned int count = stlink_usb_count_buf_rw_queue(q, len);
int retval = stlink_usb_buf_rw_segment(handle, q, count);
if (retval != ERROR_OK)
return retval;
*skip = count;
return ERROR_OK;
}
static void stlink_dap_run_internal(struct adiv5_dap *dap) static void stlink_dap_run_internal(struct adiv5_dap *dap)
{ {
int retval = stlink_dap_check_reconnect(dap); int retval = stlink_dap_check_reconnect(dap);
@ -4143,9 +4232,10 @@ static void stlink_dap_run_internal(struct adiv5_dap *dap)
unsigned int i = stlink_dap_handle->queue_index; unsigned int i = stlink_dap_handle->queue_index;
struct dap_queue *q = &stlink_dap_handle->queue[0]; struct dap_queue *q = &stlink_dap_handle->queue[0];
uint8_t buf[4];
while (i && stlink_dap_get_error() == ERROR_OK) { while (i && stlink_dap_get_error() == ERROR_OK) {
unsigned int skip = 1;
switch (q->cmd) { switch (q->cmd) {
case CMD_DP_READ: case CMD_DP_READ:
retval = stlink_dap_dp_read(q->dp_r.dap, q->dp_r.reg, q->dp_r.p_data); retval = stlink_dap_dp_read(q->dp_r.dap, q->dp_r.reg, q->dp_r.p_data);
@ -4164,50 +4254,12 @@ static void stlink_dap_run_internal(struct adiv5_dap *dap)
break; break;
case CMD_MEM_AP_READ8: case CMD_MEM_AP_READ8:
retval = stlink_dap_open_ap(q->mem_ap.ap->ap_num);
if (retval == ERROR_OK)
retval = stlink_usb_read_mem8(stlink_dap_handle, q->mem_ap.ap->ap_num, q->mem_ap.csw, q->mem_ap.addr,
1, buf);
if (retval == ERROR_OK)
*q->mem_ap.p_data = *buf << 8 * (q->mem_ap.addr & 3);
break;
case CMD_MEM_AP_READ16: case CMD_MEM_AP_READ16:
retval = stlink_dap_open_ap(q->mem_ap.ap->ap_num);
if (retval == ERROR_OK)
retval = stlink_usb_read_mem16(stlink_dap_handle, q->mem_ap.ap->ap_num, q->mem_ap.csw, q->mem_ap.addr,
2, buf);
if (retval == ERROR_OK)
*q->mem_ap.p_data = le_to_h_u16(buf) << 8 * (q->mem_ap.addr & 2);
break;
case CMD_MEM_AP_READ32: case CMD_MEM_AP_READ32:
retval = stlink_dap_open_ap(q->mem_ap.ap->ap_num);
if (retval == ERROR_OK)
retval = stlink_usb_read_mem32(stlink_dap_handle, q->mem_ap.ap->ap_num, q->mem_ap.csw, q->mem_ap.addr,
4, buf);
if (retval == ERROR_OK)
*q->mem_ap.p_data = le_to_h_u32(buf);
break;
case CMD_MEM_AP_WRITE8: case CMD_MEM_AP_WRITE8:
*buf = q->mem_ap.data >> 8 * (q->mem_ap.addr & 3);
retval = stlink_dap_open_ap(q->mem_ap.ap->ap_num);
if (retval == ERROR_OK)
retval = stlink_usb_write_mem8(stlink_dap_handle, q->mem_ap.ap->ap_num, q->mem_ap.csw, q->mem_ap.addr,
1, buf);
break;
case CMD_MEM_AP_WRITE16: case CMD_MEM_AP_WRITE16:
h_u16_to_le(buf, q->mem_ap.data >> 8 * (q->mem_ap.addr & 2));
retval = stlink_dap_open_ap(q->mem_ap.ap->ap_num);
if (retval == ERROR_OK)
retval = stlink_usb_write_mem16(stlink_dap_handle, q->mem_ap.ap->ap_num, q->mem_ap.csw, q->mem_ap.addr,
2, buf);
break;
case CMD_MEM_AP_WRITE32: case CMD_MEM_AP_WRITE32:
h_u32_to_le(buf, q->mem_ap.data); retval = stlink_usb_mem_rw_queue(stlink_dap_handle, q, i, &skip);
retval = stlink_dap_open_ap(q->mem_ap.ap->ap_num);
if (retval == ERROR_OK)
retval = stlink_usb_write_mem32(stlink_dap_handle, q->mem_ap.ap->ap_num, q->mem_ap.csw, q->mem_ap.addr,
4, buf);
break; break;
default: default:
@ -4216,8 +4268,8 @@ static void stlink_dap_run_internal(struct adiv5_dap *dap)
break; break;
} }
stlink_dap_record_error(retval); stlink_dap_record_error(retval);
q++; q += skip;
i--; i -= skip;
} }
stlink_dap_handle->queue_index = 0; stlink_dap_handle->queue_index = 0;