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

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