git.sur5r.net Git - openocd/blob - src/jtag/drivers/cmsis_dap_usb.c

   1 /***************************************************************************
   2  *   Copyright (C) 2016 by Maksym Hilliaka                                 *
   3  *   oter@frozen-team.com                                                  *
   4  *                                                                         *
   5  *   Copyright (C) 2016 by Phillip Pearson                                 *
   6  *   pp@myelin.co.nz                                                       *
   7  *                                                                         *
   8  *   Copyright (C) 2014 by Paul Fertser                                    *
   9  *   fercerpav@gmail.com                                                   *
  10  *                                                                         *
  11  *   Copyright (C) 2013 by mike brown                                      *
  12  *   mike@theshedworks.org.uk                                              *
  13  *                                                                         *
  14  *   Copyright (C) 2013 by Spencer Oliver                                  *
  15  *   spen@spen-soft.co.uk                                                  *
  16  *                                                                         *
  17  *   This program is free software; you can redistribute it and/or modify  *
  18  *   it under the terms of the GNU General Public License as published by  *
  19  *   the Free Software Foundation; either version 2 of the License, or     *
  20  *   (at your option) any later version.                                   *
  21  *                                                                         *
  22  *   This program is distributed in the hope that it will be useful,       *
  23  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  24  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  25  *   GNU General Public License for more details.                          *
  26  *                                                                         *
  27  *   You should have received a copy of the GNU General Public License     *
  28  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  29  ***************************************************************************/
  30
  31 #ifdef HAVE_CONFIG_H
  32 #include "config.h"
  33 #endif
  34
  35 #include <transport/transport.h>
  36 #include <jtag/swd.h>
  37 #include <jtag/interface.h>
  38 #include <jtag/commands.h>
  39 #include <jtag/tcl.h>
  40
  41 #include <hidapi.h>
  42
  43 /*
  44  * See CMSIS-DAP documentation:
  45  * Version 0.01 - Beta.
  46  */
  47
  48 /* USB Config */
  49
  50 /* Known vid/pid pairs:
  51  * VID 0xc251: Keil Software
  52  * PID 0xf001: LPC-Link-II CMSIS_DAP
  53  * PID 0xf002: OPEN-SDA CMSIS_DAP (Freedom Board)
  54  * PID 0x2722: Keil ULINK2 CMSIS-DAP
  55  *
  56  * VID 0x0d28: mbed Software
  57  * PID 0x0204: MBED CMSIS-DAP
  58  */
  59
  60 #define MAX_USB_IDS 8
  61 /* vid = pid = 0 marks the end of the list */
  62 static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0 };
  63 static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
  64 static wchar_t *cmsis_dap_serial;
  65 static bool swd_mode;
  66
  67 #define PACKET_SIZE       (64 + 1)      /* 64 bytes plus report id */
  68 #define USB_TIMEOUT       1000
  69
  70 /* CMSIS-DAP General Commands */
  71 #define CMD_DAP_INFO              0x00
  72 #define CMD_DAP_LED               0x01
  73 #define CMD_DAP_CONNECT           0x02
  74 #define CMD_DAP_DISCONNECT        0x03
  75 #define CMD_DAP_WRITE_ABORT       0x08
  76 #define CMD_DAP_DELAY             0x09
  77 #define CMD_DAP_RESET_TARGET      0x0A
  78
  79 /* CMD_INFO */
  80 #define INFO_ID_VID               0x00      /* string */
  81 #define INFO_ID_PID               0x02      /* string */
  82 #define INFO_ID_SERNUM            0x03      /* string */
  83 #define INFO_ID_FW_VER            0x04      /* string */
  84 #define INFO_ID_TD_VEND           0x05      /* string */
  85 #define INFO_ID_TD_NAME           0x06      /* string */
  86 #define INFO_ID_CAPS              0xf0      /* byte */
  87 #define INFO_ID_PKT_CNT           0xfe      /* byte */
  88 #define INFO_ID_PKT_SZ            0xff      /* short */
  89
  90 #define INFO_CAPS_SWD             0x01
  91 #define INFO_CAPS_JTAG            0x02
  92
  93 /* CMD_LED */
  94 #define LED_ID_CONNECT            0x00
  95 #define LED_ID_RUN                0x01
  96
  97 #define LED_OFF                   0x00
  98 #define LED_ON                    0x01
  99
 100 /* CMD_CONNECT */
 101 #define CONNECT_DEFAULT           0x00
 102 #define CONNECT_SWD               0x01
 103 #define CONNECT_JTAG              0x02
 104
 105 /* CMSIS-DAP Common SWD/JTAG Commands */
 106 #define CMD_DAP_DELAY             0x09
 107 #define CMD_DAP_SWJ_PINS          0x10
 108 #define CMD_DAP_SWJ_CLOCK         0x11
 109 #define CMD_DAP_SWJ_SEQ           0x12
 110
 111 /*
 112  * PINS
 113  * Bit 0: SWCLK/TCK
 114  * Bit 1: SWDIO/TMS
 115  * Bit 2: TDI
 116  * Bit 3: TDO
 117  * Bit 5: nTRST
 118  * Bit 7: nRESET
 119  */
 120
 121 #define SWJ_PIN_TCK               (1<<0)
 122 #define SWJ_PIN_TMS               (1<<1)
 123 #define SWJ_PIN_TDI               (1<<2)
 124 #define SWJ_PIN_TDO               (1<<3)
 125 #define SWJ_PIN_TRST              (1<<5)
 126 #define SWJ_PIN_SRST              (1<<7)
 127
 128 /* CMSIS-DAP SWD Commands */
 129 #define CMD_DAP_SWD_CONFIGURE     0x13
 130
 131 /* CMSIS-DAP JTAG Commands */
 132 #define CMD_DAP_JTAG_SEQ          0x14
 133 #define CMD_DAP_JTAG_CONFIGURE    0x15
 134 #define CMD_DAP_JTAG_IDCODE       0x16
 135
 136 /* CMSIS-DAP JTAG sequence info masks */
 137 /* Number of bits to clock through (0 means 64) */
 138 #define DAP_JTAG_SEQ_TCK          0x3F
 139 /* TMS will be set during the sequence if this bit is set */
 140 #define DAP_JTAG_SEQ_TMS          0x40
 141 /* TDO output will be captured if this bit is set */
 142 #define DAP_JTAG_SEQ_TDO          0x80
 143
 144
 145 /* CMSIS-DAP Transfer Commands */
 146 #define CMD_DAP_TFER_CONFIGURE    0x04
 147 #define CMD_DAP_TFER              0x05
 148 #define CMD_DAP_TFER_BLOCK        0x06
 149 #define CMD_DAP_TFER_ABORT        0x07
 150
 151 /* DAP Status Code */
 152 #define DAP_OK                    0
 153 #define DAP_ERROR                 0xFF
 154
 155 /* CMSIS-DAP Vendor Commands
 156  * None as yet... */
 157
 158 static const char * const info_caps_str[] = {
 159         "SWD  Supported",
 160         "JTAG Supported"
 161 };
 162
 163 /* max clock speed (kHz) */
 164 #define DAP_MAX_CLOCK             5000
 165
 166 struct cmsis_dap {
 167         hid_device *dev_handle;
 168         uint16_t packet_size;
 169         uint16_t packet_count;
 170         uint8_t *packet_buffer;
 171         uint8_t caps;
 172         uint8_t mode;
 173 };
 174
 175 struct pending_transfer_result {
 176         uint8_t cmd;
 177         uint32_t data;
 178         void *buffer;
 179 };
 180
 181 struct pending_scan_result {
 182         /** Offset in bytes in the CMD_DAP_JTAG_SEQ response buffer. */
 183         unsigned first;
 184         /** Number of bits to read. */
 185         unsigned length;
 186         /** Location to store the result */
 187         uint8_t *buffer;
 188         /** Offset in the destination buffer */
 189         unsigned buffer_offset;
 190 };
 191
 192 static int pending_transfer_count, pending_queue_len;
 193 static struct pending_transfer_result *pending_transfers;
 194
 195 /* pointers to buffers that will receive jtag scan results on the next flush */
 196 #define MAX_PENDING_SCAN_RESULTS 256
 197 static int pending_scan_result_count;
 198 static struct pending_scan_result pending_scan_results[MAX_PENDING_SCAN_RESULTS];
 199
 200 /* queued JTAG sequences that will be executed on the next flush */
 201 #define QUEUED_SEQ_BUF_LEN (cmsis_dap_handle->packet_size - 3)
 202 static int queued_seq_count;
 203 static int queued_seq_buf_end;
 204 static int queued_seq_tdo_ptr;
 205 static uint8_t queued_seq_buf[1024]; /* TODO: make dynamic / move into cmsis object */
 206
 207 static int queued_retval;
 208
 209 static uint8_t output_pins = SWJ_PIN_SRST | SWJ_PIN_TRST;
 210
 211 static struct cmsis_dap *cmsis_dap_handle;
 212
 213 static int cmsis_dap_usb_open(void)
 214 {
 215         hid_device *dev = NULL;
 216         int i;
 217         struct hid_device_info *devs, *cur_dev;
 218         unsigned short target_vid, target_pid;
 219         wchar_t *target_serial = NULL;
 220
 221         bool found = false;
 222         bool serial_found = false;
 223
 224         target_vid = 0;
 225         target_pid = 0;
 226
 227         /*
 228          * The CMSIS-DAP specification stipulates:
 229          * "The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
 230          * debuggers to identify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
 231          */
 232         devs = hid_enumerate(0x0, 0x0);
 233         cur_dev = devs;
 234         while (NULL != cur_dev) {
 235                 if (0 == cmsis_dap_vid[0]) {
 236                         if (NULL == cur_dev->product_string) {
 237                                 LOG_DEBUG("Cannot read product string of device 0x%x:0x%x",
 238                                           cur_dev->vendor_id, cur_dev->product_id);
 239                         } else {
 240                                 if (wcsstr(cur_dev->product_string, L"CMSIS-DAP")) {
 241                                         /* if the user hasn't specified VID:PID *and*
 242                                          * product string contains "CMSIS-DAP", pick it
 243                                          */
 244                                         found = true;
 245                                 }
 246                         }
 247                 } else {
 248                         /* otherwise, exhaustively compare against all VID:PID in list */
 249                         for (i = 0; cmsis_dap_vid[i] || cmsis_dap_pid[i]; i++) {
 250                                 if ((cmsis_dap_vid[i] == cur_dev->vendor_id) && (cmsis_dap_pid[i] == cur_dev->product_id))
 251                                         found = true;
 252                         }
 253
 254                         if (cmsis_dap_vid[i] || cmsis_dap_pid[i])
 255                                 found = true;
 256                 }
 257
 258                 if (found) {
 259                         /* we have found an adapter, so exit further checks */
 260                         /* check serial number matches if given */
 261                         if (cmsis_dap_serial != NULL) {
 262                                 if ((cur_dev->serial_number != NULL) && wcscmp(cmsis_dap_serial, cur_dev->serial_number) == 0) {
 263                                         serial_found = true;
 264                                         break;
 265                                 }
 266                         } else
 267                                 break;
 268
 269                         found = false;
 270                 }
 271
 272                 cur_dev = cur_dev->next;
 273         }
 274
 275         if (NULL != cur_dev) {
 276                 target_vid = cur_dev->vendor_id;
 277                 target_pid = cur_dev->product_id;
 278                 if (serial_found)
 279                         target_serial = cmsis_dap_serial;
 280         }
 281
 282         hid_free_enumeration(devs);
 283
 284         if (target_vid == 0 && target_pid == 0) {
 285                 LOG_ERROR("unable to find CMSIS-DAP device");
 286                 return ERROR_FAIL;
 287         }
 288
 289         if (hid_init() != 0) {
 290                 LOG_ERROR("unable to open HIDAPI");
 291                 return ERROR_FAIL;
 292         }
 293
 294         dev = hid_open(target_vid, target_pid, target_serial);
 295
 296         if (dev == NULL) {
 297                 LOG_ERROR("unable to open CMSIS-DAP device 0x%x:0x%x", target_vid, target_pid);
 298                 return ERROR_FAIL;
 299         }
 300
 301         struct cmsis_dap *dap = malloc(sizeof(struct cmsis_dap));
 302         if (dap == NULL) {
 303                 LOG_ERROR("unable to allocate memory");
 304                 return ERROR_FAIL;
 305         }
 306
 307         dap->dev_handle = dev;
 308         dap->caps = 0;
 309         dap->mode = 0;
 310
 311         cmsis_dap_handle = dap;
 312
 313         /* allocate default packet buffer, may be changed later.
 314          * currently with HIDAPI we have no way of getting the output report length
 315          * without this info we cannot communicate with the adapter.
 316          * For the moment we ahve to hard code the packet size */
 317
 318         int packet_size = PACKET_SIZE;
 319
 320         /* atmel cmsis-dap uses 512 byte reports */
 321         /* except when it doesn't e.g. with mEDBG on SAMD10 Xplained
 322          * board */
 323         /* TODO: HID report descriptor should be parsed instead of
 324          * hardcoding a match by VID */
 325         if (target_vid == 0x03eb && target_pid != 0x2145)
 326                 packet_size = 512 + 1;
 327
 328         cmsis_dap_handle->packet_buffer = malloc(packet_size);
 329         cmsis_dap_handle->packet_size = packet_size;
 330
 331         if (cmsis_dap_handle->packet_buffer == NULL) {
 332                 LOG_ERROR("unable to allocate memory");
 333                 return ERROR_FAIL;
 334         }
 335
 336         return ERROR_OK;
 337 }
 338
 339 static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 340 {
 341         hid_close(dap->dev_handle);
 342         hid_exit();
 343
 344         free(cmsis_dap_handle->packet_buffer);
 345         free(cmsis_dap_handle);
 346         cmsis_dap_handle = NULL;
 347         free(cmsis_dap_serial);
 348         cmsis_dap_serial = NULL;
 349         free(pending_transfers);
 350         pending_transfers = NULL;
 351
 352         return;
 353 }
 354
 355 /* Send a message and receive the reply */
 356 static int cmsis_dap_usb_xfer(struct cmsis_dap *dap, int txlen)
 357 {
 358 #ifdef CMSIS_DAP_JTAG_DEBUG
 359         LOG_DEBUG("cmsis-dap usb xfer cmd=%02X", dap->packet_buffer[1]);
 360 #endif
 361         /* Pad the rest of the TX buffer with 0's */
 362         memset(dap->packet_buffer + txlen, 0, dap->packet_size - txlen);
 363
 364         /* write data to device */
 365         int retval = hid_write(dap->dev_handle, dap->packet_buffer, dap->packet_size);
 366         if (retval == -1) {
 367                 LOG_ERROR("error writing data: %ls", hid_error(dap->dev_handle));
 368                 return ERROR_FAIL;
 369         }
 370
 371         /* get reply */
 372         retval = hid_read_timeout(dap->dev_handle, dap->packet_buffer, dap->packet_size, USB_TIMEOUT);
 373         if (retval == -1 || retval == 0) {
 374                 LOG_DEBUG("error reading data: %ls", hid_error(dap->dev_handle));
 375                 return ERROR_FAIL;
 376         }
 377
 378         return ERROR_OK;
 379 }
 380
 381 static int cmsis_dap_cmd_DAP_SWJ_Pins(uint8_t pins, uint8_t mask, uint32_t delay, uint8_t *input)
 382 {
 383         int retval;
 384         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 385
 386         buffer[0] = 0;  /* report number */
 387         buffer[1] = CMD_DAP_SWJ_PINS;
 388         buffer[2] = pins;
 389         buffer[3] = mask;
 390         buffer[4] = delay & 0xff;
 391         buffer[5] = (delay >> 8) & 0xff;
 392         buffer[6] = (delay >> 16) & 0xff;
 393         buffer[7] = (delay >> 24) & 0xff;
 394         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 8);
 395
 396         if (retval != ERROR_OK) {
 397                 LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_PINS failed.");
 398                 return ERROR_JTAG_DEVICE_ERROR;
 399         }
 400
 401         if (input)
 402                 *input = buffer[1];
 403
 404         return ERROR_OK;
 405 }
 406
 407 static int cmsis_dap_cmd_DAP_SWJ_Clock(uint32_t swj_clock)
 408 {
 409         int retval;
 410         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 411
 412         /* set clock in Hz */
 413         swj_clock *= 1000;
 414         buffer[0] = 0;  /* report number */
 415         buffer[1] = CMD_DAP_SWJ_CLOCK;
 416         buffer[2] = swj_clock & 0xff;
 417         buffer[3] = (swj_clock >> 8) & 0xff;
 418         buffer[4] = (swj_clock >> 16) & 0xff;
 419         buffer[5] = (swj_clock >> 24) & 0xff;
 420         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 6);
 421
 422         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 423                 LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_CLOCK failed.");
 424                 return ERROR_JTAG_DEVICE_ERROR;
 425         }
 426
 427         return ERROR_OK;
 428 }
 429
 430 /* clock a sequence of bits out on TMS, to change JTAG states */
 431 static int cmsis_dap_cmd_DAP_SWJ_Sequence(uint8_t s_len, const uint8_t *sequence)
 432 {
 433         int retval;
 434         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 435
 436 #ifdef CMSIS_DAP_JTAG_DEBUG
 437         LOG_DEBUG("cmsis-dap TMS sequence: len=%d", s_len);
 438         for (int i = 0; i < DIV_ROUND_UP(s_len, 8); ++i)
 439                 printf("%02X ", sequence[i]);
 440
 441         printf("\n");
 442 #endif
 443
 444         buffer[0] = 0;  /* report number */
 445         buffer[1] = CMD_DAP_SWJ_SEQ;
 446         buffer[2] = s_len;
 447         bit_copy(&buffer[3], 0, sequence, 0, s_len);
 448
 449         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, DIV_ROUND_UP(s_len, 8) + 3);
 450
 451         if (retval != ERROR_OK || buffer[1] != DAP_OK)
 452                 return ERROR_FAIL;
 453
 454         return ERROR_OK;
 455 }
 456
 457 static int cmsis_dap_cmd_DAP_Info(uint8_t info, uint8_t **data)
 458 {
 459         int retval;
 460         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 461
 462         buffer[0] = 0;  /* report number */
 463         buffer[1] = CMD_DAP_INFO;
 464         buffer[2] = info;
 465         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
 466
 467         if (retval != ERROR_OK) {
 468                 LOG_ERROR("CMSIS-DAP command CMD_INFO failed.");
 469                 return ERROR_JTAG_DEVICE_ERROR;
 470         }
 471
 472         *data = &(buffer[1]);
 473
 474         return ERROR_OK;
 475 }
 476
 477 static int cmsis_dap_cmd_DAP_LED(uint8_t leds)
 478 {
 479         int retval;
 480         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 481
 482         buffer[0] = 0;  /* report number */
 483         buffer[1] = CMD_DAP_LED;
 484         buffer[2] = 0x00;
 485         buffer[3] = leds;
 486         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
 487
 488         if (retval != ERROR_OK || buffer[1] != 0x00) {
 489                 LOG_ERROR("CMSIS-DAP command CMD_LED failed.");
 490                 return ERROR_JTAG_DEVICE_ERROR;
 491         }
 492
 493         return ERROR_OK;
 494 }
 495
 496 static int cmsis_dap_cmd_DAP_Connect(uint8_t mode)
 497 {
 498         int retval;
 499         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 500
 501         buffer[0] = 0;  /* report number */
 502         buffer[1] = CMD_DAP_CONNECT;
 503         buffer[2] = mode;
 504         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
 505
 506         if (retval != ERROR_OK) {
 507                 LOG_ERROR("CMSIS-DAP command CMD_CONNECT failed.");
 508                 return ERROR_JTAG_DEVICE_ERROR;
 509         }
 510
 511         if (buffer[1] != mode) {
 512                 LOG_ERROR("CMSIS-DAP failed to connect in mode (%d)", mode);
 513                 return ERROR_JTAG_DEVICE_ERROR;
 514         }
 515
 516         return ERROR_OK;
 517 }
 518
 519 static int cmsis_dap_cmd_DAP_Disconnect(void)
 520 {
 521         int retval;
 522         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 523
 524         buffer[0] = 0;  /* report number */
 525         buffer[1] = CMD_DAP_DISCONNECT;
 526         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 2);
 527
 528         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 529                 LOG_ERROR("CMSIS-DAP command CMD_DISCONNECT failed.");
 530                 return ERROR_JTAG_DEVICE_ERROR;
 531         }
 532
 533         return ERROR_OK;
 534 }
 535
 536 static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t retry_count, uint16_t match_retry)
 537 {
 538         int retval;
 539         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 540
 541         buffer[0] = 0;  /* report number */
 542         buffer[1] = CMD_DAP_TFER_CONFIGURE;
 543         buffer[2] = idle;
 544         buffer[3] = retry_count & 0xff;
 545         buffer[4] = (retry_count >> 8) & 0xff;
 546         buffer[5] = match_retry & 0xff;
 547         buffer[6] = (match_retry >> 8) & 0xff;
 548         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
 549
 550         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 551                 LOG_ERROR("CMSIS-DAP command CMD_TFER_Configure failed.");
 552                 return ERROR_JTAG_DEVICE_ERROR;
 553         }
 554
 555         return ERROR_OK;
 556 }
 557
 558 static int cmsis_dap_cmd_DAP_SWD_Configure(uint8_t cfg)
 559 {
 560         int retval;
 561         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 562
 563         buffer[0] = 0;  /* report number */
 564         buffer[1] = CMD_DAP_SWD_CONFIGURE;
 565         buffer[2] = cfg;
 566         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
 567
 568         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 569                 LOG_ERROR("CMSIS-DAP command CMD_SWD_Configure failed.");
 570                 return ERROR_JTAG_DEVICE_ERROR;
 571         }
 572
 573         return ERROR_OK;
 574 }
 575
 576 #if 0
 577 static int cmsis_dap_cmd_DAP_Delay(uint16_t delay_us)
 578 {
 579         int retval;
 580         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 581
 582         buffer[0] = 0;  /* report number */
 583         buffer[1] = CMD_DAP_DELAY;
 584         buffer[2] = delay_us & 0xff;
 585         buffer[3] = (delay_us >> 8) & 0xff;
 586         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
 587
 588         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 589                 LOG_ERROR("CMSIS-DAP command CMD_Delay failed.");
 590                 return ERROR_JTAG_DEVICE_ERROR;
 591         }
 592
 593         return ERROR_OK;
 594 }
 595 #endif
 596
 597 static int cmsis_dap_swd_run_queue(void)
 598 {
 599         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 600
 601         LOG_DEBUG_IO("Executing %d queued transactions", pending_transfer_count);
 602
 603         if (queued_retval != ERROR_OK) {
 604                 LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
 605                 goto skip;
 606         }
 607
 608         if (!pending_transfer_count)
 609                 goto skip;
 610
 611         size_t idx = 0;
 612         buffer[idx++] = 0;      /* report number */
 613         buffer[idx++] = CMD_DAP_TFER;
 614         buffer[idx++] = 0x00;   /* DAP Index */
 615         buffer[idx++] = pending_transfer_count;
 616
 617         for (int i = 0; i < pending_transfer_count; i++) {
 618                 uint8_t cmd = pending_transfers[i].cmd;
 619                 uint32_t data = pending_transfers[i].data;
 620
 621                 LOG_DEBUG_IO("%s %s reg %x %"PRIx32,
 622                                 cmd & SWD_CMD_APnDP ? "AP" : "DP",
 623                                 cmd & SWD_CMD_RnW ? "read" : "write",
 624                           (cmd & SWD_CMD_A32) >> 1, data);
 625
 626                 /* When proper WAIT handling is implemented in the
 627                  * common SWD framework, this kludge can be
 628                  * removed. However, this might lead to minor
 629                  * performance degradation as the adapter wouldn't be
 630                  * able to automatically retry anything (because ARM
 631                  * has forgotten to implement sticky error flags
 632                  * clearing). See also comments regarding
 633                  * cmsis_dap_cmd_DAP_TFER_Configure() and
 634                  * cmsis_dap_cmd_DAP_SWD_Configure() in
 635                  * cmsis_dap_init().
 636                  */
 637                 if (!(cmd & SWD_CMD_RnW) &&
 638                     !(cmd & SWD_CMD_APnDP) &&
 639                     (cmd & SWD_CMD_A32) >> 1 == DP_CTRL_STAT &&
 640                     (data & CORUNDETECT)) {
 641                         LOG_DEBUG("refusing to enable sticky overrun detection");
 642                         data &= ~CORUNDETECT;
 643                 }
 644
 645                 buffer[idx++] = (cmd >> 1) & 0x0f;
 646                 if (!(cmd & SWD_CMD_RnW)) {
 647                         buffer[idx++] = (data) & 0xff;
 648                         buffer[idx++] = (data >> 8) & 0xff;
 649                         buffer[idx++] = (data >> 16) & 0xff;
 650                         buffer[idx++] = (data >> 24) & 0xff;
 651                 }
 652         }
 653
 654         queued_retval = cmsis_dap_usb_xfer(cmsis_dap_handle, idx);
 655         if (queued_retval != ERROR_OK)
 656                 goto skip;
 657
 658         idx = 2;
 659         uint8_t ack = buffer[idx] & 0x07;
 660         if (ack != SWD_ACK_OK || (buffer[idx] & 0x08)) {
 661                 LOG_DEBUG("SWD ack not OK: %d %s", buffer[idx-1],
 662                           ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
 663                 queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
 664                 goto skip;
 665         }
 666         idx++;
 667
 668         if (pending_transfer_count != buffer[1])
 669                 LOG_ERROR("CMSIS-DAP transfer count mismatch: expected %d, got %d",
 670                           pending_transfer_count, buffer[1]);
 671
 672         for (int i = 0; i < buffer[1]; i++) {
 673                 if (pending_transfers[i].cmd & SWD_CMD_RnW) {
 674                         static uint32_t last_read;
 675                         uint32_t data = le_to_h_u32(&buffer[idx]);
 676                         uint32_t tmp = data;
 677                         idx += 4;
 678
 679                         LOG_DEBUG_IO("Read result: %"PRIx32, data);
 680
 681                         /* Imitate posted AP reads */
 682                         if ((pending_transfers[i].cmd & SWD_CMD_APnDP) ||
 683                             ((pending_transfers[i].cmd & SWD_CMD_A32) >> 1 == DP_RDBUFF)) {
 684                                 tmp = last_read;
 685                                 last_read = data;
 686                         }
 687
 688                         if (pending_transfers[i].buffer)
 689                                 *(uint32_t *)pending_transfers[i].buffer = tmp;
 690                 }
 691         }
 692
 693 skip:
 694         pending_transfer_count = 0;
 695         int retval = queued_retval;
 696         queued_retval = ERROR_OK;
 697
 698         return retval;
 699 }
 700
 701 static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 702 {
 703         if (pending_transfer_count == pending_queue_len) {
 704                 /* Not enough room in the queue. Run the queue. */
 705                 queued_retval = cmsis_dap_swd_run_queue();
 706         }
 707
 708         if (queued_retval != ERROR_OK)
 709                 return;
 710
 711         pending_transfers[pending_transfer_count].data = data;
 712         pending_transfers[pending_transfer_count].cmd = cmd;
 713         if (cmd & SWD_CMD_RnW) {
 714                 /* Queue a read transaction */
 715                 pending_transfers[pending_transfer_count].buffer = dst;
 716         }
 717         pending_transfer_count++;
 718 }
 719
 720 static void cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
 721 {
 722         assert(!(cmd & SWD_CMD_RnW));
 723         cmsis_dap_swd_queue_cmd(cmd, NULL, value);
 724 }
 725
 726 static void cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
 727 {
 728         assert(cmd & SWD_CMD_RnW);
 729         cmsis_dap_swd_queue_cmd(cmd, value, 0);
 730 }
 731
 732 static int cmsis_dap_get_serial_info(void)
 733 {
 734         uint8_t *data;
 735
 736         int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_SERNUM, &data);
 737         if (retval != ERROR_OK)
 738                 return retval;
 739
 740         if (data[0]) /* strlen */
 741                 LOG_INFO("CMSIS-DAP: Serial# = %s", &data[1]);
 742
 743         return ERROR_OK;
 744 }
 745
 746 static int cmsis_dap_get_version_info(void)
 747 {
 748         uint8_t *data;
 749
 750         /* INFO_ID_FW_VER - string */
 751         int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_FW_VER, &data);
 752         if (retval != ERROR_OK)
 753                 return retval;
 754
 755         if (data[0]) /* strlen */
 756                 LOG_INFO("CMSIS-DAP: FW Version = %s", &data[1]);
 757
 758         return ERROR_OK;
 759 }
 760
 761 static int cmsis_dap_get_caps_info(void)
 762 {
 763         uint8_t *data;
 764
 765         /* INFO_ID_CAPS - byte */
 766         int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_CAPS, &data);
 767         if (retval != ERROR_OK)
 768                 return retval;
 769
 770         if (data[0] == 1) {
 771                 uint8_t caps = data[1];
 772
 773                 cmsis_dap_handle->caps = caps;
 774
 775                 if (caps & INFO_CAPS_SWD)
 776                         LOG_INFO("CMSIS-DAP: %s", info_caps_str[0]);
 777                 if (caps & INFO_CAPS_JTAG)
 778                         LOG_INFO("CMSIS-DAP: %s", info_caps_str[1]);
 779         }
 780
 781         return ERROR_OK;
 782 }
 783
 784 static int cmsis_dap_get_status(void)
 785 {
 786         uint8_t d;
 787
 788         int retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, 0, 0, &d);
 789
 790         if (retval == ERROR_OK) {
 791                 LOG_INFO("SWCLK/TCK = %d SWDIO/TMS = %d TDI = %d TDO = %d nTRST = %d nRESET = %d",
 792                         (d & SWJ_PIN_TCK) ? 1 : 0,
 793                         (d & SWJ_PIN_TMS) ? 1 : 0,
 794                         (d & SWJ_PIN_TDI) ? 1 : 0,
 795                         (d & SWJ_PIN_TDO) ? 1 : 0,
 796                         (d & SWJ_PIN_TRST) ? 1 : 0,
 797                         (d & SWJ_PIN_SRST) ? 1 : 0);
 798         }
 799
 800         return retval;
 801 }
 802
 803 static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 804 {
 805         const uint8_t *s;
 806         unsigned int s_len;
 807         int retval;
 808
 809         if ((output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST)) == (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
 810                 /* Following workaround deasserts reset on most adapters.
 811                  * Do not reconnect if a reset line is active!
 812                  * Reconnecting would break connecting under reset. */
 813
 814                 /* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
 815                 cmsis_dap_cmd_DAP_Disconnect();
 816
 817                 /* When we are reconnecting, DAP_Connect needs to be rerun, at
 818                  * least on Keil ULINK-ME */
 819                 retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
 820                 if (retval != ERROR_OK)
 821                         return retval;
 822         }
 823
 824         switch (seq) {
 825         case LINE_RESET:
 826                 LOG_DEBUG("SWD line reset");
 827                 s = swd_seq_line_reset;
 828                 s_len = swd_seq_line_reset_len;
 829                 break;
 830         case JTAG_TO_SWD:
 831                 LOG_DEBUG("JTAG-to-SWD");
 832                 s = swd_seq_jtag_to_swd;
 833                 s_len = swd_seq_jtag_to_swd_len;
 834                 break;
 835         case SWD_TO_JTAG:
 836                 LOG_DEBUG("SWD-to-JTAG");
 837                 s = swd_seq_swd_to_jtag;
 838                 s_len = swd_seq_swd_to_jtag_len;
 839                 break;
 840         default:
 841                 LOG_ERROR("Sequence %d not supported", seq);
 842                 return ERROR_FAIL;
 843         }
 844
 845         retval = cmsis_dap_cmd_DAP_SWJ_Sequence(s_len, s);
 846         if (retval != ERROR_OK)
 847                 return retval;
 848
 849         /* Atmel EDBG needs renew clock setting after SWJ_Sequence
 850          * otherwise default frequency is used */
 851         return cmsis_dap_cmd_DAP_SWJ_Clock(jtag_get_speed_khz());
 852 }
 853
 854 static int cmsis_dap_swd_open(void)
 855 {
 856         int retval;
 857
 858         if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
 859                 LOG_ERROR("CMSIS-DAP: SWD not supported");
 860                 return ERROR_JTAG_DEVICE_ERROR;
 861         }
 862
 863         retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
 864         if (retval != ERROR_OK)
 865                 return retval;
 866
 867         /* Add more setup here.??... */
 868
 869         LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
 870         return ERROR_OK;
 871 }
 872
 873 static int cmsis_dap_init(void)
 874 {
 875         int retval;
 876         uint8_t *data;
 877
 878         retval = cmsis_dap_usb_open();
 879         if (retval != ERROR_OK)
 880                 return retval;
 881
 882         retval = cmsis_dap_get_caps_info();
 883         if (retval != ERROR_OK)
 884                 return retval;
 885
 886         retval = cmsis_dap_get_version_info();
 887         if (retval != ERROR_OK)
 888                 return retval;
 889
 890         retval = cmsis_dap_get_serial_info();
 891         if (retval != ERROR_OK)
 892                 return retval;
 893
 894         if (swd_mode) {
 895                 retval = cmsis_dap_swd_open();
 896                 if (retval != ERROR_OK)
 897                         return retval;
 898         }
 899
 900         if (cmsis_dap_handle == NULL) {
 901                 /* Connect in JTAG mode */
 902                 if (!(cmsis_dap_handle->caps & INFO_CAPS_JTAG)) {
 903                         LOG_ERROR("CMSIS-DAP: JTAG not supported");
 904                         return ERROR_JTAG_DEVICE_ERROR;
 905                 }
 906
 907                 retval = cmsis_dap_cmd_DAP_Connect(CONNECT_JTAG);
 908                 if (retval != ERROR_OK)
 909                         return retval;
 910
 911                 LOG_INFO("CMSIS-DAP: Interface Initialised (JTAG)");
 912         }
 913
 914         /* INFO_ID_PKT_SZ - short */
 915         retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_SZ, &data);
 916         if (retval != ERROR_OK)
 917                 return retval;
 918
 919         if (data[0] == 2) {  /* short */
 920                 uint16_t pkt_sz = data[1] + (data[2] << 8);
 921
 922                 /* 4 bytes of command header + 5 bytes per register
 923                  * write. For bulk read sequences just 4 bytes are
 924                  * needed per transfer, so this is suboptimal. */
 925                 pending_queue_len = (pkt_sz - 4) / 5;
 926                 pending_transfers = malloc(pending_queue_len * sizeof(*pending_transfers));
 927                 if (!pending_transfers) {
 928                         LOG_ERROR("Unable to allocate memory for CMSIS-DAP queue");
 929                         return ERROR_FAIL;
 930                 }
 931
 932                 if (cmsis_dap_handle->packet_size != pkt_sz + 1) {
 933                         /* reallocate buffer */
 934                         cmsis_dap_handle->packet_size = pkt_sz + 1;
 935                         cmsis_dap_handle->packet_buffer = realloc(cmsis_dap_handle->packet_buffer,
 936                                         cmsis_dap_handle->packet_size);
 937                         if (cmsis_dap_handle->packet_buffer == NULL) {
 938                                 LOG_ERROR("unable to reallocate memory");
 939                                 return ERROR_FAIL;
 940                         }
 941                 }
 942
 943                 LOG_DEBUG("CMSIS-DAP: Packet Size = %" PRId16, pkt_sz);
 944         }
 945
 946         /* INFO_ID_PKT_CNT - byte */
 947         retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_CNT, &data);
 948         if (retval != ERROR_OK)
 949                 return retval;
 950
 951         if (data[0] == 1) { /* byte */
 952                 uint16_t pkt_cnt = data[1];
 953                 cmsis_dap_handle->packet_count = pkt_cnt;
 954                 LOG_DEBUG("CMSIS-DAP: Packet Count = %" PRId16, pkt_cnt);
 955         }
 956
 957         retval = cmsis_dap_get_status();
 958         if (retval != ERROR_OK)
 959                 return ERROR_FAIL;
 960
 961         /* Now try to connect to the target
 962          * TODO: This is all SWD only @ present */
 963         retval = cmsis_dap_cmd_DAP_SWJ_Clock(jtag_get_speed_khz());
 964         if (retval != ERROR_OK)
 965                 return ERROR_FAIL;
 966
 967         /* Ask CMSIS-DAP to automatically retry on receiving WAIT for
 968          * up to 64 times. This must be changed to 0 if sticky
 969          * overrun detection is enabled. */
 970         retval = cmsis_dap_cmd_DAP_TFER_Configure(0, 64, 0);
 971         if (retval != ERROR_OK)
 972                 return ERROR_FAIL;
 973
 974         if (swd_mode) {
 975                 /* Data Phase (bit 2) must be set to 1 if sticky overrun
 976                  * detection is enabled */
 977                 retval = cmsis_dap_cmd_DAP_SWD_Configure(0);    /* 1 TRN, no Data Phase */
 978                 if (retval != ERROR_OK)
 979                         return ERROR_FAIL;
 980         }
 981
 982         retval = cmsis_dap_cmd_DAP_LED(0x03);           /* Both LEDs on */
 983         if (retval != ERROR_OK)
 984                 return ERROR_FAIL;
 985
 986         /* support connecting with srst asserted */
 987         enum reset_types jtag_reset_config = jtag_get_reset_config();
 988
 989         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
 990                 if (jtag_reset_config & RESET_SRST_NO_GATING) {
 991                         retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, (1 << 7), 0, NULL);
 992                         if (retval != ERROR_OK)
 993                                 return ERROR_FAIL;
 994                         LOG_INFO("Connecting under reset");
 995                 }
 996         }
 997
 998         cmsis_dap_cmd_DAP_LED(0x00);                    /* Both LEDs off */
 999
