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

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