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