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