git.gag.com Git - fw/openocd/blob - src/jtag/rlink/rlink.c

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