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

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