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

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