Set SWDCLK and fix jtag_reset bug

include/stlink.h

@@ -48,6 +48,8 @@ extern "C" {
 #define STLINK_JTAG_READDEBUG_32BIT 0x36
 #define STLINK_JTAG_DRIVE_NRST 0x3c
 
+#define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
+
     /* cortex core ids */
     // TODO clean this up...
 #define STM32VL_CORE_ID 0x1ba01477
@@ -57,6 +59,22 @@ extern "C" {
 #define STM32_FLASH_BASE 0x08000000
 #define STM32_SRAM_BASE 0x20000000
 
+// Baud rate divisors for SWDCLK
+#define STLINK_SWDCLK_4MHZ_DIVISOR		0
+#define STLINK_SWDCLK_1P8MHZ_DIVISOR	1
+#define STLINK_SWDCLK_1P2MHZ_DIVISOR	2
+#define STLINK_SWDCLK_950KHZ_DIVISOR	3
+#define STLINK_SWDCLK_480KHZ_DIVISOR	7
+#define STLINK_SWDCLK_240KHZ_DIVISOR	15
+#define STLINK_SWDCLK_125KHZ_DIVISOR	31
+#define STLINK_SWDCLK_100KHZ_DIVISOR	40
+#define STLINK_SWDCLK_50KHZ_DIVISOR		79
+#define STLINK_SWDCLK_25KHZ_DIVISOR		158
+#define STLINK_SWDCLK_15KHZ_DIVISOR		265
+#define STLINK_SWDCLK_5KHZ_DIVISOR		798
+
+
+
     /* Enough space to hold both a V2 command or a V1 command packaged as generic scsi*/
 #define C_BUF_LEN 32
 
@@ -177,6 +195,7 @@ typedef struct flash_loader {
     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_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);

include/stlink/backend.h

@@ -28,6 +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);		
     } stlink_backend_t;
 
 #endif /* STLINK_BACKEND_H_ */

src/common.c

@@ -642,6 +642,11 @@ int stlink_run(stlink_t *sl) {
     return sl->backend->run(sl);
 }
 
+int stlink_set_swdclk(stlink_t *sl, uint16_t divisor) {
+    DLOG("*** set_swdclk ***\n");
+    return sl->backend->set_swdclk(sl, divisor);
+}
+
 int stlink_status(stlink_t *sl) {
     int ret;
 

src/usb.c

@@ -449,6 +449,36 @@ int _stlink_usb_run(stlink_t* sl) {
     return 0;
 }
 
+
+int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) {
+    struct stlink_libusb * const slu = sl->backend_data;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd = sl->c_buf;
+    ssize_t size;
+    int rep_len = 2;
+    int i;
+
+    // clock speed only supported by stlink/v2 and for firmware >= 22
+    if (sl->version.stlink_v >= 2 && sl->version.jtag_v >= 22) {
+        i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
+
+        cmd[i++] = STLINK_DEBUG_COMMAND;
+        cmd[i++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
+        cmd[i++] = clk_divisor & 0xFF;
+        cmd[i++] = (clk_divisor >> 8) & 0xFF;
+
+        size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+        if (size == -1) {
+            printf("[!] send_recv STLINK_DEBUG_APIV2_SWD_SET_FREQ\n");
+            return (int) size;
+        }
+
+        return 0;
+    } else {
+        return -1;
+    }
+}
+
 int _stlink_usb_exit_debug_mode(stlink_t *sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const cmd = sl->c_buf;
@@ -724,7 +754,8 @@ static stlink_backend_t _stlink_usb_backend = {
     _stlink_usb_step,
     _stlink_usb_current_mode,
     _stlink_usb_force_debug,
-    _stlink_usb_target_voltage
+    _stlink_usb_target_voltage,
+    _stlink_usb_set_swdclk
 };
 
 stlink_t *stlink_open_usb(enum ugly_loglevel verbose, bool reset, char serial[16])
@@ -878,16 +909,21 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, bool reset, char serial[16
     if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
         stlink_enter_swd_mode(sl);
     }
+
+    // Initialize stlink version (sl->version)	
+    stlink_version(sl);	
 
     if (reset) {
         if( sl->version.stlink_v > 1 ) stlink_jtag_reset(sl, 2);
         stlink_reset(sl);
         usleep(10000);
     }
 
-    stlink_version(sl);
     ret = stlink_load_device_params(sl);
 
+    // Set the stlink clock speed (default is 1800kHz)
+    stlink_set_swdclk(sl, STLINK_SWDCLK_1P8MHZ_DIVISOR);    
+
 on_libusb_error:
     if (ret == -1) {
         stlink_close(sl);