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