+ 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->jtag_api == STLINK_JTAG_API_V1 ||
+ (h->jtag_api == STLINK_JTAG_API_V2 && h->version.jtag < 26)))
+ size = 1;
+
+ while (count) {
+
+ bytes_remaining = (size != 1) ? \
+ stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
+
+ 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->jtag_api == STLINK_JTAG_API_V1 ||
+ (h->jtag_api == STLINK_JTAG_API_V2 && h->version.jtag < 26)))
+ size = 1;
+
+ while (count) {
+
+ bytes_remaining = (size != 1) ? \
+ stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
+
+ 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;
+ bool match = true;
+
+ for (i = 0; i < map_size; 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 = map_size - 1;
+ 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;
+
+ /* only supported by stlink/v2 and for firmware >= 22 */
+ if (h->version.stlink == 1 || h->version.jtag < 22)
+ 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;
+
+ /* only supported by stlink/v2 and for firmware >= 24 */
+ if (h->version.stlink == 1 || h->version.jtag < 24)
+ 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++)
+ LOG_DEBUG("%d kHz", map[i].speed);
+}
+
+static int stlink_speed(void *handle, int khz, bool query)
+{
+ struct stlink_usb_handle_s *h = handle;
+
+ if (!handle)
+ return khz;
+
+ if (h->transport == HL_TRANSPORT_SWIM)
+ return stlink_speed_swim(handle, khz, query);
+ else if (h->transport == HL_TRANSPORT_SWD)
+ return stlink_speed_swd(handle, khz, query);
+ else if (h->transport == HL_TRANSPORT_JTAG)
+ return stlink_speed_jtag(handle, khz, query);
+
+ 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);
+