From 43019fcde99b8e745a767b8e2c307d5868c09878 Mon Sep 17 00:00:00 2001 From: Ant-ON Date: Thu, 5 Nov 2020 19:34:22 +0500 Subject: [PATCH] Fix change SWD frequency of stlink v3 --- doc/dev/developer.txt | 2 +- inc/backend.h | 2 +- inc/stlink.h | 2 +- src/common.c | 4 +-- src/stlink-lib/usb.c | 68 +++++++++++++++++-------------------------- 5 files changed, 31 insertions(+), 47 deletions(-) diff --git a/doc/dev/developer.txt b/doc/dev/developer.txt index 32afd4352..9c4da22f7 100644 --- a/doc/dev/developer.txt +++ b/doc/dev/developer.txt @@ -300,7 +300,7 @@ Return: -1 for error. 0 for success. int stlink_current_mode(stlink_t *sl); int stlink_force_debug(stlink_t *sl); int stlink_target_voltage(stlink_t *sl); - int stlink_set_swdclk(stlink_t *sl, uint16_t divisor); + int stlink_set_swdclk(stlink_t *sl, int freq_khz); int stlink_erase_flash_mass(stlink_t* sl); int stlink_write_flash(stlink_t* sl, stm32_addr_t address, uint8_t* data, uint32_t length, uint8_t eraseonly); diff --git a/inc/backend.h b/inc/backend.h index f852da05d..d60708031 100644 --- a/inc/backend.h +++ b/inc/backend.h @@ -28,7 +28,7 @@ int (*current_mode) (stlink_t * stl); int (*force_debug) (stlink_t *sl); int32_t (*target_voltage) (stlink_t *sl); - int (*set_swdclk) (stlink_t * stl, uint16_t divisor); + int (*set_swdclk) (stlink_t * stl, int freq_khz); } stlink_backend_t; #endif // STLINK_BACKEND_H_ diff --git a/inc/stlink.h b/inc/stlink.h index 2ec2db367..7fce9d3dc 100644 --- a/inc/stlink.h +++ b/inc/stlink.h @@ -242,7 +242,7 @@ int stlink_step(stlink_t *sl); int stlink_current_mode(stlink_t *sl); int stlink_force_debug(stlink_t *sl); int stlink_target_voltage(stlink_t *sl); -int stlink_set_swdclk(stlink_t *sl, uint16_t divisor); +int stlink_set_swdclk(stlink_t *sl, int freq_khz); int stlink_erase_flash_mass(stlink_t* sl); int stlink_write_flash(stlink_t* sl, stm32_addr_t address, uint8_t* data, uint32_t length, uint8_t eraseonly); diff --git a/src/common.c b/src/common.c index 89f27aa66..7321b39a4 100644 --- a/src/common.c +++ b/src/common.c @@ -1491,9 +1491,9 @@ int stlink_run(stlink_t *sl) { return(sl->backend->run(sl)); } -int stlink_set_swdclk(stlink_t *sl, uint16_t divisor) { +int stlink_set_swdclk(stlink_t *sl, int freq_khz) { DLOG("*** set_swdclk ***\n"); - return(sl->backend->set_swdclk(sl, divisor)); + return(sl->backend->set_swdclk(sl, freq_khz)); } int stlink_status(stlink_t *sl) { diff --git a/src/stlink-lib/usb.c b/src/stlink-lib/usb.c index 92ad4cbc5..f5a91abf4 100644 --- a/src/stlink-lib/usb.c +++ b/src/stlink-lib/usb.c @@ -651,7 +651,7 @@ int _stlink_usb_run(stlink_t* sl) { return(0); } -int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) { +int _stlink_usb_set_swdclk(stlink_t* sl, int clk_freq) { struct stlink_libusb * const slu = sl->backend_data; unsigned char* const data = sl->q_buf; unsigned char* const cmd = sl->c_buf; @@ -661,6 +661,28 @@ int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) { // clock speed only supported by stlink/v2 and for firmware >= 22 if (sl->version.stlink_v == 2 && sl->version.jtag_v >= 22) { + uint16_t clk_divisor; + if (clk_freq) { + const uint32_t map[] = {5, 15, 25, 50, 100, 125, 240, 480, 950, 1200, 1800, 4000}; + int speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), clk_freq); + switch (map[speed_index]) { + case 5: clk_divisor = STLINK_SWDCLK_5KHZ_DIVISOR; break; + case 15: clk_divisor = STLINK_SWDCLK_15KHZ_DIVISOR; break; + case 25: clk_divisor = STLINK_SWDCLK_25KHZ_DIVISOR; break; + case 50: clk_divisor = STLINK_SWDCLK_50KHZ_DIVISOR; break; + case 100: clk_divisor = STLINK_SWDCLK_100KHZ_DIVISOR; break; + case 125: clk_divisor = STLINK_SWDCLK_125KHZ_DIVISOR; break; + case 240: clk_divisor = STLINK_SWDCLK_240KHZ_DIVISOR; break; + case 480: clk_divisor = STLINK_SWDCLK_480KHZ_DIVISOR; break; + case 950: clk_divisor = STLINK_SWDCLK_950KHZ_DIVISOR; break; + case 1200: clk_divisor = STLINK_SWDCLK_1P2MHZ_DIVISOR; break; + default: + case 1800: clk_divisor = STLINK_SWDCLK_1P8MHZ_DIVISOR; break; + case 4000: clk_divisor = STLINK_SWDCLK_4MHZ_DIVISOR; break; + } + } else + clk_divisor = STLINK_SWDCLK_1P8MHZ_DIVISOR; + i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); cmd[i++] = STLINK_DEBUG_COMMAND; @@ -691,7 +713,6 @@ int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) { } int speeds_size = data[8]; - if (speeds_size > STLINK_V3_MAX_FREQ_NB) { speeds_size = STLINK_V3_MAX_FREQ_NB; } @@ -701,7 +722,8 @@ int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) { // Set to zero all the next entries for (i = speeds_size; i < STLINK_V3_MAX_FREQ_NB; i++) map[i] = 0; - speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), 1800); + if (!clk_freq) clk_freq = 1800; // set default frequency + speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), clk_freq); i = fill_command(sl, SG_DXFER_FROM_DEV, 16); @@ -1228,45 +1250,7 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, int reset, char serial[STL // should be done at this speed too // set the stlink clock speed (default is 1800kHz) DLOG("JTAG/SWD freq set to %d\n", freq); - - switch (freq) { - case 5: - stlink_set_swdclk(sl, STLINK_SWDCLK_5KHZ_DIVISOR); - break; - case 15: - stlink_set_swdclk(sl, STLINK_SWDCLK_15KHZ_DIVISOR); - break; - case 25: - stlink_set_swdclk(sl, STLINK_SWDCLK_25KHZ_DIVISOR); - break; - case 50: - stlink_set_swdclk(sl, STLINK_SWDCLK_50KHZ_DIVISOR); - break; - case 100: - stlink_set_swdclk(sl, STLINK_SWDCLK_100KHZ_DIVISOR); - break; - case 125: - stlink_set_swdclk(sl, STLINK_SWDCLK_125KHZ_DIVISOR); - break; - case 240: - stlink_set_swdclk(sl, STLINK_SWDCLK_240KHZ_DIVISOR); - break; - case 480: - stlink_set_swdclk(sl, STLINK_SWDCLK_480KHZ_DIVISOR); - break; - case 950: - stlink_set_swdclk(sl, STLINK_SWDCLK_950KHZ_DIVISOR); - break; - case 1200: - stlink_set_swdclk(sl, STLINK_SWDCLK_1P2MHZ_DIVISOR); - break; - case 0: case 1800: - stlink_set_swdclk(sl, STLINK_SWDCLK_1P8MHZ_DIVISOR); - break; - case 4000: - stlink_set_swdclk(sl, STLINK_SWDCLK_4MHZ_DIVISOR); - break; - } + stlink_set_swdclk(sl, freq); if (reset == 2) { stlink_jtag_reset(sl, 0);