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         char dtc_status;
 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                                 &dtc_status, 1,
 534                                 USB_TIMEOUT_MS
 535                 );
 536                 if (usb_err < 0) return(usb_err);
 537
 538                 if (dtc_status & 0x01) break;
 539
 540                 if (!--i) {
 541                         LOG_ERROR("too many retries waiting for DTC status");
 542                         return(-ETIMEDOUT);
 543                 }
 544         }
 545
 546
 547         if (reply_buffer && reply_buffer_size) {
 548                 usb_err = usb_bulk_read(
 549                                 pHDev_param,
 550                                 USB_EP2IN_ADDR,
 551                                 (char *)reply_buffer, reply_buffer_size,
 552                                 USB_TIMEOUT_MS
 553                 );
 554
 555                 if (usb_err < reply_buffer_size) {
 556                         LOG_ERROR("Read of endpoint 2 returned %d, expected %d",
 557                                         usb_err, reply_buffer_size
 558                         );
 559                         return(usb_err);
 560                 }
 561         }
 562
 563         return(usb_err);
 564 }
 565
 566
 567 /*
 568  * 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.
 569  */
 570
 571 struct dtc_reply_queue_entry {
 572         struct dtc_reply_queue_entry    *next;
 573         struct jtag_command     *cmd;   /* the command that resulted in this entry */
 574
 575         struct {
 576                 uint8_t         *buffer;        /* the scan buffer */
 577                 int             size;           /* size of the scan buffer in bits */
 578                 int             offset;         /* how many bits were already done before this? */
 579                 int             length;         /* how many bits are processed in this operation? */
 580                 enum scan_type  type;           /* SCAN_IN/SCAN_OUT/SCAN_IO */
 581         } scan;
 582 };
 583
 584
 585 /*
 586  * The dtc_queue consists of a buffer of pending commands and a reply queue.
 587  * rlink_scan and tap_state_run add to the command buffer and maybe to the reply queue.
 588  */
 589
 590 static
 591 struct {
 592         struct dtc_reply_queue_entry    *rq_head;
 593         struct dtc_reply_queue_entry    *rq_tail;
 594         uint32_t                        cmd_index;
 595         uint32_t                        reply_index;
 596         uint8_t                 cmd_buffer[USB_EP2BANK_SIZE];
 597 } dtc_queue;
 598
 599
 600 /*
 601  * The tap state queue is for accumulating TAP state changes wiithout needlessly flushing the dtc_queue.  When it fills or is run, it adds the accumulated bytes to the dtc_queue.
 602  */
 603
 604 static
 605 struct {
 606         uint32_t        length;
 607         uint32_t        buffer;
 608 } tap_state_queue;
 609
 610
 611
 612 static
 613 int
 614 dtc_queue_init(void) {
 615         dtc_queue.rq_head = NULL;
 616         dtc_queue.rq_tail = NULL;
 617         dtc_queue.cmd_index = 0;
 618         dtc_queue.reply_index = 0;
 619         return(0);
 620 }
 621
 622
 623 static
 624 inline
 625 struct dtc_reply_queue_entry *
 626 dtc_queue_enqueue_reply(
 627                 enum scan_type  type,
 628                 uint8_t                         *buffer,
 629                 int                             size,
 630                 int                             offset,
 631                 int                             length,
 632                 struct jtag_command     *cmd
 633 ) {
 634         struct dtc_reply_queue_entry    *rq_entry;
 635
 636         rq_entry = malloc(sizeof(struct dtc_reply_queue_entry));
 637         if (rq_entry != NULL) {
 638                 rq_entry->scan.type = type;
 639                 rq_entry->scan.buffer = buffer;
 640                 rq_entry->scan.size = size;
 641                 rq_entry->scan.offset = offset;
 642                 rq_entry->scan.length = length;
 643                 rq_entry->cmd = cmd;
 644                 rq_entry->next = NULL;
 645
 646                 if (dtc_queue.rq_head == NULL)
 647                         dtc_queue.rq_head = rq_entry;
 648                 else
 649                         dtc_queue.rq_tail->next = rq_entry;
 650
 651                 dtc_queue.rq_tail = rq_entry;
 652         }
 653
 654         return(rq_entry);
 655 }
 656
 657
 658 /*
 659  * 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.
 660  * The queue is automatically run by append when it is necessary to get space for the append.
 661  */
 662
 663 static
 664 int
 665 dtc_queue_run(void) {
 666         struct dtc_reply_queue_entry    *rq_p, *rq_next;
 667         int                     retval;
 668         int                     usb_err;
 669         int                     bit_cnt;
 670         int                     x;
 671         uint8_t                 *dtc_p, *tdo_p;
 672         uint8_t                 dtc_mask, tdo_mask;
 673         uint8_t                 reply_buffer[USB_EP2IN_SIZE];
 674
 675         assert((dtc_queue.rq_head != 0) == (dtc_queue.reply_index > 0));
 676         assert(dtc_queue.cmd_index < USB_EP2BANK_SIZE);
 677         assert(dtc_queue.reply_index <= USB_EP2IN_SIZE);
 678
 679         retval = ERROR_OK;
 680
 681         if (dtc_queue.cmd_index < 1) return(retval);
 682
 683         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = DTC_CMD_STOP;
 684
 685         usb_err = dtc_run_download(pHDev,
 686                         dtc_queue.cmd_buffer, dtc_queue.cmd_index,
 687                         reply_buffer, dtc_queue.reply_index
 688         );
 689         if (usb_err < 0) {
 690                 LOG_ERROR("dtc_run_download: %s", usb_strerror());
 691                 exit(1);
 692         }
 693
 694         if (dtc_queue.rq_head != NULL) {
 695                 /* process the reply, which empties the reply queue and frees its entries */
 696                 dtc_p = reply_buffer;
 697
 698                 /* 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. */
 699
 700                 for (
 701                                 rq_p = dtc_queue.rq_head;
 702                                 rq_p != NULL;
 703                                 rq_p = rq_next
 704                 ) {
 705                         tdo_p = rq_p->scan.buffer + (rq_p->scan.offset / 8);
 706                         tdo_mask = 1 << (rq_p->scan.offset % 8);
 707
 708
 709                         bit_cnt = rq_p->scan.length;
 710                         if (bit_cnt >= 8) {
 711                                 /* bytes */
 712
 713                                 dtc_mask = 1 << (8 - 1);
 714
 715                                 for (
 716                                                 ;
 717                                                 bit_cnt;
 718                                                 bit_cnt--
 719                                 ) {
 720                                         if (*dtc_p & dtc_mask) {
 721                                                 *tdo_p |= tdo_mask;
 722                                         } else {
 723                                                 *tdo_p &=~ tdo_mask;
 724                                         }
 725
 726                                         dtc_mask >>= 1;
 727                                         if (dtc_mask == 0) {
 728                                                 dtc_p++;
 729                                                 dtc_mask = 1 << (8 - 1);
 730                                         }
 731
 732                                         tdo_mask <<= 1;
 733                                         if (tdo_mask == 0) {
 734                                                 tdo_p++;
 735                                                 tdo_mask = 1;
 736                                         }
 737                                 }
 738                         } else {
 739                                 /*  extra bits or last bit */
 740
 741                                 x = *dtc_p++;
 742                                 if ((
 743                                                 rq_p->scan.type == SCAN_IN
 744                                 ) && (
 745                                                 rq_p->scan.offset != rq_p->scan.size - 1
 746                                 )) {
 747                                         /* extra bits were sent as a full byte with padding on the end */
 748                                         dtc_mask = 1 << (8 - 1);
 749                                 } else {
 750                                         dtc_mask = 1 << (bit_cnt - 1);
 751                                 }
 752
 753                                 for (
 754                                                 ;
 755                                                 bit_cnt;
 756                                                 bit_cnt--
 757                                 ) {
 758                                         if (x & dtc_mask) {
 759                                                 *tdo_p |= tdo_mask;
 760                                         } else {
 761                                                 *tdo_p &=~ tdo_mask;
 762                                         }
 763
 764                                         dtc_mask >>= 1;
 765
 766                                         tdo_mask <<= 1;
 767                                         if (tdo_mask == 0) {
 768                                                 tdo_p++;
 769                                                 tdo_mask = 1;
 770                                         }
 771
 772                                 }
 773                         }
 774
 775                         if ((rq_p->scan.offset + rq_p->scan.length) >= rq_p->scan.size) {
 776                                 /* feed scan buffer back into openocd and free it */
 777                                 if (jtag_read_buffer(rq_p->scan.buffer, rq_p->cmd->cmd.scan) != ERROR_OK) {
 778                                         retval = ERROR_JTAG_QUEUE_FAILED;
 779                                 }
 780                                 free(rq_p->scan.buffer);
 781                         }
 782
 783                         rq_next = rq_p->next;
 784                         free(rq_p);
 785                 }
 786                 dtc_queue.rq_head = NULL;
 787                 dtc_queue.rq_tail = NULL;
 788         }
 789
 790
 791         /* reset state for new appends */
 792         dtc_queue.cmd_index = 0;
 793         dtc_queue.reply_index = 0;
 794
 795         return(retval);
 796 }
 797
 798 /* runs the queue if it cannot take reserved_cmd bytes of command data
 799  * or reserved_reply bytes of reply data */
 800 static
 801 int
 802 dtc_queue_run_if_full(
 803                 int reserved_cmd,
 804                 int reserved_reply
 805 ) {
 806         /* reserve one additional byte for the STOP cmd appended during run */
 807         if (dtc_queue.cmd_index + reserved_cmd + 1 > USB_EP2BANK_SIZE)
 808                 return dtc_queue_run();
 809
 810         if (dtc_queue.reply_index + reserved_reply > USB_EP2IN_SIZE)
 811                 return dtc_queue_run();
 812
 813         return ERROR_OK;
 814 }
 815
 816 static
 817 int
 818 tap_state_queue_init(void) {
 819         tap_state_queue.length = 0;
 820         tap_state_queue.buffer = 0;
 821         return(0);
 822 }
 823
 824
 825 static
 826 int
 827 tap_state_queue_run(void) {
 828         int     i;
 829         int     bits;
 830         uint8_t byte_param;
 831         int     retval;
 832
 833         retval = 0;
 834         if (!tap_state_queue.length) return(retval);
 835         bits = 1;
 836         byte_param = 0;
 837         for (i = tap_state_queue.length; i--;) {
 838
 839                 byte_param <<= 1;
 840                 if (tap_state_queue.buffer & 1) {
 841                         byte_param |= 1;
 842                 }
 843                 if ((bits >= 8) || !i) {
 844                         byte_param <<= (8 - bits);
 845
 846                         /* make sure there's room for two cmd bytes */
 847                         dtc_queue_run_if_full(2, 0);
 848
 849 #ifdef USE_HARDWARE_SHIFTER_FOR_TMS
 850                         if (bits == 8) {
 851                                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
 852                                                 DTC_CMD_SHIFT_TMS_BYTES(1);
 853                         } else {
 854 #endif
 855                                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
 856                                                 DTC_CMD_SHIFT_TMS_BITS(bits);
 857 #ifdef USE_HARDWARE_SHIFTER_FOR_TMS
 858                         }
 859 #endif
 860
 861                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
 862                                         byte_param;
 863
 864                         byte_param = 0;
 865                         bits = 1;
 866                 } else {
 867                         bits++;
 868                 }
 869
 870                 tap_state_queue.buffer >>= 1;
 871         }
 872         retval = tap_state_queue_init();
 873         return(retval);
 874 }
 875
 876
 877 static
 878 int
 879 tap_state_queue_append(
 880                 uint8_t tms
 881 ) {
 882         int     retval;
 883
 884         if (tap_state_queue.length >= sizeof(tap_state_queue.buffer) * 8) {
 885                 retval = tap_state_queue_run();
 886                 if (retval != 0) return(retval);
 887         }
 888
 889         if (tms) {
 890                 tap_state_queue.buffer |= (1 << tap_state_queue.length);
 891         }
 892         tap_state_queue.length++;
 893
 894         return(0);
 895 }
 896
 897
 898 static
 899 void rlink_end_state(tap_state_t state)
 900 {
 901         if (tap_is_state_stable(state))
 902                 tap_set_end_state(state);
 903         else
 904         {
 905                 LOG_ERROR("BUG: %i is not a valid end state", state);
 906                 exit(-1);
 907         }
 908 }
 909
 910
 911 static
 912 void rlink_state_move(void) {
 913
 914         int i = 0, tms = 0;
 915         uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 916         int tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
 917
 918         for (i = 0; i < tms_count; i++)
 919         {
 920                 tms = (tms_scan >> i) & 1;
 921                 tap_state_queue_append(tms);
 922         }
 923
 924         tap_set_state(tap_get_end_state());
 925 }
 926
 927 static
 928 void rlink_path_move(struct pathmove_command *cmd)
 929 {
 930         int num_states = cmd->num_states;
 931         int state_count;
 932         int tms = 0;
 933
 934         state_count = 0;
 935         while (num_states)
 936         {
 937                 if (tap_state_transition(tap_get_state(), false) == cmd->path[state_count])
 938                 {
 939                         tms = 0;
 940                 }
 941                 else if (tap_state_transition(tap_get_state(), true) == cmd->path[state_count])
 942                 {
 943                         tms = 1;
 944                 }
 945                 else
 946                 {
 947                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(tap_get_state()), tap_state_name(cmd->path[state_count]));
 948                         exit(-1);
 949                 }
 950
 951                 tap_state_queue_append(tms);
 952
 953                 tap_set_state(cmd->path[state_count]);
 954                 state_count++;
 955                 num_states--;
 956         }
 957
 958         tap_set_end_state(tap_get_state());
 959 }
 960
 961
 962 static
 963 void rlink_runtest(int num_cycles)
 964 {
 965         int i;
 966
 967         tap_state_t saved_end_state = tap_get_end_state();
 968
 969         /* only do a state_move when we're not already in RTI */
 970         if (tap_get_state() != TAP_IDLE)
 971         {
 972                 rlink_end_state(TAP_IDLE);
 973                 rlink_state_move();
 974         }
 975
 976         /* execute num_cycles */
 977         for (i = 0; i < num_cycles; i++)
 978         {
 979                 tap_state_queue_append(0);
 980         }
 981
 982         /* finish in end_state */
 983         rlink_end_state(saved_end_state);
 984         if (tap_get_state() != tap_get_end_state())
 985                 rlink_state_move();
 986 }
 987
 988
 989 /* (1) assert or (0) deassert reset lines */
 990 static
 991 void rlink_reset(int trst, int srst)
 992 {
 993         uint8_t                 bitmap;
 994         int                     usb_err;
 995
 996         /* Read port A for bit op */
 997         usb_err = ep1_generic_commandl(
 998                         pHDev, 4,
 999                         EP1_CMD_MEMORY_READ,
1000                         ST7_PADR >> 8,
1001                         ST7_PADR,
1002                         1
1003         );
1004         if (usb_err < 0) {
1005                 LOG_ERROR("%s", usb_strerror());
1006                 exit(1);
1007         }
1008
1009         usb_err = usb_bulk_read(
1010                         pHDev, USB_EP1IN_ADDR,
1011                         (char *)&bitmap, 1,
1012                         USB_TIMEOUT_MS
1013         );
1014         if (usb_err < 1) {
1015                 LOG_ERROR("%s", usb_strerror());
1016                 exit(1);
1017         }
1018
1019         if (trst) {
1020                 bitmap &= ~ST7_PA_NTRST;
1021         } else {
1022                 bitmap |= ST7_PA_NTRST;
1023         }
1024
1025         /* Write port A and read port B for bit op */
1026         /* 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. */
1027         usb_err = ep1_generic_commandl(
1028                         pHDev, 9,
1029                         EP1_CMD_MEMORY_WRITE,
1030                         ST7_PADR >> 8,
1031                         ST7_PADR,
1032                         1,
1033                         bitmap,
1034                         EP1_CMD_MEMORY_READ,
1035                         ST7_PBDDR >> 8,
1036                         ST7_PBDDR,
1037                         1
1038         );
1039         if (usb_err < 0) {
1040                 LOG_ERROR("%s", usb_strerror());
1041                 exit(1);
1042         }
1043
1044         usb_err = usb_bulk_read(
1045                         pHDev, USB_EP1IN_ADDR,
1046                         (char *)&bitmap, 1,
1047                         USB_TIMEOUT_MS
1048         );
1049         if (usb_err < 1) {
1050                 LOG_ERROR("%s", usb_strerror());
1051                 exit(1);
1052         }
1053
1054         if (srst) {
1055                 bitmap |= ST7_PB_NSRST;
1056         } else {
1057                 bitmap &= ~ST7_PB_NSRST;
1058         }
1059
1060         /* write port B and read dummy to ensure completion before returning */
1061         usb_err = ep1_generic_commandl(
1062                         pHDev, 6,
1063                         EP1_CMD_MEMORY_WRITE,
1064                         ST7_PBDDR >> 8,
1065                         ST7_PBDDR,
1066                         1,
1067                         bitmap,
1068                         EP1_CMD_DTC_GET_CACHED_STATUS
1069         );
1070         if (usb_err < 0) {
1071                 LOG_ERROR("%s", usb_strerror());
1072                 exit(1);
1073         }
1074
1075         usb_err = usb_bulk_read(
1076                         pHDev, USB_EP1IN_ADDR,
1077                         (char *)&bitmap, 1,
1078                         USB_TIMEOUT_MS
1079         );
1080         if (usb_err < 1) {
1081                 LOG_ERROR("%s", usb_strerror());
1082                 exit(1);
1083         }
1084 }
1085
1086
1087 static
1088 int
1089 rlink_scan(
1090                 struct jtag_command     *cmd,
1091                 enum scan_type  type,
1092                 uint8_t                 *buffer,
1093                 int                     scan_size
1094 ) {
1095         bool            ir_scan;
1096         tap_state_t     saved_end_state;
1097         int                     byte_bits;
1098         int                     extra_bits;
1099         int                     chunk_bits;
1100         int                     chunk_bytes;
1101         int                     x;
1102
1103         int                     tdi_bit_offset;
1104         uint8_t                 tdi_mask, *tdi_p;
1105         uint8_t                 dtc_mask;
1106
1107         if (scan_size < 1) {
1108                 LOG_ERROR("scan_size cannot be less than 1 bit");
1109                 exit(1);
1110         }
1111
1112         ir_scan = cmd->cmd.scan->ir_scan;
1113
1114         /* Move to the proper state before starting to shift TDI/TDO. */
1115         if (!(
1116                         (!ir_scan && (tap_get_state() == TAP_DRSHIFT))
1117                         ||
1118                         (ir_scan && (tap_get_state() == TAP_IRSHIFT))
1119         )) {
1120                 saved_end_state = tap_get_end_state();
1121                 rlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
1122                 rlink_state_move();
1123                 rlink_end_state(saved_end_state);
1124         }
1125
1126         tap_state_queue_run();
1127
1128
1129 #if 0
1130         printf("scan_size = %d, type = 0x%x\n", scan_size, type);
1131         {
1132                 int   i;
1133
1134                 /* clear unused bits in scan buffer for ease of debugging */
1135                 /* (it makes diffing output easier) */
1136                 buffer[scan_size / 8] &= ((1 << ((scan_size - 1) % 8) + 1) - 1);
1137
1138                 printf("before scan:");
1139                 for (i = 0; i < (scan_size + 7) / 8; i++) {
1140                         printf(" %02x", buffer[i]);
1141                 }
1142                 printf("\n");
1143         }
1144 #endif
1145
1146         /* The number of bits that can be shifted as complete bytes */
1147         byte_bits = (int)(scan_size - 1) / 8 * 8;
1148         /* The number of bits left over, not counting the last bit */
1149         extra_bits = (scan_size - 1) - byte_bits;
1150
1151         tdi_bit_offset = 0;
1152         tdi_p = buffer;
1153         tdi_mask = 1;
1154
1155         if (extra_bits && (type == SCAN_OUT)) {
1156                 /* Schedule any extra bits into the DTC command buffer, padding as needed */
1157                 /* For SCAN_OUT, this comes before the full bytes so the (leading) padding bits will fall off the end */
1158
1159                 /* make sure there's room for two cmd bytes */
1160                 dtc_queue_run_if_full(2, 0);
1161
1162                 x = 0;
1163                 dtc_mask = 1 << (extra_bits - 1);
1164
1165                 while (extra_bits--) {
1166                         if (*tdi_p & tdi_mask) {
1167                                 x |= dtc_mask;
1168                         }
1169
1170                         dtc_mask >>= 1;
1171
1172                         tdi_mask <<= 1;
1173                         if (tdi_mask == 0) {
1174                                 tdi_p++;
1175                                 tdi_mask = 1;
1176                         }
1177                 }
1178
1179                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1180                                 DTC_CMD_SHIFT_TDI_BYTES(1);
1181
1182                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1183         }
1184
1185         /* Loop scheduling full bytes into the DTC command buffer */
1186         while (byte_bits) {
1187                 /* make sure there's room for one (for in scans) or two cmd bytes and
1188                  * at least one reply byte for in or inout scans*/
1189                 dtc_queue_run_if_full(type == SCAN_IN ? 1 : 2, type != SCAN_OUT ? 1 : 0);
1190
1191                 chunk_bits = byte_bits;
1192                 /* we can only use up to 16 bytes at a time */
1193                 if (chunk_bits > (16 * 8)) chunk_bits = (16 * 8);
1194
1195                 if (type != SCAN_IN) {
1196                         /* how much is there room for, considering stop and byte op? */
1197                         x = (sizeof(dtc_queue.cmd_buffer) - (dtc_queue.cmd_index + 1 + 1)) * 8;
1198                         if (chunk_bits > x) chunk_bits = x;
1199                 }
1200
1201                 if (type != SCAN_OUT) {
1202                         /* how much is there room for in the reply buffer? */
1203                         x = (USB_EP2IN_SIZE - dtc_queue.reply_index) * 8;
1204                         if (chunk_bits > x) chunk_bits = x;
1205                 }
1206
1207                 /* so the loop will end */
1208                 byte_bits -= chunk_bits;
1209
1210                 if (type != SCAN_OUT) {
1211                         if (dtc_queue_enqueue_reply(
1212                                         type, buffer, scan_size, tdi_bit_offset,
1213                                         chunk_bits,
1214                                         cmd
1215                         ) == NULL) {
1216                                 LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
1217                                 exit(1);
1218                         }
1219                         dtc_queue.reply_index += (chunk_bits + 7) / 8;
1220
1221                         tdi_bit_offset += chunk_bits;
1222                 }
1223
1224                 /* chunk_bits is a multiple of 8, so there are no rounding issues. */
1225                 chunk_bytes = chunk_bits / 8;
1226
1227                 switch (type) {
1228                 case SCAN_IN:
1229                         x = DTC_CMD_SHIFT_TDO_BYTES(chunk_bytes);
1230                         break;
1231                 case SCAN_OUT:
1232                         x = DTC_CMD_SHIFT_TDI_BYTES(chunk_bytes);
1233                         break;
1234                 default:
1235                         x = DTC_CMD_SHIFT_TDIO_BYTES(chunk_bytes);
1236                         break;
1237                 }
1238                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1239
1240                 if (type != SCAN_IN) {
1241                         x = 0;
1242                         dtc_mask = 1 << (8 - 1);
1243
1244                         while (chunk_bits--) {
1245                                 if (*tdi_p & tdi_mask) {
1246                                         x |= dtc_mask;
1247                                 }
1248
1249                                 dtc_mask >>= 1;
1250                                 if (dtc_mask == 0) {
1251                                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1252                                         x = 0;
1253                                         dtc_mask = 1 << (8 - 1);
1254                                 }
1255
1256                                 tdi_mask <<= 1;
1257                                 if (tdi_mask == 0) {
1258                                         tdi_p++;
1259                                         tdi_mask = 1;
1260                                 }
1261                         }
1262                 }
1263         }
1264
1265         if (extra_bits && (type != SCAN_OUT)) {
1266                 /* Schedule any extra bits into the DTC command buffer */
1267
1268                 /* make sure there's room for one (for in scans) or two cmd bytes
1269                  * and one reply byte */
1270                 dtc_queue_run_if_full(type == SCAN_IN ? 1 : 2, 1);
1271
1272                 if (dtc_queue_enqueue_reply(
1273                                 type, buffer, scan_size, tdi_bit_offset,
1274                                 extra_bits,
1275                                 cmd
1276                 ) == NULL) {
1277                         LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
1278                         exit(1);
1279                 }
1280
1281                 dtc_queue.reply_index++;
1282
1283                 tdi_bit_offset += extra_bits;
1284
1285                 if (type == SCAN_IN) {
1286                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1287                                         DTC_CMD_SHIFT_TDO_BYTES(1);
1288
1289                 } else {
1290                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1291                                         DTC_CMD_SHIFT_TDIO_BITS(extra_bits);
1292
1293                         x = 0;
1294                         dtc_mask = 1 << (8 - 1);
1295
1296                         while (extra_bits--) {
1297                                 if (*tdi_p & tdi_mask) {
1298                                         x |= dtc_mask;
1299                                 }
1300
1301                                 dtc_mask >>= 1;
1302
1303                                 tdi_mask <<= 1;
1304                                 if (tdi_mask == 0) {
1305                                         tdi_p++;
1306                                         tdi_mask = 1;
1307                                 }
1308                         }
1309
1310                         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
1311                 }
1312         }
1313
1314         /* Schedule the last bit into the DTC command buffer */
1315
1316         /* make sure there's room for one cmd byte and one reply byte
1317          * for in or inout scans*/
1318         dtc_queue_run_if_full(1, type == SCAN_OUT ? 0 : 1);
1319
1320         if (type == SCAN_OUT) {
1321                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1322                                 DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 0);
1323
1324         } else {
1325                 if (dtc_queue_enqueue_reply(
1326                                 type, buffer, scan_size, tdi_bit_offset,
1327                                 1,
1328                                 cmd
1329                 ) == NULL) {
1330                         LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
1331                         exit(1);
1332                 }
1333
1334                 dtc_queue.reply_index++;
1335
1336                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
1337                                 DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 1);
1338         }
1339
1340         /* Move to pause state */
1341         tap_state_queue_append(0);
1342         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
1343         if (tap_get_state() != tap_get_end_state()) rlink_state_move();
1344
1345         return(0);
1346 }
1347
1348
1349 static
1350 int rlink_execute_queue(void)
1351 {
1352         struct jtag_command *cmd = jtag_command_queue; /* currently processed command */
1353         int scan_size;
1354         enum scan_type type;
1355         uint8_t *buffer;
1356         int retval, tmp_retval;
1357
1358         /* return ERROR_OK, unless something goes wrong */
1359         retval = ERROR_OK;
1360
1361 #ifndef AUTOMATIC_BUSY_LED
1362         /* turn LED on */
1363         ep1_generic_commandl(pHDev, 2,
1364                         EP1_CMD_SET_PORTD_LEDS,
1365                         ~(ST7_PD_NBUSY_LED)
1366         );
1367 #endif
1368
1369         while (cmd)
1370         {
1371                 switch (cmd->type)
1372                 {
1373                 case JTAG_RUNTEST:
1374                 case JTAG_TLR_RESET:
1375                 case JTAG_PATHMOVE:
1376                 case JTAG_SCAN:
1377                         break;
1378
1379                 default:
1380                         /* some events, such as resets, need a queue flush to ensure consistency */
1381                         tap_state_queue_run();
1382                         dtc_queue_run();
1383                         break;
1384                 }
1385
1386                 switch (cmd->type)
1387                 {
1388                 case JTAG_RESET:
1389 #ifdef _DEBUG_JTAG_IO_
1390                         LOG_DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
1391 #endif
1392                         if ((cmd->cmd.reset->trst == 1) || (cmd->cmd.reset->srst && (jtag_get_reset_config() & RESET_SRST_PULLS_TRST)))
1393                         {
1394                                 tap_set_state(TAP_RESET);
1395                         }
1396                         rlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
1397                         break;
1398                 case JTAG_RUNTEST:
1399 #ifdef _DEBUG_JTAG_IO_
1400                         LOG_DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
1401 #endif
1402                         if (cmd->cmd.runtest->end_state != -1)
1403                                 rlink_end_state(cmd->cmd.runtest->end_state);
1404                         rlink_runtest(cmd->cmd.runtest->num_cycles);
1405                         break;
1406                 case JTAG_TLR_RESET:
1407 #ifdef _DEBUG_JTAG_IO_
1408                         LOG_DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
1409 #endif
1410                         if (cmd->cmd.statemove->end_state != -1)
1411                                 rlink_end_state(cmd->cmd.statemove->end_state);
1412                         rlink_state_move();
1413                         break;
1414                 case JTAG_PATHMOVE:
1415 #ifdef _DEBUG_JTAG_IO_
1416                         LOG_DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
1417 #endif
1418                         rlink_path_move(cmd->cmd.pathmove);
1419                         break;
1420                 case JTAG_SCAN:
1421 #ifdef _DEBUG_JTAG_IO_
1422                         LOG_DEBUG("%s scan end in %i",  (cmd->cmd.scan->ir_scan) ? "IR" : "DR", cmd->cmd.scan->end_state);
1423 #endif
1424                         if (cmd->cmd.scan->end_state != -1)
1425                                 rlink_end_state(cmd->cmd.scan->end_state);
1426                         scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
1427                         type = jtag_scan_type(cmd->cmd.scan);
1428                         if (rlink_scan(cmd, type, buffer, scan_size) != ERROR_OK) {
1429                                 retval = ERROR_FAIL;
1430                         }
1431                         break;
1432                 case JTAG_SLEEP:
1433 #ifdef _DEBUG_JTAG_IO_
1434                         LOG_DEBUG("sleep %i", cmd->cmd.sleep->us);
1435 #endif
1436                         jtag_sleep(cmd->cmd.sleep->us);
1437                         break;
1438                 default:
1439                         LOG_ERROR("BUG: unknown JTAG command type encountered");
1440                         exit(-1);
1441                 }
1442                 cmd = cmd->next;
1443         }
1444
1445         /* Flush the DTC queue to make sure any pending reads have been done before exiting this function */
1446         tap_state_queue_run();
1447         tmp_retval = dtc_queue_run();
1448         if (tmp_retval != ERROR_OK) {
1449                 retval = tmp_retval;
1450         }
1451
1452 #ifndef AUTOMATIC_BUSY_LED
1453         /* turn LED onff */
1454         ep1_generic_commandl(pHDev, 2,
1455                         EP1_CMD_SET_PORTD_LEDS,
1456                         ~0
1457         );
1458 #endif
1459
1460         return retval;
1461 }
1462
1463
1464 /* 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. */
1465
1466 static
1467 int rlink_speed(int speed)
1468 {
1469         int             i;
1470
1471         if (speed == 0) {
1472                 /* fastest speed */
1473                 speed = rlink_speed_table[rlink_speed_table_size - 1].prescaler;
1474         }
1475
1476         for (i = rlink_speed_table_size; i--;) {
1477                 if (rlink_speed_table[i].prescaler == speed) {
1478                         if (dtc_load_from_buffer(pHDev, rlink_speed_table[i].dtc, rlink_speed_table[i].dtc_size) != 0) {
1479                                 LOG_ERROR("An error occurred while trying to load DTC code for speed \"%d\".", speed);
1480                                 exit(1);
1481                         }
1482
1483                         if (dtc_start_download() < 0) {
1484                                 LOG_ERROR("starting DTC: %s", usb_strerror());
1485                                 exit(1);
1486                         }
1487
1488                         return ERROR_OK;
1489                 }
1490         }
1491
1492         LOG_ERROR("%d is not a supported speed", speed);
1493         return(ERROR_FAIL);
1494 }
1495
1496
1497 static
1498 int rlink_speed_div(
1499                 int speed,
1500                 int *khz
1501 ) {
1502         int     i;
1503
1504         for (i = rlink_speed_table_size; i--;) {
1505                 if (rlink_speed_table[i].prescaler == speed) {
1506                         *khz = rlink_speed_table[i].khz;
1507                         return(ERROR_OK);
1508                 }
1509         }
1510
1511         LOG_ERROR("%d is not a supported speed", speed);
1512         return(ERROR_FAIL);
1513 }
1514
1515
1516 static
1517 int rlink_khz(
1518                 int khz,
1519                 int *speed
1520 ) {
1521         int     i;
1522
1523         if (khz == 0) {
1524                 LOG_ERROR("RCLK not supported");
1525                 return ERROR_FAIL;
1526         }
1527
1528         for (i = rlink_speed_table_size; i--;) {
1529                 if (rlink_speed_table[i].khz <= khz) {
1530                         *speed = rlink_speed_table[i].prescaler;
1531                         return(ERROR_OK);
1532                 }
1533         }
1534
1535         LOG_WARNING("The lowest supported JTAG speed is %d KHz", rlink_speed_table[0].khz);
1536         *speed = rlink_speed_table[0].prescaler;
1537         return(ERROR_OK);
1538 }
1539
1540
1541 static
1542 int rlink_init(void)
1543 {
1544         int i, j, retries;
1545         uint8_t reply_buffer[USB_EP1IN_SIZE];
1546
1547         usb_init();
1548         const uint16_t vids[] = { USB_IDVENDOR, 0 };
1549         const uint16_t pids[] = { USB_IDPRODUCT, 0 };
1550         if (jtag_usb_open(vids, pids, &pHDev) != ERROR_OK)
1551                 return ERROR_FAIL;
1552
1553         struct usb_device *dev = usb_device(pHDev);
1554         if (dev->descriptor.bNumConfigurations > 1)
1555         {
1556                 LOG_ERROR("Whoops! NumConfigurations is not 1, don't know what to do...");
1557                 return ERROR_FAIL;
1558         }
1559         if (dev->config->bNumInterfaces > 1)
1560         {
1561                 LOG_ERROR("Whoops! NumInterfaces is not 1, don't know what to do...");
1562                 return ERROR_FAIL;
1563         }
1564
1565         LOG_DEBUG("Opened device, pHDev = %p", pHDev);
1566
1567         /* usb_set_configuration required under win32 */
1568         usb_set_configuration(pHDev, dev->config[0].bConfigurationValue);
1569
1570         retries = 3;
1571         do
1572         {
1573                 i = usb_claim_interface(pHDev,0);
1574                 if (i)
1575                 {
1576                         LOG_ERROR("usb_claim_interface: %s", usb_strerror());
1577 #ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
1578                         j = usb_detach_kernel_driver_np(pHDev, 0);
1579                         if (j)
1580                                 LOG_ERROR("detach kernel driver: %s", usb_strerror());
1581 #endif
1582                 }
1583                 else
1584                 {
1585                         LOG_DEBUG("interface claimed!");
1586                         break;
1587                 }
1588         } while (--retries);
1589
1590         if (i)
1591         {
1592                 LOG_ERROR("Initialisation failed.");
1593                 return ERROR_FAIL;
1594         }
1595         if (usb_set_altinterface(pHDev,0) != 0)
1596         {
1597                 LOG_ERROR("Failed to set interface.");
1598                 return ERROR_FAIL;
1599         }
1600
1601         /* The device starts out in an unknown state on open.  As such,
1602          * result reads time out, and it's not even known whether the
1603          * command was accepted.  So, for this first command, we issue
1604          * it repeatedly until its response doesn't time out.  Also, if
1605          * sending a command is going to time out, we find that out here.
1606          *
1607          * It must be possible to open the device in such a way that
1608          * this special magic isn't needed, but, so far, it escapes us.
1609          */
1610         for (i = 0; i < 5; i++) {
1611                 j = ep1_generic_commandl(
1612                                 pHDev, 1,
1613                                 EP1_CMD_GET_FWREV
1614                 );
1615                 if (j < USB_EP1OUT_SIZE) {
1616                         LOG_ERROR("USB write error: %s", usb_strerror());
1617                         return(ERROR_FAIL);
1618                 }
1619                 j = usb_bulk_read(
1620                                 pHDev, USB_EP1IN_ADDR,
1621                                 (char *)reply_buffer, sizeof(reply_buffer),
1622                                 200
1623                 );
1624                 if (j != -ETIMEDOUT) break;
1625         }
1626
1627         if (j < (int)sizeof(reply_buffer)) {
1628                 LOG_ERROR("USB read error: %s", usb_strerror());
1629                 return(ERROR_FAIL);
1630         }
1631         LOG_DEBUG(INTERFACE_NAME" firmware version: %d.%d.%d", reply_buffer[0], reply_buffer[1], reply_buffer[2]);
1632
1633         if ((reply_buffer[0] != 0) || (reply_buffer[1] != 0) || (reply_buffer[2] != 3)) {
1634                 LOG_WARNING("The rlink device is not of the version that the developers have played with.  It may or may not work.");
1635         }
1636
1637         /* Probe port E for adapter presence */
1638         ep1_generic_commandl(
1639                         pHDev, 16,
1640                         EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 0 */
1641                         ST7_PEDR >> 8,
1642                         ST7_PEDR,
1643                         3,
1644                         0x00,                                           /* DR */
1645                         ST7_PE_ADAPTER_SENSE_OUT,       /* DDR */
1646                         ST7_PE_ADAPTER_SENSE_OUT,       /* OR */
1647                         EP1_CMD_MEMORY_READ,    /* Read back */
1648                         ST7_PEDR >> 8,
1649                         ST7_PEDR,
1650                         1,
1651                         EP1_CMD_MEMORY_WRITE,   /* Drive sense pin with 1 */
1652                         ST7_PEDR >> 8,
1653                         ST7_PEDR,
1654                         1,
1655                         ST7_PE_ADAPTER_SENSE_OUT
1656         );
1657
1658         usb_bulk_read(
1659                         pHDev, USB_EP1IN_ADDR,
1660                         (char *)reply_buffer, 1,
1661                         USB_TIMEOUT_MS
1662         );
1663
1664         if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) != 0) {
1665                 LOG_WARNING("target detection problem");
1666         }
1667
1668         ep1_generic_commandl(
1669                         pHDev, 11,
1670                         EP1_CMD_MEMORY_READ,    /* Read back */
1671                         ST7_PEDR >> 8,
1672                         ST7_PEDR,
1673                         1,
1674                         EP1_CMD_MEMORY_WRITE,   /* float port E */
1675                         ST7_PEDR >> 8,
1676                         ST7_PEDR,
1677                         3,
1678                         0x00,   /* DR */
1679                         0x00,   /* DDR */
1680                         0x00    /* OR */
1681         );
1682
1683         usb_bulk_read(
1684                         pHDev, USB_EP1IN_ADDR,
1685                         (char *)reply_buffer, 1,
1686                         USB_TIMEOUT_MS
1687         );
1688
1689
1690         if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) == 0) {
1691                 LOG_WARNING("target not plugged in");
1692         }
1693
1694         /* float ports A and B */
1695         ep1_generic_commandl(
1696                         pHDev, 11,
1697                         EP1_CMD_MEMORY_WRITE,
1698                         ST7_PADDR >> 8,
1699                         ST7_PADDR,
1700                         2,
1701                         0x00,
1702                         0x00,
1703                         EP1_CMD_MEMORY_WRITE,
1704                         ST7_PBDDR >> 8,
1705                         ST7_PBDDR,
1706                         1,
1707                         0x00
1708         );
1709
1710         /* make sure DTC is stopped, set VPP control, set up ports A and B */
1711         ep1_generic_commandl(
1712                         pHDev, 14,
1713                         EP1_CMD_DTC_STOP,
1714                         EP1_CMD_SET_PORTD_VPP,
1715                         ~(ST7_PD_VPP_SHDN),
1716                         EP1_CMD_MEMORY_WRITE,
1717                         ST7_PADR >> 8,
1718                         ST7_PADR,
1719                         2,
1720                         ((~(0)) & (ST7_PA_NTRST)),
1721                         (ST7_PA_NTRST),
1722                         /* 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. */
1723                         EP1_CMD_MEMORY_WRITE,
1724                         ST7_PBDR >> 8,
1725                         ST7_PBDR,
1726                         1,
1727                         0x00
1728         );
1729
1730         /* set LED updating mode and make sure they're unlit */
1731         ep1_generic_commandl(
1732                         pHDev, 3,
1733 #ifdef AUTOMATIC_BUSY_LED
1734                         EP1_CMD_LEDUE_BUSY,
1735 #else
1736                         EP1_CMD_LEDUE_NONE,
1737 #endif
1738                         EP1_CMD_SET_PORTD_LEDS,
1739                         ~0
1740         );
1741
1742         tap_state_queue_init();
1743         dtc_queue_init();
1744         rlink_reset(0, 0);
1745
1746         return ERROR_OK;
1747 }
1748
1749
1750 static
1751 int rlink_quit(void)
1752 {
1753         /* stop DTC and make sure LEDs are off */
1754         ep1_generic_commandl(
1755                         pHDev, 6,
1756                         EP1_CMD_DTC_STOP,
1757                         EP1_CMD_LEDUE_NONE,
1758                         EP1_CMD_SET_PORTD_LEDS,
1759                         ~0,
1760                         EP1_CMD_SET_PORTD_VPP,
1761                         ~0
1762         );
1763
1764         usb_release_interface(pHDev,0);
1765         usb_close(pHDev);
1766
1767
1768         return ERROR_OK;
1769 }
1770
1771
1772 struct jtag_interface rlink_interface =
1773 {
1774                 .name = "rlink",
1775                 .init = rlink_init,
1776                 .quit = rlink_quit,
1777                 .speed = rlink_speed,
1778                 .speed_div = rlink_speed_div,
1779                 .khz = rlink_khz,
1780                 .execute_queue = rlink_execute_queue,
1781 };