git.sur5r.net Git - openocd/blob - src/jtag/jlink.c

   1 /***************************************************************************
   2  *   Copyright (C) 2007 by Juergen Stuber <juergen@jstuber.net>            *
   3  *   based on Dominic Rath's and Benedikt Sauter's usbprog.c               *
   4  *                                                                         *
   5  *   Copyright (C) 2008 by Spencer Oliver                                  *
   6  *   spen@spen-soft.co.uk                                                  *
   7  *                                                                         *
   8  *   This program is free software; you can redistribute it and/or modify  *
   9  *   it under the terms of the GNU General Public License as published by  *
  10  *   the Free Software Foundation; either version 2 of the License, or     *
  11  *   (at your option) any later version.                                   *
  12  *                                                                         *
  13  *   This program is distributed in the hope that it will be useful,       *
  14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  16  *   GNU General Public License for more details.                          *
  17  *                                                                         *
  18  *   You should have received a copy of the GNU General Public License     *
  19  *   along with this program; if not, write to the                         *
  20  *   Free Software Foundation, Inc.,                                       *
  21  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  22  ***************************************************************************/
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include "config.h"
  26 #endif
  27
  28 #include "replacements.h"
  29
  30 #include "jtag.h"
  31
  32 #include <usb.h>
  33 #include <string.h>
  34
  35 #include "log.h"
  36
  37 #define VID 0x1366
  38 #define PID 0x0101
  39
  40 #define JLINK_WRITE_ENDPOINT    0x02
  41 #define JLINK_READ_ENDPOINT             0x81
  42
  43 #define JLINK_USB_TIMEOUT               1000
  44
  45 #define JLINK_IN_BUFFER_SIZE                    8192
  46 #define JLINK_OUT_BUFFER_SIZE                   8192
  47 #define JLINK_EMU_RESULT_BUFFER_SIZE    64
  48
  49 /* Global USB buffers */
  50 static u8 usb_in_buffer[JLINK_IN_BUFFER_SIZE];
  51 static u8 usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
  52 static u8 usb_emu_result_buffer[JLINK_EMU_RESULT_BUFFER_SIZE];
  53
  54 /* Constants for JLink command */
  55 #define EMU_CMD_VERSION     0x01
  56 #define EMU_CMD_SET_SPEED   0x05
  57 #define EMU_CMD_GET_STATE   0x07
  58 #define EMU_CMD_HW_JTAG3    0xcf
  59 #define EMU_CMD_HW_RESET0   0xdc
  60 #define EMU_CMD_HW_RESET1   0xdd
  61 #define EMU_CMD_HW_TRST0    0xde
  62 #define EMU_CMD_HW_TRST1    0xdf
  63
  64 /* max speed 12MHz v5.0 jlink */
  65 #define JLINK_MAX_SPEED 12000
  66
  67 /* External interface functions */
  68 static int jlink_execute_queue(void);
  69 static int jlink_speed(int speed);
  70 static int jlink_speed_div(int speed, int* khz);
  71 static int jlink_khz(int khz, int *jtag_speed);
  72 static int jlink_register_commands(struct command_context_s *cmd_ctx);
  73 static int jlink_init(void);
  74 static int jlink_quit(void);
  75
  76 /* CLI command handler functions */
  77 static int jlink_handle_jlink_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  78
  79 /* Queue command functions */
  80 static void jlink_end_state(tap_state_t state);
  81 static void jlink_state_move(void);
  82 static void jlink_path_move(int num_states, tap_state_t *path);
  83 static void jlink_runtest(int num_cycles);
  84 static void jlink_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size, scan_command_t *command);
  85 static void jlink_reset(int trst, int srst);
  86 static void jlink_simple_command(u8 command);
  87 static int jlink_get_status(void);
  88
  89 /* J-Link tap buffer functions */
  90 static void jlink_tap_init(void);
  91 static int jlink_tap_execute(void);
  92 static void jlink_tap_ensure_space(int scans, int bits);
  93 static void jlink_tap_append_step(int tms, int tdi);
  94 static void jlink_tap_append_scan(int length, u8 *buffer, scan_command_t *command);
  95
  96 /* Jlink lowlevel functions */
  97 typedef struct jlink_jtag
  98 {
  99         struct usb_dev_handle* usb_handle;
 100 } jlink_jtag_t;
 101
 102 static jlink_jtag_t *jlink_usb_open(void);
 103 static void jlink_usb_close(jlink_jtag_t *jlink_jtag);
 104 static int jlink_usb_message(jlink_jtag_t *jlink_jtag, int out_length, int in_length);
 105 static int jlink_usb_write(jlink_jtag_t *jlink_jtag, int out_length);
 106 static int jlink_usb_read(jlink_jtag_t *jlink_jtag, int expected_size);
 107 static int jlink_usb_read_emu_result(jlink_jtag_t *jlink_jtag);
 108
 109 /* helper functions */
 110 static int jlink_get_version_info(void);
 111
 112 #ifdef _DEBUG_USB_COMMS_
 113 static void jlink_debug_buffer(u8 *buffer, int length);
 114 #endif
 115
 116 static jlink_jtag_t* jlink_jtag_handle;
 117
 118 /***************************************************************************/
 119 /* External interface implementation */
 120
 121 jtag_interface_t jlink_interface =
 122 {
 123         .name = "jlink",
 124         .execute_queue = jlink_execute_queue,
 125         .speed = jlink_speed,
 126         .speed_div = jlink_speed_div,
 127         .khz = jlink_khz,
 128         .register_commands = jlink_register_commands,
 129         .init = jlink_init,
 130         .quit = jlink_quit
 131 };
 132
 133 static void jlink_execute_end_state(jtag_command_t *cmd)
 134 {
 135         DEBUG_JTAG_IO("end_state: %i", cmd->cmd.end_state->end_state);
 136
 137         if (cmd->cmd.end_state->end_state != TAP_INVALID)
 138                 jlink_end_state(cmd->cmd.end_state->end_state);
 139 }
 140
 141 static void jlink_execute_runtest(jtag_command_t *cmd)
 142 {
 143         DEBUG_JTAG_IO("runtest %i cycles, end in %i",
 144                         cmd->cmd.runtest->num_cycles,
 145                         cmd->cmd.runtest->end_state);
 146
 147         if (cmd->cmd.runtest->end_state != TAP_INVALID)
 148                 jlink_end_state(cmd->cmd.runtest->end_state);
 149
 150         jlink_runtest(cmd->cmd.runtest->num_cycles);
 151 }
 152
 153 static void jlink_execute_statemove(jtag_command_t *cmd)
 154 {
 155         DEBUG_JTAG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
 156
 157         if (cmd->cmd.statemove->end_state != TAP_INVALID)
 158         {
 159                 jlink_end_state(cmd->cmd.statemove->end_state);
 160         }
 161         jlink_state_move();
 162 }
 163
 164 static void jlink_execute_pathmove(jtag_command_t *cmd)
 165 {
 166         DEBUG_JTAG_IO("pathmove: %i states, end in %i",
 167                 cmd->cmd.pathmove->num_states,
 168                 cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
 169
 170         jlink_path_move(cmd->cmd.pathmove->num_states,
 171                         cmd->cmd.pathmove->path);
 172 }
 173
 174 static void jlink_execute_scan(jtag_command_t *cmd)
 175 {
 176         int scan_size;
 177         enum scan_type type;
 178         u8 *buffer;
 179
 180         DEBUG_JTAG_IO("scan end in %i", cmd->cmd.scan->end_state);
 181
 182         if (cmd->cmd.scan->end_state != TAP_INVALID)
 183                 jlink_end_state(cmd->cmd.scan->end_state);
 184
 185         scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
 186         DEBUG_JTAG_IO("scan input, length = %d", scan_size);
 187
 188 #ifdef _DEBUG_USB_COMMS_
 189         jlink_debug_buffer(buffer, (scan_size + 7) / 8);
 190 #endif
 191         type = jtag_scan_type(cmd->cmd.scan);
 192         jlink_scan(cmd->cmd.scan->ir_scan,
 193                         type, buffer, scan_size, cmd->cmd.scan);
 194 }
 195
 196 static void jlink_execute_reset(jtag_command_t *cmd)
 197 {
 198         DEBUG_JTAG_IO("reset trst: %i srst %i",
 199                         cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 200
 201         jlink_tap_execute();
 202
 203         if (cmd->cmd.reset->trst == 1)
 204                 tap_set_state(TAP_RESET);
 205
 206         jlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 207 }
 208
 209 static void jlink_execute_sleep(jtag_command_t *cmd)
 210 {
 211         DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
 212         jlink_tap_execute();
 213         jtag_sleep(cmd->cmd.sleep->us);
 214 }
 215
 216 static void jlink_execute_command(jtag_command_t *cmd)
 217 {
 218         switch (cmd->type)
 219         {
 220         case JTAG_END_STATE: jlink_execute_end_state(cmd); break;
 221         case JTAG_RUNTEST:   jlink_execute_runtest(cmd); break;
 222         case JTAG_STATEMOVE: jlink_execute_statemove(cmd); break;
 223         case JTAG_PATHMOVE:  jlink_execute_pathmove(cmd); break;
 224         case JTAG_SCAN:      jlink_execute_scan(cmd); break;
 225         case JTAG_RESET:     jlink_execute_reset(cmd); break;
 226         case JTAG_SLEEP:     jlink_execute_sleep(cmd); break;
 227         default:
 228                 LOG_ERROR("BUG: unknown JTAG command type encountered");
 229                 exit(-1);
 230         }
 231 }
 232
 233 static int jlink_execute_queue(void)
 234 {
 235         jtag_command_t *cmd = jtag_command_queue;
 236
 237         while (cmd != NULL)
 238         {
 239                 jlink_execute_command(cmd);
 240                 cmd = cmd->next;
 241         }
 242
 243         return jlink_tap_execute();
 244 }
 245
 246 /* Sets speed in kHz. */
 247 static int jlink_speed(int speed)
 248 {
 249         int result;
 250
 251         if (speed > JLINK_MAX_SPEED)
 252         {
 253                 LOG_INFO("Ignoring speed request: %dkHz exceeds %dkHz maximum",
 254                                 speed, JLINK_MAX_SPEED);
 255                 return ERROR_OK;
 256         }
 257
 258         /* check for RTCK setting */
 259         if (speed == 0)
 260                 speed = -1;
 261
 262         usb_out_buffer[0] = EMU_CMD_SET_SPEED;
 263         usb_out_buffer[1] = (speed >> 0) & 0xff;
 264         usb_out_buffer[2] = (speed >> 8) & 0xff;
 265
 266         result = jlink_usb_write(jlink_jtag_handle, 3);
 267         if (result != 3)
 268         {
 269                 LOG_ERROR("J-Link setting speed failed (%d)", result);
 270                 return ERROR_JTAG_DEVICE_ERROR;
 271         }
 272
 273         return ERROR_OK;
 274 }
 275
 276 static int jlink_speed_div(int speed, int* khz)
 277 {
 278         *khz = speed;
 279
 280         return ERROR_OK;
 281 }
 282
 283 static int jlink_khz(int khz, int *jtag_speed)
 284 {
 285         *jtag_speed = khz;
 286
 287         return ERROR_OK;
 288 }
 289
 290 static int jlink_register_commands(struct command_context_s *cmd_ctx)
 291 {
 292         register_command(cmd_ctx, NULL, "jlink_info", jlink_handle_jlink_info_command, COMMAND_EXEC,
 293                 "query jlink info");
 294         return ERROR_OK;
 295 }
 296
 297 static int jlink_init(void)
 298 {
 299         int check_cnt;
 300
 301         jlink_jtag_handle = jlink_usb_open();
 302
 303         if (jlink_jtag_handle == 0)
 304         {
 305                 LOG_ERROR("Cannot find jlink Interface! Please check connection and permissions.");
 306                 return ERROR_JTAG_INIT_FAILED;
 307         }
 308
 309         check_cnt = 0;
 310         while (check_cnt < 3)
 311         {
 312                 if (jlink_get_version_info() == ERROR_OK)
 313                 {
 314                         /* attempt to get status */
 315                         jlink_get_status();
 316                         break;
 317                 }
 318
 319                 check_cnt++;
 320         }
 321
 322         if (check_cnt == 3)
 323         {
 324                 LOG_INFO("J-Link initial read failed, don't worry");
 325         }
 326
 327         LOG_INFO("J-Link JTAG Interface ready");
 328
 329         jlink_reset(0, 0);
 330         jlink_tap_init();
 331
 332         return ERROR_OK;
 333 }
 334
 335 static int jlink_quit(void)
 336 {
 337         jlink_usb_close(jlink_jtag_handle);
 338         return ERROR_OK;
 339 }
 340
 341 /***************************************************************************/
 342 /* Queue command implementations */
 343
 344 static void jlink_end_state(tap_state_t state)
 345 {
 346         if (tap_is_state_stable(state))
 347         {
 348                 tap_set_end_state(state);
 349         }
 350         else
 351         {
 352                 LOG_ERROR("BUG: %i is not a valid end state", state);
 353                 exit(-1);
 354         }
 355 }
 356
 357 /* Goes to the end state. */
 358 static void jlink_state_move(void)
 359 {
 360         int i;
 361         int tms = 0;
 362         u8 tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 363
 364         for (i = 0; i < 7; i++)
 365         {
 366                 tms = (tms_scan >> i) & 1;
 367                 jlink_tap_append_step(tms, 0);
 368         }
 369
 370         tap_set_state(tap_get_end_state());
 371 }
 372
 373 static void jlink_path_move(int num_states, tap_state_t *path)
 374 {
 375         int i;
 376
 377         for (i = 0; i < num_states; i++)
 378         {
 379                 if (path[i] == tap_state_transition(tap_get_state(), false))
 380                 {
 381                         jlink_tap_append_step(0, 0);
 382                 }
 383                 else if (path[i] == tap_state_transition(tap_get_state(), true))
 384                 {
 385                         jlink_tap_append_step(1, 0);
 386                 }
 387                 else
 388                 {
 389                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(tap_get_state()), tap_state_name(path[i]));
 390                         exit(-1);
 391                 }
 392
 393                 tap_set_state(path[i]);
 394         }
 395
 396         tap_set_end_state(tap_get_state());
 397 }
 398
 399 static void jlink_runtest(int num_cycles)
 400 {
 401         int i;
 402
 403         tap_state_t saved_end_state = tap_get_end_state();
 404
 405         /* only do a state_move when we're not already in IDLE */
 406         if (tap_get_state() != TAP_IDLE)
 407         {
 408                 jlink_end_state(TAP_IDLE);
 409                 jlink_state_move();
 410         }
 411
 412         /* execute num_cycles */
 413         for (i = 0; i < num_cycles; i++)
 414         {
 415                 jlink_tap_append_step(0, 0);
 416         }
 417
 418         /* finish in end_state */
 419         jlink_end_state(saved_end_state);
 420         if (tap_get_state() != tap_get_end_state())
 421         {
 422                 jlink_state_move();
 423         }
 424 }
 425
 426 static void jlink_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size, scan_command_t *command)
 427 {
 428         tap_state_t saved_end_state;
 429
 430         jlink_tap_ensure_space(1, scan_size + 8);
 431
 432         saved_end_state = tap_get_end_state();
 433
 434         /* Move to appropriate scan state */
 435         jlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 436
 437         /* Only move if we're not already there */
 438         if (tap_get_state() != tap_get_end_state())
 439                 jlink_state_move();
 440
 441         jlink_end_state(saved_end_state);
 442
 443         /* Scan */
 444         jlink_tap_append_scan(scan_size, buffer, command);
 445
 446         /* We are in Exit1, go to Pause */
 447         jlink_tap_append_step(0, 0);
 448
 449         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 450
 451         if (tap_get_state() != tap_get_end_state())
 452         {
 453                 jlink_state_move();
 454         }
 455 }
 456
 457 static void jlink_reset(int trst, int srst)
 458 {
 459         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
 460
 461         /* Signals are active low */
 462         if (srst == 0)
 463         {
 464                 jlink_simple_command(EMU_CMD_HW_RESET1);
 465         }
 466         else if (srst == 1)
 467         {
 468                 jlink_simple_command(EMU_CMD_HW_RESET0);
 469         }
 470
 471         if (trst == 0)
 472         {
 473                 jlink_simple_command(EMU_CMD_HW_TRST1);
 474         }
 475         else if (trst == 1)
 476         {
 477                 jlink_simple_command(EMU_CMD_HW_TRST0);
 478         }
 479 }
 480
 481 static void jlink_simple_command(u8 command)
 482 {
 483         int result;
 484
 485         DEBUG_JTAG_IO("0x%02x", command);
 486
 487         usb_out_buffer[0] = command;
 488         result = jlink_usb_write(jlink_jtag_handle, 1);
 489
 490         if (result != 1)
 491         {
 492                 LOG_ERROR("J-Link command 0x%02x failed (%d)", command, result);
 493         }
 494 }
 495
 496 static int jlink_get_status(void)
 497 {
 498         int result;
 499
 500         jlink_simple_command(EMU_CMD_GET_STATE);
 501
 502         result = jlink_usb_read(jlink_jtag_handle, 8);
 503         if (result != 8)
 504         {
 505                 LOG_ERROR("J-Link command EMU_CMD_GET_STATE failed (%d)\n", result);
 506                 return ERROR_JTAG_DEVICE_ERROR;
 507         }
 508
 509         int vref = usb_in_buffer[0] + (usb_in_buffer[1] << 8);
 510         LOG_INFO("Vref = %d.%d TCK = %d TDI = %d TDO = %d TMS = %d SRST = %d TRST = %d\n", \
 511                 vref / 1000, vref % 1000, \
 512                 usb_in_buffer[2], usb_in_buffer[3], usb_in_buffer[4], \
 513                 usb_in_buffer[5], usb_in_buffer[6], usb_in_buffer[7]);
 514
 515         if (vref < 1500)
 516                 LOG_ERROR("Vref too low. Check Target Power\n");
 517
 518         return ERROR_OK;
 519 }
 520
 521 static int jlink_get_version_info(void)
 522 {
 523         int result;
 524         int len;
 525
 526         /* query hardware version */
 527         jlink_simple_command(EMU_CMD_VERSION);
 528
 529         result = jlink_usb_read(jlink_jtag_handle, 2);
 530         if (2 != result)
 531         {
 532                 LOG_ERROR("J-Link command EMU_CMD_VERSION failed (%d)\n",
 533                                 result);
 534                 return ERROR_JTAG_DEVICE_ERROR;
 535         }
 536
 537         len = buf_get_u32(usb_in_buffer, 0, 16);
 538         result = jlink_usb_read(jlink_jtag_handle, len);
 539         if (result != len)
 540         {
 541                 LOG_ERROR("J-Link command EMU_CMD_VERSION failed (%d)\n",
 542                                 result);
 543                 return ERROR_JTAG_DEVICE_ERROR;
 544         }
 545
 546         usb_in_buffer[result] = 0;
 547         LOG_INFO("%s", (char *)usb_in_buffer);
 548
 549         return ERROR_OK;
 550 }
 551
 552 static int jlink_handle_jlink_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 553 {
 554         if (jlink_get_version_info() == ERROR_OK)
 555         {
 556                 /* attempt to get status */
 557                 jlink_get_status();
 558         }
 559
 560         return ERROR_OK;
 561 }
 562
 563 /***************************************************************************/
 564 /* J-Link tap functions */
 565
 566 /* 2048 is the max value we can use here */
 567 #define JLINK_TAP_BUFFER_SIZE 2048
 568
 569 static unsigned tap_length;
 570 static u8 tms_buffer[JLINK_TAP_BUFFER_SIZE];
 571 static u8 tdi_buffer[JLINK_TAP_BUFFER_SIZE];
 572 static u8 tdo_buffer[JLINK_TAP_BUFFER_SIZE];
 573
 574 typedef struct
 575 {
 576         int first;      /* First bit position in tdo_buffer to read */
 577         int length; /* Number of bits to read */
 578         scan_command_t *command; /* Corresponding scan command */
 579         u8 *buffer;
 580 } pending_scan_result_t;
 581
 582 #define MAX_PENDING_SCAN_RESULTS 256
 583
 584 static int pending_scan_results_length;
 585 static pending_scan_result_t pending_scan_results_buffer[MAX_PENDING_SCAN_RESULTS];
 586
 587 static int last_tms;
 588
 589 static void jlink_tap_init(void)
 590 {
 591         tap_length = 0;
 592         pending_scan_results_length = 0;
 593 }
 594
 595 static void jlink_tap_ensure_space(int scans, int bits)
 596 {
 597         int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
 598         int available_bits = JLINK_TAP_BUFFER_SIZE * 8 - tap_length;
 599
 600         if (scans > available_scans || bits > available_bits)
 601         {
 602                 jlink_tap_execute();
 603         }
 604 }
 605
 606 static void jlink_tap_append_step(int tms, int tdi)
 607 {
 608         last_tms = tms;
 609         int index = tap_length / 8;
 610
 611         if (index >= JLINK_TAP_BUFFER_SIZE)
 612         {
 613                 LOG_ERROR("jlink_tap_append_step: overflow");
 614                 exit(-1);
 615         }
 616
 617         int bit_index = tap_length % 8;
 618         u8 bit = 1 << bit_index;
 619
 620         if (tms)
 621                 tms_buffer[index] |= bit;
 622         else
 623                 tms_buffer[index] &= ~bit;
 624
 625         if (tdi)
 626                 tdi_buffer[index] |= bit;
 627         else
 628                 tdi_buffer[index] &= ~bit;
 629
 630         tap_length++;
 631 }
 632
 633 static void jlink_tap_append_scan(int length, u8 *buffer, scan_command_t *command)
 634 {
 635         pending_scan_result_t *pending_scan_result =
 636                 &pending_scan_results_buffer[pending_scan_results_length];
 637         int i;
 638
 639         pending_scan_result->first = tap_length;
 640         pending_scan_result->length = length;
 641         pending_scan_result->command = command;
 642         pending_scan_result->buffer = buffer;
 643
 644         for (i = 0; i < length; i++)
 645         {
 646                 int tms = i < length - 1 ? 0 : 1;
 647                 int tdi = buffer[i / 8] & (1 << (i % 8));
 648                 jlink_tap_append_step(tms, tdi);
 649         }
 650         pending_scan_results_length++;
 651 }
 652
 653 /* Pad and send a tap sequence to the device, and receive the answer.
 654  * For the purpose of padding we assume that we are in idle or pause state. */
 655 static int jlink_tap_execute(void)
 656 {
 657         int byte_length;
 658         int tms_offset;
 659         int tdi_offset;
 660         int i;
 661         int result;
 662
 663         if (!tap_length)
 664                 return ERROR_OK;
 665
 666         /* Pad last byte so that tap_length is divisible by 8 */
 667         while (tap_length % 8 != 0)
 668         {
 669                 /* More of the last TMS value keeps us in the same state,
 670                  * analogous to free-running JTAG interfaces. */
 671                 jlink_tap_append_step(last_tms, 0);
 672         }
 673
 674         byte_length = tap_length / 8;
 675
 676         usb_out_buffer[0] = EMU_CMD_HW_JTAG3;
 677         usb_out_buffer[1] = 0;
 678         usb_out_buffer[2] = (tap_length >> 0) & 0xff;
 679         usb_out_buffer[3] = (tap_length >> 8) & 0xff;
 680
 681         tms_offset = 4;
 682         for (i = 0; i < byte_length; i++)
 683         {
 684                 usb_out_buffer[tms_offset + i] = tms_buffer[i];
 685         }
 686
 687         tdi_offset = tms_offset + byte_length;
 688         for (i = 0; i < byte_length; i++)
 689         {
 690                 usb_out_buffer[tdi_offset + i] = tdi_buffer[i];
 691         }
 692
 693         result = jlink_usb_message(jlink_jtag_handle, 4 + 2 * byte_length, byte_length);
 694
 695         if (result != byte_length)
 696         {
 697                 LOG_ERROR("jlink_tap_execute, wrong result %d (expected %d)",
 698                                 result, byte_length);
 699                 return ERROR_JTAG_QUEUE_FAILED;
 700         }
 701
 702         for (i = 0; i < byte_length; i++)
 703                 tdo_buffer[i] = usb_in_buffer[i];
 704
 705         for (i = 0; i < pending_scan_results_length; i++)
 706         {
 707                 pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[i];
 708                 u8 *buffer = pending_scan_result->buffer;
 709                 int length = pending_scan_result->length;
 710                 int first = pending_scan_result->first;
 711                 scan_command_t *command = pending_scan_result->command;
 712
 713                 /* Copy to buffer */
 714                 buf_set_buf(tdo_buffer, first, buffer, 0, length);
 715
 716                 DEBUG_JTAG_IO("pending scan result, length = %d", length);
 717
 718 #ifdef _DEBUG_USB_COMMS_
 719                 jlink_debug_buffer(buffer, byte_length);
 720 #endif
 721
 722                 if (jtag_read_buffer(buffer, command) != ERROR_OK)
 723                 {
 724                         jlink_tap_init();
 725                         return ERROR_JTAG_QUEUE_FAILED;
 726                 }
 727
 728                 if (pending_scan_result->buffer != NULL)
 729                 {
 730                         free(pending_scan_result->buffer);
 731                 }
 732         }
 733
 734         jlink_tap_init();
 735
 736         return ERROR_OK;
 737 }
 738
 739 /*****************************************************************************/
 740 /* JLink USB low-level functions */
 741
 742 static jlink_jtag_t* jlink_usb_open()
 743 {
 744         struct usb_bus *busses;
 745         struct usb_bus *bus;
 746         struct usb_device *dev;
 747
 748         jlink_jtag_t *result;
 749
 750         result = (jlink_jtag_t*) malloc(sizeof(jlink_jtag_t));
 751
 752         usb_init();
 753         usb_find_busses();
 754         usb_find_devices();
 755
 756         busses = usb_get_busses();
 757
 758         /* find jlink_jtag device in usb bus */
 759
 760         for (bus = busses; bus; bus = bus->next)
 761         {
 762                 for (dev = bus->devices; dev; dev = dev->next)
 763                 {
 764                         if ((dev->descriptor.idVendor == VID) && (dev->descriptor.idProduct == PID))
 765                         {
 766                                 result->usb_handle = usb_open(dev);
 767
 768                                 /* usb_set_configuration required under win32 */
 769                                 usb_set_configuration(result->usb_handle, dev->config[0].bConfigurationValue);
 770                                 usb_claim_interface(result->usb_handle, 0);
 771
 772 #if 0
 773                                 /*
 774                                  * This makes problems under Mac OS X. And is not needed
 775                                  * under Windows. Hopefully this will not break a linux build
 776                                  */
 777                                 usb_set_altinterface(result->usb_handle, 0);
 778 #endif
 779                                 return result;
 780                         }
 781                 }
 782         }
 783
 784         free(result);
 785         return NULL;
 786 }
 787
 788 static void jlink_usb_close(jlink_jtag_t *jlink_jtag)
 789 {
 790         usb_close(jlink_jtag->usb_handle);
 791         free(jlink_jtag);
 792 }
 793
 794 /* Send a message and receive the reply. */
 795 static int jlink_usb_message(jlink_jtag_t *jlink_jtag, int out_length, int in_length)
 796 {
 797         int result;
 798         int result2;
 799
 800         result = jlink_usb_write(jlink_jtag, out_length);
 801         if (result != out_length)
 802         {
 803                 LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)",
 804                                 out_length, result);
 805                 return ERROR_JTAG_DEVICE_ERROR;
 806         }
 807
 808         result = jlink_usb_read(jlink_jtag, in_length);
 809         if ((result != in_length) && (result != in_length + 1))
 810         {
 811                 LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)",
 812                                 in_length, result);
 813                 return ERROR_JTAG_DEVICE_ERROR;
 814         }
 815
 816         if (result == in_length)
 817         {
 818                 /* Must read the result from the EMU too */
 819                 result2 = jlink_usb_read_emu_result(jlink_jtag);
 820                 if (1 != result2)
 821                 {
 822                         LOG_ERROR("jlink_usb_read_emu_result failed "
 823                                 "(requested=1, result=%d)", result2);
 824                         return ERROR_JTAG_DEVICE_ERROR;
 825                 }
 826
 827                 /* Check the result itself */
 828                 result2 = usb_emu_result_buffer[0];
 829         }
 830         else
 831         {
 832                 /* Save the result, then remove it from return value */
 833                 result2 = usb_in_buffer[result--];
 834         }
 835
 836         if (result2)
 837         {
 838                 LOG_ERROR("jlink_usb_message failed with result=%d)", result2);
 839                 return ERROR_JTAG_DEVICE_ERROR;
 840         }
 841
 842         return result;
 843 }
 844
 845 /* calls the given usb_bulk_* function, allowing for the data to trickle in with some timeouts  */
 846 static int usb_bulk_with_retries(
 847                 int (*f)(usb_dev_handle *, int, char *, int, int),
 848                 usb_dev_handle *dev, int ep,
 849                 char *bytes, int size, int timeout)
 850 {
 851         int rc = 0, tries = 3, this_size;
 852
 853         while (tries && size) {
 854
 855                 this_size = f(dev, ep, bytes, size, timeout);
 856                 if (this_size > 0) {
 857
 858                         size -= this_size;
 859                         rc += this_size;
 860                         bytes += this_size;
 861
 862                 } else
 863                         tries --;
 864         }
 865         return rc;
 866 }
 867 static inline int usb_bulk_write_ex(usb_dev_handle *dev, int ep,
 868                 char *bytes, int size, int timeout)
 869 {
 870         return usb_bulk_with_retries(&usb_bulk_write,
 871                         dev, ep, bytes, size, timeout);
 872 }
 873 static inline int usb_bulk_read_ex(usb_dev_handle *dev, int ep,
 874                 char *bytes, int size, int timeout)
 875 {
 876         return usb_bulk_with_retries(&usb_bulk_read,
 877                         dev, ep, bytes, size, timeout);
 878 }
 879
 880 /* Write data from out_buffer to USB. */
 881 static int jlink_usb_write(jlink_jtag_t *jlink_jtag, int out_length)
 882 {
 883         int result;
 884
 885         if (out_length > JLINK_OUT_BUFFER_SIZE)
 886         {
 887                 LOG_ERROR("jlink_jtag_write illegal out_length=%d (max=%d)", out_length, JLINK_OUT_BUFFER_SIZE);
 888                 return -1;
 889         }
 890
 891         result = usb_bulk_write_ex(jlink_jtag->usb_handle, JLINK_WRITE_ENDPOINT,
 892                 (char *)usb_out_buffer, out_length, JLINK_USB_TIMEOUT);
 893
 894         DEBUG_JTAG_IO("jlink_usb_write, out_length = %d, result = %d", out_length, result);
 895
 896 #ifdef _DEBUG_USB_COMMS_
 897         jlink_debug_buffer(usb_out_buffer, out_length);
 898 #endif
 899         return result;
 900 }
 901
 902 /* Read data from USB into in_buffer. */
 903 static int jlink_usb_read(jlink_jtag_t *jlink_jtag, int expected_size)
 904 {
 905         int result = usb_bulk_read_ex(jlink_jtag->usb_handle, JLINK_READ_ENDPOINT,
 906                 (char *)usb_in_buffer, expected_size, JLINK_USB_TIMEOUT);
 907
 908         DEBUG_JTAG_IO("jlink_usb_read, result = %d", result);
 909
 910 #ifdef _DEBUG_USB_COMMS_
 911         jlink_debug_buffer(usb_in_buffer, result);
 912 #endif
 913         return result;
 914 }
 915
 916 /* Read the result from the previous EMU cmd into result_buffer. */
 917 static int jlink_usb_read_emu_result(jlink_jtag_t *jlink_jtag)
 918 {
 919         int result = usb_bulk_read_ex(jlink_jtag->usb_handle, JLINK_READ_ENDPOINT,
 920                 (char *)usb_emu_result_buffer, 1 /* JLINK_EMU_RESULT_BUFFER_SIZE */,
 921                 JLINK_USB_TIMEOUT);
 922
 923         DEBUG_JTAG_IO("jlink_usb_read_result, result = %d", result);
 924
 925 #ifdef _DEBUG_USB_COMMS_
 926         jlink_debug_buffer(usb_emu_result_buffer, result);
 927 #endif
 928         return result;
 929 }
 930
 931 #ifdef _DEBUG_USB_COMMS_
 932 #define BYTES_PER_LINE  16
 933
 934 static void jlink_debug_buffer(u8 *buffer, int length)
 935 {
 936         char line[81];
 937         char s[4];
 938         int i;
 939         int j;
 940
 941         for (i = 0; i < length; i += BYTES_PER_LINE)
 942         {
 943                 snprintf(line, 5, "%04x", i);
 944                 for (j = i; j < i + BYTES_PER_LINE && j < length; j++)
 945                 {
 946                         snprintf(s, 4, " %02x", buffer[j]);
 947                         strcat(line, s);
 948                 }
 949                 LOG_DEBUG("%s", line);
 950         }
 951 }
 952 #endif