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

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