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