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

   1 /***************************************************************************
   2  *   Copyright (C) 2017 by Texas Instruments, Inc.                         *
   3  *                                                                         *
   4  *   This program is free software; you can redistribute it and/or modify  *
   5  *   it under the terms of the GNU General Public License as published by  *
   6  *   the Free Software Foundation; either version 2 of the License, or     *
   7  *   (at your option) any later version.                                   *
   8  *                                                                         *
   9  *   This program is distributed in the hope that it will be useful,       *
  10  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  11  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  12  *   GNU General Public License for more details.                          *
  13  *                                                                         *
  14  *   You should have received a copy of the GNU General Public License     *
  15  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  16  ***************************************************************************/
  17
  18 #ifdef HAVE_CONFIG_H
  19 #include "config.h"
  20 #endif
  21
  22 #include <transport/transport.h>
  23 #include <jtag/swd.h>
  24 #include <jtag/interface.h>
  25 #include <jtag/commands.h>
  26 #include <jtag/tcl.h>
  27 #include <libusb.h>
  28
  29 /* XDS110 USB serial number length */
  30 #define XDS110_SERIAL_LEN 8
  31
  32 /* Firmware version that introduced OpenOCD support via block accesses */
  33 #define OCD_FIRMWARE_VERSION 0x02030011
  34 #define OCD_FIRMWARE_UPGRADE \
  35         "XDS110: upgrade to version 2.3.0.11+ for improved support"
  36
  37 /***************************************************************************
  38  *   USB Connection Buffer Definitions                                     *
  39  ***************************************************************************/
  40
  41 /* Max USB packet size for up to USB 3.0 */
  42 #define MAX_PACKET 1024
  43
  44 /*
  45  * Maximum data payload that can be handled in a single call
  46  * Limitation is the size of the buffers in the XDS110 firmware
  47  */
  48 #define MAX_DATA_BLOCK 4096
  49
  50 #ifndef USB_PAYLOAD_SIZE
  51 /* Largest data block plus parameters */
  52 #define USB_PAYLOAD_SIZE (MAX_DATA_BLOCK + 60)
  53 #endif
  54 #define MAX_RESULT_QUEUE (MAX_DATA_BLOCK / 4)
  55
  56 /***************************************************************************
  57  *   USB Connection Endpoints                                              *
  58  ***************************************************************************/
  59
  60 /* Bulk endpoints used by the XDS110 debug interface */
  61 #define INTERFACE_DEBUG (2)
  62 #define ENDPOINT_DEBUG_IN (3 | LIBUSB_ENDPOINT_IN)
  63 #define ENDPOINT_DEBUG_OUT (2 | LIBUSB_ENDPOINT_OUT)
  64
  65 /***************************************************************************
  66  *   XDS110 Firmware API Definitions                                       *
  67  ***************************************************************************/
  68
  69 /*
  70  * Default values controlling how the host communicates commands
  71  * with XDS110 firmware (automatic retry count and wait timeout)
  72  */
  73 #define DEFAULT_ATTEMPTS (1)
  74 #define DEFAULT_TIMEOUT  (4000)
  75
  76 /* XDS110 API error codes */
  77 #define SC_ERR_NONE             0
  78 #define SC_ERR_XDS110_FAIL   -261
  79 #define SC_ERR_SWD_WAIT      -613
  80 #define SC_ERR_SWD_FAULT     -614
  81 #define SC_ERR_SWD_PROTOCOL  -615
  82 #define SC_ERR_SWD_PARITY    -616
  83 #define SC_ERR_SWD_DEVICE_ID -617
  84
  85 /* TCK frequency limits */
  86 #define XDS110_MIN_TCK_SPEED  100 /* kHz */
  87 #define XDS110_MAX_TCK_SPEED 2500 /* kHz */
  88 #define XDS110_TCK_PULSE_INCREMENT 66.0
  89
  90 /* Scan mode on connect */
  91 #define MODE_JTAG 1
  92
  93 /* XDS110 API JTAG state definitions */
  94 #define XDS_JTAG_STATE_RESET       1
  95 #define XDS_JTAG_STATE_IDLE        2
  96 #define XDS_JTAG_STATE_SHIFT_DR    3
  97 #define XDS_JTAG_STATE_SHIFT_IR    4
  98 #define XDS_JTAG_STATE_PAUSE_DR    5
  99 #define XDS_JTAG_STATE_PAUSE_IR    6
 100 #define XDS_JTAG_STATE_EXIT1_DR    8
 101 #define XDS_JTAG_STATE_EXIT1_IR    9
 102 #define XDS_JTAG_STATE_EXIT2_DR   10
 103 #define XDS_JTAG_STATE_EXIT2_IR   11
 104 #define XDS_JTAG_STATE_SELECT_DR  12
 105 #define XDS_JTAG_STATE_SELECT_IR  13
 106 #define XDS_JTAG_STATE_UPDATE_DR  14
 107 #define XDS_JTAG_STATE_UPDATE_IR  15
 108 #define XDS_JTAG_STATE_CAPTURE_DR 16
 109 #define XDS_JTAG_STATE_CAPTURE_IR 17
 110
 111 /* XDS110 API JTAG transit definitions */
 112 #define XDS_JTAG_TRANSIT_QUICKEST    1
 113 #define XDS_JTAG_TRANSIT_VIA_CAPTURE 2
 114 #define XDS_JTAG_TRANSIT_VIA_IDLE    3
 115
 116 /* DAP register definitions as used by XDS110 APIs */
 117
 118 #define DAP_AP 0 /* DAP AP register type */
 119 #define DAP_DP 1 /* DAP DP register type */
 120
 121 #define DAP_DP_IDCODE 0x0 /* DAP DP IDCODE register (read only) */
 122 #define DAP_DP_ABORT  0x0 /* DAP DP ABORT register (write only) */
 123 #define DAP_DP_STAT   0x4 /* DAP DP STAT register (for read only) */
 124 #define DAP_DP_CTRL   0x4 /* DAP DP CTRL register (for write only) */
 125 #define DAP_DP_ADDR   0x8 /* DAP DP SELECT register (legacy name) */
 126 #define DAP_DP_RESEND 0x8 /* DAP DP RESEND register (read only) */
 127 #define DAP_DP_SELECT 0x8 /* DAP DP SELECT register (write only) */
 128 #define DAP_DP_RDBUFF 0xc /* DAP DP RDBUFF Read Buffer register */
 129
 130 #define DAP_AP_CSW  0x00 /* DAP AP Control Status Word */
 131 #define DAP_AP_TAR  0x04 /* DAP AP Transfer Address */
 132 #define DAP_AP_DRW  0x0C /* DAP AP Data Read/Write */
 133 #define DAP_AP_BD0  0x10 /* DAP AP Banked Data 0 */
 134 #define DAP_AP_BD1  0x14 /* DAP AP Banked Data 1 */
 135 #define DAP_AP_BD2  0x18 /* DAP AP Banked Data 2 */
 136 #define DAP_AP_BD3  0x1C /* DAP AP Banked Data 3 */
 137 #define DAP_AP_RTBL 0xF8 /* DAP AP Debug ROM Table */
 138 #define DAP_AP_IDR  0xFC /* DAP AP Identification Register */
 139
 140 /* Command packet definitions */
 141
 142 #define XDS_OUT_LEN 1 /* command (byte) */
 143 #define XDS_IN_LEN  4 /* error code (int) */
 144
 145 /* XDS API Commands */
 146 #define XDS_CONNECT      0x01 /* Connect JTAG connection */
 147 #define XDS_DISCONNECT   0x02 /* Disconnect JTAG connection */
 148 #define XDS_VERSION      0x03 /* Get firmware version and hardware ID */
 149 #define XDS_SET_TCK      0x04 /* Set TCK delay (to set TCK frequency) */
 150 #define XDS_SET_TRST     0x05 /* Assert or deassert nTRST signal */
 151 #define XDS_CYCLE_TCK    0x07 /* Toggle TCK for a number of cycles */
 152 #define XDS_GOTO_STATE   0x09 /* Go to requested JTAG state */
 153 #define XDS_JTAG_SCAN    0x0c /* Send and receive JTAG scan */
 154 #define XDS_SET_SRST     0x0e /* Assert or deassert nSRST signal */
 155 #define CMAPI_CONNECT    0x0f /* CMAPI connect */
 156 #define CMAPI_DISCONNECT 0x10 /* CMAPI disconnect */
 157 #define CMAPI_ACQUIRE    0x11 /* CMAPI acquire */
 158 #define CMAPI_RELEASE    0x12 /* CMAPI release */
 159 #define CMAPI_REG_READ   0x15 /* CMAPI DAP register read */
 160 #define CMAPI_REG_WRITE  0x16 /* CMAPI DAP register write */
 161 #define SWD_CONNECT      0x17 /* Switch from JTAG to SWD connection */
 162 #define SWD_DISCONNECT   0x18 /* Switch from SWD to JTAG connection */
 163 #define CJTAG_CONNECT    0x2b /* Switch from JTAG to cJTAG connection */
 164 #define CJTAG_DISCONNECT 0x2c /* Switch from cJTAG to JTAG connection */
 165 #define OCD_DAP_REQUEST  0x3a /* Handle block of DAP requests */
 166 #define OCD_SCAN_REQUEST 0x3b /* Handle block of JTAG scan requests */
 167 #define OCD_PATHMOVE     0x3c /* Handle PATHMOVE to navigate JTAG states */
 168
 169 #define CMD_IR_SCAN      1
 170 #define CMD_DR_SCAN      2
 171 #define CMD_RUNTEST      3
 172 #define CMD_STABLECLOCKS 4
 173
 174 /* Array to convert from OpenOCD tap_state_t to XDS JTAG state */
 175 const uint32_t xds_jtag_state[] = {
 176         XDS_JTAG_STATE_EXIT2_DR,   /* TAP_DREXIT2   = 0x0 */
 177         XDS_JTAG_STATE_EXIT1_DR,   /* TAP_DREXIT1   = 0x1 */
 178         XDS_JTAG_STATE_SHIFT_DR,   /* TAP_DRSHIFT   = 0x2 */
 179         XDS_JTAG_STATE_PAUSE_DR,   /* TAP_DRPAUSE   = 0x3 */
 180         XDS_JTAG_STATE_SELECT_IR,  /* TAP_IRSELECT  = 0x4 */
 181         XDS_JTAG_STATE_UPDATE_DR,  /* TAP_DRUPDATE  = 0x5 */
 182         XDS_JTAG_STATE_CAPTURE_DR, /* TAP_DRCAPTURE = 0x6 */
 183         XDS_JTAG_STATE_SELECT_DR,  /* TAP_DRSELECT  = 0x7 */
 184         XDS_JTAG_STATE_EXIT2_IR,   /* TAP_IREXIT2   = 0x8 */
 185         XDS_JTAG_STATE_EXIT1_IR,   /* TAP_IREXIT1   = 0x9 */
 186         XDS_JTAG_STATE_SHIFT_IR,   /* TAP_IRSHIFT   = 0xa */
 187         XDS_JTAG_STATE_PAUSE_IR,   /* TAP_IRPAUSE   = 0xb */
 188         XDS_JTAG_STATE_IDLE,       /* TAP_IDLE      = 0xc */
 189         XDS_JTAG_STATE_UPDATE_IR,  /* TAP_IRUPDATE  = 0xd */
 190         XDS_JTAG_STATE_CAPTURE_IR, /* TAP_IRCAPTURE = 0xe */
 191         XDS_JTAG_STATE_RESET,      /* TAP_RESET     = 0xf */
 192 };
 193
 194 struct scan_result {
 195         bool first;
 196         uint8_t *buffer;
 197         uint32_t num_bits;
 198 };
 199
 200 struct xds110_info {
 201         /* USB connection handles and data buffers */
 202         libusb_context *ctx;
 203         libusb_device_handle *dev;
 204         unsigned char read_payload[USB_PAYLOAD_SIZE];
 205         unsigned char write_packet[3];
 206         unsigned char write_payload[USB_PAYLOAD_SIZE];
 207         /* Status flags */
 208         bool is_connected;
 209         bool is_cmapi_connected;
 210         bool is_cmapi_acquired;
 211         bool is_swd_mode;
 212         bool is_ap_dirty;
 213         /* DAP register caches */
 214         uint32_t select;
 215         uint32_t rdbuff;
 216         bool use_rdbuff;
 217         /* TCK speed and delay count*/
 218         uint32_t speed;
 219         uint32_t delay_count;
 220         /* XDS110 serial number */
 221         char serial[XDS110_SERIAL_LEN + 1];
 222         /* XDS110 firmware and hardware version */
 223         uint32_t firmware;
 224         uint16_t hardware;
 225         /* Transaction queues */
 226         unsigned char txn_requests[MAX_DATA_BLOCK];
 227         uint32_t *txn_dap_results[MAX_DATA_BLOCK / 4];
 228         struct scan_result txn_scan_results[MAX_DATA_BLOCK / 4];
 229         uint32_t txn_request_size;
 230         uint32_t txn_result_size;
 231         uint32_t txn_result_count;
 232 };
 233
 234 static struct xds110_info xds110 = {
 235         .ctx = NULL,
 236         .dev = NULL,
 237         .is_connected = false,
 238         .is_cmapi_connected = false,
 239         .is_cmapi_acquired = false,
 240         .is_swd_mode = false,
 241         .is_ap_dirty = false,
 242         .speed = XDS110_MAX_TCK_SPEED,
 243         .delay_count = 0,
 244         .serial = {0},
 245         .firmware = 0,
 246         .hardware = 0,
 247         .txn_request_size = 0,
 248         .txn_result_size = 0,
 249         .txn_result_count = 0
 250 };
 251
 252 static inline void xds110_set_u32(uint8_t *buffer, uint32_t value)
 253 {
 254         buffer[3] = (value >> 24) & 0xff;
 255         buffer[2] = (value >> 16) & 0xff;
 256         buffer[1] = (value >> 8) & 0xff;
 257         buffer[0] = (value >> 0) & 0xff;
 258 }
 259
 260 static inline void xds110_set_u16(uint8_t *buffer, uint16_t value)
 261 {
 262         buffer[1] = (value >> 8) & 0xff;
 263         buffer[0] = (value >> 0) & 0xff;
 264 }
 265
 266 static inline uint32_t xds110_get_u32(uint8_t *buffer)
 267 {
 268         uint32_t value = (((uint32_t)buffer[3]) << 24) |
 269                                          (((uint32_t)buffer[2]) << 16) |
 270                                          (((uint32_t)buffer[1]) << 8)  |
 271                                          (((uint32_t)buffer[0]) << 0);
 272         return value;
 273 }
 274
 275 static inline uint16_t xds110_get_u16(uint8_t *buffer)
 276 {
 277         uint16_t value = (((uint32_t)buffer[1]) << 8) |
 278                                          (((uint32_t)buffer[0]) << 0);
 279         return value;
 280 }
 281
 282 /***************************************************************************
 283  *   usb connection routines                                               *
 284  *                                                                         *
 285  *   The following functions handle connecting, reading, and writing to    *
 286  *   the XDS110 over USB using the libusb library.                         *
 287  ***************************************************************************/
 288
 289 static bool usb_connect(void)
 290 {
 291         libusb_context *ctx  = NULL;
 292         libusb_device **list = NULL;
 293         libusb_device_handle *dev  = NULL;
 294
 295         struct libusb_device_descriptor desc;
 296
 297         uint16_t vid = 0x0451;
 298         uint16_t pid = 0xbef3;
 299         ssize_t count = 0;
 300         ssize_t i = 0;
 301         int result = 0;
 302         bool found = false;
 303
 304         /* Initialize libusb context */
 305         result = libusb_init(&ctx);
 306
 307         if (0 == result) {
 308                 /* Get list of USB devices attached to system */
 309                 count = libusb_get_device_list(ctx, &list);
 310                 if (count <= 0) {
 311                         result = -1;
 312                         list = NULL;
 313                 }
 314         }
 315
 316         if (0 == result) {
 317                 /* Scan through list of devices for any XDS110s */
 318                 for (i = 0; i < count; i++) {
 319                         /* Check for device VID/PID match */
 320                         libusb_get_device_descriptor(list[i], &desc);
 321                         if (desc.idVendor == vid && desc.idProduct == pid) {
 322                                 result = libusb_open(list[i], &dev);
 323                                 if (0 == result) {
 324                                         const int MAX_DATA = 256;
 325                                         unsigned char data[MAX_DATA + 1];
 326                                         *data = '\0';
 327
 328                                         /* May be the requested device if serial number matches */
 329                                         if (0 == xds110.serial[0]) {
 330                                                 /* No serial number given; match first XDS110 found */
 331                                                 found = true;
 332                                                 break;
 333                                         } else {
 334                                                 /* Get the device's serial number string */
 335                                                 result = libusb_get_string_descriptor_ascii(dev,
 336                                                                         desc.iSerialNumber, data, MAX_DATA);
 337                                                 if (0 < result &&
 338                                                         0 == strcmp((char *)data, (char *)xds110.serial)) {
 339                                                         found = true;
 340                                                         break;
 341                                                 }
 342                                         }
 343
 344                                         /* If we fall though to here, we don't want this device */
 345                                         libusb_close(dev);
 346                                         dev = NULL;
 347                                 }
 348                         }
 349                 }
 350         }
 351
 352         /*
 353          * We can fall through the for() loop with two possible exit conditions:
 354          * 1) found the right XDS110, and that device is open
 355          * 2) didn't find the XDS110, and no devices are currently open
 356          */
 357
 358         if (NULL != list) {
 359                 /* Free the device list, we're done with it */
 360                 libusb_free_device_list(list, 1);
 361         }
 362
 363         if (found) {
 364                 /* Save the context and device handles */
 365                 xds110.ctx = ctx;
 366                 xds110.dev = dev;
 367
 368                 /* Set libusb to auto detach kernel and disable debug messages */
 369                 (void)libusb_set_auto_detach_kernel_driver(dev, 1);
 370                 libusb_set_debug(ctx, LIBUSB_LOG_LEVEL_NONE);
 371
 372                 /* Claim the debug interface on the XDS110 */
 373                 result = libusb_claim_interface(dev, INTERFACE_DEBUG);
 374         } else {
 375                 /* Couldn't find an XDS110, flag the error */
 376                 result = -1;
 377         }
 378
 379         /* On an error, clean up what we can */
 380         if (0 != result) {
 381                 if (NULL != dev) {
 382                         /* Release the debug and data interface on the XDS110 */
 383                         (void)libusb_release_interface(dev, INTERFACE_DEBUG);
 384                         libusb_close(dev);
 385                 }
 386                 if (NULL != ctx)
 387                         libusb_exit(ctx);
 388                 xds110.ctx = NULL;
 389                 xds110.dev = NULL;
 390         }
 391
 392         /* Log the results */
 393         if (0 == result)
 394                 LOG_INFO("XDS110: connected");
 395         else
 396                 LOG_ERROR("XDS110: failed to connect");
 397
 398         return (0 == result) ? true : false;
 399 }
 400
 401 static void usb_disconnect(void)
 402 {
 403         if (NULL != xds110.dev) {
 404                 /* Release the debug and data interface on the XDS110 */
 405                 (void)libusb_release_interface(xds110.dev, INTERFACE_DEBUG);
 406                 libusb_close(xds110.dev);
 407                 xds110.dev = NULL;
 408         }
 409         if (NULL != xds110.ctx) {
 410                 libusb_exit(xds110.ctx);
 411                 xds110.ctx = NULL;
 412         }
 413
 414         LOG_INFO("XDS110: disconnected");
 415 }
 416
 417 static bool usb_read(unsigned char *buffer, int size, int *bytes_read,
 418         int timeout)
 419 {
 420         int result;
 421
 422         if (NULL == xds110.dev || NULL == buffer || NULL == bytes_read)
 423                 return false;
 424
 425         /* Force a non-zero timeout to prevent blocking */
 426         if (0 == timeout)
 427                 timeout = DEFAULT_TIMEOUT;
 428
 429         result = libusb_bulk_transfer(xds110.dev, ENDPOINT_DEBUG_IN, buffer, size,
 430                                 bytes_read, timeout);
 431
 432         return (0 == result) ? true : false;
 433 }
 434
 435 static bool usb_write(unsigned char *buffer, int size, int *written)
 436 {
 437         int bytes_written = 0;
 438         int result = LIBUSB_SUCCESS;
 439         int retries = 0;
 440
 441         if (NULL == xds110.dev || NULL == buffer)
 442                 return false;
 443
 444         result = libusb_bulk_transfer(xds110.dev, ENDPOINT_DEBUG_OUT, buffer,
 445                                 size, &bytes_written, 0);
 446
 447         while (LIBUSB_ERROR_PIPE == result && retries < 3) {
 448                 /* Try clearing the pipe stall and retry transfer */
 449                 libusb_clear_halt(xds110.dev, ENDPOINT_DEBUG_OUT);
 450                 result = libusb_bulk_transfer(xds110.dev, ENDPOINT_DEBUG_OUT, buffer,
 451                                         size, &bytes_written, 0);
 452                 retries++;
 453         }
 454
 455         if (NULL != written)
 456                 *written = bytes_written;
 457
 458         return (0 == result && size == bytes_written) ? true : false;
 459 }
 460
 461 static bool usb_get_response(uint32_t *total_bytes_read, uint32_t timeout)
 462 {
 463         static unsigned char buffer[MAX_PACKET];
 464         int bytes_read;
 465         uint16_t size;
 466         uint16_t count;
 467         bool success;
 468
 469         size = 0;
 470         success = true;
 471         while (success) {
 472                 success = usb_read(buffer, sizeof(buffer), &bytes_read, timeout);
 473                 if (success) {
 474                         /*
 475                          * Validate that this appears to be a good response packet
 476                          * First check it contains enough data for header and error
 477                          * code, plus the first character is the start character
 478                          */
 479                         if (bytes_read >= 7 && '*' == buffer[0]) {
 480                                 /* Extract the payload size */
 481                                 size = xds110_get_u16(&buffer[1]);
 482                                 /* Sanity test on payload size */
 483                                 if (USB_PAYLOAD_SIZE >= size && 4 <= size) {
 484                                         /* Check we didn't get more data than expected */
 485                                         if ((bytes_read - 3) <= size) {
 486                                                 /* Packet appears to be valid, move on */
 487                                                 break;
 488                                         }
 489                                 }
 490                         }
 491                 }
 492                 /*
 493                  * Somehow received an invalid packet, retry till we
 494                  * time out or a valid response packet is received
 495                  */
 496         }
 497
 498         /* Abort now if we didn't receive a valid response */
 499         if (!success) {
 500                 if (NULL != total_bytes_read)
 501                         *total_bytes_read = 0;
 502                 return false;
 503         }
 504
 505         /* Build the return payload into xds110.read_payload */
 506
 507         /* Copy over payload data from received buffer (skipping header) */
 508         count = 0;
 509         bytes_read -= 3;
 510         memcpy((void *)&xds110.read_payload[count], (void *)&buffer[3], bytes_read);
 511         count += bytes_read;
 512         /*
 513          * Drop timeout to just 1/2 second. Once the XDS110 starts sending
 514          * a response, the remaining packets should arrive in short order
 515          */
 516         if (timeout > 500)
 517                 timeout = 500; /* ms */
 518
 519         /* If there's more data to retrieve, get it now */
 520         while ((count < size) && success) {
 521                 success = usb_read(buffer, sizeof(buffer), &bytes_read, timeout);
 522                 if (success) {
 523                         if ((count + bytes_read) > size) {
 524                                 /* Read too much data, not a valid packet, abort */
 525                                 success = false;
 526                         } else {
 527                                 /* Copy this data over to xds110.read_payload */
 528                                 memcpy((void *)&xds110.read_payload[count], (void *)buffer,
 529                                         bytes_read);
 530                                 count += bytes_read;
 531                         }
 532                 }
 533         }
 534
 535         if (!success)
 536                 count = 0;
 537         if (NULL != total_bytes_read)
 538                 *total_bytes_read = count;
 539
 540         return success;
 541 }
 542
 543 static bool usb_send_command(uint16_t size)
 544 {
 545         int written;
 546         bool success = true;
 547
 548         /* Check the packet length */
 549         if (size > USB_PAYLOAD_SIZE)
 550                 return false;
 551
 552         /* Place the start character into the packet buffer */
 553         xds110.write_packet[0] = '*';
 554
 555         /* Place the payload size into the packet buffer */
 556         xds110_set_u16(&xds110.write_packet[1], size);
 557
 558         /* Adjust size to include header */
 559         size += 3;
 560
 561         /* Send the data via the USB connection */
 562         success = usb_write(xds110.write_packet, (int)size, &written);
 563
 564         /* Check if the correct number of bytes was written */
 565         if (written != (int)size)
 566                 success = false;
 567
 568         return success;
 569 }
 570
 571 /***************************************************************************
 572  *   XDS110 firmware API routines                                          *
 573  *                                                                         *
 574  *   The following functions handle calling into the XDS110 firmware to    *
 575  *   perform requested debug actions.                                      *
 576  ***************************************************************************/
 577
 578 static bool xds_execute(uint32_t out_length, uint32_t in_length,
 579         uint32_t attempts, uint32_t timeout)
 580 {
 581         bool done = false;
 582         bool success = true;
 583         int error = 0;
 584         uint32_t bytes_read = 0;
 585
 586         if (NULL == xds110.dev)
 587                 return false;
 588
 589         while (!done && attempts > 0) {
 590                 attempts--;
 591
 592                 /* Send command to XDS110 */
 593                 success = usb_send_command(out_length);
 594
 595                 if (success) {
 596                         /* Get response from XDS110 */
 597                         success = usb_get_response(&bytes_read, timeout);
 598                 }
 599
 600                 if (success) {
 601                         /* Check for valid response from XDS code handling */
 602                         if (bytes_read != in_length) {
 603                                 /* Unexpected amount of data returned */
 604                                 success = false;
 605                         } else {
 606                                 /* Extract error code from return packet */
 607                                 error = (int)xds110_get_u32(&xds110.read_payload[0]);
 608                                 done = true;
 609                         }
 610                 }
 611         }
 612
 613         if (!success)
 614                 error = SC_ERR_XDS110_FAIL;
 615
 616         if (0 != error)
 617                 success = false;
 618
 619         return success;
 620 }
 621
 622 static bool xds_connect(void)
 623 {
 624         bool success;
 625
 626         xds110.write_payload[0] = XDS_CONNECT;
 627
 628         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 629                                 DEFAULT_TIMEOUT);
 630
 631         return success;
 632 }
 633
 634 static bool xds_disconnect(void)
 635 {
 636         bool success;
 637
 638         xds110.write_payload[0] = XDS_DISCONNECT;
 639
 640         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 641                                 DEFAULT_TIMEOUT);
 642
 643         return success;
 644 }
 645
 646 static bool xds_version(uint32_t *firmware_id, uint16_t *hardware_id)
 647 {
 648         uint8_t *fw_id_pntr = &xds110.read_payload[XDS_IN_LEN + 0]; /* 32-bits */
 649         uint8_t *hw_id_pntr = &xds110.read_payload[XDS_IN_LEN + 4]; /* 16-bits */
 650
 651         bool success;
 652
 653         xds110.write_payload[0] = XDS_VERSION;
 654
 655         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN + 6, DEFAULT_ATTEMPTS,
 656                                 DEFAULT_TIMEOUT);
 657
 658         if (success) {
 659                 if (NULL != firmware_id)
 660                         *firmware_id = xds110_get_u32(fw_id_pntr);
 661                 if (NULL != hardware_id)
 662                         *hardware_id = xds110_get_u16(hw_id_pntr);
 663         }
 664
 665         return success;
 666 }
 667
 668 static bool xds_set_tck_delay(uint32_t delay)
 669 {
 670         uint8_t *delay_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
 671
 672         bool success;
 673
 674         xds110.write_payload[0] = XDS_SET_TCK;
 675
 676         xds110_set_u32(delay_pntr, delay);
 677
 678         success = xds_execute(XDS_OUT_LEN + 4, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 679                                 DEFAULT_TIMEOUT);
 680
 681         return success;
 682 }
 683
 684 static bool xds_set_trst(uint8_t trst)
 685 {
 686         uint8_t *trst_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
 687
 688         bool success;
 689
 690         xds110.write_payload[0] = XDS_SET_TRST;
 691
 692         *trst_pntr = trst;
 693
 694         success = xds_execute(XDS_OUT_LEN + 1, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 695                                 DEFAULT_TIMEOUT);
 696
 697         return success;
 698 }
 699
 700 static bool xds_cycle_tck(uint32_t count)
 701 {
 702         uint8_t *count_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
 703
 704         bool success;
 705
 706         xds110.write_payload[0] = XDS_CYCLE_TCK;
 707
 708         xds110_set_u32(count_pntr, count);
 709
 710         success = xds_execute(XDS_OUT_LEN + 4, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 711                                 DEFAULT_TIMEOUT);
 712
 713         return success;
 714 }
 715
 716 static bool xds_goto_state(uint32_t state)
 717 {
 718         uint8_t *state_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
 719         uint8_t *transit_pntr = &xds110.write_payload[XDS_OUT_LEN+4]; /* 32-bits */
 720
 721         bool success;
 722
 723         xds110.write_payload[0] = XDS_GOTO_STATE;
 724
 725         xds110_set_u32(state_pntr, state);
 726         xds110_set_u32(transit_pntr, XDS_JTAG_TRANSIT_QUICKEST);
 727
 728         success = xds_execute(XDS_OUT_LEN+8, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 729                                 DEFAULT_TIMEOUT);
 730
 731         return success;
 732 }
 733
 734 static bool xds_jtag_scan(uint32_t shift_state, uint16_t shift_bits,
 735         uint32_t end_state, uint8_t *data_out, uint8_t *data_in)
 736 {
 737         uint8_t *bits_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 16-bits */
 738         uint8_t *path_pntr = &xds110.write_payload[XDS_OUT_LEN + 2]; /* 8-bits */
 739         uint8_t *trans1_pntr = &xds110.write_payload[XDS_OUT_LEN + 3]; /* 8-bits */
 740         uint8_t *end_pntr = &xds110.write_payload[XDS_OUT_LEN + 4]; /* 8-bits */
 741         uint8_t *trans2_pntr = &xds110.write_payload[XDS_OUT_LEN + 5]; /* 8-bits */
 742         uint8_t *pre_pntr = &xds110.write_payload[XDS_OUT_LEN + 6]; /* 16-bits */
 743         uint8_t *pos_pntr = &xds110.write_payload[XDS_OUT_LEN + 8]; /* 16-bits */
 744         uint8_t *delay_pntr = &xds110.write_payload[XDS_OUT_LEN + 10]; /* 16-bits */
 745         uint8_t *rep_pntr = &xds110.write_payload[XDS_OUT_LEN + 12]; /* 16-bits */
 746         uint8_t *out_pntr = &xds110.write_payload[XDS_OUT_LEN + 14]; /* 16-bits */
 747         uint8_t *in_pntr = &xds110.write_payload[XDS_OUT_LEN + 16]; /* 16-bits */
 748         uint8_t *data_out_pntr = &xds110.write_payload[XDS_OUT_LEN + 18];
 749         uint8_t *data_in_pntr = &xds110.read_payload[XDS_IN_LEN+0];
 750
 751         uint16_t total_bytes = DIV_ROUND_UP(shift_bits, 8);
 752
 753         bool success;
 754
 755         xds110.write_payload[0] = XDS_JTAG_SCAN;
 756
 757         xds110_set_u16(bits_pntr, shift_bits); /* bits to scan */
 758         *path_pntr = (uint8_t)(shift_state & 0xff); /* IR vs DR path */
 759         *trans1_pntr = (uint8_t)XDS_JTAG_TRANSIT_QUICKEST; /* start state route */
 760         *end_pntr = (uint8_t)(end_state & 0xff); /* JTAG state after scan */
 761         *trans2_pntr = (uint8_t)XDS_JTAG_TRANSIT_QUICKEST; /* end state route */
 762         xds110_set_u16(pre_pntr, 0); /* number of preamble bits */
 763         xds110_set_u16(pos_pntr, 0); /* number of postamble bits */
 764         xds110_set_u16(delay_pntr, 0); /* number of extra TCKs after scan */
 765         xds110_set_u16(rep_pntr, 1); /* number of repetitions */
 766         xds110_set_u16(out_pntr, total_bytes); /* out buffer offset (if repeats) */
 767         xds110_set_u16(in_pntr, total_bytes); /* in buffer offset (if repeats) */
 768
 769         memcpy((void *)data_out_pntr, (void *)data_out, total_bytes);
 770
 771         success = xds_execute(XDS_OUT_LEN + 18 + total_bytes,
 772                 XDS_IN_LEN + total_bytes, DEFAULT_ATTEMPTS, DEFAULT_TIMEOUT);
 773
 774         if (success)
 775                 memcpy((void *)data_in, (void *)data_in_pntr, total_bytes);
 776
 777         return success;
 778 }
 779
 780 static bool xds_set_srst(uint8_t srst)
 781 {
 782         uint8_t *srst_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
 783
 784         bool success;
 785
 786         xds110.write_payload[0] = XDS_SET_SRST;
 787
 788         *srst_pntr = srst;
 789
 790         success = xds_execute(XDS_OUT_LEN + 1, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 791                                 DEFAULT_TIMEOUT);
 792
 793         return success;
 794 }
 795
 796 static bool cmapi_connect(uint32_t *idcode)
 797 {
 798         uint8_t *idcode_pntr = &xds110.read_payload[XDS_IN_LEN + 0]; /* 32-bits */
 799
 800         bool success;
 801
 802         xds110.write_payload[0] = CMAPI_CONNECT;
 803
 804         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN+4, DEFAULT_ATTEMPTS,
 805                                 DEFAULT_TIMEOUT);
 806
 807         if (success) {
 808                 if (NULL != idcode)
 809                         *idcode = xds110_get_u32(idcode_pntr);
 810         }
 811
 812         return success;
 813 }
 814
 815 static bool cmapi_disconnect(void)
 816 {
 817         bool success;
 818
 819         xds110.write_payload[0] = CMAPI_DISCONNECT;
 820
 821         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 822                                 DEFAULT_TIMEOUT);
 823
 824         return success;
 825 }
 826
 827 static bool cmapi_acquire(void)
 828 {
 829         bool success;
 830
 831         xds110.write_payload[0] = CMAPI_ACQUIRE;
 832
 833         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 834                                 DEFAULT_TIMEOUT);
 835
 836         return success;
 837 }
 838
 839 static bool cmapi_release(void)
 840 {
 841         bool success;
 842
 843         xds110.write_payload[0] = CMAPI_RELEASE;
 844
 845         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 846                                 DEFAULT_TIMEOUT);
 847
 848         return success;
 849 }
 850
 851 static bool cmapi_read_dap_reg(uint32_t type, uint32_t ap_num,
 852         uint32_t address, uint32_t *value)
 853 {
 854         uint8_t *type_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
 855         uint8_t *ap_num_pntr = &xds110.write_payload[XDS_OUT_LEN + 1]; /* 8-bits */
 856         uint8_t *address_pntr = &xds110.write_payload[XDS_OUT_LEN + 2]; /* 8-bits */
 857         uint8_t *value_pntr = &xds110.read_payload[XDS_IN_LEN + 0]; /* 32-bits */
 858
 859         bool success;
 860
 861         xds110.write_payload[0] = CMAPI_REG_READ;
 862
 863         *type_pntr = (uint8_t)(type & 0xff);
 864         *ap_num_pntr = (uint8_t)(ap_num & 0xff);
 865         *address_pntr = (uint8_t)(address & 0xff);
 866
 867         success = xds_execute(XDS_OUT_LEN + 3, XDS_IN_LEN + 4, DEFAULT_ATTEMPTS,
 868                                 DEFAULT_TIMEOUT);
 869
 870         if (success) {
 871                 if (NULL != value)
 872                         *value = xds110_get_u32(value_pntr);
 873         }
 874
 875         return success;
 876 }
 877
 878 static bool cmapi_write_dap_reg(uint32_t type, uint32_t ap_num,
 879         uint32_t address, uint32_t *value)
 880 {
 881         uint8_t *type_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
 882         uint8_t *ap_num_pntr = &xds110.write_payload[XDS_OUT_LEN + 1]; /* 8-bits */
 883         uint8_t *address_pntr = &xds110.write_payload[XDS_OUT_LEN + 2]; /* 8-bits */
 884         uint8_t *value_pntr = &xds110.write_payload[XDS_OUT_LEN + 3]; /* 32-bits */
 885
 886         bool success;
 887
 888         if (NULL == value)
 889                 return false;
 890
 891         xds110.write_payload[0] = CMAPI_REG_WRITE;
 892
 893         *type_pntr = (uint8_t)(type & 0xff);
 894         *ap_num_pntr = (uint8_t)(ap_num & 0xff);
 895         *address_pntr = (uint8_t)(address & 0xff);
 896         xds110_set_u32(value_pntr, *value);
 897
 898         success = xds_execute(XDS_OUT_LEN + 7, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 899                                 DEFAULT_TIMEOUT);
 900
 901         return success;
 902 }
 903
 904 static bool swd_connect(void)
 905 {
 906         bool success;
 907
 908         xds110.write_payload[0] = SWD_CONNECT;
 909
 910         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 911                                 DEFAULT_TIMEOUT);
 912
 913         return success;
 914 }
 915
 916 static bool swd_disconnect(void)
 917 {
 918         bool success;
 919
 920         xds110.write_payload[0] = SWD_DISCONNECT;
 921
 922         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 923                                 DEFAULT_TIMEOUT);
 924
 925         return success;
 926 }
 927
 928 static bool cjtag_connect(uint32_t format)
 929 {
 930         uint8_t *format_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
 931
 932         bool success;
 933
 934         xds110.write_payload[0] = CJTAG_CONNECT;
 935
 936         xds110_set_u32(format_pntr, format);
 937
 938         success = xds_execute(XDS_OUT_LEN + 4, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 939                                 DEFAULT_TIMEOUT);
 940
 941         return success;
 942 }
 943
 944 static bool cjtag_disconnect(void)
 945 {
 946         bool success;
 947
 948         xds110.write_payload[0] = CJTAG_DISCONNECT;
 949
 950         success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
 951                                 DEFAULT_TIMEOUT);
 952
 953         return success;
 954 }
 955
 956 static bool ocd_dap_request(uint8_t *dap_requests, uint32_t request_size,
 957         uint32_t *dap_results, uint32_t result_count)
 958 {
 959         uint8_t *request_pntr = &xds110.write_payload[XDS_OUT_LEN + 0];
 960         uint8_t *result_pntr = &xds110.read_payload[XDS_IN_LEN + 0];
 961
 962         bool success;
 963
 964         if (NULL == dap_requests || NULL == dap_results)
 965                 return false;
 966
 967         xds110.write_payload[0] = OCD_DAP_REQUEST;
 968
 969         memcpy((void *)request_pntr, (void *)dap_requests, request_size);
 970
 971         success = xds_execute(XDS_OUT_LEN + request_size,
 972                                 XDS_IN_LEN + (result_count * 4), DEFAULT_ATTEMPTS,
 973                                 DEFAULT_TIMEOUT);
 974
 975         if (success && (result_count > 0))
 976                 memcpy((void *)dap_results, (void *)result_pntr, result_count * 4);
 977
 978         return success;
 979 }
 980
 981 static bool ocd_scan_request(uint8_t *scan_requests, uint32_t request_size,
 982         uint8_t *scan_results, uint32_t result_size)
 983 {
 984         uint8_t *request_pntr = &xds110.write_payload[XDS_OUT_LEN + 0];
 985         uint8_t *result_pntr = &xds110.read_payload[XDS_IN_LEN + 0];
 986
 987         bool success;
 988
 989         if (NULL == scan_requests || NULL == scan_results)
 990                 return false;
 991
 992         xds110.write_payload[0] = OCD_SCAN_REQUEST;
 993
 994         memcpy((void *)request_pntr, (void *)scan_requests, request_size);
 995
 996         success = xds_execute(XDS_OUT_LEN + request_size,
 997                                 XDS_IN_LEN + result_size, DEFAULT_ATTEMPTS,
 998                                 DEFAULT_TIMEOUT);
 999
1000         if (success && (result_size > 0))
1001                 memcpy((void *)scan_results, (void *)result_pntr, result_size);
1002
1003         return success;
1004 }
1005
1006 static bool ocd_pathmove(uint32_t num_states, uint8_t *path)
1007 {
1008         uint8_t *num_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
1009         uint8_t *path_pntr = &xds110.write_payload[XDS_OUT_LEN + 4];
1010
1011         bool success;
1012
1013         if (NULL == path)
1014                 return false;
1015
1016         xds110.write_payload[0] = OCD_PATHMOVE;
1017
1018         xds110_set_u32(num_pntr, num_states);
1019
1020         memcpy((void *)path_pntr, (void *)path, num_states);
1021
1022         success = xds_execute(XDS_OUT_LEN + 4 + num_states, XDS_IN_LEN,
1023                                 DEFAULT_ATTEMPTS, DEFAULT_TIMEOUT);
1024
1025         return success;
1026 }
1027
1028 /***************************************************************************
1029  *   swd driver interface                                                  *
1030  *                                                                         *
1031  *   The following functions provide SWD support to OpenOCD.               *
1032  ***************************************************************************/
1033
1034 static int xds110_swd_init(void)
1035 {
1036         xds110.is_swd_mode = true;
1037         return ERROR_OK;
1038 }
1039
1040 static int xds110_swd_switch_seq(enum swd_special_seq seq)
1041 {
1042         uint32_t idcode;
1043         bool success;
1044
1045         switch (seq) {
1046         case LINE_RESET:
1047                 LOG_ERROR("Sequence SWD line reset (%d) not supported", seq);
1048                 return ERROR_FAIL;
1049         case JTAG_TO_SWD:
1050                 LOG_DEBUG("JTAG-to-SWD");
1051                 xds110.is_swd_mode = false;
1052                 xds110.is_cmapi_connected = false;
1053                 xds110.is_cmapi_acquired = false;
1054                 /* Run sequence to put target in SWD mode */
1055                 success = swd_connect();
1056                 /* Re-iniitialize CMAPI API for DAP access */
1057                 if (success) {
1058                         xds110.is_swd_mode = true;
1059                         success = cmapi_connect(&idcode);
1060                         if (success) {
1061                                 xds110.is_cmapi_connected = true;
1062                                 success = cmapi_acquire();
1063                         }
1064                 }
1065                 break;
1066         case SWD_TO_JTAG:
1067                 LOG_DEBUG("SWD-to-JTAG");
1068                 xds110.is_swd_mode = false;
1069                 xds110.is_cmapi_connected = false;
1070                 xds110.is_cmapi_acquired = false;
1071                 /* Run sequence to put target in JTAG mode */
1072                 success = swd_disconnect();
1073                 if (success) {
1074                         /* Re-initialize JTAG interface */
1075                         success = cjtag_connect(MODE_JTAG);
1076                 }
1077                 break;
1078         default:
1079                 LOG_ERROR("Sequence %d not supported", seq);
1080                 return ERROR_FAIL;
1081         }
1082
1083         if (success)
1084                 return ERROR_OK;
1085         else
1086                 return ERROR_FAIL;
1087 }
1088
1089 static bool xds110_legacy_read_reg(uint8_t cmd, uint32_t *value)
1090 {
1091         /* Make sure this is a read request */
1092         bool is_read_request = (0 != (SWD_CMD_RnW & cmd));
1093         /* Determine whether this is a DP or AP register access */
1094         uint32_t type = (0 != (SWD_CMD_APnDP & cmd)) ? DAP_AP : DAP_DP;
1095         /* Determine the AP number from cached SELECT value */
1096         uint32_t ap_num = (xds110.select & 0xff000000) >> 24;
1097         /* Extract register address from command */
1098         uint32_t address = ((cmd & SWD_CMD_A32) >> 1);
1099         /* Extract bank address from cached SELECT value */
1100         uint32_t bank = (xds110.select & 0x000000f0);
1101
1102         uint32_t reg_value = 0;
1103         uint32_t temp_value = 0;
1104
1105         bool success;
1106
1107         if (!is_read_request)
1108                 return false;
1109
1110         if (DAP_AP == type) {
1111                 /* Add bank address to register address for CMAPI call */
1112                 address |= bank;
1113         }
1114
1115         if (DAP_DP == type && DAP_DP_RDBUFF == address && xds110.use_rdbuff) {
1116                 /* If RDBUFF is cached and this is a DP RDBUFF read, use the cache */
1117                 reg_value = xds110.rdbuff;
1118                 success = true;
1119         } else if (DAP_AP == type && DAP_AP_DRW == address && xds110.use_rdbuff) {
1120                 /* If RDBUFF is cached and this is an AP DRW read, use the cache, */
1121                 /* but still call into the firmware to get the next read. */
1122                 reg_value = xds110.rdbuff;
1123                 success = cmapi_read_dap_reg(type, ap_num, address, &temp_value);
1124         } else {
1125                 success = cmapi_read_dap_reg(type, ap_num, address, &temp_value);
1126                 if (success)
1127                         reg_value = temp_value;
1128         }
1129
1130         /* Mark that we have consumed or invalidated the RDBUFF cache */
1131         xds110.use_rdbuff = false;
1132
1133         /* Handle result of read attempt */
1134         if (!success)
1135                 LOG_ERROR("XDS110: failed to read DAP register");
1136         else if (NULL != value)
1137                 *value = reg_value;
1138
1139         if (success && DAP_AP == type) {
1140                 /*
1141                  * On a successful DAP AP read, we actually have the value from RDBUFF,
1142                  * the firmware will have run the AP request and made the RDBUFF read
1143                  */
1144                 xds110.use_rdbuff = true;
1145                 xds110.rdbuff = temp_value;
1146         }
1147
1148         return success;
1149 }
1150
1151 static bool xds110_legacy_write_reg(uint8_t cmd, uint32_t value)
1152 {
1153         /* Make sure this isn't a read request */
1154         bool is_read_request = (0 != (SWD_CMD_RnW & cmd));
1155         /* Determine whether this is a DP or AP register access */
1156         uint32_t type = (0 != (SWD_CMD_APnDP & cmd)) ? DAP_AP : DAP_DP;
1157         /* Determine the AP number from cached SELECT value */
1158         uint32_t ap_num = (xds110.select & 0xff000000) >> 24;
1159         /* Extract register address from command */
1160         uint32_t address = ((cmd & SWD_CMD_A32) >> 1);
1161         /* Extract bank address from cached SELECT value */
1162         uint32_t bank = (xds110.select & 0x000000f0);
1163
1164         bool success;
1165
1166         if (is_read_request)
1167                 return false;
1168
1169         /* Invalidate the RDBUFF cache */
1170         xds110.use_rdbuff = false;
1171
1172         if (DAP_AP == type) {
1173                 /* Add bank address to register address for CMAPI call */
1174                 address |= bank;
1175                 /* Any write to an AP register invalidates the firmware's cache */
1176                 xds110.is_ap_dirty = true;
1177         } else if (DAP_DP_SELECT == address) {
1178                 /* Any write to the SELECT register invalidates the firmware's cache */
1179                 xds110.is_ap_dirty = true;
1180         }
1181
1182         success = cmapi_write_dap_reg(type, ap_num, address, &value);
1183
1184         if (!success) {
1185                 LOG_ERROR("XDS110: failed to write DAP register");
1186         } else {
1187                 /*
1188                  * If the debugger wrote to SELECT, cache the value
1189                  * to use to build the apNum and address values above
1190                  */
1191                 if ((DAP_DP == type) && (DAP_DP_SELECT == address))
1192                         xds110.select = value;
1193         }
1194
1195         return success;
1196 }
1197
1198 static int xds110_swd_run_queue(void)
1199 {
1200         static uint32_t dap_results[MAX_RESULT_QUEUE];
1201         uint8_t cmd;
1202         uint32_t request;
1203         uint32_t result;
1204         uint32_t value;
1205         bool success = true;
1206
1207         if (0 == xds110.txn_request_size)
1208                 return ERROR_OK;
1209
1210         /* Terminate request queue */
1211         xds110.txn_requests[xds110.txn_request_size++] = 0;
1212
1213         if (xds110.firmware >= OCD_FIRMWARE_VERSION) {
1214                 /* XDS110 firmware has the API to directly handle the queue */
1215                 success = ocd_dap_request(xds110.txn_requests,
1216                         xds110.txn_request_size, dap_results, xds110.txn_result_count);
1217         } else {
1218                 /* Legacy firmware needs to handle queue via discrete DAP calls */
1219                 request = 0;
1220                 result = 0;
1221                 while (xds110.txn_requests[request] != 0) {
1222                         cmd = xds110.txn_requests[request++];
1223                         if (0 == (SWD_CMD_RnW & cmd)) {
1224                                 /* DAP register write command */
1225                                 value  = (uint32_t)(xds110.txn_requests[request++]) <<  0;
1226                                 value |= (uint32_t)(xds110.txn_requests[request++]) <<  8;
1227                                 value |= (uint32_t)(xds110.txn_requests[request++]) << 16;
1228                                 value |= (uint32_t)(xds110.txn_requests[request++]) << 24;
1229                                 if (success)
1230                                         success = xds110_legacy_write_reg(cmd, value);
1231                         } else {
1232                                 /* DAP register read command */
1233                                 value = 0;
1234                                 if (success)
1235                                         success = xds110_legacy_read_reg(cmd, &value);
1236                                 dap_results[result++] = value;
1237                         }
1238                 }
1239         }
1240
1241         /* Transfer results into caller's buffers */
1242         for (result = 0; result < xds110.txn_result_count; result++)
1243                 if (0 != xds110.txn_dap_results[result])
1244                         *xds110.txn_dap_results[result] = dap_results[result];
1245
1246         xds110.txn_request_size = 0;
1247         xds110.txn_result_size = 0;
1248         xds110.txn_result_count = 0;
1249
1250         return (success) ? ERROR_OK : ERROR_FAIL;
1251 }
1252
1253 static void xds110_swd_queue_cmd(uint8_t cmd, uint32_t *value)
1254 {
1255         /* Check if this is a read or write request */
1256         bool is_read_request = (0 != (SWD_CMD_RnW & cmd));
1257         /* Determine whether this is a DP or AP register access */
1258         uint32_t type = (0 != (SWD_CMD_APnDP & cmd)) ? DAP_AP : DAP_DP;
1259         /* Extract register address from command */
1260         uint32_t address = ((cmd & SWD_CMD_A32) >> 1);
1261         uint32_t request_size = (is_read_request) ? 1 : 5;
1262
1263         /* Check if new request would be too large to fit */
1264         if (((xds110.txn_request_size + request_size + 1) > MAX_DATA_BLOCK) ||
1265                 ((xds110.txn_result_count + 1) > MAX_RESULT_QUEUE))
1266                 xds110_swd_run_queue();
1267
1268         /* Set the START bit in cmd to ensure cmd is not zero */
1269         /* (a value of zero is used to terminate the buffer) */
1270         cmd |= SWD_CMD_START;
1271
1272         /* Add request to queue; queue is built marshalled for XDS110 call */
1273         if (is_read_request) {
1274                 /* Queue read request, save pointer to pass back result */
1275                 xds110.txn_requests[xds110.txn_request_size++] = cmd;
1276                 xds110.txn_dap_results[xds110.txn_result_count++] = value;
1277                 xds110.txn_result_size += 4;
1278         } else {
1279                 /* Check for and prevent sticky overrun detection */
1280                 if (DAP_DP == type && DAP_DP_CTRL == address &&
1281                         (*value & CORUNDETECT)) {
1282                         LOG_DEBUG("XDS110: refusing to enable sticky overrun detection");
1283                         *value &= ~CORUNDETECT;
1284                 }
1285                 /* Queue write request, add value directly to queue buffer */
1286                 xds110.txn_requests[xds110.txn_request_size++] = cmd;
1287                 xds110.txn_requests[xds110.txn_request_size++] = (*value >>  0) & 0xff;
1288                 xds110.txn_requests[xds110.txn_request_size++] = (*value >>  8) & 0xff;
1289                 xds110.txn_requests[xds110.txn_request_size++] = (*value >> 16) & 0xff;
1290                 xds110.txn_requests[xds110.txn_request_size++] = (*value >> 24) & 0xff;
1291         }
1292 }
1293
1294 static void xds110_swd_read_reg(uint8_t cmd, uint32_t *value,
1295         uint32_t ap_delay_clk)
1296 {
1297         xds110_swd_queue_cmd(cmd, value);
1298 }
1299 static void xds110_swd_write_reg(uint8_t cmd, uint32_t value,
1300         uint32_t ap_delay_clk)
1301 {
1302         xds110_swd_queue_cmd(cmd, &value);
1303 }
1304
1305 /***************************************************************************
1306  *   jtag interface                                                        *
1307  *                                                                         *
1308  *   The following functions provide XDS110 interface to OpenOCD.          *
1309  ***************************************************************************/
1310
1311 static void xds110_show_info(void)
1312 {
1313         uint32_t firmware = xds110.firmware;
1314
1315         LOG_INFO("XDS110: firmware version = %d.%d.%d.%d",
1316                 (((firmware >> 28) & 0xf) * 10) + ((firmware >> 24) & 0xf),
1317                 (((firmware >> 20) & 0xf) * 10) + ((firmware >> 16) & 0xf),
1318                 (((firmware >> 12) & 0xf) * 10) + ((firmware >>  8) & 0xf),
1319                 (((firmware >>  4) & 0xf) * 10) + ((firmware >>  0) & 0xf));
1320         LOG_INFO("XDS110: hardware version = 0x%04x", xds110.hardware);
1321         if (0 != xds110.serial[0])
1322                 LOG_INFO("XDS110: serial number = %s)", xds110.serial);
1323         if (xds110.is_swd_mode) {
1324                 LOG_INFO("XDS110: connected to target via SWD");
1325                 LOG_INFO("XDS110: SWCLK set to %d kHz", xds110.speed);
1326         } else {
1327                 LOG_INFO("XDS110: connected to target via JTAG");
1328                 LOG_INFO("XDS110: TCK set to %d kHz", xds110.speed);
1329         }
1330
1331         /* Alert user that there's a better firmware to use */
1332         if (firmware < OCD_FIRMWARE_VERSION) {
1333                 LOG_WARNING("XDS110: the firmware is not optimized for OpenOCD");
1334                 LOG_WARNING(OCD_FIRMWARE_UPGRADE);
1335         }
1336 }
1337
1338 static int xds110_quit(void)
1339 {
1340         if (xds110.is_cmapi_acquired) {
1341                 (void)cmapi_release();
1342                 xds110.is_cmapi_acquired = false;
1343         }
1344         if (xds110.is_cmapi_connected) {
1345                 (void)cmapi_disconnect();
1346                 xds110.is_cmapi_connected = false;
1347         }
1348         if (xds110.is_connected) {
1349                 if (xds110.is_swd_mode) {
1350                         /* Switch out of SWD mode */
1351                         (void)swd_disconnect();
1352                 } else {
1353                         /* Switch out of cJTAG mode */
1354                         (void)cjtag_disconnect();
1355                 }
1356                 /* Tell firmware we're disconnecting */
1357                 (void)xds_disconnect();
1358                 xds110.is_connected = false;
1359         }
1360         /* Close down the USB connection to the XDS110 debug probe */
1361         usb_disconnect();
1362
1363         return ERROR_OK;
1364 }
1365
1366 static int xds110_init(void)
1367 {
1368         bool success;
1369
1370         /* Establish USB connection to the XDS110 debug probe */
1371         success = usb_connect();
1372
1373         if (success) {
1374                 /* Send connect message to XDS110 firmware */
1375                 success = xds_connect();
1376                 if (success)
1377                         xds110.is_connected = true;
1378         }
1379
1380         if (success) {
1381                 uint32_t firmware;
1382                 uint16_t hardware;
1383
1384                 /* Retrieve version IDs from firmware */
1385                 /* Version numbers are stored in BCD format */
1386                 success = xds_version(&firmware, &hardware);
1387                 if (success) {
1388                         /* Save the firmware and hardware version */
1389                         xds110.firmware = firmware;
1390                         xds110.hardware = hardware;
1391                 }
1392         }
1393
1394         if (success) {
1395                 success = xds_set_trst(0);
1396                 if (success)
1397                         success = xds_cycle_tck(50);
1398                 if (success)
1399                         success = xds_set_trst(1);
1400                 if (success)
1401                         success = xds_cycle_tck(50);
1402         }
1403
1404         if (success) {
1405                 if (xds110.is_swd_mode) {
1406                         /* Switch to SWD if needed */
1407                         success = swd_connect();
1408                 } else {
1409                         success = cjtag_connect(MODE_JTAG);
1410                 }
1411         }
1412
1413         if (success && xds110.is_swd_mode) {
1414                 uint32_t idcode;
1415
1416                 /* Connect to CMAPI interface in XDS110 */
1417                 success = cmapi_connect(&idcode);
1418
1419                 /* Acquire exclusive access to CMAPI interface */
1420                 if (success) {
1421                         xds110.is_cmapi_connected = true;
1422                         success = cmapi_acquire();
1423                         if (success)
1424                                 xds110.is_cmapi_acquired = true;
1425                 }
1426         }
1427
1428         if (!success)
1429                 xds110_quit();
1430
1431         if (success)
1432                 xds110_show_info();
1433
1434         return (success) ? ERROR_OK : ERROR_FAIL;
1435 }
1436
1437 static void xds110_legacy_scan(uint32_t shift_state, uint32_t total_bits,
1438         uint32_t end_state, uint8_t *data_out, uint8_t *data_in)
1439 {
1440         (void)xds_jtag_scan(shift_state, total_bits, end_state, data_out, data_in);
1441 }
1442
1443 static void xds110_legacy_runtest(uint32_t clocks, uint32_t end_state)
1444 {
1445         xds_goto_state(XDS_JTAG_STATE_IDLE);
1446         xds_cycle_tck(clocks);
1447         xds_goto_state(end_state);
1448 }
1449
1450 static void xds110_legacy_stableclocks(uint32_t clocks)
1451 {
1452         xds_cycle_tck(clocks);
1453 }
1454
1455 static void xds110_flush(void)
1456 {
1457         uint8_t command;
1458         uint32_t clocks;
1459         uint32_t shift_state;
1460         uint32_t end_state;
1461         uint32_t bits;
1462         uint32_t bytes;
1463         uint32_t request;
1464         uint32_t result;
1465         uint8_t *data_out;
1466         uint8_t data_in[MAX_DATA_BLOCK];
1467         uint8_t *data_pntr;
1468
1469         if (0 == xds110.txn_request_size)
1470                 return;
1471
1472         /* Terminate request queue */
1473         xds110.txn_requests[xds110.txn_request_size++] = 0;
1474
1475         if (xds110.firmware >= OCD_FIRMWARE_VERSION) {
1476                 /* Updated firmware has the API to directly handle the queue */
1477                 (void)ocd_scan_request(xds110.txn_requests, xds110.txn_request_size,
1478                         data_in, xds110.txn_result_size);
1479         } else {
1480                 /* Legacy firmware needs to handle queue via discrete JTAG calls */
1481                 request = 0;
1482                 result = 0;
1483                 while (xds110.txn_requests[request] != 0) {
1484                         command = xds110.txn_requests[request++];
1485                         switch (command) {
1486                                 case CMD_IR_SCAN:
1487                                 case CMD_DR_SCAN:
1488                                         if (command == CMD_IR_SCAN)
1489                                                 shift_state = XDS_JTAG_STATE_SHIFT_IR;
1490                                         else
1491                                                 shift_state = XDS_JTAG_STATE_SHIFT_DR;
1492                                         end_state = (uint32_t)(xds110.txn_requests[request++]);
1493                                         bits  = (uint32_t)(xds110.txn_requests[request++]) << 0;
1494                                         bits |= (uint32_t)(xds110.txn_requests[request++]) << 8;
1495                                         data_out = &xds110.txn_requests[request];
1496                                         bytes = DIV_ROUND_UP(bits, 8);
1497                                         xds110_legacy_scan(shift_state, bits, end_state, data_out,
1498                                                 &data_in[result]);
1499                                         result += bytes;
1500                                         request += bytes;
1501                                         break;
1502                                 case CMD_RUNTEST:
1503                                         clocks  = (uint32_t)(xds110.txn_requests[request++]) <<  0;
1504                                         clocks |= (uint32_t)(xds110.txn_requests[request++]) <<  8;
1505                                         clocks |= (uint32_t)(xds110.txn_requests[request++]) << 16;
1506                                         clocks |= (uint32_t)(xds110.txn_requests[request++]) << 24;
1507                                         end_state = (uint32_t)xds110.txn_requests[request++];
1508                                         xds110_legacy_runtest(clocks, end_state);
1509                                         break;
1510                                 case CMD_STABLECLOCKS:
1511                                         clocks  = (uint32_t)(xds110.txn_requests[request++]) <<  0;
1512                                         clocks |= (uint32_t)(xds110.txn_requests[request++]) <<  8;
1513                                         clocks |= (uint32_t)(xds110.txn_requests[request++]) << 16;
1514                                         clocks |= (uint32_t)(xds110.txn_requests[request++]) << 24;
1515                                         xds110_legacy_stableclocks(clocks);
1516                                         break;
1517                                 default:
1518                                         LOG_ERROR("BUG: unknown JTAG command type 0x%x encountered",
1519                                                 command);
1520                                         exit(-1);
1521                                         break;
1522                         }
1523                 }
1524         }
1525
1526         /* Transfer results into caller's buffers from data_in buffer */
1527         bits = 0; /* Bit offset into current scan result */
1528         data_pntr = data_in;
1529         for (result = 0; result < xds110.txn_result_count; result++) {
1530                 if (xds110.txn_scan_results[result].first) {
1531                         if (bits != 0) {
1532                                 bytes = DIV_ROUND_UP(bits, 8);
1533                                 data_pntr += bytes;
1534                         }
1535                         bits = 0;
1536                 }
1537                 if (xds110.txn_scan_results[result].buffer != 0)
1538                         bit_copy(xds110.txn_scan_results[result].buffer, 0, data_pntr,
1539                                 bits, xds110.txn_scan_results[result].num_bits);
1540                 bits += xds110.txn_scan_results[result].num_bits;
1541         }
1542
1543         xds110.txn_request_size = 0;
1544         xds110.txn_result_size = 0;
1545         xds110.txn_result_count = 0;
1546 }
1547
1548 static void xds110_execute_reset(struct jtag_command *cmd)
1549 {
1550         char trst;
1551         char srst;
1552
1553         if (cmd->cmd.reset->trst != -1) {
1554                 if (cmd->cmd.reset->trst == 0) {
1555                         /* Deassert nTRST (active low) */
1556                         trst = 1;
1557                 } else {
1558                         /* Assert nTRST (active low) */
1559                         trst = 0;
1560                 }
1561                 (void)xds_set_trst(trst);
1562         }
1563
1564         if (cmd->cmd.reset->srst != -1) {
1565                 if (cmd->cmd.reset->srst == 0) {
1566                         /* Deassert nSRST (active low) */
1567                         srst = 1;
1568                 } else {
1569                         /* Assert nSRST (active low) */
1570                         srst = 0;
1571                 }
1572                 (void)xds_set_srst(srst);
1573         }
1574 }
1575
1576 static void xds110_execute_sleep(struct jtag_command *cmd)
1577 {
1578         jtag_sleep(cmd->cmd.sleep->us);
1579         return;
1580 }
1581
1582 static void xds110_execute_tlr_reset(struct jtag_command *cmd)
1583 {
1584         (void)xds_goto_state(XDS_JTAG_STATE_RESET);
1585
1586         return;
1587 }
1588
1589 static void xds110_execute_pathmove(struct jtag_command *cmd)
1590 {
1591         uint32_t i;
1592         uint32_t num_states;
1593         uint8_t *path;
1594
1595         num_states = (uint32_t)cmd->cmd.pathmove->num_states;
1596
1597         if (num_states == 0)
1598                 return;
1599
1600         path = (uint8_t *)malloc(num_states * sizeof(uint8_t));
1601         if (path == 0) {
1602                 LOG_ERROR("XDS110: unable to allocate memory");
1603                 return;
1604         }
1605
1606         /* Convert requested path states into XDS API states */
1607         for (i = 0; i < num_states; i++)
1608                 path[i] = (uint8_t)xds_jtag_state[cmd->cmd.pathmove->path[i]];
1609
1610         if (xds110.firmware >= OCD_FIRMWARE_VERSION) {
1611                 /* Updated firmware fully supports pathmove */
1612                 (void)ocd_pathmove(num_states, path);
1613         } else {
1614                 /* Notify user that legacy firmware simply cannot handle pathmove */
1615                 LOG_ERROR("XDS110: the firmware does not support pathmove command");
1616                 LOG_ERROR(OCD_FIRMWARE_UPGRADE);
1617                 /* If pathmove is required, then debug is not possible */
1618                 exit(-1);
1619         }
1620
1621         free((void *)path);
1622
1623         return;
1624 }
1625
1626 static void xds110_queue_scan(struct jtag_command *cmd)
1627 {
1628         int i;
1629         uint32_t offset;
1630         uint32_t total_fields;
1631         uint32_t total_bits;
1632         uint32_t total_bytes;
1633         uint8_t end_state;
1634         uint8_t *buffer;
1635
1636         /* Calculate the total number of bits to scan */
1637         total_bits = 0;
1638         total_fields = 0;
1639         for (i = 0; i < cmd->cmd.scan->num_fields; i++) {
1640                 total_fields++;
1641                 total_bits += (uint32_t)cmd->cmd.scan->fields[i].num_bits;
1642         }
1643
1644         if (total_bits == 0)
1645                 return;
1646
1647         total_bytes = DIV_ROUND_UP(total_bits, 8);
1648
1649         /* Check if new request would be too large to fit */
1650         if (((xds110.txn_request_size + 1 + total_bytes + sizeof(end_state) + 1)
1651                 > MAX_DATA_BLOCK) || ((xds110.txn_result_count + total_fields) >
1652                 MAX_RESULT_QUEUE))
1653                 xds110_flush();
1654
1655         /* Check if this single request is too large to fit */
1656         if ((1 + total_bytes + sizeof(end_state) + 1) > MAX_DATA_BLOCK) {
1657                 LOG_ERROR("BUG: JTAG scan request is too large to handle (%d bits)",
1658                         total_bits);
1659                 /* Failing to run this scan mucks up debug on this target */
1660                 exit(-1);
1661         }
1662
1663         if (cmd->cmd.scan->ir_scan)
1664                 xds110.txn_requests[xds110.txn_request_size++] = CMD_IR_SCAN;
1665         else
1666                 xds110.txn_requests[xds110.txn_request_size++] = CMD_DR_SCAN;
1667
1668         end_state = (uint8_t)xds_jtag_state[cmd->cmd.scan->end_state];
1669         xds110.txn_requests[xds110.txn_request_size++] = end_state;
1670
1671         xds110.txn_requests[xds110.txn_request_size++] = (total_bits >> 0) & 0xff;
1672         xds110.txn_requests[xds110.txn_request_size++] = (total_bits >> 8) & 0xff;
1673
1674         /* Build request data by flattening fields into single buffer */
1675         /* also populate the results array to return the results when run */
1676         offset = 0;
1677         buffer = &xds110.txn_requests[xds110.txn_request_size];
1678         /* Clear data out buffer to default value of all zeros */
1679         memset((void *)buffer, 0x00, total_bytes);
1680         for (i = 0; i < cmd->cmd.scan->num_fields; i++) {
1681                 if (cmd->cmd.scan->fields[i].out_value != 0) {
1682                         /* Copy over data to scan out into request buffer */
1683                         bit_copy(buffer, offset, cmd->cmd.scan->fields[i].out_value, 0,
1684                                 cmd->cmd.scan->fields[i].num_bits);
1685                 }
1686                 offset += cmd->cmd.scan->fields[i].num_bits;
1687                 xds110.txn_scan_results[xds110.txn_result_count].first = (i == 0);
1688                 xds110.txn_scan_results[xds110.txn_result_count].num_bits =
1689                         cmd->cmd.scan->fields[i].num_bits;
1690                 xds110.txn_scan_results[xds110.txn_result_count++].buffer =
1691                         cmd->cmd.scan->fields[i].in_value;
1692         }
1693         xds110.txn_request_size += total_bytes;
1694         xds110.txn_result_size += total_bytes;
1695
1696         return;
1697 }
1698
1699 static void xds110_queue_runtest(struct jtag_command *cmd)
1700 {
1701         uint32_t clocks = (uint32_t)cmd->cmd.stableclocks->num_cycles;
1702         uint8_t end_state = (uint8_t)xds_jtag_state[cmd->cmd.runtest->end_state];
1703
1704         /* Check if new request would be too large to fit */
1705         if ((xds110.txn_request_size + 1 + sizeof(clocks) + sizeof(end_state) + 1)
1706                 > MAX_DATA_BLOCK)
1707                 xds110_flush();
1708
1709         /* Queue request and cycle count directly to queue buffer */
1710         xds110.txn_requests[xds110.txn_request_size++] = CMD_RUNTEST;
1711         xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  0) & 0xff;
1712         xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  8) & 0xff;
1713         xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 16) & 0xff;
1714         xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 24) & 0xff;
1715         xds110.txn_requests[xds110.txn_request_size++] = end_state;
1716
1717         return;
1718 }
1719
1720 static void xds110_queue_stableclocks(struct jtag_command *cmd)
1721 {
1722         uint32_t clocks = (uint32_t)cmd->cmd.stableclocks->num_cycles;
1723
1724         /* Check if new request would be too large to fit */
1725         if ((xds110.txn_request_size + 1 + sizeof(clocks) + 1) > MAX_DATA_BLOCK)
1726                 xds110_flush();
1727
1728         /* Queue request and cycle count directly to queue buffer */
1729         xds110.txn_requests[xds110.txn_request_size++] = CMD_STABLECLOCKS;
1730         xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  0) & 0xff;
1731         xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  8) & 0xff;
1732         xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 16) & 0xff;
1733         xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 24) & 0xff;
1734
1735         return;
1736 }
1737
1738 static void xds110_execute_command(struct jtag_command *cmd)
1739 {
1740         switch (cmd->type) {
1741                 case JTAG_RESET:
1742                         xds110_flush();
1743                         xds110_execute_reset(cmd);
1744                         break;
1745                 case JTAG_SLEEP:
1746                         xds110_flush();
1747                         xds110_execute_sleep(cmd);
1748                         break;
1749                 case JTAG_TLR_RESET:
1750                         xds110_flush();
1751                         xds110_execute_tlr_reset(cmd);
1752                         break;
1753                 case JTAG_PATHMOVE:
1754                         xds110_flush();
1755                         xds110_execute_pathmove(cmd);
1756                         break;
1757                 case JTAG_SCAN:
1758                         xds110_queue_scan(cmd);
1759                         break;
1760                 case JTAG_RUNTEST:
1761                         xds110_queue_runtest(cmd);
1762                         break;
1763                 case JTAG_STABLECLOCKS:
1764                         xds110_queue_stableclocks(cmd);
1765                         break;
1766                 case JTAG_TMS:
1767                 default:
1768                         LOG_ERROR("BUG: unknown JTAG command type 0x%x encountered",
1769                                 cmd->type);
1770                         exit(-1);
1771         }
1772 }
1773
1774 static int xds110_execute_queue(void)
1775 {
1776         struct jtag_command *cmd = jtag_command_queue;
1777
1778         while (cmd != NULL) {
1779                 xds110_execute_command(cmd);
1780                 cmd = cmd->next;
1781         }
1782
1783         xds110_flush();
1784
1785         return ERROR_OK;
1786 }
1787
1788 static int xds110_speed(int speed)
1789 {
1790         bool success;
1791
1792         if (speed == 0) {
1793                 LOG_INFO("XDS110: RTCK not supported");
1794                 return ERROR_JTAG_NOT_IMPLEMENTED;
1795         }
1796
1797         if (speed > XDS110_MAX_TCK_SPEED) {
1798                 LOG_INFO("XDS110: reduce speed request: %dkHz to %dkHz maximum",
1799                         speed, XDS110_MAX_TCK_SPEED);
1800                 speed = XDS110_MAX_TCK_SPEED;
1801         }
1802
1803         if (speed < XDS110_MIN_TCK_SPEED) {
1804                 LOG_INFO("XDS110: increase speed request: %dkHz to %dkHz minimum",
1805                         speed, XDS110_MIN_TCK_SPEED);
1806                 speed = XDS110_MIN_TCK_SPEED;
1807         }
1808
1809         /* The default is the maximum frequency the XDS110 can support */
1810         uint32_t freq_to_use = XDS110_MAX_TCK_SPEED * 1000; /* Hz */
1811         uint32_t delay_count = 0;
1812
1813         if (XDS110_MAX_TCK_SPEED != speed) {
1814                 freq_to_use = speed * 1000; /* Hz */
1815
1816                 /* Calculate the delay count value */
1817                 double one_giga = 1000000000;
1818                 /* Get the pulse duration for the maximum frequency supported in ns */
1819                 double max_freq_pulse_duration = one_giga /
1820                         (XDS110_MAX_TCK_SPEED * 1000);
1821
1822                 /* Convert frequency to pulse duration */
1823                 double freq_to_pulse_width_in_ns = one_giga / freq_to_use;
1824
1825                 /*
1826                  * Start with the pulse duration for the maximum frequency. Keep
1827                  * decrementing the time added by each count value till the requested
1828                  * frequency pulse is less than the calculated value.
1829                  */
1830                 double current_value = max_freq_pulse_duration;
1831
1832                 while (current_value < freq_to_pulse_width_in_ns) {
1833                         current_value += XDS110_TCK_PULSE_INCREMENT;
1834                         ++delay_count;
1835                 }
1836
1837                 /*
1838                  * Determine which delay count yields the best match.
1839                  * The one obtained above or one less.
1840                  */
1841                 if (delay_count) {
1842                         double diff_freq_1 = freq_to_use -
1843                                 (one_giga / (max_freq_pulse_duration +
1844                                 (XDS110_TCK_PULSE_INCREMENT * delay_count)));
1845                         double diff_freq_2 = (one_giga / (max_freq_pulse_duration +
1846                                 (XDS110_TCK_PULSE_INCREMENT * (delay_count - 1)))) -
1847                                 freq_to_use;
1848
1849                         /* One less count value yields a better match */
1850                         if (diff_freq_1 > diff_freq_2)
1851                                 --delay_count;
1852                 }
1853         }
1854
1855         /* Send the delay count to the XDS110 firmware */
1856         success = xds_set_tck_delay(delay_count);
1857
1858         if (success) {
1859                 xds110.delay_count = delay_count;
1860                 xds110.speed = speed;
1861         }
1862
1863         return (success) ? ERROR_OK : ERROR_FAIL;
1864 }
1865
1866 static int xds110_speed_div(int speed, int *khz)
1867 {
1868         *khz = speed;
1869         return ERROR_OK;
1870 }
1871
1872 static int xds110_khz(int khz, int *jtag_speed)
1873 {
1874         *jtag_speed = khz;
1875         return ERROR_OK;
1876 }
1877
1878 static int_least32_t xds110_swd_frequency(int_least32_t hz)
1879 {
1880         if (hz > 0)
1881                 xds110_speed(hz / 1000);
1882         return hz;
1883 }
1884
1885 COMMAND_HANDLER(xds110_handle_info_command)
1886 {
1887         xds110_show_info();
1888         return ERROR_OK;
1889 }
1890
1891 COMMAND_HANDLER(xds110_handle_serial_command)
1892 {
1893         wchar_t serial[XDS110_SERIAL_LEN + 1];
1894
1895         xds110.serial[0] = 0;
1896
1897         if (CMD_ARGC == 1) {
1898                 size_t len = mbstowcs(0, CMD_ARGV[0], 0);
1899                 if (len > XDS110_SERIAL_LEN) {
1900                         LOG_ERROR("XDS110: serial number is limited to %d characters",
1901                                 XDS110_SERIAL_LEN);
1902                         return ERROR_FAIL;
1903                 }
1904                 if ((size_t)-1 == mbstowcs(serial, CMD_ARGV[0], len + 1)) {
1905                         LOG_ERROR("XDS110: unable to convert serial number");
1906                         return ERROR_FAIL;
1907                 }
1908
1909                 for (uint32_t i = 0; i < len; i++)
1910                         xds110.serial[i] = (char)serial[i];
1911
1912                 xds110.serial[len] = 0;
1913         } else {
1914                 LOG_ERROR("XDS110: expected exactly one argument to xds110_serial "
1915                         "<serial-number>");
1916                 return ERROR_FAIL;
1917         }
1918
1919         return ERROR_OK;
1920 }
1921
1922 static const struct command_registration xds110_subcommand_handlers[] = {
1923         {
1924                 .name = "info",
1925                 .handler = &xds110_handle_info_command,
1926                 .mode = COMMAND_EXEC,
1927                 .usage = "",
1928                 .help = "show XDS110 info",
1929         },
1930         COMMAND_REGISTRATION_DONE
1931 };
1932
1933 static const struct command_registration xds110_command_handlers[] = {
1934         {
1935                 .name = "xds110",
1936                 .mode = COMMAND_ANY,
1937                 .help = "perform XDS110 management",
1938                 .usage = "<cmd>",
1939                 .chain = xds110_subcommand_handlers,
1940         },
1941         {
1942                 .name = "xds110_serial",
1943                 .handler = &xds110_handle_serial_command,
1944                 .mode = COMMAND_CONFIG,
1945                 .help = "set the XDS110 probe serial number",
1946                 .usage = "serial_string",
1947         },
1948         COMMAND_REGISTRATION_DONE
1949 };
1950
1951 static const struct swd_driver xds110_swd_driver = {
1952         .init = xds110_swd_init,
1953         .frequency = xds110_swd_frequency,
1954         .switch_seq = xds110_swd_switch_seq,
1955         .read_reg = xds110_swd_read_reg,
1956         .write_reg = xds110_swd_write_reg,
1957         .run = xds110_swd_run_queue,
1958 };
1959
1960 static const char * const xds110_transport[] = { "swd", "jtag", NULL };
1961
1962 struct jtag_interface xds110_interface = {
1963         .name = "xds110",
1964         .commands = xds110_command_handlers,
1965         .swd = &xds110_swd_driver,
1966         .transports = xds110_transport,
1967
1968         .execute_queue = xds110_execute_queue,
1969         .speed = xds110_speed,
1970         .speed_div = xds110_speed_div,
1971         .khz = xds110_khz,
1972         .init = xds110_init,
1973         .quit = xds110_quit,
1974 };