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

   1 /*******************************************************************************
   2  *   Driver for OpenJTAG Project (www.openjtag.org)                            *
   3  *   Compatible with libftdi and ftd2xx drivers.                               *
   4  *                                                                             *
   5  *   Copyright (C) 2010 by Ivan Meleca <mileca@gmail.com>                      *
   6  *                                                                             *
   7  *   Copyright (C) 2013 by Ryan Corbin, GlueLogix Inc. <corbin.ryan@gmail.com> *
   8  *   Updated to work with OpenOCD v0.7.0. Fixed libftdi read speed issue.      *
   9  *                                                                             *
  10  *   Based on usb_blaster.c                                                    *
  11  *   Copyright (C) 2009 Catalin Patulea                                        *
  12  *   Copyright (C) 2006 Kolja Waschk                                           *
  13  *                                                                             *
  14  *   And jlink.c                                                               *
  15  *   Copyright (C) 2008 by Spencer Oliver                                      *
  16  *   spen@spen-soft.co.uk                                                      *
  17  *                                                                             *
  18  *   This program is free software; you can redistribute it and/or modify      *
  19  *   it under the terms of the GNU General Public License as published by      *
  20  *   the Free Software Foundation; either version 2 of the License, or         *
  21  *   (at your option) any later version.                                       *
  22  *                                                                             *
  23  *   This program is distributed in the hope that it will be useful,           *
  24  *   but WITHOUT ANY WARRANTY; without even the implied warranty of            *
  25  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
  26  *   GNU General Public License for more details.                              *
  27  *                                                                             *
  28  *   You should have received a copy of the GNU General Public License         *
  29  *   along with this program; if not, write to the                             *
  30  *   Free Software Foundation, Inc.,                                           *
  31  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.               *
  32  ***************************************************************************/
  33
  34 /***************************************************************************
  35  * Version 1.0  Tested on a MCBSTM32 board using a Cortex-M3 (stm32f103x), *
  36  *              GDB and Eclipse under Linux (Ubuntu 10.04)                 *
  37  *                                                                         *
  38  ***************************************************************************/
  39
  40 #ifdef HAVE_CONFIG_H
  41 #include "config.h"
  42 #endif
  43
  44 #include <jtag/interface.h>
  45 #include <jtag/commands.h>
  46 #include "usb_common.h"
  47
  48 /*
  49  * OpenJTAG-OpenOCD state conversion
  50  */
  51 typedef enum openjtag_tap_state {
  52         OPENJTAG_TAP_INVALID    = -1,
  53         OPENJTAG_TAP_RESET  = 0,
  54         OPENJTAG_TAP_IDLE   = 1,
  55         OPENJTAG_TAP_SELECT_DR  = 2,
  56         OPENJTAG_TAP_CAPTURE_DR = 3,
  57         OPENJTAG_TAP_SHIFT_DR   = 4,
  58         OPENJTAG_TAP_EXIT1_DR   = 5,
  59         OPENJTAG_TAP_PAUSE_DR   = 6,
  60         OPENJTAG_TAP_EXIT2_DR   = 7,
  61         OPENJTAG_TAP_UPDATE_DR  = 8,
  62         OPENJTAG_TAP_SELECT_IR  = 9,
  63         OPENJTAG_TAP_CAPURE_IR  = 10,
  64         OPENJTAG_TAP_SHIFT_IR   = 11,
  65         OPENJTAG_TAP_EXIT1_IR   = 12,
  66         OPENJTAG_TAP_PAUSE_IR   = 13,
  67         OPENJTAG_TAP_EXIT2_IR   = 14,
  68         OPENJTAG_TAP_UPDATE_IR  = 15,
  69 } openjtag_tap_state_t;
  70
  71 #if (BUILD_OPENJTAG_FTD2XX == 1 && BUILD_OPENJTAG_LIBFTDI == 1)
  72 #error "BUILD_OPENJTAG_FTD2XX && BUILD_OPENJTAG_LIBFTDI "
  73            "are mutually exclusive"
  74 #elif (BUILD_OPENJTAG_FTD2XX != 1 && BUILD_OPENJTAG_LIBFTDI != 1)
  75 #error "BUILD_OPENJTAG_FTD2XX || BUILD_OPENJTAG_LIBFTDI must be chosen"
  76 #endif
  77
  78 /* OPENJTAG access library includes */
  79 #if BUILD_OPENJTAG_FTD2XX == 1
  80 #include <ftd2xx.h>
  81 #elif BUILD_OPENJTAG_LIBFTDI == 1
  82 #include <ftdi.h>
  83 #endif
  84
  85 /* OpenJTAG vid/pid */
  86 static uint16_t openjtag_vid = 0x0403;
  87 static uint16_t openjtag_pid = 0x6001;
  88
  89 static char *openjtag_device_desc;
  90
  91 #if BUILD_OPENJTAG_FTD2XX == 1
  92 static FT_HANDLE ftdih;
  93
  94 #elif BUILD_OPENJTAG_LIBFTDI == 1
  95 static struct ftdi_context ftdic;
  96 #endif
  97
  98 #define OPENJTAG_BUFFER_SIZE        504
  99 #define OPENJTAG_MAX_PENDING_RESULTS    256
 100
 101 struct openjtag_scan_result {
 102         uint32_t bits;          /* Length in bits*/
 103         struct scan_command *command;   /* Corresponding scan command */
 104         uint8_t *buffer;
 105 };
 106
 107 /* USB RX/TX buffers */
 108 static int usb_tx_buf_offs;
 109 static uint8_t usb_tx_buf[OPENJTAG_BUFFER_SIZE];
 110 static uint32_t usb_rx_buf_len;
 111 static uint8_t usb_rx_buf[OPENJTAG_BUFFER_SIZE];
 112
 113 /* Pending readings */
 114 static struct openjtag_scan_result openjtag_scan_result_buffer[OPENJTAG_MAX_PENDING_RESULTS];
 115 static int openjtag_scan_result_count;
 116
 117 /* Openocd usb handler */
 118 struct openocd {
 119         struct usb_dev_handle *usb_handle;
 120 };
 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(
 207         uint8_t *buf, int size, uint32_t *bytes_written)
 208 {
 209 #if BUILD_OPENJTAG_FTD2XX == 1
 210         FT_STATUS status;
 211         DWORD dw_bytes_written;
 212
 213 #ifdef _DEBUG_USB_COMMS_
 214         openjtag_debug_buffer(buf, size, DEBUG_TYPE_WRITE);
 215 #endif
 216
 217         status = FT_Write(ftdih, buf, size, &dw_bytes_written);
 218         if (status != FT_OK) {
 219                 *bytes_written = dw_bytes_written;
 220                 LOG_ERROR("FT_Write returned: %u", status);
 221                 return ERROR_JTAG_DEVICE_ERROR;
 222         }
 223         *bytes_written = dw_bytes_written;
 224         return ERROR_OK;
 225 #elif BUILD_OPENJTAG_LIBFTDI == 1
 226         int retval;
 227 #ifdef _DEBUG_USB_COMMS_
 228         openjtag_debug_buffer(buf, size, DEBUG_TYPE_WRITE);
 229 #endif
 230
 231         retval = ftdi_write_data(&ftdic, buf, size);
 232         if (retval < 0) {
 233                 *bytes_written = 0;
 234                 LOG_ERROR("ftdi_write_data: %s", ftdi_get_error_string(&ftdic));
 235                 return ERROR_JTAG_DEVICE_ERROR;
 236         }
 237
 238         *bytes_written += retval;
 239
 240         return ERROR_OK;
 241 #endif
 242 }
 243
 244 static int openjtag_buf_read(uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
 245 {
 246
 247 #if BUILD_OPENJTAG_FTD2XX == 1
 248         DWORD dw_bytes_read;
 249         FT_STATUS status;
 250         int timeout = 50;
 251
 252         *bytes_read = 0;
 253         while (qty && (*bytes_read < qty) && timeout--) {
 254
 255                 status = FT_Read(ftdih, buf + *bytes_read,
 256                                 qty - *bytes_read, &dw_bytes_read);
 257                 if (status != FT_OK) {
 258                         *bytes_read = dw_bytes_read;
 259                         LOG_ERROR("FT_Read returned: %u", status);
 260                         return ERROR_JTAG_DEVICE_ERROR;
 261                 }
 262                 *bytes_read += dw_bytes_read;
 263         }
 264
 265 #ifdef _DEBUG_USB_COMMS_
 266         openjtag_debug_buffer(buf, *bytes_read, DEBUG_TYPE_READ);
 267 #endif
 268
 269         return ERROR_OK;
 270 #elif BUILD_OPENJTAG_LIBFTDI == 1
 271         int retval;
 272         int timeout = 5;
 273
 274         *bytes_read = 0;
 275
 276         while ((*bytes_read < qty) && timeout--) {
 277                 retval = ftdi_read_data(&ftdic, buf + *bytes_read,
 278                                 qty - *bytes_read);
 279                 if (retval < 0) {
 280                         *bytes_read = 0;
 281                         DEBUG_JTAG_IO("ftdi_read_data: %s",
 282                                         ftdi_get_error_string(&ftdic));
 283                         return ERROR_JTAG_DEVICE_ERROR;
 284                 }
 285                 *bytes_read += retval;
 286         }
 287
 288 #ifdef _DEBUG_USB_COMMS_
 289         openjtag_debug_buffer(buf, *bytes_read, DEBUG_TYPE_READ);
 290 #endif
 291
 292 #endif
 293         return ERROR_OK;
 294 }
 295
 296 static int openjtag_sendcommand(uint8_t cmd)
 297 {
 298         uint32_t written;
 299         return openjtag_buf_write(&cmd, 1, &written);
 300 }
 301
 302 static int openjtag_speed(int speed)
 303 {
 304         int clockcmd;
 305         switch (speed) {
 306                 case 48000:
 307                         clockcmd = 0x00;
 308                         break;
 309                 case 24000:
 310                         clockcmd = 0x20;
 311                         break;
 312                 case 12000:
 313                         clockcmd = 0x40;
 314                         break;
 315                 case 6000:
 316                         clockcmd = 0x60;
 317                         break;
 318                 case 3000:
 319                         clockcmd = 0x80;
 320                         break;
 321                 case 1500:
 322                         clockcmd = 0xA0;
 323                         break;
 324                 case 750:
 325                         clockcmd = 0xC0;
 326                         break;
 327                 case 375:
 328                         clockcmd = 0xE0;
 329                         break;
 330                 default:
 331                         clockcmd = 0xE0;
 332                         LOG_WARNING("adapter speed not recognized, reverting to 375 kHz");
 333                         break;
 334         }
 335         openjtag_sendcommand(clockcmd);
 336
 337         return ERROR_OK;
 338 }
 339
 340 static int openjtag_init(void)
 341 {
 342         uint8_t latency_timer;
 343
 344 #if BUILD_OPENJTAG_FTD2XX == 1
 345         FT_STATUS status;
 346 #endif
 347
 348 usb_tx_buf_offs = 0;
 349 usb_rx_buf_len = 0;
 350 openjtag_scan_result_count = 0;
 351
 352 #if BUILD_OPENJTAG_FTD2XX == 1
 353         LOG_DEBUG("'openjtag' interface using FTD2XX");
 354 #elif BUILD_OPENJTAG_LIBFTDI == 1
 355         LOG_DEBUG("'openjtag' interface using libftdi");
 356 #endif
 357
 358 /* Open by device description */
 359 if (openjtag_device_desc == NULL) {
 360         LOG_WARNING("no openjtag device description specified, "
 361                                 "using default 'Open JTAG Project'");
 362         openjtag_device_desc = "Open JTAG Project";
 363 }
 364
 365 #if BUILD_OPENJTAG_FTD2XX == 1
 366
 367 #if IS_WIN32 == 0
 368         /* Add non-standard Vid/Pid to the linux driver */
 369         status = FT_SetVIDPID(openjtag_vid, openjtag_pid);
 370         if (status != FT_OK) {
 371                 LOG_WARNING("couldn't add %4.4x:%4.4x",
 372                         openjtag_vid, openjtag_pid);
 373         }
 374 #endif
 375
 376         status = FT_OpenEx(openjtag_device_desc, FT_OPEN_BY_DESCRIPTION,
 377                         &ftdih);
 378         if (status != FT_OK) {
 379                 DWORD num_devices;
 380
 381                 LOG_ERROR("unable to open ftdi device: %u", status);
 382                 status = FT_ListDevices(&num_devices, NULL,
 383                                 FT_LIST_NUMBER_ONLY);
 384                 if (status == FT_OK) {
 385                         char **desc_array = malloc(sizeof(char *)
 386                                                 * (num_devices + 1));
 387                         unsigned int i;
 388
 389                         for (i = 0; i < num_devices; i++)
 390                                 desc_array[i] = malloc(64);
 391                         desc_array[num_devices] = NULL;
 392
 393                         status = FT_ListDevices(desc_array, &num_devices,
 394                                 FT_LIST_ALL | FT_OPEN_BY_DESCRIPTION);
 395
 396                         if (status == FT_OK) {
 397                                 LOG_ERROR("ListDevices: %u\n", num_devices);
 398                                 for (i = 0; i < num_devices; i++)
 399                                         LOG_ERROR("%i: %s", i, desc_array[i]);
 400                         }
 401
 402                         for (i = 0; i < num_devices; i++)
 403                                 free(desc_array[i]);
 404                         free(desc_array);
 405                 } else {
 406                         LOG_ERROR("ListDevices: NONE\n");
 407                 }
 408                 return ERROR_JTAG_INIT_FAILED;
 409         }
 410
 411         status = FT_SetLatencyTimer(ftdih, 2);
 412         if (status != FT_OK) {
 413                 LOG_ERROR("unable to set latency timer: %u", status);
 414                 return ERROR_JTAG_INIT_FAILED;
 415         }
 416
 417         status = FT_GetLatencyTimer(ftdih, &latency_timer);
 418         if (status != FT_OK) {
 419                 LOG_ERROR("unable to get latency timer: %u", status);
 420                 return ERROR_JTAG_INIT_FAILED;
 421         }
 422         LOG_DEBUG("current latency timer: %i", latency_timer);
 423
 424         status = FT_SetBitMode(ftdih, 0x00, 0x40);
 425         if (status != FT_OK) {
 426                 LOG_ERROR("unable to disable bit i/o mode: %u", status);
 427                 return ERROR_JTAG_INIT_FAILED;
 428         }
 429
 430         status = FT_SetTimeouts(ftdih, 50, 0);
 431         if (status != FT_OK) {
 432                 LOG_ERROR("unable to set timeouts: %u", status);
 433                 return ERROR_JTAG_INIT_FAILED;
 434         }
 435
 436         status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX);
 437         if (status != FT_OK) {
 438                 LOG_ERROR("unable to FT_Purge() %u", status);
 439                 return ERROR_JTAG_INIT_FAILED;
 440         }
 441
 442 #elif BUILD_OPENJTAG_LIBFTDI == 1
 443         if (ftdi_init(&ftdic) < 0)
 444                 return ERROR_JTAG_INIT_FAILED;
 445
 446         /* context, vendor id, product id, description, serial id */
 447         if (ftdi_usb_open_desc(&ftdic, openjtag_vid, openjtag_pid, openjtag_device_desc, NULL) < 0) {
 448                 LOG_ERROR("unable to open ftdi device: %s", ftdic.error_str);
 449                 return ERROR_JTAG_INIT_FAILED;
 450         }
 451
 452         if (ftdi_usb_reset(&ftdic) < 0) {
 453                 LOG_ERROR("unable to reset ftdi device");
 454                 return ERROR_JTAG_INIT_FAILED;
 455         }
 456
 457         if (ftdi_set_latency_timer(&ftdic, 2) < 0) {
 458                 LOG_ERROR("unable to set latency timer");
 459                 return ERROR_JTAG_INIT_FAILED;
 460         }
 461
 462         if (ftdi_get_latency_timer(&ftdic, &latency_timer) < 0) {
 463                 LOG_ERROR("unable to get latency timer");
 464                 return ERROR_JTAG_INIT_FAILED;
 465         }
 466         LOG_DEBUG("current latency timer: %u", latency_timer);
 467
 468         ftdi_disable_bitbang(&ftdic);
 469         /* was (3000000 / 4) with a comment about a bug in libftdi when using high baudrate */
 470         if (ftdi_set_baudrate(&ftdic, 3000000) < 0) {
 471                 LOG_ERROR("Can't set baud rate to max: %s",
 472                         ftdi_get_error_string(&ftdic));
 473                 return ERROR_JTAG_DEVICE_ERROR;
 474         }
 475 #endif
 476
 477 #if BUILD_OPENJTAG_FTD2XX == 1
 478         status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX);
 479         if (status != FT_OK)
 480                 return ERROR_JTAG_INIT_FAILED;
 481 #elif BUILD_OPENJTAG_LIBFTDI == 1
 482         if (ftdi_usb_purge_buffers(&ftdic) < 0) {
 483                 LOG_ERROR("ftdi_purge_buffers: %s", ftdic.error_str);
 484                 return ERROR_JTAG_INIT_FAILED;
 485         }
 486 #endif
 487
 488         /* OpenJTAG speed */
 489         openjtag_sendcommand(0xE0); /*Start at slowest adapter speed*/
 490
 491         /* MSB */
 492         openjtag_sendcommand(0x75);
 493
 494         return ERROR_OK;
 495 }
 496
 497 static int openjtag_quit(void)
 498 {
 499 #if BUILD_OPENJTAG_FTD2XX == 1
 500         FT_Close(ftdih);
 501 #elif BUILD_OPENJTAG_LIBFTDI == 1
 502         ftdi_usb_close(&ftdic);
 503         ftdi_deinit(&ftdic);
 504 #endif
 505
 506         return ERROR_OK;
 507 }
 508
 509 static void openjtag_write_tap_buffer(void)
 510 {
 511         uint32_t written;
 512
 513         openjtag_buf_write(usb_tx_buf, usb_tx_buf_offs, &written);
 514         openjtag_buf_read(usb_rx_buf, usb_tx_buf_offs, &usb_rx_buf_len);
 515
 516         usb_tx_buf_offs = 0;
 517 }
 518
 519 static int openjtag_execute_tap_queue(void)
 520 {
 521         openjtag_write_tap_buffer();
 522
 523         int res_count = 0;
 524
 525         if (openjtag_scan_result_count && usb_rx_buf_len) {
 526
 527                 int count;
 528                 int rx_offs = 0;
 529                 int len;
 530
 531                 /* for every pending result */
 532                 while (res_count < openjtag_scan_result_count) {
 533
 534                         /* get sent bits */
 535                         len = openjtag_scan_result_buffer[res_count].bits;
 536
 537                         count = 0;
 538
 539                         uint8_t *buffer = openjtag_scan_result_buffer[res_count].buffer;
 540
 541                         while (len) {
 542                                 if (len <= 8) {
 543                                         DEBUG_JTAG_IO("bits < 8 buf = 0x%X, will be 0x%X",
 544                                                 usb_rx_buf[rx_offs], usb_rx_buf[rx_offs] >> (8 - len));
 545                                         buffer[count] = usb_rx_buf[rx_offs] >> (8 - len);
 546                                         len = 0;
 547                                 } else {
 548                                         buffer[count] = usb_rx_buf[rx_offs];
 549                                         len -= 8;
 550                                 }
 551
 552                                 rx_offs++;
 553                                 count++;
 554                         }
 555
 556 #ifdef _DEBUG_USB_COMMS_
 557                         openjtag_debug_buffer(buffer,
 558                                 DIV_ROUND_UP(openjtag_scan_result_buffer[res_count].bits, 8), DEBUG_TYPE_OCD_READ);
 559 #endif
 560                         jtag_read_buffer(buffer, openjtag_scan_result_buffer[res_count].command);
 561
 562                         if (openjtag_scan_result_buffer[res_count].buffer)
 563                                 free(openjtag_scan_result_buffer[res_count].buffer);
 564
 565                         res_count++;
 566                 }
 567         }
 568
 569         openjtag_scan_result_count = 0;
 570
 571         return ERROR_OK;
 572 }
 573
 574 static void openjtag_add_byte(char buf)
 575 {
 576
 577         if (usb_tx_buf_offs == OPENJTAG_BUFFER_SIZE) {
 578                 DEBUG_JTAG_IO("Forcing execute_tap_queue");
 579                 DEBUG_JTAG_IO("TX Buff offs=%d", usb_tx_buf_offs);
 580                 openjtag_execute_tap_queue();
 581         }
 582
 583         usb_tx_buf[usb_tx_buf_offs] = buf;
 584         usb_tx_buf_offs++;
 585 }
 586
 587 static void openjtag_add_scan(uint8_t *buffer, int length, struct scan_command *scan_cmd)
 588 {
 589
 590         /* Ensure space to send long chains */
 591         /* We add two byte for each eight (or less) bits, one for command, one for data */
 592         if (usb_tx_buf_offs + (DIV_ROUND_UP(length, 8) * 2) >= OPENJTAG_BUFFER_SIZE) {
 593                 DEBUG_JTAG_IO("Forcing execute_tap_queue from scan");
 594                 DEBUG_JTAG_IO("TX Buff offs=%d len=%d", usb_tx_buf_offs, DIV_ROUND_UP(length, 8) * 2);
 595                 openjtag_execute_tap_queue();
 596         }
 597
 598         openjtag_scan_result_buffer[openjtag_scan_result_count].bits = length;
 599         openjtag_scan_result_buffer[openjtag_scan_result_count].command = scan_cmd;
 600         openjtag_scan_result_buffer[openjtag_scan_result_count].buffer = buffer;
 601
 602         uint8_t command;
 603         uint8_t bits;
 604         int count = 0;
 605         while (length) {
 606
 607                 /* write command */
 608                 command = 6;
 609
 610                 /* last bits? */
 611                 if (length <= 8) {
 612                         /* tms high */
 613                         command |= (1 << 4);
 614
 615                         /* bits to transfer */
 616                         bits = (length - 1);
 617                         command |= bits << 5;
 618                         length = 0;
 619                 } else {
 620                         /* whole byte */
 621
 622                         /* bits to transfer */
 623                         bits = 7;
 624                         command |= (7 << 5);
 625                         length -= 8;
 626                 }
 627
 628                 openjtag_add_byte(command);
 629                 openjtag_add_byte(buffer[count]);
 630                 count++;
 631         }
 632
 633         openjtag_scan_result_count++;
 634 }
 635
 636 static void openjtag_execute_reset(struct jtag_command *cmd)
 637 {
 638
 639         DEBUG_JTAG_IO("reset trst: %i srst %i",
 640                         cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 641
 642         uint8_t buf = 0x00;
 643
 644         if (cmd->cmd.reset->trst) {
 645                 buf = 0x03;
 646         } else {
 647                 buf |= 0x04;
 648                 buf |= 0x05 << 4;
 649         }
 650
 651         openjtag_add_byte(buf);
 652 }
 653
 654 static void openjtag_execute_sleep(struct jtag_command *cmd)
 655 {
 656         jtag_sleep(cmd->cmd.sleep->us);
 657 }
 658
 659 static void openjtag_set_state(uint8_t openocd_state)
 660 {
 661         int8_t state = openjtag_get_tap_state(openocd_state);
 662
 663         uint8_t buf = 0;
 664         buf = 0x01;
 665         buf |= state << 4;
 666
 667         openjtag_add_byte(buf);
 668 }
 669
 670 static void openjtag_execute_statemove(struct jtag_command *cmd)
 671 {
 672         DEBUG_JTAG_IO("state move to %i", cmd->cmd.statemove->end_state);
 673
 674         tap_set_end_state(cmd->cmd.statemove->end_state);
 675
 676         openjtag_set_state(cmd->cmd.statemove->end_state);
 677
 678         tap_set_state(tap_get_end_state());
 679 }
 680
 681
 682 static void openjtag_execute_scan(struct jtag_command *cmd)
 683 {
 684
 685         int scan_size, old_state;
 686         uint8_t *buffer;
 687
 688         DEBUG_JTAG_IO("scan ends in %s", tap_state_name(cmd->cmd.scan->end_state));
 689
 690         /* get scan info */
 691         tap_set_end_state(cmd->cmd.scan->end_state);
 692         scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
 693
 694 #ifdef _DEBUG_USB_COMMS_
 695         openjtag_debug_buffer(buffer, (scan_size + 7) / 8, DEBUG_TYPE_BUFFER);
 696 #endif
 697         /* set state */
 698         old_state = tap_get_end_state();
 699         openjtag_set_state(cmd->cmd.scan->ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 700         tap_set_state(cmd->cmd.scan->ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 701         tap_set_end_state(old_state);
 702
 703         openjtag_add_scan(buffer, scan_size, cmd->cmd.scan);
 704
 705         openjtag_set_state(cmd->cmd.scan->ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 706         tap_set_state(cmd->cmd.scan->ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 707
 708         if (tap_get_state() != tap_get_end_state()) {
 709                 openjtag_set_state(tap_get_end_state());
 710                 tap_set_state(tap_get_end_state());
 711         }
 712 }
 713
 714 static void openjtag_execute_runtest(struct jtag_command *cmd)
 715 {
 716
 717         tap_state_t end_state = cmd->cmd.runtest->end_state;
 718         tap_set_end_state(end_state);
 719
 720         /* only do a state_move when we're not already in IDLE */
 721         if (tap_get_state() != TAP_IDLE) {
 722                 openjtag_set_state(TAP_IDLE);
 723                 tap_set_state(TAP_IDLE);
 724         }
 725
 726         if (cmd->cmd.runtest->num_cycles > 16)
 727                 LOG_WARNING("num_cycles > 16 on run test");
 728
 729         uint8_t command;
 730         command = 7;
 731         command |= ((cmd->cmd.runtest->num_cycles - 1) & 0x0F) << 4;
 732
 733         openjtag_add_byte(command);
 734
 735         tap_set_end_state(end_state);
 736         if (tap_get_end_state() != tap_get_state()) {
 737                 openjtag_set_state(end_state);
 738                 tap_set_state(end_state);
 739         }
 740 }
 741
 742 static void openjtag_execute_command(struct jtag_command *cmd)
 743 {
 744         DEBUG_JTAG_IO("openjtag_execute_command %i", cmd->type);
 745         switch (cmd->type) {
 746         case JTAG_RESET:
 747                         openjtag_execute_reset(cmd);
 748                         break;
 749         case JTAG_SLEEP:
 750                         openjtag_execute_sleep(cmd);
 751                         break;
 752         case JTAG_TLR_RESET:
 753                         openjtag_execute_statemove(cmd);
 754                         break;
 755         case JTAG_SCAN:
 756                         openjtag_execute_scan(cmd);
 757                         break;
 758         case JTAG_RUNTEST:
 759                         openjtag_execute_runtest(cmd);
 760                         break;
 761         case JTAG_PATHMOVE:
 762                 /* jlink_execute_pathmove(cmd); break; */
 763         default:
 764                 LOG_ERROR("BUG: unknown Open JTAG command type encountered");
 765                 exit(-1);
 766         }
 767 }
 768
 769 static int openjtag_execute_queue(void)
 770 {
 771         struct jtag_command *cmd = jtag_command_queue;
 772
 773         while (cmd != NULL) {
 774                 openjtag_execute_command(cmd);
 775                 cmd = cmd->next;
 776         }
 777
 778         return openjtag_execute_tap_queue();
 779 }
 780
 781 static int openjtag_speed_div(int speed, int *khz)
 782 {
 783         *khz = speed;
 784
 785         return ERROR_OK;
 786 }
 787
 788 static int openjtag_khz(int khz, int *jtag_speed)
 789 {
 790
 791         if (khz >= 48000)
 792                 *jtag_speed = 48000;
 793         else if (khz >= 24000)
 794                 *jtag_speed = 24000;
 795         else if (khz >= 12000)
 796                 *jtag_speed = 12000;
 797         else if (khz >= 6000)
 798                 *jtag_speed = 6000;
 799         else if (khz >= 3000)
 800                 *jtag_speed = 3000;
 801         else if (khz >= 1500)
 802                 *jtag_speed = 1500;
 803         else if (khz >= 750)
 804                 *jtag_speed = 750;
 805         else
 806                 *jtag_speed = 375;
 807
 808         return ERROR_OK;
 809 }
 810
 811 COMMAND_HANDLER(openjtag_handle_device_desc_command)
 812 {
 813         if (CMD_ARGC == 1)
 814                 openjtag_device_desc = strdup(CMD_ARGV[0]);
 815         else
 816                 LOG_ERROR("require exactly one argument to "
 817                                   "openjtag_device_desc <description>");
 818         return ERROR_OK;
 819 }
 820
 821
 822 static const struct command_registration openjtag_command_handlers[] = {
 823         {
 824                 .name = "openjtag_device_desc",
 825                 .handler = openjtag_handle_device_desc_command,
 826                 .mode = COMMAND_CONFIG,
 827                 .help = "set the USB device description of the OpenJTAG",
 828                 .usage = "description-string",
 829         },
 830         COMMAND_REGISTRATION_DONE
 831 };
 832
 833 struct jtag_interface openjtag_interface = {
 834         .name = "openjtag",
 835         .commands = openjtag_command_handlers,
 836
 837         .execute_queue = openjtag_execute_queue,
 838         .speed = openjtag_speed,
 839         .speed_div = openjtag_speed_div,
 840         .khz = openjtag_khz,
 841         .init = openjtag_init,
 842         .quit = openjtag_quit,
 843 };
 844
 845