git.gag.com Git - fw/openocd/blob - src/jtag/drivers/cmsis_dap_usb.c

   1 /***************************************************************************
   2  *   Copyright (C) 2013 by mike brown                                      *
   3  *   mike@theshedworks.org.uk                                              *
   4  *                                                                         *
   5  *   Copyright (C) 2013 by Spencer Oliver                                  *
   6  *   spen@spen-soft.co.uk                                                  *
   7  *                                                                         *
   8  *   This program is free software; you can redistribute it and/or modify  *
   9  *   it under the terms of the GNU General Public License as published by  *
  10  *   the Free Software Foundation; either version 2 of the License, or     *
  11  *   (at your option) any later version.                                   *
  12  *                                                                         *
  13  *   This program is distributed in the hope that it will be useful,       *
  14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  16  *   GNU General Public License for more details.                          *
  17  *                                                                         *
  18  *   You should have received a copy of the GNU General Public License     *
  19  *   along with this program; if not, write to the                         *
  20  *   Free Software Foundation, Inc.,                                       *
  21  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  22  ***************************************************************************/
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include "config.h"
  26 #endif
  27
  28 #include <transport/transport.h>
  29 #include <jtag/swd.h>
  30 #include <jtag/interface.h>
  31 #include <jtag/commands.h>
  32 #include <jtag/tcl.h>
  33
  34 #include <hidapi.h>
  35
  36 #ifdef _DEBUG_JTAG_IO_
  37  #define DEBUG_IO(expr...) LOG_DEBUG(expr)
  38 #else
  39  #define DEBUG_IO(expr...) do {} while (0)
  40 #endif
  41
  42 /*
  43  * See CMSIS-DAP documentation:
  44  * Version 0.01 - Beta.
  45  */
  46
  47 /* USB Config */
  48
  49 /* Known vid/pid pairs:
  50  * VID 0xc251: Keil Software
  51  * PID 0xf001: LPC-Link-II CMSIS_DAP
  52  * PID 0xf002: OPEN-SDA CMSIS_DAP (Freedom Board)
  53  * PID 0x2722: Keil ULINK2 CMSIS-DAP
  54  *
  55  * VID 0x0d28: mbed Software
  56  * PID 0x0204: MBED CMSIS-DAP
  57  */
  58
  59 #define MAX_USB_IDS 8
  60 /* vid = pid = 0 marks the end of the list */
  61 static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0 };
  62 static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
  63 static bool swd_mode;
  64
  65 #define PACKET_SIZE       (64 + 1)      /* 64 bytes plus report id */
  66 #define USB_TIMEOUT       1000
  67
  68 /* CMSIS-DAP General Commands */
  69 #define CMD_DAP_INFO              0x00
  70 #define CMD_DAP_LED               0x01
  71 #define CMD_DAP_CONNECT           0x02
  72 #define CMD_DAP_DISCONNECT        0x03
  73 #define CMD_DAP_WRITE_ABORT       0x08
  74 #define CMD_DAP_DELAY             0x09
  75 #define CMD_DAP_RESET_TARGET      0x0A
  76
  77 /* CMD_INFO */
  78 #define INFO_ID_VID               0x00      /* string */
  79 #define INFO_ID_PID               0x02      /* string */
  80 #define INFO_ID_SERNUM            0x03      /* string */
  81 #define INFO_ID_FW_VER            0x04      /* string */
  82 #define INFO_ID_TD_VEND           0x05      /* string */
  83 #define INFO_ID_TD_NAME           0x06      /* string */
  84 #define INFO_ID_CAPS              0xf0      /* byte */
  85 #define INFO_ID_PKT_CNT           0xfe      /* byte */
  86 #define INFO_ID_PKT_SZ            0xff      /* short */
  87
  88 #define INFO_CAPS_SWD             0x01
  89 #define INFO_CAPS_JTAG            0x02
  90
  91 /* CMD_LED */
  92 #define LED_ID_CONNECT            0x00
  93 #define LED_ID_RUN                0x01
  94
  95 #define LED_OFF                   0x00
  96 #define LED_ON                    0x01
  97
  98 /* CMD_CONNECT */
  99 #define CONNECT_DEFAULT           0x00
 100 #define CONNECT_SWD               0x01
 101 #define CONNECT_JTAG              0x02
 102
 103 /* CMSIS-DAP Common SWD/JTAG Commands */
 104 #define CMD_DAP_DELAY             0x09
 105 #define CMD_DAP_SWJ_PINS          0x10
 106 #define CMD_DAP_SWJ_CLOCK         0x11
 107 #define CMD_DAP_SWJ_SEQ           0x12
 108
 109 /*
 110  * PINS
 111  * Bit 0: SWCLK/TCK
 112  * Bit 1: SWDIO/TMS
 113  * Bit 2: TDI
 114  * Bit 3: TDO
 115  * Bit 5: nTRST
 116  * Bit 7: nRESET
 117  */
 118
 119 /* CMSIS-DAP SWD Commands */
 120 #define CMD_DAP_SWD_CONFIGURE     0x13
 121
 122 /* CMSIS-DAP JTAG Commands */
 123 #define CMD_DAP_JTAG_SEQ          0x14
 124 #define CMD_DAP_JTAG_CONFIGURE    0x15
 125 #define CMD_DAP_JTAG_IDCODE       0x16
 126
 127 /* CMSIS-DAP Transfer Commands */
 128 #define CMD_DAP_TFER_CONFIGURE    0x04
 129 #define CMD_DAP_TFER              0x05
 130 #define CMD_DAP_TFER_BLOCK        0x06
 131 #define CMD_DAP_TFER_ABORT        0x07
 132
 133 /* DAP Status Code */
 134 #define DAP_OK                    0
 135 #define DAP_ERROR                 0xFF
 136
 137 /* CMSIS-DAP Vendor Commands
 138  * None as yet... */
 139
 140 static const char * const info_caps_str[] = {
 141         "SWD  Supported",
 142         "JTAG Supported"
 143 };
 144
 145 /* max clock speed (kHz) */
 146 #define DAP_MAX_CLOCK             5000
 147
 148 struct cmsis_dap {
 149         hid_device *dev_handle;
 150         uint16_t packet_size;
 151         uint16_t packet_count;
 152         uint8_t *packet_buffer;
 153         uint8_t caps;
 154         uint8_t mode;
 155 };
 156
 157 static struct cmsis_dap *cmsis_dap_handle;
 158
 159 static int cmsis_dap_usb_open(void)
 160 {
 161         hid_device *dev = NULL;
 162         int i;
 163         struct hid_device_info *devs, *cur_dev;
 164         unsigned short target_vid, target_pid;
 165
 166         bool found = false;
 167
 168         target_vid = 0;
 169         target_pid = 0;
 170
 171         /*
 172          * The CMSIS-DAP specification stipulates:
 173          * "The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
 174          * debuggers to identify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
 175          */
 176         devs = hid_enumerate(0x0, 0x0);
 177         cur_dev = devs;
 178         while (NULL != cur_dev) {
 179                 if (0 == cmsis_dap_vid[0]) {
 180                         if (NULL == cur_dev->product_string) {
 181                                 LOG_DEBUG("Cannot read product string of device 0x%x:0x%x",
 182                                           cur_dev->vendor_id, cur_dev->product_id);
 183                         } else {
 184                                 if (wcsstr(cur_dev->product_string, L"CMSIS-DAP")) {
 185                                         /* if the user hasn't specified VID:PID *and*
 186                                          * product string contains "CMSIS-DAP", pick it
 187                                          */
 188                                         found = true;
 189                                 }
 190                         }
 191                 } else {
 192                         /* otherwise, exhaustively compare against all VID:PID in list */
 193                         for (i = 0; cmsis_dap_vid[i] || cmsis_dap_pid[i]; i++) {
 194                                 if ((cmsis_dap_vid[i] == cur_dev->vendor_id) && (cmsis_dap_pid[i] == cur_dev->product_id))
 195                                         found = true;
 196                         }
 197
 198                         if (cmsis_dap_vid[i] || cmsis_dap_pid[i])
 199                                 found = true;
 200                 }
 201
 202                 if (found) {
 203                         /* we have found an adapter, so exit further checks */
 204                         break;
 205                 }
 206
 207                 cur_dev = cur_dev->next;
 208         }
 209
 210         if (NULL != cur_dev) {
 211                 target_vid = cur_dev->vendor_id;
 212                 target_pid = cur_dev->product_id;
 213         }
 214
 215         hid_free_enumeration(devs);
 216
 217         if (target_vid == 0 && target_pid == 0) {
 218                 LOG_ERROR("unable to find CMSIS-DAP device");
 219                 return ERROR_FAIL;
 220         }
 221
 222         if (hid_init() != 0) {
 223                 LOG_ERROR("unable to open HIDAPI");
 224                 return ERROR_FAIL;
 225         }
 226
 227         dev = hid_open(target_vid, target_pid, NULL);
 228
 229         if (dev == NULL) {
 230                 LOG_ERROR("unable to open CMSIS-DAP device");
 231                 return ERROR_FAIL;
 232         }
 233
 234         struct cmsis_dap *dap = malloc(sizeof(struct cmsis_dap));
 235         if (dap == NULL) {
 236                 LOG_ERROR("unable to allocate memory");
 237                 return ERROR_FAIL;
 238         }
 239
 240         dap->dev_handle = dev;
 241         dap->caps = 0;
 242         dap->mode = 0;
 243
 244         cmsis_dap_handle = dap;
 245
 246         /* allocate default packet buffer, may be changed later.
 247          * currently with HIDAPI we have no way of getting the output report length
 248          * without this info we cannot communicate with the adapter.
 249          * For the moment we ahve to hard code the packet size */
 250
 251         int packet_size = PACKET_SIZE;
 252
 253         /* atmel cmsis-dap uses 512 byte reports */
 254         if (target_vid == 0x03eb)
 255                 packet_size = 512 + 1;
 256
 257         cmsis_dap_handle->packet_buffer = malloc(packet_size);
 258         cmsis_dap_handle->packet_size = packet_size;
 259
 260         if (cmsis_dap_handle->packet_buffer == NULL) {
 261                 LOG_ERROR("unable to allocate memory");
 262                 return ERROR_FAIL;
 263         }
 264
 265         return ERROR_OK;
 266 }
 267
 268 static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 269 {
 270         hid_close(dap->dev_handle);
 271         hid_exit();
 272
 273         if (cmsis_dap_handle->packet_buffer)
 274                 free(cmsis_dap_handle->packet_buffer);
 275
 276         if (cmsis_dap_handle) {
 277                 free(cmsis_dap_handle);
 278                 cmsis_dap_handle = NULL;
 279         }
 280
 281         return;
 282 }
 283
 284 /* Send a message and receive the reply */
 285 static int cmsis_dap_usb_xfer(struct cmsis_dap *dap, int txlen)
 286 {
 287         /* Pad the rest of the TX buffer with 0's */
 288         memset(dap->packet_buffer + txlen, 0, dap->packet_size - 1 - txlen);
 289
 290         /* write data to device */
 291         int retval = hid_write(dap->dev_handle, dap->packet_buffer, dap->packet_size);
 292         if (retval == -1) {
 293                 LOG_ERROR("error writing data: %ls", hid_error(dap->dev_handle));
 294                 return ERROR_FAIL;
 295         }
 296
 297         /* get reply */
 298         retval = hid_read_timeout(dap->dev_handle, dap->packet_buffer, dap->packet_size, USB_TIMEOUT);
 299         if (retval == -1 || retval == 0) {
 300                 LOG_DEBUG("error reading data: %ls", hid_error(dap->dev_handle));
 301                 return ERROR_FAIL;
 302         }
 303
 304         return ERROR_OK;
 305 }
 306
 307 static int cmsis_dap_cmd_DAP_SWJ_Pins(uint8_t pins, uint8_t mask, uint32_t delay, uint8_t *input)
 308 {
 309         int retval;
 310         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 311
 312         buffer[0] = 0;  /* report number */
 313         buffer[1] = CMD_DAP_SWJ_PINS;
 314         buffer[2] = pins;
 315         buffer[3] = mask;
 316         buffer[4] = delay & 0xff;
 317         buffer[5] = (delay >> 8) & 0xff;
 318         buffer[6] = (delay >> 16) & 0xff;
 319         buffer[7] = (delay >> 24) & 0xff;
 320         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 8);
 321
 322         if (retval != ERROR_OK) {
 323                 LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_PINS failed.");
 324                 return ERROR_JTAG_DEVICE_ERROR;
 325         }
 326
 327         if (input)
 328                 *input = buffer[1];
 329
 330         return ERROR_OK;
 331 }
 332
 333 static int cmsis_dap_cmd_DAP_SWJ_Clock(uint32_t swj_clock)
 334 {
 335         int retval;
 336         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 337
 338         /* set clock in Hz */
 339         swj_clock *= 1000;
 340         buffer[0] = 0;  /* report number */
 341         buffer[1] = CMD_DAP_SWJ_CLOCK;
 342         buffer[2] = swj_clock & 0xff;
 343         buffer[3] = (swj_clock >> 8) & 0xff;
 344         buffer[4] = (swj_clock >> 16) & 0xff;
 345         buffer[5] = (swj_clock >> 24) & 0xff;
 346         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 6);
 347
 348         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 349                 LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_CLOCK failed.");
 350                 return ERROR_JTAG_DEVICE_ERROR;
 351         }
 352
 353         return ERROR_OK;
 354 }
 355
 356 static int cmsis_dap_cmd_DAP_Info(uint8_t info, uint8_t **data)
 357 {
 358         int retval;
 359         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 360
 361         buffer[0] = 0;  /* report number */
 362         buffer[1] = CMD_DAP_INFO;
 363         buffer[2] = info;
 364         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
 365
 366         if (retval != ERROR_OK) {
 367                 LOG_ERROR("CMSIS-DAP command CMD_INFO failed.");
 368                 return ERROR_JTAG_DEVICE_ERROR;
 369         }
 370
 371         *data = &(buffer[1]);
 372
 373         return ERROR_OK;
 374 }
 375
 376 static int cmsis_dap_cmd_DAP_LED(uint8_t leds)
 377 {
 378         int retval;
 379         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 380
 381         buffer[0] = 0;  /* report number */
 382         buffer[1] = CMD_DAP_LED;
 383         buffer[2] = 0x00;
 384         buffer[3] = leds;
 385         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
 386
 387         if (retval != ERROR_OK || buffer[1] != 0x00) {
 388                 LOG_ERROR("CMSIS-DAP command CMD_LED failed.");
 389                 return ERROR_JTAG_DEVICE_ERROR;
 390         }
 391
 392         return ERROR_OK;
 393 }
 394
 395 static int cmsis_dap_cmd_DAP_Connect(uint8_t mode)
 396 {
 397         int retval;
 398         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 399
 400         buffer[0] = 0;  /* report number */
 401         buffer[1] = CMD_DAP_CONNECT;
 402         buffer[2] = mode;
 403         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
 404
 405         if (retval != ERROR_OK) {
 406                 LOG_ERROR("CMSIS-DAP command CMD_CONNECT failed.");
 407                 return ERROR_JTAG_DEVICE_ERROR;
 408         }
 409
 410         if (buffer[1] != mode) {
 411                 LOG_ERROR("CMSIS-DAP failed to connect in mode (%d)", mode);
 412                 return ERROR_JTAG_DEVICE_ERROR;
 413         }
 414
 415         return ERROR_OK;
 416 }
 417
 418 static int cmsis_dap_cmd_DAP_Disconnect(void)
 419 {
 420         int retval;
 421         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 422
 423         buffer[0] = 0;  /* report number */
 424         buffer[1] = CMD_DAP_DISCONNECT;
 425         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 2);
 426
 427         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 428                 LOG_ERROR("CMSIS-DAP command CMD_DISCONNECT failed.");
 429                 return ERROR_JTAG_DEVICE_ERROR;
 430         }
 431
 432         return ERROR_OK;
 433 }
 434
 435 static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t delay, uint16_t retry)
 436 {
 437         int retval;
 438         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 439
 440         buffer[0] = 0;  /* report number */
 441         buffer[1] = CMD_DAP_TFER_CONFIGURE;
 442         buffer[2] = idle;
 443         buffer[3] = delay & 0xff;
 444         buffer[4] = (delay >> 8) & 0xff;
 445         buffer[5] = retry & 0xff;
 446         buffer[6] = (retry >> 8) & 0xff;
 447         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
 448
 449         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 450                 LOG_ERROR("CMSIS-DAP command CMD_TFER_Configure failed.");
 451                 return ERROR_JTAG_DEVICE_ERROR;
 452         }
 453
 454         return ERROR_OK;
 455 }
 456
 457 static int cmsis_dap_cmd_DAP_SWD_Configure(uint8_t cfg)
 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_SWD_CONFIGURE;
 464         buffer[2] = cfg;
 465         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
 466
 467         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 468                 LOG_ERROR("CMSIS-DAP command CMD_SWD_Configure failed.");
 469                 return ERROR_JTAG_DEVICE_ERROR;
 470         }
 471
 472         return ERROR_OK;
 473 }
 474
 475 #if 0
 476 static int cmsis_dap_cmd_DAP_Delay(uint16_t delay_us)
 477 {
 478         int retval;
 479         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 480
 481         buffer[0] = 0;  /* report number */
 482         buffer[1] = CMD_DAP_DELAY;
 483         buffer[2] = delay_us & 0xff;
 484         buffer[3] = (delay_us >> 8) & 0xff;
 485         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
 486
 487         if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 488                 LOG_ERROR("CMSIS-DAP command CMD_Delay failed.");
 489                 return ERROR_JTAG_DEVICE_ERROR;
 490         }
 491
 492         return ERROR_OK;
 493 }
 494 #endif
 495
 496 static int queued_retval;
 497
 498 static void cmsis_dap_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t *value)
 499 {
 500         if (queued_retval != ERROR_OK)
 501                 return;
 502
 503         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 504         int retval;
 505         uint32_t val;
 506
 507         DEBUG_IO("CMSIS-DAP: Read  Reg 0x%02" PRIx8, cmd);
 508
 509         buffer[0] = 0;  /* report number */
 510         buffer[1] = CMD_DAP_TFER;
 511         buffer[2] = 0x00;
 512         buffer[3] = 0x01;
 513         buffer[4] = cmd;
 514         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
 515
 516         /* TODO - need better response checking */
 517         if (retval != ERROR_OK || buffer[1] != 0x01) {
 518                 LOG_ERROR("CMSIS-DAP: Read Error (0x%02" PRIx8 ")", buffer[2]);
 519                 queued_retval = buffer[2];
 520                 return;
 521         }
 522
 523         val = le_to_h_u32(&buffer[3]);
 524         DEBUG_IO("0x%08" PRIx32, val);
 525
 526         if (value)
 527                 *value = val;
 528
 529         queued_retval = retval;
 530 }
 531
 532 static void cmsis_dap_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t value)
 533 {
 534         if (queued_retval != ERROR_OK)
 535                 return;
 536
 537         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 538
 539         DEBUG_IO("CMSIS-DAP: Write Reg 0x%02" PRIx8 " 0x%08" PRIx32, cmd, value);
 540
 541         buffer[0] = 0;  /* report number */
 542         buffer[1] = CMD_DAP_TFER;
 543         buffer[2] = 0x00;
 544         buffer[3] = 0x01;
 545         buffer[4] = cmd;
 546         buffer[5] = (value) & 0xff;
 547         buffer[6] = (value >> 8) & 0xff;
 548         buffer[7] = (value >> 16) & 0xff;
 549         buffer[8] = (value >> 24) & 0xff;
 550         int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 9);
 551
 552         if (buffer[1] != 0x01) {
 553                 LOG_ERROR("CMSIS-DAP: Write Error (0x%02" PRIx8 ")", buffer[2]);
 554                 retval = buffer[2];
 555         }
 556
 557         queued_retval = retval;
 558 }
 559
 560 static int cmsis_dap_swd_run(struct adiv5_dap *dap)
 561 {
 562         int retval = queued_retval;
 563         queued_retval = ERROR_OK;
 564         return retval;
 565 }
 566
 567 static int cmsis_dap_get_version_info(void)
 568 {
 569         uint8_t *data;
 570
 571         /* INFO_ID_FW_VER - string */
 572         int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_FW_VER, &data);
 573         if (retval != ERROR_OK)
 574                 return retval;
 575
 576         if (data[0]) /* strlen */
 577                 LOG_INFO("CMSIS-DAP: FW Version = %s", &data[1]);
 578
 579         return ERROR_OK;
 580 }
 581
 582 static int cmsis_dap_get_caps_info(void)
 583 {
 584         uint8_t *data;
 585
 586         /* INFO_ID_CAPS - byte */
 587         int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_CAPS, &data);
 588         if (retval != ERROR_OK)
 589                 return retval;
 590
 591         if (data[0] == 1) {
 592                 uint8_t caps = data[1];
 593
 594                 cmsis_dap_handle->caps = caps;
 595
 596                 if (caps & INFO_CAPS_SWD)
 597                         LOG_INFO("CMSIS-DAP: %s", info_caps_str[0]);
 598                 if (caps & INFO_CAPS_JTAG)
 599                         LOG_INFO("CMSIS-DAP: %s", info_caps_str[1]);
 600         }
 601
 602         return ERROR_OK;
 603 }
 604
 605 static int cmsis_dap_get_status(void)
 606 {
 607         uint8_t d;
 608
 609         int retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, 0, 0, &d);
 610
 611         if (retval == ERROR_OK) {
 612                 LOG_INFO("SWCLK/TCK = %d SWDIO/TMS = %d TDI = %d TDO = %d nTRST = %d nRESET = %d",
 613                         (d & (0x01 << 0)) ? 1 : 0,      /* Bit 0: SWCLK/TCK */
 614                         (d & (0x01 << 1)) ? 1 : 0,      /* Bit 1: SWDIO/TMS */
 615                         (d & (0x01 << 2)) ? 1 : 0,      /* Bit 2: TDI */
 616                         (d & (0x01 << 3)) ? 1 : 0,      /* Bit 3: TDO */
 617                         (d & (0x01 << 5)) ? 1 : 0,      /* Bit 5: nTRST */
 618                         (d & (0x01 << 7)) ? 1 : 0);     /* Bit 7: nRESET */
 619         }
 620
 621         return retval;
 622 }
 623
 624 static int cmsis_dap_reset_link(void)
 625 {
 626         uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 627
 628         LOG_DEBUG("CMSIS-DAP: cmsis_dap_reset_link");
 629         LOG_INFO("DAP_SWJ Sequence (reset: 50+ '1' followed by 0)");
 630
 631         /* reset line with SWDIO high for >50 cycles */
 632         buffer[0] = 0;  /* report number */
 633         buffer[1] = CMD_DAP_SWJ_SEQ;
 634         buffer[2] = 7 * 8;
 635         buffer[3] = 0xff;
 636         buffer[4] = 0xff;
 637         buffer[5] = 0xff;
 638         buffer[6] = 0xff;
 639         buffer[7] = 0xff;
 640         buffer[8] = 0xff;
 641         buffer[9] = 0xff;
 642         int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 10);
 643
 644         if (retval != ERROR_OK || buffer[1] != DAP_OK)
 645                 return ERROR_FAIL;
 646
 647         /* 16bit JTAG-SWD sequence */
 648         buffer[0] = 0;  /* report number */
 649         buffer[1] = CMD_DAP_SWJ_SEQ;
 650         buffer[2] = 2 * 8;
 651         buffer[3] = 0x9e;
 652         buffer[4] = 0xe7;
 653         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
 654
 655         if (retval != ERROR_OK || buffer[1] != DAP_OK)
 656                 return ERROR_FAIL;
 657
 658         /* another reset just incase */
 659         buffer[0] = 0;  /* report number */
 660         buffer[1] = CMD_DAP_SWJ_SEQ;
 661         buffer[2] = 7 * 8;
 662         buffer[3] = 0xff;
 663         buffer[4] = 0xff;
 664         buffer[5] = 0xff;
 665         buffer[6] = 0xff;
 666         buffer[7] = 0xff;
 667         buffer[8] = 0xff;
 668         buffer[9] = 0xff;
 669         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 10);
 670
 671         if (retval != ERROR_OK || buffer[1] != DAP_OK)
 672                 return ERROR_FAIL;
 673
 674         /* 16 cycle idle period */
 675         buffer[0] = 0;  /* report number */
 676         buffer[1] = CMD_DAP_SWJ_SEQ;
 677         buffer[2] = 2 * 8;
 678         buffer[3] = 0x00;
 679         buffer[4] = 0x00;
 680         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
 681
 682         if (retval != ERROR_OK || buffer[1] != DAP_OK)
 683                 return ERROR_FAIL;
 684
 685         DEBUG_IO("DAP Read IDCODE");
 686
 687         /* read the id code is always the next sequence */
 688         buffer[0] = 0;  /* report number */
 689         buffer[1] = CMD_DAP_TFER;
 690         buffer[2] = 0x00;
 691         buffer[3] = 0x01;
 692         buffer[4] = 0x02;
 693         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
 694
 695         if (retval != ERROR_OK)
 696                 return retval;
 697
 698         if (buffer[1] == 0) {
 699                 LOG_DEBUG("Result 0x%02" PRIx8 " 0x%02" PRIx8, buffer[1], buffer[2]);
 700
 701                 LOG_DEBUG("DAP Reset Target");
 702                 buffer[0] = 0;  /* report number */
 703                 buffer[1] = CMD_DAP_RESET_TARGET;
 704                 retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 2);
 705                 LOG_DEBUG("Result 0x%02" PRIx8 " 0x%02" PRIx8, buffer[1], buffer[2]);
 706
 707                 LOG_DEBUG("DAP Write Abort");
 708                 buffer[0] = 0;  /* report number */
 709                 buffer[1] = CMD_DAP_WRITE_ABORT;
 710                 buffer[2] = 0x00;
 711                 buffer[3] = 0x1e/*0x1f*/;
 712                 buffer[4] = 0x00;
 713                 buffer[5] = 0x00;
 714                 buffer[6] = 0x00;
 715                 retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
 716                 LOG_DEBUG("Result 0x%02" PRIx8, buffer[1]);
 717
 718                 return 0x80 + buffer[1];
 719         }
 720
 721         LOG_DEBUG("DAP Write Abort");
 722         buffer[0] = 0;  /* report number */
 723         buffer[1] = CMD_DAP_WRITE_ABORT;
 724         buffer[2] = 0x00;
 725         buffer[3] = 0x1e;
 726         buffer[4] = 0x00;
 727         buffer[5] = 0x00;
 728         buffer[6] = 0x00;
 729         retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
 730         LOG_DEBUG("Result 0x%02" PRIx8, buffer[1]);
 731
 732         return retval;
 733 }
 734
 735 static int cmsis_dap_swd_open(void)
 736 {
 737         int retval;
 738
 739         DEBUG_IO("CMSIS-DAP: cmsis_dap_swd_open");
 740
 741         if (cmsis_dap_handle == NULL) {
 742
 743                 /* SWD init */
 744                 retval = cmsis_dap_usb_open();
 745                 if (retval != ERROR_OK)
 746                         return retval;
 747
 748                 retval = cmsis_dap_get_caps_info();
 749                 if (retval != ERROR_OK)
 750                         return retval;
 751         }
 752
 753         if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
 754                 LOG_ERROR("CMSIS-DAP: SWD not supported");
 755                 return ERROR_JTAG_DEVICE_ERROR;
 756         }
 757
 758         retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
 759         if (retval != ERROR_OK)
 760                 return retval;
 761
 762         /* Add more setup here.??... */
 763
 764         LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
 765         return ERROR_OK;
 766 }
 767
 768 static int cmsis_dap_init(void)
 769 {
 770         int retval;
 771         uint8_t *data;
 772
 773         if (swd_mode) {
 774                 retval = cmsis_dap_swd_open();
 775                 if (retval != ERROR_OK)
 776                         return retval;
 777         }
 778
 779         if (cmsis_dap_handle == NULL) {
 780
 781                 /* JTAG init */
 782                 retval = cmsis_dap_usb_open();
 783                 if (retval != ERROR_OK)
 784                         return retval;
 785
 786                 retval = cmsis_dap_get_caps_info();
 787                 if (retval != ERROR_OK)
 788                         return retval;
 789
 790                 /* Connect in JTAG mode */
 791                 if (!(cmsis_dap_handle->caps & INFO_CAPS_JTAG)) {
 792                         LOG_ERROR("CMSIS-DAP: JTAG not supported");
 793                         return ERROR_JTAG_DEVICE_ERROR;
 794                 }
 795
 796                 retval = cmsis_dap_cmd_DAP_Connect(CONNECT_JTAG);
 797                 if (retval != ERROR_OK)
 798                         return retval;
 799
 800                 LOG_INFO("CMSIS-DAP: Interface Initialised (JTAG)");
 801         }
 802
 803         retval = cmsis_dap_get_version_info();
 804         if (retval != ERROR_OK)
 805                 return retval;
 806
 807         /* INFO_ID_PKT_SZ - short */
 808         retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_SZ, &data);
 809         if (retval != ERROR_OK)
 810                 return retval;
 811
 812         if (data[0] == 2) {  /* short */
 813                 uint16_t pkt_sz = data[1] + (data[2] << 8);
 814
 815                 if (cmsis_dap_handle->packet_size != pkt_sz + 1) {
 816                         /* reallocate buffer */
 817                         cmsis_dap_handle->packet_size = pkt_sz + 1;
 818                         cmsis_dap_handle->packet_buffer = realloc(cmsis_dap_handle->packet_buffer,
 819                                         cmsis_dap_handle->packet_size);
 820                         if (cmsis_dap_handle->packet_buffer == NULL) {
 821                                 LOG_ERROR("unable to reallocate memory");
 822                                 return ERROR_FAIL;
 823                         }
 824                 }
 825
 826                 LOG_DEBUG("CMSIS-DAP: Packet Size = %" PRId16, pkt_sz);
 827         }
 828
 829         /* INFO_ID_PKT_CNT - byte */
 830         retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_CNT, &data);
 831         if (retval != ERROR_OK)
 832                 return retval;
 833
 834         if (data[0] == 1) { /* byte */
 835                 uint16_t pkt_cnt = data[1];
 836                 cmsis_dap_handle->packet_count = pkt_cnt;
 837                 LOG_DEBUG("CMSIS-DAP: Packet Count = %" PRId16, pkt_cnt);
 838         }
 839
 840         retval = cmsis_dap_get_status();
 841         if (retval != ERROR_OK)
 842                 return ERROR_FAIL;
 843
 844         /* Now try to connect to the target
 845          * TODO: This is all SWD only @ present */
 846         retval = cmsis_dap_cmd_DAP_SWJ_Clock(100);              /* 100kHz */
 847         if (retval != ERROR_OK)
 848                 return ERROR_FAIL;
 849
 850         retval = cmsis_dap_cmd_DAP_TFER_Configure(0, 64, 0);
 851         if (retval != ERROR_OK)
 852                 return ERROR_FAIL;
 853         retval = cmsis_dap_cmd_DAP_SWD_Configure(0x00);
 854         if (retval != ERROR_OK)
 855                 return ERROR_FAIL;
 856
 857         retval = cmsis_dap_cmd_DAP_LED(0x03);           /* Both LEDs on */
 858         if (retval != ERROR_OK)
 859                 return ERROR_FAIL;
 860
 861         /* support connecting with srst asserted */
 862         enum reset_types jtag_reset_config = jtag_get_reset_config();
 863
 864         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
 865                 if (jtag_reset_config & RESET_SRST_NO_GATING) {
 866                         retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, (1 << 7), 0, NULL);
 867                         if (retval != ERROR_OK)
 868                                 return ERROR_FAIL;
 869                         LOG_INFO("Connecting under reset");
 870                 }
 871         }
 872
 873         retval = cmsis_dap_reset_link();
 874         if (retval != ERROR_OK)
 875                 return ERROR_FAIL;
 876
 877         cmsis_dap_cmd_DAP_LED(0x00);                            /* Both LEDs off */
 878
 879         LOG_INFO("CMSIS-DAP: Interface ready");
 880
 881         return ERROR_OK;
 882 }
 883
 884 static int cmsis_dap_swd_init(void)
 885 {
 886         swd_mode = true;
 887         return ERROR_OK;
 888 }
 889
 890 static int cmsis_dap_quit(void)
 891 {
 892         cmsis_dap_cmd_DAP_Disconnect();
 893         cmsis_dap_cmd_DAP_LED(0x00);            /* Both LEDs off */
 894
 895         cmsis_dap_usb_close(cmsis_dap_handle);
 896
 897         return ERROR_OK;
 898 }
 899
 900 static void cmsis_dap_execute_reset(struct jtag_command *cmd)
 901 {
 902         int retval = cmsis_dap_cmd_DAP_SWJ_Pins(cmd->cmd.reset->srst ? 0 : (1 << 7), \
 903                         (1 << 7), 0, NULL);
 904         if (retval != ERROR_OK)
 905                 LOG_ERROR("CMSIS-DAP: Interface reset failed");
 906 }
 907
 908 static void cmsis_dap_execute_sleep(struct jtag_command *cmd)
 909 {
 910 #if 0
 911         int retval = cmsis_dap_cmd_DAP_Delay(cmd->cmd.sleep->us);
 912         if (retval != ERROR_OK)
 913 #endif
 914                 jtag_sleep(cmd->cmd.sleep->us);
 915 }
 916
 917 static void cmsis_dap_execute_command(struct jtag_command *cmd)
 918 {
 919         switch (cmd->type) {
 920                 case JTAG_RESET:
 921                         cmsis_dap_execute_reset(cmd);
 922                         break;
 923                 case JTAG_SLEEP:
 924                         cmsis_dap_execute_sleep(cmd);
 925                         break;
 926                 default:
 927                         LOG_ERROR("BUG: unknown JTAG command type encountered");
 928                         exit(-1);
 929         }
 930 }
 931
 932 static int cmsis_dap_execute_queue(void)
 933 {
 934         struct jtag_command *cmd = jtag_command_queue;
 935
 936         while (cmd != NULL) {
 937                 cmsis_dap_execute_command(cmd);
 938                 cmd = cmd->next;
 939         }
 940
 941         return ERROR_OK;
 942 }
 943
 944 static int cmsis_dap_speed(int speed)
 945 {
 946         if (speed > DAP_MAX_CLOCK) {
 947                 LOG_INFO("reduce speed request: %dkHz to %dkHz maximum", speed, DAP_MAX_CLOCK);
 948                 speed = DAP_MAX_CLOCK;
 949         }
 950
 951         if (speed == 0) {
 952                 LOG_INFO("RTCK not supported");
 953                 return ERROR_JTAG_NOT_IMPLEMENTED;
 954         }
 955
 956         return cmsis_dap_cmd_DAP_SWJ_Clock(speed);
 957 }
 958
 959 static int cmsis_dap_speed_div(int speed, int *khz)
 960 {
 961         *khz = speed;
 962         return ERROR_OK;
 963 }
 964
 965 static int cmsis_dap_khz(int khz, int *jtag_speed)
 966 {
 967         *jtag_speed = khz;
 968         return ERROR_OK;
 969 }
 970
 971 COMMAND_HANDLER(cmsis_dap_handle_info_command)
 972 {
 973         if (cmsis_dap_get_version_info() == ERROR_OK)
 974                 cmsis_dap_get_status();
 975
 976         return ERROR_OK;
 977 }
 978
 979 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 980 {
 981         if (CMD_ARGC > MAX_USB_IDS * 2) {
 982                 LOG_WARNING("ignoring extra IDs in cmsis_dap_vid_pid "
 983                         "(maximum is %d pairs)", MAX_USB_IDS);
 984                 CMD_ARGC = MAX_USB_IDS * 2;
 985         }
 986         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
 987                 LOG_WARNING("incomplete cmsis_dap_vid_pid configuration directive");
 988                 if (CMD_ARGC < 2)
 989                         return ERROR_COMMAND_SYNTAX_ERROR;
 990                 /* remove the incomplete trailing id */
 991                 CMD_ARGC -= 1;
 992         }
 993
 994         unsigned i;
 995         for (i = 0; i < CMD_ARGC; i += 2) {
 996                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], cmsis_dap_vid[i >> 1]);
 997                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], cmsis_dap_pid[i >> 1]);
 998         }
 999
