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

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