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