1000         /*
1001          * Explicitly terminate, in case there are multiples instances of
1002          * cmsis_dap_vid_pid.
1003          */
1004         cmsis_dap_vid[i >> 1] = cmsis_dap_pid[i >> 1] = 0;
1005
1006         return ERROR_OK;
1007 }
1008
1009 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
1010         {
1011                 .name = "info",
1012                 .handler = &cmsis_dap_handle_info_command,
1013                 .mode = COMMAND_EXEC,
1014                 .usage = "",
1015                 .help = "show cmsis-dap info",
1016         },
1017         COMMAND_REGISTRATION_DONE
1018 };
1019
1020 static const struct command_registration cmsis_dap_command_handlers[] = {
1021         {
1022                 .name = "cmsis-dap",
1023                 .mode = COMMAND_ANY,
1024                 .help = "perform CMSIS-DAP management",
1025                 .usage = "<cmd>",
1026                 .chain = cmsis_dap_subcommand_handlers,
1027         },
1028         {
1029                 .name = "cmsis_dap_vid_pid",
1030                 .handler = &cmsis_dap_handle_vid_pid_command,
1031                 .mode = COMMAND_CONFIG,
1032                 .help = "the vendor ID and product ID of the CMSIS-DAP device",
1033                 .usage = "(vid pid)* ",
1034         },
1035         COMMAND_REGISTRATION_DONE
1036 };
1037
1038 static const struct swd_driver cmsis_dap_swd_driver = {
1039         .init = cmsis_dap_swd_init,
1040         .read_reg = cmsis_dap_swd_read_reg,
1041         .write_reg = cmsis_dap_swd_write_reg,
1042         .run = cmsis_dap_swd_run,
1043 };
1044
1045 const char *cmsis_dap_transport[] = {"cmsis-dap", NULL};
1046
1047 struct jtag_interface cmsis_dap_interface = {
1048         .name = "cmsis-dap",
1049         .commands = cmsis_dap_command_handlers,
1050         .swd = &cmsis_dap_swd_driver,
1051         .transports = cmsis_dap_transport,
1052
1053         .execute_queue = cmsis_dap_execute_queue,
1054         .speed = cmsis_dap_speed,
1055         .speed_div = cmsis_dap_speed_div,
1056         .khz = cmsis_dap_khz,
1057         .init = cmsis_dap_init,
1058         .quit = cmsis_dap_quit,
1059 };