1000         LOG_INFO("CMSIS-DAP: Interface ready");
1001
1002         return ERROR_OK;
1003 }
1004
1005 static int cmsis_dap_swd_init(void)
1006 {
1007         swd_mode = true;
1008         return ERROR_OK;
1009 }
1010
1011 static int cmsis_dap_quit(void)
1012 {
1013         cmsis_dap_cmd_DAP_Disconnect();
1014         cmsis_dap_cmd_DAP_LED(0x00);            /* Both LEDs off */
1015
1016         cmsis_dap_usb_close(cmsis_dap_handle);
1017
1018         return ERROR_OK;
1019 }
1020
1021 static void cmsis_dap_execute_reset(struct jtag_command *cmd)
1022 {
1023         /* Set both TRST and SRST even if they're not enabled as
1024          * there's no way to tristate them */
1025
1026         output_pins = 0;
1027         if (!cmd->cmd.reset->srst)
1028                 output_pins |= SWJ_PIN_SRST;
1029         if (!cmd->cmd.reset->trst)
1030                 output_pins |= SWJ_PIN_TRST;
1031
1032         int retval = cmsis_dap_cmd_DAP_SWJ_Pins(output_pins,
1033                         SWJ_PIN_TRST | SWJ_PIN_SRST, 0, NULL);
1034         if (retval != ERROR_OK)
1035                 LOG_ERROR("CMSIS-DAP: Interface reset failed");
1036 }
1037
1038 static void cmsis_dap_execute_sleep(struct jtag_command *cmd)
1039 {
1040 #if 0
1041         int retval = cmsis_dap_cmd_DAP_Delay(cmd->cmd.sleep->us);
1042         if (retval != ERROR_OK)
1043 #endif
1044                 jtag_sleep(cmd->cmd.sleep->us);
1045 }
1046
1047 /* Set TMS high for five TCK clocks, to move the TAP to the Test-Logic-Reset state */
1048 static int cmsis_dap_execute_tlr_reset(struct jtag_command *cmd)
1049 {
1050         LOG_INFO("cmsis-dap JTAG TLR_RESET");
1051         uint8_t seq = 0xff;
1052         int ret = cmsis_dap_cmd_DAP_SWJ_Sequence(8, &seq);
1053         if (ret == ERROR_OK)
1054                 tap_set_state(TAP_RESET);
1055         return ret;
1056 }
1057
1058 /* Set new end state */
1059 static void cmsis_dap_end_state(tap_state_t state)
1060 {
1061         if (tap_is_state_stable(state))
1062                 tap_set_end_state(state);
1063         else {
1064                 LOG_ERROR("BUG: %i is not a valid end state", state);
1065                 exit(-1);
1066         }
1067 }
1068
1069 #ifdef SPRINT_BINARY
1070 static void sprint_binary(char *s, const uint8_t *buf, int offset, int len)
1071 {
1072         if (!len)
1073                 return;
1074
1075         /*
1076         buf = { 0x18 } len=5 should result in: 11000
1077         buf = { 0xff 0x18 } len=13 should result in: 11111111 11000
1078         buf = { 0xc0 0x18 } offset=3 len=10 should result in: 11000 11000
1079                 i=3 there means i/8 = 0 so c = 0xFF, and
1080         */
1081         for (int i = offset; i < offset + len; ++i) {
1082                 uint8_t c = buf[i / 8], mask = 1 << (i % 8);
1083                 if ((i != offset) && !(i % 8))
1084                         putchar(' ');
1085                 *s++ = (c & mask) ? '1' : '0';
1086         }
1087         *s = 0;
1088 }
1089 #endif
1090
1091 #ifdef CMSIS_DAP_JTAG_DEBUG
1092 static void debug_parse_cmsis_buf(const uint8_t *cmd, int cmdlen)
1093 {
1094         /* cmd is a usb packet to go to the cmsis-dap interface */
1095         printf("cmsis-dap buffer (%d b): ", cmdlen);
1096         for (int i = 0; i < cmdlen; ++i)
1097                 printf(" %02x", cmd[i]);
1098         printf("\n");
1099         switch (cmd[1]) {
1100                 case CMD_DAP_JTAG_SEQ: {
1101                         printf("cmsis-dap jtag sequence command %02x (n=%d)\n", cmd[1], cmd[2]);
1102                         /*
1103                          * #2 = number of sequences
1104                          * #3 = sequence info 1
1105                          * #4...4+n_bytes-1 = sequence 1
1106                          * #4+n_bytes = sequence info 2
1107                          * #5+n_bytes = sequence 2 (single bit)
1108                          */
1109                         int pos = 3;
1110                         for (int seq = 0; seq < cmd[2]; ++seq) {
1111                                 uint8_t info = cmd[pos++];
1112                                 int len = info & DAP_JTAG_SEQ_TCK;
1113                                 if (len == 0)
1114                                         len = 64;
1115                                 printf("  sequence %d starting %d: info %02x (len=%d tms=%d read_tdo=%d): ",
1116                                         seq, pos, info, len, info & DAP_JTAG_SEQ_TMS, info & DAP_JTAG_SEQ_TDO);
1117                                 for (int i = 0; i < DIV_ROUND_UP(len, 8); ++i)
1118                                         printf(" %02x", cmd[pos+i]);
1119                                 pos += DIV_ROUND_UP(len, 8);
1120                                 printf("\n");
1121                         }
1122                         if (pos != cmdlen) {
1123                                 printf("BUFFER LENGTH MISMATCH looks like %d but %d specified", pos, cmdlen);
1124                                 exit(-1);
1125                         }
1126
1127                         break;
1128                 }
1129                 default:
1130                         LOG_DEBUG("unknown cmsis-dap command %02x", cmd[1]);
1131                         break;
1132         }
1133 }
1134 #endif
1135
1136 static void cmsis_dap_flush(void)
1137 {
1138         if (!queued_seq_count)
1139                 return;
1140
1141         DEBUG_JTAG_IO("Flushing %d queued sequences (%d bytes) with %d pending scan results to capture",
1142                 queued_seq_count, queued_seq_buf_end, pending_scan_result_count);
1143
1144         /* prep CMSIS-DAP packet */
1145         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
1146         buffer[0] = 0;  /* report number */
1147         buffer[1] = CMD_DAP_JTAG_SEQ;
1148         buffer[2] = queued_seq_count;
1149         memcpy(buffer + 3, queued_seq_buf, queued_seq_buf_end);
1150
1151 #ifdef CMSIS_DAP_JTAG_DEBUG
1152         debug_parse_cmsis_buf(buffer, queued_seq_buf_end + 3);
1153 #endif
1154
1155         /* send command to USB device */
1156         int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, queued_seq_buf_end + 3);
1157         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
1158                 LOG_ERROR("CMSIS-DAP command CMD_DAP_JTAG_SEQ failed.");
1159                 exit(-1);
1160         }
1161
1162 #ifdef CMSIS_DAP_JTAG_DEBUG
1163         DEBUG_JTAG_IO("USB response buf:");
1164         for (int c = 0; c < queued_seq_buf_end + 3; ++c)
1165                 printf("%02X ", buffer[c]);
1166         printf("\n");
1167 #endif
1168
1169         /* copy scan results into client buffers */
1170         for (int i = 0; i < pending_scan_result_count; ++i) {
1171                 struct pending_scan_result *scan = &pending_scan_results[i];
1172                 DEBUG_JTAG_IO("Copying pending_scan_result %d/%d: %d bits from byte %d -> buffer + %d bits",
1173                         i, pending_scan_result_count, scan->length, scan->first + 2, scan->buffer_offset);
1174 #ifdef CMSIS_DAP_JTAG_DEBUG
1175                 for (uint32_t b = 0; b < DIV_ROUND_UP(scan->length, 8); ++b)
1176                         printf("%02X ", buffer[2+scan->first+b]);
1177                 printf("\n");
1178 #endif
1179                 bit_copy(scan->buffer, scan->buffer_offset, buffer + 2 + scan->first, 0, scan->length);
1180         }
1181
1182         /* reset */
1183         queued_seq_count = 0;
1184         queued_seq_buf_end = 0;
1185         queued_seq_tdo_ptr = 0;
1186         pending_scan_result_count = 0;
1187 }
1188
1189 /* queue a sequence of bits to clock out TDI / in TDO, executing if the buffer is full.
1190  *
1191  * sequence=NULL means clock out zeros on TDI
1192  * tdo_buffer=NULL means don't capture TDO
1193  */
1194 static void cmsis_dap_add_jtag_sequence(int s_len, const uint8_t *sequence, int s_offset,
1195                                         bool tms, uint8_t *tdo_buffer, int tdo_buffer_offset)
1196 {
1197         DEBUG_JTAG_IO("[at %d] %d bits, tms %s, seq offset %d, tdo buf %p, tdo offset %d",
1198                 queued_seq_buf_end,
1199                 s_len, tms ? "HIGH" : "LOW", s_offset, tdo_buffer, tdo_buffer_offset);
1200
1201         if (s_len == 0)
1202                 return;
1203
1204         if (s_len > 64) {
1205                 DEBUG_JTAG_IO("START JTAG SEQ SPLIT");
1206                 for (int offset = 0; offset < s_len; offset += 64) {
1207                         int len = s_len - offset;
1208                         if (len > 64)
1209                                 len = 64;
1210                         DEBUG_JTAG_IO("Splitting long jtag sequence: %d-bit chunk starting at offset %d", len, offset);
1211                         cmsis_dap_add_jtag_sequence(
1212                                 len,
1213                                 sequence,
1214                                 s_offset + offset,
1215                                 tms,
1216                                 tdo_buffer,
1217                                 tdo_buffer == NULL ? 0 : (tdo_buffer_offset + offset)
1218                                 );
1219                 }
1220                 DEBUG_JTAG_IO("END JTAG SEQ SPLIT");
1221                 return;
1222         }
1223
1224         int cmd_len = 1 + DIV_ROUND_UP(s_len, 8);
1225         if (queued_seq_count >= 255 || queued_seq_buf_end + cmd_len > QUEUED_SEQ_BUF_LEN)
1226                 /* empty out the buffer */
1227                 cmsis_dap_flush();
1228
1229         ++queued_seq_count;
1230
1231         /* control byte */
1232         queued_seq_buf[queued_seq_buf_end] =
1233                 (tms ? DAP_JTAG_SEQ_TMS : 0) |
1234                 (tdo_buffer != NULL ? DAP_JTAG_SEQ_TDO : 0) |
1235                 (s_len == 64 ? 0 : s_len);
1236
1237         if (sequence != NULL)
1238                 bit_copy(&queued_seq_buf[queued_seq_buf_end + 1], 0, sequence, s_offset, s_len);
1239         else
1240                 memset(&queued_seq_buf[queued_seq_buf_end + 1], 0, DIV_ROUND_UP(s_len, 8));
1241
1242         queued_seq_buf_end += cmd_len;
1243
1244         if (tdo_buffer != NULL) {
1245                 struct pending_scan_result *scan = &pending_scan_results[pending_scan_result_count++];
1246                 scan->first = queued_seq_tdo_ptr;
1247                 queued_seq_tdo_ptr += DIV_ROUND_UP(s_len, 8);
1248                 scan->length = s_len;
1249                 scan->buffer = tdo_buffer;
1250                 scan->buffer_offset = tdo_buffer_offset;
1251         }
1252 }
1253
1254 /* queue a sequence of bits to clock out TMS, executing if the buffer is full */
1255 static void cmsis_dap_add_tms_sequence(const uint8_t *sequence, int s_len)
1256 {
1257         DEBUG_JTAG_IO("%d bits: %02X", s_len, *sequence);
1258         /* we use a series of CMD_DAP_JTAG_SEQ commands to toggle TMS,
1259            because even though it seems ridiculously inefficient, it
1260            allows us to combine TMS and scan sequences into the same
1261            USB packet. */
1262         /* TODO: combine runs of the same tms value */
1263         for (int i = 0; i < s_len; ++i) {
1264                 bool bit = (sequence[i / 8] & (1 << (i % 8))) != 0;
1265                 cmsis_dap_add_jtag_sequence(1, NULL, 0, bit, NULL, 0);
1266         }
1267 }
1268
1269 /* Move to the end state by queuing a sequence to clock into TMS */
1270 static void cmsis_dap_state_move(void)
1271 {
1272         uint8_t tms_scan;
1273         uint8_t tms_scan_bits;
1274
1275         tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
1276         tms_scan_bits = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
1277
1278         DEBUG_JTAG_IO("state move from %s to %s: %d clocks, %02X on tms",
1279                 tap_state_name(tap_get_state()), tap_state_name(tap_get_end_state()),
1280                 tms_scan_bits, tms_scan);
1281         cmsis_dap_add_tms_sequence(&tms_scan, tms_scan_bits);
1282
1283         tap_set_state(tap_get_end_state());
1284 }
1285
1286
1287 /* Execute a JTAG scan operation by queueing TMS and TDI/TDO sequences */
1288 static void cmsis_dap_execute_scan(struct jtag_command *cmd)
1289 {
1290         DEBUG_JTAG_IO("%s type:%d", cmd->cmd.scan->ir_scan ? "IRSCAN" : "DRSCAN",
1291                 jtag_scan_type(cmd->cmd.scan));
1292
1293         /* Make sure there are no trailing fields with num_bits == 0, or the logic below will fail. */
1294         while (cmd->cmd.scan->num_fields > 0
1295                         && cmd->cmd.scan->fields[cmd->cmd.scan->num_fields - 1].num_bits == 0) {
1296                 cmd->cmd.scan->num_fields--;
1297                 LOG_DEBUG("discarding trailing empty field");
1298         }
1299
1300         if (cmd->cmd.scan->num_fields == 0) {
1301                 LOG_DEBUG("empty scan, doing nothing");
1302                 return;
1303         }
1304
1305         if (cmd->cmd.scan->ir_scan) {
1306                 if (tap_get_state() != TAP_IRSHIFT) {
1307                         cmsis_dap_end_state(TAP_IRSHIFT);
1308                         cmsis_dap_state_move();
1309                 }
1310         } else {
1311                 if (tap_get_state() != TAP_DRSHIFT) {
1312                         cmsis_dap_end_state(TAP_DRSHIFT);
1313                         cmsis_dap_state_move();
1314                 }
1315         }
1316
1317         cmsis_dap_end_state(cmd->cmd.scan->end_state);
1318
1319         struct scan_field *field = cmd->cmd.scan->fields;
1320         unsigned scan_size = 0;
1321
1322         for (int i = 0; i < cmd->cmd.scan->num_fields; i++, field++) {
1323                 scan_size += field->num_bits;
1324                 DEBUG_JTAG_IO("%s%s field %d/%d %d bits",
1325                         field->in_value ? "in" : "",
1326                         field->out_value ? "out" : "",
1327                         i,
1328                         cmd->cmd.scan->num_fields,
1329                         field->num_bits);
1330
1331                 if (i == cmd->cmd.scan->num_fields - 1 && tap_get_state() != tap_get_end_state()) {
1332                         DEBUG_JTAG_IO("Last field and have to move out of SHIFT state");
1333                         /* Last field, and we're leaving IRSHIFT/DRSHIFT. Clock last bit during tap
1334                          * movement. This last field can't have length zero, it was checked above. */
1335                         cmsis_dap_add_jtag_sequence(
1336                                 field->num_bits - 1, /* number of bits to clock */
1337                                 field->out_value, /* output sequence */
1338                                 0, /* output offset */
1339                                 false, /* TMS low */
1340                                 field->in_value,
1341                                 0);
1342
1343                         /* Clock the last bit out, with TMS high */
1344                         uint8_t last_bit = 0;
1345                         if (field->out_value)
1346                                 bit_copy(&last_bit, 0, field->out_value, field->num_bits - 1, 1);
1347                         cmsis_dap_add_jtag_sequence(
1348                                 1,
1349                                 &last_bit,
1350                                 0,
1351                                 true,
1352                                 field->in_value,
1353                                 field->num_bits - 1);
1354                         tap_set_state(tap_state_transition(tap_get_state(), 1));
1355
1356                         /* Now clock one more cycle, with TMS low, to get us into a PAUSE state */
1357                         cmsis_dap_add_jtag_sequence(
1358                                 1,
1359                                 &last_bit,
1360                                 0,
1361                                 false,
1362                                 NULL,
1363                                 0);
1364                         tap_set_state(tap_state_transition(tap_get_state(), 0));
1365                 } else {
1366                         DEBUG_JTAG_IO("Internal field, staying in SHIFT state afterwards");
1367                         /* Clocking part of a sequence into DR or IR with TMS=0,
1368                            leaving TMS=0 at the end so we can continue later */
1369                         cmsis_dap_add_jtag_sequence(
1370                                 field->num_bits,
1371                                 field->out_value,
1372                                 0,
1373                                 false,
1374                                 field->in_value,
1375                                 0);
1376                 }
1377         }
1378
1379         if (tap_get_state() != tap_get_end_state()) {
1380                 cmsis_dap_end_state(tap_get_end_state());
1381                 cmsis_dap_state_move();
1382         }
1383
1384         DEBUG_JTAG_IO("%s scan, %i bits, end in %s",
1385                 (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
1386                 tap_state_name(tap_get_end_state()));
1387 }
1388
1389 static void cmsis_dap_pathmove(int num_states, tap_state_t *path)
1390 {
1391         int i;
1392         uint8_t tms0 = 0x00;
1393         uint8_t tms1 = 0xff;
1394
1395         for (i = 0; i < num_states; i++) {
1396                 if (path[i] == tap_state_transition(tap_get_state(), false))
1397                         cmsis_dap_add_tms_sequence(&tms0, 1);
1398                 else if (path[i] == tap_state_transition(tap_get_state(), true))
1399                         cmsis_dap_add_tms_sequence(&tms1, 1);
1400                 else {
1401                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition.",
1402                                   tap_state_name(tap_get_state()), tap_state_name(path[i]));
1403                         exit(-1);
1404                 }
1405
1406                 tap_set_state(path[i]);
1407         }
1408
1409         cmsis_dap_end_state(tap_get_state());
1410 }
1411
1412 static void cmsis_dap_execute_pathmove(struct jtag_command *cmd)
1413 {
1414         DEBUG_JTAG_IO("pathmove: %i states, end in %i",
1415                       cmd->cmd.pathmove->num_states,
1416                cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1417
1418         cmsis_dap_pathmove(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
1419 }
1420
1421 static void cmsis_dap_stableclocks(int num_cycles)
1422 {
1423         int i;
1424
1425         uint8_t tms = tap_get_state() == TAP_RESET;
1426         /* TODO: Perform optimizations? */
1427         /* Execute num_cycles. */
1428         for (i = 0; i < num_cycles; i++)
1429                 cmsis_dap_add_tms_sequence(&tms, 1);
1430 }
1431
1432 static void cmsis_dap_runtest(int num_cycles)
1433 {
1434         tap_state_t saved_end_state = tap_get_end_state();
1435
1436         /* Only do a state_move when we're not already in IDLE. */
1437         if (tap_get_state() != TAP_IDLE) {
1438                 cmsis_dap_end_state(TAP_IDLE);
1439                 cmsis_dap_state_move();
1440         }
1441         cmsis_dap_stableclocks(num_cycles);
1442
1443         /* Finish in end_state. */
1444         cmsis_dap_end_state(saved_end_state);
1445
1446         if (tap_get_state() != tap_get_end_state())
1447                 cmsis_dap_state_move();
1448 }
1449
1450 static void cmsis_dap_execute_runtest(struct jtag_command *cmd)
1451 {
1452         DEBUG_JTAG_IO("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles,
1453                       cmd->cmd.runtest->end_state);
1454
1455         cmsis_dap_end_state(cmd->cmd.runtest->end_state);
1456         cmsis_dap_runtest(cmd->cmd.runtest->num_cycles);
1457 }
1458
1459 static void cmsis_dap_execute_stableclocks(struct jtag_command *cmd)
1460 {
1461         DEBUG_JTAG_IO("stableclocks %i cycles", cmd->cmd.runtest->num_cycles);
1462         cmsis_dap_stableclocks(cmd->cmd.runtest->num_cycles);
1463 }
1464
1465 static void cmsis_dap_execute_tms(struct jtag_command *cmd)
1466 {
1467         DEBUG_JTAG_IO("TMS: %d bits", cmd->cmd.tms->num_bits);
1468         cmsis_dap_cmd_DAP_SWJ_Sequence(cmd->cmd.tms->num_bits, cmd->cmd.tms->bits);
1469 }
1470
1471 /* TODO: Is there need to call cmsis_dap_flush() for the JTAG_PATHMOVE,
1472  * JTAG_RUNTEST, JTAG_STABLECLOCKS? */
1473 static void cmsis_dap_execute_command(struct jtag_command *cmd)
1474 {
1475         switch (cmd->type) {
1476                 case JTAG_RESET:
1477                         cmsis_dap_flush();
1478                         cmsis_dap_execute_reset(cmd);
1479                         break;
1480                 case JTAG_SLEEP:
1481                         cmsis_dap_flush();
1482                         cmsis_dap_execute_sleep(cmd);
1483                         break;
1484                 case JTAG_TLR_RESET:
1485                         cmsis_dap_flush();
1486                         cmsis_dap_execute_tlr_reset(cmd);
1487                         break;
1488                 case JTAG_SCAN:
1489                         cmsis_dap_execute_scan(cmd);
1490                         break;
1491                 case JTAG_PATHMOVE:
1492                         cmsis_dap_execute_pathmove(cmd);
1493                         break;
1494                 case JTAG_RUNTEST:
1495                         cmsis_dap_execute_runtest(cmd);
1496                         break;
1497                 case JTAG_STABLECLOCKS:
1498                         cmsis_dap_execute_stableclocks(cmd);
1499                         break;
1500                 case JTAG_TMS:
1501                         cmsis_dap_execute_tms(cmd);
1502                         break;
1503                 default:
1504                         LOG_ERROR("BUG: unknown JTAG command type 0x%X encountered", cmd->type);
1505                         exit(-1);
1506         }
1507 }
1508
1509 static int cmsis_dap_execute_queue(void)
1510 {
1511         struct jtag_command *cmd = jtag_command_queue;
1512
1513         while (cmd != NULL) {
1514                 cmsis_dap_execute_command(cmd);
1515                 cmd = cmd->next;
1516         }
1517
1518         cmsis_dap_flush();
1519
1520         return ERROR_OK;
1521 }
1522
1523 static int cmsis_dap_speed(int speed)
1524 {
1525         if (speed > DAP_MAX_CLOCK)
1526                 LOG_INFO("High speed (adapter_khz %d) may be limited by adapter firmware.", speed);
1527
1528         if (speed == 0) {
1529                 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
1530                 return ERROR_JTAG_NOT_IMPLEMENTED;
1531         }
1532
1533         return cmsis_dap_cmd_DAP_SWJ_Clock(speed);
1534 }
1535
1536 static int cmsis_dap_speed_div(int speed, int *khz)
1537 {
1538         *khz = speed;
1539         return ERROR_OK;
1540 }
1541
1542 static int cmsis_dap_khz(int khz, int *jtag_speed)
1543 {
1544         *jtag_speed = khz;
1545         return ERROR_OK;
1546 }
1547
1548 static int_least32_t cmsis_dap_swd_frequency(int_least32_t hz)
1549 {
1550         if (hz > 0)
1551                 cmsis_dap_speed(hz / 1000);
1552
1553         return hz;
1554 }
1555
1556
1557 COMMAND_HANDLER(cmsis_dap_handle_info_command)
1558 {
1559         if (cmsis_dap_get_version_info() == ERROR_OK)
1560                 cmsis_dap_get_status();
1561
1562         return ERROR_OK;
1563 }
1564
1565 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
1566 {
1567         if (CMD_ARGC > MAX_USB_IDS * 2) {
1568                 LOG_WARNING("ignoring extra IDs in cmsis_dap_vid_pid "
1569                         "(maximum is %d pairs)", MAX_USB_IDS);
1570                 CMD_ARGC = MAX_USB_IDS * 2;
1571         }
1572         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
1573                 LOG_WARNING("incomplete cmsis_dap_vid_pid configuration directive");
1574                 if (CMD_ARGC < 2)
1575                         return ERROR_COMMAND_SYNTAX_ERROR;
1576                 /* remove the incomplete trailing id */
1577                 CMD_ARGC -= 1;
1578         }
1579
1580         unsigned i;
1581         for (i = 0; i < CMD_ARGC; i += 2) {
1582                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], cmsis_dap_vid[i >> 1]);
1583                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], cmsis_dap_pid[i >> 1]);
1584         }
1585
1586         /*
1587          * Explicitly terminate, in case there are multiples instances of
1588          * cmsis_dap_vid_pid.
1589          */
1590         cmsis_dap_vid[i >> 1] = cmsis_dap_pid[i >> 1] = 0;
1591
1592         return ERROR_OK;
1593 }
1594
1595 COMMAND_HANDLER(cmsis_dap_handle_serial_command)
1596 {
1597         if (CMD_ARGC == 1) {
1598                 size_t len = mbstowcs(NULL, CMD_ARGV[0], 0);
1599                 cmsis_dap_serial = calloc(len + 1, sizeof(wchar_t));
1600                 if (cmsis_dap_serial == NULL) {
1601                         LOG_ERROR("unable to allocate memory");
1602                         return ERROR_OK;
1603                 }
1604                 if (mbstowcs(cmsis_dap_serial, CMD_ARGV[0], len + 1) == (size_t)-1) {
1605                         free(cmsis_dap_serial);
1606                         cmsis_dap_serial = NULL;
1607                         LOG_ERROR("unable to convert serial");
1608                 }
1609         } else {
1610                 LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
1611         }
1612
1613         return ERROR_OK;
1614 }
1615
1616 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
1617         {
1618                 .name = "info",
1619                 .handler = &cmsis_dap_handle_info_command,
1620                 .mode = COMMAND_EXEC,
1621                 .usage = "",
1622                 .help = "show cmsis-dap info",
1623         },
1624         COMMAND_REGISTRATION_DONE
1625 };
1626
1627 static const struct command_registration cmsis_dap_command_handlers[] = {
1628         {
1629                 .name = "cmsis-dap",
1630                 .mode = COMMAND_ANY,
1631                 .help = "perform CMSIS-DAP management",
1632                 .usage = "<cmd>",
1633                 .chain = cmsis_dap_subcommand_handlers,
1634         },
1635         {
1636                 .name = "cmsis_dap_vid_pid",
1637                 .handler = &cmsis_dap_handle_vid_pid_command,
1638                 .mode = COMMAND_CONFIG,
1639                 .help = "the vendor ID and product ID of the CMSIS-DAP device",
1640                 .usage = "(vid pid)* ",
1641         },
1642         {
1643                 .name = "cmsis_dap_serial",
1644                 .handler = &cmsis_dap_handle_serial_command,
1645                 .mode = COMMAND_CONFIG,
1646                 .help = "set the serial number of the adapter",
1647                 .usage = "serial_string",
1648         },
1649         COMMAND_REGISTRATION_DONE
1650 };
1651
1652 static const struct swd_driver cmsis_dap_swd_driver = {
1653         .init = cmsis_dap_swd_init,
1654         .frequency = cmsis_dap_swd_frequency,
1655         .switch_seq = cmsis_dap_swd_switch_seq,
1656         .read_reg = cmsis_dap_swd_read_reg,
1657         .write_reg = cmsis_dap_swd_write_reg,
1658         .run = cmsis_dap_swd_run_queue,
1659 };
1660
1661 static const char * const cmsis_dap_transport[] = { "swd", "jtag", NULL };
1662
1663 struct jtag_interface cmsis_dap_interface = {
1664         .name = "cmsis-dap",
1665         .supported = DEBUG_CAP_TMS_SEQ,
1666         .commands = cmsis_dap_command_handlers,
1667         .swd = &cmsis_dap_swd_driver,
1668         .transports = cmsis_dap_transport,
1669
1670         .execute_queue = cmsis_dap_execute_queue,
1671         .speed = cmsis_dap_speed,
1672         .speed_div = cmsis_dap_speed_div,
1673         .khz = cmsis_dap_khz,
1674         .init = cmsis_dap_init,
1675         .quit = cmsis_dap_quit,
1676 };