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