From 145f8ed8178ca86fab95dc9544e81d49ddf9c12c Mon Sep 17 00:00:00 2001 From: Spencer Oliver Date: Thu, 9 Oct 2014 20:08:23 +0100 Subject: [PATCH] stlink: add reconfigurable speed support The ability to change the speed has been added to firmware versions J22 and above. Any attempt to change on earlier versions will be ignored without error, as the existing code does. For supported firmware versions the driver will attempt to get as close as possible to supported speeds (never higher). The default stlink speed on power up is 1.8MHz. The driver will now also print supported clocl speeds during init. Change-Id: Iee9bd018bb8b6f94672a12538912d41c23d48a7e Signed-off-by: Spencer Oliver Reviewed-on: http://openocd.zylin.com/2335 Tested-by: jenkins --- src/jtag/drivers/stlink_usb.c | 108 ++++++++++++++++++++++++++++++++++ 1 file changed, 108 insertions(+) diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c index 58af0dd59c..7572e871a6 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, }; -- 2.30.2