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