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