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