git.sur5r.net Git - 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 "jtag.h"
  32 #include "rlink.h"
  33 #include "st7.h"
  34 #include "ep1_cmd.h"
  35 #include "dtc_cmd.h"
  36
  37 /* system includes */
  38 #include <usb.h>
  39 #include <stdint.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 u8                *buffer,
 331         size_t                  length
 332 ) {
 333         struct header_s {
 334                 u8      type;
 335                 u8      length;
 336         };
 337
 338         int                             usb_err;
 339         struct header_s *header;
 340         u8                              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         u8      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         u8      *command_buffer,
 501         int     command_buffer_size,
 502         u8      *reply_buffer,
 503         int     reply_buffer_size
 504 ) {
 505         u8      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 = 5;;) {
 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                 u8              *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         u32                     cmd_index;
 604         u32                     reply_index;
 605         u8                      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         u32     length;
 616         u32     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         u8                              *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         u8                      *dtc_p, *tdo_p;
 681         u8                      dtc_mask, tdo_mask;
 682         u8                      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         u8      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         u8      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         u8 tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 920
 921         for (i = 0; i < 7; i++)
 922         {
 923                 tms = (tms_scan >> i) & 1;
 924                 tap_state_queue_append(tms);
 925         }
 926
 927         tap_set_state(tap_get_end_state());
 928 }
 929
 930 static
 931 void rlink_path_move(pathmove_command_t *cmd)
 932 {
 933         int num_states = cmd->num_states;
 934         int state_count;
 935         int tms = 0;
 936
 937         state_count = 0;
 938         while (num_states)
 939         {
 940                 if (tap_state_transition(tap_get_state(), false) == cmd->path[state_count])
 941                 {
 942                         tms = 0;
 943                 }
 944                 else if (tap_state_transition(tap_get_state(), true) == cmd->path[state_count])
 945                 {
 946                         tms = 1;
 947                 }
 948                 else
 949                 {
 950                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(tap_get_state()), tap_state_name(cmd->path[state_count]));
 951                         exit(-1);
 952                 }
 953
 954                 tap_state_queue_append(tms);
 955
 956                 tap_set_state(cmd->path[state_count]);
 957                 state_count++;
 958                 num_states--;
 959         }
 960
 961         tap_set_end_state(tap_get_state());
 962 }
 963
 964
 965 static
 966 void rlink_runtest(int num_cycles)
 967 {
 968         int i;
 969
 970         tap_state_t saved_end_state = tap_get_end_state();
 971
 972         /* only do a state_move when we're not already in RTI */
 973         if (tap_get_state() != TAP_IDLE)
 974         {
 975                 rlink_end_state(TAP_IDLE);
 976                 rlink_state_move();
 977         }
 978
 979         /* execute num_cycles */
 980         for (i = 0; i < num_cycles; i++)
 981         {
 982                 tap_state_queue_append(0);
 983         }
 984
 985         /* finish in end_state */
 986         rlink_end_state(saved_end_state);
 987         if (tap_get_state() != tap_get_end_state())
 988                 rlink_state_move();
 989 }
 990
 991
 992 /* (1) assert or (0) deassert reset lines */
 993 static
 994 void rlink_reset(int trst, int srst)
 995 {
 996         u8                      bitmap;
 997         int                     usb_err;
 998
 999         /* Read port A for bit op */
1000         usb_err = ep1_generic_commandl(
1001                 pHDev, 4,
1002                 EP1_CMD_MEMORY_READ,
1003                         ST7_PADR >> 8,
1004                         ST7_PADR,
1005                         1
1006         );
1007         if(usb_err < 0) {
1008                 LOG_ERROR("%s", usb_strerror());
1009                 exit(1);
1010         }
1011
1012         usb_err = usb_bulk_read(
1013                 pHDev, USB_EP1IN_ADDR,
1014                 (char *)&bitmap, 1,
1015                 USB_TIMEOUT_MS
1016         );
1017         if(usb_err < 1) {
1018                 LOG_ERROR("%s", usb_strerror());
1019                 exit(1);
1020         }
1021
1022         if(trst) {
1023                 bitmap &= ~ST7_PA_NTRST;
1024         } else {
1025                 bitmap |= ST7_PA_NTRST;
1026         }
1027
1028         /* Write port A and read port B for bit op */
1029         /* 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. */
1030         usb_err = ep1_generic_commandl(
1031                 pHDev, 9,
1032                 EP1_CMD_MEMORY_WRITE,
1033                         ST7_PADR >> 8,
1034                         ST7_PADR,
1035                         1,
1036                         bitmap,
1037                 EP1_CMD_MEMORY_READ,
1038                         ST7_PBDDR >> 8,
1039                         ST7_PBDDR,
1040                         1
1041         );
1042         if(usb_err < 0) {
1043                 LOG_ERROR("%s", usb_strerror());
1044                 exit(1);
1045         }
1046
1047         usb_err = usb_bulk_read(
1048                 pHDev, USB_EP1IN_ADDR,
1049                 (char *)&bitmap, 1,
1050                 USB_TIMEOUT_MS
1051         );
1052         if(usb_err < 1) {
1053                 LOG_ERROR("%s", usb_strerror());
1054                 exit(1);
1055         }
1056
1057         if(srst) {
1058                 bitmap |= ST7_PB_NSRST;
1059         } else {
1060                 bitmap &= ~ST7_PB_NSRST;
1061         }
1062
1063         /* write port B and read dummy to ensure completion before returning */
1064         usb_err = ep1_generic_commandl(
1065                 pHDev, 6,
1066                 EP1_CMD_MEMORY_WRITE,
1067                         ST7_PBDDR >> 8,
1068                         ST7_PBDDR,
1069                         1,
1070                         bitmap,
1071                 EP1_CMD_DTC_GET_CACHED_STATUS
1072         );
1073         if(usb_err < 0) {
1074                 LOG_ERROR("%s", usb_strerror());
1075                 exit(1);
1076         }
1077
1078         usb_err = usb_bulk_read(
1079                 pHDev, USB_EP1IN_ADDR,
1080                 (char *)&bitmap, 1,
1081                 USB_TIMEOUT_MS
1082         );
1083         if(usb_err < 1) {
1084                 LOG_ERROR("%s", usb_strerror());
1085                 exit(1);
1086         }
1087 }
1088
1089
1090 static
1091 int
1092 rlink_scan(
1093         jtag_command_t  *cmd,
1094         enum scan_type  type,
1095         u8                      *buffer,
1096         int                     scan_size
1097 ) {
1098         int                     ir_scan;
1099         tap_state_t     saved_end_state;
1100         int                     byte_bits;
1101         int                     extra_bits;
1102         int                     chunk_bits;
1103         int                     chunk_bytes;
1104         int                     x;
1105
1106         int                     tdi_bit_offset;
1107         u8                      tdi_mask, *tdi_p;
1108         u8                      dtc_mask;
1109
1110         if(scan_size < 1) {
1111                 LOG_ERROR("scan_size cannot be less than 1 bit\n");
1112                 exit(1);
1113         }
1114
1115         ir_scan = cmd->cmd.scan->ir_scan;
1116
1117         /* Move to the proper state before starting to shift TDI/TDO. */
1118         if (!(
1119                 (!ir_scan && (tap_get_state() == TAP_DRSHIFT))
1120                 ||
1121                 (ir_scan && (tap_get_state() == TAP_IRSHIFT))
1122         )) {
1123                 saved_end_state = tap_get_end_state();
1124                 rlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
1125                 rlink_state_move();
1126                 rlink_end_state(saved_end_state);
1127         }
1128
1129         tap_state_queue_run();
1130
1131
1132 #if 0
1133         printf("scan_size = %d, type=0x%x\n", scan_size, type);
1134         {
1135                 int   i;
1136
1137                 /* clear unused bits in scan buffer for ease of debugging */
1138                 /* (it makes diffing output easier) */
1139                 buffer[scan_size / 8] &= ((1 << ((scan_size - 1) % 8) + 1) - 1);
1140
1141                 printf("before scan:");
1142                 for(i = 0; i < (scan_size + 7) / 8; i++) {
1143                         printf(" %02x", buffer[i]);
1144                 }
1145                 printf("\n");
1146         }
1147 #endif
1148
1149         /* The number of bits that can be shifted as complete bytes */
1150         byte_bits = (int)(scan_size - 1) / 8 * 8;
1151         /* The number of bits left over, not counting the last bit */
1152         extra_bits = (scan_size - 1) - byte_bits;
1153
1154         tdi_bit_offset = 0;
1155         tdi_p = buffer;
1156         tdi_mask = 1;
1157
1158         if(extra_bits && (type == SCAN_OUT)) {
1159                 /* Schedule any extra bits into the DTC command buffer, padding as needed */
1160                 /* For SCAN_OUT, this comes before the full bytes so the (leading) padding bits will fall off the end */
1161                 /* make sure there's room for stop, byte op, and one byte */
1162                 if(
1163                         (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1))
1164                 ) {
1165                         dtc_queue_run();
1166                 }
1167
1168                 x = 0;
1169                 dtc_mask = 1 << (extra_bits - 1);
1170
1171                 while(extra_bits--) {
1172                         if(*tdi_p & tdi_mask) {
1173                                 x |= dtc_mask;
1174                         }
1175
1176                         dtc_mask >>= 1;
1177
1178                         tdi_mask <<= 1;
1179                         if(tdi_mask == 0) {
1180                                 tdi_p++;
1181                                 tdi_mask = 1;
1182                         }
1183                 }
1184
1185                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1186                         DTC_CMD_SHIFT_TDI_BYTES(1);
1187
1188                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1189         }
1190
1191         /* Loop scheduling full bytes into the DTC command buffer */
1192         while(byte_bits) {
1193                 if(type == SCAN_IN) {
1194                         /* make sure there's room for stop and byte op */
1195                         x = (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1));
1196                 } else {
1197                         /* make sure there's room for stop, byte op, and at least one byte */
1198                         x = (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1));
1199                 }
1200
1201                 if(type != SCAN_OUT) {
1202                         /* make sure there's room for at least one reply byte */
1203                         x |= (dtc_queue.reply_index >= USB_EP2IN_SIZE - (1));
1204                 }
1205
1206                 if(x) {
1207                         dtc_queue_run();
1208                 }
1209
1210                 chunk_bits = byte_bits;
1211                 /* we can only use up to 16 bytes at a time */
1212                 if(chunk_bits > (16 * 8)) chunk_bits = (16 * 8);
1213
1214                 if(type != SCAN_IN) {
1215                         /* how much is there room for, considering stop and byte op? */
1216                         x = (sizeof(dtc_queue.cmd_buffer) - (dtc_queue.cmd_index + 1 + 1)) * 8;
1217                         if(chunk_bits > x) chunk_bits = x;
1218                 }
1219
1220                 if(type != SCAN_OUT) {
1221                         /* how much is there room for in the reply buffer? */
1222                         x = (USB_EP2IN_SIZE - dtc_queue.reply_index) * 8;
1223                         if(chunk_bits > x) chunk_bits = x;
1224                 }
1225
1226                 /* so the loop will end */
1227                 byte_bits -= chunk_bits;
1228
1229                 if(type != SCAN_OUT) {
1230                         if(dtc_queue_enqueue_reply(
1231                                 type, buffer, scan_size, tdi_bit_offset,
1232                                 chunk_bits,
1233                                 cmd
1234                         ) == NULL) {
1235                                 LOG_ERROR("enqueuing DTC reply entry: %s\n", strerror(errno));
1236                                 exit(1);
1237                         }
1238
1239                         tdi_bit_offset += chunk_bits;
1240                 }
1241
1242                 /* chunk_bits is a multiple of 8, so there are no rounding issues. */
1243                 chunk_bytes = chunk_bits / 8;
1244
1245                 switch(type) {
1246                         case SCAN_IN:
1247                                 x = DTC_CMD_SHIFT_TDO_BYTES(chunk_bytes);
1248                                 break;
1249                         case SCAN_OUT:
1250                                 x = DTC_CMD_SHIFT_TDI_BYTES(chunk_bytes);
1251                                 break;
1252                         default:
1253                                 x = DTC_CMD_SHIFT_TDIO_BYTES(chunk_bytes);
1254                                 break;
1255                 }
1256                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1257
1258                 if(type != SCAN_IN) {
1259                         x = 0;
1260                         dtc_mask = 1 << (8 - 1);
1261
1262                         while(chunk_bits--) {
1263                                 if(*tdi_p & tdi_mask) {
1264                                         x |= dtc_mask;
1265                                 }
1266
1267                                 dtc_mask >>= 1;
1268                                 if(dtc_mask == 0) {
1269                                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1270                                         dtc_queue.reply_index++;
1271                                         x = 0;
1272                                         dtc_mask = 1 << (8 - 1);
1273                                 }
1274
1275                                 tdi_mask <<= 1;
1276                                 if(tdi_mask == 0) {
1277                                         tdi_p++;
1278                                         tdi_mask = 1;
1279                                 }
1280                         }
1281                 }
1282         }
1283
1284         if(extra_bits && (type != SCAN_OUT)) {
1285                 /* Schedule any extra bits into the DTC command buffer */
1286                 /* make sure there's room for stop, byte op, and one byte */
1287                 if(
1288                         (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1))
1289                         ||
1290                         (dtc_queue.reply_index >= USB_EP2IN_SIZE - (1))
1291                 ) {
1292                         dtc_queue_run();
1293                 }
1294
1295                 if(dtc_queue_enqueue_reply(
1296                         type, buffer, scan_size, tdi_bit_offset,
1297                         extra_bits,
1298                         cmd
1299                 ) == NULL) {
1300                         LOG_ERROR("enqueuing DTC reply entry: %s\n", strerror(errno));
1301                         exit(1);
1302                 }
1303
1304                 tdi_bit_offset += extra_bits;
1305
1306                 if(type == SCAN_IN) {
1307                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1308                                 DTC_CMD_SHIFT_TDO_BYTES(1);
1309
1310                 } else {
1311                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1312                                 DTC_CMD_SHIFT_TDIO_BITS(extra_bits);
1313
1314                         x = 0;
1315                         dtc_mask = 1 << (8 - 1);
1316
1317                         while(extra_bits--) {
1318                                 if(*tdi_p & tdi_mask) {
1319                                         x |= dtc_mask;
1320                                 }
1321
1322                                 dtc_mask >>= 1;
1323
1324                                 tdi_mask <<= 1;
1325                                 if(tdi_mask == 0) {
1326                                         tdi_p++;
1327                                         tdi_mask = 1;
1328                                 }
1329                         }
1330
1331                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1332                 }
1333
1334                 dtc_queue.reply_index++;
1335         }
1336
1337         /* Schedule the last bit into the DTC command buffer */
1338         {
1339                 /* make sure there's room for stop, and bit pair command */
1340                 if(
1341                         (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1))
1342                         ||
1343                         (dtc_queue.reply_index >= USB_EP2IN_SIZE - (1))
1344                 ) {
1345                         dtc_queue_run();
1346                 }
1347
1348                 if(type == SCAN_OUT) {
1349                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1350                                 DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 0);
1351
1352                 } else {
1353                         if(dtc_queue_enqueue_reply(
1354                                 type, buffer, scan_size, tdi_bit_offset,
1355                                 1,
1356                                 cmd
1357                         ) == NULL) {
1358                                 LOG_ERROR("enqueuing DTC reply entry: %s\n", strerror(errno));
1359                                 exit(1);
1360                         }
1361
1362                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1363                                 DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 1);
1364
1365                         dtc_queue.reply_index++;
1366                 }
1367         }
1368
1369         /* Move to pause state */
1370         tap_state_queue_append(0);
1371         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
1372         if (tap_get_state() != tap_get_end_state()) rlink_state_move();
1373
1374         return(0);
1375 }
1376
1377
1378 static
1379 int rlink_execute_queue(void)
1380 {
1381         jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
1382         int scan_size;
1383         enum scan_type type;
1384         u8 *buffer;
1385         int retval, tmp_retval;
1386
1387         /* return ERROR_OK, unless something goes wrong */
1388         retval = ERROR_OK;
1389
1390 #ifndef AUTOMATIC_BUSY_LED
1391         /* turn LED on */
1392         ep1_generic_commandl(pHDev, 2,
1393                 EP1_CMD_SET_PORTD_LEDS,
1394                 ~(ST7_PD_NBUSY_LED)
1395         );
1396 #endif
1397
1398         while (cmd)
1399         {
1400                 switch (cmd->type)
1401                 {
1402                         case JTAG_END_STATE:
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_END_STATE:
1419 #ifdef _DEBUG_JTAG_IO_
1420                                 LOG_DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
1421 #endif
1422                                 if (cmd->cmd.end_state->end_state != -1)
1423                                         rlink_end_state(cmd->cmd.end_state->end_state);
1424                                 break;
1425                         case JTAG_RESET:
1426 #ifdef _DEBUG_JTAG_IO_
1427                                 LOG_DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
1428 #endif
1429                                 if ((cmd->cmd.reset->trst == 1) || (cmd->cmd.reset->srst && (jtag_reset_config & RESET_SRST_PULLS_TRST)))
1430                                 {
1431                                         tap_set_state(TAP_RESET);
1432                                 }
1433                                 rlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
1434                                 break;
1435                         case JTAG_RUNTEST:
1436 #ifdef _DEBUG_JTAG_IO_
1437                                 LOG_DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
1438 #endif
1439                                 if (cmd->cmd.runtest->end_state != -1)
1440                                         rlink_end_state(cmd->cmd.runtest->end_state);
1441                                 rlink_runtest(cmd->cmd.runtest->num_cycles);
1442                                 break;
1443                         case JTAG_STATEMOVE:
1444 #ifdef _DEBUG_JTAG_IO_
1445                                 LOG_DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
1446 #endif
1447                                 if (cmd->cmd.statemove->end_state != -1)
1448                                         rlink_end_state(cmd->cmd.statemove->end_state);
1449                                 rlink_state_move();
1450                                 break;
1451                         case JTAG_PATHMOVE:
1452 #ifdef _DEBUG_JTAG_IO_
1453                                 LOG_DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1454 #endif
1455                                 rlink_path_move(cmd->cmd.pathmove);
1456                                 break;
1457                         case JTAG_SCAN:
1458 #ifdef _DEBUG_JTAG_IO_
1459                                 LOG_DEBUG("%s scan end in %i",  (cmd->cmd.scan->ir_scan) ? "IR" : "DR", cmd->cmd.scan->end_state);
1460 #endif
1461                                 if (cmd->cmd.scan->end_state != -1)
1462                                         rlink_end_state(cmd->cmd.scan->end_state);
1463                                 scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
1464                                 type = jtag_scan_type(cmd->cmd.scan);
1465                                 if(rlink_scan(cmd, type, buffer, scan_size) != ERROR_OK) {
1466                                         retval = ERROR_FAIL;
1467                                 }
1468                                 break;
1469                         case JTAG_SLEEP:
1470 #ifdef _DEBUG_JTAG_IO_
1471                                 LOG_DEBUG("sleep %i", cmd->cmd.sleep->us);
1472 #endif
1473                                 jtag_sleep(cmd->cmd.sleep->us);
1474                                 break;
1475                         default:
1476                                 LOG_ERROR("BUG: unknown JTAG command type encountered");
1477                                 exit(-1);
1478                 }
1479                 cmd = cmd->next;
1480         }
1481
1482         /* Flush the DTC queue to make sure any pending reads have been done before exiting this function */
1483         tap_state_queue_run();
1484         tmp_retval = dtc_queue_run();
1485         if(tmp_retval != ERROR_OK) {
1486                 retval = tmp_retval;
1487         }
1488
1489 #ifndef AUTOMATIC_BUSY_LED
1490         /* turn LED onff */
1491         ep1_generic_commandl(pHDev, 2,
1492                 EP1_CMD_SET_PORTD_LEDS,
1493                 ~0
1494         );
1495 #endif
1496
1497         return retval;
1498 }
1499
1500
1501 /* 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. */
1502
1503 static
1504 int rlink_speed(int speed)
1505 {
1506         int             i;
1507
1508         if(speed == 0) {
1509                 /* fastest speed */
1510                 speed = rlink_speed_table[rlink_speed_table_size - 1].prescaler;
1511         }
1512
1513         for(i = rlink_speed_table_size; i--; ) {
1514                 if(rlink_speed_table[i].prescaler == speed) {
1515                         if(dtc_load_from_buffer(pHDev, rlink_speed_table[i].dtc, rlink_speed_table[i].dtc_size) != 0) {
1516                                 LOG_ERROR("An error occurred while trying to load DTC code for speed \"%d\".\n", speed);
1517                                 exit(1);
1518                         }
1519
1520                         if(dtc_start_download() < 0) {
1521                                 LOG_ERROR("%s, %d: starting DTC: %s",
1522                                         __FILE__, __LINE__,
1523                                         usb_strerror()
1524                                 );
1525                                 exit(1);
1526                         }
1527
1528                         return ERROR_OK;
1529                 }
1530         }
1531
1532         LOG_ERROR("%d is not a supported speed", speed);
1533         return(ERROR_FAIL);
1534 }
1535
1536
1537 static
1538 int rlink_speed_div(
1539         int speed,
1540         int *khz
1541 ) {
1542         int     i;
1543
1544         for(i = rlink_speed_table_size; i--; ) {
1545                 if(rlink_speed_table[i].prescaler == speed) {
1546                         *khz = rlink_speed_table[i].khz;
1547                         return(ERROR_OK);
1548                 }
1549         }
1550
1551         LOG_ERROR("%d is not a supported speed", speed);
1552         return(ERROR_FAIL);
1553 }
1554
1555
1556 static
1557 int rlink_khz(
1558         int khz,
1559         int *speed
1560 ) {
1561         int     i;
1562
1563         if(khz == 0) {
1564                 LOG_ERROR("RCLK not supported");
1565                 return ERROR_FAIL;
1566         }
1567
1568         for(i = rlink_speed_table_size; i--; ) {
1569                 if(rlink_speed_table[i].khz <= khz) {
1570                         *speed = rlink_speed_table[i].prescaler;
1571                         return(ERROR_OK);
1572                 }
1573         }
1574
1575         LOG_WARNING("The lowest supported JTAG speed is %d KHz", rlink_speed_table[0].khz);
1576         *speed = rlink_speed_table[0].prescaler;
1577         return(ERROR_OK);
1578 }
1579
1580
1581 #if 0
1582 static
1583 int
1584 handle_dtc_directory_command(
1585         struct command_context_s *cmd_ctx,
1586         char *cmd,
1587         char **args,
1588         int argc
1589 ) {
1590         if(argc != 1) {
1591                 LOG_ERROR("expected exactly one argument to rlink_dtc_directory <directory-path>");
1592                 return(ERROR_INVALID_ARGUMENTS);
1593         }
1594
1595         printf("handle_dtc_directory_command called with \"%s\"\n", args[0]);
1596
1597         return(ERROR_OK);
1598 }
1599 #endif
1600
1601
1602 static
1603 int rlink_register_commands(struct command_context_s *cmd_ctx)
1604 {
1605
1606 #ifdef _DEBUG_JTAG_IO_
1607         LOG_DEBUG("rlink_register_commands called with cmd_ctx=%p\n", cmd_ctx);
1608 #endif
1609
1610 #if 0
1611         register_command(
1612                 cmd_ctx, NULL,
1613                 "rlink_dtc_directory",
1614                 handle_dtc_directory_command,
1615                 COMMAND_CONFIG,
1616                 "The directory in which to search for DTC load images"
1617         );
1618 #endif
1619
1620         return ERROR_OK;
1621 }
1622
1623
1624 static
1625 int rlink_init(void)
1626 {
1627         struct usb_bus *busses;
1628         struct usb_bus *bus;
1629         int i, j, retries;
1630         int found=0;
1631         int success=0;
1632         u8 reply_buffer[USB_EP1IN_SIZE];
1633
1634         usb_init();
1635         usb_find_busses();
1636         usb_find_devices();
1637
1638         busses = usb_get_busses();
1639
1640         for(bus = busses; bus; bus = bus->next)
1641         {
1642                 struct usb_device *dev;
1643
1644                 for(dev = bus->devices; dev; dev = dev->next)
1645                 {
1646                         if( (dev->descriptor.idVendor == USB_IDVENDOR) && (dev->descriptor.idProduct == USB_IDPRODUCT) )
1647                         {
1648                                 found = 1;
1649                                 LOG_DEBUG("Found device on bus.\n");
1650
1651                                 do
1652                                 {
1653                                         if( dev->descriptor.bNumConfigurations > 1 )
1654                                         {
1655                                                 LOG_ERROR("Whoops! NumConfigurations is not 1, don't know what to do...\n");
1656                                                 break;
1657                                         }
1658                                         if( dev->config->bNumInterfaces > 1 )
1659                                         {
1660                                                 LOG_ERROR("Whoops! NumInterfaces is not 1, don't know what to do...\n");
1661                                                 break;
1662                                         }
1663
1664                                         pHDev=usb_open(dev);
1665                                         if( !pHDev )
1666                                                 LOG_ERROR ("Failed to open device.\n");
1667                                         else
1668                                         {
1669                                                 LOG_DEBUG("Opened device, pHDev = %p\n",pHDev);
1670
1671                                                 /* usb_set_configuration required under win32 */
1672                                                 usb_set_configuration(pHDev, dev->config[0].bConfigurationValue);
1673
1674                                                 retries = 3;
1675                                                 do
1676                                                 {
1677                                                         i = usb_claim_interface(pHDev,0);
1678                                                         if(i)
1679                                                         {
1680                                                                 LOG_ERROR("usb_claim_interface: %s", usb_strerror());
1681 #ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
1682                                                                 j = usb_detach_kernel_driver_np(pHDev, 0);
1683                                                                 if(j)
1684                                                                         LOG_ERROR("detach kernel driver: %s", usb_strerror());
1685 #endif
1686                                                         }
1687                                                         else
1688                                                         {
1689                                                                 LOG_DEBUG("interface claimed!\n");
1690                                                                 break;
1691                                                         }
1692                                                 } while(--retries);
1693
1694                                                 if(!i)
1695                                                 {
1696                                                         if( usb_set_altinterface(pHDev,0) )
1697                                                         {
1698                                                                 LOG_ERROR("Failed to set interface.\n");
1699                                                                 break;
1700                                                         }
1701                                                         else
1702                                                                 success=1;
1703                                                 }
1704                                         }
1705                                 } while(0);
1706                         }
1707                 }
1708         }
1709
1710         if( !found )
1711         {
1712                 LOG_ERROR("No device found on bus.\n");
1713                 exit(1);
1714         }
1715
1716         if( !success )
1717         {
1718                 LOG_ERROR("Initialisation failed.");
1719                 exit(1);
1720         }
1721
1722
1723         /* 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. */
1724         /* 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. */
1725         for(i = 0; i < 5; i++) {
1726                 j = ep1_generic_commandl(
1727                         pHDev, 1,
1728                         EP1_CMD_GET_FWREV
1729                 );
1730                 if(j < USB_EP1OUT_SIZE) {
1731                         LOG_ERROR("USB write error: %s", usb_strerror());
1732                         return(ERROR_FAIL);
1733                 }
1734                 j = usb_bulk_read(
1735                         pHDev, USB_EP1IN_ADDR,
1736                         (char *)reply_buffer, sizeof(reply_buffer),
1737                         200
1738                 );
1739                 if(j != -ETIMEDOUT) break;
1740         }
1741
1742         if(j < (int)sizeof(reply_buffer)) {
1743                 LOG_ERROR("USB read error: %s", usb_strerror());
1744                 return(ERROR_FAIL);
1745         }
1746         LOG_DEBUG(INTERFACE_NAME" firmware version: %d.%d.%d\n", reply_buffer[0], reply_buffer[1], reply_buffer[2]);
1747
1748         if((reply_buffer[0] != 0) || (reply_buffer[1] != 0) || (reply_buffer[2] != 3)) {
1749                 LOG_WARNING("The rlink device is not of the version that the developers have played with.  It may or may not work.\n");
1750         }
1751
1752         /* Probe port E for adapter presence */
1753         ep1_generic_commandl(
1754                 pHDev, 16,
1755                 EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 0 */
1756                         ST7_PEDR >> 8,
1757                         ST7_PEDR,
1758                         3,
1759                         0x00,                                           /* DR */
1760                         ST7_PE_ADAPTER_SENSE_OUT,       /* DDR */
1761                         ST7_PE_ADAPTER_SENSE_OUT,       /* OR */
1762                 EP1_CMD_MEMORY_READ,    /* Read back */
1763                         ST7_PEDR >> 8,
1764                         ST7_PEDR,
1765                         1,
1766                 EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 1 */
1767                         ST7_PEDR >> 8,
1768                         ST7_PEDR,
1769                         1,
1770                         ST7_PE_ADAPTER_SENSE_OUT
1771         );
1772
1773         usb_bulk_read(
1774                 pHDev, USB_EP1IN_ADDR,
1775                 (char *)reply_buffer, 1,
1776                 USB_TIMEOUT_MS
1777         );
1778
1779         if((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) != 0) {
1780                 LOG_WARNING("target detection problem\n");
1781         }
1782
1783         ep1_generic_commandl(
1784                 pHDev, 11,
1785                 EP1_CMD_MEMORY_READ,    /* Read back */
1786                         ST7_PEDR >> 8,
1787                         ST7_PEDR,
1788                         1,
1789                 EP1_CMD_MEMORY_WRITE,   /* float port E */
1790                         ST7_PEDR >> 8,
1791                         ST7_PEDR,
1792                         3,
1793                         0x00,   /* DR */
1794                         0x00,   /* DDR */
1795                         0x00    /* OR */
1796         );
1797
1798         usb_bulk_read(
1799                 pHDev, USB_EP1IN_ADDR,
1800                 (char *)reply_buffer, 1,
1801                 USB_TIMEOUT_MS
1802         );
1803
1804
1805         if((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) == 0) {
1806                 LOG_WARNING("target not plugged in\n");
1807         }
1808
1809         /* float ports A and B */
1810         ep1_generic_commandl(
1811                 pHDev, 11,
1812                 EP1_CMD_MEMORY_WRITE,
1813                         ST7_PADDR >> 8,
1814                         ST7_PADDR,
1815                         2,
1816                         0x00,
1817                         0x00,
1818                 EP1_CMD_MEMORY_WRITE,
1819                         ST7_PBDDR >> 8,
1820                         ST7_PBDDR,
1821                         1,
1822                         0x00
1823         );
1824
1825         /* make sure DTC is stopped, set VPP control, set up ports A and B */
1826         ep1_generic_commandl(
1827                 pHDev, 14,
1828                 EP1_CMD_DTC_STOP,
1829                 EP1_CMD_SET_PORTD_VPP,
1830                         ~(ST7_PD_VPP_SHDN),
1831                 EP1_CMD_MEMORY_WRITE,
1832                         ST7_PADR >> 8,
1833                         ST7_PADR,
1834                         2,
1835                         ((~(0)) & (ST7_PA_NTRST)),
1836                         (ST7_PA_NTRST),
1837         /* 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. */
1838                 EP1_CMD_MEMORY_WRITE,
1839                         ST7_PBDR >> 8,
1840                         ST7_PBDR,
1841                         1,
1842                         0x00
1843         );
1844
1845         /* set LED updating mode and make sure they're unlit */
1846         ep1_generic_commandl(
1847                 pHDev, 3,
1848 #ifdef AUTOMATIC_BUSY_LED
1849                 EP1_CMD_LEDUE_BUSY,
1850 #else
1851                 EP1_CMD_LEDUE_NONE,
1852 #endif
1853                 EP1_CMD_SET_PORTD_LEDS,
1854                         ~0
1855         );
1856
1857         tap_state_queue_init();
1858         dtc_queue_init();
1859         rlink_speed(jtag_speed);
1860         rlink_reset(0, 0);
1861
1862         return ERROR_OK;
1863 }
1864
1865
1866 static
1867 int rlink_quit(void)
1868 {
1869         /* stop DTC and make sure LEDs are off */
1870         ep1_generic_commandl(
1871                 pHDev, 6,
1872                 EP1_CMD_DTC_STOP,
1873                 EP1_CMD_LEDUE_NONE,
1874                 EP1_CMD_SET_PORTD_LEDS,
1875                         ~0,
1876                 EP1_CMD_SET_PORTD_VPP,
1877                         ~0
1878         );
1879
1880         usb_release_interface(pHDev,0);
1881         usb_close(pHDev);
1882
1883
1884         return ERROR_OK;
1885 }
1886
1887
1888 jtag_interface_t rlink_interface =
1889 {
1890         .name = "rlink",
1891         .init = rlink_init,
1892         .quit = rlink_quit,
1893         .register_commands = rlink_register_commands,
1894         .speed = rlink_speed,
1895         .speed_div = rlink_speed_div,
1896         .khz = rlink_khz,
1897         .execute_queue = rlink_execute_queue,
1898 };