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_version_info(void)
 733 {
 734         uint8_t *data;
 735
 736         /* INFO_ID_FW_VER - string */
 737         int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_FW_VER, &data);
 738         if (retval != ERROR_OK)
 739                 return retval;
 740
 741         if (data[0]) /* strlen */
 742                 LOG_INFO("CMSIS-DAP: FW Version = %s", &data[1]);
 743
 744         return ERROR_OK;
 745 }
 746
 747 static int cmsis_dap_get_caps_info(void)
 748 {
 749         uint8_t *data;
 750
 751         /* INFO_ID_CAPS - byte */
 752         int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_CAPS, &data);
 753         if (retval != ERROR_OK)
 754                 return retval;
 755
 756         if (data[0] == 1) {
 757                 uint8_t caps = data[1];
 758
 759                 cmsis_dap_handle->caps = caps;
 760
 761                 if (caps & INFO_CAPS_SWD)
 762                         LOG_INFO("CMSIS-DAP: %s", info_caps_str[0]);
 763                 if (caps & INFO_CAPS_JTAG)
 764                         LOG_INFO("CMSIS-DAP: %s", info_caps_str[1]);
 765         }
 766
 767         return ERROR_OK;
 768 }
 769
 770 static int cmsis_dap_get_status(void)
 771 {
 772         uint8_t d;
 773
 774         int retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, 0, 0, &d);
 775
 776         if (retval == ERROR_OK) {
 777                 LOG_INFO("SWCLK/TCK = %d SWDIO/TMS = %d TDI = %d TDO = %d nTRST = %d nRESET = %d",
 778                         (d & SWJ_PIN_TCK) ? 1 : 0,
 779                         (d & SWJ_PIN_TMS) ? 1 : 0,
 780                         (d & SWJ_PIN_TDI) ? 1 : 0,
 781                         (d & SWJ_PIN_TDO) ? 1 : 0,
 782                         (d & SWJ_PIN_TRST) ? 1 : 0,
 783                         (d & SWJ_PIN_SRST) ? 1 : 0);
 784         }
 785
 786         return retval;
 787 }
 788
 789 static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 790 {
 791         const uint8_t *s;
 792         unsigned int s_len;
 793         int retval;
 794
 795         if ((output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST)) == (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
 796                 /* Following workaround deasserts reset on most adapters.
 797                  * Do not reconnect if a reset line is active!
 798                  * Reconnecting would break connecting under reset. */
 799
 800                 /* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
 801                 cmsis_dap_cmd_DAP_Disconnect();
 802
 803                 /* When we are reconnecting, DAP_Connect needs to be rerun, at
 804                  * least on Keil ULINK-ME */
 805                 retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
 806                 if (retval != ERROR_OK)
 807                         return retval;
 808         }
 809
 810         switch (seq) {
 811         case LINE_RESET:
 812                 LOG_DEBUG("SWD line reset");
 813                 s = swd_seq_line_reset;
 814                 s_len = swd_seq_line_reset_len;
 815                 break;
 816         case JTAG_TO_SWD:
 817                 LOG_DEBUG("JTAG-to-SWD");
 818                 s = swd_seq_jtag_to_swd;
 819                 s_len = swd_seq_jtag_to_swd_len;
 820                 break;
 821         case SWD_TO_JTAG:
 822                 LOG_DEBUG("SWD-to-JTAG");
 823                 s = swd_seq_swd_to_jtag;
 824                 s_len = swd_seq_swd_to_jtag_len;
 825                 break;
 826         default:
 827                 LOG_ERROR("Sequence %d not supported", seq);
 828                 return ERROR_FAIL;
 829         }
 830
 831         retval = cmsis_dap_cmd_DAP_SWJ_Sequence(s_len, s);
 832         if (retval != ERROR_OK)
 833                 return retval;
 834
 835         /* Atmel EDBG needs renew clock setting after SWJ_Sequence
 836          * otherwise default frequency is used */
 837         return cmsis_dap_cmd_DAP_SWJ_Clock(jtag_get_speed_khz());
 838 }
 839
 840 static int cmsis_dap_swd_open(void)
 841 {
 842         int retval;
 843
 844         if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
 845                 LOG_ERROR("CMSIS-DAP: SWD not supported");
 846                 return ERROR_JTAG_DEVICE_ERROR;
 847         }
 848
 849         retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
 850         if (retval != ERROR_OK)
 851                 return retval;
 852
 853         /* Add more setup here.??... */
 854
 855         LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
 856         return ERROR_OK;
 857 }
 858
 859 static int cmsis_dap_init(void)
 860 {
 861         int retval;
 862         uint8_t *data;
 863
 864         retval = cmsis_dap_usb_open();
 865         if (retval != ERROR_OK)
 866                 return retval;
 867
 868         retval = cmsis_dap_get_caps_info();
 869         if (retval != ERROR_OK)
 870                 return retval;
 871
 872         retval = cmsis_dap_get_version_info();
 873         if (retval != ERROR_OK)
 874                 return retval;
 875
 876         if (swd_mode) {
 877                 retval = cmsis_dap_swd_open();
 878                 if (retval != ERROR_OK)
 879                         return retval;
 880         }
 881
 882         if (cmsis_dap_handle == NULL) {
 883                 /* Connect in JTAG mode */
 884                 if (!(cmsis_dap_handle->caps & INFO_CAPS_JTAG)) {
 885                         LOG_ERROR("CMSIS-DAP: JTAG not supported");
 886                         return ERROR_JTAG_DEVICE_ERROR;
 887                 }
 888
 889                 retval = cmsis_dap_cmd_DAP_Connect(CONNECT_JTAG);
 890                 if (retval != ERROR_OK)
 891                         return retval;
 892
 893                 LOG_INFO("CMSIS-DAP: Interface Initialised (JTAG)");
 894         }
 895
 896         /* INFO_ID_PKT_SZ - short */
 897         retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_SZ, &data);
 898         if (retval != ERROR_OK)
 899                 return retval;
 900
 901         if (data[0] == 2) {  /* short */
 902                 uint16_t pkt_sz = data[1] + (data[2] << 8);
 903
 904                 /* 4 bytes of command header + 5 bytes per register
 905                  * write. For bulk read sequences just 4 bytes are
 906                  * needed per transfer, so this is suboptimal. */
 907                 pending_queue_len = (pkt_sz - 4) / 5;
 908                 pending_transfers = malloc(pending_queue_len * sizeof(*pending_transfers));
 909                 if (!pending_transfers) {
 910                         LOG_ERROR("Unable to allocate memory for CMSIS-DAP queue");
 911                         return ERROR_FAIL;
 912                 }
 913
 914                 if (cmsis_dap_handle->packet_size != pkt_sz + 1) {
 915                         /* reallocate buffer */
 916                         cmsis_dap_handle->packet_size = pkt_sz + 1;
 917                         cmsis_dap_handle->packet_buffer = realloc(cmsis_dap_handle->packet_buffer,
 918                                         cmsis_dap_handle->packet_size);
 919                         if (cmsis_dap_handle->packet_buffer == NULL) {
 920                                 LOG_ERROR("unable to reallocate memory");
 921                                 return ERROR_FAIL;
 922                         }
 923                 }
 924
 925                 LOG_DEBUG("CMSIS-DAP: Packet Size = %" PRId16, pkt_sz);
 926         }
 927
 928         /* INFO_ID_PKT_CNT - byte */
 929         retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_CNT, &data);
 930         if (retval != ERROR_OK)
 931                 return retval;
 932
 933         if (data[0] == 1) { /* byte */
 934                 uint16_t pkt_cnt = data[1];
 935                 cmsis_dap_handle->packet_count = pkt_cnt;
 936                 LOG_DEBUG("CMSIS-DAP: Packet Count = %" PRId16, pkt_cnt);
 937         }
 938
 939         retval = cmsis_dap_get_status();
 940         if (retval != ERROR_OK)
 941                 return ERROR_FAIL;
 942
 943         /* Now try to connect to the target
 944          * TODO: This is all SWD only @ present */
 945         retval = cmsis_dap_cmd_DAP_SWJ_Clock(jtag_get_speed_khz());
 946         if (retval != ERROR_OK)
 947                 return ERROR_FAIL;
 948
 949         /* Ask CMSIS-DAP to automatically retry on receiving WAIT for
 950          * up to 64 times. This must be changed to 0 if sticky
 951          * overrun detection is enabled. */
 952         retval = cmsis_dap_cmd_DAP_TFER_Configure(0, 64, 0);
 953         if (retval != ERROR_OK)
 954                 return ERROR_FAIL;
 955
 956         if (swd_mode) {
 957                 /* Data Phase (bit 2) must be set to 1 if sticky overrun
 958                  * detection is enabled */
 959                 retval = cmsis_dap_cmd_DAP_SWD_Configure(0);    /* 1 TRN, no Data Phase */
 960                 if (retval != ERROR_OK)
 961                         return ERROR_FAIL;
 962         }
 963
 964         retval = cmsis_dap_cmd_DAP_LED(0x03);           /* Both LEDs on */
 965         if (retval != ERROR_OK)
 966                 return ERROR_FAIL;
 967
 968         /* support connecting with srst asserted */
 969         enum reset_types jtag_reset_config = jtag_get_reset_config();
 970
 971         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
 972                 if (jtag_reset_config & RESET_SRST_NO_GATING) {
 973                         retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, (1 << 7), 0, NULL);
 974                         if (retval != ERROR_OK)
 975                                 return ERROR_FAIL;
 976                         LOG_INFO("Connecting under reset");
 977                 }
 978         }
 979
 980         cmsis_dap_cmd_DAP_LED(0x00);                    /* Both LEDs off */
 981
 982         LOG_INFO("CMSIS-DAP: Interface ready");
 983
 984         return ERROR_OK;
 985 }
 986
 987 static int cmsis_dap_swd_init(void)
 988 {
 989         swd_mode = true;
 990         return ERROR_OK;
 991 }
 992
 993 static int cmsis_dap_quit(void)
 994 {
 995         cmsis_dap_cmd_DAP_Disconnect();
 996         cmsis_dap_cmd_DAP_LED(0x00);            /* Both LEDs off */
 997
 998         cmsis_dap_usb_close(cmsis_dap_handle);
 999
1000         return ERROR_OK;
1001 }
1002
1003 static void cmsis_dap_execute_reset(struct jtag_command *cmd)
1004 {
1005         /* Set both TRST and SRST even if they're not enabled as
1006          * there's no way to tristate them */
1007
1008         output_pins = 0;
1009         if (!cmd->cmd.reset->srst)
1010                 output_pins |= SWJ_PIN_SRST;
1011         if (!cmd->cmd.reset->trst)
1012                 output_pins |= SWJ_PIN_TRST;
1013
1014         int retval = cmsis_dap_cmd_DAP_SWJ_Pins(output_pins,
1015                         SWJ_PIN_TRST | SWJ_PIN_SRST, 0, NULL);
1016         if (retval != ERROR_OK)
1017                 LOG_ERROR("CMSIS-DAP: Interface reset failed");
1018 }
1019
1020 static void cmsis_dap_execute_sleep(struct jtag_command *cmd)
1021 {
1022 #if 0
1023         int retval = cmsis_dap_cmd_DAP_Delay(cmd->cmd.sleep->us);
1024         if (retval != ERROR_OK)
1025 #endif
1026                 jtag_sleep(cmd->cmd.sleep->us);
1027 }
1028
1029 /* Set TMS high for five TCK clocks, to move the TAP to the Test-Logic-Reset state */
1030 static int cmsis_dap_execute_tlr_reset(struct jtag_command *cmd)
1031 {
1032         LOG_INFO("cmsis-dap JTAG TLR_RESET");
1033         uint8_t seq = 0xff;
1034         int ret = cmsis_dap_cmd_DAP_SWJ_Sequence(8, &seq);
1035         if (ret == ERROR_OK)
1036                 tap_set_state(TAP_RESET);
1037         return ret;
1038 }
1039
1040 /* Set new end state */
1041 static void cmsis_dap_end_state(tap_state_t state)
1042 {
1043         if (tap_is_state_stable(state))
1044                 tap_set_end_state(state);
1045         else {
1046                 LOG_ERROR("BUG: %i is not a valid end state", state);
1047                 exit(-1);
1048         }
1049 }
1050
1051 #ifdef SPRINT_BINARY
1052 static void sprint_binary(char *s, const uint8_t *buf, int offset, int len)
1053 {
1054         if (!len)
1055                 return;
1056
1057         /*
1058         buf = { 0x18 } len=5 should result in: 11000
1059         buf = { 0xff 0x18 } len=13 should result in: 11111111 11000
1060         buf = { 0xc0 0x18 } offset=3 len=10 should result in: 11000 11000
1061                 i=3 there means i/8 = 0 so c = 0xFF, and
1062         */
1063         for (int i = offset; i < offset + len; ++i) {
1064                 uint8_t c = buf[i / 8], mask = 1 << (i % 8);
1065                 if ((i != offset) && !(i % 8))
1066                         putchar(' ');
1067                 *s++ = (c & mask) ? '1' : '0';
1068         }
1069         *s = 0;
1070 }
1071 #endif
1072
1073 #ifdef CMSIS_DAP_JTAG_DEBUG
1074 static void debug_parse_cmsis_buf(const uint8_t *cmd, int cmdlen)
1075 {
1076         /* cmd is a usb packet to go to the cmsis-dap interface */
1077         printf("cmsis-dap buffer (%d b): ", cmdlen);
1078         for (int i = 0; i < cmdlen; ++i)
1079                 printf(" %02x", cmd[i]);
1080         printf("\n");
1081         switch (cmd[1]) {
1082                 case CMD_DAP_JTAG_SEQ: {
1083                         printf("cmsis-dap jtag sequence command %02x (n=%d)\n", cmd[1], cmd[2]);
1084                         /*
1085                          * #2 = number of sequences
1086                          * #3 = sequence info 1
1087                          * #4...4+n_bytes-1 = sequence 1
1088                          * #4+n_bytes = sequence info 2
1089                          * #5+n_bytes = sequence 2 (single bit)
1090                          */
1091                         int pos = 3;
1092                         for (int seq = 0; seq < cmd[2]; ++seq) {
1093                                 uint8_t info = cmd[pos++];
1094                                 int len = info & DAP_JTAG_SEQ_TCK;
1095                                 if (len == 0)
1096                                         len = 64;
1097                                 printf("  sequence %d starting %d: info %02x (len=%d tms=%d read_tdo=%d): ",
1098                                         seq, pos, info, len, info & DAP_JTAG_SEQ_TMS, info & DAP_JTAG_SEQ_TDO);
1099                                 for (int i = 0; i < DIV_ROUND_UP(len, 8); ++i)
1100                                         printf(" %02x", cmd[pos+i]);
1101                                 pos += DIV_ROUND_UP(len, 8);
1102                                 printf("\n");
1103                         }
1104                         if (pos != cmdlen) {
1105                                 printf("BUFFER LENGTH MISMATCH looks like %d but %d specified", pos, cmdlen);
1106                                 exit(-1);
1107                         }
1108
1109                         break;
1110                 }
1111                 default:
1112                         LOG_DEBUG("unknown cmsis-dap command %02x", cmd[1]);
1113                         break;
1114         }
1115 }
1116 #endif
1117
1118 static void cmsis_dap_flush(void)
1119 {
1120         if (!queued_seq_count)
1121                 return;
1122
1123         DEBUG_JTAG_IO("Flushing %d queued sequences (%d bytes) with %d pending scan results to capture",
1124                 queued_seq_count, queued_seq_buf_end, pending_scan_result_count);
1125
1126         /* prep CMSIS-DAP packet */
1127         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
1128         buffer[0] = 0;  /* report number */
1129         buffer[1] = CMD_DAP_JTAG_SEQ;
1130         buffer[2] = queued_seq_count;
1131         memcpy(buffer + 3, queued_seq_buf, queued_seq_buf_end);
1132
1133 #ifdef CMSIS_DAP_JTAG_DEBUG
1134         debug_parse_cmsis_buf(buffer, queued_seq_buf_end + 3);
1135 #endif
1136
1137         /* send command to USB device */
1138         int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, queued_seq_buf_end + 3);
1139         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
1140                 LOG_ERROR("CMSIS-DAP command CMD_DAP_JTAG_SEQ failed.");
1141                 exit(-1);
1142         }
1143
1144 #ifdef CMSIS_DAP_JTAG_DEBUG
1145         DEBUG_JTAG_IO("USB response buf:");
1146         for (int c = 0; c < queued_seq_buf_end + 3; ++c)
1147                 printf("%02X ", buffer[c]);
1148         printf("\n");
1149 #endif
1150
1151         /* copy scan results into client buffers */
1152         for (int i = 0; i < pending_scan_result_count; ++i) {
1153                 struct pending_scan_result *scan = &pending_scan_results[i];
1154                 DEBUG_JTAG_IO("Copying pending_scan_result %d/%d: %d bits from byte %d -> buffer + %d bits",
1155                         i, pending_scan_result_count, scan->length, scan->first + 2, scan->buffer_offset);
1156 #ifdef CMSIS_DAP_JTAG_DEBUG
1157                 for (uint32_t b = 0; b < DIV_ROUND_UP(scan->length, 8); ++b)
1158                         printf("%02X ", buffer[2+scan->first+b]);
1159                 printf("\n");
1160 #endif
1161                 bit_copy(scan->buffer, scan->buffer_offset, buffer + 2 + scan->first, 0, scan->length);
1162         }
1163
1164         /* reset */
1165         queued_seq_count = 0;
1166         queued_seq_buf_end = 0;
1167         queued_seq_tdo_ptr = 0;
1168         pending_scan_result_count = 0;
1169 }
1170
1171 /* queue a sequence of bits to clock out TDI / in TDO, executing if the buffer is full.
1172  *
1173  * sequence=NULL means clock out zeros on TDI
1174  * tdo_buffer=NULL means don't capture TDO
1175  */
1176 static void cmsis_dap_add_jtag_sequence(int s_len, const uint8_t *sequence, int s_offset,
1177                                         bool tms, uint8_t *tdo_buffer, int tdo_buffer_offset)
1178 {
1179         DEBUG_JTAG_IO("[at %d] %d bits, tms %s, seq offset %d, tdo buf %p, tdo offset %d",
1180                 queued_seq_buf_end,
1181                 s_len, tms ? "HIGH" : "LOW", s_offset, tdo_buffer, tdo_buffer_offset);
1182
1183         if (s_len == 0)
1184                 return;
1185
1186         if (s_len > 64) {
1187                 DEBUG_JTAG_IO("START JTAG SEQ SPLIT");
1188                 for (int offset = 0; offset < s_len; offset += 64) {
1189                         int len = s_len - offset;
1190                         if (len > 64)
1191                                 len = 64;
1192                         DEBUG_JTAG_IO("Splitting long jtag sequence: %d-bit chunk starting at offset %d", len, offset);
1193                         cmsis_dap_add_jtag_sequence(
1194                                 len,
1195                                 sequence,
1196                                 s_offset + offset,
1197                                 tms,
1198                                 tdo_buffer,
1199                                 tdo_buffer == NULL ? 0 : (tdo_buffer_offset + offset)
1200                                 );
1201                 }
1202                 DEBUG_JTAG_IO("END JTAG SEQ SPLIT");
1203                 return;
1204         }
1205
1206         int cmd_len = 1 + DIV_ROUND_UP(s_len, 8);
1207         if (queued_seq_count >= 255 || queued_seq_buf_end + cmd_len > QUEUED_SEQ_BUF_LEN)
1208                 /* empty out the buffer */
1209                 cmsis_dap_flush();
1210
1211         ++queued_seq_count;
1212
1213         /* control byte */
1214         queued_seq_buf[queued_seq_buf_end] =
1215                 (tms ? DAP_JTAG_SEQ_TMS : 0) |
1216                 (tdo_buffer != NULL ? DAP_JTAG_SEQ_TDO : 0) |
1217                 (s_len == 64 ? 0 : s_len);
1218
1219         if (sequence != NULL)
1220                 bit_copy(&queued_seq_buf[queued_seq_buf_end + 1], 0, sequence, s_offset, s_len);
1221         else
1222                 memset(&queued_seq_buf[queued_seq_buf_end + 1], 0, DIV_ROUND_UP(s_len, 8));
1223
1224         queued_seq_buf_end += cmd_len;
1225
1226         if (tdo_buffer != NULL) {
1227                 struct pending_scan_result *scan = &pending_scan_results[pending_scan_result_count++];
1228                 scan->first = queued_seq_tdo_ptr;
1229                 queued_seq_tdo_ptr += DIV_ROUND_UP(s_len, 8);
1230                 scan->length = s_len;
1231                 scan->buffer = tdo_buffer;
1232                 scan->buffer_offset = tdo_buffer_offset;
1233         }
1234 }
1235
1236 /* queue a sequence of bits to clock out TMS, executing if the buffer is full */
1237 static void cmsis_dap_add_tms_sequence(const uint8_t *sequence, int s_len)
1238 {
1239         DEBUG_JTAG_IO("%d bits: %02X", s_len, *sequence);
1240         /* we use a series of CMD_DAP_JTAG_SEQ commands to toggle TMS,
1241            because even though it seems ridiculously inefficient, it
1242            allows us to combine TMS and scan sequences into the same
1243            USB packet. */
1244         /* TODO: combine runs of the same tms value */
1245         for (int i = 0; i < s_len; ++i) {
1246                 bool bit = (sequence[i / 8] & (1 << (i % 8))) != 0;
1247                 cmsis_dap_add_jtag_sequence(1, NULL, 0, bit, NULL, 0);
1248         }
1249 }
1250
1251 /* Move to the end state by queuing a sequence to clock into TMS */
1252 static void cmsis_dap_state_move(void)
1253 {
1254         uint8_t tms_scan;
1255         uint8_t tms_scan_bits;
1256
1257         tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
1258         tms_scan_bits = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
1259
1260         DEBUG_JTAG_IO("state move from %s to %s: %d clocks, %02X on tms",
1261                 tap_state_name(tap_get_state()), tap_state_name(tap_get_end_state()),
1262                 tms_scan_bits, tms_scan);
1263         cmsis_dap_add_tms_sequence(&tms_scan, tms_scan_bits);
1264
1265         tap_set_state(tap_get_end_state());
1266 }
1267
1268
1269 /* Execute a JTAG scan operation by queueing TMS and TDI/TDO sequences */
1270 static void cmsis_dap_execute_scan(struct jtag_command *cmd)
1271 {
1272         DEBUG_JTAG_IO("%s type:%d", cmd->cmd.scan->ir_scan ? "IRSCAN" : "DRSCAN",
1273                 jtag_scan_type(cmd->cmd.scan));
1274
1275         /* Make sure there are no trailing fields with num_bits == 0, or the logic below will fail. */
1276         while (cmd->cmd.scan->num_fields > 0
1277                         && cmd->cmd.scan->fields[cmd->cmd.scan->num_fields - 1].num_bits == 0) {
1278                 cmd->cmd.scan->num_fields--;
1279                 LOG_DEBUG("discarding trailing empty field");
1280         }
1281
1282         if (cmd->cmd.scan->num_fields == 0) {
1283                 LOG_DEBUG("empty scan, doing nothing");
1284                 return;
1285         }
1286
1287         if (cmd->cmd.scan->ir_scan) {
1288                 if (tap_get_state() != TAP_IRSHIFT) {
1289                         cmsis_dap_end_state(TAP_IRSHIFT);
1290                         cmsis_dap_state_move();
1291                 }
1292         } else {
1293                 if (tap_get_state() != TAP_DRSHIFT) {
1294                         cmsis_dap_end_state(TAP_DRSHIFT);
1295                         cmsis_dap_state_move();
1296                 }
1297         }
1298
1299         cmsis_dap_end_state(cmd->cmd.scan->end_state);
1300
1301         struct scan_field *field = cmd->cmd.scan->fields;
1302         unsigned scan_size = 0;
1303
1304         for (int i = 0; i < cmd->cmd.scan->num_fields; i++, field++) {
1305                 scan_size += field->num_bits;
1306                 DEBUG_JTAG_IO("%s%s field %d/%d %d bits",
1307                         field->in_value ? "in" : "",
1308                         field->out_value ? "out" : "",
1309                         i,
1310                         cmd->cmd.scan->num_fields,
1311                         field->num_bits);
1312
1313                 if (i == cmd->cmd.scan->num_fields - 1 && tap_get_state() != tap_get_end_state()) {
1314                         DEBUG_JTAG_IO("Last field and have to move out of SHIFT state");
1315                         /* Last field, and we're leaving IRSHIFT/DRSHIFT. Clock last bit during tap
1316                          * movement. This last field can't have length zero, it was checked above. */
1317                         cmsis_dap_add_jtag_sequence(
1318                                 field->num_bits - 1, /* number of bits to clock */
1319                                 field->out_value, /* output sequence */
1320                                 0, /* output offset */
1321                                 false, /* TMS low */
1322                                 field->in_value,
1323                                 0);
1324
1325                         /* Clock the last bit out, with TMS high */
1326                         uint8_t last_bit = 0;
1327                         if (field->out_value)
1328                                 bit_copy(&last_bit, 0, field->out_value, field->num_bits - 1, 1);
1329                         cmsis_dap_add_jtag_sequence(
1330                                 1,
1331                                 &last_bit,
1332                                 0,
1333                                 true,
1334                                 field->in_value,
1335                                 field->num_bits - 1);
1336                         tap_set_state(tap_state_transition(tap_get_state(), 1));
1337
1338                         /* Now clock one more cycle, with TMS low, to get us into a PAUSE state */
1339                         cmsis_dap_add_jtag_sequence(
1340                                 1,
1341                                 &last_bit,
1342                                 0,
1343                                 false,
1344                                 NULL,
1345                                 0);
1346                         tap_set_state(tap_state_transition(tap_get_state(), 0));
1347                 } else {
1348                         DEBUG_JTAG_IO("Internal field, staying in SHIFT state afterwards");
1349                         /* Clocking part of a sequence into DR or IR with TMS=0,
1350                            leaving TMS=0 at the end so we can continue later */
1351                         cmsis_dap_add_jtag_sequence(
1352                                 field->num_bits,
1353                                 field->out_value,
1354                                 0,
1355                                 false,
1356                                 field->in_value,
1357                                 0);
1358                 }
1359         }
1360
1361         if (tap_get_state() != tap_get_end_state()) {
1362                 cmsis_dap_end_state(tap_get_end_state());
1363                 cmsis_dap_state_move();
1364         }
1365
1366         DEBUG_JTAG_IO("%s scan, %i bits, end in %s",
1367                 (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
1368                 tap_state_name(tap_get_end_state()));
1369 }
1370
1371 static void cmsis_dap_pathmove(int num_states, tap_state_t *path)
1372 {
1373         int i;
1374         uint8_t tms0 = 0x00;
1375         uint8_t tms1 = 0xff;
1376
1377         for (i = 0; i < num_states; i++) {
1378                 if (path[i] == tap_state_transition(tap_get_state(), false))
1379                         cmsis_dap_add_tms_sequence(&tms0, 1);
1380                 else if (path[i] == tap_state_transition(tap_get_state(), true))
1381                         cmsis_dap_add_tms_sequence(&tms1, 1);
1382                 else {
1383                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition.",
1384                                   tap_state_name(tap_get_state()), tap_state_name(path[i]));
1385                         exit(-1);
1386                 }
1387
1388                 tap_set_state(path[i]);
1389         }
1390
1391         cmsis_dap_end_state(tap_get_state());
1392 }
1393
1394 static void cmsis_dap_execute_pathmove(struct jtag_command *cmd)
1395 {
1396         DEBUG_JTAG_IO("pathmove: %i states, end in %i",
1397                       cmd->cmd.pathmove->num_states,
1398                cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1399
1400         cmsis_dap_pathmove(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
1401 }
1402
1403 static void cmsis_dap_stableclocks(int num_cycles)
1404 {
1405         int i;
1406
1407         uint8_t tms = tap_get_state() == TAP_RESET;
1408         /* TODO: Perform optimizations? */
1409         /* Execute num_cycles. */
1410         for (i = 0; i < num_cycles; i++)
1411                 cmsis_dap_add_tms_sequence(&tms, 1);
1412 }
1413
1414 static void cmsis_dap_runtest(int num_cycles)
1415 {
1416         tap_state_t saved_end_state = tap_get_end_state();
1417
1418         /* Only do a state_move when we're not already in IDLE. */
1419         if (tap_get_state() != TAP_IDLE) {
1420                 cmsis_dap_end_state(TAP_IDLE);
1421                 cmsis_dap_state_move();
1422         }
1423         cmsis_dap_stableclocks(num_cycles);
1424
1425         /* Finish in end_state. */
1426         cmsis_dap_end_state(saved_end_state);
1427
1428         if (tap_get_state() != tap_get_end_state())
1429                 cmsis_dap_state_move();
1430 }
1431
1432 static void cmsis_dap_execute_runtest(struct jtag_command *cmd)
1433 {
1434         DEBUG_JTAG_IO("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles,
1435                       cmd->cmd.runtest->end_state);
1436
1437         cmsis_dap_end_state(cmd->cmd.runtest->end_state);
1438         cmsis_dap_runtest(cmd->cmd.runtest->num_cycles);
1439 }
1440
1441 static void cmsis_dap_execute_stableclocks(struct jtag_command *cmd)
1442 {
1443         DEBUG_JTAG_IO("stableclocks %i cycles", cmd->cmd.runtest->num_cycles);
1444         cmsis_dap_stableclocks(cmd->cmd.runtest->num_cycles);
1445 }
1446
1447 static void cmsis_dap_execute_tms(struct jtag_command *cmd)
1448 {
1449         DEBUG_JTAG_IO("TMS: %d bits", cmd->cmd.tms->num_bits);
1450         cmsis_dap_cmd_DAP_SWJ_Sequence(cmd->cmd.tms->num_bits, cmd->cmd.tms->bits);
1451 }
1452
1453 /* TODO: Is there need to call cmsis_dap_flush() for the JTAG_PATHMOVE,
1454  * JTAG_RUNTEST, JTAG_STABLECLOCKS? */
1455 static void cmsis_dap_execute_command(struct jtag_command *cmd)
1456 {
1457         switch (cmd->type) {
1458                 case JTAG_RESET:
1459                         cmsis_dap_flush();
1460                         cmsis_dap_execute_reset(cmd);
1461                         break;
1462                 case JTAG_SLEEP:
1463                         cmsis_dap_flush();
1464                         cmsis_dap_execute_sleep(cmd);
1465                         break;
1466                 case JTAG_TLR_RESET:
1467                         cmsis_dap_flush();
1468                         cmsis_dap_execute_tlr_reset(cmd);
1469                         break;
1470                 case JTAG_SCAN:
1471                         cmsis_dap_execute_scan(cmd);
1472                         break;
1473                 case JTAG_PATHMOVE:
1474                         cmsis_dap_execute_pathmove(cmd);
1475                         break;
1476                 case JTAG_RUNTEST:
1477                         cmsis_dap_execute_runtest(cmd);
1478                         break;
1479                 case JTAG_STABLECLOCKS:
1480                         cmsis_dap_execute_stableclocks(cmd);
1481                         break;
1482                 case JTAG_TMS:
1483                         cmsis_dap_execute_tms(cmd);
1484                         break;
1485                 default:
1486                         LOG_ERROR("BUG: unknown JTAG command type 0x%X encountered", cmd->type);
1487                         exit(-1);
1488         }
1489 }
1490
1491 static int cmsis_dap_execute_queue(void)
1492 {
1493         struct jtag_command *cmd = jtag_command_queue;
1494
1495         while (cmd != NULL) {
1496                 cmsis_dap_execute_command(cmd);
1497                 cmd = cmd->next;
1498         }
1499
1500         cmsis_dap_flush();
1501
1502         return ERROR_OK;
1503 }
1504
1505 static int cmsis_dap_speed(int speed)
1506 {
1507         if (speed > DAP_MAX_CLOCK)
1508                 LOG_INFO("High speed (adapter_khz %d) may be limited by adapter firmware.", speed);
1509
1510         if (speed == 0) {
1511                 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
1512                 return ERROR_JTAG_NOT_IMPLEMENTED;
1513         }
1514
1515         return cmsis_dap_cmd_DAP_SWJ_Clock(speed);
1516 }
1517
1518 static int cmsis_dap_speed_div(int speed, int *khz)
1519 {
1520         *khz = speed;
1521         return ERROR_OK;
1522 }
1523
1524 static int cmsis_dap_khz(int khz, int *jtag_speed)
1525 {
1526         *jtag_speed = khz;
1527         return ERROR_OK;
1528 }
1529
1530 static int_least32_t cmsis_dap_swd_frequency(int_least32_t hz)
1531 {
1532         if (hz > 0)
1533                 cmsis_dap_speed(hz / 1000);
1534
1535         return hz;
1536 }
1537
1538
1539 COMMAND_HANDLER(cmsis_dap_handle_info_command)
1540 {
1541         if (cmsis_dap_get_version_info() == ERROR_OK)
1542                 cmsis_dap_get_status();
1543
1544         return ERROR_OK;
1545 }
1546
1547 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
1548 {
1549         if (CMD_ARGC > MAX_USB_IDS * 2) {
1550                 LOG_WARNING("ignoring extra IDs in cmsis_dap_vid_pid "
1551                         "(maximum is %d pairs)", MAX_USB_IDS);
1552                 CMD_ARGC = MAX_USB_IDS * 2;
1553         }
1554         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
1555                 LOG_WARNING("incomplete cmsis_dap_vid_pid configuration directive");
1556                 if (CMD_ARGC < 2)
1557                         return ERROR_COMMAND_SYNTAX_ERROR;
1558                 /* remove the incomplete trailing id */
1559                 CMD_ARGC -= 1;
1560         }
1561
1562         unsigned i;
1563         for (i = 0; i < CMD_ARGC; i += 2) {
1564                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], cmsis_dap_vid[i >> 1]);
1565                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], cmsis_dap_pid[i >> 1]);
1566         }
1567
1568         /*
1569          * Explicitly terminate, in case there are multiples instances of
1570          * cmsis_dap_vid_pid.
1571          */
1572         cmsis_dap_vid[i >> 1] = cmsis_dap_pid[i >> 1] = 0;
1573
1574         return ERROR_OK;
1575 }
1576
1577 COMMAND_HANDLER(cmsis_dap_handle_serial_command)
1578 {
1579         if (CMD_ARGC == 1) {
1580                 size_t len = mbstowcs(NULL, CMD_ARGV[0], 0);
1581                 cmsis_dap_serial = calloc(len + 1, sizeof(wchar_t));
1582                 if (cmsis_dap_serial == NULL) {
1583                         LOG_ERROR("unable to allocate memory");
1584                         return ERROR_OK;
1585                 }
1586                 if (mbstowcs(cmsis_dap_serial, CMD_ARGV[0], len + 1) == (size_t)-1) {
1587                         free(cmsis_dap_serial);
1588                         cmsis_dap_serial = NULL;
1589                         LOG_ERROR("unable to convert serial");
1590                 }
1591         } else {
1592                 LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
1593         }
1594
1595         return ERROR_OK;
1596 }
1597
1598 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
1599         {
1600                 .name = "info",
1601                 .handler = &cmsis_dap_handle_info_command,
1602                 .mode = COMMAND_EXEC,
1603                 .usage = "",
1604                 .help = "show cmsis-dap info",
1605         },
1606         COMMAND_REGISTRATION_DONE
1607 };
1608
1609 static const struct command_registration cmsis_dap_command_handlers[] = {
1610         {
1611                 .name = "cmsis-dap",
1612                 .mode = COMMAND_ANY,
1613                 .help = "perform CMSIS-DAP management",
1614                 .usage = "<cmd>",
1615                 .chain = cmsis_dap_subcommand_handlers,
1616         },
1617         {
1618                 .name = "cmsis_dap_vid_pid",
1619                 .handler = &cmsis_dap_handle_vid_pid_command,
1620                 .mode = COMMAND_CONFIG,
1621                 .help = "the vendor ID and product ID of the CMSIS-DAP device",
1622                 .usage = "(vid pid)* ",
1623         },
1624         {
1625                 .name = "cmsis_dap_serial",
1626                 .handler = &cmsis_dap_handle_serial_command,
1627                 .mode = COMMAND_CONFIG,
1628                 .help = "set the serial number of the adapter",
1629                 .usage = "serial_string",
1630         },
1631         COMMAND_REGISTRATION_DONE
1632 };
1633
1634 static const struct swd_driver cmsis_dap_swd_driver = {
1635         .init = cmsis_dap_swd_init,
1636         .frequency = cmsis_dap_swd_frequency,
1637         .switch_seq = cmsis_dap_swd_switch_seq,
1638         .read_reg = cmsis_dap_swd_read_reg,
1639         .write_reg = cmsis_dap_swd_write_reg,
1640         .run = cmsis_dap_swd_run_queue,
1641 };
1642
1643 static const char * const cmsis_dap_transport[] = { "swd", "jtag", NULL };
1644
1645 struct jtag_interface cmsis_dap_interface = {
1646         .name = "cmsis-dap",
1647         .supported = DEBUG_CAP_TMS_SEQ,
1648         .commands = cmsis_dap_command_handlers,
1649         .swd = &cmsis_dap_swd_driver,
1650         .transports = cmsis_dap_transport,
1651
1652         .execute_queue = cmsis_dap_execute_queue,
1653         .speed = cmsis_dap_speed,
1654         .speed_div = cmsis_dap_speed_div,
1655         .khz = cmsis_dap_khz,
1656         .init = cmsis_dap_init,
1657         .quit = cmsis_dap_quit,
1658 };