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