Remove FSF address from GPL notices
[fw/openocd] / src / jtag / drivers / vsllink.c
1 /***************************************************************************
2  *   Copyright (C) 2009-2010 by Simon Qian <SimonQian@SimonQian.com>       *
3  *                                                                         *
4  *   This program is free software; you can redistribute it and/or modify  *
5  *   it under the terms of the GNU General Public License as published by  *
6  *   the Free Software Foundation; either version 2 of the License, or     *
7  *   (at your option) any later version.                                   *
8  *                                                                         *
9  *   This program is distributed in the hope that it will be useful,       *
10  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
11  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
12  *   GNU General Public License for more details.                          *
13  *                                                                         *
14  *   You should have received a copy of the GNU General Public License     *
15  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
16  ***************************************************************************/
17
18 /* Versaloon is a programming tool for multiple MCUs.
19  * It's distributed under GPLv3.
20  * You can find it at http://www.Versaloon.com/.
21  */
22
23 #ifdef HAVE_CONFIG_H
24 #include "config.h"
25 #endif
26
27 #include <jtag/interface.h>
28 #include <jtag/commands.h>
29 #include <jtag/swd.h>
30 #include <libusb.h>
31
32 #include "versaloon/versaloon_include.h"
33 #include "versaloon/versaloon.h"
34
35 static int vsllink_tms_offset;
36
37 struct pending_scan_result {
38         int src_offset;
39         int dest_offset;
40         int length;     /* Number of bits to read */
41         struct scan_command *command;   /* Corresponding scan command */
42         uint8_t *ack;
43         uint8_t *buffer;
44         bool last;      /* indicate the last scan pending */
45 };
46
47 #define MAX_PENDING_SCAN_RESULTS 256
48
49 static int pending_scan_results_length;
50 static struct pending_scan_result
51         pending_scan_results_buffer[MAX_PENDING_SCAN_RESULTS];
52
53 /* Queue command functions */
54 static void vsllink_end_state(tap_state_t state);
55 static void vsllink_state_move(void);
56 static void vsllink_path_move(int num_states, tap_state_t *path);
57 static void vsllink_tms(int num_bits, const uint8_t *bits);
58 static void vsllink_runtest(int num_cycles);
59 static void vsllink_stableclocks(int num_cycles, int tms);
60 static void vsllink_scan(bool ir_scan, enum scan_type type,
61                 uint8_t *buffer, int scan_size, struct scan_command *command);
62 static void vsllink_reset(int trst, int srst);
63
64 /* VSLLink tap buffer functions */
65 static void vsllink_tap_append_step(int tms, int tdi);
66 static void vsllink_tap_init(void);
67 static int  vsllink_tap_execute(void);
68 static void vsllink_tap_ensure_pending(int scans);
69 static void vsllink_tap_append_scan(int length, uint8_t *buffer,
70                 struct scan_command *command);
71
72 /* VSLLink SWD functions */
73 static int_least32_t vsllink_swd_frequency(int_least32_t hz);
74 static int vsllink_swd_switch_seq(enum swd_special_seq seq);
75
76 /* VSLLink lowlevel functions */
77 struct vsllink {
78         struct libusb_context *libusb_ctx;
79         struct libusb_device_handle *usb_device_handle;
80 };
81
82 static int vsllink_usb_open(struct vsllink *vsllink);
83 static void vsllink_usb_close(struct vsllink *vsllink);
84
85 #if defined _DEBUG_JTAG_IO_
86 static void vsllink_debug_buffer(uint8_t *buffer, int length);
87 #endif
88
89 static int tap_length;
90 static int tap_buffer_size;
91 static uint8_t *tms_buffer;
92 static uint8_t *tdi_buffer;
93 static uint8_t *tdo_buffer;
94
95 static bool swd_mode;
96
97 static struct vsllink *vsllink_handle;
98
99 static int vsllink_execute_queue(void)
100 {
101         struct jtag_command *cmd = jtag_command_queue;
102         int scan_size;
103         enum scan_type type;
104         uint8_t *buffer;
105
106         DEBUG_JTAG_IO("-------------------------------------"
107                 " vsllink "
108                 "-------------------------------------");
109
110         while (cmd != NULL) {
111                 switch (cmd->type) {
112                         case JTAG_RUNTEST:
113                                 DEBUG_JTAG_IO("runtest %i cycles, end in %s",
114                                                 cmd->cmd.runtest->num_cycles,
115                                                 tap_state_name(cmd->cmd.runtest->end_state));
116
117                                 vsllink_end_state(cmd->cmd.runtest->end_state);
118                                 vsllink_runtest(cmd->cmd.runtest->num_cycles);
119                                 break;
120
121                         case JTAG_TLR_RESET:
122                                 DEBUG_JTAG_IO("statemove end in %s",
123                                                 tap_state_name(cmd->cmd.statemove->end_state));
124
125                                 vsllink_end_state(cmd->cmd.statemove->end_state);
126                                 vsllink_state_move();
127                                 break;
128
129                         case JTAG_PATHMOVE:
130                                 DEBUG_JTAG_IO("pathmove: %i states, end in %s",
131                                                 cmd->cmd.pathmove->num_states,
132                                                 tap_state_name(cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]));
133
134                                 vsllink_path_move(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
135                                 break;
136
137                         case JTAG_SCAN:
138                                 DEBUG_JTAG_IO("JTAG Scan...");
139
140                                 vsllink_end_state(cmd->cmd.scan->end_state);
141
142                                 scan_size = jtag_build_buffer(
143                                                 cmd->cmd.scan, &buffer);
144
145                                 if (cmd->cmd.scan->ir_scan)
146                                         DEBUG_JTAG_IO(
147                                                         "JTAG Scan write IR(%d bits), "
148                                                         "end in %s:",
149                                                         scan_size,
150                                                         tap_state_name(cmd->cmd.scan->end_state));
151
152                                 else
153                                         DEBUG_JTAG_IO(
154                                                         "JTAG Scan write DR(%d bits), "
155                                                         "end in %s:",
156                                                         scan_size,
157                                                         tap_state_name(cmd->cmd.scan->end_state));
158
159 #ifdef _DEBUG_JTAG_IO_
160                                 vsllink_debug_buffer(buffer,
161                                                 DIV_ROUND_UP(scan_size, 8));
162 #endif
163
164                                 type = jtag_scan_type(cmd->cmd.scan);
165
166                                 vsllink_scan(cmd->cmd.scan->ir_scan,
167                                                 type, buffer, scan_size,
168                                                 cmd->cmd.scan);
169                                 break;
170
171                         case JTAG_RESET:
172                                 DEBUG_JTAG_IO("reset trst: %i srst %i",
173                                                 cmd->cmd.reset->trst,
174                                                 cmd->cmd.reset->srst);
175
176                                 vsllink_tap_execute();
177
178                                 if (cmd->cmd.reset->trst == 1)
179                                         tap_set_state(TAP_RESET);
180
181                                 vsllink_reset(cmd->cmd.reset->trst,
182                                                 cmd->cmd.reset->srst);
183                                 break;
184
185                         case JTAG_SLEEP:
186                                 DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
187                                 vsllink_tap_execute();
188                                 jtag_sleep(cmd->cmd.sleep->us);
189                                 break;
190
191                         case JTAG_STABLECLOCKS:
192                                 DEBUG_JTAG_IO("add %d clocks",
193                                                 cmd->cmd.stableclocks->num_cycles);
194
195                                 switch (tap_get_state()) {
196                                 case TAP_RESET:
197                                         /* tms must be '1' to stay
198                                          * n TAP_RESET mode
199                                          */
200                                         scan_size = 1;
201                                         break;
202                                 case TAP_DRSHIFT:
203                                 case TAP_IDLE:
204                                 case TAP_DRPAUSE:
205                                 case TAP_IRSHIFT:
206                                 case TAP_IRPAUSE:
207                                         /* else, tms should be '0' */
208                                         scan_size = 0;
209                                         break;
210                                 /* above stable states are OK */
211                                 default:
212                                         LOG_ERROR("jtag_add_clocks() "
213                                                         "in non-stable state \"%s\"",
214                                                         tap_state_name(tap_get_state())
215                                         );
216                                         exit(-1);
217                                 }
218                                 vsllink_stableclocks(cmd->cmd.stableclocks->num_cycles, scan_size);
219                                 break;
220
221                                 case JTAG_TMS:
222                                         DEBUG_JTAG_IO("add %d jtag tms",
223                                                         cmd->cmd.tms->num_bits);
224
225                                         vsllink_tms(cmd->cmd.tms->num_bits, cmd->cmd.tms->bits);
226                                         break;
227
228                                 default:
229                                         LOG_ERROR("BUG: unknown JTAG command type "
230                                                         "encountered: %d", cmd->type);
231                                         exit(-1);
232                 }
233                 cmd = cmd->next;
234         }
235
236         return vsllink_tap_execute();
237 }
238
239 static int vsllink_speed(int speed)
240 {
241         if (swd_mode) {
242                 vsllink_swd_frequency(speed * 1000);
243                 return ERROR_OK;
244         }
245
246         versaloon_interface.adaptors.jtag_raw.config(0, (uint16_t)speed);
247         return versaloon_interface.adaptors.peripheral_commit();
248 }
249
250 static int vsllink_khz(int khz, int *jtag_speed)
251 {
252         *jtag_speed = khz;
253
254         return ERROR_OK;
255 }
256
257 static int vsllink_speed_div(int jtag_speed, int *khz)
258 {
259         *khz = jtag_speed;
260
261         return ERROR_OK;
262 }
263
264 static void vsllink_free_buffer(void)
265 {
266         if (tdi_buffer != NULL) {
267                 free(tdi_buffer);
268                 tdi_buffer = NULL;
269         }
270         if (tdo_buffer != NULL) {
271                 free(tdo_buffer);
272                 tdo_buffer = NULL;
273         }
274         if (tms_buffer != NULL) {
275                 free(tms_buffer);
276                 tms_buffer = NULL;
277         }
278 }
279
280 static int vsllink_quit(void)
281 {
282         versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
283                 0, 0, GPIO_SRST | GPIO_TRST);
284         versaloon_interface.adaptors.gpio.fini(0);
285
286         if (swd_mode)
287                 versaloon_interface.adaptors.swd.fini(0);
288         else
289                 versaloon_interface.adaptors.jtag_raw.fini(0);
290
291         versaloon_interface.adaptors.peripheral_commit();
292         versaloon_interface.fini();
293
294         vsllink_free_buffer();
295         vsllink_usb_close(vsllink_handle);
296
297         free(vsllink_handle);
298
299         return ERROR_OK;
300 }
301
302 static int vsllink_interface_init(void)
303 {
304         vsllink_handle = malloc(sizeof(struct vsllink));
305         if (NULL == vsllink_handle) {
306                 LOG_ERROR("unable to allocate memory");
307                 return ERROR_FAIL;
308         }
309
310         libusb_init(&vsllink_handle->libusb_ctx);
311
312         if (ERROR_OK != vsllink_usb_open(vsllink_handle)) {
313                 LOG_ERROR("Can't find USB JTAG Interface!" \
314                         "Please check connection and permissions.");
315                 return ERROR_JTAG_INIT_FAILED;
316         }
317         LOG_DEBUG("vsllink found on %04X:%04X",
318                 versaloon_interface.usb_setting.vid,
319                 versaloon_interface.usb_setting.pid);
320         versaloon_usb_device_handle = vsllink_handle->usb_device_handle;
321
322         if (ERROR_OK != versaloon_interface.init())
323                 return ERROR_FAIL;
324         if (versaloon_interface.usb_setting.buf_size < 32) {
325                 versaloon_interface.fini();
326                 return ERROR_FAIL;
327         }
328
329         return ERROR_OK;
330 }
331
332 static int vsllink_init(void)
333 {
334         int retval = vsllink_interface_init();
335         if (ERROR_OK != retval)
336                 return retval;
337
338         versaloon_interface.adaptors.gpio.init(0);
339         versaloon_interface.adaptors.gpio.config(0, GPIO_SRST, 0, GPIO_SRST,
340                 GPIO_SRST);
341         versaloon_interface.adaptors.delay.delayms(100);
342         versaloon_interface.adaptors.peripheral_commit();
343
344         if (swd_mode) {
345                 versaloon_interface.adaptors.gpio.config(0, GPIO_TRST, 0,
346                         GPIO_TRST, GPIO_TRST);
347                 versaloon_interface.adaptors.swd.init(0);
348                 vsllink_swd_frequency(jtag_get_speed_khz() * 1000);
349                 vsllink_swd_switch_seq(JTAG_TO_SWD);
350
351         } else {
352                 /* malloc buffer size for tap */
353                 tap_buffer_size = versaloon_interface.usb_setting.buf_size / 2 - 32;
354                 vsllink_free_buffer();
355                 tdi_buffer = malloc(tap_buffer_size);
356                 tdo_buffer = malloc(tap_buffer_size);
357                 tms_buffer = malloc(tap_buffer_size);
358                 if ((NULL == tdi_buffer) || (NULL == tdo_buffer) || (NULL == tms_buffer)) {
359                         vsllink_quit();
360                         return ERROR_FAIL;
361                 }
362
363                 versaloon_interface.adaptors.jtag_raw.init(0);
364                 versaloon_interface.adaptors.jtag_raw.config(0, jtag_get_speed_khz());
365                 versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
366                         GPIO_TRST, GPIO_SRST, GPIO_SRST);
367         }
368
369         if (ERROR_OK != versaloon_interface.adaptors.peripheral_commit())
370                 return ERROR_FAIL;
371
372         vsllink_reset(0, 0);
373         vsllink_tap_init();
374         return ERROR_OK;
375 }
376
377 /**************************************************************************
378  * Queue command implementations */
379
380 static void vsllink_end_state(tap_state_t state)
381 {
382         if (tap_is_state_stable(state))
383                 tap_set_end_state(state);
384         else {
385                 LOG_ERROR("BUG: %i is not a valid end state", state);
386                 exit(-1);
387         }
388 }
389
390 /* Goes to the end state. */
391 static void vsllink_state_move(void)
392 {
393         int i;
394         uint8_t tms_scan = tap_get_tms_path(tap_get_state(),
395                         tap_get_end_state());
396         uint8_t tms_scan_bits = tap_get_tms_path_len(tap_get_state(),
397                         tap_get_end_state());
398
399         for (i = 0; i < tms_scan_bits; i++)
400                 vsllink_tap_append_step((tms_scan >> i) & 1, 0);
401
402         tap_set_state(tap_get_end_state());
403 }
404
405 static void vsllink_path_move(int num_states, tap_state_t *path)
406 {
407         for (int i = 0; i < num_states; i++) {
408                 if (path[i] == tap_state_transition(tap_get_state(), false))
409                         vsllink_tap_append_step(0, 0);
410                 else if (path[i] == tap_state_transition(tap_get_state(), true))
411                         vsllink_tap_append_step(1, 0);
412                 else {
413                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition",
414                                 tap_state_name(tap_get_state()),
415                                 tap_state_name(path[i]));
416                         exit(-1);
417                 }
418
419                 tap_set_state(path[i]);
420         }
421
422         tap_set_end_state(tap_get_state());
423 }
424
425 static void vsllink_tms(int num_bits, const uint8_t *bits)
426 {
427         for (int i = 0; i < num_bits; i++)
428                 vsllink_tap_append_step((bits[i / 8] >> (i % 8)) & 1, 0);
429 }
430
431 static void vsllink_stableclocks(int num_cycles, int tms)
432 {
433         while (num_cycles > 0) {
434                 vsllink_tap_append_step(tms, 0);
435                 num_cycles--;
436         }
437 }
438
439 static void vsllink_runtest(int num_cycles)
440 {
441         tap_state_t saved_end_state = tap_get_end_state();
442
443         if (tap_get_state() != TAP_IDLE) {
444                 /* enter IDLE state */
445                 vsllink_end_state(TAP_IDLE);
446                 vsllink_state_move();
447         }
448
449         vsllink_stableclocks(num_cycles, 0);
450
451         /* post-process */
452         /* set end_state */
453         vsllink_end_state(saved_end_state);
454         if (tap_get_end_state() != tap_get_end_state())
455                 vsllink_state_move();
456 }
457
458 static void vsllink_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
459         int scan_size, struct scan_command *command)
460 {
461         tap_state_t saved_end_state;
462
463         saved_end_state = tap_get_end_state();
464
465         /* Move to appropriate scan state */
466         vsllink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
467
468         if (tap_get_state() != tap_get_end_state())
469                 vsllink_state_move();
470         vsllink_end_state(saved_end_state);
471
472         /* Scan */
473         vsllink_tap_append_scan(scan_size, buffer, command);
474
475         /* Goto Pause and record position to insert tms:0 */
476         vsllink_tap_append_step(0, 0);
477         vsllink_tms_offset = tap_length;
478
479         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
480
481         if (tap_get_state() != tap_get_end_state())
482                 vsllink_state_move();
483 }
484
485 static void vsllink_reset(int trst, int srst)
486 {
487         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
488
489         if (!srst)
490                 versaloon_interface.adaptors.gpio.config(0, GPIO_SRST, 0, GPIO_SRST, GPIO_SRST);
491         else
492                 versaloon_interface.adaptors.gpio.config(0, GPIO_SRST, GPIO_SRST, 0, 0);
493
494         if (!swd_mode) {
495                 if (!trst)
496                         versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, GPIO_TRST);
497                 else
498                         versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, 0);
499         }
500
501         versaloon_interface.adaptors.peripheral_commit();
502 }
503
504 COMMAND_HANDLER(vsllink_handle_usb_vid_command)
505 {
506         if (CMD_ARGC != 1)
507                 return ERROR_COMMAND_SYNTAX_ERROR;
508
509         COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0],
510                 versaloon_interface.usb_setting.vid);
511         return ERROR_OK;
512 }
513
514 COMMAND_HANDLER(vsllink_handle_usb_pid_command)
515 {
516         if (CMD_ARGC != 1)
517                 return ERROR_COMMAND_SYNTAX_ERROR;
518         COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0],
519                 versaloon_interface.usb_setting.pid);
520         return ERROR_OK;
521 }
522
523 COMMAND_HANDLER(vsllink_handle_usb_serial_command)
524 {
525         if (CMD_ARGC > 1)
526                 return ERROR_COMMAND_SYNTAX_ERROR;
527
528         free(versaloon_interface.usb_setting.serialstring);
529
530         if (CMD_ARGC == 1)
531                 versaloon_interface.usb_setting.serialstring = strdup(CMD_ARGV[0]);
532         else
533                 versaloon_interface.usb_setting.serialstring = NULL;
534
535         return ERROR_OK;
536 }
537
538 COMMAND_HANDLER(vsllink_handle_usb_bulkin_command)
539 {
540         if (CMD_ARGC != 1)
541                 return ERROR_COMMAND_SYNTAX_ERROR;
542
543         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0],
544                 versaloon_interface.usb_setting.ep_in);
545
546         versaloon_interface.usb_setting.ep_in |= 0x80;
547
548         return ERROR_OK;
549 }
550
551 COMMAND_HANDLER(vsllink_handle_usb_bulkout_command)
552 {
553         if (CMD_ARGC != 1)
554                 return ERROR_COMMAND_SYNTAX_ERROR;
555
556         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0],
557                 versaloon_interface.usb_setting.ep_out);
558
559         versaloon_interface.usb_setting.ep_out &= ~0x80;
560
561         return ERROR_OK;
562 }
563
564 COMMAND_HANDLER(vsllink_handle_usb_interface_command)
565 {
566         if (CMD_ARGC != 1)
567                 return ERROR_COMMAND_SYNTAX_ERROR;
568
569         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0],
570                 versaloon_interface.usb_setting.interface);
571         return ERROR_OK;
572 }
573
574 /**************************************************************************
575  * VSLLink tap functions */
576
577 static void vsllink_tap_init(void)
578 {
579         tap_length = 0;
580         pending_scan_results_length = 0;
581         vsllink_tms_offset = 0;
582 }
583
584 static void vsllink_tap_ensure_pending(int scans)
585 {
586         int available_scans =
587                 MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
588
589         if (scans > available_scans)
590                 vsllink_tap_execute();
591 }
592
593 static void vsllink_tap_append_step(int tms, int tdi)
594 {
595         int index_var = tap_length / 8;
596
597         int bit_index = tap_length % 8;
598         uint8_t bit = 1 << bit_index;
599
600         if (tms)
601                 tms_buffer[index_var] |= bit;
602         else
603                 tms_buffer[index_var] &= ~bit;
604
605         if (tdi)
606                 tdi_buffer[index_var] |= bit;
607         else
608                 tdi_buffer[index_var] &= ~bit;
609
610         tap_length++;
611
612         if (tap_buffer_size * 8 <= tap_length)
613                 vsllink_tap_execute();
614 }
615
616 static void vsllink_tap_append_scan(int length, uint8_t *buffer,
617         struct scan_command *command)
618 {
619         struct pending_scan_result *pending_scan_result;
620         int len_tmp, len_all, i;
621
622         len_all = 0;
623         while (len_all < length) {
624                 vsllink_tap_ensure_pending(1);
625                 pending_scan_result =
626                         &pending_scan_results_buffer[
627                                 pending_scan_results_length];
628
629                 if ((length - len_all) > (tap_buffer_size * 8 - tap_length)) {
630                         /* Use all memory available
631                            vsllink_tap_append_step will commit automatically */
632                         len_tmp = tap_buffer_size * 8 - tap_length;
633                         pending_scan_result->last = false;
634                 } else {
635                         len_tmp = length - len_all;
636                         pending_scan_result->last = true;
637                 }
638                 pending_scan_result->src_offset = tap_length;
639                 pending_scan_result->dest_offset = len_all;
640                 pending_scan_result->length = len_tmp;
641                 pending_scan_result->command = command;
642                 pending_scan_result->buffer = buffer;
643                 pending_scan_results_length++;
644
645                 for (i = 0; i < len_tmp; i++) {
646                         vsllink_tap_append_step(((len_all + i) < length-1
647                                                  ? 0 : 1),
648                                 (buffer[(len_all + i)/8]
649                                  >> ((len_all + i)%8)) & 1);
650                 }
651
652                 len_all += len_tmp;
653         }
654 }
655
656 static int vsllink_jtag_execute(void)
657 {
658         int i;
659         int result;
660
661         if (tap_length <= 0)
662                 return ERROR_OK;
663
664         versaloon_interface.adaptors.jtag_raw.execute(0, tdi_buffer, tms_buffer,
665                 tdo_buffer, tap_length);
666
667         result = versaloon_interface.adaptors.peripheral_commit();
668
669         if (result == ERROR_OK) {
670                 for (i = 0; i < pending_scan_results_length; i++) {
671                         struct pending_scan_result *pending_scan_result =
672                                 &pending_scan_results_buffer[i];
673                         uint8_t *buffer = pending_scan_result->buffer;
674                         int length = pending_scan_result->length;
675                         int src_first = pending_scan_result->src_offset;
676                         int dest_first = pending_scan_result->dest_offset;
677                         bool last = pending_scan_result->last;
678
679                         struct scan_command *command;
680
681                         command = pending_scan_result->command;
682                         buf_set_buf(tdo_buffer, src_first, buffer, dest_first, length);
683
684 #ifdef _DEBUG_JTAG_IO_
685                         DEBUG_JTAG_IO(
686                                 "JTAG scan read(%d bits, from src %d bits to dest %d bits):",
687                                 length, src_first, dest_first);
688                         vsllink_debug_buffer(buffer + dest_first / 8,
689                                 DIV_ROUND_UP(length, 7));
690 #endif
691
692                         if (last) {
693                                 if (jtag_read_buffer(buffer, command)
694                                     != ERROR_OK) {
695                                         vsllink_tap_init();
696                                         return ERROR_JTAG_QUEUE_FAILED;
697                                 }
698
699                                 if (pending_scan_result->buffer != NULL)
700                                         free(pending_scan_result->buffer);
701                         }
702                 }
703         } else {
704                 LOG_ERROR("vsllink_jtag_execute failure");
705                 return ERROR_JTAG_QUEUE_FAILED;
706         }
707
708         vsllink_tap_init();
709
710         return ERROR_OK;
711 }
712
713 static int vsllink_tap_execute(void)
714 {
715         if (swd_mode)
716                 return ERROR_OK;
717
718         return vsllink_jtag_execute();
719 }
720
721 static int vsllink_swd_init(void)
722 {
723         LOG_INFO("VSLLink SWD mode enabled");
724         swd_mode = true;
725
726         return ERROR_OK;
727 }
728
729 static int_least32_t vsllink_swd_frequency(int_least32_t hz)
730 {
731         const int_least32_t delay2hz[] = {
732                 1850000, 235000, 130000, 102000, 85000, 72000
733         };
734
735         if (hz > 0) {
736                 uint16_t delay = UINT16_MAX;
737
738                 for (uint16_t i = 0; i < ARRAY_SIZE(delay2hz); i++) {
739                         if (hz >= delay2hz[i]) {
740                                 hz = delay2hz[i];
741                                 delay = i;
742                                 break;
743                         }
744                 }
745
746                 if (delay == UINT16_MAX)
747                         delay = (500000 / hz) - 1;
748
749                 /* Calculate retry count after a WAIT response. This will give
750                  * a retry timeout at about ~250 ms. 54 is the number of bits
751                  * found in a transaction. */
752                 uint16_t retry_count = 250 * hz / 1000 / 54;
753
754                 LOG_DEBUG("SWD delay: %d, retry count: %d", delay, retry_count);
755
756                 versaloon_interface.adaptors.swd.config(0, 2, retry_count, delay);
757         }
758
759         return hz;
760 }
761
762 static int vsllink_swd_switch_seq(enum swd_special_seq seq)
763 {
764         switch (seq) {
765         case LINE_RESET:
766                 LOG_DEBUG("SWD line reset");
767                 versaloon_interface.adaptors.swd.seqout(0, swd_seq_line_reset,
768                                 swd_seq_line_reset_len);
769                 break;
770         case JTAG_TO_SWD:
771                 LOG_DEBUG("JTAG-to-SWD");
772                 versaloon_interface.adaptors.swd.seqout(0, swd_seq_jtag_to_swd,
773                                 swd_seq_jtag_to_swd_len);
774                 break;
775         case SWD_TO_JTAG:
776                 LOG_DEBUG("SWD-to-JTAG");
777                 versaloon_interface.adaptors.swd.seqout(0, swd_seq_swd_to_jtag,
778                                 swd_seq_swd_to_jtag_len);
779                 break;
780         default:
781                 LOG_ERROR("Sequence %d not supported", seq);
782                 return ERROR_FAIL;
783         }
784
785         return ERROR_OK;
786 }
787
788 static void vsllink_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
789 {
790         versaloon_interface.adaptors.swd.transact(0, cmd, value, NULL);
791 }
792
793 static void vsllink_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
794 {
795         versaloon_interface.adaptors.swd.transact(0, cmd, &value, NULL);
796 }
797
798 static int vsllink_swd_run_queue(void)
799 {
800         return versaloon_interface.adaptors.peripheral_commit();
801 }
802
803 /****************************************************************************
804  * VSLLink USB low-level functions */
805
806 static int vsllink_check_usb_strings(
807         struct libusb_device_handle *usb_device_handle,
808         struct libusb_device_descriptor *usb_desc)
809 {
810         char desc_string[256];
811         int retval;
812
813         if (NULL != versaloon_interface.usb_setting.serialstring) {
814                 retval = libusb_get_string_descriptor_ascii(usb_device_handle,
815                         usb_desc->iSerialNumber, (unsigned char *)desc_string,
816                         sizeof(desc_string));
817                 if (retval < 0)
818                         return ERROR_FAIL;
819
820                 if (strncmp(desc_string, versaloon_interface.usb_setting.serialstring,
821                                 sizeof(desc_string)))
822                         return ERROR_FAIL;
823         }
824
825         retval = libusb_get_string_descriptor_ascii(usb_device_handle,
826                 usb_desc->iProduct, (unsigned char *)desc_string,
827                 sizeof(desc_string));
828         if (retval < 0)
829                 return ERROR_FAIL;
830
831         if (strstr(desc_string, "Versaloon") == NULL)
832                 return ERROR_FAIL;
833
834         return ERROR_OK;
835 }
836
837 static int vsllink_usb_open(struct vsllink *vsllink)
838 {
839         ssize_t num_devices, i;
840         libusb_device **usb_devices;
841         struct libusb_device_descriptor usb_desc;
842         struct libusb_device_handle *usb_device_handle;
843         int retval;
844
845         num_devices = libusb_get_device_list(vsllink->libusb_ctx, &usb_devices);
846
847         if (num_devices <= 0)
848                 return ERROR_FAIL;
849
850         for (i = 0; i < num_devices; i++) {
851                 libusb_device *device = usb_devices[i];
852
853                 retval = libusb_get_device_descriptor(device, &usb_desc);
854                 if (retval != 0)
855                         continue;
856
857                 if (usb_desc.idVendor != versaloon_interface.usb_setting.vid ||
858                         usb_desc.idProduct != versaloon_interface.usb_setting.pid)
859                         continue;
860
861                 retval = libusb_open(device, &usb_device_handle);
862                 if (retval != 0)
863                         continue;
864
865                 retval = vsllink_check_usb_strings(usb_device_handle, &usb_desc);
866                 if (ERROR_OK == retval)
867                         break;
868
869                 libusb_close(usb_device_handle);
870         }
871
872         libusb_free_device_list(usb_devices, 1);
873
874         if (i == num_devices)
875                 return ERROR_FAIL;
876
877         retval = libusb_claim_interface(usb_device_handle,
878                 versaloon_interface.usb_setting.interface);
879         if (retval != 0) {
880                 LOG_ERROR("unable to claim interface");
881                 libusb_close(usb_device_handle);
882                 return ERROR_FAIL;
883         }
884
885         vsllink->usb_device_handle = usb_device_handle;
886         return ERROR_OK;
887 }
888
889 static void vsllink_usb_close(struct vsllink *vsllink)
890 {
891         libusb_release_interface(vsllink->usb_device_handle,
892                 versaloon_interface.usb_setting.interface);
893         libusb_close(vsllink->usb_device_handle);
894 }
895
896 #define BYTES_PER_LINE  16
897
898 #if defined _DEBUG_JTAG_IO_
899 static void vsllink_debug_buffer(uint8_t *buffer, int length)
900 {
901         char line[81];
902         char s[4];
903         int i;
904         int j;
905
906         for (i = 0; i < length; i += BYTES_PER_LINE) {
907                 snprintf(line, 5, "%04x", i);
908                 for (j = i; j < i + BYTES_PER_LINE && j < length; j++) {
909                         snprintf(s, 4, " %02x", buffer[j]);
910                         strcat(line, s);
911                 }
912                 LOG_DEBUG("%s", line);
913         }
914 }
915 #endif  /* _DEBUG_JTAG_IO_ */
916
917 static const struct command_registration vsllink_command_handlers[] = {
918         {
919                 .name = "vsllink_usb_vid",
920                 .handler = &vsllink_handle_usb_vid_command,
921                 .mode = COMMAND_CONFIG,
922         },
923         {
924                 .name = "vsllink_usb_pid",
925                 .handler = &vsllink_handle_usb_pid_command,
926                 .mode = COMMAND_CONFIG,
927         },
928         {
929                 .name = "vsllink_usb_serial",
930                 .handler = &vsllink_handle_usb_serial_command,
931                 .mode = COMMAND_CONFIG,
932         },
933         {
934                 .name = "vsllink_usb_bulkin",
935                 .handler = &vsllink_handle_usb_bulkin_command,
936                 .mode = COMMAND_CONFIG,
937         },
938         {
939                 .name = "vsllink_usb_bulkout",
940                 .handler = &vsllink_handle_usb_bulkout_command,
941                 .mode = COMMAND_CONFIG,
942         },
943         {
944                 .name = "vsllink_usb_interface",
945                 .handler = &vsllink_handle_usb_interface_command,
946                 .mode = COMMAND_CONFIG,
947         },
948         COMMAND_REGISTRATION_DONE
949 };
950
951 static const char * const vsllink_transports[] = {"jtag", "swd", NULL};
952
953 static const struct swd_driver vsllink_swd_driver = {
954         .init = vsllink_swd_init,
955         .frequency = vsllink_swd_frequency,
956         .switch_seq = vsllink_swd_switch_seq,
957         .read_reg = vsllink_swd_read_reg,
958         .write_reg = vsllink_swd_write_reg,
959         .run = vsllink_swd_run_queue,
960 };
961
962 struct jtag_interface vsllink_interface = {
963         .name = "vsllink",
964         .supported = DEBUG_CAP_TMS_SEQ,
965         .commands = vsllink_command_handlers,
966         .transports = vsllink_transports,
967         .swd = &vsllink_swd_driver,
968
969         .init = vsllink_init,
970         .quit = vsllink_quit,
971         .khz = vsllink_khz,
972         .speed = vsllink_speed,
973         .speed_div = vsllink_speed_div,
974         .execute_queue = vsllink_execute_queue,
975 };