#endif
/* project specific includes */
+#include <helper/align.h>
#include <helper/binarybuffer.h>
#include <helper/bits.h>
#include <helper/system.h>
unsigned int reg;
struct adiv5_ap *ap;
uint32_t data;
+ bool changes_csw_default;
} ap_w;
struct mem_ap {
uint32_t addr;
#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
/* 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)
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;
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;
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_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;
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 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 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;
/** */
LOG_DEBUG("AP %d enabled", apsel);
set_bit(apsel, opened_ap);
+ last_csw_default[apsel] = 0;
return ERROR_OK;
}
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) {
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);
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);
- return stlink_usb_write_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+ 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);
return retval;
case CMD_MEM_AP_READ32:
- retval = stlink_usb_read_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+ 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]);
};
}
+/* 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);
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;
static int stlink_usb_mem_rw_queue(void *handle, const struct dap_queue *q, unsigned int len, unsigned int *skip)
{
- unsigned int count = stlink_usb_count_buf_rw_queue(q, len);
+ unsigned int count, misc_items = 0;
+ int retval;
- int retval = stlink_usb_buf_rw_segment(handle, q, count);
+ 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;
unsigned int i = stlink_dap_handle->queue_index++;
struct dap_queue *q = &stlink_dap_handle->queue[i];
+ /* 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)) {
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) {
+ /* 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;
}
unsigned int i = stlink_dap_handle->queue_index++;
struct dap_queue *q = &stlink_dap_handle->queue[i];
+ /* 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)) {
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) {
+ /* 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;
}
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)
Code Review / openocd.git / blobdiff