+ switch (t) {
+ case HL_TRANSPORT_SWD:
+ return STLINK_MODE_DEBUG_SWD;
+ case HL_TRANSPORT_JTAG:
+ return STLINK_MODE_DEBUG_JTAG;
+ case HL_TRANSPORT_SWIM:
+ return STLINK_MODE_DEBUG_SWIM;
+ default:
+ return STLINK_MODE_UNKNOWN;
+ }
+}
+
+/** */
+static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int initial_interface_speed)
+{
+ int res;
+ uint8_t mode;
+ enum stlink_mode emode;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ res = stlink_usb_current_mode(handle, &mode);
+
+ if (res != ERROR_OK)
+ return res;
+
+ LOG_DEBUG("MODE: 0x%02X", mode);
+
+ /* try to exit current mode */
+ switch (mode) {
+ case STLINK_DEV_DFU_MODE:
+ emode = STLINK_MODE_DFU;
+ break;
+ case STLINK_DEV_DEBUG_MODE:
+ emode = STLINK_MODE_DEBUG_SWD;
+ break;
+ case STLINK_DEV_SWIM_MODE:
+ emode = STLINK_MODE_DEBUG_SWIM;
+ break;
+ case STLINK_DEV_BOOTLOADER_MODE:
+ case STLINK_DEV_MASS_MODE:
+ default:
+ emode = STLINK_MODE_UNKNOWN;
+ break;
+ }
+
+ if (emode != STLINK_MODE_UNKNOWN) {
+ res = stlink_usb_mode_leave(handle, emode);
+
+ if (res != ERROR_OK)
+ return res;
+ }
+
+ res = stlink_usb_current_mode(handle, &mode);
+
+ if (res != ERROR_OK)
+ return res;
+
+ /* we check the target voltage here as an aid to debugging connection problems.
+ * the stlink requires the target Vdd to be connected for reliable debugging.
+ * this cmd is supported in all modes except DFU
+ */
+ if (mode != STLINK_DEV_DFU_MODE) {
+
+ float target_voltage;
+
+ /* check target voltage (if supported) */
+ res = stlink_usb_check_voltage(h, &target_voltage);
+
+ if (res != ERROR_OK) {
+ if (res != ERROR_COMMAND_NOTFOUND)
+ LOG_ERROR("voltage check failed");
+ /* attempt to continue as it is not a catastrophic failure */
+ } else {
+ /* check for a sensible target voltage, operating range is 1.65-5.5v
+ * according to datasheet */
+ if (target_voltage < 1.5)
+ LOG_ERROR("target voltage may be too low for reliable debugging");
+ }
+ }
+
+ LOG_DEBUG("MODE: 0x%02X", mode);
+
+ /* set selected mode */
+ emode = stlink_get_mode(h->transport);
+
+ if (emode == STLINK_MODE_UNKNOWN) {
+ LOG_ERROR("selected mode (transport) not supported");
+ return ERROR_FAIL;
+ }
+
+ /* set the speed before entering the mode, as the chip discovery phase should be done at this speed too */
+ if (h->transport == HL_TRANSPORT_JTAG) {
+ if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
+ stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
+ stlink_speed(h, initial_interface_speed, false);
+ }
+ } else if (h->transport == HL_TRANSPORT_SWD) {
+ if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
+ stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
+ stlink_speed(h, initial_interface_speed, false);
+ }
+ }
+
+ if (h->version.jtag_api == STLINK_JTAG_API_V3) {
+ struct speed_map map[STLINK_V3_MAX_FREQ_NB];
+
+ stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map);
+ stlink_dump_speed_map(map, ARRAY_SIZE(map));
+ stlink_speed(h, initial_interface_speed, false);
+ }
+
+ /* preliminary SRST assert:
+ * We want SRST is asserted before activating debug signals (mode_enter).
+ * As the required mode has not been set, the adapter may not know what pin to use.
+ * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
+ * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
+ * after power on, SWIM_RST stays unchanged */
+ if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
+ stlink_usb_assert_srst(handle, 0);
+ /* do not check the return status here, we will
+ proceed and enter the desired mode below
+ and try asserting srst again. */
+
+ res = stlink_usb_mode_enter(handle, emode);
+ if (res != ERROR_OK)
+ return res;
+
+ /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
+ if (connect_under_reset) {
+ res = stlink_usb_assert_srst(handle, 0);
+ if (res != ERROR_OK)
+ return res;
+ }
+
+ res = stlink_usb_current_mode(handle, &mode);
+
+ if (res != ERROR_OK)
+ return res;
+
+ LOG_DEBUG("MODE: 0x%02X", mode);
+
+ return ERROR_OK;
+}
+
+/* request status from last swim request */
+static int stlink_swim_status(void *handle)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 4);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
+ /* error is checked by the caller */
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+}
+/*
+ the purpose of this function is unknown...
+ capabilites? anyway for swim v6 it returns
+ 0001020600000000
+*/
+__attribute__((unused))
+static int stlink_swim_cap(void *handle, uint8_t *cap)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 8);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
+ h->cmdbuf[h->cmdidx++] = 0x01;
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
+ if (res != ERROR_OK)
+ return res;
+ memcpy(cap, h->databuf, 8);
+ return ERROR_OK;
+}
+
+/* debug dongle assert/deassert sreset line */
+static int stlink_swim_assert_reset(void *handle, int reset)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 0);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ if (!reset)
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
+ else
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
+ res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+}
+
+/*
+ send swim enter seq
+ 1.3ms low then 750Hz then 1.5kHz
+*/
+static int stlink_swim_enter(void *handle)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 0);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
+ res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+}
+
+/* switch high/low speed swim */
+static int stlink_swim_speed(void *handle, int speed)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 0);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
+ if (speed)
+ h->cmdbuf[h->cmdidx++] = 1;
+ else
+ h->cmdbuf[h->cmdidx++] = 0;
+ res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+}
+
+/*
+ initiate srst from swim.
+ nrst is pulled low for 50us.
+*/
+static int stlink_swim_generate_rst(void *handle)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 0);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
+ res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+}
+
+/*
+ send resyncronize sequence
+ swim is pulled low for 16us
+ reply is 64 clks low
+*/
+static int stlink_swim_resync(void *handle)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 0);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
+ res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+}
+
+static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+ unsigned int i;
+ unsigned int datalen = 0;
+ int cmdsize = STLINK_CMD_SIZE_V2;
+
+ if (len > STLINK_DATA_SIZE)
+ return ERROR_FAIL;
+
+ if (h->version.stlink == 1)
+ cmdsize = STLINK_SG_SIZE;
+
+ stlink_usb_init_buffer(handle, h->tx_ep, 0);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
+ h_u16_to_be(h->cmdbuf+h->cmdidx, len);
+ h->cmdidx += 2;
+ h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+ for (i = 0; i < len; i++) {
+ if (h->cmdidx == cmdsize)
+ h->databuf[datalen++] = *(data++);
+ else
+ h->cmdbuf[h->cmdidx++] = *(data++);
+ }
+ if (h->version.stlink == 1)
+ stlink_usb_set_cbw_transfer_datalength(handle, datalen);
+
+ res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+}
+
+static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ if (len > STLINK_DATA_SIZE)
+ return ERROR_FAIL;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 0);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
+ h_u16_to_be(h->cmdbuf+h->cmdidx, len);
+ h->cmdidx += 2;
+ h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+ res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+ if (res != ERROR_OK)
+ return res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, len);
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
+ res = stlink_usb_xfer_noerrcheck(handle, data, len);
+ if (res != ERROR_OK)
+ return res;
+
+ return ERROR_OK;
+}
+
+/** */
+static int stlink_usb_idcode(void *handle, uint32_t *idcode)
+{
+ int res, offset;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ /* there is no swim read core id cmd */
+ if (h->transport == HL_TRANSPORT_SWIM) {
+ *idcode = 0;
+ return ERROR_OK;
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 12);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ if (h->version.jtag_api == STLINK_JTAG_API_V1) {
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
+
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
+ offset = 0;
+ } else {
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_IDCODES;
+
+ res = stlink_usb_xfer_errcheck(handle, h->databuf, 12);
+ offset = 4;
+ }
+
+ if (res != ERROR_OK)
+ return res;
+
+ *idcode = le_to_h_u32(h->databuf + offset);
+
+ LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
+
+ return ERROR_OK;
+}
+
+static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int res;
+
+ assert(handle != NULL);
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 8);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+
+ 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_debug_reg(void *handle, uint32_t addr, 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_WRITEDEBUGREG;
+ else
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+ h->cmdidx += 4;
+ 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_trace_read(void *handle, uint8_t *buf, size_t *size)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
+ int res;
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 10);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
+
+ res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
+ if (res != ERROR_OK)
+ return res;
+
+ size_t bytes_avail = le_to_h_u16(h->databuf);
+ *size = bytes_avail < *size ? bytes_avail : *size - 1;
+
+ if (*size > 0) {
+ res = stlink_usb_read_trace(handle, buf, *size);
+ if (res != ERROR_OK)
+ return res;
+ return ERROR_OK;
+ }
+ }
+ *size = 0;
+ return ERROR_OK;
+}
+
+static enum target_state stlink_usb_v2_get_status(void *handle)
+{
+ int result;
+ uint32_t status;
+
+ result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
+ if (result != ERROR_OK)
+ return TARGET_UNKNOWN;
+
+ if (status & S_HALT)
+ return TARGET_HALTED;
+ else if (status & S_RESET_ST)
+ return TARGET_RESET;
+
+ return TARGET_RUNNING;
+}
+
+/** */
+static enum target_state stlink_usb_state(void *handle)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ if (h->transport == HL_TRANSPORT_SWIM) {
+ res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
+ if (res != ERROR_OK)
+ return TARGET_UNKNOWN;
+
+ 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;
+
+ 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_mem32(void *handle, uint32_t addr, uint16_t len,
+ uint8_t *buffer)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ /* 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;
+ 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_mem32(void *handle, uint32_t addr, uint16_t len,
+ const uint8_t *buffer)
+{
+ int res;
+ struct stlink_usb_handle_s *h = handle;
+
+ assert(handle != NULL);
+
+ /* 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;
+ 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 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));
+ if (max_tar_block == 0)
+ max_tar_block = 4;
+ return max_tar_block;
+}
+
+static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
+ uint32_t count, uint8_t *buffer)
+{
+ int retval = ERROR_OK;
+ uint32_t bytes_remaining;
+ int retries = 0;
+ struct stlink_usb_handle_s *h = handle;
+
+ /* calculate byte count */
+ count *= size;
+
+ /* switch to 8 bit if stlink does not support 16 bit memory read */
+ if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+ size = 1;
+
+ while (count) {
+
+ bytes_remaining = (size != 1) ? \
+ stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
+
+ if (count < bytes_remaining)
+ bytes_remaining = count;
+
+ if (h->transport == HL_TRANSPORT_SWIM) {
+ retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
+ if (retval != ERROR_OK)
+ return retval;
+ } else
+ /*
+ * all stlink support 8/32bit memory read/writes and only from
+ * stlink V2J26 there is support for 16 bit memory read/write.
+ * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
+ * 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
+ * adi_v5 implementation, but the stlink is a hla adapter and so this
+ * needs implementing manually.
+ * currently this only affects jtag mode, according to ST they do single
+ * access in SWD mode - but this may change and so we do it for both modes */
+
+ /* 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);
+ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+ usleep((1<<retries++) * 1000);
+ continue;
+ }
+ if (retval != ERROR_OK)
+ return retval;
+ buffer += head_bytes;
+ addr += head_bytes;
+ count -= head_bytes;
+ bytes_remaining -= head_bytes;
+ }
+
+ if (bytes_remaining & (size - 1))
+ retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
+ else if (size == 2)
+ retval = stlink_usb_read_mem16(handle, 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);
+
+ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+ usleep((1<<retries++) * 1000);
+ continue;
+ }
+ if (retval != ERROR_OK)
+ return retval;
+
+ buffer += bytes_remaining;
+ addr += bytes_remaining;
+ count -= bytes_remaining;
+ }
+
+ return retval;
+}
+
+static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
+ uint32_t count, const uint8_t *buffer)
+{
+ int retval = ERROR_OK;
+ uint32_t bytes_remaining;
+ int retries = 0;
+ struct stlink_usb_handle_s *h = handle;
+
+ /* calculate byte count */
+ count *= size;
+
+ /* switch to 8 bit if stlink does not support 16 bit memory read */
+ if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+ size = 1;
+
+ while (count) {
+
+ bytes_remaining = (size != 1) ? \
+ stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
+
+ if (count < bytes_remaining)
+ bytes_remaining = count;
+
+ if (h->transport == HL_TRANSPORT_SWIM) {
+ retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
+ if (retval != ERROR_OK)
+ return retval;
+ } else
+ /*
+ * all stlink support 8/32bit memory read/writes and only from
+ * stlink V2J26 there is support for 16 bit memory read/write.
+ * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
+ * 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
+ * adi_v5 implementation, but the stlink is a hla adapter and so this
+ * needs implementing manually.
+ * currently this only affects jtag mode, according to ST they do single
+ * access in SWD mode - but this may change and so we do it for both modes */
+
+ /* we first need to check for any unaligned bytes */
+ if (addr & (size - 1)) {
+
+ uint32_t head_bytes = size - (addr & (size - 1));
+ retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
+ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+ usleep((1<<retries++) * 1000);
+ continue;
+ }
+ if (retval != ERROR_OK)
+ return retval;
+ buffer += head_bytes;
+ addr += head_bytes;
+ count -= head_bytes;
+ bytes_remaining -= head_bytes;
+ }
+
+ if (bytes_remaining & (size - 1))
+ retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
+ else if (size == 2)
+ retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
+ else
+ retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
+
+ } else
+ retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
+ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+ usleep((1<<retries++) * 1000);
+ continue;
+ }
+ if (retval != ERROR_OK)
+ return retval;
+
+ buffer += bytes_remaining;
+ addr += bytes_remaining;
+ count -= bytes_remaining;
+ }
+
+ return retval;
+}
+
+/** */
+static int stlink_usb_override_target(const char *targetname)
+{
+ return !strcmp(targetname, "cortex_m");
+}
+
+static int stlink_speed_swim(void *handle, int khz, bool query)
+{
+ /*
+ we dont care what the khz rate is
+ we only have low and high speed...
+ before changing speed the SWIM_CSR HS bit
+ must be updated
+ */
+ if (khz == 0)
+ stlink_swim_speed(handle, 0);
+ else
+ stlink_swim_speed(handle, 1);
+ return khz;
+}
+
+static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
+{
+ unsigned int i;
+ int speed_index = -1;
+ int speed_diff = INT_MAX;
+ int last_valid_speed = -1;
+ bool match = true;
+
+ for (i = 0; i < map_size; i++) {
+ if (!map[i].speed)
+ continue;
+ last_valid_speed = i;
+ if (khz == map[i].speed) {
+ speed_index = i;
+ break;
+ } else {
+ int current_diff = khz - map[i].speed;
+ /* get abs value for comparison */
+ current_diff = (current_diff > 0) ? current_diff : -current_diff;
+ if ((current_diff < speed_diff) && khz >= map[i].speed) {
+ speed_diff = current_diff;
+ speed_index = i;
+ }
+ }
+ }
+
+ if (speed_index == -1) {
+ /* this will only be here if we cannot match the slow speed.
+ * use the slowest speed we support.*/
+ speed_index = last_valid_speed;
+ match = false;
+ } else if (i == map_size)
+ match = false;
+
+ if (!match && query) {
+ LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
+ khz, map[speed_index].speed);
+ }
+
+ return speed_index;
+}
+
+static int stlink_speed_swd(void *handle, int khz, bool query)
+{
+ int speed_index;
+ struct stlink_usb_handle_s *h = handle;
+
+ /* old firmware cannot change it */
+ if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
+ return khz;
+
+ speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
+ ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
+
+ if (!query) {
+ int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
+ if (result != ERROR_OK) {
+ LOG_ERROR("Unable to set adapter speed");
+ return khz;
+ }
+ }
+
+ return stlink_khz_to_speed_map_swd[speed_index].speed;
+}
+
+static int stlink_speed_jtag(void *handle, int khz, bool query)
+{
+ int speed_index;
+ struct stlink_usb_handle_s *h = handle;
+
+ /* old firmware cannot change it */
+ if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
+ return khz;
+
+ speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
+ ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
+
+ if (!query) {
+ int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
+ if (result != ERROR_OK) {
+ LOG_ERROR("Unable to set adapter speed");
+ return khz;
+ }
+ }
+
+ return stlink_khz_to_speed_map_jtag[speed_index].speed;
+}
+
+void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
+{
+ unsigned int i;
+
+ LOG_DEBUG("Supported clock speeds are:");
+ for (i = 0; i < map_size; i++)
+ if (map[i].speed)
+ LOG_DEBUG("%d kHz", map[i].speed);
+}
+
+static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int i;
+
+ if (h->version.jtag_api != STLINK_JTAG_API_V3) {
+ LOG_ERROR("Unknown command");
+ return 0;
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 16);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
+ h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
+
+ int res = stlink_usb_xfer_errcheck(handle, h->databuf, 52);
+
+ int size = h->databuf[8];
+
+ if (size > STLINK_V3_MAX_FREQ_NB)
+ size = STLINK_V3_MAX_FREQ_NB;
+
+ for (i = 0; i < size; i++) {
+ map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
+ map[i].speed_divisor = i;
+ }
+
+ /* set to zero all the next entries */
+ for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
+ map[i].speed = 0;
+
+ return res;
+}
+
+static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ if (h->version.jtag_api != STLINK_JTAG_API_V3) {
+ LOG_ERROR("Unknown command");
+ return 0;
+ }
+
+ stlink_usb_init_buffer(handle, h->rx_ep, 16);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
+ h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
+ h->cmdbuf[h->cmdidx++] = 0;
+
+ h_u32_to_le(&h->cmdbuf[4], frequency);
+
+ return stlink_usb_xfer_errcheck(handle, h->databuf, 8);
+}
+
+static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
+{
+ struct stlink_usb_handle_s *h = handle;
+ int speed_index;
+ struct speed_map map[STLINK_V3_MAX_FREQ_NB];
+
+ stlink_get_com_freq(h, is_jtag, map);
+
+ speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
+
+ if (!query) {
+ int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
+ if (result != ERROR_OK) {
+ LOG_ERROR("Unable to set adapter speed");
+ return khz;
+ }
+ }
+ return map[speed_index].speed;
+}
+
+static int stlink_speed(void *handle, int khz, bool query)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ if (!handle)
+ return khz;
+
+ switch (h->transport) {
+ case HL_TRANSPORT_SWIM:
+ return stlink_speed_swim(handle, khz, query);
+ break;
+ case HL_TRANSPORT_SWD:
+ if (h->version.jtag_api == STLINK_JTAG_API_V3)
+ return stlink_speed_v3(handle, false, khz, query);
+ else
+ return stlink_speed_swd(handle, khz, query);
+ break;
+ case HL_TRANSPORT_JTAG:
+ if (h->version.jtag_api == STLINK_JTAG_API_V3)
+ return stlink_speed_v3(handle, true, khz, query);
+ else
+ return stlink_speed_jtag(handle, khz, query);
+ break;
+ default:
+ break;
+ }
+
+ return khz;
+}
+
+/** */
+static int stlink_usb_close(void *handle)
+{
+ int res;
+ uint8_t mode;
+ enum stlink_mode emode;
+ struct stlink_usb_handle_s *h = handle;
+
+ if (h && h->fd)
+ res = stlink_usb_current_mode(handle, &mode);
+ else
+ res = ERROR_FAIL;
+ /* do not exit if return code != ERROR_OK,
+ it prevents us from closing jtag_libusb */
+
+ if (res == ERROR_OK) {
+ /* try to exit current mode */
+ switch (mode) {
+ case STLINK_DEV_DFU_MODE:
+ emode = STLINK_MODE_DFU;
+ break;
+ case STLINK_DEV_DEBUG_MODE:
+ emode = STLINK_MODE_DEBUG_SWD;
+ break;
+ case STLINK_DEV_SWIM_MODE:
+ emode = STLINK_MODE_DEBUG_SWIM;
+ break;
+ case STLINK_DEV_BOOTLOADER_MODE:
+ case STLINK_DEV_MASS_MODE:
+ default:
+ emode = STLINK_MODE_UNKNOWN;
+ break;
+ }
+
+ if (emode != STLINK_MODE_UNKNOWN)
+ stlink_usb_mode_leave(handle, emode);
+ /* do not check return code, it prevent
+ us from closing jtag_libusb */
+ }
+
+ if (h && h->fd)
+ jtag_libusb_close(h->fd);
+
+ free(h);
+
+ return ERROR_OK;
+}
+
+/** */
+static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
+{
+ int err, retry_count = 1;