git.sur5r.net Git - 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         vsllink_tap_ensure_space(0, 8);
 714
 715         if (tap_get_state() == TAP_RESET)
 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_ensure_space(1, (scan_size + 7) & ~0x00000007);
1322         vsllink_tap_append_scan_dma(scan_size, buffer, command);
1323
1324         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
1325         while (tap_length % 8 != 0)
1326         {
1327                 // more 0s in Pause
1328                 vsllink_tap_append_step(0, 0);
1329         }
1330
1331         if (tap_get_state() != tap_get_end_state())
1332         {
1333                 vsllink_state_move();
1334         }
1335 }
1336
1337 static void vsllink_reset(int trst, int srst)
1338 {
1339         int result;
1340
1341         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
1342
1343         /* Signals are active low */
1344         vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_PORT;
1345         vsllink_usb_out_buffer[1] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST;
1346         vsllink_usb_out_buffer[2] = 0;
1347         if (srst == 0)
1348         {
1349                 vsllink_usb_out_buffer[2] |= JTAG_PINMSK_SRST;
1350         }
1351         if (trst == 0)
1352         {
1353                 vsllink_usb_out_buffer[2] |= JTAG_PINMSK_TRST;
1354         }
1355
1356         result = vsllink_usb_write(vsllink_jtag_handle, 3);
1357         if (result != 3)
1358         {
1359                 LOG_ERROR("VSLLink command VSLLINK_CMD_SET_PORT failed (%d)", result);
1360         }
1361 }
1362
1363 static void vsllink_simple_command(u8 command)
1364 {
1365         int result;
1366
1367         DEBUG_JTAG_IO("0x%02x", command);
1368
1369         vsllink_usb_out_buffer[0] = command;
1370         result = vsllink_usb_write(vsllink_jtag_handle, 1);
1371
1372         if (result != 1)
1373         {
1374                 LOG_ERROR("VSLLink command 0x%02x failed (%d)", command, result);
1375         }
1376 }
1377
1378 static int vsllink_register_commands(struct command_context_s *cmd_ctx)
1379 {
1380         register_command(cmd_ctx, NULL, "vsllink_usb_vid", vsllink_handle_usb_vid_command,
1381                                         COMMAND_CONFIG, NULL);
1382         register_command(cmd_ctx, NULL, "vsllink_usb_pid", vsllink_handle_usb_pid_command,
1383                                         COMMAND_CONFIG, NULL);
1384         register_command(cmd_ctx, NULL, "vsllink_usb_bulkin", vsllink_handle_usb_bulkin_command,
1385                                         COMMAND_CONFIG, NULL);
1386         register_command(cmd_ctx, NULL, "vsllink_usb_bulkout", vsllink_handle_usb_bulkout_command,
1387                                         COMMAND_CONFIG, NULL);
1388         register_command(cmd_ctx, NULL, "vsllink_usb_interface", vsllink_handle_usb_interface_command,
1389                                         COMMAND_CONFIG, NULL);
1390         register_command(cmd_ctx, NULL, "vsllink_mode", vsllink_handle_mode_command,
1391                                         COMMAND_CONFIG, NULL);
1392
1393         return ERROR_OK;
1394 }
1395
1396 static int vsllink_handle_mode_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1397 {
1398         if (argc != 1) {
1399                 LOG_ERROR("parameter error, should be one parameter for VID");
1400                 return ERROR_FAIL;
1401         }
1402
1403         if (!strcmp(args[0], "normal"))
1404         {
1405                 vsllink_mode = VSLLINK_MODE_NORMAL;
1406         }
1407         else if (!strcmp(args[0], "dma"))
1408         {
1409                 vsllink_mode = VSLLINK_MODE_DMA;
1410         }
1411         else
1412         {
1413                 LOG_ERROR("invalid vsllink_mode: %s", args[0]);
1414                 return ERROR_FAIL;
1415         }
1416
1417         return ERROR_OK;
1418 }
1419
1420 static int vsllink_handle_usb_vid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1421 {
1422         if (argc != 1)
1423         {
1424                 LOG_ERROR("parameter error, should be one parameter for VID");
1425                 return ERROR_OK;
1426         }
1427
1428         vsllink_usb_vid = strtol(args[0], NULL, 0);
1429
1430         return ERROR_OK;
1431 }
1432
1433 static int vsllink_handle_usb_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1434 {
1435         if (argc != 1)
1436         {
1437                 LOG_ERROR("parameter error, should be one parameter for PID");
1438                 return ERROR_OK;
1439         }
1440
1441         vsllink_usb_pid = strtol(args[0], NULL, 0);
1442
1443         return ERROR_OK;
1444 }
1445
1446 static int vsllink_handle_usb_bulkin_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1447 {
1448         if (argc != 1)
1449         {
1450                 LOG_ERROR("parameter error, should be one parameter for BULKIN endpoint");
1451                 return ERROR_OK;
1452         }
1453
1454         vsllink_usb_bulkin = strtol(args[0], NULL, 0) | 0x80;
1455
1456         return ERROR_OK;
1457 }
1458
1459 static int vsllink_handle_usb_bulkout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1460 {
1461         if (argc != 1)
1462         {
1463                 LOG_ERROR("parameter error, should be one parameter for BULKOUT endpoint");
1464                 return ERROR_OK;
1465         }
1466
1467         vsllink_usb_bulkout = strtol(args[0], NULL, 0);
1468
1469         return ERROR_OK;
1470 }
1471
1472 static int vsllink_handle_usb_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1473 {
1474         if (argc != 1)
1475         {
1476                 LOG_ERROR("parameter error, should be one parameter for interface number");
1477                 return ERROR_OK;
1478         }
1479
1480         vsllink_usb_interface = strtol(args[0], NULL, 0);
1481
1482         return ERROR_OK;
1483 }
1484
1485 /***************************************************************************/
1486 /* VSLLink tap functions */
1487
1488 static void vsllink_tap_init_normal(void)
1489 {
1490         vsllink_usb_out_buffer_idx = 0;
1491         vsllink_usb_in_want_length = 0;
1492         pending_scan_results_length = 0;
1493 }
1494 static void vsllink_tap_init_dma(void)
1495 {
1496         tap_length = 0;
1497         pending_scan_results_length = 0;
1498 }
1499
1500 static void vsllink_tap_ensure_space_normal(int scans, int length)
1501 {
1502         int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
1503         int available_bytes = VSLLINK_BufferSize - vsllink_usb_out_buffer_idx;
1504
1505         if (scans > available_scans || length > available_bytes)
1506         {
1507                 vsllink_tap_execute();
1508         }
1509 }
1510 static void vsllink_tap_ensure_space_dma(int scans, int length)
1511 {
1512         int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
1513         int available_bytes = tap_buffer_size * 8 - tap_length;
1514
1515         if (scans > available_scans || length > available_bytes)
1516         {
1517                 vsllink_tap_execute();
1518         }
1519 }
1520
1521 static void vsllink_tap_append_step(int tms, int tdi)
1522 {
1523         last_tms = tms;
1524         int index = tap_length / 8;
1525
1526         if (index < tap_buffer_size)
1527         {
1528                 int bit_index = tap_length % 8;
1529                 u8 bit = 1 << bit_index;
1530
1531                 if (tms)
1532                 {
1533                         tms_buffer[index] |= bit;
1534                 }
1535                 else
1536                 {
1537                         tms_buffer[index] &= ~bit;
1538                 }
1539
1540                 if (tdi)
1541                 {
1542                         tdi_buffer[index] |= bit;
1543                 }
1544                 else
1545                 {
1546                         tdi_buffer[index] &= ~bit;
1547                 }
1548
1549                 tap_length++;
1550         }
1551         else
1552         {
1553                 LOG_ERROR("buffer overflow, tap_length=%d", tap_length);
1554         }
1555 }
1556
1557 static void vsllink_tap_append_scan_normal(int length, u8 *buffer, scan_command_t *command, int offset)
1558 {
1559         pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[pending_scan_results_length];
1560         int i;
1561
1562         if (offset > 0)
1563         {
1564                 vsllink_usb_in_want_length += ((length + 7) >> 3) + 1;
1565         }
1566         else
1567         {
1568                 vsllink_usb_in_want_length += (length + 7) >> 3;
1569         }
1570         pending_scan_result->length = length;
1571         pending_scan_result->offset = offset;
1572         pending_scan_result->command = command;
1573         pending_scan_result->buffer = buffer;
1574
1575         for (i = 0; i < ((length + 7) >> 3); i++)
1576         {
1577                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = buffer[i];
1578         }
1579
1580         pending_scan_results_length++;
1581 }
1582 static void vsllink_tap_append_scan_dma(int length, u8 *buffer, scan_command_t *command)
1583 {
1584         pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[pending_scan_results_length];
1585         int i;
1586
1587         pending_scan_result->offset = tap_length;
1588         pending_scan_result->length = length;
1589         pending_scan_result->command = command;
1590         pending_scan_result->buffer = buffer;
1591
1592         for (i = 0; i < length; i++)
1593         {
1594                 vsllink_tap_append_step((i < length-1 ? 0 : 1), (buffer[i/8] >> (i%8)) & 1);
1595         }
1596
1597         pending_scan_results_length++;
1598 }
1599
1600 /* Pad and send a tap sequence to the device, and receive the answer.
1601  * For the purpose of padding we assume that we are in reset or idle or pause state. */
1602 static int vsllink_tap_execute_normal(void)
1603 {
1604         int i;
1605         int result;
1606         int first = 0;
1607
1608         if (vsllink_tms_data_len > 0)
1609         {
1610                 if((tap_get_state() != TAP_RESET) && (tap_get_state() != TAP_IDLE) && (tap_get_state() != TAP_IRPAUSE) && (tap_get_state() != TAP_DRPAUSE))
1611                 {
1612                         LOG_WARNING("%s is not in RESET or IDLE or PAUSR state", tap_state_name(tap_get_state()));
1613                 }
1614
1615                 if (vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] & (1 << (vsllink_tms_data_len - 1)))
1616                 {
1617                         // last tms bit is '1'
1618                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= 0xFF << vsllink_tms_data_len;
1619                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0xFF;
1620                         vsllink_tms_data_len = 0;
1621                 }
1622                 else
1623                 {
1624                         // last tms bit is '0'
1625                         vsllink_usb_out_buffer_idx++;
1626                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
1627                         vsllink_tms_data_len = 0;
1628                 }
1629         }
1630
1631         if (vsllink_usb_out_buffer_idx > 3)
1632         {
1633                 if (vsllink_usb_out_buffer[0] == VSLLINK_CMD_HW_JTAGSEQCMD)
1634                 {
1635                         vsllink_usb_out_buffer[1] = (vsllink_usb_out_buffer_idx >> 0) & 0xff;
1636                         vsllink_usb_out_buffer[2] = (vsllink_usb_out_buffer_idx >> 8) & 0xff;
1637                 }
1638
1639                 result = vsllink_usb_message(vsllink_jtag_handle, vsllink_usb_out_buffer_idx, vsllink_usb_in_want_length);
1640
1641                 if (result == vsllink_usb_in_want_length)
1642                 {
1643                         for (i = 0; i < pending_scan_results_length; i++)
1644                         {
1645                                 pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[i];
1646                                 u8 *buffer = pending_scan_result->buffer;
1647                                 int length = pending_scan_result->length;
1648                                 int offset = pending_scan_result->offset;
1649                                 scan_command_t *command = pending_scan_result->command;
1650
1651                                 if (buffer != NULL)
1652                                 {
1653                                         // IRSHIFT or DRSHIFT
1654                                         buf_set_buf(vsllink_usb_in_buffer, first * 8 + offset, buffer, 0, length);
1655                                         first += (length + offset + 7) >> 3;
1656
1657                                         DEBUG_JTAG_IO("JTAG scan read(%d bits):", length);
1658 #ifdef _DEBUG_JTAG_IO_
1659                                         vsllink_debug_buffer(buffer, (length + 7) >> 3);
1660 #endif
1661
1662                                         if (jtag_read_buffer(buffer, command) != ERROR_OK)
1663                                         {
1664                                                 vsllink_tap_init();
1665                                                 return ERROR_JTAG_QUEUE_FAILED;
1666                                         }
1667
1668                                         free(pending_scan_result->buffer);
1669                                         pending_scan_result->buffer = NULL;
1670                                 }
1671                                 else
1672                                 {
1673                                         first++;
1674                                 }
1675                         }
1676                 }
1677                 else
1678                 {
1679                         LOG_ERROR("vsllink_tap_execute, wrong result %d, expected %d", result, vsllink_usb_in_want_length);
1680                         return ERROR_JTAG_QUEUE_FAILED;
1681                 }
1682
1683                 vsllink_tap_init();
1684         }
1685         reset_command_pointer();
1686
1687         return ERROR_OK;
1688 }
1689 static int vsllink_tap_execute_dma(void)
1690 {
1691         int byte_length;
1692         int i;
1693         int result;
1694
1695         if (tap_length > 0)
1696         {
1697                 /* Pad last byte so that tap_length is divisible by 8 */
1698                 while (tap_length % 8 != 0)
1699                 {
1700                         /* More of the last TMS value keeps us in the same state,
1701                          * analogous to free-running JTAG interfaces. */
1702                         vsllink_tap_append_step(last_tms, 0);
1703                 }
1704                 byte_length = tap_length / 8;
1705
1706                 vsllink_usb_out_buffer[0] = VSLLINK_CMD_HW_JTAGRAWCMD;
1707                 vsllink_usb_out_buffer[1] = ((byte_length * 2 + 3) >> 0) & 0xff;                // package size
1708                 vsllink_usb_out_buffer[2] = ((byte_length * 2 + 3) >> 8) & 0xff;
1709
1710                 memcpy(&vsllink_usb_out_buffer[3], tdi_buffer, byte_length);
1711                 memcpy(&vsllink_usb_out_buffer[3 + byte_length], tms_buffer, byte_length);
1712
1713                 result = vsllink_usb_message(vsllink_jtag_handle, 3 + 2 * byte_length, byte_length);
1714                 if (result == byte_length)
1715                 {
1716                         for (i = 0; i < pending_scan_results_length; i++)
1717                         {
1718                                 pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[i];
1719                                 u8 *buffer = pending_scan_result->buffer;
1720                                 int length = pending_scan_result->length;
1721                                 int first = pending_scan_result->offset;
1722
1723                                 scan_command_t *command = pending_scan_result->command;
1724                                 buf_set_buf(vsllink_usb_in_buffer, first, buffer, 0, length);
1725
1726                                 DEBUG_JTAG_IO("JTAG scan read(%d bits, from %d bits):", length, first);
1727 #ifdef _DEBUG_JTAG_IO_
1728                                 vsllink_debug_buffer(buffer, (length + 7) >> 3);
1729 #endif
1730
1731                                 if (jtag_read_buffer(buffer, command) != ERROR_OK)
1732                                 {
1733                                         vsllink_tap_init();
1734                                         return ERROR_JTAG_QUEUE_FAILED;
1735                                 }
1736
1737                                 if (pending_scan_result->buffer != NULL)
1738                                 {
1739                                         free(pending_scan_result->buffer);
1740                                 }
1741                         }
1742                 }
1743                 else
1744                 {
1745                         LOG_ERROR("vsllink_tap_execute, wrong result %d, expected %d", result, byte_length);
1746                         return ERROR_JTAG_QUEUE_FAILED;
1747                 }
1748
1749                 vsllink_tap_init();
1750         }
1751
1752         return ERROR_OK;
1753 }
1754
1755 /*****************************************************************************/
1756 /* VSLLink USB low-level functions */
1757
1758 static vsllink_jtag_t* vsllink_usb_open(void)
1759 {
1760         struct usb_bus *busses;
1761         struct usb_bus *bus;
1762         struct usb_device *dev;
1763         int ret;
1764
1765         vsllink_jtag_t *result;
1766
1767         result = (vsllink_jtag_t*) malloc(sizeof(vsllink_jtag_t));
1768
1769         usb_init();
1770         usb_find_busses();
1771         usb_find_devices();
1772
1773         busses = usb_get_busses();
1774
1775         /* find vsllink_jtag device in usb bus */
1776
1777         for (bus = busses; bus; bus = bus->next)
1778         {
1779                 for (dev = bus->devices; dev; dev = dev->next)
1780                 {
1781                         if ((dev->descriptor.idVendor == vsllink_usb_vid) && (dev->descriptor.idProduct == vsllink_usb_pid))
1782                         {
1783                                 result->usb_handle = usb_open(dev);
1784                                 if (NULL == result->usb_handle)
1785                                 {
1786                                         LOG_ERROR("failed to open %04X:%04X, not enough permissions?", vsllink_usb_vid, vsllink_usb_pid);
1787                                         exit(-1);
1788                                 }
1789
1790                                 /* usb_set_configuration required under win32 */
1791                                 ret = usb_set_configuration(result->usb_handle, dev->config[0].bConfigurationValue);
1792                                 if (ret != 0)
1793                                 {
1794                                         LOG_ERROR("fail to set configuration to %d, %d returned, not enough permissions?", dev->config[0].bConfigurationValue, ret);
1795                                         exit(-1);
1796                                 }
1797                                 ret = usb_claim_interface(result->usb_handle, vsllink_usb_interface);
1798                                 if (ret != 0)
1799                                 {
1800                                         LOG_ERROR("fail to claim interface %d, %d returned", vsllink_usb_interface, ret);
1801                                         exit(-1);
1802                                 }
1803
1804 #if 0
1805                                 /*
1806                                  * This makes problems under Mac OS X. And is not needed
1807                                  * under Windows. Hopefully this will not break a linux build
1808                                  */
1809                                 usb_set_altinterface(result->usb_handle, 0);
1810 #endif
1811                                 return result;
1812                         }
1813                 }
1814         }
1815
1816         free(result);
1817         return NULL;
1818 }
1819
1820 static void vsllink_usb_close(vsllink_jtag_t *vsllink_jtag)
1821 {
1822         int ret;
1823
1824         ret = usb_release_interface(vsllink_jtag->usb_handle, vsllink_usb_interface);
1825         if (ret != 0)
1826         {
1827                 LOG_ERROR("fail to release interface %d, %d returned", vsllink_usb_interface, ret);
1828                 exit(-1);
1829         }
1830
1831         ret = usb_close(vsllink_jtag->usb_handle);
1832         if (ret != 0)
1833         {
1834                 LOG_ERROR("fail to close usb, %d returned", ret);
1835                 exit(-1);
1836         }
1837
1838         free(vsllink_jtag);
1839 }
1840
1841 /* Send a message and receive the reply. */
1842 static int vsllink_usb_message(vsllink_jtag_t *vsllink_jtag, int out_length, int in_length)
1843 {
1844         int result;
1845
1846         result = vsllink_usb_write(vsllink_jtag, out_length);
1847         if (result == out_length)
1848         {
1849                 if (in_length > 0)
1850                 {
1851                         result = vsllink_usb_read(vsllink_jtag);
1852                         if (result == in_length )
1853                         {
1854                                 return result;
1855                         }
1856                         else
1857                         {
1858                                 LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)", in_length, result);
1859                                 return -1;
1860                         }
1861                 }
1862                 return 0;
1863         }
1864         else
1865         {
1866                 LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)", out_length, result);
1867                 return -1;
1868         }
1869 }
1870
1871 /* Write data from out_buffer to USB. */
1872 static int vsllink_usb_write(vsllink_jtag_t *vsllink_jtag, int out_length)
1873 {
1874         int result;
1875
1876         if (out_length > VSLLINK_BufferSize)
1877         {
1878                 LOG_ERROR("vsllink_jtag_write illegal out_length=%d (max=%d)", out_length, VSLLINK_BufferSize);
1879                 return -1;
1880         }
1881
1882         result = usb_bulk_write(vsllink_jtag->usb_handle, vsllink_usb_bulkout, \
1883                 (char *)vsllink_usb_out_buffer, out_length, VSLLINK_USB_TIMEOUT);
1884
1885         DEBUG_JTAG_IO("vsllink_usb_write, out_length = %d, result = %d", out_length, result);
1886
1887 #ifdef _DEBUG_USB_COMMS_
1888         LOG_DEBUG("USB out:");
1889         vsllink_debug_buffer(vsllink_usb_out_buffer, out_length);
1890 #endif
1891
1892 #ifdef _VSLLINK_IN_DEBUG_MODE_
1893         usleep(100000);
1894 #endif
1895
1896         return result;
1897 }
1898
1899 /* Read data from USB into in_buffer. */
1900 static int vsllink_usb_read(vsllink_jtag_t *vsllink_jtag)
1901 {
1902         int result = usb_bulk_read(vsllink_jtag->usb_handle, vsllink_usb_bulkin, \
1903                 (char *)vsllink_usb_in_buffer, VSLLINK_BufferSize, VSLLINK_USB_TIMEOUT);
1904
1905         DEBUG_JTAG_IO("vsllink_usb_read, result = %d", result);
1906
1907 #ifdef _DEBUG_USB_COMMS_
1908         LOG_DEBUG("USB in:");
1909         vsllink_debug_buffer(vsllink_usb_in_buffer, result);
1910 #endif
1911         return result;
1912 }
1913
1914 #define BYTES_PER_LINE  16
1915
1916 #if defined _DEBUG_USB_COMMS_ || defined _DEBUG_JTAG_IO_
1917 static void vsllink_debug_buffer(u8 *buffer, int length)
1918 {
1919         char line[81];
1920         char s[4];
1921         int i;
1922         int j;
1923
1924         for (i = 0; i < length; i += BYTES_PER_LINE)
1925         {
1926                 snprintf(line, 5, "%04x", i);
1927                 for (j = i; j < i + BYTES_PER_LINE && j < length; j++)
1928                 {
1929                         snprintf(s, 4, " %02x", buffer[j]);
1930                         strcat(line, s);
1931                 }
1932                 LOG_DEBUG("%s", line);
1933         }
1934 }
1935 #endif // _DEBUG_USB_COMMS_ || _DEBUG_JTAG_IO_