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