diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c index 92eb06d3a..3032ad4aa 100644 --- a/src/jtag/drivers/stlink_usb.c +++ b/src/jtag/drivers/stlink_usb.c @@ -113,6 +113,10 @@ */ #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, @@ -478,6 +482,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) @@ -1306,6 +1311,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; @@ -1336,6 +1342,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; @@ -2377,8 +2384,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; @@ -2386,6 +2393,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)); @@ -2400,6 +2410,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) @@ -2416,14 +2429,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)); @@ -2438,6 +2454,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); @@ -2448,8 +2467,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; @@ -2459,6 +2478,9 @@ 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; @@ -2478,6 +2500,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); @@ -2490,8 +2515,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; @@ -2501,6 +2526,9 @@ 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; @@ -2520,6 +2548,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); @@ -2530,14 +2561,17 @@ 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; @@ -2557,6 +2591,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); @@ -2569,14 +2606,17 @@ 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; @@ -2596,6 +2636,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); @@ -2613,8 +2656,93 @@ static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t addr return max_tar_block; } +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; + int retries = 0; + struct stlink_usb_handle_s *h = handle; + + /* calculate byte count */ + count *= size; + + /* switch to 8 bit if stlink does not support 16 bit memory read */ + if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT)) + size = 1; + + while (count) { + bytes_remaining = (size != 1) ? + stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h); + + if (count < bytes_remaining) + bytes_remaining = count; + + /* + * all stlink support 8/32bit memory read/writes and only from + * stlink V2J26 there is support for 16 bit memory read/write. + * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle + * 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 + * adi_v5 implementation, but the stlink is a hla adapter and so this + * needs implementing manually. + * currently this only affects jtag mode, according to ST they do single + * access in SWD mode - but this may change and so we do it for both modes */ + + /* 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, ap_num, csw, addr, head_bytes, buffer); + if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { + usleep((1 << retries++) * 1000); + continue; + } + if (retval != ERROR_OK) + return retval; + buffer += head_bytes; + addr += head_bytes; + count -= head_bytes; + bytes_remaining -= head_bytes; + } + + if (bytes_remaining & (size - 1)) + retval = stlink_usb_read_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer); + else if (size == 2) + retval = stlink_usb_read_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer); + else + retval = stlink_usb_read_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer); + } else { + retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer); + } + + if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { + usleep((1 << retries++) * 1000); + continue; + } + if (retval != ERROR_OK) + return retval; + + buffer += bytes_remaining; + addr += bytes_remaining; + count -= bytes_remaining; + } + + return retval; +} + static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, uint32_t count, uint8_t *buffer) +{ + return stlink_usb_read_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW, + addr, size, count, buffer); +} + +static int stlink_usb_write_ap_mem(void *handle, uint8_t ap_num, uint32_t csw, + uint32_t addr, uint32_t size, uint32_t count, const uint8_t *buffer) { int retval = ERROR_OK; uint32_t bytes_remaining; @@ -2656,7 +2784,7 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, 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_write_mem8(handle, ap_num, csw, addr, head_bytes, buffer); if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { usleep((1<version.flags & STLINK_F_HAS_MEM_16BIT)) - size = 1; - - while (count) { - - bytes_remaining = (size != 1) ? - stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h); - - if (count < bytes_remaining) - bytes_remaining = count; - - /* - * all stlink support 8/32bit memory read/writes and only from - * stlink V2J26 there is support for 16 bit memory read/write. - * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle - * 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 - * adi_v5 implementation, but the stlink is a hla adapter and so this - * needs implementing manually. - * currently this only affects jtag mode, according to ST they do single - * access in SWD mode - but this may change and so we do it for both modes */ - - /* we first need to check for any unaligned bytes */ - if (addr & (size - 1)) { - - uint32_t head_bytes = size - (addr & (size - 1)); - retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer); - if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { - usleep((1<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;