diff options
Diffstat (limited to 'src/jtag/drivers')
| -rw-r--r-- | src/jtag/drivers/stlink_usb.c | 108 |
1 files changed, 108 insertions, 0 deletions
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c index 58af0dd5..7572e871 100644 --- a/src/jtag/drivers/stlink_usb.c +++ b/src/jtag/drivers/stlink_usb.c @@ -204,6 +204,7 @@ struct stlink_usb_handle_s { #define STLINK_DEBUG_APIV2_START_TRACE_RX 0x40 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX 0x41 #define STLINK_DEBUG_APIV2_GET_TRACE_NB 0x42 +#define STLINK_DEBUG_APIV2_SWD_SET_FREQ 0x43 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW 0x00 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH 0x01 @@ -226,6 +227,24 @@ enum stlink_mode { #define REQUEST_SENSE 0x03 #define REQUEST_SENSE_LENGTH 18 +static const struct { + int speed; + int speed_divisor; +} stlink_khz_to_speed_map[] = { + {4000, 0}, + {1800, 1}, /* default */ + {1200, 2}, + {950, 3}, + {480, 7}, + {240, 15}, + {125, 31}, + {100, 40}, + {50, 79}, + {25, 158}, + {15, 265}, + {5, 798} +}; + static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size); /** */ @@ -534,6 +553,31 @@ static int stlink_usb_check_voltage(void *handle, float *target_voltage) return ERROR_OK; } +static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor) +{ + struct stlink_usb_handle_s *h = handle; + + assert(handle != NULL); + + /* only supported by stlink/v2 and for firmware >= 22 */ + if (h->version.stlink == 1 || h->version.jtag < 22) + 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_SWD_SET_FREQ; + h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor); + h->cmdidx += 2; + + int result = stlink_cmd_allow_retry(handle, h->databuf, 2); + + if (result != ERROR_OK) + return result; + + return ERROR_OK; +} + /** */ static int stlink_usb_current_mode(void *handle, uint8_t *mode) { @@ -1567,6 +1611,58 @@ static int stlink_usb_override_target(const char *targetname) return !strcmp(targetname, "cortex_m"); } +static int stlink_speed(void *handle, int khz, bool query) +{ + unsigned i; + int speed_index = -1; + int speed_diff = INT_MAX; + struct stlink_usb_handle_s *h = handle; + + /* only supported by stlink/v2 and for firmware >= 22 */ + if (h && (h->version.stlink == 1 || h->version.jtag < 22)) + return khz; + + for (i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) { + if (khz == stlink_khz_to_speed_map[i].speed) { + speed_index = i; + break; + } else { + int current_diff = khz - stlink_khz_to_speed_map[i].speed; + /* get abs value for comparison */ + current_diff = (current_diff > 0) ? current_diff : -current_diff; + if ((current_diff < speed_diff) && khz >= stlink_khz_to_speed_map[i].speed) { + speed_diff = current_diff; + speed_index = i; + } + } + } + + bool match = true; + + if (speed_index == -1) { + /* this will only be here if we cannot match the slow speed. + * use the slowest speed we support.*/ + speed_index = ARRAY_SIZE(stlink_khz_to_speed_map) - 1; + match = false; + } else if (i == ARRAY_SIZE(stlink_khz_to_speed_map)) + match = false; + + if (!match && query) { + LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \ + khz, stlink_khz_to_speed_map[speed_index].speed); + } + + if (h && !query) { + int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[speed_index].speed_divisor); + if (result != ERROR_OK) { + LOG_ERROR("Unable to set adapter speed"); + return khz; + } + } + + return stlink_khz_to_speed_map[speed_index].speed; +} + /** */ static int stlink_usb_close(void *fd) { @@ -1739,6 +1835,16 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) goto error_open; } + /* clock speed only supported by stlink/v2 and for firmware >= 22 */ + if (h->version.stlink >= 2 && h->version.jtag >= 22) { + LOG_INFO("Supported clock speeds are:"); + + for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) + LOG_INFO("%d kHz", stlink_khz_to_speed_map[i].speed); + + stlink_speed(h, param->initial_interface_speed, false); + } + /* get cpuid, so we can determine the max page size * start with a safe default */ h->max_mem_packet = (1 << 10); @@ -1800,4 +1906,6 @@ struct hl_layout_api_s stlink_usb_layout_api = { .write_debug_reg = stlink_usb_write_debug_reg, /** */ .override_target = stlink_usb_override_target, + /** */ + .speed = stlink_speed, }; |