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

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /*******************************************************************************
   4  *   Driver for OpenJTAG Project (www.openjtag.org)                            *
   5  *   Compatible with libftdi drivers.                                          *
   6  *                                                                             *
   7  *   Cypress CY7C65215 support                                                 *
   8  *   Copyright (C) 2015 Vianney le Clément de Saint-Marcq, Essensium NV        *
   9  *                      <vianney.leclement@essensium.com>                      *
  10  *                                                                             *
  11  *   Copyright (C) 2010 by Ivan Meleca <mileca@gmail.com>                      *
  12  *                                                                             *
  13  *   Copyright (C) 2013 by Ryan Corbin, GlueLogix Inc. <corbin.ryan@gmail.com> *
  14  *   Updated to work with OpenOCD v0.7.0. Fixed libftdi read speed issue.      *
  15  *                                                                             *
  16  *   Based on usb_blaster.c                                                    *
  17  *   Copyright (C) 2009 Catalin Patulea                                        *
  18  *   Copyright (C) 2006 Kolja Waschk                                           *
  19  *                                                                             *
  20  *   And jlink.c                                                               *
  21  *   Copyright (C) 2008 by Spencer Oliver                                      *
  22  *   spen@spen-soft.co.uk                                                      *
  23  ***************************************************************************/
  24
  25 /***************************************************************************
  26  * Version 1.0  Tested on a MCBSTM32 board using a Cortex-M3 (stm32f103x), *
  27  *              GDB and Eclipse under Linux (Ubuntu 10.04)                 *
  28  *                                                                         *
  29  ***************************************************************************/
  30
  31 #ifdef HAVE_CONFIG_H
  32 #include "config.h"
  33 #endif
  34
  35 #include <jtag/interface.h>
  36 #include <jtag/commands.h>
  37 #include "libusb_helper.h"
  38
  39 static enum {
  40         OPENJTAG_VARIANT_STANDARD,
  41         OPENJTAG_VARIANT_CY7C65215,
  42 } openjtag_variant = OPENJTAG_VARIANT_STANDARD;
  43
  44 static const char * const openjtag_variant_names[] = {
  45         "standard",
  46         "cy7c65215",
  47         NULL
  48 };
  49
  50 /*
  51  * OpenJTAG-OpenOCD state conversion
  52  */
  53 typedef enum openjtag_tap_state {
  54         OPENJTAG_TAP_INVALID    = -1,
  55         OPENJTAG_TAP_RESET  = 0,
  56         OPENJTAG_TAP_IDLE   = 1,
  57         OPENJTAG_TAP_SELECT_DR  = 2,
  58         OPENJTAG_TAP_CAPTURE_DR = 3,
  59         OPENJTAG_TAP_SHIFT_DR   = 4,
  60         OPENJTAG_TAP_EXIT1_DR   = 5,
  61         OPENJTAG_TAP_PAUSE_DR   = 6,
  62         OPENJTAG_TAP_EXIT2_DR   = 7,
  63         OPENJTAG_TAP_UPDATE_DR  = 8,
  64         OPENJTAG_TAP_SELECT_IR  = 9,
  65         OPENJTAG_TAP_CAPURE_IR  = 10,
  66         OPENJTAG_TAP_SHIFT_IR   = 11,
  67         OPENJTAG_TAP_EXIT1_IR   = 12,
  68         OPENJTAG_TAP_PAUSE_IR   = 13,
  69         OPENJTAG_TAP_EXIT2_IR   = 14,
  70         OPENJTAG_TAP_UPDATE_IR  = 15,
  71 } openjtag_tap_state_t;
  72
  73 /* OPENJTAG access library includes */
  74 #include "libftdi_helper.h"
  75
  76 /* OpenJTAG vid/pid */
  77 static uint16_t openjtag_vid = 0x0403;
  78 static uint16_t openjtag_pid = 0x6001;
  79
  80 static char *openjtag_device_desc;
  81
  82 static struct ftdi_context ftdic;
  83
  84 #define OPENJTAG_BUFFER_SIZE        504
  85 #define OPENJTAG_MAX_PENDING_RESULTS    256
  86
  87 struct openjtag_scan_result {
  88         uint32_t bits;          /* Length in bits*/
  89         struct scan_command *command;   /* Corresponding scan command */
  90         uint8_t *buffer;
  91 };
  92
  93 /* USB RX/TX buffers */
  94 static int usb_tx_buf_offs;
  95 static uint8_t usb_tx_buf[OPENJTAG_BUFFER_SIZE];
  96 static uint32_t usb_rx_buf_len;
  97 static uint8_t usb_rx_buf[OPENJTAG_BUFFER_SIZE];
  98
  99 /* Pending readings */
 100 static struct openjtag_scan_result openjtag_scan_result_buffer[OPENJTAG_MAX_PENDING_RESULTS];
 101 static int openjtag_scan_result_count;
 102
 103 static struct libusb_device_handle *usbh;
 104
 105 /* CY7C65215 model only */
 106 #define CY7C65215_JTAG_REQUEST  0x40  /* bmRequestType: vendor host-to-device */
 107 #define CY7C65215_JTAG_ENABLE   0xD0  /* bRequest: enable JTAG */
 108 #define CY7C65215_JTAG_DISABLE  0xD1  /* bRequest: disable JTAG */
 109 #define CY7C65215_JTAG_READ     0xD2  /* bRequest: read buffer */
 110 #define CY7C65215_JTAG_WRITE    0xD3  /* bRequest: write buffer */
 111
 112 #define CY7C65215_USB_TIMEOUT   100
 113
 114 static const uint16_t cy7c65215_vids[] = {0x04b4, 0};
 115 static const uint16_t cy7c65215_pids[] = {0x0007, 0};
 116
 117 #define CY7C65215_JTAG_CLASS     0xff
 118 #define CY7C65215_JTAG_SUBCLASS  0x04
 119
 120 static unsigned int ep_in, ep_out;
 121
 122 #ifdef _DEBUG_USB_COMMS_
 123
 124 #define DEBUG_TYPE_READ     0
 125 #define DEBUG_TYPE_WRITE    1
 126 #define DEBUG_TYPE_OCD_READ 2
 127 #define DEBUG_TYPE_BUFFER   3
 128
 129 #define LINE_LEN  16
 130 static void openjtag_debug_buffer(uint8_t *buffer, int length, uint8_t type)
 131 {
 132         char line[128];
 133         char s[4];
 134         int i;
 135         int j;
 136
 137         switch (type) {
 138                 case DEBUG_TYPE_READ:
 139                         sprintf(line, "USB READ %d bytes", length);
 140                         break;
 141                 case DEBUG_TYPE_WRITE:
 142                         sprintf(line, "USB WRITE %d bytes", length);
 143                         break;
 144                 case DEBUG_TYPE_OCD_READ:
 145                         sprintf(line, "TO OpenOCD %d bytes", length);
 146                         break;
 147                 case DEBUG_TYPE_BUFFER:
 148                         sprintf(line, "Buffer %d bytes", length);
 149                         break;
 150         }
 151
 152         LOG_DEBUG("%s", line);
 153
 154         for (i = 0; i < length; i += LINE_LEN) {
 155                 switch (type) {
 156                         case DEBUG_TYPE_READ:
 157                                 sprintf(line, "USB READ: %04x", i);
 158                                 break;
 159                         case DEBUG_TYPE_WRITE:
 160                                 sprintf(line, "USB WRITE: %04x", i);
 161                                 break;
 162                         case DEBUG_TYPE_OCD_READ:
 163                                 sprintf(line, "TO OpenOCD: %04x", i);
 164                                 break;
 165                         case DEBUG_TYPE_BUFFER:
 166                                 sprintf(line, "BUFFER: %04x", i);
 167                                 break;
 168                 }
 169
 170                 for (j = i; j < i + LINE_LEN && j < length; j++) {
 171                         sprintf(s, " %02x", buffer[j]);
 172                         strcat(line, s);
 173                 }
 174                 LOG_DEBUG("%s", line);
 175         }
 176
 177 }
 178
 179 #endif
 180
 181 static int8_t openjtag_get_tap_state(int8_t state)
 182 {
 183
 184         switch (state) {
 185                 case TAP_DREXIT2:   return OPENJTAG_TAP_EXIT2_DR;
 186                 case TAP_DREXIT1:   return OPENJTAG_TAP_EXIT1_DR;
 187                 case TAP_DRSHIFT:   return OPENJTAG_TAP_SHIFT_DR;
 188                 case TAP_DRPAUSE:   return OPENJTAG_TAP_PAUSE_DR;
 189                 case TAP_IRSELECT:  return OPENJTAG_TAP_SELECT_IR;
 190                 case TAP_DRUPDATE:  return OPENJTAG_TAP_UPDATE_DR;
 191                 case TAP_DRCAPTURE: return OPENJTAG_TAP_CAPTURE_DR;
 192                 case TAP_DRSELECT:  return OPENJTAG_TAP_SELECT_DR;
 193                 case TAP_IREXIT2:   return OPENJTAG_TAP_EXIT2_IR;
 194                 case TAP_IREXIT1:   return OPENJTAG_TAP_EXIT1_IR;
 195                 case TAP_IRSHIFT:   return OPENJTAG_TAP_SHIFT_IR;
 196                 case TAP_IRPAUSE:   return OPENJTAG_TAP_PAUSE_IR;
 197                 case TAP_IDLE:      return OPENJTAG_TAP_IDLE;
 198                 case TAP_IRUPDATE:  return OPENJTAG_TAP_UPDATE_IR;
 199                 case TAP_IRCAPTURE: return OPENJTAG_TAP_CAPURE_IR;
 200                 case TAP_RESET:     return OPENJTAG_TAP_RESET;
 201                 case TAP_INVALID:
 202                 default:            return OPENJTAG_TAP_INVALID;
 203         }
 204 }
 205
 206 static int openjtag_buf_write_standard(
 207         uint8_t *buf, int size, uint32_t *bytes_written)
 208 {
 209         int retval;
 210 #ifdef _DEBUG_USB_COMMS_
 211         openjtag_debug_buffer(buf, size, DEBUG_TYPE_WRITE);
 212 #endif
 213
 214         retval = ftdi_write_data(&ftdic, buf, size);
 215         if (retval < 0) {
 216                 *bytes_written = 0;
 217                 LOG_ERROR("ftdi_write_data: %s", ftdi_get_error_string(&ftdic));
 218                 return ERROR_JTAG_DEVICE_ERROR;
 219         }
 220
 221         *bytes_written = retval;
 222
 223         return ERROR_OK;
 224 }
 225
 226 static int openjtag_buf_write_cy7c65215(
 227         uint8_t *buf, int size, uint32_t *bytes_written)
 228 {
 229         int ret;
 230
 231 #ifdef _DEBUG_USB_COMMS_
 232         openjtag_debug_buffer(buf, size, DEBUG_TYPE_WRITE);
 233 #endif
 234
 235         if (size == 0) {
 236                 *bytes_written = 0;
 237                 return ERROR_OK;
 238         }
 239
 240         ret = jtag_libusb_control_transfer(usbh, CY7C65215_JTAG_REQUEST,
 241                                                                            CY7C65215_JTAG_WRITE, size, 0,
 242                                                                            NULL, 0, CY7C65215_USB_TIMEOUT);
 243         if (ret < 0) {
 244                 LOG_ERROR("vendor command failed, error %d", ret);
 245                 return ERROR_JTAG_DEVICE_ERROR;
 246         }
 247
 248         if (jtag_libusb_bulk_write(usbh, ep_out, (char *)buf, size,
 249                                    CY7C65215_USB_TIMEOUT, &ret)) {
 250                 LOG_ERROR("bulk write failed, error");
 251                 return ERROR_JTAG_DEVICE_ERROR;
 252         }
 253         *bytes_written = ret;
 254
 255         return ERROR_OK;
 256 }
 257
 258 static int openjtag_buf_write(
 259         uint8_t *buf, int size, uint32_t *bytes_written)
 260 {
 261         switch (openjtag_variant) {
 262         case OPENJTAG_VARIANT_CY7C65215:
 263                 return openjtag_buf_write_cy7c65215(buf, size, bytes_written);
 264         default:
 265                 return openjtag_buf_write_standard(buf, size, bytes_written);
 266         }
 267 }
 268
 269 static int openjtag_buf_read_standard(
 270         uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
 271 {
 272
 273         int retval;
 274         int timeout = 5;
 275
 276         *bytes_read = 0;
 277
 278         while ((*bytes_read < qty) && timeout--) {
 279                 retval = ftdi_read_data(&ftdic, buf + *bytes_read,
 280                                 qty - *bytes_read);
 281                 if (retval < 0) {
 282                         *bytes_read = 0;
 283                         LOG_DEBUG_IO("ftdi_read_data: %s",
 284                                         ftdi_get_error_string(&ftdic));
 285                         return ERROR_JTAG_DEVICE_ERROR;
 286                 }
 287                 *bytes_read += retval;
 288         }
 289
 290 #ifdef _DEBUG_USB_COMMS_
 291         openjtag_debug_buffer(buf, *bytes_read, DEBUG_TYPE_READ);
 292 #endif
 293
 294         return ERROR_OK;
 295 }
 296
 297 static int openjtag_buf_read_cy7c65215(
 298         uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
 299 {
 300         int ret;
 301
 302         if (qty == 0) {
 303                 *bytes_read = 0;
 304                 goto out;
 305         }
 306
 307         ret = jtag_libusb_control_transfer(usbh, CY7C65215_JTAG_REQUEST,
 308                                                                            CY7C65215_JTAG_READ, qty, 0,
 309                                                                            NULL, 0, CY7C65215_USB_TIMEOUT);
 310         if (ret < 0) {
 311                 LOG_ERROR("vendor command failed, error %d", ret);
 312                 return ERROR_JTAG_DEVICE_ERROR;
 313         }
 314
 315         if (jtag_libusb_bulk_read(usbh, ep_in, (char *)buf, qty,
 316                                   CY7C65215_USB_TIMEOUT, &ret)) {
 317                 LOG_ERROR("bulk read failed, error");
 318                 return ERROR_JTAG_DEVICE_ERROR;
 319         }
 320         *bytes_read = ret;
 321
 322 out:
 323 #ifdef _DEBUG_USB_COMMS_
 324         openjtag_debug_buffer(buf, *bytes_read, DEBUG_TYPE_READ);
 325 #endif
 326
 327         return ERROR_OK;
 328 }
 329
 330 static int openjtag_buf_read(uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
 331 {
 332         switch (openjtag_variant) {
 333         case OPENJTAG_VARIANT_CY7C65215:
 334                 return openjtag_buf_read_cy7c65215(buf, qty, bytes_read);
 335         default:
 336                 return openjtag_buf_read_standard(buf, qty, bytes_read);
 337         }
 338 }
 339
 340 static int openjtag_sendcommand(uint8_t cmd)
 341 {
 342         uint32_t written;
 343         return openjtag_buf_write(&cmd, 1, &written);
 344 }
 345
 346 static int openjtag_speed(int speed)
 347 {
 348         int clockcmd;
 349         switch (speed) {
 350                 case 48000:
 351                         clockcmd = 0x00;
 352                         break;
 353                 case 24000:
 354                         clockcmd = 0x20;
 355                         break;
 356                 case 12000:
 357                         clockcmd = 0x40;
 358                         break;
 359                 case 6000:
 360                         clockcmd = 0x60;
 361                         break;
 362                 case 3000:
 363                         clockcmd = 0x80;
 364                         break;
 365                 case 1500:
 366                         clockcmd = 0xA0;
 367                         break;
 368                 case 750:
 369                         clockcmd = 0xC0;
 370                         break;
 371                 case 375:
 372                         clockcmd = 0xE0;
 373                         break;
 374                 default:
 375                         clockcmd = 0xE0;
 376                         LOG_WARNING("adapter speed not recognized, reverting to 375 kHz");
 377                         break;
 378         }
 379         openjtag_sendcommand(clockcmd);
 380
 381         return ERROR_OK;
 382 }
 383
 384 static int openjtag_init_standard(void)
 385 {
 386         uint8_t latency_timer;
 387
 388         /* Open by device description */
 389         if (!openjtag_device_desc) {
 390                 LOG_WARNING("no openjtag device description specified, "
 391                                 "using default 'Open JTAG Project'");
 392                 openjtag_device_desc = "Open JTAG Project";
 393         }
 394
 395         if (ftdi_init(&ftdic) < 0)
 396                 return ERROR_JTAG_INIT_FAILED;
 397
 398         /* context, vendor id, product id, description, serial id */
 399         if (ftdi_usb_open_desc(&ftdic, openjtag_vid, openjtag_pid, openjtag_device_desc, NULL) < 0) {
 400                 LOG_ERROR("unable to open ftdi device: %s", ftdic.error_str);
 401                 return ERROR_JTAG_INIT_FAILED;
 402         }
 403
 404         if (ftdi_usb_reset(&ftdic) < 0) {
 405                 LOG_ERROR("unable to reset ftdi device");
 406                 return ERROR_JTAG_INIT_FAILED;
 407         }
 408
 409         if (ftdi_set_latency_timer(&ftdic, 2) < 0) {
 410                 LOG_ERROR("unable to set latency timer");
 411                 return ERROR_JTAG_INIT_FAILED;
 412         }
 413
 414         if (ftdi_get_latency_timer(&ftdic, &latency_timer) < 0) {
 415                 LOG_ERROR("unable to get latency timer");
 416                 return ERROR_JTAG_INIT_FAILED;
 417         }
 418         LOG_DEBUG("current latency timer: %u", latency_timer);
 419
 420         ftdi_disable_bitbang(&ftdic);
 421         /* was (3000000 / 4) with a comment about a bug in libftdi when using high baudrate */
 422         if (ftdi_set_baudrate(&ftdic, 3000000) < 0) {
 423                 LOG_ERROR("Can't set baud rate to max: %s",
 424                         ftdi_get_error_string(&ftdic));
 425                 return ERROR_JTAG_DEVICE_ERROR;
 426         }
 427
 428         if (ftdi_tcioflush(&ftdic) < 0) {
 429                 LOG_ERROR("ftdi flush: %s", ftdic.error_str);
 430                 return ERROR_JTAG_INIT_FAILED;
 431         }
 432
 433         return ERROR_OK;
 434 }
 435
 436 static int openjtag_init_cy7c65215(void)
 437 {
 438         int ret;
 439
 440         usbh = NULL;
 441         ret = jtag_libusb_open(cy7c65215_vids, cy7c65215_pids, &usbh, NULL);
 442         if (ret != ERROR_OK) {
 443                 LOG_ERROR("unable to open cy7c65215 device");
 444                 goto err;
 445         }
 446
 447         ret = jtag_libusb_choose_interface(usbh, &ep_in, &ep_out,
 448                                                                            CY7C65215_JTAG_CLASS,
 449                                                                            CY7C65215_JTAG_SUBCLASS, -1, LIBUSB_TRANSFER_TYPE_BULK);
 450         if (ret != ERROR_OK) {
 451                 LOG_ERROR("unable to claim JTAG interface");
 452                 goto err;
 453         }
 454
 455         ret = jtag_libusb_control_transfer(usbh,
 456                                                                            CY7C65215_JTAG_REQUEST,
 457                                                                            CY7C65215_JTAG_ENABLE,
 458                                                                            0, 0, NULL, 0, CY7C65215_USB_TIMEOUT);
 459         if (ret < 0) {
 460                 LOG_ERROR("could not enable JTAG module");
 461                 goto err;
 462         }
 463
 464         return ERROR_OK;
 465
 466 err:
 467         if (usbh)
 468                 jtag_libusb_close(usbh);
 469         return ERROR_JTAG_INIT_FAILED;
 470 }
 471
 472 static int openjtag_init(void)
 473 {
 474         int ret;
 475
 476         usb_tx_buf_offs = 0;
 477         usb_rx_buf_len = 0;
 478         openjtag_scan_result_count = 0;
 479
 480         switch (openjtag_variant) {
 481         case OPENJTAG_VARIANT_CY7C65215:
 482                 ret = openjtag_init_cy7c65215();
 483                 break;
 484         default:
 485                 ret = openjtag_init_standard();
 486         }
 487         if (ret != ERROR_OK)
 488                 return ret;
 489
 490         openjtag_speed(375); /* Start at slowest adapter speed */
 491         openjtag_sendcommand(0x75); /* MSB */
 492
 493         return ERROR_OK;
 494 }
 495
 496 static int openjtag_quit_standard(void)
 497 {
 498         ftdi_usb_close(&ftdic);
 499         ftdi_deinit(&ftdic);
 500
 501         return ERROR_OK;
 502 }
 503
 504 static int openjtag_quit_cy7c65215(void)
 505 {
 506         int ret;
 507
 508         ret = jtag_libusb_control_transfer(usbh,
 509                                                                            CY7C65215_JTAG_REQUEST,
 510                                                                            CY7C65215_JTAG_DISABLE,
 511                                                                            0, 0, NULL, 0, CY7C65215_USB_TIMEOUT);
 512         if (ret < 0)
 513                 LOG_WARNING("could not disable JTAG module");
 514
 515         jtag_libusb_close(usbh);
 516
 517         return ERROR_OK;
 518 }
 519
 520 static int openjtag_quit(void)
 521 {
 522         switch (openjtag_variant) {
 523         case OPENJTAG_VARIANT_CY7C65215:
 524                 return openjtag_quit_cy7c65215();
 525         default:
 526                 return openjtag_quit_standard();
 527         }
 528 }
 529
 530 static void openjtag_write_tap_buffer(void)
 531 {
 532         uint32_t written;
 533
 534         openjtag_buf_write(usb_tx_buf, usb_tx_buf_offs, &written);
 535         openjtag_buf_read(usb_rx_buf, usb_tx_buf_offs, &usb_rx_buf_len);
 536
 537         usb_tx_buf_offs = 0;
 538 }
 539
 540 static int openjtag_execute_tap_queue(void)
 541 {
 542         openjtag_write_tap_buffer();
 543
 544         int res_count = 0;
 545
 546         if (openjtag_scan_result_count && usb_rx_buf_len) {
 547
 548                 int count;
 549                 int rx_offs = 0;
 550                 int len;
 551
 552                 /* for every pending result */
 553                 while (res_count < openjtag_scan_result_count) {
 554
 555                         /* get sent bits */
 556                         len = openjtag_scan_result_buffer[res_count].bits;
 557
 558                         count = 0;
 559
 560                         uint8_t *buffer = openjtag_scan_result_buffer[res_count].buffer;
 561
 562                         while (len > 0) {
 563                                 if (len <= 8 && openjtag_variant != OPENJTAG_VARIANT_CY7C65215) {
 564                                         LOG_DEBUG_IO("bits < 8 buf = 0x%X, will be 0x%X",
 565                                                 usb_rx_buf[rx_offs], usb_rx_buf[rx_offs] >> (8 - len));
 566                                         buffer[count] = usb_rx_buf[rx_offs] >> (8 - len);
 567                                         len = 0;
 568                                 } else {
 569                                         buffer[count] = usb_rx_buf[rx_offs];
 570                                         len -= 8;
 571                                 }
 572
 573                                 rx_offs++;
 574                                 count++;
 575                         }
 576
 577 #ifdef _DEBUG_USB_COMMS_
 578                         openjtag_debug_buffer(buffer,
 579                                 DIV_ROUND_UP(openjtag_scan_result_buffer[res_count].bits, 8), DEBUG_TYPE_OCD_READ);
 580 #endif
 581                         jtag_read_buffer(buffer, openjtag_scan_result_buffer[res_count].command);
 582
 583                         free(openjtag_scan_result_buffer[res_count].buffer);
 584
 585                         res_count++;
 586                 }
 587         }
 588
 589         openjtag_scan_result_count = 0;
 590
 591         return ERROR_OK;
 592 }
 593
 594 static void openjtag_add_byte(char buf)
 595 {
 596
 597         if (usb_tx_buf_offs == OPENJTAG_BUFFER_SIZE) {
 598                 LOG_DEBUG_IO("Forcing execute_tap_queue");
 599                 LOG_DEBUG_IO("TX Buff offs=%d", usb_tx_buf_offs);
 600                 openjtag_execute_tap_queue();
 601         }
 602
 603         usb_tx_buf[usb_tx_buf_offs] = buf;
 604         usb_tx_buf_offs++;
 605 }
 606
 607 static void openjtag_add_scan(uint8_t *buffer, int length, struct scan_command *scan_cmd)
 608 {
 609
 610         /* Ensure space to send long chains */
 611         /* We add two byte for each eight (or less) bits, one for command, one for data */
 612         if (usb_tx_buf_offs + (DIV_ROUND_UP(length, 8) * 2) >= OPENJTAG_BUFFER_SIZE) {
 613                 LOG_DEBUG_IO("Forcing execute_tap_queue from scan");
 614                 LOG_DEBUG_IO("TX Buff offs=%d len=%d", usb_tx_buf_offs, DIV_ROUND_UP(length, 8) * 2);
 615                 openjtag_execute_tap_queue();
 616         }
 617
 618         openjtag_scan_result_buffer[openjtag_scan_result_count].bits = length;
 619         openjtag_scan_result_buffer[openjtag_scan_result_count].command = scan_cmd;
 620         openjtag_scan_result_buffer[openjtag_scan_result_count].buffer = buffer;
 621
 622         uint8_t command;
 623         uint8_t bits;
 624         int count = 0;
 625         while (length) {
 626
 627                 /* write command */
 628                 command = 6;
 629
 630                 /* last bits? */
 631                 if (length <= 8) {
 632                         /* tms high */
 633                         command |= (1 << 4);
 634
 635                         /* bits to transfer */
 636                         bits = (length - 1);
 637                         command |= bits << 5;
 638                         length = 0;
 639                 } else {
 640                         /* whole byte */
 641
 642                         /* bits to transfer */
 643                         command |= (7 << 5);
 644                         length -= 8;
 645                 }
 646
 647                 openjtag_add_byte(command);
 648                 openjtag_add_byte(buffer[count]);
 649                 count++;
 650         }
 651
 652         openjtag_scan_result_count++;
 653 }
 654
 655 static void openjtag_execute_reset(struct jtag_command *cmd)
 656 {
 657
 658         LOG_DEBUG_IO("reset trst: %i srst %i",
 659                         cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 660
 661         uint8_t buf = 0x00;
 662
 663         if (cmd->cmd.reset->trst) {
 664                 buf = 0x03;
 665         } else {
 666                 buf |= 0x04;
 667                 buf |= 0x05 << 4;
 668         }
 669
 670         openjtag_add_byte(buf);
 671 }
 672
 673 static void openjtag_execute_sleep(struct jtag_command *cmd)
 674 {
 675         jtag_sleep(cmd->cmd.sleep->us);
 676 }
 677
 678 static void openjtag_set_state(uint8_t openocd_state)
 679 {
 680         uint8_t state = openjtag_get_tap_state(openocd_state);
 681
 682         uint8_t buf = 0;
 683         buf = 0x01;
 684         buf |= state << 4;
 685
 686         openjtag_add_byte(buf);
 687 }
 688
 689 static void openjtag_execute_statemove(struct jtag_command *cmd)
 690 {
 691         LOG_DEBUG_IO("state move to %i", cmd->cmd.statemove->end_state);
 692
 693         tap_set_end_state(cmd->cmd.statemove->end_state);
 694
 695         openjtag_set_state(cmd->cmd.statemove->end_state);
 696
 697         tap_set_state(tap_get_end_state());
 698 }
 699
 700
 701 static void openjtag_execute_scan(struct jtag_command *cmd)
 702 {
 703
 704         int scan_size, old_state;
 705         uint8_t *buffer;
 706
 707         LOG_DEBUG_IO("scan ends in %s", tap_state_name(cmd->cmd.scan->end_state));
 708
 709         /* get scan info */
 710         tap_set_end_state(cmd->cmd.scan->end_state);
 711         scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
 712
 713 #ifdef _DEBUG_USB_COMMS_
 714         openjtag_debug_buffer(buffer, (scan_size + 7) / 8, DEBUG_TYPE_BUFFER);
 715 #endif
 716         /* set state */
 717         old_state = tap_get_end_state();
 718         openjtag_set_state(cmd->cmd.scan->ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 719         tap_set_state(cmd->cmd.scan->ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 720         tap_set_end_state(old_state);
 721
 722         openjtag_add_scan(buffer, scan_size, cmd->cmd.scan);
 723
 724         openjtag_set_state(cmd->cmd.scan->ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 725         tap_set_state(cmd->cmd.scan->ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 726
 727         if (tap_get_state() != tap_get_end_state()) {
 728                 openjtag_set_state(tap_get_end_state());
 729                 tap_set_state(tap_get_end_state());
 730         }
 731 }
 732
 733 static void openjtag_execute_runtest(struct jtag_command *cmd)
 734 {
 735
 736         tap_state_t end_state = cmd->cmd.runtest->end_state;
 737         tap_set_end_state(end_state);
 738
 739         /* only do a state_move when we're not already in IDLE */
 740         if (tap_get_state() != TAP_IDLE) {
 741                 openjtag_set_state(TAP_IDLE);
 742                 tap_set_state(TAP_IDLE);
 743         }
 744
 745         if (cmd->cmd.runtest->num_cycles > 16)
 746                 LOG_WARNING("num_cycles > 16 on run test");
 747
 748         if (openjtag_variant != OPENJTAG_VARIANT_CY7C65215 ||
 749                 cmd->cmd.runtest->num_cycles) {
 750                 uint8_t command;
 751                 command = 7;
 752                 command |= ((cmd->cmd.runtest->num_cycles - 1) & 0x0F) << 4;
 753
 754                 openjtag_add_byte(command);
 755         }
 756
 757         tap_set_end_state(end_state);
 758         if (tap_get_end_state() != tap_get_state()) {
 759                 openjtag_set_state(end_state);
 760                 tap_set_state(end_state);
 761         }
 762 }
 763
 764 static void openjtag_execute_command(struct jtag_command *cmd)
 765 {
 766         LOG_DEBUG_IO("openjtag_execute_command %i", cmd->type);
 767         switch (cmd->type) {
 768         case JTAG_RESET:
 769                         openjtag_execute_reset(cmd);
 770                         break;
 771         case JTAG_SLEEP:
 772                         openjtag_execute_sleep(cmd);
 773                         break;
 774         case JTAG_TLR_RESET:
 775                         openjtag_execute_statemove(cmd);
 776                         break;
 777         case JTAG_SCAN:
 778                         openjtag_execute_scan(cmd);
 779                         break;
 780         case JTAG_RUNTEST:
 781                         openjtag_execute_runtest(cmd);
 782                         break;
 783         case JTAG_PATHMOVE:
 784                 /* jlink_execute_pathmove(cmd); break; */
 785         default:
 786                 LOG_ERROR("BUG: unknown Open JTAG command type encountered");
 787                 exit(-1);
 788         }
 789 }
 790
 791 static int openjtag_execute_queue(void)
 792 {
 793         struct jtag_command *cmd = jtag_command_queue;
 794
 795         while (cmd) {
 796                 openjtag_execute_command(cmd);
 797                 cmd = cmd->next;
 798         }
 799
 800         return openjtag_execute_tap_queue();
 801 }
 802
 803 static int openjtag_speed_div(int speed, int *khz)
 804 {
 805         *khz = speed;
 806
 807         return ERROR_OK;
 808 }
 809
 810 static int openjtag_khz(int khz, int *jtag_speed)
 811 {
 812
 813         if (khz >= 48000)
 814                 *jtag_speed = 48000;
 815         else if (khz >= 24000)
 816                 *jtag_speed = 24000;
 817         else if (khz >= 12000)
 818                 *jtag_speed = 12000;
 819         else if (khz >= 6000)
 820                 *jtag_speed = 6000;
 821         else if (khz >= 3000)
 822                 *jtag_speed = 3000;
 823         else if (khz >= 1500)
 824                 *jtag_speed = 1500;
 825         else if (khz >= 750)
 826                 *jtag_speed = 750;
 827         else
 828                 *jtag_speed = 375;
 829
 830         return ERROR_OK;
 831 }
 832
 833 COMMAND_HANDLER(openjtag_handle_device_desc_command)
 834 {
 835         if (CMD_ARGC == 1)
 836                 openjtag_device_desc = strdup(CMD_ARGV[0]);
 837         else
 838                 LOG_ERROR("require exactly one argument to "
 839                                   "openjtag_device_desc <description>");
 840         return ERROR_OK;
 841 }
 842
 843 COMMAND_HANDLER(openjtag_handle_variant_command)
 844 {
 845         if (CMD_ARGC == 1) {
 846                 const char * const *name = openjtag_variant_names;
 847                 int variant = 0;
 848                 for (; *name; name++, variant++) {
 849                         if (strcasecmp(CMD_ARGV[0], *name) == 0) {
 850                                 openjtag_variant = variant;
 851                                 return ERROR_OK;
 852                         }
 853                 }
 854                 LOG_ERROR("unknown openjtag variant '%s'", CMD_ARGV[0]);
 855         } else {
 856                 LOG_ERROR("require exactly one argument to "
 857                                 "openjtag_variant <variant>");
 858         }
 859         return ERROR_OK;
 860 }
 861
 862 static const struct command_registration openjtag_subcommand_handlers[] = {
 863         {
 864                 .name = "device_desc",
 865                 .handler = openjtag_handle_device_desc_command,
 866                 .mode = COMMAND_CONFIG,
 867                 .help = "set the USB device description of the OpenJTAG",
 868                 .usage = "description-string",
 869         },
 870         {
 871                 .name = "variant",
 872                 .handler = openjtag_handle_variant_command,
 873                 .mode = COMMAND_CONFIG,
 874                 .help = "set the OpenJTAG variant",
 875                 .usage = "variant-string",
 876         },
 877         COMMAND_REGISTRATION_DONE
 878 };
 879
 880 static const struct command_registration openjtag_command_handlers[] = {
 881         {
 882                 .name = "openjtag",
 883                 .mode = COMMAND_ANY,
 884                 .help = "perform openjtag management",
 885                 .chain = openjtag_subcommand_handlers,
 886                 .usage = "",
 887         },
 888         COMMAND_REGISTRATION_DONE
 889 };
 890
 891 static struct jtag_interface openjtag_interface = {
 892         .execute_queue = openjtag_execute_queue,
 893 };
 894
 895 struct adapter_driver openjtag_adapter_driver = {
 896         .name = "openjtag",
 897         .transports = jtag_only,
 898         .commands = openjtag_command_handlers,
 899
 900         .init = openjtag_init,
 901         .quit = openjtag_quit,
 902         .speed = openjtag_speed,
 903         .khz = openjtag_khz,
 904         .speed_div = openjtag_speed_div,
 905
 906         .jtag_ops = &openjtag_interface,
 907 };