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