Code Review / openocd.git / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | review | tree
raw | inline | side by side
stlink: skip rw-misc commands with TCP server
[openocd.git] / src / jtag / drivers / stlink_usb.c
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 3032ad4aaa13a07c591f0629304004c9b032f334..b51a0c2bacd23d51232657e510ed3c0848c54d48 100644 (file)
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -33,6 +33,7 @@
 #endif
 
 /* project specific includes */
 #endif
 
 /* project specific includes */
+#include <helper/align.h>
 #include <helper/binarybuffer.h>
 #include <helper/bits.h>
 #include <helper/system.h>
 #include <helper/binarybuffer.h>
 #include <helper/bits.h>
 #include <helper/system.h>
@@ -186,10 +187,25 @@ struct stlink_backend_s {
 enum queue_cmd {
        CMD_DP_READ = 1,
        CMD_DP_WRITE,
 enum queue_cmd {
        CMD_DP_READ = 1,
        CMD_DP_WRITE,
+
        CMD_AP_READ,
        CMD_AP_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 dap_queue {
        enum queue_cmd cmd;
        union {
@@ -212,7 +228,17 @@ struct dap_queue {
                        unsigned int reg;
                        struct adiv5_ap *ap;
                        uint32_t data;
                        unsigned int reg;
                        struct adiv5_ap *ap;
                        uint32_t data;
+                       bool changes_csw_default;
                } ap_w;
                } 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;
        };
 };
 
        };
 };
 
@@ -410,6 +436,12 @@ static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, i
 #define STLINK_DEBUG_APIV2_INIT_AP         0x4B
 #define STLINK_DEBUG_APIV2_CLOSE_AP_DBG    0x4C
 
 #define STLINK_DEBUG_APIV2_INIT_AP         0x4B
 #define STLINK_DEBUG_APIV2_CLOSE_AP_DBG    0x4C
 
+#define STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC         0x50
+#define STLINK_DEBUG_APIV2_RW_MISC_OUT     0x51
+#define STLINK_DEBUG_APIV2_RW_MISC_IN      0x52
+
+#define STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC          0x54
+
 #define STLINK_APIV3_SET_COM_FREQ           0x61
 #define STLINK_APIV3_GET_COM_FREQ           0x62
 
 #define STLINK_APIV3_SET_COM_FREQ           0x61
 #define STLINK_APIV3_GET_COM_FREQ           0x62
 
@@ -482,6 +514,9 @@ 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
 /* 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_MEM_WR_NO_INC      STLINK_F_HAS_MEM_16BIT
+#define STLINK_F_HAS_MEM_RD_NO_INC      STLINK_F_HAS_DPBANKSEL
+#define STLINK_F_HAS_RW_MISC            STLINK_F_HAS_DPBANKSEL
 #define STLINK_F_HAS_CSW                STLINK_F_HAS_DPBANKSEL
 
 #define STLINK_REGSEL_IS_FPU(x)         ((x) > 0x1F)
 #define STLINK_F_HAS_CSW                STLINK_F_HAS_DPBANKSEL
 
 #define STLINK_REGSEL_IS_FPU(x)         ((x) > 0x1F)
@@ -1299,6 +1334,7 @@ static int stlink_usb_version(void *handle)
                        flags |= STLINK_F_QUIRK_JTAG_DP_READ;
 
                /* API to read/write memory at 16 bit from J26 */
                        flags |= STLINK_F_QUIRK_JTAG_DP_READ;
 
                /* API to read/write memory at 16 bit from J26 */
+               /* API to write memory without address increment from J26 */
                if (h->version.jtag >= 26)
                        flags |= STLINK_F_HAS_MEM_16BIT;
 
                if (h->version.jtag >= 26)
                        flags |= STLINK_F_HAS_MEM_16BIT;
 
