+ res = stlink_swim_resync(handle);
+ if (res != ERROR_OK)
+ return TARGET_UNKNOWN;
+
+ return ERROR_OK;
+ }
+
+ if (h->reconnect_pending) {
+ LOG_INFO("Previous state query failed, trying to reconnect");
+ res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
+
+ if (res != ERROR_OK)
+ return TARGET_UNKNOWN;
+
+ h->reconnect_pending = false;
+ }
+
+ if (h->version.jtag_api != STLINK_JTAG_API_V1) {
+ res = stlink_usb_v2_get_status(handle);
+ if (res == TARGET_UNKNOWN)
+ h->reconnect_pending = true;
+ return res;
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
+
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
+
+ if (res != ERROR_OK)
+ return TARGET_UNKNOWN;
+
+ if (h->databuf[0] == STLINK_CORE_RUNNING)
+ return TARGET_RUNNING;
+ if (h->databuf[0] == STLINK_CORE_HALTED)
+ return TARGET_HALTED;
+
+ h->reconnect_pending = true;
+
+ return TARGET_UNKNOWN;
+}
+
+static int stlink_usb_assert_srst(void *handle, int srst)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->transport == HL_TRANSPORT_SWIM)
+ return stlink_swim_assert_reset(handle, srst);
+
+ if (h->version.stlink == 1)
+ return ERROR_COMMAND_NOTFOUND;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
+ h->cmdbuf[h->cmdidx++] = srst;
+
+ return stlink_cmd_allow_retry(handle, h->databuf, 2);
+}
+
+/** */
+static void stlink_usb_trace_disable(void *handle)
+{
+ int res = ERROR_OK;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ assert(h->version.flags & STLINK_F_HAS_TRACE);
+
+ LOG_DEBUG("Tracing: disable");
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
+ res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
+
+ if (res == ERROR_OK)
+ h->trace.enabled = false;
+}
+
+
+/** */
+static int stlink_usb_trace_enable(void *handle)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->version.flags & STLINK_F_HAS_TRACE) {
+ stlink_usb_init_buffer(handle, h->rx_ep, 10);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
+ h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
+ h->cmdidx += 2;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
+ h->cmdidx += 4;
+
+ res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
+
+ if (res == ERROR_OK) {
+ h->trace.enabled = true;
+ LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
+ }
+ } else {
+ LOG_ERROR("Tracing is not supported by this version.");
+ res = ERROR_FAIL;
+ }
+
+ return res;
+}
+
+/** */
+static int stlink_usb_reset(void *handle)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int retval;
+
+ assert(handle != NULL);
+
+ if (h->transport == HL_TRANSPORT_SWIM)
+ return stlink_swim_generate_rst(handle);
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+
+ if (h->version.jtag_api == STLINK_JTAG_API_V1)
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
+ else
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
+
+ retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (h->trace.enabled) {
+ stlink_usb_trace_disable(h);
+ return stlink_usb_trace_enable(h);
+ }
+
+ return ERROR_OK;
+}
+
+/** */
+static int stlink_usb_run(void *handle)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->version.jtag_api != STLINK_JTAG_API_V1) {
+ res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
+
+ return res;
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
+
+ return stlink_cmd_allow_retry(handle, h->databuf, 2);
+}
+
+/** */
+static int stlink_usb_halt(void *handle)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->version.jtag_api != STLINK_JTAG_API_V1) {
+ res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
+
+ return res;
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
+
+ return stlink_cmd_allow_retry(handle, h->databuf, 2);
+}
+
+/** */
+static int stlink_usb_step(void *handle)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->version.jtag_api != STLINK_JTAG_API_V1) {
+ /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
+ * that the Cortex-M3 currently does. */
+ stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
+ stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
+ return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
+
+ return stlink_cmd_allow_retry(handle, h->databuf, 2);
+}
+
+/** */
+static int stlink_usb_read_regs(void *handle)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 88);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ if (h->version.jtag_api == STLINK_JTAG_API_V1) {
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 84);
+ /* regs data from offset 0 */
+ } else {
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
+ res = stlink_usb_xfer_errcheck(handle, h->databuf, 88);
+ /* status at offset 0, regs data from offset 4 */
+ }
+
+ return res;
+}
+
+/** */
+static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ if (h->version.jtag_api == STLINK_JTAG_API_V1)
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
+ else
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
+ h->cmdbuf[h->cmdidx++] = num;
+
+ if (h->version.jtag_api == STLINK_JTAG_API_V1) {
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
+ if (res != ERROR_OK)
+ return res;
+ *val = le_to_h_u32(h->databuf);
+ return ERROR_OK;
+ } else {
+ res = stlink_cmd_allow_retry(handle, h->databuf, 8);
+ if (res != ERROR_OK)
+ return res;
+ *val = le_to_h_u32(h->databuf + 4);
+ return ERROR_OK;
+ }
+}
+
+/** */
+static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ if (h->version.jtag_api == STLINK_JTAG_API_V1)
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
+ else
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
+ h->cmdbuf[h->cmdidx++] = num;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, val);
+ h->cmdidx += 4;
+
+ return stlink_cmd_allow_retry(handle, h->databuf, 2);
+}
+
+static int stlink_usb_get_rw_status(void *handle)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->version.jtag_api == STLINK_JTAG_API_V1)
+ return ERROR_OK;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
+ return stlink_usb_xfer_errcheck(handle, h->databuf, 12);
+ } else {
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
+ return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
+ }
+}
+
+/** */
+static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
+ uint8_t *buffer)
+{
+ int res;
+ uint16_t read_len = len;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ /* 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));
+ return ERROR_FAIL;
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, read_len);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+ h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+ h->cmdidx += 2;
+
+ /* we need to fix read length for single bytes */
+ if (read_len == 1)
+ read_len++;
+
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, read_len);
+
+ if (res != ERROR_OK)
+ return res;
+
+ memcpy(buffer, h->databuf, len);
+
+ return stlink_usb_get_rw_status(handle);
+}
+
+/** */
+static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
+ const uint8_t *buffer)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ /* 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));
+ return ERROR_FAIL;
+ }
+
+ stlink_usb_init_buffer(handle, h->tx_ep, len);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+ h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+ h->cmdidx += 2;
+
+ res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
+
+ if (res != ERROR_OK)
+ return res;
+
+ return stlink_usb_get_rw_status(handle);
+}
+
+/** */
+static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
+ uint8_t *buffer)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+ return ERROR_COMMAND_NOTFOUND;
+
+ /* data must be a multiple of 2 and half-word aligned */
+ if (len % 2 || addr % 2) {
+ 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_APIV2_READMEM_16BIT;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+ h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+ h->cmdidx += 2;
+
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
+
+ if (res != ERROR_OK)
+ return res;
+
+ memcpy(buffer, h->databuf, len);
+
+ return stlink_usb_get_rw_status(handle);
+}
+
+/** */
+static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
+ const uint8_t *buffer)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+ return ERROR_COMMAND_NOTFOUND;
+
+ /* data must be a multiple of 2 and half-word aligned */
+ if (len % 2 || addr % 2) {
+ 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_APIV2_WRITEMEM_16BIT;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+ h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+ h->cmdidx += 2;