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