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