rlink.c

Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2005 by Dominic Rath                                    *
00003  *   Dominic.Rath@gmx.de                                                   *
00004  *                                                                         *
00005  *   Copyright (C) 2007,2008 √ėyvind Harboe                                 *
00006  *   oyvind.harboe@zylin.com                                               *
00007  *                                                                         *
00008  *   Copyright (C) 2008 Rob Brown, Lou Deluxe                              *
00009  *   rob@cobbleware.com, lou.openocd012@fixit.nospammail.net               *
00010  *                                                                         *
00011  *   This program is free software; you can redistribute it and/or modify  *
00012  *   it under the terms of the GNU General Public License as published by  *
00013  *   the Free Software Foundation; either version 2 of the License, or     *
00014  *   (at your option) any later version.                                   *
00015  *                                                                         *
00016  *   This program is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00019  *   GNU General Public License for more details.                          *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU General Public License     *
00022  *   along with this program; if not, write to the                         *
00023  *   Free Software Foundation, Inc.,                                       *
00024  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
00025  ***************************************************************************/
00026 
00027 #ifdef HAVE_CONFIG_H
00028 #include "config.h"
00029 #endif
00030 
00031 /* project specific includes */
00032 #include <jtag/interface.h>
00033 #include <jtag/commands.h>
00034 #include "rlink.h"
00035 #include "rlink_st7.h"
00036 #include "rlink_ep1_cmd.h"
00037 #include "rlink_dtc_cmd.h"
00038 #include "usb_common.h"
00039 
00040 /* This feature is made useless by running the DTC all the time.  When automatic, the LED is on
00041  *whenever the DTC is running.  Otherwise, USB messages are sent to turn it on and off. */
00042 #undef AUTOMATIC_BUSY_LED
00043 
00044 /* This feature may require derating the speed due to reduced hold time. */
00045 #undef USE_HARDWARE_SHIFTER_FOR_TMS
00046 
00047 #define INTERFACE_NAME          "RLink"
00048 
00049 #define USB_IDVENDOR            (0x138e)
00050 #define USB_IDPRODUCT           (0x9000)
00051 
00052 #define USB_EP1OUT_ADDR         (0x01)
00053 #define USB_EP1OUT_SIZE         (16)
00054 #define USB_EP1IN_ADDR          (USB_EP1OUT_ADDR | 0x80)
00055 #define USB_EP1IN_SIZE          (USB_EP1OUT_SIZE)
00056 
00057 #define USB_EP2OUT_ADDR         (0x02)
00058 #define USB_EP2OUT_SIZE         (64)
00059 #define USB_EP2IN_ADDR          (USB_EP2OUT_ADDR | 0x80)
00060 #define USB_EP2IN_SIZE          (USB_EP2OUT_SIZE)
00061 #define USB_EP2BANK_SIZE        (512)
00062 
00063 #define USB_TIMEOUT_MS          (3 * 1000)
00064 
00065 #define DTC_STATUS_POLL_BYTE    (ST7_USB_BUF_EP0OUT + 0xff)
00066 
00067 #define ST7_PD_NBUSY_LED                ST7_PD0
00068 #define ST7_PD_NRUN_LED                 ST7_PD1
00069 /* low enables VPP at adapter header, high connects it to GND instead */
00070 #define ST7_PD_VPP_SEL                  ST7_PD6
00071 /* low: VPP = 12v, high: VPP <= 5v */
00072 #define ST7_PD_VPP_SHDN                 ST7_PD7
00073 
00074 /* These pins are connected together */
00075 #define ST7_PE_ADAPTER_SENSE_IN         ST7_PE3
00076 #define ST7_PE_ADAPTER_SENSE_OUT        ST7_PE4
00077 
00078 /* Symbolic mapping between port pins and numbered IO lines */
00079 #define ST7_PA_IO1      ST7_PA1
00080 #define ST7_PA_IO2      ST7_PA2
00081 #define ST7_PA_IO4      ST7_PA4
00082 #define ST7_PA_IO8      ST7_PA6
00083 #define ST7_PA_IO10     ST7_PA7
00084 #define ST7_PB_IO5      ST7_PB5
00085 #define ST7_PC_IO9      ST7_PC1
00086 #define ST7_PC_IO3      ST7_PC2
00087 #define ST7_PC_IO7      ST7_PC3
00088 #define ST7_PE_IO6      ST7_PE5
00089 
00090 /* Symbolic mapping between numbered IO lines and adapter signals */
00091 #define ST7_PA_RTCK     ST7_PA_IO0
00092 #define ST7_PA_NTRST    ST7_PA_IO1
00093 #define ST7_PC_TDI      ST7_PC_IO3
00094 #define ST7_PA_DBGRQ    ST7_PA_IO4
00095 #define ST7_PB_NSRST    ST7_PB_IO5
00096 #define ST7_PE_TMS      ST7_PE_IO6
00097 #define ST7_PC_TCK      ST7_PC_IO7
00098 #define ST7_PC_TDO      ST7_PC_IO9
00099 #define ST7_PA_DBGACK   ST7_PA_IO10
00100 
00101 static usb_dev_handle *pHDev;
00102 
00103 /*
00104  * ep1 commands are up to USB_EP1OUT_SIZE bytes in length.
00105  * This function takes care of zeroing the unused bytes before sending the packet.
00106  * Any reply packet is not handled by this function.
00107  */
00108 static int ep1_generic_commandl(usb_dev_handle *pHDev_param, size_t length, ...)
00109 {
00110     uint8_t usb_buffer[USB_EP1OUT_SIZE];
00111     uint8_t *usb_buffer_p;
00112     va_list ap;
00113     int usb_ret;
00114 
00115     if (length > sizeof(usb_buffer))
00116         length = sizeof(usb_buffer);
00117 
00118     usb_buffer_p = usb_buffer;
00119 
00120     va_start(ap, length);
00121     while (length > 0) {
00122         *usb_buffer_p++ = va_arg(ap, int);
00123         length--;
00124     }
00125 
00126     memset(
00127         usb_buffer_p,
00128         0,
00129         sizeof(usb_buffer) - (usb_buffer_p - usb_buffer)
00130         );
00131 
00132     usb_ret = usb_bulk_write(
00133             pHDev_param,
00134             USB_EP1OUT_ADDR,
00135             (char *)usb_buffer, sizeof(usb_buffer),
00136             USB_TIMEOUT_MS
00137             );
00138 
00139     return usb_ret;
00140 }
00141 
00142 #if 0
00143 static ssize_t ep1_memory_read(
00144     usb_dev_handle *pHDev, uint16_t addr,
00145     size_t length, uint8_t *buffer)
00146 {
00147     uint8_t usb_buffer[USB_EP1OUT_SIZE];
00148     int usb_ret;
00149     size_t remain;
00150     ssize_t count;
00151 
00152     usb_buffer[0] = EP1_CMD_MEMORY_READ;
00153     memset(
00154         usb_buffer + 4,
00155         0,
00156         sizeof(usb_buffer) - 4
00157         );
00158 
00159     remain = length;
00160     count = 0;
00161 
00162     while (remain) {
00163         if (remain > sizeof(usb_buffer))
00164             length = sizeof(usb_buffer);
00165         else
00166             length = remain;
00167 
00168         usb_buffer[1] = addr >> 8;
00169         usb_buffer[2] = addr;
00170         usb_buffer[3] = length;
00171 
00172         usb_ret = usb_bulk_write(
00173                 pHDev, USB_EP1OUT_ADDR,
00174                 usb_buffer, sizeof(usb_buffer),
00175                 USB_TIMEOUT_MS
00176                 );
00177 
00178         if (usb_ret < sizeof(usb_buffer))
00179             break;
00180 
00181         usb_ret = usb_bulk_read(
00182                 pHDev, USB_EP1IN_ADDR,
00183                 buffer, length,
00184                 USB_TIMEOUT_MS
00185                 );
00186 
00187         if (usb_ret < length)
00188             break;
00189 
00190         addr += length;
00191         buffer += length;
00192         count += length;
00193         remain -= length;
00194     }
00195 
00196     return count;
00197 }
00198 #endif
00199 
00200 static ssize_t ep1_memory_write(usb_dev_handle *pHDev_param, uint16_t addr,
00201     size_t length, uint8_t const *buffer)
00202 {
00203     uint8_t usb_buffer[USB_EP1OUT_SIZE];
00204     int usb_ret;
00205     size_t remain;
00206     ssize_t count;
00207 
00208     usb_buffer[0] = EP1_CMD_MEMORY_WRITE;
00209 
00210     remain = length;
00211     count = 0;
00212 
00213     while (remain) {
00214         if (remain > (sizeof(usb_buffer) - 4))
00215             length = (sizeof(usb_buffer) - 4);
00216         else
00217             length = remain;
00218 
00219         usb_buffer[1] = addr >> 8;
00220         usb_buffer[2] = addr;
00221         usb_buffer[3] = length;
00222         memcpy(
00223             usb_buffer + 4,
00224             buffer,
00225             length
00226             );
00227         memset(
00228             usb_buffer + 4 + length,
00229             0,
00230             sizeof(usb_buffer) - 4 - length
00231             );
00232 
00233         usb_ret = usb_bulk_write(
00234                 pHDev_param, USB_EP1OUT_ADDR,
00235                 (char *)usb_buffer, sizeof(usb_buffer),
00236                 USB_TIMEOUT_MS
00237                 );
00238 
00239         if ((size_t)usb_ret < sizeof(usb_buffer))
00240             break;
00241 
00242         addr += length;
00243         buffer += length;
00244         count += length;
00245         remain -= length;
00246     }
00247 
00248     return count;
00249 }
00250 
00251 
00252 #if 0
00253 static ssize_t ep1_memory_writel(usb_dev_handle *pHDev, uint16_t addr,
00254     size_t length, ...)
00255 {
00256     uint8_t buffer[USB_EP1OUT_SIZE - 4];
00257     uint8_t *buffer_p;
00258     va_list ap;
00259     size_t remain;
00260 
00261     if (length > sizeof(buffer))
00262         length = sizeof(buffer);
00263 
00264     remain = length;
00265     buffer_p = buffer;
00266 
00267     va_start(ap, length);
00268     while (remain > 0) {
00269         *buffer_p++ = va_arg(ap, int);
00270         remain--;
00271     }
00272 
00273     return ep1_memory_write(pHDev, addr, length, buffer);
00274 }
00275 #endif
00276 
00277 #define DTCLOAD_COMMENT         (0)
00278 #define DTCLOAD_ENTRY           (1)
00279 #define DTCLOAD_LOAD            (2)
00280 #define DTCLOAD_RUN                     (3)
00281 #define DTCLOAD_LUT_START       (4)
00282 #define DTCLOAD_LUT                     (5)
00283 
00284 #define DTC_LOAD_BUFFER         ST7_USB_BUF_EP2UIDO
00285 
00286 /* This gets set by the DTC loader */
00287 static uint8_t dtc_entry_download;
00288 
00289 /* The buffer is specially formatted to represent a valid image to load into the DTC. */
00290 static int dtc_load_from_buffer(usb_dev_handle *pHDev_param, const uint8_t *buffer,
00291         size_t length)
00292 {
00293     struct header_s {
00294         uint8_t type;
00295         uint8_t length;
00296     };
00297 
00298     int usb_err;
00299     struct header_s *header;
00300     uint8_t lut_start = 0xc0;
00301 
00302     dtc_entry_download = 0;
00303 
00304     /* Stop the DTC before loading anything. */
00305     usb_err = ep1_generic_commandl(
00306             pHDev_param, 1,
00307             EP1_CMD_DTC_STOP
00308             );
00309     if (usb_err < 0)
00310         return usb_err;
00311 
00312     while (length) {
00313         if (length < sizeof(*header)) {
00314             LOG_ERROR("Malformed DTC image");
00315             exit(1);
00316         }
00317 
00318         header = (struct header_s *)buffer;
00319         buffer += sizeof(*header);
00320         length -= sizeof(*header);
00321 
00322         if (length < (size_t)header->length + 1) {
00323             LOG_ERROR("Malformed DTC image");
00324             exit(1);
00325         }
00326 
00327         switch (header->type) {
00328             case DTCLOAD_COMMENT:
00329                 break;
00330 
00331             case DTCLOAD_ENTRY:
00332                 /* store entry addresses somewhere */
00333                 if (!strncmp("download", (char *)buffer + 1, 8))
00334                     dtc_entry_download = buffer[0];
00335                 break;
00336 
00337             case DTCLOAD_LOAD:
00338                 /* Send the DTC program to ST7 RAM. */
00339                 usb_err = ep1_memory_write(
00340                         pHDev_param,
00341                         DTC_LOAD_BUFFER,
00342                         header->length + 1, buffer
00343                     );
00344                 if (usb_err < 0)
00345                     return usb_err;
00346 
00347                 /* Load it into the DTC. */
00348                 usb_err = ep1_generic_commandl(
00349                         pHDev_param, 3,
00350                         EP1_CMD_DTC_LOAD,
00351                         (DTC_LOAD_BUFFER >> 8),
00352                         DTC_LOAD_BUFFER
00353                     );
00354                 if (usb_err < 0)
00355                     return usb_err;
00356 
00357                 break;
00358 
00359             case DTCLOAD_RUN:
00360                 usb_err = ep1_generic_commandl(
00361                         pHDev_param, 3,
00362                         EP1_CMD_DTC_CALL,
00363                         buffer[0],
00364                         EP1_CMD_DTC_WAIT
00365                     );
00366                 if (usb_err < 0)
00367                     return usb_err;
00368 
00369                 break;
00370 
00371             case DTCLOAD_LUT_START:
00372                 lut_start = buffer[0];
00373                 break;
00374 
00375             case DTCLOAD_LUT:
00376                 usb_err = ep1_memory_write(
00377                         pHDev_param,
00378                         ST7_USB_BUF_EP0OUT + lut_start,
00379                         header->length + 1, buffer
00380                     );
00381                 if (usb_err < 0)
00382                     return usb_err;
00383                 break;
00384 
00385             default:
00386                 LOG_ERROR("Invalid DTC image record type: 0x%02x", header->type);
00387                 exit(1);
00388                 break;
00389         }
00390 
00391         buffer += (header->length + 1);
00392         length -= (header->length + 1);
00393     }
00394 
00395     return 0;
00396 }
00397 
00398 /*
00399  * Start the DTC running in download mode (waiting for 512 byte command packets on ep2).
00400  */
00401 static int dtc_start_download(void)
00402 {
00403     int usb_err;
00404     uint8_t ep2txr;
00405 
00406     /* set up for download mode and make sure EP2 is set up to transmit */
00407     usb_err = ep1_generic_commandl(
00408             pHDev, 7,
00409 
00410             EP1_CMD_DTC_STOP,
00411             EP1_CMD_SET_UPLOAD,
00412             EP1_CMD_SET_DOWNLOAD,
00413             EP1_CMD_MEMORY_READ,    /* read EP2TXR for its data toggle */
00414             ST7_EP2TXR >> 8,
00415             ST7_EP2TXR,
00416             1
00417             );
00418     if (usb_err < 0)
00419         return usb_err;
00420 
00421     /* read back ep2txr */
00422     usb_err = usb_bulk_read(
00423             pHDev, USB_EP1IN_ADDR,
00424             (char *)&ep2txr, 1,
00425             USB_TIMEOUT_MS
00426             );
00427     if (usb_err < 0)
00428         return usb_err;
00429 
00430     usb_err = ep1_generic_commandl(
00431             pHDev, 13,
00432 
00433             EP1_CMD_MEMORY_WRITE,   /* preinitialize poll byte */
00434             DTC_STATUS_POLL_BYTE >> 8,
00435             DTC_STATUS_POLL_BYTE,
00436             1,
00437             0x00,
00438             EP1_CMD_MEMORY_WRITE,   /* set EP2IN to return data */
00439             ST7_EP2TXR >> 8,
00440             ST7_EP2TXR,
00441             1,
00442             (ep2txr & ST7_EP2TXR_DTOG_TX) | ST7_EP2TXR_STAT_VALID,
00443             EP1_CMD_DTC_CALL,   /* start running the DTC */
00444             dtc_entry_download,
00445             EP1_CMD_DTC_GET_CACHED_STATUS
00446             );
00447     if (usb_err < 0)
00448         return usb_err;
00449 
00450     /* wait for completion */
00451     usb_err = usb_bulk_read(
00452             pHDev, USB_EP1IN_ADDR,
00453             (char *)&ep2txr, 1,
00454             USB_TIMEOUT_MS
00455             );
00456 
00457     return usb_err;
00458 }
00459 
00460 static int dtc_run_download(
00461     usb_dev_handle *pHDev_param,
00462     uint8_t *command_buffer,
00463     int command_buffer_size,
00464     uint8_t *reply_buffer,
00465     int reply_buffer_size
00466     )
00467 {
00468     char dtc_status;
00469     int usb_err;
00470     int i;
00471 
00472     LOG_DEBUG("%d/%d", command_buffer_size, reply_buffer_size);
00473 
00474     usb_err = usb_bulk_write(
00475             pHDev_param,
00476             USB_EP2OUT_ADDR,
00477             (char *)command_buffer, USB_EP2BANK_SIZE,
00478             USB_TIMEOUT_MS
00479             );
00480     if (usb_err < 0)
00481         return usb_err;
00482 
00483 
00484     /* Wait for DTC to finish running command buffer */
00485     for (i = 50;; ) {
00486         usb_err = ep1_generic_commandl(
00487                 pHDev_param, 4,
00488 
00489                 EP1_CMD_MEMORY_READ,
00490                 DTC_STATUS_POLL_BYTE >> 8,
00491                 DTC_STATUS_POLL_BYTE,
00492                 1
00493                 );
00494         if (usb_err < 0)
00495             return usb_err;
00496 
00497         usb_err = usb_bulk_read(
00498                 pHDev_param,
00499                 USB_EP1IN_ADDR,
00500                 &dtc_status, 1,
00501                 USB_TIMEOUT_MS
00502                 );
00503         if (usb_err < 0)
00504             return usb_err;
00505 
00506         if (dtc_status & 0x01)
00507             break;
00508 
00509         if (!--i) {
00510             LOG_ERROR("too many retries waiting for DTC status");
00511             return -ETIMEDOUT;
00512         }
00513     }
00514 
00515 
00516     if (reply_buffer && reply_buffer_size) {
00517         usb_err = usb_bulk_read(
00518                 pHDev_param,
00519                 USB_EP2IN_ADDR,
00520                 (char *)reply_buffer, reply_buffer_size,
00521                 USB_TIMEOUT_MS
00522                 );
00523 
00524         if (usb_err < reply_buffer_size) {
00525             LOG_ERROR("Read of endpoint 2 returned %d, expected %d",
00526                 usb_err, reply_buffer_size
00527                 );
00528             return usb_err;
00529         }
00530     }
00531 
00532     return usb_err;
00533 }
00534 
00535 /*
00536  * The dtc reply queue is a singly linked list that describes what to do
00537  * with the reply packet that comes from the DTC.  Only SCAN_IN and SCAN_IO generate
00538  * these entries.
00539  */
00540 
00541 struct dtc_reply_queue_entry {
00542     struct dtc_reply_queue_entry *next;
00543     struct jtag_command *cmd;   /* the command that resulted in this entry */
00544 
00545     struct {
00546         uint8_t *buffer;        /* the scan buffer */
00547         int size;           /* size of the scan buffer in bits */
00548         int offset;         /* how many bits were already done before this? */
00549         int length;         /* how many bits are processed in this operation? */
00550         enum scan_type type;        /* SCAN_IN/SCAN_OUT/SCAN_IO */
00551     } scan;
00552 };
00553 
00554 
00555 /*
00556  * The dtc_queue consists of a buffer of pending commands and a reply queue.
00557  * rlink_scan and tap_state_run add to the command buffer and maybe to the reply queue.
00558  */
00559 
00560 static struct {
00561     struct dtc_reply_queue_entry *rq_head;
00562     struct dtc_reply_queue_entry *rq_tail;
00563     uint32_t cmd_index;
00564     uint32_t reply_index;
00565     uint8_t cmd_buffer[USB_EP2BANK_SIZE];
00566 } dtc_queue;
00567 
00568 /*
00569  * The tap state queue is for accumulating TAP state changes wiithout needlessly
00570  * flushing the dtc_queue.  When it fills or is run, it adds the accumulated bytes to
00571  * the dtc_queue.
00572  */
00573 
00574 static struct {
00575     uint32_t length;
00576     uint32_t buffer;
00577 } tap_state_queue;
00578 
00579 static int dtc_queue_init(void)
00580 {
00581     dtc_queue.rq_head = NULL;
00582     dtc_queue.rq_tail = NULL;
00583     dtc_queue.cmd_index = 0;
00584     dtc_queue.reply_index = 0;
00585     return 0;
00586 }
00587 
00588 static inline struct dtc_reply_queue_entry *dtc_queue_enqueue_reply(
00589     enum scan_type type, uint8_t *buffer, int size, int offset,
00590     int length, struct jtag_command *cmd)
00591 {
00592     struct dtc_reply_queue_entry *rq_entry;
00593 
00594     rq_entry = malloc(sizeof(struct dtc_reply_queue_entry));
00595     if (rq_entry != NULL) {
00596         rq_entry->scan.type = type;
00597         rq_entry->scan.buffer = buffer;
00598         rq_entry->scan.size = size;
00599         rq_entry->scan.offset = offset;
00600         rq_entry->scan.length = length;
00601         rq_entry->cmd = cmd;
00602         rq_entry->next = NULL;
00603 
00604         if (dtc_queue.rq_head == NULL)
00605             dtc_queue.rq_head = rq_entry;
00606         else
00607             dtc_queue.rq_tail->next = rq_entry;
00608 
00609         dtc_queue.rq_tail = rq_entry;
00610     }
00611 
00612     return rq_entry;
00613 }
00614 
00615 /*
00616  * Running the queue means that any pending command buffer is run
00617  * and any reply data dealt with.  The command buffer is then cleared for subsequent processing.
00618  * The queue is automatically run by append when it is necessary to get space for the append.
00619  */
00620 
00621 static int dtc_queue_run(void)
00622 {
00623     struct dtc_reply_queue_entry *rq_p, *rq_next;
00624     int retval;
00625     int usb_err;
00626     int bit_cnt;
00627     int x;
00628     uint8_t *dtc_p, *tdo_p;
00629     uint8_t dtc_mask, tdo_mask;
00630     uint8_t reply_buffer[USB_EP2IN_SIZE];
00631 
00632     assert((dtc_queue.rq_head != 0) == (dtc_queue.reply_index > 0));
00633     assert(dtc_queue.cmd_index < USB_EP2BANK_SIZE);
00634     assert(dtc_queue.reply_index <= USB_EP2IN_SIZE);
00635 
00636     retval = ERROR_OK;
00637 
00638     if (dtc_queue.cmd_index < 1)
00639         return retval;
00640 
00641     dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = DTC_CMD_STOP;
00642 
00643     usb_err = dtc_run_download(pHDev,
00644             dtc_queue.cmd_buffer, dtc_queue.cmd_index,
00645             reply_buffer, sizeof(reply_buffer)
00646             );
00647     if (usb_err < 0) {
00648         LOG_ERROR("dtc_run_download: %s", usb_strerror());
00649         exit(1);
00650     }
00651 
00652     if (dtc_queue.rq_head != NULL) {
00653         /* process the reply, which empties the reply queue and frees its entries */
00654         dtc_p = reply_buffer;
00655 
00656         /* The rigamarole with the masks and doing it bit-by-bit is due to the fact that the
00657          *scan buffer is LSb-first and the DTC code is MSb-first for hardware reasons.   It
00658          *was that or craft a function to do the reversal, and that wouldn't work with
00659          *bit-stuffing (supplying extra bits to use mostly byte operations), or any other
00660          *scheme which would throw the byte alignment off. */
00661 
00662         for (
00663             rq_p = dtc_queue.rq_head;
00664             rq_p != NULL;
00665             rq_p = rq_next
00666             ) {
00667             tdo_p = rq_p->scan.buffer + (rq_p->scan.offset / 8);
00668             tdo_mask = 1 << (rq_p->scan.offset % 8);
00669 
00670 
00671             bit_cnt = rq_p->scan.length;
00672             if (bit_cnt >= 8) {
00673                 /* bytes */
00674 
00675                 dtc_mask = 1 << (8 - 1);
00676 
00677                 for (
00678                     ;
00679                     bit_cnt;
00680                     bit_cnt--
00681                     ) {
00682                     if (*dtc_p & dtc_mask)
00683                         *tdo_p |= tdo_mask;
00684                     else
00685                         *tdo_p &= ~tdo_mask;
00686 
00687                     dtc_mask >>= 1;
00688                     if (dtc_mask == 0) {
00689                         dtc_p++;
00690                         dtc_mask = 1 << (8 - 1);
00691                     }
00692 
00693                     tdo_mask <<= 1;
00694                     if (tdo_mask == 0) {
00695                         tdo_p++;
00696                         tdo_mask = 1;
00697                     }
00698                 }
00699             } else {
00700                 /*  extra bits or last bit */
00701 
00702                 x = *dtc_p++;
00703                 if ((rq_p->scan.type == SCAN_IN) && (
00704                         rq_p->scan.offset != rq_p->scan.size - 1
00705                     )) {
00706                     /* extra bits were sent as a full byte with padding on the
00707                      *end */
00708                     dtc_mask = 1 << (8 - 1);
00709                 } else
00710                     dtc_mask = 1 << (bit_cnt - 1);
00711 
00712                 for (
00713                     ;
00714                     bit_cnt;
00715                     bit_cnt--
00716                     ) {
00717                     if (x & dtc_mask)
00718                         *tdo_p |= tdo_mask;
00719                     else
00720                         *tdo_p &= ~tdo_mask;
00721 
00722                     dtc_mask >>= 1;
00723 
00724                     tdo_mask <<= 1;
00725                     if (tdo_mask == 0) {
00726                         tdo_p++;
00727                         tdo_mask = 1;
00728                     }
00729 
00730                 }
00731             }
00732 
00733             if ((rq_p->scan.offset + rq_p->scan.length) >= rq_p->scan.size) {
00734                 /* feed scan buffer back into openocd and free it */
00735                 if (jtag_read_buffer(rq_p->scan.buffer,
00736                         rq_p->cmd->cmd.scan) != ERROR_OK)
00737                     retval = ERROR_JTAG_QUEUE_FAILED;
00738                 free(rq_p->scan.buffer);
00739             }
00740 
00741             rq_next = rq_p->next;
00742             free(rq_p);
00743         }
00744         dtc_queue.rq_head = NULL;
00745         dtc_queue.rq_tail = NULL;
00746     }
00747 
00748     /* reset state for new appends */
00749     dtc_queue.cmd_index = 0;
00750     dtc_queue.reply_index = 0;
00751 
00752     return retval;
00753 }
00754 
00755 /* runs the queue if it cannot take reserved_cmd bytes of command data
00756  * or reserved_reply bytes of reply data */
00757 static int dtc_queue_run_if_full(int reserved_cmd, int reserved_reply)
00758 {
00759     /* reserve one additional byte for the STOP cmd appended during run */
00760     if (dtc_queue.cmd_index + reserved_cmd + 1 > USB_EP2BANK_SIZE)
00761         return dtc_queue_run();
00762 
00763     if (dtc_queue.reply_index + reserved_reply > USB_EP2IN_SIZE)
00764         return dtc_queue_run();
00765 
00766     return ERROR_OK;
00767 }
00768 
00769 static int tap_state_queue_init(void)
00770 {
00771     tap_state_queue.length = 0;
00772     tap_state_queue.buffer = 0;
00773     return 0;
00774 }
00775 
00776 static int tap_state_queue_run(void)
00777 {
00778     int i;
00779     int bits;
00780     uint8_t byte_param;
00781     int retval;
00782 
00783     retval = 0;
00784     if (!tap_state_queue.length)
00785         return retval;
00786     bits = 1;
00787     byte_param = 0;
00788     for (i = tap_state_queue.length; i--; ) {
00789 
00790         byte_param <<= 1;
00791         if (tap_state_queue.buffer & 1)
00792             byte_param |= 1;
00793         if ((bits >= 8) || !i) {
00794             byte_param <<= (8 - bits);
00795 
00796             /* make sure there's room for two cmd bytes */
00797             dtc_queue_run_if_full(2, 0);
00798 
00799 #ifdef USE_HARDWARE_SHIFTER_FOR_TMS
00800             if (bits == 8) {
00801                 dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
00802                     DTC_CMD_SHIFT_TMS_BYTES(1);
00803             } else {
00804 #endif
00805             dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
00806                 DTC_CMD_SHIFT_TMS_BITS(bits);
00807 #ifdef USE_HARDWARE_SHIFTER_FOR_TMS
00808         }
00809 #endif
00810 
00811             dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
00812                 byte_param;
00813 
00814             byte_param = 0;
00815             bits = 1;
00816         } else
00817             bits++;
00818 
00819         tap_state_queue.buffer >>= 1;
00820     }
00821     retval = tap_state_queue_init();
00822     return retval;
00823 }
00824 
00825 static int tap_state_queue_append(uint8_t tms)
00826 {
00827     int retval;
00828 
00829     if (tap_state_queue.length >= sizeof(tap_state_queue.buffer) * 8) {
00830         retval = tap_state_queue_run();
00831         if (retval != 0)
00832             return retval;
00833     }
00834 
00835     if (tms)
00836         tap_state_queue.buffer |= (1 << tap_state_queue.length);
00837     tap_state_queue.length++;
00838 
00839     return 0;
00840 }
00841 
00842 static void rlink_end_state(tap_state_t state)
00843 {
00844     if (tap_is_state_stable(state))
00845         tap_set_end_state(state);
00846     else {
00847         LOG_ERROR("BUG: %i is not a valid end state", state);
00848         exit(-1);
00849     }
00850 }
00851 
00852 static void rlink_state_move(void)
00853 {
00854 
00855     int i = 0, tms = 0;
00856     uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
00857     int tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
00858 
00859     for (i = 0; i < tms_count; i++) {
00860         tms = (tms_scan >> i) & 1;
00861         tap_state_queue_append(tms);
00862     }
00863 
00864     tap_set_state(tap_get_end_state());
00865 }
00866 
00867 static void rlink_path_move(struct pathmove_command *cmd)
00868 {
00869     int num_states = cmd->num_states;
00870     int state_count;
00871     int tms = 0;
00872 
00873     state_count = 0;
00874     while (num_states) {
00875         if (tap_state_transition(tap_get_state(), false) == cmd->path[state_count])
00876             tms = 0;
00877         else if (tap_state_transition(tap_get_state(), true) == cmd->path[state_count])
00878             tms = 1;
00879         else {
00880             LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition",
00881                 tap_state_name(tap_get_state()),
00882                 tap_state_name(cmd->path[state_count]));
00883             exit(-1);
00884         }
00885 
00886         tap_state_queue_append(tms);
00887 
00888         tap_set_state(cmd->path[state_count]);
00889         state_count++;
00890         num_states--;
00891     }
00892 
00893     tap_set_end_state(tap_get_state());
00894 }
00895 
00896 static void rlink_runtest(int num_cycles)
00897 {
00898     int i;
00899 
00900     tap_state_t saved_end_state = tap_get_end_state();
00901 
00902     /* only do a state_move when we're not already in RTI */
00903     if (tap_get_state() != TAP_IDLE) {
00904         rlink_end_state(TAP_IDLE);
00905         rlink_state_move();
00906     }
00907 
00908     /* execute num_cycles */
00909     for (i = 0; i < num_cycles; i++)
00910         tap_state_queue_append(0);
00911 
00912     /* finish in end_state */
00913     rlink_end_state(saved_end_state);
00914     if (tap_get_state() != tap_get_end_state())
00915         rlink_state_move();
00916 }
00917 
00918 /* (1) assert or (0) deassert reset lines */
00919 static void rlink_reset(int trst, int srst)
00920 {
00921     uint8_t bitmap;
00922     int usb_err;
00923 
00924     /* Read port A for bit op */
00925     usb_err = ep1_generic_commandl(
00926             pHDev, 4,
00927             EP1_CMD_MEMORY_READ,
00928             ST7_PADR >> 8,
00929             ST7_PADR,
00930             1
00931             );
00932     if (usb_err < 0) {
00933         LOG_ERROR("%s", usb_strerror());
00934         exit(1);
00935     }
00936 
00937     usb_err = usb_bulk_read(
00938             pHDev, USB_EP1IN_ADDR,
00939             (char *)&bitmap, 1,
00940             USB_TIMEOUT_MS
00941             );
00942     if (usb_err < 1) {
00943         LOG_ERROR("%s", usb_strerror());
00944         exit(1);
00945     }
00946 
00947     if (trst)
00948         bitmap &= ~ST7_PA_NTRST;
00949     else
00950         bitmap |= ST7_PA_NTRST;
00951 
00952     /* Write port A and read port B for bit op
00953      * port B has no OR, and we want to emulate open drain on NSRST, so we initialize DR to 0
00954      *and assert NSRST by setting DDR to 1. */
00955     usb_err = ep1_generic_commandl(
00956             pHDev, 9,
00957             EP1_CMD_MEMORY_WRITE,
00958             ST7_PADR >> 8,
00959             ST7_PADR,
00960             1,
00961             bitmap,
00962             EP1_CMD_MEMORY_READ,
00963             ST7_PBDDR >> 8,
00964             ST7_PBDDR,
00965             1
00966             );
00967     if (usb_err < 0) {
00968         LOG_ERROR("%s", usb_strerror());
00969         exit(1);
00970     }
00971 
00972     usb_err = usb_bulk_read(
00973             pHDev, USB_EP1IN_ADDR,
00974             (char *)&bitmap, 1,
00975             USB_TIMEOUT_MS
00976             );
00977     if (usb_err < 1) {
00978         LOG_ERROR("%s", usb_strerror());
00979         exit(1);
00980     }
00981 
00982     if (srst)
00983         bitmap |= ST7_PB_NSRST;
00984     else
00985         bitmap &= ~ST7_PB_NSRST;
00986 
00987     /* write port B and read dummy to ensure completion before returning */
00988     usb_err = ep1_generic_commandl(
00989             pHDev, 6,
00990             EP1_CMD_MEMORY_WRITE,
00991             ST7_PBDDR >> 8,
00992             ST7_PBDDR,
00993             1,
00994             bitmap,
00995             EP1_CMD_DTC_GET_CACHED_STATUS
00996             );
00997     if (usb_err < 0) {
00998         LOG_ERROR("%s", usb_strerror());
00999         exit(1);
01000     }
01001 
01002     usb_err = usb_bulk_read(
01003             pHDev, USB_EP1IN_ADDR,
01004             (char *)&bitmap, 1,
01005             USB_TIMEOUT_MS
01006             );
01007     if (usb_err < 1) {
01008         LOG_ERROR("%s", usb_strerror());
01009         exit(1);
01010     }
01011 }
01012 
01013 static int rlink_scan(struct jtag_command *cmd, enum scan_type type,
01014         uint8_t *buffer, int scan_size)
01015 {
01016     bool ir_scan;
01017     tap_state_t saved_end_state;
01018     int byte_bits;
01019     int extra_bits;
01020     int chunk_bits;
01021     int chunk_bytes;
01022     int x;
01023 
01024     int tdi_bit_offset;
01025     uint8_t tdi_mask, *tdi_p;
01026     uint8_t dtc_mask;
01027 
01028     if (scan_size < 1) {
01029         LOG_ERROR("scan_size cannot be less than 1 bit");
01030         exit(1);
01031     }
01032 
01033     ir_scan = cmd->cmd.scan->ir_scan;
01034 
01035     /* Move to the proper state before starting to shift TDI/TDO. */
01036     if (!((!ir_scan && (tap_get_state() == TAP_DRSHIFT)) ||
01037             (ir_scan && (tap_get_state() == TAP_IRSHIFT)))) {
01038         saved_end_state = tap_get_end_state();
01039         rlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
01040         rlink_state_move();
01041         rlink_end_state(saved_end_state);
01042     }
01043 
01044     tap_state_queue_run();
01045 
01046 
01047 #if 0
01048     printf("scan_size = %d, type = 0x%x\n", scan_size, type);
01049     {
01050         int i;
01051 
01052         /* clear unused bits in scan buffer for ease of debugging
01053          * (it makes diffing output easier) */
01054         buffer[scan_size / 8] &= ((1 << ((scan_size - 1) % 8) + 1) - 1);
01055 
01056         printf("before scan:");
01057         for (i = 0; i < (scan_size + 7) / 8; i++)
01058             printf(" %02x", buffer[i]);
01059         printf("\n");
01060     }
01061 #endif
01062 
01063     /* The number of bits that can be shifted as complete bytes */
01064     byte_bits = (int)(scan_size - 1) / 8 * 8;
01065     /* The number of bits left over, not counting the last bit */
01066     extra_bits = (scan_size - 1) - byte_bits;
01067 
01068     tdi_bit_offset = 0;
01069     tdi_p = buffer;
01070     tdi_mask = 1;
01071 
01072     if (extra_bits && (type == SCAN_OUT)) {
01073         /* Schedule any extra bits into the DTC command buffer, padding as needed
01074          * For SCAN_OUT, this comes before the full bytes so the (leading) padding bits will
01075          *fall off the end */
01076 
01077         /* make sure there's room for two cmd bytes */
01078         dtc_queue_run_if_full(2, 0);
01079 
01080         x = 0;
01081         dtc_mask = 1 << (extra_bits - 1);
01082 
01083         while (extra_bits--) {
01084             if (*tdi_p & tdi_mask)
01085                 x |= dtc_mask;
01086 
01087             dtc_mask >>= 1;
01088 
01089             tdi_mask <<= 1;
01090             if (tdi_mask == 0) {
01091                 tdi_p++;
01092                 tdi_mask = 1;
01093             }
01094         }
01095 
01096         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
01097             DTC_CMD_SHIFT_TDI_BYTES(1);
01098 
01099         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
01100     }
01101 
01102     /* Loop scheduling full bytes into the DTC command buffer */
01103     while (byte_bits) {
01104         /* make sure there's room for one (for in scans) or two cmd bytes and
01105          * at least one reply byte for in or inout scans*/
01106         dtc_queue_run_if_full(type == SCAN_IN ? 1 : 2, type != SCAN_OUT ? 1 : 0);
01107 
01108         chunk_bits = byte_bits;
01109         /* we can only use up to 16 bytes at a time */
01110         if (chunk_bits > (16 * 8))
01111             chunk_bits = (16 * 8);
01112 
01113         if (type != SCAN_IN) {
01114             /* how much is there room for, considering stop and byte op? */
01115             x = (sizeof(dtc_queue.cmd_buffer) - (dtc_queue.cmd_index + 1 + 1)) * 8;
01116             if (chunk_bits > x)
01117                 chunk_bits = x;
01118         }
01119 
01120         if (type != SCAN_OUT) {
01121             /* how much is there room for in the reply buffer? */
01122             x = (USB_EP2IN_SIZE - dtc_queue.reply_index) * 8;
01123             if (chunk_bits > x)
01124                 chunk_bits = x;
01125         }
01126 
01127         /* so the loop will end */
01128         byte_bits -= chunk_bits;
01129 
01130         if (type != SCAN_OUT) {
01131             if (dtc_queue_enqueue_reply(
01132                     type, buffer, scan_size, tdi_bit_offset,
01133                     chunk_bits,
01134                     cmd
01135                 ) == NULL) {
01136                 LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
01137                 exit(1);
01138             }
01139             dtc_queue.reply_index += (chunk_bits + 7) / 8;
01140 
01141             tdi_bit_offset += chunk_bits;
01142         }
01143 
01144         /* chunk_bits is a multiple of 8, so there are no rounding issues. */
01145         chunk_bytes = chunk_bits / 8;
01146 
01147         switch (type) {
01148             case SCAN_IN:
01149                 x = DTC_CMD_SHIFT_TDO_BYTES(chunk_bytes);
01150                 break;
01151             case SCAN_OUT:
01152                 x = DTC_CMD_SHIFT_TDI_BYTES(chunk_bytes);
01153                 break;
01154             default:
01155                 x = DTC_CMD_SHIFT_TDIO_BYTES(chunk_bytes);
01156                 break;
01157         }
01158         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
01159 
01160         if (type != SCAN_IN) {
01161             x = 0;
01162             dtc_mask = 1 << (8 - 1);
01163 
01164             while (chunk_bits--) {
01165                 if (*tdi_p & tdi_mask)
01166                     x |= dtc_mask;
01167 
01168                 dtc_mask >>= 1;
01169                 if (dtc_mask == 0) {
01170                     dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
01171                     x = 0;
01172                     dtc_mask = 1 << (8 - 1);
01173                 }
01174 
01175                 tdi_mask <<= 1;
01176                 if (tdi_mask == 0) {
01177                     tdi_p++;
01178                     tdi_mask = 1;
01179                 }
01180             }
01181         }
01182     }
01183 
01184     if (extra_bits && (type != SCAN_OUT)) {
01185         /* Schedule any extra bits into the DTC command buffer */
01186 
01187         /* make sure there's room for one (for in scans) or two cmd bytes
01188          * and one reply byte */
01189         dtc_queue_run_if_full(type == SCAN_IN ? 1 : 2, 1);
01190 
01191         if (dtc_queue_enqueue_reply(
01192                 type, buffer, scan_size, tdi_bit_offset,
01193                 extra_bits,
01194                 cmd
01195             ) == NULL) {
01196             LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
01197             exit(1);
01198         }
01199 
01200         dtc_queue.reply_index++;
01201 
01202         tdi_bit_offset += extra_bits;
01203 
01204         if (type == SCAN_IN) {
01205             dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
01206                 DTC_CMD_SHIFT_TDO_BYTES(1);
01207 
01208         } else {
01209             dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
01210                 DTC_CMD_SHIFT_TDIO_BITS(extra_bits);
01211 
01212             x = 0;
01213             dtc_mask = 1 << (8 - 1);
01214 
01215             while (extra_bits--) {
01216                 if (*tdi_p & tdi_mask)
01217                     x |= dtc_mask;
01218 
01219                 dtc_mask >>= 1;
01220 
01221                 tdi_mask <<= 1;
01222                 if (tdi_mask == 0) {
01223                     tdi_p++;
01224                     tdi_mask = 1;
01225                 }
01226             }
01227 
01228             dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
01229         }
01230     }
01231 
01232     /* Schedule the last bit into the DTC command buffer */
01233 
01234     /* make sure there's room for one cmd byte and one reply byte
01235      * for in or inout scans*/
01236     dtc_queue_run_if_full(1, type == SCAN_OUT ? 0 : 1);
01237 
01238     if (type == SCAN_OUT) {
01239         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
01240             DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 0);
01241 
01242     } else {
01243         if (dtc_queue_enqueue_reply(
01244                 type, buffer, scan_size, tdi_bit_offset,
01245                 1,
01246                 cmd
01247                 ) == NULL) {
01248             LOG_ERROR("enqueuing DTC reply entry: %s", strerror(errno));
01249             exit(1);
01250         }
01251 
01252         dtc_queue.reply_index++;
01253 
01254         dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
01255             DTC_CMD_SHIFT_TMS_TDI_BIT_PAIR(1, (*tdi_p & tdi_mask), 1);
01256     }
01257 
01258     /* Move to pause state */
01259     tap_state_queue_append(0);
01260     tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
01261     if (tap_get_state() != tap_get_end_state())
01262         rlink_state_move();
01263 
01264     return 0;
01265 }
01266 
01267 static int rlink_execute_queue(void)
01268 {
01269     struct jtag_command *cmd = jtag_command_queue;  /* currently processed command */
01270     int scan_size;
01271     enum scan_type type;
01272     uint8_t *buffer;
01273     int retval, tmp_retval;
01274 
01275     /* return ERROR_OK, unless something goes wrong */
01276     retval = ERROR_OK;
01277 
01278 #ifndef AUTOMATIC_BUSY_LED
01279     /* turn LED on */
01280     ep1_generic_commandl(pHDev, 2,
01281         EP1_CMD_SET_PORTD_LEDS,
01282         ~(ST7_PD_NBUSY_LED)
01283         );
01284 #endif
01285 
01286     while (cmd) {
01287         switch (cmd->type) {
01288             case JTAG_RUNTEST:
01289             case JTAG_TLR_RESET:
01290             case JTAG_PATHMOVE:
01291             case JTAG_SCAN:
01292                 break;
01293 
01294             default:
01295                 /* some events, such as resets, need a queue flush to ensure
01296                  *consistency */
01297                 tap_state_queue_run();
01298                 dtc_queue_run();
01299                 break;
01300         }
01301 
01302         switch (cmd->type) {
01303             case JTAG_RESET:
01304 #ifdef _DEBUG_JTAG_IO_
01305                 LOG_DEBUG("reset trst: %i srst %i",
01306                         cmd->cmd.reset->trst,
01307                         cmd->cmd.reset->srst);
01308 #endif
01309                 if ((cmd->cmd.reset->trst == 1) ||
01310                         (cmd->cmd.reset->srst &&
01311                                 (jtag_get_reset_config() & RESET_SRST_PULLS_TRST)))
01312                     tap_set_state(TAP_RESET);
01313                 rlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
01314                 break;
01315             case JTAG_RUNTEST:
01316 #ifdef _DEBUG_JTAG_IO_
01317                 LOG_DEBUG("runtest %i cycles, end in %i",
01318                         cmd->cmd.runtest->num_cycles,
01319                         cmd->cmd.runtest->end_state);
01320 #endif
01321                 if (cmd->cmd.runtest->end_state != -1)
01322                     rlink_end_state(cmd->cmd.runtest->end_state);
01323                 rlink_runtest(cmd->cmd.runtest->num_cycles);
01324                 break;
01325             case JTAG_TLR_RESET:
01326 #ifdef _DEBUG_JTAG_IO_
01327                 LOG_DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
01328 #endif
01329                 if (cmd->cmd.statemove->end_state != -1)
01330                     rlink_end_state(cmd->cmd.statemove->end_state);
01331                 rlink_state_move();
01332                 break;
01333             case JTAG_PATHMOVE:
01334 #ifdef _DEBUG_JTAG_IO_
01335                 LOG_DEBUG("pathmove: %i states, end in %i",
01336                 cmd->cmd.pathmove->num_states,
01337                 cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
01338 #endif
01339                 rlink_path_move(cmd->cmd.pathmove);
01340                 break;
01341             case JTAG_SCAN:
01342 #ifdef _DEBUG_JTAG_IO_
01343                 LOG_DEBUG("%s scan end in %i",
01344                 (cmd->cmd.scan->ir_scan) ? "IR" : "DR",
01345                 cmd->cmd.scan->end_state);
01346 #endif
01347                 if (cmd->cmd.scan->end_state != -1)
01348                     rlink_end_state(cmd->cmd.scan->end_state);
01349                 scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
01350                 type = jtag_scan_type(cmd->cmd.scan);
01351                 if (rlink_scan(cmd, type, buffer, scan_size) != ERROR_OK)
01352                     retval = ERROR_FAIL;
01353                 break;
01354             case JTAG_SLEEP:
01355 #ifdef _DEBUG_JTAG_IO_
01356                 LOG_DEBUG("sleep %i", cmd->cmd.sleep->us);
01357 #endif
01358                 jtag_sleep(cmd->cmd.sleep->us);
01359                 break;
01360             default:
01361                 LOG_ERROR("BUG: unknown JTAG command type encountered");
01362                 exit(-1);
01363         }
01364         cmd = cmd->next;
01365     }
01366 
01367     /* Flush the DTC queue to make sure any pending reads have been done before exiting this
01368      *function */
01369     tap_state_queue_run();
01370     tmp_retval = dtc_queue_run();
01371     if (tmp_retval != ERROR_OK)
01372         retval = tmp_retval;
01373 
01374 #ifndef AUTOMATIC_BUSY_LED
01375     /* turn LED onff */
01376     ep1_generic_commandl(pHDev, 2,
01377         EP1_CMD_SET_PORTD_LEDS,
01378         ~0
01379         );
01380 #endif
01381 
01382     return retval;
01383 }
01384 
01385 /* Using an unindexed table because it is infrequently accessed and it is short.  The table must be
01386  *in order of ascending speed (and descending prescaler), as it is scanned in reverse. */
01387 
01388 static int rlink_speed(int speed)
01389 {
01390     int i;
01391 
01392     if (speed == 0) {
01393         /* fastest speed */
01394         speed = rlink_speed_table[rlink_speed_table_size - 1].prescaler;
01395     }
01396 
01397     for (i = rlink_speed_table_size; i--; ) {
01398         if (rlink_speed_table[i].prescaler == speed) {
01399             if (dtc_load_from_buffer(pHDev, rlink_speed_table[i].dtc,
01400                     rlink_speed_table[i].dtc_size) != 0) {
01401                 LOG_ERROR(
01402                     "An error occurred while trying to load DTC code for speed \"%d\".",
01403                     speed);
01404                 exit(1);
01405             }
01406 
01407             if (dtc_start_download() < 0) {
01408                 LOG_ERROR("starting DTC: %s", usb_strerror());
01409                 exit(1);
01410             }
01411 
01412             return ERROR_OK;
01413         }
01414     }
01415 
01416     LOG_ERROR("%d is not a supported speed", speed);
01417     return ERROR_FAIL;
01418 }
01419 
01420 static int rlink_speed_div(int speed, int *khz)
01421 {
01422     int i;
01423 
01424     for (i = rlink_speed_table_size; i--; ) {
01425         if (rlink_speed_table[i].prescaler == speed) {
01426             *khz = rlink_speed_table[i].khz;
01427             return ERROR_OK;
01428         }
01429     }
01430 
01431     LOG_ERROR("%d is not a supported speed", speed);
01432     return ERROR_FAIL;
01433 }
01434 
01435 static int rlink_khz(int khz, int *speed)
01436 {
01437     int i;
01438 
01439     if (khz == 0) {
01440         LOG_ERROR("RCLK not supported");
01441         return ERROR_FAIL;
01442     }
01443 
01444     for (i = rlink_speed_table_size; i--; ) {
01445         if (rlink_speed_table[i].khz <= khz) {
01446             *speed = rlink_speed_table[i].prescaler;
01447             return ERROR_OK;
01448         }
01449     }
01450 
01451     LOG_WARNING("The lowest supported JTAG speed is %d KHz", rlink_speed_table[0].khz);
01452     *speed = rlink_speed_table[0].prescaler;
01453     return ERROR_OK;
01454 }
01455 
01456 static int rlink_init(void)
01457 {
01458     int i, j, retries;
01459     uint8_t reply_buffer[USB_EP1IN_SIZE];
01460 
01461     usb_init();
01462     const uint16_t vids[] = { USB_IDVENDOR, 0 };
01463     const uint16_t pids[] = { USB_IDPRODUCT, 0 };
01464     if (jtag_usb_open(vids, pids, &pHDev) != ERROR_OK)
01465         return ERROR_FAIL;
01466 
01467     struct usb_device *dev = usb_device(pHDev);
01468     if (dev->descriptor.bNumConfigurations > 1) {
01469         LOG_ERROR("Whoops! NumConfigurations is not 1, don't know what to do...");
01470         return ERROR_FAIL;
01471     }
01472     if (dev->config->bNumInterfaces > 1) {
01473         LOG_ERROR("Whoops! NumInterfaces is not 1, don't know what to do...");
01474         return ERROR_FAIL;
01475     }
01476 
01477     LOG_DEBUG("Opened device, pHDev = %p", pHDev);
01478 
01479     /* usb_set_configuration required under win32 */
01480     usb_set_configuration(pHDev, dev->config[0].bConfigurationValue);
01481 
01482     retries = 3;
01483     do {
01484         i = usb_claim_interface(pHDev, 0);
01485         if (i) {
01486             LOG_ERROR("usb_claim_interface: %s", usb_strerror());
01487 #ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
01488             j = usb_detach_kernel_driver_np(pHDev, 0);
01489             if (j)
01490                 LOG_ERROR("detach kernel driver: %s", usb_strerror());
01491 #endif
01492         } else {
01493             LOG_DEBUG("interface claimed!");
01494             break;
01495         }
01496     } while (--retries);
01497 
01498     if (i) {
01499         LOG_ERROR("Initialisation failed.");
01500         return ERROR_FAIL;
01501     }
01502     if (usb_set_altinterface(pHDev, 0) != 0) {
01503         LOG_ERROR("Failed to set interface.");
01504         return ERROR_FAIL;
01505     }
01506 
01507     /* The device starts out in an unknown state on open.  As such,
01508      * result reads time out, and it's not even known whether the
01509      * command was accepted.  So, for this first command, we issue
01510      * it repeatedly until its response doesn't time out.  Also, if
01511      * sending a command is going to time out, we find that out here.
01512      *
01513      * It must be possible to open the device in such a way that
01514      * this special magic isn't needed, but, so far, it escapes us.
01515      */
01516     for (i = 0; i < 5; i++) {
01517         j = ep1_generic_commandl(
01518                 pHDev, 1,
01519                 EP1_CMD_GET_FWREV
01520                 );
01521         if (j < USB_EP1OUT_SIZE) {
01522             LOG_ERROR("USB write error: %s", usb_strerror());
01523             return ERROR_FAIL;
01524         }
01525         j = usb_bulk_read(
01526                 pHDev, USB_EP1IN_ADDR,
01527                 (char *)reply_buffer, sizeof(reply_buffer),
01528                 200
01529                 );
01530         if (j != -ETIMEDOUT)
01531             break;
01532     }
01533 
01534     if (j < (int)sizeof(reply_buffer)) {
01535         LOG_ERROR("USB read error: %s", usb_strerror());
01536         return ERROR_FAIL;
01537     }
01538     LOG_DEBUG(INTERFACE_NAME " firmware version: %d.%d.%d",
01539         reply_buffer[0],
01540         reply_buffer[1],
01541         reply_buffer[2]);
01542 
01543     if ((reply_buffer[0] != 0) || (reply_buffer[1] != 0) || (reply_buffer[2] != 3))
01544         LOG_WARNING(
01545             "The rlink device is not of the version that the developers have played with.  It may or may not work.");
01546 
01547     /* Probe port E for adapter presence */
01548     ep1_generic_commandl(
01549         pHDev, 16,
01550         EP1_CMD_MEMORY_WRITE,       /* Drive sense pin with 0 */
01551         ST7_PEDR >> 8,
01552         ST7_PEDR,
01553         3,
01554         0x00,                           /* DR */
01555         ST7_PE_ADAPTER_SENSE_OUT,       /* DDR */
01556         ST7_PE_ADAPTER_SENSE_OUT,       /* OR */
01557         EP1_CMD_MEMORY_READ,        /* Read back */
01558         ST7_PEDR >> 8,
01559         ST7_PEDR,
01560         1,
01561         EP1_CMD_MEMORY_WRITE,       /* Drive sense pin with 1 */
01562         ST7_PEDR >> 8,
01563         ST7_PEDR,
01564         1,
01565         ST7_PE_ADAPTER_SENSE_OUT
01566         );
01567 
01568     usb_bulk_read(
01569         pHDev, USB_EP1IN_ADDR,
01570         (char *)reply_buffer, 1,
01571         USB_TIMEOUT_MS
01572         );
01573 
01574     if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) != 0)
01575         LOG_WARNING("target detection problem");
01576 
01577     ep1_generic_commandl(
01578         pHDev, 11,
01579         EP1_CMD_MEMORY_READ,        /* Read back */
01580         ST7_PEDR >> 8,
01581         ST7_PEDR,
01582         1,
01583         EP1_CMD_MEMORY_WRITE,       /* float port E */
01584         ST7_PEDR >> 8,
01585         ST7_PEDR,
01586         3,
01587         0x00,       /* DR */
01588         0x00,       /* DDR */
01589         0x00        /* OR */
01590         );
01591 
01592     usb_bulk_read(
01593         pHDev, USB_EP1IN_ADDR,
01594         (char *)reply_buffer, 1,
01595         USB_TIMEOUT_MS
01596         );
01597 
01598 
01599     if ((reply_buffer[0] & ST7_PE_ADAPTER_SENSE_IN) == 0)
01600         LOG_WARNING("target not plugged in");
01601 
01602     /* float ports A and B */
01603     ep1_generic_commandl(
01604         pHDev, 11,
01605         EP1_CMD_MEMORY_WRITE,
01606         ST7_PADDR >> 8,
01607         ST7_PADDR,
01608         2,
01609         0x00,
01610         0x00,
01611         EP1_CMD_MEMORY_WRITE,
01612         ST7_PBDDR >> 8,
01613         ST7_PBDDR,
01614         1,
01615         0x00
01616         );
01617 
01618     /* make sure DTC is stopped, set VPP control, set up ports A and B */
01619     ep1_generic_commandl(
01620         pHDev, 14,
01621         EP1_CMD_DTC_STOP,
01622         EP1_CMD_SET_PORTD_VPP,
01623         ~(ST7_PD_VPP_SHDN),
01624         EP1_CMD_MEMORY_WRITE,
01625         ST7_PADR >> 8,
01626         ST7_PADR,
01627         2,
01628         ((~(0)) & (ST7_PA_NTRST)),
01629         (ST7_PA_NTRST),
01630         /* port B has no OR, and we want to emulate open drain on NSRST, so we set DR to 0
01631          *here and later assert NSRST by setting DDR bit to 1. */
01632         EP1_CMD_MEMORY_WRITE,
01633         ST7_PBDR >> 8,
01634         ST7_PBDR,
01635         1,
01636         0x00
01637         );
01638 
01639     /* set LED updating mode and make sure they're unlit */
01640     ep1_generic_commandl(
01641         pHDev, 3,
01642 #ifdef AUTOMATIC_BUSY_LED
01643         EP1_CMD_LEDUE_BUSY,
01644 #else
01645         EP1_CMD_LEDUE_NONE,
01646 #endif
01647         EP1_CMD_SET_PORTD_LEDS,
01648         ~0
01649         );
01650 
01651     tap_state_queue_init();
01652     dtc_queue_init();
01653     rlink_reset(0, 0);
01654 
01655     return ERROR_OK;
01656 }
01657 
01658 static int rlink_quit(void)
01659 {
01660     /* stop DTC and make sure LEDs are off */
01661     ep1_generic_commandl(
01662         pHDev, 6,
01663         EP1_CMD_DTC_STOP,
01664         EP1_CMD_LEDUE_NONE,
01665         EP1_CMD_SET_PORTD_LEDS,
01666         ~0,
01667         EP1_CMD_SET_PORTD_VPP,
01668         ~0
01669         );
01670 
01671     usb_release_interface(pHDev, 0);
01672     usb_close(pHDev);
01673 
01674     return ERROR_OK;
01675 }
01676 
01677 struct jtag_interface rlink_interface = {
01678     .name = "rlink",
01679     .init = rlink_init,
01680     .quit = rlink_quit,
01681     .speed = rlink_speed,
01682     .speed_div = rlink_speed_div,
01683     .khz = rlink_khz,
01684     .execute_queue = rlink_execute_queue,
01685 };