@@ -1311,6 +1347,7 @@ static int stlink_usb_version(void *handle)
                        flags |= STLINK_F_FIX_CLOSE_AP;
 
                /* Banked regs (DPv1 & DPv2) support from V2J32 */
                        flags |= STLINK_F_FIX_CLOSE_AP;
 
                /* Banked regs (DPv1 & DPv2) support from V2J32 */
+               /* API to read memory without address increment from V2J32 */
                /* Memory R/W supports CSW from V2J32 */
                if (h->version.jtag >= 32)
                        flags |= STLINK_F_HAS_DPBANKSEL;
                /* Memory R/W supports CSW from V2J32 */
                if (h->version.jtag >= 32)
                        flags |= STLINK_F_HAS_DPBANKSEL;
@@ -1333,6 +1370,7 @@ static int stlink_usb_version(void *handle)
                flags |= STLINK_F_HAS_DAP_REG;
 
                /* API to read/write memory at 16 bit */
                flags |= STLINK_F_HAS_DAP_REG;
 
                /* API to read/write memory at 16 bit */
+               /* API to write memory without address increment */
                flags |= STLINK_F_HAS_MEM_16BIT;
 
                /* API required to init AP before any AP access */
                flags |= STLINK_F_HAS_MEM_16BIT;
 
                /* API required to init AP before any AP access */
@@ -1342,6 +1380,7 @@ static int stlink_usb_version(void *handle)
                flags |= STLINK_F_FIX_CLOSE_AP;
 
                /* Banked regs (DPv1 & DPv2) support from V3J2 */
                flags |= STLINK_F_FIX_CLOSE_AP;
 
                /* Banked regs (DPv1 & DPv2) support from V3J2 */
+               /* API to read memory without address increment from V3J2 */
                /* Memory R/W supports CSW from V3J2 */
                if (h->version.jtag >= 2)
                        flags |= STLINK_F_HAS_DPBANKSEL;
                /* Memory R/W supports CSW from V3J2 */
                if (h->version.jtag >= 2)
                        flags |= STLINK_F_HAS_DPBANKSEL;
@@ -2648,6 +2687,88 @@ static int stlink_usb_write_mem32(void *handle, uint8_t ap_num, uint32_t csw,
        return stlink_usb_get_rw_status(handle);
 }
 
        return stlink_usb_get_rw_status(handle);
 }
 
+static int stlink_usb_read_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
+               uint32_t addr, uint16_t len, uint8_t *buffer)
+{
+       struct stlink_usb_handle_s *h = handle;
+
+       assert(handle != NULL);
+
+       if (!(h->version.flags & STLINK_F_HAS_MEM_RD_NO_INC))
+               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");
+               return ERROR_TARGET_UNALIGNED_ACCESS;
+       }
+
+       stlink_usb_init_buffer(handle, h->rx_ep, len);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC;
+       h_u32_to_le(h->cmdbuf + h->cmdidx, addr);
+       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;
+
+       int retval = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
+       if (retval != ERROR_OK)
+               return retval;
+
+       memcpy(buffer, h->databuf, len);
+
+       return stlink_usb_get_rw_status(handle);
+}
+
+static int stlink_usb_write_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
+               uint32_t addr, uint16_t len, const uint8_t *buffer)
+{
+       struct stlink_usb_handle_s *h = handle;
+
+       assert(handle != NULL);
+
+       if (!(h->version.flags & STLINK_F_HAS_MEM_WR_NO_INC))
+               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");
+               return ERROR_TARGET_UNALIGNED_ACCESS;
+       }
+
+       stlink_usb_init_buffer(handle, h->tx_ep, len);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC;
+       h_u32_to_le(h->cmdbuf + h->cmdidx, addr);
+       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;
+
+       int retval = stlink_usb_xfer_noerrcheck(handle, buffer, len);
+       if (retval != ERROR_OK)
+               return retval;
+
+       return stlink_usb_get_rw_status(handle);
+}
+
 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
 {
        uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
 {
        uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
@@ -3750,6 +3871,53 @@ static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
 
 }
 
 
 }
 
