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 <borneo.antonio@gmail.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/6602
Tested-by: jenkins
Reviewed-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 92eb06d3ab028c201fae38f07e085d609565b773..3032ad4aaa13a07c591f0629304004c9b032f334 100644 (file)
--- 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<<retries++) * 1000);
+                                       usleep((1 << retries++) * 1000);
                                        continue;
                                }
                                if (retval != ERROR_OK)
@@ -2670,16 +2710,17 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
                        }
 
                        if (bytes_remaining & (size - 1))
-                               retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
+                               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, addr, bytes_remaining, buffer);
+                               retval = stlink_usb_read_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
                        else
-                               retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
-               } else
-                       retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
+                               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);
+                       usleep((1 << retries++) * 1000);
                        continue;
                }
                if (retval != ERROR_OK)
@@ -2693,8 +2734,15 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
        return retval;
 }
 
-static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
-               uint32_t count, const uint8_t *buffer)
+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;
@@ -2736,7 +2784,7 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
                        if (addr & (size - 1)) {
 
                                uint32_t head_bytes = size - (addr & (size - 1));
-                               retval = stlink_usb_write_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<<retries++) * 1000);
                                        continue;
@@ -2750,14 +2798,14 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
                        }
 
                        if (bytes_remaining & (size - 1))
-                               retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
+                               retval = stlink_usb_write_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
                        else if (size == 2)
-                               retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
+                               retval = stlink_usb_write_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
                        else
-                               retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
+                               retval = stlink_usb_write_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
 
                } else
-                       retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
+                       retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
                if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
                        usleep((1<<retries++) * 1000);
                        continue;
@@ -2773,6 +2821,13 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
        return retval;
 }
 
+static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
+               uint32_t count, const uint8_t *buffer)
+{
+       return stlink_usb_write_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
+                                                                  addr, size, count, buffer);
+}
+
 /** */
 static int stlink_usb_override_target(const char *targetname)
 {
@@ -3561,8 +3616,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;