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