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