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