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