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 "interface.h"
  32 #include "commands.h"
  33 #include "rlink.h"
  34 #include "st7.h"
  35 #include "ep1_cmd.h"
  36 #include "dtc_cmd.h"
  37
  38 /* system includes */
  39 #include <usb.h>
  40
  41
  42 /* 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. */
  43 #undef AUTOMATIC_BUSY_LED
  44
  45 /* This feature may require derating the speed due to reduced hold time. */
  46 #undef USE_HARDWARE_SHIFTER_FOR_TMS
  47
  48
  49 #define INTERFACE_NAME          "RLink"
  50
  51 #define USB_IDVENDOR            (0x138e)
  52 #define USB_IDPRODUCT           (0x9000)
  53
  54 #define USB_EP1OUT_ADDR         (0x01)
  55 #define USB_EP1OUT_SIZE         (16)
  56 #define USB_EP1IN_ADDR          (USB_EP1OUT_ADDR | 0x80)
  57 #define USB_EP1IN_SIZE          (USB_EP1OUT_SIZE)
  58
  59 #define USB_EP2OUT_ADDR         (0x02)
  60 #define USB_EP2OUT_SIZE         (64)
  61 #define USB_EP2IN_ADDR          (USB_EP2OUT_ADDR | 0x80)
  62 #define USB_EP2IN_SIZE          (USB_EP2OUT_SIZE)
  63 #define USB_EP2BANK_SIZE        (512)
  64
  65 #define USB_TIMEOUT_MS          (3 * 1000)
  66
  67 #define DTC_STATUS_POLL_BYTE    (ST7_USB_BUF_EP0OUT + 0xff)
  68
  69
  70 #define ST7_PD_NBUSY_LED                ST7_PD0
  71 #define ST7_PD_NRUN_LED                 ST7_PD1
  72 /* low enables VPP at adapter header, high connects it to GND instead */
  73 #define ST7_PD_VPP_SEL                  ST7_PD6
  74 /* low: VPP = 12v, high: VPP <= 5v */
  75 #define ST7_PD_VPP_SHDN                 ST7_PD7
  76
  77 /* These pins are connected together */
  78 #define ST7_PE_ADAPTER_SENSE_IN         ST7_PE3
  79 #define ST7_PE_ADAPTER_SENSE_OUT        ST7_PE4
  80
  81 /* Symbolic mapping between port pins and numbered IO lines */
  82 #define ST7_PA_IO1      ST7_PA1
  83 #define ST7_PA_IO2      ST7_PA2
  84 #define ST7_PA_IO4      ST7_PA4
  85 #define ST7_PA_IO8      ST7_PA6
  86 #define ST7_PA_IO10     ST7_PA7
  87 #define ST7_PB_IO5      ST7_PB5
  88 #define ST7_PC_IO9      ST7_PC1
  89 #define ST7_PC_IO3      ST7_PC2
  90 #define ST7_PC_IO7      ST7_PC3
  91 #define ST7_PE_IO6      ST7_PE5
  92
  93 /* Symbolic mapping between numbered IO lines and adapter signals */
  94 #define ST7_PA_RTCK     ST7_PA_IO0
  95 #define ST7_PA_NTRST    ST7_PA_IO1
  96 #define ST7_PC_TDI      ST7_PC_IO3
  97 #define ST7_PA_DBGRQ    ST7_PA_IO4
  98 #define ST7_PB_NSRST    ST7_PB_IO5
  99 #define ST7_PE_TMS      ST7_PE_IO6
 100 #define ST7_PC_TCK      ST7_PC_IO7
 101 #define ST7_PC_TDO      ST7_PC_IO9
 102 #define ST7_PA_DBGACK   ST7_PA_IO10
 103
 104 static usb_dev_handle *pHDev;
 105
 106
 107 /*
 108  * ep1 commands are up to USB_EP1OUT_SIZE bytes in length.
 109  * This function takes care of zeroing the unused bytes before sending the packet.
 110  * Any reply packet is not handled by this function.
 111  */
 112 static
 113 int
 114 ep1_generic_commandl(
 115         usb_dev_handle  *pHDev,
 116         size_t          length,
 117         ...
 118 ) {
 119         uint8_t         usb_buffer[USB_EP1OUT_SIZE];
 120         uint8_t         *usb_buffer_p;
 121         va_list         ap;
 122         int             usb_ret;
 123
 124         if (length > sizeof(usb_buffer)) {
 125                 length = sizeof(usb_buffer);
 126         }
 127
 128         usb_buffer_p = usb_buffer;
 129
 130         va_start(ap, length);
 131         while (length > 0) {
 132                 *usb_buffer_p++ = va_arg(ap, int);
 133                 length--;
 134         }
 135
 136         memset(
 137                 usb_buffer_p,
 138                 0,
 139                 sizeof(usb_buffer) - (usb_buffer_p - usb_buffer)
 140 );
 141
 142         usb_ret = usb_bulk_write(
 143                 pHDev,
 144                 USB_EP1OUT_ADDR,
 145                 (char *)usb_buffer, sizeof(usb_buffer),
 146                 USB_TIMEOUT_MS
 147 );
 148
 149         return(usb_ret);
 150 }
 151
 152
 153
 154 #if 0
 155 static
 156 ssize_t
 157 ep1_memory_read(
 158         usb_dev_handle  *pHDev,
 159         uint16_t        addr,
 160         size_t          length,
 161         uint8_t         *buffer
 162 ) {
 163         uint8_t         usb_buffer[USB_EP1OUT_SIZE];
 164         int             usb_ret;
 165         size_t          remain;
 166         ssize_t         count;
 167
 168         usb_buffer[0] = EP1_CMD_MEMORY_READ;
 169         memset(
 170                 usb_buffer + 4,
 171                 0,
 172                 sizeof(usb_buffer) - 4
 173 );
 174
 175         remain = length;
 176         count = 0;
 177
 178         while (remain) {
 179                 if (remain > sizeof(usb_buffer)) {
 180                         length = sizeof(usb_buffer);
 181                 } else {
 182                         length = remain;
 183                 }
 184
 185                 usb_buffer[1] = addr >> 8;
 186                 usb_buffer[2] = addr;
 187                 usb_buffer[3] = length;
 188
 189                         usb_ret = usb_bulk_write(
 190                         pHDev, USB_EP1OUT_ADDR,
 191                         usb_buffer, sizeof(usb_buffer),
 192                         USB_TIMEOUT_MS
 193 );
 194
 195                 if (usb_ret < sizeof(usb_buffer)) {
 196                         break;
 197                 }
 198
 199                 usb_ret = usb_bulk_read(
 200                         pHDev, USB_EP1IN_ADDR,
 201                         buffer, length,
 202                         USB_TIMEOUT_MS
 203 );
 204
 205                 if (usb_ret < length) {
 206                         break;
 207                 }
 208
 209                 addr += length;
 210                 buffer += length;
 211                 count += length;
 212                 remain -= length;
 213         }
 214
 215         return(count);
 216 }
 217 #endif
 218
 219
 220
 221 static
 222 ssize_t
 223 ep1_memory_write(
 224         usb_dev_handle  *pHDev,
 225         uint16_t        addr,
 226         size_t          length,
 227         uint8_t const   *buffer
 228 ) {
 229         uint8_t         usb_buffer[USB_EP1OUT_SIZE];
 230         int             usb_ret;
 231         size_t          remain;
 232         ssize_t         count;
 233
 234         usb_buffer[0] = EP1_CMD_MEMORY_WRITE;
 235
 236         remain = length;
 237         count = 0;
 238
 239         while (remain) {
 240                 if (remain > (sizeof(usb_buffer) - 4)) {
 241                         length = (sizeof(usb_buffer) - 4);
 242                 } else {
 243                         length = remain;
 244                 }
 245
 246                 usb_buffer[1] = addr >> 8;
 247                 usb_buffer[2] = addr;
 248                 usb_buffer[3] = length;
 249                 memcpy(
 250                         usb_buffer + 4,
 251                         buffer,
 252                         length
 253 );
 254                 memset(
 255                         usb_buffer + 4 + length,
 256                         0,
 257                         sizeof(usb_buffer) - 4 - length
 258 );
 259
 260                         usb_ret = usb_bulk_write(
 261                         pHDev, USB_EP1OUT_ADDR,
 262                         (char *)usb_buffer, sizeof(usb_buffer),
 263                         USB_TIMEOUT_MS
 264 );
 265
 266                 if ((size_t)usb_ret < sizeof(usb_buffer)) {
 267                         break;
 268                 }
 269
 270                 addr += length;
 271                 buffer += length;
 272                 count += length;
 273                 remain -= length;
 274         }
 275
 276         return(count);
 277 }
 278
 279
 280 #if 0
 281 static
 282 ssize_t
 283 ep1_memory_writel(
 284         usb_dev_handle  *pHDev,
 285         uint16_t        addr,
 286         size_t          length,
 287         ...
 288 ) {
 289         uint8_t         buffer[USB_EP1OUT_SIZE - 4];
 290         uint8_t         *buffer_p;
 291         va_list         ap;
 292         size_t          remain;
 293
 294         if (length > sizeof(buffer)) {
 295                 length = sizeof(buffer);
 296         }
 297
 298         remain = length;
 299         buffer_p = buffer;
 300
 301         va_start(ap, length);
 302         while (remain > 0) {
 303                 *buffer_p++ = va_arg(ap, int);
 304                 remain--;
 305         }
 306
 307         return(ep1_memory_write(pHDev, addr, length, buffer));
 308 }
 309 #endif
 310
 311
 312 #define DTCLOAD_COMMENT         (0)
 313 #define DTCLOAD_ENTRY           (1)
 314 #define DTCLOAD_LOAD            (2)
 315 #define DTCLOAD_RUN                     (3)
 316 #define DTCLOAD_LUT_START       (4)
 317 #define DTCLOAD_LUT                     (5)
 318
 319 #define DTC_LOAD_BUFFER         ST7_USB_BUF_EP2UIDO
 320
 321 /* This gets set by the DTC loader */
 322 static uint8_t dtc_entry_download;
 323
 324
 325 /* The buffer is specially formatted to represent a valid image to load into the DTC. */
 326 static
 327 int
 328 dtc_load_from_buffer(
 329         usb_dev_handle  *pHDev,
 330         const uint8_t           *buffer,
 331         size_t                  length
 332 ) {
 333         struct header_s {
 334                 uint8_t type;
 335                 uint8_t length;
 336         };
 337
 338         int                             usb_err;
 339         struct header_s *header;
 340         uint8_t                         lut_start = 0xc0;
 341
 342         dtc_entry_download = 0;
 343
 344         /* Stop the DTC before loading anything. */
 345         usb_err = ep1_generic_commandl(
 346                 pHDev, 1,
 347                 EP1_CMD_DTC_STOP
 348 );
 349         if (usb_err < 0) return(usb_err);
 350
 351         while (length) {
 352                 if (length < sizeof(*header)) {
 353                         LOG_ERROR("Malformed DTC image\n");
 354                         exit(1);
 355                 }
 356
 357                 header = (struct header_s *)buffer;
 358                 buffer += sizeof(*header);
 359                 length -= sizeof(*header);
 360
 361                 if (length < (size_t)header->length + 1) {
 362                         LOG_ERROR("Malformed DTC image\n");
 363                         exit(1);
 364                 }
 365
 366                 switch (header->type) {
 367                         case DTCLOAD_COMMENT:
 368                                 break;
 369
 370                         case DTCLOAD_ENTRY:
 371                                 /* store entry addresses somewhere */
 372                                 if (!strncmp("download", (char *)buffer + 1, 8)) {
 373                                         dtc_entry_download = buffer[0];
 374                                 }
 375                                 break;
 376
 377                         case DTCLOAD_LOAD:
 378                                 /* Send the DTC program to ST7 RAM. */
 379                                 usb_err = ep1_memory_write(
 380                                         pHDev,
 381                                         DTC_LOAD_BUFFER,
 382                                         header->length + 1, buffer
 383 );
 384                                 if (usb_err < 0) return(usb_err);
 385
 386                                 /* Load it into the DTC. */
 387                                 usb_err = ep1_generic_commandl(
 388                                         pHDev, 3,
 389                                         EP1_CMD_DTC_LOAD,
 390                                                 (DTC_LOAD_BUFFER >> 8),
 391                                                 DTC_LOAD_BUFFER
 392 );
 393                                 if (usb_err < 0) return(usb_err);
 394
 395                                 break;
 396
 397                         case DTCLOAD_RUN:
 398                                 usb_err = ep1_generic_commandl(
 399                                         pHDev, 3,
 400                                         EP1_CMD_DTC_CALL,
 401                                                 buffer[0],
 402                                         EP1_CMD_DTC_WAIT
 403 );
 404                                 if (usb_err < 0) return(usb_err);
 405
 406                                 break;
 407
 408                         case DTCLOAD_LUT_START:
 409                                 lut_start = buffer[0];
 410                                 break;
 411
 412                         case DTCLOAD_LUT:
 413                                 usb_err = ep1_memory_write(
 414                                         pHDev,
 415                                         ST7_USB_BUF_EP0OUT + lut_start,
 416                                         header->length + 1, buffer
 417 );
 418                                 if (usb_err < 0) return(usb_err);
 419                                 break;
 420
 421                         default:
 422                                 LOG_ERROR("Invalid DTC image record type: 0x%02x\n", header->type);
 423                                 exit(1);
 424                                 break;
 425                 }
 426
 427                 buffer += (header->length + 1);
 428                 length -= (header->length + 1);
 429         }
 430
 431         return(0);
 432 }
 433
 434
 435 /*
 436  * Start the DTC running in download mode (waiting for 512 byte command packets on ep2).
 437  */
 438 static
 439 int
 440 dtc_start_download(void) {
 441         int     usb_err;
 442         uint8_t ep2txr;
 443
 444         /* set up for download mode and make sure EP2 is set up to transmit */
 445         usb_err = ep1_generic_commandl(
 446                 pHDev, 7,
 447
 448                 EP1_CMD_DTC_STOP,
 449                 EP1_CMD_SET_UPLOAD,
 450                 EP1_CMD_SET_DOWNLOAD,
 451                 EP1_CMD_MEMORY_READ,    /* read EP2TXR for its data toggle */
 452                         ST7_EP2TXR >> 8,
 453                         ST7_EP2TXR,
 454                         1
 455 );
 456         if (usb_err < 0) return(usb_err);
 457
 458         /* read back ep2txr */
 459         usb_err = usb_bulk_read(
 460                 pHDev, USB_EP1IN_ADDR,
 461                 (char *)&ep2txr, 1,
 462                 USB_TIMEOUT_MS
 463 );
 464         if (usb_err < 0) return(usb_err);
 465
 466         usb_err = ep1_generic_commandl(
 467                 pHDev, 13,
 468
 469                 EP1_CMD_MEMORY_WRITE,   /* preinitialize poll byte */
 470                         DTC_STATUS_POLL_BYTE >> 8,
 471                         DTC_STATUS_POLL_BYTE,
 472                         1,
 473                         0x00,
 474                 EP1_CMD_MEMORY_WRITE,   /* set EP2IN to return data */
 475                         ST7_EP2TXR >> 8,
 476                         ST7_EP2TXR,
 477                         1,
 478                         (ep2txr & ST7_EP2TXR_DTOG_TX) | ST7_EP2TXR_STAT_VALID,
 479                 EP1_CMD_DTC_CALL,       /* start running the DTC */
 480                         dtc_entry_download,
 481                 EP1_CMD_DTC_GET_CACHED_STATUS
 482 );
 483         if (usb_err < 0) return(usb_err);
 484
 485         /* wait for completion */
 486         usb_err = usb_bulk_read(
 487                 pHDev, USB_EP1IN_ADDR,
 488                 (char *)&ep2txr, 1,
 489                 USB_TIMEOUT_MS
 490 );
 491
 492         return(usb_err);
 493 }
 494
 495
 496 static
 497 int
 498 dtc_run_download(
 499         usb_dev_handle  *pHDev,
 500         uint8_t *command_buffer,
 501         int     command_buffer_size,
 502         uint8_t *reply_buffer,
 503         int     reply_buffer_size
 504 ) {
 505         uint8_t ep2_buffer[USB_EP2IN_SIZE];
 506         int     usb_err;
 507         int     i;
 508
 509         LOG_DEBUG(": %d/%d\n", command_buffer_size, reply_buffer_size);
 510
 511         usb_err = usb_bulk_write(
 512                 pHDev,
 513                 USB_EP2OUT_ADDR,
 514                 (char *)command_buffer, USB_EP2BANK_SIZE,
 515                 USB_TIMEOUT_MS
 516 );
 517         if (usb_err < 0) return(usb_err);
 518
 519
 520         /* Wait for DTC to finish running command buffer */
 521         for (i = 10;;) {
 522                 usb_err = ep1_generic_commandl(
 523                         pHDev, 4,
 524
 525                         EP1_CMD_MEMORY_READ,
 526                                 DTC_STATUS_POLL_BYTE >> 8,
 527                                 DTC_STATUS_POLL_BYTE,
 528                                 1
 529 );
 530                 if (usb_err < 0) return(usb_err);
 531
 532                 usb_err = usb_bulk_read(
 533                         pHDev,
 534                         USB_EP1IN_ADDR,
 535                         (char *)ep2_buffer, 1,
 536                         USB_TIMEOUT_MS
 537 );
 538                 if (usb_err < 0) return(usb_err);
 539
 540                 if (ep2_buffer[0] & 0x01) break;
 541
 542                 if (!--i) {
 543                         LOG_ERROR("%s, %d: too many retries waiting for DTC status\n",
 544                                 __FILE__, __LINE__
 545 );
 546                         return(-ETIMEDOUT);
 547                 }
 548         }
 549
 550
 551         if (!reply_buffer) reply_buffer_size = 0;
 552         if (reply_buffer_size) {
 553                 usb_err = usb_bulk_read(
 554                         pHDev,
 555                         USB_EP2IN_ADDR,
 556                         (char *)ep2_buffer, sizeof(ep2_buffer),
 557                         USB_TIMEOUT_MS
 558 );
 559
 560                 if (usb_err < (int)sizeof(ep2_buffer)) {
 561                         LOG_ERROR("%s, %d: Read of endpoint 2 returned %d\n",
 562                                 __FILE__, __LINE__, usb_err
 563 );
 564                         return(usb_err);
 565                 }
 566
 567                 memcpy(reply_buffer, ep2_buffer, reply_buffer_size);
 568
 569         }
 570
 571         return(usb_err);
 572 }
 573
 574
 575 /*
 576  * 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.
 577  */
 578
 579 struct dtc_reply_queue_entry {
 580         struct dtc_reply_queue_entry    *next;
 581         struct jtag_command     *cmd;   /* the command that resulted in this entry */
 582
 583         struct {
 584                 uint8_t         *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 };
 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         struct dtc_reply_queue_entry    *rq_head;
 601         struct dtc_reply_queue_entry    *rq_tail;
 602         uint32_t                        cmd_index;
 603         uint32_t                        reply_index;
 604         uint8_t                 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         uint32_t        length;
 615         uint32_t        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 struct dtc_reply_queue_entry *
 634 dtc_queue_enqueue_reply(
 635         enum scan_type  type,
 636         uint8_t                         *buffer,
 637         int                             size,
 638         int                             offset,
 639         int                             length,
 640         struct jtag_command     *cmd
 641 ) {
 642         struct dtc_reply_queue_entry    *rq_entry;
 643
 644         rq_entry = malloc(sizeof(struct dtc_reply_queue_entry));
 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         struct dtc_reply_queue_entry    *rq_p, *rq_next;
 675         int                     retval;
 676         int                     usb_err;
 677         int                     bit_cnt;
 678         int                     x;
 679         uint8_t                 *dtc_p, *tdo_p;
 680         uint8_t                 dtc_mask, tdo_mask;
 681         uint8_t                 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         uint8_t 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         uint8_t 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         uint8_t 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(struct pathmove_command *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         uint8_t                 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         struct jtag_command     *cmd,
1094         enum scan_type  type,
1095         uint8_t                 *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         uint8_t                 tdi_mask, *tdi_p;
1108         uint8_t                 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         struct jtag_command *cmd = jtag_command_queue; /* currently processed command */
1382         int scan_size;
1383         enum scan_type type;
1384         uint8_t *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_get_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 *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 *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         uint8_t 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) ||
1639                                 (dev->descriptor.idProduct != USB_IDPRODUCT))
1640                         {
1641                                 continue;
1642                         }
1643                         found = 1;
1644                         LOG_DEBUG("Found device on bus.\n");
1645
1646                         if (dev->descriptor.bNumConfigurations > 1)
1647                         {
1648                                 LOG_ERROR("Whoops! NumConfigurations is not 1, don't know what to do...\n");
1649                                 break;
1650                         }
1651                         if (dev->config->bNumInterfaces > 1)
1652                         {
1653                                 LOG_ERROR("Whoops! NumInterfaces is not 1, don't know what to do...\n");
1654                                 break;
1655                         }
1656
1657                         pHDev = usb_open(dev);
1658                         if (!pHDev)
1659                         {
1660                                 LOG_ERROR ("Failed to open device.\n");
1661                                 break;
1662                         }
1663                         LOG_DEBUG("Opened device, pHDev = %p\n",pHDev);
1664
1665                         /* usb_set_configuration required under win32 */
1666                         usb_set_configuration(pHDev, dev->config[0].bConfigurationValue);
1667
1668                         retries = 3;
1669                         do
1670                         {
1671                                 i = usb_claim_interface(pHDev,0);
1672                                 if (i)
1673                                 {
1674                                         LOG_ERROR("usb_claim_interface: %s", usb_strerror());
1675 #ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
1676                                         j = usb_detach_kernel_driver_np(pHDev, 0);
1677                                         if (j)
1678                                                 LOG_ERROR("detach kernel driver: %s", usb_strerror());
1679 #endif
1680                                 }
1681                                 else
1682                                 {
1683                                         LOG_DEBUG("interface claimed!\n");
1684                                         break;
1685                                 }
1686                         } while (--retries);
1687
1688                         if (!i)
1689                         {
1690                                 if (usb_set_altinterface(pHDev,0))
1691                                 {
1692                                         LOG_ERROR("Failed to set interface.\n");
1693                                         break;
1694                                 }
1695                                 else
1696                                         success = 1;
1697                         }
1698                 }
1699         }
1700
1701         if (!found)
1702         {
1703                 LOG_ERROR("No device found on bus.\n");
1704                 exit(1);
1705         }
1706
1707         if (!success)
1708         {
1709                 LOG_ERROR("Initialisation failed.");
1710                 exit(1);
1711         }
1712
1713
1714         /* 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. */
1715         /* 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. */
1716         for (i = 0; i < 5; i++) {
1717                 j = ep1_generic_commandl(
1718                         pHDev, 1,
1719                         EP1_CMD_GET_FWREV
1720 );
1721                 if (j < USB_EP1OUT_SIZE) {
1722                         LOG_ERROR("USB write error: %s", usb_strerror());
1723                         return(ERROR_FAIL);
1724                 }
1725                 j = usb_bulk_read(
1726                         pHDev, USB_EP1IN_ADDR,
1727                         (char *)reply_buffer, sizeof(reply_buffer),
1728                         200
1729 );
1730                 if (j != -ETIMEDOUT) break;
1731         }
1732
1733         if (j < (int)sizeof(reply_buffer)) {
1734                 LOG_ERROR("USB read error: %s", usb_strerror());
1735                 return(ERROR_FAIL);
1736         }
1737         LOG_DEBUG(INTERFACE_NAME" firmware version: %d.%d.%d\n", reply_buffer[0], reply_buffer[1], reply_buffer[2]);
1738
1739         if ((reply_buffer[0] != 0) || (reply_buffer[1] != 0) || (reply_buffer[2] != 3)) {
1740                 LOG_WARNING("The rlink device is not of the version that the developers have played with.  It may or may not work.\n");
1741         }
1742
1743         /* Probe port E for adapter presence */
1744         ep1_generic_commandl(
1745                 pHDev, 16,
1746                 EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 0 */
1747                         ST7_PEDR >> 8,
1748                         ST7_PEDR,
1749                         3,
1750                         0x00,                                           /* DR */
1751                         ST7_PE_ADAPTER_SENSE_OUT,       /* DDR */
1752                         ST7_PE_ADAPTER_SENSE_OUT,       /* OR */
1753                 EP1_CMD_MEMORY_READ,    /* Read back */
1754                         ST7_PEDR >> 8,
1755                         ST7_PEDR,
1756                         1,
1757                 EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 1 */
1758                         ST7_PEDR >> 8,
1759                         ST7_PEDR,
1760                         1,
1761                         ST7_PE_ADAPTER_SENSE_OUT
1762 );
1763
1764         usb_bulk_read(
1765                 pHDev, USB_EP1IN_ADDR,
1766                 (char *)reply_buffer, 1,
1767                 USB_TIMEOUT_MS
1768 );
1769
1770         if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) != 0) {
1771                 LOG_WARNING("target detection problem\n");
1772         }
1773
1774         ep1_generic_commandl(
1775                 pHDev, 11,
1776                 EP1_CMD_MEMORY_READ,    /* Read back */
1777                         ST7_PEDR >> 8,
1778                         ST7_PEDR,
1779                         1,
1780                 EP1_CMD_MEMORY_WRITE,   /* float port E */
1781                         ST7_PEDR >> 8,
1782                         ST7_PEDR,
1783                         3,
1784                         0x00,   /* DR */
1785                         0x00,   /* DDR */
1786                         0x00    /* OR */
1787 );
1788
1789         usb_bulk_read(
1790                 pHDev, USB_EP1IN_ADDR,
1791                 (char *)reply_buffer, 1,
1792                 USB_TIMEOUT_MS
1793 );
1794
1795
1796         if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) == 0) {
1797                 LOG_WARNING("target not plugged in\n");
1798         }
1799
1800         /* float ports A and B */
1801         ep1_generic_commandl(
1802                 pHDev, 11,
1803                 EP1_CMD_MEMORY_WRITE,
1804                         ST7_PADDR >> 8,
1805                         ST7_PADDR,
1806                         2,
1807                         0x00,
1808                         0x00,
1809                 EP1_CMD_MEMORY_WRITE,
1810                         ST7_PBDDR >> 8,
1811                         ST7_PBDDR,
1812                         1,
1813                         0x00
1814 );
1815
1816         /* make sure DTC is stopped, set VPP control, set up ports A and B */
1817         ep1_generic_commandl(
1818                 pHDev, 14,
1819                 EP1_CMD_DTC_STOP,
1820                 EP1_CMD_SET_PORTD_VPP,
1821                         ~(ST7_PD_VPP_SHDN),
1822                 EP1_CMD_MEMORY_WRITE,
1823                         ST7_PADR >> 8,
1824                         ST7_PADR,
1825                         2,
1826                         ((~(0)) & (ST7_PA_NTRST)),
1827                         (ST7_PA_NTRST),
1828         /* 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. */
1829                 EP1_CMD_MEMORY_WRITE,
1830                         ST7_PBDR >> 8,
1831                         ST7_PBDR,
1832                         1,
1833                         0x00
1834 );
1835
1836         /* set LED updating mode and make sure they're unlit */
1837         ep1_generic_commandl(
1838                 pHDev, 3,
1839 #ifdef AUTOMATIC_BUSY_LED
1840                 EP1_CMD_LEDUE_BUSY,
1841 #else
1842                 EP1_CMD_LEDUE_NONE,
1843 #endif
1844                 EP1_CMD_SET_PORTD_LEDS,
1845                         ~0
1846 );
1847
1848         tap_state_queue_init();
1849         dtc_queue_init();
1850         rlink_speed(jtag_get_speed());
1851         rlink_reset(0, 0);
1852
1853         return ERROR_OK;
1854 }
1855
1856
1857 static
1858 int rlink_quit(void)
1859 {
1860         /* stop DTC and make sure LEDs are off */
1861         ep1_generic_commandl(
1862                 pHDev, 6,
1863                 EP1_CMD_DTC_STOP,
1864                 EP1_CMD_LEDUE_NONE,
1865                 EP1_CMD_SET_PORTD_LEDS,
1866                         ~0,
1867                 EP1_CMD_SET_PORTD_VPP,
1868                         ~0
1869 );
1870
1871         usb_release_interface(pHDev,0);
1872         usb_close(pHDev);
1873
1874
1875         return ERROR_OK;
1876 }
1877
1878
1879 struct jtag_interface rlink_interface =
1880 {
1881         .name = "rlink",
1882         .init = rlink_init,
1883         .quit = rlink_quit,
1884         .register_commands = rlink_register_commands,
1885         .speed = rlink_speed,
1886         .speed_div = rlink_speed_div,
1887         .khz = rlink_khz,
1888         .execute_queue = rlink_execute_queue,
1889 };