+static int stlink_usb_rw_misc_out(void *handle, uint32_t items, const uint8_t *buffer)
+{
+       struct stlink_usb_handle_s *h = handle;
+       unsigned int buflen = ALIGN_UP(items, 4) + 4 * items;
+
+       LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
+
+       assert(handle != NULL);
+
+       if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
+               return ERROR_COMMAND_NOTFOUND;
+
+       stlink_usb_init_buffer(handle, h->tx_ep, buflen);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_OUT;
+       h_u32_to_le(&h->cmdbuf[2], items);
+
+       return stlink_usb_xfer_noerrcheck(handle, buffer, buflen);
+}
+
+static int stlink_usb_rw_misc_in(void *handle, uint32_t items, uint8_t *buffer)
+{
+       struct stlink_usb_handle_s *h = handle;
+       unsigned int buflen = 2 * 4 * items;
+
+       LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
+
+       assert(handle != NULL);
+
+       if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
+               return ERROR_COMMAND_NOTFOUND;
+
+       stlink_usb_init_buffer(handle, h->rx_ep, buflen);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_IN;
+
+       int res = stlink_usb_xfer_noerrcheck(handle, h->databuf, buflen);
+       if (res != ERROR_OK)
+               return res;
+
+       memcpy(buffer, h->databuf, buflen);
+
+       return ERROR_OK;
+}
+
 /** */
 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
                        unsigned short addr, uint32_t *val)
 /** */
 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
                        unsigned short addr, uint32_t *val)
@@ -3844,6 +4012,7 @@ struct hl_layout_api_s stlink_usb_layout_api = {
 static struct stlink_usb_handle_s *stlink_dap_handle;
 static struct hl_interface_param_s stlink_dap_param;
 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
 static struct stlink_usb_handle_s *stlink_dap_handle;
 static struct hl_interface_param_s stlink_dap_param;
 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
+static uint32_t last_csw_default[DP_APSEL_MAX + 1];
 static int stlink_dap_error = ERROR_OK;
 
 /** */
 static int stlink_dap_error = ERROR_OK;
 
 /** */
@@ -3888,6 +4057,7 @@ static int stlink_usb_open_ap(void *handle, unsigned short apsel)
 
        LOG_DEBUG("AP %d enabled", apsel);
        set_bit(apsel, opened_ap);
 
        LOG_DEBUG("AP %d enabled", apsel);
        set_bit(apsel, opened_ap);
+       last_csw_default[apsel] = 0;
        return ERROR_OK;
 }
 
        return ERROR_OK;
 }
 
@@ -3971,6 +4141,8 @@ static int stlink_dap_op_connect(struct adiv5_dap *dap)
 
        dap->do_reconnect = false;
        dap_invalidate_cache(dap);
 
        dap->do_reconnect = false;
        dap_invalidate_cache(dap);
+       for (unsigned int i = 0; i <= DP_APSEL_MAX; i++)
+               last_csw_default[i] = 0;
 
        retval = dap_dp_init(dap);
        if (retval != ERROR_OK) {
 
        retval = dap_dp_init(dap);
        if (retval != ERROR_OK) {
@@ -4108,6 +4280,248 @@ static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
        return ERROR_OK;
 }
 
        return ERROR_OK;
 }
 
