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 <spen@spen-soft.co.uk>
Reviewed-on: http://openocd.zylin.com/2335
Tested-by: jenkins

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 58af0dd59c42e693883ce30a4fa1eae9e9cf1266..7572e871a650d7a13b18389457edc99855f55a32 100644 (file)
--- 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,
 };