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