+#define RW_MISC_CMD_ADDRESS     1
+#define RW_MISC_CMD_WRITE       2
+#define RW_MISC_CMD_READ        3
+#define RW_MISC_CMD_APNUM       5
+
+static int stlink_usb_misc_rw_segment(void *handle, const struct dap_queue *q, unsigned int len, unsigned int items)
+{
+       uint8_t buf[2 * 4 * items];
+
+       LOG_DEBUG("Queue: %u commands in %u items", len, items);
+
+       int ap_num = DP_APSEL_INVALID;
+       unsigned int cmd_index = 0;
+       unsigned int val_index = ALIGN_UP(items, 4);
+       for (unsigned int i = 0; i < len; i++) {
+               if (ap_num != q[i].mem_ap.ap->ap_num) {
+                       ap_num = q[i].mem_ap.ap->ap_num;
+                       buf[cmd_index++] = RW_MISC_CMD_APNUM;
+                       h_u32_to_le(&buf[val_index], ap_num);
+                       val_index += 4;
+               }
+
+               switch (q[i].cmd) {
+               case CMD_MEM_AP_READ32:
+                       buf[cmd_index++] = RW_MISC_CMD_READ;
+                       h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
+                       val_index += 4;
+                       break;
+               case CMD_MEM_AP_WRITE32:
+                       buf[cmd_index++] = RW_MISC_CMD_ADDRESS;
+                       h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
+                       val_index += 4;
+                       buf[cmd_index++] = RW_MISC_CMD_WRITE;
+                       h_u32_to_le(&buf[val_index], q[i].mem_ap.data);
+                       val_index += 4;
+                       break;
+               default:
+                       /* Not supposed to happen */
+                       return ERROR_FAIL;
+               }
+       }
+       /* pad after last command */
+       while (!IS_ALIGNED(cmd_index, 4))
+               buf[cmd_index++] = 0;
+
+       int retval = stlink_usb_rw_misc_out(handle, items, buf);
+       if (retval != ERROR_OK)
+               return retval;
+
+       retval = stlink_usb_rw_misc_in(handle, items, buf);
+       if (retval != ERROR_OK)
+               return retval;
+
+       ap_num = DP_APSEL_INVALID;
+       val_index = 0;
+       unsigned int err_index = 4 * items;
+       for (unsigned int i = 0; i < len; i++) {
+               uint32_t errcode = le_to_h_u32(&buf[err_index]);
+               if (errcode != STLINK_DEBUG_ERR_OK) {
+                       LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+                       return ERROR_FAIL;
+               }
+               if (ap_num != q[i].mem_ap.ap->ap_num) {
+                       ap_num = q[i].mem_ap.ap->ap_num;
+                       err_index += 4;
+                       val_index += 4;
+                       errcode = le_to_h_u32(&buf[err_index]);
+                       if (errcode != STLINK_DEBUG_ERR_OK) {
+                               LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+                               return ERROR_FAIL;
+                       }
+               }
+
+               if (q[i].cmd == CMD_MEM_AP_READ32) {
+                       *q[i].mem_ap.p_data = le_to_h_u32(&buf[val_index]);
+               } else { /* q[i]->cmd == CMD_MEM_AP_WRITE32 */
+                       err_index += 4;
+                       val_index += 4;
+                       errcode = le_to_h_u32(&buf[err_index]);
+                       if (errcode != STLINK_DEBUG_ERR_OK) {
+                               LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+                               return ERROR_FAIL;
+                       }
+               }
+               err_index += 4;
+               val_index += 4;
+       }
+
+       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);
+               if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
+                       return stlink_usb_write_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+               else
+                       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:
+               if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
+                       retval = stlink_usb_read_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+               else
+                       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;
+       };
+}
+
+/* TODO: recover these values with cmd STLINK_DEBUG_APIV2_RW_MISC_GET_MAX (0x53) */
+#define STLINK_V2_RW_MISC_SIZE (64)
+#define STLINK_V3_RW_MISC_SIZE (1227)
+
+static int stlink_usb_count_misc_rw_queue(void *handle, const struct dap_queue *q, unsigned int len,
+               unsigned int *pkt_items)
+{
+       struct stlink_usb_handle_s *h = handle;
+       unsigned int i, items = 0;
+       int ap_num = DP_APSEL_INVALID;
+       unsigned int misc_max_items = (h->version.stlink == 2) ? STLINK_V2_RW_MISC_SIZE : STLINK_V3_RW_MISC_SIZE;
+
+       if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
+               return 0;
+       /*
+        * RW_MISC sequence doesn't lock the st-link, so are not safe in shared mode.
+        * Don't use it with TCP backend to prevent any issue in case of sharing.
+        * This further degrades the performance, on top of TCP server overhead.
+        */
+       if (h->backend == &stlink_tcp_backend)
+               return 0;
+
+       for (i = 0; i < len; i++) {
+               if (q[i].cmd != CMD_MEM_AP_READ32 && q[i].cmd != CMD_MEM_AP_WRITE32)
+                       break;
+               unsigned int count = 1;
+               if (ap_num != q[i].mem_ap.ap->ap_num) {
+                       count++;
+                       ap_num = q[i].mem_ap.ap->ap_num;
+               }
+               if (q[i].cmd == CMD_MEM_AP_WRITE32)
+                       count++;
+               if (items + count > misc_max_items)
+                       break;
+               items += count;
+       }
+
+       *pkt_items = items;
+
+       return i;
+}
+
+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;
+
+       /* check for no address increment, 32 bits only */
+       if (len > 1 && incr == 4 && q[0].mem_ap.addr == q[1].mem_ap.addr)
+               incr = 0;
+
+       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, misc_items = 0;
+       int retval;
+
+       unsigned int count_misc = stlink_usb_count_misc_rw_queue(handle, q, len, &misc_items);
+       unsigned int count_buf = stlink_usb_count_buf_rw_queue(q, len);
+
+       if (count_misc > count_buf) {
+               count = count_misc;
+               retval = stlink_usb_misc_rw_segment(handle, q, count, misc_items);
+       } else {
+               count = count_buf;
+               retval = stlink_usb_buf_rw_segment(handle, q, count_buf);
+       }
+       if (retval != ERROR_OK)
+               return retval;
+
+       *skip = count;
+       return ERROR_OK;
+}
+
 static void stlink_dap_run_internal(struct adiv5_dap *dap)
 {
        int retval = stlink_dap_check_reconnect(dap);
 static void stlink_dap_run_internal(struct adiv5_dap *dap)
 {
        int retval = stlink_dap_check_reconnect(dap);
@@ -4121,6 +4535,8 @@ static void stlink_dap_run_internal(struct adiv5_dap *dap)
        struct dap_queue *q = &stlink_dap_handle->queue[0];
 
        while (i && stlink_dap_get_error() == ERROR_OK) {
        struct dap_queue *q = &stlink_dap_handle->queue[0];
 
        while (i && stlink_dap_get_error() == ERROR_OK) {
+               unsigned int skip = 1;
+
                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);
                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);
@@ -4132,16 +4548,29 @@ static void stlink_dap_run_internal(struct adiv5_dap *dap)
                        retval = stlink_dap_ap_read(q->ap_r.ap, q->ap_r.reg, q->ap_r.p_data);
                        break;
                case CMD_AP_WRITE:
                        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;
                        retval = stlink_dap_ap_write(q->ap_w.ap, q->ap_w.reg, q->ap_w.data);
                        break;
+
+               case CMD_MEM_AP_READ8:
+               case CMD_MEM_AP_READ16:
+               case CMD_MEM_AP_READ32:
+               case CMD_MEM_AP_WRITE8:
+               case CMD_MEM_AP_WRITE16:
+               case CMD_MEM_AP_WRITE32:
+                       retval = stlink_usb_mem_rw_queue(stlink_dap_handle, q, i, &skip);
+                       break;
+
                default:
                        LOG_ERROR("ST-Link: Unknown queue command %d", q->cmd);
                        retval = ERROR_FAIL;
                        break;
                }
                stlink_dap_record_error(retval);
                default:
                        LOG_ERROR("ST-Link: Unknown queue command %d", q->cmd);
                        retval = ERROR_FAIL;
                        break;
                }
                stlink_dap_record_error(retval);
