+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;
+}
+