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