-               q++;
-               i--;
+               q += skip;
+               i -= skip;
        }
 
        stlink_dap_handle->queue_index = 0;
        }
 
        stlink_dap_handle->queue_index = 0;
@@ -4261,10 +4690,57 @@ static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned int reg,
 
        unsigned int i = stlink_dap_handle->queue_index++;
        struct dap_queue *q = &stlink_dap_handle->queue[i];
 
        unsigned int i = stlink_dap_handle->queue_index++;
        struct dap_queue *q = &stlink_dap_handle->queue[i];
-       q->cmd = CMD_AP_READ;
-       q->ap_r.reg = reg;
-       q->ap_r.ap = ap;
-       q->ap_r.p_data = data;
+
+       /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_RD_NO_INC
+        * and STLINK_F_HAS_RW_MISC */
+       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 it didn't changed ap->csw_default */
+               if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW &&
+                               !prev_q->ap_w.changes_csw_default) {
+                       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);
 
        if (i == MAX_QUEUE_DEPTH - 1)
                stlink_dap_run_internal(ap->dap);
@@ -4280,10 +4756,63 @@ static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned int reg,
 
        unsigned int i = stlink_dap_handle->queue_index++;
        struct dap_queue *q = &stlink_dap_handle->queue[i];
 
        unsigned int i = stlink_dap_handle->queue_index++;
        struct dap_queue *q = &stlink_dap_handle->queue[i];
