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