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