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  *   Copyright (C) 2015 by Marc Schink                                     *
  12  *   openocd-dev@marcschink.de                                             *
  13  *                                                                         *
  14  *   Copyright (C) 2015 by Paul Fertser                                    *
  15  *   fercerpav@gmail.com                                                   *
  16  *                                                                         *
  17  *   This program is free software; you can redistribute it and/or modify  *
  18  *   it under the terms of the GNU General Public License as published by  *
  19  *   the Free Software Foundation; either version 2 of the License, or     *
  20  *   (at your option) any later version.                                   *
  21  *                                                                         *
  22  *   This program is distributed in the hope that it will be useful,       *
  23  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  24  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  25  *   GNU General Public License for more details.                          *
  26  *                                                                         *
  27  *   You should have received a copy of the GNU General Public License     *
  28  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  29  ***************************************************************************/
  30
  31 #ifdef HAVE_CONFIG_H
  32 #include "config.h"
  33 #endif
  34
  35 #include <stdint.h>
  36 #include <math.h>
  37
  38 #include <jtag/interface.h>
  39 #include <jtag/swd.h>
  40 #include <jtag/commands.h>
  41 #include <jtag/drivers/jtag_usb_common.h>
  42
  43 #include <libjaylink/libjaylink.h>
  44
  45 static struct jaylink_context *jayctx;
  46 static struct jaylink_device_handle *devh;
  47 static struct jaylink_connection conn;
  48 static struct jaylink_connection connlist[JAYLINK_MAX_CONNECTIONS];
  49 static enum jaylink_jtag_version jtag_command_version;
  50 static uint8_t caps[JAYLINK_DEV_EXT_CAPS_SIZE];
  51
  52 static uint32_t serial_number;
  53 static bool use_serial_number;
  54 static bool use_usb_location;
  55 static enum jaylink_usb_address usb_address;
  56 static bool use_usb_address;
  57 static enum jaylink_target_interface iface = JAYLINK_TIF_JTAG;
  58 static bool trace_enabled;
  59
  60 #define JLINK_MAX_SPEED                 12000
  61 #define JLINK_TAP_BUFFER_SIZE   2048
  62
  63 static unsigned int swd_buffer_size = JLINK_TAP_BUFFER_SIZE;
  64
  65 /* 256 byte non-volatile memory */
  66 struct device_config {
  67         uint8_t usb_address;
  68         /* 0ffset 0x01 to 0x03 */
  69         uint8_t reserved_1[3];
  70         uint32_t target_power;
  71         /* 0ffset 0x08 to 0x1f */
  72         uint8_t reserved_2[24];
  73         /* IP only for J-Link Pro */
  74         uint8_t ip_address[4];
  75         uint8_t subnet_mask[4];
  76         /* 0ffset 0x28 to 0x2f */
  77         uint8_t reserved_3[8];
  78         uint8_t mac_address[6];
  79         /* 0ffset 0x36 to 0xff */
  80         uint8_t reserved_4[202];
  81 } __attribute__ ((packed));
  82
  83 static struct device_config config;
  84 static struct device_config tmp_config;
  85
  86 /* Queue command functions */
  87 static void jlink_end_state(tap_state_t state);
  88 static void jlink_state_move(void);
  89 static void jlink_path_move(int num_states, tap_state_t *path);
  90 static void jlink_stableclocks(int num_cycles);
  91 static void jlink_runtest(int num_cycles);
  92 static void jlink_reset(int trst, int srst);
  93 static int jlink_swd_run_queue(void);
  94 static void jlink_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data, uint32_t ap_delay_clk);
  95 static int jlink_swd_switch_seq(enum swd_special_seq seq);
  96
  97 /* J-Link tap buffer functions */
  98 static void jlink_tap_init(void);
  99 static int jlink_flush(void);
 100 /**
 101  * Queue data to go out and in, flushing the queue as many times as
 102  * necessary.
 103  *
 104  * @param out A pointer to TDI data, if NULL, old stale data will be used.
 105  * @param out_offset A bit offset for TDI data.
 106  * @param tms_out A pointer to TMS data, if NULL, zeroes will be emitted.
 107  * @param tms_offset A bit offset for TMS data.
 108  * @param in A pointer to store TDO data to, if NULL the data will be discarded.
 109  * @param in_offset A bit offset for TDO data.
 110  * @param length Amount of bits to transfer out and in.
 111  *
 112  * @retval This function doesn't return any value.
 113  */
 114 static void jlink_clock_data(const uint8_t *out, unsigned out_offset,
 115                              const uint8_t *tms_out, unsigned tms_offset,
 116                              uint8_t *in, unsigned in_offset,
 117                              unsigned length);
 118
 119 static enum tap_state jlink_last_state = TAP_RESET;
 120 static int queued_retval;
 121
 122 /***************************************************************************/
 123 /* External interface implementation */
 124
 125 static void jlink_execute_stableclocks(struct jtag_command *cmd)
 126 {
 127         DEBUG_JTAG_IO("stableclocks %i cycles", cmd->cmd.runtest->num_cycles);
 128         jlink_stableclocks(cmd->cmd.runtest->num_cycles);
 129 }
 130
 131 static void jlink_execute_runtest(struct jtag_command *cmd)
 132 {
 133         DEBUG_JTAG_IO("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles,
 134                 cmd->cmd.runtest->end_state);
 135
 136         jlink_end_state(cmd->cmd.runtest->end_state);
 137         jlink_runtest(cmd->cmd.runtest->num_cycles);
 138 }
 139
 140 static void jlink_execute_statemove(struct jtag_command *cmd)
 141 {
 142         DEBUG_JTAG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
 143
 144         jlink_end_state(cmd->cmd.statemove->end_state);
 145         jlink_state_move();
 146 }
 147
 148 static void jlink_execute_pathmove(struct jtag_command *cmd)
 149 {
 150         DEBUG_JTAG_IO("pathmove: %i states, end in %i",
 151                 cmd->cmd.pathmove->num_states,
 152                 cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
 153
 154         jlink_path_move(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
 155 }
 156
 157 static void jlink_execute_scan(struct jtag_command *cmd)
 158 {
 159         DEBUG_JTAG_IO("%s type:%d", cmd->cmd.scan->ir_scan ? "IRSCAN" : "DRSCAN",
 160                 jtag_scan_type(cmd->cmd.scan));
 161
 162         /* Make sure there are no trailing fields with num_bits == 0, or the logic below will fail. */
 163         while (cmd->cmd.scan->num_fields > 0
 164                         && cmd->cmd.scan->fields[cmd->cmd.scan->num_fields - 1].num_bits == 0) {
 165                 cmd->cmd.scan->num_fields--;
 166                 LOG_DEBUG("discarding trailing empty field");
 167         }
 168
 169         if (cmd->cmd.scan->num_fields == 0) {
 170                 LOG_DEBUG("empty scan, doing nothing");
 171                 return;
 172         }
 173
 174         if (cmd->cmd.scan->ir_scan) {
 175                 if (tap_get_state() != TAP_IRSHIFT) {
 176                         jlink_end_state(TAP_IRSHIFT);
 177                         jlink_state_move();
 178                 }
 179         } else {
 180                 if (tap_get_state() != TAP_DRSHIFT) {
 181                         jlink_end_state(TAP_DRSHIFT);
 182                         jlink_state_move();
 183                 }
 184         }
 185
 186         jlink_end_state(cmd->cmd.scan->end_state);
 187
 188         struct scan_field *field = cmd->cmd.scan->fields;
 189         unsigned scan_size = 0;
 190
 191         for (int i = 0; i < cmd->cmd.scan->num_fields; i++, field++) {
 192                 scan_size += field->num_bits;
 193                 DEBUG_JTAG_IO("%s%s field %d/%d %d bits",
 194                         field->in_value ? "in" : "",
 195                         field->out_value ? "out" : "",
 196                         i,
 197                         cmd->cmd.scan->num_fields,
 198                         field->num_bits);
 199
 200                 if (i == cmd->cmd.scan->num_fields - 1 && tap_get_state() != tap_get_end_state()) {
 201                         /* Last field, and we're leaving IRSHIFT/DRSHIFT. Clock last bit during tap
 202                          * movement. This last field can't have length zero, it was checked above. */
 203                         jlink_clock_data(field->out_value,
 204                                          0,
 205                                          NULL,
 206                                          0,
 207                                          field->in_value,
 208                                          0,
 209                                          field->num_bits - 1);
 210                         uint8_t last_bit = 0;
 211                         if (field->out_value)
 212                                 bit_copy(&last_bit, 0, field->out_value, field->num_bits - 1, 1);
 213                         uint8_t tms_bits = 0x01;
 214                         jlink_clock_data(&last_bit,
 215                                          0,
 216                                          &tms_bits,
 217                                          0,
 218                                          field->in_value,
 219                                          field->num_bits - 1,
 220                                          1);
 221                         tap_set_state(tap_state_transition(tap_get_state(), 1));
 222                         jlink_clock_data(NULL,
 223                                          0,
 224                                          &tms_bits,
 225                                          1,
 226                                          NULL,
 227                                          0,
 228                                          1);
 229                         tap_set_state(tap_state_transition(tap_get_state(), 0));
 230                 } else
 231                         jlink_clock_data(field->out_value,
 232                                          0,
 233                                          NULL,
 234                                          0,
 235                                          field->in_value,
 236                                          0,
 237                                          field->num_bits);
 238         }
 239
 240         if (tap_get_state() != tap_get_end_state()) {
 241                 jlink_end_state(tap_get_end_state());
 242                 jlink_state_move();
 243         }
 244
 245         DEBUG_JTAG_IO("%s scan, %i bits, end in %s",
 246                 (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
 247                 tap_state_name(tap_get_end_state()));
 248 }
 249
 250 static void jlink_execute_reset(struct jtag_command *cmd)
 251 {
 252         DEBUG_JTAG_IO("reset trst: %i srst %i", cmd->cmd.reset->trst,
 253                 cmd->cmd.reset->srst);
 254
 255         jlink_flush();
 256         jlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 257         jlink_flush();
 258 }
 259
 260 static void jlink_execute_sleep(struct jtag_command *cmd)
 261 {
 262         DEBUG_JTAG_IO("sleep %" PRIi32 "", cmd->cmd.sleep->us);
 263         jlink_flush();
 264         jtag_sleep(cmd->cmd.sleep->us);
 265 }
 266
 267 static int jlink_execute_command(struct jtag_command *cmd)
 268 {
 269         switch (cmd->type) {
 270                 case JTAG_STABLECLOCKS:
 271                         jlink_execute_stableclocks(cmd);
 272                         break;
 273                 case JTAG_RUNTEST:
 274                         jlink_execute_runtest(cmd);
 275                         break;
 276                 case JTAG_TLR_RESET:
 277                         jlink_execute_statemove(cmd);
 278                         break;
 279                 case JTAG_PATHMOVE:
 280                         jlink_execute_pathmove(cmd);
 281                         break;
 282                 case JTAG_SCAN:
 283                         jlink_execute_scan(cmd);
 284                         break;
 285                 case JTAG_RESET:
 286                         jlink_execute_reset(cmd);
 287                         break;
 288                 case JTAG_SLEEP:
 289                         jlink_execute_sleep(cmd);
 290                         break;
 291                 default:
 292                         LOG_ERROR("BUG: Unknown JTAG command type encountered.");
 293                         return ERROR_JTAG_QUEUE_FAILED;
 294         }
 295
 296         return ERROR_OK;
 297 }
 298
 299 static int jlink_execute_queue(void)
 300 {
 301         int ret;
 302         struct jtag_command *cmd = jtag_command_queue;
 303
 304         while (cmd != NULL) {
 305                 ret = jlink_execute_command(cmd);
 306
 307                 if (ret != ERROR_OK)
 308                         return ret;
 309
 310                 cmd = cmd->next;
 311         }
 312
 313         return jlink_flush();
 314 }
 315
 316 static int jlink_speed(int speed)
 317 {
 318         int ret;
 319         struct jaylink_speed tmp;
 320         int max_speed;
 321
 322         if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_SPEEDS)) {
 323                 ret = jaylink_get_speeds(devh, &tmp);
 324
 325                 if (ret != JAYLINK_OK) {
 326                         LOG_ERROR("jaylink_get_speeds() failed: %s.",
 327                                 jaylink_strerror(ret));
 328                         return ERROR_JTAG_DEVICE_ERROR;
 329                 }
 330
 331                 tmp.freq /= 1000;
 332                 max_speed = tmp.freq / tmp.div;
 333         } else {
 334                 max_speed = JLINK_MAX_SPEED;
 335         }
 336
 337         if (!speed) {
 338                 if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_ADAPTIVE_CLOCKING)) {
 339                         LOG_ERROR("Adaptive clocking is not supported by the device.");
 340                         return ERROR_JTAG_NOT_IMPLEMENTED;
 341                 }
 342
 343                 speed = JAYLINK_SPEED_ADAPTIVE_CLOCKING;
 344         } else if (speed > max_speed) {
 345                 LOG_INFO("Reduced speed from %d kHz to %d kHz (maximum).", speed,
 346                         max_speed);
 347                 speed = max_speed;
 348         }
 349
 350         ret = jaylink_set_speed(devh, speed);
 351
 352         if (ret != JAYLINK_OK) {
 353                 LOG_ERROR("jaylink_set_speed() failed: %s.",
 354                         jaylink_strerror(ret));
 355                 return ERROR_JTAG_DEVICE_ERROR;
 356         }
 357
 358         return ERROR_OK;
 359 }
 360
 361 static int jlink_speed_div(int speed, int *khz)
 362 {
 363         *khz = speed;
 364
 365         return ERROR_OK;
 366 }
 367
 368 static int jlink_khz(int khz, int *jtag_speed)
 369 {
 370         *jtag_speed = khz;
 371
 372         return ERROR_OK;
 373 }
 374
 375 static bool read_device_config(struct device_config *cfg)
 376 {
 377         int ret;
 378
 379         ret = jaylink_read_raw_config(devh, (uint8_t *)cfg);
 380
 381         if (ret != JAYLINK_OK) {
 382                 LOG_ERROR("jaylink_read_raw_config() failed: %s.",
 383                         jaylink_strerror(ret));
 384                 return false;
 385         }
 386
 387         if (cfg->usb_address == 0xff)
 388                 cfg->usb_address = 0x00;
 389
 390         if (cfg->target_power == 0xffffffff)
 391                 cfg->target_power = 0;
 392
 393         return true;
 394 }
 395
 396 static int select_interface(void)
 397 {
 398         int ret;
 399         uint32_t interfaces;
 400
 401         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_SELECT_TIF)) {
 402                 if (iface != JAYLINK_TIF_JTAG) {
 403                         LOG_ERROR("Device supports JTAG transport only.");
 404                         return ERROR_JTAG_INIT_FAILED;
 405                 }
 406
 407                 return ERROR_OK;
 408         }
 409
 410         ret = jaylink_get_available_interfaces(devh, &interfaces);
 411
 412         if (ret != JAYLINK_OK) {
 413                 LOG_ERROR("jaylink_get_available_interfaces() failed: %s.",
 414                         jaylink_strerror(ret));
 415                 return ERROR_JTAG_INIT_FAILED;
 416         }
 417
 418         if (!(interfaces & (1 << iface))) {
 419                 LOG_ERROR("Selected transport is not supported by the device.");
 420                 return ERROR_JTAG_INIT_FAILED;
 421         }
 422
 423         ret = jaylink_select_interface(devh, iface, NULL);
 424
 425         if (ret < 0) {
 426                 LOG_ERROR("jaylink_select_interface() failed: %s.",
 427                         jaylink_strerror(ret));
 428                 return ERROR_JTAG_INIT_FAILED;
 429         }
 430
 431         return ERROR_OK;
 432 }
 433
 434 static int jlink_register(void)
 435 {
 436         int ret;
 437         size_t i;
 438         bool handle_found;
 439         size_t count;
 440
 441         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_REGISTER))
 442                 return ERROR_OK;
 443
 444         ret = jaylink_register(devh, &conn, connlist, &count);
 445
 446         if (ret != JAYLINK_OK) {
 447                 LOG_ERROR("jaylink_register() failed: %s.", jaylink_strerror(ret));
 448                 return ERROR_FAIL;
 449         }
 450
 451         handle_found = false;
 452
 453         for (i = 0; i < count; i++) {
 454                 if (connlist[i].handle == conn.handle) {
 455                         handle_found = true;
 456                         break;
 457                 }
 458         }
 459
 460         if (!handle_found) {
 461                 LOG_ERROR("Registration failed: maximum number of connections on the "
 462                         "device reached.");
 463                 return ERROR_FAIL;
 464         }
 465
 466         return ERROR_OK;
 467 }
 468
 469 /*
 470  * Adjust the SWD transaction buffer size depending on the free device internal
 471  * memory. This ensures that the SWD transactions sent to the device do not
 472  * exceed the internal memory of the device.
 473  */
 474 static bool adjust_swd_buffer_size(void)
 475 {
 476         int ret;
 477         uint32_t tmp;
 478
 479         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_FREE_MEMORY))
 480                 return true;
 481
 482         ret = jaylink_get_free_memory(devh, &tmp);
 483
 484         if (ret != JAYLINK_OK) {
 485                 LOG_ERROR("jaylink_get_free_memory() failed: %s.",
 486                         jaylink_strerror(ret));
 487                 return false;
 488         }
 489
 490         if (tmp < 143) {
 491                 LOG_ERROR("Not enough free device internal memory: %u bytes.", tmp);
 492                 return false;
 493         }
 494
 495         tmp = MIN(JLINK_TAP_BUFFER_SIZE, (tmp - 16) / 2);
 496
 497         if (tmp != swd_buffer_size) {
 498                 swd_buffer_size = tmp;
 499                 LOG_DEBUG("Adjusted SWD transaction buffer size to %u bytes.",
 500                         swd_buffer_size);
 501         }
 502
 503         return true;
 504 }
 505
 506 static int jaylink_log_handler(const struct jaylink_context *ctx,
 507                 enum jaylink_log_level level, const char *format, va_list args,
 508                 void *user_data)
 509 {
 510         enum log_levels tmp;
 511
 512         switch (level) {
 513         case JAYLINK_LOG_LEVEL_ERROR:
 514                 tmp = LOG_LVL_ERROR;
 515                 break;
 516         case JAYLINK_LOG_LEVEL_WARNING:
 517                 tmp = LOG_LVL_WARNING;
 518                 break;
 519         /*
 520          * Forward info messages to the debug output because they are more verbose
 521          * than info messages of OpenOCD.
 522          */
 523         case JAYLINK_LOG_LEVEL_INFO:
 524         case JAYLINK_LOG_LEVEL_DEBUG:
 525                 tmp = LOG_LVL_DEBUG;
 526                 break;
 527         case JAYLINK_LOG_LEVEL_DEBUG_IO:
 528                 tmp = LOG_LVL_DEBUG_IO;
 529                 break;
 530         default:
 531                 tmp = LOG_LVL_WARNING;
 532         }
 533
 534         log_vprintf_lf(tmp, __FILE__, __LINE__, __func__, format, args);
 535
 536         return 0;
 537 }
 538
 539 static bool jlink_usb_location_equal(struct jaylink_device *dev)
 540 {
 541         int retval;
 542         uint8_t bus;
 543         uint8_t *ports;
 544         size_t num_ports;
 545         bool equal = false;
 546
 547         retval = jaylink_device_get_usb_bus_ports(dev, &bus, &ports, &num_ports);
 548
 549         if (retval == JAYLINK_ERR_NOT_SUPPORTED) {
 550                 return false;
 551         } else if (retval != JAYLINK_OK) {
 552                 LOG_WARNING("jaylink_device_get_usb_bus_ports() failed: %s.",
 553                         jaylink_strerror(retval));
 554                 return false;
 555         }
 556
 557         equal = jtag_usb_location_equal(bus, ports,     num_ports);
 558         free(ports);
 559
 560         return equal;
 561 }
 562
 563
 564 static int jlink_init(void)
 565 {
 566         int ret;
 567         struct jaylink_device **devs;
 568         unsigned int i;
 569         bool found_device;
 570         uint32_t tmp;
 571         char *firmware_version;
 572         struct jaylink_hardware_version hwver;
 573         struct jaylink_hardware_status hwstatus;
 574         enum jaylink_usb_address address;
 575         size_t length;
 576         size_t num_devices;
 577         uint32_t host_interfaces;
 578
 579         LOG_DEBUG("Using libjaylink %s (compiled with %s).",
 580                 jaylink_version_package_get_string(), JAYLINK_VERSION_PACKAGE_STRING);
 581
 582         if (!jaylink_library_has_cap(JAYLINK_CAP_HIF_USB) && use_usb_address) {
 583                 LOG_ERROR("J-Link driver does not support USB devices.");
 584                 return ERROR_JTAG_INIT_FAILED;
 585         }
 586
 587         ret = jaylink_init(&jayctx);
 588
 589         if (ret != JAYLINK_OK) {
 590                 LOG_ERROR("jaylink_init() failed: %s.", jaylink_strerror(ret));
 591                 return ERROR_JTAG_INIT_FAILED;
 592         }
 593
 594         ret = jaylink_log_set_callback(jayctx, &jaylink_log_handler, NULL);
 595
 596         if (ret != JAYLINK_OK) {
 597                 LOG_ERROR("jaylink_log_set_callback() failed: %s.",
 598                         jaylink_strerror(ret));
 599                 jaylink_exit(jayctx);
 600                 return ERROR_JTAG_INIT_FAILED;
 601         }
 602
 603         host_interfaces = JAYLINK_HIF_USB;
 604
 605         if (use_serial_number)
 606                 host_interfaces |= JAYLINK_HIF_TCP;
 607
 608         ret = jaylink_discovery_scan(jayctx, host_interfaces);
 609
 610         if (ret != JAYLINK_OK) {
 611                 LOG_ERROR("jaylink_discovery_scan() failed: %s.",
 612                         jaylink_strerror(ret));
 613                 jaylink_exit(jayctx);
 614                 return ERROR_JTAG_INIT_FAILED;
 615         }
 616
 617         ret = jaylink_get_devices(jayctx, &devs, &num_devices);
 618
 619         if (ret != JAYLINK_OK) {
 620                 LOG_ERROR("jaylink_get_devices() failed: %s.", jaylink_strerror(ret));
 621                 jaylink_exit(jayctx);
 622                 return ERROR_JTAG_INIT_FAILED;
 623         }
 624
 625         use_usb_location = (jtag_usb_get_location() != NULL);
 626
 627         if (!use_serial_number && !use_usb_address && !use_usb_location && num_devices > 1) {
 628                 LOG_ERROR("Multiple devices found, specify the desired device.");
 629                 jaylink_free_devices(devs, true);
 630                 jaylink_exit(jayctx);
 631                 return ERROR_JTAG_INIT_FAILED;
 632         }
 633
 634         found_device = false;
 635
 636         for (i = 0; devs[i]; i++) {
 637                 struct jaylink_device *dev = devs[i];
 638
 639                 if (use_serial_number) {
 640                         ret = jaylink_device_get_serial_number(dev, &tmp);
 641
 642                         if (ret == JAYLINK_ERR_NOT_AVAILABLE) {
 643                                 continue;
 644                         } else if (ret != JAYLINK_OK) {
 645                                 LOG_WARNING("jaylink_device_get_serial_number() failed: %s.",
 646                                         jaylink_strerror(ret));
 647                                 continue;
 648                         }
 649
 650                         if (serial_number != tmp)
 651                                 continue;
 652                 }
 653
 654                 if (use_usb_address) {
 655                         ret = jaylink_device_get_usb_address(dev, &address);
 656
 657                         if (ret == JAYLINK_ERR_NOT_SUPPORTED) {
 658                                 continue;
 659                         } else if (ret != JAYLINK_OK) {
 660                                 LOG_WARNING("jaylink_device_get_usb_address() failed: %s.",
 661                                         jaylink_strerror(ret));
 662                                 continue;
 663                         }
 664
 665                         if (usb_address != address)
 666                                 continue;
 667                 }
 668
 669                 if (use_usb_location && !jlink_usb_location_equal(dev))
 670                         continue;
 671
 672                 ret = jaylink_open(dev, &devh);
 673
 674                 if (ret == JAYLINK_OK) {
 675                         found_device = true;
 676                         break;
 677                 }
 678
 679                 LOG_ERROR("Failed to open device: %s.", jaylink_strerror(ret));
 680         }
 681
 682         jaylink_free_devices(devs, true);
 683
 684         if (!found_device) {
 685                 LOG_ERROR("No J-Link device found.");
 686                 jaylink_exit(jayctx);
 687                 return ERROR_JTAG_INIT_FAILED;
 688         }
 689
 690         /*
 691          * Be careful with changing the following initialization sequence because
 692          * some devices are known to be sensitive regarding the order.
 693          */
 694
 695         ret = jaylink_get_firmware_version(devh, &firmware_version, &length);
 696
 697         if (ret != JAYLINK_OK) {
 698                 LOG_ERROR("jaylink_get_firmware_version() failed: %s.",
 699                         jaylink_strerror(ret));
 700                 jaylink_close(devh);
 701                 jaylink_exit(jayctx);
 702                 return ERROR_JTAG_INIT_FAILED;
 703         } else if (length > 0) {
 704                 LOG_INFO("%s", firmware_version);
 705                 free(firmware_version);
 706         } else {
 707                 LOG_WARNING("Device responds empty firmware version string.");
 708         }
 709
 710         memset(caps, 0, JAYLINK_DEV_EXT_CAPS_SIZE);
 711         ret = jaylink_get_caps(devh, caps);
 712
 713         if (ret != JAYLINK_OK) {
 714                 LOG_ERROR("jaylink_get_caps() failed: %s.", jaylink_strerror(ret));
 715                 jaylink_close(devh);
 716                 jaylink_exit(jayctx);
 717                 return ERROR_JTAG_INIT_FAILED;
 718         }
 719
 720         if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_EXT_CAPS)) {
 721                 ret = jaylink_get_extended_caps(devh, caps);
 722
 723                 if (ret != JAYLINK_OK) {
 724                         LOG_ERROR("jaylink_get_extended_caps() failed:  %s.",
 725                                 jaylink_strerror(ret));
 726                         jaylink_close(devh);
 727                         jaylink_exit(jayctx);
 728                         return ERROR_JTAG_INIT_FAILED;
 729                 }
 730         }
 731
 732         jtag_command_version = JAYLINK_JTAG_VERSION_2;
 733
 734         if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_HW_VERSION)) {
 735                 ret = jaylink_get_hardware_version(devh, &hwver);
 736
 737                 if (ret != JAYLINK_OK) {
 738                         LOG_ERROR("Failed to retrieve hardware version: %s.",
 739                                 jaylink_strerror(ret));
 740                         jaylink_close(devh);
 741                         jaylink_exit(jayctx);
 742                         return ERROR_JTAG_INIT_FAILED;
 743                 }
 744
 745                 LOG_INFO("Hardware version: %u.%02u", hwver.major, hwver.minor);
 746
 747                 if (hwver.major >= 5)
 748                         jtag_command_version = JAYLINK_JTAG_VERSION_3;
 749         }
 750
 751         if (iface == JAYLINK_TIF_SWD) {
 752                 /*
 753                  * Adjust the SWD transaction buffer size in case there is already
 754                  * allocated memory on the device. This happens for example if the
 755                  * memory for SWO capturing is still allocated because the software
 756                  * which used the device before has not been shut down properly.
 757                  */
 758                 if (!adjust_swd_buffer_size()) {
 759                         jaylink_close(devh);
 760                         jaylink_exit(jayctx);
 761                         return ERROR_JTAG_INIT_FAILED;
 762                 }
 763         }
 764
 765         if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG)) {
 766                 if (!read_device_config(&config)) {
 767                         LOG_ERROR("Failed to read device configuration data.");
 768                         jaylink_close(devh);
 769                         jaylink_exit(jayctx);
 770                         return ERROR_JTAG_INIT_FAILED;
 771                 }
 772
 773                 memcpy(&tmp_config, &config, sizeof(struct device_config));
 774         }
 775
 776         ret = jaylink_get_hardware_status(devh, &hwstatus);
 777
 778         if (ret != JAYLINK_OK) {
 779                 LOG_ERROR("jaylink_get_hardware_status() failed: %s.",
 780                         jaylink_strerror(ret));
 781                 jaylink_close(devh);
 782                 jaylink_exit(jayctx);
 783                 return ERROR_JTAG_INIT_FAILED;
 784         }
 785
 786         LOG_INFO("VTarget = %u.%03u V", hwstatus.target_voltage / 1000,
 787                 hwstatus.target_voltage % 1000);
 788
 789         conn.handle = 0;
 790         conn.pid = 0;
 791         strcpy(conn.hid, "0.0.0.0");
 792         conn.iid = 0;
 793         conn.cid = 0;
 794
 795         ret = jlink_register();
 796
 797         if (ret != ERROR_OK) {
 798                 jaylink_close(devh);
 799                 jaylink_exit(jayctx);
 800                 return ERROR_JTAG_INIT_FAILED;
 801         }
 802
 803         ret = select_interface();
 804
 805         if (ret != ERROR_OK) {
 806                 jaylink_close(devh);
 807                 jaylink_exit(jayctx);
 808                 return ret;
 809         }
 810
 811         jlink_reset(0, 0);
 812         jtag_sleep(3000);
 813         jlink_tap_init();
 814
 815         jlink_speed(jtag_get_speed_khz());
 816
 817         if (iface == JAYLINK_TIF_JTAG) {
 818                 /*
 819                  * J-Link devices with firmware version v5 and v6 seems to have an issue
 820                  * if the first tap move is not divisible by 8, so we send a TLR on
 821                  * first power up.
 822                  */
 823                 uint8_t tms = 0xff;
 824                 jlink_clock_data(NULL, 0, &tms, 0, NULL, 0, 8);
 825
 826                 jlink_flush();
 827         }
 828
 829         return ERROR_OK;
 830 }
 831
 832 static int jlink_quit(void)
 833 {
 834         int ret;
 835         size_t count;
 836
 837         if (trace_enabled) {
 838                 ret = jaylink_swo_stop(devh);
 839
 840                 if (ret != JAYLINK_OK)
 841                         LOG_ERROR("jaylink_swo_stop() failed: %s.", jaylink_strerror(ret));
 842         }
 843
 844         if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_REGISTER)) {
 845                 ret = jaylink_unregister(devh, &conn, connlist, &count);
 846
 847                 if (ret != JAYLINK_OK)
 848                         LOG_ERROR("jaylink_unregister() failed: %s.",
 849                                 jaylink_strerror(ret));
 850         }
 851
 852         jaylink_close(devh);
 853         jaylink_exit(jayctx);
 854
 855         return ERROR_OK;
 856 }
 857
 858 /***************************************************************************/
 859 /* Queue command implementations */
 860
 861 static void jlink_end_state(tap_state_t state)
 862 {
 863         if (tap_is_state_stable(state))
 864                 tap_set_end_state(state);
 865         else {
 866                 LOG_ERROR("BUG: %i is not a valid end state", state);
 867                 exit(-1);
 868         }
 869 }
 870
 871 /* Goes to the end state. */
 872 static void jlink_state_move(void)
 873 {
 874         uint8_t tms_scan;
 875         uint8_t tms_scan_bits;
 876
 877         tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 878         tms_scan_bits = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
 879
 880         jlink_clock_data(NULL, 0, &tms_scan, 0, NULL, 0, tms_scan_bits);
 881
 882         tap_set_state(tap_get_end_state());
 883 }
 884
 885 static void jlink_path_move(int num_states, tap_state_t *path)
 886 {
 887         int i;
 888         uint8_t tms = 0xff;
 889
 890         for (i = 0; i < num_states; i++) {
 891                 if (path[i] == tap_state_transition(tap_get_state(), false))
 892                         jlink_clock_data(NULL, 0, NULL, 0, NULL, 0, 1);
 893                 else if (path[i] == tap_state_transition(tap_get_state(), true))
 894                         jlink_clock_data(NULL, 0, &tms, 0, NULL, 0, 1);
 895                 else {
 896                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition.",
 897                                 tap_state_name(tap_get_state()), tap_state_name(path[i]));
 898                         exit(-1);
 899                 }
 900
 901                 tap_set_state(path[i]);
 902         }
 903
 904         tap_set_end_state(tap_get_state());
 905 }
 906
 907 static void jlink_stableclocks(int num_cycles)
 908 {
 909         int i;
 910
 911         uint8_t tms = tap_get_state() == TAP_RESET;
 912         /* Execute num_cycles. */
 913         for (i = 0; i < num_cycles; i++)
 914                 jlink_clock_data(NULL, 0, &tms, 0, NULL, 0, 1);
 915 }
 916
 917 static void jlink_runtest(int num_cycles)
 918 {
 919         tap_state_t saved_end_state = tap_get_end_state();
 920
 921         /* Only do a state_move when we're not already in IDLE. */
 922         if (tap_get_state() != TAP_IDLE) {
 923                 jlink_end_state(TAP_IDLE);
 924                 jlink_state_move();
 925                 /* num_cycles--; */
 926         }
 927
 928         jlink_stableclocks(num_cycles);
 929
 930         /* Finish in end_state. */
 931         jlink_end_state(saved_end_state);
 932
 933         if (tap_get_state() != tap_get_end_state())
 934                 jlink_state_move();
 935 }
 936
 937 static void jlink_reset(int trst, int srst)
 938 {
 939         LOG_DEBUG("TRST: %i, SRST: %i.", trst, srst);
 940
 941         /* Signals are active low. */
 942         if (srst == 0)
 943                 jaylink_set_reset(devh);
 944
 945         if (srst == 1)
 946                 jaylink_clear_reset(devh);
 947
 948         if (trst == 1)
 949                 jaylink_jtag_clear_trst(devh);
 950
 951         if (trst == 0)
 952                 jaylink_jtag_set_trst(devh);
 953 }
 954
 955 COMMAND_HANDLER(jlink_usb_command)
 956 {
 957         int tmp;
 958
 959         if (CMD_ARGC != 1) {
 960                 command_print(CMD, "Need exactly one argument for jlink usb.");
 961                 return ERROR_COMMAND_SYNTAX_ERROR;
 962         }
 963
 964         if (sscanf(CMD_ARGV[0], "%i", &tmp) != 1) {
 965                 command_print(CMD, "Invalid USB address: %s.", CMD_ARGV[0]);
 966                 return ERROR_FAIL;
 967         }
 968
 969         if (tmp < JAYLINK_USB_ADDRESS_0 || tmp > JAYLINK_USB_ADDRESS_3) {
 970                 command_print(CMD, "Invalid USB address: %s.", CMD_ARGV[0]);
 971                 return ERROR_FAIL;
 972         }
 973
 974         usb_address = tmp;
 975
 976         use_serial_number = false;
 977         use_usb_address = true;
 978
 979         return ERROR_OK;
 980 }
 981
 982 COMMAND_HANDLER(jlink_serial_command)
 983 {
 984         int ret;
 985
 986         if (CMD_ARGC != 1) {
 987                 command_print(CMD, "Need exactly one argument for jlink serial.");
 988                 return ERROR_COMMAND_SYNTAX_ERROR;
 989         }
 990
 991         ret = jaylink_parse_serial_number(CMD_ARGV[0], &serial_number);
 992
 993         if (ret == JAYLINK_ERR) {
 994                 command_print(CMD, "Invalid serial number: %s.", CMD_ARGV[0]);
 995                 return ERROR_FAIL;
 996         } else if (ret != JAYLINK_OK) {
 997                 command_print(CMD, "jaylink_parse_serial_number() failed: %s.",
 998                         jaylink_strerror(ret));
 999                 return ERROR_FAIL;
1000         }
1001
1002         use_serial_number = true;
1003         use_usb_address = false;
1004
1005         return ERROR_OK;
1006 }
1007
1008 COMMAND_HANDLER(jlink_handle_hwstatus_command)
1009 {
1010         int ret;
1011         struct jaylink_hardware_status status;
1012
1013         ret = jaylink_get_hardware_status(devh, &status);
1014
1015         if (ret != JAYLINK_OK) {
1016                 command_print(CMD, "jaylink_get_hardware_status() failed: %s.",
1017                         jaylink_strerror(ret));
1018                 return ERROR_FAIL;
1019         }
1020
1021         command_print(CMD, "VTarget = %u.%03u V",
1022                 status.target_voltage / 1000, status.target_voltage % 1000);
1023
1024         command_print(CMD, "TCK = %u TDI = %u TDO = %u TMS = %u SRST = %u "
1025                 "TRST = %u", status.tck, status.tdi, status.tdo, status.tms,
1026                 status.tres, status.trst);
1027
1028         if (status.target_voltage < 1500)
1029                 command_print(CMD, "Target voltage too low. Check target power.");
1030
1031         return ERROR_OK;
1032 }
1033
1034 COMMAND_HANDLER(jlink_handle_free_memory_command)
1035 {
1036         int ret;
1037         uint32_t tmp;
1038
1039         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_FREE_MEMORY)) {
1040                 command_print(CMD, "Retrieval of free memory is not supported by "
1041                         "the device.");
1042                 return ERROR_OK;
1043         }
1044
1045         ret = jaylink_get_free_memory(devh, &tmp);
1046
1047         if (ret != JAYLINK_OK) {
1048                 command_print(CMD, "jaylink_get_free_memory() failed: %s.",
1049                         jaylink_strerror(ret));
1050                 return ERROR_FAIL;
1051         }
1052
1053         command_print(CMD, "Device has %u bytes of free memory.", tmp);
1054
1055         return ERROR_OK;
1056 }
1057
1058 COMMAND_HANDLER(jlink_handle_jlink_jtag_command)
1059 {
1060         int tmp;
1061         int version;
1062
1063         if (!CMD_ARGC) {
1064                 switch (jtag_command_version) {
1065                         case JAYLINK_JTAG_VERSION_2:
1066                                 version = 2;
1067                                 break;
1068                         case JAYLINK_JTAG_VERSION_3:
1069                                 version = 3;
1070                                 break;
1071                         default:
1072                                 return ERROR_FAIL;
1073                 }
1074
1075                 command_print(CMD, "JTAG command version: %i", version);
1076         } else if (CMD_ARGC == 1) {
1077                 if (sscanf(CMD_ARGV[0], "%i", &tmp) != 1) {
1078                         command_print(CMD, "Invalid argument: %s.", CMD_ARGV[0]);
1079                         return ERROR_COMMAND_SYNTAX_ERROR;
1080                 }
1081
1082                 switch (tmp) {
1083                         case 2:
1084                                 jtag_command_version = JAYLINK_JTAG_VERSION_2;
1085                                 break;
1086                         case 3:
1087                                 jtag_command_version = JAYLINK_JTAG_VERSION_3;
1088                                 break;
1089                         default:
1090                                 command_print(CMD, "Invalid argument: %s.", CMD_ARGV[0]);
1091                                 return ERROR_COMMAND_SYNTAX_ERROR;
1092                 }
1093         } else {
1094                 command_print(CMD, "Need exactly one argument for jlink jtag.");
1095                 return ERROR_COMMAND_SYNTAX_ERROR;
1096         }
1097
1098         return ERROR_OK;
1099 }
1100
1101 COMMAND_HANDLER(jlink_handle_target_power_command)
1102 {
1103         int ret;
1104         int enable;
1105
1106         if (CMD_ARGC != 1) {
1107                 command_print(CMD, "Need exactly one argument for jlink "
1108                         "targetpower.");
1109                 return ERROR_COMMAND_SYNTAX_ERROR;
1110         }
1111
1112         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_SET_TARGET_POWER)) {
1113                 command_print(CMD, "Target power supply is not supported by the "
1114                         "device.");
1115                 return ERROR_OK;
1116         }
1117
1118         if (!strcmp(CMD_ARGV[0], "on")) {
1119                 enable = true;
1120         } else if (!strcmp(CMD_ARGV[0], "off")) {
1121                 enable = false;
1122         } else {
1123                 command_print(CMD, "Invalid argument: %s.", CMD_ARGV[0]);
1124                 return ERROR_FAIL;
1125         }
1126
1127         ret = jaylink_set_target_power(devh, enable);
1128
1129         if (ret != JAYLINK_OK) {
1130                 command_print(CMD, "jaylink_set_target_power() failed: %s.",
1131                         jaylink_strerror(ret));
1132                 return ERROR_FAIL;
1133         }
1134
1135         return ERROR_OK;
1136 }
1137
1138 static void show_config_usb_address(struct command_invocation *cmd)
1139 {
1140         if (config.usb_address != tmp_config.usb_address)
1141                 command_print(cmd, "USB address: %u [%u]", config.usb_address,
1142                         tmp_config.usb_address);
1143         else
1144                 command_print(cmd, "USB address: %u", config.usb_address);
1145 }
1146
1147 static void show_config_ip_address(struct command_invocation *cmd)
1148 {
1149         if (!memcmp(config.ip_address, tmp_config.ip_address, 4))
1150                 command_print(cmd, "IP address: %d.%d.%d.%d",
1151                         config.ip_address[3], config.ip_address[2],
1152                         config.ip_address[1], config.ip_address[0]);
1153         else
1154                 command_print(cmd, "IP address: %d.%d.%d.%d [%d.%d.%d.%d]",
1155                         config.ip_address[3], config.ip_address[2],
1156                         config.ip_address[1], config.ip_address[0],
1157                         tmp_config.ip_address[3], tmp_config.ip_address[2],
1158                         tmp_config.ip_address[1], tmp_config.ip_address[0]);
1159
1160         if (!memcmp(config.subnet_mask, tmp_config.subnet_mask, 4))
1161                 command_print(cmd, "Subnet mask: %d.%d.%d.%d",
1162                         config.subnet_mask[3], config.subnet_mask[2],
1163                         config.subnet_mask[1], config.subnet_mask[0]);
1164         else
1165                 command_print(cmd, "Subnet mask: %d.%d.%d.%d [%d.%d.%d.%d]",
1166                         config.subnet_mask[3], config.subnet_mask[2],
1167                         config.subnet_mask[1], config.subnet_mask[0],
1168                         tmp_config.subnet_mask[3], tmp_config.subnet_mask[2],
1169                         tmp_config.subnet_mask[1], tmp_config.subnet_mask[0]);
1170 }
1171
1172 static void show_config_mac_address(struct command_invocation *cmd)
1173 {
1174         if (!memcmp(config.mac_address, tmp_config.mac_address, 6))
1175                 command_print(cmd, "MAC address: %.02x:%.02x:%.02x:%.02x:%.02x:%.02x",
1176                         config.mac_address[5], config.mac_address[4],
1177                         config.mac_address[3], config.mac_address[2],
1178                         config.mac_address[1], config.mac_address[0]);
1179         else
1180                 command_print(cmd, "MAC address: %.02x:%.02x:%.02x:%.02x:%.02x:%.02x "
1181                         "[%.02x:%.02x:%.02x:%.02x:%.02x:%.02x]",
1182                         config.mac_address[5], config.mac_address[4],
1183                         config.mac_address[3], config.mac_address[2],
1184                         config.mac_address[1], config.mac_address[0],
1185                         tmp_config.mac_address[5], tmp_config.mac_address[4],
1186                         tmp_config.mac_address[3], tmp_config.mac_address[2],
1187                         tmp_config.mac_address[1], tmp_config.mac_address[0]);
1188 }
1189
1190 static void show_config_target_power(struct command_invocation *cmd)
1191 {
1192         const char *target_power;
1193         const char *current_target_power;
1194
1195         if (!config.target_power)
1196                 target_power = "off";
1197         else
1198                 target_power = "on";
1199
1200         if (!tmp_config.target_power)
1201                 current_target_power = "off";
1202         else
1203                 current_target_power = "on";
1204
1205         if (config.target_power != tmp_config.target_power)
1206                 command_print(cmd, "Target power supply: %s [%s]", target_power,
1207                         current_target_power);
1208         else
1209                 command_print(cmd, "Target power supply: %s", target_power);
1210 }
1211
1212 static void show_config(struct command_invocation *cmd)
1213 {
1214         command_print(cmd, "J-Link device configuration:");
1215
1216         show_config_usb_address(cmd);
1217
1218         if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_SET_TARGET_POWER))
1219                 show_config_target_power(cmd);
1220
1221         if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_ETHERNET)) {
1222                 show_config_ip_address(cmd);
1223                 show_config_mac_address(cmd);
1224         }
1225 }
1226
1227 static int poll_trace(uint8_t *buf, size_t *size)
1228 {
1229         int ret;
1230         uint32_t length;
1231
1232         length = *size;
1233
1234         ret = jaylink_swo_read(devh, buf, &length);
1235
1236         if (ret != JAYLINK_OK) {
1237                 LOG_ERROR("jaylink_swo_read() failed: %s.", jaylink_strerror(ret));
1238                 return ERROR_FAIL;
1239         }
1240
1241         *size = length;
1242
1243         return ERROR_OK;
1244 }
1245
1246 static uint32_t calculate_trace_buffer_size(void)
1247 {
1248         int ret;
1249         uint32_t tmp;
1250
1251         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_FREE_MEMORY))
1252                 return 0;
1253
1254         ret = jaylink_get_free_memory(devh, &tmp);
1255
1256         if (ret != JAYLINK_OK) {
1257                 LOG_ERROR("jaylink_get_free_memory() failed: %s.",
1258                         jaylink_strerror(ret));
1259                 return ERROR_FAIL;
1260         }
1261
1262         if (tmp > 0x3fff || tmp <= 0x600)
1263                 tmp = tmp >> 1;
1264         else
1265                 tmp = tmp - 0x400;
1266
1267         return tmp & 0xffffff00;
1268 }
1269
1270 static bool check_trace_freq(struct jaylink_swo_speed speed,
1271                 uint32_t trace_freq)
1272 {
1273         double min;
1274         double deviation;
1275         uint32_t divider;
1276
1277         min = fabs(1.0 - (speed.freq / ((double)trace_freq * speed.min_div)));
1278
1279         for (divider = speed.min_div; divider < speed.max_div; divider++) {
1280                 deviation = fabs(1.0 - (speed.freq / ((double)trace_freq * divider)));
1281
1282                 if (deviation < 0.03) {
1283                         LOG_DEBUG("Found suitable frequency divider %u with deviation of "
1284                                 "%.02f %%.", divider, deviation);
1285                         return true;
1286                 }
1287
1288                 if (deviation < min)
1289                         min = deviation;
1290         }
1291
1292         LOG_ERROR("Selected trace frequency is not supported by the device. "
1293                 "Please choose a different trace frequency.");
1294         LOG_ERROR("Maximum permitted deviation is 3.00 %%, but only %.02f %% "
1295                 "could be achieved.", min * 100);
1296
1297         return false;
1298 }
1299
1300 static int config_trace(bool enabled, enum tpiu_pin_protocol pin_protocol,
1301                 uint32_t port_size, unsigned int *trace_freq)
1302 {
1303         int ret;
1304         uint32_t buffer_size;
1305         struct jaylink_swo_speed speed;
1306
1307         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_SWO)) {
1308                 LOG_ERROR("Trace capturing is not supported by the device.");
1309                 return ERROR_FAIL;
1310         }
1311
1312         if (pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART) {
1313                 LOG_ERROR("Selected pin protocol is not supported.");
1314                 return ERROR_FAIL;
1315         }
1316
1317         trace_enabled = enabled;
1318
1319         ret = jaylink_swo_stop(devh);
1320
1321         if (ret != JAYLINK_OK) {
1322                 LOG_ERROR("jaylink_swo_stop() failed: %s.", jaylink_strerror(ret));
1323                 return ERROR_FAIL;
1324         }
1325
1326         if (!enabled) {
1327                 /*
1328                  * Adjust the SWD transaction buffer size as stopping SWO capturing
1329                  * deallocates device internal memory.
1330                  */
1331                 if (!adjust_swd_buffer_size())
1332                         return ERROR_FAIL;
1333
1334                 return ERROR_OK;
1335         }
1336
1337         buffer_size = calculate_trace_buffer_size();
1338
1339         if (!buffer_size) {
1340                 LOG_ERROR("Not enough free device memory to start trace capturing.");
1341                 return ERROR_FAIL;
1342         }
1343
1344         ret = jaylink_swo_get_speeds(devh, JAYLINK_SWO_MODE_UART, &speed);
1345
1346         if (ret != JAYLINK_OK) {
1347                 LOG_ERROR("jaylink_swo_get_speeds() failed: %s.",
1348                         jaylink_strerror(ret));
1349                 return ERROR_FAIL;
1350         }
1351
1352         if (!*trace_freq)
1353                 *trace_freq = speed.freq / speed.min_div;
1354
1355         if (!check_trace_freq(speed, *trace_freq))
1356                 return ERROR_FAIL;
1357
1358         LOG_DEBUG("Using %u bytes device memory for trace capturing.", buffer_size);
1359
1360         ret = jaylink_swo_start(devh, JAYLINK_SWO_MODE_UART, *trace_freq,
1361                 buffer_size);
1362
1363         if (ret != JAYLINK_OK) {
1364                 LOG_ERROR("jaylink_start_swo() failed: %s.", jaylink_strerror(ret));
1365                 return ERROR_FAIL;
1366         }
1367
1368         /*
1369          * Adjust the SWD transaction buffer size as starting SWO capturing
1370          * allocates device internal memory.
1371          */
1372         if (!adjust_swd_buffer_size())
1373                 return ERROR_FAIL;
1374
1375         return ERROR_OK;
1376 }
1377
1378 COMMAND_HANDLER(jlink_handle_config_usb_address_command)
1379 {
1380         uint8_t tmp;
1381
1382         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG)) {
1383                 command_print(CMD, "Reading configuration is not supported by the "
1384                         "device.");
1385                 return ERROR_OK;
1386         }
1387
1388         if (!CMD_ARGC) {
1389                 show_config_usb_address(CMD);
1390         } else if (CMD_ARGC == 1) {
1391                 if (sscanf(CMD_ARGV[0], "%" SCNd8, &tmp) != 1) {
1392                         command_print(CMD, "Invalid USB address: %s.", CMD_ARGV[0]);
1393                         return ERROR_FAIL;
1394                 }
1395
1396                 if (tmp > JAYLINK_USB_ADDRESS_3) {
1397                         command_print(CMD, "Invalid USB address: %u.", tmp);
1398                         return ERROR_FAIL;
1399                 }
1400
1401                 tmp_config.usb_address = tmp;
1402         } else {
1403                 command_print(CMD, "Need exactly one argument for jlink config "
1404                         "usb.");
1405                 return ERROR_COMMAND_SYNTAX_ERROR;
1406         }
1407
1408         return ERROR_OK;
1409 }
1410
1411 COMMAND_HANDLER(jlink_handle_config_target_power_command)
1412 {
1413         int enable;
1414
1415         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG)) {
1416                 command_print(CMD, "Reading configuration is not supported by the "
1417                         "device.");
1418                 return ERROR_OK;
1419         }
1420
1421         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_SET_TARGET_POWER)) {
1422                 command_print(CMD, "Target power supply is not supported by the "
1423                         "device.");
1424                 return ERROR_OK;
1425         }
1426
1427         if (!CMD_ARGC) {
1428                 show_config_target_power(CMD);
1429         } else if (CMD_ARGC == 1) {
1430                 if (!strcmp(CMD_ARGV[0], "on")) {
1431                         enable = true;
1432                 } else if (!strcmp(CMD_ARGV[0], "off")) {
1433                         enable = false;
1434                 } else {
1435                         command_print(CMD, "Invalid argument: %s.", CMD_ARGV[0]);
1436                         return ERROR_FAIL;
1437                 }
1438
1439                 tmp_config.target_power = enable;
1440         } else {
1441                 command_print(CMD, "Need exactly one argument for jlink config "
1442                         "targetpower.");
1443                 return ERROR_COMMAND_SYNTAX_ERROR;
1444         }
1445
1446         return ERROR_OK;
1447 }
1448
1449 COMMAND_HANDLER(jlink_handle_config_mac_address_command)
1450 {
1451         uint8_t addr[6];
1452         int i;
1453         char *e;
1454         const char *str;
1455
1456         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG)) {
1457                 command_print(CMD, "Reading configuration is not supported by the "
1458                         "device.");
1459                 return ERROR_OK;
1460         }
1461
1462         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_ETHERNET)) {
1463                 command_print(CMD, "Ethernet connectivity is not supported by the "
1464                         "device.");
1465                 return ERROR_OK;
1466         }
1467
1468         if (!CMD_ARGC) {
1469                 show_config_mac_address(CMD);
1470         } else if (CMD_ARGC == 1) {
1471                 str = CMD_ARGV[0];
1472
1473                 if ((strlen(str) != 17) || (str[2] != ':' || str[5] != ':' || \
1474                                 str[8] != ':' || str[11] != ':' || str[14] != ':')) {
1475                         command_print(CMD, "Invalid MAC address format.");
1476                         return ERROR_COMMAND_SYNTAX_ERROR;
1477                 }
1478
1479                 for (i = 5; i >= 0; i--) {
1480                         addr[i] = strtoul(str, &e, 16);
1481                         str = e + 1;
1482                 }
1483
1484                 if (!(addr[0] | addr[1] | addr[2] | addr[3] | addr[4] | addr[5])) {
1485                         command_print(CMD, "Invalid MAC address: zero address.");
1486                         return ERROR_COMMAND_SYNTAX_ERROR;
1487                 }
1488
1489                 if (!(0x01 & addr[0])) {
1490                         command_print(CMD, "Invalid MAC address: multicast address.");
1491                         return ERROR_COMMAND_SYNTAX_ERROR;
1492                 }
1493
1494                 memcpy(tmp_config.mac_address, addr, sizeof(addr));
1495         } else {
1496                 command_print(CMD, "Need exactly one argument for jlink config "
1497                         " mac.");
1498                 return ERROR_COMMAND_SYNTAX_ERROR;
1499         }
1500
1501         return ERROR_OK;
1502 }
1503
1504 static bool string_to_ip(const char *s, uint8_t *ip, int *pos)
1505 {
1506         uint8_t lip[4];
1507         char *e;
1508         const char *s_save = s;
1509         int i;
1510
1511         if (!s)
1512                 return false;
1513
1514         for (i = 0; i < 4; i++) {
1515                 lip[i] = strtoul(s, &e, 10);
1516
1517                 if (*e != '.' && i != 3)
1518                         return false;
1519
1520                 s = e + 1;
1521         }
1522
1523         *pos = e - s_save;
1524         memcpy(ip, lip, sizeof(lip));
1525
1526         return true;
1527 }
1528
1529 static void cpy_ip(uint8_t *dst, uint8_t *src)
1530 {
1531         int i, j;
1532
1533         for (i = 0, j = 3; i < 4; i++, j--)
1534                 dst[i] = src[j];
1535 }
1536
1537 COMMAND_HANDLER(jlink_handle_config_ip_address_command)
1538 {
1539         uint8_t ip_address[4];
1540         uint32_t subnet_mask = 0;
1541         int i, len;
1542         uint8_t subnet_bits = 24;
1543
1544         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG)) {
1545                 command_print(CMD, "Reading configuration is not supported by the "
1546                         "device.");
1547                 return ERROR_OK;
1548         }
1549
1550         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_ETHERNET)) {
1551                 command_print(CMD, "Ethernet connectivity is not supported by the "
1552                         "device.");
1553                 return ERROR_OK;
1554         }
1555
1556         if (!CMD_ARGC) {
1557                 show_config_ip_address(CMD);
1558         } else {
1559                 if (!string_to_ip(CMD_ARGV[0], ip_address, &i))
1560                         return ERROR_COMMAND_SYNTAX_ERROR;
1561
1562                 len = strlen(CMD_ARGV[0]);
1563
1564                 /* Check for format A.B.C.D/E. */
1565                 if (i < len) {
1566                         if (CMD_ARGV[0][i] != '/')
1567                                 return ERROR_COMMAND_SYNTAX_ERROR;
1568
1569                         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0] + i + 1, subnet_bits);
1570                 } else if (CMD_ARGC > 1) {
1571                         if (!string_to_ip(CMD_ARGV[1], (uint8_t *)&subnet_mask, &i))
1572                                 return ERROR_COMMAND_SYNTAX_ERROR;
1573                 }
1574
1575                 if (!subnet_mask)
1576                         subnet_mask = (uint32_t)(subnet_bits < 32 ?
1577                                 ((1ULL << subnet_bits) - 1) : 0xffffffff);
1578
1579                 cpy_ip(tmp_config.ip_address, ip_address);
1580                 cpy_ip(tmp_config.subnet_mask, (uint8_t *)&subnet_mask);
1581         }
1582
1583         return ERROR_OK;
1584 }
1585
1586 COMMAND_HANDLER(jlink_handle_config_reset_command)
1587 {
1588         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG))
1589                 return ERROR_OK;
1590
1591         memcpy(&tmp_config, &config, sizeof(struct device_config));
1592
1593         return ERROR_OK;
1594 }
1595
1596
1597 COMMAND_HANDLER(jlink_handle_config_write_command)
1598 {
1599         int ret;
1600
1601         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG)) {
1602                 command_print(CMD, "Reading configuration is not supported by the "
1603                         "device.");
1604                 return ERROR_OK;
1605         }
1606
1607         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_WRITE_CONFIG)) {
1608                 command_print(CMD, "Writing configuration is not supported by the "
1609                         "device.");
1610                 return ERROR_OK;
1611         }
1612
1613         if (!memcmp(&config, &tmp_config, sizeof(struct device_config))) {
1614                 command_print(CMD, "Operation not performed due to no changes in "
1615                         "the configuration.");
1616                 return ERROR_OK;
1617         }
1618
1619         ret = jaylink_write_raw_config(devh, (const uint8_t *)&tmp_config);
1620
1621         if (ret != JAYLINK_OK) {
1622                 LOG_ERROR("jaylink_write_raw_config() failed: %s.",
1623                         jaylink_strerror(ret));
1624                 return ERROR_FAIL;
1625         }
1626
1627         if (!read_device_config(&config)) {
1628                 LOG_ERROR("Failed to read device configuration for verification.");
1629                 return ERROR_FAIL;
1630         }
1631
1632         if (memcmp(&config, &tmp_config, sizeof(struct device_config))) {
1633                 LOG_ERROR("Verification of device configuration failed. Please check "
1634                         "your device.");
1635                 return ERROR_FAIL;
1636         }
1637
1638         memcpy(&tmp_config, &config, sizeof(struct device_config));
1639         command_print(CMD, "The new device configuration applies after power "
1640                 "cycling the J-Link device.");
1641
1642         return ERROR_OK;
1643 }
1644
1645 COMMAND_HANDLER(jlink_handle_config_command)
1646 {
1647         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_READ_CONFIG)) {
1648                 command_print(CMD, "Device doesn't support reading configuration.");
1649                 return ERROR_OK;
1650         }
1651
1652         if (CMD_ARGC == 0)
1653                 show_config(CMD);
1654
1655         return ERROR_OK;
1656 }
1657
1658 COMMAND_HANDLER(jlink_handle_emucom_write_command)
1659 {
1660         int ret;
1661         size_t tmp;
1662         uint32_t channel;
1663         uint32_t length;
1664         uint8_t *buf;
1665         size_t dummy;
1666
1667         if (CMD_ARGC != 2)
1668                 return ERROR_COMMAND_SYNTAX_ERROR;
1669
1670         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_EMUCOM)) {
1671                 LOG_ERROR("Device does not support EMUCOM.");
1672                 return ERROR_FAIL;
1673         }
1674
1675         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], channel);
1676
1677         tmp = strlen(CMD_ARGV[1]);
1678
1679         if (tmp % 2 != 0) {
1680                 LOG_ERROR("Data must be encoded as hexadecimal pairs.");
1681                 return ERROR_COMMAND_ARGUMENT_INVALID;
1682         }
1683
1684         buf = malloc(tmp / 2);
1685
1686         if (!buf) {
1687                 LOG_ERROR("Failed to allocate buffer.");
1688                 return ERROR_FAIL;
1689         }
1690
1691         dummy = unhexify(buf, CMD_ARGV[1], tmp / 2);
1692
1693         if (dummy != (tmp / 2)) {
1694                 LOG_ERROR("Data must be encoded as hexadecimal pairs.");
1695                 free(buf);
1696                 return ERROR_COMMAND_ARGUMENT_INVALID;
1697         }
1698
1699         length = tmp / 2;
1700         ret = jaylink_emucom_write(devh, channel, buf, &length);
1701
1702         free(buf);
1703
1704         if (ret == JAYLINK_ERR_DEV_NOT_SUPPORTED) {
1705                 LOG_ERROR("Channel not supported by the device.");
1706                 return ERROR_FAIL;
1707         } else if (ret != JAYLINK_OK) {
1708                 LOG_ERROR("Failed to write to channel: %s.", jaylink_strerror(ret));
1709                 return ERROR_FAIL;
1710         }
1711
1712         if (length != (tmp / 2))
1713                 LOG_WARNING("Only %" PRIu32 " bytes written to the channel.", length);
1714
1715         return ERROR_OK;
1716 }
1717
1718 COMMAND_HANDLER(jlink_handle_emucom_read_command)
1719 {
1720         int ret;
1721         uint32_t channel;
1722         uint32_t length;
1723         uint8_t *buf;
1724         size_t tmp;
1725
1726         if (CMD_ARGC != 2)
1727                 return ERROR_COMMAND_SYNTAX_ERROR;
1728
1729         if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_EMUCOM)) {
1730                 LOG_ERROR("Device does not support EMUCOM.");
1731                 return ERROR_FAIL;
1732         }
1733
1734         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], channel);
1735         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], length);
1736
1737         buf = malloc(length * 3 + 1);
1738
1739         if (!buf) {
1740                 LOG_ERROR("Failed to allocate buffer.");
1741                 return ERROR_FAIL;
1742         }
1743
1744         ret = jaylink_emucom_read(devh, channel, buf, &length);
1745
1746         if (ret == JAYLINK_ERR_DEV_NOT_SUPPORTED) {
1747                 LOG_ERROR("Channel is not supported by the device.");
1748                 free(buf);
1749                 return ERROR_FAIL;
1750         } else if (ret == JAYLINK_ERR_DEV_NOT_AVAILABLE) {
1751                 LOG_ERROR("Channel is not available for the requested amount of data. "
1752                         "%" PRIu32 " bytes are avilable.", length);
1753                 free(buf);
1754                 return ERROR_FAIL;
1755         } else if (ret != JAYLINK_OK) {
1756                 LOG_ERROR("Failed to read from channel: %s.", jaylink_strerror(ret));
1757                 free(buf);
1758                 return ERROR_FAIL;
1759         }
1760
1761         tmp = hexify((char *)buf + length, buf, length, 2 * length + 1);
1762
1763         if (tmp != 2 * length) {
1764                 LOG_ERROR("Failed to convert data into hexadecimal string.");
1765                 free(buf);
1766                 return ERROR_FAIL;
1767         }
1768
1769         command_print(CMD, "%s", buf + length);
1770         free(buf);
1771
1772         return ERROR_OK;
1773 }
1774
1775 static const struct command_registration jlink_config_subcommand_handlers[] = {
1776         {
1777                 .name = "usb",
1778                 .handler = &jlink_handle_config_usb_address_command,
1779                 .mode = COMMAND_EXEC,
1780                 .help = "set the USB address",
1781                 .usage = "[0-3]",
1782         },
1783         {
1784                 .name = "targetpower",
1785                 .handler = &jlink_handle_config_target_power_command,
1786                 .mode = COMMAND_EXEC,
1787                 .help = "set the target power supply",
1788                 .usage = "[on|off]"
1789         },
1790         {
1791                 .name = "mac",
1792                 .handler = &jlink_handle_config_mac_address_command,
1793                 .mode = COMMAND_EXEC,
1794                 .help = "set the MAC Address",
1795                 .usage = "[ff:ff:ff:ff:ff:ff]",
1796         },
1797         {
1798                 .name = "ip",
1799                 .handler = &jlink_handle_config_ip_address_command,
1800                 .mode = COMMAND_EXEC,
1801                 .help = "set the IP address, where A.B.C.D is the IP address, "
1802                         "E the bit of the subnet mask, F.G.H.I the subnet mask",
1803                 .usage = "[A.B.C.D[/E] [F.G.H.I]]",
1804         },
1805         {
1806                 .name = "reset",
1807                 .handler = &jlink_handle_config_reset_command,
1808                 .mode = COMMAND_EXEC,
1809                 .help = "undo configuration changes",
1810                 .usage = "",
1811         },
1812         {
1813                 .name = "write",
1814                 .handler = &jlink_handle_config_write_command,
1815                 .mode = COMMAND_EXEC,
1816                 .help = "write configuration to the device",
1817                 .usage = "",
1818         },
1819         COMMAND_REGISTRATION_DONE
1820 };
1821
1822 static const struct command_registration jlink_emucom_subcommand_handlers[] = {
1823         {
1824                 .name = "write",
1825                 .handler = &jlink_handle_emucom_write_command,
1826                 .mode = COMMAND_EXEC,
1827                 .help = "write to a channel",
1828                 .usage = "<channel> <data>",
1829         },
1830         {
1831                 .name = "read",
1832                 .handler = &jlink_handle_emucom_read_command,
1833                 .mode = COMMAND_EXEC,
1834                 .help = "read from a channel",
1835                 .usage = "<channel> <length>"
1836         },
1837         COMMAND_REGISTRATION_DONE
1838 };
1839
1840 static const struct command_registration jlink_subcommand_handlers[] = {
1841         {
1842                 .name = "jtag",
1843                 .handler = &jlink_handle_jlink_jtag_command,
1844                 .mode = COMMAND_EXEC,
1845                 .help = "select the JTAG command version",
1846                 .usage = "[2|3]",
1847         },
1848         {
1849                 .name = "targetpower",
1850                 .handler = &jlink_handle_target_power_command,
1851                 .mode = COMMAND_EXEC,
1852                 .help = "set the target power supply",
1853                 .usage = "<on|off>"
1854         },
1855         {
1856                 .name = "freemem",
1857                 .handler = &jlink_handle_free_memory_command,
1858                 .mode = COMMAND_EXEC,
1859                 .help = "show free device memory",
1860                 .usage = "",
1861         },
1862         {
1863                 .name = "hwstatus",
1864                 .handler = &jlink_handle_hwstatus_command,
1865                 .mode = COMMAND_EXEC,
1866                 .help = "show the hardware status",
1867                 .usage = "",
1868         },
1869         {
1870                 .name = "usb",
1871                 .handler = &jlink_usb_command,
1872                 .mode = COMMAND_CONFIG,
1873                 .help = "set the USB address of the device that should be used",
1874                 .usage = "<0-3>"
1875         },
1876         {
1877                 .name = "serial",
1878                 .handler = &jlink_serial_command,
1879                 .mode = COMMAND_CONFIG,
1880                 .help = "set the serial number of the device that should be used",
1881                 .usage = "<serial number>"
1882         },
1883         {
1884                 .name = "config",
1885                 .handler = &jlink_handle_config_command,
1886                 .mode = COMMAND_EXEC,
1887                 .help = "access the device configuration. If no argument is given "
1888                         "this will show the device configuration",
1889                 .chain = jlink_config_subcommand_handlers,
1890                 .usage = "[<cmd>]",
1891         },
1892         {
1893                 .name = "emucom",
1894                 .mode = COMMAND_EXEC,
1895                 .help = "access EMUCOM channel",
1896                 .chain = jlink_emucom_subcommand_handlers,
1897                 .usage = "",
1898         },
1899         COMMAND_REGISTRATION_DONE
1900 };
1901
1902 static const struct command_registration jlink_command_handlers[] = {
1903         {
1904                 .name = "jlink",
1905                 .mode = COMMAND_ANY,
1906                 .help = "perform jlink management",
1907                 .chain = jlink_subcommand_handlers,
1908                 .usage = "",
1909         },
1910         COMMAND_REGISTRATION_DONE
1911 };
1912
1913 static int jlink_swd_init(void)
1914 {
1915         iface = JAYLINK_TIF_SWD;
1916
1917         return ERROR_OK;
1918 }
1919
1920 static void jlink_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
1921 {
1922         assert(!(cmd & SWD_CMD_RnW));
1923         jlink_swd_queue_cmd(cmd, NULL, value, ap_delay_clk);
1924 }
1925
1926 static void jlink_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
1927 {
1928         assert(cmd & SWD_CMD_RnW);
1929         jlink_swd_queue_cmd(cmd, value, 0, ap_delay_clk);
1930 }
1931
1932 static int_least32_t jlink_swd_frequency(int_least32_t hz)
1933 {
1934         if (hz > 0)
1935                 jlink_speed(hz / 1000);
1936
1937         return hz;
1938 }
1939
1940 /***************************************************************************/
1941 /* J-Link tap functions */
1942
1943 static unsigned tap_length;
1944 /* In SWD mode use tms buffer for direction control */
1945 static uint8_t tms_buffer[JLINK_TAP_BUFFER_SIZE];
1946 static uint8_t tdi_buffer[JLINK_TAP_BUFFER_SIZE];
1947 static uint8_t tdo_buffer[JLINK_TAP_BUFFER_SIZE];
1948
1949 struct pending_scan_result {
1950         /** First bit position in tdo_buffer to read. */
1951         unsigned first;
1952         /** Number of bits to read. */
1953         unsigned length;
1954         /** Location to store the result */
1955         void *buffer;
1956         /** Offset in the destination buffer */
1957         unsigned buffer_offset;
1958 };
1959
1960 #define MAX_PENDING_SCAN_RESULTS 256
1961
1962 static int pending_scan_results_length;
1963 static struct pending_scan_result pending_scan_results_buffer[MAX_PENDING_SCAN_RESULTS];
1964
1965 static void jlink_tap_init(void)
1966 {
1967         tap_length = 0;
1968         pending_scan_results_length = 0;
1969         memset(tms_buffer, 0, sizeof(tms_buffer));
1970         memset(tdi_buffer, 0, sizeof(tdi_buffer));
1971 }
1972
1973 static void jlink_clock_data(const uint8_t *out, unsigned out_offset,
1974                              const uint8_t *tms_out, unsigned tms_offset,
1975                              uint8_t *in, unsigned in_offset,
1976                              unsigned length)
1977 {
1978         do {
1979                 unsigned available_length = JLINK_TAP_BUFFER_SIZE - tap_length / 8;
1980
1981                 if (!available_length ||
1982                     (in && pending_scan_results_length == MAX_PENDING_SCAN_RESULTS)) {
1983                         if (jlink_flush() != ERROR_OK)
1984                                 return;
1985                         available_length = JLINK_TAP_BUFFER_SIZE;
1986                 }
1987
1988                 struct pending_scan_result *pending_scan_result =
1989                         &pending_scan_results_buffer[pending_scan_results_length];
1990
1991                 unsigned scan_length = length > available_length ?
1992                         available_length : length;
1993
1994                 if (out)
1995                         buf_set_buf(out, out_offset, tdi_buffer, tap_length, scan_length);
1996                 if (tms_out)
1997                         buf_set_buf(tms_out, tms_offset, tms_buffer, tap_length, scan_length);
1998
1999                 if (in) {
2000                         pending_scan_result->first = tap_length;
2001                         pending_scan_result->length = scan_length;
2002                         pending_scan_result->buffer = in;
2003                         pending_scan_result->buffer_offset = in_offset;
2004                         pending_scan_results_length++;
2005                 }
2006
2007                 tap_length += scan_length;
2008                 out_offset += scan_length;
2009                 tms_offset += scan_length;
2010                 in_offset += scan_length;
2011                 length -= scan_length;
2012         } while (length > 0);
2013 }
2014
2015 static int jlink_flush(void)
2016 {
2017         int i;
2018         int ret;
2019
2020         if (!tap_length)
2021                 return ERROR_OK;
2022
2023         jlink_last_state = jtag_debug_state_machine(tms_buffer, tdi_buffer,
2024                 tap_length, jlink_last_state);
2025
2026         ret = jaylink_jtag_io(devh, tms_buffer, tdi_buffer, tdo_buffer,
2027                 tap_length, jtag_command_version);
2028
2029         if (ret != JAYLINK_OK) {
2030                 LOG_ERROR("jaylink_jtag_io() failed: %s.", jaylink_strerror(ret));
2031                 jlink_tap_init();
2032                 return ERROR_JTAG_QUEUE_FAILED;
2033         }
2034
2035         for (i = 0; i < pending_scan_results_length; i++) {
2036                 struct pending_scan_result *p = &pending_scan_results_buffer[i];
2037
2038                 buf_set_buf(tdo_buffer, p->first, p->buffer,
2039                             p->buffer_offset, p->length);
2040
2041                 DEBUG_JTAG_IO("Pending scan result, length = %d.", p->length);
2042         }
2043
2044         jlink_tap_init();
2045
2046         return ERROR_OK;
2047 }
2048
2049 static void fill_buffer(uint8_t *buf, uint32_t val, uint32_t len)
2050 {
2051         unsigned int tap_pos = tap_length;
2052
2053         while (len > 32) {
2054                 buf_set_u32(buf, tap_pos, 32, val);
2055                 len -= 32;
2056                 tap_pos += 32;
2057         }
2058
2059         if (len)
2060                 buf_set_u32(buf, tap_pos, len, val);
2061 }
2062
2063 static void jlink_queue_data_out(const uint8_t *data, uint32_t len)
2064 {
2065         const uint32_t dir_out = 0xffffffff;
2066
2067         if (data)
2068                 bit_copy(tdi_buffer, tap_length, data, 0, len);
2069         else
2070                 fill_buffer(tdi_buffer, 0, len);
2071
2072         fill_buffer(tms_buffer, dir_out, len);
2073         tap_length += len;
2074 }
2075
2076 static void jlink_queue_data_in(uint32_t len)
2077 {
2078         const uint32_t dir_in = 0;
2079
2080         fill_buffer(tms_buffer, dir_in, len);
2081         tap_length += len;
2082 }
2083
2084 static int jlink_swd_switch_seq(enum swd_special_seq seq)
2085 {
2086         const uint8_t *s;
2087         unsigned int s_len;
2088
2089         switch (seq) {
2090                 case LINE_RESET:
2091                         LOG_DEBUG("SWD line reset");
2092                         s = swd_seq_line_reset;
2093                         s_len = swd_seq_line_reset_len;
2094                         break;
2095                 case JTAG_TO_SWD:
2096                         LOG_DEBUG("JTAG-to-SWD");
2097                         s = swd_seq_jtag_to_swd;
2098                         s_len = swd_seq_jtag_to_swd_len;
2099                         break;
2100                 case SWD_TO_JTAG:
2101                         LOG_DEBUG("SWD-to-JTAG");
2102                         s = swd_seq_swd_to_jtag;
2103                         s_len = swd_seq_swd_to_jtag_len;
2104                         break;
2105                 default:
2106                         LOG_ERROR("Sequence %d not supported.", seq);
2107                         return ERROR_FAIL;
2108         }
2109
2110         jlink_queue_data_out(s, s_len);
2111
2112         return ERROR_OK;
2113 }
2114
2115 static int jlink_swd_run_queue(void)
2116 {
2117         int i;
2118         int ret;
2119
2120         LOG_DEBUG("Executing %d queued transactions.", pending_scan_results_length);
2121
2122         if (queued_retval != ERROR_OK) {
2123                 LOG_DEBUG("Skipping due to previous errors: %d.", queued_retval);
2124                 goto skip;
2125         }
2126
2127         /*
2128          * A transaction must be followed by another transaction or at least 8 idle
2129          * cycles to ensure that data is clocked through the AP.
2130          */
2131         jlink_queue_data_out(NULL, 8);
2132
2133         ret = jaylink_swd_io(devh, tms_buffer, tdi_buffer, tdo_buffer, tap_length);
2134
2135         if (ret != JAYLINK_OK) {
2136                 LOG_ERROR("jaylink_swd_io() failed: %s.", jaylink_strerror(ret));
2137                 goto skip;
2138         }
2139
2140         for (i = 0; i < pending_scan_results_length; i++) {
2141                 int ack = buf_get_u32(tdo_buffer, pending_scan_results_buffer[i].first, 3);
2142
2143                 if (ack != SWD_ACK_OK) {
2144                         LOG_DEBUG("SWD ack not OK: %d %s", ack,
2145                                   ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
2146                         queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
2147                         goto skip;
2148                 } else if (pending_scan_results_buffer[i].length) {
2149                         uint32_t data = buf_get_u32(tdo_buffer, 3 + pending_scan_results_buffer[i].first, 32);
2150                         int parity = buf_get_u32(tdo_buffer, 3 + 32 + pending_scan_results_buffer[i].first, 1);
2151
2152                         if (parity != parity_u32(data)) {
2153                                 LOG_ERROR("SWD: Read data parity mismatch.");
2154                                 queued_retval = ERROR_FAIL;
2155                                 goto skip;
2156                         }
2157
2158                         if (pending_scan_results_buffer[i].buffer)
2159                                 *(uint32_t *)pending_scan_results_buffer[i].buffer = data;
2160                 }
2161         }
2162
2163 skip:
2164         jlink_tap_init();
2165         ret = queued_retval;
2166         queued_retval = ERROR_OK;
2167
2168         return ret;
2169 }
2170
2171 static void jlink_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data, uint32_t ap_delay_clk)
2172 {
2173         uint8_t data_parity_trn[DIV_ROUND_UP(32 + 1, 8)];
2174         if (tap_length + 46 + 8 + ap_delay_clk >= swd_buffer_size * 8 ||
2175             pending_scan_results_length == MAX_PENDING_SCAN_RESULTS) {
2176                 /* Not enough room in the queue. Run the queue. */
2177                 queued_retval = jlink_swd_run_queue();
2178         }
2179
2180         if (queued_retval != ERROR_OK)
2181                 return;
2182
2183         cmd |= SWD_CMD_START | SWD_CMD_PARK;
2184
2185         jlink_queue_data_out(&cmd, 8);
2186
2187         pending_scan_results_buffer[pending_scan_results_length].first = tap_length;
2188
2189         if (cmd & SWD_CMD_RnW) {
2190                 /* Queue a read transaction. */
2191                 pending_scan_results_buffer[pending_scan_results_length].length = 32;
2192                 pending_scan_results_buffer[pending_scan_results_length].buffer = dst;
2193
2194                 jlink_queue_data_in(1 + 3 + 32 + 1 + 1);
2195         } else {
2196                 /* Queue a write transaction. */
2197                 pending_scan_results_buffer[pending_scan_results_length].length = 0;
2198                 jlink_queue_data_in(1 + 3 + 1);
2199
2200                 buf_set_u32(data_parity_trn, 0, 32, data);
2201                 buf_set_u32(data_parity_trn, 32, 1, parity_u32(data));
2202
2203                 jlink_queue_data_out(data_parity_trn, 32 + 1);
2204         }
2205
2206         pending_scan_results_length++;
2207
2208         /* Insert idle cycles after AP accesses to avoid WAIT. */
2209         if (cmd & SWD_CMD_APnDP)
2210                 jlink_queue_data_out(NULL, ap_delay_clk);
2211 }
2212
2213 static const struct swd_driver jlink_swd = {
2214         .init = &jlink_swd_init,
2215         .frequency = &jlink_swd_frequency,
2216         .switch_seq = &jlink_swd_switch_seq,
2217         .read_reg = &jlink_swd_read_reg,
2218         .write_reg = &jlink_swd_write_reg,
2219         .run = &jlink_swd_run_queue,
2220 };
2221
2222 static const char * const jlink_transports[] = { "jtag", "swd", NULL };
2223
2224 struct jtag_interface jlink_interface = {
2225         .name = "jlink",
2226         .commands = jlink_command_handlers,
2227         .transports = jlink_transports,
2228         .swd = &jlink_swd,
2229         .execute_queue = &jlink_execute_queue,
2230         .speed = &jlink_speed,
2231         .speed_div = &jlink_speed_div,
2232         .khz = &jlink_khz,
2233         .init = &jlink_init,
2234         .quit = &jlink_quit,
2235         .config_trace = &config_trace,
2236         .poll_trace = &poll_trace,
2237 };