git.gag.com Git - fw/openocd/blob - src/jtag/drivers/jlink.c

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