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