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

   1 /***************************************************************************
   2  *   Copyright (C) 2021 by Adrian Negreanu                                 *
   3  *   groleo@gmail.com                                                      *
   4  *                                                                         *
   5  *   Copyright (C) 2018 by Mickaël Thomas                                  *
   6  *   mickael9@gmail.com                                                    *
   7  *                                                                         *
   8  *   Copyright (C) 2016 by Maksym Hilliaka                                 *
   9  *   oter@frozen-team.com                                                  *
  10  *                                                                         *
  11  *   Copyright (C) 2016 by Phillip Pearson                                 *
  12  *   pp@myelin.co.nz                                                       *
  13  *                                                                         *
  14  *   Copyright (C) 2014 by Paul Fertser                                    *
  15  *   fercerpav@gmail.com                                                   *
  16  *                                                                         *
  17  *   Copyright (C) 2013 by mike brown                                      *
  18  *   mike@theshedworks.org.uk                                              *
  19  *                                                                         *
  20  *   Copyright (C) 2013 by Spencer Oliver                                  *
  21  *   spen@spen-soft.co.uk                                                  *
  22  *                                                                         *
  23  *   This program is free software; you can redistribute it and/or modify  *
  24  *   it under the terms of the GNU General Public License as published by  *
  25  *   the Free Software Foundation; either version 2 of the License, or     *
  26  *   (at your option) any later version.                                   *
  27  *                                                                         *
  28  *   This program is distributed in the hope that it will be useful,       *
  29  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  30  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  31  *   GNU General Public License for more details.                          *
  32  *                                                                         *
  33  *   You should have received a copy of the GNU General Public License     *
  34  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  35  ***************************************************************************/
  36
  37 #ifdef HAVE_CONFIG_H
  38 #include "config.h"
  39 #endif
  40
  41 #include <transport/transport.h>
  42 #include "helper/replacements.h"
  43 #include <jtag/adapter.h>
  44 #include <jtag/swd.h>
  45 #include <jtag/interface.h>
  46 #include <jtag/commands.h>
  47 #include <jtag/tcl.h>
  48 #include <target/cortex_m.h>
  49
  50 #include "cmsis_dap.h"
  51
  52 static const struct cmsis_dap_backend *const cmsis_dap_backends[] = {
  53 #if BUILD_CMSIS_DAP_USB == 1
  54         &cmsis_dap_usb_backend,
  55 #endif
  56
  57 #if BUILD_CMSIS_DAP_HID == 1
  58         &cmsis_dap_hid_backend,
  59 #endif
  60 };
  61
  62 /* USB Config */
  63
  64 /* Known vid/pid pairs:
  65  * VID 0xc251: Keil Software
  66  * PID 0xf001: LPC-Link-II CMSIS_DAP
  67  * PID 0xf002: OPEN-SDA CMSIS_DAP (Freedom Board)
  68  * PID 0x2722: Keil ULINK2 CMSIS-DAP
  69  * PID 0x2750: Keil ULINKplus CMSIS-DAP
  70  *
  71  * VID 0x0d28: mbed Software
  72  * PID 0x0204: MBED CMSIS-DAP
  73  */
  74
  75 #define MAX_USB_IDS 8
  76 /* vid = pid = 0 marks the end of the list */
  77 static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0 };
  78 static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
  79 static int cmsis_dap_backend = -1;
  80 static bool swd_mode;
  81
  82 #define USB_TIMEOUT       1000
  83
  84 /* CMSIS-DAP General Commands */
  85 #define CMD_DAP_INFO              0x00
  86 #define CMD_DAP_LED               0x01
  87 #define CMD_DAP_CONNECT           0x02
  88 #define CMD_DAP_DISCONNECT        0x03
  89 #define CMD_DAP_WRITE_ABORT       0x08
  90 #define CMD_DAP_DELAY             0x09
  91 #define CMD_DAP_RESET_TARGET      0x0A
  92
  93 /* CMD_INFO */
  94 #define INFO_ID_VENDOR            0x01      /* string */
  95 #define INFO_ID_PRODUCT           0x02      /* string */
  96 #define INFO_ID_SERNUM            0x03      /* string */
  97 #define INFO_ID_FW_VER            0x04      /* string */
  98 #define INFO_ID_TD_VEND           0x05      /* string */
  99 #define INFO_ID_TD_NAME           0x06      /* string */
 100 #define INFO_ID_CAPS              0xf0      /* byte */
 101 #define INFO_ID_PKT_CNT           0xfe      /* byte */
 102 #define INFO_ID_PKT_SZ            0xff      /* short */
 103 #define INFO_ID_SWO_BUF_SZ        0xfd      /* word */
 104
 105 #define INFO_CAPS_SWD                 BIT(0)
 106 #define INFO_CAPS_JTAG                BIT(1)
 107 #define INFO_CAPS_SWO_UART            BIT(2)
 108 #define INFO_CAPS_SWO_MANCHESTER      BIT(3)
 109 #define INFO_CAPS_ATOMIC_CMDS         BIT(4)
 110 #define INFO_CAPS_TEST_DOMAIN_TIMER   BIT(5)
 111 #define INFO_CAPS_SWO_STREAMING_TRACE BIT(6)
 112 #define INFO_CAPS_UART_PORT           BIT(7)
 113 #define INFO_CAPS_USB_COM_PORT        BIT(8)
 114 #define INFO_CAPS__NUM_CAPS               9
 115
 116 /* CMD_LED */
 117 #define LED_ID_CONNECT            0x00
 118 #define LED_ID_RUN                0x01
 119
 120 #define LED_OFF                   0x00
 121 #define LED_ON                    0x01
 122
 123 /* CMD_CONNECT */
 124 #define CONNECT_DEFAULT           0x00
 125 #define CONNECT_SWD               0x01
 126 #define CONNECT_JTAG              0x02
 127
 128 /* CMSIS-DAP Common SWD/JTAG Commands */
 129 #define CMD_DAP_DELAY             0x09
 130 #define CMD_DAP_SWJ_PINS          0x10
 131 #define CMD_DAP_SWJ_CLOCK         0x11
 132 #define CMD_DAP_SWJ_SEQ           0x12
 133
 134 /*
 135  * PINS
 136  * Bit 0: SWCLK/TCK
 137  * Bit 1: SWDIO/TMS
 138  * Bit 2: TDI
 139  * Bit 3: TDO
 140  * Bit 5: nTRST
 141  * Bit 7: nRESET
 142  */
 143
 144 #define SWJ_PIN_TCK               (1<<0)
 145 #define SWJ_PIN_TMS               (1<<1)
 146 #define SWJ_PIN_TDI               (1<<2)
 147 #define SWJ_PIN_TDO               (1<<3)
 148 #define SWJ_PIN_TRST              (1<<5)
 149 #define SWJ_PIN_SRST              (1<<7)
 150
 151 /* CMSIS-DAP SWD Commands */
 152 #define CMD_DAP_SWD_CONFIGURE     0x13
 153 #define CMD_DAP_SWD_SEQUENCE      0x1D
 154
 155 /* CMSIS-DAP JTAG Commands */
 156 #define CMD_DAP_JTAG_SEQ          0x14
 157 #define CMD_DAP_JTAG_CONFIGURE    0x15
 158 #define CMD_DAP_JTAG_IDCODE       0x16
 159
 160 /* CMSIS-DAP JTAG sequence info masks */
 161 /* Number of bits to clock through (0 means 64) */
 162 #define DAP_JTAG_SEQ_TCK          0x3F
 163 /* TMS will be set during the sequence if this bit is set */
 164 #define DAP_JTAG_SEQ_TMS          0x40
 165 /* TDO output will be captured if this bit is set */
 166 #define DAP_JTAG_SEQ_TDO          0x80
 167
 168
 169 /* CMSIS-DAP Transfer Commands */
 170 #define CMD_DAP_TFER_CONFIGURE    0x04
 171 #define CMD_DAP_TFER              0x05
 172 #define CMD_DAP_TFER_BLOCK        0x06
 173 #define CMD_DAP_TFER_ABORT        0x07
 174
 175 /* DAP Status Code */
 176 #define DAP_OK                    0
 177 #define DAP_ERROR                 0xFF
 178
 179 /* CMSIS-DAP SWO Commands */
 180 #define CMD_DAP_SWO_TRANSPORT     0x17
 181 #define CMD_DAP_SWO_MODE          0x18
 182 #define CMD_DAP_SWO_BAUDRATE      0x19
 183 #define CMD_DAP_SWO_CONTROL       0x1A
 184 #define CMD_DAP_SWO_STATUS        0x1B
 185 #define CMD_DAP_SWO_DATA          0x1C
 186 #define CMD_DAP_SWO_EX_STATUS     0x1E
 187
 188 /* SWO transport mode for reading trace data */
 189 #define DAP_SWO_TRANSPORT_NONE    0
 190 #define DAP_SWO_TRANSPORT_DATA    1
 191 #define DAP_SWO_TRANSPORT_WINUSB  2
 192
 193 /* SWO trace capture mode */
 194 #define DAP_SWO_MODE_OFF          0
 195 #define DAP_SWO_MODE_UART         1
 196 #define DAP_SWO_MODE_MANCHESTER   2
 197
 198 /* SWO trace data capture */
 199 #define DAP_SWO_CONTROL_STOP      0
 200 #define DAP_SWO_CONTROL_START     1
 201
 202 /* SWO trace status */
 203 #define DAP_SWO_STATUS_CAPTURE_INACTIVE      0
 204 #define DAP_SWO_STATUS_CAPTURE_ACTIVE        1
 205 #define DAP_SWO_STATUS_CAPTURE_MASK          BIT(0)
 206 #define DAP_SWO_STATUS_STREAM_ERROR_MASK     BIT(6)
 207 #define DAP_SWO_STATUS_BUFFER_OVERRUN_MASK   BIT(7)
 208
 209 /* CMSIS-DAP Vendor Commands
 210  * None as yet... */
 211
 212 static const char * const info_caps_str[INFO_CAPS__NUM_CAPS] = {
 213         "SWD  supported",
 214         "JTAG supported",
 215         "SWO-UART supported",
 216         "SWO-MANCHESTER supported",
 217         "Atomic commands supported",
 218         "Test domain timer supported",
 219         "SWO streaming trace supported",
 220         "UART communication port supported",
 221         "UART via USB COM port supported",
 222 };
 223
 224 struct pending_transfer_result {
 225         uint8_t cmd;
 226         uint32_t data;
 227         void *buffer;
 228 };
 229
 230 struct pending_request_block {
 231         struct pending_transfer_result *transfers;
 232         int transfer_count;
 233 };
 234
 235 struct pending_scan_result {
 236         /** Offset in bytes in the CMD_DAP_JTAG_SEQ response buffer. */
 237         unsigned first;
 238         /** Number of bits to read. */
 239         unsigned length;
 240         /** Location to store the result */
 241         uint8_t *buffer;
 242         /** Offset in the destination buffer */
 243         unsigned buffer_offset;
 244 };
 245
 246 /* Up to MIN(packet_count, MAX_PENDING_REQUESTS) requests may be issued
 247  * until the first response arrives */
 248 #define MAX_PENDING_REQUESTS 3
 249
 250 /* Pending requests are organized as a FIFO - circular buffer */
 251 /* Each block in FIFO can contain up to pending_queue_len transfers */
 252 static int pending_queue_len;
 253 static struct pending_request_block pending_fifo[MAX_PENDING_REQUESTS];
 254 static int pending_fifo_put_idx, pending_fifo_get_idx;
 255 static int pending_fifo_block_count;
 256
 257 /* pointers to buffers that will receive jtag scan results on the next flush */
 258 #define MAX_PENDING_SCAN_RESULTS 256
 259 static int pending_scan_result_count;
 260 static struct pending_scan_result pending_scan_results[MAX_PENDING_SCAN_RESULTS];
 261
 262 /* queued JTAG sequences that will be executed on the next flush */
 263 #define QUEUED_SEQ_BUF_LEN (cmsis_dap_handle->packet_size - 3)
 264 static int queued_seq_count;
 265 static int queued_seq_buf_end;
 266 static int queued_seq_tdo_ptr;
 267 static uint8_t queued_seq_buf[1024]; /* TODO: make dynamic / move into cmsis object */
 268
 269 static int queued_retval;
 270
 271 static uint8_t output_pins = SWJ_PIN_SRST | SWJ_PIN_TRST;
 272
 273 static struct cmsis_dap *cmsis_dap_handle;
 274
 275
 276 static int cmsis_dap_quit(void);
 277
 278 static int cmsis_dap_open(void)
 279 {
 280         const struct cmsis_dap_backend *backend = NULL;
 281
 282         struct cmsis_dap *dap = calloc(1, sizeof(struct cmsis_dap));
 283         if (!dap) {
 284                 LOG_ERROR("unable to allocate memory");
 285                 return ERROR_FAIL;
 286         }
 287
 288         if (cmsis_dap_backend >= 0) {
 289                 /* Use forced backend */
 290                 backend = cmsis_dap_backends[cmsis_dap_backend];
 291                 if (backend->open(dap, cmsis_dap_vid, cmsis_dap_pid, adapter_get_required_serial()) != ERROR_OK)
 292                         backend = NULL;
 293         } else {
 294                 /* Try all backends */
 295                 for (unsigned int i = 0; i < ARRAY_SIZE(cmsis_dap_backends); i++) {
 296                         backend = cmsis_dap_backends[i];
 297                         if (backend->open(dap, cmsis_dap_vid, cmsis_dap_pid, adapter_get_required_serial()) == ERROR_OK)
 298                                 break;
 299                         else
 300                                 backend = NULL;
 301                 }
 302         }
 303
 304         if (!backend) {
 305                 LOG_ERROR("unable to find a matching CMSIS-DAP device");
 306                 free(dap);
 307                 return ERROR_FAIL;
 308         }
 309
 310         dap->backend = backend;
 311
 312         cmsis_dap_handle = dap;
 313
 314         return ERROR_OK;
 315 }
 316
 317 static void cmsis_dap_close(struct cmsis_dap *dap)
 318 {
 319         if (dap->backend) {
 320                 dap->backend->close(dap);
 321                 dap->backend = NULL;
 322         }
 323
 324         free(cmsis_dap_handle->packet_buffer);
 325         free(cmsis_dap_handle);
 326         cmsis_dap_handle = NULL;
 327
 328         for (int i = 0; i < MAX_PENDING_REQUESTS; i++) {
 329                 free(pending_fifo[i].transfers);
 330                 pending_fifo[i].transfers = NULL;
 331         }
 332 }
 333
 334 static void cmsis_dap_flush_read(struct cmsis_dap *dap)
 335 {
 336         unsigned int i;
 337         /* Some CMSIS-DAP adapters keep buffered packets over
 338          * USB close/open so we need to flush up to 64 old packets
 339          * to be sure all buffers are empty */
 340         for (i = 0; i < 64; i++) {
 341                 int retval = dap->backend->read(dap, 10);
 342                 if (retval == ERROR_TIMEOUT_REACHED)
 343                         break;
 344         }
 345         if (i)
 346                 LOG_DEBUG("Flushed %u packets", i);
 347 }
 348
 349 /* Send a message and receive the reply */
 350 static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
 351 {
 352         if (pending_fifo_block_count) {
 353                 LOG_ERROR("pending %d blocks, flushing", pending_fifo_block_count);
 354                 while (pending_fifo_block_count) {
 355                         dap->backend->read(dap, 10);
 356                         pending_fifo_block_count--;
 357                 }
 358                 pending_fifo_put_idx = 0;
 359                 pending_fifo_get_idx = 0;
 360         }
 361
 362         uint8_t current_cmd = cmsis_dap_handle->command[0];
 363         int retval = dap->backend->write(dap, txlen, USB_TIMEOUT);
 364         if (retval < 0)
 365                 return retval;
 366
 367         /* get reply */
 368         retval = dap->backend->read(dap, USB_TIMEOUT);
 369         if (retval < 0)
 370                 return retval;
 371
 372         uint8_t *resp = cmsis_dap_handle->response;
 373         if (resp[0] == DAP_ERROR) {
 374                 LOG_ERROR("CMSIS-DAP command 0x%" PRIx8 " not implemented", current_cmd);
 375                 return ERROR_NOT_IMPLEMENTED;
 376         }
 377
 378         if (resp[0] != current_cmd) {
 379                 LOG_ERROR("CMSIS-DAP command mismatch. Sent 0x%" PRIx8
 380                          " received 0x%" PRIx8, current_cmd, resp[0]);
 381
 382                 cmsis_dap_flush_read(dap);
 383                 return ERROR_FAIL;
 384         }
 385
 386         return ERROR_OK;
 387 }
 388
 389 static int cmsis_dap_cmd_dap_swj_pins(uint8_t pins, uint8_t mask, uint32_t delay, uint8_t *input)
 390 {
 391         uint8_t *command = cmsis_dap_handle->command;
 392
 393         command[0] = CMD_DAP_SWJ_PINS;
 394         command[1] = pins;
 395         command[2] = mask;
 396         h_u32_to_le(&command[3], delay);
 397
 398         int retval = cmsis_dap_xfer(cmsis_dap_handle, 7);
 399         if (retval != ERROR_OK) {
 400                 LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_PINS failed.");
 401                 return ERROR_JTAG_DEVICE_ERROR;
 402         }
 403
 404         if (input)
 405                 *input = cmsis_dap_handle->response[1];
 406
 407         return ERROR_OK;
 408 }
 409
 410 static int cmsis_dap_cmd_dap_swj_clock(uint32_t swj_clock)
 411 {
 412         uint8_t *command = cmsis_dap_handle->command;
 413
 414         /* set clock in Hz */
 415         swj_clock *= 1000;
 416
 417         command[0] = CMD_DAP_SWJ_CLOCK;
 418         h_u32_to_le(&command[1], swj_clock);
 419
 420         int retval = cmsis_dap_xfer(cmsis_dap_handle, 5);
 421         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 422                 LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_CLOCK failed.");
 423                 return ERROR_JTAG_DEVICE_ERROR;
 424         }
 425
 426         return ERROR_OK;
 427 }
 428
 429 /* clock a sequence of bits out on TMS, to change JTAG states */
 430 static int cmsis_dap_cmd_dap_swj_sequence(uint8_t s_len, const uint8_t *sequence)
 431 {
 432         uint8_t *command = cmsis_dap_handle->command;
 433
 434 #ifdef CMSIS_DAP_JTAG_DEBUG
 435         LOG_DEBUG("cmsis-dap TMS sequence: len=%d", s_len);
 436         for (int i = 0; i < DIV_ROUND_UP(s_len, 8); ++i)
 437                 printf("%02X ", sequence[i]);
 438
 439         printf("\n");
 440 #endif
 441
 442         command[0] = CMD_DAP_SWJ_SEQ;
 443         command[1] = s_len;
 444         bit_copy(&command[2], 0, sequence, 0, s_len);
 445
 446         int retval = cmsis_dap_xfer(cmsis_dap_handle, 2 + DIV_ROUND_UP(s_len, 8));
 447         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK)
 448                 return ERROR_FAIL;
 449
 450         return ERROR_OK;
 451 }
 452
 453 static int cmsis_dap_cmd_dap_info(uint8_t info, uint8_t **data)
 454 {
 455         uint8_t *command = cmsis_dap_handle->command;
 456
 457         command[0] = CMD_DAP_INFO;
 458         command[1] = info;
 459
 460         int retval = cmsis_dap_xfer(cmsis_dap_handle, 2);
 461         if (retval != ERROR_OK) {
 462                 LOG_ERROR("CMSIS-DAP command CMD_INFO failed.");
 463                 return ERROR_JTAG_DEVICE_ERROR;
 464         }
 465
 466         *data = &cmsis_dap_handle->response[1];
 467
 468         return ERROR_OK;
 469 }
 470
 471 static int cmsis_dap_cmd_dap_led(uint8_t led, uint8_t state)
 472 {
 473         uint8_t *command = cmsis_dap_handle->command;
 474
 475         command[0] = CMD_DAP_LED;
 476         command[1] = led;
 477         command[2] = state;
 478
 479         int retval = cmsis_dap_xfer(cmsis_dap_handle, 3);
 480         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 481                 LOG_ERROR("CMSIS-DAP command CMD_LED failed.");
 482                 return ERROR_JTAG_DEVICE_ERROR;
 483         }
 484
 485         return ERROR_OK;
 486 }
 487
 488 static int cmsis_dap_cmd_dap_connect(uint8_t mode)
 489 {
 490         uint8_t *command = cmsis_dap_handle->command;
 491
 492         command[0] = CMD_DAP_CONNECT;
 493         command[1] = mode;
 494
 495         int retval = cmsis_dap_xfer(cmsis_dap_handle, 2);
 496         if (retval != ERROR_OK) {
 497                 LOG_ERROR("CMSIS-DAP command CMD_CONNECT failed.");
 498                 return ERROR_JTAG_DEVICE_ERROR;
 499         }
 500
 501         if (cmsis_dap_handle->response[1] != mode) {
 502                 LOG_ERROR("CMSIS-DAP failed to connect in mode (%d)", mode);
 503                 return ERROR_JTAG_DEVICE_ERROR;
 504         }
 505
 506         return ERROR_OK;
 507 }
 508
 509 static int cmsis_dap_cmd_dap_disconnect(void)
 510 {
 511         uint8_t *command = cmsis_dap_handle->command;
 512
 513         command[0] = CMD_DAP_DISCONNECT;
 514
 515         int retval = cmsis_dap_xfer(cmsis_dap_handle, 1);
 516         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 517                 LOG_ERROR("CMSIS-DAP command CMD_DISCONNECT failed.");
 518                 return ERROR_JTAG_DEVICE_ERROR;
 519         }
 520
 521         return ERROR_OK;
 522 }
 523
 524 static int cmsis_dap_cmd_dap_tfer_configure(uint8_t idle, uint16_t retry_count, uint16_t match_retry)
 525 {
 526         uint8_t *command = cmsis_dap_handle->command;
 527
 528         command[0] = CMD_DAP_TFER_CONFIGURE;
 529         command[1] = idle;
 530         h_u16_to_le(&command[2], retry_count);
 531         h_u16_to_le(&command[4], match_retry);
 532
 533         int retval = cmsis_dap_xfer(cmsis_dap_handle, 6);
 534         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 535                 LOG_ERROR("CMSIS-DAP command CMD_TFER_Configure failed.");
 536                 return ERROR_JTAG_DEVICE_ERROR;
 537         }
 538
 539         return ERROR_OK;
 540 }
 541
 542 static int cmsis_dap_cmd_dap_swd_configure(uint8_t cfg)
 543 {
 544         uint8_t *command = cmsis_dap_handle->command;
 545
 546         command[0] = CMD_DAP_SWD_CONFIGURE;
 547         command[1] = cfg;
 548
 549         int retval = cmsis_dap_xfer(cmsis_dap_handle, 2);
 550         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 551                 LOG_ERROR("CMSIS-DAP command CMD_SWD_Configure failed.");
 552                 return ERROR_JTAG_DEVICE_ERROR;
 553         }
 554
 555         return ERROR_OK;
 556 }
 557
 558 #if 0
 559 static int cmsis_dap_cmd_dap_delay(uint16_t delay_us)
 560 {
 561         uint8_t *command = cmsis_dap_handle->command;
 562
 563         command[0] = CMD_DAP_DELAY;
 564         h_u16_to_le(&command[1], delay_us);
 565
 566         int retval = cmsis_dap_xfer(cmsis_dap_handle, 3);
 567         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 568                 LOG_ERROR("CMSIS-DAP command CMD_Delay failed.");
 569                 return ERROR_JTAG_DEVICE_ERROR;
 570         }
 571
 572         return ERROR_OK;
 573 }
 574 #endif
 575
 576 static int cmsis_dap_metacmd_targetsel(uint32_t instance_id)
 577 {
 578         uint8_t *command = cmsis_dap_handle->command;
 579         const uint32_t SEQ_RD = 0x80, SEQ_WR = 0x00;
 580
 581         /* SWD multi-drop requires a transfer ala CMD_DAP_TFER,
 582         but with no expectation of an SWD ACK response.  In
 583         CMSIS-DAP v1.20 and v2.00, CMD_DAP_SWD_SEQUENCE was
 584         added to allow this special sequence to be generated.
 585         The purpose of this operation is to select the target
 586         corresponding to the instance_id that is written */
 587
 588         size_t idx = 0;
 589         command[idx++] = CMD_DAP_SWD_SEQUENCE;
 590         command[idx++] = 3;     /* sequence count */
 591
 592         /* sequence 0: packet request for TARGETSEL */
 593         command[idx++] = SEQ_WR | 8;
 594         command[idx++] = SWD_CMD_START | swd_cmd(false, false, DP_TARGETSEL) | SWD_CMD_STOP | SWD_CMD_PARK;
 595
 596         /* sequence 1: read Trn ACK Trn, no expectation for target to ACK  */
 597         command[idx++] = SEQ_RD | 5;
 598
 599         /* sequence 2: WDATA plus parity */
 600         command[idx++] = SEQ_WR | (32 + 1);
 601         h_u32_to_le(command + idx, instance_id);
 602         idx += 4;
 603         command[idx++] = parity_u32(instance_id);
 604
 605         int retval = cmsis_dap_xfer(cmsis_dap_handle, idx);
 606         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 607                 LOG_ERROR("CMSIS-DAP command SWD_Sequence failed.");
 608                 return ERROR_JTAG_DEVICE_ERROR;
 609         }
 610
 611         return ERROR_OK;
 612 }
 613
 614 /**
 615  * Sets the SWO transport mode.
 616  * @param[in] transport     The transport mode. Can be None, SWO_Data or
 617  *                          WinUSB (requires CMSIS-DAP v2).
 618  */
 619 static int cmsis_dap_cmd_dap_swo_transport(uint8_t transport)
 620 {
 621         uint8_t *command = cmsis_dap_handle->command;
 622
 623         command[0] = CMD_DAP_SWO_TRANSPORT;
 624         command[1] = transport;
 625
 626         int retval = cmsis_dap_xfer(cmsis_dap_handle, 2);
 627         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 628                 LOG_ERROR("CMSIS-DAP: command CMD_SWO_Transport(%d) failed.", transport);
 629                 return ERROR_JTAG_DEVICE_ERROR;
 630         }
 631
 632         return ERROR_OK;
 633 }
 634
 635 /**
 636  * Sets the SWO trace capture mode.
 637  * @param[in] mode          Trace capture mode. Can be UART or MANCHESTER.
 638  */
 639 static int cmsis_dap_cmd_dap_swo_mode(uint8_t mode)
 640 {
 641         uint8_t *command = cmsis_dap_handle->command;
 642
 643         command[0] = CMD_DAP_SWO_MODE;
 644         command[1] = mode;
 645
 646         int retval = cmsis_dap_xfer(cmsis_dap_handle, 2);
 647         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 648                 LOG_ERROR("CMSIS-DAP: command CMD_SWO_Mode(%d) failed.", mode);
 649                 return ERROR_JTAG_DEVICE_ERROR;
 650         }
 651
 652         return ERROR_OK;
 653 }
 654
 655 /**
 656  * Sets the baudrate for capturing SWO trace data.
 657  * Can be called iteratively to determine supported baudrates.
 658  * @param[in]  in_baudrate  Requested baudrate.
 659  * @param[out] dev_baudrate Actual baudrate or 0 (baudrate not configured).
 660  *                          When requested baudrate is not achievable the
 661  *                          closest configured baudrate can be returned or
 662  *                          0 which indicates that baudrate was not configured.
 663  */
 664 static int cmsis_dap_cmd_dap_swo_baudrate(
 665                                         uint32_t in_baudrate,
 666                                         uint32_t *dev_baudrate)
 667 {
 668         uint8_t *command = cmsis_dap_handle->command;
 669
 670         command[0] = CMD_DAP_SWO_BAUDRATE;
 671         h_u32_to_le(&command[1], in_baudrate);
 672
 673         int retval = cmsis_dap_xfer(cmsis_dap_handle, 4);
 674         uint32_t rvbr = le_to_h_u32(&cmsis_dap_handle->response[1]);
 675         if (retval != ERROR_OK || rvbr == 0) {
 676                 LOG_ERROR("CMSIS-DAP: command CMD_SWO_Baudrate(%u) -> %u failed.", in_baudrate, rvbr);
 677                 if (dev_baudrate)
 678                         *dev_baudrate = 0;
 679                 return ERROR_JTAG_DEVICE_ERROR;
 680         }
 681
 682         if (dev_baudrate)
 683                 *dev_baudrate = rvbr;
 684
 685         return ERROR_OK;
 686 }
 687
 688 /**
 689  * Controls the SWO trace data capture.
 690  * @param[in] control       Start or stop a trace. Starting capture automatically
 691  *                          flushes any existing trace data in buffers which has
 692  *                          not yet been read.
 693  */
 694 static int cmsis_dap_cmd_dap_swo_control(uint8_t control)
 695 {
 696         uint8_t *command = cmsis_dap_handle->command;
 697
 698         command[0] = CMD_DAP_SWO_CONTROL;
 699         command[1] = control;
 700
 701         int retval = cmsis_dap_xfer(cmsis_dap_handle, 2);
 702         if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
 703                 LOG_ERROR("CMSIS-DAP: command CMD_SWO_Control(%d) failed.", control);
 704                 return ERROR_JTAG_DEVICE_ERROR;
 705         }
 706
 707         return ERROR_OK;
 708 }
 709
 710 /**
 711  * Reads the SWO trace status.
 712  * @param[out] trace_status The trace's status.
 713  *                          Bit0: Trace Capture (1 - active, 0 - inactive).
 714  *                          Bit6: Trace Stream Error.
 715  *                          Bit7: Trace Buffer Overrun.
 716  * @param[out] trace_count  Number of bytes in Trace Buffer (not yet read).
 717  */
 718 static int cmsis_dap_cmd_dap_swo_status(
 719                                         uint8_t *trace_status,
 720                                         size_t *trace_count)
 721 {
 722         uint8_t *command = cmsis_dap_handle->command;
 723
 724         command[0] = CMD_DAP_SWO_STATUS;
 725
 726         int retval = cmsis_dap_xfer(cmsis_dap_handle, 1);
 727         if (retval != ERROR_OK) {
 728                 LOG_ERROR("CMSIS-DAP: command CMD_SWO_Status failed.");
 729                 return ERROR_JTAG_DEVICE_ERROR;
 730         }
 731
 732         if (trace_status)
 733                 *trace_status = cmsis_dap_handle->response[1];
 734         if (trace_count)
 735                 *trace_count = le_to_h_u32(&cmsis_dap_handle->response[2]);
 736
 737         return ERROR_OK;
 738 }
 739
 740 /**
 741  * Reads the captured SWO trace data from Trace Buffer.
 742  * @param[in]  max_trace_count Maximum number of Trace Data bytes to read.
 743  * @param[out] trace_status The trace's status.
 744  * @param[out] trace_count  Number of Trace Data bytes read.
 745  * @param[out] data         Trace Data bytes read.
 746  */
 747 static int cmsis_dap_cmd_dap_swo_data(
 748                                         size_t max_trace_count,
 749                                         uint8_t *trace_status,
 750                                         size_t *trace_count,
 751                                         uint8_t *data)
 752 {
 753         uint8_t *command = cmsis_dap_handle->command;
 754
 755         command[0] = CMD_DAP_SWO_DATA;
 756         h_u16_to_le(&command[1], max_trace_count);
 757
 758         int retval = cmsis_dap_xfer(cmsis_dap_handle, 3);
 759         if (retval != ERROR_OK) {
 760                 LOG_ERROR("CMSIS-DAP: command CMD_SWO_Data failed.");
 761                 return ERROR_JTAG_DEVICE_ERROR;
 762         }
 763
 764         *trace_status = cmsis_dap_handle->response[1];
 765         *trace_count = le_to_h_u16(&cmsis_dap_handle->response[2]);
 766
 767         if (*trace_count > 0)
 768                 memcpy(data, &cmsis_dap_handle->response[4], *trace_count);
 769
 770         return ERROR_OK;
 771 }
 772
 773 static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 774 {
 775         uint8_t *command = cmsis_dap_handle->command;
 776         struct pending_request_block *block = &pending_fifo[pending_fifo_put_idx];
 777
 778         LOG_DEBUG_IO("Executing %d queued transactions from FIFO index %d", block->transfer_count, pending_fifo_put_idx);
 779
 780         if (queued_retval != ERROR_OK) {
 781                 LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
 782                 goto skip;
 783         }
 784
 785         if (block->transfer_count == 0)
 786                 goto skip;
 787
 788         command[0] = CMD_DAP_TFER;
 789         command[1] = 0x00;      /* DAP Index */
 790         command[2] = block->transfer_count;
 791         size_t idx = 3;
 792
 793         for (int i = 0; i < block->transfer_count; i++) {
 794                 struct pending_transfer_result *transfer = &(block->transfers[i]);
 795                 uint8_t cmd = transfer->cmd;
 796                 uint32_t data = transfer->data;
 797
 798                 LOG_DEBUG_IO("%s %s reg %x %"PRIx32,
 799                                 cmd & SWD_CMD_APNDP ? "AP" : "DP",
 800                                 cmd & SWD_CMD_RNW ? "read" : "write",
 801                           (cmd & SWD_CMD_A32) >> 1, data);
 802
 803                 /* When proper WAIT handling is implemented in the
 804                  * common SWD framework, this kludge can be
 805                  * removed. However, this might lead to minor
 806                  * performance degradation as the adapter wouldn't be
 807                  * able to automatically retry anything (because ARM
 808                  * has forgotten to implement sticky error flags
 809                  * clearing). See also comments regarding
 810                  * cmsis_dap_cmd_dap_tfer_configure() and
 811                  * cmsis_dap_cmd_dap_swd_configure() in
 812                  * cmsis_dap_init().
 813                  */
 814                 if (!(cmd & SWD_CMD_RNW) &&
 815                     !(cmd & SWD_CMD_APNDP) &&
 816                     (cmd & SWD_CMD_A32) >> 1 == DP_CTRL_STAT &&
 817                     (data & CORUNDETECT)) {
 818                         LOG_DEBUG("refusing to enable sticky overrun detection");
 819                         data &= ~CORUNDETECT;
 820                 }
 821
 822                 command[idx++] = (cmd >> 1) & 0x0f;
 823                 if (!(cmd & SWD_CMD_RNW)) {
 824                         h_u32_to_le(&command[idx], data);
 825                         idx += 4;
 826                 }
 827         }
 828
 829         int retval = dap->backend->write(dap, idx, USB_TIMEOUT);
 830         if (retval < 0) {
 831                 queued_retval = retval;
 832                 goto skip;
 833         } else {
 834                 queued_retval = ERROR_OK;
 835         }
 836
 837         pending_fifo_put_idx = (pending_fifo_put_idx + 1) % dap->packet_count;
 838         pending_fifo_block_count++;
 839         if (pending_fifo_block_count > dap->packet_count)
 840                 LOG_ERROR("too much pending writes %d", pending_fifo_block_count);
 841
 842         return;
 843
 844 skip:
 845         block->transfer_count = 0;
 846 }
 847
 848 static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
 849 {
 850         struct pending_request_block *block = &pending_fifo[pending_fifo_get_idx];
 851
 852         if (pending_fifo_block_count == 0)
 853                 LOG_ERROR("no pending write");
 854
 855         /* get reply */
 856         int retval = dap->backend->read(dap, timeout_ms);
 857         if (retval == ERROR_TIMEOUT_REACHED && timeout_ms < USB_TIMEOUT)
 858                 return;
 859
 860         if (retval <= 0) {
 861                 LOG_DEBUG("error reading data");
 862                 queued_retval = ERROR_FAIL;
 863                 goto skip;
 864         }
 865
 866         uint8_t *resp = dap->response;
 867         if (resp[0] != CMD_DAP_TFER) {
 868                 LOG_ERROR("CMSIS-DAP command mismatch. Expected 0x%x received 0x%" PRIx8,
 869                         CMD_DAP_TFER, resp[0]);
 870                 queued_retval = ERROR_FAIL;
 871                 goto skip;
 872         }
 873
 874         uint8_t transfer_count = resp[1];
 875         uint8_t ack = resp[2] & 0x07;
 876         if (resp[2] & 0x08) {
 877                 LOG_DEBUG("CMSIS-DAP Protocol Error @ %d (wrong parity)", transfer_count);
 878                 queued_retval = ERROR_FAIL;
 879                 goto skip;
 880         }
 881         if (ack != SWD_ACK_OK) {
 882                 LOG_DEBUG("SWD ack not OK @ %d %s", transfer_count,
 883                           ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
 884                 queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
 885                 /* TODO: use results of transfers completed before the error occurred? */
 886                 goto skip;
 887         }
 888
 889         if (block->transfer_count != transfer_count)
 890                 LOG_ERROR("CMSIS-DAP transfer count mismatch: expected %d, got %d",
 891                           block->transfer_count, transfer_count);
 892
 893         LOG_DEBUG_IO("Received results of %d queued transactions FIFO index %d",
 894                  transfer_count, pending_fifo_get_idx);
 895         size_t idx = 3;
 896         for (int i = 0; i < transfer_count; i++) {
 897                 struct pending_transfer_result *transfer = &(block->transfers[i]);
 898                 if (transfer->cmd & SWD_CMD_RNW) {
 899                         static uint32_t last_read;
 900                         uint32_t data = le_to_h_u32(&resp[idx]);
 901                         uint32_t tmp = data;
 902                         idx += 4;
 903
 904                         LOG_DEBUG_IO("Read result: %"PRIx32, data);
 905
 906                         /* Imitate posted AP reads */
 907                         if ((transfer->cmd & SWD_CMD_APNDP) ||
 908                             ((transfer->cmd & SWD_CMD_A32) >> 1 == DP_RDBUFF)) {
 909                                 tmp = last_read;
 910                                 last_read = data;
 911                         }
 912
 913                         if (transfer->buffer)
 914                                 *(uint32_t *)(transfer->buffer) = tmp;
 915                 }
 916         }
 917
 918 skip:
 919         block->transfer_count = 0;
 920         pending_fifo_get_idx = (pending_fifo_get_idx + 1) % dap->packet_count;
 921         pending_fifo_block_count--;
 922 }
 923
 924 static int cmsis_dap_swd_run_queue(void)
 925 {
 926         if (pending_fifo_block_count)
 927                 cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
 928
 929         cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
 930
 931         while (pending_fifo_block_count)
 932                 cmsis_dap_swd_read_process(cmsis_dap_handle, USB_TIMEOUT);
 933
 934         pending_fifo_put_idx = 0;
 935         pending_fifo_get_idx = 0;
 936
 937         int retval = queued_retval;
 938         queued_retval = ERROR_OK;
 939
 940         return retval;
 941 }
 942
 943 static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 944 {
 945         bool targetsel_cmd = swd_cmd(false, false, DP_TARGETSEL) == cmd;
 946
 947         if (pending_fifo[pending_fifo_put_idx].transfer_count == pending_queue_len
 948                          || targetsel_cmd) {
 949                 if (pending_fifo_block_count)
 950                         cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
 951
 952                 /* Not enough room in the queue. Run the queue. */
 953                 cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
 954
 955                 if (pending_fifo_block_count >= cmsis_dap_handle->packet_count)
 956                         cmsis_dap_swd_read_process(cmsis_dap_handle, USB_TIMEOUT);
 957         }
 958
 959         if (queued_retval != ERROR_OK)
 960                 return;
 961
 962         if (targetsel_cmd) {
 963                 cmsis_dap_metacmd_targetsel(data);
 964                 return;
 965         }
 966
 967         struct pending_request_block *block = &pending_fifo[pending_fifo_put_idx];
 968         struct pending_transfer_result *transfer = &(block->transfers[block->transfer_count]);
 969         transfer->data = data;
 970         transfer->cmd = cmd;
 971         if (cmd & SWD_CMD_RNW) {
 972                 /* Queue a read transaction */
 973                 transfer->buffer = dst;
 974         }
 975         block->transfer_count++;
 976 }
 977
 978 static void cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
 979 {
 980         assert(!(cmd & SWD_CMD_RNW));
 981         cmsis_dap_swd_queue_cmd(cmd, NULL, value);
 982 }
 983
 984 static void cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
 985 {
 986         assert(cmd & SWD_CMD_RNW);
 987         cmsis_dap_swd_queue_cmd(cmd, value, 0);
 988 }
 989
 990 static int cmsis_dap_get_serial_info(void)
 991 {
 992         uint8_t *data;
 993
 994         int retval = cmsis_dap_cmd_dap_info(INFO_ID_SERNUM, &data);
 995         if (retval != ERROR_OK)
 996                 return retval;
 997
 998         if (data[0]) /* strlen */
 999                 LOG_INFO("CMSIS-DAP: Serial# = %s", &data[1]);
1000
1001         return ERROR_OK;
1002 }
1003
1004 static int cmsis_dap_get_version_info(void)
1005 {
1006         uint8_t *data;
1007
1008         /* INFO_ID_FW_VER - string */
1009         int retval = cmsis_dap_cmd_dap_info(INFO_ID_FW_VER, &data);
1010         if (retval != ERROR_OK)
1011                 return retval;
1012
1013         if (data[0]) /* strlen */
1014                 LOG_INFO("CMSIS-DAP: FW Version = %s", &data[1]);
1015
1016         return ERROR_OK;
1017 }
1018
1019 static int cmsis_dap_get_caps_info(void)
1020 {
1021         uint8_t *data;
1022
1023         /* INFO_ID_CAPS - byte */
1024         int retval = cmsis_dap_cmd_dap_info(INFO_ID_CAPS, &data);
1025         if (retval != ERROR_OK)
1026                 return retval;
1027
1028         if (data[0] == 1 || data[0] == 2) {
1029                 uint16_t caps = data[1];
1030                 if (data[0] == 2)
1031                         caps |= (uint16_t)data[2] << 8;
1032
1033                 cmsis_dap_handle->caps = caps;
1034
1035                 for (int i = 0; i < INFO_CAPS__NUM_CAPS; ++i) {
1036                         if (caps & BIT(i))
1037                                 LOG_INFO("CMSIS-DAP: %s", info_caps_str[i]);
1038                 }
1039         }
1040
1041         return ERROR_OK;
1042 }
1043
1044 static int cmsis_dap_get_swo_buf_sz(uint32_t *swo_buf_sz)
1045 {
1046         uint8_t *data;
1047
1048         /* INFO_ID_SWO_BUF_SZ - word */
1049         int retval = cmsis_dap_cmd_dap_info(INFO_ID_SWO_BUF_SZ, &data);
1050         if (retval != ERROR_OK)
1051                 return retval;
1052
1053         if (data[0] != 4)
1054                 return ERROR_FAIL;
1055
1056         *swo_buf_sz = le_to_h_u32(&data[1]);
1057
1058         LOG_INFO("CMSIS-DAP: SWO Trace Buffer Size = %u bytes", *swo_buf_sz);
1059
1060         return ERROR_OK;
1061 }
1062
1063 static int cmsis_dap_get_status(void)
1064 {
1065         uint8_t d;
1066
1067         int retval = cmsis_dap_cmd_dap_swj_pins(0, 0, 0, &d);
1068
1069         if (retval == ERROR_OK) {
1070                 LOG_INFO("SWCLK/TCK = %d SWDIO/TMS = %d TDI = %d TDO = %d nTRST = %d nRESET = %d",
1071                         (d & SWJ_PIN_TCK) ? 1 : 0,
1072                         (d & SWJ_PIN_TMS) ? 1 : 0,
1073                         (d & SWJ_PIN_TDI) ? 1 : 0,
1074                         (d & SWJ_PIN_TDO) ? 1 : 0,
1075                         (d & SWJ_PIN_TRST) ? 1 : 0,
1076                         (d & SWJ_PIN_SRST) ? 1 : 0);
1077         }
1078
1079         return retval;
1080 }
1081
1082 static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
1083 {
1084         const uint8_t *s;
1085         unsigned int s_len;
1086         int retval;
1087
1088         if ((output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST)) == (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
1089                 /* Following workaround deasserts reset on most adapters.
1090                  * Do not reconnect if a reset line is active!
1091                  * Reconnecting would break connecting under reset. */
1092
1093                 /* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
1094                 cmsis_dap_cmd_dap_disconnect();
1095
1096                 /* When we are reconnecting, DAP_Connect needs to be rerun, at
1097                  * least on Keil ULINK-ME */
1098                 retval = cmsis_dap_cmd_dap_connect(CONNECT_SWD);
1099                 if (retval != ERROR_OK)
1100                         return retval;
1101         }
1102
1103         switch (seq) {
1104         case LINE_RESET:
1105                 LOG_DEBUG_IO("SWD line reset");
1106                 s = swd_seq_line_reset;
1107                 s_len = swd_seq_line_reset_len;
1108                 break;
1109         case JTAG_TO_SWD:
1110                 LOG_DEBUG("JTAG-to-SWD");
1111                 s = swd_seq_jtag_to_swd;
1112                 s_len = swd_seq_jtag_to_swd_len;
1113                 break;
1114         case JTAG_TO_DORMANT:
1115                 LOG_DEBUG("JTAG-to-DORMANT");
1116                 s = swd_seq_jtag_to_dormant;
1117                 s_len = swd_seq_jtag_to_dormant_len;
1118                 break;
1119         case SWD_TO_JTAG:
1120                 LOG_DEBUG("SWD-to-JTAG");
1121                 s = swd_seq_swd_to_jtag;
1122                 s_len = swd_seq_swd_to_jtag_len;
1123                 break;
1124         case SWD_TO_DORMANT:
1125                 LOG_DEBUG("SWD-to-DORMANT");
1126                 s = swd_seq_swd_to_dormant;
1127                 s_len = swd_seq_swd_to_dormant_len;
1128                 break;
1129         case DORMANT_TO_SWD:
1130                 LOG_DEBUG("DORMANT-to-SWD");
1131                 s = swd_seq_dormant_to_swd;
1132                 s_len = swd_seq_dormant_to_swd_len;
1133                 break;
1134         case DORMANT_TO_JTAG:
1135                 LOG_DEBUG("DORMANT-to-JTAG");
1136                 s = swd_seq_dormant_to_jtag;
1137                 s_len = swd_seq_dormant_to_jtag_len;
1138                 break;
1139         default:
1140                 LOG_ERROR("Sequence %d not supported", seq);
1141                 return ERROR_FAIL;
1142         }
1143
1144         retval = cmsis_dap_cmd_dap_swj_sequence(s_len, s);
1145         if (retval != ERROR_OK)
1146                 return retval;
1147
1148         /* Atmel EDBG needs renew clock setting after SWJ_Sequence
1149          * otherwise default frequency is used */
1150         return cmsis_dap_cmd_dap_swj_clock(adapter_get_speed_khz());
1151 }
1152
1153 static int cmsis_dap_swd_open(void)
1154 {
1155         if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
1156                 LOG_ERROR("CMSIS-DAP: SWD not supported");
1157                 return ERROR_JTAG_DEVICE_ERROR;
1158         }
1159
1160         int retval = cmsis_dap_cmd_dap_connect(CONNECT_SWD);
1161         if (retval != ERROR_OK)
1162                 return retval;
1163
1164         /* Add more setup here.??... */
1165
1166         LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
1167         return ERROR_OK;
1168 }
1169
1170 static int cmsis_dap_init(void)
1171 {
1172         uint8_t *data;
1173
1174         int retval = cmsis_dap_open();
1175         if (retval != ERROR_OK)
1176                 return retval;
1177
1178         cmsis_dap_flush_read(cmsis_dap_handle);
1179
1180         retval = cmsis_dap_get_caps_info();
1181         if (retval != ERROR_OK)
1182                 return retval;
1183
1184         retval = cmsis_dap_get_version_info();
1185         if (retval != ERROR_OK)
1186                 return retval;
1187
1188         retval = cmsis_dap_get_serial_info();
1189         if (retval != ERROR_OK)
1190                 return retval;
1191
1192         if (swd_mode) {
1193                 retval = cmsis_dap_swd_open();
1194                 if (retval != ERROR_OK)
1195                         return retval;
1196         } else {
1197                 /* Connect in JTAG mode */
1198                 if (!(cmsis_dap_handle->caps & INFO_CAPS_JTAG)) {
1199                         LOG_ERROR("CMSIS-DAP: JTAG not supported");
1200                         return ERROR_JTAG_DEVICE_ERROR;
1201                 }
1202
1203                 retval = cmsis_dap_cmd_dap_connect(CONNECT_JTAG);
1204                 if (retval != ERROR_OK)
1205                         return retval;
1206
1207                 LOG_INFO("CMSIS-DAP: Interface Initialised (JTAG)");
1208         }
1209
1210         /* Be conservative and suppress submitting multiple HID requests
1211          * until we get packet count info from the adaptor */
1212         cmsis_dap_handle->packet_count = 1;
1213         pending_queue_len = 12;
1214
1215         /* INFO_ID_PKT_SZ - short */
1216         retval = cmsis_dap_cmd_dap_info(INFO_ID_PKT_SZ, &data);
1217         if (retval != ERROR_OK)
1218                 goto init_err;
1219
1220         if (data[0] == 2) {  /* short */
1221                 uint16_t pkt_sz = data[1] + (data[2] << 8);
1222                 if (pkt_sz != cmsis_dap_handle->packet_size) {
1223
1224                         /* 4 bytes of command header + 5 bytes per register
1225                          * write. For bulk read sequences just 4 bytes are
1226                          * needed per transfer, so this is suboptimal. */
1227                         pending_queue_len = (pkt_sz - 4) / 5;
1228
1229                         free(cmsis_dap_handle->packet_buffer);
1230                         retval = cmsis_dap_handle->backend->packet_buffer_alloc(cmsis_dap_handle, pkt_sz);
1231                         if (retval != ERROR_OK)
1232                                 goto init_err;
1233
1234                         LOG_DEBUG("CMSIS-DAP: Packet Size = %" PRIu16, pkt_sz);
1235                 }
1236         }
1237
1238         /* INFO_ID_PKT_CNT - byte */
1239         retval = cmsis_dap_cmd_dap_info(INFO_ID_PKT_CNT, &data);
1240         if (retval != ERROR_OK)
1241                 goto init_err;
1242
1243         if (data[0] == 1) { /* byte */
1244                 int pkt_cnt = data[1];
1245                 if (pkt_cnt > 1)
1246                         cmsis_dap_handle->packet_count = MIN(MAX_PENDING_REQUESTS, pkt_cnt);
1247
1248                 LOG_DEBUG("CMSIS-DAP: Packet Count = %d", pkt_cnt);
1249         }
1250
1251         LOG_DEBUG("Allocating FIFO for %d pending packets", cmsis_dap_handle->packet_count);
1252         for (int i = 0; i < cmsis_dap_handle->packet_count; i++) {
1253                 pending_fifo[i].transfers = malloc(pending_queue_len * sizeof(struct pending_transfer_result));
1254                 if (!pending_fifo[i].transfers) {
1255                         LOG_ERROR("Unable to allocate memory for CMSIS-DAP queue");
1256                         retval = ERROR_FAIL;
1257                         goto init_err;
1258                 }
1259         }
1260
1261         /* Intentionally not checked for error, just logs an info message
1262          * not vital for further debugging */
1263         (void)cmsis_dap_get_status();
1264
1265         /* Now try to connect to the target
1266          * TODO: This is all SWD only @ present */
1267         retval = cmsis_dap_cmd_dap_swj_clock(adapter_get_speed_khz());
1268         if (retval != ERROR_OK)
1269                 goto init_err;
1270
1271         /* Ask CMSIS-DAP to automatically retry on receiving WAIT for
1272          * up to 64 times. This must be changed to 0 if sticky
1273          * overrun detection is enabled. */
1274         retval = cmsis_dap_cmd_dap_tfer_configure(0, 64, 0);
1275         if (retval != ERROR_OK)
1276                 goto init_err;
1277
1278         if (swd_mode) {
1279                 /* Data Phase (bit 2) must be set to 1 if sticky overrun
1280                  * detection is enabled */
1281                 retval = cmsis_dap_cmd_dap_swd_configure(0);    /* 1 TRN, no Data Phase */
1282                 if (retval != ERROR_OK)
1283                         goto init_err;
1284         }
1285         /* Both LEDs on */
1286         /* Intentionally not checked for error, debugging will work
1287          * without LEDs */
1288         (void)cmsis_dap_cmd_dap_led(LED_ID_CONNECT, LED_ON);
1289         (void)cmsis_dap_cmd_dap_led(LED_ID_RUN, LED_ON);
1290
1291         /* support connecting with srst asserted */
1292         enum reset_types jtag_reset_config = jtag_get_reset_config();
1293
1294         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
1295                 if (jtag_reset_config & RESET_SRST_NO_GATING) {
1296                         retval = cmsis_dap_cmd_dap_swj_pins(0, SWJ_PIN_SRST, 0, NULL);
1297                         if (retval != ERROR_OK)
1298                                 goto init_err;
1299                         LOG_INFO("Connecting under reset");
1300                 }
1301         }
1302         LOG_INFO("CMSIS-DAP: Interface ready");
1303         return ERROR_OK;
1304
1305 init_err:
1306         cmsis_dap_quit();
1307         return retval;
1308 }
1309
1310 static int cmsis_dap_swd_init(void)
1311 {
1312         swd_mode = true;
1313         return ERROR_OK;
1314 }
1315
1316 static int cmsis_dap_quit(void)
1317 {
1318         cmsis_dap_cmd_dap_disconnect();
1319
1320         /* Both LEDs off */
1321         cmsis_dap_cmd_dap_led(LED_ID_RUN, LED_OFF);
1322         cmsis_dap_cmd_dap_led(LED_ID_CONNECT, LED_OFF);
1323
1324         cmsis_dap_close(cmsis_dap_handle);
1325
1326         return ERROR_OK;
1327 }
1328
1329 static int cmsis_dap_reset(int trst, int srst)
1330 {
1331         /* Set both TRST and SRST even if they're not enabled as
1332          * there's no way to tristate them */
1333
1334         output_pins = 0;
1335         if (!srst)
1336                 output_pins |= SWJ_PIN_SRST;
1337         if (!trst)
1338                 output_pins |= SWJ_PIN_TRST;
1339
1340         int retval = cmsis_dap_cmd_dap_swj_pins(output_pins,
1341                         SWJ_PIN_TRST | SWJ_PIN_SRST, 0, NULL);
1342         if (retval != ERROR_OK)
1343                 LOG_ERROR("CMSIS-DAP: Interface reset failed");
1344         return retval;
1345 }
1346
1347 static void cmsis_dap_execute_sleep(struct jtag_command *cmd)
1348 {
1349 #if 0
1350         int retval = cmsis_dap_cmd_dap_delay(cmd->cmd.sleep->us);
1351         if (retval != ERROR_OK)
1352 #endif
1353                 jtag_sleep(cmd->cmd.sleep->us);
1354 }
1355
1356 /* Set TMS high for five TCK clocks, to move the TAP to the Test-Logic-Reset state */
1357 static int cmsis_dap_execute_tlr_reset(struct jtag_command *cmd)
1358 {
1359         LOG_INFO("cmsis-dap JTAG TLR_RESET");
1360         uint8_t seq = 0xff;
1361
1362         int retval = cmsis_dap_cmd_dap_swj_sequence(8, &seq);
1363         if (retval == ERROR_OK)
1364                 tap_set_state(TAP_RESET);
1365         return retval;
1366 }
1367
1368 /* Set new end state */
1369 static void cmsis_dap_end_state(tap_state_t state)
1370 {
1371         if (tap_is_state_stable(state))
1372                 tap_set_end_state(state);
1373         else {
1374                 LOG_ERROR("BUG: %i is not a valid end state", state);
1375                 exit(-1);
1376         }
1377 }
1378
1379 #ifdef SPRINT_BINARY
1380 static void sprint_binary(char *s, const uint8_t *buf, int offset, int len)
1381 {
1382         if (!len)
1383                 return;
1384
1385         /*
1386         buf = { 0x18 } len=5 should result in: 11000
1387         buf = { 0xff 0x18 } len=13 should result in: 11111111 11000
1388         buf = { 0xc0 0x18 } offset=3 len=10 should result in: 11000 11000
1389                 i=3 there means i/8 = 0 so c = 0xFF, and
1390         */
1391         for (int i = offset; i < offset + len; ++i) {
1392                 uint8_t c = buf[i / 8], mask = 1 << (i % 8);
1393                 if ((i != offset) && !(i % 8))
1394                         putchar(' ');
1395                 *s++ = (c & mask) ? '1' : '0';
1396         }
1397         *s = 0;
1398 }
1399 #endif
1400
1401 #ifdef CMSIS_DAP_JTAG_DEBUG
1402 static void debug_parse_cmsis_buf(const uint8_t *cmd, int cmdlen)
1403 {
1404         /* cmd is a usb packet to go to the cmsis-dap interface */
1405         printf("cmsis-dap buffer (%d b): ", cmdlen);
1406         for (int i = 0; i < cmdlen; ++i)
1407                 printf(" %02x", cmd[i]);
1408         printf("\n");
1409         switch (cmd[1]) {
1410                 case CMD_DAP_JTAG_SEQ: {
1411                         printf("cmsis-dap jtag sequence command %02x (n=%d)\n", cmd[1], cmd[2]);
1412                         /*
1413                          * #2 = number of sequences
1414                          * #3 = sequence info 1
1415                          * #4...4+n_bytes-1 = sequence 1
1416                          * #4+n_bytes = sequence info 2
1417                          * #5+n_bytes = sequence 2 (single bit)
1418                          */
1419                         int pos = 3;
1420                         for (int seq = 0; seq < cmd[2]; ++seq) {
1421                                 uint8_t info = cmd[pos++];
1422                                 int len = info & DAP_JTAG_SEQ_TCK;
1423                                 if (len == 0)
1424                                         len = 64;
1425                                 printf("  sequence %d starting %d: info %02x (len=%d tms=%d read_tdo=%d): ",
1426                                         seq, pos, info, len, info & DAP_JTAG_SEQ_TMS, info & DAP_JTAG_SEQ_TDO);
1427                                 for (int i = 0; i < DIV_ROUND_UP(len, 8); ++i)
1428                                         printf(" %02x", cmd[pos+i]);
1429                                 pos += DIV_ROUND_UP(len, 8);
1430                                 printf("\n");
1431                         }
1432                         if (pos != cmdlen) {
1433                                 printf("BUFFER LENGTH MISMATCH looks like %d but %d specified", pos, cmdlen);
1434                                 exit(-1);
1435                         }
1436
1437                         break;
1438                 }
1439                 default:
1440                         LOG_DEBUG("unknown cmsis-dap command %02x", cmd[1]);
1441                         break;
1442         }
1443 }
1444 #endif
1445
1446 static void cmsis_dap_flush(void)
1447 {
1448         if (!queued_seq_count)
1449                 return;
1450
1451         LOG_DEBUG_IO("Flushing %d queued sequences (%d bytes) with %d pending scan results to capture",
1452                 queued_seq_count, queued_seq_buf_end, pending_scan_result_count);
1453
1454         /* prepare CMSIS-DAP packet */
1455         uint8_t *command = cmsis_dap_handle->command;
1456         command[0] = CMD_DAP_JTAG_SEQ;
1457         command[1] = queued_seq_count;
1458         memcpy(&command[2], queued_seq_buf, queued_seq_buf_end);
1459
1460 #ifdef CMSIS_DAP_JTAG_DEBUG
1461         debug_parse_cmsis_buf(command, queued_seq_buf_end + 2);
1462 #endif
1463
1464         /* send command to USB device */
1465         int retval = cmsis_dap_xfer(cmsis_dap_handle, queued_seq_buf_end + 2);
1466
1467         uint8_t *resp = cmsis_dap_handle->response;
1468         if (retval != ERROR_OK || resp[1] != DAP_OK) {
1469                 LOG_ERROR("CMSIS-DAP command CMD_DAP_JTAG_SEQ failed.");
1470                 exit(-1);
1471         }
1472
1473 #ifdef CMSIS_DAP_JTAG_DEBUG
1474         LOG_DEBUG_IO("USB response buf:");
1475         for (int c = 0; c < queued_seq_buf_end + 3; ++c)
1476                 printf("%02X ", resp[c]);
1477         printf("\n");
1478 #endif
1479
1480         /* copy scan results into client buffers */
1481         for (int i = 0; i < pending_scan_result_count; ++i) {
1482                 struct pending_scan_result *scan = &pending_scan_results[i];
1483                 LOG_DEBUG_IO("Copying pending_scan_result %d/%d: %d bits from byte %d -> buffer + %d bits",
1484                         i, pending_scan_result_count, scan->length, scan->first + 2, scan->buffer_offset);
1485 #ifdef CMSIS_DAP_JTAG_DEBUG
1486                 for (uint32_t b = 0; b < DIV_ROUND_UP(scan->length, 8); ++b)
1487                         printf("%02X ", resp[2+scan->first+b]);
1488                 printf("\n");
1489 #endif
1490                 bit_copy(scan->buffer, scan->buffer_offset, &resp[2 + scan->first], 0, scan->length);
1491         }
1492
1493         /* reset */
1494         queued_seq_count = 0;
1495         queued_seq_buf_end = 0;
1496         queued_seq_tdo_ptr = 0;
1497         pending_scan_result_count = 0;
1498 }
1499
1500 /* queue a sequence of bits to clock out TDI / in TDO, executing if the buffer is full.
1501  *
1502  * sequence=NULL means clock out zeros on TDI
1503  * tdo_buffer=NULL means don't capture TDO
1504  */
1505 static void cmsis_dap_add_jtag_sequence(int s_len, const uint8_t *sequence, int s_offset,
1506                                         bool tms, uint8_t *tdo_buffer, int tdo_buffer_offset)
1507 {
1508         LOG_DEBUG_IO("[at %d] %d bits, tms %s, seq offset %d, tdo buf %p, tdo offset %d",
1509                 queued_seq_buf_end,
1510                 s_len, tms ? "HIGH" : "LOW", s_offset, tdo_buffer, tdo_buffer_offset);
1511
1512         if (s_len == 0)
1513                 return;
1514
1515         if (s_len > 64) {
1516                 LOG_DEBUG_IO("START JTAG SEQ SPLIT");
1517                 for (int offset = 0; offset < s_len; offset += 64) {
1518                         int len = s_len - offset;
1519                         if (len > 64)
1520                                 len = 64;
1521                         LOG_DEBUG_IO("Splitting long jtag sequence: %d-bit chunk starting at offset %d", len, offset);
1522                         cmsis_dap_add_jtag_sequence(
1523                                 len,
1524                                 sequence,
1525                                 s_offset + offset,
1526                                 tms,
1527                                 tdo_buffer,
1528                                 !tdo_buffer ? 0 : (tdo_buffer_offset + offset)
1529                                 );
1530                 }
1531                 LOG_DEBUG_IO("END JTAG SEQ SPLIT");
1532                 return;
1533         }
1534
1535         int cmd_len = 1 + DIV_ROUND_UP(s_len, 8);
1536         if (queued_seq_count >= 255 || queued_seq_buf_end + cmd_len > QUEUED_SEQ_BUF_LEN)
1537                 /* empty out the buffer */
1538                 cmsis_dap_flush();
1539
1540         ++queued_seq_count;
1541
1542         /* control byte */
1543         queued_seq_buf[queued_seq_buf_end] =
1544                 (tms ? DAP_JTAG_SEQ_TMS : 0) |
1545                 (tdo_buffer ? DAP_JTAG_SEQ_TDO : 0) |
1546                 (s_len == 64 ? 0 : s_len);
1547
1548         if (sequence)
1549                 bit_copy(&queued_seq_buf[queued_seq_buf_end + 1], 0, sequence, s_offset, s_len);
1550         else
1551                 memset(&queued_seq_buf[queued_seq_buf_end + 1], 0, DIV_ROUND_UP(s_len, 8));
1552
1553         queued_seq_buf_end += cmd_len;
1554
1555         if (tdo_buffer) {
1556                 struct pending_scan_result *scan = &pending_scan_results[pending_scan_result_count++];
1557                 scan->first = queued_seq_tdo_ptr;
1558                 queued_seq_tdo_ptr += DIV_ROUND_UP(s_len, 8);
1559                 scan->length = s_len;
1560                 scan->buffer = tdo_buffer;
1561                 scan->buffer_offset = tdo_buffer_offset;
1562         }
1563 }
1564
1565 /* queue a sequence of bits to clock out TMS, executing if the buffer is full */
1566 static void cmsis_dap_add_tms_sequence(const uint8_t *sequence, int s_len)
1567 {
1568         LOG_DEBUG_IO("%d bits: %02X", s_len, *sequence);
1569         /* we use a series of CMD_DAP_JTAG_SEQ commands to toggle TMS,
1570            because even though it seems ridiculously inefficient, it
1571            allows us to combine TMS and scan sequences into the same
1572            USB packet. */
1573         /* TODO: combine runs of the same tms value */
1574         for (int i = 0; i < s_len; ++i) {
1575                 bool bit = (sequence[i / 8] & (1 << (i % 8))) != 0;
1576                 cmsis_dap_add_jtag_sequence(1, NULL, 0, bit, NULL, 0);
1577         }
1578 }
1579
1580 /* Move to the end state by queuing a sequence to clock into TMS */
1581 static void cmsis_dap_state_move(void)
1582 {
1583         uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
1584         uint8_t tms_scan_bits = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
1585
1586         LOG_DEBUG_IO("state move from %s to %s: %d clocks, %02X on tms",
1587                 tap_state_name(tap_get_state()), tap_state_name(tap_get_end_state()),
1588                 tms_scan_bits, tms_scan);
1589         cmsis_dap_add_tms_sequence(&tms_scan, tms_scan_bits);
1590
1591         tap_set_state(tap_get_end_state());
1592 }
1593
1594
1595 /* Execute a JTAG scan operation by queueing TMS and TDI/TDO sequences */
1596 static void cmsis_dap_execute_scan(struct jtag_command *cmd)
1597 {
1598         LOG_DEBUG_IO("%s type:%d", cmd->cmd.scan->ir_scan ? "IRSCAN" : "DRSCAN",
1599                 jtag_scan_type(cmd->cmd.scan));
1600
1601         /* Make sure there are no trailing fields with num_bits == 0, or the logic below will fail. */
1602         while (cmd->cmd.scan->num_fields > 0
1603                         && cmd->cmd.scan->fields[cmd->cmd.scan->num_fields - 1].num_bits == 0) {
1604                 cmd->cmd.scan->num_fields--;
1605                 LOG_DEBUG("discarding trailing empty field");
1606         }
1607
1608         if (cmd->cmd.scan->num_fields == 0) {
1609                 LOG_DEBUG("empty scan, doing nothing");
1610                 return;
1611         }
1612
1613         if (cmd->cmd.scan->ir_scan) {
1614                 if (tap_get_state() != TAP_IRSHIFT) {
1615                         cmsis_dap_end_state(TAP_IRSHIFT);
1616                         cmsis_dap_state_move();
1617                 }
1618         } else {
1619                 if (tap_get_state() != TAP_DRSHIFT) {
1620                         cmsis_dap_end_state(TAP_DRSHIFT);
1621                         cmsis_dap_state_move();
1622                 }
1623         }
1624
1625         cmsis_dap_end_state(cmd->cmd.scan->end_state);
1626
1627         struct scan_field *field = cmd->cmd.scan->fields;
1628         unsigned scan_size = 0;
1629
1630         for (int i = 0; i < cmd->cmd.scan->num_fields; i++, field++) {
1631                 scan_size += field->num_bits;
1632                 LOG_DEBUG_IO("%s%s field %d/%d %d bits",
1633                         field->in_value ? "in" : "",
1634                         field->out_value ? "out" : "",
1635                         i,
1636                         cmd->cmd.scan->num_fields,
1637                         field->num_bits);
1638
1639                 if (i == cmd->cmd.scan->num_fields - 1 && tap_get_state() != tap_get_end_state()) {
1640                         LOG_DEBUG_IO("Last field and have to move out of SHIFT state");
1641                         /* Last field, and we're leaving IRSHIFT/DRSHIFT. Clock last bit during tap
1642                          * movement. This last field can't have length zero, it was checked above. */
1643                         cmsis_dap_add_jtag_sequence(
1644                                 field->num_bits - 1, /* number of bits to clock */
1645                                 field->out_value, /* output sequence */
1646                                 0, /* output offset */
1647                                 false, /* TMS low */
1648                                 field->in_value,
1649                                 0);
1650
1651                         /* Clock the last bit out, with TMS high */
1652                         uint8_t last_bit = 0;
1653                         if (field->out_value)
1654                                 bit_copy(&last_bit, 0, field->out_value, field->num_bits - 1, 1);
1655                         cmsis_dap_add_jtag_sequence(
1656                                 1,
1657                                 &last_bit,
1658                                 0,
1659                                 true,
1660                                 field->in_value,
1661                                 field->num_bits - 1);
1662                         tap_set_state(tap_state_transition(tap_get_state(), 1));
1663
1664                         /* Now clock one more cycle, with TMS low, to get us into a PAUSE state */
1665                         cmsis_dap_add_jtag_sequence(
1666                                 1,
1667                                 &last_bit,
1668                                 0,
1669                                 false,
1670                                 NULL,
1671                                 0);
1672                         tap_set_state(tap_state_transition(tap_get_state(), 0));
1673                 } else {
1674                         LOG_DEBUG_IO("Internal field, staying in SHIFT state afterwards");
1675                         /* Clocking part of a sequence into DR or IR with TMS=0,
1676                            leaving TMS=0 at the end so we can continue later */
1677                         cmsis_dap_add_jtag_sequence(
1678                                 field->num_bits,
1679                                 field->out_value,
1680                                 0,
1681                                 false,
1682                                 field->in_value,
1683                                 0);
1684                 }
1685         }
1686
1687         if (tap_get_state() != tap_get_end_state()) {
1688                 cmsis_dap_end_state(tap_get_end_state());
1689                 cmsis_dap_state_move();
1690         }
1691
1692         LOG_DEBUG_IO("%s scan, %i bits, end in %s",
1693                 (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
1694                 tap_state_name(tap_get_end_state()));
1695 }
1696
1697 static void cmsis_dap_pathmove(int num_states, tap_state_t *path)
1698 {
1699         uint8_t tms0 = 0x00;
1700         uint8_t tms1 = 0xff;
1701
1702         for (int i = 0; i < num_states; i++) {
1703                 if (path[i] == tap_state_transition(tap_get_state(), false))
1704                         cmsis_dap_add_tms_sequence(&tms0, 1);
1705                 else if (path[i] == tap_state_transition(tap_get_state(), true))
1706                         cmsis_dap_add_tms_sequence(&tms1, 1);
1707                 else {
1708                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition.",
1709                                   tap_state_name(tap_get_state()), tap_state_name(path[i]));
1710                         exit(-1);
1711                 }
1712
1713                 tap_set_state(path[i]);
1714         }
1715
1716         cmsis_dap_end_state(tap_get_state());
1717 }
1718
1719 static void cmsis_dap_execute_pathmove(struct jtag_command *cmd)
1720 {
1721         LOG_DEBUG_IO("pathmove: %i states, end in %i",
1722                       cmd->cmd.pathmove->num_states,
1723                cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1724
1725         cmsis_dap_pathmove(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
1726 }
1727
1728 static void cmsis_dap_stableclocks(int num_cycles)
1729 {
1730         uint8_t tms = tap_get_state() == TAP_RESET;
1731         /* TODO: Perform optimizations? */
1732         /* Execute num_cycles. */
1733         for (int i = 0; i < num_cycles; i++)
1734                 cmsis_dap_add_tms_sequence(&tms, 1);
1735 }
1736
1737 static void cmsis_dap_runtest(int num_cycles)
1738 {
1739         tap_state_t saved_end_state = tap_get_end_state();
1740
1741         /* Only do a state_move when we're not already in IDLE. */
1742         if (tap_get_state() != TAP_IDLE) {
1743                 cmsis_dap_end_state(TAP_IDLE);
1744                 cmsis_dap_state_move();
1745         }
1746         cmsis_dap_stableclocks(num_cycles);
1747
1748         /* Finish in end_state. */
1749         cmsis_dap_end_state(saved_end_state);
1750
1751         if (tap_get_state() != tap_get_end_state())
1752                 cmsis_dap_state_move();
1753 }
1754
1755 static void cmsis_dap_execute_runtest(struct jtag_command *cmd)
1756 {
1757         LOG_DEBUG_IO("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles,
1758                       cmd->cmd.runtest->end_state);
1759
1760         cmsis_dap_end_state(cmd->cmd.runtest->end_state);
1761         cmsis_dap_runtest(cmd->cmd.runtest->num_cycles);
1762 }
1763
1764 static void cmsis_dap_execute_stableclocks(struct jtag_command *cmd)
1765 {
1766         LOG_DEBUG_IO("stableclocks %i cycles", cmd->cmd.runtest->num_cycles);
1767         cmsis_dap_stableclocks(cmd->cmd.runtest->num_cycles);
1768 }
1769
1770 static void cmsis_dap_execute_tms(struct jtag_command *cmd)
1771 {
1772         LOG_DEBUG_IO("TMS: %d bits", cmd->cmd.tms->num_bits);
1773         cmsis_dap_cmd_dap_swj_sequence(cmd->cmd.tms->num_bits, cmd->cmd.tms->bits);
1774 }
1775
1776 /* TODO: Is there need to call cmsis_dap_flush() for the JTAG_PATHMOVE,
1777  * JTAG_RUNTEST, JTAG_STABLECLOCKS? */
1778 static void cmsis_dap_execute_command(struct jtag_command *cmd)
1779 {
1780         switch (cmd->type) {
1781                 case JTAG_SLEEP:
1782                         cmsis_dap_flush();
1783                         cmsis_dap_execute_sleep(cmd);
1784                         break;
1785                 case JTAG_TLR_RESET:
1786                         cmsis_dap_flush();
1787                         cmsis_dap_execute_tlr_reset(cmd);
1788                         break;
1789                 case JTAG_SCAN:
1790                         cmsis_dap_execute_scan(cmd);
1791                         break;
1792                 case JTAG_PATHMOVE:
1793                         cmsis_dap_execute_pathmove(cmd);
1794                         break;
1795                 case JTAG_RUNTEST:
1796                         cmsis_dap_execute_runtest(cmd);
1797                         break;
1798                 case JTAG_STABLECLOCKS:
1799                         cmsis_dap_execute_stableclocks(cmd);
1800                         break;
1801                 case JTAG_TMS:
1802                         cmsis_dap_execute_tms(cmd);
1803                         break;
1804                 default:
1805                         LOG_ERROR("BUG: unknown JTAG command type 0x%X encountered", cmd->type);
1806                         exit(-1);
1807         }
1808 }
1809
1810 static int cmsis_dap_execute_queue(void)
1811 {
1812         struct jtag_command *cmd = jtag_command_queue;
1813
1814         while (cmd) {
1815                 cmsis_dap_execute_command(cmd);
1816                 cmd = cmd->next;
1817         }
1818
1819         cmsis_dap_flush();
1820
1821         return ERROR_OK;
1822 }
1823
1824 static int cmsis_dap_speed(int speed)
1825 {
1826         if (speed == 0) {
1827                 LOG_ERROR("RTCK not supported. Set nonzero \"adapter speed\".");
1828                 return ERROR_JTAG_NOT_IMPLEMENTED;
1829         }
1830
1831         return cmsis_dap_cmd_dap_swj_clock(speed);
1832 }
1833
1834 static int cmsis_dap_speed_div(int speed, int *khz)
1835 {
1836         *khz = speed;
1837         return ERROR_OK;
1838 }
1839
1840 static int cmsis_dap_khz(int khz, int *jtag_speed)
1841 {
1842         *jtag_speed = khz;
1843         return ERROR_OK;
1844 }
1845
1846 static bool calculate_swo_prescaler(unsigned int traceclkin_freq,
1847                 uint32_t trace_freq, uint16_t *prescaler)
1848 {
1849         unsigned int presc = (traceclkin_freq + trace_freq / 2) / trace_freq;
1850         if (presc == 0 || presc > TPIU_ACPR_MAX_SWOSCALER + 1)
1851                 return false;
1852
1853         /* Probe's UART speed must be within 3% of the TPIU's SWO baud rate. */
1854         unsigned int max_deviation = (traceclkin_freq * 3) / 100;
1855         if (presc * trace_freq < traceclkin_freq - max_deviation ||
1856             presc * trace_freq > traceclkin_freq + max_deviation)
1857                 return false;
1858
1859         *prescaler = presc;
1860
1861         return true;
1862 }
1863
1864 /**
1865  * @see adapter_driver::config_trace
1866  */
1867 static int cmsis_dap_config_trace(
1868                                 bool trace_enabled,
1869                                 enum tpiu_pin_protocol pin_protocol,
1870                                 uint32_t port_size,
1871                                 unsigned int *swo_freq,
1872                                 unsigned int traceclkin_hz,
1873                                 uint16_t *swo_prescaler)
1874 {
1875         int retval;
1876
1877         if (!trace_enabled) {
1878                 if (cmsis_dap_handle->trace_enabled) {
1879                         retval = cmsis_dap_cmd_dap_swo_control(DAP_SWO_CONTROL_STOP);
1880                         if (retval != ERROR_OK) {
1881                                 LOG_ERROR("Failed to disable the SWO-trace.");
1882                                 return retval;
1883                         }
1884                 }
1885                 cmsis_dap_handle->trace_enabled = false;
1886                 LOG_INFO("SWO-trace disabled.");
1887                 return ERROR_OK;
1888         }
1889
1890         if (!(cmsis_dap_handle->caps & INFO_CAPS_SWO_UART) &&
1891             !(cmsis_dap_handle->caps & INFO_CAPS_SWO_MANCHESTER)) {
1892                 LOG_ERROR("SWO-trace is not supported by the device.");
1893                 return ERROR_FAIL;
1894         }
1895
1896         uint8_t swo_mode;
1897         if (pin_protocol == TPIU_PIN_PROTOCOL_ASYNC_UART &&
1898            (cmsis_dap_handle->caps & INFO_CAPS_SWO_UART)) {
1899                 swo_mode = DAP_SWO_MODE_UART;
1900         } else if (pin_protocol == TPIU_PIN_PROTOCOL_ASYNC_MANCHESTER &&
1901                   (cmsis_dap_handle->caps & INFO_CAPS_SWO_MANCHESTER)) {
1902                 swo_mode = DAP_SWO_MODE_MANCHESTER;
1903         } else {
1904                 LOG_ERROR("Selected pin protocol is not supported.");
1905                 return ERROR_FAIL;
1906         }
1907
1908         if (*swo_freq == 0) {
1909                 LOG_INFO("SWO-trace frequency autodetection not implemented.");
1910                 return ERROR_FAIL;
1911         }
1912
1913         retval = cmsis_dap_cmd_dap_swo_control(DAP_SWO_CONTROL_STOP);
1914         if (retval != ERROR_OK)
1915                 return retval;
1916
1917         cmsis_dap_handle->trace_enabled = false;
1918
1919         retval = cmsis_dap_get_swo_buf_sz(&cmsis_dap_handle->swo_buf_sz);
1920         if (retval != ERROR_OK)
1921                 return retval;
1922
1923         retval = cmsis_dap_cmd_dap_swo_transport(DAP_SWO_TRANSPORT_DATA);
1924         if (retval != ERROR_OK)
1925                 return retval;
1926
1927         retval = cmsis_dap_cmd_dap_swo_mode(swo_mode);
1928         if (retval != ERROR_OK)
1929                 return retval;
1930
1931         retval = cmsis_dap_cmd_dap_swo_baudrate(*swo_freq, swo_freq);
1932         if (retval != ERROR_OK)
1933                 return retval;
1934
1935         if (!calculate_swo_prescaler(traceclkin_hz, *swo_freq,
1936                         swo_prescaler)) {
1937                 LOG_ERROR("SWO frequency is not suitable. Please choose a "
1938                         "different frequency or use auto-detection.");
1939                 return ERROR_FAIL;
1940         }
1941
1942         LOG_INFO("SWO frequency: %u Hz.", *swo_freq);
1943         LOG_INFO("SWO prescaler: %u.", *swo_prescaler);
1944
1945         retval = cmsis_dap_cmd_dap_swo_control(DAP_SWO_CONTROL_START);
1946         if (retval != ERROR_OK)
1947                 return retval;
1948
1949         cmsis_dap_handle->trace_enabled = true;
1950
1951         return ERROR_OK;
1952 }
1953
1954 /**
1955  * @see adapter_driver::poll_trace
1956  */
1957 static int cmsis_dap_poll_trace(uint8_t *buf, size_t *size)
1958 {
1959         uint8_t trace_status;
1960         size_t trace_count;
1961
1962         if (!cmsis_dap_handle->trace_enabled) {
1963                 *size = 0;
1964                 return ERROR_OK;
1965         }
1966
1967         int retval = cmsis_dap_cmd_dap_swo_status(&trace_status, &trace_count);
1968         if (retval != ERROR_OK)
1969                 return retval;
1970         if ((trace_status & DAP_SWO_STATUS_CAPTURE_MASK) != DAP_SWO_STATUS_CAPTURE_ACTIVE)
1971                 return ERROR_FAIL;
1972
1973         *size = trace_count < *size ? trace_count : *size;
1974         size_t read_so_far = 0;
1975         do {
1976                 size_t rb = 0;
1977                 uint32_t packet_size = cmsis_dap_handle->packet_size - 4 /*data-reply*/;
1978                 uint32_t remaining = *size - read_so_far;
1979                 if (remaining < packet_size)
1980                         packet_size = remaining;
1981                 retval = cmsis_dap_cmd_dap_swo_data(
1982                                                 packet_size,
1983                                                 &trace_status,
1984                                                 &rb,
1985                                                 &buf[read_so_far]);
1986                 if (retval != ERROR_OK)
1987                         return retval;
1988                 if ((trace_status & DAP_SWO_STATUS_CAPTURE_MASK) != DAP_SWO_STATUS_CAPTURE_ACTIVE)
1989                         return ERROR_FAIL;
1990
1991                 read_so_far += rb;
1992         } while (read_so_far < *size);
1993
1994         return ERROR_OK;
1995 }
1996
1997 COMMAND_HANDLER(cmsis_dap_handle_info_command)
1998 {
1999         if (cmsis_dap_get_version_info() == ERROR_OK)
2000                 cmsis_dap_get_status();
2001
2002         return ERROR_OK;
2003 }
2004
2005 COMMAND_HANDLER(cmsis_dap_handle_cmd_command)
2006 {
2007         uint8_t *command = cmsis_dap_handle->command;
2008
2009         for (unsigned i = 0; i < CMD_ARGC; i++)
2010                 COMMAND_PARSE_NUMBER(u8, CMD_ARGV[i], command[i]);
2011
2012         int retval = cmsis_dap_xfer(cmsis_dap_handle, CMD_ARGC);
2013
2014         if (retval != ERROR_OK) {
2015                 LOG_ERROR("CMSIS-DAP command failed.");
2016                 return ERROR_JTAG_DEVICE_ERROR;
2017         }
2018
2019         uint8_t *resp = cmsis_dap_handle->response;
2020         LOG_INFO("Returned data %02" PRIx8 " %02" PRIx8 " %02" PRIx8 " %02" PRIx8,
2021                 resp[1], resp[2], resp[3], resp[4]);
2022
2023         return ERROR_OK;
2024 }
2025
2026 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
2027 {
2028         if (CMD_ARGC > MAX_USB_IDS * 2) {
2029                 LOG_WARNING("ignoring extra IDs in cmsis_dap_vid_pid "
2030                         "(maximum is %d pairs)", MAX_USB_IDS);
2031                 CMD_ARGC = MAX_USB_IDS * 2;
2032         }
2033         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
2034                 LOG_WARNING("incomplete cmsis_dap_vid_pid configuration directive");
2035                 if (CMD_ARGC < 2)
2036                         return ERROR_COMMAND_SYNTAX_ERROR;
2037                 /* remove the incomplete trailing id */
2038                 CMD_ARGC -= 1;
2039         }
2040
2041         unsigned i;
2042         for (i = 0; i < CMD_ARGC; i += 2) {
2043                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], cmsis_dap_vid[i >> 1]);
2044                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], cmsis_dap_pid[i >> 1]);
2045         }
2046
2047         /*
2048          * Explicitly terminate, in case there are multiples instances of
2049          * cmsis_dap_vid_pid.
2050          */
2051         cmsis_dap_vid[i >> 1] = cmsis_dap_pid[i >> 1] = 0;
2052
2053         return ERROR_OK;
2054 }
2055
2056 COMMAND_HANDLER(cmsis_dap_handle_backend_command)
2057 {
2058         if (CMD_ARGC == 1) {
2059                 if (strcmp(CMD_ARGV[0], "auto") == 0) {
2060                         cmsis_dap_backend = -1; /* autoselect */
2061                 } else {
2062                         for (unsigned int i = 0; i < ARRAY_SIZE(cmsis_dap_backends); i++) {
2063                                 if (strcasecmp(cmsis_dap_backends[i]->name, CMD_ARGV[0]) == 0) {
2064                                         cmsis_dap_backend = i;
2065                                         return ERROR_OK;
2066                                 }
2067                         }
2068
2069                         LOG_ERROR("invalid backend argument to cmsis_dap_backend <backend>");
2070                 }
2071         } else {
2072                 LOG_ERROR("expected exactly one argument to cmsis_dap_backend <backend>");
2073         }
2074
2075         return ERROR_OK;
2076 }
2077
2078 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
2079         {
2080                 .name = "info",
2081                 .handler = &cmsis_dap_handle_info_command,
2082                 .mode = COMMAND_EXEC,
2083                 .usage = "",
2084                 .help = "show cmsis-dap info",
2085         },
2086         {
2087                 .name = "cmd",
2088                 .handler = &cmsis_dap_handle_cmd_command,
2089                 .mode = COMMAND_EXEC,
2090                 .usage = "",
2091                 .help = "issue cmsis-dap command",
2092         },
2093         COMMAND_REGISTRATION_DONE
2094 };
2095
2096
2097 static const struct command_registration cmsis_dap_command_handlers[] = {
2098         {
2099                 .name = "cmsis-dap",
2100                 .mode = COMMAND_ANY,
2101                 .help = "perform CMSIS-DAP management",
2102                 .usage = "<cmd>",
2103                 .chain = cmsis_dap_subcommand_handlers,
2104         },
2105         {
2106                 .name = "cmsis_dap_vid_pid",
2107                 .handler = &cmsis_dap_handle_vid_pid_command,
2108                 .mode = COMMAND_CONFIG,
2109                 .help = "the vendor ID and product ID of the CMSIS-DAP device",
2110                 .usage = "(vid pid)*",
2111         },
2112         {
2113                 .name = "cmsis_dap_backend",
2114                 .handler = &cmsis_dap_handle_backend_command,
2115                 .mode = COMMAND_CONFIG,
2116                 .help = "set the communication backend to use (USB bulk or HID).",
2117                 .usage = "(auto | usb_bulk | hid)",
2118         },
2119 #if BUILD_CMSIS_DAP_USB
2120         {
2121                 .name = "cmsis_dap_usb",
2122                 .chain = cmsis_dap_usb_subcommand_handlers,
2123                 .mode = COMMAND_ANY,
2124                 .help = "USB bulk backend-specific commands",
2125                 .usage = "<cmd>",
2126         },
2127 #endif
2128         COMMAND_REGISTRATION_DONE
2129 };
2130
2131 static const struct swd_driver cmsis_dap_swd_driver = {
2132         .init = cmsis_dap_swd_init,
2133         .switch_seq = cmsis_dap_swd_switch_seq,
2134         .read_reg = cmsis_dap_swd_read_reg,
2135         .write_reg = cmsis_dap_swd_write_reg,
2136         .run = cmsis_dap_swd_run_queue,
2137 };
2138
2139 static const char * const cmsis_dap_transport[] = { "swd", "jtag", NULL };
2140
2141 static struct jtag_interface cmsis_dap_interface = {
2142         .supported = DEBUG_CAP_TMS_SEQ,
2143         .execute_queue = cmsis_dap_execute_queue,
2144 };
2145
2146 struct adapter_driver cmsis_dap_adapter_driver = {
2147         .name = "cmsis-dap",
2148         .transports = cmsis_dap_transport,
2149         .commands = cmsis_dap_command_handlers,
2150
2151         .init = cmsis_dap_init,
2152         .quit = cmsis_dap_quit,
2153         .reset = cmsis_dap_reset,
2154         .speed = cmsis_dap_speed,
2155         .khz = cmsis_dap_khz,
2156         .speed_div = cmsis_dap_speed_div,
2157         .config_trace = cmsis_dap_config_trace,
2158         .poll_trace = cmsis_dap_poll_trace,
2159
2160         .jtag_ops = &cmsis_dap_interface,
2161         .swd_ops = &cmsis_dap_swd_driver,
2162 };