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