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