From 6f914cd8990435eafc85f8fc75fbecd7fbb96214 Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Wed, 5 Feb 2020 16:20:37 +0100 Subject: [PATCH] stlink: expose ap number and csw in memory r/w Recent versions of stlink firmware allow accessing access port other than zero and setting the CSW. Modify the internal API to provide ap_num and csw. There is no interest to modify HLA to use ap_num and csw, so set and use some backward compatible defaults. Change-Id: I3f6dfc6c670d19467d9f5e717c6c956db6faf7f3 Signed-off-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/6602 Tested-by: jenkins Reviewed-by: Tarek BOCHKATI --- src/jtag/drivers/stlink_usb.c | 123 ++++++++++++++++++++++++---------- 1 file changed, 89 insertions(+), 34 deletions(-) diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c index 92eb06d3ab..3032ad4aaa 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,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; @@ -2629,7 +2672,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); @@ -2643,7 +2685,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 @@ -2654,11 +2695,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<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; -- 2.30.2