-       q->cmd = CMD_AP_WRITE;
-       q->ap_w.reg = reg;
-       q->ap_w.ap = ap;
-       q->ap_w.data = data;
+
+       /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_WR_NO_INC
+        * and STLINK_F_HAS_RW_MISC */
+       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 it didn't changed ap->csw_default */
+               if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW &&
+                               !prev_q->ap_w.changes_csw_default) {
+                       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 (reg == MEM_AP_REG_CSW && ap->csw_default != last_csw_default[ap->ap_num]) {
+                       q->ap_w.changes_csw_default = true;
+                       last_csw_default[ap->ap_num] = ap->csw_default;
+               } else {
+                       q->ap_w.changes_csw_default = false;
+               }
+       }
 
        if (i == MAX_QUEUE_DEPTH - 1)
                stlink_dap_run_internal(ap->dap);
 
        if (i == MAX_QUEUE_DEPTH - 1)
                stlink_dap_run_internal(ap->dap);
Open On-Chip Debugger

Linking to existing account procedure

If you already have an account and want to add another login method you MUST first sign in with your existing account and then change URL to read https://review.openocd.org/login/?link to get to this page again but this time it'll work for linking. Thank you.

SSH host keys fingerprints

1024 SHA256:YKx8b7u5ZWdcbp7/4AeXNaqElP49m6QrwfXaqQGJAOk gerrit-code-review@openocd.zylin.com (DSA)
384 SHA256:jHIbSQa4REvwCFG4cq5LBlBLxmxSqelQPem/EXIrxjk gerrit-code-review@openocd.org (ECDSA)
521 SHA256:UAOPYkU9Fjtcao0Ul/Rrlnj/OsQvt+pgdYSZ4jOYdgs gerrit-code-review@openocd.org (ECDSA)
256 SHA256:A13M5QlnozFOvTllybRZH6vm7iSt0XLxbA48yfc2yfY gerrit-code-review@openocd.org (ECDSA)
256 SHA256:spYMBqEYoAOtK7yZBrcwE8ZpYt6b68Cfh9yEVetvbXg gerrit-code-review@openocd.org (ED25519)
+--[ED25519 256]--+
|=..              |
|+o..   .         |
|*.o   . .        |
|+B . . .         |
|Bo. = o S        |
|Oo.+ + =         |
|oB=.* = . o      |
| =+=.+   + E     |
|. .=o   . o      |
+----[SHA256]-----+
2048 SHA256:0Onrb7/PHjpo6iVZ7xQX2riKN83FJ3KGU0TvI0TaFG4 gerrit-code-review@openocd.zylin.com (RSA)