git.gag.com Git - fw/openocd/blob - src/jtag/drivers/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  *   Copyright (C) 2011 by Jean-Christophe PLAGNIOL-VIILARD                *
   9  *   plagnioj@jcrosoft.com                                                 *
  10  *                                                                         *
  11  *   This program is free software; you can redistribute it and/or modify  *
  12  *   it under the terms of the GNU General Public License as published by  *
  13  *   the Free Software Foundation; either version 2 of the License, or     *
  14  *   (at your option) any later version.                                   *
  15  *                                                                         *
  16  *   This program is distributed in the hope that it will be useful,       *
  17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  19  *   GNU General Public License for more details.                          *
  20  *                                                                         *
  21  *   You should have received a copy of the GNU General Public License     *
  22  *   along with this program; if not, write to the                         *
  23  *   Free Software Foundation, Inc.,                                       *
  24  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  25  ***************************************************************************/
  26
  27 #ifdef HAVE_CONFIG_H
  28 #include "config.h"
  29 #endif
  30
  31 #include <jtag/interface.h>
  32 #include <jtag/commands.h>
  33 #include "libusb_common.h"
  34
  35 /* See Segger's public documentation:
  36  * Reference manual for J-Link USB Protocol
  37  * Document RM08001-R6 Date: June 16, 2009
  38  * (Or newer, with some SWD information).
  39  * http://www.segger.com/cms/admin/uploads/productDocs/RM08001_JLinkUSBProtocol.pdf
  40  */
  41
  42 /*
  43  * The default pid of the segger is 0x0101
  44  * But when you change the USB Address it will also
  45  *
  46  * pid = ( usb_address > 0x4) ? 0x0101 : (0x101 + usb_address)
  47  */
  48
  49 #define VID 0x1366, 0x1366, 0x1366, 0x1366
  50 #define PID 0x0101, 0x0102, 0x0103, 0x0104
  51
  52 #define JLINK_WRITE_ENDPOINT    0x02
  53 #define JLINK_READ_ENDPOINT             0x81
  54
  55 static unsigned int jlink_write_ep = JLINK_WRITE_ENDPOINT;
  56 static unsigned int jlink_read_ep = JLINK_READ_ENDPOINT;
  57 static unsigned int jlink_hw_jtag_version = 2;
  58
  59 #define JLINK_USB_TIMEOUT 1000
  60
  61 /* See Section 3.3.2 of the Segger JLink USB protocol manual */
  62 /* 2048 is the max value we can use here */
  63 #define JLINK_TAP_BUFFER_SIZE 2048
  64 /*#define JLINK_TAP_BUFFER_SIZE 256*/
  65 /*#define JLINK_TAP_BUFFER_SIZE 384*/
  66
  67 #define JLINK_IN_BUFFER_SIZE                    2048
  68 #define JLINK_OUT_BUFFER_SIZE                   (2*2048 + 4)
  69 #define JLINK_EMU_RESULT_BUFFER_SIZE    64
  70
  71 /* Global USB buffers */
  72 static uint8_t usb_in_buffer[JLINK_IN_BUFFER_SIZE];
  73 static uint8_t usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
  74 static uint8_t usb_emu_result_buffer[JLINK_EMU_RESULT_BUFFER_SIZE];
  75
  76 /* Constants for JLink command */
  77 #define EMU_CMD_VERSION                 0x01
  78 #define EMU_CMD_SET_SPEED               0x05
  79 #define EMU_CMD_GET_STATE               0x07
  80 #define EMU_CMD_HW_CLOCK                0xc8
  81 #define EMU_CMD_HW_TMS0                 0xc9
  82 #define EMU_CMD_HW_TMS1                 0xca
  83 #define EMU_CMD_HW_JTAG2                0xce
  84 #define EMU_CMD_HW_JTAG3                0xcf
  85 #define EMU_CMD_GET_MAX_MEM_BLOCK       0xd4
  86 #define EMU_CMD_HW_RESET0               0xdc
  87 #define EMU_CMD_HW_RESET1               0xdd
  88 #define EMU_CMD_HW_TRST0                0xde
  89 #define EMU_CMD_HW_TRST1                0xdf
  90 #define EMU_CMD_GET_CAPS                0xe8
  91 #define EMU_CMD_GET_HW_VERSION  0xf0
  92 #define EMU_CMD_READ_CONFIG             0xf2
  93 #define EMU_CMD_WRITE_CONFIG            0xf3
  94
  95 /* bits return from EMU_CMD_GET_CAPS */
  96 #define EMU_CAP_RESERVED_1              0
  97 #define EMU_CAP_GET_HW_VERSION          1
  98 #define EMU_CAP_WRITE_DCC               2
  99 #define EMU_CAP_ADAPTIVE_CLOCKING       3
 100 #define EMU_CAP_READ_CONFIG             4
 101 #define EMU_CAP_WRITE_CONFIG            5
 102 #define EMU_CAP_TRACE                   6
 103 #define EMU_CAP_WRITE_MEM               7
 104 #define EMU_CAP_READ_MEM                8
 105 #define EMU_CAP_SPEED_INFO              9
 106 #define EMU_CAP_EXEC_CODE               10
 107 #define EMU_CAP_GET_MAX_BLOCK_SIZE      11
 108 #define EMU_CAP_GET_HW_INFO             12
 109 #define EMU_CAP_SET_KS_POWER            13
 110 #define EMU_CAP_RESET_STOP_TIMED        14
 111 #define EMU_CAP_RESERVED_2              15
 112 #define EMU_CAP_MEASURE_RTCK_REACT      16
 113 #define EMU_CAP_SELECT_IF               17
 114 #define EMU_CAP_RW_MEM_ARM79            18
 115 #define EMU_CAP_GET_COUNTERS            19
 116 #define EMU_CAP_READ_DCC                20
 117 #define EMU_CAP_GET_CPU_CAPS            21
 118 #define EMU_CAP_EXEC_CPU_CMD            22
 119 #define EMU_CAP_SWO                     23
 120 #define EMU_CAP_WRITE_DCC_EX            24
 121 #define EMU_CAP_UPDATE_FIRMWARE_EX      25
 122 #define EMU_CAP_FILE_IO                 26
 123 #define EMU_CAP_REGISTER                27
 124 #define EMU_CAP_INDICATORS              28
 125 #define EMU_CAP_TEST_NET_SPEED          29
 126 #define EMU_CAP_RAWTRACE                30
 127 #define EMU_CAP_RESERVED_3              31
 128
 129 static char *jlink_cap_str[] = {
 130         "Always 1.",
 131         "Supports command EMU_CMD_GET_HARDWARE_VERSION",
 132         "Supports command EMU_CMD_WRITE_DCC",
 133         "Supports adaptive clocking",
 134         "Supports command EMU_CMD_READ_CONFIG",
 135         "Supports command EMU_CMD_WRITE_CONFIG",
 136         "Supports trace commands",
 137         "Supports command EMU_CMD_WRITE_MEM",
 138         "Supports command EMU_CMD_READ_MEM",
 139         "Supports command EMU_CMD_GET_SPEED",
 140         "Supports command EMU_CMD_CODE_...",
 141         "Supports command EMU_CMD_GET_MAX_BLOCK_SIZE",
 142         "Supports command EMU_CMD_GET_HW_INFO",
 143         "Supports command EMU_CMD_SET_KS_POWER",
 144         "Supports command EMU_CMD_HW_RELEASE_RESET_STOP_TIMED",
 145         "Reserved",
 146         "Supports command EMU_CMD_MEASURE_RTCK_REACT",
 147         "Supports command EMU_CMD_HW_SELECT_IF",
 148         "Supports command EMU_CMD_READ/WRITE_MEM_ARM79",
 149         "Supports command EMU_CMD_GET_COUNTERS",
 150         "Supports command EMU_CMD_READ_DCC",
 151         "Supports command EMU_CMD_GET_CPU_CAPS",
 152         "Supports command EMU_CMD_EXEC_CPU_CMD",
 153         "Supports command EMU_CMD_SWO",
 154         "Supports command EMU_CMD_WRITE_DCC_EX",
 155         "Supports command EMU_CMD_UPDATE_FIRMWARE_EX",
 156         "Supports command EMU_CMD_FILE_IO",
 157         "Supports command EMU_CMD_REGISTER",
 158         "Supports command EMU_CMD_INDICATORS",
 159         "Supports command EMU_CMD_TEST_NET_SPEED",
 160         "Supports command EMU_CMD_RAWTRACE",
 161         "Reserved",
 162 };
 163
 164 /* max speed 12MHz v5.0 jlink */
 165 #define JLINK_MAX_SPEED 12000
 166
 167 /* J-Link hardware versions */
 168 #define JLINK_HW_TYPE_JLINK     0
 169 #define JLINK_HW_TYPE_JTRACE    1
 170 #define JLINK_HW_TYPE_FLASHER   2
 171 #define JLINK_HW_TYPE_JLINK_PRO 3
 172 #define JLINK_HW_TYPE_MAX       4
 173
 174 static char *jlink_hw_type_str[] = {
 175         "J-Link",
 176         "J-Trace",
 177         "Flasher",
 178         "J-Link Pro",
 179 };
 180
 181 /* Queue command functions */
 182 static void jlink_end_state(tap_state_t state);
 183 static void jlink_state_move(void);
 184 static void jlink_path_move(int num_states, tap_state_t *path);
 185 static void jlink_runtest(int num_cycles);
 186 static void jlink_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 187                 int scan_size, struct scan_command *command);
 188 static void jlink_reset(int trst, int srst);
 189 static void jlink_simple_command(uint8_t command);
 190 static int jlink_get_status(void);
 191
 192 /* J-Link tap buffer functions */
 193 static void jlink_tap_init(void);
 194 static int jlink_tap_execute(void);
 195 static void jlink_tap_ensure_space(int scans, int bits);
 196 static void jlink_tap_append_step(int tms, int tdi);
 197 static void jlink_tap_append_scan(int length, uint8_t *buffer,
 198                 struct scan_command *command);
 199
 200 /* Jlink lowlevel functions */
 201 struct jlink {
 202         struct jtag_libusb_device_handle *usb_handle;
 203 };
 204
 205 static struct jlink *jlink_usb_open(void);
 206 static void jlink_usb_close(struct jlink *jlink);
 207 static int jlink_usb_message(struct jlink *jlink, int out_length, int in_length);
 208 static int jlink_usb_io(struct jlink *jlink, int out_length, int in_length);
 209 static int jlink_usb_write(struct jlink *jlink, int out_length);
 210 static int jlink_usb_read(struct jlink *jlink, int expected_size);
 211 static int jlink_usb_read_emu_result(struct jlink *jlink);
 212
 213 /* helper functions */
 214 static int jlink_get_version_info(void);
 215
 216 #ifdef _DEBUG_USB_COMMS_
 217 static void jlink_debug_buffer(uint8_t *buffer, int length);
 218 #else
 219 static inline void jlink_debug_buffer(uint8_t *buffer, int length)
 220 {
 221 }
 222 #endif
 223
 224 static enum tap_state jlink_last_state = TAP_RESET;
 225
 226 static struct jlink *jlink_handle;
 227
 228 /* pid could be specified at runtime */
 229 static uint16_t vids[] = { VID, 0 };
 230 static uint16_t pids[] = { PID, 0 };
 231
 232 static uint32_t jlink_caps;
 233 static uint32_t jlink_hw_type;
 234
 235 /* 256 byte non-volatile memory */
 236 struct jlink_config {
 237         uint8_t usb_address;
 238         /* 0ffset 0x01 to 0x03 */
 239         uint8_t reserved_1[3];
 240         uint32_t kickstart_power_on_jtag_pin_19;
 241         /* 0ffset 0x08 to 0x1f */
 242         uint8_t reserved_2[24];
 243         /* IP only for J-Link Pro */
 244         uint8_t ip_address[4];
 245         uint8_t subnet_mask[4];
 246         /* 0ffset 0x28 to 0x2f */
 247         uint8_t reserved_3[8];
 248         uint8_t mac_address[6];
 249         /* 0ffset 0x36 to 0xff */
 250         uint8_t reserved_4[202];
 251 } __attribute__ ((packed));
 252 struct jlink_config jlink_cfg;
 253
 254 /***************************************************************************/
 255 /* External interface implementation */
 256
 257 static void jlink_execute_runtest(struct jtag_command *cmd)
 258 {
 259         DEBUG_JTAG_IO("runtest %i cycles, end in %i",
 260                         cmd->cmd.runtest->num_cycles,
 261                         cmd->cmd.runtest->end_state);
 262
 263         jlink_end_state(cmd->cmd.runtest->end_state);
 264
 265         jlink_runtest(cmd->cmd.runtest->num_cycles);
 266 }
 267
 268 static void jlink_execute_statemove(struct jtag_command *cmd)
 269 {
 270         DEBUG_JTAG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
 271
 272         jlink_end_state(cmd->cmd.statemove->end_state);
 273         jlink_state_move();
 274 }
 275
 276 static void jlink_execute_pathmove(struct jtag_command *cmd)
 277 {
 278         DEBUG_JTAG_IO("pathmove: %i states, end in %i",
 279                 cmd->cmd.pathmove->num_states,
 280                 cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
 281
 282         jlink_path_move(cmd->cmd.pathmove->num_states,
 283                         cmd->cmd.pathmove->path);
 284 }
 285
 286 static void jlink_execute_scan(struct jtag_command *cmd)
 287 {
 288         int scan_size;
 289         enum scan_type type;
 290         uint8_t *buffer;
 291
 292         DEBUG_JTAG_IO("scan end in %s", tap_state_name(cmd->cmd.scan->end_state));
 293
 294         jlink_end_state(cmd->cmd.scan->end_state);
 295
 296         scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
 297         DEBUG_JTAG_IO("scan input, length = %d", scan_size);
 298
 299         jlink_debug_buffer(buffer, (scan_size + 7) / 8);
 300         type = jtag_scan_type(cmd->cmd.scan);
 301         jlink_scan(cmd->cmd.scan->ir_scan,
 302                         type, buffer, scan_size, cmd->cmd.scan);
 303 }
 304
 305 static void jlink_execute_reset(struct jtag_command *cmd)
 306 {
 307         DEBUG_JTAG_IO("reset trst: %i srst %i",
 308                         cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 309
 310         jlink_tap_execute();
 311         jlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 312         jlink_tap_execute();
 313 }
 314
 315 static void jlink_execute_sleep(struct jtag_command *cmd)
 316 {
 317         DEBUG_JTAG_IO("sleep %" PRIi32 "", cmd->cmd.sleep->us);
 318         jlink_tap_execute();
 319         jtag_sleep(cmd->cmd.sleep->us);
 320 }
 321
 322 static void jlink_execute_command(struct jtag_command *cmd)
 323 {
 324         switch (cmd->type) {
 325                 case JTAG_RUNTEST:
 326                         jlink_execute_runtest(cmd);
 327                         break;
 328                 case JTAG_TLR_RESET:
 329                         jlink_execute_statemove(cmd);
 330                         break;
 331                 case JTAG_PATHMOVE:
 332                         jlink_execute_pathmove(cmd);
 333                         break;
 334                 case JTAG_SCAN:
 335                         jlink_execute_scan(cmd);
 336                         break;
 337                 case JTAG_RESET:
 338                         jlink_execute_reset(cmd);
 339                         break;
 340                 case JTAG_SLEEP:
 341                         jlink_execute_sleep(cmd);
 342                         break;
 343                 default:
 344                         LOG_ERROR("BUG: unknown JTAG command type encountered");
 345                         exit(-1);
 346         }
 347 }
 348
 349 static int jlink_execute_queue(void)
 350 {
 351         struct jtag_command *cmd = jtag_command_queue;
 352
 353         while (cmd != NULL) {
 354                 jlink_execute_command(cmd);
 355                 cmd = cmd->next;
 356         }
 357
 358         return jlink_tap_execute();
 359 }
 360
 361 /* Sets speed in kHz. */
 362 static int jlink_speed(int speed)
 363 {
 364         int result;
 365
 366         if (speed > JLINK_MAX_SPEED) {
 367                 LOG_INFO("reduce speed request: %dkHz to %dkHz maximum",
 368                                 speed, JLINK_MAX_SPEED);
 369                 speed = JLINK_MAX_SPEED;
 370         }
 371
 372         /* check for RTCK setting */
 373         if (speed == 0)
 374                 speed = -1;
 375
 376         usb_out_buffer[0] = EMU_CMD_SET_SPEED;
 377         usb_out_buffer[1] = (speed >> 0) & 0xff;
 378         usb_out_buffer[2] = (speed >> 8) & 0xff;
 379
 380         result = jlink_usb_write(jlink_handle, 3);
 381         if (result != 3) {
 382                 LOG_ERROR("J-Link setting speed failed (%d)", result);
 383                 return ERROR_JTAG_DEVICE_ERROR;
 384         }
 385
 386         return ERROR_OK;
 387 }
 388
 389 static int jlink_speed_div(int speed, int *khz)
 390 {
 391         *khz = speed;
 392
 393         return ERROR_OK;
 394 }
 395
 396 static int jlink_khz(int khz, int *jtag_speed)
 397 {
 398         *jtag_speed = khz;
 399
 400         return ERROR_OK;
 401 }
 402
 403 static int jlink_init(void)
 404 {
 405         int i;
 406
 407         jlink_handle = jlink_usb_open();
 408
 409         if (jlink_handle == 0) {
 410                 LOG_ERROR("Cannot find jlink Interface! Please check "
 411                                 "connection and permissions.");
 412                 return ERROR_JTAG_INIT_FAILED;
 413         }
 414
 415         /*
 416          * The next three instructions were added after discovering a problem
 417          * while using an oscilloscope.
 418          * For the V8 SAM-ICE dongle (and likely other j-link device variants),
 419          * the reset line to the target microprocessor was found to cycle only
 420          * intermittently during emulator startup (even after encountering the
 421          * downstream reset instruction later in the code).
 422          * This was found to create two issues:
 423          * 1) In general it is a bad practice to not reset a CPU to a known
 424          * state when starting an emulator and
 425          * 2) something critical happens inside the dongle when it does the
 426          * first read following a new USB session.
 427          * Keeping the processor in reset during the first read collecting
 428          * version information seems to prevent errant
 429          * "J-Link command EMU_CMD_VERSION failed" issues.
 430          */
 431
 432         LOG_INFO("J-Link initialization started / target CPU reset initiated");
 433         jlink_simple_command(EMU_CMD_HW_TRST0);
 434         jlink_simple_command(EMU_CMD_HW_RESET0);
 435         usleep(1000);
 436
 437         jlink_hw_jtag_version = 2;
 438
 439         if (jlink_get_version_info() == ERROR_OK) {
 440                 /* attempt to get status */
 441                 jlink_get_status();
 442         }
 443
 444         LOG_INFO("J-Link JTAG Interface ready");
 445
 446         jlink_reset(0, 0);
 447         jtag_sleep(3000);
 448         jlink_tap_init();
 449
 450         /* v5/6 jlink seems to have an issue if the first tap move
 451          * is not divisible by 8, so we send a TLR on first power up */
 452         for (i = 0; i < 8; i++)
 453                 jlink_tap_append_step(1, 0);
 454         jlink_tap_execute();
 455
 456         return ERROR_OK;
 457 }
 458
 459 static int jlink_quit(void)
 460 {
 461         jlink_usb_close(jlink_handle);
 462         return ERROR_OK;
 463 }
 464
 465 /***************************************************************************/
 466 /* Queue command implementations */
 467
 468 static void jlink_end_state(tap_state_t state)
 469 {
 470         if (tap_is_state_stable(state))
 471                 tap_set_end_state(state);
 472         else {
 473                 LOG_ERROR("BUG: %i is not a valid end state", state);
 474                 exit(-1);
 475         }
 476 }
 477
 478 /* Goes to the end state. */
 479 static void jlink_state_move(void)
 480 {
 481         int i;
 482         int tms = 0;
 483         uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 484         uint8_t tms_scan_bits = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
 485
 486         for (i = 0; i < tms_scan_bits; i++) {
 487                 tms = (tms_scan >> i) & 1;
 488                 jlink_tap_append_step(tms, 0);
 489         }
 490
 491         tap_set_state(tap_get_end_state());
 492 }
 493
 494 static void jlink_path_move(int num_states, tap_state_t *path)
 495 {
 496         int i;
 497
 498         for (i = 0; i < num_states; i++) {
 499                 if (path[i] == tap_state_transition(tap_get_state(), false))
 500                         jlink_tap_append_step(0, 0);
 501                 else if (path[i] == tap_state_transition(tap_get_state(), true))
 502                         jlink_tap_append_step(1, 0);
 503                 else {
 504                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition",
 505                                         tap_state_name(tap_get_state()), tap_state_name(path[i]));
 506                         exit(-1);
 507                 }
 508
 509                 tap_set_state(path[i]);
 510         }
 511
 512         tap_set_end_state(tap_get_state());
 513 }
 514
 515 static void jlink_runtest(int num_cycles)
 516 {
 517         int i;
 518
 519         tap_state_t saved_end_state = tap_get_end_state();
 520
 521         jlink_tap_ensure_space(1, num_cycles + 16);
 522
 523         /* only do a state_move when we're not already in IDLE */
 524         if (tap_get_state() != TAP_IDLE) {
 525                 jlink_end_state(TAP_IDLE);
 526                 jlink_state_move();
 527                 /* num_cycles--; */
 528         }
 529
 530         /* execute num_cycles */
 531         for (i = 0; i < num_cycles; i++)
 532                 jlink_tap_append_step(0, 0);
 533
 534         /* finish in end_state */
 535         jlink_end_state(saved_end_state);
 536         if (tap_get_state() != tap_get_end_state())
 537                 jlink_state_move();
 538 }
 539
 540 static void jlink_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 541                 int scan_size, struct scan_command *command)
 542 {
 543         tap_state_t saved_end_state;
 544
 545         jlink_tap_ensure_space(1, scan_size + 16);
 546
 547         saved_end_state = tap_get_end_state();
 548
 549         /* Move to appropriate scan state */
 550         jlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 551
 552         /* Only move if we're not already there */
 553         if (tap_get_state() != tap_get_end_state())
 554                 jlink_state_move();
 555
 556         jlink_end_state(saved_end_state);
 557
 558         /* Scan */
 559         jlink_tap_append_scan(scan_size, buffer, command);
 560
 561         /* We are in Exit1, go to Pause */
 562         jlink_tap_append_step(0, 0);
 563
 564         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 565
 566         if (tap_get_state() != tap_get_end_state())
 567                 jlink_state_move();
 568 }
 569
 570 static void jlink_reset(int trst, int srst)
 571 {
 572         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
 573
 574         /* Signals are active low */
 575         if (srst == 0)
 576                 jlink_simple_command(EMU_CMD_HW_RESET1);
 577
 578         if (srst == 1)
 579                 jlink_simple_command(EMU_CMD_HW_RESET0);
 580
 581         if (trst == 1)
 582                 jlink_simple_command(EMU_CMD_HW_TRST0);
 583
 584         if (trst == 0)
 585                 jlink_simple_command(EMU_CMD_HW_TRST1);
 586 }
 587
 588 static void jlink_simple_command(uint8_t command)
 589 {
 590         int result;
 591
 592         DEBUG_JTAG_IO("0x%02x", command);
 593
 594         usb_out_buffer[0] = command;
 595         result = jlink_usb_write(jlink_handle, 1);
 596
 597         if (result != 1)
 598                 LOG_ERROR("J-Link command 0x%02x failed (%d)", command, result);
 599 }
 600
 601 static int jlink_get_status(void)
 602 {
 603         int result;
 604
 605         jlink_simple_command(EMU_CMD_GET_STATE);
 606
 607         result = jlink_usb_read(jlink_handle, 8);
 608         if (result != 8) {
 609                 LOG_ERROR("J-Link command EMU_CMD_GET_STATE failed (%d)", result);
 610                 return ERROR_JTAG_DEVICE_ERROR;
 611         }
 612
 613         int vref = usb_in_buffer[0] + (usb_in_buffer[1] << 8);
 614         LOG_INFO("Vref = %d.%d TCK = %d TDI = %d TDO = %d TMS = %d SRST = %d TRST = %d", \
 615                 vref / 1000, vref % 1000, \
 616                 usb_in_buffer[2], usb_in_buffer[3], usb_in_buffer[4], \
 617                 usb_in_buffer[5], usb_in_buffer[6], usb_in_buffer[7]);
 618
 619         if (vref < 1500)
 620                 LOG_ERROR("Vref too low. Check Target Power");
 621
 622         return ERROR_OK;
 623 }
 624
 625 #define jlink_dump_printf(context, expr ...) \
 626         do { \
 627                 if (context) \
 628                         command_print(context, expr); \
 629                         else \
 630                         LOG_INFO(expr); \
 631         } while (0);
 632
 633 static void jlink_caps_dump(struct command_context *ctx)
 634 {
 635         int i;
 636
 637         jlink_dump_printf(ctx, "J-Link Capabilities");
 638
 639         for (i = 1; i < 31; i++)
 640                 if (jlink_caps & (1 << i))
 641                         jlink_dump_printf(ctx, "%s", jlink_cap_str[i]);
 642 }
 643
 644 static void jlink_config_usb_address_dump(struct command_context *ctx, struct jlink_config *cfg)
 645 {
 646         if (!cfg)
 647                 return;
 648
 649         jlink_dump_printf(ctx, "USB-Address: 0x%x", cfg->usb_address);
 650 }
 651
 652 static void jlink_config_kickstart_dump(struct command_context *ctx, struct jlink_config *cfg)
 653 {
 654         if (!cfg)
 655                 return;
 656
 657         jlink_dump_printf(ctx, "Kickstart power on JTAG-pin 19: 0x%x",
 658                 cfg->kickstart_power_on_jtag_pin_19);
 659 }
 660
 661 static void jlink_config_mac_address_dump(struct command_context *ctx, struct jlink_config *cfg)
 662 {
 663         if (!cfg)
 664                 return;
 665
 666         jlink_dump_printf(ctx, "MAC Address: %.02x:%.02x:%.02x:%.02x:%.02x:%.02x",
 667                 cfg->mac_address[5], cfg->mac_address[4],
 668                 cfg->mac_address[3], cfg->mac_address[2],
 669                 cfg->mac_address[1], cfg->mac_address[0]);
 670 }
 671
 672 static void jlink_config_ip_dump(struct command_context *ctx, struct jlink_config *cfg)
 673 {
 674         if (!cfg)
 675                 return;
 676
 677         jlink_dump_printf(ctx, "IP Address: %d.%d.%d.%d",
 678                 cfg->ip_address[3], cfg->ip_address[2],
 679                 cfg->ip_address[1], cfg->ip_address[0]);
 680         jlink_dump_printf(ctx, "Subnet Mask: %d.%d.%d.%d",
 681                 cfg->subnet_mask[3], cfg->subnet_mask[2],
 682                 cfg->subnet_mask[1], cfg->subnet_mask[0]);
 683 }
 684
 685 static void jlink_config_dump(struct command_context *ctx, struct jlink_config *cfg)
 686 {
 687         if (!cfg)
 688                 return;
 689
 690         jlink_dump_printf(ctx, "J-Link configuration");
 691         jlink_config_usb_address_dump(ctx, cfg);
 692         jlink_config_kickstart_dump(ctx, cfg);
 693
 694         if (jlink_hw_type == JLINK_HW_TYPE_JLINK_PRO) {
 695                 jlink_config_ip_dump(ctx, cfg);
 696                 jlink_config_mac_address_dump(ctx, cfg);
 697         }
 698 }
 699
 700 static int jlink_get_config(struct jlink_config *cfg)
 701 {
 702         int result;
 703         int size = sizeof(struct jlink_config);
 704
 705         usb_out_buffer[0] = EMU_CMD_READ_CONFIG;
 706         result = jlink_usb_io(jlink_handle, 1, size);
 707
 708         if (result != ERROR_OK) {
 709                 LOG_ERROR("jlink_usb_read failed (requested=%d, result=%d)", size, result);
 710                 return ERROR_FAIL;
 711         }
 712
 713         memcpy(cfg, usb_in_buffer, size);
 714         return ERROR_OK;
 715 }
 716
 717 static int jlink_set_config(struct jlink_config *cfg)
 718 {
 719         int result;
 720         int size = sizeof(struct jlink_config);
 721
 722         jlink_simple_command(EMU_CMD_WRITE_CONFIG);
 723
 724         memcpy(usb_out_buffer, cfg, size);
 725
 726         result = jlink_usb_write(jlink_handle, size);
 727         if (result != size) {
 728                 LOG_ERROR("jlink_usb_write failed (requested=%d, result=%d)", 256, result);
 729                 return ERROR_FAIL;
 730         }
 731
 732         return ERROR_OK;
 733 }
 734
 735 /*
 736  * List of unsupported version string markers.
 737  *
 738  * The firmware versions does not correspond directly with
 739  * "Software and documentation pack for Windows", it may be
 740  * distinguished by the "compile" date in the information string.
 741  *
 742  * For example, version string is:
 743  *   "J-Link ARM V8 compiled May  3 2012 18:36:22"
 744  * Marker sould be:
 745  *   "May  3 2012"
 746  *
 747  * The list must be terminated by NULL string.
 748  */
 749 static const char * const unsupported_versions[] = {
 750         "Jan 31 2011",
 751         "JAN 31 2011",
 752         "Mar 19 2012",  /* V4.44 */
 753         "May  3 2012",  /* V4.46 "J-Link ARM V8 compiled May  3 2012 18:36:22" */
 754         NULL                    /* End of list */
 755 };
 756
 757 static void jlink_check_supported(const char *str)
 758 {
 759         const char * const *p = unsupported_versions;
 760         while (*p) {
 761                 if (NULL != strstr(str, *p)) {
 762                         LOG_WARNING(
 763                         "Unsupported J-Link firmware version.\n"
 764                         "       Please check http://www.segger.com/j-link-older-versions.html for updates");
 765                         return;
 766                 }
 767                 p++;
 768         }
 769 }
 770
 771 static int jlink_get_version_info(void)
 772 {
 773         int result;
 774         int len;
 775         uint32_t jlink_max_size;
 776
 777         /* query hardware version */
 778         jlink_simple_command(EMU_CMD_VERSION);
 779
 780         result = jlink_usb_read(jlink_handle, 2);
 781         if (2 != result) {
 782                 LOG_ERROR("J-Link command EMU_CMD_VERSION failed (%d)", result);
 783                 return ERROR_JTAG_DEVICE_ERROR;
 784         }
 785
 786         len = buf_get_u32(usb_in_buffer, 0, 16);
 787         if (len > JLINK_IN_BUFFER_SIZE) {
 788                 LOG_ERROR("J-Link command EMU_CMD_VERSION impossible return length 0x%0x", len);
 789                 len = JLINK_IN_BUFFER_SIZE;
 790         }
 791
 792         result = jlink_usb_read(jlink_handle, len);
 793         if (result != len) {
 794                 LOG_ERROR("J-Link command EMU_CMD_VERSION failed (%d)", result);
 795                 return ERROR_JTAG_DEVICE_ERROR;
 796         }
 797
 798         usb_in_buffer[result] = 0;
 799         LOG_INFO("%s", (char *)usb_in_buffer);
 800         jlink_check_supported((char *)usb_in_buffer);
 801
 802         /* query hardware capabilities */
 803         jlink_simple_command(EMU_CMD_GET_CAPS);
 804
 805         result = jlink_usb_read(jlink_handle, 4);
 806         if (4 != result) {
 807                 LOG_ERROR("J-Link command EMU_CMD_GET_CAPS failed (%d)", result);
 808                 return ERROR_JTAG_DEVICE_ERROR;
 809         }
 810
 811         jlink_caps = buf_get_u32(usb_in_buffer, 0, 32);
 812         LOG_INFO("J-Link caps 0x%x", (unsigned)jlink_caps);
 813
 814         if (jlink_caps & (1 << EMU_CAP_GET_HW_VERSION)) {
 815                 /* query hardware version */
 816                 jlink_simple_command(EMU_CMD_GET_HW_VERSION);
 817
 818                 result = jlink_usb_read(jlink_handle, 4);
 819                 if (4 != result) {
 820                         LOG_ERROR("J-Link command EMU_CMD_GET_HW_VERSION failed (%d)", result);
 821                         return ERROR_JTAG_DEVICE_ERROR;
 822                 }
 823
 824                 uint32_t jlink_hw_version = buf_get_u32(usb_in_buffer, 0, 32);
 825                 uint32_t major_revision = (jlink_hw_version / 10000) % 100;
 826                 jlink_hw_type = (jlink_hw_version / 1000000) % 100;
 827                 if (major_revision >= 5)
 828                         jlink_hw_jtag_version = 3;
 829
 830                 LOG_INFO("J-Link hw version %i", (int)jlink_hw_version);
 831
 832                 if (jlink_hw_type >= JLINK_HW_TYPE_MAX)
 833                         LOG_INFO("J-Link hw type uknown 0x%x", jlink_hw_type);
 834                 else
 835                         LOG_INFO("J-Link hw type %s", jlink_hw_type_str[jlink_hw_type]);
 836         }
 837
 838         if (jlink_caps & (1 << EMU_CAP_GET_MAX_BLOCK_SIZE)) {
 839                 /* query hardware maximum memory block */
 840                 jlink_simple_command(EMU_CMD_GET_MAX_MEM_BLOCK);
 841
 842                 result = jlink_usb_read(jlink_handle, 4);
 843                 if (4 != result) {
 844                         LOG_ERROR("J-Link command EMU_CMD_GET_MAX_MEM_BLOCK failed (%d)", result);
 845                         return ERROR_JTAG_DEVICE_ERROR;
 846                 }
 847
 848                 jlink_max_size = buf_get_u32(usb_in_buffer, 0, 32);
 849                 LOG_INFO("J-Link max mem block %i", (int)jlink_max_size);
 850         }
 851
 852         if (jlink_caps & (1 << EMU_CAP_READ_CONFIG)) {
 853                 if (jlink_get_config(&jlink_cfg) != ERROR_OK)
 854                         return ERROR_JTAG_DEVICE_ERROR;
 855
 856                 jlink_config_dump(NULL, &jlink_cfg);
 857         }
 858
 859         return ERROR_OK;
 860 }
 861
 862 COMMAND_HANDLER(jlink_pid_command)
 863 {
 864         if (CMD_ARGC != 1) {
 865                 LOG_ERROR("Need exactly one argument to jlink_pid");
 866                 return ERROR_FAIL;
 867         }
 868
 869         pids[0] = strtoul(CMD_ARGV[0], NULL, 16);
 870         pids[1] = 0;
 871         vids[1] = 0;
 872
 873         return ERROR_OK;
 874 }
 875
 876 COMMAND_HANDLER(jlink_handle_jlink_info_command)
 877 {
 878         if (jlink_get_version_info() == ERROR_OK) {
 879                 /* attempt to get status */
 880                 jlink_get_status();
 881         }
 882
 883         return ERROR_OK;
 884 }
 885
 886 COMMAND_HANDLER(jlink_handle_jlink_caps_command)
 887 {
 888         jlink_caps_dump(CMD_CTX);
 889
 890         return ERROR_OK;
 891 }
 892
 893 COMMAND_HANDLER(jlink_handle_jlink_hw_jtag_command)
 894 {
 895         switch (CMD_ARGC) {
 896                 case 0:
 897                         command_print(CMD_CTX, "J-Link hw jtag  %i", jlink_hw_jtag_version);
 898                         break;
 899                 case 1: {
 900                         int request_version = atoi(CMD_ARGV[0]);
 901                         switch (request_version) {
 902                                 case 2:
 903                                 case 3:
 904                                         jlink_hw_jtag_version = request_version;
 905                                         break;
 906                                 default:
 907                                         return ERROR_COMMAND_SYNTAX_ERROR;
 908                         }
 909                         break;
 910                 }
 911                 default:
 912                         return ERROR_COMMAND_SYNTAX_ERROR;
 913         }
 914
 915         return ERROR_OK;
 916 }
 917
 918 COMMAND_HANDLER(jlink_handle_jlink_kickstart_command)
 919 {
 920         uint32_t kickstart;
 921
 922         if (CMD_ARGC < 1) {
 923                 jlink_config_kickstart_dump(CMD_CTX, &jlink_cfg);
 924                 return ERROR_OK;
 925         }
 926
 927         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], kickstart);
 928
 929         jlink_cfg.kickstart_power_on_jtag_pin_19 = kickstart;
 930         return ERROR_OK;
 931 }
 932
 933 COMMAND_HANDLER(jlink_handle_jlink_mac_address_command)
 934 {
 935         uint8_t addr[6];
 936         int i;
 937         char *e;
 938         const char *str;
 939
 940         if (CMD_ARGC < 1) {
 941                 jlink_config_mac_address_dump(CMD_CTX, &jlink_cfg);
 942                 return ERROR_OK;
 943         }
 944
 945         str = CMD_ARGV[0];
 946
 947         if ((strlen(str) != 17) || (str[2] != ':' || str[5] != ':' || str[8] != ':' ||
 948                 str[11] != ':' || str[14] != ':')) {
 949                 command_print(CMD_CTX, "ethaddr miss format ff:ff:ff:ff:ff:ff");
 950                 return ERROR_COMMAND_SYNTAX_ERROR;
 951         }
 952
 953         for (i = 5; i >= 0; i--) {
 954                 addr[i] = strtoul(str, &e, 16);
 955                 str = e + 1;
 956         }
 957
 958         if (!(addr[0] | addr[1] | addr[2] | addr[3] | addr[4] | addr[5])) {
 959                 command_print(CMD_CTX, "invalid it's zero mac_address");
 960                 return ERROR_COMMAND_SYNTAX_ERROR;
 961         }
 962
 963         if (!(0x01 & addr[0])) {
 964                 command_print(CMD_CTX, "invalid it's a multicat mac_address");
 965                 return ERROR_COMMAND_SYNTAX_ERROR;
 966         }
 967
 968         memcpy(jlink_cfg.mac_address, addr, sizeof(addr));
 969
 970         return ERROR_OK;
 971 }
 972
 973 static int string_to_ip(const char *s, uint8_t *ip, int *pos)
 974 {
 975         uint8_t lip[4];
 976         char *e;
 977         const char *s_save = s;
 978         int i;
 979
 980         if (!s)
 981                 return -EINVAL;
 982
 983         for (i = 0; i < 4; i++) {
 984                 lip[i] = strtoul(s, &e, 10);
 985
 986                 if (*e != '.' && i != 3)
 987                         return -EINVAL;
 988
 989                 s = e + 1;
 990         }
 991
 992         *pos = e - s_save;
 993
 994         memcpy(ip, lip, sizeof(lip));
 995         return ERROR_OK;
 996 }
 997
 998 static void cpy_ip(uint8_t *dst, uint8_t *src)
 999 {
1000         int i, j;
1001
1002         for (i = 0, j = 3; i < 4; i++, j--)
1003                 dst[i] = src[j];
1004 }
1005
1006 COMMAND_HANDLER(jlink_handle_jlink_ip_command)
1007 {
1008         uint32_t ip_address;
1009         uint32_t subnet_mask = 0;
1010         int i, len;
1011         int ret;
1012         uint8_t subnet_bits = 24;
1013
1014         if (CMD_ARGC < 1) {
1015                 jlink_config_ip_dump(CMD_CTX, &jlink_cfg);
1016                 return ERROR_OK;
1017         }
1018
1019         ret = string_to_ip(CMD_ARGV[0], (uint8_t *)&ip_address, &i);
1020         if (ret != ERROR_OK)
1021                 return ret;
1022
1023         len = strlen(CMD_ARGV[0]);
1024
1025         /* check for this format A.B.C.D/E */
1026
1027         if (i < len) {
1028                 if (CMD_ARGV[0][i] != '/')
1029                         return ERROR_COMMAND_SYNTAX_ERROR;
1030
1031                 COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0] + i + 1, subnet_bits);
1032         } else {
1033                 if (CMD_ARGC > 1) {
1034                         ret = string_to_ip(CMD_ARGV[1], (uint8_t *)&subnet_mask, &i);
1035                         if (ret != ERROR_OK)
1036                                 return ret;
1037                 }
1038         }
1039
1040         if (!subnet_mask)
1041                 subnet_mask = (uint32_t)(subnet_bits < 32 ?
1042                                 ((1ULL << subnet_bits) - 1) : 0xffffffff);
1043
1044         cpy_ip(jlink_cfg.ip_address, (uint8_t *)&ip_address);
1045         cpy_ip(jlink_cfg.subnet_mask, (uint8_t *)&subnet_mask);
1046
1047         return ERROR_OK;
1048 }
1049
1050 COMMAND_HANDLER(jlink_handle_jlink_reset_command)
1051 {
1052         memset(&jlink_cfg, 0xff, sizeof(jlink_cfg));
1053         return ERROR_OK;
1054 }
1055
1056 COMMAND_HANDLER(jlink_handle_jlink_save_command)
1057 {
1058         if (!(jlink_caps & (1 << EMU_CAP_WRITE_CONFIG))) {
1059                 command_print(CMD_CTX, "J-Link write emulator configuration not supported");
1060                 return ERROR_OK;
1061         }
1062
1063         command_print(CMD_CTX, "The J-Link need to be unpluged and repluged ta have the config effective");
1064         return jlink_set_config(&jlink_cfg);
1065 }
1066
1067 COMMAND_HANDLER(jlink_handle_jlink_usb_address_command)
1068 {
1069         uint32_t address;
1070
1071         if (CMD_ARGC < 1) {
1072                 jlink_config_usb_address_dump(CMD_CTX, &jlink_cfg);
1073                 return ERROR_OK;
1074         }
1075
1076         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
1077
1078         if (address > 0x3 && address != 0xff) {
1079                 command_print(CMD_CTX, "USB Address must be between 0x00 and 0x03 or 0xff");
1080                 return ERROR_COMMAND_SYNTAX_ERROR;
1081         }
1082
1083         jlink_cfg.usb_address = address;
1084         return ERROR_OK;
1085 }
1086
1087 COMMAND_HANDLER(jlink_handle_jlink_config_command)
1088 {
1089         struct jlink_config cfg;
1090         int ret = ERROR_OK;
1091
1092         if (CMD_ARGC == 0) {
1093                 if (!(jlink_caps & (1 << EMU_CAP_READ_CONFIG))) {
1094                         command_print(CMD_CTX, "J-Link read emulator configuration not supported");
1095                         goto exit;
1096                 }
1097
1098                 ret = jlink_get_config(&cfg);
1099
1100                 if (ret != ERROR_OK)
1101                         command_print(CMD_CTX, "J-Link read emulator configuration failled");
1102                 else
1103                         jlink_config_dump(CMD_CTX, &jlink_cfg);
1104         }
1105
1106 exit:
1107         return ret;
1108 }
1109
1110 static const struct command_registration jlink_config_subcommand_handlers[] = {
1111         {
1112                 .name = "kickstart",
1113                 .handler = &jlink_handle_jlink_kickstart_command,
1114                 .mode = COMMAND_EXEC,
1115                 .help = "set Kickstart power on JTAG-pin 19.",
1116                 .usage = "[val]",
1117         },
1118         {
1119                 .name = "mac_address",
1120                 .handler = &jlink_handle_jlink_mac_address_command,
1121                 .mode = COMMAND_EXEC,
1122                 .help = "set the MAC Address",
1123                 .usage = "[ff:ff:ff:ff:ff:ff]",
1124         },
1125         {
1126                 .name = "ip",
1127                 .handler = &jlink_handle_jlink_ip_command,
1128                 .mode = COMMAND_EXEC,
1129                 .help = "set the ip address of the J-Link Pro, "
1130                         "where A.B.C.D is the ip, "
1131                         "E the bit of the subnet mask, "
1132                         "F.G.H.I the subnet mask",
1133                 .usage = "[A.B.C.D[/E] [F.G.H.I]]",
1134         },
1135         {
1136                 .name = "reset",
1137                 .handler = &jlink_handle_jlink_reset_command,
1138                 .mode = COMMAND_EXEC,
1139                 .help = "reset the current config",
1140         },
1141         {
1142                 .name = "save",
1143                 .handler = &jlink_handle_jlink_save_command,
1144                 .mode = COMMAND_EXEC,
1145                 .help = "save the current config",
1146         },
1147         {
1148                 .name = "usb_address",
1149                 .handler = &jlink_handle_jlink_usb_address_command,
1150                 .mode = COMMAND_EXEC,
1151                 .help = "set the USB-Address, "
1152                         "This will change the product id",
1153                 .usage = "[0x00 to 0x03 or 0xff]",
1154         },
1155         COMMAND_REGISTRATION_DONE
1156 };
1157
1158 static const struct command_registration jlink_subcommand_handlers[] = {
1159         {
1160                 .name = "caps",
1161                 .handler = &jlink_handle_jlink_caps_command,
1162                 .mode = COMMAND_EXEC,
1163                 .help = "show jlink capabilities",
1164         },
1165         {
1166                 .name = "info",
1167                 .handler = &jlink_handle_jlink_info_command,
1168                 .mode = COMMAND_EXEC,
1169                 .help = "show jlink info",
1170         },
1171         {
1172                 .name = "hw_jtag",
1173                 .handler = &jlink_handle_jlink_hw_jtag_command,
1174                 .mode = COMMAND_EXEC,
1175                 .help = "access J-Link HW JTAG command version",
1176                 .usage = "[2|3]",
1177         },
1178         {
1179                 .name = "config",
1180                 .handler = &jlink_handle_jlink_config_command,
1181                 .mode = COMMAND_EXEC,
1182                 .help = "access J-Link configuration, "
1183                         "if no argument this will dump the config",
1184                 .chain = jlink_config_subcommand_handlers,
1185         },
1186         {
1187                 .name = "pid",
1188                 .handler = &jlink_pid_command,
1189                 .mode = COMMAND_CONFIG,
1190                 .help = "set the pid of the interface we want to use",
1191         },
1192         COMMAND_REGISTRATION_DONE
1193 };
1194
1195 static const struct command_registration jlink_command_handlers[] = {
1196         {
1197                 .name = "jlink",
1198                 .mode = COMMAND_ANY,
1199                 .help = "perform jlink management",
1200                 .chain = jlink_subcommand_handlers,
1201         },
1202         COMMAND_REGISTRATION_DONE
1203 };
1204
1205 struct jtag_interface jlink_interface = {
1206         .name = "jlink",
1207         .commands = jlink_command_handlers,
1208
1209         .execute_queue = jlink_execute_queue,
1210         .speed = jlink_speed,
1211         .speed_div = jlink_speed_div,
1212         .khz = jlink_khz,
1213         .init = jlink_init,
1214         .quit = jlink_quit,
1215 };
1216
1217 /***************************************************************************/
1218 /* J-Link tap functions */
1219
1220
1221 static unsigned tap_length;
1222 static uint8_t tms_buffer[JLINK_TAP_BUFFER_SIZE];
1223 static uint8_t tdi_buffer[JLINK_TAP_BUFFER_SIZE];
1224 static uint8_t tdo_buffer[JLINK_TAP_BUFFER_SIZE];
1225
1226 struct pending_scan_result {
1227         int first;      /* First bit position in tdo_buffer to read */
1228         int length; /* Number of bits to read */
1229         struct scan_command *command; /* Corresponding scan command */
1230         uint8_t *buffer;
1231 };
1232
1233 #define MAX_PENDING_SCAN_RESULTS 256
1234
1235 static int pending_scan_results_length;
1236 static struct pending_scan_result pending_scan_results_buffer[MAX_PENDING_SCAN_RESULTS];
1237
1238 static void jlink_tap_init(void)
1239 {
1240         tap_length = 0;
1241         pending_scan_results_length = 0;
1242 }
1243
1244 static void jlink_tap_ensure_space(int scans, int bits)
1245 {
1246         int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
1247         int available_bits = JLINK_TAP_BUFFER_SIZE * 8 - tap_length - 32;
1248
1249         if (scans > available_scans || bits > available_bits)
1250                 jlink_tap_execute();
1251 }
1252
1253 static void jlink_tap_append_step(int tms, int tdi)
1254 {
1255         int index_var = tap_length / 8;
1256
1257         if (index_var >= JLINK_TAP_BUFFER_SIZE) {
1258                 LOG_ERROR("jlink_tap_append_step: overflow");
1259                 *(uint32_t *)0xFFFFFFFF = 0;
1260                 exit(-1);
1261         }
1262
1263         int bit_index = tap_length % 8;
1264         uint8_t bit = 1 << bit_index;
1265
1266         /* we do not pad TMS, so be sure to initialize all bits */
1267         if (0 == bit_index)
1268                 tms_buffer[index_var] = tdi_buffer[index_var] = 0;
1269
1270         if (tms)
1271                 tms_buffer[index_var] |= bit;
1272         else
1273                 tms_buffer[index_var] &= ~bit;
1274
1275         if (tdi)
1276                 tdi_buffer[index_var] |= bit;
1277         else
1278                 tdi_buffer[index_var] &= ~bit;
1279
1280         tap_length++;
1281 }
1282
1283 static void jlink_tap_append_scan(int length, uint8_t *buffer,
1284                 struct scan_command *command)
1285 {
1286         struct pending_scan_result *pending_scan_result =
1287                 &pending_scan_results_buffer[pending_scan_results_length];
1288         int i;
1289
1290         pending_scan_result->first = tap_length;
1291         pending_scan_result->length = length;
1292         pending_scan_result->command = command;
1293         pending_scan_result->buffer = buffer;
1294
1295         for (i = 0; i < length; i++) {
1296                 int tms = (i < (length - 1)) ? 0 : 1;
1297                 int tdi = (buffer[i / 8] & (1 << (i % 8))) != 0;
1298                 jlink_tap_append_step(tms, tdi);
1299         }
1300         pending_scan_results_length++;
1301 }
1302
1303 /* Pad and send a tap sequence to the device, and receive the answer.
1304  * For the purpose of padding we assume that we are in idle or pause state. */
1305 static int jlink_tap_execute(void)
1306 {
1307         int byte_length;
1308         int i;
1309         int result;
1310
1311         if (!tap_length)
1312                 return ERROR_OK;
1313
1314         /* JLink returns an extra NULL in packet when size of incoming
1315          * message is a multiple of 64, creates problems with USB comms.
1316          * WARNING: This will interfere with tap state counting. */
1317         while ((DIV_ROUND_UP(tap_length, 8) % 64) == 0)
1318                 jlink_tap_append_step((tap_get_state() == TAP_RESET) ? 1 : 0, 0);
1319
1320         /* number of full bytes (plus one if some would be left over) */
1321         byte_length = DIV_ROUND_UP(tap_length, 8);
1322
1323         bool use_jtag3 = jlink_hw_jtag_version >= 3;
1324         usb_out_buffer[0] = use_jtag3 ? EMU_CMD_HW_JTAG3 : EMU_CMD_HW_JTAG2;
1325         usb_out_buffer[1] = 0;
1326         usb_out_buffer[2] = (tap_length >> 0) & 0xff;
1327         usb_out_buffer[3] = (tap_length >> 8) & 0xff;
1328         memcpy(usb_out_buffer + 4, tms_buffer, byte_length);
1329         memcpy(usb_out_buffer + 4 + byte_length, tdi_buffer, byte_length);
1330
1331         jlink_last_state = jtag_debug_state_machine(tms_buffer, tdi_buffer,
1332                         tap_length, jlink_last_state);
1333
1334         result = jlink_usb_message(jlink_handle, 4 + 2 * byte_length, byte_length);
1335         if (result != byte_length) {
1336                 LOG_ERROR("jlink_tap_execute, wrong result %d (expected %d)",
1337                                 result, byte_length);
1338                 jlink_tap_init();
1339                 return ERROR_JTAG_QUEUE_FAILED;
1340         }
1341
1342         memcpy(tdo_buffer, usb_in_buffer, byte_length);
1343
1344         for (i = 0; i < pending_scan_results_length; i++) {
1345                 struct pending_scan_result *pending_scan_result = &pending_scan_results_buffer[i];
1346                 uint8_t *buffer = pending_scan_result->buffer;
1347                 int length = pending_scan_result->length;
1348                 int first = pending_scan_result->first;
1349                 struct scan_command *command = pending_scan_result->command;
1350
1351                 /* Copy to buffer */
1352                 buf_set_buf(tdo_buffer, first, buffer, 0, length);
1353
1354                 DEBUG_JTAG_IO("pending scan result, length = %d", length);
1355
1356                 jlink_debug_buffer(buffer, DIV_ROUND_UP(length, 8));
1357
1358                 if (jtag_read_buffer(buffer, command) != ERROR_OK) {
1359                         jlink_tap_init();
1360                         return ERROR_JTAG_QUEUE_FAILED;
1361                 }
1362
1363                 if (pending_scan_result->buffer != NULL)
1364                         free(pending_scan_result->buffer);
1365         }
1366
1367         jlink_tap_init();
1368         return ERROR_OK;
1369 }
1370
1371 /*****************************************************************************/
1372 /* JLink USB low-level functions */
1373
1374 static struct jlink *jlink_usb_open()
1375 {
1376         struct jtag_libusb_device_handle *devh;
1377         if (jtag_libusb_open(vids, pids, &devh) != ERROR_OK)
1378                 return NULL;
1379
1380         /* BE ***VERY CAREFUL*** ABOUT MAKING CHANGES IN THIS
1381          * AREA!!!!!!!!!!!  The behavior of libusb is not completely
1382          * consistent across Windows, Linux, and Mac OS X platforms.
1383          * The actions taken in the following compiler conditionals may
1384          * not agree with published documentation for libusb, but were
1385          * found to be necessary through trials and tribulations.  Even
1386          * little tweaks can break one or more platforms, so if you do
1387          * make changes test them carefully on all platforms before
1388          * committing them!
1389          */
1390
1391 #if IS_WIN32 == 0
1392
1393         jtag_libusb_reset_device(devh);
1394
1395 #if IS_DARWIN == 0
1396
1397         int timeout = 5;
1398         /* reopen jlink after usb_reset
1399          * on win32 this may take a second or two to re-enumerate */
1400         int retval;
1401         while ((retval = jtag_libusb_open(vids, pids, &devh)) != ERROR_OK) {
1402                 usleep(1000);
1403                 timeout--;
1404                 if (!timeout)
1405                         break;
1406         }
1407         if (ERROR_OK != retval)
1408                 return NULL;
1409 #endif
1410
1411 #endif
1412
1413         /* usb_set_configuration required under win32 */
1414         struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
1415         jtag_libusb_set_configuration(devh, 0);
1416         jtag_libusb_claim_interface(devh, 0);
1417
1418 #if 0
1419         /*
1420          * This makes problems under Mac OS X. And is not needed
1421          * under Windows. Hopefully this will not break a linux build
1422          */
1423         usb_set_altinterface(result->usb_handle, 0);
1424 #endif
1425
1426         jtag_libusb_get_endpoints(udev, &jlink_read_ep, &jlink_write_ep);
1427
1428         struct jlink *result = malloc(sizeof(struct jlink));
1429         result->usb_handle = devh;
1430         return result;
1431 }
1432
1433 static void jlink_usb_close(struct jlink *jlink)
1434 {
1435         jtag_libusb_close(jlink->usb_handle);
1436         free(jlink);
1437 }
1438
1439 /* Send a message and receive the reply. */
1440 static int jlink_usb_message(struct jlink *jlink, int out_length, int in_length)
1441 {
1442         int result;
1443
1444         result = jlink_usb_write(jlink, out_length);
1445         if (result != out_length) {
1446                 LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)",
1447                                 out_length, result);
1448                 return ERROR_JTAG_DEVICE_ERROR;
1449         }
1450
1451         result = jlink_usb_read(jlink, in_length);
1452         if ((result != in_length) && (result != (in_length + 1))) {
1453                 LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)",
1454                                 in_length, result);
1455                 return ERROR_JTAG_DEVICE_ERROR;
1456         }
1457
1458         if (jlink_hw_jtag_version < 3)
1459                 return result;
1460
1461         int result2 = ERROR_OK;
1462         if (result == in_length) {
1463                 /* Must read the result from the EMU too */
1464                 result2 = jlink_usb_read_emu_result(jlink);
1465                 if (1 != result2) {
1466                         LOG_ERROR("jlink_usb_read_emu_result retried requested = 1, "
1467                                         "result=%d, in_length=%i", result2, in_length);
1468                         /* Try again once, should only happen if (in_length%64 == 0) */
1469                         result2 = jlink_usb_read_emu_result(jlink);
1470                         if (1 != result2) {
1471                                 LOG_ERROR("jlink_usb_read_emu_result failed "
1472                                         "(requested = 1, result=%d)", result2);
1473                                 return ERROR_JTAG_DEVICE_ERROR;
1474                         }
1475                 }
1476
1477                 /* Check the result itself */
1478                 result2 = usb_emu_result_buffer[0];
1479         } else {
1480                 /* Save the result, then remove it from return value */
1481                 result2 = usb_in_buffer[result--];
1482         }
1483
1484         if (result2) {
1485                 LOG_ERROR("jlink_usb_message failed with result=%d)", result2);
1486                 return ERROR_JTAG_DEVICE_ERROR;
1487         }
1488
1489         return result;
1490 }
1491
1492 /* calls the given usb_bulk_* function, allowing for the data to
1493  * trickle in with some timeouts  */
1494 static int usb_bulk_with_retries(
1495                 int (*f)(jtag_libusb_device_handle *, int, char *, int, int),
1496                 jtag_libusb_device_handle *dev, int ep,
1497                 char *bytes, int size, int timeout)
1498 {
1499         int tries = 3, count = 0;
1500
1501         while (tries && (count < size)) {
1502                 int result = f(dev, ep, bytes + count, size - count, timeout);
1503                 if (result > 0)
1504                         count += result;
1505                 else if ((-ETIMEDOUT != result) || !--tries)
1506                         return result;
1507         }
1508         return count;
1509 }
1510
1511 static int wrap_usb_bulk_write(jtag_libusb_device_handle *dev, int ep,
1512                 char *buff, int size, int timeout)
1513 {
1514         /* usb_bulk_write() takes const char *buff */
1515         return jtag_libusb_bulk_write(dev, ep, buff, size, timeout);
1516 }
1517
1518 static inline int usb_bulk_write_ex(jtag_libusb_device_handle *dev, int ep,
1519                 char *bytes, int size, int timeout)
1520 {
1521         return usb_bulk_with_retries(&wrap_usb_bulk_write,
1522                         dev, ep, bytes, size, timeout);
1523 }
1524
1525 static inline int usb_bulk_read_ex(jtag_libusb_device_handle *dev, int ep,
1526                 char *bytes, int size, int timeout)
1527 {
1528         return usb_bulk_with_retries(&jtag_libusb_bulk_read,
1529                         dev, ep, bytes, size, timeout);
1530 }
1531
1532 /* Write data from out_buffer to USB. */
1533 static int jlink_usb_write(struct jlink *jlink, int out_length)
1534 {
1535         int result;
1536
1537         if (out_length > JLINK_OUT_BUFFER_SIZE) {
1538                 LOG_ERROR("jlink_write illegal out_length=%d (max=%d)",
1539                                 out_length, JLINK_OUT_BUFFER_SIZE);
1540                 return -1;
1541         }
1542
1543         result = usb_bulk_write_ex(jlink->usb_handle, jlink_write_ep,
1544                 (char *)usb_out_buffer, out_length, JLINK_USB_TIMEOUT);
1545
1546         DEBUG_JTAG_IO("jlink_usb_write, out_length = %d, result = %d",
1547                         out_length, result);
1548
1549         jlink_debug_buffer(usb_out_buffer, out_length);
1550         return result;
1551 }
1552
1553 /* Read data from USB into in_buffer. */
1554 static int jlink_usb_read(struct jlink *jlink, int expected_size)
1555 {
1556         int result = usb_bulk_read_ex(jlink->usb_handle, jlink_read_ep,
1557                 (char *)usb_in_buffer, expected_size, JLINK_USB_TIMEOUT);
1558
1559         DEBUG_JTAG_IO("jlink_usb_read, result = %d", result);
1560
1561         jlink_debug_buffer(usb_in_buffer, result);
1562         return result;
1563 }
1564
1565 /* Read the result from the previous EMU cmd into result_buffer. */
1566 static int jlink_usb_read_emu_result(struct jlink *jlink)
1567 {
1568         int result = usb_bulk_read_ex(jlink->usb_handle, jlink_read_ep,
1569                 (char *)usb_emu_result_buffer, 1 /* JLINK_EMU_RESULT_BUFFER_SIZE */,
1570                 JLINK_USB_TIMEOUT);
1571
1572         DEBUG_JTAG_IO("jlink_usb_read_result, result = %d", result);
1573
1574         jlink_debug_buffer(usb_emu_result_buffer, result);
1575         return result;
1576 }
1577
1578 /*
1579  * Send a message and receive the reply - simple messages.
1580  *
1581  * @param jlink pointer to driver data
1582  * @param out_length data length in @c usb_out_buffer
1583  * @param in_length data length to be read to @c usb_in_buffer
1584  */
1585 static int jlink_usb_io(struct jlink *jlink, int out_length, int in_length)
1586 {
1587         int result;
1588
1589         result = jlink_usb_write(jlink, out_length);
1590         if (result != out_length) {
1591                 LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)",
1592                                 out_length, result);
1593                 return ERROR_JTAG_DEVICE_ERROR;
1594         }
1595
1596         result = jlink_usb_read(jlink, in_length);
1597         if (result != in_length) {
1598                 LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)",
1599                                 in_length, result);
1600                 return ERROR_JTAG_DEVICE_ERROR;
1601         }
1602
1603         /*
1604          * Section 4.2.4 IN-transaction:
1605          * read dummy 0-byte packet if transaction size is
1606          * multiple of 64 bytes but not max. size of 0x8000
1607          */
1608         if ((in_length % 64) == 0 && in_length != 0x8000) {
1609                 char dummy_buffer;
1610                 result = usb_bulk_read_ex(jlink->usb_handle, jlink_read_ep,
1611                         &dummy_buffer, 1, JLINK_USB_TIMEOUT);
1612                 if (result != 0) {
1613                         LOG_ERROR("dummy byte read failed");
1614                         return ERROR_JTAG_DEVICE_ERROR;
1615                 }
1616         }
1617         return ERROR_OK;
1618 }
1619
1620 #ifdef _DEBUG_USB_COMMS_
1621 #define BYTES_PER_LINE  16
1622
1623 static void jlink_debug_buffer(uint8_t *buffer, int length)
1624 {
1625         char line[81];
1626         char s[4];
1627         int i;
1628         int j;
1629
1630         for (i = 0; i < length; i += BYTES_PER_LINE) {
1631                 snprintf(line, 5, "%04x", i);
1632                 for (j = i; j < i + BYTES_PER_LINE && j < length; j++) {
1633                         snprintf(s, 4, " %02x", buffer[j]);
1634                         strcat(line, s);
1635                 }
1636                 LOG_DEBUG("%s", line);
1637         }
1638 }
1639 #endif