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                 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                         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", 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                 &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                 &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                 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                         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                         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, 5,
1005
1006                 EP1_CMD_MEMORY_WRITE,
1007                         ST7_PADR >> 8,
1008                         ST7_PADR,
1009                         1,
1010                         bitmap
1011         );
1012         if(usb_err < 0) {
1013                 LOG_ERROR("%s: %s\n", __func__, usb_strerror());
1014                 exit(1);
1015         }
1016 }
1017
1018
1019 static
1020 int
1021 rlink_scan(
1022         jtag_command_t  *cmd,
1023         enum scan_type  type,
1024         u8                      *buffer,
1025         int                     scan_size
1026 ) {
1027         int                     ir_scan;
1028         enum tap_state  saved_end_state;
1029         int                     byte_bits;
1030         int                     extra_bits;
1031         int                     chunk_bits;
1032         int                     chunk_bytes;
1033         int                     x;
1034
1035         int                     tdi_bit_offset;
1036         u8                      tdi_mask, *tdi_p;
1037         u8                      dtc_mask;
1038
1039         if(scan_size < 1) {
1040                 LOG_ERROR("scan_size cannot be less than 1 bit\n");
1041                 exit(1);
1042         }
1043
1044         ir_scan = cmd->cmd.scan->ir_scan;
1045
1046         /* Move to the proper state before starting to shift TDI/TDO. */
1047         if (!(
1048                 (!ir_scan && (cur_state == TAP_DRSHIFT))
1049                 ||
1050                 (ir_scan && (cur_state == TAP_IRSHIFT))
1051         )) {
1052                 saved_end_state = end_state;
1053                 rlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
1054                 rlink_state_move();
1055                 rlink_end_state(saved_end_state);
1056         }
1057
1058         tap_state_queue_run();
1059
1060
1061 #if 0
1062         printf("scan_size = %d, type=0x%x\n", scan_size, type);
1063         {
1064                 int   i;
1065
1066                 /* clear unused bits in scan buffer for ease of debugging */
1067                 /* (it makes diffing output easier) */
1068                 buffer[scan_size / 8] &= ((1 << ((scan_size - 1) % 8) + 1) - 1);
1069
1070                 printf("before scan:");
1071                 for(i = 0; i < (scan_size + 7) / 8; i++) {
1072                         printf(" %02x", buffer[i]);
1073                 }
1074                 printf("\n");
1075         }
1076 #endif
1077
1078         /* The number of bits that can be shifted as complete bytes */
1079         byte_bits = (int)(scan_size - 1) / 8 * 8;
1080         /* The number of bits left over, not counting the last bit */
1081         extra_bits = (scan_size - 1) - byte_bits;
1082
1083         tdi_bit_offset = 0;
1084         tdi_p = buffer;
1085         tdi_mask = 1;
1086
1087         if(extra_bits && (type == SCAN_OUT)) {
1088                 /* Schedule any extra bits into the DTC command buffer, padding as needed */
1089                 /* For SCAN_OUT, this comes before the full bytes so the (leading) padding bits will fall off the end */
1090                 /* make sure there's room for stop, byte op, and one byte */
1091                 if(
1092                         (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1))
1093                 ) {
1094                         dtc_queue_run();
1095                 }
1096
1097                 x = 0;
1098                 dtc_mask = 1 << (extra_bits - 1);
1099
1100                 while(extra_bits--) {
1101                         if(*tdi_p & tdi_mask) {
1102                                 x |= dtc_mask;
1103                         }
1104
1105                         dtc_mask >>= 1;
1106
1107                         tdi_mask <<= 1;
1108                         if(tdi_mask == 0) {
1109                                 tdi_p++;
1110                                 tdi_mask = 1;
1111                         }
1112                 }
1113
1114                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1115                         DTC_CMD_SHIFT_TDI_BYTES(1);
1116
1117                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1118         }
1119
1120         /* Loop scheduling full bytes into the DTC command buffer */
1121         while(byte_bits) {
1122                 if(type == SCAN_IN) {
1123                         /* make sure there's room for stop and byte op */
1124                         x = (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1));
1125                 } else {
1126                         /* make sure there's room for stop, byte op, and at least one byte */
1127                         x = (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1));
1128                 }
1129
1130                 if(type != SCAN_OUT) {
1131                         /* make sure there's room for at least one reply byte */
1132                         x |= (dtc_queue.reply_index >= USB_EP2IN_SIZE - (1));
1133                 }
1134
1135                 if(x) {
1136                         dtc_queue_run();
1137                 }
1138
1139                 chunk_bits = byte_bits;
1140                 /* we can only use up to 16 bytes at a time */
1141                 if(chunk_bits > (16 * 8)) chunk_bits = (16 * 8);
1142
1143                 if(type != SCAN_IN) {
1144                         /* how much is there room for, considering stop and byte op? */
1145                         x = (sizeof(dtc_queue.cmd_buffer) - (dtc_queue.cmd_index + 1 + 1)) * 8;
1146                         if(chunk_bits > x) chunk_bits = x;
1147                 }
1148
1149                 if(type != SCAN_OUT) {
1150                         /* how much is there room for in the reply buffer? */
1151                         x = (USB_EP2IN_SIZE - dtc_queue.reply_index) * 8;
1152                         if(chunk_bits > x) chunk_bits = x;
1153                 }
1154
1155                 /* so the loop will end */
1156                 byte_bits -= chunk_bits;
1157
1158                 if(type != SCAN_OUT) {
1159                         if(dtc_queue_enqueue_reply(
1160                                 type, buffer, scan_size, tdi_bit_offset,
1161                                 chunk_bits,
1162                                 cmd
1163                         ) == NULL) {
1164                                 LOG_ERROR("enqueuing DTC reply entry: %s\n", strerror(errno));
1165                                 exit(1);
1166                         }
1167
1168                         tdi_bit_offset += chunk_bits;
1169                 }
1170
1171                 /* chunk_bits is a multiple of 8, so there are no rounding issues. */
1172                 chunk_bytes = chunk_bits / 8;
1173
1174                 switch(type) {
1175                         case SCAN_IN:
1176                                 x = DTC_CMD_SHIFT_TDO_BYTES(chunk_bytes);
1177                                 break;
1178                         case SCAN_OUT:
1179                                 x = DTC_CMD_SHIFT_TDI_BYTES(chunk_bytes);
1180                                 break;
1181                         default:
1182                                 x = DTC_CMD_SHIFT_TDIO_BYTES(chunk_bytes);
1183                                 break;
1184                 }
1185                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1186
1187                 if(type != SCAN_IN) {
1188                         x = 0;
1189                         dtc_mask = 1 << (8 - 1);
1190
1191                         while(chunk_bits--) {
1192                                 if(*tdi_p & tdi_mask) {
1193                                         x |= dtc_mask;
1194                                 }
1195
1196                                 dtc_mask >>= 1;
1197                                 if(dtc_mask == 0) {
1198                                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1199                                         dtc_queue.reply_index++;
1200                                         x = 0;
1201                                         dtc_mask = 1 << (8 - 1);
1202                                 }
1203
1204                                 tdi_mask <<= 1;
1205                                 if(tdi_mask == 0) {
1206                                         tdi_p++;
1207                                         tdi_mask = 1;
1208                                 }
1209                         }
1210                 }
1211         }
1212
1213         if(extra_bits && (type != SCAN_OUT)) {
1214                 /* Schedule any extra bits into the DTC command buffer */
1215                 /* make sure there's room for stop, byte op, and one byte */
1216                 if(
1217                         (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1))
1218                         ||
1219                         (dtc_queue.reply_index >= USB_EP2IN_SIZE - (1))
1220                 ) {
1221                         dtc_queue_run();
1222                 }
1223
1224                 if(dtc_queue_enqueue_reply(
1225                         type, buffer, scan_size, tdi_bit_offset,
1226                         extra_bits,
1227                         cmd
1228                 ) == NULL) {
1229                         LOG_ERROR("enqueuing DTC reply entry: %s\n", strerror(errno));
1230                         exit(1);
1231                 }
1232
1233                 tdi_bit_offset += extra_bits;
1234
1235                 if(type == SCAN_IN) {
1236                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1237                                 DTC_CMD_SHIFT_TDO_BYTES(1);
1238
1239                 } else {
1240                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1241                                 DTC_CMD_SHIFT_TDIO_BITS(extra_bits);
1242
1243                         x = 0;
1244                         dtc_mask = 1 << (8 - 1);
1245
1246                         while(extra_bits--) {
1247                                 if(*tdi_p & tdi_mask) {
1248                                         x |= dtc_mask;
1249                                 }
1250
1251                                 dtc_mask >>= 1;
1252
1253                                 tdi_mask <<= 1;
1254                                 if(tdi_mask == 0) {
1255                                         tdi_p++;
1256                                         tdi_mask = 1;
1257                                 }
1258                         }
1259
1260                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1261                 }
1262
1263                 dtc_queue.reply_index++;
1264         }
1265
1266         /* Schedule the last bit into the DTC command buffer */
1267         {
1268                 /* make sure there's room for stop, and bit pair command */
1269                 if(
1270                         (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1))
1271                         ||
1272                         (dtc_queue.reply_index >= USB_EP2IN_SIZE - (1))
1273                 ) {
1274                         dtc_queue_run();
1275                 }
1276
1277                 if(type == SCAN_OUT) {
1278                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1279                                 DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 0);
1280
1281                 } else {
1282                         if(dtc_queue_enqueue_reply(
1283                                 type, buffer, scan_size, tdi_bit_offset,
1284                                 1,
1285                                 cmd
1286                         ) == NULL) {
1287                                 LOG_ERROR("enqueuing DTC reply entry: %s\n", strerror(errno));
1288                                 exit(1);
1289                         }
1290
1291                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1292                                 DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 1);
1293
1294                         dtc_queue.reply_index++;
1295                 }
1296         }
1297
1298         /* Move to pause state */
1299         tap_state_queue_append(0);
1300         cur_state = ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE;
1301         if (cur_state != end_state) rlink_state_move();
1302
1303         return(0);
1304 }
1305
1306
1307 static
1308 int rlink_execute_queue(void)
1309 {
1310         jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
1311         int scan_size;
1312         enum scan_type type;
1313         u8 *buffer;
1314         int retval, tmp_retval;
1315
1316         /* return ERROR_OK, unless something goes wrong */
1317         retval = ERROR_OK;
1318
1319 #ifndef AUTOMATIC_BUSY_LED
1320         /* turn LED on */
1321         ep1_generic_commandl(pHDev, 2,
1322                 EP1_CMD_SET_PORTD_LEDS,
1323                 ~(ST7_PD_NBUSY_LED)
1324         );
1325 #endif
1326
1327         while (cmd)
1328         {
1329                 switch (cmd->type)
1330                 {
1331                         case JTAG_END_STATE:
1332                         case JTAG_RUNTEST:
1333                         case JTAG_STATEMOVE:
1334                         case JTAG_PATHMOVE:
1335                         case JTAG_SCAN:
1336                                 break;
1337
1338                         default:
1339                                 /* some events, such as resets, need a queue flush to ensure consistency */
1340                                 tap_state_queue_run();
1341                                 dtc_queue_run();
1342                                 break;
1343                 }
1344
1345                 switch (cmd->type)
1346                 {
1347                         case JTAG_END_STATE:
1348 #ifdef _DEBUG_JTAG_IO_
1349                                 LOG_DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
1350 #endif
1351                                 if (cmd->cmd.end_state->end_state != -1)
1352                                         rlink_end_state(cmd->cmd.end_state->end_state);
1353                                 break;
1354                         case JTAG_RESET:
1355 #ifdef _DEBUG_JTAG_IO_
1356                                 LOG_DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
1357 #endif
1358                                 if ((cmd->cmd.reset->trst == 1) || (cmd->cmd.reset->srst && (jtag_reset_config & RESET_SRST_PULLS_TRST)))
1359                                 {
1360                                         cur_state = TAP_RESET;
1361                                 }
1362                                 rlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
1363                                 break;
1364                         case JTAG_RUNTEST:
1365 #ifdef _DEBUG_JTAG_IO_
1366                                 LOG_DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
1367 #endif
1368                                 if (cmd->cmd.runtest->end_state != -1)
1369                                         rlink_end_state(cmd->cmd.runtest->end_state);
1370                                 rlink_runtest(cmd->cmd.runtest->num_cycles);
1371                                 break;
1372                         case JTAG_STATEMOVE:
1373 #ifdef _DEBUG_JTAG_IO_
1374                                 LOG_DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
1375 #endif
1376                                 if (cmd->cmd.statemove->end_state != -1)
1377                                         rlink_end_state(cmd->cmd.statemove->end_state);
1378                                 rlink_state_move();
1379                                 break;
1380                         case JTAG_PATHMOVE:
1381 #ifdef _DEBUG_JTAG_IO_
1382                                 LOG_DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1383 #endif
1384                                 rlink_path_move(cmd->cmd.pathmove);
1385                                 break;
1386                         case JTAG_SCAN:
1387 #ifdef _DEBUG_JTAG_IO_
1388                                 LOG_DEBUG("%s scan end in %i",  (cmd->cmd.scan->ir_scan) ? "IR" : "DR", cmd->cmd.scan->end_state);
1389 #endif
1390                                 if (cmd->cmd.scan->end_state != -1)
1391                                         rlink_end_state(cmd->cmd.scan->end_state);
1392                                 scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
1393                                 type = jtag_scan_type(cmd->cmd.scan);
1394                                 if(rlink_scan(cmd, type, buffer, scan_size) != ERROR_OK) {
1395                                         retval = ERROR_FAIL;
1396                                 }
1397                                 break;
1398                         case JTAG_SLEEP:
1399 #ifdef _DEBUG_JTAG_IO_
1400                                 LOG_DEBUG("sleep %i", cmd->cmd.sleep->us);
1401 #endif
1402                                 jtag_sleep(cmd->cmd.sleep->us);
1403                                 break;
1404                         default:
1405                                 LOG_ERROR("BUG: unknown JTAG command type encountered");
1406                                 exit(-1);
1407                 }
1408                 cmd = cmd->next;
1409         }
1410
1411         /* Flush the DTC queue to make sure any pending reads have been done before exiting this function */
1412         tap_state_queue_run();
1413         tmp_retval = dtc_queue_run();
1414         if(tmp_retval != ERROR_OK) {
1415                 retval = tmp_retval;
1416         }
1417
1418 #ifndef AUTOMATIC_BUSY_LED
1419         /* turn LED onff */
1420         ep1_generic_commandl(pHDev, 2,
1421                 EP1_CMD_SET_PORTD_LEDS,
1422                 ~0
1423         );
1424 #endif
1425
1426         return retval;
1427 }
1428
1429
1430 /* Using an unindexed table because it is infrequently accessed and it is short.  The table must be in order of ascending speed (and descending prescaler), as it is scanned in reverse. */
1431
1432 static
1433 int rlink_speed(int speed)
1434 {
1435         int             i;
1436
1437         if(speed == 0) {
1438                 /* fastest speed */
1439                 speed = rlink_speed_table[rlink_speed_table_size - 1].prescaler;
1440         }
1441
1442         for(i = rlink_speed_table_size; i--; ) {
1443                 if(rlink_speed_table[i].prescaler == speed) {
1444                         if(dtc_load_from_buffer(pHDev, rlink_speed_table[i].dtc, rlink_speed_table[i].dtc_size) != 0) {
1445                                 LOG_ERROR("An error occurred while trying to load DTC code for speed \"%d\".\n", speed);
1446                                 exit(1);
1447                         }
1448
1449                         if(dtc_start_download() < 0) {
1450                                 LOG_ERROR("%s, %d: starting DTC: %s",
1451                                         __FILE__, __LINE__,
1452                                         usb_strerror()
1453                                 );
1454                                 exit(1);
1455                         }
1456
1457                         return ERROR_OK;
1458                 }
1459         }
1460
1461         LOG_ERROR("%d is not a supported speed", speed);
1462         return(ERROR_FAIL);
1463 }
1464
1465
1466 static
1467 int rlink_speed_div(
1468         int speed,
1469         int *khz
1470 ) {
1471         int     i;
1472
1473         for(i = rlink_speed_table_size; i--; ) {
1474                 if(rlink_speed_table[i].prescaler == speed) {
1475                         *khz = rlink_speed_table[i].khz;
1476                         return(ERROR_OK);
1477                 }
1478         }
1479
1480         LOG_ERROR("%d is not a supported speed", speed);
1481         return(ERROR_FAIL);
1482 }
1483
1484
1485 static
1486 int rlink_khz(
1487         int khz,
1488         int *speed
1489 ) {
1490         int     i;
1491
1492         if(khz == 0) {
1493                 LOG_ERROR("RCLK not supported");
1494                 return ERROR_FAIL;
1495         }
1496
1497         for(i = rlink_speed_table_size; i--; ) {
1498                 if(rlink_speed_table[i].khz <= khz) {
1499                         *speed = rlink_speed_table[i].prescaler;
1500                         return(ERROR_OK);
1501                 }
1502         }
1503
1504         LOG_WARNING("The lowest supported JTAG speed is %d KHz", rlink_speed_table[0].khz);
1505         *speed = rlink_speed_table[0].prescaler;
1506         return(ERROR_OK);
1507 }
1508
1509
1510 #if 0
1511 static
1512 int
1513 handle_dtc_directory_command(
1514         struct command_context_s *cmd_ctx,
1515         char *cmd,
1516         char **args,
1517         int argc
1518 ) {
1519         if(argc != 1) {
1520                 LOG_ERROR("expected exactly one argument to rlink_dtc_directory <directory-path>");
1521                 return(ERROR_INVALID_ARGUMENTS);
1522         }
1523
1524         printf("handle_dtc_directory_command called with \"%s\"\n", args[0]);
1525
1526         return(ERROR_OK);
1527 }
1528 #endif
1529
1530
1531 static
1532 int rlink_register_commands(struct command_context_s *cmd_ctx)
1533 {
1534
1535 #ifdef _DEBUG_JTAG_IO_
1536         LOG_DEBUG("rlink_register_commands called with cmd_ctx=%p\n", cmd_ctx);
1537 #endif
1538
1539 #if 0
1540         register_command(
1541                 cmd_ctx, NULL,
1542                 "rlink_dtc_directory",
1543                 handle_dtc_directory_command,
1544                 COMMAND_CONFIG,
1545                 "The directory in which to search for DTC load images"
1546         );
1547 #endif
1548
1549         return ERROR_OK;
1550 }
1551
1552
1553 static
1554 int rlink_init(void)
1555 {
1556         struct usb_bus *busses;
1557         struct usb_bus *bus;
1558         int i, j, retries;
1559         int found=0;
1560         int success=0;
1561         u8 reply_buffer[USB_EP1IN_SIZE];
1562
1563         usb_init();
1564         usb_find_busses();
1565         usb_find_devices();
1566
1567         busses = usb_get_busses();
1568
1569         for(bus = busses; bus; bus = bus->next)
1570         {
1571                 struct usb_device *dev;
1572
1573                 for(dev = bus->devices; dev; dev = dev->next)
1574                 {
1575                         if( (dev->descriptor.idVendor == USB_IDVENDOR) && (dev->descriptor.idProduct == USB_IDPRODUCT) )
1576                         {
1577                                 found = 1;
1578                                 LOG_DEBUG("Found device on bus.\n");
1579
1580                                 do
1581                                 {
1582                                         if( dev->descriptor.bNumConfigurations > 1 )
1583                                         {
1584                                                 LOG_ERROR("Whoops! NumConfigurations is not 1, don't know what to do...\n");
1585                                                 break;
1586                                         }
1587                                         if( dev->config->bNumInterfaces > 1 )
1588                                         {
1589                                                 LOG_ERROR("Whoops! NumInterfaces is not 1, don't know what to do...\n");
1590                                                 break;
1591                                         }
1592
1593                                         pHDev=usb_open(dev);
1594                                         if( !pHDev )
1595                                                 LOG_ERROR ("Failed to open device.\n");
1596                                         else
1597                                         {
1598                                                 LOG_DEBUG("Opened device, pHDev = %p\n",pHDev);
1599
1600                                                 /* usb_set_configuration required under win32 */
1601                                                 usb_set_configuration(pHDev, dev->config[0].bConfigurationValue);
1602
1603                                                 retries = 3;
1604                                                 do
1605                                                 {
1606                                                         i = usb_claim_interface(pHDev,0);
1607                                                         if(i)
1608                                                         {
1609                                                                 LOG_ERROR("usb_claim_interface: %s", usb_strerror());
1610 #ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
1611                                                                 j = usb_detach_kernel_driver_np(pHDev, 0);
1612                                                                 if(j)
1613                                                                         LOG_ERROR("detach kernel driver: %s", usb_strerror());
1614 #endif
1615                                                         }
1616                                                         else
1617                                                         {
1618                                                                 LOG_DEBUG("interface claimed!\n");
1619                                                                 break;
1620                                                         }
1621                                                 } while(--retries);
1622
1623                                                 if(!i)
1624                                                 {
1625                                                         if( usb_set_altinterface(pHDev,0) )
1626                                                         {
1627                                                                 LOG_ERROR("Failed to set interface.\n");
1628                                                                 break;
1629                                                         }
1630                                                         else
1631                                                                 success=1;
1632                                                 }
1633                                         }
1634                                 } while(0);
1635                         }
1636                 }
1637         }
1638
1639         if( !found )
1640         {
1641                 LOG_ERROR("No device found on bus.\n");
1642                 exit(1);
1643         }
1644
1645         if( !success )
1646         {
1647                 LOG_ERROR("Initialisation failed.");
1648                 exit(1);
1649         }
1650
1651
1652         /* 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. */
1653         /* 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. */
1654         for(i = 0; i < 5; i++) {
1655                 j = ep1_generic_commandl(
1656                         pHDev, 1,
1657                         EP1_CMD_GET_FWREV
1658                 );
1659                 if(j < USB_EP1OUT_SIZE) {
1660                         LOG_ERROR("USB write error: %s", usb_strerror());
1661                         return(ERROR_FAIL);
1662                 }
1663                 j = usb_bulk_read(
1664                         pHDev, USB_EP1IN_ADDR,
1665                         reply_buffer, sizeof(reply_buffer),
1666                         200
1667                 );
1668                 if(j != -ETIMEDOUT) break;
1669         }
1670
1671         if(j < (int)sizeof(reply_buffer)) {
1672                 LOG_ERROR("USB read error: %s", usb_strerror());
1673                 return(ERROR_FAIL);
1674         }
1675         LOG_DEBUG(INTERFACE_NAME" firmware version: %d.%d.%d\n", reply_buffer[0], reply_buffer[1], reply_buffer[2]);
1676
1677         if((reply_buffer[0] != 0) || (reply_buffer[1] != 0) || (reply_buffer[2] != 3)) {
1678                 LOG_WARNING("The rlink device is not of the version that the developers have played with.  It may or may not work.\n");
1679         }
1680
1681         /* Probe port E for adapter presence */
1682         ep1_generic_commandl(
1683                 pHDev, 16,
1684                 EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 0 */
1685                         ST7_PEDR >> 8,
1686                         ST7_PEDR,
1687                         3,
1688                         0x00,
1689                         ST7_PE_ADAPTER_SENSE_OUT,
1690                         ST7_PE_ADAPTER_SENSE_OUT,
1691                 EP1_CMD_MEMORY_READ,    /* Read back */
1692                         ST7_PEDR >> 8,
1693                         ST7_PEDR,
1694                         1,
1695                 EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 1 */
1696                         ST7_PEDR >> 8,
1697                         ST7_PEDR,
1698                         1,
1699                         ST7_PE_ADAPTER_SENSE_OUT
1700         );
1701
1702         usb_bulk_read(
1703                 pHDev, USB_EP1IN_ADDR,
1704                 reply_buffer, 1,
1705                 USB_TIMEOUT_MS
1706         );
1707
1708         if((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) != 0) {
1709                 LOG_WARNING("target detection problem\n");
1710         }
1711
1712         ep1_generic_commandl(
1713                 pHDev, 11,
1714                 EP1_CMD_MEMORY_READ,    /* Read back */
1715                         ST7_PEDR >> 8,
1716                         ST7_PEDR,
1717                         1,
1718                 EP1_CMD_MEMORY_WRITE,   /* float port E */
1719                         ST7_PEDR >> 8,
1720                         ST7_PEDR,
1721                         3,
1722                         0x00,
1723                         0x00,
1724                         0x00
1725         );
1726
1727         usb_bulk_read(
1728                 pHDev, USB_EP1IN_ADDR,
1729                 reply_buffer, 1,
1730                 USB_TIMEOUT_MS
1731         );
1732
1733
1734         if((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) == 0) {
1735                 LOG_WARNING("target not plugged in\n");
1736         }
1737
1738         /* float port A, make sure DTC is stopped, set upper 2 bits of port D, and set up port A */
1739         ep1_generic_commandl(
1740                 pHDev, 15,
1741                 EP1_CMD_MEMORY_WRITE,
1742                         ST7_PADDR >> 8,
1743                         ST7_PADDR,
1744                         2,
1745                         0x00,
1746                         0x00,
1747                 EP1_CMD_DTC_STOP,
1748                 EP1_CMD_SET_PORTD_UPPER,
1749                         ~(ST7_PD_NRUN_LED),
1750                 EP1_CMD_MEMORY_WRITE,
1751                         ST7_PADR >> 8,
1752                         ST7_PADR,
1753                         2,
1754                         ((~(ST7_PA_NLINE_DRIVER_ENABLE)) & ST7_PA_NUNASSERTED),
1755                         (ST7_PA_NLINE_DRIVER_ENABLE | ST7_PA_NRLINK_RST | ST7_PA_NJTAG_TRST)
1756         );
1757
1758         /* set LED updating mode and make sure they're unlit */
1759         ep1_generic_commandl(
1760                 pHDev, 3,
1761 #ifdef AUTOMATIC_BUSY_LED
1762                 EP1_CMD_LEDUE_BUSY,
1763 #else
1764                 EP1_CMD_LEDUE_NONE,
1765 #endif
1766                 EP1_CMD_SET_PORTD_LEDS,
1767                         ~0
1768         );
1769
1770         tap_state_queue_init();
1771         dtc_queue_init();
1772         rlink_speed(jtag_speed);
1773         rlink_reset(0, 0);
1774
1775         return ERROR_OK;
1776 }
1777
1778
1779 static
1780 int rlink_quit(void)
1781 {
1782         /* stop DTC and make sure LEDs are off */
1783         ep1_generic_commandl(
1784                 pHDev, 6,
1785                 EP1_CMD_DTC_STOP,
1786                 EP1_CMD_LEDUE_NONE,
1787                 EP1_CMD_SET_PORTD_LEDS,
1788                         ~0,
1789                 EP1_CMD_SET_PORTD_UPPER,
1790                         ~0
1791         );
1792
1793         usb_release_interface(pHDev,0);
1794         usb_close(pHDev);
1795
1796
1797         return ERROR_OK;
1798 }
1799
1800
1801 jtag_interface_t rlink_interface =
1802 {
1803         .name = "rlink",
1804         .init = rlink_init,
1805         .quit = rlink_quit,
1806         .register_commands = rlink_register_commands,
1807         .speed = rlink_speed,
1808         .speed_div = rlink_speed_div,
1809         .khz = rlink_khz,
1810         .execute_queue = rlink_execute_queue,
1811 };