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