git.sur5r.net Git - openocd/blob - src/jtag/drivers/cmsis_dap_usb.c

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