git.gag.com Git - fw/openocd/blob - src/jtag/vsllink.c

   1 /***************************************************************************
   2  *   Copyright (C) 2007 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 /* enable this to view USB communication
  41  */
  42 #if 0
  43 #define _DEBUG_USB_COMMS_
  44 #endif
  45
  46 #ifdef _DEBUG_JTAG_IO_
  47 #define DEBUG_JTAG_IO(expr ...) LOG_DEBUG(expr)
  48 #else
  49 #define DEBUG_JTAG_IO(expr ...)
  50 #endif
  51
  52 #define VID                                                     0x03EB
  53 #define PID                                                     0x2103
  54 #define VSLLINK_WRITE_ENDPOINT          0x02
  55 #define VSLLINK_READ_ENDPOINT           0x82
  56
  57 u16 vsllink_vid = VID;
  58 u16 vsllink_pid = PID;
  59 u8 vsllink_bulkout = VSLLINK_WRITE_ENDPOINT;
  60 u8 vsllink_bulkin = VSLLINK_READ_ENDPOINT;
  61
  62 #define VSLLINK_USB_TIMEOUT                     1000
  63
  64 static int VSLLINK_BufferSize = 1024;
  65
  66 /* Global USB buffers */
  67 static int vsllink_usb_out_buffer_idx;
  68 static int vsllink_usb_in_want_length;
  69 static u8* vsllink_usb_in_buffer = NULL;
  70 static u8* vsllink_usb_out_buffer = NULL;
  71
  72 /* Constants for VSLLink command */
  73 #define VSLLINK_CMD_CONN                        0x80
  74 #define VSLLINK_CMD_DISCONN                     0x81
  75 #define VSLLINK_CMD_SET_SPEED           0x82
  76 #define VSLLINK_CMD_SET_PORT            0x90
  77 #define VSLLINK_CMD_GET_PORT            0x91
  78 #define VSLLINK_CMD_SET_PORTDIR         0x92
  79 #define VSLLINK_CMD_HW_JTAGSEQCMD       0xA0
  80
  81 #define VSLLINK_CMDJTAGSEQ_TMSBYTE      0x00
  82 #define VSLLINK_CMDJTAGSEQ_SCAN         0x80
  83
  84 #define VSLLINK_CMDJTAGSEQ_CMDMSK       0xC0
  85 #define VSLLINK_CMDJTAGSEQ_LENMSK       0x3F
  86
  87 #define JTAG_PINMSK_SRST                        (1 << 0)
  88 #define JTAG_PINMSK_TRST                        (1 << 1)
  89 #define JTAG_PINMSK_USR1                        (1 << 2)
  90 #define JTAG_PINMSK_USR2                        (1 << 3)
  91 #define JTAG_PINMSK_TCK                         (1 << 4)
  92 #define JTAG_PINMSK_TMS                         (1 << 5)
  93 #define JTAG_PINMSK_TDI                         (1 << 6)
  94 #define JTAG_PINMSK_TDO                         (1 << 7)
  95
  96
  97 #define VSLLINK_TAP_MOVE(from, to) VSLLINK_tap_move[tap_move_map[from]][tap_move_map[to]]
  98
  99 /* VSLLINK_tap_move[i][j]: tap movement command to go from state i to state j
 100  * 0: Test-Logic-Reset
 101  * 1: Run-Test/Idle
 102  * 2: Shift-DR
 103  * 3: Pause-DR
 104  * 4: Shift-IR
 105  * 5: Pause-IR
 106  *
 107  * SD->SD and SI->SI have to be caught in interface specific code
 108  */
 109 u8 VSLLINK_tap_move[6][6] =
 110 {
 111 /*        TLR   RTI   SD    PD    SI    PI             */
 112         {0xff, 0x00, 0x2f, 0x0a, 0x37, 0x16},   /* TLR */
 113         {0xff, 0x00, 0x45, 0x05, 0x4b, 0x0b},   /* RTI */
 114         {0xff, 0x61, 0x00, 0x01, 0x0f, 0x2f},   /* SD  */
 115         {0xff, 0x60, 0x40, 0x17, 0x3c, 0x2f},   /* PD  */
 116         {0xff, 0x61, 0x07, 0x17, 0x00, 0x01},   /* SI  */
 117         {0xff, 0x60, 0x38, 0x17, 0x40, 0x2f}    /* PI  */
 118 };
 119
 120 u8 VSLLINK_TAP_MOVE_FROM_E1[6] =
 121 {
 122 //      TLR             RTI             SD              PD              SI              PI
 123         0xff,   0x60,   0x38,   0x5c,   0x3c,   0x5E
 124 };
 125
 126 u8 VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[7][6][2] =
 127 {
 128 /*       stuff  offset   */
 129         {/*     TLR     */
 130         {1,             0,},    /* TLR */
 131         {1,             0,},    /* RTI */
 132         {1,             0,},    /* SD  */
 133         {1,             0,},    /* PD  */
 134         {1,             0,},    /* SI  */
 135         {1,             0,}},   /* PI  */
 136         {/*     RTI     */
 137         {1,             0,},    /* TLR */
 138         {0,             0,},    /* RTI */
 139         {0,             4,},    /* SD  */
 140         {0,             7,},    /* PD  */
 141         {0,             5,},    /* SI  */
 142         {0,             7,}},   /* PI  */
 143         {/*     SD      */
 144         {0,             0,},    /* TLR */
 145         {0,             0,},    /* RTI */
 146         {0,             0,},    /* SD  */
 147         {0,             0,},    /* PD  */
 148         {0,             0,},    /* SI  */
 149         {0,             0,}},   /* PI  */
 150         {/*     PD      */
 151         {0,             0,},    /* TLR */
 152         {0,             0,},    /* RTI */
 153         {0,             0,},    /* SD  */
 154         {0,             0,},    /* PD  */
 155         {0,             0,},    /* SI  */
 156         {0,             0,}},   /* PI  */
 157         {/*     SI      */
 158         {0,             0,},    /* TLR */
 159         {0,             0,},    /* RTI */
 160         {0,             0,},    /* SD  */
 161         {0,             0,},    /* PD  */
 162         {0,             0,},    /* SI  */
 163         {0,             0,}},   /* PI  */
 164         {/*     PI      */
 165         {0,             0,},    /* TLR */
 166         {0,             0,},    /* RTI */
 167         {0,             0,},    /* SD  */
 168         {0,             0,},    /* PD  */
 169         {0,             0,},    /* SI  */
 170         {0,             0,}},   /* PI  */
 171 };
 172
 173 u8 VSLLINK_BIT_MSK[8] =
 174 {
 175         0x00, 0x01, 0x03, 0x07, 0x0f, 0x1f, 0x3f, 0x7f
 176 };
 177
 178 /* External interface functions */
 179 int vsllink_execute_queue(void);
 180 int vsllink_speed(int speed);
 181 int vsllink_khz(int khz, int *jtag_speed);
 182 int vsllink_speed_div(int jtag_speed, int *khz);
 183 int vsllink_register_commands(struct command_context_s *cmd_ctx);
 184 int vsllink_init(void);
 185 int vsllink_quit(void);
 186
 187 /* CLI command handler functions */
 188 int vsllink_handle_usb_vid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 189 int vsllink_handle_usb_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 190 int vsllink_handle_usb_bulkin_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 191 int vsllink_handle_usb_bulkout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 192
 193 /* Queue command functions */
 194 void vsllink_end_state(enum tap_state state);
 195 void vsllink_state_move(void);
 196 void vsllink_path_move(int num_states, enum tap_state *path);
 197 void vsllink_runtest(int num_cycles);
 198 void vsllink_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size, scan_command_t *command);
 199 void vsllink_reset(int trst, int srst);
 200 void vsllink_simple_command(u8 command);
 201
 202 /* VSLLink tap buffer functions */
 203 void vsllink_tap_init(void);
 204 int vsllink_tap_execute(void);
 205 void vsllink_tap_ensure_space(int scans, int bytes);
 206 void vsllink_tap_append_scan(int length, u8 *buffer, scan_command_t *command, int offset);
 207
 208 /* VSLLink lowlevel functions */
 209 typedef struct vsllink_jtag
 210 {
 211         struct usb_dev_handle* usb_handle;
 212 } vsllink_jtag_t;
 213
 214 vsllink_jtag_t *vsllink_usb_open(void);
 215 void vsllink_usb_close(vsllink_jtag_t *vsllink_jtag);
 216 int vsllink_usb_message(vsllink_jtag_t *vsllink_jtag, int out_length, int in_length);
 217 int vsllink_usb_write(vsllink_jtag_t *vsllink_jtag, int out_length);
 218 int vsllink_usb_read(vsllink_jtag_t *vsllink_jtag);
 219
 220 void vsllink_debug_buffer(u8 *buffer, int length);
 221
 222 int vsllink_tms_data_len = 0;
 223 u8* vsllink_tms_cmd_pos;
 224
 225 vsllink_jtag_t* vsllink_jtag_handle;
 226
 227 /***************************************************************************/
 228 /* External interface implementation */
 229
 230 jtag_interface_t vsllink_interface =
 231 {
 232         .name = "vsllink",
 233         .execute_queue = vsllink_execute_queue,
 234         .speed = vsllink_speed,
 235         .khz = vsllink_khz,
 236         .speed_div = vsllink_speed_div,
 237         .register_commands = vsllink_register_commands,
 238         .init = vsllink_init,
 239         .quit = vsllink_quit
 240 };
 241
 242 int vsllink_execute_queue(void)
 243 {
 244         jtag_command_t *cmd = jtag_command_queue;
 245         int scan_size;
 246         enum scan_type type;
 247         u8 *buffer;
 248
 249         DEBUG_JTAG_IO("--------------------------------------------------------------------------------");
 250
 251         vsllink_usb_out_buffer[0] = VSLLINK_CMD_HW_JTAGSEQCMD;
 252         vsllink_usb_out_buffer_idx = 3;
 253         while (cmd != NULL)
 254         {
 255                 switch (cmd->type)
 256                 {
 257                         case JTAG_END_STATE:
 258                                 DEBUG_JTAG_IO("end_state: %i", cmd->cmd.end_state->end_state);
 259
 260                                 if (cmd->cmd.end_state->end_state != -1)
 261                                 {
 262                                         vsllink_end_state(cmd->cmd.end_state->end_state);
 263                                 }
 264                                 break;
 265
 266                         case JTAG_RUNTEST:
 267                                 DEBUG_JTAG_IO( "runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, \
 268                                         cmd->cmd.runtest->end_state);
 269
 270                                 if (cmd->cmd.runtest->end_state != -1)
 271                                 {
 272                                         vsllink_end_state(cmd->cmd.runtest->end_state);
 273                                 }
 274                                 vsllink_runtest(cmd->cmd.runtest->num_cycles);
 275                                 break;
 276
 277                         case JTAG_STATEMOVE:
 278                                 DEBUG_JTAG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
 279
 280                                 if (cmd->cmd.statemove->end_state != -1)
 281                                 {
 282                                         vsllink_end_state(cmd->cmd.statemove->end_state);
 283                                 }
 284                                 vsllink_state_move();
 285                                 break;
 286
 287                         case JTAG_PATHMOVE:
 288                                 DEBUG_JTAG_IO("pathmove: %i states, end in %i", \
 289                                         cmd->cmd.pathmove->num_states, \
 290                                         cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
 291
 292                                 vsllink_path_move(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
 293                                 break;
 294
 295                         case JTAG_SCAN:
 296                                 if (cmd->cmd.scan->end_state != -1)
 297                                 {
 298                                         vsllink_end_state(cmd->cmd.scan->end_state);
 299                                 }
 300
 301                                 scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
 302                                 if (cmd->cmd.scan->ir_scan)
 303                                 {
 304                                         DEBUG_JTAG_IO("JTAG Scan write IR(%d bits), end in %d:", scan_size, cmd->cmd.scan->end_state);
 305                                 }
 306                                 else
 307                                 {
 308                                         DEBUG_JTAG_IO("JTAG Scan write DR(%d bits), end in %d:", scan_size, cmd->cmd.scan->end_state);
 309                                 }
 310
 311 #ifdef _DEBUG_JTAG_IO_
 312                                 vsllink_debug_buffer(buffer, (scan_size + 7) >> 3);
 313 #endif
 314
 315                                 type = jtag_scan_type(cmd->cmd.scan);
 316
 317                                 vsllink_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size, cmd->cmd.scan);
 318                                 break;
 319
 320                         case JTAG_RESET:
 321                                 DEBUG_JTAG_IO("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 322
 323                                 vsllink_tap_execute();
 324
 325                                 if (cmd->cmd.reset->trst == 1)
 326                                 {
 327                                         cur_state = TAP_RESET;
 328                                 }
 329                                 vsllink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 330
 331                                 vsllink_usb_out_buffer[0] = VSLLINK_CMD_HW_JTAGSEQCMD;
 332                                 vsllink_usb_out_buffer_idx = 3;
 333                                 break;
 334
 335                         case JTAG_SLEEP:
 336                                 DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
 337                                 vsllink_tap_execute();
 338                                 jtag_sleep(cmd->cmd.sleep->us);
 339                                 break;
 340
 341                         default:
 342                                 LOG_ERROR("BUG: unknown JTAG command type encountered");
 343                                 exit(-1);
 344                 }
 345                 cmd = cmd->next;
 346         }
 347
 348         return vsllink_tap_execute();
 349 }
 350
 351 int vsllink_speed(int speed)
 352 {
 353         int result;
 354
 355         vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_SPEED;
 356         vsllink_usb_out_buffer[1] = (speed >> 0) & 0xff;
 357         vsllink_usb_out_buffer[2] = (speed >> 8) & 0xFF;
 358
 359         result = vsllink_usb_write(vsllink_jtag_handle, 3);
 360
 361         if (result == 3)
 362         {
 363                 return ERROR_OK;
 364         }
 365         else
 366         {
 367                 LOG_ERROR("VSLLink setting speed failed (%d)", result);
 368                 return ERROR_JTAG_DEVICE_ERROR;
 369         }
 370
 371         return ERROR_OK;
 372 }
 373
 374 int vsllink_khz(int khz, int *jtag_speed)
 375 {
 376         *jtag_speed = khz;
 377
 378         return ERROR_OK;
 379 }
 380
 381 int vsllink_speed_div(int jtag_speed, int *khz)
 382 {
 383         *khz = jtag_speed;
 384
 385         return ERROR_OK;
 386 }
 387
 388 int vsllink_register_commands(struct command_context_s *cmd_ctx)
 389 {
 390         register_command(cmd_ctx, NULL, "vsllink_usb_vid", vsllink_handle_usb_vid_command,
 391                                         COMMAND_CONFIG, NULL);
 392         register_command(cmd_ctx, NULL, "vsllink_usb_pid", vsllink_handle_usb_pid_command,
 393                                         COMMAND_CONFIG, NULL);
 394         register_command(cmd_ctx, NULL, "vsllink_usb_bulkin", vsllink_handle_usb_bulkin_command,
 395                                         COMMAND_CONFIG, NULL);
 396         register_command(cmd_ctx, NULL, "vsllink_usb_bulkout", vsllink_handle_usb_bulkout_command,
 397                                         COMMAND_CONFIG, NULL);
 398
 399         return ERROR_OK;
 400 }
 401
 402 int vsllink_init(void)
 403 {
 404         int check_cnt;
 405         int result;
 406         char version_str[100];
 407
 408         vsllink_usb_in_buffer = malloc(VSLLINK_BufferSize);
 409         vsllink_usb_out_buffer = malloc(VSLLINK_BufferSize);
 410         if ((vsllink_usb_in_buffer == NULL) || (vsllink_usb_out_buffer == NULL))
 411         {
 412                 LOG_ERROR("Not enough memory");
 413                 exit(-1);
 414         }
 415
 416         vsllink_jtag_handle = vsllink_usb_open();
 417
 418         if (vsllink_jtag_handle == 0)
 419         {
 420                 LOG_ERROR("Can't find USB JTAG Interface! Please check connection and permissions.");
 421                 return ERROR_JTAG_INIT_FAILED;
 422         }
 423
 424         check_cnt = 0;
 425         while (check_cnt < 3)
 426         {
 427                 vsllink_simple_command(VSLLINK_CMD_CONN);
 428                 result = vsllink_usb_read(vsllink_jtag_handle);
 429
 430                 if (result > 2)
 431                 {
 432                         vsllink_usb_in_buffer[result] = 0;
 433                         VSLLINK_BufferSize = vsllink_usb_in_buffer[0] + (vsllink_usb_in_buffer[1] << 8);
 434                         strncpy(version_str, (char *)vsllink_usb_in_buffer + 2, sizeof(version_str));
 435                         LOG_INFO(version_str);
 436
 437                         // free the pre-alloc memroy
 438                         free(vsllink_usb_in_buffer);
 439                         free(vsllink_usb_out_buffer);
 440                         vsllink_usb_in_buffer = NULL;
 441                         vsllink_usb_out_buffer = NULL;
 442
 443                         // alloc new memory
 444                         vsllink_usb_in_buffer = malloc(VSLLINK_BufferSize);
 445                         vsllink_usb_out_buffer = malloc(VSLLINK_BufferSize);
 446                         if ((vsllink_usb_in_buffer == NULL) || (vsllink_usb_out_buffer == NULL))
 447                         {
 448                                 LOG_ERROR("Not enough memory");
 449                                 exit(-1);
 450                         }
 451                         else
 452                         {
 453                                 LOG_INFO("buffer size for USB is %d bytes", VSLLINK_BufferSize);
 454                         }
 455                         break;
 456                 }
 457                 vsllink_simple_command(VSLLINK_CMD_DISCONN);
 458
 459                 check_cnt++;
 460         }
 461
 462         if (check_cnt == 3)
 463         {
 464                 // It's dangerout to proced
 465                 LOG_ERROR("VSLLink initial failed");
 466                 exit(-1);
 467         }
 468
 469         // Set SRST and TRST to output, Set USR1 and USR2 to input
 470         vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_PORTDIR;
 471         vsllink_usb_out_buffer[1] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST | JTAG_PINMSK_USR1 | JTAG_PINMSK_USR2;
 472         vsllink_usb_out_buffer[2] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST;
 473         result = vsllink_usb_write(vsllink_jtag_handle, 3);
 474         if (result != 3)
 475         {
 476                 LOG_ERROR("VSLLink USB send data error");
 477                 exit(-1);
 478         }
 479
 480         vsllink_reset(0, 0);
 481
 482         LOG_INFO("VSLLink JTAG Interface ready");
 483
 484         vsllink_tap_init();
 485
 486         return ERROR_OK;
 487 }
 488
 489 int vsllink_quit(void)
 490 {
 491         if ((vsllink_usb_in_buffer != NULL) && (vsllink_usb_out_buffer != NULL))
 492         {
 493                 vsllink_simple_command(VSLLINK_CMD_DISCONN);
 494                 vsllink_usb_close(vsllink_jtag_handle);
 495         }
 496
 497         if (vsllink_usb_in_buffer != NULL)
 498         {
 499                 free(vsllink_usb_in_buffer);
 500         }
 501         if (vsllink_usb_out_buffer != NULL)
 502         {
 503                 free(vsllink_usb_out_buffer);
 504         }
 505         return ERROR_OK;
 506 }
 507
 508 // when vsllink_tms_data_len > 0, vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] is the byte that need to be appended.
 509 // length of VSLLINK_CMDJTAGSEQ_TMSBYTE has been set.
 510 void VSLLINK_add_tms_from_RTI(enum tap_state state)
 511 {
 512         u8 tms_scan = VSLLINK_TAP_MOVE(TAP_IDLE, state);
 513         u16 tms2;
 514
 515         if ((cur_state != TAP_IDLE) || (state == TAP_IDLE) || (vsllink_tms_data_len <= 0) || (vsllink_tms_data_len >= 8) || (vsllink_tms_cmd_pos == NULL))
 516         {
 517                 LOG_ERROR("There MUST be some bugs in the driver");
 518                 exit(-1);
 519         }
 520
 521         tms2 = (tms_scan & VSLLINK_BIT_MSK[VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[1][tap_move_map[state]][1]]) << vsllink_tms_data_len;
 522         if (VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[1][tap_move_map[state]][0] == 1)
 523         {
 524                 tms2 |= VSLLINK_BIT_MSK[8 - vsllink_tms_data_len] << (vsllink_tms_data_len + VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[1][tap_move_map[state]][1]);
 525         }
 526         tms2 |= (tms_scan >> VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[1][tap_move_map[state]][1]) << (8 + VSLLINK_TAP_MOVE_INSERT_INSIGNIFICANT[1][tap_move_map[state]][1]);
 527
 528         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (tms2 >> 0) & 0xff;
 529         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms2 >> 8) & 0xff;
 530
 531         vsllink_tms_data_len = 0;
 532         vsllink_tms_cmd_pos = NULL;
 533 }
 534
 535 /***************************************************************************/
 536 /* Queue command implementations */
 537
 538 void vsllink_end_state(enum tap_state state)
 539 {
 540         if (tap_move_map[state] != -1)
 541         {
 542                 end_state = state;
 543         }
 544         else
 545         {
 546                 LOG_ERROR("BUG: %i is not a valid end state", state);
 547                 exit(-1);
 548         }
 549 }
 550
 551 /* Goes to the end state. */
 552 void vsllink_state_move(void)
 553 {
 554         if (vsllink_tms_data_len > 0)
 555         {
 556                 VSLLINK_add_tms_from_RTI(end_state);
 557         }
 558         else
 559         {
 560                 vsllink_tap_ensure_space(0, 2);
 561
 562                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE;
 563                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(cur_state, end_state);
 564         }
 565
 566         cur_state = end_state;
 567 }
 568
 569 // write tms from current vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx]
 570 void vsllink_add_path(int start, int num, enum tap_state *path)
 571 {
 572         int i;
 573
 574         for (i = start; i < (start + num); i++)
 575         {
 576                 if ((i & 7) == 0)
 577                 {
 578                         if (i > 0)
 579                         {
 580                                 vsllink_usb_out_buffer[++vsllink_usb_out_buffer_idx] = 0;
 581                         }
 582                         else
 583                         {
 584                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = 0;
 585                         }
 586                 }
 587
 588                 if (path[i - start] == tap_transitions[cur_state].high)
 589                 {
 590                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] |= 1 << (i & 7);
 591                 }
 592                 else
 593                 {
 594                         LOG_ERROR("BUG: %d -> %d isn't a valid TAP transition", cur_state, path[i]);
 595                         exit(-1);
 596                 }
 597
 598                 cur_state = path[i];
 599         }
 600         end_state = cur_state;
 601 }
 602
 603 void vsllink_path_move(int num_states, enum tap_state *path)
 604 {
 605         int i, tms_len, tms_cmd_pos, path_idx = 0;
 606
 607         if (vsllink_tms_data_len > 0)
 608         {
 609                 if ((vsllink_tms_data_len + num_states) < 8)
 610                 {
 611                         vsllink_add_path(vsllink_tms_data_len, num_states, path);
 612                         num_states = 0;
 613                 }
 614                 else if ((vsllink_tms_data_len + num_states) < 16)
 615                 {
 616                         if ((*vsllink_tms_cmd_pos & VSLLINK_CMDJTAGSEQ_LENMSK) \
 617                                         < VSLLINK_CMDJTAGSEQ_LENMSK)
 618                         {
 619                                 *vsllink_tms_cmd_pos++;
 620                                 vsllink_add_path(vsllink_tms_data_len, num_states, path);
 621                         }
 622                         else
 623                         {
 624                                 // need a new VSLLINK_CMDJTAGSEQ_TMSBYTE command
 625                                 // if vsllink_tms_data_len > 0, length of VSLLINK_CMDJTAGSEQ_TMSBYTE MUST be > 1(tms_len > 2)
 626                                 *vsllink_tms_cmd_pos--;
 627                                 vsllink_add_path(vsllink_tms_data_len, 8 - vsllink_tms_data_len, path);
 628                                 vsllink_usb_out_buffer_idx++;
 629                                 vsllink_tap_ensure_space(0, 3);
 630                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
 631                                 vsllink_tms_cmd_pos = vsllink_usb_out_buffer + vsllink_usb_out_buffer_idx;
 632                                 vsllink_add_path(vsllink_tms_data_len, num_states + vsllink_tms_data_len - 8, path + 8 - vsllink_tms_data_len);
 633                         }
 634                         vsllink_tms_data_len = (vsllink_tms_data_len + num_states) & 7;
 635                         num_states = 0;
 636                 }
 637                 else
 638                 {
 639                         vsllink_add_path(vsllink_tms_data_len, 16 - vsllink_tms_data_len, path);
 640                         path_idx = 16 - vsllink_tms_data_len;
 641                         vsllink_usb_out_buffer_idx++;
 642
 643                         num_states -= 16 - vsllink_tms_data_len;
 644                         path += 16 - vsllink_tms_data_len;
 645                         vsllink_tms_data_len = 0;
 646                         vsllink_tms_cmd_pos = NULL;
 647                 }
 648         }
 649
 650         if (num_states > 0)
 651         {
 652                 // Normal operation, don't need to append tms data
 653                 vsllink_tms_data_len = num_states & 7;
 654
 655                 while (num_states > 0)
 656                 {
 657                         if (num_states > ((VSLLINK_CMDJTAGSEQ_LENMSK + 1) * 8))
 658                         {
 659                                 i = (VSLLINK_CMDJTAGSEQ_LENMSK + 1) * 8;
 660                         }
 661                         else
 662                         {
 663                                 i = num_states;
 664                         }
 665                         tms_len = (i + 7) >> 3;
 666                         vsllink_tap_ensure_space(0, tms_len + 2);
 667                         tms_cmd_pos = vsllink_usb_out_buffer_idx;
 668                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | (tms_len - 1);
 669
 670                         vsllink_add_path(0, i, path + path_idx);
 671
 672                         path_idx += i;
 673                         num_states -= i;
 674                 }
 675
 676                 if (vsllink_tms_data_len > 0)
 677                 {
 678                         if (tms_len < (VSLLINK_CMDJTAGSEQ_LENMSK + 1))
 679                         {
 680                                 vsllink_usb_out_buffer[tms_cmd_pos]++;
 681                                 vsllink_usb_out_buffer = vsllink_usb_out_buffer + tms_cmd_pos;
 682                         }
 683                         else
 684                         {
 685                                 vsllink_usb_out_buffer[tms_cmd_pos]--;
 686                                 tms_len = vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
 687                                 vsllink_tap_ensure_space(0, 3);
 688                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
 689                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = tms_len;
 690                         }
 691                 }
 692         }
 693 }
 694
 695 void vsllink_runtest(int num_cycles)
 696 {
 697         int i = 0, j;
 698         int tms_len, first_tms = 0, tms_cmd_pos = 0;
 699         enum tap_state saved_end_state = end_state;
 700
 701         if (cur_state != TAP_IDLE)
 702         {
 703                 vsllink_end_state(TAP_IDLE);
 704
 705                 if (vsllink_tms_data_len > 0)
 706                 {
 707                         VSLLINK_add_tms_from_RTI(end_state);
 708                 }
 709                 else
 710                 {
 711                         first_tms = 1;
 712                 }
 713         }
 714
 715         if (vsllink_tms_data_len > 0)
 716         {
 717                 // cur_state == TAP_IDLE
 718                 if ((vsllink_tms_data_len + num_cycles) < 8)
 719                 {
 720                         vsllink_tms_data_len += num_cycles;
 721                         num_cycles = 0;
 722                 }
 723                 else if ((vsllink_tms_data_len + num_cycles) < 16)
 724                 {
 725                         if ((*vsllink_tms_cmd_pos & VSLLINK_CMDJTAGSEQ_LENMSK) \
 726                                         < VSLLINK_CMDJTAGSEQ_LENMSK)
 727                         {
 728                                 *vsllink_tms_cmd_pos++;
 729                                 vsllink_usb_out_buffer[++vsllink_usb_out_buffer_idx] = 0;
 730                         }
 731                         else
 732                         {
 733                                 // need a new VSLLINK_CMDJTAGSEQ_TMSBYTE command
 734                                 // if vsllink_tms_data_len > 0, length of VSLLINK_CMDJTAGSEQ_TMSBYTE MUST be > 1(tms_len > 2)
 735                                 *vsllink_tms_cmd_pos--;
 736                                 vsllink_tap_ensure_space(0, 3);
 737                                 vsllink_usb_out_buffer[++vsllink_usb_out_buffer_idx] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
 738                                 vsllink_tms_cmd_pos = vsllink_usb_out_buffer + vsllink_usb_out_buffer_idx;
 739                                 vsllink_usb_out_buffer[++vsllink_usb_out_buffer_idx] = 0;
 740                         }
 741                         vsllink_tms_data_len = (vsllink_tms_data_len + num_cycles) & 7;
 742                         num_cycles = 0;
 743                 }
 744                 else
 745                 {
 746                         vsllink_usb_out_buffer[++vsllink_usb_out_buffer_idx] = 0;
 747                         vsllink_usb_out_buffer_idx++;
 748
 749                         num_cycles -= 16 - vsllink_tms_data_len;
 750                         vsllink_tms_data_len = 0;
 751                         vsllink_tms_cmd_pos = NULL;
 752                 }
 753         }
 754
 755         tms_len = ((num_cycles + 7) >> 3) + first_tms;
 756         if (tms_len > 0)
 757         {
 758                 // Normal operation, don't need to append tms data
 759                 vsllink_tms_data_len = num_cycles & 7;
 760
 761                 if (vsllink_tms_data_len > 0)
 762                 {
 763                         tms_len += 1;
 764                 }
 765                 // tms_len includes the length of tms byte to append
 766
 767                 // Make sure there is enough space
 768                 // 1 more byte maybe needed for the last tms move
 769                 vsllink_tap_ensure_space(0, (tms_len / VSLLINK_CMDJTAGSEQ_LENMSK) + tms_len + 1);
 770
 771                 while(tms_len > 0)
 772                 {
 773                         if (tms_len > (VSLLINK_CMDJTAGSEQ_LENMSK + 1))
 774                         {
 775                                 i = VSLLINK_CMDJTAGSEQ_LENMSK + 1;
 776                         }
 777                         else
 778                         {
 779                                 i = tms_len;
 780                         }
 781
 782                         tms_cmd_pos = vsllink_usb_out_buffer_idx;
 783
 784                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | (i - 1);
 785
 786                         if (first_tms)
 787                         {
 788                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(cur_state, end_state);
 789                                 first_tms = 0;
 790                                 j = i - 1;
 791                         }
 792                         else
 793                         {
 794                                 j = i;
 795                         }
 796
 797                         while (j-- > 0)
 798                         {
 799                                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
 800                         }
 801
 802                         tms_len -= i;
 803                 }
 804
 805                 // post process vsllink_usb_out_buffer_idx
 806                 if (vsllink_tms_data_len > 0)
 807                 {
 808                         vsllink_usb_out_buffer_idx -= 2;
 809                 }
 810
 811                 // Set end_state
 812                 vsllink_end_state(saved_end_state);
 813                 cur_state = TAP_IDLE;
 814                 if (saved_end_state != TAP_IDLE)
 815                 {
 816                         if (vsllink_tms_data_len > 0)
 817                         {
 818                                 VSLLINK_add_tms_from_RTI(end_state);
 819                         }
 820                         else
 821                         {
 822                                 if (i < (VSLLINK_CMDJTAGSEQ_LENMSK + 1))
 823                                 {
 824                                         vsllink_usb_out_buffer[tms_cmd_pos]++;
 825                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(TAP_IDLE, end_state);
 826                                 }
 827                                 else
 828                                 {
 829                                         vsllink_tap_ensure_space(0, 2);
 830                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE;
 831                                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(TAP_IDLE, end_state);
 832                                 }
 833                         }
 834                         cur_state = saved_end_state;
 835                 }
 836
 837                 if (vsllink_tms_data_len > 0)
 838                 {
 839                         vsllink_tms_cmd_pos = vsllink_usb_out_buffer + tms_cmd_pos;
 840                 }
 841         }
 842         else
 843         {
 844                 // Set end_state if no RTI shifts
 845                 vsllink_end_state(saved_end_state);
 846                 cur_state = TAP_IDLE;
 847                 if (saved_end_state != TAP_IDLE)
 848                 {
 849                         vsllink_tap_ensure_space(0, 2);
 850                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE;
 851                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(TAP_IDLE, end_state);
 852
 853                         cur_state = saved_end_state;
 854                 }
 855         }
 856 }
 857
 858 void vsllink_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size, scan_command_t *command)
 859 {
 860         enum tap_state saved_end_state;
 861         u8 bits_left, tms_tmp, tdi_len;
 862         int i;
 863
 864         tdi_len = ((scan_size + 7) >> 3);
 865         if ((tdi_len + 7) > VSLLINK_BufferSize)
 866         {
 867                 LOG_ERROR("Your implementation of VSLLink has not enough buffer");
 868                 exit(-1);
 869         }
 870
 871         saved_end_state = end_state;
 872
 873         /* Move to appropriate scan state */
 874         vsllink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 875
 876         if (vsllink_tms_data_len > 0)
 877         {
 878                 if (cur_state == end_state)
 879                 {
 880                         *vsllink_tms_cmd_pos--;
 881                         tms_tmp = vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
 882                         vsllink_tap_ensure_space(1, tdi_len + 7);
 883
 884                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_SCAN | 1;
 885                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1) >> 0) & 0xff;
 886                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1)>> 8) & 0xff;
 887                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = tms_tmp;
 888                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = buffer[0] << (8 - vsllink_tms_data_len);
 889
 890                         for (i = 0; i < tdi_len; i++)
 891                         {
 892                                 buffer[i] >>= 8 - vsllink_tms_data_len;
 893                                 if (i != tdi_len)
 894                                 {
 895                                         buffer[i] += buffer[i + 1] << vsllink_tms_data_len;
 896                                 }
 897                         }
 898
 899                         vsllink_tap_append_scan(scan_size - vsllink_tms_data_len, buffer, command, vsllink_tms_data_len);
 900                         scan_size -= 8 - vsllink_tms_data_len;
 901                 }
 902                 else
 903                 {
 904                         VSLLINK_add_tms_from_RTI(end_state);
 905                         vsllink_tap_ensure_space(1, tdi_len + 5);
 906
 907                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_SCAN;
 908                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tdi_len >> 0) & 0xff;
 909                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tdi_len >> 8) & 0xff;
 910
 911                         vsllink_tap_append_scan(scan_size, buffer, command, 0);
 912                 }
 913         }
 914         else
 915         {
 916                 vsllink_tap_ensure_space(1, tdi_len + 7);
 917
 918                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_SCAN | 1;
 919                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1) >> 0) & 0xff;
 920                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = ((tdi_len + 1)>> 8) & 0xff;
 921                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE(cur_state, end_state);
 922                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
 923
 924                 vsllink_tap_append_scan(scan_size, buffer, command, 8);
 925         }
 926         vsllink_end_state(saved_end_state);
 927
 928         bits_left = scan_size & 0x07;
 929         cur_state = ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE;
 930
 931         if (bits_left > 0)
 932         {
 933                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 1 << (bits_left - 1);
 934         }
 935         else
 936         {
 937                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 1 << 7;
 938         }
 939
 940         if (cur_state != end_state)
 941         {
 942                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_TAP_MOVE_FROM_E1[tap_move_map[end_state]];
 943         }
 944         else
 945         {
 946                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
 947         }
 948
 949         cur_state = end_state;
 950 }
 951
 952 void vsllink_reset(int trst, int srst)
 953 {
 954         int result;
 955
 956         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
 957
 958         /* Signals are active low */
 959         vsllink_usb_out_buffer[0] = VSLLINK_CMD_SET_PORT;
 960         vsllink_usb_out_buffer[1] = JTAG_PINMSK_SRST | JTAG_PINMSK_TRST;
 961         vsllink_usb_out_buffer[2] = 0;
 962         if (srst == 0)
 963         {
 964                 vsllink_usb_out_buffer[2] |= JTAG_PINMSK_SRST;
 965         }
 966         if (trst == 0)
 967         {
 968                 vsllink_usb_out_buffer[2] |= JTAG_PINMSK_TRST;
 969         }
 970
 971         result = vsllink_usb_write(vsllink_jtag_handle, 3);
 972         if (result != 3)
 973         {
 974                 LOG_ERROR("VSLLink command VSLLINK_CMD_SET_PORT failed (%d)", result);
 975         }
 976 }
 977
 978 void vsllink_simple_command(u8 command)
 979 {
 980         int result;
 981
 982         DEBUG_JTAG_IO("0x%02x", command);
 983
 984         vsllink_usb_out_buffer[0] = command;
 985         result = vsllink_usb_write(vsllink_jtag_handle, 1);
 986
 987         if (result != 1)
 988         {
 989                 LOG_ERROR("VSLLink command 0x%02x failed (%d)", command, result);
 990         }
 991 }
 992
 993 int vsllink_handle_usb_vid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 994 {
 995         if (argc != 1) {
 996             LOG_ERROR("parameter error, should be one parameter for VID");
 997                 return ERROR_OK;
 998         }
 999
1000         vsllink_vid = strtol(args[0], NULL, 0);
1001
1002         return ERROR_OK;
1003 }
1004
1005 int vsllink_handle_usb_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1006 {
1007         if (argc != 1) {
1008             LOG_ERROR("parameter error, should be one parameter for PID");
1009                 return ERROR_OK;
1010         }
1011
1012         vsllink_pid = strtol(args[0], NULL, 0);
1013
1014         return ERROR_OK;
1015 }
1016
1017 int vsllink_handle_usb_bulkin_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1018 {
1019         if (argc != 1) {
1020             LOG_ERROR("parameter error, should be one parameter for BULKIN endpoint");
1021                 return ERROR_OK;
1022         }
1023
1024         vsllink_bulkin = strtol(args[0], NULL, 0) | 0x80;
1025
1026         return ERROR_OK;
1027 }
1028
1029 int vsllink_handle_usb_bulkout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1030 {
1031         if (argc != 1) {
1032             LOG_ERROR("parameter error, should be one parameter for BULKOUT endpoint");
1033                 return ERROR_OK;
1034         }
1035
1036         vsllink_bulkout = strtol(args[0], NULL, 0);
1037
1038         return ERROR_OK;
1039 }
1040
1041 /***************************************************************************/
1042 /* VSLLink tap functions */
1043
1044 typedef struct
1045 {
1046         int length; /* Number of bits to read */
1047         int offset;
1048         scan_command_t *command; /* Corresponding scan command */
1049         u8 *buffer;
1050 } pending_scan_result_t;
1051
1052 #define MAX_PENDING_SCAN_RESULTS 256
1053
1054 static int pending_scan_results_length;
1055 static pending_scan_result_t pending_scan_results_buffer[MAX_PENDING_SCAN_RESULTS];
1056
1057 void vsllink_tap_init(void)
1058 {
1059         vsllink_usb_out_buffer_idx = 0;
1060         vsllink_usb_in_want_length = 0;
1061         pending_scan_results_length = 0;
1062 }
1063
1064 void vsllink_tap_ensure_space(int scans, int bytes)
1065 {
1066         int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
1067         int available_bytes = VSLLINK_BufferSize - vsllink_usb_out_buffer_idx;
1068
1069         if (scans > available_scans || bytes > available_bytes)
1070         {
1071                 vsllink_tap_execute();
1072                 vsllink_usb_out_buffer[0] = VSLLINK_CMD_HW_JTAGSEQCMD;
1073                 vsllink_usb_out_buffer_idx = 3;
1074         }
1075 }
1076
1077 void vsllink_tap_append_scan(int length, u8 *buffer, scan_command_t *command, int offset)
1078 {
1079         pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[pending_scan_results_length];
1080         int i;
1081
1082         if (offset > 0)
1083         {
1084                 vsllink_usb_in_want_length += ((length + 7) >> 3) + 1;
1085         }
1086         else
1087         {
1088                 vsllink_usb_in_want_length += (length + 7) >> 3;
1089         }
1090         pending_scan_result->length = length;
1091         pending_scan_result->offset = offset;
1092         pending_scan_result->command = command;
1093         pending_scan_result->buffer = buffer;
1094
1095         for (i = 0; i < ((length + 7) >> 3); i++)
1096         {
1097                 vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = buffer[i];
1098         }
1099
1100         pending_scan_results_length++;
1101 }
1102
1103 /* Pad and send a tap sequence to the device, and receive the answer.
1104  * For the purpose of padding we assume that we are in idle or pause state. */
1105 int vsllink_tap_execute(void)
1106 {
1107         int i;
1108         int result;
1109         int first = 0;
1110
1111         if (vsllink_tms_data_len > 0)
1112         {
1113                 if (vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] & (1 << (vsllink_tms_data_len - 1)))
1114                 {
1115                         // last tms bit is '1'
1116                         // the only possible state is TLR, no need to control the number of shifts in RLT
1117                         // There MUST be some errors in the code
1118                         LOG_ERROR("last tms bit is '1'");
1119                         exit(-1);
1120                 }
1121                 else
1122                 {
1123                         // last tms bit is '0'
1124                         vsllink_usb_out_buffer_idx++;
1125                         vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = 0;
1126                         vsllink_tms_data_len = 0;
1127                 }
1128         }
1129
1130         if (vsllink_usb_out_buffer_idx > 3)
1131         {
1132                 if (vsllink_usb_out_buffer[0] == VSLLINK_CMD_HW_JTAGSEQCMD)
1133                 {
1134                         vsllink_usb_out_buffer[1] = (vsllink_usb_out_buffer_idx >> 0) & 0xff;
1135                         vsllink_usb_out_buffer[2] = (vsllink_usb_out_buffer_idx >> 8) & 0xff;
1136                 }
1137
1138                 result = vsllink_usb_message(vsllink_jtag_handle, vsllink_usb_out_buffer_idx, vsllink_usb_in_want_length);
1139
1140                 if (result == vsllink_usb_in_want_length)
1141                 {
1142                         for (i = 0; i < pending_scan_results_length; i++)
1143                         {
1144                                 pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[i];
1145                                 u8 *buffer = pending_scan_result->buffer;
1146                                 int length = pending_scan_result->length;
1147                                 int offset = pending_scan_result->offset;
1148                                 scan_command_t *command = pending_scan_result->command;
1149
1150                                 /* Copy to buffer */
1151                                 buf_set_buf(vsllink_usb_in_buffer, first * 8 + offset, buffer, 0, length);
1152                                 first += (length + offset + 7) >> 3;
1153
1154                                 DEBUG_JTAG_IO("JTAG scan read(%d bits):", length);
1155 #ifdef _DEBUG_JTAG_IO_
1156                                 vsllink_debug_buffer(buffer, (length + 7) >> 3);
1157 #endif
1158
1159                                 if (jtag_read_buffer(buffer, command) != ERROR_OK)
1160                                 {
1161                                         vsllink_tap_init();
1162                                         return ERROR_JTAG_QUEUE_FAILED;
1163                                 }
1164
1165                                 if (pending_scan_result->buffer != NULL)
1166                                 {
1167                                         free(pending_scan_result->buffer);
1168                                         pending_scan_result->buffer = NULL;
1169                                 }
1170                         }
1171                 }
1172                 else
1173                 {
1174                         LOG_ERROR("vsllink_tap_execute, wrong result %d, expected %d", result, vsllink_usb_in_want_length);
1175                         return ERROR_JTAG_QUEUE_FAILED;
1176                 }
1177
1178                 vsllink_tap_init();
1179         }
1180
1181         return ERROR_OK;
1182 }
1183
1184 /*****************************************************************************/
1185 /* VSLLink USB low-level functions */
1186
1187 vsllink_jtag_t* vsllink_usb_open(void)
1188 {
1189         struct usb_bus *busses;
1190         struct usb_bus *bus;
1191         struct usb_device *dev;
1192
1193         vsllink_jtag_t *result;
1194
1195         result = (vsllink_jtag_t*) malloc(sizeof(vsllink_jtag_t));
1196
1197         usb_init();
1198         usb_find_busses();
1199         usb_find_devices();
1200
1201         busses = usb_get_busses();
1202
1203         /* find vsllink_jtag device in usb bus */
1204
1205         for (bus = busses; bus; bus = bus->next)
1206         {
1207                 for (dev = bus->devices; dev; dev = dev->next)
1208                 {
1209                         if ((dev->descriptor.idVendor == vsllink_vid) && (dev->descriptor.idProduct == vsllink_pid))
1210                         {
1211                                 result->usb_handle = usb_open(dev);
1212
1213                                 /* usb_set_configuration required under win32 */
1214                                 usb_set_configuration(result->usb_handle, dev->config[0].bConfigurationValue);
1215                                 usb_claim_interface(result->usb_handle, 0);
1216
1217 #if 0
1218                                 /*
1219                                  * This makes problems under Mac OS X. And is not needed
1220                                  * under Windows. Hopefully this will not break a linux build
1221                                  */
1222                                 usb_set_altinterface(result->usb_handle, 0);
1223 #endif
1224                                 return result;
1225                         }
1226                 }
1227         }
1228
1229         free(result);
1230         return NULL;
1231 }
1232
1233 void vsllink_usb_close(vsllink_jtag_t *vsllink_jtag)
1234 {
1235         usb_close(vsllink_jtag->usb_handle);
1236         free(vsllink_jtag);
1237 }
1238
1239 /* Send a message and receive the reply. */
1240 int vsllink_usb_message(vsllink_jtag_t *vsllink_jtag, int out_length, int in_length)
1241 {
1242         int result;
1243
1244         result = vsllink_usb_write(vsllink_jtag, out_length);
1245         if (result == out_length)
1246         {
1247                 if (in_length > 0)
1248                 {
1249                         result = vsllink_usb_read(vsllink_jtag);
1250                         if (result == in_length )
1251                         {
1252                                 return result;
1253                         }
1254                         else
1255                         {
1256                                 LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)", in_length, result);
1257                                 return -1;
1258                         }
1259                 }
1260                 return 0;
1261         }
1262         else
1263         {
1264                 LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)", out_length, result);
1265                 return -1;
1266         }
1267 }
1268
1269 /* Write data from out_buffer to USB. */
1270 int vsllink_usb_write(vsllink_jtag_t *vsllink_jtag, int out_length)
1271 {
1272         int result;
1273
1274         if (out_length > VSLLINK_BufferSize)
1275         {
1276                 LOG_ERROR("vsllink_jtag_write illegal out_length=%d (max=%d)", out_length, VSLLINK_BufferSize);
1277                 return -1;
1278         }
1279
1280         result = usb_bulk_write(vsllink_jtag->usb_handle, vsllink_bulkout, \
1281                 (char *)vsllink_usb_out_buffer, out_length, VSLLINK_USB_TIMEOUT);
1282
1283         DEBUG_JTAG_IO("vsllink_usb_write, out_length = %d, result = %d", out_length, result);
1284
1285 #ifdef _DEBUG_USB_COMMS_
1286         LOG_DEBUG("USB out:");
1287         vsllink_debug_buffer(vsllink_usb_out_buffer, out_length);
1288 #endif
1289
1290 #ifdef _VSLLINK_IN_DEBUG_MODE_
1291         usleep(100000);
1292 #endif
1293
1294         return result;
1295 }
1296
1297 /* Read data from USB into in_buffer. */
1298 int vsllink_usb_read(vsllink_jtag_t *vsllink_jtag)
1299 {
1300         int result = usb_bulk_read(vsllink_jtag->usb_handle, vsllink_bulkin, \
1301                 (char *)vsllink_usb_in_buffer, VSLLINK_BufferSize, VSLLINK_USB_TIMEOUT);
1302
1303         DEBUG_JTAG_IO("vsllink_usb_read, result = %d", result);
1304
1305 #ifdef _DEBUG_USB_COMMS_
1306         LOG_DEBUG("USB in:");
1307         vsllink_debug_buffer(vsllink_usb_in_buffer, result);
1308 #endif
1309         return result;
1310 }
1311
1312 #define BYTES_PER_LINE  16
1313
1314 void vsllink_debug_buffer(u8 *buffer, int length)
1315 {
1316         char line[81];
1317         char s[4];
1318         int i;
1319         int j;
1320
1321         for (i = 0; i < length; i += BYTES_PER_LINE)
1322         {
1323                 snprintf(line, 5, "%04x", i);
1324                 for (j = i; j < i + BYTES_PER_LINE && j < length; j++)
1325                 {
1326                         snprintf(s, 4, " %02x", buffer[j]);
1327                         strcat(line, s);
1328                 }
1329                 LOG_DEBUG(line);
1330         }
1331 }