From 1a51d493de08a7e2c1d336a82a5d5c97b0bd4e69 Mon Sep 17 00:00:00 2001 From: Laurent LEMELE Date: Fri, 13 Apr 2018 16:35:45 +0200 Subject: [PATCH] stlink: add JTAG speed selection ST-Link uses two separate API for setting the interface speed in SWD and JTAG. Add the missing API for JTAG and run the proper API depending on the selected mode. Change-Id: I71e955e23c1d82c2ea1c8bfade7612c5a0377a74 Author: Laurent LEMELE Rebased-by: Antonio Borneo Signed-off-by: Antonio Borneo Reviewed-on: http://openocd.zylin.com/4703 Tested-by: jenkins Reviewed-by: Spencer Oliver --- src/jtag/drivers/stlink_usb.c | 184 ++++++++++++++++++++++++++-------- 1 file changed, 144 insertions(+), 40 deletions(-) diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c index 6e6ad1b5bc..f6e0e3f940 100644 --- a/src/jtag/drivers/stlink_usb.c +++ b/src/jtag/drivers/stlink_usb.c @@ -246,6 +246,7 @@ struct stlink_usb_handle_s { #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_JTAG_SET_FREQ 0x44 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW 0x00 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH 0x01 @@ -268,10 +269,13 @@ enum stlink_mode { #define REQUEST_SENSE 0x03 #define REQUEST_SENSE_LENGTH 18 -static const struct { +struct speed_map { int speed; int speed_divisor; -} stlink_khz_to_speed_map[] = { +}; + +/* SWD clock speed */ +static const struct speed_map stlink_khz_to_speed_map_swd[] = { {4000, 0}, {1800, 1}, /* default */ {1200, 2}, @@ -286,6 +290,18 @@ static const struct { {5, 798} }; +/* JTAG clock speed */ +static const struct speed_map stlink_khz_to_speed_map_jtag[] = { + {18000, 2}, + {9000, 4}, + {4500, 8}, + {2250, 16}, + {1125, 32}, /* default */ + {562, 64}, + {281, 128}, + {140, 256} +}; + static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size); static int stlink_swim_status(void *handle); @@ -701,6 +717,31 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor) return ERROR_OK; } +static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor) +{ + struct stlink_usb_handle_s *h = handle; + + assert(handle != NULL); + + /* only supported by stlink/v2 and for firmware >= 24 */ + if (h->version.stlink == 1 || h->version.jtag < 24) + 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_JTAG_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) { @@ -1887,70 +1928,129 @@ static int stlink_usb_override_target(const char *targetname) return !strcmp(targetname, "cortex_m"); } -static int stlink_speed(void *handle, int khz, bool query) +static int stlink_speed_swim(void *handle, int khz, bool query) { - unsigned i; - int speed_index = -1; - int speed_diff = INT_MAX; - struct stlink_usb_handle_s *h = handle; - - if (h && (h->transport == HL_TRANSPORT_SWIM)) { - /* + /* 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; - } + */ + if (khz == 0) + stlink_swim_speed(handle, 0); + else + stlink_swim_speed(handle, 1); + return khz; +} - /* only supported by stlink/v2 and for firmware >= 22 */ - if (h && (h->version.stlink == 1 || h->version.jtag < 22)) - 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 < ARRAY_SIZE(stlink_khz_to_speed_map); i++) { - if (khz == stlink_khz_to_speed_map[i].speed) { + for (i = 0; i < map_size; i++) { + if (khz == map[i].speed) { speed_index = i; break; } else { - int current_diff = khz - stlink_khz_to_speed_map[i].speed; + 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 >= stlink_khz_to_speed_map[i].speed) { + if ((current_diff < speed_diff) && khz >= 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; + speed_index = map_size - 1; match = false; - } else if (i == ARRAY_SIZE(stlink_khz_to_speed_map)) + } else if (i == map_size) 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); + 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; + } } - if (h && !query) { - int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[speed_index].speed_divisor); + 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[speed_index].speed; + 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; } /** */ @@ -2159,14 +2259,18 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) return ERROR_OK; } - /* clock speed only supported by stlink/v2 and for firmware >= 22 */ - if (h->version.stlink >= 2 && h->version.jtag >= 22) { - LOG_DEBUG("Supported clock speeds are:"); - - for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) - LOG_DEBUG("%d kHz", stlink_khz_to_speed_map[i].speed); - - stlink_speed(h, param->initial_interface_speed, false); + if (h->transport == HL_TRANSPORT_JTAG) { + /* jtag clock speed only supported by stlink/v2 and for firmware >= 24 */ + if (h->version.stlink >= 2 && h->version.jtag >= 24) { + stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag)); + stlink_speed(h, param->initial_interface_speed, false); + } + } else if (h->transport == HL_TRANSPORT_SWD) { + /* clock speed only supported by stlink/v2 and for firmware >= 22 */ + if (h->version.stlink >= 2 && h->version.jtag >= 22) { + stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd)); + stlink_speed(h, param->initial_interface_speed, false); + } } /* get cpuid, so we can determine the max page size -- 2.30.2