git.gag.com Git - fw/openocd/blob - src/jtag/rlink/rlink.c

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