git.gag.com Git - fw/openocd/blob - src/jtag/vsllink.c

   1 /***************************************************************************
   2  *   Copyright (C) 2009 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  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  18  ***************************************************************************/
  19
  20 /* Versaloon is a programming tool for multiple MCUs.
  21  * OpenOCD and MSP430 supports are distributed under GPLv2.
  22  * You can find it at http://www.SimonQian.com/en/Versaloon.
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include "config.h"
  27 #endif
  28
  29 #include "interface.h"
  30 #include "commands.h"
  31
  32 #include <usb.h>
  33
  34
  35 //#define _VSLLINK_IN_DEBUG_MODE_
  36
  37 #define VSLLINK_MODE_NORMAL                     0
  38 #define VSLLINK_MODE_DMA                        1
  39
  40 static uint16_t vsllink_usb_vid;
  41 static uint16_t vsllink_usb_pid;
  42 static uint8_t vsllink_usb_bulkout;
  43 static uint8_t vsllink_usb_bulkin;
  44 static uint8_t vsllink_usb_interface;
  45 static uint8_t vsllink_mode = VSLLINK_MODE_NORMAL;
  46 static int VSLLINK_USB_TIMEOUT = 10000;
  47
  48 static int VSLLINK_BufferSize = 1024;
  49
  50 /* Global USB buffers */
  51 static int vsllink_usb_out_buffer_idx;
  52 static int vsllink_usb_in_want_length;
  53 static uint8_t* vsllink_usb_in_buffer = NULL;
  54 static uint8_t* vsllink_usb_out_buffer = NULL;
  55
  56 /* Constants for VSLLink command */
  57 #define VSLLINK_CMD_CONN                        0x80
  58 #define VSLLINK_CMD_DISCONN                     0x81
  59 #define VSLLINK_CMD_SET_SPEED           0x82
  60 #define VSLLINK_CMD_SET_PORT            0x90
  61 #define VSLLINK_CMD_GET_PORT            0x91
  62 #define VSLLINK_CMD_SET_PORTDIR         0x92
  63 #define VSLLINK_CMD_HW_JTAGSEQCMD       0xA0
  64 #define VSLLINK_CMD_HW_JTAGHLCMD        0xA1
  65 #define VSLLINK_CMD_HW_SWDCMD           0xA2
  66 #define VSLLINK_CMD_HW_JTAGRAWCMD       0xA3
  67
  68 #define VSLLINK_CMDJTAGSEQ_TMSBYTE      0x00
  69 #define VSLLINK_CMDJTAGSEQ_TMSCLOCK     0x40
  70 #define VSLLINK_CMDJTAGSEQ_SCAN         0x80
  71
  72 #define VSLLINK_CMDJTAGSEQ_CMDMSK       0xC0
  73 #define VSLLINK_CMDJTAGSEQ_LENMSK       0x3F
  74
  75 #define JTAG_PINMSK_SRST                        (1 << 0)
  76 #define JTAG_PINMSK_TRST                        (1 << 1)
  77 #define JTAG_PINMSK_USR1                        (1 << 2)
  78 #define JTAG_PINMSK_USR2                        (1 << 3)
  79 #define JTAG_PINMSK_TCK                         (1 << 4)
  80 #define JTAG_PINMSK_TMS                         (1 << 5)
  81 #define JTAG_PINMSK_TDI                         (1 << 6)
  82 #define JTAG_PINMSK_TDO                         (1 << 7)
  83
  84
  85 #define VSLLINK_TAP_MOVE(from, to)      VSLLINK_tap_move[tap_move_ndx(from)][tap_move_ndx(to)]
  86
  87 /* VSLLINK_tap_move[i][j]: tap movement command to go from state i to state j
  88  * 0: Test-Logic-Reset
  89  * 1: Run-Test/Idle
  90  * 2: Shift-DR
  91  * 3: Pause-DR
  92  * 4: Shift-IR
  93  * 5: Pause-IR
  94  *
  95  * SD->SD and SI->SI have to be caught in interface specific code
  96  */
  97 static uint8_t VSLLINK_tap_move[6][6] =
  98 {
  99 /*        TLR   RTI   SD    PD    SI    PI             */
 100         {0xff, 0x7f, 0x2f, 0x0a, 0x37, 0x16},   /* TLR */
 101         {0xff, 0x00, 0x45, 0x05, 0x4b, 0x0b},   /* RTI */
 102         {0xff, 0x61, 0x00, 0x01, 0x0f, 0x2f},   /* SD  */
 103         {0xfe, 0x60, 0x40, 0x5c, 0x3c, 0x5e},   /* PD  */
 104         {0xff, 0x61, 0x07, 0x17, 0x00, 0x01},   /* SI  */
 105         {0xfe, 0x60, 0x38, 0x5c, 0x40, 0x5e}    /* PI  */
 106 };
 107
 108 typedef struct insert_insignificant_operation
 109 {
 110         unsigned char insert_value;
 111         unsigned char insert_position;
 112 }insert_insignificant_operation_t;
 113
 114 static insert_insignificant_operation_t VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[6][6] =
 115 {
 116 /*       stuff  offset   */
 117         {/*     TLR     */
 118         {1,             0,},    /* TLR */
 119         {1,             0,},    /* RTI */
 120         {1,             0,},    /* SD  */
 121         {1,             0,},    /* PD  */
 122         {1,             0,},    /* SI  */
 123         {1,             0,}},   /* PI  */
 124         {/*     RTI     */
 125         {1,             0,},    /* TLR */
 126         {0,             0,},    /* RTI */
 127         {0,             4,},    /* SD  */
 128         {0,             7,},    /* PD  */
 129         {0,             5,},    /* SI  */
 130         {0,             7,}},   /* PI  */
 131         {/*     SD      */
 132         {0,             0,},    /* TLR */
 133         {0,             0,},    /* RTI */
 134         {0,             0,},    /* SD  */
 135         {0,             0,},    /* PD  */
 136         {0,             0,},    /* SI  */
 137         {0,             0,}},   /* PI  */
 138         {/*     PD      */
 139         {0,             0,},    /* TLR */
 140         {0,             0,},    /* RTI */
 141         {0,             0,},    /* SD  */
 142         {0,             0,},    /* PD  */
 143         {0,             0,},    /* SI  */
 144         {0,             0,}},   /* PI  */
 145         {/*     SI      */
 146         {0,             0,},    /* TLR */
 147         {0,             0,},    /* RTI */
 148         {0,             0,},    /* SD  */
 149         {0,             0,},    /* PD  */
 150         {0,             0,},    /* SI  */
 151         {0,             0,}},   /* PI  */
 152         {/*     PI      */
 153         {0,             0,},    /* TLR */
 154         {0,             0,},    /* RTI */
 155         {0,             0,},    /* SD  */
 156         {0,             0,},    /* PD  */
 157         {0,             0,},    /* SI  */
 158         {0,             0,}},   /* PI  */
 159 };
 160
 161 static uint8_t VSLLINK_BIT_MSK[8] =
 162 {
 163         0x00, 0x01, 0x03, 0x07, 0x0f, 0x1f, 0x3f, 0x7f
 164 };
 165
 166 struct pending_scan_result {
 167         int offset;
 168         int length; /* Number of bits to read */
 169         scan_command_t *command; /* Corresponding scan command */
 170         uint8_t *buffer;
 171 };
 172
 173 #define MAX_PENDING_SCAN_RESULTS 256
 174
 175 static int pending_scan_results_length;
 176 static struct pending_scan_result pending_scan_results_buffer[MAX_PENDING_SCAN_RESULTS];
 177
 178 /* Queue command functions */
 179 static void vsllink_end_state(tap_state_t state);
 180 static void vsllink_state_move_dma(void);
 181 static void vsllink_state_move_normal(void);
 182 static void (*vsllink_state_move)(void);
 183 static void vsllink_path_move_dma(int num_states, tap_state_t *path);
 184 static void vsllink_path_move_normal(int num_states, tap_state_t *path);
 185 static void (*vsllink_path_move)(int num_states, tap_state_t *path);
 186 static void vsllink_runtest(int num_cycles);
 187 static void vsllink_stableclocks_dma(int num_cycles, int tms);
 188 static void vsllink_stableclocks_normal(int num_cycles, int tms);
 189 static void (*vsllink_stableclocks)(int num_cycles, int tms);
 190 static void vsllink_scan_dma(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size, scan_command_t *command);
 191 static void vsllink_scan_normal(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size, scan_command_t *command);
 192 static void (*vsllink_scan)(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size, scan_command_t *command);
 193 static void vsllink_reset(int trst, int srst);
 194 static void vsllink_simple_command(uint8_t command);
 195 static int vsllink_connect(void);
 196 static int vsllink_disconnect(void);
 197
 198 /* VSLLink tap buffer functions */
 199 static void vsllink_tap_append_step(int tms, int tdi);
 200 static void vsllink_tap_init_dma(void);
 201 static void vsllink_tap_init_normal(void);
 202 static void (*vsllink_tap_init)(void);
 203 static int vsllink_tap_execute_dma(void);
 204 static int vsllink_tap_execute_normal(void);
 205 static int (*vsllink_tap_execute)(void);
 206 static void vsllink_tap_ensure_space_dma(int scans, int length);
 207 static void vsllink_tap_ensure_space_normal(int scans, int length);
 208 static void (*vsllink_tap_ensure_space)(int scans, int length);
 209 static void vsllink_tap_append_scan_dma(int length, uint8_t *buffer, scan_command_t *command);
 210 static void vsllink_tap_append_scan_normal(int length, uint8_t *buffer, scan_command_t *command, int offset);
 211
 212 /* VSLLink lowlevel functions */
 213 typedef struct vsllink_jtag
 214 {
 215         struct usb_dev_handle* usb_handle;
 216 } vsllink_jtag_t;
 217
 218 static vsllink_jtag_t *vsllink_usb_open(void);
 219 static void vsllink_usb_close(vsllink_jtag_t *vsllink_jtag);
 220 static int vsllink_usb_message(vsllink_jtag_t *vsllink_jtag, int out_length, int in_length);
 221 static int vsllink_usb_write(vsllink_jtag_t *vsllink_jtag, int out_length);
 222 static int vsllink_usb_read(vsllink_jtag_t *vsllink_jtag);
 223
 224 #if defined _DEBUG_USB_COMMS_ || defined _DEBUG_JTAG_IO_
 225 static void vsllink_debug_buffer(uint8_t *buffer, int length);
 226 #endif
 227
 228 static int vsllink_tms_data_len = 0;
 229 static uint8_t* vsllink_tms_cmd_pos;
 230
 231 static int tap_length = 0;
 232 static int tap_buffer_size = 0;
 233 static uint8_t *tms_buffer = NULL;
 234 static uint8_t *tdi_buffer = NULL;
 235 static uint8_t *tdo_buffer = NULL;
 236 static int last_tms;
 237
 238 static vsllink_jtag_t* vsllink_jtag_handle = NULL;
 239
 240 static void reset_command_pointer(void)
 241 {
 242         if (vsllink_mode == VSLLINK_MODE_NORMAL)
 243         {
 244                 vsllink_usb_out_buffer[0] = VSLLINK_CMD_HW_JTAGSEQCMD;
 245                 vsllink_usb_out_buffer_idx = 3;
 246         }
 247         else
 248         {
 249                 tap_length = 0;
 250         }
 251 }
 252
 253 static int vsllink_execute_queue(void)
 254 {
 255         jtag_command_t *cmd = jtag_command_queue;
 256         int scan_size;
 257         enum scan_type type;
 258         uint8_t *buffer;
 259
 260         DEBUG_JTAG_IO("--------------------------------- vsllink -------------------------------------");
 261
 262         reset_command_pointer();
 263         while (cmd != NULL)
 264         {
 265                 switch (cmd->type)
 266                 {
 267                         case JTAG_RUNTEST:
 268                                 DEBUG_JTAG_IO("runtest %i cycles, end in %s", cmd->cmd.runtest->num_cycles, \
 269                                         tap_state_name(cmd->cmd.runtest->end_state));
 270
 271                                 vsllink_end_state(cmd->cmd.runtest->end_state);
 272                                 vsllink_runtest(cmd->cmd.runtest->num_cycles);
 273                                 break;
 274
 275                         case JTAG_STATEMOVE:
 276                                 DEBUG_JTAG_IO("statemove end in %s", tap_state_name(cmd->cmd.statemove->end_state));
 277
 278                                 vsllink_end_state(cmd->cmd.statemove->end_state);
 279                                 vsllink_state_move();
 280                                 break;
 281
 282                         case JTAG_PATHMOVE:
 283                                 DEBUG_JTAG_IO("pathmove: %i states, end in %s", \
 284                                         cmd->cmd.pathmove->num_states, \
 285                                         tap_state_name(cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]));
 286
 287                                 vsllink_path_move(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
 288                                 break;
 289
 290                         case JTAG_SCAN:
 291                                 vsllink_end_state(cmd->cmd.scan->end_state);
 292
 293                                 scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
 294                                 if (cmd->cmd.scan->ir_scan)
 295                                 {
 296                                         DEBUG_JTAG_IO("JTAG Scan write IR(%d bits), end in %s:", scan_size, tap_state_name(cmd->cmd.scan->end_state));
 297                                 }
 298                                 else
 299                                 {
 300                                         DEBUG_JTAG_IO("JTAG Scan write DR(%d bits), end in %s:", scan_size, tap_state_name(cmd->cmd.scan->end_state));
 301                                 }
 302
 303 #ifdef _DEBUG_JTAG_IO_
 304                                 vsllink_debug_buffer(buffer, (scan_size + 7) >> 3);
 305 #endif
 306
 307                                 type = jtag_scan_type(cmd->cmd.scan);
 308
 309                                 vsllink_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size, cmd->cmd.scan);
 310                                 break;
 311
 312                         case JTAG_RESET:
 313                                 DEBUG_JTAG_IO("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 314
 315                                 vsllink_tap_execute();
 316
 317                                 if (cmd->cmd.reset->trst == 1)
 318                                 {
 319                                         tap_set_state(TAP_RESET);
 320                                 }
 321                                 vsllink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 322                                 break;
 323
 324                         case JTAG_SLEEP:
 325                                 DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
 326                                 vsllink_tap_execute();
 327                                 jtag_sleep(cmd->cmd.sleep->us);
 328                                 break;
 329
 330                         case JTAG_STABLECLOCKS:
 331                                 DEBUG_JTAG_IO("add %d clocks", cmd->cmd.stableclocks->num_cycles);
 332                                 switch (tap_get_state())
 333                                 {
 334                                 case TAP_RESET:
 335                                         // tms should be '1' to stay in TAP_RESET mode
 336                                         scan_size = 1;
 337                                         break;
 338                                 case TAP_DRSHIFT:
 339                                 case TAP_IDLE:
 340                                 case TAP_DRPAUSE:
 341                                 case TAP_IRSHIFT:
 342                                 case TAP_IRPAUSE:
 343                                         // in other mode, tms should be '0'
 344                                         scan_size = 0;
 345                                         break;                  /* above stable states are OK */
 346                                 default:
 347                                          LOG_ERROR("jtag_add_clocks() was called with TAP in non-stable state \"%s\"",
 348                                                          tap_state_name(tap_get_state()));
 349                                          exit(-1);
 350                                 }
 351                                 vsllink_stableclocks(cmd->cmd.stableclocks->num_cycles, scan_size);
 352                                 break;
 353
 354                         default:
 355                                 LOG_ERROR("BUG: unknown JTAG command type encountered: %d", cmd->type);
 356                                 exit(-1);
 357                 }
 358                 cmd = cmd->next;
 359         }
 360
 361         return vsllink_tap_execute();
 362 }
 363
 364 static int vsllink_speed(int speed)
 365 {
 366         int result;
 367
 368         vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_SPEED;
 369         vsllink_usb_out_buffer[1] = (speed >> 0) & 0xff;
 370         vsllink_usb_out_buffer[2] = (speed >> 8) & 0xFF;
 371
 372         result = vsllink_usb_write(vsllink_jtag_handle, 3);
 373
 374         if (result == 3)
 375         {
 376                 return ERROR_OK;
 377         }
 378         else
 379         {
 380                 LOG_ERROR("VSLLink setting speed failed (%d)", result);
 381                 return ERROR_JTAG_DEVICE_ERROR;
 382         }
 383
 384         return ERROR_OK;
 385 }
 386
 387 static int vsllink_khz(int khz, int *jtag_speed)
 388 {
 389         *jtag_speed = khz;
 390
 391         return ERROR_OK;
 392 }
 393
 394 static int vsllink_speed_div(int jtag_speed, int *khz)
 395 {
 396         *khz = jtag_speed;
 397
 398         return ERROR_OK;
 399 }
 400
 401 static int vsllink_init(void)
 402 {
 403         int check_cnt, to_tmp;
 404         int result;
 405         char version_str[100];
 406
 407         vsllink_usb_in_buffer = malloc(VSLLINK_BufferSize);
 408         vsllink_usb_out_buffer = malloc(VSLLINK_BufferSize);
 409         if ((vsllink_usb_in_buffer == NULL) || (vsllink_usb_out_buffer == NULL))
 410         {
 411                 LOG_ERROR("Not enough memory");
 412                 exit(-1);
 413         }
 414
 415         vsllink_jtag_handle = vsllink_usb_open();
 416
 417         if (vsllink_jtag_handle == 0)
 418         {
 419                 LOG_ERROR("Can't find USB JTAG Interface! Please check connection and permissions.");
 420                 return ERROR_JTAG_INIT_FAILED;
 421         }
 422         LOG_DEBUG("vsllink found on %04X:%04X", vsllink_usb_vid, vsllink_usb_pid);
 423
 424         to_tmp = VSLLINK_USB_TIMEOUT;
 425         VSLLINK_USB_TIMEOUT = 100;
 426         check_cnt = 0;
 427         while (check_cnt < 5)
 428         {
 429                 vsllink_simple_command(0x00);
 430                 result = vsllink_usb_read(vsllink_jtag_handle);
 431
 432                 if (result > 2)
 433                 {
 434                         vsllink_usb_in_buffer[result] = 0;
 435                         VSLLINK_BufferSize = vsllink_usb_in_buffer[0] + (vsllink_usb_in_buffer[1] << 8);
 436                         strncpy(version_str, (char *)vsllink_usb_in_buffer + 2, sizeof(version_str));
 437                         LOG_INFO("%s", version_str);
 438
 439                         // free the pre-alloc memroy
 440                         free(vsllink_usb_in_buffer);
 441                         free(vsllink_usb_out_buffer);
 442                         vsllink_usb_in_buffer = NULL;
 443                         vsllink_usb_out_buffer = NULL;
 444
 445                         // alloc new memory
 446                         vsllink_usb_in_buffer = malloc(VSLLINK_BufferSize);
 447                         vsllink_usb_out_buffer = malloc(VSLLINK_BufferSize);
 448                         if ((vsllink_usb_in_buffer == NULL) || (vsllink_usb_out_buffer == NULL))
 449                         {
 450                                 LOG_ERROR("Not enough memory");
 451                                 exit(-1);
 452                         }
 453                         else
 454                         {
 455                                 LOG_INFO("buffer size for USB is %d bytes", VSLLINK_BufferSize);
 456                         }
 457                         // alloc memory for dma mode
 458                         if (vsllink_mode == VSLLINK_MODE_DMA)
 459                         {
 460                                 tap_buffer_size = (VSLLINK_BufferSize - 3) / 2;
 461                                 tms_buffer = (uint8_t*)malloc(tap_buffer_size);
 462                                 tdi_buffer = (uint8_t*)malloc(tap_buffer_size);
 463                                 tdo_buffer = (uint8_t*)malloc(tap_buffer_size);
 464                                 if ((tms_buffer == NULL) || (tdi_buffer == NULL) || (tdo_buffer == NULL))
 465                                 {
 466                                         LOG_ERROR("Not enough memory");
 467                                         exit(-1);
 468                                 }
 469                         }
 470                         break;
 471                 }
 472                 vsllink_simple_command(VSLLINK_CMD_DISCONN);
 473                 check_cnt++;
 474         }
 475         if (check_cnt == 3)
 476         {
 477                 // It's dangerout to proced
 478                 LOG_ERROR("VSLLink initial failed");
 479                 exit(-1);
 480         }
 481         VSLLINK_USB_TIMEOUT = to_tmp;
 482
 483         // connect to vsllink
 484         vsllink_connect();
 485         // initialize function pointers
 486         if (vsllink_mode == VSLLINK_MODE_NORMAL)
 487         {
 488                 // normal mode
 489                 vsllink_state_move = vsllink_state_move_normal;
 490                 vsllink_path_move = vsllink_path_move_normal;
 491                 vsllink_stableclocks = vsllink_stableclocks_normal;
 492                 vsllink_scan = vsllink_scan_normal;
 493
 494                 vsllink_tap_init = vsllink_tap_init_normal;
 495                 vsllink_tap_execute = vsllink_tap_execute_normal;
 496                 vsllink_tap_ensure_space = vsllink_tap_ensure_space_normal;
 497
 498                 LOG_INFO("vsllink run in NORMAL mode");
 499         }
 500         else
 501         {
 502                 // dma mode
 503                 vsllink_state_move = vsllink_state_move_dma;
 504                 vsllink_path_move = vsllink_path_move_dma;
 505                 vsllink_stableclocks = vsllink_stableclocks_dma;
 506                 vsllink_scan = vsllink_scan_dma;
 507
 508                 vsllink_tap_init = vsllink_tap_init_dma;
 509                 vsllink_tap_execute = vsllink_tap_execute_dma;
 510                 vsllink_tap_ensure_space = vsllink_tap_ensure_space_dma;
 511
 512                 LOG_INFO("vsllink run in DMA mode");
 513         }
 514
 515         // Set SRST and TRST to output, Set USR1 and USR2 to input
 516         vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_PORTDIR;
 517         vsllink_usb_out_buffer[1] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST | JTAG_PINMSK_USR1 | JTAG_PINMSK_USR2;
 518         vsllink_usb_out_buffer[2] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST;
 519         if (vsllink_usb_write(vsllink_jtag_handle, 3) != 3)
 520         {
 521                 LOG_ERROR("VSLLink USB send data error");
 522                 exit(-1);
 523         }
 524
 525         vsllink_reset(0, 0);
 526
 527         LOG_INFO("VSLLink JTAG Interface ready");
 528
 529         vsllink_tap_init();
 530
 531         return ERROR_OK;
 532 }
 533
 534 static int vsllink_quit(void)
 535 {
 536         if ((vsllink_usb_in_buffer != NULL) && (vsllink_usb_out_buffer != NULL))
 537         {
 538                 // Set all pins to input
 539                 vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_PORTDIR;
 540                 vsllink_usb_out_buffer[1] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST | JTAG_PINMSK_USR1 | JTAG_PINMSK_USR2;
 541                 vsllink_usb_out_buffer[2] = 0;
 542                 if (vsllink_usb_write(vsllink_jtag_handle, 3) != 3)
 543                 {
 544                         LOG_ERROR("VSLLink USB send data error");
 545                         exit(-1);
 546                 }
 547
 548                 // disconnect
 549                 vsllink_disconnect();
 550                 vsllink_usb_close(vsllink_jtag_handle);
 551                 vsllink_jtag_handle = NULL;
 552         }
 553
 554         if (vsllink_usb_in_buffer != NULL)
 555         {
 556                 free(vsllink_usb_in_buffer);
 557                 vsllink_usb_in_buffer = NULL;
 558         }
 559         if (vsllink_usb_out_buffer != NULL)
 560         {
 561                 free(vsllink_usb_out_buffer);
 562                 vsllink_usb_out_buffer = NULL;
 563         }
 564
 565         return ERROR_OK;
 566 }
 567
 568 /***************************************************************************/
 569 /* Queue command implementations */
 570 static int vsllink_disconnect(void)
 571 {
 572         vsllink_simple_command(VSLLINK_CMD_DISCONN);
 573         return ERROR_OK;
 574 }
 575
 576 static int vsllink_connect(void)
 577 {
 578         char vsllink_str[100];
 579
 580         vsllink_usb_out_buffer[0] = VSLLINK_CMD_CONN;
 581         vsllink_usb_out_buffer[1] = vsllink_mode;
 582         vsllink_usb_message(vsllink_jtag_handle, 2, 0);
 583         if (vsllink_usb_read(vsllink_jtag_handle) > 2)
 584         {
 585                 strncpy(vsllink_str, (char *)vsllink_usb_in_buffer + 2, sizeof(vsllink_str));
 586                 LOG_INFO("%s", vsllink_str);
 587         }
 588
 589         return ERROR_OK;
 590 }
 591
 592 // when vsllink_tms_data_len > 0, vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] is the byte that need to be appended.
 593 // length of VSLLINK_CMDJTAGSEQ_TMSBYTE has been set, no need to set it here.
 594 static void vsllink_append_tms(void)
 595 {
 596         uint8_t tms_scan = VSLLINK_TAP_MOVE(tap_get_state(), tap_get_end_state());
 597         uint16_t tms2;
 598         insert_insignificant_operation_t *insert = \
 599                 &VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[tap_move_ndx(tap_get_state())][tap_move_ndx(tap_get_end_state())];
 600
 601         if (((tap_get_state() != TAP_RESET) && (tap_get_state() != TAP_IDLE) && (tap_get_state() != TAP_DRPAUSE) && (tap_get_state() != TAP_IRPAUSE)) || \
 602                         (vsllink_tms_data_len <= 0) || (vsllink_tms_data_len >= 8) || \
 603                         (vsllink_tms_cmd_pos == NULL))
 604         {
 605                 LOG_ERROR("There MUST be some bugs in the driver");
 606                 exit(-1);
 607         }
 608
 609         tms2 = (tms_scan & VSLLINK_BIT_MSK[insert->insert_position]) << \
 610                                 vsllink_tms_data_len;
 611         if (insert->insert_value == 1)
 612         {
 613                 tms2 |= VSLLINK_BIT_MSK[8 - vsllink_tms_data_len] << \
 614                                 (vsllink_tms_data_len + insert->insert_position);
 615         }
 616         tms2 |= (tms_scan >> insert->insert_position) << \
 617                                 (8 + insert->insert_position);
 618
 619         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (tms2 >> 0) & 0xff;
 620         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms2 >> 8) & 0xff;
 621
 622         vsllink_tms_data_len = 0;
 623         vsllink_tms_cmd_pos = NULL;
 624 }
 625
 626 static void vsllink_end_state(tap_state_t state)
 627 {
 628         if (tap_is_state_stable(state))
 629         {
 630                 tap_set_end_state(state);
 631         }
 632         else
 633         {
 634                 LOG_ERROR("BUG: %i is not a valid end state", state);
 635                 exit(-1);
 636         }
 637 }
 638
 639 /* Goes to the end state. */
 640 static void vsllink_state_move_normal(void)
 641 {
 642         if (vsllink_tms_data_len > 0)
 643         {
 644                 vsllink_append_tms();
 645         }
 646         else
 647         {
 648                 vsllink_tap_ensure_space(0, 2);
 649
 650                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE;
 651                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(tap_get_state(), tap_get_end_state());
 652         }
 653
 654         tap_set_state(tap_get_end_state());
 655 }
 656 static void vsllink_state_move_dma(void)
 657 {
 658         int i, insert_length = (tap_length % 8) ? (8 - (tap_length % 8)) : 0;
 659         insert_insignificant_operation_t *insert = \
 660                 &VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[tap_move_ndx(tap_get_state())][tap_move_ndx(tap_get_end_state())];
 661         uint8_t tms_scan = VSLLINK_TAP_MOVE(tap_get_state(), tap_get_end_state());
 662
 663         if (tap_get_state() == TAP_RESET)
 664         {
 665                 vsllink_tap_ensure_space(0, 8);
 666
 667                 for (i = 0; i < 8; i++)
 668                 {
 669                         vsllink_tap_append_step(1, 0);
 670                 }
 671         }
 672
 673         if (insert_length > 0)
 674         {
 675                 vsllink_tap_ensure_space(0, 16);
 676
 677                 for (i = 0; i < insert->insert_position; i++)
 678                 {
 679                         vsllink_tap_append_step((tms_scan >> i) & 1, 0);
 680                 }
 681                 for (i = 0; i < insert_length; i++)
 682                 {
 683                         vsllink_tap_append_step(insert->insert_value, 0);
 684                 }
 685                 for (i = insert->insert_position; i < 8; i++)
 686                 {
 687                         vsllink_tap_append_step((tms_scan >> i) & 1, 0);
 688                 }
 689         }
 690         else
 691         {
 692                 vsllink_tap_ensure_space(0, 8);
 693
 694                 for (i = 0; i < 8; i++)
 695                 {
 696                         vsllink_tap_append_step((tms_scan >> i) & 1, 0);
 697                 }
 698         }
 699
 700         tap_set_state(tap_get_end_state());
 701 }
 702
 703 // write tms from current vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx]
 704 static void vsllink_add_path(int start, int num, tap_state_t *path)
 705 {
 706         int i;
 707
 708         for (i = start; i < (start + num); i++)
 709         {
 710                 if ((i & 7) == 0)
 711                 {
 712                         if (i > 0)
 713                         {
 714                                 vsllink_usb_out_buffer_idx++;
 715                         }
 716                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = 0;
 717                 }
 718
 719                 if (path[i - start] == tap_state_transition(tap_get_state(), true))
 720                 {
 721                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] |= 1 << (i & 7);
 722                 }
 723                 else if (path[i - start] == tap_state_transition(tap_get_state(), false))
 724                 {
 725                         // nothing to do
 726                 }
 727                 else
 728                 {
 729                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(tap_get_state()), tap_state_name(path[i]));
 730                         exit(-1);
 731                 }
 732                 tap_set_state(path[i - start]);
 733         }
 734         if ((i > 0) && ((i & 7) == 0))
 735         {
 736                 vsllink_usb_out_buffer_idx++;
 737                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = 0;
 738         }
 739
 740         tap_set_end_state(tap_get_state());
 741 }
 742
 743 static void vsllink_path_move_normal(int num_states, tap_state_t *path)
 744 {
 745         int i, tms_len, tms_cmd_pos, path_idx = 0;
 746
 747         if (vsllink_tms_data_len > 0)
 748         {
 749                 // there are vsllink_tms_data_len more tms bits to be shifted
 750                 // so there are vsllink_tms_data_len + num_states tms bits in all
 751                 tms_len = vsllink_tms_data_len + num_states;
 752                 if (tms_len <= 16)
 753                 {
 754                         // merge into last tms shift
 755                         if (tms_len < 8)
 756                         {
 757                                 // just append tms data to the last tms byte
 758                                 vsllink_add_path(vsllink_tms_data_len, num_states, path);
 759                         }
 760                         else if (tms_len == 8)
 761                         {
 762                                 // end last tms shift command
 763                                 (*vsllink_tms_cmd_pos)--;
 764                                 vsllink_add_path(vsllink_tms_data_len, num_states, path);
 765                         }
 766                         else if (tms_len < 16)
 767                         {
 768                                 if ((*vsllink_tms_cmd_pos & VSLLINK_CMDJTAGSEQ_LENMSK) < VSLLINK_CMDJTAGSEQ_LENMSK)
 769                                 {
 770                                         // every tms shift command can contain VSLLINK_CMDJTAGSEQ_LENMSK + 1 bytes in most
 771                                         // there is enought tms length in the current tms shift command
 772                                         (*vsllink_tms_cmd_pos)++;
 773                                         vsllink_add_path(vsllink_tms_data_len, num_states, path);
 774                                 }
 775                                 else
 776                                 {
 777                                         // every tms shift command can contain VSLLINK_CMDJTAGSEQ_LENMSK + 1 bytes in most
 778                                         // not enough tms length in the current tms shift command
 779                                         // so a new command should be added
 780                                         // first decrease byte length of last tms shift command
 781                                         (*vsllink_tms_cmd_pos)--;
 782                                         // append tms data to the last tms byte
 783                                         vsllink_add_path(vsllink_tms_data_len, 8 - vsllink_tms_data_len, path);
 784                                         path += 8 - vsllink_tms_data_len;
 785                                         // add new command(3 bytes)
 786                                         vsllink_tap_ensure_space(0, 3);
 787                                         vsllink_tms_cmd_pos = &vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
 788                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
 789                                         vsllink_add_path(0, num_states - (8 - vsllink_tms_data_len), path);
 790                                 }
 791                         }
 792                         else if (tms_len == 16)
 793                         {
 794                                 // end last tms shift command
 795                                 vsllink_add_path(vsllink_tms_data_len, num_states, path);
 796                         }
 797
 798                         vsllink_tms_data_len = (vsllink_tms_data_len + num_states) & 7;
 799                         if (vsllink_tms_data_len == 0)
 800                         {
 801                                 vsllink_tms_cmd_pos = NULL;
 802                         }
 803                         num_states = 0;
 804                 }
 805                 else
 806                 {
 807                         vsllink_add_path(vsllink_tms_data_len, 16 - vsllink_tms_data_len, path);
 808
 809                         path += 16 - vsllink_tms_data_len;
 810                         num_states -= 16 - vsllink_tms_data_len;
 811                         vsllink_tms_data_len = 0;
 812                         vsllink_tms_cmd_pos = NULL;
 813                 }
 814         }
 815
 816         if (num_states > 0)
 817         {
 818                 // Normal operation, don't need to append tms data
 819                 vsllink_tms_data_len = num_states & 7;
 820
 821                 while (num_states > 0)
 822                 {
 823                         if (num_states > ((VSLLINK_CMDJTAGSEQ_LENMSK + 1) * 8))
 824                         {
 825                                 i = (VSLLINK_CMDJTAGSEQ_LENMSK + 1) * 8;
 826                         }
 827                         else
 828                         {
 829                                 i = num_states;
 830                         }
 831                         tms_len = (i + 7) >> 3;
 832                         vsllink_tap_ensure_space(0, tms_len + 2);
 833                         tms_cmd_pos = vsllink_usb_out_buffer_idx;
 834                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | (tms_len - 1);
 835
 836                         vsllink_add_path(0, i, path + path_idx);
 837
 838                         path_idx += i;
 839                         num_states -= i;
 840                 }
 841
 842                 if (vsllink_tms_data_len > 0)
 843                 {
 844                         if (tms_len < (VSLLINK_CMDJTAGSEQ_LENMSK + 1))
 845                         {
 846                                 vsllink_tms_cmd_pos = &vsllink_usb_out_buffer[tms_cmd_pos];
 847                                 (*vsllink_tms_cmd_pos)++;
 848                         }
 849                         else
 850                         {
 851                                 vsllink_usb_out_buffer[tms_cmd_pos]--;
 852
 853                                 tms_len = vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
 854                                 vsllink_tap_ensure_space(0, 3);
 855                                 vsllink_tms_cmd_pos = &vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
 856                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
 857                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = tms_len;
 858                         }
 859                 }
 860         }
 861 }
 862 static void vsllink_path_move_dma(int num_states, tap_state_t *path)
 863 {
 864         int i, j = 0;
 865
 866         if (tap_length & 7)
 867         {
 868                 if ((8 - (tap_length & 7)) < num_states)
 869                 {
 870                         j = 8 - (tap_length & 7);
 871                 }
 872                 else
 873                 {
 874                         j = num_states;
 875                 }
 876                 for (i = 0; i < j; i++)
 877                 {
 878                         if (path[i] == tap_state_transition(tap_get_state(), false))
 879                         {
 880                                 vsllink_tap_append_step(0, 0);
 881                         }
 882                         else if (path[i] == tap_state_transition(tap_get_state(), true))
 883                         {
 884                                 vsllink_tap_append_step(1, 0);
 885                         }
 886                         else
 887                         {
 888                                 LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(tap_get_state()), tap_state_name(path[i]));
 889                                 exit(-1);
 890                         }
 891                         tap_set_state(path[i]);
 892                 }
 893                 num_states -= j;
 894         }
 895
 896         if (num_states > 0)
 897         {
 898                 vsllink_tap_ensure_space(0, num_states);
 899
 900                 for (i = 0; i < num_states; i++)
 901                 {
 902                         if (path[j + i] == tap_state_transition(tap_get_state(), false))
 903                         {
 904                                 vsllink_tap_append_step(0, 0);
 905                         }
 906                         else if (path[j + i] == tap_state_transition(tap_get_state(), true))
 907                         {
 908                                 vsllink_tap_append_step(1, 0);
 909                         }
 910                         else
 911                         {
 912                                 LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(tap_get_state()), tap_state_name(path[i]));
 913                                 exit(-1);
 914                         }
 915                         tap_set_state(path[j + i]);
 916                 }
 917         }
 918
 919         tap_set_end_state(tap_get_state());
 920 }
 921
 922 static void vsllink_stableclocks_normal(int num_cycles, int tms)
 923 {
 924         int tms_len;
 925         uint16_t tms_append_byte;
 926
 927         if (vsllink_tms_data_len > 0)
 928         {
 929                 // there are vsllink_tms_data_len more tms bits to be shifted
 930                 // so there are vsllink_tms_data_len + num_cycles tms bits in all
 931                 tms_len = vsllink_tms_data_len + num_cycles;
 932                 if (tms > 0)
 933                 {
 934                         // append '1' for tms
 935                         tms_append_byte = (uint16_t)((((1 << num_cycles) - 1) << vsllink_tms_data_len) & 0xFFFF);
 936                 }
 937                 else
 938                 {
 939                         // append '0' for tms
 940                         tms_append_byte = 0;
 941                 }
 942                 if (tms_len <= 16)
 943                 {
 944                         // merge into last tms shift
 945                         if (tms_len < 8)
 946                         {
 947                                 // just add to vsllink_tms_data_len
 948                                 // same result if tun through
 949                                 //vsllink_tms_data_len += num_cycles;
 950                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] |= (uint8_t)(tms_append_byte & 0xFF);
 951                         }
 952                         else if (tms_len == 8)
 953                         {
 954                                 // end last tms shift command
 955                                 // just reduce it, and append last tms byte
 956                                 (*vsllink_tms_cmd_pos)--;
 957                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
 958                         }
 959                         else if (tms_len < 16)
 960                         {
 961                                 if ((*vsllink_tms_cmd_pos & VSLLINK_CMDJTAGSEQ_LENMSK) < VSLLINK_CMDJTAGSEQ_LENMSK)
 962                                 {
 963                                         // every tms shift command can contain VSLLINK_CMDJTAGSEQ_LENMSK + 1 bytes in most
 964                                         // there is enought tms length in the current tms shift command
 965                                         // increase the tms byte length by 1 and set the last byte to 0
 966                                         (*vsllink_tms_cmd_pos)++;
 967                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
 968                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = (uint8_t)(tms_append_byte >> 8);
 969                                 }
 970                                 else
 971                                 {
 972                                         // every tms shift command can contain VSLLINK_CMDJTAGSEQ_LENMSK + 1 bytes in most
 973                                         // not enough tms length in the current tms shift command
 974                                         // so a new command should be added
 975                                         // first decrease byte length of last tms shift command
 976                                         (*vsllink_tms_cmd_pos)--;
 977                                         // append last tms byte and move the command pointer to the next empty position
 978                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
 979                                         // add new command(3 bytes)
 980                                         vsllink_tap_ensure_space(0, 3);
 981                                         vsllink_tms_cmd_pos = &vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
 982                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
 983                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = (uint8_t)(tms_append_byte >> 8);
 984                                 }
 985                         }
 986                         else if (tms_len == 16)
 987                         {
 988                                 // end last tms shift command
 989                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
 990                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (uint8_t)(tms_append_byte >> 8);
 991                         }
 992
 993                         vsllink_tms_data_len = tms_len & 7;
 994                         if (vsllink_tms_data_len == 0)
 995                         {
 996                                 vsllink_tms_cmd_pos = NULL;
 997                         }
 998                         num_cycles = 0;
 999                 }
1000                 else
1001                 {
1002                         // more shifts will be needed
1003                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
1004                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (uint8_t)(tms_append_byte >> 8);
1005
1006                         num_cycles -= 16 - vsllink_tms_data_len;
1007                         vsllink_tms_data_len = 0;
1008                         vsllink_tms_cmd_pos = NULL;
1009                 }
1010         }
1011         // from here vsllink_tms_data_len == 0 or num_cycles == 0
1012
1013         if (vsllink_tms_data_len > 0)
1014         {
1015                 // num_cycles == 0
1016                 // no need to shift
1017                 if (num_cycles > 0)
1018                 {
1019                         LOG_ERROR("There MUST be some bugs in the driver");
1020                         exit(-1);
1021                 }
1022         }
1023         else
1024         {
1025                 // get number of bytes left to be sent
1026                 tms_len = num_cycles >> 3;
1027                 if (tms_len > 0)
1028                 {
1029                         vsllink_tap_ensure_space(1, 5);
1030                         // if tms_len > 0, vsllink_tms_data_len == 0
1031                         // so just add new command
1032                         // LSB of the command byte is the tms value when do the shifting
1033                         if (tms > 0)
1034                         {
1035                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSCLOCK | 1;
1036                         }
1037                         else
1038                         {
1039                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSCLOCK;
1040                         }
1041                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 0) & 0xff;
1042                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 8) & 0xff;
1043                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 16) & 0xff;
1044                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 24) & 0xff;
1045
1046                         vsllink_usb_in_want_length += 1;
1047                         pending_scan_results_buffer[pending_scan_results_length].buffer = NULL;
1048                         pending_scan_results_length++;
1049
1050                         if (tms_len > 0xFFFF)
1051                         {
1052                                 vsllink_tap_execute();
1053                         }
1054                 }
1055
1056                 // post-process
1057                 vsllink_tms_data_len = num_cycles & 7;
1058                 if (vsllink_tms_data_len > 0)
1059                 {
1060                         vsllink_tap_ensure_space(0, 3);
1061                         vsllink_tms_cmd_pos = &vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
1062                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
1063                         if (tms > 0)
1064                         {
1065                                 // append '1' for tms
1066                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = (1 << vsllink_tms_data_len) - 1;
1067                         }
1068                         else
1069                         {
1070                                 // append '0' for tms
1071                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = 0x00;
1072                         }
1073                 }
1074         }
1075 }
1076 static void vsllink_stableclocks_dma(int num_cycles, int tms)
1077 {
1078         int i, cur_cycles;
1079
1080         if (tap_length & 7)
1081         {
1082                 if ((8 - (tap_length & 7)) < num_cycles)
1083                 {
1084                         cur_cycles = 8 - (tap_length & 7);
1085                 }
1086                 else
1087                 {
1088                         cur_cycles = num_cycles;
1089                 }
1090                 for (i = 0; i < cur_cycles; i++)
1091                 {
1092                         vsllink_tap_append_step(tms, 0);
1093                 }
1094                 num_cycles -= cur_cycles;
1095         }
1096
1097         while (num_cycles > 0)
1098         {
1099                 if (num_cycles > 8 * tap_buffer_size)
1100                 {
1101                         cur_cycles = 8 * tap_buffer_size;
1102                 }
1103                 else
1104                 {
1105                         cur_cycles = num_cycles;
1106                 }
1107
1108                 vsllink_tap_ensure_space(0, cur_cycles);
1109
1110                 for (i = 0; i < cur_cycles; i++)
1111                 {
1112                         vsllink_tap_append_step(tms, 0);
1113                 }
1114
1115                 num_cycles -= cur_cycles;
1116         }
1117 }
1118
1119 static void vsllink_runtest(int num_cycles)
1120 {
1121         tap_state_t saved_end_state = tap_get_end_state();
1122
1123         if (tap_get_state() != TAP_IDLE)
1124         {
1125                 // enter into IDLE state
1126                 vsllink_end_state(TAP_IDLE);
1127                 vsllink_state_move();
1128         }
1129
1130         vsllink_stableclocks(num_cycles, 0);
1131
1132         // post-process
1133         // set end_state
1134         vsllink_end_state(saved_end_state);
1135         tap_set_state(TAP_IDLE);
1136         if (tap_get_end_state() != TAP_IDLE)
1137         {
1138                 vsllink_state_move();
1139         }
1140 }
1141
1142 static void vsllink_scan_normal(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size, scan_command_t *command)
1143 {
1144         tap_state_t saved_end_state;
1145         uint8_t bits_left, tms_tmp, tdi_len;
1146         int i;
1147
1148         if (0 == scan_size)
1149         {
1150                 return;
1151         }
1152
1153         tdi_len = ((scan_size + 7) >> 3);
1154         if ((tdi_len + 7) > VSLLINK_BufferSize)
1155         {
1156                 LOG_ERROR("Your implementation of VSLLink has not enough buffer");
1157                 exit(-1);
1158         }
1159
1160         saved_end_state = tap_get_end_state();
1161
1162         /* Move to appropriate scan state */
1163         vsllink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
1164
1165         if (vsllink_tms_data_len > 0)
1166         {
1167                 if (tap_get_state() == tap_get_end_state())
1168                 {
1169                         // already in IRSHIFT or DRSHIFT state
1170                         // merge tms data in the last tms shift command into next scan command
1171                         if (*vsllink_tms_cmd_pos < 1)
1172                         {
1173                                 LOG_ERROR("There MUST be some bugs in the driver");
1174                                 exit(-1);
1175                         }
1176                         else if (*vsllink_tms_cmd_pos < 2)
1177                         {
1178                                 tms_tmp = vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
1179                                 vsllink_usb_out_buffer_idx--;
1180                         }
1181                         else
1182                         {
1183                                 tms_tmp = vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
1184                                 *vsllink_tms_cmd_pos -= 2;
1185                         }
1186
1187                         vsllink_tap_ensure_space(1, tdi_len + 7);
1188                         // VSLLINK_CMDJTAGSEQ_SCAN ored by 1 means that tms_before is valid
1189                         // which is merged from the last tms shift command
1190                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_SCAN | 1;
1191                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1) >> 0) & 0xff;
1192                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1) >> 8) & 0xff;
1193                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = tms_tmp;
1194                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = buffer[0] << (8 - vsllink_tms_data_len);
1195
1196                         for (i = 0; i < tdi_len; i++)
1197                         {
1198                                 buffer[i] >>= 8 - vsllink_tms_data_len;
1199                                 if (i != tdi_len)
1200                                 {
1201                                         buffer[i] += buffer[i + 1] << vsllink_tms_data_len;
1202                                 }
1203                         }
1204
1205                         vsllink_tap_append_scan_normal(scan_size - vsllink_tms_data_len, buffer, command, vsllink_tms_data_len);
1206                         scan_size -= 8 - vsllink_tms_data_len;
1207                         vsllink_tms_data_len = 0;
1208                 }
1209                 else
1210                 {
1211                         vsllink_state_move();
1212                         vsllink_tap_ensure_space(1, tdi_len + 5);
1213
1214                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_SCAN;
1215                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tdi_len >> 0) & 0xff;
1216                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tdi_len >> 8) & 0xff;
1217
1218                         vsllink_tap_append_scan_normal(scan_size, buffer, command, 0);
1219                 }
1220         }
1221         else
1222         {
1223                 vsllink_tap_ensure_space(1, tdi_len + 7);
1224
1225                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_SCAN | 1;
1226                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1) >> 0) & 0xff;
1227                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1)>> 8) & 0xff;
1228                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(tap_get_state(), tap_get_end_state());
1229                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
1230
1231                 vsllink_tap_append_scan_normal(scan_size, buffer, command, 8);
1232         }
1233         vsllink_end_state(saved_end_state);
1234
1235         bits_left = scan_size & 0x07;
1236         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
1237
1238         if (bits_left > 0)
1239         {
1240                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 1 << (bits_left - 1);
1241         }
1242         else
1243         {
1244                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 1 << 7;
1245         }
1246
1247         if (tap_get_state() != tap_get_end_state())
1248         {
1249                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(tap_get_state(), tap_get_end_state());
1250         }
1251         else
1252         {
1253                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
1254         }
1255
1256         tap_set_state(tap_get_end_state());
1257 }
1258 static void vsllink_scan_dma(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size, scan_command_t *command)
1259 {
1260         tap_state_t saved_end_state;
1261
1262         saved_end_state = tap_get_end_state();
1263
1264         /* Move to appropriate scan state */
1265         vsllink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
1266
1267         vsllink_state_move();
1268         vsllink_end_state(saved_end_state);
1269
1270         /* Scan */
1271         vsllink_tap_append_scan_dma(scan_size, buffer, command);
1272
1273         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
1274         while (tap_length % 8 != 0)
1275         {
1276                 // more 0s in Pause
1277                 vsllink_tap_append_step(0, 0);
1278         }
1279
1280         if (tap_get_state() != tap_get_end_state())
1281         {
1282                 vsllink_state_move();
1283         }
1284 }
1285
1286 static void vsllink_reset(int trst, int srst)
1287 {
1288         int result;
1289
1290         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
1291
1292         /* Signals are active low */
1293         vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_PORT;
1294         vsllink_usb_out_buffer[1] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST;
1295         vsllink_usb_out_buffer[2] = 0;
1296         if (srst == 0)
1297         {
1298                 vsllink_usb_out_buffer[2] |= JTAG_PINMSK_SRST;
1299         }
1300         if (trst == 0)
1301         {
1302                 vsllink_usb_out_buffer[2] |= JTAG_PINMSK_TRST;
1303         }
1304
1305         result = vsllink_usb_write(vsllink_jtag_handle, 3);
1306         if (result != 3)
1307         {
1308                 LOG_ERROR("VSLLink command VSLLINK_CMD_SET_PORT failed (%d)", result);
1309         }
1310 }
1311
1312 static void vsllink_simple_command(uint8_t command)
1313 {
1314         int result;
1315
1316         DEBUG_JTAG_IO("0x%02x", command);
1317
1318         vsllink_usb_out_buffer[0] = command;
1319         result = vsllink_usb_write(vsllink_jtag_handle, 1);
1320
1321         if (result != 1)
1322         {
1323                 LOG_ERROR("VSLLink command 0x%02x failed (%d)", command, result);
1324         }
1325 }
1326
1327 COMMAND_HANDLER(vsllink_handle_mode_command)
1328 {
1329         if (argc != 1) {
1330                 LOG_ERROR("parameter error, should be one parameter for VID");
1331                 return ERROR_FAIL;
1332         }
1333
1334         if (!strcmp(args[0], "normal"))
1335         {
1336                 vsllink_mode = VSLLINK_MODE_NORMAL;
1337         }
1338         else if (!strcmp(args[0], "dma"))
1339         {
1340                 vsllink_mode = VSLLINK_MODE_DMA;
1341         }
1342         else
1343         {
1344                 LOG_ERROR("invalid vsllink_mode: %s", args[0]);
1345                 return ERROR_FAIL;
1346         }
1347
1348         return ERROR_OK;
1349 }
1350
1351 COMMAND_HANDLER(vsllink_handle_usb_vid_command)
1352 {
1353         if (argc != 1)
1354         {
1355                 LOG_ERROR("parameter error, should be one parameter for VID");
1356                 return ERROR_OK;
1357         }
1358
1359         COMMAND_PARSE_NUMBER(u16, args[0], vsllink_usb_vid);
1360         return ERROR_OK;
1361 }
1362
1363 COMMAND_HANDLER(vsllink_handle_usb_pid_command)
1364 {
1365         if (argc != 1)
1366         {
1367                 LOG_ERROR("parameter error, should be one parameter for PID");
1368                 return ERROR_OK;
1369         }
1370         COMMAND_PARSE_NUMBER(u16, args[0], vsllink_usb_pid);
1371         return ERROR_OK;
1372 }
1373
1374 COMMAND_HANDLER(vsllink_handle_usb_bulkin_command)
1375 {
1376         if (argc != 1)
1377         {
1378                 LOG_ERROR("parameter error, should be one parameter for BULKIN endpoint");
1379                 return ERROR_OK;
1380         }
1381
1382         COMMAND_PARSE_NUMBER(u8, args[0], vsllink_usb_bulkin);
1383
1384         vsllink_usb_bulkin |= 0x80;
1385
1386         return ERROR_OK;
1387 }
1388
1389 COMMAND_HANDLER(vsllink_handle_usb_bulkout_command)
1390 {
1391         if (argc != 1)
1392         {
1393                 LOG_ERROR("parameter error, should be one parameter for BULKOUT endpoint");
1394                 return ERROR_OK;
1395         }
1396
1397         COMMAND_PARSE_NUMBER(u8, args[0], vsllink_usb_bulkout);
1398
1399         vsllink_usb_bulkout &= ~0x80;
1400
1401         return ERROR_OK;
1402 }
1403
1404 COMMAND_HANDLER(vsllink_handle_usb_interface_command)
1405 {
1406         if (argc != 1)
1407         {
1408                 LOG_ERROR("parameter error, should be one parameter for interface number");
1409                 return ERROR_OK;
1410         }
1411
1412         COMMAND_PARSE_NUMBER(u8, args[0], vsllink_usb_interface);
1413         return ERROR_OK;
1414 }
1415
1416 /***************************************************************************/
1417 /* VSLLink tap functions */
1418
1419 static void vsllink_tap_init_normal(void)
1420 {
1421         vsllink_usb_out_buffer_idx = 0;
1422         vsllink_usb_in_want_length = 0;
1423         pending_scan_results_length = 0;
1424 }
1425 static void vsllink_tap_init_dma(void)
1426 {
1427         tap_length = 0;
1428         pending_scan_results_length = 0;
1429 }
1430
1431 static void vsllink_tap_ensure_space_normal(int scans, int length)
1432 {
1433         int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
1434         int available_bytes = VSLLINK_BufferSize - vsllink_usb_out_buffer_idx;
1435
1436         if (scans > available_scans || length > available_bytes)
1437         {
1438                 vsllink_tap_execute();
1439         }
1440 }
1441 static void vsllink_tap_ensure_space_dma(int scans, int length)
1442 {
1443         int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
1444         int available_bytes = tap_buffer_size * 8 - tap_length;
1445
1446         if (scans > available_scans || length > available_bytes)
1447         {
1448                 vsllink_tap_execute();
1449         }
1450 }
1451
1452 static void vsllink_tap_append_step(int tms, int tdi)
1453 {
1454         last_tms = tms;
1455         int index = tap_length / 8;
1456
1457         if (index < tap_buffer_size)
1458         {
1459                 int bit_index = tap_length % 8;
1460                 uint8_t bit = 1 << bit_index;
1461
1462                 if (tms)
1463                 {
1464                         tms_buffer[index] |= bit;
1465                 }
1466                 else
1467                 {
1468                         tms_buffer[index] &= ~bit;
1469                 }
1470
1471                 if (tdi)
1472                 {
1473                         tdi_buffer[index] |= bit;
1474                 }
1475                 else
1476                 {
1477                         tdi_buffer[index] &= ~bit;
1478                 }
1479
1480                 tap_length++;
1481         }
1482         else
1483         {
1484                 LOG_ERROR("buffer overflow, tap_length=%d", tap_length);
1485         }
1486 }
1487
1488 static void vsllink_tap_append_scan_normal(int length, uint8_t *buffer, scan_command_t *command, int offset)
1489 {
1490         struct pending_scan_result *pending_scan_result = &pending_scan_results_buffer[pending_scan_results_length];
1491         int i;
1492
1493         if (offset > 0)
1494         {
1495                 vsllink_usb_in_want_length += ((length + 7) >> 3) + 1;
1496         }
1497         else
1498         {
1499                 vsllink_usb_in_want_length += (length + 7) >> 3;
1500         }
1501         pending_scan_result->length = length;
1502         pending_scan_result->offset = offset;
1503         pending_scan_result->command = command;
1504         pending_scan_result->buffer = buffer;
1505
1506         for (i = 0; i < ((length + 7) >> 3); i++)
1507         {
1508                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = buffer[i];
1509         }
1510
1511         pending_scan_results_length++;
1512 }
1513 static void vsllink_tap_append_scan_dma(int length, uint8_t *buffer, scan_command_t *command)
1514 {
1515         struct pending_scan_result *pending_scan_result;
1516         int len_tmp, len_all, i;
1517
1518         len_all = 0;
1519         while (len_all < length)
1520         {
1521                 if ((length - len_all) > tap_buffer_size * 8)
1522                 {
1523                         len_tmp = tap_buffer_size * 8;
1524                 }
1525                 else
1526                 {
1527                         len_tmp = length - len_all;
1528                 }
1529
1530                 vsllink_tap_ensure_space(1, (len_tmp + 7) & ~7);
1531
1532                 pending_scan_result = &pending_scan_results_buffer[pending_scan_results_length];
1533                 pending_scan_result->offset = tap_length;
1534                 pending_scan_result->length = len_tmp;
1535                 pending_scan_result->command = command;
1536                 pending_scan_result->buffer = buffer + len_all / 8;
1537
1538                 for (i = 0; i < len_tmp; i++)
1539                 {
1540                         vsllink_tap_append_step(((len_all + i) < length-1 ? 0 : 1), (buffer[(len_all + i)/8] >> ((len_all + i)%8)) & 1);
1541                 }
1542
1543                 pending_scan_results_length++;
1544                 len_all += len_tmp;
1545         }
1546 }
1547
1548 /* Pad and send a tap sequence to the device, and receive the answer.
1549  * For the purpose of padding we assume that we are in reset or idle or pause state. */
1550 static int vsllink_tap_execute_normal(void)
1551 {
1552         int i;
1553         int result;
1554         int first = 0;
1555
1556         if (vsllink_tms_data_len > 0)
1557         {
1558                 if ((tap_get_state() != TAP_RESET) && (tap_get_state() != TAP_IDLE) && (tap_get_state() != TAP_IRPAUSE) && (tap_get_state() != TAP_DRPAUSE))
1559                 {
1560                         LOG_WARNING("%s is not in RESET or IDLE or PAUSR state", tap_state_name(tap_get_state()));
1561                 }
1562
1563                 if (vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] & (1 << (vsllink_tms_data_len - 1)))
1564                 {
1565                         // last tms bit is '1'
1566                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= 0xFF << vsllink_tms_data_len;
1567                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0xFF;
1568                         vsllink_tms_data_len = 0;
1569                 }
1570                 else
1571                 {
1572                         // last tms bit is '0'
1573                         vsllink_usb_out_buffer_idx++;
1574                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
1575                         vsllink_tms_data_len = 0;
1576                 }
1577         }
1578
1579         if (vsllink_usb_out_buffer_idx > 3)
1580         {
1581                 if (vsllink_usb_out_buffer[0] == VSLLINK_CMD_HW_JTAGSEQCMD)
1582                 {
1583                         vsllink_usb_out_buffer[1] = (vsllink_usb_out_buffer_idx >> 0) & 0xff;
1584                         vsllink_usb_out_buffer[2] = (vsllink_usb_out_buffer_idx >> 8) & 0xff;
1585                 }
1586
1587                 result = vsllink_usb_message(vsllink_jtag_handle, vsllink_usb_out_buffer_idx, vsllink_usb_in_want_length);
1588
1589                 if (result == vsllink_usb_in_want_length)
1590                 {
1591                         for (i = 0; i < pending_scan_results_length; i++)
1592                         {
1593                                 struct pending_scan_result *pending_scan_result = &pending_scan_results_buffer[i];
1594                                 uint8_t *buffer = pending_scan_result->buffer;
1595                                 int length = pending_scan_result->length;
1596                                 int offset = pending_scan_result->offset;
1597                                 scan_command_t *command = pending_scan_result->command;
1598
1599                                 if (buffer != NULL)
1600                                 {
1601                                         // IRSHIFT or DRSHIFT
1602                                         buf_set_buf(vsllink_usb_in_buffer, first * 8 + offset, buffer, 0, length);
1603                                         first += (length + offset + 7) >> 3;
1604
1605                                         DEBUG_JTAG_IO("JTAG scan read(%d bits):", length);
1606 #ifdef _DEBUG_JTAG_IO_
1607                                         vsllink_debug_buffer(buffer, (length + 7) >> 3);
1608 #endif
1609
1610                                         if (jtag_read_buffer(buffer, command) != ERROR_OK)
1611                                         {
1612                                                 vsllink_tap_init();
1613                                                 return ERROR_JTAG_QUEUE_FAILED;
1614                                         }
1615
1616                                         free(pending_scan_result->buffer);
1617                                         pending_scan_result->buffer = NULL;
1618                                 }
1619                                 else
1620                                 {
1621                                         first++;
1622                                 }
1623                         }
1624                 }
1625                 else
1626                 {
1627                         LOG_ERROR("vsllink_tap_execute, wrong result %d, expected %d", result, vsllink_usb_in_want_length);
1628                         return ERROR_JTAG_QUEUE_FAILED;
1629                 }
1630
1631                 vsllink_tap_init();
1632         }
1633         reset_command_pointer();
1634
1635         return ERROR_OK;
1636 }
1637 static int vsllink_tap_execute_dma(void)
1638 {
1639         int byte_length;
1640         int i;
1641         int result;
1642
1643         if (tap_length > 0)
1644         {
1645                 /* Pad last byte so that tap_length is divisible by 8 */
1646                 while (tap_length % 8 != 0)
1647                 {
1648                         /* More of the last TMS value keeps us in the same state,
1649                          * analogous to free-running JTAG interfaces. */
1650                         vsllink_tap_append_step(last_tms, 0);
1651                 }
1652                 byte_length = tap_length / 8;
1653
1654                 vsllink_usb_out_buffer[0] = VSLLINK_CMD_HW_JTAGRAWCMD;
1655                 vsllink_usb_out_buffer[1] = ((byte_length * 2 + 3) >> 0) & 0xff;                // package size
1656                 vsllink_usb_out_buffer[2] = ((byte_length * 2 + 3) >> 8) & 0xff;
1657
1658                 memcpy(&vsllink_usb_out_buffer[3], tdi_buffer, byte_length);
1659                 memcpy(&vsllink_usb_out_buffer[3 + byte_length], tms_buffer, byte_length);
1660
1661                 result = vsllink_usb_message(vsllink_jtag_handle, 3 + 2 * byte_length, byte_length);
1662                 if (result == byte_length)
1663                 {
1664                         for (i = 0; i < pending_scan_results_length; i++)
1665                         {
1666                                 struct pending_scan_result *pending_scan_result = &pending_scan_results_buffer[i];
1667                                 uint8_t *buffer = pending_scan_result->buffer;
1668                                 int length = pending_scan_result->length;
1669                                 int first = pending_scan_result->offset;
1670
1671                                 scan_command_t *command = pending_scan_result->command;
1672                                 buf_set_buf(vsllink_usb_in_buffer, first, buffer, 0, length);
1673
1674                                 DEBUG_JTAG_IO("JTAG scan read(%d bits, from %d bits):", length, first);
1675 #ifdef _DEBUG_JTAG_IO_
1676                                 vsllink_debug_buffer(buffer, (length + 7) >> 3);
1677 #endif
1678
1679                                 if (jtag_read_buffer(buffer, command) != ERROR_OK)
1680                                 {
1681                                         vsllink_tap_init();
1682                                         return ERROR_JTAG_QUEUE_FAILED;
1683                                 }
1684
1685                                 if (pending_scan_result->buffer != NULL)
1686                                 {
1687                                         free(pending_scan_result->buffer);
1688                                 }
1689                         }
1690                 }
1691                 else
1692                 {
1693                         LOG_ERROR("vsllink_tap_execute, wrong result %d, expected %d", result, byte_length);
1694                         return ERROR_JTAG_QUEUE_FAILED;
1695                 }
1696
1697                 vsllink_tap_init();
1698         }
1699
1700         return ERROR_OK;
1701 }
1702
1703 /*****************************************************************************/
1704 /* VSLLink USB low-level functions */
1705
1706 static vsllink_jtag_t* vsllink_usb_open(void)
1707 {
1708         struct usb_bus *busses;
1709         struct usb_bus *bus;
1710         struct usb_device *dev;
1711         int ret;
1712
1713         vsllink_jtag_t *result;
1714
1715         result = (vsllink_jtag_t*) malloc(sizeof(vsllink_jtag_t));
1716
1717         usb_init();
1718         usb_find_busses();
1719         usb_find_devices();
1720
1721         busses = usb_get_busses();
1722
1723         /* find vsllink_jtag device in usb bus */
1724
1725         for (bus = busses; bus; bus = bus->next)
1726         {
1727                 for (dev = bus->devices; dev; dev = dev->next)
1728                 {
1729                         if ((dev->descriptor.idVendor == vsllink_usb_vid) && (dev->descriptor.idProduct == vsllink_usb_pid))
1730                         {
1731                                 result->usb_handle = usb_open(dev);
1732                                 if (NULL == result->usb_handle)
1733                                 {
1734                                         LOG_ERROR("failed to open %04X:%04X, not enough permissions?", vsllink_usb_vid, vsllink_usb_pid);
1735                                         exit(-1);
1736                                 }
1737
1738                                 /* usb_set_configuration required under win32 */
1739                                 ret = usb_set_configuration(result->usb_handle, dev->config[0].bConfigurationValue);
1740                                 if (ret != 0)
1741                                 {
1742                                         LOG_ERROR("fail to set configuration to %d, %d returned, not enough permissions?", dev->config[0].bConfigurationValue, ret);
1743                                         exit(-1);
1744                                 }
1745                                 ret = usb_claim_interface(result->usb_handle, vsllink_usb_interface);
1746                                 if (ret != 0)
1747                                 {
1748                                         LOG_ERROR("fail to claim interface %d, %d returned", vsllink_usb_interface, ret);
1749                                         exit(-1);
1750                                 }
1751
1752 #if 0
1753                                 /*
1754                                  * This makes problems under Mac OS X. And is not needed
1755                                  * under Windows. Hopefully this will not break a linux build
1756                                  */
1757                                 usb_set_altinterface(result->usb_handle, 0);
1758 #endif
1759                                 return result;
1760                         }
1761                 }
1762         }
1763
1764         free(result);
1765         return NULL;
1766 }
1767
1768 static void vsllink_usb_close(vsllink_jtag_t *vsllink_jtag)
1769 {
1770         int ret;
1771
1772         ret = usb_release_interface(vsllink_jtag->usb_handle, vsllink_usb_interface);
1773         if (ret != 0)
1774         {
1775                 LOG_ERROR("fail to release interface %d, %d returned", vsllink_usb_interface, ret);
1776                 exit(-1);
1777         }
1778
1779         ret = usb_close(vsllink_jtag->usb_handle);
1780         if (ret != 0)
1781         {
1782                 LOG_ERROR("fail to close usb, %d returned", ret);
1783                 exit(-1);
1784         }
1785
1786         free(vsllink_jtag);
1787 }
1788
1789 /* Send a message and receive the reply. */
1790 static int vsllink_usb_message(vsllink_jtag_t *vsllink_jtag, int out_length, int in_length)
1791 {
1792         int result;
1793
1794         result = vsllink_usb_write(vsllink_jtag, out_length);
1795         if (result == out_length)
1796         {
1797                 if (in_length > 0)
1798                 {
1799                         result = vsllink_usb_read(vsllink_jtag);
1800                         if (result == in_length)
1801                         {
1802                                 return result;
1803                         }
1804                         else
1805                         {
1806                                 LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)", in_length, result);
1807                                 return -1;
1808                         }
1809                 }
1810                 return 0;
1811         }
1812         else
1813         {
1814                 LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)", out_length, result);
1815                 return -1;
1816         }
1817 }
1818
1819 /* Write data from out_buffer to USB. */
1820 static int vsllink_usb_write(vsllink_jtag_t *vsllink_jtag, int out_length)
1821 {
1822         int result;
1823
1824         if (out_length > VSLLINK_BufferSize)
1825         {
1826                 LOG_ERROR("vsllink_jtag_write illegal out_length=%d (max=%d)", out_length, VSLLINK_BufferSize);
1827                 return -1;
1828         }
1829
1830         result = usb_bulk_write(vsllink_jtag->usb_handle, vsllink_usb_bulkout, \
1831                 (char *)vsllink_usb_out_buffer, out_length, VSLLINK_USB_TIMEOUT);
1832
1833         DEBUG_JTAG_IO("vsllink_usb_write, out_length = %d, result = %d", out_length, result);
1834
1835 #ifdef _DEBUG_USB_COMMS_
1836         LOG_DEBUG("USB out:");
1837         vsllink_debug_buffer(vsllink_usb_out_buffer, out_length);
1838 #endif
1839
1840 #ifdef _VSLLINK_IN_DEBUG_MODE_
1841         usleep(100000);
1842 #endif
1843
1844         return result;
1845 }
1846
1847 /* Read data from USB into in_buffer. */
1848 static int vsllink_usb_read(vsllink_jtag_t *vsllink_jtag)
1849 {
1850         int result = usb_bulk_read(vsllink_jtag->usb_handle, vsllink_usb_bulkin, \
1851                 (char *)vsllink_usb_in_buffer, VSLLINK_BufferSize, VSLLINK_USB_TIMEOUT);
1852
1853         DEBUG_JTAG_IO("vsllink_usb_read, result = %d", result);
1854
1855 #ifdef _DEBUG_USB_COMMS_
1856         LOG_DEBUG("USB in:");
1857         vsllink_debug_buffer(vsllink_usb_in_buffer, result);
1858 #endif
1859         return result;
1860 }
1861
1862 #define BYTES_PER_LINE  16
1863
1864 #if defined _DEBUG_USB_COMMS_ || defined _DEBUG_JTAG_IO_
1865 static void vsllink_debug_buffer(uint8_t *buffer, int length)
1866 {
1867         char line[81];
1868         char s[4];
1869         int i;
1870         int j;
1871
1872         for (i = 0; i < length; i += BYTES_PER_LINE)
1873         {
1874                 snprintf(line, 5, "%04x", i);
1875                 for (j = i; j < i + BYTES_PER_LINE && j < length; j++)
1876                 {
1877                         snprintf(s, 4, " %02x", buffer[j]);
1878                         strcat(line, s);
1879                 }
1880                 LOG_DEBUG("%s", line);
1881         }
1882 }
1883 #endif // _DEBUG_USB_COMMS_ || _DEBUG_JTAG_IO_
1884
1885 static int vsllink_register_commands(struct command_context_s *cmd_ctx)
1886 {
1887         register_command(cmd_ctx, NULL, "vsllink_usb_vid",
1888                         vsllink_handle_usb_vid_command, COMMAND_CONFIG,
1889                         NULL);
1890         register_command(cmd_ctx, NULL, "vsllink_usb_pid",
1891                         vsllink_handle_usb_pid_command, COMMAND_CONFIG,
1892                         NULL);
1893         register_command(cmd_ctx, NULL, "vsllink_usb_bulkin",
1894                         vsllink_handle_usb_bulkin_command, COMMAND_CONFIG,
1895                         NULL);
1896         register_command(cmd_ctx, NULL, "vsllink_usb_bulkout",
1897                         vsllink_handle_usb_bulkout_command, COMMAND_CONFIG,
1898                         NULL);
1899         register_command(cmd_ctx, NULL, "vsllink_usb_interface",
1900                         vsllink_handle_usb_interface_command, COMMAND_CONFIG,
1901                         NULL);
1902         register_command(cmd_ctx, NULL, "vsllink_mode",
1903                         vsllink_handle_mode_command, COMMAND_CONFIG,
1904                         NULL);
1905
1906         return ERROR_OK;
1907 }
1908
1909 struct jtag_interface vsllink_interface = {
1910                 .name = "vsllink",
1911                 .register_commands = &vsllink_register_commands,
1912                 .init = &vsllink_init,
1913                 .quit = &vsllink_quit,
1914                 .khz = &vsllink_khz,
1915                 .speed = &vsllink_speed,
1916                 .speed_div = &vsllink_speed_div,
1917                 .execute_queue = &vsllink_execute_queue,
1918         };