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

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
   6  *   oyvind.harboe@zylin.com                                               *
   7  *                                                                         *
   8  *   Copyright (C) 2008 Rob Brown, Lou Deluxe                              *
   9  *   rob@cobbleware.com, lou.openocd012@fixit.nospammail.net               *
  10  *                                                                         *
  11  *   This program is free software; you can redistribute it and/or modify  *
  12  *   it under the terms of the GNU General Public License as published by  *
  13  *   the Free Software Foundation; either version 2 of the License, or     *
  14  *   (at your option) any later version.                                   *
  15  *                                                                         *
  16  *   This program is distributed in the hope that it will be useful,       *
  17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  19  *   GNU General Public License for more details.                          *
  20  *                                                                         *
  21  *   You should have received a copy of the GNU General Public License     *
  22  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  23  ***************************************************************************/
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include "config.h"
  27 #endif
  28
  29 /* project specific includes */
  30 #include <jtag/interface.h>
  31 #include <jtag/commands.h>
  32 #include "rlink.h"
  33 #include "rlink_st7.h"
  34 #include "rlink_ep1_cmd.h"
  35 #include "rlink_dtc_cmd.h"
  36 #include "libusb_helper.h"
  37
  38 /* This feature is made useless by running the DTC all the time.  When automatic, the LED is on
  39  *whenever the DTC is running.  Otherwise, USB messages are sent to turn it on and off. */
  40 #undef AUTOMATIC_BUSY_LED
  41
  42 /* This feature may require derating the speed due to reduced hold time. */
  43 #undef USE_HARDWARE_SHIFTER_FOR_TMS
  44
  45 #define INTERFACE_NAME          "RLink"
  46
  47 #define USB_IDVENDOR            (0x138e)
  48 #define USB_IDPRODUCT           (0x9000)
  49
  50 #define USB_EP1OUT_ADDR         (0x01)
  51 #define USB_EP1OUT_SIZE         (16)
  52 #define USB_EP1IN_ADDR          (USB_EP1OUT_ADDR | 0x80)
  53 #define USB_EP1IN_SIZE          (USB_EP1OUT_SIZE)
  54
  55 #define USB_EP2OUT_ADDR         (0x02)
  56 #define USB_EP2OUT_SIZE         (64)
  57 #define USB_EP2IN_ADDR          (USB_EP2OUT_ADDR | 0x80)
  58 #define USB_EP2IN_SIZE          (USB_EP2OUT_SIZE)
  59 #define USB_EP2BANK_SIZE        (512)
  60
  61 #define USB_TIMEOUT_MS          (3 * 1000)
  62
  63 #define DTC_STATUS_POLL_BYTE    (ST7_USB_BUF_EP0OUT + 0xff)
  64
  65 #define ST7_PD_NBUSY_LED                ST7_PD0
  66 #define ST7_PD_NRUN_LED                 ST7_PD1
  67 /* low enables VPP at adapter header, high connects it to GND instead */
  68 #define ST7_PD_VPP_SEL                  ST7_PD6
  69 /* low: VPP = 12v, high: VPP <= 5v */
  70 #define ST7_PD_VPP_SHDN                 ST7_PD7
  71
  72 /* These pins are connected together */
  73 #define ST7_PE_ADAPTER_SENSE_IN         ST7_PE3
  74 #define ST7_PE_ADAPTER_SENSE_OUT        ST7_PE4
  75
  76 /* Symbolic mapping between port pins and numbered IO lines */
  77 #define ST7_PA_IO1      ST7_PA1
  78 #define ST7_PA_IO2      ST7_PA2
  79 #define ST7_PA_IO4      ST7_PA4
  80 #define ST7_PA_IO8      ST7_PA6
  81 #define ST7_PA_IO10     ST7_PA7
  82 #define ST7_PB_IO5      ST7_PB5
  83 #define ST7_PC_IO9      ST7_PC1
  84 #define ST7_PC_IO3      ST7_PC2
  85 #define ST7_PC_IO7      ST7_PC3
  86 #define ST7_PE_IO6      ST7_PE5
  87
  88 /* Symbolic mapping between numbered IO lines and adapter signals */
  89 #define ST7_PA_RTCK     ST7_PA_IO0
  90 #define ST7_PA_NTRST    ST7_PA_IO1
  91 #define ST7_PC_TDI      ST7_PC_IO3
  92 #define ST7_PA_DBGRQ    ST7_PA_IO4
  93 #define ST7_PB_NSRST    ST7_PB_IO5
  94 #define ST7_PE_TMS      ST7_PE_IO6
  95 #define ST7_PC_TCK      ST7_PC_IO7
  96 #define ST7_PC_TDO      ST7_PC_IO9
  97 #define ST7_PA_DBGACK   ST7_PA_IO10
  98
  99 static libusb_device_handle *pHDev;
 100
 101 /*
 102  * ep1 commands are up to USB_EP1OUT_SIZE bytes in length.
 103  * This function takes care of zeroing the unused bytes before sending the packet.
 104  * Any reply packet is not handled by this function.
 105  */
 106 static int ep1_generic_commandl(libusb_device_handle *pHDev_param, size_t length, ...)
 107 {
 108         uint8_t usb_buffer[USB_EP1OUT_SIZE];
 109         uint8_t *usb_buffer_p;
 110         va_list ap;
 111         int usb_ret;
 112         int transferred;
 113
 114         if (length > sizeof(usb_buffer))
 115                 length = sizeof(usb_buffer);
 116
 117         usb_buffer_p = usb_buffer;
 118
 119         va_start(ap, length);
 120         while (length > 0) {
 121                 *usb_buffer_p++ = va_arg(ap, int);
 122                 length--;
 123         }
 124
 125         memset(
 126                 usb_buffer_p,
 127                 0,
 128                 sizeof(usb_buffer) - (usb_buffer_p - usb_buffer)
 129                 );
 130
 131         usb_ret = jtag_libusb_bulk_write(
 132                         pHDev_param,
 133                         USB_EP1OUT_ADDR,
 134                         (char *)usb_buffer, sizeof(usb_buffer),
 135                         USB_TIMEOUT_MS,
 136                         &transferred
 137                         );
 138
 139         if (usb_ret != ERROR_OK)
 140                 return usb_ret;
 141         return transferred;
 142 }
 143
 144 #if 0
 145 static ssize_t ep1_memory_read(
 146         libusb_device_handle *pHDev_param, uint16_t addr,
 147         size_t length, uint8_t *buffer)
 148 {
 149         uint8_t usb_buffer[USB_EP1OUT_SIZE];
 150         int usb_ret;
 151         size_t remain;
 152         ssize_t count;
 153         int transferred;
 154
 155         usb_buffer[0] = EP1_CMD_MEMORY_READ;
 156         memset(
 157                 usb_buffer + 4,
 158                 0,
 159                 sizeof(usb_buffer) - 4
 160                 );
 161
 162         remain = length;
 163         count = 0;
 164
 165         while (remain) {
 166                 if (remain > sizeof(usb_buffer))
 167                         length = sizeof(usb_buffer);
 168                 else
 169                         length = remain;
 170
 171                 usb_buffer[1] = addr >> 8;
 172                 usb_buffer[2] = addr;
 173                 usb_buffer[3] = length;
 174
 175                 usb_ret = jtag_libusb_bulk_write(
 176                                 pHDev_param, USB_EP1OUT_ADDR,
 177                                 (char *)usb_buffer, sizeof(usb_buffer),
 178                                 USB_TIMEOUT_MS,
 179                                 &transferred
 180                                 );
 181
 182                 if (usb_ret != ERROR_OK || transferred < (int)sizeof(usb_buffer))
 183                         break;
 184
 185                 usb_ret = jtag_libusb_bulk_read(
 186                                 pHDev_param, USB_EP1IN_ADDR,
 187                                 (char *)buffer, length,
 188                                 USB_TIMEOUT_MS,
 189                                 &transferred
 190                                 );
 191
 192                 if (usb_ret != ERROR_OK || transferred < (int)length)
 193                         break;
 194
 195                 addr += length;
 196                 buffer += length;
 197                 count += length;
 198                 remain -= length;
 199         }
 200
 201         return count;
 202 }
 203 #endif
 204
 205 static ssize_t ep1_memory_write(libusb_device_handle *pHDev_param, uint16_t addr,
 206         size_t length, uint8_t const *buffer)
 207 {
 208         uint8_t usb_buffer[USB_EP1OUT_SIZE];
 209         int usb_ret;
 210         size_t remain;
 211         ssize_t count;
 212
 213         usb_buffer[0] = EP1_CMD_MEMORY_WRITE;
 214
 215         remain = length;
 216         count = 0;
 217
 218         while (remain) {
 219                 if (remain > (sizeof(usb_buffer) - 4))
 220                         length = (sizeof(usb_buffer) - 4);
 221                 else
 222                         length = remain;
 223
 224                 usb_buffer[1] = addr >> 8;
 225                 usb_buffer[2] = addr;
 226                 usb_buffer[3] = length;
 227                 memcpy(
 228                         usb_buffer + 4,
 229                         buffer,
 230                         length
 231                         );
 232                 memset(
 233                         usb_buffer + 4 + length,
 234                         0,
 235                         sizeof(usb_buffer) - 4 - length
 236                         );
 237
 238                 int transferred;
 239
 240                 usb_ret = jtag_libusb_bulk_write(
 241                                 pHDev_param, USB_EP1OUT_ADDR,
 242                                 (char *)usb_buffer, sizeof(usb_buffer),
 243                                 USB_TIMEOUT_MS,
 244                                 &transferred
 245                                 );
 246
 247                 if (usb_ret != ERROR_OK || transferred < (int)sizeof(usb_buffer))
 248                         break;
 249
 250                 addr += length;
 251                 buffer += length;
 252                 count += length;
 253                 remain -= length;
 254         }
 255
 256         return count;
 257 }
 258
 259
 260 #if 0
 261 static ssize_t ep1_memory_writel(libusb_device_handle *pHDev_param, uint16_t addr,
 262         size_t length, ...)
 263 {
 264         uint8_t buffer[USB_EP1OUT_SIZE - 4];
 265         uint8_t *buffer_p;
 266         va_list ap;
 267         size_t remain;
 268
 269         if (length > sizeof(buffer))
 270                 length = sizeof(buffer);
 271
 272         remain = length;
 273         buffer_p = buffer;
 274
 275         va_start(ap, length);
 276         while (remain > 0) {
 277                 *buffer_p++ = va_arg(ap, int);
 278                 remain--;
 279         }
 280
 281         return ep1_memory_write(pHDev_param, addr, length, buffer);
 282 }
 283 #endif
 284
 285 #define DTCLOAD_COMMENT         (0)
 286 #define DTCLOAD_ENTRY           (1)
 287 #define DTCLOAD_LOAD            (2)
 288 #define DTCLOAD_RUN                     (3)
 289 #define DTCLOAD_LUT_START       (4)
 290 #define DTCLOAD_LUT                     (5)
 291
 292 #define DTC_LOAD_BUFFER         ST7_USB_BUF_EP2UIDO
 293
 294 /* This gets set by the DTC loader */
 295 static uint8_t dtc_entry_download;
 296
 297 /* The buffer is specially formatted to represent a valid image to load into the DTC. */
 298 static int dtc_load_from_buffer(libusb_device_handle *pHDev_param, const uint8_t *buffer,
 299                 size_t length)
 300 {
 301         struct header_s {
 302                 uint8_t type;
 303                 uint8_t length;
 304         };
 305
 306         int usb_err;
 307         struct header_s *header;
 308         uint8_t lut_start = 0xc0;
 309
 310         dtc_entry_download = 0;
 311
 312         /* Stop the DTC before loading anything. */
 313         usb_err = ep1_generic_commandl(
 314                         pHDev_param, 1,
 315                         EP1_CMD_DTC_STOP
 316                         );
 317         if (usb_err < 0)
 318                 return usb_err;
 319
 320         while (length) {
 321                 if (length < sizeof(*header)) {
 322                         LOG_ERROR("Malformed DTC image");
 323                         exit(1);
 324                 }
 325
 326                 header = (struct header_s *)buffer;
 327                 buffer += sizeof(*header);
 328                 length -= sizeof(*header);
 329
 330                 if (length < (size_t)header->length + 1) {
 331                         LOG_ERROR("Malformed DTC image");
 332                         exit(1);
 333                 }
 334
 335                 switch (header->type) {
 336                         case DTCLOAD_COMMENT:
 337                                 break;
 338
 339                         case DTCLOAD_ENTRY:
 340                                 /* store entry addresses somewhere */
 341                                 if (!strncmp("download", (char *)buffer + 1, 8))
 342                                         dtc_entry_download = buffer[0];
 343                                 break;
 344
 345                         case DTCLOAD_LOAD:
 346                                 /* Send the DTC program to ST7 RAM. */
 347                                 usb_err = ep1_memory_write(
 348                                                 pHDev_param,
 349                                                 DTC_LOAD_BUFFER,
 350                                                 header->length + 1, buffer
 351                                         );
 352                                 if (usb_err < 0)
 353                                         return usb_err;
 354
 355                                 /* Load it into the DTC. */
 356                                 usb_err = ep1_generic_commandl(
 357                                                 pHDev_param, 3,
 358                                                 EP1_CMD_DTC_LOAD,
 359                                                 (DTC_LOAD_BUFFER >> 8),
 360                                                 DTC_LOAD_BUFFER
 361                                         );
 362                                 if (usb_err < 0)
 363                                         return usb_err;
 364
 365                                 break;
 366
 367                         case DTCLOAD_RUN:
 368                                 usb_err = ep1_generic_commandl(
 369                                                 pHDev_param, 3,
 370                                                 EP1_CMD_DTC_CALL,
 371                                                 buffer[0],
 372                                                 EP1_CMD_DTC_WAIT
 373                                         );
 374                                 if (usb_err < 0)
 375                                         return usb_err;
 376
 377                                 break;
 378
 379                         case DTCLOAD_LUT_START:
 380                                 lut_start = buffer[0];
 381                                 break;
 382
 383                         case DTCLOAD_LUT:
 384                                 usb_err = ep1_memory_write(
 385                                                 pHDev_param,
 386                                                 ST7_USB_BUF_EP0OUT + lut_start,
 387                                                 header->length + 1, buffer
 388                                         );
 389                                 if (usb_err < 0)
 390                                         return usb_err;
 391                                 break;
 392
 393                         default:
 394                                 LOG_ERROR("Invalid DTC image record type: 0x%02x", header->type);
 395                                 exit(1);
 396                                 break;
 397                 }
 398
 399                 buffer += (header->length + 1);
 400                 length -= (header->length + 1);
 401         }
 402
 403         return 0;
 404 }
 405
 406 /*
 407  * Start the DTC running in download mode (waiting for 512 byte command packets on ep2).
 408  */
 409 static int dtc_start_download(void)
 410 {
 411         int usb_err;
 412         uint8_t ep2txr;
 413         int transferred;
 414
 415         /* set up for download mode and make sure EP2 is set up to transmit */
 416         usb_err = ep1_generic_commandl(
 417                         pHDev, 7,
 418
 419                         EP1_CMD_DTC_STOP,
 420                         EP1_CMD_SET_UPLOAD,
 421                         EP1_CMD_SET_DOWNLOAD,
 422                         EP1_CMD_MEMORY_READ,    /* read EP2TXR for its data toggle */
 423                         ST7_EP2TXR >> 8,
 424                         ST7_EP2TXR,
 425                         1
 426                         );
 427         if (usb_err < 0)
 428                 return usb_err;
 429
 430         /* read back ep2txr */
 431         usb_err = jtag_libusb_bulk_read(
 432                         pHDev, USB_EP1IN_ADDR,
 433                         (char *)&ep2txr, 1,
 434                         USB_TIMEOUT_MS,
 435                         &transferred
 436                         );
 437         if (usb_err != ERROR_OK)
 438                 return usb_err;
 439
 440         usb_err = ep1_generic_commandl(
 441                         pHDev, 13,
 442
 443                         EP1_CMD_MEMORY_WRITE,   /* preinitialize poll byte */
 444                         DTC_STATUS_POLL_BYTE >> 8,
 445                         DTC_STATUS_POLL_BYTE,
 446                         1,
 447                         0x00,
 448                         EP1_CMD_MEMORY_WRITE,   /* set EP2IN to return data */
 449                         ST7_EP2TXR >> 8,
 450                         ST7_EP2TXR,
 451                         1,
 452                         (ep2txr & ST7_EP2TXR_DTOG_TX) | ST7_EP2TXR_STAT_VALID,
 453                         EP1_CMD_DTC_CALL,       /* start running the DTC */
 454                         dtc_entry_download,
 455                         EP1_CMD_DTC_GET_CACHED_STATUS
 456                         );
 457         if (usb_err < 0)
 458                 return usb_err;
 459
 460         /* wait for completion */
 461         usb_err = jtag_libusb_bulk_read(
 462                         pHDev, USB_EP1IN_ADDR,
 463                         (char *)&ep2txr, 1,
 464                         USB_TIMEOUT_MS,
 465                         &transferred
 466                         );
 467
 468         return usb_err;
 469 }
 470
 471 static int dtc_run_download(
 472         libusb_device_handle *pHDev_param,
 473         uint8_t *command_buffer,
 474         int command_buffer_size,
 475         uint8_t *reply_buffer,
 476         int reply_buffer_size
 477         )
 478 {
 479         char dtc_status;
 480         int usb_err;
 481         int i;
 482         int transferred;
 483
 484         LOG_DEBUG("%d/%d", command_buffer_size, reply_buffer_size);
 485
 486         usb_err = jtag_libusb_bulk_write(
 487                         pHDev_param,
 488                         USB_EP2OUT_ADDR,
 489                         (char *)command_buffer, USB_EP2BANK_SIZE,
 490                         USB_TIMEOUT_MS,
 491                         &transferred
 492                         );
 493         if (usb_err < 0)
 494                 return usb_err;
 495
 496
 497         /* Wait for DTC to finish running command buffer */
 498         for (i = 50;; ) {
 499                 usb_err = ep1_generic_commandl(
 500                                 pHDev_param, 4,
 501
 502                                 EP1_CMD_MEMORY_READ,
 503                                 DTC_STATUS_POLL_BYTE >> 8,
 504                                 DTC_STATUS_POLL_BYTE,
 505                                 1
 506                                 );
 507                 if (usb_err < 0)
 508                         return usb_err;
 509
 510                 usb_err = jtag_libusb_bulk_read(
 511                                 pHDev_param,
 512                                 USB_EP1IN_ADDR,
 513                                 &dtc_status, 1,
 514                                 USB_TIMEOUT_MS,
 515                                 &transferred
 516                                 );
 517                 if (usb_err < 0)
 518                         return usb_err;
 519
 520                 if (dtc_status & 0x01)
 521                         break;
 522
 523                 if (!--i) {
 524                         LOG_ERROR("too many retries waiting for DTC status");
 525                         return LIBUSB_ERROR_TIMEOUT;
 526                 }
 527         }
 528
 529
 530         if (reply_buffer && reply_buffer_size) {
 531                 usb_err = jtag_libusb_bulk_read(
 532                                 pHDev_param,
 533                                 USB_EP2IN_ADDR,
 534                                 (char *)reply_buffer, reply_buffer_size,
 535                                 USB_TIMEOUT_MS,
 536                                 &transferred
 537                                 );
 538
 539                 if (usb_err != ERROR_OK || transferred < reply_buffer_size) {
 540                         LOG_ERROR("Read of endpoint 2 returned %d, expected %d",
 541                                 usb_err, reply_buffer_size
 542                                 );
 543                         return usb_err;
 544                 }
 545         }
 546
 547         return usb_err;
 548 }
 549
 550 /*
 551  * The dtc reply queue is a singly linked list that describes what to do
 552  * with the reply packet that comes from the DTC.  Only SCAN_IN and SCAN_IO generate
 553  * these entries.
 554  */
 555
 556 struct dtc_reply_queue_entry {
 557         struct dtc_reply_queue_entry *next;
 558         struct jtag_command *cmd;       /* the command that resulted in this entry */
 559
 560         struct {
 561                 uint8_t *buffer;                /* the scan buffer */
 562                 int size;                       /* size of the scan buffer in bits */
 563                 int offset;                     /* how many bits were already done before this? */
 564                 int length;                     /* how many bits are processed in this operation? */
 565                 enum scan_type type;            /* SCAN_IN/SCAN_OUT/SCAN_IO */
 566         } scan;
 567 };
 568
 569
 570 /*
 571  * The dtc_queue consists of a buffer of pending commands and a reply queue.
 572  * rlink_scan and tap_state_run add to the command buffer and maybe to the reply queue.
 573  */
 574
 575 static struct {
 576         struct dtc_reply_queue_entry *rq_head;
 577         struct dtc_reply_queue_entry *rq_tail;
 578         uint32_t cmd_index;
 579         uint32_t reply_index;
 580         uint8_t cmd_buffer[USB_EP2BANK_SIZE];
 581 } dtc_queue;
 582
 583 /*
 584  * The tap state queue is for accumulating TAP state changes without needlessly
 585  * flushing the dtc_queue.  When it fills or is run, it adds the accumulated bytes to
 586  * the dtc_queue.
 587  */
 588
 589 static struct {
 590         uint32_t length;
 591         uint32_t buffer;
 592 } tap_state_queue;
 593
 594 static int dtc_queue_init(void)
 595 {
 596         dtc_queue.rq_head = NULL;
 597         dtc_queue.rq_tail = NULL;
 598         dtc_queue.cmd_index = 0;
 599         dtc_queue.reply_index = 0;
 600         return 0;
 601 }
 602
 603 static inline struct dtc_reply_queue_entry *dtc_queue_enqueue_reply(
 604         enum scan_type type, uint8_t *buffer, int size, int offset,
 605         int length, struct jtag_command *cmd)
 606 {
 607         struct dtc_reply_queue_entry *rq_entry;
 608
 609         rq_entry = malloc(sizeof(struct dtc_reply_queue_entry));
 610         if (rq_entry != NULL) {
 611                 rq_entry->scan.type = type;
 612                 rq_entry->scan.buffer = buffer;
 613                 rq_entry->scan.size = size;
 614                 rq_entry->scan.offset = offset;
 615                 rq_entry->scan.length = length;
 616                 rq_entry->cmd = cmd;
 617                 rq_entry->next = NULL;
 618
 619                 if (dtc_queue.rq_head == NULL)
 620                         dtc_queue.rq_head = rq_entry;
 621                 else
 622                         dtc_queue.rq_tail->next = rq_entry;
 623
 624                 dtc_queue.rq_tail = rq_entry;
 625         }
 626
 627         return rq_entry;
 628 }
 629
 630 /*
 631  * Running the queue means that any pending command buffer is run
 632  * and any reply data dealt with.  The command buffer is then cleared for subsequent processing.
 633  * The queue is automatically run by append when it is necessary to get space for the append.
 634  */
 635
 636 static int dtc_queue_run(void)
 637 {
 638         struct dtc_reply_queue_entry *rq_p, *rq_next;
 639         int retval;
 640         int usb_err;
 641         int bit_cnt;
 642         int x;
 643         uint8_t *dtc_p, *tdo_p;
 644         uint8_t dtc_mask, tdo_mask;
 645         uint8_t reply_buffer[USB_EP2IN_SIZE];
 646
 647         assert((dtc_queue.rq_head != 0) == (dtc_queue.reply_index > 0));
 648         assert(dtc_queue.cmd_index < USB_EP2BANK_SIZE);
 649         assert(dtc_queue.reply_index <= USB_EP2IN_SIZE);
 650
 651         retval = ERROR_OK;
 652
 653         if (dtc_queue.cmd_index < 1)
 654                 return retval;
 655
 656         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = DTC_CMD_STOP;
 657
 658         usb_err = dtc_run_download(pHDev,
 659                         dtc_queue.cmd_buffer, dtc_queue.cmd_index,
 660                         reply_buffer, sizeof(reply_buffer)
 661                         );
 662         if (usb_err < 0) {
 663                 LOG_ERROR("dtc_run_download: %s", libusb_error_name(usb_err));
 664                 exit(1);
 665         }
 666
 667         if (dtc_queue.rq_head != NULL) {
 668                 /* process the reply, which empties the reply queue and frees its entries */
 669                 dtc_p = reply_buffer;
 670
 671                 /* The rigamarole with the masks and doing it bit-by-bit is due to the fact that the
 672                  *scan buffer is LSb-first and the DTC code is MSb-first for hardware reasons.   It
 673                  *was that or craft a function to do the reversal, and that wouldn't work with
 674                  *bit-stuffing (supplying extra bits to use mostly byte operations), or any other
 675                  *scheme which would throw the byte alignment off. */
 676
 677                 for (
 678                         rq_p = dtc_queue.rq_head;
 679                         rq_p != NULL;
 680                         rq_p = rq_next
 681                         ) {
 682                         tdo_p = rq_p->scan.buffer + (rq_p->scan.offset / 8);
 683                         tdo_mask = 1 << (rq_p->scan.offset % 8);
 684
 685
 686                         bit_cnt = rq_p->scan.length;
 687                         if (bit_cnt >= 8) {
 688                                 /* bytes */
 689
 690                                 dtc_mask = 1 << (8 - 1);
 691
 692                                 for (
 693                                         ;
 694                                         bit_cnt;
 695                                         bit_cnt--
 696                                         ) {
 697                                         if (*dtc_p & dtc_mask)
 698                                                 *tdo_p |= tdo_mask;
 699                                         else
 700                                                 *tdo_p &= ~tdo_mask;
 701
 702                                         dtc_mask >>= 1;
 703                                         if (dtc_mask == 0) {
 704                                                 dtc_p++;
 705                                                 dtc_mask = 1 << (8 - 1);
 706                                         }
 707
 708                                         tdo_mask <<= 1;
 709                                         if (tdo_mask == 0) {
 710                                                 tdo_p++;
 711                                                 tdo_mask = 1;
 712                                         }
 713                                 }
 714                         } else {
 715                                 /*  extra bits or last bit */
 716
 717                                 x = *dtc_p++;
 718                                 if ((rq_p->scan.type == SCAN_IN) && (
 719                                                 rq_p->scan.offset != rq_p->scan.size - 1
 720                                         )) {
 721                                         /* extra bits were sent as a full byte with padding on the
 722                                          *end */
 723                                         dtc_mask = 1 << (8 - 1);
 724                                 } else
 725                                         dtc_mask = 1 << (bit_cnt - 1);
 726
 727                                 for (
 728                                         ;
 729                                         bit_cnt;
 730                                         bit_cnt--
 731                                         ) {
 732                                         if (x & dtc_mask)
 733                                                 *tdo_p |= tdo_mask;
 734                                         else
 735                                                 *tdo_p &= ~tdo_mask;
 736
 737                                         dtc_mask >>= 1;
 738
 739                                         tdo_mask <<= 1;
 740                                         if (tdo_mask == 0) {
 741                                                 tdo_p++;
 742                                                 tdo_mask = 1;
 743                                         }
 744
 745                                 }
 746                         }
 747
 748                         if ((rq_p->scan.offset + rq_p->scan.length) >= rq_p->scan.size) {
 749                                 /* feed scan buffer back into openocd and free it */
 750                                 if (jtag_read_buffer(rq_p->scan.buffer,
 751                                                 rq_p->cmd->cmd.scan) != ERROR_OK)
 752                                         retval = ERROR_JTAG_QUEUE_FAILED;
 753                                 free(rq_p->scan.buffer);
 754                         }
 755
 756                         rq_next = rq_p->next;
 757                         free(rq_p);
 758                 }
 759                 dtc_queue.rq_head = NULL;
 760                 dtc_queue.rq_tail = NULL;
 761         }
 762
 763         /* reset state for new appends */
 764         dtc_queue.cmd_index = 0;
 765         dtc_queue.reply_index = 0;
 766
 767         return retval;
 768 }
 769
 770 /* runs the queue if it cannot take reserved_cmd bytes of command data
 771  * or reserved_reply bytes of reply data */
 772 static int dtc_queue_run_if_full(int reserved_cmd, int reserved_reply)
 773 {
 774         /* reserve one additional byte for the STOP cmd appended during run */
 775         if (dtc_queue.cmd_index + reserved_cmd + 1 > USB_EP2BANK_SIZE)
 776                 return dtc_queue_run();
 777
 778         if (dtc_queue.reply_index + reserved_reply > USB_EP2IN_SIZE)
 779                 return dtc_queue_run();
 780
 781         return ERROR_OK;
 782 }
 783
 784 static int tap_state_queue_init(void)
 785 {
 786         tap_state_queue.length = 0;
 787         tap_state_queue.buffer = 0;
 788         return 0;
 789 }
 790
 791 static int tap_state_queue_run(void)
 792 {
 793         int i;
 794         int bits;
 795         uint8_t byte_param;
 796         int retval;
 797
 798         retval = 0;
 799         if (!tap_state_queue.length)
 800                 return retval;
 801         bits = 1;
 802         byte_param = 0;
 803         for (i = tap_state_queue.length; i--; ) {
 804
 805                 byte_param <<= 1;
 806                 if (tap_state_queue.buffer & 1)
 807                         byte_param |= 1;
 808                 if ((bits >= 8) || !i) {
 809                         byte_param <<= (8 - bits);
 810
 811                         /* make sure there's room for two cmd bytes */
 812                         dtc_queue_run_if_full(2, 0);
 813
 814 #ifdef USE_HARDWARE_SHIFTER_FOR_TMS
 815                         if (bits == 8) {
 816                                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
 817                                         DTC_CMD_SHIFT_TMS_BYTES(1);
 818                         } else {
 819 #endif
 820                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
 821                                 DTC_CMD_SHIFT_TMS_BITS(bits);
 822 #ifdef USE_HARDWARE_SHIFTER_FOR_TMS
 823                 }
 824 #endif
 825
 826                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
 827                                 byte_param;
 828
 829                         byte_param = 0;
 830                         bits = 1;
 831                 } else
 832                         bits++;
 833
 834                 tap_state_queue.buffer >>= 1;
 835         }
 836         retval = tap_state_queue_init();
 837         return retval;
 838 }
 839
 840 static int tap_state_queue_append(uint8_t tms)
 841 {
 842         int retval;
 843
 844         if (tap_state_queue.length >= sizeof(tap_state_queue.buffer) * 8) {
 845                 retval = tap_state_queue_run();
 846                 if (retval != 0)
 847                         return retval;
 848         }
 849
 850         if (tms)
 851                 tap_state_queue.buffer |= (1 << tap_state_queue.length);
 852         tap_state_queue.length++;
 853
 854         return 0;
 855 }
 856
 857 static void rlink_end_state(tap_state_t state)
 858 {
 859         if (tap_is_state_stable(state))
 860                 tap_set_end_state(state);
 861         else {
 862                 LOG_ERROR("BUG: %i is not a valid end state", state);
 863                 exit(-1);
 864         }
 865 }
 866
 867 static void rlink_state_move(void)
 868 {
 869
 870         int i = 0, tms = 0;
 871         uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 872         int tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
 873
 874         for (i = 0; i < tms_count; i++) {
 875                 tms = (tms_scan >> i) & 1;
 876                 tap_state_queue_append(tms);
 877         }
 878
 879         tap_set_state(tap_get_end_state());
 880 }
 881
 882 static void rlink_path_move(struct pathmove_command *cmd)
 883 {
 884         int num_states = cmd->num_states;
 885         int state_count;
 886         int tms = 0;
 887
 888         state_count = 0;
 889         while (num_states) {
 890                 if (tap_state_transition(tap_get_state(), false) == cmd->path[state_count])
 891                         tms = 0;
 892                 else if (tap_state_transition(tap_get_state(), true) == cmd->path[state_count])
 893                         tms = 1;
 894                 else {
 895                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition",
 896                                 tap_state_name(tap_get_state()),
 897                                 tap_state_name(cmd->path[state_count]));
 898                         exit(-1);
 899                 }
 900
 901                 tap_state_queue_append(tms);
 902
 903                 tap_set_state(cmd->path[state_count]);
 904                 state_count++;
 905                 num_states--;
 906         }
 907
 908         tap_set_end_state(tap_get_state());
 909 }
 910
 911 static void rlink_runtest(int num_cycles)
 912 {
 913         int i;
 914
 915         tap_state_t saved_end_state = tap_get_end_state();
 916
 917         /* only do a state_move when we're not already in RTI */
 918         if (tap_get_state() != TAP_IDLE) {
 919                 rlink_end_state(TAP_IDLE);
 920                 rlink_state_move();
 921         }
 922
 923         /* execute num_cycles */
 924         for (i = 0; i < num_cycles; i++)
 925                 tap_state_queue_append(0);
 926
 927         /* finish in end_state */
 928         rlink_end_state(saved_end_state);
 929         if (tap_get_state() != tap_get_end_state())
 930                 rlink_state_move();
 931 }
 932
 933 /* (1) assert or (0) deassert reset lines */
 934 static void rlink_reset(int trst, int srst)
 935 {
 936         uint8_t bitmap;
 937         int usb_err;
 938         int transferred;
 939
 940         /* Read port A for bit op */
 941         usb_err = ep1_generic_commandl(
 942                         pHDev, 4,
 943                         EP1_CMD_MEMORY_READ,
 944                         ST7_PADR >> 8,
 945                         ST7_PADR,
 946                         1
 947                         );
 948         if (usb_err < 0) {
 949                 LOG_ERROR("%s", libusb_error_name(usb_err));
 950                 exit(1);
 951         }
 952
 953         usb_err = jtag_libusb_bulk_read(
 954                         pHDev, USB_EP1IN_ADDR,
 955                         (char *)&bitmap, 1,
 956                         USB_TIMEOUT_MS,
 957                         &transferred
 958                         );
 959         if (usb_err != ERROR_OK || transferred < 1) {
 960                 LOG_ERROR("%s", libusb_error_name(usb_err));
 961                 exit(1);
 962         }
 963
 964         if (trst)
 965                 bitmap &= ~ST7_PA_NTRST;
 966         else
 967                 bitmap |= ST7_PA_NTRST;
 968
 969         /* Write port A and read port B for bit op
 970          * port B has no OR, and we want to emulate open drain on NSRST, so we initialize DR to 0
 971          *and assert NSRST by setting DDR to 1. */
 972         usb_err = ep1_generic_commandl(
 973                         pHDev, 9,
 974                         EP1_CMD_MEMORY_WRITE,
 975                         ST7_PADR >> 8,
 976                         ST7_PADR,
 977                         1,
 978                         bitmap,
 979                         EP1_CMD_MEMORY_READ,
 980                         ST7_PBDDR >> 8,
 981                         ST7_PBDDR,
 982                         1
 983                         );
 984         if (usb_err < 0) {
 985                 LOG_ERROR("%s", libusb_error_name(usb_err));
 986                 exit(1);
 987         }
 988
 989         usb_err = jtag_libusb_bulk_read(
 990                         pHDev, USB_EP1IN_ADDR,
 991                         (char *)&bitmap, 1,
 992                         USB_TIMEOUT_MS,
 993                         &transferred
 994                         );
 995         if (usb_err != ERROR_OK || transferred < 1) {
 996                 LOG_ERROR("%s", libusb_error_name(usb_err));
 997                 exit(1);
 998         }
 999
1000         if (srst)
1001                 bitmap |= ST7_PB_NSRST;
1002         else
1003                 bitmap &= ~ST7_PB_NSRST;
1004
1005         /* write port B and read dummy to ensure completion before returning */
1006         usb_err = ep1_generic_commandl(
1007                         pHDev, 6,
1008                         EP1_CMD_MEMORY_WRITE,
1009                         ST7_PBDDR >> 8,
1010                         ST7_PBDDR,
1011                         1,
1012                         bitmap,
1013                         EP1_CMD_DTC_GET_CACHED_STATUS
1014                         );
1015         if (usb_err < 0) {
1016                 LOG_ERROR("%s", libusb_error_name(usb_err));
1017                 exit(1);
1018         }
1019
1020         usb_err = jtag_libusb_bulk_read(
1021                         pHDev, USB_EP1IN_ADDR,
1022                         (char *)&bitmap, 1,
1023                         USB_TIMEOUT_MS,
1024                         &transferred
1025                         );
1026         if (usb_err != ERROR_OK || transferred < 1) {
1027                 LOG_ERROR("%s", libusb_error_name(usb_err));
1028                 exit(1);
1029         }
1030 }
1031
1032 static int rlink_scan(struct jtag_command *cmd, enum scan_type type,
1033                 uint8_t *buffer, int scan_size)
1034 {
1035         bool ir_scan;
1036         tap_state_t saved_end_state;
1037         int byte_bits;
1038         int extra_bits;
1039         int chunk_bits;
1040         int chunk_bytes;
1041         int x;
1042
1043         int tdi_bit_offset;
1044         uint8_t tdi_mask, *tdi_p;
1045         uint8_t dtc_mask;
1046
1047         if (scan_size < 1) {
1048                 LOG_ERROR("scan_size cannot be less than 1 bit");
1049                 exit(1);
1050         }
1051
1052         ir_scan = cmd->cmd.scan->ir_scan;
1053
1054         /* Move to the proper state before starting to shift TDI/TDO. */
1055         if (!((!ir_scan && (tap_get_state() == TAP_DRSHIFT)) ||
1056                         (ir_scan && (tap_get_state() == TAP_IRSHIFT)))) {
1057                 saved_end_state = tap_get_end_state();
1058                 rlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
1059                 rlink_state_move();
1060                 rlink_end_state(saved_end_state);
1061         }
1062
1063         tap_state_queue_run();
1064
1065
1066 #if 0
1067         printf("scan_size = %d, type = 0x%x\n", scan_size, type);
1068         {
1069                 int i;
1070
1071                 /* clear unused bits in scan buffer for ease of debugging
1072                  * (it makes diffing output easier) */
1073                 buffer[scan_size / 8] &= ((1 << ((scan_size - 1) % 8) + 1) - 1);
1074
1075                 printf("before scan:");
1076                 for (i = 0; i < (scan_size + 7) / 8; i++)
1077                         printf(" %02x", buffer[i]);
1078                 printf("\n");
1079         }
1080 #endif
1081
1082         /* The number of bits that can be shifted as complete bytes */
1083         byte_bits = (int)(scan_size - 1) / 8 * 8;
1084         /* The number of bits left over, not counting the last bit */
1085         extra_bits = (scan_size - 1) - byte_bits;
1086
1087         tdi_bit_offset = 0;
1088         tdi_p = buffer;
1089         tdi_mask = 1;
1090
1091         if (extra_bits && (type == SCAN_OUT)) {
1092                 /* Schedule any extra bits into the DTC command buffer, padding as needed
1093                  * For SCAN_OUT, this comes before the full bytes so the (leading) padding bits will
1094                  *fall off the end */
1095
1096                 /* make sure there's room for two cmd bytes */
1097                 dtc_queue_run_if_full(2, 0);
1098
1099                 x = 0;
1100                 dtc_mask = 1 << (extra_bits - 1);
1101
1102                 while (extra_bits--) {
1103                         if (*tdi_p & tdi_mask)
1104                                 x |= dtc_mask;
1105
1106                         dtc_mask >>= 1;
1107
1108                         tdi_mask <<= 1;
1109                         if (tdi_mask == 0) {
1110                                 tdi_p++;
1111                                 tdi_mask = 1;
1112                         }
1113                 }
1114
1115                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1116                         DTC_CMD_SHIFT_TDI_BYTES(1);
1117
1118                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1119         }
1120
1121         /* Loop scheduling full bytes into the DTC command buffer */
1122         while (byte_bits) {
1123                 /* make sure there's room for one (for in scans) or two cmd bytes and
1124                  * at least one reply byte for in or inout scans*/
1125                 dtc_queue_run_if_full(type == SCAN_IN ? 1 : 2, type != SCAN_OUT ? 1 : 0);
1126
1127                 chunk_bits = byte_bits;
1128                 /* we can only use up to 16 bytes at a time */
1129                 if (chunk_bits > (16 * 8))
1130                         chunk_bits = (16 * 8);
1131
1132                 if (type != SCAN_IN) {
1133                         /* how much is there room for, considering stop and byte op? */
1134                         x = (sizeof(dtc_queue.cmd_buffer) - (dtc_queue.cmd_index + 1 + 1)) * 8;
1135                         if (chunk_bits > x)
1136                                 chunk_bits = x;
1137                 }
1138
1139                 if (type != SCAN_OUT) {
1140                         /* how much is there room for in the reply buffer? */
1141                         x = (USB_EP2IN_SIZE - dtc_queue.reply_index) * 8;
1142                         if (chunk_bits > x)
1143                                 chunk_bits = x;
1144                 }
1145
1146                 /* so the loop will end */
1147                 byte_bits -= chunk_bits;
1148
1149                 if (type != SCAN_OUT) {
1150                         if (dtc_queue_enqueue_reply(
1151                                         type, buffer, scan_size, tdi_bit_offset,
1152                                         chunk_bits,
1153                                         cmd
1154                                 ) == NULL) {
1155                                 LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
1156                                 exit(1);
1157                         }
1158                         dtc_queue.reply_index += (chunk_bits + 7) / 8;
1159
1160                         tdi_bit_offset += chunk_bits;
1161                 }
1162
1163                 /* chunk_bits is a multiple of 8, so there are no rounding issues. */
1164                 chunk_bytes = chunk_bits / 8;
1165
1166                 switch (type) {
1167                         case SCAN_IN:
1168                                 x = DTC_CMD_SHIFT_TDO_BYTES(chunk_bytes);
1169                                 break;
1170                         case SCAN_OUT:
1171                                 x = DTC_CMD_SHIFT_TDI_BYTES(chunk_bytes);
1172                                 break;
1173                         default:
1174                                 x = DTC_CMD_SHIFT_TDIO_BYTES(chunk_bytes);
1175                                 break;
1176                 }
1177                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1178
1179                 if (type != SCAN_IN) {
1180                         x = 0;
1181                         dtc_mask = 1 << (8 - 1);
1182
1183                         while (chunk_bits--) {
1184                                 if (*tdi_p & tdi_mask)
1185                                         x |= dtc_mask;
1186
1187                                 dtc_mask >>= 1;
1188                                 if (dtc_mask == 0) {
1189                                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1190                                         x = 0;
1191                                         dtc_mask = 1 << (8 - 1);
1192                                 }
1193
1194                                 tdi_mask <<= 1;
1195                                 if (tdi_mask == 0) {
1196                                         tdi_p++;
1197                                         tdi_mask = 1;
1198                                 }
1199                         }
1200                 }
1201         }
1202
1203         if (extra_bits && (type != SCAN_OUT)) {
1204                 /* Schedule any extra bits into the DTC command buffer */
1205
1206                 /* make sure there's room for one (for in scans) or two cmd bytes
1207                  * and one reply byte */
1208                 dtc_queue_run_if_full(type == SCAN_IN ? 1 : 2, 1);
1209
1210                 if (dtc_queue_enqueue_reply(
1211                                 type, buffer, scan_size, tdi_bit_offset,
1212                                 extra_bits,
1213                                 cmd
1214                         ) == NULL) {
1215                         LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
1216                         exit(1);
1217                 }
1218
1219                 dtc_queue.reply_index++;
1220
1221                 tdi_bit_offset += extra_bits;
1222
1223                 if (type == SCAN_IN) {
1224                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1225                                 DTC_CMD_SHIFT_TDO_BYTES(1);
1226
1227                 } else {
1228                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1229                                 DTC_CMD_SHIFT_TDIO_BITS(extra_bits);
1230
1231                         x = 0;
1232                         dtc_mask = 1 << (8 - 1);
1233
1234                         while (extra_bits--) {
1235                                 if (*tdi_p & tdi_mask)
1236                                         x |= dtc_mask;
1237
1238                                 dtc_mask >>= 1;
1239
1240                                 tdi_mask <<= 1;
1241                                 if (tdi_mask == 0) {
1242                                         tdi_p++;
1243                                         tdi_mask = 1;
1244                                 }
1245                         }
1246
1247                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1248                 }
1249         }
1250
1251         /* Schedule the last bit into the DTC command buffer */
1252
1253         /* make sure there's room for one cmd byte and one reply byte
1254          * for in or inout scans*/
1255         dtc_queue_run_if_full(1, type == SCAN_OUT ? 0 : 1);
1256
1257         if (type == SCAN_OUT) {
1258                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1259                         DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 0);
1260
1261         } else {
1262                 if (dtc_queue_enqueue_reply(
1263                                 type, buffer, scan_size, tdi_bit_offset,
1264                                 1,
1265                                 cmd
1266                                 ) == NULL) {
1267                         LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
1268                         exit(1);
1269                 }
1270
1271                 dtc_queue.reply_index++;
1272
1273                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1274                         DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 1);
1275         }
1276
1277         /* Move to pause state */
1278         tap_state_queue_append(0);
1279         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
1280         if (tap_get_state() != tap_get_end_state())
1281                 rlink_state_move();
1282
1283         return 0;
1284 }
1285
1286 static int rlink_execute_queue(void)
1287 {
1288         struct jtag_command *cmd = jtag_command_queue;  /* currently processed command */
1289         int scan_size;
1290         enum scan_type type;
1291         uint8_t *buffer;
1292         int retval, tmp_retval;
1293
1294         /* return ERROR_OK, unless something goes wrong */
1295         retval = ERROR_OK;
1296
1297 #ifndef AUTOMATIC_BUSY_LED
1298         /* turn LED on */
1299         ep1_generic_commandl(pHDev, 2,
1300                 EP1_CMD_SET_PORTD_LEDS,
1301                 ~(ST7_PD_NBUSY_LED)
1302                 );
1303 #endif
1304
1305         while (cmd) {
1306                 switch (cmd->type) {
1307                         case JTAG_RUNTEST:
1308                         case JTAG_TLR_RESET:
1309                         case JTAG_PATHMOVE:
1310                         case JTAG_SCAN:
1311                                 break;
1312
1313                         default:
1314                                 /* some events, such as resets, need a queue flush to ensure
1315                                  *consistency */
1316                                 tap_state_queue_run();
1317                                 dtc_queue_run();
1318                                 break;
1319                 }
1320
1321                 switch (cmd->type) {
1322                         case JTAG_RESET:
1323                                 LOG_DEBUG_IO("reset trst: %i srst %i",
1324                                                 cmd->cmd.reset->trst,
1325                                                 cmd->cmd.reset->srst);
1326                                 if ((cmd->cmd.reset->trst == 1) ||
1327                                                 (cmd->cmd.reset->srst &&
1328                                                                 (jtag_get_reset_config() & RESET_SRST_PULLS_TRST)))
1329                                         tap_set_state(TAP_RESET);
1330                                 rlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
1331                                 break;
1332                         case JTAG_RUNTEST:
1333                                 LOG_DEBUG_IO("runtest %i cycles, end in %i",
1334                                                 cmd->cmd.runtest->num_cycles,
1335                                                 cmd->cmd.runtest->end_state);
1336                                 if (cmd->cmd.runtest->end_state != -1)
1337                                         rlink_end_state(cmd->cmd.runtest->end_state);
1338                                 rlink_runtest(cmd->cmd.runtest->num_cycles);
1339                                 break;
1340                         case JTAG_TLR_RESET:
1341                                 LOG_DEBUG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
1342                                 if (cmd->cmd.statemove->end_state != -1)
1343                                         rlink_end_state(cmd->cmd.statemove->end_state);
1344                                 rlink_state_move();
1345                                 break;
1346                         case JTAG_PATHMOVE:
1347                                 LOG_DEBUG_IO("pathmove: %i states, end in %i",
1348                                                 cmd->cmd.pathmove->num_states,
1349                                                 cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1350                                 rlink_path_move(cmd->cmd.pathmove);
1351                                 break;
1352                         case JTAG_SCAN:
1353                                 LOG_DEBUG_IO("%s scan end in %i",
1354                                                 (cmd->cmd.scan->ir_scan) ? "IR" : "DR",
1355                                                                 cmd->cmd.scan->end_state);
1356                                 if (cmd->cmd.scan->end_state != -1)
1357                                         rlink_end_state(cmd->cmd.scan->end_state);
1358                                 scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
1359                                 type = jtag_scan_type(cmd->cmd.scan);
1360                                 if (rlink_scan(cmd, type, buffer, scan_size) != ERROR_OK)
1361                                         retval = ERROR_FAIL;
1362                                 break;
1363                         case JTAG_SLEEP:
1364                                 LOG_DEBUG_IO("sleep %" PRIu32, cmd->cmd.sleep->us);
1365                                 jtag_sleep(cmd->cmd.sleep->us);
1366                                 break;
1367                         default:
1368                                 LOG_ERROR("BUG: unknown JTAG command type encountered");
1369                                 exit(-1);
1370                 }
1371                 cmd = cmd->next;
1372         }
1373
1374         /* Flush the DTC queue to make sure any pending reads have been done before exiting this
1375          *function */
1376         tap_state_queue_run();
1377         tmp_retval = dtc_queue_run();
1378         if (tmp_retval != ERROR_OK)
1379                 retval = tmp_retval;
1380
1381 #ifndef AUTOMATIC_BUSY_LED
1382         /* turn LED off */
1383         ep1_generic_commandl(pHDev, 2,
1384                 EP1_CMD_SET_PORTD_LEDS,
1385                 ~0
1386                 );
1387 #endif
1388
1389         return retval;
1390 }
1391
1392 /* Using an unindexed table because it is infrequently accessed and it is short.  The table must be
1393  *in order of ascending speed (and descending prescaler), as it is scanned in reverse. */
1394
1395 static int rlink_speed(int speed)
1396 {
1397         int i;
1398
1399         if (speed == 0) {
1400                 /* fastest speed */
1401                 speed = rlink_speed_table[rlink_speed_table_size - 1].prescaler;
1402         }
1403
1404         for (i = rlink_speed_table_size; i--; ) {
1405                 if (rlink_speed_table[i].prescaler == speed) {
1406                         if (dtc_load_from_buffer(pHDev, rlink_speed_table[i].dtc,
1407                                         rlink_speed_table[i].dtc_size) != 0) {
1408                                 LOG_ERROR(
1409                                         "An error occurred while trying to load DTC code for speed \"%d\".",
1410                                         speed);
1411                                 exit(1);
1412                         }
1413
1414                         int ret = dtc_start_download();
1415                         if (ret < 0) {
1416                                 LOG_ERROR("starting DTC: %s", libusb_error_name(ret));
1417                                 exit(1);
1418                         }
1419
1420                         return ERROR_OK;
1421                 }
1422         }
1423
1424         LOG_ERROR("%d is not a supported speed", speed);
1425         return ERROR_FAIL;
1426 }
1427
1428 static int rlink_speed_div(int speed, int *khz)
1429 {
1430         int i;
1431
1432         for (i = rlink_speed_table_size; i--; ) {
1433                 if (rlink_speed_table[i].prescaler == speed) {
1434                         *khz = rlink_speed_table[i].khz;
1435                         return ERROR_OK;
1436                 }
1437         }
1438
1439         LOG_ERROR("%d is not a supported speed", speed);
1440         return ERROR_FAIL;
1441 }
1442
1443 static int rlink_khz(int khz, int *speed)
1444 {
1445         int i;
1446
1447         if (khz == 0) {
1448                 LOG_ERROR("RCLK not supported");
1449                 return ERROR_FAIL;
1450         }
1451
1452         for (i = rlink_speed_table_size; i--; ) {
1453                 if (rlink_speed_table[i].khz <= khz) {
1454                         *speed = rlink_speed_table[i].prescaler;
1455                         return ERROR_OK;
1456                 }
1457         }
1458
1459         LOG_WARNING("The lowest supported JTAG speed is %d KHz", rlink_speed_table[0].khz);
1460         *speed = rlink_speed_table[0].prescaler;
1461         return ERROR_OK;
1462 }
1463
1464 static int rlink_init(void)
1465 {
1466         int i, j, retries;
1467         uint8_t reply_buffer[USB_EP1IN_SIZE];
1468         int transferred;
1469
1470         const uint16_t vids[] = { USB_IDVENDOR, 0 };
1471         const uint16_t pids[] = { USB_IDPRODUCT, 0 };
1472         if (jtag_libusb_open(vids, pids, NULL, &pHDev, NULL) != ERROR_OK)
1473                 return ERROR_FAIL;
1474
1475         struct libusb_device_descriptor descriptor;
1476         struct libusb_device *usb_dev = libusb_get_device(pHDev);
1477         int r = libusb_get_device_descriptor(usb_dev, &descriptor);
1478         if (r < 0) {
1479                 LOG_ERROR("error %d getting device descriptor", r);
1480                 return ERROR_FAIL;
1481         }
1482
1483         if (descriptor.bNumConfigurations > 1) {
1484                 LOG_ERROR("Whoops! NumConfigurations is not 1, don't know what to do...");
1485                 return ERROR_FAIL;
1486         }
1487         struct libusb_config_descriptor *config;
1488         libusb_get_config_descriptor(usb_dev, 0, &config);
1489         if (config->bNumInterfaces > 1) {
1490                 LOG_ERROR("Whoops! NumInterfaces is not 1, don't know what to do...");
1491                 return ERROR_FAIL;
1492         }
1493
1494         LOG_DEBUG("Opened device, pHDev = %p", pHDev);
1495
1496         /* usb_set_configuration required under win32 */
1497         libusb_set_configuration(pHDev, config->bConfigurationValue);
1498
1499         retries = 3;
1500         do {
1501                 i = libusb_claim_interface(pHDev, 0);
1502                 if (i != LIBUSB_SUCCESS) {
1503                         LOG_ERROR("usb_claim_interface: %s", libusb_error_name(i));
1504                         j = libusb_detach_kernel_driver(pHDev, 0);
1505                         if (j != LIBUSB_SUCCESS)
1506                                 LOG_ERROR("detach kernel driver: %s", libusb_error_name(j));
1507                 } else {
1508                         LOG_DEBUG("interface claimed!");
1509                         break;
1510                 }
1511         } while (--retries);
1512
1513         if (i != LIBUSB_SUCCESS) {
1514                 LOG_ERROR("Initialisation failed.");
1515                 return ERROR_FAIL;
1516         }
1517         if (libusb_set_interface_alt_setting(pHDev, 0, 0) != LIBUSB_SUCCESS) {
1518                 LOG_ERROR("Failed to set interface.");
1519                 return ERROR_FAIL;
1520         }
1521
1522         /* The device starts out in an unknown state on open.  As such,
1523          * result reads time out, and it's not even known whether the
1524          * command was accepted.  So, for this first command, we issue
1525          * it repeatedly until its response doesn't time out.  Also, if
1526          * sending a command is going to time out, we find that out here.
1527          *
1528          * It must be possible to open the device in such a way that
1529          * this special magic isn't needed, but, so far, it escapes us.
1530          */
1531         for (i = 0; i < 5; i++) {
1532                 j = ep1_generic_commandl(
1533                                 pHDev, 1,
1534                                 EP1_CMD_GET_FWREV
1535                                 );
1536                 if (j < USB_EP1OUT_SIZE) {
1537                         LOG_ERROR("USB write error: %s", libusb_error_name(j));
1538                         return ERROR_FAIL;
1539                 }
1540                 j = jtag_libusb_bulk_read(
1541                                 pHDev, USB_EP1IN_ADDR,
1542                                 (char *)reply_buffer, sizeof(reply_buffer),
1543                                 200,
1544                                 &transferred
1545                                 );
1546                 if (j != LIBUSB_ERROR_TIMEOUT)
1547                         break;
1548         }
1549
1550         if (j != ERROR_OK || transferred != (int)sizeof(reply_buffer)) {
1551                 LOG_ERROR("USB read error: %s", libusb_error_name(j));
1552                 return ERROR_FAIL;
1553         }
1554         LOG_DEBUG(INTERFACE_NAME " firmware version: %d.%d.%d",
1555                 reply_buffer[0],
1556                 reply_buffer[1],
1557                 reply_buffer[2]);
1558
1559         if ((reply_buffer[0] != 0) || (reply_buffer[1] != 0) || (reply_buffer[2] != 3))
1560                 LOG_WARNING(
1561                         "The rlink device is not of the version that the developers have played with.  It may or may not work.");
1562
1563         /* Probe port E for adapter presence */
1564         ep1_generic_commandl(
1565                 pHDev, 16,
1566                 EP1_CMD_MEMORY_WRITE,           /* Drive sense pin with 0 */
1567                 ST7_PEDR >> 8,
1568                 ST7_PEDR,
1569                 3,
1570                 0x00,                                                   /* DR */
1571                 ST7_PE_ADAPTER_SENSE_OUT,               /* DDR */
1572                 ST7_PE_ADAPTER_SENSE_OUT,               /* OR */
1573                 EP1_CMD_MEMORY_READ,            /* Read back */
1574                 ST7_PEDR >> 8,
1575                 ST7_PEDR,
1576                 1,
1577                 EP1_CMD_MEMORY_WRITE,           /* Drive sense pin with 1 */
1578                 ST7_PEDR >> 8,
1579                 ST7_PEDR,
1580                 1,
1581                 ST7_PE_ADAPTER_SENSE_OUT
1582                 );
1583
1584         jtag_libusb_bulk_read(
1585                 pHDev, USB_EP1IN_ADDR,
1586                 (char *)reply_buffer, 1,
1587                 USB_TIMEOUT_MS,
1588                 &transferred
1589                 );
1590
1591         if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) != 0)
1592                 LOG_WARNING("target detection problem");
1593
1594         ep1_generic_commandl(
1595                 pHDev, 11,
1596                 EP1_CMD_MEMORY_READ,            /* Read back */
1597                 ST7_PEDR >> 8,
1598                 ST7_PEDR,
1599                 1,
1600                 EP1_CMD_MEMORY_WRITE,           /* float port E */
1601                 ST7_PEDR >> 8,
1602                 ST7_PEDR,
1603                 3,
1604                 0x00,           /* DR */
1605                 0x00,           /* DDR */
1606                 0x00            /* OR */
1607                 );
1608
1609         jtag_libusb_bulk_read(
1610                 pHDev, USB_EP1IN_ADDR,
1611                 (char *)reply_buffer, 1,
1612                 USB_TIMEOUT_MS,
1613                 &transferred
1614                 );
1615
1616
1617         if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) == 0)
1618                 LOG_WARNING("target not plugged in");
1619
1620         /* float ports A and B */
1621         ep1_generic_commandl(
1622                 pHDev, 11,
1623                 EP1_CMD_MEMORY_WRITE,
1624                 ST7_PADDR >> 8,
1625                 ST7_PADDR,
1626                 2,
1627                 0x00,
1628                 0x00,
1629                 EP1_CMD_MEMORY_WRITE,
1630                 ST7_PBDDR >> 8,
1631                 ST7_PBDDR,
1632                 1,
1633                 0x00
1634                 );
1635
1636         /* make sure DTC is stopped, set VPP control, set up ports A and B */
1637         ep1_generic_commandl(
1638                 pHDev, 14,
1639                 EP1_CMD_DTC_STOP,
1640                 EP1_CMD_SET_PORTD_VPP,
1641                 ~(ST7_PD_VPP_SHDN),
1642                 EP1_CMD_MEMORY_WRITE,
1643                 ST7_PADR >> 8,
1644                 ST7_PADR,
1645                 2,
1646                 ((~(0)) & (ST7_PA_NTRST)),
1647                 (ST7_PA_NTRST),
1648                 /* port B has no OR, and we want to emulate open drain on NSRST, so we set DR to 0
1649                  *here and later assert NSRST by setting DDR bit to 1. */
1650                 EP1_CMD_MEMORY_WRITE,
1651                 ST7_PBDR >> 8,
1652                 ST7_PBDR,
1653                 1,
1654                 0x00
1655                 );
1656
1657         /* set LED updating mode and make sure they're unlit */
1658         ep1_generic_commandl(
1659                 pHDev, 3,
1660 #ifdef AUTOMATIC_BUSY_LED
1661                 EP1_CMD_LEDUE_BUSY,
1662 #else
1663                 EP1_CMD_LEDUE_NONE,
1664 #endif
1665                 EP1_CMD_SET_PORTD_LEDS,
1666                 ~0
1667                 );
1668
1669         tap_state_queue_init();
1670         dtc_queue_init();
1671         rlink_reset(0, 0);
1672
1673         return ERROR_OK;
1674 }
1675
1676 static int rlink_quit(void)
1677 {
1678         /* stop DTC and make sure LEDs are off */
1679         ep1_generic_commandl(
1680                 pHDev, 6,
1681                 EP1_CMD_DTC_STOP,
1682                 EP1_CMD_LEDUE_NONE,
1683                 EP1_CMD_SET_PORTD_LEDS,
1684                 ~0,
1685                 EP1_CMD_SET_PORTD_VPP,
1686                 ~0
1687                 );
1688
1689         libusb_release_interface(pHDev, 0);
1690         libusb_close(pHDev);
1691
1692         return ERROR_OK;
1693 }
1694
1695 static struct jtag_interface rlink_interface = {
1696         .execute_queue = rlink_execute_queue,
1697 };
1698
1699 struct adapter_driver rlink_adapter_driver = {
1700         .name = "rlink",
1701         .transports = jtag_only,
1702
1703         .init = rlink_init,
1704         .quit = rlink_quit,
1705         .speed = rlink_speed,
1706         .khz = rlink_khz,
1707         .speed_div = rlink_speed_div,
1708
1709         .jtag_ops = &rlink_interface,
1710 };