X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fjtag%2Fdrivers%2Fstlink_usb.c;h=1cd68c50ba6544314387c40b8d866ee4629f8342;hp=2bbd03b6a4db657a29df90a9faec6176316262d8;hb=7920110665630c8fdc42277ed17f6849597ea63e;hpb=044e0f8c9aec9fe94d77199e64c813fb4336984b diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c index 2bbd03b6a4..1cd68c50ba 100644 --- a/src/jtag/drivers/stlink_usb.c +++ b/src/jtag/drivers/stlink_usb.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -78,7 +79,7 @@ #define STLINK_V2_1_TRACE_EP (2|ENDPOINT_IN) #define STLINK_SG_SIZE (31) -#define STLINK_DATA_SIZE (4096) +#define STLINK_DATA_SIZE (6144) #define STLINK_CMD_SIZE_V2 (16) #define STLINK_CMD_SIZE_V1 (10) @@ -90,20 +91,32 @@ #define STLINK_V3E_PID (0x374E) #define STLINK_V3S_PID (0x374F) #define STLINK_V3_2VCP_PID (0x3753) +#define STLINK_V3E_NO_MSD_PID (0x3754) /* * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and * this limits the bulk packet size and the 8bit read/writes to max 64 bytes. * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes from FW V3J6. + * + * For 16 and 32bit read/writes stlink handles USB packet split and the limit + * is the internal buffer size of 6144 bytes. + * TODO: override ADIv5 layer's tar_autoincr_block that limits the transfer + * to 1024 or 4096 bytes */ -#define STLINK_MAX_RW8 (64) -#define STLINKV3_MAX_RW8 (512) +#define STLINK_MAX_RW8 (64) +#define STLINKV3_MAX_RW8 (512) +#define STLINK_MAX_RW16_32 STLINK_DATA_SIZE +#define STLINK_SWIM_DATA_SIZE STLINK_DATA_SIZE /* "WAIT" responses will be retried (with exponential backoff) at * most this many times before failing to caller. */ #define MAX_WAIT_RETRIES 8 +/* HLA is currently limited at AP#0 and no control on CSW */ +#define STLINK_HLA_AP_NUM 0 +#define STLINK_HLA_CSW 0 + enum stlink_jtag_api_version { STLINK_JTAG_API_V1 = 1, STLINK_JTAG_API_V2, @@ -166,6 +179,67 @@ 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, + + /* + * encode the bytes size in the enum's value. This makes easy to extract it + * with a simple logic AND, by using the macro CMD_MEM_AP_2_SIZE() below + */ + CMD_MEM_AP_READ8 = 0x10 + 1, + CMD_MEM_AP_READ16 = 0x10 + 2, + CMD_MEM_AP_READ32 = 0x10 + 4, + + CMD_MEM_AP_WRITE8 = 0x20 + 1, + CMD_MEM_AP_WRITE16 = 0x20 + 2, + CMD_MEM_AP_WRITE32 = 0x20 + 4, +}; + +#define CMD_MEM_AP_2_SIZE(cmd) ((cmd) & 7) + +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 mem_ap { + uint32_t addr; + struct adiv5_ap *ap; + union { + uint32_t *p_data; + uint32_t data; + }; + uint32_t csw; + } mem_ap; + }; +}; + /** */ struct stlink_usb_handle_s { /** */ @@ -209,6 +283,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; }; /** */ @@ -428,6 +506,7 @@ static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, i /* aliases */ #define STLINK_F_HAS_TARGET_VOLT STLINK_F_HAS_TRACE #define STLINK_F_HAS_FPU_REG STLINK_F_HAS_GETLASTRWSTATUS2 +#define STLINK_F_HAS_CSW STLINK_F_HAS_DPBANKSEL #define STLINK_REGSEL_IS_FPU(x) ((x) > 0x1F) @@ -1256,6 +1335,7 @@ static int stlink_usb_version(void *handle) flags |= STLINK_F_FIX_CLOSE_AP; /* Banked regs (DPv1 & DPv2) support from V2J32 */ + /* Memory R/W supports CSW from V2J32 */ if (h->version.jtag >= 32) flags |= STLINK_F_HAS_DPBANKSEL; @@ -1286,6 +1366,7 @@ static int stlink_usb_version(void *handle) flags |= STLINK_F_FIX_CLOSE_AP; /* Banked regs (DPv1 & DPv2) support from V3J2 */ + /* Memory R/W supports CSW from V3J2 */ if (h->version.jtag >= 2) flags |= STLINK_F_HAS_DPBANKSEL; @@ -1805,7 +1886,7 @@ static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, con unsigned int datalen = 0; int cmdsize = STLINK_CMD_SIZE_V2; - if (len > STLINK_DATA_SIZE) + if (len > STLINK_SWIM_DATA_SIZE) return ERROR_FAIL; if (h->version.stlink == 1) @@ -1838,7 +1919,7 @@ static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint struct stlink_usb_handle_s *h = handle; int res; - if (len > STLINK_DATA_SIZE) + if (len > STLINK_SWIM_DATA_SIZE) return ERROR_FAIL; stlink_usb_init_buffer(handle, h->rx_ep, 0); @@ -2327,8 +2408,8 @@ static int stlink_usb_get_rw_status(void *handle) } /** */ -static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, - uint8_t *buffer) +static int stlink_usb_read_mem8(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint16_t len, uint8_t *buffer) { int res; uint16_t read_len = len; @@ -2336,6 +2417,9 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, assert(handle); + if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW)) + return ERROR_COMMAND_NOTFOUND; + /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */ if (len > stlink_usb_block(h)) { LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h)); @@ -2350,6 +2434,9 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, h->cmdidx += 4; h_u16_to_le(h->cmdbuf+h->cmdidx, len); h->cmdidx += 2; + h->cmdbuf[h->cmdidx++] = ap_num; + h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8); + h->cmdidx += 3; /* we need to fix read length for single bytes */ if (read_len == 1) @@ -2366,14 +2453,17 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, } /** */ -static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, - const uint8_t *buffer) +static int stlink_usb_write_mem8(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint16_t len, const uint8_t *buffer) { int res; struct stlink_usb_handle_s *h = handle; assert(handle); + if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW)) + return ERROR_COMMAND_NOTFOUND; + /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */ if (len > stlink_usb_block(h)) { LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h)); @@ -2388,6 +2478,9 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, h->cmdidx += 4; h_u16_to_le(h->cmdbuf+h->cmdidx, len); h->cmdidx += 2; + h->cmdbuf[h->cmdidx++] = ap_num; + h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8); + h->cmdidx += 3; res = stlink_usb_xfer_noerrcheck(handle, buffer, len); @@ -2398,8 +2491,8 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, } /** */ -static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len, - uint8_t *buffer) +static int stlink_usb_read_mem16(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint16_t len, uint8_t *buffer) { int res; struct stlink_usb_handle_s *h = handle; @@ -2409,6 +2502,14 @@ static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len, if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT)) return ERROR_COMMAND_NOTFOUND; + if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW)) + return ERROR_COMMAND_NOTFOUND; + + if (len > STLINK_MAX_RW16_32) { + LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32); + return ERROR_FAIL; + } + /* data must be a multiple of 2 and half-word aligned */ if (len % 2 || addr % 2) { LOG_DEBUG("Invalid data alignment"); @@ -2423,6 +2524,9 @@ static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len, h->cmdidx += 4; h_u16_to_le(h->cmdbuf+h->cmdidx, len); h->cmdidx += 2; + h->cmdbuf[h->cmdidx++] = ap_num; + h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8); + h->cmdidx += 3; res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len); @@ -2435,8 +2539,8 @@ static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len, } /** */ -static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len, - const uint8_t *buffer) +static int stlink_usb_write_mem16(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint16_t len, const uint8_t *buffer) { int res; struct stlink_usb_handle_s *h = handle; @@ -2446,6 +2550,14 @@ static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len, if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT)) return ERROR_COMMAND_NOTFOUND; + if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW)) + return ERROR_COMMAND_NOTFOUND; + + if (len > STLINK_MAX_RW16_32) { + LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32); + return ERROR_FAIL; + } + /* data must be a multiple of 2 and half-word aligned */ if (len % 2 || addr % 2) { LOG_DEBUG("Invalid data alignment"); @@ -2460,6 +2572,9 @@ static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len, h->cmdidx += 4; h_u16_to_le(h->cmdbuf+h->cmdidx, len); h->cmdidx += 2; + h->cmdbuf[h->cmdidx++] = ap_num; + h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8); + h->cmdidx += 3; res = stlink_usb_xfer_noerrcheck(handle, buffer, len); @@ -2470,14 +2585,22 @@ static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len, } /** */ -static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, - uint8_t *buffer) +static int stlink_usb_read_mem32(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint16_t len, uint8_t *buffer) { int res; struct stlink_usb_handle_s *h = handle; assert(handle); + if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW)) + return ERROR_COMMAND_NOTFOUND; + + if (len > STLINK_MAX_RW16_32) { + LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32); + return ERROR_FAIL; + } + /* data must be a multiple of 4 and word aligned */ if (len % 4 || addr % 4) { LOG_DEBUG("Invalid data alignment"); @@ -2492,6 +2615,9 @@ static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, h->cmdidx += 4; h_u16_to_le(h->cmdbuf+h->cmdidx, len); h->cmdidx += 2; + h->cmdbuf[h->cmdidx++] = ap_num; + h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8); + h->cmdidx += 3; res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len); @@ -2504,14 +2630,22 @@ static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, } /** */ -static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len, - const uint8_t *buffer) +static int stlink_usb_write_mem32(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint16_t len, const uint8_t *buffer) { int res; struct stlink_usb_handle_s *h = handle; assert(handle); + if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW)) + return ERROR_COMMAND_NOTFOUND; + + if (len > STLINK_MAX_RW16_32) { + LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32); + return ERROR_FAIL; + } + /* data must be a multiple of 4 and word aligned */ if (len % 4 || addr % 4) { LOG_DEBUG("Invalid data alignment"); @@ -2526,6 +2660,9 @@ static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len, h->cmdidx += 4; h_u16_to_le(h->cmdbuf+h->cmdidx, len); h->cmdidx += 2; + h->cmdbuf[h->cmdidx++] = ap_num; + h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8); + h->cmdidx += 3; res = stlink_usb_xfer_noerrcheck(handle, buffer, len); @@ -2543,8 +2680,8 @@ static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t addr return max_tar_block; } -static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, - uint32_t count, uint8_t *buffer) +static int stlink_usb_read_ap_mem(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint32_t size, uint32_t count, uint8_t *buffer) { int retval = ERROR_OK; uint32_t bytes_remaining; @@ -2559,7 +2696,6 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, size = 1; while (count) { - bytes_remaining = (size != 1) ? stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h); @@ -2573,7 +2709,6 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, * as 8bit access. */ if (size != 1) { - /* When in jtag mode the stlink uses the auto-increment functionality. * However it expects us to pass the data correctly, this includes * alignment and any page boundaries. We already do this as part of the @@ -2584,11 +2719,10 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, /* we first need to check for any unaligned bytes */ if (addr & (size - 1)) { - uint32_t head_bytes = size - (addr & (size - 1)); - retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer); + retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, head_bytes, buffer); if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { - usleep((1<version.stlink = 3; h->tx_ep = STLINK_V2_1_TX_EP; h->trace_ep = STLINK_V2_1_TRACE_EP; @@ -3479,7 +3629,7 @@ static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode goto error_open; } *fd = h; - h->max_mem_packet = STLINK_DATA_SIZE; + h->max_mem_packet = STLINK_SWIM_DATA_SIZE; return ERROR_OK; } @@ -3490,8 +3640,8 @@ static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode h->max_mem_packet = (1 << 10); uint8_t buffer[4]; - stlink_usb_open_ap(h, 0); - err = stlink_usb_read_mem32(h, CPUID, 4, buffer); + stlink_usb_open_ap(h, STLINK_HLA_AP_NUM); + err = stlink_usb_read_mem32(h, STLINK_HLA_AP_NUM, STLINK_HLA_CSW, CPUID, 4, buffer); if (err == ERROR_OK) { uint32_t cpuid = le_to_h_u32(buffer); int i = (cpuid >> 4) & 0xf; @@ -3720,9 +3870,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) { @@ -3739,6 +3886,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; @@ -3884,8 +4036,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; @@ -3896,10 +4047,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) { @@ -3914,12 +4061,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; @@ -3935,31 +4081,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) @@ -3969,20 +4106,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; @@ -3990,7 +4122,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; } /** */ @@ -4000,8 +4132,99 @@ 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]; + uint8_t buf[4]; + + 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: + /* ignore increment packed, not supported */ + if (q->ap_w.reg == MEM_AP_REG_CSW) + q->ap_w.data &= ~CSW_ADDRINC_PACKED; + retval = stlink_dap_ap_write(q->ap_w.ap, q->ap_w.reg, q->ap_w.data); + break; + + 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: + 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: + 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: + *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: + 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: + h_u32_to_le(buf, q->mem_ap.data); + 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; + + 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; @@ -4016,7 +4239,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; @@ -4025,12 +4248,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; @@ -4039,15 +4257,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; @@ -4062,6 +4275,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) { @@ -4072,6 +4291,170 @@ 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]; + + if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) && + (reg == MEM_AP_REG_DRW || reg == MEM_AP_REG_BD0 || reg == MEM_AP_REG_BD1 || + reg == MEM_AP_REG_BD2 || reg == MEM_AP_REG_BD3)) { + /* de-queue previous write-TAR */ + struct dap_queue *prev_q = q - 1; + if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_TAR) { + stlink_dap_handle->queue_index = i; + i--; + q = prev_q; + prev_q--; + } + /* de-queue previous write-CSW */ + if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW) { + stlink_dap_handle->queue_index = i; + q = prev_q; + } + + switch (ap->csw_value & CSW_SIZE_MASK) { + case CSW_8BIT: + q->cmd = CMD_MEM_AP_READ8; + break; + case CSW_16BIT: + q->cmd = CMD_MEM_AP_READ16; + break; + case CSW_32BIT: + q->cmd = CMD_MEM_AP_READ32; + break; + default: + LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK); + stlink_dap_record_error(ERROR_FAIL); + return ERROR_FAIL; + } + + q->mem_ap.addr = (reg == MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c)); + q->mem_ap.ap = ap; + q->mem_ap.p_data = data; + q->mem_ap.csw = ap->csw_default; + + /* force TAR and CSW update */ + ap->tar_valid = false; + ap->csw_value = 0; + } else { + 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]; + + if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) && + (reg == MEM_AP_REG_DRW || reg == MEM_AP_REG_BD0 || reg == MEM_AP_REG_BD1 || + reg == MEM_AP_REG_BD2 || reg == MEM_AP_REG_BD3)) { + /* de-queue previous write-TAR */ + struct dap_queue *prev_q = q - 1; + if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_TAR) { + stlink_dap_handle->queue_index = i; + i--; + q = prev_q; + prev_q--; + } + /* de-queue previous write-CSW */ + if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW) { + stlink_dap_handle->queue_index = i; + q = prev_q; + } + + switch (ap->csw_value & CSW_SIZE_MASK) { + case CSW_8BIT: + q->cmd = CMD_MEM_AP_WRITE8; + break; + case CSW_16BIT: + q->cmd = CMD_MEM_AP_WRITE16; + break; + case CSW_32BIT: + q->cmd = CMD_MEM_AP_WRITE32; + break; + default: + LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK); + stlink_dap_record_error(ERROR_FAIL); + return ERROR_FAIL; + } + + q->mem_ap.addr = (reg == MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c)); + q->mem_ap.ap = ap; + q->mem_ap.data = data; + q->mem_ap.csw = ap->csw_default; + + /* force TAR and CSW update */ + ap->tar_valid = false; + ap->csw_value = 0; + } else { + 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); @@ -4087,7 +4470,7 @@ static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size, count *= size; while (count) { - bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count; + bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count; retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer); if (retval != ERROR_OK) return retval; @@ -4110,7 +4493,7 @@ static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size, count *= size; while (count) { - bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count; + bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count; retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer); if (retval != ERROR_OK) return retval; @@ -4219,6 +4602,43 @@ COMMAND_HANDLER(stlink_dap_backend_command) return ERROR_OK; } +#define BYTES_PER_LINE 16 +COMMAND_HANDLER(stlink_dap_cmd_command) +{ + unsigned int rx_n, tx_n; + struct stlink_usb_handle_s *h = stlink_dap_handle; + + if (CMD_ARGC < 2) + return ERROR_COMMAND_SYNTAX_ERROR; + + COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], rx_n); + tx_n = CMD_ARGC - 1; + if (tx_n > STLINK_SG_SIZE || rx_n > STLINK_DATA_SIZE) { + LOG_ERROR("max %x byte sent and %d received", STLINK_SG_SIZE, STLINK_DATA_SIZE); + return ERROR_COMMAND_SYNTAX_ERROR; + } + + stlink_usb_init_buffer(h, h->rx_ep, rx_n); + + for (unsigned int i = 0; i < tx_n; i++) { + uint8_t byte; + COMMAND_PARSE_NUMBER(u8, CMD_ARGV[i + 1], byte); + h->cmdbuf[h->cmdidx++] = byte; + } + + int retval = stlink_usb_xfer_noerrcheck(h, h->databuf, rx_n); + if (retval != ERROR_OK) { + LOG_ERROR("Error %d", retval); + return retval; + } + + for (unsigned int i = 0; i < rx_n; i++) + command_print_sameline(CMD, "0x%02x%c", h->databuf[i], + ((i == (rx_n - 1)) || ((i % BYTES_PER_LINE) == (BYTES_PER_LINE - 1))) ? '\n' : ' '); + + return ERROR_OK; +} + /** */ static const struct command_registration stlink_dap_subcommand_handlers[] = { { @@ -4242,6 +4662,13 @@ static const struct command_registration stlink_dap_subcommand_handlers[] = { .help = "select which ST-Link backend to use", .usage = "usb | tcp [port]", }, + { + .name = "cmd", + .handler = stlink_dap_cmd_command, + .mode = COMMAND_EXEC, + .help = "send arbitrary command", + .usage = "rx_n (tx_byte)+", + }, COMMAND_REGISTRATION_DONE }; @@ -4356,7 +4783,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 */ };