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