git.gag.com Git - fw/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("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("%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("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         /* Data Phase (bit 2) must be set to 1 if sticky overrun
 962          * detection is enabled */
 963         retval = cmsis_dap_cmd_DAP_SWD_Configure(0);    /* 1 TRN, no Data Phase */
 964         if (retval != ERROR_OK)
 965                 return ERROR_FAIL;
 966
 967         retval = cmsis_dap_cmd_DAP_LED(0x03);           /* Both LEDs on */
 968         if (retval != ERROR_OK)
 969                 return ERROR_FAIL;
 970
 971         /* support connecting with srst asserted */
 972         enum reset_types jtag_reset_config = jtag_get_reset_config();
 973
 974         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
 975                 if (jtag_reset_config & RESET_SRST_NO_GATING) {
 976                         retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, (1 << 7), 0, NULL);
 977                         if (retval != ERROR_OK)
 978                                 return ERROR_FAIL;
 979                         LOG_INFO("Connecting under reset");
 980                 }
 981         }
 982
 983         cmsis_dap_cmd_DAP_LED(0x00);                    /* Both LEDs off */
 984
 985         LOG_INFO("CMSIS-DAP: Interface ready");
 986
 987         return ERROR_OK;
 988 }
 989
 990 static int cmsis_dap_swd_init(void)
 991 {
 992         swd_mode = true;
 993         return ERROR_OK;
 994 }
 995
 996 static int cmsis_dap_quit(void)
 997 {
 998         cmsis_dap_cmd_DAP_Disconnect();
 999         cmsis_dap_cmd_DAP_LED(0x00);            /* Both LEDs off */
1000
1001         cmsis_dap_usb_close(cmsis_dap_handle);
1002
1003         return ERROR_OK;
1004 }
1005
1006 static void cmsis_dap_execute_reset(struct jtag_command *cmd)
1007 {
1008         /* Set both TRST and SRST even if they're not enabled as
1009          * there's no way to tristate them */
1010         uint8_t pins = 0;
1011
1012         if (!cmd->cmd.reset->srst)
1013                 pins |= SWJ_PIN_SRST;
1014         if (!cmd->cmd.reset->trst)
1015                 pins |= SWJ_PIN_TRST;
1016
1017         int retval = cmsis_dap_cmd_DAP_SWJ_Pins(pins,
1018                         SWJ_PIN_TRST | SWJ_PIN_SRST, 0, NULL);
1019         if (retval != ERROR_OK)
1020                 LOG_ERROR("CMSIS-DAP: Interface reset failed");
1021 }
1022
1023 static void cmsis_dap_execute_sleep(struct jtag_command *cmd)
1024 {
1025 #if 0
1026         int retval = cmsis_dap_cmd_DAP_Delay(cmd->cmd.sleep->us);
1027         if (retval != ERROR_OK)
1028 #endif
1029                 jtag_sleep(cmd->cmd.sleep->us);
1030 }
1031
1032 /* Set TMS high for five TCK clocks, to move the TAP to the Test-Logic-Reset state */
1033 static int cmsis_dap_execute_tlr_reset(struct jtag_command *cmd)
1034 {
1035         LOG_INFO("cmsis-dap JTAG TLR_RESET");
1036         uint8_t seq = 0xff;
1037         int ret = cmsis_dap_cmd_DAP_SWJ_Sequence(8, &seq);
1038         if (ret == ERROR_OK)
1039                 tap_set_state(TAP_RESET);
1040         return ret;
1041 }
1042
1043 /* Set new end state */
1044 static void cmsis_dap_end_state(tap_state_t state)
1045 {
1046         if (tap_is_state_stable(state))
1047                 tap_set_end_state(state);
1048         else {
1049                 LOG_ERROR("BUG: %i is not a valid end state", state);
1050                 exit(-1);
1051         }
1052 }
1053
1054 #ifdef SPRINT_BINARY
1055 static void sprint_binary(char *s, const uint8_t *buf, int offset, int len)
1056 {
1057         if (!len)
1058                 return;
1059
1060         /*
1061         buf = { 0x18 } len=5 should result in: 11000
1062         buf = { 0xff 0x18 } len=13 should result in: 11111111 11000
1063         buf = { 0xc0 0x18 } offset=3 len=10 should result in: 11000 11000
1064                 i=3 there means i/8 = 0 so c = 0xFF, and
1065         */
1066         for (int i = offset; i < offset + len; ++i) {
1067                 uint8_t c = buf[i / 8], mask = 1 << (i % 8);
1068                 if ((i != offset) && !(i % 8))
1069                         putchar(' ');
1070                 *s++ = (c & mask) ? '1' : '0';
1071         }
1072         *s = 0;
1073 }
1074 #endif
1075
1076 #ifdef CMSIS_DAP_JTAG_DEBUG
1077 static void debug_parse_cmsis_buf(const uint8_t *cmd, int cmdlen)
1078 {
1079         /* cmd is a usb packet to go to the cmsis-dap interface */
1080         printf("cmsis-dap buffer (%d b): ", cmdlen);
1081         for (int i = 0; i < cmdlen; ++i)
1082                 printf(" %02x", cmd[i]);
1083         printf("\n");
1084         switch (cmd[1]) {
1085                 case CMD_DAP_JTAG_SEQ: {
1086                         printf("cmsis-dap jtag sequence command %02x (n=%d)\n", cmd[1], cmd[2]);
1087                         /*
1088                          * #2 = number of sequences
1089                          * #3 = sequence info 1
1090                          * #4...4+n_bytes-1 = sequence 1
1091                          * #4+n_bytes = sequence info 2
1092                          * #5+n_bytes = sequence 2 (single bit)
1093                          */
1094                         int pos = 3;
1095                         for (int seq = 0; seq < cmd[2]; ++seq) {
1096                                 uint8_t info = cmd[pos++];
1097                                 int len = info & DAP_JTAG_SEQ_TCK;
1098                                 if (len == 0)
1099                                         len = 64;
1100                                 printf("  sequence %d starting %d: info %02x (len=%d tms=%d read_tdo=%d): ",
1101                                         seq, pos, info, len, info & DAP_JTAG_SEQ_TMS, info & DAP_JTAG_SEQ_TDO);
1102                                 for (int i = 0; i < DIV_ROUND_UP(len, 8); ++i)
1103                                         printf(" %02x", cmd[pos+i]);
1104                                 pos += DIV_ROUND_UP(len, 8);
1105                                 printf("\n");
1106                         }
1107                         if (pos != cmdlen) {
1108                                 printf("BUFFER LENGTH MISMATCH looks like %d but %d specified", pos, cmdlen);
1109                                 exit(-1);
1110                         }
1111
1112                         break;
1113                 }
1114                 default:
1115                         LOG_DEBUG("unknown cmsis-dap command %02x", cmd[1]);
1116                         break;
1117         }
1118 }
1119 #endif
1120
1121 static void cmsis_dap_flush(void)
1122 {
1123         if (!queued_seq_count)
1124                 return;
1125
1126         DEBUG_JTAG_IO("Flushing %d queued sequences (%d bytes) with %d pending scan results to capture",
1127                 queued_seq_count, queued_seq_buf_end, pending_scan_result_count);
1128
1129         /* prep CMSIS-DAP packet */
1130         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
1131         buffer[0] = 0;  /* report number */
1132         buffer[1] = CMD_DAP_JTAG_SEQ;
1133         buffer[2] = queued_seq_count;
1134         memcpy(buffer + 3, queued_seq_buf, queued_seq_buf_end);
1135
1136 #ifdef CMSIS_DAP_JTAG_DEBUG
1137         debug_parse_cmsis_buf(buffer, queued_seq_buf_end + 3);
1138 #endif
1139
1140         /* send command to USB device */
1141         int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, queued_seq_buf_end + 3);
1142         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
1143                 LOG_ERROR("CMSIS-DAP command CMD_DAP_JTAG_SEQ failed.");
1144                 exit(-1);
1145         }
1146
1147 #ifdef CMSIS_DAP_JTAG_DEBUG
1148         DEBUG_JTAG_IO("USB response buf:");
1149         for (int c = 0; c < queued_seq_buf_end + 3; ++c)
1150                 printf("%02X ", buffer[c]);
1151         printf("\n");
1152 #endif
1153
1154         /* copy scan results into client buffers */
1155         for (int i = 0; i < pending_scan_result_count; ++i) {
1156                 struct pending_scan_result *scan = &pending_scan_results[i];
1157                 DEBUG_JTAG_IO("Copying pending_scan_result %d/%d: %d bits from byte %d -> buffer + %d bits",
1158                         i, pending_scan_result_count, scan->length, scan->first + 2, scan->buffer_offset);
1159 #ifdef CMSIS_DAP_JTAG_DEBUG
1160                 for (uint32_t b = 0; b < DIV_ROUND_UP(scan->length, 8); ++b)
1161                         printf("%02X ", buffer[2+scan->first+b]);
1162                 printf("\n");
1163 #endif
1164                 bit_copy(scan->buffer, scan->buffer_offset, buffer + 2 + scan->first, 0, scan->length);
1165         }
1166
1167         /* reset */
1168         queued_seq_count = 0;
1169         queued_seq_buf_end = 0;
1170         queued_seq_tdo_ptr = 0;
1171         pending_scan_result_count = 0;
1172 }
1173
1174 /* queue a sequence of bits to clock out TDI / in TDO, executing if the buffer is full.
1175  *
1176  * sequence=NULL means clock out zeros on TDI
1177  * tdo_buffer=NULL means don't capture TDO
1178  */
1179 static void cmsis_dap_add_jtag_sequence(int s_len, const uint8_t *sequence, int s_offset,
1180                                         bool tms, uint8_t *tdo_buffer, int tdo_buffer_offset)
1181 {
1182         DEBUG_JTAG_IO("[at %d] %d bits, tms %s, seq offset %d, tdo buf %p, tdo offset %d",
1183                 queued_seq_buf_end,
1184                 s_len, tms ? "HIGH" : "LOW", s_offset, tdo_buffer, tdo_buffer_offset);
1185
1186         if (s_len == 0)
1187                 return;
1188
1189         if (s_len > 64) {
1190                 DEBUG_JTAG_IO("START JTAG SEQ SPLIT");
1191                 for (int offset = 0; offset < s_len; offset += 64) {
1192                         int len = s_len - offset;
1193                         if (len > 64)
1194                                 len = 64;
1195                         DEBUG_JTAG_IO("Splitting long jtag sequence: %d-bit chunk starting at offset %d", len, offset);
1196                         cmsis_dap_add_jtag_sequence(
1197                                 len,
1198                                 sequence,
1199                                 s_offset + offset,
1200                                 tms,
1201                                 tdo_buffer,
1202                                 tdo_buffer == NULL ? 0 : (tdo_buffer_offset + offset)
1203                                 );
1204                 }
1205                 DEBUG_JTAG_IO("END JTAG SEQ SPLIT");
1206                 return;
1207         }
1208
1209         int cmd_len = 1 + DIV_ROUND_UP(s_len, 8);
1210         if (queued_seq_count >= 255 || queued_seq_buf_end + cmd_len > QUEUED_SEQ_BUF_LEN)
1211                 /* empty out the buffer */
1212                 cmsis_dap_flush();
1213
1214         ++queued_seq_count;
1215
1216         /* control byte */
1217         queued_seq_buf[queued_seq_buf_end] =
1218                 (tms ? DAP_JTAG_SEQ_TMS : 0) |
1219                 (tdo_buffer != NULL ? DAP_JTAG_SEQ_TDO : 0) |
1220                 (s_len == 64 ? 0 : s_len);
1221
1222         if (sequence != NULL)
1223                 bit_copy(&queued_seq_buf[queued_seq_buf_end + 1], 0, sequence, s_offset, s_len);
1224         else
1225                 memset(&queued_seq_buf[queued_seq_buf_end + 1], 0, DIV_ROUND_UP(s_len, 8));
1226
1227         queued_seq_buf_end += cmd_len;
1228
1229         if (tdo_buffer != NULL) {
1230                 struct pending_scan_result *scan = &pending_scan_results[pending_scan_result_count++];
1231                 scan->first = queued_seq_tdo_ptr;
1232                 queued_seq_tdo_ptr += DIV_ROUND_UP(s_len, 8);
1233                 scan->length = s_len;
1234                 scan->buffer = tdo_buffer;
1235                 scan->buffer_offset = tdo_buffer_offset;
1236         }
1237 }
1238
1239 /* queue a sequence of bits to clock out TMS, executing if the buffer is full */
1240 static void cmsis_dap_add_tms_sequence(const uint8_t *sequence, int s_len)
1241 {
1242         DEBUG_JTAG_IO("%d bits: %02X", s_len, *sequence);
1243         /* we use a series of CMD_DAP_JTAG_SEQ commands to toggle TMS,
1244            because even though it seems ridiculously inefficient, it
1245            allows us to combine TMS and scan sequences into the same
1246            USB packet. */
1247         /* TODO: combine runs of the same tms value */
1248         for (int i = 0; i < s_len; ++i) {
1249                 bool bit = (sequence[i / 8] & (1 << (i % 8))) != 0;
1250                 cmsis_dap_add_jtag_sequence(1, NULL, 0, bit, NULL, 0);
1251         }
1252 }
1253
1254 /* Move to the end state by queuing a sequence to clock into TMS */
1255 static void cmsis_dap_state_move(void)
1256 {
1257         uint8_t tms_scan;
1258         uint8_t tms_scan_bits;
1259
1260         tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
1261         tms_scan_bits = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
1262
1263         DEBUG_JTAG_IO("state move from %s to %s: %d clocks, %02X on tms",
1264                 tap_state_name(tap_get_state()), tap_state_name(tap_get_end_state()),
1265                 tms_scan_bits, tms_scan);
1266         cmsis_dap_add_tms_sequence(&tms_scan, tms_scan_bits);
1267
1268         tap_set_state(tap_get_end_state());
1269 }
1270
1271
1272 /* Execute a JTAG scan operation by queueing TMS and TDI/TDO sequences */
1273 static void cmsis_dap_execute_scan(struct jtag_command *cmd)
1274 {
1275         DEBUG_JTAG_IO("%s type:%d", cmd->cmd.scan->ir_scan ? "IRSCAN" : "DRSCAN",
1276                 jtag_scan_type(cmd->cmd.scan));
1277
1278         /* Make sure there are no trailing fields with num_bits == 0, or the logic below will fail. */
1279         while (cmd->cmd.scan->num_fields > 0
1280                         && cmd->cmd.scan->fields[cmd->cmd.scan->num_fields - 1].num_bits == 0) {
1281                 cmd->cmd.scan->num_fields--;
1282                 LOG_DEBUG("discarding trailing empty field");
1283         }
1284
1285         if (cmd->cmd.scan->num_fields == 0) {
1286                 LOG_DEBUG("empty scan, doing nothing");
1287                 return;
1288         }
1289
1290         if (cmd->cmd.scan->ir_scan) {
1291                 if (tap_get_state() != TAP_IRSHIFT) {
1292                         cmsis_dap_end_state(TAP_IRSHIFT);
1293                         cmsis_dap_state_move();
1294                 }
1295         } else {
1296                 if (tap_get_state() != TAP_DRSHIFT) {
1297                         cmsis_dap_end_state(TAP_DRSHIFT);
1298                         cmsis_dap_state_move();
1299                 }
1300         }
1301
1302         cmsis_dap_end_state(cmd->cmd.scan->end_state);
1303
1304         struct scan_field *field = cmd->cmd.scan->fields;
1305         unsigned scan_size = 0;
1306
1307         for (int i = 0; i < cmd->cmd.scan->num_fields; i++, field++) {
1308                 scan_size += field->num_bits;
1309                 DEBUG_JTAG_IO("%s%s field %d/%d %d bits",
1310                         field->in_value ? "in" : "",
1311                         field->out_value ? "out" : "",
1312                         i,
1313                         cmd->cmd.scan->num_fields,
1314                         field->num_bits);
1315
1316                 if (i == cmd->cmd.scan->num_fields - 1 && tap_get_state() != tap_get_end_state()) {
1317                         DEBUG_JTAG_IO("Last field and have to move out of SHIFT state");
1318                         /* Last field, and we're leaving IRSHIFT/DRSHIFT. Clock last bit during tap
1319                          * movement. This last field can't have length zero, it was checked above. */
1320                         cmsis_dap_add_jtag_sequence(
1321                                 field->num_bits - 1, /* number of bits to clock */
1322                                 field->out_value, /* output sequence */
1323                                 0, /* output offset */
1324                                 false, /* TMS low */
1325                                 field->in_value,
1326                                 0);
1327
1328                         /* Clock the last bit out, with TMS high */
1329                         uint8_t last_bit = 0;
1330                         if (field->out_value)
1331                                 bit_copy(&last_bit, 0, field->out_value, field->num_bits - 1, 1);
1332                         cmsis_dap_add_jtag_sequence(
1333                                 1,
1334                                 &last_bit,
1335                                 0,
1336                                 true,
1337                                 field->in_value,
1338                                 field->num_bits - 1);
1339                         tap_set_state(tap_state_transition(tap_get_state(), 1));
1340
1341                         /* Now clock one more cycle, with TMS low, to get us into a PAUSE state */
1342                         cmsis_dap_add_jtag_sequence(
1343                                 1,
1344                                 &last_bit,
1345                                 0,
1346                                 false,
1347                                 NULL,
1348                                 0);
1349                         tap_set_state(tap_state_transition(tap_get_state(), 0));
1350                 } else {
1351                         DEBUG_JTAG_IO("Internal field, staying in SHIFT state afterwards");
1352                         /* Clocking part of a sequence into DR or IR with TMS=0,
1353                            leaving TMS=0 at the end so we can continue later */
1354                         cmsis_dap_add_jtag_sequence(
1355                                 field->num_bits,
1356                                 field->out_value,
1357                                 0,
1358                                 false,
1359                                 field->in_value,
1360                                 0);
1361                 }
1362         }
1363
1364         if (tap_get_state() != tap_get_end_state()) {
1365                 cmsis_dap_end_state(tap_get_end_state());
1366                 cmsis_dap_state_move();
1367         }
1368
1369         DEBUG_JTAG_IO("%s scan, %i bits, end in %s",
1370                 (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
1371                 tap_state_name(tap_get_end_state()));
1372 }
1373
1374 static void cmsis_dap_pathmove(int num_states, tap_state_t *path)
1375 {
1376         int i;
1377         uint8_t tms0 = 0x00;
1378         uint8_t tms1 = 0xff;
1379
1380         for (i = 0; i < num_states; i++) {
1381                 if (path[i] == tap_state_transition(tap_get_state(), false))
1382                         cmsis_dap_add_tms_sequence(&tms0, 1);
1383                 else if (path[i] == tap_state_transition(tap_get_state(), true))
1384                         cmsis_dap_add_tms_sequence(&tms1, 1);
1385                 else {
1386                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition.",
1387                                   tap_state_name(tap_get_state()), tap_state_name(path[i]));
1388                         exit(-1);
1389                 }
1390
1391                 tap_set_state(path[i]);
1392         }
1393
1394         cmsis_dap_end_state(tap_get_state());
1395 }
1396
1397 static void cmsis_dap_execute_pathmove(struct jtag_command *cmd)
1398 {
1399         DEBUG_JTAG_IO("pathmove: %i states, end in %i",
1400                       cmd->cmd.pathmove->num_states,
1401                cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1402
1403         cmsis_dap_pathmove(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
1404 }
1405
1406 static void cmsis_dap_stableclocks(int num_cycles)
1407 {
1408         int i;
1409
1410         uint8_t tms = tap_get_state() == TAP_RESET;
1411         /* TODO: Perform optimizations? */
1412         /* Execute num_cycles. */
1413         for (i = 0; i < num_cycles; i++)
1414                 cmsis_dap_add_tms_sequence(&tms, 1);
1415 }
1416
1417 static void cmsis_dap_runtest(int num_cycles)
1418 {
1419         tap_state_t saved_end_state = tap_get_end_state();
1420
1421         /* Only do a state_move when we're not already in IDLE. */
1422         if (tap_get_state() != TAP_IDLE) {
1423                 cmsis_dap_end_state(TAP_IDLE);
1424                 cmsis_dap_state_move();
1425         }
1426         cmsis_dap_stableclocks(num_cycles);
1427
1428         /* Finish in end_state. */
1429         cmsis_dap_end_state(saved_end_state);
1430
1431         if (tap_get_state() != tap_get_end_state())
1432                 cmsis_dap_state_move();
1433 }
1434
1435 static void cmsis_dap_execute_runtest(struct jtag_command *cmd)
1436 {
1437         DEBUG_JTAG_IO("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles,
1438                       cmd->cmd.runtest->end_state);
1439
1440         cmsis_dap_end_state(cmd->cmd.runtest->end_state);
1441         cmsis_dap_runtest(cmd->cmd.runtest->num_cycles);
1442 }
1443
1444 static void cmsis_dap_execute_stableclocks(struct jtag_command *cmd)
1445 {
1446         DEBUG_JTAG_IO("stableclocks %i cycles", cmd->cmd.runtest->num_cycles);
1447         cmsis_dap_stableclocks(cmd->cmd.runtest->num_cycles);
1448 }
1449
1450 /* TODO: Is there need to call cmsis_dap_flush() for the JTAG_PATHMOVE,
1451  * JTAG_RUNTEST, JTAG_STABLECLOCKS? */
1452 static void cmsis_dap_execute_command(struct jtag_command *cmd)
1453 {
1454         switch (cmd->type) {
1455                 case JTAG_RESET:
1456                         cmsis_dap_flush();
1457                         cmsis_dap_execute_reset(cmd);
1458                         break;
1459                 case JTAG_SLEEP:
1460                         cmsis_dap_flush();
1461                         cmsis_dap_execute_sleep(cmd);
1462                         break;
1463                 case JTAG_TLR_RESET:
1464                         cmsis_dap_flush();
1465                         cmsis_dap_execute_tlr_reset(cmd);
1466                         break;
1467                 case JTAG_SCAN:
1468                         cmsis_dap_execute_scan(cmd);
1469                         break;
1470                 case JTAG_PATHMOVE:
1471                         cmsis_dap_execute_pathmove(cmd);
1472                         break;
1473                 case JTAG_RUNTEST:
1474                         cmsis_dap_execute_runtest(cmd);
1475                         break;
1476                 case JTAG_STABLECLOCKS:
1477                         cmsis_dap_execute_stableclocks(cmd);
1478                         break;
1479                 case JTAG_TMS:
1480                 default:
1481                         LOG_ERROR("BUG: unknown JTAG command type 0x%X encountered", cmd->type);
1482                         exit(-1);
1483         }
1484 }
1485
1486 static int cmsis_dap_execute_queue(void)
1487 {
1488         struct jtag_command *cmd = jtag_command_queue;
1489
1490         while (cmd != NULL) {
1491                 cmsis_dap_execute_command(cmd);
1492                 cmd = cmd->next;
1493         }
1494
1495         cmsis_dap_flush();
1496
1497         return ERROR_OK;
1498 }
1499
1500 static int cmsis_dap_speed(int speed)
1501 {
1502         if (speed > DAP_MAX_CLOCK) {
1503                 LOG_INFO("reduce speed request: %dkHz to %dkHz maximum", speed, DAP_MAX_CLOCK);
1504                 speed = DAP_MAX_CLOCK;
1505         }
1506
1507         if (speed == 0) {
1508                 LOG_INFO("RTCK not supported");
1509                 return ERROR_JTAG_NOT_IMPLEMENTED;
1510         }
1511
1512         return cmsis_dap_cmd_DAP_SWJ_Clock(speed);
1513 }
1514
1515 static int cmsis_dap_speed_div(int speed, int *khz)
1516 {
1517         *khz = speed;
1518         return ERROR_OK;
1519 }
1520
1521 static int cmsis_dap_khz(int khz, int *jtag_speed)
1522 {
1523         *jtag_speed = khz;
1524         return ERROR_OK;
1525 }
1526
1527 static int_least32_t cmsis_dap_swd_frequency(int_least32_t hz)
1528 {
1529         if (hz > 0)
1530                 cmsis_dap_speed(hz / 1000);
1531
1532         return hz;
1533 }
1534
1535
1536 COMMAND_HANDLER(cmsis_dap_handle_info_command)
1537 {
1538         if (cmsis_dap_get_version_info() == ERROR_OK)
1539                 cmsis_dap_get_status();
1540
1541         return ERROR_OK;
1542 }
1543
1544 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
1545 {
1546         if (CMD_ARGC > MAX_USB_IDS * 2) {
1547                 LOG_WARNING("ignoring extra IDs in cmsis_dap_vid_pid "
1548                         "(maximum is %d pairs)", MAX_USB_IDS);
1549                 CMD_ARGC = MAX_USB_IDS * 2;
1550         }
1551         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
1552                 LOG_WARNING("incomplete cmsis_dap_vid_pid configuration directive");
1553                 if (CMD_ARGC < 2)
1554                         return ERROR_COMMAND_SYNTAX_ERROR;
1555                 /* remove the incomplete trailing id */
1556                 CMD_ARGC -= 1;
1557         }
1558
1559         unsigned i;
1560         for (i = 0; i < CMD_ARGC; i += 2) {
1561                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], cmsis_dap_vid[i >> 1]);
1562                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], cmsis_dap_pid[i >> 1]);
1563         }
1564
1565         /*
1566          * Explicitly terminate, in case there are multiples instances of
1567          * cmsis_dap_vid_pid.
1568          */
1569         cmsis_dap_vid[i >> 1] = cmsis_dap_pid[i >> 1] = 0;
1570
1571         return ERROR_OK;
1572 }
1573
1574 COMMAND_HANDLER(cmsis_dap_handle_serial_command)
1575 {
1576         if (CMD_ARGC == 1) {
1577                 size_t len = mbstowcs(NULL, CMD_ARGV[0], 0);
1578                 cmsis_dap_serial = calloc(len + 1, sizeof(wchar_t));
1579                 if (cmsis_dap_serial == NULL) {
1580                         LOG_ERROR("unable to allocate memory");
1581                         return ERROR_OK;
1582                 }
1583                 if (mbstowcs(cmsis_dap_serial, CMD_ARGV[0], len + 1) == (size_t)-1) {
1584                         free(cmsis_dap_serial);
1585                         cmsis_dap_serial = NULL;
1586                         LOG_ERROR("unable to convert serial");
1587                 }
1588         } else {
1589                 LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
1590         }
1591
1592         return ERROR_OK;
1593 }
1594
1595 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
1596         {
1597                 .name = "info",
1598                 .handler = &cmsis_dap_handle_info_command,
1599                 .mode = COMMAND_EXEC,
1600                 .usage = "",
1601                 .help = "show cmsis-dap info",
1602         },
1603         COMMAND_REGISTRATION_DONE
1604 };
1605
1606 static const struct command_registration cmsis_dap_command_handlers[] = {
1607         {
1608                 .name = "cmsis-dap",
1609                 .mode = COMMAND_ANY,
1610                 .help = "perform CMSIS-DAP management",
1611                 .usage = "<cmd>",
1612                 .chain = cmsis_dap_subcommand_handlers,
1613         },
1614         {
1615                 .name = "cmsis_dap_vid_pid",
1616                 .handler = &cmsis_dap_handle_vid_pid_command,
1617                 .mode = COMMAND_CONFIG,
1618                 .help = "the vendor ID and product ID of the CMSIS-DAP device",
1619                 .usage = "(vid pid)* ",
1620         },
1621         {
1622                 .name = "cmsis_dap_serial",
1623                 .handler = &cmsis_dap_handle_serial_command,
1624                 .mode = COMMAND_CONFIG,
1625                 .help = "set the serial number of the adapter",
1626                 .usage = "serial_string",
1627         },
1628         COMMAND_REGISTRATION_DONE
1629 };
1630
1631 static const struct swd_driver cmsis_dap_swd_driver = {
1632         .init = cmsis_dap_swd_init,
1633         .frequency = cmsis_dap_swd_frequency,
1634         .switch_seq = cmsis_dap_swd_switch_seq,
1635         .read_reg = cmsis_dap_swd_read_reg,
1636         .write_reg = cmsis_dap_swd_write_reg,
1637         .run = cmsis_dap_swd_run_queue,
1638 };
1639
1640 static const char * const cmsis_dap_transport[] = { "swd", "jtag", NULL };
1641
1642 struct jtag_interface cmsis_dap_interface = {
1643         .name = "cmsis-dap",
1644         .commands = cmsis_dap_command_handlers,
1645         .swd = &cmsis_dap_swd_driver,
1646         .transports = cmsis_dap_transport,
1647
1648         .execute_queue = cmsis_dap_execute_queue,
1649         .speed = cmsis_dap_speed,
1650         .speed_div = cmsis_dap_speed_div,
1651         .khz = cmsis_dap_khz,
1652         .init = cmsis_dap_init,
1653         .quit = cmsis_dap_quit,
1654 };