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