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